技术领域
[0001] 本
发明涉及无人机技术领域，特别是涉及一种视觉辅助微型无人机室内飞行三维姿态算法。
背景技术
[0002] 微型无人机由于其体积小、重量轻和隐蔽的特点，非常适用于室内环境的飞行应用，如室内侦查、搜救和环境探测等。但微型无人机也因为自身体积、重量和计算能
力的限制，无法采用体积、重量和功耗有更高要求的导航
传感器以及计算量大的视觉导航算法来实现室内自主飞行。同时，室内
磁场环境复杂，磁力计在室内应用受到很大限制，无人机的姿态解算将会受到干扰、
精度降低，甚至航向
角解算错误。因此，微型无人机室内飞行时，三维姿态角的解算也是一个需要解决的关键问题。
发明内容
[0003] 本发明所要解决的技术问题是：为了克服
现有技术中的不足，本发明提供一种视觉辅助微型无人机室内飞行三维姿态算法。
[0004] 本发明解决其技术问题所要采用的技术方案是：一种视觉辅助微型无人机室内飞行三维姿态算法，包含磁场正常环境下无人机三维姿态角的解算，在解算过程中对
水平姿态角和航向角解算进行解耦；磁场异常环境下无人机三维姿态角的解算；以及走廊飞行视觉辅助航向角修正；具体包括以下步骤：
[0005] S1：无人机
起飞前机头朝向与室内任一走廊方向平行，上电初始化；获取IMU和三轴磁力计输出数据；同时，视觉模
块利用狭道检测方法对前方廊道开始检测，并计算无人机偏离廊道线的角度；其中，视觉模块包括前视单目相机和
图像处理计算机。
[0006] S2：根据磁力计输出计算出起飞点地磁感应强度，将地磁感应强度数值与设定范围比较，当起飞点地磁感应强度在设定范围以内，则判定为飞行环境磁场正常，以磁力计给出的磁航向作为初始航向角，进入飞行状态，并执行S3；当起飞点地磁感应强度在设定范围以外，则判定为飞行环境磁场异常，将初始航向角设为0，进入飞行状态，并执行S4；
[0007] S3：无人机在磁场正常环境下飞行过程中，计算飞行中的地磁感应强度，并判断飞行途中地磁感应强度是否在设定范围内，如果不在设定范围内则判定为飞行环境磁场由正常转为异常，进入步骤S4；如果在设定范围内，则利用IMU与磁力计共9轴传感器对无人机三维姿态角进行测量，并对水平姿态角解算和航向角解算进行解耦，然后进入步骤S5；
[0008] S4：只利用IMU数据解算姿态四元数和三维姿态角，然后进入步骤S5；
[0009] S5：读取步骤S1中视觉模块输出的无人机偏离廊道线的角度，判断是否有视觉航向测量更新，如果有更新，则进行EKF数据融合实现航向角修正并输出三维姿态；如果无更新，则直接输出三维姿态角。
[0010] 进一步，步骤S1中计算无人机偏离廊道线角度的过程具体为：无人机在走廊飞行时，通过前视单目相机对前方廊道进行拍照获得前视图像，并将前视图像发送至图像处理计算机，图像处理计算机利用前视图像通过Hough变换实现狭道检测，从而得到
像素坐标系中廊道线的直线方程；再结合无人机
俯仰角数据以及相机标定得到的内参数，通过三角关系，计算出无人机偏离廊道线的角度。
[0011] 进一步，步骤S2中判断磁场是否正常包括以下步骤：
[0012] 步骤S2.1：对磁力计的输出进行标定，获得正常地磁感应强度HE，并将HE±5％作为地磁感应强度的设定范围，将该设定范围保存在无人机中；
[0013] 步骤S2.2：无人机上电运行后，计算运行中三轴磁力计输出的平方和的算术平方根，得到飞行环境中三轴磁力计测量的磁感应强度H；
[0014] 步骤S2.3：无人机将三轴磁力计测量的磁感应强度H与地磁感应强度的设定范围HE±5％进行比较，当H在HE±5％范围内则磁力计数据可用；H不在HE±5％范围内则磁力计数据不可用。
[0015] 进一步，步骤S2.1中磁力计的标定方法为：在室内磁场干扰小的开阔空间中，采用水平360°旋转标定方法对磁力计进行标定，完成标定后，在标定地点，计算标定后的三轴磁力计输出的平方和的算术平方根，即为正常地磁感应强度HE，并将HE±5％作为地磁感应强度的设定范围。
[0016] 进一步，步骤S3中利用IMU与磁力计共9轴传感器对无人机三维姿态角进行测量，并对水平姿态角解算和航向角解算进行解耦，计算过程如下：
[0017] 将IMU中
加速度计输出进行归一化处理，记为[axk ayk azk]T，计算当前k时刻
角速度校正量eak＝[eakx eaky eakz]T：
[0018]
[0019] 其中，[0 0 1]T代表归一化
重力加速度， 为导航坐标系到载体坐标系的姿态矩阵。
[0020] 对磁力计输出进行归一化处理，记为[mxk myk mzk]T，计算当前k时刻角速度校正量emk＝[emkx emky emkz]T：
[0021]
[0022] 其中，[mE mN mU]T为归一化的当地地理坐标系下的地磁矢量。
[0023] 利用eak与emk元素重组得k时刻角速度校正量ek，该角速度校正量可以实现水平姿态和航向角解算的解耦：
[0024]
[0025] 利用ek对
陀螺仪角速度进行校正，得到k时刻校正后的陀螺仪角速度输出[gxk gyk gzk]T计算：
[0026]
[0027] 其中，[gx gy gz]T为IMU中陀螺仪输出的原始角速度数据；Ki为积分系数；Kp为比例系数。
[0028] 利用校正后的陀螺仪角速度采用龙格库塔法解四元数微分方程实现姿态四元数q0～q3的更新，从而解算出无人机三维姿态。
[0029] 进一步，步骤S4中仅采用IMU的3轴陀螺仪和3轴加速度计数据对无人机的三维姿态角进行解算，计算过程如下：
[0030] 将公式(1)中的eakz＝0，实现抑制航向角误差的增长，即：
[0031]
[0032] 利用eak对陀螺仪角速度进行校正，得到k时刻校正后的陀螺仪角速度输出[gxk gyk gzk]T计算：
[0033]
[0034] 利用校正后的陀螺仪角速度，采用龙格库塔法解四元数微分方程实现姿态四元数q0～q3的更新，从而解算出无人机三维姿态。
[0035] 利用无人机偏离廊道线的角度，采用EKF作为数据融合工具，实现无人机航向角的修正计算过程如下：
[0036] 无人机走廊飞行时，通过Hough变换实现狭道检测，从而得到廊道线在像素平面中心为原点的像素坐标系下的直线方程：
[0037] v＝mu+n (7)
[0038] 式中，u和v分别为像素坐标系的u轴和v轴的坐标，m为直线的斜率，n为直线在v轴上的截距。
[0039] 由于无人机前飞时属于低头飞行，前视相机安装时使其光轴与无人机本体坐标系的前向平行。相机坐标系原点O为
光心，Zc轴与光轴重合指向相机前方，Xc轴指向相机本体右侧，Yc轴向下，三个坐标轴符合右手定则。相机俯角α通过无人机航姿系统计算出的俯仰角θ计算：α＝-θ；β为无人机偏离廊道线的角度，d为无人机与廊道线的距离。为不失一般性，无论相机光轴偏向走廊左侧还是右侧，设P为廊道线上的任一点，P点在相机坐标系中的坐标为(a’，b，c)。显然当相机光轴偏向于左侧走廊时，P点为左侧廊道线上的点，则a’大于0。a为a’的绝对值，由三角关系可得：
[0040]
[0041]
[0042] 在相机成像的像素平面的中心建立像素坐标系，原点为像素平面中心O’，横纵坐标轴分别为u、v。P’为P点在图像中对应的像素点，像素坐标为(u，v)，则根据相机小孔成像原理则有：
[0043] u＝fxa/c，v＝fyb/c (10)
[0044] 式中，fx和fy为前视相机内参数矩阵中与焦距相关的参数，通过相机标定获取。式(7)中直线方程可表达为：
[0045] fyb/c＝mfxa/c+n (11)
[0046] 将式(8)和(9)代入式(11)，消除a和b，仅保留c，
整理后可得：
[0047]
[0048] 由于P点是狭道直线上的任意点，上式中对任意c值都成立，于是可得：
[0049] ncosαcosβ+fy sinαcosβ-mfx sinβ＝0 (13)
[0050] 由上式可以计算出无人机偏离廊道线的角度β：
[0051]
[0052] 无人机偏离廊道线的角度β最后输出的极性定义为：前视相机光轴朝向走廊左侧廊道线时，β以正值输出；前视相机光轴朝向走廊右侧廊道线时，β转化为负值输出。
[0053] 无人机在走廊飞行时，利用视觉计算出得到的偏离廊道线的角度β对无人机航向ψ进行修正，数据融合方法采用卡尔曼滤波，
状态方程如下：
[0054]
[0055] 式中，为系统状态，ωx、ωy、ωz为陀螺仪输出角速度，q0～q3为姿态四元数，wq为系统过程噪声。
[0056] 量测方程为：
[0057]
[0058] 式中，z表示量测；v表示量测噪声；ψc为视觉计算得到的航向角，根据无人机偏离廊道线的角度β和走廊朝向确定。
[0059] 无人机航向角ψ定义为北偏西为正，0～2π范围内。当无人机起飞点地磁感应强度在设定范围以外时，无人机初始朝向的走廊方向定义为0度，即确定为伪北向，则在该飞行过程中姿态解算用到的东西南北方向均指伪东西南北方向，该情况下视觉航向测量值ψc定义为：
[0060] d)当无人机沿着南北向走廊往北飞行时：β＞0,ψc＝2π-β；β＜0,ψc＝-β；
[0061] e)当无人机沿着南北向走廊往南飞行时：ψc＝π-β；
[0062] f)当无人机沿着东西向走廊往东飞行时：
[0063] 当无人机沿着东西向走廊往西飞行时：
[0064] 当无人机起飞点地磁感应强度在设定范围以内时，整个飞行过程采用的坐标为真实的北向，需要利用无人机初始对准走廊的磁航向，以及其它走廊与初始对准走廊的几何关系，将偏离廊道线的角度β转化为北偏西为正0～2π的视觉航向测量值ψc。
[0065] 将S2和S3中得到的姿态四元数作为EKF航向修正算法中姿态四元数的一步预测。
[0066] 卡尔曼滤波矩阵如下：
[0067]
[0068]
[0069] H＝[Hq0 Hq1 Hq2 Hq3] (19)
[0070] 其中：
[0071]
[0072]
[0073]
[0074]
[0075] 连续系统离散化：
[0076] Φ≈I+FT；Γ＝GT (24)
[0077] 完成以上计算，即可利用EKF完成数据融合，得到校正后的姿态四元数，从而实现航向角的修正。
[0078] 本发明的有益效果是：本发明提供的一种视觉辅助微型无人机室内飞行三维姿态算法，实现了微型无人机室内飞行时在所有磁场环境下的三维姿态的正常解算，并且实现水平姿态角解算和航向角解算的解耦，提高了航向角的解算精度；通过计算量小的视觉狭道检测算法的结果来修正了无人机航向角的误差。本方法计算量小，精度高，适合微型无人机室内飞行姿态解算。
附图说明
[0079] 下面结合附图和
实施例对本发明作进一步说明。
[0080] 图1是本发明三维姿态解算流程示意图。
[0081] 图2是走廊飞行无人机廊道线偏离角度提取中坐标系定义示意图。
[0082] 图3是以像素平面中心为原点的像素坐标系定义示意图。
具体实施方式
[0083] 现在结合附图对本发明作详细的说明。此图为简化的示意图，仅以示意方式说明本发明的基本结构，因此其仅显示与本发明有关的构成。
[0084] 如图1所示，本发明的一种视觉辅助微型无人机室内飞行三维姿态算法，包含磁场正常环境下无人机三维姿态角的解算，在解算过程中对水平姿态角和航向角解算进行解耦；磁场异常环境下无人机三维姿态角的解算；以及走廊飞行视觉辅助航向角修正；本实施例中视觉模块采用的是前视单目相机，相机采集的图片发送至图像处理计算机进行处理。
[0085] 三维姿态算法具体包括以下步骤：
[0086] S1：无人机起飞前机头朝向与室内任一走廊方向平行，上电初始化，获取IMU和三轴磁力计输出数据；同时，前视单目相机对前方廊道开始拍照，将照片发送至图像处理计算机，利用狭道检测方法实现廊道检测，并计算无人机偏离廊道线的角度。
[0087] S2：根据磁力计输出计算出起飞点地磁感应强度，将地磁感应强度数值与设定范围比较，如果地磁感应强度在设定范围内，则判定为飞行环境磁场正常，以磁力计给出的磁航向角作为初始航向角，进入飞行状态，并执行S3；如果起飞点地磁感应强度在设定范围以外，则判定为飞行环境磁场异常，将初始航向角设为0，进入飞行状态，并执行S4；在环境磁场异常的情况下，整个飞行过程不使用磁力计数据进行姿态解算；当无人机进入到走廊飞行时，前视单目相机进行狭道检测并计算出无人机偏离廊道线的角度，用该角度对无人机航向角进行修正，执行S5。
[0088] 其中，判断磁场是否正常，即通过磁力计输出计算地磁感应强度，将地磁感应强度与设定范围进行对比，计算过程如下：
[0089] 步骤S2.1：在进行判断前首先需要对磁力计的输出进行标定，以确定设定范围的大小，标定只需进行一次，将该设定范围保存在无人机中，在进行判断时可以调用该数据；具体的标定过程为：在室内磁场干扰小的开阔空间中，采用水平360°旋转标定方法对磁力计进行标定，完成标定后，在标定地点，计算标定后的三轴磁力计输出的平方和的算术平方根，即为正常地磁感应强度HE，并将HE±5％作为地磁感应强度的设定范围；
[0090] 步骤S2.2：无人机上电运行后，计算运行中三轴磁力计输出的平方和的算术平方根，得到飞行环境中三轴磁力计测量的磁感应强度H；
[0091] 步骤S2.3：无人机将三轴磁力计测量的磁感应强度H与地磁感应强度的设定范围HE±5％进行比较，当H在HE±5％范围内则磁力计数据可用；H不在HE±5％范围内则磁力计数据不可用。
[0092] S3：无人机在磁场正常环境下飞行过程中，以磁航向作为初始航向角，计算飞行中的地磁感应强度，并判断飞行途中地磁感应强度是否在设定范围内，如果不在设定范围内则判定为飞行环境磁场由正常转为异常，进入步骤S4；如果在设定范围内，则利用IMU与磁力计共9轴传感器对无人机三维姿态角进行测量，并对水平姿态角解算和航向角解算进行解耦，然后进入步骤S5；
[0093] 其中，飞行中磁力计测量的地磁感应强度在设定范围内，且起飞点地磁感应强度也在设定范围内，将采用9轴
传感器数据计算出无人机的三维姿态角，并实现水平姿态解算和航向角解算的解耦，计算过程如下：
[0094] 将IMU中加速度计输出进行归一化处理，记为[axk ayk azk]T，计算当前k时刻角速度校正量eak＝[eakx eaky eakz]T：
[0095]
[0096] 其中，[0 0 1]T代表归一化重力加速度， 为导航坐标系到载体坐标系的姿态矩阵；
[0097] 对磁力计输出进行归一化处理，记为[mxk myk mzk]T，计算当前k时刻角速度校正量emk＝[emkx emky emkz]T：
[0098]
[0099] [mE mN mU]T为归一化的当地地理坐标系下的地磁矢量；
[0100] 利用eak与emk元素重组得k时刻角速度校正量ek，该角速度校正量可以实现水平姿态和航向角解算的解耦：
[0101]
[0102] 利用ek对陀螺仪角速度进行校正，得到k时刻校正后的陀螺仪角速度输出[gxk gyk gzk]T计算：
[0103]
[0104] 其中，[gx gy gz]T为IMU中陀螺仪输出的原始角速度数据；Ki为积分系数；Kp为比例系数；
[0105] 利用校正后的陀螺仪角速度采用龙格库塔法解四元数微分方程实现姿态四元数q0～q3的更新，从而解算出无人机三维姿态。
[0106] S4：根据步骤S2和S3的判断，当飞行中地磁感应强度在设定范围以外时，或飞行中地磁感应强度在设定范围以内，但起飞点地磁感应强度在设定范围以外时，仅利用IMU数据解算三维姿态角，其中，水平姿态角由加速度计数据修正，并通过将
偏航轴陀螺仪的补偿值置零，实现加速度计对航向角的影响降为0，从而抑制航向角误差的增长，满足微型无人机续航时间内航向精度要求；然后进入步骤S5；
[0107] 其中，当飞行中地磁感应强度在设定范围以外时，或飞行中地磁感应强度在设定范围以内，但起飞点地磁感应强度在设定范围以外时，将仅采用IMU的3轴陀螺仪和3轴加速度计数据对无人机的三维姿态角进行解算，计算过程如下：
[0108] 将公式(1)中的eakz＝0，实现抑制航向角误差的增长，即：
[0109]
[0110] 利用eak对陀螺仪角速度进行校正，得到k时刻校正后的陀螺仪角速度输出[gxk gyk gzk]T计算：
[0111]
[0112] 利用校正后的陀螺仪角速度，采用龙格库塔法解四元数微分方程实现姿态四元数q0～q3的更新，从而解算出无人机三维姿态。
[0113] S5：读取步骤S1中计算的无人机偏离廊道线的角度，判断是否有视觉航向测量更新，即判断该角度是否发生变化，如果有更新，则进行EKF数据融合实现航向角修正并输出三维姿态；如果无更新，则直接输出三维姿态角。
[0114] 当无人机在走廊飞行时，通过前视单目相机对前方廊道进行拍照，图像处理计算机利用前视图像通过Hough变换实现狭道检测，从而得到像素坐标系中廊道线的直线方程；利用无人机航姿系统给出的俯仰角数据、廊道线在像素平面中的直线方程以及相机标定得到的内参数，通过三角关系，计算出无人机偏离廊道线的角度和距离信息。利用无人机偏离廊道线的角度，采用EKF作为数据融合工具，实现无人机航向角的修正计算过程如下：
[0115] 无人机走廊飞行时，通过Hough变换实现狭道检测，从而得到廊道线在像素平面中心为原点的像素坐标系下的直线方程：
[0116] v＝mu+n (7)
[0117] 式中，u和v分别为像素坐标系的u轴和v轴的坐标，m为直线的斜率，n为直线在v轴上的截距。
[0118] 由于无人机前飞时属于低头飞行，前视相机安装时使其光轴与无人机本体坐标系的前向平行。如图2所示，相机坐标系原点O为光心，Zc轴与光轴重合指向相机前方，Xc轴指向相机本体右侧，Yc轴向下，三个坐标轴符合右手定则。相机俯角α通过无人机航姿系统计算出的俯仰角θ计算：α＝-θ；β为无人机偏离廊道线的角度，d为无人机与廊道线的距离。为不失一般性，无论相机光轴偏向走廊左侧还是右侧，设P为廊道线上的任一点，P点在相机坐标系中的坐标为(a’，b，c)。显然当相机光轴偏向于左侧走廊时，P点为左侧廊道线上的点，则a’大于0。a为a’的绝对值，由三角关系可得：
[0119]
[0120]
[0121] 图3所示，在相机成像的像素平面的中心建立像素坐标系，原点为像素平面中心O’，横纵坐标轴分别为u、v。P’为P点在图像中对应的像素点，像素坐标为(u，v)，则根据相机小孔成像原理则有：
[0122] u＝fxa/c，v＝fyb/c (10)
[0123] 式中，fx和fy为前视相机内参数矩阵中与焦距相关的参数，通过相机标定获取。式(7)中直线方程可表达为：
[0124] fyb/c＝mfx a/c+n (11)
[0125] 将式(8)和(9)代入式(11)，消除a和b，仅保留c，整理后可得：
[0126]
[0127] 由于P点是狭道直线上的任意点，上式中对任意c值都成立，于是可得：
[0128] ncosαcosβ+fy sinαcosβ-mfx sinβ＝0 (13)
[0129] 由上式可以计算出无人机偏离廊道线的角度β：
[0130]
[0131] 无人机偏离廊道线的角度β最后输出的极性定义为：前视相机光轴朝向走廊左侧廊道线时，β以正值输出；前视相机光轴朝向走廊右侧廊道线时，β转化为负值输出。
[0132] 无人机在走廊飞行时，利用视觉计算出得到的偏离廊道线的角度β对无人机航向ψ进行修正，数据融合方法采用卡尔曼滤波，状态方程如下：
[0133]
[0134] 式中，为系统状态，ωx、ωy、ωz为陀螺仪输出角速度，q0～q3为姿态四元数，wq为系统过程噪声。
[0135] 量测方程为：
[0136]
[0137] 式中，z表示量测；v表示量测噪声；ψc为视觉计算得到的航向角，根据无人机偏离廊道线的角度β和走廊朝向确定。
[0138] 无人机航向角ψ定义为北偏西为正，0～2π范围内。当无人机起飞点地磁感应强度在设定范围以外时，无人机初始朝向的走廊方向定义为0度，即确定为伪北向，则在该飞行过程中姿态解算用到的东西南北方向均指伪东西南北方向，该情况下视觉航向测量值ψc定义为：
[0139] g)当无人机沿着南北向走廊往北飞行时：β＞0,ψc＝2π-β；β＜0,ψc＝-β；
[0140] h)当无人机沿着南北向走廊往南飞行时：ψc＝π-β；
[0141] i)当无人机沿着东西向走廊往东飞行时：
[0142] 当无人机沿着东西向走廊往西飞行时：
[0143] 当无人机起飞点地磁感应强度在设定范围以内时，整个飞行过程采用的坐标为真实的北向，需要利用无人机初始对准走廊的磁航向，以及其它走廊与初始对准走廊的几何关系，将偏离廊道线的角度β转化为北偏西为正0～2π的视觉航向测量值ψc。
[0144] 将S2和S3中得到的姿态四元数作为EKF航向修正算法中姿态四元数的一步预测。
[0145] 卡尔曼滤波矩阵如下：
[0146]
[0147]
[0148] H＝[Hq0 Hq1 Hq2 Hq3] (19)
[0149] 其中：
[0150]
[0151]
[0152]
[0153]
[0154] 连续系统离散化：
[0155] Φ≈I+FT；Γ＝GT (24)
[0156] 完成以上计算，即可利用EKF完成数据融合，得到校正后的姿态四元数，从而实现航向角的修正。
[0157] 本发明可以实现在磁场正常和异常环境下对无人机俯仰角、
横滚角和航向角的全自主解算，且水平姿态角的解算和航向角的解算实现解耦。本算法采用MEMS IMU、三轴磁力计以及前视相机作为数据来源；通过三轴磁力计输出地磁感应强度的大小进行判定，当地磁感应强度大小在设定范围内时，算法利用IMU和磁力计共9轴传感器数据实现无人机三维姿态的解算，且同时实现水平姿态角解算和航向角解算的解耦，加速度计输出仅补偿水平姿态角，磁力计输出仅补偿航向角，避免了磁场
波动对水平姿态角的影响；当地磁感应强度在设定范围外时，算法不再使用磁力计数据对航向角进行修正，仅用加速度计数据对水平姿态进行修正，并将加速度计对航向角的影响降为0，抑制航向角误差的增长，可以使得无人机航向角解算在无磁力计辅助环境下维持续航时间内精度要求；当无人机在室内走廊飞行时，通过前视单目相机实现狭道检测，提取出无人机偏离廊道线的角度，并用此偏离角对航向角进行修正，解决室内飞行时磁航向误差大或磁航向不可用时无人机航向误差累积的问题。本算法依靠由三轴陀螺仪和三轴加速度计构成的低精度MEMS IMU、三轴磁力计以及前视单目相机，实现对无人机室内飞行时三维姿态角在所有磁环境下的全自主解算。
[0158] 以上述依据本发明的理想实施例为启示，通过上述的说明内容，相关的工作人员完全可以在不偏离本发明的范围内，进行多样的变更以及
修改。本项发明的技术范围并不局限于
说明书上的内容，必须要根据
权利要求范围来确定其技术性范围。
