当前位置：首页 > news >正文

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

news 2024/9/22 13:46:10

已经知道轨迹（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 pointpoint_temp.x = 9;point_temp.y = ;point_temp.theta = 0.5;	path.push_back(point_temp);//second pointpoint_temp.x = 1.3;point_temp.y = 2.5;point_temp.theta = 0.7;path.push_back(point_temp);//third pointpoint_temp.x = 1.7;point_temp.y = 2.9;point_temp.theta = 2.0;path.push_back(point_temp);*///calculate Path Curveint 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 Curveint 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;