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$$