轮式里程计与激光雷达进行标定1--线性最小二乘直接线性方法
轮式里程计与激光雷达进行标定--线性最小二乘直接线性方法
- 轮式里程计定义
- 轮式里程计与激光SLAM的关系
- 轮式里程计标定前言
- 线性最小二乘
- 直接线性方法里程计标定
- Code
- Result
轮式里程计定义
什么是轮式里程计:
利用轮子转速来测量机器人行程的装置
原理如下图,简单直接
实物就张这个样子
机器人领域通常使用光电编码器来测量轮子转速,轮子转动时光电编码器接收到脉冲信号,脉冲数乘以系数可以得
知轮子转了多少圈。
两轮机器人,通过轮速不断的积分运算,可以得知机器人前进了多少,同时可以利用两轮速之差,算出机器人转了
多少度,从而实现机器人的航迹推算定位。
轮式里程计与激光SLAM的关系
激光SLAM的其中的一个步骤进行帧间匹配时就是通过两个时刻的点云匹配得到这两个时刻的相对位姿。
上面说了轮式里程计也可以通过统计电机的转速来计算得到机器人的位姿.
既然都可以得到位姿,那么就又涉及到了多传感器融合的问题了,谈到融合必然各传感器有各自的优缺点
激光SLAM在对于环境几何特征点少的情况下不适用,比如长走廊的情况下,显然轮式里程计和这个情况没有关系
轮式里程计在出现路面打滑情况下则会出现累计误差,显然激光SLAM不会有这种情况.既然两者各有各的长处,就融合呗.
融合的思想如下:
轮式里程计的定位可以为激光SLAM提供两个时刻之间匹配的初值,初值准确的情况下可以提升激光SLAM的精度。
此外,轮式里程计可以单独实现短距离航迹推算定位,激光SLAM也可以实现定位,两者融合可以加强定位的准确
性和鲁棒性.
轮式里程计标定前言
轮式里程计的基本原理就是利用两轮速之差,算出机器人转了多少度。这一前提是,已知车轮半径以及两轮间距。
所以在一下情况下需要对轮式里程计进行标定
- 轮速计硬件变形
- 车轮半径以及两轮间距发生变化
- 脉冲信号到轮速转换系数不准
标定方法可以分为两类
- 白盒标定:在已知传感器物理模型时,需要采用基于模型的方法
- 黑盒标定:在不知道传感器物理模型时,可尝试直接线性方法
线性最小二乘
针对的也就是线性的情况
Ax=b
这种的情况根据A矩阵的维数分成了几种情况
m为A的行数，n为A的列数
- 当m=n时，适定方程组，方程组有唯一解
- 当m
- 当m>n时，超定方程组，方程组有通常无解
在工程应用里通常超定的情况较多,超定情况又通常无解,所以就有了 ,线性最小二乘,相当于退而求其次,也就是和解最像的解. - 当m>n时，超定方程组，方程组有通常无解
通解的公式就是
直接线性方法里程计标定
核心思想：
通过采集激光雷达数据和对应时刻的轮式里程计数据，将每帧雷达数据的位姿差作为真值，计算一个变换矩阵，使得变换后的轮式里程计位姿差与激光雷达数据位姿差相等。
对应数学模型：
Code
有了上面的数学模型,下面进入代码实践环节
其中本篇内容重点在于里程计的标定,以激光雷达的数据进行ICP的匹配位姿作为真值.代码中激光数据的处理不做详细描述
int main(int argc,char** argv)
{
ros::init(argc, argv, "Odom_Calib_node");
ros::Time::init();
ros::NodeHandle n;
//实例 Scan2的类对象
Scan2 scan;
首先在main函数中进行节点初始化,声明句柄等常规操作
然后实例 Scan2的类对象
下面看Scan2的构造函数
++++++++++++++++++++++++++++++++++++++++++++++++
//构造函数
Scan2::Scan2()
{
//声明句柄
ros::NodeHandle private_nh_("~");
//激光数据处理内容
m_prevLDP = NULL;
SetPIICPParams();//设置PI_ICP参数
scan_pos_cal.setZero();//激光数据位姿清零
odom_pos_cal.setZero();//里程计数据位姿清零
odom_increments.clear();//用来存储里程计两帧间的增量 清零
//设置坐标系名称
if(!private_nh_.getParam("odom_frame", odom_frame_))
odom_frame_ = "odom";
if(!private_nh_.getParam("base_frame", base_frame_))
base_frame_ = "base_link";
//订阅对应的topic 接受到这个topic 系统就开始进行最小二乘的解算 所以在节点运行后接收到认为足够的数据了,人为发布一个这个topic就开始进行解算
calib_flag_sub_ = node_.subscribe("calib_flag",5,&Scan2::CalibFlagCallBack,this);
//发布路径 的句柄
odom_path_pub_ = node_.advertise<nav_msgs::Path>("odom_path_pub_",1,true);//轮式里程计路径
scan_path_pub_ = node_.advertise<nav_msgs::Path>("scan_path_pub_",1,true);//激光路径
calib_path_pub_ = node_.advertise<nav_msgs::Path>("calib_path_pub_",1,true);//校正后的路径
current_time = ros::Time::now();
path_odom.header.stamp=current_time;
path_scan.header.stamp=current_time;
path_odom.header.frame_id="odom";
path_scan.header.frame_id="odom";
//进行里程计和激光雷达数据的同步 通过message_filters的方法进行时间同步
scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "/sick_scan", 10);
scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 10);
scan_filter_->registerCallback(boost::bind(&Scan2::scanCallBack, this, _1));
std::cout <<"Calibration Online,Wait for Data!!!!!!!"<<std::endl;
}
基本内容已注释
主要做了相关变量初始化
设置订阅和发布topic的句柄
最后通过通过message_filters的方法进行进行里程计和激光雷达数据的时间同步
下面就可以看scanCallBack 这个回调函数了
++++++++++++++++++++++++++++++++++++++++++++++++
//激光数据回调函数
void Scan2::scanCallBack(const sensor_msgs::LaserScan::ConstPtr &_laserScanMsg)
{
static long int dataCnt = 0;
sensor_msgs::LaserScan scan;
Eigen::Vector3d odom_pose; //激光对应的里程计位姿
Eigen::Vector3d d_point_odom; //里程计计算的dpose
Eigen::Vector3d d_point_scan; //激光的scanmatch计算的dpose
Eigen::MatrixXd transform_matrix(3,3); //临时的变量
double c,s;//cos和sin
scan = *_laserScanMsg;//取得本帧数据
//得到对应的里程计数据
if(!getOdomPose(odom_pose, _laserScanMsg->header.stamp))
return ;
//前后两帧里程计的位姿差
d_point_odom = cal_delta_distence(odom_pose);
先看到这里,这边仅生明了几个变量,变量对应的含义已注释
根据激光数据该帧的时间找到对应的里程计的位姿 注意这个位姿是世界坐标系下的
然后通过*cal_delta_distence()函数计算 与前帧轮式里程计数据的位姿差
**注意:**由于轮式里程计的位姿是世界坐标系的,所以位姿差也是世界坐标系下的位姿差
但是激光数据计算出的位姿差是以上一帧激光数据为原点的坐标系下的,所以两个位姿差直接算出来的不匹配
那么在cal_delta_distence()函数中需要将世界坐标系下的位姿差转为上一帧坐标系下的位姿差
这里很容易忽略
++++++++++++++++++++++++++++++++++++++++++++++++
下面先看cal_delta_distence()*函数
//求解得到两帧数据之间的位姿差
//即求解当前位姿 在 上一时刻 坐标系中的坐标
Eigen::Vector3d cal_delta_distence(Eigen::Vector3d odom_pose)
{
Eigen::Vector3d d_pos; //return value
now_pos = odom_pose;
/*方法一: 用坐标变换矩阵方式求解 */
double c = cos(now_pos(2));//计算cos
double s = sin(now_pos(2));//计算sin
//当前时刻在世界坐标系的位姿转换矩阵
Eigen::Matrix3d TWN;
TWN <<c,-s,now_pos(0),
s, c,now_pos(1),
0, 0,1;
c = cos(last_pos(2));
s = sin(last_pos(2));
//上一时刻时刻在世界坐标系的位姿转换矩阵
Eigen::Matrix3d TWL;
TWL<<c,-s,last_pos(0),
s, c,last_pos(1),
0, 0,1;
Eigen::Matrix3d TLN = TWL.inverse()*TWN;//求当前时刻到上一时刻的坐标变换矩阵
d_pos << TLN(0,2),TLN(1,2),atan2(TLN(1,0),TLN(0,0));//赋值坐标变换矩阵 即位姿差
/*方法二: 将世界坐标系向量转到上一时刻坐标系 */
/*
d_pos=TWL.inverse()*( now_pos- last_pos);//相当与在世界坐标系中的一偏差向量转到上一时刻坐标系下
*/
/*方法三: 方法同方法二 只是code方式用到了旋转轴 */
/*
Eigen::AngleAxisd temp(last_pos(2),Eigen::Vector3d(0,0,1));
Eigen::Matrix3d trans=temp.matrix().inverse();
d_pos=trans*(now_pos- last_pos);
*/
return d_pos;
}
这里写了三种计算方法:
方法一:用坐标变换矩阵方式求解
根据当前位姿,计算当前位姿到世界坐标系的变换矩阵,根据上一帧位姿,计算上一帧到世界坐标系的变换矩阵
然后通过两个变换矩阵可以计算得到当前时刻到上一时刻的坐标变换矩阵,这个变换矩阵就可以转换为位姿差
方法二: 将世界坐标系向量转到上一时刻坐标系
相当与在世界坐标系中的一偏差向量转到上一时刻坐标系下
方法三: 方法同方法二 只是code方式用到了旋转轴
打印的其中某帧数据的位姿差
下面继续回到激光数据回调函数中
++++++++++++++++++++++++++++++++++++++++++++++++
//如果运动的距离太短，则不进行处理．
if(d_point_odom(0) < 0.05 &&
d_point_odom(1) < 0.05 &&
d_point_odom(2) < tfRadians(5.0))
{
return ;
}
如果运动的距离太短，则不进行处理．
last_pos = now_pos;//将当前位姿保存为上一帧位姿 迭代用
//记录下里程计的增量数据
odom_increments.push_back(d_point_odom);
//把当前的激光数据转换为 pl-icp能识别的数据 & 进行矫正
//d_point_scan就是用激光计算得到的两帧数据之间的旋转 & 平移
LDP currentLDP;
if(m_prevLDP != NULL)
{
LaserScanToLDP(&scan,currentLDP);
d_point_scan = PIICPBetweenTwoFrames(currentLDP,d_point_odom);
}
else
{
LaserScanToLDP(&scan,m_prevLDP);
}
把当前的激光数据转换为 pl-icp能识别的数据 & 进行矫正
d_point_scan就是用激光计算得到的两帧数据之间的旋转 & 平移
激光数据的处理部分,不做详述
// 两针scan计算本身累计的位姿 for laser_path visualization
c = cos(scan_pos_cal(2));
s = sin(scan_pos_cal(2));
transform_matrix<<c,-s,0,
s, c,0,
0, 0,1;
scan_pos_cal+=(transform_matrix*d_point_scan);
// 里程计累计的位姿 for odom_path visualization
c = cos(odom_pos_cal(2));
s = sin(odom_pos_cal(2));
transform_matrix<<c,-s,0,
s, c,0,
0, 0,1;
odom_pos_cal+=(transform_matrix*d_point_odom);
//放到路径当中 //for visualization
pub_msg(odom_pos_cal,path_odom,odom_path_pub_);
pub_msg(scan_pos_cal,path_scan,scan_path_pub_);
将激光计算的两帧直接的误差累积为世界坐标下的位姿,然后发布path,在rviz里看
同理将轮式里程计计算的两帧直接的误差累积为世界坐标下的位姿,然后发布path,在rviz里看
//构造超定方程组
Odom_calib.Add_Data(d_point_odom,d_point_scan);
//校正用的数量
dataCnt++;
std::cout <<"Data Cnt:"<<dataCnt<<std::endl;//终端输出用的数据量
将两个位姿差添加到 方程组中
就是这个方程 Ai是一帧的里程计位姿数据,A是所有的里程计位姿数据
bi是一帧激光雷达位姿数据,b是所有的激光雷达位姿数据
所以在Add_Data() 函数中 需要将每帧数据组合成A矩阵和b矩阵
++++++++++++++++++++++++++++++++++++++++++++++++
下面看Add_Data() 函数实现方式
bool OdomCalib::Add_Data(Eigen::Vector3d Odom,Eigen::Vector3d scan)
{
if(now_len<INT_MAX)
{
/*构建矩阵A*/
Eigen::MatrixXd Ai;//第i次的A矩阵
Ai.resize(3,9); //初始化Ai的大小
//赋值Ａｉ
Ai<<Odom(0),Odom(1),Odom(2),0,0,0,0,0,0,
0,0,0, Odom(0),Odom(1),Odom(2),0,0,0,
0,0,0,0,0,0,Odom(0),Odom(1),Odom(2);
Eigen::Vector3d bi;//第i次的b向量
bi<<scan(0),scan(1),scan(2);//赋值bi
if(now_len==0)
{ //第一次数据
A.resize(3,9);
A = Ai;
b.resize(3,1);
b=bi;
}else{
A.conservativeResize(3+3*now_len,9);//重新定义A矩阵的大小 但不改变原值 conservativeResize与resize的区别就是是否初始化原值
A.block<3,9>(3*now_len,0)=Ai;//在指定块叠加
b.conservativeResize(3+3*now_len,1);//重新定义b矩阵的大小 但不改变原值
b.block<3,1>(3*now_len,0)=bi;//在指定块叠加
}
//std::cout <<"A: "<
//std::cout <
//std::cout <<"b: "<
//std::cout <
now_len++;
return true;
}
else
{
return false;
}
}
首先通过形参 对应的里程计位姿和激光雷达位姿,构建Ai和bi
然后不断向A和b中添加对应矩阵
eigen的矩阵添加没有opencv的方便,没有特定的函数,这里用到的方法就是
声明动态矩阵 A 就是不指定A的大小,然后在添加的时候通过*conservativeResize()函数,设置矩阵大小,
注意这个函数和resize()*区别就在于是否保存之前的值,resize的话之前就全变为0了
然后通过eigen的block模块在指定块添加就可以了
A矩阵不断添加结果是这样的:
b矩阵是这样的
至此源源不断的数据进来,然后A和b逐渐添加各自的矩阵数据,数学方程就构建好了
之后就可以通过对应的公式求解了
就是这个公式,已知了A和b求X
++++++++++++++++++++++++++++++++++++++++++++++++
前面在Scan2()的构造函数中,声明了一个订阅topic
会订阅"calib_flag".
在节点运行后接收到认为足够的数据了,人为发布一个这个topic就开始进行解算
程序进入*CalibFlagCallBack()*回调函数
//订阅topic表示开始进行标定.
void Scan2::CalibFlagCallBack(const std_msgs::Empty &msg)
{
//进行线性最小二乘求解
Eigen::Matrix3d correct_matrix = Odom_calib.Solve();
Eigen::Matrix3d tmp_transform_matrix;
std::cout<<"correct_matrix:"<<std::endl<<correct_matrix<<std::endl;
//计算矫正之后的路径
Eigen::Vector3d calib_pos(0,0,0); //矫正之后的位姿
std::vector<Eigen::Vector3d> calib_path_eigen; //矫正之后的路径
for(int i = 0; i < odom_increments.size();i++)
{
Eigen::Vector3d odom_inc = odom_increments[i];//本次里程计的位姿差
Eigen::Vector3d correct_inc = correct_matrix * odom_inc;//位姿差进行矫正
tmp_transform_matrix << cos(calib_pos(2)),-sin(calib_pos(2)),0,
sin(calib_pos(2)), cos(calib_pos(2)),0,
0, 0,1;
calib_pos += tmp_transform_matrix * correct_inc;//累积世界坐标系下的矫正后的位姿
calib_path_eigen.push_back(calib_pos);//构成path
}
//发布矫正之后的路径
publishPathEigen(calib_path_eigen,calib_path_pub_);
//矫正完毕，退出订阅
scan_filter_sub_->unsubscribe();
std::cout <<"calibration over!!!!"<<std::endl;
}
首先通过*Solve()*进行线性最小二乘求解
然后通过将每帧里程计的位姿差进行矫正,再转到世界坐标系下不断累积,形成路径,最后发布矫正的路径
++++++++++++++++++++++++++++++++++++++++++++++++
最后看下*Solve()*函数如何进行线性最小二乘求解的
/*
* 求解线性最小二乘Ax=b
* 返回得到的矫正矩阵
*/
Eigen::Matrix3d OdomCalib::Solve()
{
Eigen::Matrix3d correct_matrix;
/*方法一:按最小二乘公式求解*/
//按线性最小二乘公式求解得到 解向量
Eigen::Matrix<double,9,1> correct_vector = (A.transpose()*A).inverse()*A.transpose()*b;
//赋值校正矩阵
correct_matrix << correct_vector(0),correct_vector(1),correct_vector(2),
correct_vector(3),correct_vector(4),correct_vector(5),
correct_vector(6),correct_vector(7),correct_vector(8);
/*方法二: 按矩阵QR分解求解*/
/*
correct_vector = A.colPivHouseholderQr().solve(b);
correct_matrix << correct_vector(0),correct_vector(1),correct_vector(2),
correct_vector(3),correct_vector(4),correct_vector(5),
correct_vector(6),correct_vector(7),correct_vector(8);
*/
return correct_matrix;
}
方法一:按最小二乘公式求解
方法二: 按矩阵QR分解求解
++++++++++++++++++++++++++++++++++++++++++++++++
Result
其中红色为里程计路径，蓝色为激光雷达路径，绿色为校准后路径。
可以看到校准后，更贴和激光雷达路径了
校准后得到的矩阵