最近在不同环境下进行lio-sam建图时，遇到一个问题：在通过狭窄道路时，两侧高耸的建筑反射卫星信号，导致出现RTK解算status=1/2 RTK增强定位时（1 SBAS_FIX星基增强系统修复，2 GBAS_FIX地基增强系统修复），仍然出现大幅度的位置漂移，从而导致GPS因子介入制图的位置出现错位。
一、概述
这种现象称为多路径效应（Multi-Path Effect），有时也简称多径效应。它出现的原因是在GPS接收机接收卫星信号的过程中，除了接收到通过直线路径到达接收机的信号以外，还可能收到经接收机周围物体的反射再传播过来的卫星信号，这些反射的信号改变了传播方向、振幅、极化以及相位等，与直线信号产生叠加，从而使观测值偏离其真值而产生误差。多径效应会在接收天线处与反射信号干扰导致原始信号失真。这可能导致超过10米的位置误差。
二、解决方案
遇到这个问题后，反复播报看了下跳动幅度，相当的大，100ms内位置可能就会漂移超过10米，既然漂移这么激烈，完全可以通过一个简单的位置跳动过滤来滤掉这类数据，于是仔细研读了下LIO-SAM的源码，涉及的几个关键位置如下：
1. GPS介入频率（gpsFrequence）
config/params.yaml中，存在几个与GPS相关的配置。gpsFrequence是其中的一个参数，默认为10（1.0 / gpsFrequence，相当于10hz，间隔100ms），在mapOptmizationGps.cpp的syncGPS函数调用时传入：
...
void updateInitialGuess() { # 使用GPS初始化pose定位
if (syncGPS(gpsQueue, alignedGPS, timeLaserInfoCur, 1.0 / gpsFrequence)) {
...
void addGPSFactor() { # 建图过程中添加GPS因子
if (syncGPS(gpsQueue, thisGPS, timeLaserInfoCur, 1.0 / gpsFrequence)) {
...
当前帧激光雷达数据时间+-100ms，过早的数据丢弃，过晚的数据暂不使用，只使用激光雷达时间戳前后100ms的数据：
bool syncGPS(std::deque<nav_msgs::Odometry> &gpsBuf,
nav_msgs::Odometry &aligedGps, double timestamp,
double eps_cam) {
bool hasGPS = false;
while (!gpsQueue.empty()) {
mtxGpsInfo.lock();
if (gpsQueue.front().header.stamp.toSec() < timestamp - eps_cam) {
// message too old，丢弃
gpsQueue.pop_front();
mtxGpsInfo.unlock();
} else if (gpsQueue.front().header.stamp.toSec() > timestamp + eps_cam) {
// message too new，暂不使用
mtxGpsInfo.unlock();
break;
} else {
hasGPS = true;
aligedGps = gpsQueue.front();
gpsQueue.pop_front();
mtxGpsInfo.unlock();
}
}
return hasGPS;
}gpsQueue是通过sub gps_odom topic获取：
...
subGPS = nh.subscribe<nav_msgs::Odometry>( "gps_odom", 200, &mapOptimization::gpsHandler, this, ros::TransportHints().tcpNoDelay());
...
std::deque<nav_msgs::Odometry> gpsQueue;
void gpsHandler(const nav_msgs::Odometry::ConstPtr &gpsMsg) {
if (useGPS) {
mtxGpsInfo.lock();
gpsQueue.push_back(*gpsMsg);
mtxGpsInfo.unlock();
}
}而gps_odom是由simpleGpsOdom.cpp订阅/gps/fix并转换后发布的，所以这里可以先增加一个gps增强信号质量过滤：
...
# gpsTopic 在params.yaml中配置
gpsSub = nh.subscribe(gpsTopic, 1000, &GNSSOdom::GNSSCB, this, ros::TransportHints().tcpNoDelay());
left_odom_pub = nh.advertise<nav_msgs::Odometry>("/gps_odom", 100, false);
...
void GNSSCB(const sensor_msgs::NavSatFixConstPtr &msg) {
...
/** if you have some satellite info or rtk status info, put it here*/
int status = -1;
int satell_num = -1;
double x, y, z;
// LLA->ENU, better accuacy than gpsTools especially for z value
geo_converter.Forward(lla[0], lla[1], lla[2], x, y, z);
Eigen::Vector3d enu(x, y, z);
if (abs(enu.x()) > 10000 || abs(enu.x()) > 10000 || abs(enu.x()) > 10000) {
/** check your lla coordinate */
ROS_INFO("Error ogigin : %f, %f, %f", enu(0), enu(1), enu(2));
return;
}
# 过滤低质量数据 status：当状态 >= 0 STATUS_FIX时，此定位才有效；-1 NO_FIX，0 FIX（标准GPS定位），1 SBAS_FIX（星基增强系统修复），2 GBAS_FIX（地基增强系统修复）
if (msg->status.status != 1) {
ROS_WARN("GPS poor signal! gps status: %d", msg->status.status);
return;
}
bool orientationReady = false;
double yaw = 0.0;
double distance =
sqrt(pow(enu(1) - prev_pose_left(1), 2) + pow(enu(0) - prev_pose_left(0), 2));
if (distance > 0.1) {
// 返回值是此点与远点连线与x轴正方向的夹角
yaw = atan2(enu(1) - prev_pose_left(1), enu(0) - prev_pose_left(0));
yaw_quat_left = tf::createQuaternionMsgFromYaw(yaw);
prev_pose_left = enu;
orientationReady = true;
}
/** pub gps odometry*/
nav_msgs::Odometry odom_msg;
odom_msg.header.stamp = msg->header.stamp;
odom_msg.header.frame_id = odometryFrame;
odom_msg.child_frame_id = "gps";
odom_msg.pose.pose.position.x = enu(0);
odom_msg.pose.pose.position.y = enu(1);
odom_msg.pose.pose.position.z = enu(2);
odom_msg.pose.covariance[0] = msg->position_covariance[0];
odom_msg.pose.covariance[7] = msg->position_covariance[4];
odom_msg.pose.covariance[14] = msg->position_covariance[8];
odom_msg.pose.covariance[1] = lla[0];
odom_msg.pose.covariance[2] = lla[1];
odom_msg.pose.covariance[3] = lla[2];
odom_msg.pose.covariance[4] = status;
odom_msg.pose.covariance[5] = satell_num;
odom_msg.pose.covariance[6] = orientationReady;
odom_msg.pose.pose.orientation = yaw_quat_left;
left_odom_pub.publish(odom_msg);
...2. GPS协方差噪声阈值（gpsCovThreshold）
这个参数也在config/params.yaml中，默认为8。它主要通过协方差噪声进行过滤，这里可以把值打印出来，观察下在多路径漂移和正常时的噪声，然后配置一个合适的值进行过滤。
...
void addGPSFactor() {
...
if (syncGPS(gpsQueue, thisGPS, timeLaserInfoCur, 1.0 / gpsFrequence)) {
// GPS too noisy, skip
float noise_x = thisGPS.pose.covariance[0];
float noise_y = thisGPS.pose.covariance[7];
float noise_z = thisGPS.pose.covariance[14];
// make sure the gps data is stable encough
ROS_INFO("curr gps noise: %f, %f , %f", noise_x, noise_y, noise_z);
if (abs(noise_x) > gpsCovThreshold || abs(noise_y) > gpsCovThreshold) {
ROS_WARN("GPS noise too large: %d,%d > %d", abs(noise_x), abs(noise_y), gpsCovThreshold);
return;
}
...3. 距离跳动过滤（GPSDISTANCE）
这个参数也在config/params.yaml中，对应参数名为gpsDistance，默认为0.5，单位为米。当最新有效gps位置与上一帧有效gps位置之间的距离小于0.5米时，会丢弃数据。这里设计本意应该是为了降低频繁的lidar-rtk位置对齐，可以借用改造下，增加gps跳动过大数据的过滤。这里的过滤需要考虑时间间隔和车速因素，可以以“rtk跳动距离 > 最大车速*时间可能行驶的最远距离”为过滤条件，车辆不可能在那么短时间跑那么远，就意味着gps数据已经失真了。
...
void addGPSFactor() {
static float lastGPSTime = 0;
if (syncGPS(gpsQueue, thisGPS, timeLaserInfoCur, 1.0 / gpsFrequence)) {
...
// Add GPS every a few meters
PointType curGPSPoint;
curGPSPoint.x = gps_x;
curGPSPoint.y = gps_y;
curGPSPoint.z = gps_z;
float point_dist = pointDistance(curGPSPoint, lastGPSPoint);
float time_diff = thisGPS.header.stamp.toSec() - lastGPSTime; // yan 25.8.28 最新有效gps位置与上一帧有效gps位置之间的时间差（秒）
float vehicleMaxSecSpeed = 1.5; // yan 25.8.28 车辆最大行驶速度（米/每秒）
time_diff = thisGPS.header.stamp.toSec() - lastGPSTime;
if (point_dist < GPSDISTANCE) {
ROS_WARN("Skip addGPSFactor, Point Distance < gpsDistance: %f < %f", point_dist, GPSDISTANCE);
return;
} else if (point_dist > time_diff * vehicleMaxSecSpeed) { // yan 25.8.28 增加gps跳动过大的过滤，需要考虑时间间隔和车速因素
ROS_WARN("Skip addGPSFactor, Point Distance > time_diff * vehicleMaxSecSpeed: %f > %f * %f", point_dist, time_diff, vehicleMaxSecSpeed);
return;
} else {
lastGPSPoint = curGPSPoint;
lastGPSTime = thisGPS.header.stamp.toSec();
}
...先这样，我先验证下改动效果后再更新。
yan 25.8.28