一种基于视觉的无人机着陆姿态估计方法与流程

1.本发明涉及无人机，尤其是一种基于视觉的无人机着陆姿态估计方法。


背景技术：

2.目前无人机的姿态估计通常需要使用卫星导航系统，卫星导航系统非常适合中分辨率导航要求，具有rtk功能的更先进的单元提供几英寸量级的精度，甚至在完美的条件下提供更好的精度。然而，这种装置的性能取决于卫星的可用性、动态条件，甚至地理特征。卫星导航系统容易受到干扰，主要来自密集的云层、太阳耀斑以及如树木和建筑物等永久性障碍物，不满足细粒度导航任务的性能要求，例如无人机在跑道、直升机停机坪或网中着陆。此外，无人机经常降落在偏远地区，显然需要着陆辅助设备在不同的天气条件下准确检测跑道，迫切需要对无人机的着陆姿态进行实时监控。


技术实现要素：

3.本发明为了克服现有技术方案的不足，提供了一种基于视觉的无人机着陆姿态估计方法。
4.为了实现上述目的，本发明的技术方案为：一种基于视觉的无人机着陆姿态估计方法，使用聚类技术来识别跑道标记，使用边缘检测器和特征检测变换来验证标记是否位于恢复目标，使用优化器来将检测到的标记与三维空间中的坐标进行匹配，使用非线性变换和投影解算器来观察摄像机的位置和方向，使用扩展卡尔曼滤波器获取最优姿态估计。
5.控制机载摄像机的视场角或变焦，机载摄像机安装在万向架上，无论无人机的方向或位置如何，万向架都将机载摄像机的视角定向在跑道方向。
6.将目标图像、角标记颜色、边缘颜色和跑道目标几何形状作为输入，机载摄像机将提供图像，姿态估计将被传递给摄像机控制器和自动着陆控制器，前者用于调整视场角。
7.步骤1、确定坐标系和图像几何约束,
8.控制机载摄像机的视场角或变焦，机载摄像机安装在万向架上，无论无人机的方向或位置如何，万向架都将机载摄像机的视角定向在跑道方向；
9.将目标图像、角标记颜色、边缘颜色和跑道目标几何形状作为输入，机载摄像机将提供图像，姿态估计将被传递给摄像机控制器和自动着陆控制器，前者用于调整视场角；
10.步骤1包括如下步骤：
11.步骤1.1，归一化；
12.步骤1.2，建立视场角等式；
13.步骤2、跑道识别；
14.在给定图像中选择最佳二维四边形并将目标图像中的四个最终二维点与世界坐标系中的四个三维点相关联；
15.步骤2包括如下步骤：
16.步骤2.1、图像对比；
17.步骤2.2、聚类计算；
18.步骤2.3、边缘检测；
19.计算出梯度大小和方向后，进行如下步骤:
20.步骤2.3.1、确定阈值梯度大小，产生边缘候选二值图像,算法中使用10％总强度的恒定值；
21.步骤2.3.2、将边缘方向离散成45度的面元；
22.步骤2.3.3、对于来自步骤2.3.1的图像中的每个像素，如果该像素位于具有一致方向的区域中，则认为是边缘；
23.步骤2.4、特征检测变换；
24.步骤2.4、还包括如下步骤：
25.步骤2.4.1、将特征检测的映射初始化为全零；
26.步骤2.4.2、每当在图像空间中遇到非零元素时，沿着特征检测变换图中相应的正弦曲线增加值以及最强相关的线位于特征检测变换的峰值处，进行图像的特征检测变换；
27.步骤2.5、图像过滤；
28.步骤2.6、二维图像坐标到三维世界坐标的优化；
29.步骤3、姿态观测；
30.步骤4、最优姿态估计；
31.利用扩展卡尔曼滤波和当前姿态观测值，结合无人机姿态和动力学的先验知识，获得无人机真实姿态的近似最优估计的算法；
32.步骤4包括如下步骤：
33.步骤4.1,系统动态学；
34.步骤4.2，预测校正。
35.本发明与现有技术相比，其有益效果体现在：用于增强和备份基于卫星导航系统的系统，该方法可用于固定翼常规着陆、旋翼直升机停机坪着陆或固定翼网着陆。此外，它可以适用于可选驾驶无人机、飞行员训练设备，甚至其他机器人平台。使用该系统的要求是良好的照明条件、恢复目标清晰的边缘、恢复目标角落的单色标记以及恢复目标物理尺寸和位置的知识。
36.在实际的测试中，该方法的准确性确保了横向误差在2米以内，纵向误差在0.4米以内，随着跑道进入更大的视野，误差逐渐减小到接近0，其精度水平超过了卫星导航系统。俯仰和滚转的精度保持在0.1度以内，摆动角具有大约-0.75度的一致偏差，远远优于其他方法。算法的准确性已经在一组2米宽的跑道上的得到验证，横向误差在1.2厘米以内，纵向误差在1厘米以内，当跑道进入更大视野时，高度误差表现出接近0，对于小的下滑角，高度灵敏度比横向位置灵敏度低得多，位置估计误差很小，可以归因于系统误差，该系统误差涉及在使用跟踪设备拍照时在机载摄像机上呈现不对称的力。欧拉角的误差比合成试验中的要大一些，滚转误差可达1.2度，俯仰误差可达0.6度。同样，航向角呈现出轻微的负偏差。
附图说明
37.图1为本发明的流程框图；
具体实施方式
38.下面结合附图与实施例对本发明作进一步的说明。
39.一种基于视觉的无人机着陆姿态估计方法，该方法用于增强或支持卫星导航系统，作为无人机着陆期间的主要导航手段，使用聚类技术来识别跑道标记，使用边缘检测器和特征检测变换来验证标记是否位于恢复目标，使用优化器来将检测到的标记与三维空间中的坐标进行匹配，使用非线性变换和投影解算器来观察摄像机的位置和方向，使用扩展卡尔曼滤波器改进姿态估计。
40.该方法所提出的算法给出了比现有视觉系统更好的姿态估计。
41.该方法包括：
42.步骤1、确定坐标系和图像几何约束,
43.控制机载摄像机的视场角或变焦，机载摄像机安装在万向架上，无论无人机的方向或位置如何，万向架都将机载摄像机的视角定向在跑道方向。
44.将目标图像、角标记颜色、边缘颜色和跑道目标几何形状作为输入，机载摄像机将提供图像，姿态估计将被传递给摄像机控制器和自动着陆控制器，前者用于调整视场角。
45.步骤1包括如下步骤：
46.步骤1.1，归一化
47.显示用于图像几何的约束，表明垂直和水平两个方向上视场角的约束，以及用于在垂直和水平方向上归一化区间[-1，1]上的每个图像的归一化界面坐标，归一化等式如下：
[0048][0049][0050]
其中，x
ndc
为归一化界面x坐标，y
ndc
为归一化界面y坐标，x
img
为图像x坐标，y
img
为图像y坐标w为图像宽度，h为图像高度
[0051]
步骤1.2，建立视场角等式，
[0052]
建立x和y方向的视场角等式，
[0053][0054][0055]
其中，fov
x
为图像x方向上的机载摄像机视场角，fovy图像y方向上的机载摄像机视场角，d为分隔近平面和远平面的距离，
[0056]
y方向上的视场根据x方向上的视场和图像纵横比来求解，有助于约束之间进行转换，如下所示
[0057]
[0058]
其中，为图像纵横比；
[0059]
步骤2、跑道识别
[0060]
在给定图像中选择最佳二维四边形并将目标图像中的四个最终二维点与世界坐标系中的四个三维点相关联；
[0061]
步骤2包括如下步骤：
[0062]
步骤2.1、图像对比
[0063]
基于感兴趣的颜色从目标图像计算高对比度图像，生成两个和方误差图像，一个和方误差来自目标标记颜色，一个和方误差来自目标边缘颜色。
[0064]
大大减少了算法后期必须解决的空间误差问题。
[0065]
计算图像i中由《r，g，b》定义的目标颜色的和方误差，如下所示：
[0066][0067]
其中，表示第i行第j列像素的目标整体颜色的和方误差，表示第i行第j列像素的红色分量，表示第i行第j列像素的绿色分量，表示第i行第j列像素的蓝色分量，r，g，b为分别为图像i整体红色、绿色和蓝色的均值。
[0068]
步骤2.2、聚类计算
[0069]
根据步骤2.1确定目标图像，并找出目标图像的哪些像素属于连续的候选标记，并返回每个结果聚类的中心坐标，聚类的子集将以二维屏幕坐标呈现，其坐标被映射到三维世界坐标并被传递给姿态估计器，有以下输入:来自目标标记颜色的图像阈值，聚类的搜索半径，输出一组具有x和y图像空间坐标的聚类。
[0070]
对目标图像进行阈值化，迭代通过每个超过阈值的像素，如果该像素在一个聚类的附近，在该聚类中任何其他像素的搜索半径内，则该像素被添加到该聚类，如果该像素不属于现有的聚类，则创建一个新的聚类，其中像素是唯一的成员，像素都从阈值集中按照阈值集中移除，计算每个聚类的平均值；
[0071]
步骤2.3,边缘检测
[0072]
边缘检测定位目标图像，边缘检测依靠目标图像的梯度大小和梯度方向来定位边缘，将高斯与导数结合使用，以减少生成的边缘图中的噪声。边缘检测的输入是高斯算子使用的标准偏差值、线强度使用的最小阈值以及目标图像本身。二维高斯函数g(x,y)由如下等式给出。
[0073][0074]
其中，分别为高斯平滑器在x、y方向的标准偏差，
[0075]
取x和y的偏导数，得到如下等式：
[0076][0077][0078]
其中，g
x
(x,y)，gy(x,y)分别为二维高斯函数g(x,y)在x、y偏导函数，
[0079]
将图像i(x,y)与由g(x,y)的两个偏导数生成的图像进行卷积，以生成x方向和y方向上的图像梯度，使用卷积定理，卷积由如下等式给出，
[0080][0081][0082]
*表示卷积，为傅里叶变换，ωυ分别为与x,y向对应的傅立叶变换中的频率,
[0083]
分别对x和y方向上的二维高斯偏导数进行傅里叶变换得到等式，如下所示，
[0084][0085][0086]
总梯度大小i
gm
(x,y)和梯度方向i
gd
(x,y)由如下等式给出，
[0087][0088]igd
(x,y)＝arctan2((i(x,y)*gy(x,y)，(i(x,y)*g
x
(x,y))
[0089]
计算出梯度大小和方向后，进行如下步骤:
[0090]
步骤2.3.1,确定阈值梯度大小，产生边缘候选二值图像,算法中使用10％总强度的恒定值；
[0091]
步骤2.3.2,将边缘方向离散成45度的面元；
[0092]
步骤2.3.3,对于来自步骤2.3.1的图像中的每个像素，如果该像素位于具有一致方向的区域中，则认为是边缘；
[0093]
步骤2.4特征检测变换
[0094]
对步骤2.3边缘检测的输出执行特征检测变换，特征检测变换针对二进制图像，并返回一个图像，该图像指示图像中特征最显著的线的位置，通过将图像空间中的每个点视为特征检测空间中的正弦曲线来处理，特征检测空间中具有许多正弦曲线交点的区域对应于图像空间中的强相关线，图像空间中的点到特征检测空间中的正弦曲线的映射由如下等式给出：
[0095]
ρ＝xcosφ+ysinφ
[0096]
其中，ρ为特征检测变换中的半径，φ为特征检测变换中的角方向；
[0097]
步骤2.4还包括如下步骤：
[0098]
步骤2.4.1,将特征检测的映射初始化为全零；
[0099]
步骤2.4.2,每当在图像空间中遇到非零元素时，沿着特征检测变换图中相应的正弦曲线增加值以及最强相关的线位于特征检测变换的峰值处，进行图像的特征检测变换；
[0100]
步骤2.5图像过滤
[0101]
为了减少图像中候选标记的数量，使用图像过滤来检查标记与强边缘的接近度，给定候选标记a，通过图像过滤，存在标记b，使得由ab形成的线c基本上与跑道边缘共线。
[0102]
图像过滤具有以下输入:一组输入候选标记、特征检测变换、特征检测变换中的φandρvalues范围、归一化间隔[0，1]上的最小线强度以及以像素为单位的搜索半径。
[0103]
由图像空间中的两个候选标记形成的线通过找到以下等式组的解而在特征检测空间中找到:
[0104]
x1cosφ+y1sinφ＝ρ
[0105]
x2cosφ+y2sinφ＝ρ
[0106]
x
1 y1和x
2 y2分别为两个候选标所在坐标，
[0107]
改写为:
[0108][0109]
给出如下解:
[0110]
b＝a-1c[0111]
φ＝cot-1
(b)
[0112]
ρ＝bsin(φ)
[0113]
步骤2.6二维图像坐标到三维世界坐标的优化，
[0114]
在执行姿态观察之前，算法的最后一步是将前一步中的二维聚类坐标与真实的三维世界坐标进行匹配。该步骤依赖于先验知识来执行优化。给定先前的映射新的最佳映射最小化了前一组二维点和新的一组二维点之间的平方和距离，保持如下式所示的顺序，
[0115][0116]
表示第k个期望簇的二维图像坐标中的跑道拐角位置向量集，表示第k个期望簇的三维图像坐标中的跑道拐角位置向量集，表示第k步第i个点的向量，n是候选簇的数量，k是期望簇的数量，那么找到最佳结果所需的排列是优化o(n！)次，该算法极大地减少了这个阶段必须解决的问题的运算量。
[0117]
步骤3、姿态观测
[0118]
根据二维四边形和相应的三维几何形状估计欧拉角和局部位置，姿态观测算法负
责获取姿态的初始估计值、机载摄像机的纵横比和视场角、归一化界面坐标列表以及这些点的相应物理坐标，并获得观测者姿态的最佳估计值，
[0119]
常数是:ar，机载摄像机的长宽比；fovy，摄像机的视野角度；动态输入为:二维聚类坐标与真实的三维世界坐标的映射，观测者姿态的初始估计、观测者方向的三个欧拉角、从图像原点开始的三维位移向量。
[0120]
建立方程组，使用欧拉角和平移向量将三维点投影到二维图像上，机载摄像机安装在无人机重心处，垂直放置，z轴指向无人机前方，x轴指向右舷翼展方向，通过旋转或平移矩阵用来解释无人机重心和机载摄像机位置之间的差异，所用的变换矩阵假设x方向最初指向右侧，y方向指向上方，初始化变换矩阵需要考虑必要的旋转，必要的旋转是围绕y轴和x轴旋转90度进行的，如下式所示：
[0121][0122]
其中，ry为y轴旋转矩阵r
x
为x轴旋转矩阵
[0123]
欧拉旋转矩阵是航空航天坐标系的标准，如等式所示，
[0124][0125][0126]rφ
为滚动角旋转矩阵；φ为滚动角；r
θ
为俯仰角旋转矩阵θ为俯仰角；r
ψ
为航向角旋转矩阵；ψ为航向角；
[0127]
平移矩阵t将摄像机移动到原点，如下所示：
[0128][0129]
其中，t为平移矩阵；b
x b
y bz分别为世界坐标系中的机载摄像机机身x、y、z坐标
[0130]
等式给出了投影矩阵p，z
near
和z
far
参数，区间[-1，1]上的归一化z坐标表示观察体积内的几何形状。由于没有可用的深度信息，并且不使用z坐标，该信息对于投影矩阵p有如下表示：
[0131][0132]
p为投影矩阵，z
near
和z
far
分别为远场和近场姿态观察，将所有这些矩阵以适当的顺序组合起来，得到：
[0133]
m＝p
×ry
×rx
×rφ
×rθ
×rψ
×
t
[0134]
m为变换矩阵，就可以将对象坐标(即世界坐标系中的三维跑道坐标)变换为裁剪坐标，如下式所示
[0135][0136]
x
clip y
clip z
clip
为裁剪坐标，w
clip
为裁剪w坐标，x
obj y
obj z
obj
为对象坐标；
[0137]
通过投影操作将裁剪坐标转换为归一化界面坐标，将物体坐标正向投影到观察平面上，如下式所示：
[0138][0139]
得到的非线性等式组用牛顿法以自适应步长求解，最终得到的等式是非常非线性的，在总体上是局部线性的，计算雅可比矩阵，如下式所示：
[0140][0141]
雅可比矩阵j中的12个唯一项中的每一个进行分析计算，通过计算雅可比，将应用等式收敛到一个解，该点对应的世界坐标，迭代该等式，直到收敛达到合理的容差；
[0142][0143]
κ为自适应步长常数，基于方程组近似局部线性，变换等式在任意欧拉角的π/6弧度变化范围内和任意距离变化范围内大致呈线性，寻找接近初始条件的解，并从整体上提高求解器的稳定性，κ的表达式在如下所示：
[0144][0145]
求解该系统只需要三个二维点，推广到过约束系统，产生最小二乘意义上的最优解，从而提高解的精度以及算法实现的收敛特性，使用雅可比矩阵j
[0146]
的奇异值分解来计算，j的奇异值分解如下所示：
[0147]j+
＝vσ
+ut
[0148]
其中，j
+
为j的奇异值,u和v分别为酉矩阵，σ是对角矩阵，σ
+
是通过取矩阵每个对角元素的标量倒数给出的，用于求解系统在区间[3，∞]上的二维点的数量；
[0149]
步骤4、最优姿态估计
[0150]
利用扩展卡尔曼滤波和当前姿态观测值，结合无人机姿态和动力学的先验知识，获得无人机真实姿态的近似最优估计的算法；
[0151]
步骤4包括如下步骤：
[0152]
步骤4.1,系统动态学
[0153]
无人机动力学等式式被设计用于模拟运动，目标是提供一个模拟平台，用于模拟无人机的轨迹，而不会被真实无人机动力学的细节所拖累。使用一组简化的动态特性，更容易调整扩展卡尔曼滤波器。
[0154]
所讨论的动力学集合是使用以下假设开发的:无人机总是朝着无人机机头的方向飞行:即没有迎角或侧滑角；无人机的总前进速度是恒定的。三个控制输入直接和瞬时影响欧拉角。无人机在进场时的轨迹用这组动力学来模拟，该算法可以跟踪飞行中运动物体的轨迹。则系统动力学等式如下式所示。
[0155][0156]
其中分别为φ、θ、ψ、b
x
、by、by的一阶导数，u为无人机总速度，u
φ
为滚动速率控制输入；u
θ
为俯仰速率控制输入；u
ψ
为横摆率控制输入δt为间隔时间；
[0157]
步骤4.2，预测校正
[0158]
包括两个步骤:预测步骤和校正步骤，
[0159]
预测步骤通过如下等式实现：
[0160][0161][0162]
其中，为第k步的最佳姿态估计；γk为第k步的为第k步的输入矢量；pk为第k步的透视矩阵φk为第k步的透视矩阵qk为过程噪声协方差；(-)表示当前步的负向侧。
[0163]
最佳卡尔曼增益k使用如下公式计算.然后使用相应等式执行校正，
[0164][0165][0166][0167]kk
为第k步的卡尔曼增益，hk为第k步的卡尔曼观察矩阵，rk为第k步的测量噪声协方差矩阵；(+)表示当前步的正向侧zk为第k步的姿态观测向量，
[0168]
下式实现了转移矩阵的离散化，
[0169][0170]
测量矩阵hk如下等式所示形成,
[0171][0172]
其中，h为测量函数
[0173]
由于上述的输出为欧拉角和笛卡尔坐标，因此没有对u的直接观测。因此，离散测量等式变成等式
[0174][0175]
其中，分别为zk、xk、vk线性离散量，
[0176]
求解q过程噪声协方差和r测量噪声协方差，q和r是如下所示等式中的常数矩阵，在无限长的时间内，过程噪声的方差是测量噪声的1/10，并且所有噪声完全不相关，会产生良好的结果，
[0177][0178]
在每次测试中，算法给出的观测值从未差于横向位置跑道总长度的0.48％或垂直位置跑道总长度的0.44％。事实上，当机载摄像机接近跑道时，观测通常会变得更好，卡尔曼滤波器大大改善了系统的跟踪。将图像精度数字应用于大约457.2米的更典型的无人机跑道长度，并且在所有其他条件相同的情况下，那么在最坏的情况下，位置精度可以预期在1.5米之内，并且当无人机接近阈值时，通过更精确的标记、对标记质心检测的轻微修改以及对真实视场角的更好了解，甚至可以实现更好的位置估计。这种水平的位置精度对于无人机自动着陆来说肯定是足够的。
[0179]
以上所述实施方式仅表达了本发明的一种实施方式，但并不能因此而理解为对本发明范围的限制。应当指出，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。