基于视觉图像径/法向运动的无人机组合测距方法

1.本发明属于飞行器组合导航和计算机视觉技术领域，具体基于视觉图像径/法向运动的无人机组合测距方法。


背景技术：

2.蜂群系统的关键在于低成本抵消策略，降低飞行平台造价成本是从业人员一直追求的目标。传统通过硬件精简或复用来降低成本的方法，由于摩尔定律渐趋极限等原因，其降低成本的效果面临重大瓶颈。基于飞行平台信息冗余现状，通过软件复用替代硬件设备，有助于在保证测距功能的前提下显著降低成本。
3.对于小型无人飞行器而言，主要由飞行平台和任务载荷两部分组成，其中平台包括外壳结构、飞控系统、回收系统、电台天线、舵机电机等部件，任务载荷最典型的有光电吊舱。在这些设备中，光电吊舱通常是价格最高的单体系统。特别地，当光电吊舱具备激光测距功能时，一方面，由于需要昂贵的精密光学设备，将进一步使得其价格成倍增长；另一方面，加装激光测距吊舱意味着更大的重量、体积与功耗，给整机系统带来更大负担，进一步增加附加成本。如果能利用飞行器平台上的冗余信息，基于不带测距功能的普通光电吊舱，深度融合机上设备，对视线径/法向运动进行分解，以组合导航的思路实现系统级的软件测距，有助于显著降低飞行平台成本。
4.目前在中小型无人机平台上主要存在如下测距手段：1)激光测距：以激光器作为光源照射到目标，测量往返时间或相位差，再结合光在介质中的传播速度得到飞机到目标的距离。该方法精度较高，但会增加额外的硬件成本，且大气中烟雾、灰尘、雨滴会对精度造成一定干扰。2)单目视觉测距：通过图像匹配进行目标识别，再通过目标先验尺寸及其在图像中的大小依据相似三角形原理估算目标距离。该方法精度相对较低，且依赖于先验信息并更新维护庞大的样本数据库，对库外目标无法进行判断和测距。3)双目视觉测距：在左右相机图像中精确匹配到同一像素点，计算出两幅图像视差，结合基线长度估算像素点距离。该方法精度比单目高，但受限于基线长度，远距测量相对精度会下降，同时由于算法复杂在mcu上实现困难，难以在快速飞行平台上保证高刷新率，在应用上存在诸多限制。4)三角形法估算：近似以飞机平台与起飞点相对高度作为与目标的高度差，依据三角形几何关系估算距离。该方法精度较低，且需要目标与起飞点高度基本一致，测算距离往往参考价值有限，在使用场景上存在明显限制。
5.综上所述，以新的思路开展小型飞行平台系统级测距功能开发，通过设备融合实现视线径/法向运动分解，设计组合滤波模块进行距离推断，可以显著降低成本需求，同时较大程度地消除场景限制和先验信息依赖，具有较大的研究前景和应用价值。


技术实现要素：

6.本发明的目的是提供一种基于视觉图像径/法向运动的无人机组合测距方法，提供一种新的利用平台冗余信息的软件测距思路，解决激光测距硬件成本高而单/双目视觉
测距和三角形测距法场景受限明显、依赖先验信息的技术问题。
7.为了达到上述目的，解决上述技术问题，本发明的技术方案如下：
8.一种基于视觉图像径/法向运动的无人机组合测距方法，包括如下步骤：
9.步骤1、光电吊舱对选中的目标图像通过像素匹配进行持续识别，并驱动吊舱电机改变框架角使其锁定在图像中央位置，以保证测距期间吊舱轴线空间角度等于飞机-目标视线角度；
10.步骤2、光电吊舱将测量的视线俯仰角速率ω
θ,l
、视线方位角速率ω
ψ,l
、俯仰框架角θf、方位框架角ψf通过串口通讯发送至飞控mcu；
11.步骤3、gps/ins系统将测量的平台俯仰角θ和方位姿态角ψ通过串口通讯发送至飞控mcu，并与实时接收到的框架角θf和方位框架角ψf分别进行叠加：
[0012][0013]
步骤4、差分定位系统将测量的平台精确相对位置信息通过串口通讯发送至飞控mcu，通过微分估计实时计算无人机飞机水平地速vd、升降速度vh、航迹方位角ψv；
[0014]
步骤5、基于法向运动的距离推断模块进行多模计算，并通过三余度表决机制进行选择，使得当l
n,1
,l
n,2
,l
n,3
计算结果存在明显差异时，通过三判二的原则筛选出更为正确的距离推测值；
[0015]
待测距离的法向推算通道1为：
[0016][0017]
待测距离的法向推算通道2为：
[0018][0019]
待测距离的法向推算通道3为：
[0020][0021]
其中μ＝vdcosθ
l
cos(ψ
v-ψ
l
)+vhsinθ
l
；
[0022]
表决原则为：
[0023]
原则1、对法向运动三通道推算距离执行三判二处理，依次计算每个通道距离与其他两个通道距离的差值，若均大于另外两通道距离差值的5倍，表明该通道严重偏离另外两个通道，则将其舍去，否则保留；
[0024]
原则2、对剩下通道距离取平均值，为统一公式表达，记舍去通道编号的集合为ω，当没有通道被舍去时，ω为空集，其平均值为
[0025][0026]
步骤6、基于径向运动的距离推断模块进行积分计算，利用积分的天然特性实现对白噪声的抑制效果
[0027]
待测距离的径向推算公式为：
[0028][0029]
其中l
r0
为初始化值，可任取，当ω
θ,l
,ω
ψ,l
绝对值较大时，为加快收敛速度可以初始化为l
n0
；
[0030]
步骤7、将步骤5和步骤6中基于法向和径向运动推断的距离同时输入带开关切换机制的组合滤波模块中，开关切换机制用于对法向运动推断的距离进行坏值筛选；引入表示公式(2)-(4)中分母绝对值的上确界，当σ≤ε时，小值ε为设定的灵敏度常数，开关断开，平时保持常闭状态；
[0031]
以径向运动推断为主系统，实现平滑输出，法向运动推断为辅助系统，抑制漂移误差，带开关切换机制的径/法向运动组合滤波选用开环架构或闭环架构。
[0032]
进一步的，无人机组合测距方法由组合测距系统实现，包括飞控计算机、gps/ins系统、差分定位系统、光电吊舱、；
[0033]
飞控计算机与其它设备通过串口通讯，对设备信息进行中继处理，对设备行为进行调度协调；
[0034]
gps/ins系统将平台姿态角发送给飞控计算机，飞控计算机将平台姿态角转发给光电吊舱，并将其与吊舱框架角进行叠加得到视线角；
[0035]
差分定位系统将相对基站的精确位置信息发送给飞控计算机，对平台绝对位置进行精度修正，计算得到飞机的速度大小和方向，传送到飞控计算机的片上径/法向运动距离推断模块；
[0036]
光电吊舱将测量的视线转动角速率发送给飞控计算机，与融合计算得到的视线角度一起传送到飞控计算机的片上径/法向运动距离推断模块，汇总所有信息后，径/法向运动距离推断模块以径向运动推算子模块作为主要系统，以法向运动推算子模块作为辅助系统并进行三余度表决，对以上两个距离推算系统进行组合滤波，输出最终的推断距离。
[0037]
进一步的，公式(2)、(3)、(4)的运算中，为避免计算出现奇异，当分母十分接近零时，取计算距离为一充分大的合理值。
[0038]
进一步的，开环架构需要首先对σ≤ε进行判断，如果不成立，则将径向运动推测距离与法向运动推测距离的差输入到卡尔曼滤波器，径向运动推测距离再减去滤波器输出得到融合后的距离。
[0039]
进一步的，闭环架构首先对σ≤ε进行判断，如果不成立，则将滤波输出到径向运动推测距离模块进行闭环修正，并以修正后的径向运动推测距离作为融合后的距离。
[0040]
本发明的有效收益是：
[0041]
1、本发明基于不带测距功能的普通光电吊舱，通过机上设备交互融合与视线径/法向运动分解，以组合导航的思路实现系统级的软件测距，有助于显著降低飞行平台成本。以某小型光电吊舱为例，不带激光测距设备的三光吊舱市场价格在4万左右，带激光测距设备的光电吊舱市场价格在8万以上，成本增加一倍左右，本发明基于不带测距设备的普通吊舱，对视线径/法向运动进行分解，并利用飞行器平台上的冗余速度信息，以组合导航的思路实现系统级的软件测距，有助于显著降低飞行平台成本
[0042]
2、此外，相比于单/双目视觉测距和三角形测距法，本发明能够更大程度地消除场景限制和先验信息依赖，具有较大的研究前景和应用价值。单目视觉测距依赖于目标先验尺寸信息并更新维护庞大的样本数据库，对库外目标无法进行判断和测距，本发明不依赖目标尺寸信息；双目视觉测距存在明显的场景限制，主要表现在远距测量相对精度下降，同时算法复杂在mcu上实现困难，难以在快速飞行平台上保证高刷新率，本发明基于不同的测距原理，算法相对简单，能在快速平台mcu上保证高刷新率和远距精度；三角形法精度较低，且需要目标与起飞点高度基本一致，本发明不存在这种场景限制。
附图说明
[0043]
图1本发明系统组成与测距工作原理示意图；
[0044]
图2本发明法向运动推测距离三余度表决示意图；
[0045]
图3本发明带开关切换机制的径/法向运动组合滤波开环架构示意图；
[0046]
图4本发明带开关切换机制的径/法向运动组合滤波闭环架构示意图，
[0047]
图5、本发明的仿真结果示意图。
具体实施方式
[0048]
本发明的设计思路是针对不带测距功能的普通光电吊舱，利用飞行器平台上的冗余信息，设计融合推断机制实现系统级的软件测距功能，以满足传统设备级的硬件测距需求，在保证功能完备的前提下显著降低飞行平台成本。本发明针对无人机飞行动态几何关系，将无人机飞行视线运动沿径向和法向分解，分别构造了基于法向运动的距离推断模块及其三余度表决机制，以及基于径向运动的距离推断模块及其噪声抑制机制。据此将原始的距离融合推断巧妙转化为组合导航问题，以径向运动推断距离模块作为主要系统，以法向运动推断距离模块作为辅助系统，并对前者进行修正，抑制漂移误差。
[0049]
为使本领域技术人员更好地理解本发明的技术方案，下面结合系统结构框架和具体实施方式对本方法作进一步详细描述。基于视觉图像径/法向运动的低成本组合测距系统组成结构如图1所示：
[0050]
基于视觉图像径/法向运动的低成本组合测距系统，包括飞控计算机、光电吊舱、gps/ins系统、差分定位系统等。飞控计算机是核心部件，与其它设备通过串口通讯，对设备信息进行中继处理，对设备行为进行调度协调。gps/ins系统将平台姿态角发送给飞控计算机，飞控计算机将平台姿态角转发给光电吊舱，并将其与吊舱框架角进行叠加得到视线角。差分定位系统将相对基站的精确位置信息发送给飞控计算机，可对平台绝对位置进行精度修正，计算得到飞机的速度大小和方向，传送到飞控计算机的片上径/法向运动距离推断模块。光电吊舱将测量的视线转动角速率发送给飞控计算机，与融合计算得到的视线角度一
起传送到飞控计算机的片上径/法向运动距离推断模块。汇总所有信息后，径/法向运动距离推断模块以径向运动推算子模块作为主要系统，以法向运动推算子模块作为辅助系统并进行三余度表决，对以上两个距离推算系统进行组合滤波，输出最终的推断距离。
[0051]
基于视觉图像径/法向运动的无人机组合测距方法，包括如下步骤：
[0052]
步骤1、光电吊舱对选中的目标图像通过像素匹配进行持续识别，并驱动吊舱电机改变框架角使其锁定在图像中央位置，以保证测距期间吊舱轴线空间角度等于飞机-目标视线角度；
[0053]
步骤2、光电吊舱将测量的视线俯仰角速率ω
θ,l
和视线方位角速率ω
ψ,l
以及俯仰框架角θf和方位框架角ψf通过串口通讯发送至飞控mcu；
[0054]
步骤3、gps/ins系统将测量的平台俯仰和方位姿态角θ,ψ通过串口通讯发送至飞控mcu，并与实时接收到的俯仰和方位框架角分别进行叠加：
[0055][0056]
步骤4、差分定位系统将测量的平台精确相对位置信息通过串口通讯发送至飞控mcu，通过微分估计实时计算无人机飞机水平地速vd、升降速度vh、航迹方位角ψv；
[0057]
步骤5、基于法向运动的距离推断模块进行多模计算，并通过三余度表决机制进行选择，使得当l
n,1
,l
n,2
,l
n,3
计算结果存在明显差异时，能够通过三判二的原则筛选出更为正确的距离推测值；
[0058]
待测距离的法向推算通道1为：
[0059][0060]
待测距离的法向推算通道2为：
[0061][0062]
待测距离的法向推算通道3为：
[0063][0064]
其中μ＝vdcosθ
l
cos(ψ
v-ψ
l
)+vhsinθ
l
，为表达方便将其引入；
[0065]
以上所有运算中，为避免计算奇异，当分母十分接近零时，取计算距离为一充分大的合理值，例如历史最大值；
[0066]
表决原则为：
[0067]
原则1、对法向运动三通道推算距离执行三判二处理，依次计算每个通道距离与其他两个通道距离的差值，若均大于另外两通道距离差值的5倍，表明该通道严重偏离另外两个通道，则将其舍去，否则保留；
[0068]
原则2、对剩下通道距离取平均值，为统一公式表达，记舍去通道编号的集合为ω(当没有通道被舍去时，ω为空集)，其平均值为
[0069][0070]
具体操作流程如图2所示。
[0071]
步骤6、基于径向运动的距离推断模块进行积分计算，利用积分的天然特性实现对白噪声的抑制效果。
[0072]
待测距离的径向推算公式为：
[0073][0074]
其中l
r0
为初始化值，可任取，当ω
θ,l
,ω
ψ,l
绝对值较大时，为加快收敛速度可以初始化为l
n0
。
[0075]
步骤7、将基于径向和法向运动推断的距离同时输入带开关切换机制的组合滤波模块，开关切换机制用于对法向运动推断的距离进行坏值筛选，引入表示公式(2)-(4)中分母绝对值的上确界，当σ≤ε时，小值ε(例如0.5)为设定的灵敏度常数，开关断开，平时保持常闭状态。以径向运动推断为主系统，实现平滑输出，法向运动推断为辅助系统，抑制漂移误差，带开关切换机制的径/法向运动组合滤波可选用开环或闭环架构。开环架构需要首先对σ≤ε进行判断，如果不成立，则将径向运动推测距离与法向运动推测距离的差输入到卡尔曼滤波器，径向运动推测距离再减去滤波器输出得到融合后的距离。闭环架构也需执行类似的比较和滤波过程，区别在于将滤波输出到径向运动推测距离模块进行闭环修正，并以修正后的径向运动推测距离作为融合距离。上述带开关切换机制开环和闭环架构分别如图3和图4所示。
[0076]
综上所述，现有技术中单目视觉测距需要通过图像匹配进行目标识别，严重依赖于目标先验尺寸信息并更新维护庞大的样本数据库，对库外目标无法进行判断和测距；而双目视觉测距在进行远距测量时相对精度下降，场景限制因素非常显著，同时双目视觉测距的算法复杂在mcu上实现困难，难以在快速飞行平台上保证高刷新率，而本发明将无人机飞行视线运动沿径向和法向分解，分别构造了基于法向运动的距离推断模块及其三余度表决机制，以及基于径向运动的距离推断模块及其噪声抑制机制，算法相对简单，能在快速平台mcu上保证高刷新率和远距精度；三角形法精度较低，且需要目标与起飞点高度基本一致，本发明不存在这种场景限制。
[0077]
本发明在matlab/simulink环境下，搭建飞行器末制导模型进行了本发明的的仿真验证，组合测距模块接收飞控输出的速度和姿态信息以及光电吊舱输出的视线角信息(叠加幅值10％左右的测量白噪声)，对弹目距离进行推算，图5展示了仿真结果，推算距离很快收敛并保持在真实距离附近，表明本测距方法的有效性。
[0078]
以上内容是结合具体的实施方式对本发明所作的进一步详细说明，不能认定本发
明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。

技术特征：
1.一种基于视觉图像径/法向运动的无人机组合测距方法，其特征在于，包括如下步骤：步骤1、光电吊舱对选中的目标图像通过像素匹配进行持续识别，并驱动吊舱电机改变框架角使其锁定在图像中央位置，以保证测距期间吊舱轴线空间角度等于飞机-目标视线角度；步骤2、光电吊舱将测量的视线俯仰角速率ω
θ,l
、视线方位角速率ω
ψ,l
、俯仰框架角θ
f
、方位框架角ψ
f
通过串口通讯发送至飞控mcu；步骤3、gps/ins系统将测量的平台俯仰角θ和方位姿态角ψ通过串口通讯发送至飞控mcu，并与实时接收到的框架角θ
f
和方位框架角ψ
f
分别进行叠加：步骤4、差分定位系统将测量的平台精确相对位置信息通过串口通讯发送至飞控mcu，通过微分估计实时计算无人机飞机水平地速v
d
、升降速度v
h
、航迹方位角ψ
v
；步骤5、基于法向运动的距离推断模块进行多模计算，并通过三余度表决机制进行选择，使得当l
n,1
,l
n,2
,l
n,3
计算结果存在明显差异时，通过三判二的原则筛选出更为正确的距离推测值；待测距离的法向推算通道1为：待测距离的法向推算通道2为：待测距离的法向推算通道3为：其中μ＝v
d cosθ
l
cos(ψ
v-ψ
l
)+v
h
sinθ
l
；步骤6、基于径向运动的距离推断模块进行积分计算，利用积分的天然特性实现对白噪声的抑制效果待测距离的径向推算公式为：
其中l
r0
为初始化值，可任取，当ω
θ,l
,ω
ψ,l
绝对值较大时，为加快收敛速度可以初始化为l
n0
；步骤7、将步骤5和步骤6中基于法向和径向运动推断的距离同时输入带开关切换机制的组合滤波模块中，开关切换机制用于对法向运动推断的距离进行坏值筛选；引入表示公式(2)-(4)中分母绝对值的上确界，当σ≤ε时，小值ε为设定的灵敏度常数，开关断开，平时保持常闭状态；以径向运动推断为主系统，实现平滑输出，法向运动推断为辅助系统，抑制漂移误差，带开关切换机制的径/法向运动组合滤波选用开环架构或闭环架构。2.根据权利要求1所述的一种基于视觉图像径/法向运动的无人机组合测距方法，其特征在于，所述的无人机组合测距方法由组合测距系统实现，包括飞控计算机、gps/ins系统、差分定位系统、光电吊舱、；飞控计算机与其它设备通过串口通讯，对设备信息进行中继处理，对设备行为进行调度协调；gps/ins系统将平台姿态角发送给飞控计算机，飞控计算机将平台姿态角转发给光电吊舱，并将其与吊舱框架角进行叠加得到视线角；差分定位系统将相对基站的精确位置信息发送给飞控计算机，对平台绝对位置进行精度修正，计算得到飞机的速度大小和方向，传送到飞控计算机的片上径/法向运动距离推断模块；光电吊舱将测量的视线转动角速率发送给飞控计算机，与融合计算得到的视线角度一起传送到飞控计算机的片上径/法向运动距离推断模块，汇总所有信息后，径/法向运动距离推断模块以径向运动推算子模块作为主要系统，以法向运动推算子模块作为辅助系统并进行三余度表决，对以上两个距离推算系统进行组合滤波，输出最终的推断距离。3.根据权利要求1或2所述的一种基于视觉图像径/法向运动的无人机组合测距方法，其特征在于，公式(2)、(3)、(4)的运算中，为避免计算出现奇异，当分母十分接近零时，取计算距离为一充分大的合理值。4.根据权利要求1或2所述的一种基于视觉图像径/法向运动的无人机组合测距方法，其特征在于，所述开环架构需要首先对σ≤ε进行判断，如果不成立，则将径向运动推测距离与法向运动推测距离的差输入到卡尔曼滤波器，径向运动推测距离再减去滤波器输出得到融合后的距离。5.根据权利要求1或2所述的一种基于视觉图像径/法向运动的无人机组合测距方法，其特征在于，闭环架构首先对σ≤ε进行判断，如果不成立，则将滤波输出到径向运动推测距离模块进行闭环修正，并以修正后的径向运动推测距离作为融合后的距离。

技术总结
本发明公开一种基于视觉图像径/法向运动的无人机组合测距方法，属于飞行器组合导航和计算机视觉技术领域；本发明针对无人机飞行动态几何关系，将无人机飞行视线运动沿径向和法向分解，分别构造了基于法向运动的距离推断模块及其三余度表决机制，以及基于径向运动的距离推断模块及其噪声抑制机制，据此将原始的距离融合推断巧妙转化为组合导航问题，以径向运动推断距离模块作为主要系统，以法向运动推断距离模块作为辅助系统，并对前者进行修正，抑制漂移误差。本发明有助于显著降低飞行平台成本，且本发明基于不同的测距原理，计算过程相对简单，能在快速平台MCU上保证高刷新率和远距精度，保证精度的同时测距场景不受限制。保证精度的同时测距场景不受限制。保证精度的同时测距场景不受限制。


技术研发人员：丁超 孟志鹏 陈小庆 王波 朱得糠 李彤 吕良
受保护的技术使用者：中国人民解放军军事科学院国防科技创新研究院
技术研发日：2023.03.07
技术公布日：2023/7/25