一前言

  最近是由于实验室需求，做了一个简单的无人车轨迹跟踪算法，最主要要完成的功能还是希望无人车能够跟随离线轨迹动起来，因为自己也是在学习阶段，第一次接触这一块的知识，所以就写了这个博客，希望自己能够更深入的学习这一块，同时也能够记下自己在实践过程中一次问题，因为是第一次实践，可能会存在一些不太确定的问题或者说是一种错误的问题，如果大佬们发现了，欢迎大佬指出我的错误。

二实现方案

实验室有一辆无人车，传感器有导远电子的车载组合惯导定位系统和激光雷达，相机。实际上我只是用了组合惯导。
采用方案流程：
1.人为开车通过惯导记录车轨迹
2.采用纯跟踪算法来对其进行跟踪

具体实现流程会在下一节讲，因为主要是记录在实现过程中出现的一些问题，所以一些基础知识，能够很容易查到的就不细讲

三具体实现步骤

3.1 数据采集

首先我们需要完成离线轨迹的获取，这里我采用的是通过Ros接口获取车在运动过程中每时刻的经纬度，然后转换为东北高坐标系。我这里的传感器里面获取的数据是WGS84坐标系，消息格式如下:

float64 latitude
float64 longitude
float64 altitude
我这里存在的问题:因为我一直不太清楚离线轨迹和车状态应该在什么情况下才能在同一个坐标系下，并且离线轨迹下的航向角和车航向角一致，所以在这种情况下，我直接考虑将两个都转换为同一东北高坐标系下，起点采用相同的经纬高来进行准换，这样在后续的考虑中我并没有再进行离线轨迹上点转换为车辆坐标系上，不知道这样的操作是否正确。
下面是我具体代码

void doMsg(const fsd_common_msgs::ASENSING::ConstPtr &msgs){ROS_INFO("进入回调函数进行处理");            car.GeoDetic_TO_ENU( (msgs->latitude)*PII /180, (msgs->longitude)*PII /180, msgs->altitude, first_lat*PII /180,first_lon*PII /180,first_alt, &enu_xyz[0]);}

通过回调函数进行惯导消息处理，将该消息转换为东北高坐标系
这里的first_lat，first_lon，first_alt是记录第一个数据的经纬度，也就是车开启惯导时记录的第一个位置，我们要先记录惯导的第一个位置作为我们的起点，后续车自身状态的转换实际上也是采用相同的值，注意这里的值一定要相同，这样才可以简化转换车辆坐标系，目前离线测试这样的想法是对的。

void  huatcar ::GeoDetic_TO_ENU(double lat, double lon, double h, double lat0, double lon0, double h0, double enu_xyz[3])
{double a, b, f, e_sq;a = 6378137;b = 6356752.3142;f = (a - b) / a;e_sq = f * (2 - f);// 站点（非原点）double lamb, phi, s, N, sin_lambda, cos_lambda, sin_phi, cos_phi, x, y, z;lamb = lat;  phi = lon;s = sin(lamb);N = a / sqrt(1 - e_sq * s * s);sin_lambda = sin(lamb);cos_lambda = cos(lamb);sin_phi = sin(phi);cos_phi = cos(phi);x = (h + N) * cos_lambda * cos_phi;y = (h + N) * cos_lambda * sin_phi;z = (h + (1 - e_sq) * N) * sin_lambda;// 原点坐标转换double lamb0, phi0, s0, N0, sin_lambda0, cos_lambda0, sin_phi0, cos_phi0, x0, y0, z0;lamb0 = lat0;phi0 = lon0;s0 = sin(lamb0);N0 = a / sqrt(1 - e_sq * s0 * s0);sin_lambda0 = sin(lamb0);cos_lambda0 = cos(lamb0);sin_phi0 = sin(phi0);cos_phi0 = cos(phi0);x0 = (h0 + N0) * cos_lambda0 * cos_phi0;y0 = (h0 + N0) * cos_lambda0 * sin_phi0;z0 = (h0 + (1 - e_sq) * N0) * sin_lambda0;// ECEF 转 ENUdouble xd, yd, zd, t;xd = x - x0;yd = y - y0;zd = z - z0;t = -cos_phi0 * xd - sin_phi0 * yd;enu_xyz[0] = -sin_phi0 * xd + cos_phi0 * yd;enu_xyz[1] = t * sin_lambda0 + cos_lambda0 * zd;enu_xyz[2] = cos_lambda0 * cos_phi0 * xd + cos_lambda0 * sin_phi0 * yd + sin_lambda0 * zd;ROS_INFO("输出转换完成坐标");   cout << "current_x = "<<enu_xyz[0] <<"current_y = "<<enu_xyz[1]<<endl;points.push_back({enu_xyz[0],enu_xyz[1]});
}

这个就是WGS84坐标系转东北高坐标系。具体流程我就没看，直接知道接口用了。

void huatcar::Save_Point()
{ofstream p;                                                                                                          p.open("/home/yeziheng/PP_car_PID/src/map/1.txt",ios::out);                                                        p << std::setprecision(20);                                 for(int i = 0; i < points.size(); i++){p << " "<< points[i][0]<< " " <<points[i][1]<< " " << '\n';}p.close();   }

这个就是将转换完成的数据放进txt文件里面，只记录x，y坐标值，实际上我们也不需要其他的值，纯跟踪算法只用跟踪点的坐标，其余参数是不需要的，同时因为传感器获取的数值是角度，所以我们这里需要把他改变为弧度处理，进行单位转换，这个我在刚开始做的时候就忘记了。
这个时候是已经完成了离线轨迹获取模块的操作，这里由于自己代码写的不够完善的问题，可能在进行第一次测试的时候就需要先修改代码让车先输出第一个经纬度，然后将第一个经纬度写进程序，然后可以人为开车进行数据的获取。这个模块不会有太多复杂的东西，就是完成人开车离线轨迹的获取，ROS节点的编写操作，后续就是控制算法的设定。

3.2 纯跟踪算法实现

纯跟踪算法的一个具体原理这里就不说了，有很多大佬的博客讲的都挺好，我这里只讲我这具体实现的时候所出现的问题。
首先是自身状态的输入，我这里还是采用惯导来作为状态输入，这里就跟上一节一样采用东北高坐标系来进行处理，后续我会在用纯跟踪算法时省略了离线轨迹上点往车身坐标系上转换的操作。代码给获取离线轨迹坐标的代码一样就不写了。
第二步我们需要把txt文件里面的坐标值获取出来，下面是我的代码

void robot::cal_point(const string s)
{ROS_INFO("构造函数进入");ifstream infile;infile.open(s);if (!infile){cout << "Unable to open file";exit(1);}double a, b;for (int i = 0; i < 343; i++){infile >> a >> b;refx.push_back(a);refy.push_back(b);}
}

这里是读取txt文件，然后建立两个容器来存放坐标点的x和y值，这里refx，refy就是用来存放轨迹点的x和y值。343实际上是有多少个点，这个是看TXT文件有多少行来进行输入的。

第三步就是需要考虑纯跟踪算法的具体实现流程
先放具体实现代码

void robot::vel_cmd(fsd_common_msgs::ControlCommand  &cmd, ros::Publisher &pub, ros::Rate &rate)
{int idx = 0;int temp_idx = label_for_pp;float delta_max = 0.5061;                           //最大弧度float ld1 = 2.5;                                              //前瞻距离  2.7for (int i = label_for_pp; i < 343; i++)            {float dis = sqrt(pow(gy - refy[i], 2) + pow(gx - refx[i], 2));if (dis <= ld1){temp_idx = i;}else{break;}}label_for_pp = temp_idx;idx = temp_idx;//转角float alpha = atan2(refx[idx] - gx,refy[idx] - gy) - gheading;                                alpha = (alpha > pi) ? (alpha - 2 * pi) : (alpha < -pi) ? (alpha + 2 * pi) : alpha;float delta_pp = atan2(2 * l * sin(alpha) / ld1, 1.0);float delta = delta_pp;delta = max(min(delta_max, delta), -delta_max);cmd.steering_angle.data= delta;

设定最大前瞻距离，然后依次遍历轨迹里面所有点与当前位置的距离，如果超过前瞻距离，则选取该索引的点作为跟踪点，alpha直接采用跟踪点与自身位置的夹角减去偏航角，并且对其进行一个范围限制，防止角度突变。最后通过纯跟踪算法公式计算出车轮转角，因为计算出的是弧度后续需要在传给底层控制模块的时候需要转换为角度。
关于速度的控制的代码是采用了一个p控制器来对速度进行处理，因为我们车辆可能只会在3m/s左右的速度行驶就可以满足要求，所以初次确定就是以一个p控制器来进行处理。

currentSpeed = sqrt(pow(my_ins.east_velocity,2)+pow(my_ins.north_velocity,2)+pow(my_ins.ground_velocity,2));
longError = 3.0 - currentSpeed;
longCurrent = 0.5*longError + longLast;
longLast = longCurrent;                                                  if(currentSpeed >2){longCurrent = 10;}if(longCurrent > 30){longCurrent = 30;}if (currentSpeed <= 0.3){longCurrent = 50;}

总结

上面就是自己做的一个简单的路径跟踪，在离线测试过程中，输出转角的变化率最大变化是-5快速变化到20，所以基本是认为可行的，后续会等实验室的车修好，在实车上进行测试，再具体对整体测试效果进行梳理

无人车采用纯跟踪算法跟随离线路径（ROS，C++实现）第一部分相关推荐

纯跟踪算法用于无人车自动泊车
目的使用简单的"纯跟踪算法"实现无人车自动泊车或者位姿调整.在泊车或者工业场景,如果空间不够,那么车辆经常需要做一些大角度的转向或者倒车,例如叉车.这些场景与一般的道路行驶场景可 ...
自动驾驶笔记-轨迹跟踪之①纯跟踪算法（Pure Pursuit）
提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档文章目录前言一.阿克曼转向模型 1.1 模型理解 1.2 模型表达二.纯跟踪算法(Pure Pursuit) 2.1 算法理解 ...
Pure Pursuit纯跟踪算法Python/Matlab算法实现
本文的python源代码来自: https://github.com/gameinskysky/PythonRobotics/blob/master/PathTracking/pure_pursuit ...
matlab实现纯跟踪算法
对<Pure Pursuit纯跟踪算法Python/Matlab算法实现>该博客matlab算法提升: 该博客链接:https://blog.csdn.net/gophae/article ...
自动驾驶纯跟踪算法仿真基于Carsim-ros-simulink联合仿真平台的pp算法仿真
自动驾驶纯跟踪算法仿真基于Carsim-ros-simulink联合仿真平台的pp算法仿真 pure pursuit纯跟踪算法轨迹跟踪仿真可用于两个PC端或者虚拟机支持Carsim2018和m ...
路径跟踪算法之PID路径跟踪与PP跟踪
路径跟踪算法之PID路径跟踪与PP跟踪 1路径跟踪原理 2 常见的路径跟踪算法 2.1 Pure puresuit(pp)纯跟踪 2.2 PID 跟踪 1路径跟踪原理在运用好的路径规划算法,规划好一 ...
北京内推 | 美团无人车团队招聘视觉算法实习生
合适的工作难找?最新的招聘信息也不知道? AI 求职为大家精选人工智能领域最新鲜的招聘信息,助你先人一步投递,快人一步入职! 美团美团无人车配送中心成立于 2016 年,由美团首席科学家夏华夏博士领 ...
滴滴正式分拆无人车业务，复刻Uber上市路径，传孙正义再加持
李根发自凹非寺量子位报道 | 公众号 QbitAI 滴滴无人车,终于也官宣分拆了. 今天(8月5日)滴滴出行宣布旗下自动驾驶部门,升级为独立公司,专注于自动驾驶研发.产品应用及相关业务拓展. ...
MATLAB Simulink开发ROS无人车与机器人应用详细教程
引言:MATLAB在机器人中的应用现在大多数机器人开发者都会选择ROS,在ROS整个框架下"调包"极其容易.很多ROS开发者热衷于"调包"来实现功能,却难以在 ...

无人车采用纯跟踪算法跟随离线路径（ROS，C++实现）第一部分

一前言

二实现方案

三具体实现步骤

3.1 数据采集

3.2 纯跟踪算法实现

总结

无人车采用纯跟踪算法跟随离线路径（ROS，C++实现）第一部分相关推荐

最新文章

热门文章

无人车采用纯跟踪算法跟随离线路径（ROS，C++实现）第一部分

一 前言

二 实现方案

三 具体实现步骤

3.1 数据采集

3.2 纯跟踪算法实现

总结

无人车采用纯跟踪算法跟随离线路径（ROS，C++实现）第一部分相关推荐

最新文章

热门文章

一前言

二实现方案

三具体实现步骤