视觉辅助微型无人机室内飞行三维姿态算法制造技术

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

3D attitude algorithm of vision assisted micro UAV in indoor flight

【技术实现步骤摘要】
视觉辅助微型无人机室内飞行三维姿态算法
本专利技术涉及无人机
，特别是涉及一种视觉辅助微型无人机室内飞行三维姿态算法。
技术介绍
微型无人机由于其体积小、重量轻和隐蔽的特点，非常适用于室内环境的飞行应用，如室内侦查、搜救和环境探测等。但微型无人机也因为自身体积、重量和计算能力的限制，无法采用体积、重量和功耗有更高要求的导航传感器以及计算量大的视觉导航算法来实现室内自主飞行。同时，室内磁场环境复杂，磁力计在室内应用受到很大限制，无人机的姿态解算将会受到干扰、精度降低，甚至航向角解算错误。因此，微型无人机室内飞行时，三维姿态角的解算也是一个需要解决的关键问题。
技术实现思路
本专利技术所要解决的技术问题是：为了克服现有技术中的不足，本专利技术提供一种视觉辅助微型无人机室内飞行三维姿态算法。本专利技术解决其技术问题所要采用的技术方案是：一种视觉辅助微型无人机室内飞行三维姿态算法，包含磁场正常环境下无人机三维姿态角的解算，在解算过程中对水平姿态角和航向角解算进行解耦；磁场异常环境下无人机三维姿态角的解算；以及走廊飞行视本文档来自技高网...

【技术保护点】
1.一种视觉辅助微型无人机室内飞行三维姿态算法，其特征在于：包含磁场正常环境下无人机三维姿态角的解算，在解算过程中对水平姿态角和航向角解算进行解耦；磁场异常环境下无人机三维姿态角的解算；以及走廊飞行视觉辅助航向角修正；具体包括以下步骤：/nS1：无人机起飞前机头朝向与室内任一走廊方向平行，上电初始化；获取IMU和三轴磁力计输出数据；同时，视觉模块利用狭道检测方法对前方廊道开始检测，并计算无人机偏离廊道线的角度；/nS2：根据磁力计输出计算出起飞点地磁感应强度，将地磁感应强度数值与设定范围比较，当起飞点地磁感应强度在设定范围以内，则判定为飞行环境磁场正常，以磁力计给出的磁航向作为初始航向角，进入...

【技术特征摘要】
1.一种视觉辅助微型无人机室内飞行三维姿态算法，其特征在于：包含磁场正常环境下无人机三维姿态角的解算，在解算过程中对水平姿态角和航向角解算进行解耦；磁场异常环境下无人机三维姿态角的解算；以及走廊飞行视觉辅助航向角修正；具体包括以下步骤：
S1：无人机起飞前机头朝向与室内任一走廊方向平行，上电初始化；获取IMU和三轴磁力计输出数据；同时，视觉模块利用狭道检测方法对前方廊道开始检测，并计算无人机偏离廊道线的角度；
S2：根据磁力计输出计算出起飞点地磁感应强度，将地磁感应强度数值与设定范围比较，当起飞点地磁感应强度在设定范围以内，则判定为飞行环境磁场正常，以磁力计给出的磁航向作为初始航向角，进入飞行状态，并执行S3；当起飞点地磁感应强度在设定范围以外，则判定为飞行环境磁场异常，将初始航向角设为0，进入飞行状态，并执行S4；
S3：无人机在磁场正常环境下飞行过程中，计算飞行中的地磁感应强度，并判断飞行途中地磁感应强度是否在设定范围内，如果不在设定范围内则判定为飞行环境磁场由正常转为异常，进入步骤S4；如果在设定范围内，则利用IMU与磁力计共9轴传感器对无人机三维姿态角进行测量，并对水平姿态角解算和航向角解算进行解耦，然后进入步骤S5；
S4：只利用IMU数据解算姿态四元数和三维姿态角，然后进入步骤S5；
S5：读取步骤S1中视觉模块输出的无人机偏离廊道线的角度，判断是否有视觉航向测量更新，如果有更新，则进行EKF数据融合实现航向角修正并输出三维姿态；如果无更新，则直接输出三维姿态角。


2.如权利要求1所述的视觉辅助微型无人机室内飞行三维姿态算法，其特征在于：步骤S1中计算无人机偏离廊道线角度的过程具体为：
无人机在走廊飞行时，通过前视单目相机对前方廊道进行拍照获得前视图像，并将前视图像发送至图像处理计算机，图像处理计算机利用前视图像通过Hough变换实现狭道检测，从而得到像素坐标系中廊道线的直线方程；再结合无人机俯仰角数据以及相机标定得到的内参数，通过三角关系，计算出无人机偏离廊道线的角度。


3.如权利要求1所述的视觉辅助微型无人机室内飞行三维姿态算法，其特征在于：步骤S2中判断磁场是否正常包括以下步骤：
步骤S2.1：对磁力计的输出进行标定，获得正常地磁感应强度HE，并将HE±5％作为地磁感应强度的设定范围，将该设定范围保存在无人机中；
步骤S2.2：无人机上电运行后，计算运行中三轴磁力计输出的平方和的算术平方根，得到飞行环境中三轴磁力计测量的磁感应强度H；
步骤S2.3：无人机将三轴磁力计测量的磁感应强度H与地磁感应强度的设定范围HE±5％进行比较，当H在HE±5％范围内则磁力计数据可用；H不在HE±5％范围内则磁力计数据不可用。


4.如权利要求3所述的视觉辅助微型无人机室内飞行三维姿态算法，其特征在于：步骤S2.1中磁力计的标定方法为：在室内磁场干扰小的开阔空间中，采用水平360°旋转标定方法对磁力计进行标定，完成标定后，在标定地点，计算标定后的三轴磁力计输出的平方和的算术平方根，即为正常地磁感应强度HE，并将HE±5％作为地磁感应强度的设定范围。


5.如权利要求1所述的视觉辅助微型无人机室内飞行三维姿态算法，其特征在于：步骤S3中利用IMU与磁力计共9轴传感器对无人机三维姿态角进行测量，并对水平姿态角解算和航向角解算进行解耦，计算过程如下：
将IMU中加速度计输出进行归一化处理，记为[axkaykazk]T，计算当前k时刻角速度校正量eak＝[eakxeakyeakz]T：



其中，[001]T代表归一化重力加速度，为导航坐标系到载体坐标系的姿态矩阵；
对磁力计输出进行归一化处理，记为[mxkmykmzk]T，计算当前k时刻角速度校正量emk＝[emkxemkyemkz]T：



其中，[mEmNmU]T为归一化的当地地理坐标系下的地磁矢量；
利用eak与emk元素重组得k时刻角速度校正量ek，该角速度校正量可以实现水平姿态和航向角解算的解耦：



利用ek对陀螺仪角速度进行校正，得到k时刻校正后的陀螺仪角速度输出[gxkgykgzk]T计算：



其中，[gxgygz]T为IMU中陀螺仪输出的原始角速度数据；Ki为积分系数；Kp为比例系数；
利用校正后的陀螺仪角速度采用龙格库塔法解四元数微分方程实现姿态四元数q0～q3的更新，从而解算出无人机三维姿态。

【专利技术属性】
技术研发人员：李德辉，王冠林，史海庆，邓成端，
申请(专利权)人：北京智新翼行科技有限公司，
类型：发明
国别省市：北京;11