基于误差状态卡尔曼滤波器的传感器参数校准及状态估计装置与方法与流程

本发明属于惯性导航和状态估计，尤其涉及一种基于误差状态卡尔曼滤波器的传感器参数校准及状态估计装置与方法。

背景技术：

1、随着科技的不断进步，自动驾驶技术成为人工智能领域中备受瞩目的应用之一，其发展潜力巨大。在自动驾驶车辆和智能机器人的发展过程中，确保车辆精准、可靠的定位至关重要，因为这决定了导航、碰撞预警和自动停车等功能的实现。传统的定位技术主要依赖于全球导航卫星系统(gnss)和惯性测量单元(imu)的组合使用。然而，在城市峡谷、树林、地下停车场和隧道等复杂环境中，gnss信号可能会受到遮挡，这给车辆定位带来了挑战。为了提高定位精度和鲁棒性，需要开发新的多传感器融合定位系统，并设计新的滤波器以确保系统实时性的同时提高准确性。同时，常用的滤波器方法多为如卡尔曼滤波及其变体，极其依赖于准确的初始传感器参数校准，需要改进以提高系统的稳定性和鲁棒性。

2、因此现有技术需要一种基于多传感器融合的传感器参数估计与状态估计的定位方法，来解决传感器参数变化导致的定位精度和鲁棒性差的问题。同时需要设计一种新的滤波器，来保证定位系统良好的实时性和鲁棒性。

技术实现思路

1、针对上述现有技术存在的不足，本发明提供一种基于误差状态卡尔曼滤波器的传感器参数校准及状态估计装置与方法，旨在解决机器人及智能车辆定位精度和鲁棒性差的问题，同时保证定位系统具有良好的实时性。

2、为了解决上述问题，本发明的一种基于误差状态卡尔曼滤波器的传感器参数校准及状态估计装置与方法的技术方案如下：

3、一种基于误差状态卡尔曼滤波器的传感器参数校准及状态估计装置，包括传感器输入模块、基于特征提取的激光雷达位姿估计模块、传感器数据融合模块、误差传播模块、协方差矩阵更新模块、传感器参数校准模块以及位姿输出模块；

4、所述传感器输入模块用于测量三轴加速度和角速度、车辆瞬时速度、激光雷达数据序列以及位置和速度观测数据；

5、所述基于深度学习的雷达位姿估计模块用于接收激光雷达发送的激光雷达序列，利用神经网络对激光雷达序列进行处理，从而得到雷达的位姿，并将最终得到的雷达位姿发送给传感器数据融合模块；

6、所述传感器数据融合模块用于接收传感器模块发送的三轴加速度和角速度，以及基于深度学习的雷达位姿估计模块发送的相机的位姿，以及传感器模块发送的位置和速度观测数据；对三轴加速度和角速度进行预测处理得到预测结果并将其发送给误差传播模块；对雷达的位姿、位置和速度观测数据进行处理得到观测方程并将其发送给误差传播模块；所述误差传播模块用于接收传感器数据融合模块发送的预测状态，利用误差状态动力学模型得到误差状态向量并将其发送给更新模块；

7、所述协方差矩阵更新模块用于接收传感器数据融合模块发送的传感器观测数据以及预测状态向量，接收误差传播模块发送的误差状态向量，并将误差状态向量与预测状态向量进行广义求和，输出最终的位置速度和姿态信息；

8、所述传感器参数校准模块用于更新状态向量中的协方差矩阵p；

9、所述位姿输出模块用于使用传感器测量的残差来更新真实状态。

10、进一步的，所述传感器模块包括imu、轮速里程计、激光雷达和gps四种传感器；所述imu用于测量三轴加速度和角速度并将测量结果发送给传感器数据融合模块；所述轮速里程计用于获取车辆瞬时速度并发送给传感器数据融合模块；所述激光雷达用于获取激光雷达数据序列并将其发送给激光雷达位姿估计模块；所述gps用于接收位置和速度观测数据并将接收到的位置观测数据发送给传感器数据融合模块。

11、进一步的，所述基于深度学习的雷达位姿估计模块包括测距网络、掩膜网络和激光雷达损失函数模块，所述测距网络，采用resnet50网络作为第二编码器和全连接层模块，负责处理激光雷达序列并提取特征，生成激光雷达特征；全连接层模块包含两个串联的全连接层，接收激光雷达特征和注意力模块发送的特征重要性权重，通过全连接层完成位姿回归，得到激光雷达位姿；

12、所述掩膜网络，采用transformer架构的编码器和解码器，通过self-attention自注意力机制处理激光雷达序列，检测连续帧中的一致性区域，生成预测掩膜；

13、所述激光雷达损失函数模块，接收激光雷达位姿、预测掩膜和融合位姿，计算激光雷达的强度损失函数和预测掩膜的交叉熵损失函数；分配缩放因子后得到最终的激光雷达损失函数。

14、本发明还公开了一种基于误差状态卡尔曼滤波器的传感器参数校准及状态估计装置的传感器参数校准及状态估计方法，包括以下步骤：

15、步骤1：对车载传感器进行粗标定，包括空间标定和时间标定，车载传感器指imu、轮速里程计、激光雷达和gps；

16、步骤2：利用标定好的激光雷达获取激光雷达序列，并根据激光雷达序列，使用神经网络估计激光雷达位姿。

17、进一步的，所述步骤2包括以下步骤：步骤2.1：将激光雷达序列输入测距网络和掩膜网络，通过测距网络提取特征，得到激光雷达特征，通过掩膜网络检测激光雷达序列的连续帧中的一致性区域，得到预测掩膜；

18、步骤2.2：将得到的激光雷达特征输入第三编码器和softmax激活函数，得到特征重要性权重；

19、步骤2.3：将特征重要性权重分配给特征，利用全连接层模块进行位姿回归，得到激光雷达位姿；

20、步骤2.4：将得到的激光雷达位姿输入多层感知机，得到融合位姿；

21、步骤2.5：根据激光雷达位姿和融合位姿计算激光雷达的损失函数，根据预测掩膜和融合位姿计算预测掩膜的损失函数。

22、进一步的，所述激光雷达的强度损失函数为

23、

24、其中，ll表示激光雷达的强度损失函数；ms(xt)表示从源帧到目标帧的误差权重；it(xx)表示目标帧集合；表示重建的源帧集合；xt表示目标帧；n表示激光雷达序列的总帧数；n表示激光雷达序列的第一帧。

25、进一步的，所述预测掩膜的交叉熵损失函数为：

26、

27、其中，im表示预测掩膜的交叉熵损失函数；p(ms(xt)＝1)表示当目标帧的预测掩膜为1时的交叉熵。

28、进一步的，包括机器人状态量的计算，所述机器人状态量为

29、

30、其中wq是旋转角度，wp是机器人在坐标系下的位置，wv是机器人在全局坐标系下的速度，ba，bg分别是imu测量加速度和角速度的偏差，vqimu是imu的角度安装误差；观测量的估计值与测量值关系如下：

31、

32、其中表示四元数的增量运算，exp表示指数映射函数，将李代数中的元素映射到一个旋转矩阵，与标准的矩阵指数函数相符合。g是已知的重力向量，g的值是9.79m/s2。am和ωm是来自imu的测得的线性加速度和角速度，dt是两个imu时间戳之间的时间间隔；

33、根据各观测量的估计值与实际值，得到所述误差状态的传播模型为：

34、

35、其中是上一时间戳的更新状态，是当前时间戳的预测状态。函数f是状态预测函数，矩阵f是线性化系统的预测雅可比矩阵，向量ω是噪声部分；

36、所述传播模型中的传递矩阵f为

37、

38、所述误差传播模块中的误差状态协方差为：

39、ppred＝fpft+uqut

40、其中矩阵ppred是在当前预测时间戳的预测协方差矩阵，p是在上一次更新时间戳的更新协方差矩阵，u和q分别是噪声矩阵和扰动量的雅可比矩阵：

41、

42、其中是由imu决定的或来自实验测量的噪声方差。

43、进一步的，所述协方差矩阵更新模块使用传感器的观测值来更新状态，定义所述传感器中的轮速编码器在车体坐标系中测量速度定义测量函数h(x)来对齐车体坐标系中的速度：

44、所述协方差矩阵更新模块中对轮速的更新部分为：

45、y＝z-h(x)＝wvm-wq-1wv

46、

47、其中wv是机器人在世界坐标系中的速度；

48、所述协方差矩阵更新模块中对gnss的更新部分为：

49、y＝z-h(x)＝wpm-(wp+wqpgnss)

50、

51、其中pgnss是车辆和gnss天线之间的天线安装偏差，这个外参数在安装天线时进行测量；wpm是gnss在世界坐标系中测量车辆位置；

52、所述协方差矩阵更新模块中对激光雷达测量的更新部分为：

53、y＝z-h(x)＝wqmmpl-(wp+wqplidar)

54、

55、其中plidar是车辆和激光雷达之间的安装偏差，这个外参数可以在安装时进行测量；

56、mpl是激光雷达在地图坐标系中测量车辆位置。

57、进一步的，所述传感器参数校准模块，用于更新状态向量中的协方差矩阵p，其更新方程为：

58、

59、s＝hpht+r

60、p＝(i-kh)ppred

61、k＝fphts-1

62、其中矩阵r为各个传感器的测量噪声；

63、所述位姿输出模块，用于使用传感器测量的残差来更新真实状态：

64、

65、其中为名义状态，为真实状态，ky为残差；

66、取出所述真实状态向量中的姿态估计、位置估计和速度估计并输出，

67、即可得到最终融合后的定位结果：

68、

69、其中，q，p，v分别为最终的姿态估计、位置估计和速度估计。

70、本发明的有益效果包括：

71、1.提高定位精度和鲁棒性：通过采用eskf方法，结合多传感器融合技术，利用传感器观测量和车辆动力学模型进行状态估计，从而提高了定位的精度和鲁棒性。即使在部分观测信息缺失或传感器参数不准确的情况下，系统仍能够有效地实现定位任务。

72、2.自适应性强：本发明采用具有参数校准能力的误差状态卡尔曼滤波算法，能够根据传播误差的自动调整滤波器的协方差矩阵，从而保证定位系统参数校准和状态估计的精度和实时性。这使得系统能够适应不同环境和工况下的变化，具有较强的自适应性。

73、3.提升用户体验：通过准确的定位结果，本发明的方法和装置可以为用户提供更可靠、更精准的定位服务，提升了用户的使用体验。无论是在车辆导航、无人驾驶或其他定位应用中，都能够更好地满足用户需求。

74、通过上述设计，本发明的带有参数校准功能的鲁棒高精度定位系统可以实现准确的传感器参数和状态估计，适用于各种定位应用场景，包括但不限于智能车辆导航、智能交通系统、机器人室内定位等领域，具有广泛的应用前景和市场潜力。