一种轮式移动机器人精确定位方法技术

本发明专利技术公开了一种轮式移动机器人精确定位方法，首先初始化系统模型，之后使用IMU数据进行状态预测，接着根据接收到的传感器数据类型更新系统状态向量以及协方差矩阵，最后持续不断循环这个流程。本发明专利技术的方法在常规MSCKF算法执行过程之中，特别是接受图像信息的间隙中，通过处理相对高频的轮式里程计信息和IMU信息，修正常规MSCKF在IMU预测步骤中产生的误差，有效降低移动机器人的闭环误差，在接受图像信息之后，使用更加准确的状态向量进行更新，在没有增加复杂计算的情况下加强常规MSCKF的算法鲁棒性，充分利用三种传感器的信息的互补性，提高移动机器人定位算法的鲁棒性，加强移动机器人定位精度。加强移动机器人定位精度。加强移动机器人定位精度。

【技术实现步骤摘要】
一种轮式移动机器人精确定位方法


[0001]本专利技术属于移动机器人定位
，具体涉及一种轮式移动机器人精确定位方法。

技术介绍

[0002]在移动机器人定位
中，移动机器人用于定位与探索的传感器有惯性导航单元(Inertial Measurement Unit，IMU)、相机、激光雷达等。由于单个传感器性能的局限性，通常要使用多传感器融合的方法进行机器人定位。视觉惯性里程计(Visual
‑
Inertial Odometry，VIO)是一种以视觉和IMU融合的方法实现定位的里程计，按照融合方式可以分为松耦合和紧耦合两种方法，紧耦合相对于松耦合充分利用了传感器信息，可以实现更好的效果，也是目前VIO发展的研究重点。
[0003]常用VIO的紧耦合方法可以分为基于优化的VIO算法，如VINS，OKVIS等，以及基于滤波的VIO算法，如MSCKF，ROVIO等。相较于基于滤波的VIO算法，MSCFK精度相当，速度更快，更适合在计算资源有限的平台上使用。由于IMU输出频率比相机输出频率高，在MSCKF流程中需要处理多帧IMU数据才能融合一帧图像数据，在处理连续多帧IMU数据时会带来一定的误差，而且在MSCKF(Multi
‑
State Constraint Kalman Filter，MSCKF)框架中对机器人位置的估计使用了IMU中的加速度计，其中的重力加速度也会导致MSCFK的精度有一定下降，从而影响移动机器人定位结果。

技术实现思路

[0004]为解决上述技术问题，本专利技术提出了一种轮式移动机器人精确定位方法。
[0005]为方便描述
技术实现思路
，首先进行数学符号的定义：
[0006]1.坐标系定义：
[0007]G：世界坐标系，也叫做地面(ground)坐标系，通常定义为算法初始化时刻的机器人的“前右下”坐标系或“北东地”坐标系。
[0008]I：IMU坐标系，IMU加速度计、角速度计的测量坐标系。
[0009]B：Body坐标系，通常是机器人的“前右下”坐标系，通常将B系定义为I系。
[0010]C：Camera坐标系，用来表示相机状态，因为有多个历史相机状态，所以有下标，其中，C
i
表示第i个。
[0011]2.物理量定义：
[0012]四元数，表示A坐标系到B坐标系的旋转，向量长度为4。
[0013]表示旋转的误差量，通常会带上波浪号上标，向量长度为3。
[0014]A
v
B
：A坐标系下物体B的速度，向量长度为3。
[0015]A
p
B
：A坐标系下物体B的位置，向量长度为4。
[0016]b
a
，b
g
：分别表示IMU中加速度计和陀螺仪的零偏，向量长度为3。
[0017]3.估计量定义，对于任意变量，不同上标有不同含义，例如变量x表示真实值，在实
际中真实值一般未知；表示估计值，在本专利技术中数据处理后的结果为估计值；表示误差值，三者关系为：
[0018][0019]4.旋转的表示，设为上述定义的四元数，则表示四元数对应的旋转矩阵，表示旋转误差量，也可写成δθ，E表示单位矩阵，操作符表示其操作向量对应的反对称阵，四元数定义采用JPL形式，δθ量统一采用左乘。
[0020]5.观测量定义，用右下标m表示，IMU中加速度计测量的加速度a
m
，陀螺仪测量的角速度ω
m
，具体为：
[0021]ω
m
＝ω+b
g
+n
g
[0022][0023]其中，ω为IMU系的角速度，维度为3，即ω＝[ω
x ω
y ω
z
]T
，其中三项分别为绕x、y、z三轴的角速度，T表示矩阵转置，n
g
为陀螺仪测量误差，
G
a
I
为G系加速度，维度为3，即
G
a
I
＝[
G
a
x G
a
y G
a
z
]T
，其中三项分别为x、y、z三轴的线加速度，
G
g为重力加速度，n
a
为加速度计测量误差，表示四元数q对应的旋转矩阵G系到I系的旋转，表示四元数qG系到I系的旋转。
[0024]本专利技术的技术方案：一种轮式移动机器人精确定位方法，具体步骤如下：
[0025]S1、初始化系统模型，根据物理关系，构建系统状态向量X和系统误差状态向量状态向量X中包含IMU状态量X
I
和N个相机历史位姿其中，X
I
的维度是16，的维度是15，的维度为15+6N；
[0026]S2、IMU状态预测，构建系统误差向量中IMU分量的误差状态方程、以及滤波器的状态协方差矩阵P
k|k
，之后每收到一帧IMU数据则进行一次预测，预测系统状态向量中IMU分量以及滤波器的协方差矩阵；
[0027]S3、处理方式决策，在每个时刻根据接收到的数据类型建立不同的观测方程，若当前时刻接收到轮式里程计信息则执行步骤S4，若接收到图像信息或同时接收到两个传感器信息则执行步骤S5，若机器人停止工作或断电，则结束算法；
[0028]S4、建立里程计信息观测模型，并更新状态向量与协方差矩阵，使用轮式里程计的位置观测量构建状态向量残差观测量的观测方程，使用EKF框架更新状态向量以及协方差矩阵，最后重复执行步骤S2；
[0029]S5、处理图像信息，使用特征点检测以及光流追踪法处理图像信息，跟踪、匹配之前图像中的特征点以及获取新一帧图像的特征点，之后将新一帧图像的位姿pose加入到状态向量，并扩充协方差矩阵；
[0030]S6、根据特征点跟踪结果建立图像信息观测模型，并更新状态向量与协方差矩阵，对于在步骤S5丢失跟踪的特征点使用特征点三角化计算特征点的3D坐标，结合历史图像信息建立约束关系，使用该约束关系建立状态向量残差观测方程，最后更新状态向量以及协方差矩阵；
[0031]S7、修正系统状态向量与协方差矩阵维度，对于更新之后的状态向量，如果状态向量中相机状态个数大于N，则剔除距离当前时刻最久的相机状态以及对应的协方差，最后重
复执行步骤S2。
[0032]进一步地，步骤S2的具体流程如下：
[0033]S21、IMU的误差状态向量定义为则误差状态方程为：
[0034][0035]其中，F是状态转移矩阵，G
I
是噪声转移矩阵，n
I
是系统噪声向量，表示对时间的积分，滤波器的协方差矩阵为：
[0036][0037]其中，为15*15的IMU协方差矩阵，为IMU与相机位姿的协方差相关矩阵，为6N*6N的相机位姿协方差矩阵。<本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种轮式移动机器人精确定位方法，具体步骤如下：S1、初始化系统模型，构建系统状态向量X和系统误差状态向量状态向量X中包含IMU状态量X
I
和N个相机历史位姿S2、IMU状态预测，构建系统误差向量中IMU分量的误差状态方程、以及滤波器的状态协方差矩阵P
k|k
，之后每收到一帧IMU数据则进行一次预测，预测系统状态向量中IMU分量以及滤波器的协方差矩阵；S3、处理方式决策，在每个时刻根据接收到的数据类型建立不同的观测方程，若当前时刻接收到轮式里程计信息则执行步骤S4，若接收到图像信息或同时接收到两个传感器信息则执行步骤S5，若机器人停止工作或断电，则结束算法；S4、建立里程计信息观测模型，并更新状态向量与协方差矩阵，使用轮式里程计的位置观测量构建状态向量残差观测量的观测方程，使用EKF框架更新状态向量以及协方差矩阵，最后重复执行步骤S2；S5、处理图像信息，使用特征点检测以及光流追踪法处理图像信息，跟踪、匹配之前图像中的特征点以及获取新一帧图像的特征点，之后将新一帧图像的位姿pose加入到状态向量，并扩充协方差矩阵；S6、根据特征点跟踪结果建立图像信息观测模型，并更新状态向量与协方差矩阵，对于在步骤S5丢失跟踪的特征点使用特征点三角化计算特征点的3D坐标，结合历史图像信息建立约束关系，使用该约束关系建立状态向量残差观测方程，最后更新状态向量以及协方差矩阵；S7、修正系统状态向量与协方差矩阵维度，对于更新之后的状态向量，如果状态向量中相机状态个数大于N，则剔除距离当前时刻最久的相机状态以及对应的协方差，最后重复执行步骤S2。2.根据权利要求1所述的一种轮式移动机器人精确定位方法，其特征在于，所述步骤S2中，具体步骤如下：S21、IMU的误差状态向量定义为误差状态方程为：其中，F是状态转移矩阵，C
I
是噪声转移矩阵，n
I
是系统噪声向量，表示对时间的积分，滤波器的协方差矩阵为：其中，为15*15的IMU协方差矩阵，为IMU与相机位姿的协方差相关矩阵，为6N*6N的相机位姿协方差矩阵；S22、IMU分量中姿态四元数预测采用0阶积分，速度和位置预测采用Euler积分，加速度计零偏b
a
、陀螺仪零偏b
g
保持不变；S23、IMU数据协方差矩阵预测迭代公式如下：
其中，下标k+1|k表示k时刻进行预测k+1时刻的变量值，当前Φ(t
k
+t，t
k
)根据下式微分方程数值积分计算而成，初值Φ(t
k
，t
k
)＝E，t
k
表示开始接收IMU信息的时刻，t表示两帧数据的时间间隔，E为单位矩阵，为Φ(t
k
+t，t
k
)对时间的微分：3.根据权利要求1所述的一种轮式移动机器人精确定位方法，其特征在于，所述步骤S4中，具体步骤如下：建立观测模型，设轮式里程计位置观测为p
m
，构建残差观测模型r＝HX，H是观测矩阵，更新流程如下：H＝[O3×
3 O3×
3 O3×
3 O3×
3 E3×
3 O3×
6N
]K＝P
k+1|k
H
T
(HP
k+1|k
H

【专利技术属性】
技术研发人员：邵继业，杨文章，彭倍，葛森，
申请(专利权)人：四川阿泰因机器人智能装备有限公司，
类型：发明
国别省市：