轨迹规划——估计规划轨迹曲率代码实现

已经知道轨迹（x，y，theta）一系列点集合，根据之前一篇文章：

Estimating the Trajectory Curvature Using Three Trajectory Points Output From Motion Planning

代码如下：

#include<iostream>
#include<vector>
#include<math.h>
using namespace std;

struct point
{
	double x;
	double y;
	double theta;
};


int main()
{
	
	vector<point> path;
    point point_temp;
    
    double R;
	std::cout << "请输入一个圆的曲率半径的R:" << std::endl;
	cin>>R;
	
	std::cout << "曲率 curve is" << 1/R << std::endl;
	
	double x,y;
	double phi = 0;
	while(phi < 3.14)
	{	
		x = R * cos(phi);
		y = R * sin(phi);
		phi += 0.5;
		std::cout << "x =" << x << " " << "y =" << y  << "phi = " << phi << std::endl; 
		point_temp.x = x;
		point_temp.y = y;
		point_temp.theta = phi;
		
		path.push_back(point_temp);
	}
	
/*
	//first point
	point_temp.x = 9;
	point_temp.y = ;
	point_temp.theta = 0.5;	
	path.push_back(point_temp);
	
	//second point
	point_temp.x = 1.3;
	point_temp.y = 2.5;
	point_temp.theta = 0.7;
	path.push_back(point_temp);
	
	
	//third point
	point_temp.x = 1.7;
	point_temp.y = 2.9;
	point_temp.theta = 2.0;
	path.push_back(point_temp);
	
	
	*/
	//calculate Path Curve
	
	int i = 1;
	double phi_error = path[i+1].theta - path[i-1].theta;
	
	double dist_1 =  (path[i].x - path[i-1].x) * (path[i].x - path[i-1].x) + (path[i].y - path[i-1].y) *  (path[i].y - path[i-1].y);
	double dist_2 =  (path[i+1].x - path[i].x) * (path[i+1].x - path[i].x) + (path[i+1].y - path[i].y) *  (path[i+1].y - path[i].y);
	
	double curve = phi_error/(sqrt(dist_1) + sqrt(dist_2));
	
	
	std::cout << "算法估计的曲率是：" << curve << std::endl;
	
	
	
	
	
	
	
	
 } 

这段代码模拟一个圆形轨迹，曲率固定为圆去测试。

	vector<point> path;
    point point_temp;
    
    double R;
	std::cout << "请输入一个圆的曲率半径的R:" << std::endl;
	cin>>R;
	
	std::cout << "曲率 curve is" << 1/R << std::endl;
	
	double x,y;
	double phi = 0;
	while(phi < 3.14)
	{	
		x = R * cos(phi);
		y = R * sin(phi);
		phi += 0.5;
		std::cout << "x =" << x << " " << "y =" << y  << "phi = " << phi << std::endl; 
		point_temp.x = x;
		point_temp.y = y;
		point_temp.theta = phi;
		
		path.push_back(point_temp);
	}

利用上述的几个点，去验证下面估计的曲率。

	//calculate Path Curve
	
	int i = 1;
	double phi_error = path[i+1].theta - path[i-1].theta;
	
	double dist_1 =  (path[i].x - path[i-1].x) * (path[i].x - path[i-1].x) + (path[i].y - path[i-1].y) *  (path[i].y - path[i-1].y);
	double dist_2 =  (path[i+1].x - path[i].x) * (path[i+1].x - path[i].x) + (path[i+1].y - path[i].y) *  (path[i+1].y - path[i].y);
	
	double curve = phi_error/(sqrt(dist_1) + sqrt(dist_2));
	
	
	std::cout << "算法估计的曲率是：" << curve << std::endl;

输出结果：