monoslam融合里程计版

实验环境

实验采用车载RGB-D相机模型，使用monoslam算法。忽略深度信息，融合odom。

---

1. ekf-predict 运动方程推导

1.1 一般里程计模型

一般里程计模型，默认机器人的单次运动是由下列三次运动组成：
第一次旋转： $delt{a}_{rot1}$
平移： $delt{a}_{trans}$
第二次旋转： $delt{a}_{rot2}$

---

1.2 monoslam运动模型

monoslam状态向量维度为13X1，内容为：

state_vector	x	y	z	$Qua{t}_1$	$Qua{t}_2$	$Qua{t}_3$	$Qua{t}_4$	$v_x$	$v_y$	$v_z$	${\omega }_x$	${\omega }_y$	${\omega }_z$
matlab序列	1	2	3	4	5	6	7	8	9	10	11	12	13
c/c++序列	0	1	2	3	4	5	6	7	8	9	10	11	12

---

1.3 monoslam + 里程计运动方程：

$x^{\prime }=x+{\delta }_{trans}cos({\theta }_z+{\delta }_{rot1})$
$y^{\prime }=y+{\delta }_{trans}sin({\theta }_z+{\delta }_{rot1})$
$z^{\prime }=0$
${\theta }_x^{\prime }=0$
${\theta }_y^{\prime }=0$
${\theta }_z^{\prime }=\theta +{\delta }_{rot1}+{\delta }_{rot2}$
${\omega }_x^{\prime }=0$
${\omega }_y^{\prime }=0$
${\omega }_x^{\prime }=0$
${\omega }_y^{\prime }=0$
${\omega }_z^{\prime }=({\delta }_{rot1}+{\delta }_{rot2})/{\Delta }_t$
$v_x^{\prime }=0$
$v_y^{\prime }={\delta }_{trans}cos(\theta +{\delta }_{rot1})/{\Delta }_t$
$v_z^{\prime }={\delta }_{trans}sin(\theta +{\delta }_{rot1})/{\Delta }_t$

根据特定算法，可以将${\theta }_x$ ${\theta }_y$ ${\theta }_z$转化为四元数
这个版本的运动方程因该是没有错误，但是跟原文中的并不符合，改起来会很麻烦

---

1.4 将里程计模型视为恒线速度、角速度模型（采样间隔之间）

${\check{x}}_{t+1}={\hat{x}}_t+v_x^{add}{\Delta }_t$
${\check{y}}_{t+1}={\hat{y}}_t+v_y^{add}{\Delta }_t$
${\check{z}}_{t+1}=0$
$\check{Q}{1}_{t+1}$
$Q2$
$Q3$
$Q4$
${\omega }_x^{\prime }=0$
${\omega }_y^{\prime }=0$
${\omega }_z^{\prime }=({\delta }_{rot1}+{\delta }_{rot2})/{\Delta }_t$
$v_x^{\prime }=v_x^{add}$
$v_y^{\prime }=v_y^{add}$
$v_z^{\prime }=0$
也就是将系统的输入由：
$1.{\delta }_{trans}$
$2.{\delta }_{rot1}$
$3.{\delta }_{rot2}$

换成：
${\delta }_{trans}/{\Delta }_t$
${\delta }_{rot1}/{\Delta }_t+{\delta }_{rot2}/{\Delta }_t$

即输入为：
$v_x$
$v_y$
${\theta }_z$

注意，monoslam中的相机是人手拿移动的，所以有六个自由度，而本实验中的机器人实际上只有三个自由度的$(x,y,{\theta }_z)$

写到一半发现映射起来更麻烦，弃坑

1.5 还是原版里程计 四元数替换欧拉角版本

${\check{x}}_{t+1}={\hat{x}}_t+{\delta }_{trans}cos(atan2(2({\hat{Q}}_t^1{\hat{Q}}_t^4),1-2({\hat{Q}}_t^4{)}^2)+{\delta }_{rot1})$
${\check{y}}_{t+1}={\hat{y}}_t+{\delta }_{trans}sin(atan2(2({\hat{Q}}_t^1{\hat{Q}}_t^4),1-2({\hat{Q}}_t^4{)}^2)+{\delta }_{rot1})$
${\check{z}}_{t+1}=0$
${\check{Q}}_{t+1}^1={\hat{Q}}_t^1{P}^1-{\hat{Q}}_t^2{P}^2-{\hat{Q}}_t^3{P}^3-{\hat{Q}}_t^4{P}^4$
${\check{Q}}_{t+1}^2={\hat{Q}}_t^1{P}^2+{\hat{Q}}_t^2{P}^1+{\hat{Q}}_t^3{P}^4-{\hat{Q}}_t^4{P}^3$
${\check{Q}}_{t+1}^3={\hat{Q}}_t^1{P}^3-{\hat{Q}}_t^2{P}^4+{\hat{Q}}_t^3{P}^1+{\hat{Q}}_t^4{P}^2$
${\check{Q}}_{t+1}^4={\hat{Q}}_t^1{P}^4+{\hat{Q}}_t^2{P}^3-{\hat{Q}}_t^3{P}^2+{\hat{Q}}_t^4{P}^1$
${\check{v}}_{t+1}^x={\delta }_{trans}cos(arccos({\hat{Q}}_t^1)+{\delta }_{rot1})/{\Delta }_t$
${\check{v}}_{t+1}^y={\delta }_{trans}sin(arccos({\hat{Q}}_t^1)+{\delta }_{rot1})/{\Delta }_t$
${\check{v}}_{t+1}^z=0$
${\check{\omega }}_{t+1}^x=0$
${\check{\omega }}_{t+1}^y=0$
${\check{\omega }}_{t+1}^z=({\delta }_{rot1}+{\delta }_{rot2})/{\Delta }_t$

其中$P^1,P^2,P^3,P^4$ 的计算，参考2.1，总之：
$P^1=cos({\delta }_{rot1}+{\delta }_{rot2})$
$P^2=0$
$P^3=0$
$P^4=sin({\delta }_{rot1}+{\delta }_{rot2})$

---

1.6 更进一步 运动方程

${\check{x}}_{t+1}={\hat{x}}_t+{\delta }_{trans}cos(atan2(2({\hat{Q}}_t^1{\hat{Q}}_t^4),1-2({\hat{Q}}_t^4{)}^2)+{\delta }_{rot1})$
${\check{y}}_{t+1}={\hat{y}}_t+{\delta }_{trans}sin(atan2(2({\hat{Q}}_t^1{\hat{Q}}_t^4),1-2({\hat{Q}}_t^4{)}^2)+{\delta }_{rot1})$
${\check{z}}_{t+1}=0$
${\check{Q}}_{t+1}^1={\hat{Q}}_t^1{P}^1-{\hat{Q}}_t^2{P}^2-{\hat{Q}}_t^3{P}^3-{\hat{Q}}_t^4{P}^4$
${\check{Q}}_{t+1}^2={\hat{Q}}_t^1{P}^2+{\hat{Q}}_t^2{P}^1+{\hat{Q}}_t^3{P}^4-{\hat{Q}}_t^4{P}^3$
${\check{Q}}_{t+1}^3={\hat{Q}}_t^1{P}^3-{\hat{Q}}_t^2{P}^4+{\hat{Q}}_t^3{P}^1+{\hat{Q}}_t^4{P}^2$
${\check{Q}}_{t+1}^4={\hat{Q}}_t^1{P}^4+{\hat{Q}}_t^2{P}^3-{\hat{Q}}_t^3{P}^2+{\hat{Q}}_t^4{P}^1$
${\check{v}}_{t+1}^x={\delta }_{trans}cos(atan2(2({\hat{Q}}_t^1{\hat{Q}}_t^4),1-2({\hat{Q}}_t^4{)}^2)+{\delta }_{rot1})/{\Delta }_t$
${\check{v}}_{t+1}^y={\delta }_{trans}sin(atan2(2({\hat{Q}}_t^1{\hat{Q}}_t^4),1-2({\hat{Q}}_t^4{)}^2)+{\delta }_{rot1})/{\Delta }_t$
${\check{v}}_{t+1}^z=0$
${\check{\omega }}_{t+1}^x=0$
${\check{\omega }}_{t+1}^y=0$
${\check{\omega }}_{t+1}^z=({\delta }_{rot1}+{\delta }_{rot2})/{\Delta }_t$

其中$P^1,P^2,P^3,P^4$ 的计算，参考2.1，总之：
$P^1=cos(\Delta \theta )$
$P^2=0$
$P^3=0$
$P^4=sin(\Delta \theta )$

---

1.7 世界坐标系对齐

md，monoslam的世界坐标系，是跟相机坐标系平行的，不同于我们平面机器人习惯的z轴表示高度，xy表示平面位置…

${\check{x}}_{t+1}^c={\hat{x}}_t^c+{\delta }_{trans}sin(asin(2({\hat{Q}}_t^1{\hat{Q}}_t^3))+{\delta }_{rot1})$
${\check{y}}_{t+1}^c=0$
${\check{z}}_{t+1}^c={\hat{z}}_t^c+{\delta }_{trans}cos(asin(2({\hat{Q}}_t^1{\hat{Q}}_t^3))+{\delta }_{rot1})$
${\check{Q}}_{t+1}^1={\hat{Q}}_t^1{P}^1-{\hat{Q}}_t^2{P}^2-{\hat{Q}}_t^3{P}^3-{\hat{Q}}_t^4{P}^4$
${\check{Q}}_{t+1}^2={\hat{Q}}_t^1{P}^2+{\hat{Q}}_t^2{P}^1+{\hat{Q}}_t^3{P}^4-{\hat{Q}}_t^4{P}^3$
${\check{Q}}_{t+1}^3={\hat{Q}}_t^1{P}^3-{\hat{Q}}_t^2{P}^4+{\hat{Q}}_t^3{P}^1+{\hat{Q}}_t^4{P}^2$
${\check{Q}}_{t+1}^4={\hat{Q}}_t^1{P}^4+{\hat{Q}}_t^2{P}^3-{\hat{Q}}_t^3{P}^2+{\hat{Q}}_t^4{P}^1$
${\check{v}}_{t+1}^x={\delta }_{trans}sin(asin(2({\hat{Q}}_t^1{\hat{Q}}_t^3))+{\delta }_{rot1})/{\Delta }_t$
${\check{v}}_{t+1}^y=0$
${\check{v}}_{t+1}^z={\delta }_{trans}cos(asin(2({\hat{Q}}_t^1{\hat{Q}}_t^3))+{\delta }_{rot1})/{\Delta }_t$
${\check{\omega }}_{t+1}^x=0$
${\check{\omega }}_{t+1}^y=-({\delta }_{rot1}+{\delta }_{rot2})/{\Delta }_t$
${\check{\omega }}_{t+1}^z=$

其中$P^1,P^2,P^3,P^4$ 的计算，参考2.1，总之：
$P^1=cos(-\Delta \theta )$
$P^2=0$
$P^3=sin(-\Delta \theta )$
$P^4=0$

2. ekf-predict 雅各比矩阵

所以有1.7
$$\begin{array}{c}\left[\begin{array}{ccccccccccccc}1& 0& 0& {\delta }_{trans}cos(arcsin(2({\hat{Q}}_t^1{\hat{Q}}_t^3))+{\delta }_{rot1})\frac{2{\hat{Q}}_t^1}{\sqrt{1-4({\hat{Q}}_t^1{\hat{Q}}_t^3{)}^2}}& 0& {\delta }_{trans}cos(arcsin(2({\hat{Q}}_t^1{\hat{Q}}_t^3))+{\delta }_{rot1})\frac{2{\hat{Q}}_t^3}{\sqrt{1-4({\hat{Q}}_t^1{\hat{Q}}_t^3{)}^2}}& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& -{\delta }_{trans}sin(arcsin(2({\hat{Q}}_t^1{\hat{Q}}_t^3))+{\delta }_{rot1})\frac{2{\hat{Q}}_t^1}{\sqrt{1-4({\hat{Q}}_t^1{\hat{Q}}_t^3{)}^2}}& 0& -{\delta }_{trans}sin(arcsin(2({\hat{Q}}_t^1{\hat{Q}}_t^3))+{\delta }_{rot1})\frac{2{\hat{Q}}_t^3}{\sqrt{1-4({\hat{Q}}_t^1{\hat{Q}}_t^3{)}^2}}& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& P_1& -P_2& -P_3& -P_4& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& P_2& P_1& P_4& -P_3& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& P_3& -P_4& P_1& P_2& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& P_4& P_3& -P_2& P_1& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& \frac{{\delta }_{trans}}{\Delta t}cos(arcsin(2({\hat{Q}}_t^1{\hat{Q}}_t^3))+{\delta }_{rot1})\frac{2{\hat{Q}}_t^1}{\sqrt{1-4({\hat{Q}}_t^1{\hat{Q}}_t^3{)}^2}}& 0& \frac{{\delta }_{trans}}{\Delta t}cos(arcsin(2({\hat{Q}}_t^1{\hat{Q}}_t^3))+{\delta }_{rot1})\frac{2{\hat{Q}}_t^3}{\sqrt{1-4({\hat{Q}}_t^1{\hat{Q}}_t^3{)}^2}}& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& -\frac{{\delta }_{trans}}{\Delta t}sin(arcsin(2({\hat{Q}}_t^1{\hat{Q}}_t^3))+{\delta }_{rot1})\frac{2{\hat{Q}}_t^1}{\sqrt{1-4({\hat{Q}}_t^1{\hat{Q}}_t^3{)}^2}}& 0& -\frac{{\delta }_{trans}}{\Delta t}sin(arcsin(2({\hat{Q}}_t^1{\hat{Q}}_t^3))+{\delta }_{rot1})\frac{2{\hat{Q}}_t^3}{\sqrt{1-4({\hat{Q}}_t^1{\hat{Q}}_t^3{)}^2}}& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]\end{array}$$

---

根据1.5中的运动方程，有：

$$\begin{array}{c}\left[\begin{array}{ccccccccccccc}1& 0& 0& \frac{{\delta }_{trans}sin(arccos({\hat{Q}}_t^1)+{\delta }_{rot1})}{\sqrt{1-({\hat{Q}}_t^1{)}^2}}& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& -\frac{{\delta }_{trans}cos(arccos({\hat{Q}}_t^1)+{\delta }_{rot1})}{\sqrt{1-({\hat{Q}}_t^1{)}^2}}& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& P_1& -P_2& -P_3& -P_4& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& P_2& P_1& P_4& -P_3& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& P_3& -P_4& P_1& P_2& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& P_4& P_3& -P_2& P_1& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& \frac{{\delta }_{trans}sin(arccos({\hat{Q}}_t^1)+{\delta }_{rot1})}{{\Delta }_t\sqrt{1-({\hat{Q}}_t^1{)}^2}}& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& -\frac{{\delta }_{trans}cos(arccos({\hat{Q}}_t^1)+{\delta }_{rot1})}{{\Delta }_t\sqrt{1-({\hat{Q}}_t^1{)}^2}}& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]\end{array}$$

---

3. 算法引入

3.1 引入1：欧拉角转四元数

首先是一般欧拉角转四元数的matlab代码（c++代码块展示）：

function q=v2q(v)
theta=norm(v);
if (theta <eps)
    q=[1 0 0 0];
else
    v_n=v/norm(v);
    q=my_quaternion(v_n,theta);
end

$$+$$

function q=quaternion(v,theta)
q=[cos(theta/2)  sin(theta/2)*reshape(v,1,3)/norm(v)];

但是本实验中的机器人只有绕z轴的旋转，所以比上述内容来得简单，如对于转角$\Delta {\theta }_z$ ,其对应的四元数p为：
$p=[p_1,p_2,p_3,p_4]$

其中：
$p_1=cos(\Delta {\theta }_z/2)$
$p_2=0$
$p_3=0$
$p_4=sin(\Delta {\theta }_z/2)$

---

3.2 引入2：旋转角度a与旋转角度b相加的四元数表达

注意 ：角度a与角度b并不是一维向量，而是3X1的表达欧拉角（yaw等）的向量
假设，对应于转角a与转交b的四元数为：q、p，有：

$q=[q_1,q_2,q_3,q_4]$
$p=[p_1,p_2,p_3,p_4]$

那么对应于a+b的四元数q‘的计算：

matlab代码：（用的c++模板）

function qp=qprod(q,p)
a=q(1);
v=reshape(q(2:4),3,1);
x=p(1);
u=reshape(p(2:4),3,1);
qp = [a*x-v'*u, (a*u+x*v)' + cross(v,u)'];

其中cross函数的运算（a ,b为输入参数，对应于上边的v和u）：

c = [a(2).*b(3)-a(3).*b(2);
        a(3).*b(1)-a(1).*b(3);
        a(1).*b(2)-a(2).*b(1)];

总之可以得到下边的公式：
$$\begin{array}{c}\begin{array}{c}{q}_1^{\prime }=q_1{p}_1-q_2{p}_2-q_3{p}_3-q_4{p}_4\\ q_2^{\prime }=q_1{p}_2+q_2{p}_1+q_3{p}_4-q_4{p}_3\\ q_3^{\prime }=q_1{p}_3-q_2{p}_4+q_3{p}_1+q_4{p}_2\\ q_4^{\prime }=q_1{p}_4+q_2{p}_3-q_3{p}_2+q_4{p}_1\end{array}\end{array}$$

---

3.3 引入3 反三角函数求导

$y=arccos(x)$ >> $y^{\prime }=-\frac{1}{\sqrt{1-x^2}}$

$y=arcsin(x)$ >> $y^{\prime }=\frac{1}{\sqrt{1-x^2}}$

$y=arctan(x)$ >> $y^{\prime }=\frac{1}{1+x^2}$

$y=arccot(x)$ >> $y^{\prime }=-\frac{1}{1+x^2}$