为什么要位姿解算

手动瞄准目标由于瞄准慢且操作难度大已经不适合在Robomaster的赛场上使用，那么代替手瞄的自动瞄准就变得尤其重要。而自动瞄准的流程是先识别到目标，然后得到目标与相机的位置关系，最后将位置关系传递给下位机，由下位机控制云台偏转角度，实现自瞄效果。而目标与相机位置关系的计算就需要使用到位姿结算来实现。位姿解算可以利用三角形的相似等性质计算出当前相机与目标相对位置关系，通过相对位置关系即可推出如何让云台进行调整。

选择PnP原因

相机姿态估计的基本思想是利用相机的内参数以及已知物体在空间坐标与与之一一对应的图像坐标的投影关系来求出此时物体相对于已知物体在三维空间坐标的外参数，也即是旋转矩阵以及平移向量。此关键算法核心是解决N点透视投影问题，也称作PNP（Perspective-N-Point）问题。 木鸢Birdiebot结合自身和大多数队伍使用单目相机的情况，选择使用PnP算法进行位姿解算。PnP(Perspective-n-Point)是求解3D到2D点对的运动的方法:即给出n个3D空间点时，如何求解相机的位姿。它描述了当我们知道n个3D空间点以及它们的投影位置时,如何估计相机所在的位姿。PnP算法支持不定数量的3D空间点，可以根据使用时3D点的数量来进行不同的估算方法。

PnP算法介绍

P3P算法简单介绍

由于存在有4个3D空间点（矩形装甲板的四个顶点），所以选用P3P用于位姿解算。P3P利用给定的四个点中的三个进行3D-2D几何关系的匹配，而剩余的一对验证点,是用来从三对点解算出来的可能的解中，选出最正确的那一个(类似于对极几何情形)。

原理图：

PnP算法详细介绍

• PnP算法原理
• PnP算法简介与代码解析

PnP使用

木鸢Birdiebot整体是基于python的开源框架，所以此处主要介绍python下PnP的使用。 我们使用了opencv开源库下的solvePnP算法来进行位姿解算。solvePnP详解

solvePnP数据定义

• objectPoints：目标世界坐标，数据类型numpy.array
• imagePoints: 目标像素坐标，数据类型numpy.array
• cameraMatrix: 相机的内参数矩阵projection，通过相机棋盘标定获取
• distCoeffs：相机的畸变系数distortion，通过相机棋盘标定获取
• rvec：旋转向量rotation_vector
• tvec：平移向量translation_vector

位姿解算过程

坐标系是以相机为坐标中心的左手坐标系。通过平移向量可以得出目标在坐标系中的坐标位置，通过目标坐标位置与坐标中心进行角度计算可以得出云台所需偏转的角度。 得出的角度包括yaw轴和pitch轴上所需旋转的角度。