CN103424126A - 一种无人机视觉自主着陆仿真验证系统及方法 - Google Patents

Abstract

本发明涉及一种无人机视觉自主着陆仿真验证系统及方法，利用虚拟现实技术和可视化技术，模拟了真实的无人机飞行环境，具有沉浸式的真实感，解决了无人机自主着陆视景仿真摄像机模拟的问题，将视觉着陆算法与VegaPrime结合，通过多通道展示整个着陆阶段并实时的计算和显示视觉着陆导航所需数据。仿真结果表明，该系统可以有效的对着陆地标进行检测，能够直观、实时的显示位姿的估计值，实现了仿真验证的功能，为实际工程应用提供了一个良好的仿真平台。

Description

一种无人机视觉自主着陆仿真验证系统及方法

技术领域

本发明涉及一种无人机视觉自主着陆仿真验证系统及方法，能够在真实地形下对无人机自主着陆视觉算法进行仿真验证，利用虚拟现实和可视化技术，构建了机载摄像机和着陆地标，通过多通道显示整个着陆阶段和摄像机拍摄的图像并实时计算和输出视觉着陆导航所需数据。

背景技术

在整个无人机的飞行导航过程中，安全着陆是一个非常重要的阶段。在现有的着陆导航方式中，惯性导航由于需要通过积分运算来得到飞行器大概位置和速度信息，使得误差会随着时间的推移不断增加；GPS卫星导航容易受到电子干扰；地面控制导航则受控于人类因素。这些问题都可能导致着陆的失败。

传统的载人机着陆时，飞行员可以依靠视觉所提供的信息来定位直升机空间位置和直升机相对于着陆平台的旋转关系。而无人机一般通过机载传感器提供这些信息，因此可以考虑使用计算机视觉技术模拟飞行员的视野，并通过视觉算法对机载摄像机所获取的实时图像进行处理和分析，估计出无人机相对于着陆地标的位置与姿态信息，进而完成自主着陆。由于视觉着陆导航技术尚不成熟，需要进行大量的飞行试验，因此有必要建立一个仿真验证系统完成在实验室环境下对视觉着陆算法的验证。现有的验证方法中，要么是对已知位姿值的单帧图像处理进行验证，而无人机自主着陆是一个动态过程，单帧图像的意义不大；要么使用MATLAB模拟无人机自主着陆过程进行验证，该方法一定程度上解决了无人机自主着陆动态过程展示的问题，但其对无人机及飞行环境的呈现过于简单，仿真过程往往缺乏沉浸式的真实感；基于此，如何通过先进的虚拟现实技术以及计算机视觉技术构建一个可视化良好的无人机自主着陆仿真验证系统是急需解决的问题，其中视景仿真软件与视觉着陆算法如何结合是问题的关键所在。

发明内容

本发明的目的是提供一种无人机视觉自主着陆仿真验证系统及方法，使用VegaPrime软件和视觉算法结合，在真实地形中对无人机自主着陆的视觉算法进行验证，能够展示整个着陆阶段并实时的计算和显示视觉着陆导航所需数据，可以有效的降低开发成本，减少飞行试验数量，缩短视觉导航技术的研发周期。

本发明的技术方案是：一种无人机视觉自主着陆仿真验证系统，其特征是：至少包括VegaPrime模块、着陆地标检测模块、无人机位姿估计模块，VegaPrime模块用于展示无人机着陆的视景，着陆地标检测模块用于对当前帧图像中进行图像处理，使用DP多边形拟合算法对着陆地标进行多边形拟合并提取角点，对所得角点重新排序与着陆地标对应后将结果传至无人机位姿估计模块；无人机位姿估计模块用于利用Courtney的方法得到无人机相对于着陆地标的旋转矩阵和平移矩阵，并通过旋转矩阵与欧拉角的关系得到无人机的三个姿态角，实时估计无人机位姿。

所述VegaPrime模块至少包括无人机模块、着陆地标模块、摄像机模块、显示模块，无人机模块中使用了Vegaprime模型库中的阿帕奇直升机，并在直升机中加入了前视摄像机和着陆用摄像机；着陆地标模块中使用Creator建模工具建立了一个长5米，宽3米的H型地标；正视地标时，左上角角点定为地标坐标系原点，坐标为（0,0），其余各点顺时针排序，坐标依次为（1,0）、（1,2）、（2,2）、（2,0）、（3,0）、（3,5）、（2，5）（2,3）、（1，3）、（1,5）、（0,5）；摄像机模块为着陆用摄像机，其位置固定于无人机中心，垂直向下拍摄，根据一般机载摄像头视场角范围设置该观察者的水平FOV为30度，垂直FOV为20度，拍摄视频大小为360*240像素；通过无人机姿态估计算法反解求得模拟摄像机的内参矩阵，通过配置，将无人机位置调整到着陆地标原点正上方，且3个姿态角均为零，此时无人机相对于着陆地标的旋转矩阵R为单位阵，平移矩阵t=(0 0 h)^T；在不同高度h下截取摄像头所拍摄到的地标图像，并通过图像处理得到图像中的12个角点像素坐标，分别将12个角点的物理坐标和像素坐标以及旋转矩阵、平移矩阵带入公式:

$\{\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)\left(\begin{array}{ccc}0& 0& 1\end{array}\right)-A\}\left(R_{3\×3}{t}_{3\×1}\right)\left(\begin{array}{c}x\\ y\\ 0\\ 1\end{array}\right)=0$

解此方程组便可求得摄像机内参矩阵A，其中 $A=\left(\begin{array}{ccc}\frac{f}{d_x}& 0& u_0\\ 0& \frac{f}{d_y}& v_0\\ 0& 0& 1\end{array}\right)$ 为摄像机内参矩阵，u、v为角点像素坐标，x、y为角点物理坐标，R为旋转矩阵，t为平移矩阵，f为摄像机焦距，d_x为u方向上单位像素的物理尺寸，d_y为v方向上单位像素的物理尺寸，u_n为图像中心点u方向像素坐标，v₀为图像中心点v方向像素坐标；显示模块用于显示着陆系统仿真验证的最终结果，并根据仿真的目的设计需要的界面，展示视觉着陆导航的整个过程，在多窗口上显示摄像机拍摄的图像，给出无人机真实位姿与估计值的比较输出。

所述着陆地标检测模块，至少包括角点检测、角点排序，角点检测首先对显示模块的摄像机图像进行灰度化处理，利用阈值分割的方法分离出地标区域，然后经过轮廓提取，并保留单连通闭合且长度为图像周长的0.5倍到1.5倍的轮廓得到地标的轮廓，对该轮廓进行DP多边形拟合后提取角点，保留角点像素坐标；角点排序用于对角点检测所得到的角点进行排序，角点检测所保留的角点顺序为像素坐标系下v坐标最小的点为初始点，其余各点顺时针依次排列；当角点检测所得图像角点a、b两点之间距离小于a、l两点之间距离时角点检测所得排序即为正确顺序，当角点检测所得图像角点a、b两点之间距离大于a、l两点之间距离时需要对所有点重新排序，遍历轮廓，取图像像素坐标系下u坐标最小的点为初始点，其余各点按顺时针依次排序；角点排序所得图像角点最终排序结果和物理坐标系下地标各角点一一对应。

所述无人机位姿估计模块，至少包括姿态估计、位置估计，姿态估计利用Courtney的方法，已知提前求得的摄像机内参矩阵A、12个地标角点物理坐标(x_i,y_i)和图像像素坐标(u_i,v_i)，并带入公式：

$\{\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)\left(\begin{array}{ccc}0& 0& 1\end{array}\right)-A\}\left(R_{3\×3}{t}_{3\×1}\right)\left(\begin{array}{c}x\\ y\\ 0\\ 1\end{array}\right)=0$

得到一个超定方程组，通过SVD奇异值分解法解此超定方程组得到摄像机坐标系相对于着陆地标坐标系的旋转矩阵R和平移矩阵t，然后由欧拉角与旋转矩阵的关系可得：

$\left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\ψ}\cos \mathrm{\φ}+\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}& \sin \mathrm{\ψ}\sin \mathrm{\φ}-\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}\\ -\sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\φ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\ψ}\sin \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}\\ \sin \mathrm{\θ}& -\cos \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}\end{array}\right]=\begin{array}{c}\left[\begin{array}{ccc}{\tilde{r}}_1& {\tilde{r}}_2& {\tilde{r}}_3\end{array}\right]\end{array}$

式中，无人机的俯仰角θ、滚转角φ、偏航角ψ分别为θ=arcsinr₃₁，φ=arctan(-r₃₂/r₃₃)，ψ=arctan(-r₂₁/r₁₁)；位置估计(11)用于计算无人机所处位置，已知着陆地标的原点在VegaPrime坐标系中的坐标为(x,y,z)和姿态估计(10)所求得的平移向量(t_x,t_y,t_z)，由坐标系转关关系知无人机位置为(x-t_x,y+t_y,z+t_z)；O_vX_vY_vZ_v为VegaPrime坐标系，O_uX_uY_uZ_u为无人机坐标系，O_cX_cY_cZ_c为摄像机坐标系，O_wX_wY_wZ_w为着陆地标坐标系，uv为图像像素坐标系，其中VegaPrime坐标系与着陆地标坐标系X轴平行同向，Y、Z轴平行反向，摄像机坐标系与无人机坐标系原点始终重合且任何时间不存在旋转和平移；在无人机姿态角为0时，摄像机坐标系与着陆地标坐标系三轴平行同向，定义摄像机坐标系X轴绕着陆地标坐标系X轴转过的角度θ为俯仰角且逆时针为正，摄像机坐标系Y轴绕着陆地标坐标系Y轴转过的角度φ为滚转角且逆时针为正，摄像机坐标系Z轴绕着陆地标坐标系Z轴转过的角度ψ为偏航角且逆时针为正。

所述的一种无人机视觉自主着陆仿真验证系统，还包括对摄像机模块和无人机模块进行模块配置数据初始化模块，初始化模块将摄像机模块内参矩阵与着陆地标角点物理坐标传送至姿态估计；摄像机模块拍摄地标图像并发送图像数据到角点检测，同时通过多窗口显示将摄像机模块所拍摄到画面呈现在屏幕上；角点检测对摄像机模块传送的图像数据进行图像处理提取着陆地标角点像素坐标将结果交由角点排序；角点排序对所得到着陆地标角点进行排序，与物理坐标系下着陆地标各角点一一对应并将角点序列传送至姿态估计；姿态估计使用摄像机内参矩阵、着陆地标角点物理坐标和着陆地标角点像素坐标计算摄像机坐标系相对于着陆地标坐标系的旋转矩阵R和平移矩阵t，并利用欧拉角与旋转矩阵关系得到无人机姿态估计值；位置估计使用姿态估计所计算得到的平移矩阵和系统坐标系转换关系计算得到无人机位置估计值；最后显示模块对由无人机模块传送的位姿真实值和由姿态估计、位置估计传送的位姿估计值进行对比显示。

一种无人机视觉自主着陆仿真验证方法，至少包括如下步骤：

步骤501：开始无人机自主着陆视觉导航仿真验证过程；

步骤502：载入acf配置文件，包括地形、无人机模型、着陆地标模型；

步骤503：初始化VegaPrime各模块，初始化摄像机模块内参矩阵，载入着陆地标角点物理坐标，地形场景渲染；

步骤504：基于系统时间控制无人机飞行，并提取无人机位姿值传至步骤513；

步骤505：通过系统时间判断无人机是否到达着陆地标上空，若是则执行步骤506，否则执行步骤504，继续控制飞行；

步骤506：获取摄像机拍摄的实时图像数据；

步骤507：通过阈值分割，轮廓提取，并保留单连通闭合且长度为图像周长的0.5倍到1.5倍的轮廓得到地标的轮廓；

步骤508：对步骤507获得的地标轮廓使用DP算法进行多边形拟合；

步骤509：对步骤508的拟合结果提取角点，结果为像素坐标系下y坐标最小的点为初始点，其余各点依轮廓顺时针排列，并保留每个角点的像素坐标；

步骤510：判断步骤508得到的角点排列方式，当a、b两点之间距离小于a、l两点之间距离时步骤509所得排序即为正确顺序，当a、b两点之间距离大于a、l两点之间距离时需要对所有点重新排序，遍历轮廓，取x坐标最小的点为初始点，其余各点按轮廓顺时针依次排序；

步骤511：使用步骤510得到的角点像素坐标、初始化得到的角点物理坐标和摄像机内参矩阵求解无人机位姿估计值；

步骤512：将步骤511得到的无人机位姿估计值输出至显示模块；

步骤513：将步骤504获取的无人机位姿真实值传输至显示模块；

步骤514：结合步骤512与513所得数据，将无人机位姿真实值和估计值以列表的形式对比显示；

步骤515：判断着陆阶段是否完成，如果是，则无人机自主着陆视觉导航仿真验证过程结束，否则转至步骤504；

步骤516：无人机自主着陆视觉导航仿真验证过程结束。

其中虚线框内为VegaPrime程序的帧循环，程序正常执行后将始终在虚线框内执行，其余步骤只执行一次。

本发明设计了一种无人机视觉自主着陆仿真验证系统，其优点在于：利用VegaPrime和可视化技术，模拟了真实的无人机飞行环境，具有沉浸式的真实感，解决了无人机自主着陆视景仿真摄像机模拟的问题，将视觉着陆算法与VegaPrime结合，通过多通道展示整个着陆阶段并实时的计算和显示视觉着陆导航所需数据，可以很好的验证视觉导航算法的可行性和减少飞行试验次数。该系统可以有效的对着陆地标进行检测，能够直观、实时的显示位姿的估计值，实现了仿真验证的功能，能为实际工程应用提供了一个良好的仿真平台。

附图说明

图1无人机视觉自主着陆仿真验证系统结构图；

图2模块间数据交换关系图；

图3角点排序结果图；

图4系统坐标系关系图；

图5无人机视觉自主着陆的仿真程序流程图；

图6视觉仿真效果图；

图7位置值对比图，其中蓝色曲线为真实值，红色曲线为估计值；

图8姿态值对比图，其中蓝色曲线为真实值，红色曲线为估计值；

图中：1、VegaPrime模块，2、着陆地标检测模块，3、无人机位姿估计模块，4、无人机模块，5、着陆地标模块，6、摄像机模块，7、显示模块，8、角点检测，9、角点排序，10、姿态估计，11、位置估计，12、初始化模块。

具体实施方式

无人机的整个着陆过程分为几个阶段，本发明假设无人机依靠GPS或其他导航设备引导其飞行至着陆地标上空，并且着陆地标总是在摄像机图像范围内。通过VegaPrime观察者模拟了机载摄像机功能，并利用VegaPrime能够设置无人机位姿的特点，通过无人机姿态估计算法的反解成功获取了模拟摄像机的内参矩阵。使用Creator三维建模工具设计了一个已知尺寸的H型着陆地标。在此基础上，使用VegaPrime和视觉算法结合，设计并实现了一种无人机视觉自主着陆仿真验证系统应用。

如图1所示，一种无人机视觉自主着陆仿真验证系统，其特征是：至少包括VegaPrime模块1、着陆地标检测模块2、无人机位姿估计模块3，VegaPrime模块1用于展示无人机着陆的视景，着陆地标检测模块2用于对当前帧图像中进行图像处理，使用DP多边形拟合算法对着陆地标进行多边形拟合并提取角点，对所得角点重新排序与着陆地标对应后将结果传至无人机位姿估计模块3；无人机位姿估计模块3用于利用Courtney的方法得到无人机相对于着陆地标的旋转矩阵和平移矩阵，并通过旋转矩阵与欧拉角的关系得到无人机的三个姿态角，实时估计无人机位姿。

所述VegaPrime模块1，至少包括无人机模块4、着陆地标模块5、摄像机模块6、显示模块7，无人机模块4中使用了Vegaprime模型库中的阿帕奇直升机，并在直升机中加入了前视摄像机和着陆用摄像机；着陆地标模块5中使用Creator建模工具建立了一个长5米，宽3米的H型地标；正视地标时，左上角角点定为地标坐标系原点，坐标为（0,0），其余各点顺时针排序，坐标依次为（1,0）、（1,2）、（2,2）、（2,0）、（3,0）、（3,5）、（2，5）（2,3）、（1，3）、（1,5）、（0,5）；摄像机模块6为着陆用摄像机，其位置固定于无人机中心，垂直向下拍摄，根据一般机载摄像头视场角范围设置该观察者的水平FOV为30度，垂直FOV为20度，拍摄视频大小为360*240像素；通过无人机姿态估计算法反解求得模拟摄像机的内参矩阵，通过配置，将无人机位置调整到着陆地标原点正上方，且3个姿态角均为零，此时无人机相对于着陆地标的旋转矩阵R为单位阵，平移矩阵t=(0 0 h)^T；在不同高度h下截取摄像头所拍摄到的地标图像，并通过图像处理得到图像中的12个角点像素坐标，分别将12个角点的物理坐标和像素坐标以及旋转矩阵、平移矩阵带入公式:

$\{\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)\left(\begin{array}{ccc}0& 0& 1\end{array}\right)-A\}\left(R_{3\×3}{t}_{3\×1}\right)\left(\begin{array}{c}x\\ y\\ 0\\ 1\end{array}\right)=0$

解此方程组便可求得摄像机内参矩阵A，其中 $A=\left(\begin{array}{ccc}\frac{f}{d_x}& 0& u_0\\ 0& \frac{f}{d_y}& v_0\\ 0& 0& 1\end{array}\right)$ 为摄像机内参矩阵，u、v为角点像素坐标，x、y为角点物理坐标，R为旋转矩阵，t为平移矩阵,f为摄像机焦距，d_x为u方向上单位像素的物理尺寸，d_y为v方向上单位像素的物理尺寸，u_n为图像中心点u方向像素坐标，v₀为图像中心点v方向像素坐标；显示模块7用于显示着陆系统仿真验证的最终结果，并根据仿真的目的设计需要的界面，展示视觉着陆导航的整个过程，在多窗口上显示摄像机拍摄的图像，给出无人机真实位姿与估计值的比较输出。

如图3所示，所述着陆地标检测模块2，至少包括角点检测8、角点排序9，角点检测8首先对显示模块的摄像机图像进行灰度化处理，利用阈值分割的方法分离出地标区域，然后经过轮廓提取，并保留单连通闭合且长度为图像周长的0.5倍到1.5倍的轮廓得到地标的轮廓，对该轮廓进行DP多边形拟合后提取角点，保留角点像素坐标；角点排序9用于对角点检测8所得到的角点进行排序，角点检测8所保留的角点顺序为像素坐标系下v坐标最小的点为初始点，其余各点顺时针依次排列；当角点检测8所得图像角点ａ、ｂ两点之间距离小于ａ、ｌ两点之间距离时角点检测8所得排序即为正确顺序，当角点检测8所得图像角点ａ、ｂ两点之间距离大于ａ、ｌ两点之间距离时需要对所有点重新排序，遍历轮廓，取图像像素坐标系下u坐标最小的点为初始点，其余各点按顺时针依次排序。角点排序9所得图像角点最终排序结果和物理坐标系下地标各角点一一对应。

如图4所示，所述无人机位姿估计模块3，至少包括姿态估计10、位置估计11，姿态估计10利用Courtney的方法，已知提前求得的摄像机内参矩阵A、地标12个角点物理坐标(x_i,y_i)和图像像素坐标(u_i,v_i)，并带入公式：

$\{\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)\left(\begin{array}{ccc}0& 0& 1\end{array}\right)-A\}\left(R_{3\×3}{t}_{3\×1}\right)\left(\begin{array}{c}x\\ y\\ 0\\ 1\end{array}\right)=0$

得到一个超定方程组，通过SVD奇异值分解法解此超定方程组得到摄像机坐标系相对于着陆地标坐标系的旋转矩阵R和平移矩阵t，然后由欧拉角与旋转矩阵的关系可得：

$\left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\ψ}\cos \mathrm{\φ}+\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}& \sin \mathrm{\ψ}\sin \mathrm{\φ}-\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}\\ -\sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\φ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\ψ}\sin \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}\\ \sin \mathrm{\θ}& -\cos \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}\end{array}\right]=\begin{array}{c}\left[\begin{array}{ccc}{\tilde{r}}_1& {\tilde{r}}_2& {\tilde{r}}_3\end{array}\right]\end{array}$

式中，无人机的俯仰角θ、滚转角φ、偏航角ψ分别为θ=arcsinr₃₁，φ=arctan(-r₃₂/r₃₃)，ψ=arctan(-r₂₁/r₁₁)；位置估计11用于计算无人机所处位置，已知着陆地标的原点在VegaPrime坐标系中的坐标为(x,y,z)和姿态估计10所求得的平移向量(t_x,t_y,t_z)，由坐标系转关关系知无人机位置为(x-t_x,y+t_y,z+t_z)；O_vX_vY_vZ_v为VegaPrime坐标系，O_uX_uY_uZ_u为无人机坐标系，O_cX_cY_cZ_c为摄像机坐标系，O_wX_wY_wZ_w为着陆地标坐标系，uv为图像像素坐标系，其中VegaPrime坐标系与着陆地标坐标系X轴平行同向，Y、Z轴平行反向，摄像机坐标系与无人机坐标系原点始终重合且任何时间不存在旋转和平移；在无人机姿态角为0时，摄像机坐标系与着陆地标坐标系三轴平行同向，定义摄像机坐标系X轴绕着陆地标坐标系X轴转过的角度θ为俯仰角且逆时针为正，摄像机坐标系Y轴绕着陆地标坐标系Y轴转过的角度φ为滚转角且逆时针为正，摄像机坐标系Z轴绕着陆地标坐标系Z轴转过的角度ψ为偏航角且逆时针为正。

如图2所示，一种无人机视觉自主着陆仿真验证系统还包括初始化模块12，初始化模块12对摄像机模块6和无人机模块4进行模块配置数据初始化，将摄像机模块内参矩阵与着陆地标角点物理坐标传送至姿态估计10；摄像机模块6拍摄地标图像并发送图像数据到角点检测8，同时通过多窗口显示将摄像机模块6所拍摄到画面呈现在屏幕上；角点检测8对摄像机模块4传送的图像数据进行图像处理提取着陆地标角点像素坐标将结果交由角点排序9；角点排序9对所得到着陆地标角点进行排序，与物理坐标系下着陆地标各角点一一对应并将角点序列传送至姿态估计10；姿态估计10使用摄像机内参矩阵、着陆地标角点物理坐标和着陆地标角点像素坐标计算摄像机坐标系相对于着陆地标坐标系的旋转矩阵R和平移矩阵t，并利用欧拉角与旋转矩阵关系得到无人机姿态估计值；位置估计11使用姿态估计10所计算得到的平移矩阵和系统坐标系转换关系计算得到无人机位置估计值；最后显示模块7对由无人机模块4传送的位姿真实值和由姿态估计10、位置估计11传送的位姿估计值进行对比显示。

如图5所示，一种无人机视觉自主着陆仿真验证方法，至少包括如下步骤：

步骤501：开始无人机自主着陆视觉导航仿真验证过程；

步骤502：载入acf配置文件，包括地形、无人机模型、着陆地标模型；

步骤503：初始化VegaPrime各模块，初始化摄像机模块内参矩阵，载入着陆地标角点物理坐标，地形场景渲染；

步骤504：基于系统时间控制无人机飞行，并提取无人机位姿值传至步骤513；

步骤505：通过系统时间判断无人机是否到达着陆地标上空，若是则执行步骤506，否则执行步骤504，继续控制飞行；

步骤506：获取摄像机拍摄的实时图像数据；

步骤507：通过阈值分割，轮廓提取，并保留单连通闭合且长度为图像周长的0.5倍到1.5倍的轮廓得到地标的轮廓；

步骤508：对步骤507获得的地标轮廓使用DP算法进行多边形拟合；

步骤509：对步骤508的拟合结果提取角点，结果为像素坐标系下y坐标最小的点为初始点，其余各点依轮廓顺时针排列，并保留每个角点的像素坐标；

步骤510：判断步骤508得到的角点排列方式，当ａ、ｂ两点之间距离小于ａ、ｌ两点之间距离时步骤509所得排序即为正确顺序，当ａ、ｂ两点之间距离大于ａ、ｌ两点之间距离时需要对所有点重新排序，遍历轮廓，取x坐标最小的点为初始点，其余各点按轮廓顺时针依次排序；

步骤511：使用步骤510得到的角点像素坐标、初始化得到的角点物理坐标和摄像机内参矩阵求解无人机位姿估计值；

步骤512：将步骤511得到的无人机位姿估计值输出至显示模块；

步骤513：将步骤504获取的无人机位姿真实值传输至显示模块；

步骤514：结合步骤512与513所得数据，将无人机位姿真实值和估计值以列表的形式对比显示；

步骤515：判断着陆阶段是否完成，如果是，则无人机自主着陆视觉导航仿真验证过程结束，否则转至步骤504；

步骤516：无人机自主着陆视觉导航仿真验证过程结束。

其中虚线框内为VegaPrime程序的帧循环，程序正常执行后将始终在虚线框内执行，其余步骤只执行一次。

视觉仿真效果如图6所示，该仿真系统可以实时显示机载摄像机所拍摄的图像，展示无人机整个着陆过程，并且能将无人机真实位姿值和估计值对比实时输出，这样我们就可以直观的测试和分析视觉导航算法的可行性。

位姿估计仿真结果如图7、图8所示，对比位姿真实值与估计值曲线可知，估计值基本符合真实情况，在高度为40米时无人机已基本调整好着陆姿态，且位置误差不超过1米，姿态误差在2度以内，仿真结果证明了该系统的有效性和正确性。

本实施例没有详细叙述的部件和结构属本行业的公知部件和常用结构或常用手段，这里不一一叙述。

Claims (6)

1.一种无人机视觉自主着陆仿真验证系统，其特征是：至少包括VegaPrime模块（1）、着陆地标检测模块（2）、无人机位姿估计模块（3），VegaPrime模块（1）用于展示无人机着陆的视景，着陆地标检测模块（2）用于对当前帧图像中进行图像处理，使用DP多边形拟合算法对着陆地标进行多边形拟合并提取角点，对所得角点重新排序与着陆地标对应后将结果传至无人机位姿估计模块（3）；无人机位姿估计模块（3）用于利用Courtney的方法得到无人机相对于着陆地标的旋转矩阵和平移矩阵，并通过旋转矩阵与欧拉角的关系得到无人机的三个姿态角，实时估计无人机位姿。

2.根据权利要求1所述的一种无人机视觉自主着陆仿真验证系统，其特征是：所述VegaPrime模块（1）至少包括无人机模块（4）、着陆地标模块（5）、摄像机模块（6）、显示模块（7），无人机模块（4）中使用了Vegaprime模型库中的阿帕奇直升机，并在直升机中加入了前视摄像机和着陆用摄像机；着陆地标模块（5）中使用Creator建模工具建立了一个长5米，宽3米的H型地标；正视地标时，左上角角点定为地标坐标系原点，坐标为（0,0），其余各点顺时针排序，坐标依次为（1,0）、（1,2）、（2,2）、（2,0）、（3,0）、（3,5）、（2，5）（2,3）、（1，3）、（1,5）、（0,5）；摄像机模块（6）为着陆用摄像机，其位置固定于无人机中心，垂直向下拍摄，根据一般机载摄像头视场角范围设置该观察者的水平FOV为30度，垂直FOV为20度，拍摄视频大小为360*240像素；通过无人机姿态估计算法反解求得模拟摄像机的内参矩阵，通过配置，将无人机位置调整到着陆地标原点正上方，且3个姿态角均为零，此时无人机相对于着陆地标的旋转矩阵R为单位阵，平移矩阵t=(0 0 h)^T；在不同高度h下截取摄像头所拍摄到的地标图像，并通过图像处理得到图像中的12个角点像素坐标，分别将12个角点的物理坐标和像素坐标以及旋转矩阵、平移矩阵带入公式:

$\{\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)\left(\begin{array}{ccc}0& 0& 1\end{array}\right)-A\}\left(R_{3\×3}{t}_{3\×1}\right)\left(\begin{array}{c}x\\ y\\ 0\\ 1\end{array}\right)=0$

解此方程组便可求得摄像机内参矩阵A，其中 $A=\left(\begin{array}{ccc}\frac{f}{d_x}& 0& u_0\\ 0& \frac{f}{d_y}& v_0\\ 0& 0& 1\end{array}\right)$ 为摄像机内参矩阵，u、v为角点像素坐标，x、y为角点物理坐标，R为旋转矩阵，t为平移矩阵，f为摄像机焦距，d_x为u方向上单位像素的物理尺寸，d_y为v方向上单位像素的物理尺寸，u_n为图像中心点u方向像素坐标，v₀为图像中心点v方向像素坐标；显示模块（7）用于显示着陆系统仿真验证的最终结果，并根据仿真的目的设计需要的界面，展示视觉着陆导航的整个过程，在多窗口上显示摄像机拍摄的图像，给出无人机真实位姿与估计值的比较输出。

3.根据权利要求1所述的一种无人机视觉自主着陆仿真验证系统，其特征是：所述着陆地标检测模块（2），至少包括角点检测（8）、角点排序（9），角点检测（8）首先对显示模块的摄像机图像进行灰度化处理，利用阈值分割的方法分离出地标区域，然后经过轮廓提取，并保留单连通闭合且长度为图像周长的0.5倍到1.5倍的轮廓得到地标的轮廓，对该轮廓进行DP多边形拟合后提取角点，保留角点像素坐标；角点排序（9）用于对角点检测（8）所得到的角点进行排序，角点检测（8）所保留的角点顺序为像素坐标系下v坐标最小的点为初始点，其余各点顺时针依次排列；当角点检测（8）所得图像角点a、b两点之间距离小于a、l两点之间距离时角点检测(8)所得排序即为正确顺序，当角点检测(8)所得图像角点a、b两点之间距离大于a、l两点之间距离时需要对所有点重新排序，遍历轮廓，取图像像素坐标系下u坐标最小的点为初始点，其余各点按顺时针依次排序；角点排序(9)所得图像角点最终排序结果和物理坐标系下地标各角点一一对应。

4.根据权利要求1所述的一种无人机视觉自主着陆仿真验证系统，其特征是：所述无人机位姿估计模块(3)，至少包括姿态估计(10)、位置估计(11)，姿态估计(10)利用Courtney的方法，已知提前求得的摄像机内参矩阵A、12个地标角点物理坐标(x_i,y_i)和图像像素坐标(u_i,v_i)，并带入公式：

$\{\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)\left(\begin{array}{ccc}0& 0& 1\end{array}\right)-A\}\left(R_{3\×3}{t}_{3\×1}\right)\left(\begin{array}{c}x\\ y\\ 0\\ 1\end{array}\right)=0$

得到一个超定方程组，通过SVD奇异值分解法解此超定方程组得到摄像机坐标系相对于着陆地标坐标系的旋转矩阵R和平移矩阵t，然后由欧拉角与旋转矩阵的关系可得：

$\left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\ψ}\cos \mathrm{\φ}+\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}& \sin \mathrm{\ψ}\sin \mathrm{\φ}-\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}\\ -\sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\φ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\ψ}\sin \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}\\ \sin \mathrm{\θ}& -\cos \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}\end{array}\right]=\begin{array}{c}\left[\begin{array}{ccc}{\tilde{r}}_1& {\tilde{r}}_2& {\tilde{r}}_3\end{array}\right]\end{array}$

式中，无人机的俯仰角θ、滚转角φ、偏航角ψ分别为θ=arcsinr₃₁，φ=arctan(-r₃₂/r₃₃)，ψ=arctan(-r₂₁/r₁₁)；位置估计(11)用于计算无人机所处位置，已知着陆地标的原点在VegaPrime坐标系中的坐标为(x,y,z)和姿态估计(10)所求得的平移向量(t_x,t_y,t_z)，由坐标系转关关系知无人机位置为(x-t_x,y+t_y,z+t_z)；O_vX_vY_vZ_v为VegaPrime坐标系，O_uX_uY_uZ_u为无人机坐标系，O_cX_cY_cZ_c为摄像机坐标系，O_wX_wY_wZ_w为着陆地标坐标系，uv为图像像素坐标系，其中VegaPrime坐标系与着陆地标坐标系X轴平行同向，Y、Z轴平行反向，摄像机坐标系与无人机坐标系原点始终重合且任何时间不存在旋转和平移；在无人机姿态角为0时，摄像机坐标系与着陆地标坐标系三轴平行同向，定义摄像机坐标系X轴绕着陆地标坐标系X轴转过的角度θ为俯仰角且逆时针为正，摄像机坐标系Y轴绕着陆地标坐标系Y轴转过的角度φ为滚转角且逆时针为正，摄像机坐标系Z轴绕着陆地标坐标系Z轴转过的角度ψ为偏航角且逆时针为正。

5.根据权利要求1所述的一种无人机视觉自主着陆仿真验证系统，其特征是：包括对摄像机模块(6)和无人机模块(4)进行模块配置数据初始化模块(12)，初始化模块（12）将摄像机模块内参矩阵与着陆地标角点物理坐标传送至姿态估计（10）；摄像机模块（6）拍摄地标图像并发送图像数据到角点检测（8），同时通过多窗口显示将摄像机模块（6）所拍摄到画面呈现在屏幕上；角点检测（8）对摄像机模块（4）传送的图像数据进行图像处理提取着陆地标角点像素坐标将结果交由角点排序（9）；角点排序（9）对所得到着陆地标角点进行排序，与物理坐标系下着陆地标各角点一一对应并将角点序列传送至姿态估计（10）；姿态估计（10）使用摄像机内参矩阵、着陆地标角点物理坐标和着陆地标角点像素坐标计算摄像机坐标系相对于着陆地标坐标系的旋转矩阵R和平移矩阵t，并利用欧拉角与旋转矩阵关系得到无人机姿态估计值；位置估计（11）使用姿态估计（10）所计算得到的平移矩阵和系统坐标系转换关系计算得到无人机位置估计值；最后显示模块（7）对由无人机模块（4）传送的位姿真实值和由姿态估计（10）、位置估计（11）传送的位姿估计值进行对比显示。

6.一种无人机视觉自主着陆仿真验证方法，至少包括如下步骤：

步骤501：开始无人机自主着陆视觉导航仿真验证过程；

步骤502：载入acf配置文件，包括地形、无人机模型、着陆地标模型；

步骤503：初始化VegaPrime各模块，初始化摄像机模块内参矩阵，载入着陆地标角点物理坐标，地形场景渲染；

步骤504：基于系统时间控制无人机飞行，并提取无人机位姿值传至步骤513；

步骤505：通过系统时间判断无人机是否到达着陆地标上空，若是则执行步骤506，否则执行步骤504，继续控制飞行；

步骤506：获取摄像机拍摄的实时图像数据；

步骤507：通过阈值分割，轮廓提取，并保留单连通闭合且长度为图像周长的0.5倍到1.5倍的轮廓得到地标的轮廓；

步骤508：对步骤507获得的地标轮廓使用DP算法进行多边形拟合；

步骤509：对步骤508的拟合结果提取角点，结果为像素坐标系下y坐标最小的点为初始点，其余各点依轮廓顺时针排列，并保留每个角点的像素坐标；

步骤510：判断步骤508得到的角点排列方式，当a、b两点之间距离小于a、l两点之间距离时步骤509所得排序即为正确顺序，当a、b两点之间距离大于a、l两点之间距离时需要对所有点重新排序，遍历轮廓，取x坐标最小的点为初始点，其余各点按轮廓顺时针依次排序；

步骤511：使用步骤510得到的角点像素坐标、初始化得到的角点物理坐标和摄像机内参矩阵求解无人机位姿估计值；

步骤512：将步骤511得到的无人机位姿估计值输出至显示模块；

步骤513：将步骤504获取的无人机位姿真实值传输至显示模块；

步骤514：结合步骤512与513所得数据，将无人机位姿真实值和估计值以列表的形式对比显示；

步骤515：判断着陆阶段是否完成，如果是，则无人机自主着陆视觉导航仿真验证过程结束，否则转至步骤504；

步骤516：无人机自主着陆视觉导航仿真验证过程结束;

其中虚线框内为VegaPrime程序的帧循环，程序正常执行后将始终在虚线框内执行，其余步骤只执行一次。

Images