美文网首页
旋转约束与姿态约束的误差方程及其相关求导

旋转约束与姿态约束的误差方程及其相关求导

作者: 轻骑兵1390 | 来源:发表于2020-08-13 10:51 被阅读0次

旋转约束与姿态约束的误差方程及其相关求导

1. 问题描述

在构建地图的过程中, 可能会存在先验的姿态或者先验的位置, 这些先验信息可能来自不同的传感器, 也可能来自同一传感器的不同算法. 比如使用激光雷达建图, 提供先验位置给视觉建图; 或者使用线特征估计Manhattan World进而提供先验姿态. 本文先后求解对于姿态和位置约束的雅克比矩阵

2. 姿态约束

已知当前估计位置为R_{wr}为3\times3的矩阵, 表示平移, w表示world, r表示robot. 此时有先验姿态\hat{R}_{wr}. 那么构造误差方程为:
\begin{aligned} e &= log(R_{wr}^T \cdot \hat{R}_{wr})^\vee \\ &= log(R_{rw} \cdot \hat{R}_{wr})^\vee \end{aligned}
本文中^\wedge为反对成运算, ^\vee为其反运算.

2.1 对于局部扰动的求导

下面求解误差e对于局部扰动\phi_r的导数
\begin{aligned} \frac{\partial e}{\partial \phi_r} &= \frac{\partial \left(log(exp(\phi_r^\wedge)R_{rw} \cdot \hat{R}_{wr})^\vee \right)}{\partial \phi_r} \\ &\approx \frac{\partial \left(log(exp(\phi_r^\wedge))^\vee \right)}{\partial \phi_r} \\ &= I \end{aligned}
这里认为R_{rw} \cdot \hat{R}_{wr} \approx I, 进而求得雅克比矩阵.

2.2 对于全局扰动的求导

下面求解误差e关于全局扰动\phi_r的求导
\begin{aligned} \frac{\partial e}{\partial \phi_w} &= \frac{\partial \left(R_{rw} \cdot log(exp(\phi_w^\wedge) \cdot\hat{R}_{wr})^\vee \right)}{\partial \phi_w} \\ &= \frac{\partial \left(R_{rw} \cdot log(exp(\phi_w^\wedge) \cdot\hat{R}_{rw}^T)^\vee \right)}{\partial \phi_w} \\ &= \frac{\partial \left(log(R_{rw}\hat{R}_{rw}^T \cdot \boxed{\hat{R}_{rw} exp(\phi_w^\wedge) \hat{R}_{rw}^T})^\vee \right)}{\partial \phi_w} \\ &\approx \frac{\partial \left(log \left(exp((\hat{R}_{rw}\cdot \phi_w)^{\wedge})\right)^\vee\right)}{\partial \phi_w} \\ &=\frac{\partial \hat{R}_{rw}\phi_w}{\partial \phi_w} \\ &= \hat{R}_{rw} \\ \end{aligned}
参考State Estimation for Robotics表格7-2中的公式exp((Ru)^\wedge) = R\cdot exp(u^\wedge)\cdot R^T. 另外近似R_{rw} \cdot \hat{R}_{wr} \approx I.

3. 姿态约束的误差方程及其求导

定位当前估计位姿为T_{wr}, T为4x4矩阵, 表示旋转和平移, w表示world, r表示robot. 有先验位姿\hat{T}_{wr}. 那么构建误差方程:
e = log(T_{rw} \cdot \hat{T}_{wr})^\vee

本文中(.)^\wedge为反对称, (.)^\vee与反对称运算相反.

3.1 对于局部扰动求导

下面对于求解误差e对于局部扰动\xi_r的导数
\begin{aligned} \frac{\partial e}{\partial \xi_r} &= \frac{\partial \left(log(exp(\xi_r^\wedge)T_{rw} \cdot \hat{T}_{wr})^\vee\right)}{\partial \xi_r} \\ & \approx \frac{\partial \left(log(exp(\xi_r^\wedge))^\vee\right)}{\partial \xi_r} \\ &= I \end{aligned}
这里认为T_{rw} \cdot \hat{T}_{wr}的误差很小, 得到雅克比矩阵.

3.2 对于全局扰动求导

下面对于求解误差e对于全部扰动\xi_w的导数
\begin{aligned} \frac{\partial e}{\partial \xi_w} &= \frac{\partial \left(log(T_{rw} \cdot exp(\xi_w^\wedge) \cdot \hat{T}_{wr})^\vee\right)}{\partial \xi_w} \\ & = \frac{\partial \left(log(T_{rw} \hat{T}_{wr}\cdot \hat{T}_{rw}exp(\xi_r^\wedge)\hat{T}_{wr})^\vee\right)}{\partial \xi_w} \\ & \approx \frac{\partial \left(log(\hat{T}_{rw}exp(\xi_w^\wedge)\hat{T}_{wr})^\vee\right)}{\partial \xi_w} \\ & = \frac{\partial \left(log\left(exp\left((\hat{\mathcal{T}}_{rw}\xi_w)^\wedge\right)\right)^\vee\right)}{\partial \xi_w} \\ &=\frac{\hat{\mathcal{T}}_{rw}\xi_w}{\partial \xi_w} = \hat{\mathcal{T}}_{rw} \\ \end{aligned}
其中\hat{\mathcal{T}}_{rw}是\hat{T}_{rw}的伴随阵.
\begin{aligned} T &= \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix}_{4\times4} \\ \mathcal{T} &= \begin{bmatrix} R & t^\wedge \\ 0 & R \end{bmatrix}_{6\times6} \end{aligned}
注意以上伴随阵的表达方式和\xi = \begin{bmatrix}\phi & \tau\end{bmatrix}^T对应, 其中\xi为6维向量, \phi为三维, 表示旋转. \tau为两维, 表示平移. 论文Local Accuracy and Global Consistency for Efficient Visual SLAM提供了一种更复杂的求解方式, 具体内容查看器附录B.

4 最小二乘求解

最小二乘最终求得的对应关系为
\begin{aligned} (J^T \Omega J) \Delta x + J^T \Omega e = 0\\ (J^T \Omega J) \Delta x = - J^T \Omega e \end{aligned}
本问题中J = \frac{\partial e}{\partial \xi}, J为2x6的矩阵. \Omega为权重, 为2x2的对角阵.

4.1 对于全局扰动和局部扰动的不同迭代方法

对于局部扰动和全局扰动最终的迭代略有不同, 另优化后局部扰动计算的步长为\Delta \xi_r, 全局扰动为\Delta \xi_w, 那么:
\begin{aligned} T_{rw} &= exp(\Delta \xi_r^\wedge)T_{rw} \\ T_{rw} &= T_{rw}exp(\Delta \xi_w^\wedge) \end{aligned}

姿态同理.

相关文章

网友评论

      本文标题：旋转约束与姿态约束的误差方程及其相关求导

      本文链接：https://www.haomeiwen.com/subject/zrcbdktx.html

      延伸阅读

      深度阅读

      • 您也可以注册成为美文阅读网的作者，发表您的原创作品、分享您的心情！

      栏目导航

      热点阅读

      关于我们|服务条款|联系我们|旋转约束与姿态约束的误差方程及其相关求导|投稿指南|网站地图|RSS订阅|排版工具|手机版

      提供经典美文摘抄,优美散文欣赏,现代诗歌精选,短篇小说,心情随笔,表白情书范文,故事会在线阅读欣赏

      Copyright © 2014-2023 Haomeiwen.com All Rights Reserved. 好美文阅读网 版权所有

      备案信息：桂公网安备 45052102000051号 · 桂ICP备13007215号-3

      本站所收录作品、热点评论等信息部分来源互联网，目的只是为了系统归纳学习和传递资讯

      所有作品版权归原创作者所有，与本站立场无关，如不慎侵犯了你的权益，请联系我们告知，我们将做删除处理！