[发明专利]基于视觉信息的无人飞行器自主着陆的姿态参数估算方法

权利要求书

1.一种基于视觉信息的无人飞行器自主着陆姿态参数估算方法，其特征在于：所述估算方法包括以下步骤：

1)俯仰角估计：

设置双目摄像机与无人飞行器的机体平行，摄像机坐标系与无人飞行器的坐标系重合，假设无人飞行器沿着直线下滑，无人飞行器位于在离地面高H′的位置，其飞行方向与地面成α角，俯仰角α通过公式(1)求得：

α=tan-1Hfp-tan-1Lfp=tan-1fp(H-L)fp2+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)：

xwywzw=1000cosα-sinα0sinαcosαxcyczc---(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=1TT2/201T001---(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个采样时刻的测量噪声矢量。