一种运动目标跟随方法与流程

本发明涉及无人机自主控制，特别是涉及一种基于微型光电吊舱的运动目标跟随方法。

背景技术：

1、基于光电吊舱的运动目标跟随技术，其核心是一个感知—评估—控制的闭环系统，最重要是通过感知的数据，如何有效评估得到目标准确的坐标位置，从而实现精准的跟随控制。

2、根据所搭载光电吊舱的配置，现有技术主要分为以下两类：

3、1. 基于激光测距的精确定位跟随

4、当光电吊舱集成高精度激光测距机（lrf）时，系统可获取目标斜距 r。结合吊舱框架角、无人机姿态与rtk定位，通过坐标系转换链（像素系→吊舱系→机体系→导航系→大地系）实时解算出目标的绝对经纬度与海拔。飞控利用该坐标与目标速度矢量，通过视线法（los）或非线性导引律计算期望位置，实现精确的定点或定距跟随。

5、该方案的局限在于：激光测距机成本高、重量大，无法集成于日益普及的微型吊舱中，限制了小型无人机的应用。

6、2. 基于纯视觉的无激光跟随

7、针对微型吊舱，其具有小型化、低成本、无激光的特点，需通过其他信息源恢复深度信息或直接规避对深度信息的依赖，现有无激光方案主要依赖以下两种方法：

8、（1）定高平面假设法

9、假设目标附着于水平地面，利用无人机离地高度h，再结合吊舱俯仰角 θ，通过三角关系估算目标水平距离，进而控制无人机保持设定距离。

10、该方法的核心缺陷是：当地形突变时（如飞越楼房、高架桥或坑洼），相对高度h发生跳变，但算法误认为目标距离突变。在飞越楼房场景中，系统会误判为“目标瞬间靠近”，触发无人机急刹车、抬头并后退；飞越楼顶后h又突增，系统误判为“目标瞬间远离”，引发无人机低头俯冲前追。这种剧烈误判不仅导致跟随轨迹严重振荡、目标丢失，更存在严重安全隐患。

11、（2）运动模型滤波法（纯视觉偏差伺服）

12、该方法放弃绝对距离测量，仅以吊舱输出的目标偏离图像中心的像素脱靶量作为误差，通过pi控制器输出偏航/俯仰角速率，使无人机转动以消除偏差，维持目标在画面中央。速度控制通常依赖目标框面积变化率或固定巡航速度。

13、该方法的缺陷在于：无法直接获得目标距离与物理坐标，不能实现精确的定距跟随；速度控制粗糙，容易在加速/减速时产生振荡。更重要的是，当目标被短暂遮挡（如飞越楼房时被楼顶边缘遮挡），视觉反馈丢失后，系统缺乏开环预测能力，无人机易停滞或盲目漂移；遮挡结束重捕获时，极易因画面中出现相似目标而错误锁定。

14、综上，现有微型吊舱跟随方案存在“地形鲁棒性”与“精确距离控制”不可兼得的矛盾：定高平面假设法不能适应地形突变，会引发危险机动；运动模型滤波法地形适应性强但无法控距，且遮挡后易跟丢。

15、因此，如何提供一种能够融合两者优势、智能切换、安全可靠的跟随方法，是迫切需要本领域技术人员解决的技术问题。

技术实现思路

1、鉴于上述问题，本发明提供用于克服上述问题或者至少部分地解决上述问题的一种运动目标跟随方法。解决了现有无激光测距微型吊舱跟随方案在复杂地形与遮挡场景下，因单一依赖定高平面测距或纯视觉伺服，而导致的安全风险高、跟随稳定性差、重捕获成功率低等问题。该方法利用光电吊舱（尤其是无激光测距的微型吊舱）实现对运动目标稳定跟随，能够应对地形突变与目标遮挡等复杂环境。

2、本发明提供了如下方案：

3、一种运动目标跟随方法，包括：

4、所述无人机搭载无激光测距的微型光电吊舱、机载高度传感器和定位模块，所述方法包括：

5、获取所述机载高度传感器测量的离地高度、所述微型光电吊舱输出的吊舱俯仰角、图像脱靶量及目标置信度；

6、默认运行模式一，基于定高平面假设法，利用所述离地高度和所述吊舱俯仰角计算目标水平距离，并根据所述目标水平距离生成定距跟随指令，所述模式一为距离环模式；

7、实时监测所述离地高度h的高度变化率和所述吊舱俯仰角的抖动幅值，当所述高度变化率超过第一预设阈值，或所述抖动幅值超过第二预设阈值时，生成地形突变标志位，由所述模式一切换至模式二，禁用距离环，并激活运动模型预测保持功能；所述模式二为视觉偏差环模式，仅根据所述图像脱靶量驱动无人机航向与俯仰，保持目标在画面中心；

8、在所述模式二下，当所述目标置信度低于第三预设阈值时，判定为目标被短暂遮挡，利用遮挡前通过扩展卡尔曼滤波器估计出的目标运动速度维持所述无人机的速度与航向开环输出；

9、当所述高度变化率恢复至所述第一预设阈值以内，且所述抖动幅值恢复至所述第二预设阈值以内，且所述目标置信度大于或等于第四预设阈值并持续第一预设时间时，通过动态调整所述扩展卡尔曼滤波器的观测噪声协方差，由所述模式二平滑切回所述模式一。

10、优选地：所述高度变化率超过第一预设阈值为：

11、所述离地高度经过中值滤波处理后，计算前后帧的差分，当差分的绝对值大于或等于3.0米时触发；

12、所述抖动幅值超过第二预设阈值为：

13、所述吊舱俯仰角的角速度绝对值大于或等于10°/秒且连续5帧触发；

14、所述目标置信度低于第三预设阈值为：

15、所述目标置信度c小于0.4且连续5帧触发。

16、优选地：通过动态调整所述扩展卡尔曼滤波器的观测噪声协方差实现平滑切回，具体包括：

17、在所述模式一中，所述扩展卡尔曼滤波器采用标称值的所述观测噪声方差融合定高平面距离观测，所述标称值为2.0m²；

18、在由所述模式一切换至所述模式二的瞬间，将所述观测噪声协方差设置为极大值，使所述扩展卡尔曼滤波器的卡尔曼增益趋于零，等效静默所述距离观测；

19、在由所述模式二切回所述模式一时，将所述观测噪声协方差从所述极大值按指数衰减平滑降低至所述标称值，使所述距离观测的影响逐渐增大。

20、优选地：所述扩展卡尔曼滤波器的状态向量为12维，定义为：

21、

22、其中：

23、为目标在ned坐标系下的相对位置；

24、为目标在ned坐标系下的速度；

25、为目标在ned坐标系下的加速度；

26、为吊舱框架角的测量偏置；

27、为定高雷达的高度偏置。

28、优选地：所述扩展卡尔曼滤波器支持三类观测，并根据数据有效性动态切换使用：

29、观测类型一为基于定高平面假设的距离观测，当地形突变标志位为false时启用，观测方程为：

30、

31、观测类型二为基于吊舱视线的角度观测，始终启用，观测方程为：

32、

33、观测类型三为基于图像脱靶量的像素观测，当目标识别置信度高于阈值时启用，观测方程为：

34、

35、其中，表示相机内参矩阵，表示吊舱到相机的变换矩阵，、和表示观测噪声。

36、优选地：所述开环输出的持续时间为0.5秒至1.5秒；当所述目标置信度低于第三预设阈值的持续时间超过第二预设时间时，由所述模式二切换至目标丢失模式lost，控制所述无人机悬停并缓慢爬升，同时控制所述吊舱进入螺旋搜索模式，所述悬停爬升的爬升率为1米/秒，爬升上限为30米；所述第二预设时间为2.0秒。

37、优选地：还包括目标重识别：

38、在所述螺旋搜索模式中重新捕获目标时，基于深度学习的目标重识别re-id特征匹配确认原目标，当匹配度大于0.7且连续3帧稳定时，由所述lost模式切换至所述模式二，并利用新观测重新初始化所述扩展卡尔曼滤波器。

39、优选地：还包括目标速度死区处理：

40、当所述扩展卡尔曼滤波器输出的目标速度估计模长小于0.5米/秒时，冻结期望跟随方位角，保持最后一次有效值，所述无人机仅在当前位置进行平移修正，所述吊舱保持当前锁定状态；当所述目标速度估计模长大于或等于0.5米/秒且持续1秒以上时，退出死区，恢复正常前馈跟随。

41、优选地：所述目标水平距离d的计算公式为：

42、

43、其中，表示视线向量在ned系下的俯角；

44、所述定距跟随指令的生成方式为：

45、设目标当前位置为，目标速度矢量为，期望保持的相对距离为，期望无人机位置为:

46、

47、位置误差为:

48、

49、飞控将位置误差转换为速度指令:

50、

51、其中,表示大地坐标，为比例增益，为前馈项。

52、优选地：所述方法通过节点化软件架构实现，包括：

53、吊舱驱动节点，用于订阅所述微型光电吊舱输出的脱靶量、框架角、目标类别与置信度；

54、高度预处理节点，用于对所述离地高度进行中值滤波以抑制杂波；

55、目标状态估计器节点，用于执行所述扩展卡尔曼滤波器滤波及所述模式一与模式二的切换逻辑；

56、制导控制节点，用于根据所述目标状态估计器节点的输出生成速度指令。

57、根据本发明提供的具体实施例，本发明公开了以下技术效果：

58、本发明实施例提供的一种运动目标跟随方法，具有良好的安全性，彻底避免无人机在飞越楼房等场景下因高度误判导致的危险俯冲与急停动作，飞行轨迹平稳。同时具有良好的跟随连续性，通过短时惯性预测与智能重捕获，显著降低目标丢失概率，实现遮挡后的无缝衔接。另外还实现了精确与鲁棒的兼顾，在平坦地形下保持精确的定距跟随能力，在地形突变时自动退化为稳定的视觉跟随。

59、当然，实施本发明的任一产品并不一定需要同时达到以上所述的所有优点。