轮式里程计与激光雷达进行标定–线性最小二乘直接线性方法
轮式里程计定义
什么是轮式里程计:
利用轮子转速来测量机器人行程的装置
原理如下图,简单直接
实物就张这个样子
机器人领域通常使用光电编码器来测量轮子转速,轮子转动时光电编码器接收到脉冲信号,脉冲数乘以系数可以得
知轮子转了多少圈。
两轮机器人,通过轮速不断的积分运算,可以得知机器人前进了多少,同时可以利用两轮速之差,算出机器人转了
多少度, 从而实现机器人的航迹推算定位。
; 轮式里程计与激光SLAM的关系
激光SLAM的其中的一个步骤进行帧间匹配时就是通过两个时刻的点云匹配得到这两个时刻的相对位姿。
上面说了轮式里程计也可以通过统计电机的转速来计算得到机器人的位姿.
既然都可以得到位姿,那么就又涉及到了多传感器融合的问题了,谈到融合必然各传感器有各自的优缺点
激光SLAM在对于环境几何特征点少的情况下不适用,比如长走廊的情况下,显然轮式里程计和这个情况没有关系
轮式里程计在出现路面打滑情况下则会出现累计误差,显然激光SLAM不会有这种情况.既然两者各有各的长处,就融合呗.
融合的思想如下:
轮式里程计的定位可以为激光SLAM提供两个时刻之间匹配的初值,初值准确的情况下可以提升激光SLAM的精度。
此外,轮式里程计可以单独实现短距离航迹推算定位,激光SLAM也可以实现定位,两者融合可以加强定位的准确
性和鲁棒性.
轮式里程计标定前言
轮式里程计的基本原理就是利用两轮速之差,算出机器人转了多少度。这一前提是,已知车轮半径以及两轮间距。
所以在一下情况下需要对轮式里程计进行标定
- 轮速计硬件变形
- 车轮半径以及两轮间距发生变化
- 脉冲信号到轮速转换系数不准
标定方法可以分为两类
- 白盒标定:在已知传感器物理模型时,需要采用基于模型的方法
- 黑盒标定:在不知道传感器物理模型时,可尝试直接线性方法
线性最小二乘
针对的也就是线性的情况
Ax=b
这种的情况根据A矩阵的维数分成了几种情况
m为A的行数，n为A的列数
- 当m=n时，适定方程组，方程组有唯一解
- 当m
通解的公式就是
; 直接线性方法里程计标定
核心思想：
通过采集激光雷达数据和对应时刻的轮式里程计数据，将每帧雷达数据的位姿差作为真值，计算一个变换矩阵，使得变换后的轮式里程计位姿差与激光雷达数据位姿差相等。
对应数学模型：
Code
有了上面的数学模型,下面进入代码实践环节
其中本篇内容重点在于里程计的标定,以激光雷达的数据进行ICP的匹配位姿作为真值.代码中激光数据的处理不做详细描述
int main(int argc,char** argv)
{
ros::init(argc, argv, "Odom_Calib_node");
ros::Time::init();
ros::NodeHandle n;
Scan2 scan;
首先在main函数中进行节点初始化,声明句柄等常规操作
然后实例 Scan2的类对象
下面看Scan2的构造函数
++++++++++++++++++++++++++++++++++++++++++++++++
Scan2::Scan2()
{
ros::NodeHandle private_nh_("~");
m_prevLDP = NULL;
SetPIICPParams();
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";
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";
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;
Eigen::Vector3d d_point_scan;
Eigen::MatrixXd transform_matrix(3,3);
double c,s;
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;
now_pos = odom_pose;
double c = cos(now_pos(2));
double s = sin(now_pos(2));
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));
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);
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就是用激光计算得到的两帧数据之间的旋转 & 平移
激光数据的处理部分,不做详述
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);
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);
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)
{
Eigen::MatrixXd Ai;
Ai.resize(3,9);
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;
bi<<scan(0),scan(1),scan(2);
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.block<3,9>(3*now_len,0)=Ai;
b.conservativeResize(3+3*now_len,1);
b.block<3,1>(3*now_len,0)=bi;
}
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()回调函数
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);
}
publishPathEigen(calib_path_eigen,calib_path_pub_);
scan_filter_sub_->unsubscribe();
std::cout <<"calibration over!!!!"<<std::endl;
}
首先通过Solve()进行线性最小二乘求解
然后通过将每帧里程计的位姿差进行矫正,再转到世界坐标系下不断累积,形成路径,最后发布矫正的路径
++++++++++++++++++++++++++++++++++++++++++++++++
最后看下Solve()函数如何进行线性最小二乘求解的
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);
return correct_matrix;
}
方法一:按最小二乘公式求解
方法二: 按矩阵QR分解求解
++++++++++++++++++++++++++++++++++++++++++++++++
Result
其中红色为里程计路径，蓝色为激光雷达路径，绿色为校准后路径。
可以看到校准后，更贴和激光雷达路径了
校准后得到的矩阵
Original: https://blog.csdn.net/qq_32761549/article/details/121820409
Author: 月照银海似蛟龙
Title: 轮式里程计与激光雷达进行标定1–线性最小二乘直接线性方法
原创文章受到原创版权保护。转载请注明出处：https://www.johngo689.com/533002/
转载文章受原作者版权保护。转载请注明原作者出处！