机器人学中的状态估计第八章——迭代式求解ICP问题部分公式推导

最新推荐文章于 2021-11-17 21:08:20 发布

Lance Yu

最新推荐文章于 2021-11-17 21:08:20 发布

阅读量744

点赞数 3

分类专栏： SLAM 文章标签：机器人学中的状态估计

本文链接：https://blog.csdn.net/yulongshanmuxi/article/details/89474410

版权

SLAM 专栏收录该内容

9 篇文章 2 订阅

订阅专栏

本文主要记录在研读高博翻译的机器人学中的状态估计一书中公式的推导，不理解之处。

式（8.83）推导——迭代式使用旋转矩阵求解ICP问题：

首先我们有关于旋转矩阵的目标函数——式(8.81)：

$J(C)=\frac{1}{2}\sum\limits_{j=1}^M{w_j((y_j-y)-C(p_j-p))^T((y_j-y)-C(p_j-p))}$

根据高斯-牛顿的思想，使用扰动形式处理旋转矩阵有式（8.82）：

$C=\exp(\psi^\wedge)C_{op}\approx (1+\psi^\wedge)C_{op}$

将式（8.82）代入到式(8.81)中，则可将目标函数转换成下式：

$J(C)=\frac{1}{2}\sum\limits_{j=1}^M{w_j((y_j-y)-(1+\psi^\wedge)C_{op}(p_j-p))^T((y_j-y)-(1+\psi^\wedge)C_{op}(p_j-p))}$

为了方便书写，现只考虑目标函数中的一项。将上式展开有：

（1+ $[J(C)]_j=((1+\psi^\wedge)C_{op}(p_j-p))^T((1+\psi^\wedge)C_{op}(p_j-p))-((1+\psi^\wedge)C_{op}(p_j-p))^T(y_j-y)-(y_j-y)^T((1+\psi^\wedge)C_{op}(p_j-p))+(y_j-y)^T(y_j-y)$

对上式两边同时对扰动求导，并令导数为0.同时注意到上式和扰动相关的只有前三部分。因此，首先处理扰动的二次项有：

$((1+\psi^\wedge)C_{op}(p_j-p))^T((1+\psi^\wedge)C_{op}(p_j-p))\\ &=(C_{op}(p_j-p))^T(1+\psi^\wedge)^T(1+\psi^\wedge)C_{op}(p_j-p)\\ &=(C_{op}(p_j-p))^T(1-\psi^\wedge)(1+\psi^\wedge)(C_{op}(p_j-p))\\ &=(C_{op}(p_j-p))^T(1-\psi^\wedge\psi^\wedge)(C_{op}(p_j-p))\\ &=(C_{op}(p_j-p))^T(C_{op}(p_j-p))-(C_{op}(p_j-p))^T\psi^\wedge\psi^\wedge(C_{op}(p_j-p))$

其中对于上式的处理第2步中用到了反对称矩阵的性质。

$(1+\psi^\wedge)^T=(1-\psi^\wedge)$

从上式可以看出扰动的二次项又可以分离出和扰动无关的项，求导时不予考虑。因此，对于上式后半部分按如下处理。

$-(C_{op}(p_j-p))^T\psi^\wedge\psi^\wedge(C_{op}(p_j-p))\\ &=-\psi^T(C_{op}(p_j-p))^\wedge(C_{op}(p_j-p))^\wedge\psi\\ &=-\psi^TC_{op}(p_j-p)^\wedge{C_{op}}^TC_{op}(p_j-p)^\wedge{C_{op}}^T\psi\\ &=-\psi^TC_{op}(p_j-p)^\wedge(p_j-p)^\wedge{C_{op}}^T\psi$

此时根据矩阵求导的知识便可以很容易的得到式（8.83）中等号的左边。

然后处理关于扰动的一次项，同样忽略和扰动无关项。

$-((1+\psi^\wedge)C_{op}(p_j-p))^T(y_j-y)\\ &\Rightarrow -(\psi^\wedge C_{op}(p_j-p))^T(y_j-y)\\ &\Rightarrow ((C_{op}(p_j-p))^\wedge\psi)^T(y_j-y)\\ &\Rightarrow -\psi^T(C_{op}(p_j-p))^\wedge(y_j-y)\\&\Rightarrow \psi^T(y_j-y)^\wedge C_{op}(p_j-p)$

对于另一个一次项同样处理。

$-(y_j-y)^T\psi^\wedge C_{op}(p_j-p)\\ &\Rightarrow \psi^T(y_j-y)^\wedge C_{op}(p_j-p)$

上面使用到了下面的性质。

$-(y_j-y)^T\psi^\wedge\\ & =(y_j-y)^T{\psi^\wedge}^T\\ &=(\psi^\wedge(y_j-y))^T\\ &=(-(y_j-y)^\wedge \psi)^T\\ &=\psi^T(y_j-y)^\wedge$

对于上面两个一次项关于扰动求导，即可得到式（8.83）中的等号右边。关于矩阵微分求导的总结可以参考下面链接。

欢迎批评与指正！

https://www.baidu.com/link?url=Tw5MwR77Du5s6sM1DvcLARaMdKntCfqJhPCcp-LI3akinTOYTbRXXtEspnfTpKZDdmBexjUDFFa1cTJBCqVVu_&wd=&eqid=e3cade1e00030288000000035cbed620

Lance Yu

关注

3
点赞
踩
3

收藏

觉得还不错? 一键收藏
0
评论
机器人学中的状态估计第八章——迭代式求解ICP问题部分公式推导

本文主要记录在研读高博翻译的机器人学中的状态估计一书中公式的推导，不理解之处。式（8.83）推导——迭代式使用旋转矩阵求解ICP问题：首先我们有关于旋转矩阵的目标函数——式(8.81)：根据高斯-牛顿的思想，使用扰动形式处理旋转矩阵有式（8.82）：...
复制链接

扫一扫