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

权利要求书

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：对磁力计的输出进行标定，获得正常地磁感应强度H_E，并将H_E±5％作为地磁感应强度的设定范围，将该设定范围保存在无人机中；

步骤S2.2：无人机上电运行后，计算运行中三轴磁力计输出的平方和的算术平方根，得到飞行环境中三轴磁力计测量的磁感应强度H；

步骤S2.3：无人机将三轴磁力计测量的磁感应强度H与地磁感应强度的设定范围H_E±5％进行比较，当H在H_E±5％范围内则磁力计数据可用；H不在H_E±5％范围内则磁力计数据不可用。

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