使用IMU與輪速計傳感器進行雷達數據的運動畸變校正
1 IMU與輪速計的簡介
1.1 IMU
1.1.1 簡介
IMU全稱Inertial Measurement Unit，慣性測量單元，主要用來檢測和測量加速度與旋轉運動的傳感器。
IMU一般包括3個功能，
3軸加速度計：測量 x y z 3個方向上的加速度，z軸的加速度也就是重力值的大小（IMU水平的情況下）
3軸陀螺儀（角度度計）：測量出IMU分別繞著 x y z 軸旋轉的角速度
3軸磁力計：測出地球磁場的方向，用于確定IMU 在 x y z 軸的方向，但是受磁鐵，金屬等影響較大．
前兩個功能的組合被稱為6軸IMU，這3個功能的組合被稱為9軸IMU．
IMU的頻率可以很高，10hz-400hz 不等．
1.1.2 ros中的消息格式
IMU在ROS中的消息格式如下，有些IMU會提供積分后的姿態值，也就是orientation，有些不會提供，就需要自己通過對角速度進行積分來獲取姿態值．
比較常用的是角速度與加速度這兩項．
$ rosmsg show sensor_msgs/Imu std_msgs/Header header # 消息頭uint32 seq time stamp # 時間戳string frame_id # 坐標系geometry_msgs/Quaternion orientation # IMU的當前姿態，4元數的形式表示float64 xfloat64 yfloat64 zfloat64 wfloat64[9] orientation_covariance # 姿態的協方差geometry_msgs/Vector3 angular_velocity # IMU的3軸角速度float64 xfloat64 yfloat64 zfloat64[9] angular_velocity_covariance # IMU的3軸角速度的協方差geometry_msgs/Vector3 linear_acceleration # IMU的3軸加速度float64 xfloat64 yfloat64 zfloat64[9] linear_acceleration_covariance # IMU的3軸加速度的協方差1.2 輪速計
1.2.1 簡介
輪速計就是安裝在電機上的編碼器，通過電機旋轉的圈數來計算機器人所走過的距離與角度，在ROS中稱為Odometry，譯為里程計．后來，隨著SLAM的發展，里程計代表的意思多了起來，如激光雷達里程計，視覺里程計等等，這些代表的意思與輪速計差不多，都是通過各種方式，獲取雷達或者相機這段時間內移動的距離與角度．輪速計的頻率一般不會太高，一般為20hz-50hz．本文所用的里程計指的是輪速計，也就是通過編碼器獲取的距離與角度．1.2.2 ros中的消息格式
$ rosmsg show nav_msgs/Odometry std_msgs/Header headeruint32 seq time stamp # 時間戳string frame_id # 里程計坐標系名字，一般為odomstring child_frame_id # 里程計坐標系指向的子坐標系名字，一般為base_link或者footprintgeometry_msgs/PoseWithCovariance pose # 帶協方差的位姿 geometry_msgs/Pose pose geometry_msgs/Point position #位置，x y zfloat64 xfloat64 yfloat64 z geometry_msgs/Quaternion orientation #四元數表示的姿態float64 xfloat64 yfloat64 zfloat64 wfloat64[36] covariancegeometry_msgs/TwistWithCovariance twist # 帶協方差的速度值 geometry_msgs/Twist twist geometry_msgs/Vector3 linear #線速度float64 xfloat64 yfloat64 z geometry_msgs/Vector3 angular #角速度float64 xfloat64 yfloat64 zfloat64[36] covariance里程計中也存在線速度角速度，但是這個線速度角速度是通過距離除以時間得到的，沒有IMU的精確．大多數情況下，優先使用IMU的角速度，與輪速計的距離值．2 代碼講解
2.1 獲取代碼
代碼已經提交在github上了，github的地址為 https://github.com/xiangli0608/Creating-2D-laser-slam-from-scratch推薦使用 git clone 的方式進行下載, 因為代碼是正處于更新狀態的, git clone 下載的代碼可以使用 git pull 很方便地進行更新.本篇文章對應的代碼為Creating-2D-laser-slam-from-scratch/lesson5/src/lidar_undistortion.ccCreating-2D-laser-slam-from-scratch/lesson5/include/lesson5/lidar_undistortion.h2.2 回調函數
由于使用了3個線程，所以在保存imu與odom數據時，需要先上鎖，以防止同一時間有2個地方對imu_queue_ 與 odom_queue_ 進行更改．// imu的回調函數void LidarUndistortion::ImuCallback(const sensor_msgs::ConstPtr &imuMsg){std::lock_guard lock(imu_lock_); imu_queue_.push_back(*imuMsg);} // odom的回調函數void LidarUndistortion::OdomCallback(const nav_msgs::ConstPtr &odometryMsg){std::lock_guard lock(odom_lock_); odom_queue_.push_back(*odometryMsg);} // scan的回調函數void LidarUndistortion::ScanCallback(const sensor_msgs::ConstPtr &laserScanMsg){// 緩存雷達數據if (!CacheLaserScan(laserScanMsg))return; // 如果使用imu，就對imu的數據進行修剪，進行時間同步，并計算雷達數據時間內的旋轉if (use_imu_) {if (!PruneImuDeque())return; } // 如果使用odom，就對odom的數據進行修剪，進行時間同步，并計算雷達數據時間內的平移if (use_odom_) {if (!PruneOdomDeque())return; } // 對雷達數據的每一個點進行畸變的去除 CorrectLaserScan(); // 將較正過的雷達數據以PointCloud2的形式發布出去 PublishCorrectedPointCloud(); // 參數重置 ResetParameters();}2.3 緩存雷達數據
通過雙端隊列進行雷達數據的保存，并且確保隊列中至少有2個數據，以防止imu或者odom的數據時間不能包含雷達數據的時間．之后，獲取了current_laserscan_的時間戳，以及點云結束的時間戳．// 緩存雷達數據bool LidarUndistortion::CacheLaserScan(const sensor_msgs::ConstPtr &laserScanMsg){ if (first_scan_) { first_scan_ = false; // 雷達數據間的角度是固定的，因此可以將對應角度的cos與sin值緩存下來，不用每次都計算 CreateAngleCache(laserScanMsg); scan_count_ = laserScanMsg->ranges.size(); } corrected_pointcloud_->points.resize(laserScanMsg->ranges.size()); // 緩存雷達數據 laser_queue_.push_back(*laserScanMsg); // 緩存兩幀雷達數據，以防止imu或者odom的數據不能包含雷達數據if (laser_queue_.size() < 2)return false; // 取出隊列中的第一個數據 current_laserscan_ = laser_queue_.front(); laser_queue_.pop_front(); // 獲取這幀雷達數據的起始，結束時間 current_laserscan_header_ = current_laserscan_.header; current_scan_time_start_ = current_laserscan_header_.stamp.toSec(); // 認為ros中header的時間為這一幀雷達數據的起始時間 current_scan_time_increment_ = current_laserscan_.time_increment; current_scan_time_end_ = current_scan_time_start_ + current_scan_time_increment_ * (scan_count_ - 1); return true;}2.4 IMU的時間同步與角度積分
由于是使用雙端隊列進行IMU數據的存儲，所以，隊列中IMU的時間是連續的．同時由于IMU是100hz的，而雷達是10hz的，二者頻率不同，時間戳也就對不上，所以需要做一下時間同步．時間同步的思想就是，先將imu的雙端隊列中時間太早的數據刪去．之后找到IMU的時間與當前幀雷達的起始時間早的第一個IMU數據．以這個IMU數據作為起點，使用之后的IMU數據進行積分，分別獲得2個IMU數據之間的旋轉，imu_rot_x_ 保存的是當前時刻相對于第一幀imu數據時刻，轉動的角度值．重復這個操作，直到IMU的時間戳比當前幀雷達數據旋轉一周之后的時間要晚．也就是這些IMU數據的時間是包含了雷達數據轉一圈的時間．// 修剪imu隊列，以獲取包含 當前幀雷達時間 的imu數據及轉角bool LidarUndistortion::PruneImuDeque(){std::lock_guard lock(imu_lock_); // imu數據隊列的頭尾的時間戳要在雷達數據的時間段外if (imu_queue_.empty() || imu_queue_.front().header.stamp.toSec() > current_scan_time_start_ || imu_queue_.back().header.stamp.toSec() < current_scan_time_end_) { ROS_WARN("Waiting for IMU data ...");return false; } // 修剪imu的數據隊列，直到imu的時間接近這幀點云的時間while (!imu_queue_.empty()) {if (imu_queue_.front().header.stamp.toSec() < current_scan_time_start_ - 0.1) imu_queue_.pop_front();elsebreak; } if (imu_queue_.empty())return false; current_imu_index_ = 0; sensor_msgs::Imu tmp_imu_msg;double current_imu_time, time_diff; for (int i = 0; i < (int)imu_queue_.size(); i++) { tmp_imu_msg = imu_queue_[i]; current_imu_time = tmp_imu_msg.header.stamp.toSec(); if (current_imu_time < current_scan_time_start_) {// 初始角度為0if (current_imu_index_ == 0) { imu_rot_x_[0] = 0; imu_rot_y_[0] = 0; imu_rot_z_[0] = 0; imu_time_[0] = current_imu_time; ++current_imu_index_; }continue; } // imu時間比雷達結束時間晚，就退出if (current_imu_time > current_scan_time_end_)break; // get angular velocitydouble angular_x, angular_y, angular_z; angular_x = tmp_imu_msg.angular_velocity.x; angular_y = tmp_imu_msg.angular_velocity.y; angular_z = tmp_imu_msg.angular_velocity.z; // 對imu的角速度進行積分，當前幀的角度 = 上一幀的角度 + 當前幀角速度 * (當前幀imu的時間 - 上一幀imu的時間)double time_diff = current_imu_time - imu_time_[current_imu_index_ - 1]; imu_rot_x_[current_imu_index_] = imu_rot_x_[current_imu_index_ - 1] + angular_x * time_diff; imu_rot_y_[current_imu_index_] = imu_rot_y_[current_imu_index_ - 1] + angular_y * time_diff; imu_rot_z_[current_imu_index_] = imu_rot_z_[current_imu_index_ - 1] + angular_z * time_diff; imu_time_[current_imu_index_] = current_imu_time; ++current_imu_index_; } // 對current_imu_index_進行-1操作后，current_imu_index_指向當前雷達時間內的最后一個imu數據 --current_imu_index_; return true;}2.5 輪速計的時間同步與平移計算
輪速計的時間同步方式與IMU的基本一致，只是由于輪速計是固定坐標系下的位姿變換，所以不需要由我們自己對每一幀數據進行積分．直接找到雷達數據開始前的一幀odom數據，與雷達旋轉一圈之后的一幀odom數據，求這兩個odom數據之間的坐標變換，即可得到雷達旋轉一圈時間附近的總平移量．// 修剪imu隊列，以獲取包含 當前幀雷達時間 的odom的平移距離bool LidarUndistortion::PruneOdomDeque(){std::lock_guard lock(odom_lock_); // imu數據隊列的頭尾的時間戳要在雷達數據的時間段外if (odom_queue_.empty() || odom_queue_.front().header.stamp.toSec() > current_scan_time_start_ || odom_queue_.back().header.stamp.toSec() < current_scan_time_end_) { ROS_WARN("Waiting for Odometry data ...");return false; } // 修剪odom的數據隊列，直到odom的時間接近這幀點云的時間while (!odom_queue_.empty()) {if (odom_queue_.front().header.stamp.toSec() < current_scan_time_start_ - 0.1) odom_queue_.pop_front();elsebreak; } if (odom_queue_.empty())return false; // get start odometry at the beinning of the scandouble current_odom_time; for (int i = 0; i < (int)odom_queue_.size(); i++) { current_odom_time = odom_queue_[i].header.stamp.toSec(); if (current_odom_time < current_scan_time_start_) { start_odom_msg_ = odom_queue_[i];continue; } if (current_odom_time <= current_scan_time_end_) { end_odom_msg_ = odom_queue_[i]; }elsebreak; } // if (int(round(start_odom_msg_.pose.covariance[0])) != int(round(end_odom_msg_.pose.covariance[0])))// return false; start_odom_time_ = start_odom_msg_.header.stamp.toSec(); end_odom_time_ = end_odom_msg_.header.stamp.toSec(); tf::Quaternion orientation;double roll, pitch, yaw; // 獲取起始odom消息的位移與旋轉 tf::quaternionMsgToTF(start_odom_msg_.pose.pose.orientation, orientation); tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); Eigen::Affine3f transBegin = pcl::getTransformation( start_odom_msg_.pose.pose.position.x, start_odom_msg_.pose.pose.position.y, start_odom_msg_.pose.pose.position.z, roll, pitch, yaw); // 獲取終止odom消息的位移與旋轉 tf::quaternionMsgToTF(end_odom_msg_.pose.pose.orientation, orientation); tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); Eigen::Affine3f transEnd = pcl::getTransformation( end_odom_msg_.pose.pose.position.x, end_odom_msg_.pose.pose.position.y, end_odom_msg_.pose.pose.position.z, roll, pitch, yaw); // 求得這之間的變換 Eigen::Affine3f transBt = transBegin.inverse() * transEnd; // 通過transBt獲取odomIncreX等，一幀點云數據時間的odom變化量float rollIncre, pitchIncre, yawIncre; pcl::getTranslationAndEulerAngles(transBt, odom_incre_x_, odom_incre_y_, odom_incre_z_, rollIncre, pitchIncre, yawIncre);return true;}2.6 對雷達數據的每個點進行畸變校正
文字說明如何進行畸變校正
目前的單線激光雷達只有一個單點激光發射器，通過旋轉反射鏡或者旋轉激光發射器一周，實現了360度范圍的掃描．而由于這一周的激光點數據不是同一時間得到的，就導致了雷達數據會在雷達發生運動時雷達數據會產生運動畸變．
舉個例子
一個360的單線激光雷達，發射第一個點時激光雷達的位置是在（1， 0）處，碰到物體反射回來，測得的距離值為1.3m．由于激光雷達處于前進的狀態，激光器旋轉一周后，發射最后一個點時激光雷達的位置時假設變成了（1.1， 0），再次碰到上述物體反射回來，測得的距離值就變為了1.2m．而一般的激光雷達驅動是不會對這種現象進行處理的，最終得到的數據也就變成了，激光雷達處于（1， 0）位置處，觀測前方1.3m的物體，雷達數據的第一個點返回的值為1.3m，而雷達數據的最后一個點測得的相同物體的距離為1.2m．這就是畸變的發生的原理，是由于不同時刻，獲取距離值時的激光雷達的坐標系發生了變化導致的．
如何進行畸變校正
知道了畸變是如何發生的，那如何進行畸變校正就清晰了．只需要找到每個激光點對應時刻的激光雷達坐標系，相對于發射第一個點時刻的激光雷達坐標系，間的坐標變換，就可以將每個激光點都變換到發射第一個點時刻的激光雷達坐標系下，就完成了畸變的校正．首先，假設雷達發射第一個點的時刻為 time_start，這時的雷達坐標系為 frame_start．雷達其余點的時刻為 time_point，這時的雷達坐標系為 frame_point，其余點在 frame_point 坐標系下的坐標為 point．只需要找到 frame_start 與 frame_point 間的坐標變換，就可以將 其余點的坐標 point 通過坐標變換 變換到 frame_start 坐標系下．對所有點都進行上述操作后，得到的點的坐標，就是去畸變后的坐標了．這就是畸變校正的過程．
畸變校正的代碼實現
代碼的實現和上述文字是一樣的，就不再細說了．
// 對雷達數據的每一個點進行畸變的去除void LidarUndistortion::CorrectLaserScan(){bool first_point_flag = true;double current_point_time = 0;double current_point_x = 0, current_point_y = 0, current_point_z = 1.0; Eigen::Affine3f transStartInverse, transFinal, transBt; for (int i = 0; i < scan_count_; i++) {// 如果是無效點，就跳過if (!std::isfinite(current_laserscan_.ranges[i]) || current_laserscan_.ranges[i] < current_laserscan_.range_min || current_laserscan_.ranges[i] > current_laserscan_.range_max)continue; // 畸變校正后的數據 PointT &point_tmp = corrected_pointcloud_->points[i]; current_point_time = current_scan_time_start_ + i * current_scan_time_increment_; // 計算雷達數據的 x y 坐標 current_point_x = current_laserscan_.ranges[i] * a_cos_[i]; current_point_y = current_laserscan_.ranges[i] * a_sin_[i]; float rotXCur = 0, rotYCur = 0, rotZCur = 0;float posXCur = 0, posYCur = 0, posZCur = 0; // 求得當前點對應時刻 相對于start_odom_time_ 的平移與旋轉if (use_imu_) ComputeRotation(current_point_time, &rotXCur, &rotYCur, &rotZCur);if (use_odom_) ComputePosition(current_point_time, &posXCur, &posYCur, &posZCur); // 雷達數據的第一個點對應時刻 相對于start_odom_time_ 的平移與旋轉，之后在這幀數據畸變過程中不再改變if (first_point_flag == true) { transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)) .inverse(); first_point_flag = false; } // 當前點對應時刻 相對于start_odom_time_ 的平移與旋轉 transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur); // 雷達數據的第一個點對應時刻的激光雷達坐標系 到 雷達數據當前點對應時刻的激光雷達坐標系 間的坐標變換 transBt = transStartInverse * transFinal; // 將當前點的坐標 加上 兩個時刻坐標系間的坐標變換 // 得到 當前點在 雷達數據的第一個點對應時刻的激光雷達坐標系 下的坐標 point_tmp.x = transBt(0, 0) * current_point_x + transBt(0, 1) * current_point_y + transBt(0, 2) * current_point_z + transBt(0, 3); point_tmp.y = transBt(1, 0) * current_point_x + transBt(1, 1) * current_point_y + transBt(1, 2) * current_point_z + transBt(1, 3); point_tmp.z = transBt(2, 0) * current_point_x + transBt(2, 1) * current_point_y + transBt(2, 2) * current_point_z + transBt(2, 3); }}2.6.1 獲取這個時刻的旋轉
ComputeRotation() 這個函數返回的旋轉值，是pointTime 相對于 比當前幀雷達數據時間早的第一個imu數據的時間 產生的總的旋轉量.// 根據點云中某點的時間戳賦予其 通過插值 得到的旋轉量void LidarUndistortion::ComputeRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur){ *rotXCur = 0; *rotYCur = 0; *rotZCur = 0; // 找到在pointTime之后的imu數據int imuPointerFront = 0;while (imuPointerFront < current_imu_index_) {if (pointTime < imu_time_[imuPointerFront])break; ++imuPointerFront; } // 如果上邊的循環沒進去或者到了最大執行次數，則只能近似的將當前的旋轉賦值過去if (pointTime > imu_time_[imuPointerFront] || imuPointerFront == 0) { *rotXCur = imu_rot_x_[imuPointerFront]; *rotYCur = imu_rot_y_[imuPointerFront]; *rotZCur = imu_rot_z_[imuPointerFront]; }else {int imuPointerBack = imuPointerFront - 1; // 根據線性插值計算 pointTime 時刻的旋轉double ratioFront = (pointTime - imu_time_[imuPointerBack]) / (imu_time_[imuPointerFront] - imu_time_[imuPointerBack]);double ratioBack = (imu_time_[imuPointerFront] - pointTime) / (imu_time_[imuPointerFront] - imu_time_[imuPointerBack]); *rotXCur = imu_rot_x_[imuPointerFront] * ratioFront + imu_rot_x_[imuPointerBack] * ratioBack; *rotYCur = imu_rot_y_[imuPointerFront] * ratioFront + imu_rot_y_[imuPointerBack] * ratioBack; *rotZCur = imu_rot_z_[imuPointerFront] * ratioFront + imu_rot_z_[imuPointerBack] * ratioBack; }}2.6.2 獲取這個時刻的平移
雷達數據的畸變產生主要是由于旋轉產生的，由平移產生的畸變比較少．而且輪速計的頻率一般不會很高．所以這里就直接用雷達數據前后的輪速計的坐標差當做這段的平移量了，沒有對雷達旋轉時間范圍內的每個值進行保存．用線性插值計算了pointTime時刻的平移量．// 根據點云中某點的時間戳賦予其 通過插值 得到的平移量void LidarUndistortion::ComputePosition(double pointTime, float *posXCur, float *posYCur, float *posZCur){ *posXCur = 0; *posYCur = 0; *posZCur = 0; // 根據線性插值計算 pointTime 時刻的旋轉double ratioFront = (pointTime - start_odom_time_) / (end_odom_time_ - start_odom_time_); *posXCur = odom_incre_x_ * ratioFront; *posYCur = odom_incre_y_ * ratioFront; *posZCur = odom_incre_z_ * ratioFront;}3 運行
3.1 運行
本篇文章對應的數據包, 請在網盤獲得https://pan.baidu.com/s/10qKKVsp–iCQ7B1JPec0BQ提取碼:v332下載之后將launch中的bag_filename更改成您實際的目錄名。通過如下命令運行本篇文章對應的程序roslaunch lesson5 lidar_undistortion.launch
3.2 運行結果與分析
啟動之后，會在rviz中顯示出如下畫面．
畫面中的黃色點云就是畸變之后的點云．如果想要看原始點云就把左側的 LaserScan 右邊的空白方框點一下，會出現紅色的點云，是原始數據．在錄數據的時候，在走廊的交叉口處以0.8rad/s的角速度進行了長時間的旋轉，以檢測畸變校正后的點云的效果，如下圖片就是旋轉時的截圖．紅色是原始雷達數據，黃色是畸變校正后的點云數據．機器人運動的挺快的，如果看不清楚，可以在終端中里 按空格 ， 暫停播放bag文件，以觀察雷達數據的狀態．
可以看到，畸變校正的效果還是很明顯的，在旋轉時的雷達數據明顯地變得整齊了．在機器人主要進行平移運動時，產生的畸變比較小，所以畸變校正后的效果也不明顯．
4 總結與Next
本篇文章首先簡要介紹了IMU與輪速計兩種傳感器，在代碼中展示了如何對這兩種傳感器數據進行簡單處理，進行了IMU，Odom，與雷達數據的時間同步，并使用這兩種傳感器進行了雷達數據的運動畸變校正．通過仔細觀察校正前后的點云效果，可以發現，在旋轉時激光雷達的畸變尤其明顯．審核編輯：郭婷
聲明：本文內容及配圖由入駐作者撰寫或者入駐合作網站授權轉載。文章觀點僅代表作者本人，不代表電子發燒友網立場。文章及其配圖僅供工程師學習之用，如有內容侵權或者其他違規問題，請聯系本站處理。
舉報投訴
-
傳感器
+關注
關注
2552文章
51382瀏覽量
755891 -
編碼器
+關注
關注
45文章
3664瀏覽量
135088 -
激光雷達
+關注
關注
968文章
4024瀏覽量
190273
原文標題：使用IMU與輪速計進行單線激光雷達的運動畸變校正
文章出處：【微信號：3D視覺工坊，微信公眾號：3D視覺工坊】歡迎添加關注！文章轉載請注明出處。
發布評論請先 登錄
相關推薦
ABS輪速傳感器檢測
ABS防抱死制動系統各元件安裝置如圖1所示。 圖1 ABS／TCS電控系統各元件安裝位置 ①拆下車輪，檢查輪速傳感器的安裝情況，并清潔傳感器感應端子，必要時應
發表于 10-30 17:13
實車ABS四輪輪速傳感器信號再現系統---設備
運動部件模擬車輪轉動，感應傳感器輸出信號，進而送給ABS/ESP系統的耐久性試驗中，存在輪速信號真實性差，響應速度低、精度差、機械慣性大的問題，從而保證ABS剎車防抱死系統和ESP電子
發表于 03-26 15:47
利用Visual C++6.0實現對海底大地電磁探測數據進行畸變校正處理
的數據進行方位畸變校正。另外，當測量電磁場分量的傳感器系統放入海底時，由于海水的各種各樣的運動，
工商網監
評論