全球卫星导航系统(Global Navigation Satelite System,GNSS)，简称卫星导航，是室外机器人定位的一个主要信息来源。
卫星导航能给机器人提供什么信息？
正常工作时，实际上可以提供机器人所需的所有定位信息，包括：
- 位置
- 姿态
- 速度等物理量
但是仅依靠卫星导航还不足以让机器人在室外完成自主导航任务，主要原因有一下几点：
- GNSS提供的定位精度不能满足要求，GNSS分多个细分种类，有些GNSS定位方法可以提供很高的精度，但要求物体必须静止一段时间（通常十分钟以上）；也有的方法可以提供较好的动态物体定位，但需要事先架设一个或多个基站。
- GNSS的定位频率不能满足要求，一般在5-10hz
- GNSS定位可用性存在问题，不能够全天候、全场地使用，稳定性与场景、结构、物体的遮挡关系，甚至和天气有关。
GNSS定位原理：
GNSS通过测量自身与地球周围各卫星的距离来确定自身位置，与卫星的距离主要通过测量时间间隔来确定。一个卫星信号从卫星上发出时，带有一个发送时间,而GNSS接收机接收到它时，有一个接收时间，通过比较时间间隔，就能估算各卫星离我们的距离。GNSS本质上可以看成一种高精度的授时系统。
GNSS定位技术分为以下两种：
- 单点GNSS定位，即传统的米级精度卫星定位。这种定位方式价格低廉，应用广泛。大多数手机、车机等终端都具备单点卫星定位能力。在普通车辆的道路级导航中，单点定位的精度足以让驾驶员辨认出车辆位于哪条道路，但在多条道路并排时，它的精度不足以区分车辆在高速路上还是辅路上。
- RTK定位。由于卫星信号在传输中可能产生误差，发展了差分定位技术，即通过地面上的一个已知精确位置的基站与车辆通信，校正车辆卫星接收机的信号。差分定位又进一步分为位置、伪距和载波相位差分定位。
世界坐标系
GNSS提供的位置坐标为世界坐标系下的坐标
物理世界中存在多种普遍使用的世界坐标系
地理坐标系
地球上最常见的坐标系就是经纬度（Latitude Longitude）坐标系，也称为地理坐标系（Geography Coordinate System）。
再加上高度就形成了经纬高（Latitude-longitude-altitude,LLA）坐标系。
经纬度是指按横向和纵向对地球表面进行均匀的切分。经度是从本初子午线向东西各180°，纬度则是从赤道向南北各90°。这两个数值均为角度值或弧度值。高度方面则可使用海拔高度或者地心高度，它们都是相对某个基准水平面的高度。
经纬高坐标系优势：
经纬度是十分直观、好用的坐标系，能够覆盖整个地球。许多地图系统都会首选使用经纬度坐标系作为默认的坐标系。
经纬高坐标系劣势：
机器人导航地图通常覆盖城市级别或者更小的范围，经纬高坐标系会让坐标系统的有效数字变多（建筑物级别的经纬度通常要精确到小数点后8至9位），读起来比较费力。
它们与日常接触的米制单位的转换关系不够线性，例如一度经度在北京可以对应0米，在赤道可能对应上百千米。
UTM坐标系
UTM（Universal Transverse Mercator Grid System）坐标系是将地球视为一个椭球体（Wold Geodetic System 84 椭球体），投影至横躺的圆柱体上，将其展开并进行分区得到。
UTM坐标系特点：
- 它将经度分为60个区，将纬度分为20个区，并赋予标号；
- 经度方向为数字标号，纬度方向为字母标号；
- 除了各别地方，这些分区大体是均匀分布的（沿经纬度均匀分布）；
- 由于地球本身是球面，它们在米制单位上并不是均匀的；
- 两级区域在平铺之后有较大畸变，所以UTM的经纬度有效范围是南北80°以内。
看下面的示意图，可以更好理解上面的特点
在每个分区内，UTM坐标以正东、正北的米制坐标来表达机器人位置。由于地球半径约为6378千米，可以算得UTM一格在东西向最宽约66.7万米。于是，UTM正动坐标是指，将该区的经度中心线取x=500000，然后取向东的偏移量，如果某个点落在中心线以西，则x坐标将小于500000米，但仍为正数。正北方向则以赤道的投影距离为原点，取这个点偏离赤道的距离为y坐标。那么在北半球中，将正东视为X轴，正北视为Y轴，按照右手坐标系，Z轴应该指向天空。这样就定义了一个分区内的世界坐标系，且符合东北天坐标系的习惯。或者也可以将正北视为X轴，将正东视为Y轴，Z轴指向地面，定义北东地坐标系。
UTM的优点是使用了米制坐标，与其它传感器的兼容性好，缺点是某些地区可能在两个分区的跨界处，需要进行额外的坐标处理。
由于地球的投影畸变，实际的UTM坐标与米制单位之间还有一个0.9996的倍数关系，在高精度场合中需要将其考虑进去。
经纬度到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_macro
namespace="${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坐标
```c
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格式定义如下：
```c
/// 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 xy
double 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}=\begin{bmatrix}
R_{Z}(\theta ) & t\
0 &1
\end{bmatrix}
机器人坐标系到世界坐标系的变化矩阵$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进行融合，实现组合导航。
评论（0）
您还未登录，请登录后发表或查看评论