ardupilot开发 --- 视觉伺服篇

风驰电掣云端飘，相机无法对上焦

视觉伺服分类
视觉伺服中的坐标系
成像模型推导
IBVS推导
参考文献

视觉伺服分类

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

视觉伺服中的坐标系

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

成像模型推导

相关概念：透视投影模型。
这篇文章讲的很好：深蓝AI：经典干货｜相机模型与张氏标定。参考了这篇文章。
小孔成像模型
光心位于成像平面的前方，成倒立的像，这样不方便IBVS的推导。
透视投影模型
光心位于成像平面的后方，成正立的实像，更符合实际成像过程，方便IBVS的推导。
世界系、相机系、图像系、像素系的轴向、原点位置示意图

相机系记作O-XcYcZc
图像系记作o-xy
像素系记作o-uv
相机坐标系的原点在光心，Xc水平向右，Yc轴竖直向下，Zc轴水平向前。
图像坐标系的原点在Zc轴与成像平面的交点处，x、y轴分别与Xc、Yc轴同向。
像素坐标系的原点在成像平面的左上角，u、v轴分别于x、y轴同向。
图像系原点在像素系中的坐标为【u0,v0】,也被称为主点坐标。
相机系原点到成像平面的距离为f，即焦距。
像素系坐标与图像系坐标间的关系，（式1）：

其中【u0,v0】是图像系原点在像素系中的坐标。px，py是图像系中 xy 轴的单位长度对应的像素个数。
图像系坐标与相机系坐标间的关系，（式2）：

其中，f 是相机焦距，Zc 是目标点（被拍摄的点）在相机系中的Z轴坐标值。
相机内参
上述提到的参数 u0，v0，px，py 被称为相机的内参，通过相机标定得到。

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）
在这里插入图片描述
将（式2）对时间求导可得 （式4）：

将（式2）和（式3）代入（式4）可得 （式5）：

写成矩阵形式 （式6）：

其中Ls被称为图像雅可比矩阵或相互作用矩阵，s被称为视觉特征 （式7）：

记视觉特征s的期望值为sd，则视觉特征误差为 （式8）：
$s_e=s-s_d$
因为sd是常量因此有 （式9）：
$s_d=0$
将（式8）对时间求导，得到误差系统的状态空间方程 （式10）：
$s_e=\.s-\.s_d=L_sV_c$
设计一个控制律Vc使得（式10）表示的误差系统的全部状态可以随着时间呈指数衰减到0，即控制律使得最终的误差系统变成如下形式 （式11）：
$\.s_e=-\lambda s_e$
那么可以反推出控制律要满足 （式12）：
$V_c=-\lambda L_s^+ s_e$
其中 $L_s^+$ 是 $L_s$ 的广义逆矩阵， $\lambda$ 是一个常量。