开发者说丨地理与平面坐标系对齐及INS数据可视化

地理坐标：为球面坐标。  参考平面地是椭球面，坐标单位：经纬度。

大地坐标：为平面坐标。参考平面地是水平面，坐标单位：米、千米等。

地理坐标转换到大地坐标的过程可理解为投影（将不规则的地球曲面转换为平面）。

在ArcGIS中预定义了两套坐标系：地理坐标系（Geographic coordinate system）和投影坐标系（Projected coordinate system）。

那么这两套坐标系如何实现转换呢？

下面是由社区开发者—胡想成提供的文章，对地理与平面坐标系对齐及INS数据可视化进行详细讲解，希望这篇文章能给感兴趣的开发者带来更多帮助。

  以下，ENJOY  

社区公众号之前推送的一篇文章《开发者说丨组合导航调试与驱动实践》，按步骤完成组合导航配置之后可以进行精度的测试，这里我们首先录制一个bag包，用以下两种方式来进行分析。

PlotJugger是ROS提供的的插件工具，安装之后可以通过rosrun plotjugger plotjugger来启动，是我们查看bag包数据的常用工具。直接load bag数据包，选定指定的topic数据字段到程序中进行分析即可。如下图，这里我们主要分析位置（LLA）和姿态（YAW）变化情况。

• 经纬度轨迹是否和预期一致

• yaw角变化是否和预期一致

• 加速度/角速度变化情况

对于角速度而言，在静止情况下应该都为0，加速度水平的话，y轴为0，z轴为重力加速度，基本为1。

左侧选定经纬度字段（也可以是utm_east、utm_north字段），鼠标右键拖到右侧plot中可以明显看到小车INS记录的运动轨迹，点击启动按钮，观察红色点的变动情况，可跟踪小车运行状态。显然这里我们的GPS轨迹是符合预期的。

继续查看yaw角，起始位置一直保持yaw角为160度，我们需要关注的是yaw角变化的一些节点，一般来说小车是在进行转向操作。

对于yaw角的取值，我们需要明确其来源。以组合导航为例，有双天线RTK时可以计算heading，即与正北方向的夹角，这里惯导输出的融合之后的yaw角肯定就是相对于正北方向的夹角，简单的说就是此时刻的yaw即为车头方向。若RTK无解，我们知道IMU上电之后测量的yaw角为0，此种情况下组合导航输出的yaw角即为相对于惯导上电时刻朝向的夹角。

组合导航仪在定位过程中，会遇到RTK无解的情况，此时输出的yaw角为第二种情况，在后续运动过程中找回了RTK有解了，也会逐步较准回来。

另外，针对于单个IMU而言，九轴IMU配有地磁仪，可以考虑为第一种情况。而六轴IMU无地磁仪，考虑为第二种情况。

观察yaw角大小变化还有一种简单的测试方法，打开手机指南针软件，朝向车头方向沿着车的行进轨迹走，观察与正北方向的夹角变化情况，特别注意转弯地方的夹角变化，若基本一致，则yaw角数据正常。

最后我们还可以查看一下测试路面的坡度变化，这里x轴朝向车头，坡度变化对应的是pitch角，也符合我们的预期。

这里提醒一下,车子在平面上加速的时候,轮胎的传动轴会有扭矩作用,让车轮转动.车子本身会有反扭矩,这个反作用力在传动轴上会有一个向上或者向下的作用力,从而使车头向上或向下.这个角度虽然肉眼看不出来,但车身的传感器能够感受到,从而产生一个较大的pitch,此pitch角可能会被误当做一个坡度信息。

这个问题我之前一直没考虑过,但在实车测试的时候发现的,目前只能通过其他的传感器来解决此问题。

这里我们完成一个简单的测试程序，在地图上观察GPS轨迹和IMU朝向是否正常。

我们需要一张点云地图及其地图原点坐标，可以通过SLAM算法完成地图构建并记录地图起点的经纬坐标。

程序的大致流程：

• 接收ins数据，LL转XY为位置，yaw角为姿态。

• 发布odometry。

首先地理系和平面系如何进行对齐呢？

我们知道建图的时候，如果不记录GPS数据的话，一般的SLAM算法完成的地图就是点云数据，地图里面存储的这些点的坐标原点，就是（0，0，0）了。如果记录下起点对应的经纬LLA坐标，那么点云地图里面的每一个位置都可以转换成经纬坐标。这样在定位的时候，我们可以把每一个GPS点在地图上标出来，就可以清晰看到小车在地图上的运动轨迹了。

这里点坐标，比如（1，1，1），即为平面系，将所有的GPS LLA坐标转换到平面系中进行后续的计算。这里坐标系转换流程，LLA->ECEF->ENU，ENU即为我们的平面系，东北天。

核心代码如下：

 1nh.getParam("origin_longitude", origin_longitude);//点云地图原点LLA
 2nh.getParam("origin_latitude", origin_latitude);
 3nh.getParam("origin_altitude", origin_altitude);
 4
 5nh.getParam("imu_topic", imu_topic);//TOPIC
 6nh.getParam("ins_topic", ins_topic);
 7nh.getParam("gps_topic", gps_topic);   
 8
 9pose_pub = nh.advertise<:odometry>("/odom", 1, false);//PUB or SUB
10ros::Subscriber imu_sub = nh.subscribe(imu_topic, 5, imu_callback);
11ros::Subscriber ins_sub = nh.subscribe(ins_topic, 5, ins_callback);

<左右滑动以查看完整代码>

INS_callback(const raw_sensor_msgs::GnssImuInfo &msg)

 1sensor_msgs::NavSatFix gps_msgs;
 2gps_msgs.status.status = msg.gps_status;
 3
 4gps_msgs.header.seq = msg.header.seq;
 5gps_msgs.header.stamp = msg.header.stamp;
 6gps_msgs.header.frame_id = msg.header.frame_id;
 7
 8gps_msgs.latitude = msg.latitude;
 9gps_msgs.longitude = msg.longitude;
10gps_msgs.altitude = msg.altitude; 
11
12
13if (std::isnan(msg.latitude + msg.longitude + msg.altitude)) {
14     ROS_INFO("GPS LLA NAN..."); //去NAN
15     return;
16 }
17
18gpsTools gpsTools;
19gpsTools.lla_origin_ << origin_latitude, origin_longitude, origin_altitude; //点云地图原点
20
21//    if (!(msg.gps_status == 48 || msg.gps_status == 49 || msg.gps_status == 50)) {
22if (msg.gps_status == 0) {
23    ROS_ERROR("NO RTK.....");
24    return;
25}
26
27//由于我们原来的工具类是使用NavsatFix标准格式写的转换工具类，这里我的ins是自定义的msg格式，所以就用了//gps_msg存储了经纬，方便转换
28Eigen::Vector3d lla = gpsTools.GpsMsg2Eigen(gps_msgs);//LLA->ENU 
29Eigen::Vector3d ecef = gpsTools.LLA2ECEF(lla);
30Eigen::Vector3d enu = gpsTools.ECEF2ENU(ecef);
31gps_pos_ = enu;
32
33// publish the odometry
34nav_msgs::Odometry odom;
35odom.header.stamp = msg.header.stamp;
36odom.header.frame_id = "map";
37
38odom.pose.pose.position.x = gps_pos_(0);
39odom.pose.pose.position.y = gps_pos_(1);
40odom.pose.pose.position.z = gps_pos_(2);
41//这里需要补偿这个固定的夹角，根据自己车上的情况来处理
42odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(-imu_yaw+M_PI/2);
43
44odom.child_frame_id = "base_link";
45odom.twist.twist.linear.x = 0.0;
46odom.twist.twist.linear.y = 0.0;
47odom.twist.twist.angular.z = 0.0;
48
49pose_pub.publish(odom);

<左右滑动以查看完整代码>

IMU_callback(const sensor_msgs::Imu::Ptr &input)

1tf::Quaternion imu_orientation;
2tf::quaternionMsgToTF(input->orientation, imu_orientation);
3tf::Matrix3x3(imu_orientation).getRPY(imu_roll, imu_pitch, imu_yaw);
4
5//  角度转弧度
6imu_roll = wrapToPmPi(imu_roll);
7imu_pitch = wrapToPmPi(imu_pitch);
8imu_yaw = wrapToPmPi(imu_yaw);

<左右滑动以查看完整代码>

工具类及全局变量

 1#include 
 2#include 
 3#include 
 4#include 
 5#include 
 6
 7#define DEG_TO_RAD 0.01745329252
 8#define EARTH_MAJOR 6378137.0            ///< WGS84 MAJOR AXIS
 9#define EARTH_MINOR 6356752.31424518    ///< WGS84 MINOR AXIS
10
11double imu_roll = 0.0, imu_pitch = 0.0, imu_yaw = 0.0;
12//1.lla的起点
13Eigen::Vector3d lla_origin_;
14//2.enu下的坐标
15Eigen::Vector3d gps_pos_;
16
17static double wrapToPm(double a_num, const double a_max) {
18    if (a_num >= a_max) {
19        a_num -= 2.0 * a_max;
20    }
21    return a_num;
22}
23
24static double wrapToPmPi(const double a_angle_rad) {
25    return wrapToPm(a_angle_rad, M_PI);
26}
27
28static double calcDiffForRadian(const double lhs_rad, const double rhs_rad) {
29    double diff_rad = lhs_rad - rhs_rad;
30    if (diff_rad >= M_PI)
31        diff_rad = diff_rad - 2 * M_PI;
32    else if (diff_rad < -M_PI)
33        diff_rad = diff_rad + 2 * M_PI;
34    return diff_rad;
35}
36
37Eigen::Vector3d gpsTools::GpsMsg2Eigen(const sensor_msgs::NavSatFix &gps_msgs) {
38    Eigen::Vector3d
39            lla(gps_msgs.latitude, gps_msgs.longitude, gps_msgs.altitude);
40    return lla;
41}
42
43Eigen::Vector3d gpsTools::LLA2ECEF(const Eigen::Vector3d &lla) {
44    Eigen::Vector3d ecef;
45    double lat = deg2rad(lla.x());
46    double lon = deg2rad(lla.y());
47    double alt = lla.z();
48    double earth_r = pow(EARTH_MAJOR, 2)
49                     / sqrt(pow(EARTH_MAJOR * cos(lat), 2) + pow(EARTH_MINOR * sin(lat), 2));
50    ecef.x() = (earth_r + alt) * cos(lat) * cos(lon);
51    ecef.y() = (earth_r + alt) * cos(lat) * sin(lon);
52    ecef.z() = (pow(EARTH_MINOR / EARTH_MAJOR, 2) * earth_r + alt) * sin(lat);
53
54    return ecef;
55}
56
57Eigen::Vector3d gpsTools::ECEF2ENU(const Eigen::Vector3d &ecef) {
58    double lat = deg2rad(lla_origin_.x());
59    double lon = deg2rad(lla_origin_.y());
60
61    Eigen::Vector3d t = -LLA2ECEF(lla_origin_);
62    Eigen::Matrix3d r;
63    r << -sin(lon), cos(lon), 0,
64            -cos(lon) * sin(lat), -sin(lat) * sin(lon), cos(lat),
65            cos(lon) * cos(lat), sin(lon) * cos(lat), sin(lat);
66
67    Eigen::Vector3d enu;
68    enu = ecef + t;
69    enu = r * enu;
70    return enu;
71}
72
73//这俩函数自己稍微改一下
74static inline double deg2rad(const double °) {
75        return deg * DEG_TO_RAD;
76};
77static inline double rad2deg(const double &rad) {
78    return rad / DEG_TO_RAD;
79}

<左右滑动以查看完整代码>

本节三张图即为测试结果，一图是GPS位置+IMU姿态，二图是有RTK，三图为厘米级RTK。我们的测试场地是两栋大楼之间即侧面的路段，其中无RTK的那段路为经过楼道。

可以看到小车运动轨迹符合预期，虽然数据质量确实不太好。姿态yaw角朝车头方向，这里惯导输出的yaw角可能会和车头有一个固定的夹角（M_PI或者M_PI/2），需要注意。我这里面对ins输出的yaw角转为弧度之后，取M_PI/2 - yaw才拿到车头方向。

综合分析，在园区等GPS信号不太好的情况下，组合导航就显得比较鸡肋了，但是用来作为点云定位初始化还是比较普遍的。

参考：INS550D、MD-649说明手册。

感谢：一清科技各位大佬、广州导远电子程工、中科院自动化所飞哥、主线科技刘工、哈工大导航专家刘锦鲤刘博等。

以上是"地理与平面坐标系对齐及INS数据可视化"的全部内容，更多话题讨论、技术交流可以扫描下方二维码添加『Apollo小哥哥』为好友，进开发者交流群。

* 以上内容为开发者原创，不代表百度官方言论。

内容来自攻城狮の家：

http://xchu.net/2020/01/10/37gnss-localizer/，欢迎大家收藏点赞。已获开发者授权，原文地址请戳阅读原文。