地理坐标系与UTM坐标系转换并进行gazebo测试

本文主要是介绍地理坐标系与UTM坐标系转换并进行gazebo测试，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

全球卫星导航系统(Global Navigation Satelite System,GNSS)，简称卫星导航，是室外机器人定位的一个主要信息来源。

卫星导航能给机器人提供什么信息？
正常工作时，实际上可以提供机器人所需的所有定位信息，包括：

• 位置
• 姿态
• 速度等物理量

但是仅依靠卫星导航还不足以让机器人在室外完成自主导航任务，主要原因有一下几点：

• GNSS提供的定位精度不能满足要求，GNSS分多个细分种类，有些GNSS定位方法可以提供很高的精度，但要求物体必须静止一段时间（通常十分钟以上）；也有的方法可以提供较好的动态物体定位，但需要事先架设一个或多个基站。
• GNSS的定位频率不能满足要求，一般在5-10hz
• GNSS定位可用性存在问题，不能够全天候、全场地使用，稳定性与场景、结构、物体的遮挡关系，甚至和天气有关。

GNSS定位原理：
GNSS通过测量自身与地球周围各卫星的距离来确定自身位置，与卫星的距离主要通过测量时间间隔来确定。一个卫星信号从卫星上发出时，带有一个发送时间,而GNSS接收机接收到它时，有一个接收时间，通过比较时间间隔，就能估算各卫星离我们的距离。GNSS本质上可以看成一种高精度的授时系统。

经纬度到UTM坐标的转换

一般的单点GNSS或者RTK都会输出接收器测量到的经纬度。

下面通过程序将测量到的经纬度转换为米制的UTM坐标。

有的RTK带有双天线，在测量经纬度的同时，还可以输出方向角，如果不考虑机器人的俯仰和横滚，将它们视为零。 即可以将双天线RTK输出的四自由度坐标，在假设机器人的俯仰和横滚为零的前提下，可以把RTK输出视为六自由度的位置变换，即SE(3)的位姿。

经纬度转UTM的算法比较复杂和琐碎，使用一个开源的转换方法来实现这部分内容。

gazebo测试环境

在飞机上添加一个GPS模块

  <!-- Mount an  Default GPS. --><xacro:default_gps namespace="${namespace}" parent_link="${namespace}/base_link" />

默认的GPS 用的 rotors_simulator的 gps 插件 ，可以设置水平、垂直位置精度，水平垂直速度精度

  <xacro:macro name="default_gps" params="namespace parent_link"><!-- Default GPS. --><xacro:gps_plugin_macronamespace="${namespace}"gps_suffix=""parent_link="${parent_link}"gps_topic="gps"ground_speed_topic="ground_speed"mass_gps_sensor="0.015"horizontal_pos_std_dev="3.0"vertical_pos_std_dev="6.0"horizontal_vel_std_dev="0.1"vertical_vel_std_dev="0.1"><inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" /><origin xyz="0 0 0" rpy="0 0 0" /></xacro:gps_plugin_macro></xacro:macro>

设置好后，启动gazebo

打印gps发出的数据如下

header:
seq: 285
stamp:
secs: 57
nsecs: 200000000
frame_id: “firefly/gps_link”
status:
status: 0
service: 1
latitude: 47.366698580832654
longitude: 8.550017892718087
altitude: 499.89846703596413
position_covariance: [9.0, 0.0, 0.0, 0.0, 9.0, 0.0, 0.0, 0.0, 36.0]
position_covariance_type: 3

经纬度转UTM坐标

bool LatLon2UTM(const Vec2d& latlon, UTMCoordinate& utm_coor) {long zone = 0;char char_north = 0;long ret = Convert_Geodetic_To_UTM(latlon[0] * math::kDEG2RAD, latlon[1] * math::kDEG2RAD, &zone, &char_north,&utm_coor.xy_[0], &utm_coor.xy_[1]);utm_coor.zone_ = (int)zone;utm_coor.north_ = char_north == 'N';return ret == 0;
}

输入 向量 Vec2d 就是 Eigen::Vector2d 的经纬度值
输出 UTMCoordinate格式的 utm数据，UTMCoordinate格式定义如下：

/// UTM 坐标
struct UTMCoordinate {UTMCoordinate() = default;explicit UTMCoordinate(int zone, const Vec2d& xy = Vec2d::Zero(), bool north = true): zone_(zone), xy_(xy), north_(north) {}int zone_ = 0;              // utm 区域Vec2d xy_ = Vec2d::Zero();  // utm xydouble z_ = 0;              // z 高度（直接来自于gps）bool north_ = true;         // 是否在北半球
};

其中调用了 Convert_Geodetic_To_UTM 函数是调用的 utm_convert库中的函数
参数解释如下

     *    Latitude          : Latitude in radians                 (input)*    Longitude         : Longitude in radians                (input)*    Zone              : UTM zone                            (output)*    Hemisphere        : North or South hemisphere           (output)*    Easting           : Easting (X) in meters               (output)*    Northing          : Northing (Y) in meters              (output)

输出结果

        std::cout<< "utm zone:  " <<utm_coor.zone_<<std::endl;std::cout<< "utm x:  " <<utm_coor.xy_[0]<<std::endl;std::cout<< "utm y:  " <<utm_coor.xy_[1]<<std::endl;

其中的一帧转换值

utm zone: 32
utm x: 466024
utm y: 5.24601e+06

也就是说
latitude: 47.366698580832654
longitude: 8.550017892718087
转到UTM坐标系约为
zone: 32
x: 466024
y: 5.24601e+06

可以看到x 和 y的数值很大，不好进行判断，一般把获取第一帧数据位置标为零点，也就是后面的数据都减去起始位置

代码如下：

        sensor_msgs::NavSatFix Gps = *Gps_msg;Vec2d gps_latlon;UTMCoordinate utm_coor;static bool first_gnss_set = false;static Vec2d origin = Vec2d::Zero();gps_latlon[0] = Gps.latitude;gps_latlon[1] = Gps.longitude;LatLon2UTM(gps_latlon,utm_coor);if (!first_gnss_set) {origin << utm_coor.xy_[0],utm_coor.xy_[1];first_gnss_set = true;}utm_coor.xy_ = utm_coor.xy_ - origin;

上面是机器人上最简单的获得卫星导航位置的情况，该情况上默认导航的GPS或RTK安装在机器人的正中心位置。

下面介绍复杂一些的情况：GPS的位置与机器人中心有偏差。

还有另一种情况，RTK可以输出双天线的角度，双天线也不与机器人前向一致。

上面的图中，红色坐标代表机器人坐标系，蓝色代表GNSS坐标系。

双天线RTK输出的UTM坐标和方向角的读数，可视为$T_{WG}$,其中W代表世界坐标系，G代表GNSS坐标系。

机器人需要的导航定位数据为$T_{WB}$，其中B为机器人坐标系。

为了将RTK输出的$T_{WG}$转为机器人需要的导航定位数据为$T_{WB}$，则需进行GNSS接收器与机器人中心的外参，来组成$T_{BG}$。

在参数标定中，可以规定如下：
指定安装偏移量$t$,为$O_B$指向$O_G$的矢量，在B系中取坐标，这就是$T_{BG}$的平移分量。
指定安装偏角$\theta$，为B系的x轴转向G系x轴之间的转角，这就是$T_{BG}$的旋转分量。
得到转换矩阵$T_{BG}$为：
$$T_{BG}=\left[\begin{array}{cc}{R}_Z(\theta )& t\\ 0& 1\end{array}\right]$$

机器人坐标系到世界坐标系的变化矩阵$T_{WB}$，最终由RTK的读数和标定的外参计算可得为：
$T_{WB}=T_{WG}{T}_{GB}$
分开写为：
$R_{WB}=R_{WG}{R}_{GB},t_{WB}=R_{WG}{t}_{GB}+t_{WG}$

代码如下：
将方法构造成一个函数
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

bool ConvertGps2UTM(GNSS& gps_msg, const Vec2d& antenna_pos, const double& antenna_angle, const Vec3d& map_origin) 

其中 第一个参数 GNSS 其结构体定义如下：

/// 一个GNSS读数结构
struct GNSS {GNSS() = default;GNSS(double unix_time, int status, const Vec3d& lat_lon_alt, double heading, bool heading_valid): unix_time_(unix_time), lat_lon_alt_(lat_lon_alt), heading_(heading), heading_valid_(heading_valid) {status_ = GpsStatusType(status);}double unix_time_ = 0;                                  // unix系统时间GpsStatusType status_ = GpsStatusType::GNSS_NOT_EXIST;  // GNSS 状态位Vec3d lat_lon_alt_ = Vec3d::Zero();                     // 经度、纬度、高度，前二者单位为度double heading_ = 0.0;                                  // 双天线读到的方位角，单位为度bool heading_valid_ = false;                            // 方位角是否有效UTMCoordinate utm_;       // UTM 坐标（区域之类的也在内）bool utm_valid_ = false;  // UTM 坐标是否已经计算（若经纬度给出错误数值，此处也为false）SE3 utm_pose_;  // 用于后处理的6DoF Pose
};

第二个参数是安装偏移量，第三个参数是安装偏移角度，第四个参数是地图原点。

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
经纬高转换为UTM

    UTMCoordinate utm_rtk;if (!LatLon2UTM(gps_msg.lat_lon_alt_.head<2>(), utm_rtk)) {return false;}utm_rtk.z_ = gps_msg.lat_lon_alt_[2];

用之前介绍的的LatLon2UTM函数
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
如果 卫星导航方案是 双天线RTK则数据中带有方位角信息 ，根据方位角信息是否效，来设置rtk方位角
有的rtk的方位角是北东地坐标系，如果转成东北天坐标系则需要进行一个转换
一个北东地坐标系的方位角h,转换到东北天坐标系下的角度$h^{’}$的转换公式为：
$h^{’}=\pi /2-h$

    /// RTK heading 转成弧度double heading = 0;if (gps_msg.heading_valid_) {heading = (90 - gps_msg.heading_) * math::kDEG2RAD;  // 北东地转到东北天}

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
构建卫星导航坐标系到机器人坐标系的转换矩阵，根据外参的测量结果

此时存在两种情况，第一种双天线RTK，存在安装偏移量与安装偏移角度。

    SE3 TBG(SO3::rotZ(antenna_angle * math::kDEG2RAD), Vec3d(antenna_pos[0], antenna_pos[1], 0));SE3 TGB = TBG.inverse();

其中 antenna_angle 就是安装偏移角度，是机器人坐标系x轴转向卫星导航坐标系x轴之间的转角
antenna_pos 就是安装偏移量，是机器人坐标系原点指向卫星导航坐标系原点的矢量，在机器人坐标系中取坐标

然后就可以求得$T_{WB}$

        // 若指明地图原点，则减去地图原点double x = utm_rtk.xy_[0] - map_origin[0];double y = utm_rtk.xy_[1] - map_origin[1];double z = utm_rtk.z_ - map_origin[2];SE3 TWG(SO3::rotZ(heading), Vec3d(x, y, z));TWB = TWG * TGB;

另一种情况，单点GPS，存在安装偏移量，但是单点GPS不输出方位角，没有卫星导航坐标系，仅有机器人坐标系到GPS中心的角度
需要通过状态估计算法得到的机器人姿态的方位角直接构成$R_{WB}$，此时还需要设置一个$R_{BG}$，因为没有偏移角度，所以说叫设置一个，比如就认为B系和G系的方向一致，那么$R_{BG}=I$,然后再得到$R_{WG}$。

        // 单点gps方案情况float uav_yaw;SO3 RWB = SO3::rotZ(uav_yaw * math::kDEG2RAD);SO3 RBG = SO3::rotZ(0 * math::kDEG2RAD);SO3 RWG = RWB*RBG;// 若指明地图原点，则减去地图原点double x = utm_rtk.xy_[0] - map_origin[0];double y = utm_rtk.xy_[1] - map_origin[1];double z = utm_rtk.z_ - map_origin[2];SE3 TWG(RWG, Vec3d(x, y, z));SE3 TBG(RBG,Vec3d(antenna_pos[0], antenna_pos[1], 0));SE3 TGB = TBG.inverse();TWB = TWG * TGB;

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
然后则对gps结构体中utm的坐标进行赋值即可

    gps_msg.utm_valid_ = true;gps_msg.utm_.xy_[0] = TWB.translation().x();gps_msg.utm_.xy_[1] = TWB.translation().y();gps_msg.utm_.z_ = TWB.translation().z();if (gps_msg.heading_valid_) {// 组装为带旋转的位姿gps_msg.utm_pose_ = TWB;} else {// 组装为仅有平移的SE3// 注意当安装偏移存在时，并不能实际推出车辆位姿gps_msg.utm_pose_ = SE3(SO3(), TWB.translation());}

在gazebo中，设置gps在中心位置，那么则可以设置偏移量和偏移角度为0，然后设置起点位置为原点。

添加路径显示

        geometry_msgs::PoseStamped pose_stamped;pose_stamped.header.stamp = ros::Time::now();pose_stamped.header.frame_id = "/local_frame";pose_stamped.pose.position.x = gps_msg.utm_.xy_[0];pose_stamped.pose.position.y = gps_msg.utm_.xy_[1];pose_stamped.pose.position.z = gps_msg.utm_.z_;pose_stamped.pose.orientation.x = 0;pose_stamped.pose.orientation.y = 0;pose_stamped.pose.orientation.z = 0;pose_stamped.pose.orientation.w = 1;UavGpsPath_.poses.push_back(pose_stamped);UavGpsPath_.header.stamp = ros::Time::now();UavGpsPath_.header.frame_id = "/local_frame";GPS_Path_pub_.publish(UavGpsPath_);

模拟场gps定位精度好，rtk的水平定位精度可以到达2cm，垂直6cm
设置gps的定位噪声如下：

      horizontal_pos_std_dev="0.02"vertical_pos_std_dev="0.06"

随便飞了一个轨迹

前面场景下的gps定位效果好的情况下，如果gps定位的精度不高，则会出现下面的情况

      horizontal_pos_std_dev="0.2"vertical_pos_std_dev="0.4"

由于GPS输出的频率低，并且在信号不好的情况下，会出现较大的噪声，所以并不能直接用于机器人导航，一般会和IMU进行融合，实现组合导航。

这篇关于地理坐标系与UTM坐标系转换并进行gazebo测试的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---