使用IMU與輪速計(jì)傳感器進(jìn)行雷達(dá)數(shù)據(jù)的運(yùn)動(dòng)畸變校正
1 IMU與輪速計(jì)的簡(jiǎn)介
1.1 IMU
1.1.1 簡(jiǎn)介
IMU全稱(chēng)Inertial Measurement Unit，慣性測(cè)量單元，主要用來(lái)檢測(cè)和測(cè)量加速度與旋轉(zhuǎn)運(yùn)動(dòng)的傳感器。
IMU一般包括3個(gè)功能，
3軸加速度計(jì)：測(cè)量 x y z 3個(gè)方向上的加速度，z軸的加速度也就是重力值的大?。↖MU水平的情況下）
3軸陀螺儀（角度度計(jì)）：測(cè)量出IMU分別繞著 x y z 軸旋轉(zhuǎn)的角速度
3軸磁力計(jì)：測(cè)出地球磁場(chǎng)的方向，用于確定IMU 在 x y z 軸的方向，但是受磁鐵，金屬等影響較大．
前兩個(gè)功能的組合被稱(chēng)為6軸IMU，這3個(gè)功能的組合被稱(chēng)為9軸IMU．
IMU的頻率可以很高，10hz-400hz 不等．
1.1.2 ros中的消息格式
IMU在ROS中的消息格式如下，有些IMU會(huì)提供積分后的姿態(tài)值，也就是orientation，有些不會(huì)提供，就需要自己通過(guò)對(duì)角速度進(jìn)行積分來(lái)獲取姿態(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 簡(jiǎn)介
輪速計(jì)就是安裝在電機(jī)上的編碼器，通過(guò)電機(jī)旋轉(zhuǎn)的圈數(shù)來(lái)計(jì)算機(jī)器人所走過(guò)的距離與角度，在ROS中稱(chēng)為Odometry，譯為里程計(jì)．后來(lái)，隨著SLAM的發(fā)展，里程計(jì)代表的意思多了起來(lái)，如激光雷達(dá)里程計(jì)，視覺(jué)里程計(jì)等等，這些代表的意思與輪速計(jì)差不多，都是通過(guò)各種方式，獲取雷達(dá)或者相機(jī)這段時(shí)間內(nèi)移動(dòng)的距離與角度．輪速計(jì)的頻率一般不會(huì)太高，一般為20hz-50hz．本文所用的里程計(jì)指的是輪速計(jì)，也就是通過(guò)編碼器獲取的距離與角度．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 #線(xiàn)速度float64 xfloat64 yfloat64 z geometry_msgs/Vector3 angular #角速度float64 xfloat64 yfloat64 zfloat64[36] covariance里程計(jì)中也存在線(xiàn)速度角速度，但是這個(gè)線(xiàn)速度角速度是通過(guò)距離除以時(shí)間得到的，沒(méi)有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īng)顟B(tài)的, git clone 下載的代碼可以使用 git pull 很方便地進(jìn)行更新.本篇文章對(duì)應(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è)線(xiàn)程，所以在保存imu與odom數(shù)據(jù)時(shí)，需要先上鎖，以防止同一時(shí)間有2個(gè)地方對(duì)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，就對(duì)imu的數(shù)據(jù)進(jìn)行修剪，進(jìn)行時(shí)間同步，并計(jì)算雷達(dá)數(shù)據(jù)時(shí)間內(nèi)的旋轉(zhuǎn)if (use_imu_) {if (!PruneImuDeque())return; } // 如果使用odom，就對(duì)odom的數(shù)據(jù)進(jìn)行修剪，進(jìn)行時(shí)間同步，并計(jì)算雷達(dá)數(shù)據(jù)時(shí)間內(nèi)的平移if (use_odom_) {if (!PruneOdomDeque())return; } // 對(duì)雷達(dá)數(shù)據(jù)的每一個(gè)點(diǎn)進(jìn)行畸變的去除 CorrectLaserScan(); // 將較正過(guò)的雷達(dá)數(shù)據(jù)以PointCloud2的形式發(fā)布出去 PublishCorrectedPointCloud(); // 參數(shù)重置 ResetParameters();}2.3 緩存雷達(dá)數(shù)據(jù)
通過(guò)雙端隊(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ù)間的角度是固定的，因此可以將對(duì)應(yīng)角度的cos與sin值緩存下來(lái)，不用每次都計(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ù)的存儲(chǔ)，所以，隊(duì)列中IMU的時(shí)間是連續(xù)的．同時(shí)由于IMU是100hz的，而雷達(dá)是10hz的，二者頻率不同，時(shí)間戳也就對(duì)不上，所以需要做一下時(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í)刻相對(duì)于第一幀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; // 對(duì)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_; } // 對(duì)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)系下的位姿變換，所以不需要由我們自己對(duì)每一幀數(shù)據(jù)進(jìn)行積分．直接找到雷達(dá)數(shù)據(jù)開(kāi)始前的一幀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; // 通過(guò)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 對(duì)雷達(dá)數(shù)據(jù)的每個(gè)點(diǎn)進(jìn)行畸變校正
文字說(shuō)明如何進(jìn)行畸變校正
目前的單線(xiàn)激光雷達(dá)只有一個(gè)單點(diǎn)激光發(fā)射器，通過(guò)旋轉(zhuǎn)反射鏡或者旋轉(zhuǎn)激光發(fā)射器一周，實(shí)現(xiàn)了360度范圍的掃描．而由于這一周的激光點(diǎn)數(shù)據(jù)不是同一時(shí)間得到的，就導(dǎo)致了雷達(dá)數(shù)據(jù)會(huì)在雷達(dá)發(fā)生運(yùn)動(dòng)時(shí)雷達(dá)數(shù)據(jù)會(huì)產(chǎn)生運(yùn)動(dòng)畸變．
舉個(gè)例子
一個(gè)360的單線(xiàn)激光雷達(dá)，發(fā)射第一個(gè)點(diǎn)時(shí)激光雷達(dá)的位置是在（1， 0）處，碰到物體反射回來(lái)，測(cè)得的距離值為1.3m．由于激光雷達(dá)處于前進(jìn)的狀態(tài)，激光器旋轉(zhuǎn)一周后，發(fā)射最后一個(gè)點(diǎn)時(shí)激光雷達(dá)的位置時(shí)假設(shè)變成了（1.1， 0），再次碰到上述物體反射回來(lái)，測(cè)得的距離值就變?yōu)榱?.2m．而一般的激光雷達(dá)驅(qū)動(dòng)是不會(huì)對(duì)這種現(xiàn)象進(jìn)行處理的，最終得到的數(shù)據(jù)也就變成了，激光雷達(dá)處于（1， 0）位置處，觀測(cè)前方1.3m的物體，雷達(dá)數(shù)據(jù)的第一個(gè)點(diǎn)返回的值為1.3m，而雷達(dá)數(shù)據(jù)的最后一個(gè)點(diǎn)測(cè)得的相同物體的距離為1.2m．這就是畸變的發(fā)生的原理，是由于不同時(shí)刻，獲取距離值時(shí)的激光雷達(dá)的坐標(biāo)系發(fā)生了變化導(dǎo)致的．
如何進(jìn)行畸變校正
知道了畸變是如何發(fā)生的，那如何進(jìn)行畸變校正就清晰了．只需要找到每個(gè)激光點(diǎn)對(duì)應(yīng)時(shí)刻的激光雷達(dá)坐標(biāo)系，相對(duì)于發(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 通過(guò)坐標(biāo)變換 變換到 frame_start 坐標(biāo)系下．對(duì)所有點(diǎn)都進(jìn)行上述操作后，得到的點(diǎn)的坐標(biāo)，就是去畸變后的坐標(biāo)了．這就是畸變校正的過(guò)程．
畸變校正的代碼實(shí)現(xiàn)
代碼的實(shí)現(xiàn)和上述文字是一樣的，就不再細(xì)說(shuō)了．
// 對(duì)雷達(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++) {// 如果是無(wú)效點(diǎn)，就跳過(guò)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)對(duì)應(yīng)時(shí)刻 相對(duì)于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)對(duì)應(yīng)時(shí)刻 相對(duì)于start_odom_time_ 的平移與旋轉(zhuǎn)，之后在這幀數(shù)據(jù)畸變過(guò)程中不再改變if (first_point_flag == true) { transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)) .inverse(); first_point_flag = false; } // 當(dāng)前點(diǎn)對(duì)應(yīng)時(shí)刻 相對(duì)于start_odom_time_ 的平移與旋轉(zhuǎn) transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur); // 雷達(dá)數(shù)據(jù)的第一個(gè)點(diǎn)對(duì)應(yīng)時(shí)刻的激光雷達(dá)坐標(biāo)系 到 雷達(dá)數(shù)據(jù)當(dāng)前點(diǎn)對(duì)應(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)對(duì)應(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 相對(duì)于 比當(dāng)前幀雷達(dá)數(shù)據(jù)時(shí)間早的第一個(gè)imu數(shù)據(jù)的時(shí)間 產(chǎn)生的總的旋轉(zhuǎn)量.// 根據(jù)點(diǎn)云中某點(diǎn)的時(shí)間戳賦予其 通過(guò)插值 得到的旋轉(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)沒(méi)進(jìn)去或者到了最大執(zhí)行次數(shù)，則只能近似的將當(dāng)前的旋轉(zhuǎn)賦值過(guò)去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ù)線(xiàn)性插值計(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ì)的頻率一般不會(huì)很高．所以這里就直接用雷達(dá)數(shù)據(jù)前后的輪速計(jì)的坐標(biāo)差當(dāng)做這段的平移量了，沒(méi)有對(duì)雷達(dá)旋轉(zhuǎn)時(shí)間范圍內(nèi)的每個(gè)值進(jìn)行保存．用線(xiàn)性插值計(jì)算了pointTime時(shí)刻的平移量．// 根據(jù)點(diǎn)云中某點(diǎn)的時(shí)間戳賦予其 通過(guò)插值 得到的平移量void LidarUndistortion::ComputePosition(double pointTime, float *posXCur, float *posYCur, float *posZCur){ *posXCur = 0; *posYCur = 0; *posZCur = 0; // 根據(jù)線(xiàn)性插值計(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)行
本篇文章對(duì)應(yīng)的數(shù)據(jù)包, 請(qǐng)?jiān)诰W(wǎng)盤(pán)獲得https://pan.baidu.com/s/10qKKVsp–iCQ7B1JPec0BQ提取碼:v332下載之后將launch中的bag_filename更改成您實(shí)際的目錄名。通過(guò)如下命令運(yùn)行本篇文章對(duì)應(yīng)的程序roslaunch lesson5 lidar_undistortion.launch
3.2 運(yùn)行結(jié)果與分析
啟動(dòng)之后，會(huì)在rviz中顯示出如下畫(huà)面．
畫(huà)面中的黃色點(diǎn)云就是畸變之后的點(diǎn)云．如果想要看原始點(diǎn)云就把左側(cè)的 LaserScan 右邊的空白方框點(diǎn)一下，會(huì)出現(xiàn)紅色的點(diǎn)云，是原始數(shù)據(jù)．在錄數(shù)據(jù)的時(shí)候，在走廊的交叉口處以0.8rad/s的角速度進(jìn)行了長(zhǎng)時(shí)間的旋轉(zhuǎn)，以檢測(cè)畸變校正后的點(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
本篇文章首先簡(jiǎn)要介紹了IMU與輪速計(jì)兩種傳感器，在代碼中展示了如何對(duì)這兩種傳感器數(shù)據(jù)進(jìn)行簡(jiǎn)單處理，進(jìn)行了IMU，Odom，與雷達(dá)數(shù)據(jù)的時(shí)間同步，并使用這兩種傳感器進(jìn)行了雷達(dá)數(shù)據(jù)的運(yùn)動(dòng)畸變校正．通過(guò)仔細(xì)觀察校正前后的點(diǎn)云效果，可以發(fā)現(xiàn)，在旋轉(zhuǎn)時(shí)激光雷達(dá)的畸變尤其明顯．審核編輯：郭婷
聲明：本文內(nèi)容及配圖由入駐作者撰寫(xiě)或者入駐合作網(wǎng)站授權(quán)轉(zhuǎn)載。文章觀點(diǎn)僅代表作者本人，不代表電子發(fā)燒友網(wǎng)立場(chǎng)。文章及其配圖僅供工程師學(xué)習(xí)之用，如有內(nèi)容侵權(quán)或者其他違規(guī)問(wèn)題，請(qǐng)聯(lián)系本站處理。
舉報(bào)投訴
-
傳感器
+關(guān)注
關(guān)注
2542文章
50267瀏覽量
750168 -
編碼器
+關(guān)注
關(guān)注
44文章
3553瀏覽量
133803 -
激光雷達(dá)
+關(guān)注
關(guān)注
967文章
3891瀏覽量
189204
原文標(biāo)題：使用IMU與輪速計(jì)進(jìn)行單線(xiàn)激光雷達(dá)的運(yùn)動(dòng)畸變校正
文章出處：【微信號(hào)：3D視覺(jué)工坊，微信公眾號(hào)：3D視覺(jué)工坊】歡迎添加關(guān)注！文章轉(zhuǎn)載請(qǐng)注明出處。
發(fā)布評(píng)論請(qǐng)先 登錄
相關(guān)推薦
ABS輪速傳感器檢測(cè)
ABS防抱死制動(dòng)系統(tǒng)各元件安裝置如圖1所示?！ D1 ABS／TCS電控系統(tǒng)各元件安裝位置 ?、俨鹣萝?chē)輪，檢查輪速傳感器的安裝情況，并清潔傳感器感應(yīng)端子，必要時(shí)應(yīng)
發(fā)表于 10-30 17:13
磁阻式輪速傳感器在ABS中的應(yīng)用
摘要：介紹一種磁阻式輪速傳感器,并設(shè)計(jì)了該輪速傳感器的信號(hào)處理電路,其中包含兩級(jí)帶通濾波放大電路和遲滯比較
發(fā)表于 11-14 16:42
ABS輪速傳感器波形測(cè)試
?。?）標(biāo)準(zhǔn)波形特點(diǎn) 輪速傳感器標(biāo)準(zhǔn)波形如圖1所示?！ D1 輪速傳感器標(biāo)準(zhǔn)波形 （2）波
發(fā)表于 11-15 16:04
電磁感應(yīng)式輪速傳感器的識(shí)別與檢測(cè)
輪速（車(chē)輪速度）傳感器大多采用電磁感應(yīng)式，豐田系列汽車(chē)使用的輪速傳感器。
發(fā)表于 11-16 16:13
霍爾式輪速傳感器的識(shí)別與檢測(cè)
0.2～O.5mm，否則應(yīng)進(jìn)行調(diào)整。圖5輪速傳感器在ABS系統(tǒng)中的布置 1蓄電池；2點(diǎn)火開(kāi)關(guān)；3右前輪速
發(fā)表于 11-16 11:11
實(shí)車(chē)ABS四輪輪速傳感器信號(hào)再現(xiàn)系統(tǒng)---設(shè)備
運(yùn)動(dòng)部件模擬車(chē)輪轉(zhuǎn)動(dòng)，感應(yīng)傳感器輸出信號(hào)，進(jìn)而送給ABS/ESP系統(tǒng)的耐久性試驗(yàn)中，存在輪速信號(hào)真實(shí)性差，響應(yīng)速度低、精度差、機(jī)械慣性大的問(wèn)題，從而保證ABS剎車(chē)防抱死系統(tǒng)和ESP電子
發(fā)表于 03-26 15:47
如何使用汽車(chē)示波器檢查ABS輪速傳感器
與地面的附著力在最大值。而ABS中輪速傳感器的作用是測(cè)量汽車(chē)車(chē)輪轉(zhuǎn)速。輪速傳感器檢測(cè)每個(gè)車(chē)輪轉(zhuǎn)動(dòng)
發(fā)表于 08-24 14:22
請(qǐng)問(wèn)如何理解SLAM用到的傳感器輪式里程計(jì)IMU、雷達(dá)、相機(jī)的工作原理？
請(qǐng)問(wèn)如何理解SLAM用到的傳感器輪式里程計(jì)IMU、雷達(dá)、相機(jī)的工作原理？
發(fā)表于 10-09 08:52
輪速傳感器工作原理_輪速傳感器的作用
輪速傳感器是用來(lái)測(cè)量汽車(chē)車(chē)輪轉(zhuǎn)速的傳感器。對(duì)于現(xiàn)代汽車(chē)而言，輪速信息是必不可少的，汽車(chē)動(dòng)態(tài)控制系
發(fā)表于 11-11 08:53
?1.6w次閱讀
輪速傳感器故障現(xiàn)象_輪速傳感器安裝位置
輪速傳感器，就是用來(lái)測(cè)試汽車(chē)車(chē)輪輪速的一種傳感器。然而，大家有所不知的是，其實(shí)
發(fā)表于 11-11 09:01
?7915次閱讀
輪速傳感器壞了能開(kāi)嗎
輪速傳感器壞了是可以繼續(xù)開(kāi)的，但我們最好就是以較為緩慢的車(chē)速開(kāi)到最近的汽修店進(jìn)行維修更換。畢竟輪速
發(fā)表于 11-11 09:17
?2.4w次閱讀
利用Visual C++6.0實(shí)現(xiàn)對(duì)海底大地電磁探測(cè)數(shù)據(jù)進(jìn)行畸變校正處理
的數(shù)據(jù)進(jìn)行方位畸變校正。另外，當(dāng)測(cè)量電磁場(chǎng)分量的傳感器系統(tǒng)放入海底時(shí)，由于海水的各種各樣的運(yùn)動(dòng)，
輪速傳感器壞了的現(xiàn)象_輪速傳感器壞了是什么原因造成的
輪速傳感器，就是用來(lái)測(cè)試汽車(chē)車(chē)輪輪速的一種傳感器。然而，大家有所不知的是，其實(shí)
發(fā)表于 08-06 08:51
?6.3w次閱讀
輪速傳感器的類(lèi)型和基本原理
電磁式輪速傳感器大致分為電感式、霍爾式和磁阻式三種類(lèi)型。其中，電感式輪速傳感器是被動(dòng)式
發(fā)表于 12-13 10:53
?1.2w次閱讀
輪速傳感器有什么作用，輪速傳感器如何工作
輪速傳感器，也稱(chēng)為車(chē)速傳感器（VSS），是用于測(cè)量車(chē)輪轉(zhuǎn)向的傳感器。常用的輪
發(fā)表于 06-30 11:07
?1.1w次閱讀
工商網(wǎng)監(jiān)
評(píng)論