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

一 前言

  最近是由于项目需求，完成了一个无人车路径跟踪算法，方便后续进行更复杂的算法实现。无人车搭载定位传感器通过人为驾驶获取跟随路径点，然后采用纯跟踪算法完成车辆的自主跟随运动。

二 实现方案

项目有一辆无人车，传感器有导远电子的车载组合惯导定位系统和激光雷达，相机。实际上我只是用了组合惯导。
采用方案流程：
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是记录第一个数据的经纬度，也就是车开启惯导时记录的第一个位置，我们要先记录惯导的第一个位置作为我们的起点，后续车自身状态的转换实际上也是采用相同的值，注意这里的值一定要相同，这样才可以简化转换车辆坐标系，目前离线测试这样的想法是对的。
转换的坐标的部分代码如下：

{
	    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 转 ENU
	double xd, yd, zd, t;
	xd = x - x0;
	yd = y - y0;
	zd = z - z0;
	t = -cos_phi0 * xd - sin_phi0 * yd;
}

这个就是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文件有多少行来进行输入的。

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

 //转角
	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，所以基本是认为可行的，后续会等实验室的车修好，在实车上进行测试，再具体对整体测试效果进行梳理。（后续文章会对所有存在的问题都一一进行讲解）