文章目录
- 一、激光雷达运动畸变产生的原因
- 二、为什么需要解决这个问题?
- 三、去除运动畸变的原理
- 四、运动去畸变的方法
- 1、纯估计方法(ICP / VICP)
- 2、传感器辅助的方法(odom,IMU)
- 3、融合的方法
本文针对二维激光雷达产生的运动畸变进行了分析,如有错误请指正!
一、激光雷达运动畸变产生的原因
在机器人运动过程中,每个激光点都在不同的基准位姿上产生(就是在不同的时刻发出激光时机器人的位置是不同的(由于机器人自身的运动),这也是运动畸变产生的根源所在)。
激光扫描时伴随着机器人的运动,每个角度的激光数据都不是瞬时获得的,当激光雷达扫描的频率比较低的时候,机器人运动带来的激光帧的运动误差是不能被忽略的。
当扫描频率是5Hz的激光雷达,一帧数据的收尾时间差是200ms,如果机器人以0.5m/s的速度沿着x方向行走并扫描前面的墙体,那么200ms后尾部的测量距离和首部的测量距离在x方向上就差10cm。
综上:激光雷达产生畸变的原因有三:
- 激光点数据不是瞬时获得
- 激光测量时伴随着机器人的运动
- 激光帧率较低时,机器人的运动不能忽略(目前 国产的激光雷达 旋转的频率大约在是10Hz ~~100ms)
蓝色是产生运动畸变之后轨迹。
二、为什么需要解决这个问题?
一般激光雷达驱动封装一帧数据时, 默认一帧激光数据的所有激光点是在同一个时刻和同一个位姿下采集的,在低频率扫描的激光雷达应用中(5-10Hz),该问题是不可忽视的。当频率比较高时,我们默认激光数据是在同一个时刻同一个位姿下采集的,可忽视该问题。
三、去除运动畸变的原理
去除激光雷达运动畸变的原理是把一帧激光雷达数据的每个激光点对应的激光雷达坐标转换到不同时刻的机器人里程计上(近似对应的里程计的位置,达到尽可能去除畸变的目的)
四、运动去畸变的方法
1、纯估计方法(ICP / VICP)
- 《1》ICP(Iterative Cloest Point)方法、
目的:ICP方法是最通用的用来求解两个点云集合转换关系的方法。简单的理解:就是匹配两幅点云,通过ICP算法实现,我们需要求解点云配准的误差最小即可。
已知对应点的求解方法(解析解):
当我们知道了两幅点云的点对时(也就是两幅点云的对应点),我们会有解析解(不需要最小二乘去求解):
数学描述:
未知对应点的求解方法:
ICP算法的缺点:
没有考虑激光的运动畸变,因此得到的激光数据是错误的(计算的时候假设是正确的),从而造成点云匹配发生错误,通过不断的迭代,虽然能够收敛,能够求解,但是存在误差。
VICP算法考虑了激光的运动畸变,也就是考虑了机器人的运动和速度的存在。
- ICP算法的变种
- 考虑了机器人的运动
- 匀速运动(假设是匀速运动)
- 进行匹配的同时估计机器人的速度
矫正后的结果
VICP 方法的缺点:(1)低频率情况下(5Hz),匀速运动假设不成立 (2)数据预处理和状态估计过程耦合
如何解决上述问题呢?
我们尽可能的反应运动情况,实现预处理和状态估计的解耦,我们提出了传感器辅助的方法。
2、传感器辅助的方法(odom,IMU)
传感器辅助的方法就是极高的位姿更新频率(200Hz),可以比较准确的反应运动情况,以及较高精度的局部位姿估计和状态估计完全解耦
《1》惯性测量单元IMU
- 直接测量角速度和线加速度
- 具有较高的角速度测量精度(比较准确)
- 测量频率极高(1kHz~8kHz)
- 线加速度精度太差,二次积分在局部的精度依然很差(这一点我们就可以否定他在移动机器人上面的使用,主要原因是精度不高)
《2》轮式里程计odom
- 直接测量机器人的位移和角度
- 具有较高的局部角度测量精度
- 具有较高的局部位置测量精度
- 更新速度较高(100Hz~200Hz)
如何使用odom去除运动畸变?
COde
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/LaserScan.h>
#include <iostream>
#include <dirent.h>
#include <fstream>
#include <iostream>
#include <string>
#define debug_ 0
#if debug_
#include <pcl-1.7/pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl-1.7/pcl/visualization/cloud_viewer.h>
pcl::visualization::CloudViewer g_PointCloudView("PointCloud View");
#endif
class LidarMotionCalibrator
{
public:
LidarMotionCalibrator(tf::TransformListener* tf)
{
tf_ = tf;
scan_sub_ = nh_.subscribe(scan_sub_name_, 10, &LidarMotionCalibrator::ScanCallBack, this);
scan_pub_ = nh_.advertise<sensor_msgs::LaserScan>(scan_pub_name_, 1000);
}
~LidarMotionCalibrator()
{
if(tf_!=NULL)
delete tf_;
}
void ScanCallBack(const sensor_msgs::LaserScanConstPtr& scan_msg)
{
ros::Time startTime, endTime;
startTime = scan_msg->header.stamp;
sensor_msgs::LaserScan laserScanMsg = *scan_msg;
int beamNum = laserScanMsg.ranges.size();
endTime = startTime + ros::Duration(laserScanMsg.time_increment * beamNum);
std::vector<double> angles,ranges;
for(int i = 0; i < beamNum;i++)
{
double lidar_dist = laserScanMsg.ranges[i];
double lidar_angle = laserScanMsg.angle_min + laserScanMsg.angle_increment * i;
ranges.push_back(lidar_dist);
angles.push_back(lidar_angle);
}
#if debug_
visual_cloud_.clear();
visual_cloud_scan(ranges,angles,255,0,0);
#endif
Lidar_Calibration(ranges,angles,startTime,endTime,tf_);
#if debug_
visual_cloud_scan(ranges,angles,0,255,0);
#endif
scan_cal_pub(ranges,startTime,ros::Duration(laserScanMsg.time_increment * beamNum).toSec());
#if debug_
g_PointCloudView.showCloud(visual_cloud_.makeShared());
#endif
}
void scan_cal_pub(const std::vector<double> &ranges,ros::Time pubTime,double scan_time)
{
sensor_msgs::LaserScan tempScan;
tempScan.header.stamp=pubTime;
tempScan.header.frame_id=scan_frame_name_;
tempScan.angle_min=-3.12413907051;
tempScan.angle_max=3.14159274101;
tempScan.angle_increment=(tempScan.angle_max-tempScan.angle_min)/(ranges.size()-1);
tempScan.scan_time=(float)scan_time;
tempScan.time_increment=scan_time/(ranges.size()-1);
tempScan.range_min=0.15;
tempScan.range_max=6.0;
tempScan.ranges.resize(ranges.size());
tempScan.intensities.resize(ranges.size());
for(size_t i=0;i<ranges.size();++i)
{
tempScan.ranges[i]=ranges[i];
tempScan.intensities[i]=15.0;
}
#if debug_
std::vector<double> angles_temp,ranges_temp;
for(int i = 0; i < tempScan.ranges.size();i++)
{
double lidar_dist = tempScan.ranges[i];
double lidar_angle = tempScan.angle_min + tempScan.angle_increment * i;
ranges_temp.push_back(lidar_dist);
angles_temp.push_back(lidar_angle);
}
visual_cloud_scan(ranges_temp,angles_temp,255,255,255);
#endif
scan_pub_.publish(tempScan);
}
#if debug_
void visual_cloud_scan(const std::vector<double> &ranges_,const std::vector<double> &angles_,unsigned char r_,unsigned char g_,unsigned char b_)
{
unsigned char r = r_, g = g_, b = b_;
for(int i = 0; i < ranges_.size();i++)
{
if(ranges_[i] < 0.05 || std::isnan(ranges_[i]) || std::isinf(ranges_[i]))
continue;
pcl::PointXYZRGB pt;
pt.x = ranges_[i] * cos(angles_[i]);
pt.y = ranges_[i] * sin(angles_[i]);
pt.z = 1.0;
unsigned int rgb = ((unsigned int)r << 16 | (unsigned int)g << 8 | (unsigned int)b);
pt.rgb = *reinterpret_cast<float*>(&rgb);
visual_cloud_.push_back(pt);
}
}
#endif
bool getLaserPose(tf::Stamped<tf::Pose> &odom_pose,
ros::Time dt,
tf::TransformListener * tf_)
{
odom_pose.setIdentity();
tf::Stamped < tf::Pose > robot_pose;
robot_pose.setIdentity();
robot_pose.frame_id_ = scan_frame_name_;
robot_pose.stamp_ = dt;
try
{
if(!tf_->waitForTransform(odom_name_, scan_frame_name_, dt, ros::Duration(0.5)))
{
ROS_ERROR("LidarMotion-Can not Wait Transform()");
return false;
}
tf_->transformPose(odom_name_, robot_pose, odom_pose);
}
catch (tf::LookupException& ex)
{
ROS_ERROR("LidarMotion: No Transform available Error looking up robot pose: %s\n", ex.what());
return false;
}
catch (tf::ConnectivityException& ex)
{
ROS_ERROR("LidarMotion: Connectivity Error looking up looking up robot pose: %s\n", ex.what());
return false;
}
catch (tf::ExtrapolationException& ex)
{
ROS_ERROR("LidarMotion: Extrapolation Error looking up looking up robot pose: %s\n", ex.what());
return false;
}
return true;
}
void Lidar_MotionCalibration(
tf::Stamped<tf::Pose> frame_base_pose,
tf::Stamped<tf::Pose> frame_start_pose,
tf::Stamped<tf::Pose> frame_end_pose,
std::vector<double>& ranges,
std::vector<double>& angles,
int startIndex,
int& beam_number)
{
double beam_step = 1.0 / (beam_number-1);
tf::Quaternion start_angle_q = frame_start_pose.getRotation();
tf::Quaternion end_angle_q = frame_end_pose.getRotation();
double start_angle_r = tf::getYaw(start_angle_q);
double base_angle_r = tf::getYaw(frame_base_pose.getRotation());
tf::Vector3 start_pos = frame_start_pose.getOrigin();
start_pos.setZ(0);
tf::Vector3 end_pos = frame_end_pose.getOrigin();
end_pos.setZ(0);
tf::Vector3 base_pos = frame_base_pose.getOrigin();
base_pos.setZ(0);
double mid_angle;
tf::Vector3 mid_pos;
tf::Vector3 mid_point;
double lidar_angle, lidar_dist;
for(int i = 0; i< beam_number;i++)
{
mid_angle = tf::getYaw(start_angle_q.slerp(end_angle_q, beam_step * i));
mid_pos = start_pos.lerp(end_pos, beam_step * i);
double tmp_angle;
if( tfFuzzyZero(ranges[startIndex + i]) == false)
{
lidar_dist = ranges[startIndex+i];
lidar_angle = angles[startIndex+i];
double laser_x,laser_y;
laser_x = lidar_dist * cos(lidar_angle);
laser_y = lidar_dist * sin(lidar_angle);
double odom_x,odom_y;
odom_x = laser_x * cos(mid_angle) - laser_y * sin(mid_angle) + mid_pos.x();
odom_y = laser_x * sin(mid_angle) + laser_y * cos(mid_angle) + mid_pos.y();
mid_point.setValue(odom_x, odom_y, 0);
double x0,y0,a0,s,c;
x0 = base_pos.x();
y0 = base_pos.y();
a0 = base_angle_r;
s = sin(a0);
c = cos(a0);
double tmp_x,tmp_y;
tmp_x = mid_point.x()*c + mid_point.y()*s - x0*c - y0*s;
tmp_y = -mid_point.x()*s + mid_point.y()*c + x0*s - y0*c;
mid_point.setValue(tmp_x,tmp_y,0);
double dx,dy;
dx = (mid_point.x());
dy = (mid_point.y());
lidar_dist = sqrt(dx*dx + dy*dy);
lidar_angle = atan2(dy,dx);
ranges[startIndex+i] = lidar_dist;
angles[startIndex+i] = lidar_angle;
}
else
{
lidar_angle = angles[startIndex+i];
tmp_angle = mid_angle + lidar_angle;
tmp_angle = tfNormalizeAngle(tmp_angle);
lidar_angle = tfNormalizeAngle(tmp_angle - start_angle_r);
angles[startIndex+i] = lidar_angle;
}
}
}
void Lidar_Calibration(std::vector<double>& ranges,
std::vector<double>& angles,
ros::Time startTime,
ros::Time endTime,
tf::TransformListener * tf_)
{
int beamNumber = ranges.size();
if(beamNumber != angles.size())
{
ROS_ERROR("Error:ranges not match to the angles");
return ;
}
int interpolation_time_duration = 5 * 1000;
tf::Stamped<tf::Pose> frame_base_pose;
tf::Stamped<tf::Pose> frame_start_pose;
tf::Stamped<tf::Pose> frame_mid_pose;
tf::Stamped<tf::Pose> frame_end_pose;
double start_time = startTime.toSec() * 1000 * 1000;
double end_time = endTime.toSec() * 1000 * 1000;
double time_inc = (end_time - start_time) / beamNumber;
int start_index = 0;
if(!getLaserPose(frame_start_pose, ros::Time(start_time /1000000.0), tf_))
{
ROS_WARN("Not Start Pose,Can not Calib");
return ;
}
if(!getLaserPose(frame_end_pose,ros::Time(end_time / 1000000.0),tf_))
{
ROS_WARN("Not End Pose, Can not Calib");
return ;
}
int cnt = 0;
frame_base_pose = frame_start_pose;
for(int i = 0; i < beamNumber; i++)
{
double mid_time = start_time + time_inc * (i - start_index);
if(mid_time - start_time > interpolation_time_duration || (i == beamNumber - 1))
{
cnt++;
if(!getLaserPose(frame_mid_pose, ros::Time(mid_time/1000000.0), tf_))
{
ROS_ERROR("Mid %d Pose Error",cnt);
return ;
}
int interp_count = i - start_index + 1;
Lidar_MotionCalibration(frame_base_pose,
frame_start_pose,
frame_mid_pose,
ranges,
angles,
start_index,
interp_count);
start_time = mid_time;
start_index = i;
frame_start_pose = frame_mid_pose;
}
}
}
public:
tf::TransformListener* tf_;
ros::NodeHandle nh_;
ros::Subscriber scan_sub_;
ros::Publisher scan_pub_;
const std::string scan_frame_name_="laser_link";
const std::string odom_name_="odom";
const std::string scan_sub_name_="scan";
const std::string scan_pub_name_="scan_cal";
#if debug_
pcl::PointCloud<pcl::PointXYZRGB> visual_cloud_;
#endif
};
int main(int argc,char ** argv)
{
ros::init(argc,argv,"LidarMotionCalib");
tf::TransformListener tf(ros::Duration(10.0));
LidarMotionCalibrator tmpLidarMotionCalib(&tf);
ros::spin();
return 0;
}
3、融合的方法
参考博客:https://blog.csdn.net/zhao_ke_xue/article/details/105734833
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)