CN115326053A - 一种基于双层视觉的移动机器人多传感器融合定位方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明属于传感器和定位领域，具体涉及一种基于双层视觉的多传感器融合方法。

背景技术

随着我国智能制造技术的发展，各类拥有高智能的移动机器人在人们的生产，生活中得到愈加广泛的应该。作为移动机器人的基础，定位系统在协助完成机器人的移动过程中起到至关重要的作用。

目前移动机器人在应用领域用于导航和定位的传感器类型多样，例如激光雷达、IMU、UWB、蓝牙、里程计、GNSS、磁力计等。在室外情况下，通常采用成熟的GNSS定位技术，该技术能够获取机器人在地球上的绝对位置坐标，除此之外还有基站定位技术和ip定位(Girbés-Juan V，Armesto L，Hernández-Ferrándiz D，et al.Asynchronous SensorFusion of GPS，IMU and CAN-Based Odometry for Heavy-Duty Vehicles[J].IEEETransactions 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等高精度传感器以提高定位系统的精度，但使用这些传感器不可避免的就使得定位成本的大幅提高，随着移动机器人的数量增加，其成本提高更加明显。高精定位系统的成本问题成为影响其向中低端领域扩展的一大障碍。对于常见的定位传感器，其获取的信息通常具有连续且固定的采样周期，在可能出现的异步信息处理方案研究较少。

发明内容

为解决所述技术问题，本发明提出一种基于双层视觉的移动机器人多传感器融合定位方法，其中包括：通过轮式里程计获取移动机器人轮组的速度，并且通过IMU获取移动机器人的航向角等信息，同时利用顶部摄像头与底部摄像头进行特征码的识别，顶部摄像头在移动机器人到达特征码前能够提前识别特征码，并且获得移动机器人的位姿状态。底部摄像头在移动机器人到达特征码上方时开始作用，其识别定位精度高，通常用于精确定位以及各类高精度操作的偏差补偿。

本发明至少通过如下技术方案之一实现。

一种基于双层视觉的移动机器人多传感器融合定位方法，包括以下步骤：

S1、基于差分轮式移动机器人相应的系统控制量以及传感器获取的信息构建动力学模型；

S2、通过移动机器人顶部摄像头识别新型特征码中的外部定位码，在移动机器人到达新型特征码前提取获取移动机器人的位姿；

S3、采用移动机器人底部摄像头识别位于移动机器人底部的中部定位码，当移动机器人移动至新型特征码上方时，通过底部摄像识别新型特征码中的中部定位码，通过坐标转换算法能够获得定位偏差，同时通过匹配算法获得移动机器人的位姿；

S4、基于移动机器人的位姿和定位误差构建摄像头传感器测量模型，分析定位算法过程中的误差，获得移动机器人的状态向量以及与摄像头测量误差相关的协方差矩阵；

S5、通过IMU传感器作为状态预测传感器获取移动机器人的角速度，结合IMU角速度动力学模型以及差分轮动力学模型进行下一时刻移动机器人的状态预测，状态预测结果与摄像头状态向量和协方差矩阵中航向角部分进行数据融合；

S6、通过里程计获取移动机器人车轮的角速度，结合动力学模型进行下一时刻移动机器人的状态预测进行状态预测，状态预测结果与摄像头状态向量和协方差矩阵中坐标部分进行数据融合；

S7、在数据融合完成后，采用Rauch-Tung-Striebel反向平滑对获得的移动机器人的位姿数据进一步平滑处理，获得移动机器人状态的最优估计。

进一步地，动力学模型如下：

定义移动机器人的状态向量

其中[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时刻的时间间隔。

进一步地，新型特征码包括外部定位码以及中部定位码；

外部定位码采用自行编码的方式，通过识别黑白特征实现移动机器人的相对位置定位，通过识别数字实现移动机器人绝对位姿的定位；

中部定位码采用Apriltag码，其类型为36h11。

进一步地，移动机器人顶部摄像头识别新型特征码，包括以下步骤：

首先通过顶部摄像头获取移动机器人前方新型特征码的图像，通过视觉处理对图像进行特征提取，通过逐行扫描的方式将获取的数据进行识别，外部特征码具有识别的图案；

其次，通过识别的定位进行仿射变换，将特征码转换为正对摄像头状态，同时提取特征码中数字部分进行识别，识别结果表示为定位特征码在世界坐标系上的绝对位置；

最后，通过偏差以及特征码的在世界坐标系上的绝对位置计算得到移动机器人的位姿，具体计算方式如下：

摄像头识别特征码后，获得特征码中心点在图像坐标系上的坐标(u，v)，计算摄像头的相对位姿：

其中，x_c、y_c为特征码中心在相对于摄像头中心的坐标，z_c表示为特征码中心在光轴方向上与摄像头中心的距离，f_x、f_y、u₀、v₀为通过摄像头内部标定获取的参数，u、v表示某一确定点在图像上的坐标；

通过获得摄像头的定位根据摄像头在移动机器人上的安装位置，获得移动机器人的位姿：

其中R为3x3的旋转矩阵，M表示坐标的平移关系，通过变换矩阵获得移动机器人在世界坐标系上的位置。

进一步地，定位偏差表示为：

其中Δx_b、Δy_b为底部摄像头图像坐标系中中部定位码与图像中心的偏差，f_x、f_y、u₀、v₀为摄像头内部参数，zc表示摄像头光轴方向上摄像头中心与地面的距离。

进一步地，步骤S4具体包括以下步骤：

通过摄像头获得的摄像头位置与特征码中心的相对距离Δx、Δy和相对角度Δθ，移动机器人的状态向量为其在世界坐标系上的坐标以及航向角，为实现由相对距离到状态向量的转换，构建摄像头传感器测量模型：

其中

为摄像头的状态测量向量，h^c(X_k)摄像头的状态转移函数，Δx^c、Δy^c、Δθ^c分别为移动机器人与特征码中心的偏移量，x_q、y_q为特征码中心在世界坐标系上的坐标，x_k、y_k为移动机器人的坐标，θ_k为移动机器人的航向角，

和

为摄像头与移动机器人中心的固定安装距离；上述过程中并不考虑摄像头在识别定位过程中的误差，在实际情况为实现精确定位，需要加入相应的误差项，因此，摄像头识别到特征码时其相应的测量方程表示为：

其中

为通过摄像头获得的测量值，其包含摄像头的测量误差，Δx_k ^c、Δy_k ^c、Δθ_k ^c分别为Δx^c、Δy^c、Δθ_k通过摄像头获得的测量值，

为在第k时刻的测量不确定度，

分别为x、y、θ关于自身的不确定度，不确定度通过在固定点进行多次定位获得。

进一步地，步骤S5具体包括以下步骤：

定义移动机器人角度状态向量X^θ＝[θ^g，u^g]，其中θ^g为IMU测量获得的角度，u^g为IMU测量获得的角度与角速度相关测量误差系数，每次传感器测量时间间隔T_s，则传感器角度测量模型表示为：

其中

为关于航向角的状态向量，下标k为k时刻时的变量值，类似的下标k-1为k-1时刻，

表示k时刻时IMU测量获得的角度，

k表示k时刻时IMU测量获得的角度与角速度相关测量误差系数，ω_k为k时刻移动机器人的角速度，

表示为IMU产生的噪声误差，

为测量状态向量，

为状态转移函数，顶部摄像头与底部摄像头完成特征识别定位后，获取的观测量为相对新型特征码的角度偏移量Δθc，即

其中θ^tag为特征码的安装角度；

相应的测量方程表示为：

其中

为观测的真实值，

表示传感器测量值，

为视觉识别的获得移动机器人与特征码的角度偏差，

表示为摄像头的测量误差，如果

与ug为白噪声且相互之间互不影响，则通过扩展卡尔曼滤波进行融合；

预测阶段表示为：

其中，

为预测阶段移动机器人的状态预测值，

为预测阶段的协方差矩阵，

为k-1时刻的最佳状态估计值，

是k时刻到k+1时刻的变换矩阵，

为IMU测量获取的在第k时刻的角速度，

分别是根据测量方程关于状态向量X和角速度ω的雅可比矩阵，Qk-₁为上一时刻的协方差矩阵，

为IMU在的噪声方差；当摄像头未识别到特征码时，无法获取观测值，将预测结果中的状态向量和协方差矩阵作为当前时刻的最优估计；

若摄像头识别特征码即存在观测值，则更新动机器人的状态，更新过程亦是融合过程：

其中I为单位矩阵，F为系统的输出矩阵，在此处F＝[-1,0]，

为扩展卡尔曼滤波增益，用于确定更新时传感器测量值的权重，通过上式实现扩展卡尔曼滤波数据融合并获得优化后的航向角，顶部摄像头与底部摄像头在更新过程是相同的，通过改变在于测量过程中定位信息的方差

完成。

进一步地，步骤S6具体包括以下步骤：

定义移动机器人位置信息的状态向量X＝[x，y，δ¹，δ^r]，其中x，y表示在平面世界坐标系上移动机器人对应的位置坐标，δ¹、δ^r分别表示为与左右轮速度相关的里程计的测量误差系数，相应的其状态预测模型表示为：

其中x_k、y_k表示k时刻在平面世界坐标系上移动机器人对应的位置坐标，

分别为k时刻里程计的测量误差系数，

分别为k时刻里程计的测量左右轮的角速度，

表示里程计的噪声误差，r表示差分轮式移动机器人轮子的半径，θ_k-1为k-1时刻的航向角。

进一步地，Rauch-Tung-Striebel反向平滑具体如下：

其中，

为k+1时刻预测协方差矩阵，Q_k为k时刻数据融合后的协方差矩阵，G_k为转换矩阵，

为第k时刻优化后的移动机器人状态向量，通过利用k+1时刻的数据进行反向平滑，对k时刻的移动机器人状态进行优化，M_k、N_k分别是第k时刻根据测量方程关于状态向量X和角速度ω的雅可比矩阵，

为第k时刻IMU在的噪声方差，

为k时刻的最佳状态估计值。

进一步地，所述特征码上的外部定位码上包括数字特征，特征码的绝对位置通过两个数字，其中定义数字识别结果分别为a、b、c，特征码对应坐标表示为：

(x^tag，y^tag)＝(a*L，b*L)

其中，L为特征码在实际场景中的铺设间距，(x^tag，y^tag)为特征码在世界坐标系中的坐标，识别结果c并不用于定位，在实际场景中应用于特征码节点定义。

与现有的技术相比，本发明的有益效果为：

本发明建立在双层视觉伺服定位系统的基础上，采用一种新型的特征码，融合了外部定位码和中部特征码，利用顶部与底部摄像头分别获取移动机器人的位姿，通过与IMU以及里程计进行融合，实现高精度的定位方案。通过双视觉摄像头定位，移动机器人在运动过程中能够通过顶部摄像头实现预测定位，提高了移动机器人的灵活性，通过底部摄像头能够提高移动机器人的停站精度，对于多点精确定位需求，极大的降低了移动机器人的成本，同时多传感器的数据冗余能够提高了移动机器人的鲁棒性，稳定性。

附图说明

图1为本实施例一种基于双层视觉的移动机器人多传感器融合定位方法的流程示意图；

图2为本实施例外部定位码与中部融合特征码示意图；

图3为本实施例基于差分轮式移动机器人车配置多传感器的结构图；

图4为本实施例移动机车双层视觉识别特征码示意图；

图5为本实施例移动机器人车顶部视觉在实际场景中实现环境感知示意图。

具体实施方式

以下通过结合实施例与相应附图，对本发明的应用进行详细说明，但本发明不局限所述实施方式。

实施例1

如图1所示，一种基于双层视觉的移动机器人多传感器融合定位方法，应用于运动控制的双层视觉伺服导航，包括如下步骤：

1)设计新型特征码，采用新型特征码进行视觉识别，实现在远距离与近距离情况下对新型特征码进行的准确识别与定位；

如图2所示，本发明的融合特征码包括外部定位码以及中部定位码，除中部定位码外特征皆属于外部定位码，新型特征码通过将外部定位码与中部定位码进行融合，采用同一定位特征码，实现多种单目视觉定位方案，两个的融合同时满足了顶部摄像头的识别与底部摄像头的识别，能够实现多种情况下，多角度多方案的实现各种的识别定位，实现多角度的识别方式，为双层视觉定位系统提供基础。

采用顶部视觉识别特征码的外部定位码，顶部特征码识别过程包括以下步骤：首先通过顶部摄像头获取移动机器人前方图像，通过视觉处理对图像进行特征提取，减少图像数据同时提高识别速度。然后通过逐行扫描的方式将获取的数据进行识别，外部特征码具有易于识别的图案，所述图案包括黑色正方形方块和黑色正方形方块外部包围的黑色正方形环，在视觉特征上结构简单，特征易于识别，在垂直于黑色边方向上，由外至内黑色区域长度与白色区域长度存在比例特征关系其中，黑色∶白色∶黑色∶白色黑色＝1∶1∶4∶1∶1，通过识别所述的比例特征能够获取特征码在图像中的定位，通过坐标转换获取移动机器人与特征码的偏差；

其次，通过识别的定位进行仿射变换，将特征码装换为正对摄像头状态，同时提取特征码中数字部分进行识别，识别结果表示为定位特征码在世界坐标系上的绝对位置。

最后，通过偏差以及特征码的位置能够计算得到移动机器人的位姿，具体计算方式如下：

摄像头识别特征码后，获得特征码中心点在图像坐标系上的坐标(u，v)，计算摄像头的相对位姿：

其中，x_c、y_c为特征码中心在相对于摄像头中心的坐标，zc表示为特征码中心在光轴方向上与摄像头中心的距离，f_x、f_y、u₀、v₀为通过摄像头内部标定获取的参数，u、v表示某一确定点在图像上的坐标，由于相机安装时拥有确定的高度与姿态，所铺设特征码其尺寸特征已知，因此通过上述方法能够实现单目视觉的深度估计。通过获得摄像头的定位根据摄像头在移动机器人上的安装位置，获得移动机器人的位姿，具体的：

其中R为3x3的旋转矩阵，M表示坐标的平移关系，通过变换矩阵获得移动机器人在世界坐标系上的位置。

外部定位码采用自行编码的方式，通过识别黑白特征实现移动机器人的相对位置定位，通过识别数字实现移动机器人绝对位姿的定位。移动机器人通过对黑白方块进行扫码，由于移动机器人在运动过程中，摄像头拍摄存在模糊的情况，因此舍弃外部定位码的列特性，通过对图像根据行特性进行分析，完成特征的识别过程，由于各特征方块在实际中存在固定的距离，通过坐标变换将图像坐标系与世界坐标系进行变换，获得移动机器人与定位特征码的相对位置，即获得移动机器人于特征码的位姿偏差。进一步，所述特征码根据识别数字实现移动机器人绝对位姿的定位，其中数字信息中包含特征码在世界坐标系中的绝对位置，结合获得的相对位置偏差能够求得移动机器人的绝对位姿。

采用底部视觉识别新型特征码的中部定位码，包括以下步骤：底部摄像头安置于移动机器人底部中心位置，由于底部摄像头光轴垂直于地面，相机的中心与地面为固定距离且中部定位码大小在设计中已经确定，当移动机器人运动至定位码上方时，通过识别算法与匹配算法实现单目视觉的定位，同时当移动机器人在定位码上方进行旋转或移动时，能够实时通过识别结果反馈定位精确信息，可以作为站点或工作点完成各类需要精确定位的需求。

中部定位码采用Apriltag码，其识别过程较为简单，采用36h11类型，其中有效方格为36个，外围黑色方格便于算法的定位，中部36格通过黑白颜色的交替进行编码，其满足旋转不变性。通过对内部36格黑白编码分别进行识别，识别黑色编码为0，是别为白色编码为1，以此将编码转换为二进制码，36格能获得长度为36的二进制码，实现解码。为确定移动机器人的相对旋转方向，需要对Apriltag码进行三次90°的旋转，四次解码，并通过与已有的编码库进行二进制码匹配。编码库中二进制码与绝对坐标进行绑定，因此能够实现中部定位码的绝对坐标的获取，通过坐标变换可以获得移动机器人与中部定位码的相对偏差，相对偏差向量可以表示为：

其中Δx_b、Δy_b为底部摄像头图像坐标系中中部定位码与图像中心的偏差，f_x、f_y、u₀、v₀为摄像头内部参数。

在识别外部定位码的过程中，由于中部定位码采用Apriltag码，其特征与外部定位码的定位图案不同，通过算法过滤，外部定位码在识别过程中不受中部特征码的变化影响。

具体的，本发明采用差分轮式移动机器人作为整体配置的基础，如图3所示，顶部摄像头安装于移动机器人顶部，光轴与移动机器人前进方向处于统一平面，光轴与地面的投影与该光轴形成一夹角，夹角大小为35°。安装于顶部的摄像头用于识别外部定位码，外部定位码特征较为明显，能够实现较远距离的识别定位，为顶部特征码的识别提供了条件，降低了对摄像头的精度要求。底部特征码安装于移动机器人底部中心位置，其与地面距离较近，用于识别中部特征码，同时配置相应的光源，具有识别稳定，定位精度高的特点。在控制上，通过工控机实现对摄像头获取信息的处理，并通过扩展卡尔曼滤波算法和RTSS反向平滑算法融合多个传感器数据。

移动机器人在运动过程中识别特征码过程如图4所示，当移动机器人运动至特征码上方时，通过底部摄像头实现对中部定位码的识别与定位。当移动机器人在移动过程中，若前方出现另一特征码，能够在移动机器人到达前进行预先识别，能够提前获取相应的偏差信息。

2)基于差分轮式移动机器人构建相应动力学模型，将相应的系统控制量与传感器获取的信息进行联系，为实现多传感器融合技术提供实际基础。定义移动机器人的状态向量

其中[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+1＝h(X)

其中X_k+1、X_k分别表示为在k+1时刻和k时刻移动机器人的状态向量，Z_k+1表示为k时刻的状态观测向量；Δω＝ω_wk+ε_k，ω_wk为第k时刻的车轮的转速向量，ε_k为第k时刻的系统噪声，通过k时刻的状态变化可以获得k+1时刻的状态向量。

通过摄像头可以获得的摄像头位置与特征码中心的相对距离Δx和Δy和相对角度Δθ，然而摄像头的位置并不代表移动机器人位置，实际过程中通常摄像头与移动机器人中心存在一定的偏移，但摄像头固定位置为已知的，可以通过坐标变化获得移动机器人于定位码中心的相对距离。因此摄像头的测量方程表示为：

其中

为摄像头的状态测量向量，h^c(X_k)摄像头的状态转移函数，

Δθ^c分别为机器人与特征码中心的偏移量，x_q、y_q为特征码中心在世界坐标系上的坐标，x_k、y_k为移动机器人的坐标，θ_k为移动机器人的航向角，

和

为摄像头与移动机器人中心的固定安装距离。以顶部摄像头为例，其测量过程如图5所示，地面铺设等间距特征码，相邻特征码之间距离为L，特征码的固定坐标为(x^tag，y^tag)，特征码与世界坐标系x轴的夹角为θ^tag，移动机器人通过顶部摄像头识别特征码后，通过相应的解码过程获得相对偏差，其中Δx^c、Δy^c、Δθ^c在顶部摄像都的识别中为Δx^top、Δy^top、Δθ^top。

当摄像头识别到特征码时其相应的测量方程为：

其中

通过摄像头获得的测量值其包含了摄像头的测量误差，相似的Δx_k ^c、Δy_k ^c、Δθ_k ^c分别为Δx^c、Δy^c、Δθ_k通过摄像头获得的测量值，

为在第kT_s时刻的测量不确定度，

分别为x、y、θ关于自身的不确定度。3)由于特征码的识别存在间歇性，存在无特征码识别的情况，因此需要采用IMU传感器作为状态预测传感器，IMU传感器的高频信息反馈特点受外界的因素干扰小，作为定位系统的保障，对移动机器人状态进行预测；

通过IMU获取移动机器人的角速度，与移动机器人扫码获得的角度进行数据融合获得精确的定位效果。具体的实现方式如下：

定义状态向量x＝[θ^g，u^g]，其中θ^g定义为IMU测量获得的角度，u^g定位为与角速度相关测量误差系数，其中每次传感器测量时间间隔T_s，则传感器角度测量模型表示为：

其中

表示为关于航向角的状态向量，下标k表示为k时刻时的变量值，类似的下标k-1表示为k-1时刻，ω_k表示为k时刻移动机器人的角速度，

表示为IMU产生的噪声误差，

为测量状态向量，

为状态转移函数，由于顶部摄像头与底部摄像头完成特征识别定位后，获取的观测量为相对新型特征码的角度偏移量Δθ^c。即

因此，相应的测量方程可以表示为

其中

为观测的真实值，

表示传感器测量值，

为视觉识别的获得移动机器人与特征码的角度偏差，

表示为摄像头的测量误差，如果

与u^g为白噪声且相互之间互不影响，通过扩展卡尔曼滤波进行融合。根据上述式子，预测阶段可以表示为

其中，

为预测阶段移动机器人的状态预测值，

为预测阶段的协方差矩阵，

为k时刻的最佳状态估计值，

是k时刻到k+1时刻的变换矩阵，

为IMU测量获取的在第k时刻的角速度，

分别是根据测量模型方程关于状态向量X和角速度ω的雅可比矩阵，Q_k-1为上一时刻的协方差矩阵，

为IMU在的噪声方差；

进一步，更新步骤可以表示为

其中I为单位矩阵，F为系统的输出矩阵，F＝[-1，0]，

为扩展卡尔曼滤波增益，用于确定更新时传感器测量值的权重，通过上式实现扩展卡尔曼滤波数据融合并获得优化后的航向角，顶部摄像头与底部摄像头在更新过程是相同的，通过改变在于测量过程中定位信息的方差

完成。

类似的，定义移动机器人位置信息的状态向量X＝[x，y，δ¹，δ^r]，其中δ¹、δ^r分别表示为与左右轮速度相关的里程计的测量误差系数。相应的其状态预测模型可以表示为：

其中

分别为k时刻里程计的测量误差系数，

分别为k时刻里程计的测量左右轮的角速度，r表示差分轮式移动机器人轮子的半径，θ_k-1为k-1时刻的航向角，

表示里程计的噪声误差，在实际运用过程中，摄像头的定位噪声与里程计的噪声是互不干扰的，若同时为白噪声可采用扩展卡尔曼滤波方法将摄像头获取的机器人坐标

和里程计预测的坐标值

进行融合，具体融合过程与航向角融合类似，不同的在于其中相应的雅各比矩阵以及后续的协方差矩阵的不同。

本发明采用双层视觉进行融合，在实现过程中，通过顶部与底部摄像头识别获取相应的位姿信息作为测量方程的输入，由于识别的定位特征码部位不同，摄像头的外参不同，其定位协方差参数也不同，同时在定位时间过程中采用连续的状态预测和间歇性的状态更新，以应对不连续的定位特征码的识别。本发明采用固定多点位测试方法，用于确定摄像头的定位误差参数。

实施例2

本发明提出一种基于双层视觉的移动机器人多传感器融合定位方法，发明包括：(1)采用创新性特征码实现，多角度识别定位功能，在移动机器人车顶部与底部放置摄像头，在移动机器人运动至特征码上方前，通过对固定与地面的特征码进行识别，获取移动机器人车的位姿，在移动机器人运动至特征码上方时，通过对新型特征码中部定位码进行扫描识别获取移动机器人的位姿。(2)一种基于新型特征码的双层伺服导航方式，通过双摄像头分别识别特征码，通过融合IMU、轮式里程计以及间歇性获取的视觉定位码获得高精度的定位信息。

在实现卡尔曼滤波后，通过其保存滤波数据对其进行方向平滑处理，实现数据优化，以提高移动机器人定位轨迹与实际情况的匹配度。平滑技术作为事后或准实时数据处理的一种方法，可以在一定程度上提高数据处理的精度，在测绘领域获得了广泛的应用。平滑技术总的来说分为三类：固定区间平滑、固定点平滑和固定滞后平滑。其中在数据后处理中应用最为广泛的方法就是固定区间平滑。

R-T-S算法是一种固定区间最优平滑算法，通过Rauch-Tung-Striebel平滑算法对获得的数据进行反向平滑处理，能够很好的解决因特征码识别的间歇性导致视觉定位数据不连续，进而产生定位偏移的情况，充分利用区间内所有时刻的测量值对某一时刻的状态进行估计，实现定位精度的进一步提高。反向平滑技术在卡尔曼滤波之后进行，同时利用卡尔曼滤波后的数据完成平滑，具体步骤如下：

其中，

为k+1时刻预测协方差矩阵，Qk为k时刻数据融合后的协方差矩阵，Gk为转换矩阵，

为第k时刻优化后的移动机器人状态向量，通过利用k+1时刻的数据进行反向平滑，对k时刻的移动机器人状态进行优化。

本发明所述基于双层视觉的移动机器人多传感器融合定位方法还可用于无人机的定点降落，用于提高无人机降落的稳定性与精确度。当无人机进行降落准备时，首先通过视觉对外部特征码进行识别，对降落点进行定位，当降落至一定高度时，能够通过视觉对中部特征码进行识别，获取更为精准的定位。由于无人机在较高的位置飞行，在降落过程中，由于飞行中的干扰，会导致降落位置的偏离，定位不精准的情况，在降落过程中需要预留较大的空间，同时利用平滑算法对定位轨迹进行处理能够获得更为精确的定位效果。

在降落过程前期，无人机与地面距离较远，视觉识别所需特征较大，采用设计的创新性特征码，识别外部特征码，同时具有抗运动模糊的特性，无人机通过识别外部定位码实现定位，在较远距离便能够更快更及时的调整姿态。当无人机进一步的降低高度，运动至距离特征码较近的距离时，外部定位码与中部定位码能够同时识别，此时能够通过融合算法对定位效果进行优化。当无人机高度进一步降低，外部定位码在摄像头的视野中以及无法完全显示，此时，外部定位码已无法识别，通过中部定位码对无人机的精确定位进行控制。值得一提的是，当识别到外部特征码时，通过基于偏差的控制算法控制无人机运动至特征码上方，确保中部定位码的识别。

实施例3

双层视觉定位方式在用于多节点需求的工业情况下，能够通过设计节点类型的方式提高移动机器人的运动效率，适用于物流仓储，高节拍工业运输的场景中。在外部特征码的识别过程中，外部特征码中的数字不仅仅能够包括定位信息，同时能够包含特征码节点信息，通过对该节点进行定义，例如快速移动节点，停止节点，转向节点等。移动机器人在运行过程中，顶部摄像头能够在未到达节点的情况下实现预知的功能，在经过快速运动节点时无需在特征码上识别等待。同时对于高精度定位节点，能够在牺牲控制时间的基础上，提供更加精确的定位效果。

根据不同的定位节点，本发明可采用如图2所示特征码，该特征码在初始基础上将外部定位码其中一个数字识别区域进行更改，将其改为更加易于识别的方格形状。

通过所采用的创新性特征码可通过自编码方式进行节点类型设定，具体可分为快速移动节点，停止节点等。具体实现过程如下，所述特征码上的外部定位码上，数字特征不仅仅能够包含所定位特征码的位置信息，同时能够对特征码进行相应的定义，实现移动机器人在不停顿的情况下，既实现了自身的定位，实现位置状态更新，又能够通过节点定义对移动机器人的控制进行预先判定。特征码的绝对位置能够通过两个数字实现，数字数量不足时通过字母进行扩充。顶部摄像头识别外部定位码过程中的已经对数字特征进行识别，定义数字识别结果分别为a、b、c。特征码对应坐标表示为：

(x^tag，y^tag)＝(a*L，b*L)

其中，L为特征码在实际场景中的铺设间距，(x^tag，y^tag)表示为特征码在世界坐标系中的坐标。当实际场景中所需要的场地更加复杂时，通过数字能够实现的定位的方式仅仅能够实现10L*10L范围内的定位，通过扩展字母特征替换数字特征的方式实现定位范围的拓展，由于所采用的符号为标准字符，通过仿射变换后，在识别上难度较低，能够实现精准识别。其特征码的定位坐标计算方式与数字方案相同，但是别结果采用十进制ASCII码的方式表示。

数字识别结果c应用于特征码节点定义，识别结果通过与所预先定义的节点库进行比对，结合顶部视觉具有提前识别的特点，实现在移动机器人为到达特征码是提前获得节点信息，可应用于优化系统。剩余一个数字部分用于节点定义，当系统可以根据需求对不同节点采用不同的定位策略实现系统的优化控制。

实施例4

本发明中的基于双层视觉的多传感器融合定位方法可应用于复合机器人中。由于移动机器人的运动性能各不相同，在控制精度上也会产生一定的误差，本发明通过多传感器融合的方式实现了高精度的定位，当移动机器人的运动控制系统无法保证其运动至所需的定位精度时，通过提供定位偏差的方式进行补偿。

复合机器人通常在移动机器人的基础上添加机械臂，抓取装置等，在实际场景中，移动机器人作为运动的基础通常只需要满足运输上层结构的作用，但随着对高精度的移动需求的提高，对移动底盘的要求也在提高，相应的控制时长以及底盘结构设计的要求也相应提高，不可避免造成了成本的提升。采用双层视觉进行融合定位，能够获得高精度的定位需求，即使移动机器人无法在需求的位置达到需求的精度，依旧能够反馈定位偏差，通过误差补偿的方式将误差反馈给上层机构，通过上层机构的协调控制完成上层机构的精确操作。

以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为所述的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制。

Claims (10)

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、u₀、v₀为通过摄像头内部标定获取的参数，u、v表示某一确定点在图像上的坐标；

通过获得摄像头的定位根据摄像头在移动机器人上的安装位置，获得移动机器人的位姿：

其中R为3x3的旋转矩阵，M表示坐标的平移关系，通过变换矩阵获得移动机器人在世界坐标系上的位置。

5.根据权利要求1所述的基于双层视觉的移动机器人多传感器融合定位方法，其特征在于，定位偏差表示为：

其中Δx_b、Δy_b为底部摄像头图像坐标系中中部定位码与图像中心的偏差，f_x、f_y、u₀、v₀为摄像头内部参数，z_c表示摄像头光轴方向上摄像头中心与地面的距离。

6.据权利要求1所述的基于双层视觉的移动机器人多传感器融合定位方法，其特征在于，步骤S4具体包括以下步骤：

通过摄像头获得的摄像头位置与特征码中心的相对距离Δx、Δy和相对角度Δθ，移动机器人的状态向量为其在世界坐标系上的坐标以及航向角，为实现由相对距离到状态向量的转换，构建摄像头传感器测量模型：

其中

为摄像头的状态测量向量，h^c(X_k)摄像头的状态转移函数，Δx^c、Δy^c、Δθ^c分别为移动机器人与特征码中心的偏移量，x_q、y_q为特征码中心在世界坐标系上的坐标，x_k、y_k为移动机器人的坐标，θ_k为移动机器人的航向角，

和

为摄像头与移动机器人中心的固定安装距离；上述过程中并不考虑摄像头在识别定位过程中的误差，在实际情况为实现精确定位，需要加入相应的误差项，因此，摄像头识别到特征码时其相应的测量方程表示为：

其中

为通过摄像头获得的测量值，其包含摄像头的测量误差，Δx_k ^c、Δy_k ^c、Δθ_k ^c分别为Δx^c、Δy^c、Δθ_k通过摄像头获得的测量值，

为在第k时刻的测量不确定度，

分别为x、y、θ关于自身的不确定度，不确定度通过在固定点进行多次定位获得。

7.根据权利要求1所述的基于双层视觉的移动机器人多传感器融合定位方法，其特征在于，步骤S5具体包括以下步骤：

定义移动机器人角度状态向量X^θ＝[θ^g，u^g]，其中θ^g为IMU测量获得的角度，u^g为IMU测量获得的角度与角速度相关测量误差系数，每次传感器测量时间间隔T_s，则传感器角度测量模型表示为：

其中

为关于航向角的状态向量，下标k为k时刻时的变量值，类似的下标k-1为k-1时刻，

表示k时刻时IMU测量获得的角度，

表示k时刻时IMU测量获得的角度与角速度相关测量误差系数，ω_k为k时刻移动机器人的角速度，

表示为IMU产生的噪声误差，

为测量状态向量，

为状态转移函数，顶部摄像头与底部摄像头完成特征识别定位后，获取的观测量为相对新型特征码的角度偏移量Δθ^c，即

其中θ^tag为特征码的安装角度；

相应的测量方程表示为：

其中

为观测的真实值，

表示传感器测量值，

为视觉识别的获得移动机器人与特征码的角度偏差，

表示为摄像头的测量误差，如果

与ug为白噪声且相互之间互不影响，则通过扩展卡尔曼滤波进行融合；

预测阶段表示为：

其中，

为预测阶段移动机器人的状态预测值，

为预测阶段的协方差矩阵，

为k-1时刻的最佳状态估计值，

是k时刻到k+1时刻的变换矩阵，

为IMU测量获取的在第k时刻的角速度，

分别是根据测量方程关于状态向量X和角速度ω的雅可比矩阵，Q_k-1为上一时刻的协方差矩阵，

为IMU在的噪声方差；当摄像头未识别到特征码时，无法获取观测值，将预测结果中的状态向量和协方差矩阵作为当前时刻的最优估计；

若摄像头识别特征码即存在观测值，则更新动机器人的状态，更新过程亦是融合过程：

其中I为单位矩阵，F为系统的输出矩阵，在此处F＝[-1，0]，

为扩展卡尔曼滤波增益，用于确定更新时传感器测量值的权重，通过上式实现扩展卡尔曼滤波数据融合并获得优化后的航向角，顶部摄像头与底部摄像头在更新过程是相同的，通过改变在于测量过程中定位信息的方差

完成。

8.根据权利要求1所述的基于双层视觉的移动机器人多传感器融合定位方法，其特征在于，步骤S6具体包括以下步骤：

定义移动机器人位置信息的状态向量X＝[x，y，δ¹，δ^r]，其中x，y表示在平面世界坐标系上移动机器人对应的位置坐标，δ¹、δ^r分别表示为与左右轮速度相关的里程计的测量误差系数，相应的其状态预测模型表示为：

其中x_k、y_k表示k时刻在平面世界坐标系上移动机器人对应的位置坐标，

分别为k时刻里程计的测量误差系数，

分别为k时刻里程计的测量左右轮的角速度，

表示里程计的噪声误差，r表示差分轮式移动机器人轮子的半径，θ_k-1为k-1时刻的航向角。

9.根据权利要求1所述的基于双层视觉的移动机器人多传感器融合定位方法，其特征在于，Rauch-Tung-Striebel反向平滑具体如下：

其中，

为k+1时刻预测协方差矩阵，Q_k为k时刻数据融合后的协方差矩阵，G_k为转换矩阵，

为第k时刻优化后的移动机器人状态向量，通过利用k+1时刻的数据进行反向平滑，对k时刻的移动机器人状态进行优化，M_k、N_k分别是第k时刻根据测量方程关于状态向量X和角速度ω的雅可比矩阵，

为第k时刻IMU在的噪声方差，

为k时刻的最佳状态估计值。

10.根据权利要求2所述的基于双层视觉的移动机器人多传感器融合定位方法，其特征在于，所述特征码上的外部定位码上包括数字特征，特征码的绝对位置通过两个数字，其中定义数字识别结果分别为a、b、c，特征码对应坐标表示为：

(x^tag，y^tag)＝(a*L，b*L)

其中，L为特征码在实际场景中的铺设间距，(x^tag，y^tag)为特征码在世界坐标系中的坐标，识别结果c并不用于定位，在实际场景中应用于特征码节点定义。

Images