标签：begin end 标定 里程计 bmatrix 线性 theta now
里程计标定：直接线性方法
通过线性最小二乘的方式标定里程计信息，使其提高精度。
航迹推算
已知里程计信息，上一帧底盘位姿和当前底盘位姿：
l
a
s
t
_
p
o
s
e
=
[
x
l
a
s
t
y
l
a
s
t
θ
l
a
s
t
]
n
o
w
_
p
o
s
e
=
[
x
n
o
w
y
n
o
w
θ
n
o
w
]
last\_pose=\begin{bmatrix} x_{last}\\y_{last}\\\theta_{last} \end{bmatrix}\quad\quad now\_pose=\begin{bmatrix} x_{now}\\y_{now}\\\theta_{now} \end{bmatrix}
last_pose=⎣⎡xlastylastθlast⎦⎤now_pose=⎣⎡xnowynowθnow⎦⎤
则两帧之间具有如下等式成立：
n
o
w
_
p
o
s
e
=
l
a
s
t
_
p
o
s
e
+
R
⋅
[
d
x
d
y
d
θ
]
now\_pose=last\_pose+R\cdot\begin{bmatrix}dx\\dy\\d\theta\end{bmatrix}
now_pose=last_pose+R⋅⎣⎡dxdydθ⎦⎤
其中，旋转矩阵R计算公式如下：
R
=
[
cos
θ
n
o
w
−
sin
θ
n
o
w
0
sin
θ
n
o
w
cos
θ
n
o
w
0
0
0
1
]
R = \begin{bmatrix} \cos\theta_{now}&-\sin\theta_{now}&0\\ \sin\theta_{now}&\cos\theta_{now}&0\\ 0&0&1 \end{bmatrix}
R=⎣⎡cosθnowsinθnow0−sinθnowcosθnow0001⎦⎤
有时，由于里程计存在噪音，会对运动学解算增量加上噪声估计
ϵ
\epsilon
ϵ
[
x
n
o
w
y
n
o
w
θ
n
o
w
]
=
[
x
l
a
s
t
y
l
a
s
t
θ
l
a
s
t
]
+
[
cos
θ
n
o
w
−
sin
θ
n
o
w
0
sin
θ
n
o
w
cos
θ
n
o
w
0
0
0
1
]
⋅
[
d
x
+
ϵ
x
d
y
+
ϵ
y
d
θ
+
ϵ
θ
]
\begin{bmatrix} x_{now}\\y_{now}\\\theta_{now} \end{bmatrix}= \begin{bmatrix} x_{last}\\y_{last}\\\theta_{last} \end{bmatrix}+ \begin{bmatrix} \cos\theta_{now}&-\sin\theta_{now}&0\\ \sin\theta_{now}&\cos\theta_{now}&0\\ 0&0&1 \end{bmatrix}\cdot \begin{bmatrix}dx+\epsilon_x\\dy+\epsilon_y\\d\theta+\epsilon_\theta\end{bmatrix}
⎣⎡xnowynowθnow⎦⎤=⎣⎡xlastylastθlast⎦⎤+⎣⎡cosθnowsinθnow0−sinθnowcosθnow0001⎦⎤⋅⎣⎡dx+ϵxdy+ϵydθ+ϵθ⎦⎤
线性最小二乘
1.代数意义
对于一个线性方程组 A x = b Ax=b Ax=b，其中 A A A为 m × n m\times n m×n的矩阵， x x x为 n × 1 n\times 1 n×1的向量：
- 当 m < n m<n m<n时，欠定方程组，存在无穷多解
- 当 m = n m=n m=n时，适定方程组，存在唯一解
- 当 m > n m>n m>n时，超定方程组，通常无解
对于超定方程组虽然无解，但可以寻找到最靠近真解的解，称为最小二乘解，其通解如下：
x
∗
=
(
A
T
A
)
−
1
A
T
b
x^*=(A^TA)^{-1}A^Tb
x∗=(ATA)−1ATb
2.几何意义
从线性空间角度看， A x Ax Ax表示 A A A的列向量空间 S S S。
方程组无解即表面向量 b b b不在空间 S S S中，最小二乘解就表示向量 b b b在空间 S S S中的投影。
设向量
b
b
b在空间
S
S
S中的投影为
A
x
∗
Ax^*
Ax∗，则：
(
b
−
A
x
∗
)
⊥
S
(b-Ax^*)\bot S
(b−Ax∗)⊥S
即，向量
(
b
−
A
x
∗
)
(b-Ax^*)
(b−Ax∗)垂直于矩阵
A
A
A的每一个列向量。
设矩阵
A
=
[
a
1
a
2
…
a
n
]
A=\begin{bmatrix}a_1 & a_2 &…&a_n\end{bmatrix}
A=[a1a2…an]，其中
a
i
a_i
ai表示矩阵第
i
i
i个列向量，则：
a
i
T
(
b
−
A
x
∗
)
=
0
即
A
T
(
b
−
A
x
∗
)
=
0
A
T
b
=
A
T
A
x
∗
a^T_i(b-Ax^*)=0\\ 即A^T(b-Ax^*)=0\\ A^Tb=A^TAx^*\\
aiT(b−Ax∗)=0即AT(b−Ax∗)=0ATb=ATAx∗
对上式化简得到最小二乘通解：
x
∗
=
(
A
T
A
)
−
1
A
T
b
x^*=(A^TA)^{-1}A^Tb
x∗=(ATA)−1ATb
里程计标定
通过激光雷达scan-match信息，获取到观测值 u i ∗ u^*_i ui∗
通过里程计测量，获得预测值 u i u_i ui
假设两者成线性关系：
u
i
∗
=
X
⋅
u
i
u_i^*=X\cdot u_i
ui∗=X⋅ui，即：
[
u
i
x
∗
u
i
y
∗
u
i
θ
∗
]
=
[
x
11
x
12
x
13
x
21
x
22
x
23
x
31
x
32
x
33
]
[
u
i
x
u
i
y
u
i
θ
]
\begin{bmatrix}u_{ix}^*\\u_{iy}^*\\u_{i\theta}^*\end{bmatrix}= \begin{bmatrix} x_{11} & x_{12} & x_{13}\\ x_{21} & x_{22} & x_{23}\\ x_{31} & x_{32} & x_{33} \end{bmatrix} \begin{bmatrix}u_{ix}\\u_{iy}\\u_{i\theta}\end{bmatrix}
⎣⎡uix∗uiy∗uiθ∗⎦⎤=⎣⎡x11x21x31x12x22x32x13x23x33⎦⎤⎣⎡uixuiyuiθ⎦⎤
将矩阵
X
X
X由
3
×
3
3\times3
3×3转为
9
×
1
9\times1
9×1的向量，得到如下方程组：
[
u
i
x
u
i
y
u
i
θ
0
0
0
0
0
0
0
0
0
u
i
x
u
i
y
u
i
θ
0
0
0
0
0
0
0
0
0
u
i
x
u
i
y
u
i
θ
]
[
x
11
⋅
⋅
⋅
x
33
]
=
[
u
i
x
∗
u
i
y
∗
u
i
θ
∗
]
\begin{bmatrix} u_{ix}&u_{iy}&u_{i\theta}&0&0&0&0&0&0\\ 0&0&0&u_{ix}&u_{iy}&u_{i\theta}&0&0&0\\ 0&0&0&0&0&0&u_{ix}&u_{iy}&u_{i\theta}\\ \end{bmatrix} \begin{bmatrix}x_{11}\\\cdot\\\cdot\\\cdot\\x_{33}\end{bmatrix}= \begin{bmatrix}u_{ix}^*\\u_{iy}^*\\u_{i\theta}^*\end{bmatrix}
⎣⎡uix00uiy00uiθ000uix00uiy00uiθ000uix00uiy00uiθ⎦⎤⎣⎢⎢⎢⎢⎡x11⋅⋅⋅x33⎦⎥⎥⎥⎥⎤=⎣⎡uix∗uiy∗uiθ∗⎦⎤
即对每一帧的观测数据，可以得到一个方程组：
A
i
X
→
=
b
i
其
中
A
i
=
[
u
i
x
u
i
y
u
i
θ
0
0
0
0
0
0
0
0
0
u
i
x
u
i
y
u
i
θ
0
0
0
0
0
0
0
0
0
u
i
x
u
i
y
u
i
θ
]
X
→
=
[
x
11
⋅
⋅
⋅
x
33
]
b
i
=
[
u
i
x
∗
u
i
y
∗
u
i
θ
∗
]
A_i\overrightarrow{X}=b_i\\ 其中A_i=\begin{bmatrix} u_{ix}&u_{iy}&u_{i\theta}&0&0&0&0&0&0\\ 0&0&0&u_{ix}&u_{iy}&u_{i\theta}&0&0&0\\ 0&0&0&0&0&0&u_{ix}&u_{iy}&u_{i\theta}\\ \end{bmatrix}\\ \overrightarrow{X}=\begin{bmatrix}x_{11}\\\cdot\\\cdot\\\cdot\\x_{33}\end{bmatrix}\\ b_i=\begin{bmatrix}u_{ix}^*\\u_{iy}^*\\u_{i\theta}^*\end{bmatrix}
AiX
=bi其中Ai=⎣⎡uix00uiy00uiθ000uix00uiy00uiθ000uix00uiy00uiθ⎦⎤X
=⎣⎢⎢⎢⎢⎡x11⋅⋅⋅x33⎦⎥⎥⎥⎥⎤bi=⎣⎡uix∗uiy∗uiθ∗⎦⎤
对一段时间内的数据，可以得到：
A
=
A
1
⋅
⋅
⋅
A
n
b
=
b
1
⋅
⋅
⋅
b
n
A=\begin{matrix}A_1\\\cdot\\\cdot\\\cdot\\A_n\end{matrix}\quad\quad\quad b=\begin{matrix}b_1\\\cdot\\\cdot\\\cdot\\b_n\end{matrix}
A=A1⋅⋅⋅Anb=b1⋅⋅⋅bn
由此，可以计算出最小二乘解：
X
→
=
(
A
T
A
)
−
1
A
T
b
\overrightarrow{X}=(A^TA)^{-1}A^Tb
X
=(ATA)−1ATb
在获得到最小二乘解后，用其对里程计进行矫正，可以得到矫正后的数据：
u
i
(
c
o
r
r
)
=
X
→
u
i
u_i^{(corr)}=\overrightarrow{X}u_i
ui(corr)=X
ui
功能实现
首先需要安装PL-ICP的环境
sudo apt install ros-melodic-csm
1.构建超定方程组
改函数用于获取里程计信息及观测值，并构建超定方程组。
函数位于Odom_Calib.cpp中Add_Data()函数
/*
* 输入:里程计和激光数据
* 构建最小二乘需要的超定方程组
* Ax = b
* */
bool OdomCalib::Add_Data(Eigen::Vector3d Odom,Eigen::Vector3d scan)
{
if(now_len<INT_MAX)
{
/*
* 三行九列浮点型矩阵
* u_ix u_iy u_iθ 0 0 0 0 0 0
* 0 0 0 u_ix u_iy u_iθ 0 0 0
* 0 0 0 0 0 0 u_ix u_iy u_iθ
* */
Eigen::Matrix<double, 3, 9> A_i;
/*
* 三行一列浮点型矩阵，接收观测值scan
* u_ix^*
* u_iy^*
* u_iθ^*
* */
Eigen::Matrix<double, 3, 1> b_i;
// 初始化置零
A_i.setZero();
b_i.setZero();
// 对b_i赋值，获取观测数据
b_i = scan;
// A_i赋值，获取里程计数据
for(int i = 0; i < 3; i++)
{
// 一行九列矩阵，初始置零
Eigen::Matrix<double, 1, 9> tmp;
tmp.setZero();
/*
* 从第0行第i*3列开始，定义一个1行3列的块，用于存储里程计信息
* 第0行取第 0 1 2 个元素
* 第1行取第 3 4 5 个元素
* 第2行取第 6 7 8 个元素
* */
tmp.block<1, 3>(0, i*3) = Odom.transpose();
// 按行赋值
A_i.block<1, 9>(i, 0) = tmp;
}
// 将A_i、b_i置于A、b中
A.block<3, 9>(now_len * 3.0) = A_i;
b.block<3, 1>(now_len * 3.0) = b_i;
now_len++;
return true;
}
else
{
return false;
}
}
2.求解最小二乘解
该函数用于求解超定方程组的最小二乘解并返回矫正矩阵 X X X
函数位于Odom_Calib.cpp中的Solve()
/*
* 求解线性最小二乘Ax=b
* 线性最小二乘通解：x^*=(A^T A)^{-1} A^T b
* A_tmp * x = b_tmp
* 返回得到的矫正矩阵
*/
Eigen::Matrix3d OdomCalib::Solve()
{
// 矫正矩阵
Eigen::Matrix3d correct_matrix;
// 存储通解
Eigen::Matrix<double, 9, 1> x;
// 存储 A^T A
Eigen::MatrixXd A_tmp;
// 存储 A^T b
Eigen::MatrixXd b_tmp;
// 计算
A_tmp = A.transpose() * A;
b_tmp = A.transpose() * b;
// LDLT分解
x = A_Tmp.ldlt().solve;
// 赋值，返回
correct_matrix << x(0), x(1), x(2),
x(3), x(4), x(5),
x(6), x(7), x(8);
return correct_matrix;
}
3.位姿变换
改函数用于求解当前时刻机器人位姿在上一帧中坐标系下的坐标。即用于得到两帧数据之间的位姿差
函数位于main.cpp中 cal_delta_distance()
/*
* 求解得到两帧数据之间的位姿差
* 即求解当前位姿 在 上一时刻 坐标系中的坐标
* now_pos = last_pos + R * d_pos
* */
Eigen::Vector3d cal_delta_distance(Eigen::Vector3d odom_pose)
{
/*
* 两帧时刻 机器人的位姿 三行一列向量
* X
* Y
* θ
* */
static Eigen::Vector3d now_pos,last_pos;
/*
* 返回值，位姿差 三行一列向量
* dx
* dy
* dθ
* */
Eigen::Vector3d d_pos;
// 获取当前位姿
now_pos = odom_pose;
// 航迹推算公式
Eigen::Matrix3d R;
R << cos(now_pos(2)), -sin(now_pos(2)), 0,
sin(now_pos(2)), cos(now_pos(2)), 0,
0 , 0 , 1;
// 解算
d_pos = R.inverse() * (now_pos - last_pos);
// 更新数据
last_pos = now_pos;
return d_pos;
}
4.程序运行方式
本程序参考深蓝学院课程进行编写，源码下载地址如下：
下载程序并编译成功后，在odom_ws下进行source
source devel/setup.bash
随后运行launch文件
roslaunch calib_odom odomCalib.launch
开启rviz并添加话题：
- Fix_frame选择为odom_frame
- Add选项卡订阅Path信息：
- 原始里程计topic：odom_path_pub_
- 激光雷达topic：scan_path_pub_
- 标定里程计topic：calib_path_pub_
- 分别选择不同的颜色展示即可
然后，进入到odom_ws/bag目录下播放里程计信息
rosbag play –clock odom.bag
如果一切正常，则能看到运行矫正程序的终端会打印数据，并且rviz中可以看到两条路径。当打印的数据到达一个的数量之后，则可以开始矫正。矫正的命令如下：
rostopic pub /calib_flag std_msgs/Empty "{}"
程序矫正完毕会输出对应的矫正矩阵，并且会在rviz中显示出第三条路径，即calib_path。可以观察里程计路径odom_path和矫正路径calib_path区别来判断此次矫正的效果。
标签：begin,end,标定,里程计,bmatrix,线性,theta,now 来源： https://blog.csdn.net/weixin_45929038/article/details/122638114
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。