1、192023 年第 12 期/新产品新技术NEW PRODUCT NEW TECHNOLOGY基于刚性信息约束改进的蒙特卡罗定位算法研究倪鹏程 王 强武汉理工大学交通与物流工程学院 武汉 430063摘 要：由于传统蒙特卡罗定位算法无法适用于 2 个以上多传感器融合定位的情况，文中针对需要安装多种传感器的重载自动导引车提出一种改进的蒙特卡洛定位算法。改进的蒙特卡洛定位算法以 IMU 与里程计融合的扩展卡尔曼滤波模型为改进算法的预测步，修正里程计模型带来的误差。在蒙特卡洛定位算法的更新步中引入刚性信息约束来修正粒子权重，提高定位精度。在重载自动导引车上进行绝对定位实验和重复定位实验，验证了改进的
2、算法可有效地提高重载自动导引车的定位精度，同时保证了算法的鲁棒性，证明改进的蒙特卡罗定位算法优于传统的蒙特卡罗定位算法。关键词：重载自动导引车；蒙特卡洛算法；定位精度；模型中图分类号：TH242 文献标识码：A 文章编号：1001-0785（2023）12-0019-08Abstract:Considering that the traditional Monte Carlo localization algorithm is not suitable for the fusion localization of more than two sensors,in this paper,it i
3、s necessary to install heavy-duty AGV with multiple sensors,and an improved Monte Carlo localization algorithm is proposed.The improved algorithm takes the extended Kalman filter model combined with IMU and odometer as the prediction step in order to correct the error caused by odometer model.In the
4、 updating step of the algorithm,the rigid information constraint is introduced to correct the particle weight in order to improve the localization accuracy.Absolute localization experiment and repeated localization experiment are carried out on heavy-duty AGV,which proves that the improved algorithm
5、 can effectively improve the localization accuracy of heavy-duty AGV and ensure the robustness of the algorithm,and proves that the improved Monte Carlo localization algorithm is superior to the traditional Monte Carlo localization algorithm.Keywords:overloaded AGV;Monte Carlo localization;localizat
6、ion accuracy;model0 引言重载自动导引车(Automated Guided Vehicle，AGV)1是用于物料搬运的移动机器人。自主运动能力是决定重载 AGV 搬运效率的关键之一，精确的定位能力是重载AGV 进行自主运动的前提条件。针对于 AGV 定位领域，定位方法可以分为 2 类：一类是基于辅助物进行定位的方法，包括磁钉定位2、磁带定位3和反光板定位4等技术；另一类不需要使用辅助物，包括卡尔曼滤波5和蒙特卡洛滤波6等技术。蒙特卡罗定位(MCL)是解决全局定位问题最常用的方法，是一种经典的概率定位算法，通过使用一组带有权重的粒子来近似概率密度函数，在非线性系统上处理具有优势
7、。为使蒙特卡罗算法能够更广泛地应用于各种定位领域，研究人员做了大量研究。冯佳萌等7将扫描匹配(SM)和离散傅里叶变换(DFT)与传统自适应蒙特卡罗定位算法相结合，提高了激光雷达点云与障碍物边缘的对准精度；徐波等8将北斗卫星系统、惯性测量单元和里程计进行多传感器融合，通过蒙特卡罗定位算法进行定位，实现大范围开阔环境的精准定位；刘鹏等9将Wi-Fi 和自适应蒙特卡洛算法相结合，通过对 Wi-Fi 信号的分布进行分析，利用 Wi-Fi 信号强度的三角定位结果完成自适应蒙特卡洛算法的初始化，有效加快粒子的收敛速度、定位时间；骆增辉等10将相似能量区采样与蒙特卡罗定位方法相结合，当机器人失去定位时，能够
8、快速的恢复定位；姜毅等11将高阶共轭粒子滤波与蒙特卡罗算法相结合，利用高阶共轭无迹粒子滤波器精倪鹏程，王强.基于刚性信息约束改进的蒙特卡罗定位算法研究 J.起重运输机械，2023（12）：19-26.引 用 格 式20/2023 年第 12 期新产品新技术NEW PRODUCT NEW TECHNOLOGY确设计蒙特卡洛算法的提议分布，使算法的位置状态估计达到高阶精度；王灵矫等12提出了一种改进的粒子群蒙特卡洛定位算法，将蒙特卡洛算法与粒子群算法相结合，提高定位的效率和定位精度；Miguel M N D 等13通过在蒙特卡洛定位算法中增加全球导航卫星系统信息来增强粒子的权重，使得该方法能够用于
9、全球导航卫星系统信号差的场景；Farag W14提出一种应用于高实时蒙特卡洛定位方法，由融合的 GPS+IMU 测量初始化，使用 GB-DBSCAN 算法对位姿估计进行聚类，以减少计算成本和内存需求，达到较均衡的位姿估计精度及实时性；Ge G 等15提出了一种基于文本信息和激光扫描数据的定位方法，通过寻找文本级的语义信息进行全局的粗定位，然后利用激光数据得到精细的局部定位，能够在具有相似且重复的几何结构位置完成精准定位；He J Q16对传感器节点的通信半径进行模糊化处理，减少节点的采样面积，提出了一种基于模糊理论的改进蒙特卡罗定位算法；章洋等17将蒙特卡洛定位与线段特征匹配相结合，缩小定位的
10、范围，从而改善定位精度和实时性。由以上文献可知，目前对 AGV 定位领域的研究多集中于轻载荷、小体积的 AGV，而针对重载荷、大体积的 AGV 定位研究较少。近年来，重载 AGV 越来越广泛应用于仓库、码头、生产线等领域。由于重载 AGV 的车身长，载荷大，车身需要承载货物，为了行驶视野最大化，通常会在车头和车尾的位置安装 2 个激光雷达。传统的蒙特卡洛算法只能作为 2 个传感器的融合定位算法，而无法处理 2 个以上的传感器信息。基于此，本文提出一种基于刚性信息约束的改进蒙特卡洛定位算法，利用扩展卡尔曼滤波算法的思想，在传统的蒙特卡洛算法中融合 IMU 和里程计信息，减小里程计的角度漂移，引入
11、安装于车头与车尾 2 个激光雷达之间的刚性信息约束，增加粒子权重的准确性，融合里程计、IMU 以及 2 个激光雷达信息，并将其与传统蒙特卡洛定位算法进行比较，得出重载 AGV 实验平台和实验结果。测试结果表明，改进后的定位算法能较好地融合多个传感器，显著提高了重载AGV 精度。1 基于刚性信息约束的改进蒙特卡罗定位算法基于刚性信息约束的改进蒙特卡罗定位算法框图如图 1 所示。与传统的蒙特卡罗定位算法相同，本文提出的基于刚性信息约束的蒙特卡罗定位算法分为预测步和更新步。传统的蒙特卡罗定位算法在预测步中只采用里程计模型作为系统状态转移方程，但随着时间的推移，里程计的误差会一直累积，为了减小里程计模
12、型带来的误差，本文采用 IMU 与里程计融合的扩展卡尔曼滤波模型作为改进的蒙特卡洛定位算法的预测步。传统的蒙特卡洛定位算法在更新步中只利用 1 个激光雷达的信息，能够利用车头前方或车尾后方中的某个激光雷达环境信息。为了充分利用多传感器信息充足的优势，本文在蒙特卡洛定位算法的更新步中引入刚性信息约束，该刚性信息是指 2 个传感器之间距离一定，角度一定。传统的蒙特卡洛算法权重的更新只依赖于激光的观测，而改进的算法不仅能获得激光观测更新粒子的权重，还能依赖刚性信息为粒子带来额外的传感器之间的固有几何关系信息以修正粒子的权重，提高符合刚性信息约束的粒子权重，降低不符合刚性信息约束的粒子权重，以此提高定
13、位精度。图 1 基于刚性信息的改进蒙特卡罗定位算法图1.1 基于扩展卡尔曼滤波的 IMU 和里程计融合算法本文以重载 AGV 为研究对象，其运动平面是二维平面。系统的状态为重载 AGV 的位姿，已知 t 时刻重载 AGV 的后验位姿估计为 tx（Xt，Yt，t），（Xt，212023 年第 12 期/新产品新技术NEW PRODUCT NEW TECHNOLOGYYt）为 t 时刻重载 AGV 的几何中心，t 为 t 时刻重载AGV 在导航坐标系中的运动方向。改进的蒙特卡洛定位算法采用 IMU 与里程计融合的扩展卡尔曼滤波模型作为算法的预测步，IMU 与里程计融合的扩展卡尔曼滤波模型分为预测和
14、更新 2 步，在预测步中使用里程计的数据，在更新步中使用 IMU 提供的姿态数据做测量更新。1）预测步里程计提供的控制信息 ut（vt，t）。其中，vt为 t 时刻的线速度，t为 t 时刻的角速度。AGV 的状态转移函数为()()()ttttttttttttttttttttsinsincoscosvvtXvvf xuYtt+，（1）式中：xt为 t 时刻重载 AGV 真实位姿。f（xt，ut）在上一时刻重载 AGV 的后验状态估计 处进行泰勒一阶线性近似，得到的雅可比矩阵为ttttttttttt10coscos01sinsin001vtvtF （2）控制量的噪声协方差矩阵 Mt定义为 221
15、t2tt223t4t00vvvv+M （3）将 f（tx，ut）对运动参数 vt和 t进行线性近似，其雅可比矩阵 Vt可表示为()()()()()()tttttttttttt2ttttttttttttt2ttsinsinsinsincoscoscoscoscossin00vttvttvttvtt+V（4）根据系统状态转移方程，在预测过程中有 ()t+1ttTTt+1ttttttxfxuPP+，FFV M V （5）2）测量更新t+1 时刻 IMU 的二维姿态观测信息为（0，0，t+1），此时的观测值为 k+1t+100z （6）此时的观测函数为 ()t+1t+100h x （7）当 h（xt+
16、1）在 t+1 时刻重载 AGV 的先验状态处进行泰勒一阶线性近似时，得到的雅可比矩阵为 t+1000000001H （8）卡尔曼增益为 ()1TTt+1t+1t+1t+1t+1t+1t+1KP HP H+HR （9）t+100000000R （10）式中：Rt+1为观测噪声的协方差矩阵，为 IMU 输出的关于姿态 的观测噪声方差。由此，卡尔曼增益可以写成22/2023 年第 12 期新产品新技术NEW PRODUCT NEW TECHNOLOGY t+100000000KK （11）重载 AGV 后验状态估计可以写为 t+1t+1t+1t+1t+1t+1()XxYK+（12）将协方差矩阵更新为状态变量后验估计值的协方差矩阵，即 ()t+1t+1t+1IKPP （13）1.2 基于刚性信息的粒子权重更新观测由 2 个激光传感器信息 z1和 z2组成，粒子集分别为 S1（xi1，wi1）|i（1，2，N）和 S2（xi2，wi2）|i（1，2，N）。xi1为车头激光所得到的粒子群中第 i 个粒子的位姿，wi1为 xi1的权重，xi2为车尾激光所得到的粒子群中第 i 个粒子的位姿，wi2为
