无人车采用纯跟踪算法跟踪离线路径（ROS，C++）坐标系转换选取问题

m0_46699649

已于 2024-05-26 08:57:22 修改

阅读量1k

点赞数 23

文章标签：算法 c++ 开发语言人工智能智慧城市

于 2024-05-22 21:55:29 首次发布

本文链接：https://blog.csdn.net/m0_46699649/article/details/139129043

版权

一：序言

上个文章写了如何采用纯跟踪算法跟踪已知路径的整体项目流程，包括如何采集路径点，算法实现，底层模块通信等等，感兴趣的可以去看看我写的前两个文章，作为学习完C++，ROS不知道怎么入门机器人的同学是一个很好的上手项目。
链接: 无人车采用纯跟踪算法跟随离线路径（ROS，C++实现）第一部分
链接: 无人车采用纯跟踪算法跟随离线路径（ROS，C++实现）第二部分实车验证

当时在文章中写了一个问题就是我的车必须指向一个位置的时候，它才能获取正确的坐标点，不然数据就会出现反向问题，后面成功完成了问题的定位，现在把这个自动驾驶坐标系转换进行整体梳理一遍。

二：自动驾驶坐标系

定位传感器一般式以GPS等进行完成，该传感器获取的数据为经纬度处于WGS84坐标系下，因为网上有很多坐标系的介绍，我就不做多的介绍，只对跟踪离线路径这个项目中出现的问题进行解释，
本次无人车进行运动是以车辆起点为原点的东北天坐标系，所以需要对传感器获得的数据进行处理，转换为东北天坐标系：
之前转换方法：经纬度（LLA）转换成地心地固坐标系（ECEF）的坐标，地心地固坐标系（ECEF）的坐标转换成东北天（ENU）坐标系坐标。

经纬度（LLA）转换为地心地固坐标系（ECEF）的坐标：经纬度是一种用于表示地球上某一点位置的坐标系统，包括经度、纬度和海拔高度。将经纬度转换为地心地固坐标系的坐标可以将地球上的点映射到一个以地球中心为原点的坐标系中。这种转换通常使用大地测量学的方法，将经纬度转换为地球上的三维直角坐标系。
地心地固坐标系（ECEF）转换为东北天坐标系（ENU）的坐标：地心地固坐标系是以地球中心为原点的坐标系，其中X轴指向经度0度的交点，Y轴指向经度90度的交点，Z轴指向北极。而东北天坐标系是以参考点为原点的坐标系，其中X轴指向东方，Y轴指向北方，Z轴指向天空。将地心地固坐标系转换为东北天坐标系，可以将全球坐标映射到以某个参考点为中心的局部坐标系。

当时完成这个操作后就已经结束了，实际上车辆在坐标系上的定位不仅仅是位置坐标，车辆的偏航角也是很重要的一个部分，东北天坐标实际上是把这个坐标系固定死，不论开头车辆偏航角指向何处，所以就会存在之前的坐标转换出现错误的问题，在进行控制，导航等过程中，是需要将车辆自身的车辆起始前进位置和这个X轴一致，这样就不会存在车辆车头不摆放正确就出现错误的问题。

解决方法：在完成东北天坐标系建立后，获取车辆的初始偏航角，经过旋转矩阵就可以完成修改后的东北天坐标系，后续坐标均在该坐标系下就可以成功完成。

三：代码

LLA转ENC的C++代码如下大家可以参考下

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