基于虚拟预测目标的自适应三维鲁棒导航控制方法

本技术的实施例涉及现代智能物流，特别涉及一种基于虚拟预测目标的自适应三维鲁棒导航控制方法。

背景技术：

1、在现代智能物流与自动驾驶领域，随着无人机配送、自动驾驶卡车编队以及服务机器人的普及，配送车辆需要通过调度跟随车辆来完成对动态目标的高效交接与避障保护。例如，在复杂的城市物流场景中，配送车辆需要引导跟随车辆去接近一辆正在行驶中的目标运输车辆，或者避开突然出现的障碍物。

2、现有运动体导航方法主要分为两类：一是基于经典比例导航及其改进形式（如增广比例导航），这类方法通过计算相对视线角速度来引导追踪器接近目标。然而，此类方法对目标物体的变速、变向运动补偿能力有限，在追踪高速运动或频繁变道的动态目标时，容易出现追踪精度显著下降的问题；二是基于预测的控制方法，通过预测未来一段时间内的交汇点并引导追踪器飞向该点。

3、然而，现有的预测导航方法通常假设目标物体的运动规律（如速度、加速度模型）已知或采用简单的固定模型，且严重依赖于剩余交汇时间的准确估计。在实际复杂的交通与物流环境中，目标物体的运动意图往往是未知且时变的（例如前方车辆突然减速或变道），同时车载传感器（如激光雷达、视觉传感器）的测量噪声和模型不确定性使得固定参数的滤波器难以适应。这导致预测的交汇点波动大，追踪器的控制指令出现高频抖振，甚至导致跟随失败。

4、此外，传统方法常将三维空间内的导航解耦为横向和纵向两个平面分别设计，这不仅增加了控制逻辑的复杂性，还忽略了空间运动中的耦合影响。

5、因此，如何能够克服目标运动规律未知、运动模式不确定以及传感器噪声时变等挑战，并在三维空间内实现一体化的鲁棒导航控制，是目前亟需解决的技术问题。

技术实现思路

1、有鉴于此，本技术的实施例提出了一种基于虚拟预测目标的自适应三维鲁棒导航控制方法，以解决在参考目标运动规律未知、运动模式不确定以及传感器测量噪声统计特性时变条件下，跟随车辆对高速动态目标的精确、稳定交汇问题。本技术的实施例通过构建虚拟目标将高速动态目标导航转化为对低速平滑目标的导航，并采用自适应滤波，显著提升导航控制系统对不确定性的鲁棒性和交汇精度。

2、为了实现上述目的，本技术的实施例提出了一种基于虚拟预测目标的自适应三维鲁棒导航控制方法，所述方法包括以下步骤：

3、构建包含配送车辆、参考目标和跟随车辆的三维运动学模型；

4、在每个导航控制周期，基于当前时刻配送车辆、参考目标和跟随车辆的状态，实时迭代预测参考目标与跟随车辆的交汇点，并将该预测交汇点作为虚拟目标；

5、基于虚拟目标的位置观测量，采用自适应滤波器对虚拟目标的状态进行最优估计，得到包含位置和速度的最优状态估计；

6、基于最优状态估计，生成跟随车辆的导航控制指令；

7、将导航控制指令输入跟随车辆的控制系统，实现闭环导航控制。

8、为了实现上述目的，本技术的实施例还提出了一种基于虚拟预测目标的自适应三维鲁棒导航系统，所述系统包括：

9、模型构建模块，用于构建包含配送车辆、参考目标和跟随车辆的三维运动学模型；

10、迭代预测模块，用于在每个导航控制周期，基于当前时刻配送车辆、参考目标和跟随车辆的状态，实时迭代预测参考目标与跟随车辆的交汇点，并将该预测交汇点作为虚拟目标；

11、最优评估模块，用于基于虚拟目标的位置观测量，采用自适应滤波器对虚拟目标的状态进行最优估计，得到包含位置和速度的最优状态估计；

12、指令生成模块，用于基于最优状态估计，生成跟随车辆的导航控制指令；

13、输入模块，用于将导航控制指令输入跟随车辆的控制系统，实现闭环导航控制。

14、为了实现上述目的，本技术的实施例还提出了一种电子设备，包括：处理器和存储器，所述存储器中存储有所述处理器能够执行的指令，所述处理器被配置为执行所述指令时，使得所述电子设备能够实现如上述所述的一种基于虚拟预测目标的自适应三维鲁棒导航控制方法。

15、为了实现上述目的，本技术的实施例还提出了一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，能够实现如上述所述的一种基于虚拟预测目标的自适应三维鲁棒导航控制方法。

16、本技术的实施例提出的一种基于虚拟预测目标的自适应三维鲁棒导航控制方法，构建包含配送车辆、参考目标和跟随车辆的三维运动学模型；在每个导航控制周期，基于当前时刻配送车辆、参考目标和跟随车辆的状态，实时迭代预测参考目标与跟随车辆的交汇点，并将该预测交汇点作为虚拟目标；基于虚拟目标的位置观测量，采用自适应滤波器对虚拟目标的状态进行最优估计，得到包含位置和速度的最优状态估计；基于最优状态估计，生成跟随车辆的导航控制指令；将导航控制指令输入跟随车辆的控制系统，实现闭环导航控制；本方案通过构建虚拟目标将高速动态目标导航转化为对低速平滑目标的导航，并采用自适应滤波，显著提升导航控制系统对不确定性的鲁棒性和交汇精度，进而解决了在参考目标运动规律未知、运动模式不确定以及传感器测量噪声统计特性时变条件下，跟随车辆对高速动态目标的精确、稳定交汇问题。

17、可选地，构建包含配送车辆、参考目标和跟随车辆的三维运动学模型，包括：定义配送车辆、参考目标和跟随车辆的状态向量；状态向量包含三维位置、三维速度和三维加速度信息；基于状态向量，建立描述三者相对位置和相对速度的运动方程；引入一阶惯性模型描述导航控制指令到实际加速度的动态响应。

18、可选地，在每个导航控制周期，基于当前时刻配送车辆、参考目标和跟随车辆的状态，实时迭代预测参考目标与跟随车辆的交汇点，并将该预测交汇点作为虚拟目标，包括：输入当前时刻配送车辆、参考目标和跟随车辆的状态，并设置迭代步数n=0；计算参考目标相对于配送车辆的相对位置与相对速度，以及跟随车辆相对于参考目标的相对位置与相对速度；假设参考目标与跟随车辆均采用比例导航控制策略，分别计算其加速度指令；基于等加速度模型，对配送车辆、参考目标和跟随车辆的状态进行一个步长的前向积分，更新状态并令迭代步数n=n+1；重复执行所述计算参考目标相对于配送车辆的相对位置与相对速度，以及跟随车辆相对于参考目标的相对位置与相对速度，直至满足交汇条件，并将迭代结束时参考目标的位置作为当前周期的预测交汇点。

19、可选地，自适应滤波器为sage-husa自适应卡尔曼滤波器，其过程噪声协方差矩阵qk和观测噪声协方差矩阵rk能够根据新息序列在线递推更新，以适应预测误差的时变统计特性。

20、可选地，基于最优状态估计，生成跟随车辆的导航控制指令，包括：将导航控制指令分解为比例导航基准项和滑模校正项；基于虚拟目标的最优状态估计，计算比例导航基准项；基于跟随车辆与虚拟目标之间的跟踪误差，计算滑模校正项；合成比例导航基准项与滑模校正项，得到最终的导航控制指令。

21、可选地，基于跟随车辆与虚拟目标之间的跟踪误差，计算滑模校正项，包括：计算跟随车辆与虚拟目标之间的速度误差；将速度误差投影至比例导航基准项的法向子空间，得到投影误差；利用与相对距离相关的连续衰减函数对投影误差进行加权，并结合正定阻尼增益矩阵，生成滑模校正项。

22、可选地，比例导航基准项的计算公式表示如下：

23、；

24、其中，为有效导航比；表示跟随车辆与虚拟目标之间的接近速度，为相应的视线角速度矢量；为跟随车辆的速度矢量。