ardupilot开发 --- 视觉伺服 篇

1.视觉伺服分类

控制量是在图像空间中推导得到还是在欧式空间中推导得到，视觉伺服又可以分类为基于位置(PBVS)和基于图像的(IBVS)视觉伺服。

2.视觉伺服中的坐标系

• 概述
  世界坐标系W：用于测量（估计）飞机、机器人的位姿（位置和姿态）。
  飞机机体坐标系B：最终运动控制量应转换到这个坐标系。
  目标机体坐标系O：用于描述目标物体与相机间的位姿，用于描述相机坐标系和目标物体机体坐标系之间的位姿关系。
  相机坐标系C：是推导IBVS最重要的坐标系。
  图像坐标系I：是描述特征点运动状态的坐标系。
  像素坐标系P：最终的图像数据最终以该坐标系的形式存储信息。
• 表示
  W 即 world，表示世界坐标系,E即 end，表示末端坐标系，类似还有I表示 image，O表示 object，C 表示 Camera等。而各种坐标系齐次变换矩阵T的左上标表示转换后的坐标系，右下标表示转换前的坐标系。如 ${}^c{T}_e$或 ${}^c{V}_e$表示从末端坐标系E到相机坐标系C的坐标变换矩阵或称为齐次变换矩阵(齐次变换矩阵即旋转变换和位移变换融合到了一个矩阵当中)。
  

3.成像模型推导

• 相关概念：透视投影模型。

• 关于透视投影这篇文章讲的很好： 深蓝AI：经典干货｜相机模型与张氏标定。参考了这篇文章。

• 小孔成像模型
  光心位于成像平面的前方，成倒立的像，这样不方便IBVS的推导。
  

• 透视投影模型
  光心位于成像平面的后方，成正立的实像，更符合实际成像过程，方便IBVS的推导。
  

• 世界系、相机系、图像系、像素系的轴向、原点位置示意图
  
  
  
  相机系记作Oc-XcYcZc.
  图像系记作o-xy.
  像素系记作o-uv.
  相机系的原点在光心，Xc轴水平向右，Yc轴竖直向下，Zc轴水平向前。
  图像坐标系的原点在Zc轴与成像平面的交点处，x、y轴分别与Xc、Yc轴同向。
  像素坐标系的原点在成像平面的左上角，u、v轴分别与图像系的x、y轴同向。
  图像系原点在像素系中的坐标为【u0,v0】,也被称为主点坐标。
  相机系原点到成像平面的距离为 f，即焦距。

• 像素系坐标与图像系坐标间的关系
  $$\{\begin{array}{l}u=\frac{x}{dx}+u_0=p_x+u_0\\ v=\frac{x}{dx}+u_0=p_x+u_0\end{array}（式1）$$
  其中：
  $[u_0,v_0]$是图像系原点在像素系中的坐标;
  $p_x,p_y$是图像系中 xy 轴的单位长度对应的像素个数;
  $u,v$是像素系中的坐标；
  $x,y$是图像系中的坐标。

• 图像系坐标与相机系坐标间的关系
  $$\{\begin{array}{l}x=\frac{f}{Z}X\\ y=\frac{f}{Z}Y\end{array}（式2）$$
  其中：
  $f$ 是相机焦距；
  $xy$ 是图像系中的坐标；
  $XYZ$ 是目标点在相机系中的坐标。

• 相机内参
  (式1)、(式2) 提到的参数 $u_0,v_0,px,p_y$ 被称为相机的内参，通过相机标定得到。

4.IBVS理论推导

问题描述：
假设在世界3维空间中有一点P，
在相机系中的坐标记作$[X,Y,Z]$，
在图像系中的坐标记作$[x,y]$，
在像素系中的坐标记作$[u,v]$。
记相机的6自由度运动速度矢量（相机坐标系的速度矢量）为：
$$V_c=[v_x,v_y,v_z,w_x,w_y,w_z{]}^T（式3）$$

根据物体的旋转运动和直线运动的经典理论公式，可得到点P在相机系中的运动方程为：
$$\left[\begin{array}{c}\dot{X}\\ \dot{Y}\\ \dot{Z}\end{array}\right]=-\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]-\left[\begin{array}{c}{w}_x\\ w_y\\ w_z\end{array}\right]\times \left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]（式4）$$
注意：因为$V_c=[v_x,v_y,v_z,w_x,w_y,w_z{]}^T$是相机的速度矢量，正好与点P的速度矢量相反，因此（式4）右边取的是负号！！
将（式2）对时间求导可得：
$$\{\begin{array}{l}\dot{x}=\dot{X}/Z-X\dot{Z}/Z^2\\ \dot{y}=\dot{Y}/Z-Y\dot{Z}/Z^2\end{array}（式5）$$
将（式2）和（式4）代入（式5）可得：
$$\{\begin{array}{l}\dot{x}=-v_x/Z+x{v}_z/Z+xy{w}_x-(1+x^2)w_y+y{w}_z\\ \dot{y}=-v_y/Z+y{v}_z/Z-xy{w}_y+(1+y^2)w_x-x{w}_z\end{array}（式6）$$
写成矩阵形式：
$$\dot{s}=\left[\begin{array}{c}\dot{x}\\ \dot{y}\end{array}\right]=L_s{V}_c（式7）$$
其中$s$ 被称为视觉特征，$L_s$被称为图像雅可比矩阵或相互作用矩阵：
$$L_s=\left[\begin{array}{cccccc}-1/Z& 0& x/Z& xy& -(1+x^2)& y\\ 0& -1/Z& y/Z& 1+y^2& -xy& -x\end{array}\right]（式8）$$
记视觉特征$s$的期望值为$s_d$，则视觉特征误差为：
$$s_e=s-s_d（式9）$$
对于IBVS场景中$s_d$一般是常量，因此有：
$${\dot{s}}_d=0（式10）$$
将（式9）对时间求导，得到误差系统的状态空间方程：
$${\dot{s}}_e=\dot{s}-{\dot{s}}_d=L_s{V}_c（式11）$$
设计一个控制律$Vc$使得（式11）表示的误差系统的全部状态随着时间呈指数衰减到0，即控制律使得最终的误差系统变成如下形式：
$${\dot{s}}_e=-\lambda s_e（式12）$$
那么可以反推出控制律：
$$V_c=-\lambda L_s^{+}{s}_e（式13）$$
其中$L_s^{+}$是$L_s$的广义逆矩阵，$\lambda$是一个常量。

待续…

5.IBVS面临的挑战

• 计算$L_s^{+}$时会产生奇异值。
• $L_s$不容易得到，$L_s$的几种计算方式请参考：https://zhuanlan.zhihu.com/p/422634446
• 待续…

6.visp 实践

• ${}^c{J}_c$ ：相机坐标系的运动控制自由度，可以看作是运动控制自由度雅可比矩阵。
• L：图像雅可比矩阵，相互作用矩阵
• $J_1$：task雅可比矩阵
• signInteractionMatrix：相互作用矩阵的符号，1 for eye-in-hand, -1 for eye-to-hand
• inversionType：指定求广义逆矩阵还是求转置矩阵
• Transpose matrix：转置矩阵
• Inverse matrix：逆矩阵
• task Jacobian 是什么？$J_1$？？？
  $V_c=-\lambda {\hat{\mathbf{L}}}_s^{+}\mathbf{e}$，得到的控制律$V_c$是相机系的运动矢量！此时的$J_1=L_s$.
  $V_e=-\lambda {\left({\hat{\mathbf{L}}}_s{}^c{\mathbf{V}}_e{}^e{\mathbf{J}}_e\right)}^{+}\mathbf{e}$，得到的控制律$V_e$是终端系的运动矢量！此时的$J_1=L_s{}^c{V}_e{}^e{J}_e$.
  其中${\hat{L}}_s$是$L_s$的估计值！$J_1$被称为 task Jacobian.
  why ？？
  推导如下：
  $\dot{s}=L_s{V}_c$
  记终端系的速度矢量为$V_e$，终端系到相机系的坐标变换矩阵为${}^c{V}_e$，允许控制的速度矢量自由度记为${}^{e}Je$，则：
  $\dot{s}=L_s{V}_c=L_s^c{V}_e{}^{e}Je{V}_e$
  那么控制率就变成了：
  $V_e=-\lambda (L_s{{}^c{V}_e}^{e}Je{)}^{-1}$
  对于无人机视觉伺服，${}^c{V}_e$即飞机机体坐标系FRD到相机系RDF的齐次变换矩阵（坐标变换）！！！！

参考文献

https://zhuanlan.zhihu.com/p/422634446
https://zhuanlan.zhihu.com/p/389903710
深蓝AI：经典干货｜相机模型与张氏标定
硕士论文：基于无标定视觉伺服的定位研究-王博