8.4.2　基于标志物的无人机位置姿态估计

用8.4.1小节中得到的四个角点坐标，即可进行PNP（N点透视）问题解算。

世界坐标系和相机坐标系之间的关系如下：

式中，R=RURC 为相机坐标系相对于世界坐标系的旋转矩阵，RU为无人机坐标系相对于世界坐标系的旋转矩阵，它由无人机的三个欧拉角的方向余弦经过计算得出，CR为相机坐标系相对于无人机坐标系的旋转矩阵，它由云台的两个欧拉角（俯仰角、偏航角）的方向余弦经过计算得出。前面已经给出这两个旋转矩阵的具体表示形式。

T=[txtytz ]T为无人机的位移矢量，tx、ty、tz为位移在相机坐标系三个坐标轴方向的分量，即世界坐标系原点在相机坐标系下的坐标。

这里用之前所述的Tsai法对得出的角点进行解算，将得出旋转矩阵R以及位移矢量T。因为云台相机的姿态角CR是可以测得的，由此可求出无人机的旋转矩阵UR，从而解算出无人机的三个欧拉角。