ROS学习笔记（19）：建图与定位（3）

星影沉璧

已于 2024-07-19 22:05:05 修改

阅读量639

点赞数 13

分类专栏： ROS学习 SLAM 睿抗-ROS仿真赛文章标签：学习笔记

于 2024-07-13 21:31:15 首次发布

本文链接：https://blog.csdn.net/weixin_75067193/article/details/140191119

版权

ROS学习同时被 3 个专栏收录

22 篇文章 10 订阅

订阅专栏

睿抗-ROS仿真赛

7 篇文章 0 订阅

订阅专栏

SLAM

3 篇文章 0 订阅

订阅专栏

0.前言

上文参考论文:2008-icra-plicp.pdf (censi.science)和lowk_point-to-plane_icp.doc (unc.edu)上期文章对扫描匹配进行了简单了解，本期我将分两期对上述两篇公式进行演算一遍。

1.正文

1.第一篇论文

本期参考论文：Microsoft Word - lowk_point-to-plane_icp.doc (unc.edu)https://www.cs.unc.edu/techreports/04-004.pdf

1.ICP迭代目标

ICP迭代的目标就是找到 $M_{opt}$ :

$M_{opt}=argmin_{M} \sum_{i}((M\cdot s_i-d_i)\cdot n_i)^2$

$M$ 和 $M_{opt}$ 都是 $4\times 4$ 的三维变换矩阵。

$M=Trans(\Delta x,\Delta y,\Delta z)\cdot Rot(\alpha ,\beta ,\gamma )=T(t_x,t_y,t_z)\cdot R(\alpha ,\beta ,\gamma )$

2.变换矩阵

1.平移矩阵

$T(t_x,t_y,t_z)=Trans(t_x,t_y,t_z)=\begin{bmatrix} 1& 0& 0& t_x\\ 0& 1& 0& t_y\\ 0& 0& 1& t_z\\ 0& 0& 0& 1 \end{bmatrix}$

2.旋转矩阵

旋转矩阵

$R(\alpha ,\beta ,\gamma ) = R(z,\gamma ) \cdot R(y,\beta ) \cdot R(x,\alpha )$ //注意点乘顺序是z到y到x，矩阵相乘不能调换。

$R_x(\alpha )=Rot(x,\alpha ) =\begin{bmatrix} 1& 0& 0& 0\\ 0& cos\alpha& -sin\alpha& 0\\ 0& sin\alpha& cos\alpha& 0\\ 0& 0& 0& 1 \end{bmatrix}$

$R_y(\beta )=Rot(y,\beta ) =\begin{bmatrix} cos\beta & 0& sin\beta & 0\\ 0& 1& 0& 0\\ -sin\beta &0& cos\beta & 0\\ 0& 0& 0& 1 \end{bmatrix}$

$R_z(\gamma )=Rot(z,\gamma ) =\begin{bmatrix} cos\gamma & -sin\gamma & 0& 0\\ sin\gamma & cos\gamma & 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1 \end{bmatrix}$

经过计算后得到：

$R(\alpha ,\beta ,\gamma )=\begin{bmatrix} c\gamma c\beta& -s\gamma c\alpha+c\gamma s\beta s\alpha& s\gamma s\alpha+c\gamma s\beta c\alpha &0\\ s\gamma c\beta& c\gamma c\alpha+s\gamma s\beta s\alpha& -c\gamma s\alpha +s\gamma s\beta c\alpha & 0\\ -s\beta & c\beta s\alpha & c\beta c\alpha & 0\\ 0& 0& 0& 1 \end{bmatrix}$

下图为我的手稿：

3.线性近似

$M_{opt}=argmin_{M} \sum_{i}((M\cdot s_i-d_i)\cdot n_i)^2$

$M$ 和 $M_{opt}$ 都是 $4\times 4$ 的三维变换矩阵。

$M=Trans(\Delta x,\Delta y,\Delta z)\cdot Rot(\alpha ,\beta ,\gamma )=T(t_x,t_y,t_z)\cdot R(\alpha ,\beta ,\gamma )$

上述是一个最小二乘优化问题，求解时需要确认 $\alpha$ 、 $\beta$ 、 $\gamma$ 、 $t_x$ 、 $t_y$ 、 $t_z$ 的值， $\alpha$ 、 $\beta$ 、 $\gamma$ 在旋转矩阵中是非线性的（即不符合 $f(x)=ax+b$ ）三角函数的参数，最小二乘法是应用在线性问题，将非线性转化成线性可引入高数极限的等价无穷小： $lim_{x\rightarrow 0}sin\theta = \theta$ 和 $lim_{x\rightarrow 0}cos\theta =1$ 。

同时采用保留高阶无穷小的方式再次简化上面的公式得到：

4.回到ICP迭代目标 $M_{opt}$ :

$M_{opt}=argmin_{M} \sum_{i}((M\cdot s_i-d_i)\cdot n_i)^2$

列出 $s_i$ 、 $d_i$ 和 $n_i$ 含xyz三轴的矩阵(线性表达)后会得到：

$\widehat{M_{opt}}=argmin_{\widehat{M}}\sum_{i}((\begin{bmatrix} 1& -\gamma & \beta & t_x\\ \gamma & 1& -\alpha & t_y\\ -\beta & \alpha & 1& t_z\\ 0& 0& 0& 1 \end{bmatrix}\cdot \begin{bmatrix} s_{ix}\\ s_{iy}\\ s_{iz}\\ 1 \end{bmatrix}-\begin{bmatrix} d_{ix}\\ d_{iy}\\ d_{iz}\\ 1 \end{bmatrix})\cdot \begin{bmatrix} n_{ix}\\ n_{iy}\\ n_{iz}\\ 1 \end{bmatrix})^2$

计算后得到：

$\widehat{M_{opt}}=argmin_{\widehat{M}}\sum _{i}[(s_{ix}n_{iy}-s_{iy}n_{ix})\gamma +(s_{iz}n_{ix}-s_{ix}n_{iz})\beta +(s_{iy}n_{iz}-s_{iz}n_{iy})\alpha+(n_{ix}t_x+n_{iy}t_y+n_{iz}t_z)-(n_{ix}d_{ix}+n_{iy}d_{iy}+n_{iz}d_{iz})+s_{ix}n_{ix}+s_{iy}n_{iy}+s_{iz}n_{iz} ]^2$

下图为手稿：

5.克莱姆法则求解

按克莱姆法则得到下述公式：

$\widehat{M_{opt}}=argmin_x(Ax-b)^2=argmin_{\widehat{M}}\sum _{i}[(s_{ix}n_{iy}-s_{iy}n_{ix})\gamma +(s_{iz}n_{ix}-s_{ix}n_{iz})\beta +(s_{iy}n_{iz}-s_{iz}n_{iy})\alpha+(n_{ix}t_x+n_{iy}t_y+n_{iz}t_z)-(n_{ix}d_{ix}+n_{iy}d_{iy}+n_{iz}d_{iz})+s_{ix}n_{ix}+s_{iy}n_{iy}+s_{iz}n_{iz} ]^2$

$f(x)=Ax-b=\sum_i[(s_{ix}n_{iy}-s_{iy}n_{ix})\gamma +(s_{iz}n_{ix}-s_{ix}n_{iz})\beta +(s_{iy}n_{iz}-s_{iz}n_{iy})\alpha+(n_{ix}t_x+n_{iy}t_y+n_{iz}t_z)-(n_{ix}d_{ix}+n_{iy}d_{iy}+n_{iz}d_{iz})+s_{ix}n_{ix}+s_{iy}n_{iy}+s_{iz}n_{iz}]$ $b=\begin{bmatrix} n_{1x}d_{1x}+n_{1y}d_{1y}+n_{1z}d_{1z}-s_{1x}n_{1x}-s_{1y}n_{1y}-s_{1z}n_{1z}\\ n_{2x}d_{2x}+n_{2y}d_{2y}+n_{2z}d_{2z}-s_{2x}n_{2x}-s_{2y}n_{2y}-s_{2z}n_{2z}\\\ \vdots \\ n_{ix}d_{ix}+n_{iy}d_{iy}+n_{iz}d_{iz}-s_{ix}n_{ix}-s_{iy}n_{iy}-s_{iz}n_{iz}\ \end{bmatrix}$

$x=\begin{bmatrix} \alpha \\ \beta \\ \gamma \\ t_x \\ t_y\\ t_z \end{bmatrix}$

$A=\begin{bmatrix} a_{11} & a_{12}& a_{13}& a_{14}& a_{15}& a_{16}\\ a_{21}& a_{22}& a_{23}& a_{24}& a_{25}& a_{26}\\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ a_{i1}& a_{i2}& a_{i3}& a_{i4}& a_{i5}& a_{i6} \end{bmatrix}$

$\left\{\begin{matrix} a_{i1}= n_{iz}s_{iy} -n_{iy}s_{iz}\\ a_{i2}= n_{ix}s_{iz} -n_{iz}s_{ix}\\ a_{i3}= n_{iy}s_{ix} -n_{ix}s_{iy}\\ a_{i4}=n_{ix}\\ a_{i5}=n_{iy}\\ a_{i6}=n_{iz} \end{matrix}\right.$