ESIKF测量协方差自适应调整方法、系统及相关设备与流程

摘要: 本发明涉及机器人导航定位,尤其涉及一种基于深度强化学习的esikf测量协方差自适应调整方法、系统及相关设备。背景技术:1、在现有livo系统(如fast-liv...
ESIKF测量协方差自适应调整方法、系统及相关设备与流程

本发明涉及机器人导航定位,尤其涉及一种基于深度强化学习的esikf测量协方差自适应调整方法、系统及相关设备。

背景技术:

1、在现有livo系统(如fast-livo2、r3live、lvi-sam)中,esikf框架用于融合lidar几何测量、视觉光度测量和imu惯性测量。在esikf的迭代过程中,测量噪声协方差矩阵r(包括lidar测量协方差r_lio和视觉测量协方差r_vio)直接决定卡尔曼增益k的计算,从而控制各传感器测量在状态更新中的权重。然而,现有系统中这些协方差矩阵通常采用预设的固定值,存在以下关键缺陷:

2、(1)固定协方差无法反映传感器质量的动态变化:当lidar在隧道、长走廊等几何退化场景中测量质量下降时,固定的r_lio无法增大以反映真实的不确定性,导致esikf错误地信任低质量测量,引发位姿估计发散甚至跟踪丢失。

3、(2)固定协方差无法适应视觉传感器的性能波动:当视觉传感器在低光照、弱纹理、高速运动等场景中性能退化时,固定的r_vio无法相应增大,使得视觉约束在退化场景中仍被过度信任,降低系统鲁棒性。

4、(3)固定协方差无法平衡多传感器间的置信度:在不同环境条件下,lidar、视觉和imu三者的测量质量此消彼长,固定协方差无法实现传感器间最优的置信度分配,导致融合效果次优。

5、因此,现有技术还有待于改进和发展。

技术实现思路

1、本发明提供了一种基于深度强化学习的esikf测量协方差自适应调整方法、系统及相关设备,本发明的主要目的在于解决现有技术所存在背景技术中所提到的技术问题。

2、本发明第一方面提供了一种基于深度强化学习的esikf测量协方差自适应调整方法,包括:

3、在esikf的更新周期中,采集当前时刻的多维度环境感知数据,构建状态空间向量;

4、将所述状态空间向量输入预训练的深度强化学习智能体,输出对应当前环境的连续动作向量;

5、将所述连续动作向量映射为协方差调整参数,并结合环境退化方向向量,生成正定缩放矩阵,依据所述正定缩放矩阵对预设的初始lidar测量协方差矩阵和初始视觉测量协方差矩阵进行调整;

6、使用调整后得到的目标lidar测量协方差矩阵和目标视觉测量协方差矩阵,计算esikf迭代更新过程中的卡尔曼增益,并执行lidar几何测量与视觉光度测量的状态更新。

7、在本发明第一方面一种可选的实施方式中,所述状态空间向量包括:

8、lidar几何特征,包括有效特征点数量比、分布均匀性、残差统计及曲率分布;

9、视觉跟踪质量,包括跟踪特征点数量、光度残差、特征分布及梯度强度;

10、imu稳定性,包括零偏稳定性、预积分协方差迹及多传感器一致性;

11、环境退化因子,包括hessian条件数、fisher特征值和退化方向检测;

12、esikf内部状态,包括后验协方差迹、增益范数、迭代次数及残差收敛;

13、历史动作:包括前一时刻的协方差调整动作。

14、在本发明第一方面一种可选的实施方式中,所述连续动作向量包括:

15、控制lidar测量协方差整体缩放的因子alpha_lidar;

16、控制视觉测量协方差整体缩放的因子alpha_visual;

17、控制沿lidar退化方向进行各向异性增强的强度系数rho_lidar;

18、控制沿视觉退化方向进行各向异性增强的强度系数rho_visual。

19、在本发明第一方面一种可选的实施方式中,所述依据所述正定缩放矩阵对预设的初始lidar测量协方差矩阵和初始视觉测量协方差矩阵进行调整包括:

20、调整lidar测量协方差矩阵r_lio的公式为:

21、r_lio=alpha_lidar·d_lio^(1/2)·r_lio_0·d_lio^(1/2)+εi;

22、d_lio=diag(clip(1+rho_lidar·d_lio,i,d_min, d_max));

23、调整视觉测量协方差矩阵r_vio的公式为:

24、r_vio=alpha_visual·d_vio^(1/2)·r_vio_0·d_vio^(1/2)+εi;

25、d_vio=diag(clip(1+rho_visual·d_vio,i, d_min, d_max)) ;

26、其中,alpha>1表示降低对应测量置信度,alpha<1表示提高对应测量置信度;rho用于增强退化方向的不确定性;clip、εi和上下界约束用于保证协方差矩阵始终对称正定并避免卡尔曼增益数值发散。

27、在本发明第一方面一种可选的实施方式中,所述深度强化学习智能体采用双延迟深度确定性策略梯度算法构建,包括:

28、actor网络:用于输入状态向量,并输出连续的确定的协方差调整动作向量;

29、双critic网络:用于在训练阶段评估所述actor网络输出动作的q值,通过双延迟更新机制缓解q值过估计问题。

30、在本发明第一方面一种可选的实施方式中,所述双延迟深度确定性策略梯度算法在训练阶段基于设计的组合奖励函数进行策略更新,所述组合奖励函数如下:

31、r_t=w_1·r_accuracy+w_2·r_convergence+w_3·r_consistency+w_4·r_smoothness+w_5·r_safety;

32、其中,r_accuracy在仿真或带真值数据集中由ate/rpe计算,在真实在线运行中由归一化创新平方、残差白化程度、相邻位姿与imu预积分一致性和局部地图一致性替代;r_convergence基于esikf迭代次数和残差下降速率计算;r_consistency衡量lidar、视觉和imu更新的一致性;r_smoothness惩罚动作大幅跳变;r_safety惩罚协方差越界、滤波发散和更新超时,w_1至w_5为预设的权重系数。

33、在本发明第一方面一种可选的实施方式中,所述actor网络的结构为多层感知机,在实机部署阶段,关闭网络探索噪声,采用确定性策略进行推理;当且仅当系统检测到奖励可信且滤波器未发散时,以预设的低学习率启动在线持续学习,针对高风险样本的经验存入缓冲区进行离线再训练;所述深度强化学习智能体的推理与esikf的状态估计异步执行;协方差调整频率与关键帧频率同步;在非调整周期,对状态空间特征进行缓存与增量更新,以避免重复计算。

34、本发明第二方面提供了一种基于深度强化学习的esikf测量协方差自适应调整系统,所述esikf测量协方差自适应调整系统包括:

35、环境感知模块,用于在esikf的更新周期中,采集当前时刻的多维度环境感知数据,构建状态空间向量;

36、智能体处理模块,用于将所述状态空间向量输入预训练的深度强化学习智能体,输出对应当前环境的连续动作向量;

37、协方差调整模块,用于将所述连续动作向量映射为协方差调整参数,并结合环境退化方向向量,生成正定缩放矩阵,依据所述正定缩放矩阵对预设的初始lidar测量协方差矩阵和初始视觉测量协方差矩阵进行调整;

38、状态更新模块,用于使用调整后得到的目标lidar测量协方差矩阵和目标视觉测量协方差矩阵,计算esikf迭代更新过程中的卡尔曼增益,并执行lidar几何测量与视觉光度测量的状态更新。

39、本发明第三方面提供了一种导航设备,所述导航设备包括:存储器和至少一个处理器,所述存储器中存储有指令,所述存储器和所述至少一个处理器通过线路互连;

40、所述至少一个处理器调用所述存储器中的所述指令,以使得所述导航设备执行如本发明第一方面中任一项所述的基于深度强化学习的esikf测量协方差自适应调整方法。

41、本发明第四方面提供了一种计算机可读存储介质,所述计算机可读存储介质上存储有计算机程序,所述计算机程序被处理器执行时实现如本发明第一方面中任一项所述的基于深度强化学习的esikf测量协方差自适应调整方法。

42、有益效果:本发明提供了一种基于深度强化学习的esikf测量协方差自适应调整方法、系统及相关设备,方法包括在esikf更新周期中,提取多维度环境感知构建状态空间向量,并将其输入预训练的强化学习智能体中,输出连续动作向量;随后,将连续动作变量映射为调整参数,并结合环境退化方向向量,生成正定缩放矩阵,接着,依据正定缩放矩阵对初始lidar测量协方差矩阵和初始视觉测量协方差矩阵进行在线动态调整;最后,利用调整后得到的目标lidar测量协方差矩阵和目标视觉测量协方差矩阵计算卡尔曼增益并执行状态更新。本发明克服了传统固定协方差无法适应传感器性能动态波动的缺陷,在保证数学严密性的同时,提升了系统在复杂及退化场景下的鲁棒性与定位精度。