一种基于双层视觉的移动机器人多传感器融合定位方法技术

本发明专利技术公开了一种基于双层视觉的移动机器人多传感器融合定位方法，采用新型特征码实现顶部底部摄像头的协同识别定位功能，同时利用获得的位姿信息用于移动机器人的数据融合算法，实现高精度的机器人定位功能，在行驶过程中，顶部摄像头用于识别铺设于地面的特征码，移动机器人能够提前对自身位姿进行纠偏，提高系统的鲁棒性，底部摄像头仅当机器人在运行至特征码上方时工作，用于移动机器人的精确定位以及在旋转过程中的冗余控制，由于摄像头识别特征码在时间上为间歇性的，引入信息更新频率高，同时没有间断的IMU传感器和轮式里程计，利用IMU和里程计提供的信息进行状态预测，通过摄像头间歇性的数据实现数据融合滤波器的状态更新。的状态更新。的状态更新。

【技术实现步骤摘要】
一种基于双层视觉的移动机器人多传感器融合定位方法


[0001]本专利技术属于传感器和定位领域，具体涉及一种基于双层视觉的多传感器融合方法。

技术介绍

[0002]随着我国智能制造技术的发展，各类拥有高智能的移动机器人在人们的生产，生活中得到愈加广泛的应该。作为移动机器人的基础，定位系统在协助完成机器人的移动过程中起到至关重要的作用。
[0003]目前移动机器人在应用领域用于导航和定位的传感器类型多样，例如激光雷达、IMU、UWB、蓝牙、里程计、GNSS、磁力计等。在室外情况下，通常采用成熟的GNSS定位技术，该技术能够获取机器人在地球上的绝对位置坐标，除此之外还有基站定位技术和ip定位(Girb
é
s
‑
Juan V，Armesto L，Hern
á
ndez
‑
Ferr
á
ndiz D，et al.Asynchronous Sensor Fusion of GPS，IMU and CAN
‑
Based Odometry for Heavy
‑
Duty Vehicles[J].IEEE Transactions on Vehicular Technology,2021，70(9)：8617
‑
8626.)。室内定位技术包含多种，常见的定位方案有有激光雷达、IMU、里程计、深度视觉、蓝牙、WIFI以及UWB。由于室内环境的复杂多变，经常存在定位传感器因为遮挡，信号问题出现定位丢失，或者是产生定位较大的定位误差，因此对于室内定位而言，寻求一种稳定的高精度定位方案是十分必要的。目前室内的定位通常采用多传感器融合的方案以提高精确度以及稳定性，在传感器中有多重选择，常见的利用激光雷达、IMU、里程计和UWB作为定位传感器(Lee Y，Lim D.Vision/UWB/IMU sensor fusion based localization using an extended Kalman filter[C]//2019 IEEE Eurasia Conference on IOT，Communication and Engineering(ECICE).IEEE，2019：401
‑
403.)。其中多数融合定位方式中采用激光雷达、以及UWB等高精度传感器以提高定位系统的精度，但使用这些传感器不可避免的就使得定位成本的大幅提高，随着移动机器人的数量增加，其成本提高更加明显。高精定位系统的成本问题成为影响其向中低端领域扩展的一大障碍。对于常见的定位传感器，其获取的信息通常具有连续且固定的采样周期，在可能出现的异步信息处理方案研究较少。

技术实现思路

[0004]为解决所述技术问题，本专利技术提出一种基于双层视觉的移动机器人多传感器融合定位方法，其中包括：通过轮式里程计获取移动机器人轮组的速度，并且通过IMU获取移动机器人的航向角等信息，同时利用顶部摄像头与底部摄像头进行特征码的识别，顶部摄像头在移动机器人到达特征码前能够提前识别特征码，并且获得移动机器人的位姿状态。底部摄像头在移动机器人到达特征码上方时开始作用，其识别定位精度高，通常用于精确定位以及各类高精度操作的偏差补偿。
[0005]本专利技术至少通过如下技术方案之一实现。
[0006]一种基于双层视觉的移动机器人多传感器融合定位方法，包括以下步骤：
[0007]S1、基于差分轮式移动机器人相应的系统控制量以及传感器获取的信息构建动力学模型；
[0008]S2、通过移动机器人顶部摄像头识别新型特征码中的外部定位码，在移动机器人到达新型特征码前提取获取移动机器人的位姿；
[0009]S3、采用移动机器人底部摄像头识别位于移动机器人底部的中部定位码，当移动机器人移动至新型特征码上方时，通过底部摄像识别新型特征码中的中部定位码，通过坐标转换算法能够获得定位偏差，同时通过匹配算法获得移动机器人的位姿；
[0010]S4、基于移动机器人的位姿和定位误差构建摄像头传感器测量模型，分析定位算法过程中的误差，获得移动机器人的状态向量以及与摄像头测量误差相关的协方差矩阵；
[0011]S5、通过IMU传感器作为状态预测传感器获取移动机器人的角速度，结合IMU角速度动力学模型以及差分轮动力学模型进行下一时刻移动机器人的状态预测，状态预测结果与摄像头状态向量和协方差矩阵中航向角部分进行数据融合；
[0012]S6、通过里程计获取移动机器人车轮的角速度，结合动力学模型进行下一时刻移动机器人的状态预测进行状态预测，状态预测结果与摄像头状态向量和协方差矩阵中坐标部分进行数据融合；
[0013]S7、在数据融合完成后，采用Rauch
‑
Tung
‑
Striebel反向平滑对获得的移动机器人的位姿数据进一步平滑处理，获得移动机器人状态的最优估计。
[0014]进一步地，动力学模型如下：
[0015]定义移动机器人的状态向量其中[x，y]表示在平面世界坐标系上移动机器人对应的位置坐标，θ为移动机器人朝向相对于世界坐标系中x轴方向的夹角，δ、分别表示为受到线速度与角速度影响而产生的对应系统偏差参数；相应的动力学模型表示为：
[0016][0017]其中，为状态向量关于时间的导数，反应移动机器人的状态变化，相对应的分别为x、y、θ的导数，ω
w
为轮子旋转的角速度向量定义为[ω
r
，ω
l
]T
，ω
r
、ω
l
分别表示移动机器人右轮以及左轮旋转的角速度，f(X)为移动机器人的状态变化的计算函数；
[0018]根据差分轮式机器人的结构，f(X)表示为：
[0019][0020]其中r表示差分轮式移动机器人轮子的半径，d表示为移动机器人两个轮子之间的
距离；
[0021]传感器对移动机器人的的状态观测向量Z通过测量方程表示为：
[0022]Z＝h(X)
[0023]其中X为移动机器人的状态向量，h(
·
)为非线性的转移矩阵，表示传感器的观测量与移动机器人状态的相对关系；
[0024]通过加入时间周期T
s
对系统进行离散化，同时加入因IMU及里程计产生的系统噪声误差：
[0025]X
k+1
＝X
k
+f(X
k
)T
s
Δω
[0026]Z
k
＝h(X
k
)
[0027]其中X
k+1
、X
k
分别表示为在k+1时刻和k时刻移动机器人的状态向量，Z
k
表示为k时刻的状态观测向量；Δω＝ω
wk
+ε
k
，ω
wk
为第k时刻的车轮的转速向量本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于双层视觉的移动机器人多传感器融合定位方法，其特征在于，包括以下步骤：S1、基于差分轮式移动机器人相应的系统控制量以及传感器获取的信息构建动力学模型；S2、通过移动机器人顶部摄像头识别新型特征码中的外部定位码，在移动机器人到达新型特征码前提取获取移动机器人的位姿；S3、采用移动机器人底部摄像头识别位于移动机器人底部的中部定位码，当移动机器人移动至新型特征码上方时，通过底部摄像识别新型特征码中的中部定位码，通过坐标转换算法能够获得定位偏差，同时通过匹配算法获得移动机器人的位姿；S4、基于移动机器人的位姿和定位误差构建摄像头传感器测量模型，分析定位算法过程中的误差，获得移动机器人的状态向量以及与摄像头测量误差相关的协方差矩阵；S5、通过IMU传感器作为状态预测传感器获取移动机器人的角速度，结合IMU角速度动力学模型以及差分轮动力学模型进行下一时刻移动机器人的状态预测，状态预测结果与摄像头状态向量和协方差矩阵中航向角部分进行数据融合；S6、通过里程计获取移动机器人车轮的角速度，结合动力学模型进行下一时刻移动机器人的状态预测进行状态预测，状态预测结果与摄像头状态向量和协方差矩阵中坐标部分进行数据融合；S7、在数据融合完成后，采用Rauch
‑
Tung
‑
Striebel反向平滑对获得的移动机器人的位姿数据进一步平滑处理，获得移动机器人状态的最优估计。2.根据权利要求1所述的基于双层视觉的移动机器人多传感器融合定位方法，其特征在于，动力学模型如下：定义移动机器人的状态向量其中[x，y]表示在平面世界坐标系上移动机器人对应的位置坐标，θ为移动机器人朝向相对于世界坐标系中x轴方向的夹角，δ、分别表示为受到线速度与角速度影响而产生的对应系统偏差参数；相应的动力学模型表示为：其中，为状态向量关于时间的导数，反应移动机器人的状态变化，相对应的分别为x、y、θ的导数，ω
w
为轮子旋转的角速度向量定义为[ω
r
，ω
l
]
T
，ω
r
、ω
l
分别表示移动机器人右轮以及左轮旋转的角速度，f(X)为移动机器人的状态变化的计算函数；根据差分轮式机器人的结构，f(X)表示为：
其中r表示差分轮式移动机器人轮子的半径，d表示为移动机器人两个轮子之间的距离；传感器对移动机器人的的状态观测向量Z通过测量方程表示为：Z＝h(X)其中X为移动机器人的状态向量，h(
·
)为非线性的转移矩阵，表示传感器的观测量与移动机器人状态的相对关系；通过加入时间周期T
s
对系统进行离散化，同时加入因IMU及里程计产生的系统噪声误差：X
k+1
＝X
k
+f(X
k
)T
s
ΔωZ
k
＝h(X
k
)其中X
k+1
、X
k
分别表示为在k+1时刻和k时刻移动机器人的状态向量，Z
k
表示为k时刻的状态观测向量；Δω＝ω
wk
+ε
k
，ω
wk
为第k时刻的车轮的转速向量，ε
k
为第k时刻的系统噪声，通过k时刻的状态变获得k+1时刻的状态向量，T
s
为两相邻时刻之间的时间间隔即k与k+1时刻的时间间隔。3.根据权利要求1所述的基于双层视觉的移动机器人多传感器融合定位方法，其特征在于，新型特征码包括外部定位码以及中部定位码；外部定位码采用自行编码的方式，通过识别黑白特征实现移动机器人的相对位置定位，通过识别数字实现移动机器人绝对位姿的定位；中部定位码采用Apriltag码，其类型为36h11。4.根据权利要求1所述的基于双层视觉的移动机器人多传感器融合定位方法，其特征在于，移动机器人顶部摄像头识别新型特征码，包括以下步骤：首先通过顶部摄像头获取移动机器人前方新型特征码的图像，通过视觉处理对图像进行特征提取，通过逐行扫描的方式将获取的数据进行识别，外部特征码具有识别的图案；其次，通过识别的定位进行仿射变换，将特征码转换为正对摄像头状态，同时提取特征码中数字部分进行识别，识别结果表示为定位特征码在世界坐标系上的绝对位置；最后，通过偏差以及特征码的在世界坐标系上的绝对位置计算得到移动机器人的位姿，具体计算方式如下：摄像头识别特征码后，获得特征码中心点在图像坐标系上的坐标(u，v)，计算摄像头的相对位姿：
其中，x
c
、y
c
为特征码中心在相对于摄像头中心的坐标，zc表示为特征码中心在光轴方向上与摄像头中心的距离，f
x
、f
y
、u0、v0为通过摄像头内部标定获取的参数，u、v表示某一确定点在图像上的坐标；通过获得摄像头的定位根据摄像头在移动机器人上的安装位置，获得移动机器人的位姿：其中R为3x3的旋转矩阵，M表示坐标的平移关系，通过变换矩阵获得移动机器人在世界坐标系上的位置。5.根据权利要求1所述的基于双层视觉的移动机器人多传感器融合定位方法，其特征在于，定位偏差表示为：其中Δx
b
、Δy
b
为底部摄像头图像坐标系中中部定位码与图像中心的偏差，f
x
、f
y
、u0、v0为摄像头内部参数，z
c
表示摄像头光轴方向上摄像头中心与地面的距离。6.据权利要求1所述的基于双层视觉的移动机器人多传感器融合定位方法，其特征在于，步骤S4具体包括以下步骤：通过摄像头获得的摄像头位置与特征码中心的相对距离Δx、Δy和相对角度Δθ，移动机器人的状态向量为其在世界坐标系上的坐标以及航向角，为实现由相对距离到状态向量的转换，构建摄像头传感器测量模型：其中为摄像头的状态测量向量，h
c
(X...

【专利技术属性】
技术研发人员：张浪文，杨奕斌，谢巍，刘彦汝，
申请(专利权)人：华南理工大学，
类型：发明
国别省市：