一种智能汽车运动多传感器数据处理及融合方法

本发明属于自动驾驶领域，涉及一种智能汽车运动多传感器数据处理及融合方法。

背景技术：

1、智能汽车多传感器数据融合(multi-sensor information fusion,msif)是一种利用计算机科学技术将来自各个传感器或多源头的信息和数据在预定的规则下进行数学分析加上信息综合，以完成相应的决策和必要的估算而执行的信息处理过程。这个过程中，要充分地利用多源数据进行合理操作与使用，信息融合不但运用了多个传感器互相协同工作的优势，并且运用算法综合处理了大量其它信息源数据，使得整个传感器系统更加智能。多传感器数据融合是目标跟踪的重要组成部分，通过融合多个传感器的测量值，系统可以获得比单传感器情况更好的跟踪性能。

2、现有的采用多传感器融合技术对智能汽车进行gnss定位，在环境干扰下精度较低，加速度计和陀螺仪采集的imu数据也不准确。另外，虽然多传感器数据融合可以提高系统的性能和可靠性，但由于不同传感器之间的差异和不确定性，可能会导致融合结果的不准确性和不可靠性。多传感器数据融合需要设计和实现相应的算法，以实现对多源数据的合理融合和处理。然而，由于不同传感器之间的数据质量和特征差异，以及算法设计和实现上的难度和复杂性，可能会导致算法的效率和精度不高。多传感器数据融合需要处理大量的实时数据，因此需要高效的算法和硬件支持，以确保数据处理的实时性和高效性。多传感器数据融合涉及到多个传感器之间的数据交互和共享，因此需要考虑数据隐私和安全问题，以保护用户的隐私和数据安全。多传感器数据融合需要适应不同的应用场景和需求，因此需要具备可扩展性和灵活性，以支持不同类型的数据源和处理任务。

技术实现思路

1、有鉴于此，本发明的目的在于提供一种智能汽车运动多传感器数据处理及融合方法，解决环境干扰下gnss精度低的问题。

2、为达到上述目的，本发明提供如下技术方案：

3、一种智能汽车运动多传感器数据处理及融合方法，包括以下步骤：

4、s1：将imu原始数据通过误差校正算法得到精准基本数据，包括智能汽车的加速度三轴adc值、陀螺仪三轴adc值、磁罗盘三轴adc值；

5、s2：建立状态方程和观测方程，构建最优估计算法，结合历史数据拟合的实时数据与真实数据，进行惯性导航预测和解算，得到精准惯性导航数据，包括智能汽车俯仰角、横滚角、航向角数据；

6、s3：gnss基于一次线性预测和深度二次预测算法设置频率预测器，滤波和预测gnss原始数据，输出经纬度和高程数据；

7、s4：将经纬度和高程数据，与精准惯性导航数据对比，筛出不合格数据，将误差范围内数据进行互补融合，同时将融合结果反馈于惯性导航解算，实时修正智能汽车的三轴线速度、线加速度、角速度和角加速度信息；

8、s5：滤波校正gnss高频位置信息，得到智能汽车的定位定姿运动参数，再结合传感器测得的温度、湿度和气压信息，得到完善的智能汽车运动参数定位定姿信息。

9、进一步，所述误差校正算法：所述误差校正算法具体包括：

10、静止放置imu t秒，旋转imu使其保持不同的姿态，在某种姿态下，至少保持静态t秒，判断是否旋转imu n次，估计误差参数；

11、判断传感器是否处于静止状态：

12、

13、其中，ξ(tw)为判断值，表示加速度at在时间段tw内的方差；

14、判断的时候将ξ(tw)和阈值ξ(tinit)进行比较，小于则静态，反之动态；

15、加速度计的校准过程是将加速度计静止，然后根据测量值的二范数等于当地重力加速度这一规则进行校准，用静止状态检测算法在采样序列上挑选出静态的测量片段，然后按照下述内容进行校准：

16、(1)加速度的常规校准方法：校准imu的噪声、尺度误差和轴偏差；假设坐标系bf的x轴和坐标系af的x轴重合，且bf的y轴处于af的x轴和y轴的平面上，变换矩阵写为，

17、

18、其中，t为变换矩阵，a表示加速度计，α为向量，x,y,z为三轴；

19、待求解优化参数变为：

20、

21、其中，θacc为优化参数，α为向量，s表示非正交的选准坐标系，a表示加速度计，b表示bias，x,y,z为三轴；

22、定义状态方程：

23、ao＝h(as,θacc)＝taka(as+bs)

24、其中，ao为状态方程，t表示轴偏差的变换矩阵，k表示尺度因子；

25、优化的代价函数为：

26、

27、将m组旋转量来放入代价函数中求解待求解参数，选取残差最小的一组所对应的参数；

28、(2)六面法校准加速度计

29、建立加速度计的校准模型：

30、

31、其中at表示最终的真实值，am*表示测量值，offsetxyz表示测量中的零偏，r[3*3]表示旋转，scalex表示尺度缩放，将该式做变换得：

32、

33、再将其变为齐次坐标形式：

34、

35、再将其转置：

36、

37、采用最小二乘求解，写作β＝(xtx)-1xty，其中β为上面的4*3矩阵，x是左边的测量值，y是右边的真值，把加速度计按三个轴向6个位置放置，从而对加速度计进行校准；

38、陀螺仪校准：allan方差校准陀螺仪bias

39、在imu校准的最开始，将imu静止搁置时间tinit；在allan方差分析中，共有5个噪声参数：量化噪声、角度随机游走、零偏不稳定性、速度随机游走和速度爬升；

40、a.计算allan方差

41、①将陀螺仪静止放置时间t，单个采样周期为τ0，共有n组采样值；

42、②计算单次采样输出角度θ和平均因子m；

43、θ(n)＝∫τ0ω(n)

44、

45、③计算allan方差，当m取不同的值的时候会有不同的allan方差值：

46、

47、其中，k表示尺度因子；

48、b.计算各部分噪声参数

49、计算出的allan方差是各部分误差的平方和：

50、

51、其中，σ为高斯白噪声的方差，值越大，表示噪声程度越大；q、n、b、k、r分别表示五个参数：量化噪声、角度随机游走、零偏不稳定性、速度随机游走和速度爬升；

52、整理得：

53、

54、根据τ和allan方差值，依据最小二乘法进行拟合，进而得到ci的值，最终得到相应的误差系数：

55、

56、进一步，步骤s2所述建立状态方程和观测方程如下：

57、

58、xk为当前状态的值，即状态方程，uk为每次信号的输入，wk为过程噪声，a为状态转移矩阵，b为控制矩阵；yk为观测方程，vk为观测噪声，包括陀螺仪误差、gnss接收机噪声、加速度计误差、卫星空间位置的误差。

59、进一步，步骤s2所述构建最优估计算法，结合历史数据拟合的实时数据与真实数据，进行惯性导航预测和解算，得到精准惯性导航数据，包括智能汽车俯仰角、横滚角、航向角数据，具体包括：

60、使用上一时刻的最优结果预测这一时刻的预测值，同时使用这一时刻传感器测得的观测值修正这一时刻预测值，得到这一时刻的最优结果：

61、

62、

63、其中，xt为这一时刻的最优结果，f为状态函数，xt-1为上一时刻的最优结果，b为控制矩阵，ut-1为上一次信号的输入；

64、根据上一时刻最优估计值协方差和过程噪声方差q求得该时刻的预测值协方差与置信度kt：

65、

66、

67、其中，为预测值协方差，f为状态函数，pt-1为上一时刻最优估计值协方差，q为过程噪声方差，kt为置信度，h为观测阵，r为误差校正值；

68、根据该时刻的预测值、观测值、置信度推出这一时刻的最优估计值与其协方差：

69、

70、

71、其中，xt为这一时刻的最优估计值，kt为置信度，h为观测阵，zt为该时刻的预测值；pt为这一时刻的协方差，i为校正值，为预测值协方差。

72、进一步，步骤s3中，首先用第一个状态矢量的测量值来刷新所有的预测值和实际测量值，并用这些值来预测下一个的位置；对于没有定位数据输出的时刻，用有数据输出时刻估计的线性和二次预测系数外推定位数据，并进行凸线性联合预测得到没有输出数据时的定位数据；对于有定位数据输出的时刻，用有数据输出时刻估计的线性和二次预测系数，并进行凸线性联合预测得到有输出数据时的定位数据；

73、设gps定位经纬数据分别为λ(t),ψ(t)，滤波预测器的状态矢量为θ(t)＝λ(t)，ψ(t)，位置状态矢量的n点线性多项式滤波预测器的一般形式为：

74、θt＝a0+a1t

75、式中，a0，a1为预测器的系数，t为时间，其最小方差估计为

76、θt(tk丨tk)＝a0+a1tk

77、其中，n＝2，tk表示k点时的时间，k＝0,1：

78、

79、在tk+1时刻gnss位置状态矢量预测值为：

80、θt(tk+1丨tk)＝a0+a1tk+1

81、gnss位置状态矢量五点二次多项式滤波预测器：

82、θq(t)＝b0+b1t+b2t2

83、

84、

85、其中n＝5，θt(tk)表示线性预测位置，tk表示k点时的时间，k＝0,1,2,3,4；

86、在tk+1时刻gnss位置状态矢量预测值为：

87、

88、θq＝α1θt+α2θq

89、

90、其中，θ为最终gps位置状态矢量，σ(θl)和σ(θq)分别由前面五个gnss的线性预测位置和二次预测位置与实际位置的累计平方差求得。

91、进一步，步骤s3具体包括以下步骤：

92、s31：初始化真实姿态定位θ(tk)，线性预测位置θt(tk)，二次预测位置θq(tk)，k＝0,1,2,3,4；

93、s32：在有gnss输出数据时刻，计算线性滤波预测系数a0和a1，以及tk+1时刻两点线性预测值；计算二次滤波预测系数b0，b1，b2，计算tk+1时刻五点二次预测值；

94、s33：计算线性凸联合系数α1和α2，最后计算tk+1时刻的两点线性和五点二次凸联合预测值。

95、进一步，步骤s4中，将gnss的误差作为imu的偏差阈值，在imu出现偏差时，gnss实时地将估计点进行校正；

96、以gnss定位信息为圆心，以偏差阈值作为半径画圆，即为gnss定位信息的误差范围，当imu解算的位置位于gnss定位信息的误差范围内时，系统完全相信imu的解算；当imu解算的位置位于gnss定位信息的误差范围之外时，将imu解算的位置点与圆心进行连线，连线与误差圆的交点即为估计的坐标点。

97、进一步，步骤s5中，完善的智能汽车运动参数定位定姿信息具体包括经纬度、高程、速度，航向、横滚、俯仰三轴向的角度、角速度、速度、加速度，以及温度、湿度和气压。

98、本发明的有益效果在于：本发明将gnss的误差作为imu的偏差阈值，在imu出现偏差时，gnss能实时地将估计点拉回来，这样做增强了系统的抗干扰能力，gnss校准imu的误差，同时imu也可以校准gnss的误差，大大提高了系统的定位能力；本发明充分利用了imu解算的可靠性，大大排除gnss漂移的干扰，本方法可使gnss与imu的数据进行互补，最大限度地提升了定位精准度，使运动轨迹重新回到正确的路径上且定位的鲁棒性明显增强。

99、本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。