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

一 前言

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

二 实现方案

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

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

三 具体实现步骤

3.1 数据采集

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

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]);

}
1
2
3
4
5
6

通过回调函数进行惯导消息处理，将该消息转换为东北高坐标系
这里的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 转 ENU
	double 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]});    
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51

这个就是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();   

}

1
2
3
4
5
6
7
8
9
10
11
12
13
14

这个就是将转换完成的数据放进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);
		}
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

这里是读取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.7
	for (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;                                         
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

设定最大前瞻距离，然后依次遍历轨迹里面所有点与当前位置的距离，如果超过前瞻距离，则选取该索引的点作为跟踪点，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;
 }
1
2
3
4
5
6
7
8
9
10
11
12
13

总结

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