一种轮式机器人里程计自适应融合矫正方法技术

本发明专利技术公开一种轮式机器人里程计自适应融合矫正方法，本方法先读取轮式机器人惯导IMU的测量信息和轮式机器人里程计的测量信息，然后以惯导IMU的测量信息作为模型预测值，里程计的测量信息作为观测值，利用自适应kalman融合算法对里程计数据进行矫正，并且对矫正后的里程计数据计算轮式机器人行驶路程与激光雷达数据相比较，计算数据的精确度。本发明专利技术充分探索了融合过程中预测值和观测值的变化关系，利用自适应kalman融合算法对里程计数据进行矫正，能够得到轮式机器人准确的位置、速度、加速度信息。速度、加速度信息。

【技术实现步骤摘要】
一种轮式机器人里程计自适应融合矫正方法


[0001]本专利技术涉及传感器数据处理领域，具体是一种结合惯导IMU的轮式机器人里程计自适应kalman融合矫正方法。

技术介绍

[0002]里程计作为测量传感器广泛应用于轮式机器人，然而由于安装、运行环境等因素，里程计的测量数据中存在一定的误差，且这个误差会随着时间的累积而变大，导致轮式机器人的行驶距离计算偏差较大。为了解决这个问题，目前大部分的解决方案是将里程计与GPS等绝对测量传感器进行融合，但是由于民用GPS数据是误差较大，融合出来的数据可能无法使用。而基于惯导IMU的kalman融合方法也比较常见，但是大都是基于固定的协方差矩阵来进行计算kalman增益，融合的效果随着传感器噪声协方差的标定效果而变化。

技术实现思路

[0003]为了解决上述现有技术中存在的技术问题，本专利技术提供一种轮式机器人里程计数据自适应融合矫正方法，结合惯导IMU信息，对轮式机器人里程计数据采用自适应kalman融合算法进行矫正，得到轮式机器人准确的位置、速度、加速度信息。
[0004]为了解决所述技术问题，本专利技术采用的技术方案是：一种轮式机器人里程计自适应融合矫正方法，包括以下步骤：
[0005]S01)、读取轮式机器人惯导IMU的测量信息；
[0006]S02)、读取轮式机器人里程计的测量信息；
[0007]S03)、将惯导IMU的测量信息作为预测值，里程计的测量信息作为观测值，利用自适应kalman融合算法对里程计数据进行矫正；/>[0008]步骤S03)具体为：将惯导IMU的测量数据imu.v作为自适应kalman融合算法的预测值，即：
[0009][0010][0011]其中kalman.state为kalman融合算法的状态矩阵，transform.matrix为状态转移矩阵，imu.x和imu.y分别为IMU的x轴位置信息和y轴位置信息，imu.vx和imu.vy分别为IMU的x轴速度信息和Y轴速度信息，imu.accx和imu.accy分别为IMU的x轴加速度信息和y轴加速度信息；
[0012]惯导IMU的状态协方差矩阵state.uncertainty为：
[0013]state.uncertainty
[0014]＝transform.matrix
·
state.uncertainty
·
transform.matrix
′
[0015]+pricdit.noise，
[0016]其中pricdit.noise为预测噪声，根据惯导IMU与真实值之间的关系确定，
[0017]transform.matrix
′
为状态转移矩阵的逆矩阵；
[0018]里程计传感器的协方差矩阵odom.uncertainty为：
[0019]odom.uncertainty＝
[0020]transform.matrix
·
odom.uncertainty
·
transform.matrix
′
[0021]+odom.noise，
[0022]其中odom.noise为里程计噪声，根据里程计传感器的特性确定；
[0023]融合协方差矩阵novel.uncertainty为：
[0024]novel.uncertainty＝state.uncertainty+odom.uncertainty
[0025]自适应kalman融合算法的kalman增益kalman.gain为：
[0026]kalman.gain＝state.uncertainty
·
novel.uncertainty
′
，
[0027]其中novel.uncertainty
′
为融合协方差矩阵的逆矩阵；
[0028]自适应kalman融合算法的观测值矩阵kalman.observation设置为惯导里程计的数据，即：
[0029][0030]其中odom.x和odom.y分别为里程计x轴位置信息和y轴位置信息，odom.vx和odom.vy分别为里程计x轴速度信息和y轴速度信息，odom.accx和odom.accy分别为里程计x
轴加速度信息和y轴加速度信息；
[0031]则融合最优值kalman.optimstate为：
[0032]kalman.optimstate
[0033]＝kalman.state+kalman.gain
·
[kalman.observation
‑
kalman.state]最后更新kalman融合算法的后验协方差state.uncertainty_为：
[0034]state.uncertainty_＝(I
‑
kalman.gain)state.uncertainty，其中I为单位矩阵。
[0035]进一步的，还包括步骤S04)；
[0036]S04)、根据矫正后的里程计数据计算轮式机器人行驶路程，并与激光雷达数据相比较，计算数据的精确度。
[0037]进一步的，步骤S04)具体为：
[0038]读取激光雷达计算的轮式机器人行驶距离数据lidar.d，计算里程计融合出来的最优值迭代计算轮式机器人的行驶距离：
[0039]其中
[0040]kalman.optimstate[0]为融合最优值位置x轴位置信息，kalman.optimstate[1]为融合最优值位置y轴位置信息，计算矫正精确度accuracy为：
[0041][0042]当accuracy≥0.95时，说明矫正有效。
[0043]进一步的，步骤S01)读取的轮式机器人导IMU的测量信息包括轮式机器人的速度imu.v和加速度imu.a，然后判断对惯导IMU数据进行阈值判断，即：
[0044]如果imu.v≥MaxVelocity，则imu.v＝MaxVelocity；如果
[0045]imu.v≤MinVelocity，则imu.v＝MinVelocity，如果
[0046]imu.a≥MaxAcc，则imu.a＝MaxAcc；如果imu.a≤MinAcc，则imu.a＝MinAcc，其中，MaxVelocity和MaxAcc分别为轮式机器人可行驶的最大速度和最大加速度。
[0047]进一步的，步骤S01)读取的里程计测量信息，包括轮式机器人左轮的转速odom.LeftSpeed、右轮的转速odom.RightSpeed，依据里程计测量的转速计算轮式机器人坐标系下的速度为
[0048]odom.v＝(2π
·
odom.LeftSpeed
·
wheel.r+2π
·
odom本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种轮式机器人里程计自适应融合矫正方法，其特征在于：包括以下步骤：S01)、读取轮式机器人惯导IMU的测量信息；S02)、读取轮式机器人里程计的测量信息；S03)、将惯导IMU的测量信息作为预测值，里程计的测量信息作为观测值，利用自适应kalman融合算法对里程计数据进行矫正；步骤S03)具体为：将惯导IMU的测量数据作为自适应kalman融合算法的预测值，即：步骤S03)具体为：将惯导IMU的测量数据作为自适应kalman融合算法的预测值，即：其中kalman.state为kalman融合算法的状态矩阵，transform.matrix为状态转移矩阵，imu.x和imu.y分别为IMU的x轴位置信息和y轴位置信息，imu.vx和imu.vy分别为IMU的x轴速度信息和Y轴速度信息，imu.accx和imu.accy分别为IMU的x轴加速度信息和y轴加速度信息；计算惯导IMU的状态协方差矩阵state.uncertainty为：其中pricdit.noise为预测噪声，可根据惯导IMU与真实测量值之间的关系确定，transform.matrix
′
为状态转移矩阵的逆矩阵；计算里程计传感器的协方差矩阵odom.uncertainty为：其中odom.noise为里程计噪声协方差，根据里程计传感器的特性确定；我们提出了一种新的融合协方差矩阵novel.uncertainty计算方法，为：novel.uncertainty＝state.uncertainty+odom.uncertainty，根据novel.uncertainty计算自适应kalman融合算法的kalman增益kalman.gain为：
kalman.gain＝state.uncertainty
·
novel.uncertainty
′
，其中novel.uncertainty
′
为融合协方差矩阵的逆矩阵；自适应kalman融合算法的观测值矩阵kalman.observation设置为惯导里程计的数据，即：其中odom.x和odom.y分别为里程计x轴位置信息和y轴位置信息，odom.vx和odom.vy分别为里程计x轴速度信息和y轴速度信息，odom.accx和odom.accy分别为里程计x轴加速度信息和y轴加速度信息；则融合最优值kalman.optimstate为：kalman.optimstate＝kalman.state+kalman.gain
·
[kalman.observation
‑
kalm...

【专利技术属性】
技术研发人员：黄毅，马辰，程瑶，王佩良，
申请(专利权)人：山东新一代信息产业技术研究院有限公司，
类型：发明
国别省市：