【技术实现步骤摘要】
本专利技术属于车辆定位，尤其涉及工程车辆位姿感知方法。
技术介绍
1、工程车辆在矿山开采、路桥建设、城市基础设施建设以及抢险救援等关键能源和民生领域中扮演着至关重要的角色。然而，这些车辆常常面临恶劣的工作环境，以及频繁的加速、转向等复杂操作，这对驾驶员的健康和工作效率构成了挑战。为了解决这一问题，实现具备辅助/自主作业能力的智能化工程车辆成为了追求的目标。位姿感知作为关键技术之一，能够获取车辆的位姿信息，使自动驾驶系统能够做出更加安全和智能的驾驶决策。因此，具有较好的定位精度对工程车辆的无人驾驶十分重要。
2、在工程车辆行走装置的位姿感知中，当前多采用多传感器融合的方法来提高感知的精度和稳定性。具体来说，通过使用高频率的imu(惯性测量单元)来补偿低频率的rtk(实时动态差分定位技术)，可以获得更高的测量精度。然而工程车辆的作业行驶速度远低于乘用车，因此在固定解状态下的rtk通常已经能够提供较高的测量精度，但因工程车辆特殊的工作环境，这对其位姿感知带来了额外的挑战。一方面，rtk信号可能会因遮挡而中断；另一方面，在矿山等环境中，磁场干扰可能导致rtk信号出现突变，进而引发信息错误。此外，在工程车辆位姿感知的过程中，还需要考虑车辆的振动以及复杂的工作环境对传感器的影响。同时，为了保证自动驾驶系统的实时性，还需要保证算法的高效率求解。鉴于乘用车的位姿感知方法无法直接应用于工程车辆，我们需要根据工程车辆的特点和作业环境，建立适合其的位姿感知方法。因此，本专利技术提出了工程车辆位姿感知方法。
技术
1、本专利技术的目的在于提供工程车辆位姿感知方法，旨在解决上述
技术介绍
中提出的问题。
2、本专利技术的目的通过以下技术方案得以实现：
3、工程车辆位姿感知方法，包括以下步骤：
4、步骤s1、通过串口通信读取rtk和imu的数据；
5、步骤s2、使用均值滤波方法对读取的数据进行预处理；
6、步骤s3、判断rtk的解状态和位姿信息是否准确；
7、步骤s4、根据imu/轮速计组合定位模型建立系统的状态方程；
8、步骤s5、基于自适应扩展卡尔曼滤波计算出位姿信息。
9、进一步的，所述步骤s1中，imu由加速度计和陀螺仪组成，通过下式校准系统误差：
10、ao＝taka(αs+ba)
11、ωo＝tgkg(ωs+bg)
12、式中，ao和ωo分别表示理想的测量加速度和角速度值；ta和tg分别表示加速度和角速度旋转引起的误差；ka和kg分别表示加速度和角速度的比例；as和ωs分别表示实际测量的加速度和角速度值；ba和bg分别表示加速度和角速度偏移所引起的误差。
13、进一步的，所述步骤s2的具体过程如下：
14、对加速度和角速度进行平滑处理，以减少振动带来的噪声；采用移动平均滤波的方式对数据进行处理，如下式所示：
15、
16、式中，x表示输入信号，y表示输出信号，n表示当前计算点，n表示样本大小。
17、进一步的，所述步骤s3的具体过程如下：
18、根据rtk的状态，以及与上一时刻的数据进行对比，设定条件(xi-xi-1)2+(yi-yi-1)2<d，其中xi表示当前时刻i的横向位置，xi-1表示上一时刻i-1的横向位置，yi表示当前时刻i的纵向位置，yi-1表示当前时刻i-1的纵向位置，d表示位置误差；当满足条件时，直接输出rtk测得的数据，当不满足条件时，使用自适用卡尔曼滤波的方法进行处理，将imu/轮速计组合定位信息作为扩展卡尔曼滤波的预测信息，将rtk获得的位姿信息作为扩展卡尔曼滤波器的观测信息。
19、进一步的，所述步骤s4的具体过程如下：
20、根据imu/轮速计组合定位模型建立系统的状态方程如下式所示：
21、
22、式中，xk+1为k+1时刻车辆的横向位置，yk+1为k+1时刻车辆的纵向位置，θk+1为k+1时刻车辆的航向角，xk为k时刻车辆的横向位置，yk为k时刻车辆的纵向位置，t为采样时间，θk为k时刻车辆的航向角；vk为k时刻工程车辆的速度，由安装在驱动电机轴上的轮速编码器测得；ωk为k时刻工程车辆的航向角速度，通过imu获得；ωv为速度噪声；ωω为角速度的噪声；
23、将系统的过程噪声用向量wk表示，则wk＝[wv ωω]t，协方差为q，xk为系统状态向量，表示车辆的位姿信息，xk＝[xk yk θk]；通过泰勒级数展开将状态方程处线性化为公式的形式，是扩展卡尔曼滤波中的预测状态估计值，其中状态方程中的外部输入影响矩阵ak+1为零，雅可比矩阵fk+1和系统过程噪声矩阵γk分别表示为：
24、
25、式中，为航向角估计值。
26、进一步的，所述步骤s5的具体过程如下：
27、使用雅可比矩阵fk+1、系统过程噪声矩阵γk和协方差q，计算出状态误差协方差的先验估计为：
28、
29、式中，pk为状态估计误差协方差矩阵；
30、将rtk测量的数据作为系统的观测数据zk，zk＝[xrtk,kyrtk,kθrtk,k]，其中xrtk,k为k时刻rtk测量的横向位置，yrtk,k为k时刻rtk测量的纵向位置，θrtk,k为k时刻rtk测量的航向角，得到观测矩阵h为：
31、
32、计算出卡尔曼增益kk+1为：
33、
34、式中，e为观测噪声的协方差矩阵；
35、目标状态的后验估计为下式：
36、
37、式中，为目标状态的先验估计，zk+1为k+1时刻的观测矩阵；
38、误差协方差的后验估计pk+1为下式：
39、
40、利用自适应估计方法来估计ekf中的过程噪声协方差q和观测噪声协方差r，根据理论值调整残差的协方差矩阵；
41、残差εk是实际测量值与后验估计之间的差值，由下式计算得出：
42、
43、令sk为测量残差的协方差，则rk表示为：
44、
45、式中，rk为协方差矩阵；使用基于残差的自适应方法来估计rk，公式如下式所示：
46、
47、式中，为残差的实际协方差矩阵，e为数学期望，εk为残差，vk为测量噪声，在卡尔曼滤波中
48、引入遗忘因子α来估计rk，如下式所示：
49、
50、式中，rk-1为k-1时刻的协方差矩阵；
51、令dk为测量值与预测值之间的偏差，其中为扩展卡尔曼滤波中的预测状态先验估计值，得到：
52、
53、wk-1＝xk-φ(xk-1,uk-1)
54、
55、式中，φ(xk,uk)为状态转移函数，wk-1为k-1时刻的过程噪声，xk为k时刻的系本文档来自技高网...
【技术保护点】
1.工程车辆位姿感知方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的工程车辆位姿感知方法，其特征在于，所述步骤S1中，IMU由加速度计和陀螺仪组成，通过下式校准系统误差：
3.根据权利要求2所述的工程车辆位姿感知方法，其特征在于，所述步骤S2的具体过程如下：
4.根据权利要求3所述的工程车辆位姿感知方法，其特征在于，所述步骤S3的具体过程如下：
5.根据权利要求4所述的工程车辆位姿感知方法，其特征在于，所述步骤S4的具体过程如下：
6.根据权利要求5所述的工程车辆位姿感知方法，其特征在于，所述步骤S5的具体过程如下：
【技术特征摘要】
1.工程车辆位姿感知方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的工程车辆位姿感知方法，其特征在于，所述步骤s1中，imu由加速度计和陀螺仪组成，通过下式校准系统误差：
3.根据权利要求2所述的工程车辆位姿感知方法，其特征在于，所述步骤s2的具体过程如下：
【专利技术属性】
技术研发人员：王永，姚宗伟，毕秋实，李学飞，刘昕晖，潘宝智，
申请(专利权)人：吉林大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。