[发明专利]视觉辅助微型无人机室内飞行三维姿态算法

说明书

本发明提供一种视觉辅助微型无人机室内飞行三维姿态算法，可以实现无人机室内飞行时俯仰角、横滚角和航向角的纯自主解算。当磁力计数据有效时，利用IMU和磁力计的数据实现无人机三维姿态的解算，并实现水平姿态角解算和航向角解算的解耦；当磁力计数据失效时，仅用IMU数据实现三维姿态解算，并切断加速度计对航向角解算的影响，确保航向角解算在无磁力计辅助时维持续航时间内精度要求；当无人机在室内走廊飞行时，通过前视单目相机模块实现狭道检测，提取出无人机偏离廊道线的角度，并用此偏离角通过扩展卡尔曼滤波实现对无人机航向角累积误差的修正。

技术领域

本发明涉及无人机技术领域，特别是涉及一种视觉辅助微型无人机室内飞行三维姿态算法。

背景技术

微型无人机由于其体积小、重量轻和隐蔽的特点，非常适用于室内环境的飞行应用，如室内侦查、搜救和环境探测等。但微型无人机也因为自身体积、重量和计算能力的限制，无法采用体积、重量和功耗有更高要求的导航传感器以及计算量大的视觉导航算法来实现室内自主飞行。同时，室内磁场环境复杂，磁力计在室内应用受到很大限制，无人机的姿态解算将会受到干扰、精度降低，甚至航向角解算错误。因此，微型无人机室内飞行时，三维姿态角的解算也是一个需要解决的关键问题。

发明内容

本发明所要解决的技术问题是：为了克服现有技术中的不足，本发明提供一种视觉辅助微型无人机室内飞行三维姿态算法。

本发明解决其技术问题所要采用的技术方案是：一种视觉辅助微型无人机室内飞行三维姿态算法，包含磁场正常环境下无人机三维姿态角的解算，在解算过程中对水平姿态角和航向角解算进行解耦；磁场异常环境下无人机三维姿态角的解算；以及走廊飞行视觉辅助航向角修正；具体包括以下步骤：

S1：无人机起飞前机头朝向与室内任一走廊方向平行，上电初始化；获取IMU和三轴磁力计输出数据；同时，视觉模块利用狭道检测方法对前方廊道开始检测，并计算无人机偏离廊道线的角度；其中，视觉模块包括前视单目相机和图像处理计算机。

S2：根据磁力计输出计算出起飞点地磁感应强度，将地磁感应强度数值与设定范围比较，当起飞点地磁感应强度在设定范围以内，则判定为飞行环境磁场正常，以磁力计给出的磁航向作为初始航向角，进入飞行状态，并执行S3；当起飞点地磁感应强度在设定范围以外，则判定为飞行环境磁场异常，将初始航向角设为0，进入飞行状态，并执行S4；

S3：无人机在磁场正常环境下飞行过程中，计算飞行中的地磁感应强度，并判断飞行途中地磁感应强度是否在设定范围内，如果不在设定范围内则判定为飞行环境磁场由正常转为异常，进入步骤S4；如果在设定范围内，则利用IMU与磁力计共9轴传感器对无人机三维姿态角进行测量，并对水平姿态角解算和航向角解算进行解耦，然后进入步骤S5；

S4：只利用IMU数据解算姿态四元数和三维姿态角，然后进入步骤S5；

S5：读取步骤S1中视觉模块输出的无人机偏离廊道线的角度，判断是否有视觉航向测量更新，如果有更新，则进行EKF数据融合实现航向角修正并输出三维姿态；如果无更新，则直接输出三维姿态角。

进一步，步骤S1中计算无人机偏离廊道线角度的过程具体为：无人机在走廊飞行时，通过前视单目相机对前方廊道进行拍照获得前视图像，并将前视图像发送至图像处理计算机，图像处理计算机利用前视图像通过Hough变换实现狭道检测，从而得到像素坐标系中廊道线的直线方程；再结合无人机俯仰角数据以及相机标定得到的内参数，通过三角关系，计算出无人机偏离廊道线的角度。

进一步，步骤S2中判断磁场是否正常包括以下步骤：

步骤S2.1：对磁力计的输出进行标定，获得正常地磁感应强度H_E，并将H_E±5％作为地磁感应强度的设定范围，将该设定范围保存在无人机中；