ardupilot开发 --- 机载(边缘)计算机-VISP高阶 篇

0. 基础

VISP基础知识点请参考基础篇。

1. 视觉伺服

参考：Visual servoing

1.1 视觉伺服基础理论

理论讲解和公式推导请参考：视觉伺服篇

1.1.1 基本理论

• 几点说明
  对于本文，作以下几点定义：
  【X, Y, Z】：是相机坐标系下的坐标，3D；
  【x, y】：是图像坐标系下的坐标，2D；
  【u, v】：是像素坐标系下的坐标，2D；
  详细请参考 相机标定篇。

• 基于图像的视觉伺服(IBVS)：image-based visual servo
  基于图像点特征的视觉伺服；

• 基于位置的视觉伺服(PBVS)：position-based visual servo

• 视觉伺服控制系统
  以【实现：搭载深度相机的无人机在竖立的二维码正前方保持悬停】为例。
  

  • 被控对象：无人机、相机、二维码等组成的广义对象。

  • 系统输出： 视觉特征（visual features）。

  • 输出期望值：视觉特征期望值。

  • 系统误差：视觉特征误差。

  • 控制输入：相机坐标系下相机的速度向量Vc=[u,w]，其中u为线速度，w角速度。其实就是控制飞机的飞行速度(因为相机与飞机是固连的)。

  • 控制律：如下。理论推导请参考，或直接搜索视觉伺服。
    
    其中lambda是控制器的常数增益，一般设为0.5。

  • 相互作用矩阵Lx的表达式为：
    
    其中 Lx 为交互矩阵(interaction matrix)， 又称特征雅可比矩阵(feature Jacobian)。
    Lx一般不可以完全得到，可通过估计的方式获得：视觉伺服学习笔记——基本方法

  • 分析
    众所周知，在控制系统中，如何定义系统输出、输出期望值、系统误差 是跟我们的控制目标有关的。
    在本例子中，要实现的控制目标是：保持飞机（也就是相机）悬停在二维码正前方设定距离处。
    如果将该二维码看做一个3D点 p，那么对于相机而言，只要使得点 p 在相机系下的坐标保持在一个固定值如【0, 0，L 】即可实现控制目标。
    如此，暂时可以把 p 点在相机系下的坐标定义为控制系统的输出量，将二维码的中心定义为p点，那么【0, 0，L 】自然就是系统的输出量期望值，期望值与输出量之间的误差就是系统误差。
    但对于2D视觉伺服控制系统而言，为了方便运算一般要把所有纲量统一到图像坐标系来运算，下面做进一步分析：
    对于RGB-D相机：在视野范围内，3维空间的某点 p 在相机系下的深度值（Z值）是已知的，设为Z；
    对于RGB相机：在视野范围内，3维空间的某点 p 在像素系中的坐标值也是已知的，设为【u,v】；
    为什么说是已知的呢？：利用目标检测算法可以得到目标相对于相机的位姿，目标检测的结果就包括目标几何中心点的像素坐标。
    已知相机内参为 u0，v0，px，py ；
    根据下面的相机内参标定的原理公式可知，点p在图像系的坐标【x,y】，和相机系下坐标【X,Y】是可以计算得到的。
    
    到此，点p的已知信息包括（按时间顺序列出）：像素系坐标【u, v】，图像系坐标【x, y】，相机系坐标【X, Y, Z】。
    点 p 在图像坐标系中的坐标值【x,y】就是最终要定义的控制系统输出量，把【0, 0，L 】代替【X, Y, Z】或把图像中心坐标代替【u,v】就可以计算得到控制系统期望值【xr,yr】，那么系统误差就是两者之差了。
    在传统的视觉伺服控制系统中，一般把点 p (或其他合适的点) 在图像系中的坐标值【x,y】定义为 视觉特征（visual features）又称点特征、视觉点特征，但是这并不是定义视觉特征的唯一选择，只能要能有助于实现控制目标的量都可以定义为视觉特征。
    到此就解释了为什么视觉特征为什么可以定义为视觉伺服系统的输出量了，也弄清了什么是视觉特征。
    对于本文而言，我们暂且把这些p点定义为视觉伺服系统的特征点，注意该概念与机器视觉中的特征点的不同之处！

• 视觉伺服理论中的一些结论
  详细推导请参考：https://zhuanlan.zhihu.com/p/422634446 或 https://zhuanlan.zhihu.com/p/389903710

  • 在IBVS中，视觉特征一般用一系列合适的点的图像坐标系坐标来表示。

  • 以点坐标表征的视觉特征称为点特征，被选作视觉特征的点称为特征点。

  • eye-to-hand 和 eye-in-hand
    eye-to-hand：相机固定在一个地方，机械手的运动不会带着相机一起移动。
    eye-in-hand：相机安装在机械手上，随着机械手一起移动。

  • 视觉特征向量，例如：
    假设定义了n个视觉特征（选取n个点），视觉特征向量 s 的表达式为：

    $$s=\left[\begin{array}{c}{p}_1\\ ⋮\\ p_n\end{array}\right]={\left[\begin{array}{c}{x}_1\\ y_1\\ \\ ⋮\\ x_n\\ y_n\end{array}\right]}_{2n\ast 1}(式1)$$x,y是特征点在图像坐标系下的坐标值，什么是图像坐标系请查阅：相机标定篇

  • 控制向量Vc
    $$V_c=\left[\begin{array}{c}{v}_c\\ w_c\end{array}\right]={\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\\ w_x\\ w_y\\ w_z\end{array}\right]}_{6\ast 1}(式2)$$其中，vc, wc 分别是线速度和角速度（相机系）。

  • 控制率
    $$V_c=-\lambda L_s^{+}e(式3)$$其中：$$e=s_e={\left[\begin{array}{c}x{1}_e\\ y{1}_e\\ ⋮\\ x{n}_e\\ y{n}_e\end{array}\right]}_{2n\ast 1}(式4)$$
    L s = [ L 1 s ⋮ L n s