最近在測試legoloam的時候，總是會碰到雷達坐標系劇烈漂移的情況，即使現實中雷達靜止，在rviz中的顯示也漂移的很厲害。建的地圖也十分糟糕。

首先明確，legoloam中真正實現功能的代碼只有三個部分：image.cpp map.cpp feature.cpp?? 特征識別和雷達、IMU位置變換都集中在了feature.cpp文件中。legoloam的整體結構如下所示（盜的圖）

系統接收來自3D激光雷達的輸入并輸出6個DOF姿勢估計。 整個系統分為五個模塊。 首先是segmentation，使用單次掃描的點云，并將其投影到范圍圖像上進行分段（線）。 然后將分段的點云發送到feature extraction模塊。 然后，激光雷達測距儀使用從前一模塊中提取的特征來找到與連續掃描相關的變換。 這些特征在lidar mapping中進一步處理，將它們注冊到全局點云圖。 最后，transform integration模塊融合了激光雷達測距和激光雷達測繪的姿態估計結果，并輸出最終的姿態估計。?
?

一、定位及位姿變換部分

1.雷達位姿估計

legoloam不加IMU傳感器也可以很好的實現定位和自身位姿估計等功能。但是有些情形下，雷達會出現坐標嚴重漂移的情況，導致無法建圖。

那么雷達的坐標為什么會漂移，雷達的位置信息是如何計算和確定的？繼續查找，我發現雷達的位置信息發布被寫在了featureAssociation中的PublishOdometry函數中，具體變量為transform數組！

void publishOdometry(){geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformSum[2], -transformSum[0], -transformSum[1]);laserOdometry.header.stamp = cloudHeader.stamp;laserOdometry.pose.pose.orientation.x = -geoQuat.y;//四元數laserOdometry.pose.pose.orientation.y = -geoQuat.z;laserOdometry.pose.pose.orientation.z = geoQuat.x;laserOdometry.pose.pose.orientation.w = geoQuat.w;laserOdometry.pose.pose.position.x =transformSum[3];//這里改了雷達坐標就變了laserOdometry.pose.pose.position.y =transformSum[4];laserOdometry.pose.pose.position.z =transformSum[5];pubLaserOdometry.publish(laserOdometry);laserOdometryTrans.stamp_ = cloudHeader.stamp;laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));laserOdometryTrans.setOrigin(tf::Vector3(transformSum[3], transformSum[4], transformSum[5]));tfBroadcaster.sendTransform(laserOdometryTrans);}

transformSum數組的信息更新函數如下integrateTransformation, 該函數不斷的通過旋轉矩陣更新雷達的位姿狀態：

void integrateTransformation(){float rx, ry, rz, tx, ty, tz;AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], -transformCur[0], -transformCur[1], -transformCur[2], rx, ry, rz);float x1 = cos(rz) * (transformCur[3] - imuShiftFromStartX) - sin(rz) * (transformCur[4] - imuShiftFromStartY);float y1 = sin(rz) * (transformCur[3] - imuShiftFromStartX) + cos(rz) * (transformCur[4] - imuShiftFromStartY);float z1 = transformCur[5] - imuShiftFromStartZ;float x2 = x1;float y2 = cos(rx) * y1 - sin(rx) * z1;float z2 = sin(rx) * y1 + cos(rx) * z1;tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);ty = transformSum[4] - y2;tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);transformSum[0] = rx;transformSum[1] = ry;transformSum[2] = rz;transformSum[3] = tx;transformSum[4] = ty;transformSum[5] = tz;}

那么雷達是如何估算自身的位姿的？由于featrure.cpp文件進行了大量的特征匹配，比如面特征匹配，棱邊匹配。和ICP匹配一樣，在進行配準之后相應的就得到了位姿的變換矩陣，這是legoloam進行定位的基本原理，對應成代碼的話，integrateTransformation函數之前需要加入一個計算變換矩陣的代碼結構，就是updateTransformation函數，該函數利用前后幀匹配計算兩幀之間的相對位姿變換,計算兩幀點云的變換矩陣,函數內部調用了calculateTransformationSurf等特征匹配函數。

void updateTransformation(){if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100)return;for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) {laserCloudOri->clear();coeffSel->clear();findCorrespondingSurfFeatures(iterCount1);if (laserCloudOri->points.size() < 10)continue;if (calculateTransformationSurf(iterCount1) == false)break;}for (int iterCount2 = 0; iterCount2 < 25; iterCount2++) {laserCloudOri->clear();coeffSel->clear();findCorrespondingCornerFeatures(iterCount2);if (laserCloudOri->points.size() < 10)continue;if (calculateTransformationCorner(iterCount2) == false)break;}}

特征函數一共兩個一個是點到線的對應特征findCorrespondingCornerFeatures，一種是點到面的對應特征findCorrespondingSurfFeatures。總結來說，雷達的位姿就是通過點云的特征匹配來求得變換矩陣，再進行位姿更新的。

在legoloam的頭文件中，對雷達的機械參數進行了定義，這一部分要參考對應雷達的技術手冊，我以鐳神16線為例，設定的參數對應如下：

extern const int N_SCAN = 16; extern const int Horizon_SCAN = 2000; extern const float ang_res_x = 0.18; extern const float ang_res_y = 2.0; extern const float ang_bottom = 15.0; extern const int groundScanInd = 10;

N_SCAN是線數，Horizon_SCAN是點云數 ang_res_x/ang_res_y分別是水平和垂直分辨率，ang_bottom是垂直視場角，在調試過程中發現 ang_res_x ang_res_y ang_bottom的細小數據差異都會對建圖效果產生巨大的影響。猜測是因為在imageproject沒款中引入了這三個參數并進行了點云圖像化處理，所以參數的設定會影響最后所形成的地圖以及定位效果。

2.IMU位姿計算

legoloam有一個特點，不需要IMU也可以進行很好的定位，但是在legoloam中也進行了/imu/data話題的訂閱，在調試時，當我訂閱IMU的話題時，雷達坐標系漂移的十分劇烈，直接飛了，在這里先不討論IMU的矩陣坐標轉換，直接看IMU的數據解析部分，首先看IMU的回調函數：

void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn){ROS_INFO("IMU hae been start!");double roll, pitch, yaw;tf::Quaternion orientation;tf::quaternionMsgToTF(imuIn->orientation, orientation);tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);//加速度去除重力影響，同時坐標軸進行變換(室內無姿態角，先設置為0)float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;imuPointerLast = (imuPointerLast + 1) % imuQueLength;imuTime[imuPointerLast] = imuIn->header.stamp.toSec();imuRoll[imuPointerLast] = roll;imuPitch[imuPointerLast] = pitch;imuYaw[imuPointerLast] = yaw;imuRoll[imuPointerLast] = roll;imuPitch[imuPointerLast] = pitch;imuYaw[imuPointerLast] = yaw;imuAccX[imuPointerLast] = accX;imuAccY[imuPointerLast] = accY;imuAccZ[imuPointerLast] = accZ;imuAngularVeloX[imuPointerLast] =imuIn->angular_velocity.x;imuAngularVeloY[imuPointerLast] = imuIn->angular_velocity.y;imuAngularVeloZ[imuPointerLast] = imuIn->angular_velocity.z;AccumulateIMUShiftAndRotation();}

回調函數對IMU里取得的xyz三個方向上的加速度進行了去重力處理。然后將姿態角和加速度，角速度進行了儲存。函數最后調用了AccumulateIMUShiftAndRotation函數，這個函數使用IMU姿態計算的矩陣將IMU的測量值從b-frame投影至n-frame (從導航的角度是n系，或者叫global IMU frame) 此坐標系其實是當地地理系(導航的角度看)。之后在此固定坐標系（global IMU frame）下進行積分運算，計算得到0.1s間隔內的位置變換（而且位置、速度變換只能在這個坐標系下n系下完成）。

void AccumulateIMUShiftAndRotation(){float roll = imuRoll[imuPointerLast];float pitch = imuPitch[imuPointerLast];float yaw = imuYaw[imuPointerLast];float accX = imuAccX[imuPointerLast];float accY = imuAccY[imuPointerLast];float accZ = imuAccZ[imuPointerLast];float x1 = cos(roll) * accX - sin(roll) * accY;float y1 = sin(roll) * accX + cos(roll) * accY;float z1 = accZ;float x2 = x1;float y2 = cos(pitch) * y1 - sin(pitch) * z1;float z2 = sin(pitch) * y1 + cos(pitch) * z1;accX = cos(yaw) * x2 + sin(yaw) * z2;accY = y2;accZ = -sin(yaw) * x2 + cos(yaw) * z2;int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];if (timeDiff < scanPeriod) {//imushift就是此刻imu的位置信息imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff + accX * timeDiff * timeDiff / 2;imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + imuVeloY[imuPointerBack] * timeDiff + accY * timeDiff * timeDiff / 2;imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + imuVeloZ[imuPointerBack] * timeDiff + accZ * timeDiff * timeDiff / 2;ROS_INFO("x: %f,y: %f,z: %f",imuShiftX[imuPointerLast],imuShiftY[imuPointerLast],imuShiftZ[imuPointerLast]);imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;imuAngularRotationX[imuPointerLast] = imuAngularRotationX[imuPointerBack] + imuAngularVeloX[imuPointerBack] * timeDiff;imuAngularRotationY[imuPointerLast] = imuAngularRotationY[imuPointerBack] + imuAngularVeloY[imuPointerBack] * timeDiff;imuAngularRotationZ[imuPointerLast] = imuAngularRotationZ[imuPointerBack] + imuAngularVeloZ[imuPointerBack] * timeDiff;}}

這里可以看到legoloam對IMU的位姿信息計算是很草率的，對于位移信息直接引用的加速度計算位移公式? 但是對于大部分的IMU來講，它的內部加速計是十分靈敏的，就算它處于靜止狀態，加速度也會有一定的值（可能是0.002這種量級的），但是隨著時間的累加，位置的誤差累計也是越來越大的。但是最新的LIO_SLAM好像解決了這個問題。個人覺得在IMU部分最好引入預積分處理，不能單純的用加速度位移公式來計算。并且legoloam中沒有將IMU的坐標可視化的部分，這些都是可以優化的點。

legoloam的主要功能都在feature文件中進行實現，image和map部分代碼量都較少，legoloam中引入的坐標變換矩陣十分多，這也是代碼閱讀中比較頭痛的一點。

總結

以上是生活随笔為你收集整理的关于lego-loam的总结（一）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。