[发明专利]微型无人机室内走廊飞行航向角误差视觉修正方法

权利要求书

1.一种微型无人机室内走廊飞行航向角误差视觉修正方法，其特征在于：包括廊道线检测、廊道线偏离角度和距离的计算以及航向角修正，具体包括以下步骤：

S1：获取无人机前方廊道场景图像；

S2：对获取的图像进行处理，利用Hough变换实现廊道线检测，得到廊道线在像素平面中像素坐标系下的直线方程；

S3：根据廊道线在像素平面内的直线方程，利用无人机导航系统给出的无人机飞行高度和俯仰角以及前视相机内参数中的f_x和f_y，通过三角关系，计算出无人机偏离廊道线的角度和距离；

S4：利用无人机偏离廊道线的角度，采用卡尔曼滤波实现航向角的修正以及偏航轴Z陀螺零偏的估计。

2.如权利要求1所述的微型无人机室内走廊飞行航向角误差视觉修正方法，其特征在于：步骤S2中廊道线在像素平面内的直线方程，计算过程如下：

步骤S2.1：采用自适应局部二值阈值检测方法，对步骤S1获取的廊道场景图像进行二值化处理，获取廊道基本信息，实现廊道初步检测；

步骤S2.2：对步骤S2.1得到的二值化后的图像进行图像形态学处理；

步骤S2.3：检测经过处理后的图像中的白色连通域，通过对其形状和大小的限定，从中选出墙面区域；

步骤S2.4：在选出的非墙面区域内，利用Hough变换实现廊道直线检测，通过对检测到的直线的角度、长度以及与墙面区域的位置关系，找出墙壁与地面的夹角线，完成廊道线的直线检测，得到廊道线在像素坐标系下的直线方程表达式为：

v＝mu+n (3)

式中，u和v分别为像素坐标系的u轴和v轴的的坐标，m为直线的斜率，n为直线在v轴上的截距。

3.如权利要求2所述的微型无人机室内走廊飞行航向角误差视觉修正方法，其特征在于：所述图像形态学处理包括膨胀、腐蚀、去噪中的一种或几种。

4.如权利要求2所述的微型无人机室内走廊飞行航向角误差视觉修正方法，其特征在于：步骤S3中无人机偏离廊道线的角度和距离计算过程具体包括以下步骤：

通过无人机导航系统获得无人机距离地面的高度h和无人机俯仰角θ，由于相机距离地面的高度与无人机距离地面的高度相等，相机俯角α满足：α＝-θ，从而确定相机距离地面的高度以及相机俯角α；

设P为廊道线上的任一点，P点在相机坐标系中的坐标为(a’，b，c)，由三角关系可得：

其中，β为无人机偏离廊道线的角度，d为无人机与廊道线的距离，a为a’的绝对值；

P’为P点在图像中对应的像素点，像素坐标为(u，v)，则根据相机小孔成像原理则有：

u＝f_xa/c，v＝f_yb/c (4)

式中，f_x和f_y为前视相机内参数矩阵中与焦距相关的参数，通过相机标定获取；

则式(3)中直线方程可表达为：

f_yb/c＝mf_xa/c+n (5)

将式(1)和(2)代入式(5)，消除a和b，仅保留c，整理后可得：

由于P点是狭道直线上的任意点，上式中对任意c值都成立，于是得到下面两个表达式：

由上式(7)可以计算出无人机偏离廊道线的角度β和距离d：

无人机偏离廊道线的角度β最后输出的极性定义为：前视相机光轴朝向走廊左侧廊道线时，β以正值输出；前视相机光轴朝向走廊右侧廊道线时，β转化为负值输出。

5.如权利要求1或4所述的微型无人机室内走廊飞行航向角误差视觉修正方法，其特征在于：步骤S4中以无人机走廊飞行中的偏离廊道线的角度提取出无人机航向的测量值，以无人机航向角以及偏航轴Z陀螺的零偏作为状态，采用卡尔曼滤波实现数据融合，具体步骤如下：

将无人机上电时朝向与廊道线重合，初始朝向的航向角为0度，即将无人机初始朝向当作是北向，以该方向为基准，重新定义室内地图的东西南方向，即给室内地图定义了一个伪东西南北方向，无人机导航系统以此坐标系进行导航，无人机导航系统中的AHRS算法解算的航向ψ，定义为北偏东为正，范围0～2π内；无人机在廊道飞行时，利用视觉计算出得到的偏离廊道线的角度β对无人机航向ψ进行修正，数据融合方法采用卡尔曼滤波，状态方程如下：

式中，为系统状态，ω_z为Z陀螺输出，ε_z为Z陀螺常值零偏，w_ωz为Z陀螺输出的随机白噪声；

量测方程为：

z＝ψ_c+v (10)

式中，z表示量测；v表示量测噪声；ψ_c为视觉计算得到的航向角，根据无人机偏离廊道线的角度β和走廊朝向确定；走廊朝向的东西南北方向依据无人机上电初始化时的朝向的走廊方向作为北向而确定：

当无人机沿着南北向走廊往北飞行时：β＞0,ψ_c＝2π-β；β＜0,ψ_c＝-β；

当无人机沿着南北向走廊往南飞行时：ψ_c＝π-β；

当无人机沿着东西向走廊往东飞行时：

当无人机沿着东西向走廊往西飞行时：

将AHRS算法得到的航向角作为卡尔曼航向修正算法中航向角的一步预测；

卡尔曼滤波矩阵如下：

Φ≈I+FT；Γ＝GT (12)

得到视觉航向角ψ_c和卡尔曼滤波矩阵后，即可利用卡尔曼滤波完成数据融合，得到校正后的无人机航向ψ和Z陀螺零偏ε_z；利用校正后的无人机航向ψ与无人机AHRS算法中得到的俯仰角θ和横滚角γ，重新计算四元数，并反馈给AHRS算法；四元数重新计算按照公式(13)：

其中，q₀、q₁、q₂、q₃表示姿态四元数。