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