基于视觉信息的无人飞行器自主着陆的姿态参数估算方法.pdf

一种基于视觉信息的无人飞行器自主着陆的姿态参数估算方法，包括以下步骤：1)假设无人飞行器沿着直线下滑，对于单目视觉信息，基于时间处理计算运动矢量、估计膨胀中心，根据几何灭点模型估计俯仰角；2)利用双目立体视觉信息，基于特征点匹配形成稀疏的视差图，进一步估计UAV到地面特征点的深度信息，结合俯仰角参数，通过摄像机坐标到世界坐标的转换获得UAV相对地面的高度；3)建立无人飞行器运动模型，基于Kalman滤波进一步提高UAV高度参数估计精度。本发明提供一种有效提高可靠性的基于视觉信息的无人飞行器自主着陆的姿态参数估算方法。

1、  一种基于视觉信息的无人飞行器自主着陆姿态参数估算方法，其特征在于：所述估算方法包括以下步骤：
1)俯仰角估计：
设置双目摄像机与无人飞行器的机体平行，摄像机坐标系与无人飞行器的坐标系重合，假设无人飞行器沿着直线下滑，无人飞行器位于在离地面高H′的位置，其飞行方向与地面成α角，俯仰角α通过公式(1)求得：
α = tan - 1 H f p - tan - 1 L f p = tan - 1 f p ( H - L ) f p 2 + HL - - - ( 1 ) ]]>
上式中，f_p为摄像机的焦距，H为FOVC点和C点之间的距离，L为FOE点和FOVC点之间的距离，FOVC是摄像机视野区EF的中心，FOE为膨胀中心，C点是像平面内FOE点和FOVC点连线与平面B的灭线的交点；
2)深度估计：
采用双目立体视觉方法测量摄像机到地面特征点的深度，从双目图像中找到匹配点，形成视差图，根据公式(2)求出深度参数
Y_c＝bf_c/D   (2)
其中，Y_c是深度，b是两个摄像机之间的基线长度，f_c是摄像机的焦距，D是双目视差。

2、  如权利要求1所述的基于视觉信息的无人飞行器自主着陆姿态参数估算方法，其特征在于：所述估算方法还包括以下步骤：
3)高度估计：
在摄像机坐标系下获得相关参数，通过坐标变换得到UAV相对地面的高度信息，从摄像机坐标系到世界坐标系的转换关系见公式(3)：
x w y w z w = 1 0 0 0 cos α - sin α 0 sin α cos α x c y c z c - - - ( 3 ) ]]>
其中，α是UAV的俯仰角，x_c，y_c，z_c是UAV在摄像机坐标系下的位置，x_w，y_w，z_w是UAV在世界坐标系下的位置，所求的UAV高度H′即公式中的z_w：
H′＝z_w＝y_c×sin α+z_c×cos α   (4)
利用基于特征点的匹配方法形成稀疏的视差图，就由公式(2)求得UAV相对各特征点深度信息，再通过公式(4)将每个特征点求得的高度进行统计平均，获得该时刻UAV的高度参数。

3、  如权利要求1或2所述的基于视觉信息的无人飞行器自主着陆姿态参数估算方法，其特征在于：在所述步骤2)中，利用Kalman滤波器提高高度参数估计精度的过程为：
设UAV运动为直线运动，定义状态矢量为：
y_n＝[y_p(n)，y_v(n)，y_a(n)]^T   (5)
式中y_p(n)为第n个采样时刻UAV的高度，y_v(n)为第n个采样时刻UAV的速度，而y_a(n)为第n个采样时刻UAV的加速度；
根据牛顿运动定理，分别建立系统模型和测量模型，系统模型：
y_n＝Ay_n-1+ω_n
A = 1 T T 2 / 2 0 1 T 0 0 1 - - - ( 6 ) ]]>
式中，下标n为第n个采样时刻，y_n为第n个采样时刻的状态矢量，A为状态转移矩阵，T为采样时间间隔，ω_n为第n个采样时刻的系统噪声矢量；
x_n＝Hy_n+v_n
测量模型：                   (7)；
H＝[1，0，0]
式中x_n为第n个采样时刻的测量值，即UAV的高度，H为输出矩阵，v_n为第n个采样时刻的测量噪声矢量。

基于视觉信息的无人飞行器自主着陆的姿态参数估算方法
技术领域
本发明涉及无人飞行器技术领域，尤其是一种无人飞行器(Unmanned Aerial Vehicle，UAV)导航技术，侧重视觉信息的UAV姿态参数估计。
背景技术
无人飞行器用于航空摄影，灾情监测以及地球物理探矿等，有着广泛的应用前景。UAV自主着陆是指UAV依赖机载的导航设备和飞行控制系统来进行定位导航并最终控制UAV降落在着陆场的过程。要想实现自主着陆，UAV必须具备自主导航能力，即需要实时估计其姿态参数以用于自主着陆导航。
目前国内外研究的用于UAV自主着陆的导航技术包括：惯性导航系统(INS)、GPS导航系统和INS/GPS组合导航系统。其中，惯性导航是最早最成熟的导航技术，它是利用陀螺，加速度机等惯性元器件感受UAV在运动过程中的加速度，然后通过积分计算，得到机体大概位置与速度等导航参数，它最大的缺点是误差会随时间的推移而不断累加；GPS应用最为广泛，技术也相对成熟，它利用卫星进行导航定位，具有精度高，使用简单等优点，但在山区等特殊地方易阻塞。对于小型UAV，由于负荷有限，视觉导航系统是理想选择，一方面其体积小，另一方面功耗低，同时对着陆地形有直观的感知。
对于UAV，视觉导航系统侧重飞行路径的规划、障碍物的检测等方面，并且多数算法基于单目视觉信息的感知与理解。随着机器视觉的发展，立体视觉的研究成果逐渐用于工业智能化领域。双目立体视觉用于UAV处于起步阶段。
发明内容
为了克服已有无人飞行器的导航系统的可靠性差的不足，本发明提供一种有效提高可靠性的基于视觉信息的无人飞行器自主着陆的姿态参数估算方法。
为解决上述技术问题的技术方案是：
一种基于视觉信息的无人飞行器自主着陆姿态参数估算方法，所述估算方法包括以下步骤：
1)、俯仰角估计：
设置双目摄像机与无人飞行器的机体平行，摄像机坐标系与无人飞行器的坐标系重合，假设无人飞行器沿着直线下滑，无人飞行器位于在离地面高H′的位置，其飞行方向与地面成α角，俯仰角α通过公式(1)求得：
α = tan - 1 H f p - tan - 1 L f p = tan - 1 f p ( H - L ) f p 2 + HL - - - ( 1 ) ]]>
上式中，f_p为摄像机的焦距，H为FOVC点和C点之间的距离，L为FOE点和FOVC点之间的距离，FOVC是摄像机视野区EF的中心，FOE为膨胀中心，C点是像平面内FOE点和FOVC点连线与平面B的灭线的交点；
2)深度估计：
采用双目立体视觉方法测量摄像机到地面特征点的深度，从双目图像中找到匹配点，形成视差图，根据公式(2)求出深度参数
Y_c＝bf_c/D              (2)
其中，Y_c是深度，b是两个摄像机之间的基线长度，f_c是摄像机的焦距，D是双目视差。
作为优选的一种方案：所述估算方法还包括以下步骤：
3)高度估计：
在摄像机坐标系下获得相关参数，通过坐标变换得到UAV相对地面的高度信息，从摄像机坐标系到世界坐标系的转换关系见公式(3)：
x w y w z w = 1 0 0 0 cos α - sin α 0 sin α cos α x c y c z c - - - ( 3 ) ]]>
其中，α是UAV的俯仰角，x_c，y_c，z_c是UAV在摄像机坐标系下的位置，x_w，y_w，z_w是UAV在世界坐标系下的位置，所求的UAV高度H′即公式中的z_w：
H′＝z_w＝y_c×sinα+z_c×cosα            (4)
利用基于特征点的匹配方法形成稀疏的视差图，就由公式(2)求得UAV相对各特征点深度信息，再通过公式(4)将每个特征点求得的高度进行统计平均，获得该时刻UAV的高度参数。
作为优选的另一种方案：在所述步骤2)中，利用Kalman滤波器提高高度参数估计精度的过程为：
设UAV运动为直线运动，定义状态矢量为：
y_n＝[y_p(n)，y_v(n)，y_a(n)]^T          (5)
式中y_p(n)为第n个采样时刻UAV的高度，y_v(n)为第n个采样时刻UAV的速度，而y_a(n)为第n个采样时刻UAV的加速度；
根据牛顿运动定理，分别建立系统模型和测量模型，系统模型：
y_n＝Ay_n-1+ω_n
A = 1 T T 2 / 2 0 1 T 0 0 1 - - - ( 6 ) ]]>
式中，下标n为第n个采样时刻，y_n为第n个采样时刻的状态矢量，A为状态转移矩阵，T为采样时间间隔，ω_n为第n个采样时刻的系统噪声矢量；
             x_n＝Hy_n+v_n
测量模型：                                    (7)；
             H＝[1，0，0]
式中x_n为第n个采样时刻的测量值，即UAV的高度，H为输出矩阵，v_n为第n个采样时刻的测量噪声矢量。
本发明的技术效果：基于视觉信息的感知与理解为无人飞行器提供自主着陆所需的导航信息。假设无人飞行器沿着直线下滑，对于单目视觉信息，基于时间处理计算运动矢量、估计膨胀中心，根据几何灭点模型估计俯仰角。利用双目立体视觉信息，基于特征点匹配形成稀疏的视差图，进一步估计UAV到地面特征点的深度信息，结合俯仰角参数，通过摄像机坐标到世界坐标的转换获得UAV相对地面的高度。建立无人飞行器运动模型，基于Kalman滤波进一步提高UAV高度参数估计精度。
附图说明
图1是UAV俯仰角测量模型图。
图2是RANSAC算法拟合灭点图。
图3是立体视觉模型示意图。
图4是俯仰角估计结果的示意图。
图5是UAV高度估计结果的示意图。
具体实施方式
下面结合附图对本发明作进一步描述。
参照图1—图5，基于视觉信息的无人飞行器自主着陆的姿态参数估计方法具体实现包括以下部分：
1.俯仰角估计
设置双目摄像机与无人飞行器的机体平行，摄像机坐标系与无人飞行器的坐标系重合，假设无人飞行器沿着直线下滑，无人飞行器位于在离地面高H′的位置，其飞行方向与地面成α角，参考图1和图2。俯仰角α通过公式(1)求得：
α = tan - 1 H f p - tan - 1 L f p = tan - 1 f p ( H - L ) f p 2 + HL - - - ( 1 ) ]]>
上式中，f_p为摄像机的焦距，H为FOVC点和C点之间的距离，L为FOE点和FOVC点之间的距离，FOVC是摄像机视野区EF的中心，FOE为膨胀中心，C点是像平面内FOE点和FOVC点连线与平面B的灭线的交点；
因此，只要确定了L、H和f_p就可以得到俯仰角，f_p可以通过摄像机标定得到，得到H和L的关键是提取出灭线和膨胀中心。
根据透视学理论，水平面的灭线即为视平线。UAV下降俯冲的过程中如果没有左右摇晃，而且两台摄像机被固定在同一水平面上，则两台摄像机的光心距地面的高度(视高)是相同的，那么两台摄像机的视平线共线；水平面上任一平行直线投影在像平面后形成的灭点落在视平线上。因此，可以分别提取左右图中的灭点来确定灭线。一组平行线具有相同的灭点，相互平行的道路边界和斑马线在2D图像中交于一个灭点，通过改进型的Hough变换提取道路中的边缘，利用RANSAC(RANdom SAmple Consensus)算法拟合出鲁棒性高的灭点，如图2所示。
根据运动视觉理论，当人们经过静止景物时，景物在视网膜上的投影好像向后流动，事实上，对给定的平移运动方向和给定的注视方向来说，景物的视觉投影似乎是从一个特定的视网膜点流出来，这个点就是膨胀中心FOE。由于固定翼无人机在下降过程中速度很快，位移较大，选用特征点的运动矢量法计算FOE。
2.深度估计
采用双目立体视觉方法测量摄像机到地面特征点的深度，从双目图像中找到匹配点，形成稀疏的视差图，根据公式(2)求出深度参数
Y_c＝bf_c/D                 (2)
其中，Y_c是深度，b是两个摄像机之间的基线长度，f_c是摄像机的焦距，D是双目视差，且D＝x_l′-x_r′，x_l′表示P_A与P_l之间的距离，x_r′表示P_B与P_r之间的距离，见图3。
3.高度估计
在摄像机坐标系下获得相关参数，需要进一步通过坐标变换，得到UAV相对地面的高度信息。从摄像机坐标系到世界坐标系的转换关系见公式(3)。
x w y w z w = 1 0 0 0 cos α - sin α 0 sin α cos α x c y c z c - - - ( 3 ) ]]>
其中，α是UAV的俯仰角，x_c，y_c，z_c是UAV在摄像机坐标系下的位置，x_w，y_w，z_w是UAV在世界坐标系下的位置。因此，所求的UAV高度H′即公式中的z_w：
H′＝z_w＝y_c×sinα+z_c×cosα             (4)
利用基于特征点的匹配方法形成稀疏的视差图，就可由公式(2)求得UAV相对各特征点深度信息，进一步通过公式(4)将每个特征点求得的高度进行统计平均，获得该时刻UAV的高度参数。
4.利用Kalman滤波器提高高度参数估计精度
设UAV运动为直线运动，定义状态矢量为
y_n＝[y_p(n)，y_v(n)，y_a(n)]^T        (5)
式中：y_p(n)为第n个采样时刻UAV的高度，y_v(n)为第n个采样时刻UAV的速度，而y_a(n)为第n个采样时刻UAV的加速度。根据牛顿运动定理，分别建立系统模型和测量模型。
系统模型：
y_n＝Ay_n-1+ω_n
A = 1 T T 2 / 2 0 1 T 0 0 1 - - - ( 6 ) ]]>
式中下标n为第n个采样时刻，y_n为第n个采样时刻的状态矢量，A为状态转移矩阵，T为采样时间间隔，ω_n为第n个采样时刻的系统噪声矢量。
测量模型：
x_n＝Hy_n+v_n
                                      (7)
H＝[1，0，0]
式中x_n为第n个采样时刻的测量值，即UAV的高度，H为输出矩阵，v_n为第n个采样时刻的测量噪声矢量。
模拟UAV着陆的仿真实验平台模型，实际实验构架由双目摄像机(JVC1481)，时钟同步卡(GL202)，图像采集卡(Daheng CQ300)，位移台和主机(主频p43.06GHz，内存512M)组成。将摄像机固定在位移台上，使摄像机从位移台的顶端平移至底端，模拟UAV着陆时摄像机的运动，在下滑过程中，利用双目视觉系统采集道路视频数据。
图4为姿态角估计结果，横坐标的帧数与时间对应，从中可以看出算法估计的结果与真实值24.5°相吻合。平均误差为0.66°。图5是对高度估计的仿真结果，虚线为Harris特征点匹配的结果，实线为Kalman滤波器滤波后结果，滤波后平均误差：1.57cm，可以看出，由于部分特征点失配和系统误差引起的高度偏差已被Kalman滤波器有效的抑制了。
无人直升机实验平台携带嵌入式立体视觉数据采集设备。在飞行过程中，图像数据以5帧/秒的速率存入CF卡中。由于无人机航模本身的局限性，在飞行过程中直升机基本处于水平运动，其俯仰角可视为0⁰，因此，只对直升机的高度参数进行估计。在这样的前提条件下，本发明开发的算法基于直升机空中实拍图像序列估计出直升机的高度值为10.848m。直升机实际飞行高度在10m到11m米之间，因此，本实施例开发的算法是有效的。