基于编码标志点的无人机定位以及姿态角测量方法与流程

本发明属于近景摄影测量以及机器视觉技术领域，具体涉及一种基于编码标志点的无人机定位以及姿态角测量方法。

背景技术：

无人机的姿态测量技术在军事试验、航天对接、航空摄影、航海地形、工业测量等领域有着十分重要的地位，越来越多专家学者开始对无人机的姿态角测量展开了深入探讨与研究。其中姿态角是反映飞行器运动状态的重要参数，获得这些参数对飞行器的运动分析、气动参量辨识、优化设计、故障分析等有着重要的意义。无人机姿态角测量在无人机着舰，以及导航方面要求精确度极高，出现问题可能造成不可挽回的损失，无人机姿态角的测量日益彰显出极大的重要作用。随着视觉测量、图像处理、计算机技术等技术的不断成熟和完善，计算机视觉测量方法也获得了快速发展，测量更加快速、精确，视觉系统具有轻便、低功耗、体积小、精度高，不受电子干扰等优点，此外，视觉导航还具有很高的隐蔽性。视觉系统是无人机系统的重要组成部分，根据视觉系统求取无人机姿态角逐渐成为发展趋势。

现存的基于视觉的无人机姿态角测量大多采取的是基于合作物的测量。如一种无人机定位的方法，该方法为：在无人机到达位于目标位置点的预设距离范围内的位置点之后，根据预设的图像采集参数，采集用预设标志物进行标记的所述目标位置点的图像。提取所述图像的特征信息，将所述特征信息与预存储特征信息进行比对，判断是否一致。当一致时，根据当前的所述图像采集参数，计算出所述位置点和所述目标位置点之间的距离。但是，该方法存在一定的局限性，当无人机不在目标范围内时，就无法实现无人机定位；并且，需要采集标志点和预存储的特征信息进行对比判断，从而得到无人机位置信息，这个过程耗时较长，不能快速得到无人机的位置信息。因此，设计一种没有特殊飞行位置限制，又能快速得出无人机位置坐标以及其他信息的方法具有较大的意义和研究价值。

技术实现要素：

为了解决现有的的上述技术缺陷，本发明提供一种基于编码标志点的无人机定位以及姿态角测量方法。

本发明是通过以下技术方案实现的：

基于编码标志点的无人机定位以及姿态角测量方法，包括如下步骤：

用搭载在无人机上的相机拍摄地面上预先布置的若干编码标志点，获取拍摄图像，并进行处理；编码标志点为带初始点的二进制编码合作点；

对搭载在无人机上的相机进行标定，获得相机的内参数；

求取世界坐标系相对于相机坐标系的旋转矩阵rw2c和平移矩阵tw2c；

根据三维坐标转换关系计算无人机坐标系相对于相机坐标系的旋转矩阵ra2c和平移矩阵ta2c；

根据世界坐标系相对于相机坐标系的旋转矩阵rw2c以及无人机坐标系相对于相机坐标系的旋转矩阵ra2c计算无人机坐标系相对于世界坐标系的旋转矩阵ra2w；根据世界坐标系相对于相机坐标系的旋转矩阵rw2c和平移矩阵tw2c以及无人机坐标系相对于相机坐标系的平移矩阵ta2c计算无人机坐标系相对于世界坐标系的平移矩阵ta2w；

根据无人机坐标系相对于世界坐标系的旋转矩阵ra2w，计算无人机的姿态角，包括俯仰角、偏航角以及翻滚角；根据无人机坐标系相对于世界坐标系的平移矩阵ta2w，计算无人机在世界坐标系下的位置坐标。

本发明相对于现有技术的有益效果在于：

1.本发明采用带初始点的二进制编码合作点，在很大程度上扩大了无人机拍摄标识点的视场范围，增强了无人机定位以及姿态角测量的灵活性以及可行性，使得无人机的定位以及姿态角的测量精度明显提高，且易于制作。

2.本发明根据无人机坐标系，相机坐标系，世界坐标系三坐标之间的坐标转换关系，计算出无人机相对世界坐标系的位置坐标以及在世界坐标系下的姿态角，使无人机定位以及姿态角的测量速度进一步提高，算法简单易于实现，有效的增强了该算法的实时性。3.本发明不仅解决了无人机的搭载质量限制问题，也满足了精度要求。

附图说明

图1为基于编码标志点的无人机定位以及姿态角测量方法的总流程图；

图2为带起始点的二进制编码标志点的示意图；

图3为无人机对地拍摄过程的示意图；

图4为无人机坐标系到相机坐标系之间的关系图；

图5为任意拍摄到四个编码标志点的示意图；

图6为任意拍摄到五个编码标志点的示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施方式仅仅用以解释本发明，并不用于限定本发明。

实施例1：

实现本发明所需的无人机须满足以下要求：搭载高速相机，相机能够对地面对地面进行拍摄，并能够将拍摄到的图像传输到地面实验平台进行处理。

下面具体说明本实施例提供的基于编码标志点的无人机定位以及姿态角测量方法，如图1所示，包括如下步骤：

步骤s1，用搭载在无人机上的相机拍摄地面上预先布置的若干编码标志点，获取拍摄图像，并进行处理；编码标志点为带初始点的二进制编码合作点，如图1所示。拍摄过程示意图如图3所示。

本步骤中提到的图像处理方法具体为获取拍摄到图像上的任意若干个编码标志点，并对获取的编码标志点解码匹配。获取编码标志点的数量可根据步骤s3中使用的求取世界坐标系相对于相机坐标系的旋转矩阵rw2c和平移矩阵tw2c的方法确定，若采用直接线性法，需要至少获取四个编码标志点；若采用tsai两步标定法，需要至少获取五个编码标志点。

具体的编码标志点的形状、结构设置以及解码方法可以参考申请号为201611108392.9的中国发明专利申请说明书第31至53段。

步骤s2，对无人机的相机进行标定，获得相机的内参数。

在本实施例中，利用张正友的棋盘格标定相机，得到相机的内参数，内参数具体包括：主点坐标(u0,v0)，焦距f，等效焦距fx，fy，径向畸变参数k1，k2，k3，切向畸变参数p1,p2。

步骤s3，求取世界坐标系相对于相机坐标系的旋转矩阵rw2c和平移矩阵tw2c。

可以根据直接线性法或tsai两步标定法计算世界坐标系相对于相机坐标系的旋转矩阵rw2c和平移矩阵tw2c。若采用直接线性法，需要保证相机至少任意拍摄到四个编码标志点，如图5所示；若采用tsai两步标定法，需要保证相机至少任意拍摄到五个编码标志点，如图6所示。

本实施例以直接线性法为例进行说明：

根据直接线性法计算世界坐标系相对于相机坐标系的旋转矩阵rw2c和平移矩阵tw2c，具体步骤如下：

根据公式(1)所示的世界坐标系相对于像素坐标系之间的关系，公式(2)所示的平面中心透视投影矩阵，得到公式(3)所示的平面中心透视投影关系式；

并根据平面中心透视投影关系式计算世界坐标系相对于相机坐标系的旋转矩阵rw2c和平移矩阵tw2c。

步骤s4，根据三维坐标转换关系计算无人机坐标系相对于相机坐标系的旋转矩阵ra2c和平移矩阵ta2c。

无人机坐标系相对于相机坐标系之间的关系图如图4所示，计算无人机坐标系相对于相机坐标系的旋转矩阵ra2c和平移矩阵ta2c的具体步骤如下：

设无人机坐标系x轴与相机坐标系x’轴之间的夹角为90°，无人机坐标系y轴与相机坐标系y’轴之间的夹角为90°，无人机坐标系z轴与相机坐标系z’轴之间的夹角为180°；根据公式(4)和(5)计算得到无人机坐标系相对于相机坐标系的旋转矩阵ra2c和平移矩阵ta2c，

ta2c＝[0,0,-h](5)

其中，h为相机坐标系原点到无人机坐标系原点的距离。

步骤s5，根据世界坐标系相对于相机坐标系的旋转矩阵rw2c以及无人机坐标系相对于相机坐标系的旋转矩阵ra2c计算无人机坐标系相对于世界坐标系的旋转矩阵ra2w；根据世界坐标系相对于相机坐标系的旋转矩阵rw2c和平移矩阵tw2c以及无人机坐标系相对于相机坐标系的平移矩阵ta2c计算无人机坐标系相对于世界坐标系的平移矩阵ta2w。

根据公式(6)计算无人机坐标系相对于世界坐标系的旋转矩阵ra2w；

根据公式(7)计算无人机坐标系相对于世界坐标系的平移矩阵ta2w；

步骤s6，根据无人机坐标系相对于世界坐标系的旋转矩阵ra2w，计算无人机的姿态角，包括俯仰角、偏航角以及翻滚角；根据无人机坐标系相对于世界坐标系的平移矩阵ta2w，计算无人机在世界坐标系下的位置坐标。

无人机坐标系相对于世界坐标系的旋转矩阵ra2w如公式(8)所示：

根据公式(9)计算俯仰角b；

b＝arcsin(-r2)(9)

根据公式(10)计算偏航角c；

根据公式(11)计算翻滚角a；

根据公式(12)计算无人机在世界坐标系下的位置坐标oa；

oa＝ta2w(12)。

本领域的技术人员容易理解，以上仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。