CN116972844A - 基于ArUco阵列的移动机器人室内定位系统及方法 - Google Patents

Abstract

本发明公开了一种基于ArUco阵列的移动机器人室内定位系统及方法，设计机器人室内定位技术领域。该系统包括驱动模块，用于计算机器人在世界坐标系中的位姿信息；惯性测量模块，用于获取IMU坐标系中的位姿信息，并计算世界坐标系与IMU坐标系的转换关系；视觉信息模块，用于通过对含有ArUco阵列的图像进行识别解析得到相机在世界坐标系中的位姿信息；数据融合模块，用于采用融合定位算法进行融合预测，得到机器人当前的位姿状态。本发明解决了机器人位姿解算过程中不同移动距离的识别问题，且使用动态方差融合定位算法有效地提高了室内定位的精度并减少了动态环境的影响。

Description

基于ArUco阵列的移动机器人室内定位系统及方法

技术领域

本发明涉及机器人室内定位技术领域，尤其涉及一种基于ArUco阵列的移动机器人室内定位系统及方法。

背景技术

本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。

在移动机器人系统中，目标探测和自主导航都需要进行自身位姿的准确估计。而针对不同的应用场景，估计的方式也会有所区别，主要表现为室外定位和室内定位两种形式。在室外环境中，一般会使用GPS/INS组合导航的方式，以获取尽可能准确的定位数据。而在室内定位技术中，常常会借助各种传感器，例如Wi-Fi、激光传感器、视觉以及超宽带传感器等，来实现对移动机器人准确定位。对于UWB(超宽频)定位方法，需要在后台放置大量的基站，并且在非视距条件下精度会大幅降低。而单一传感器进行定位往往需要更高成本或者得到不够准确的定位结果。

视觉定位通过采集周围环境的图像信息来实现定位，具有信息丰富、结构简单、成本低廉和安装方便的优点，被广泛应用于移动机器人的定位研究中。视觉定位可进一步分为基于环境特征的定位和基于人造特征的定位。基于环境特征点的视觉定位方法，如ORB-SLAM2、LDSO、OpenVSLAM等，但是由于光照变化、运动模糊及感知混叠等问题，导致定位方法不够鲁棒。基于人造特征的定位，如基于二维码的人工路标，相比于自然路标具有信息稳定、制作容易、检测快速的优点，能够为位姿估计提供绝对的尺度信息。然而大尺寸标记更容易从长距离检测到，但是在短距离内，大尺寸标记超出了摄像头视野范围。因此，如何获取机器人在长短距离移动过程中的准确定位数据，并实现高精度的室内定位成为现有技术亟待解决的问题。

发明内容

针对现有技术存在的不足，本发明的目的是提供一种基于ArUco阵列的移动机器人室内定位系统及方法，使用ArUco标记作为人造特征，通过求解3D到2D点对运动，得到相机坐标系相对世界坐标系的位姿。并通过融合定位算法将视觉信息、轮式编码器信息和IMU数据融合，有效地提高了移动机器人室内定位的精度并减少了动态环境的影响。

为了实现上述目的，本发明是通过如下的技术方案来实现：

本发明第一方面提供了一种基于ArUco阵列的移动机器人室内定位系统，包括：

驱动模块，用于获取机器人当前的运动状态，通过构建运动学模型计算机器人在世界坐标系中的位姿信息；

惯性测量模块，用于获取IMU坐标系中的位姿信息，并计算世界坐标系与IMU坐标系的转换关系；

视觉信息模块，用于获取相机拍摄的含有ArUco阵列的图像，通过对图像进行识别解析得到相机在世界坐标系中的位姿信息；

数据融合模块，用于将驱动模块、惯性测量模块和视觉信息模块中的位姿信息采用融合定位算法进行融合预测，得到机器人当前的位姿状态。

进一步的，驱动模块包括磁编码器，利用编码器获得单位时间内的轮速，构建运动学模型，根据轮速计算机器人相对于出发点的定位数据从而得到机器人在世界坐标系中的位姿信息。

更进一步的，驱动模块还包括电机和电机驱动，电机安装在机器人的车轮上，磁编码器安装在电机上，电机和电机驱动用于为机器人提供动力。

进一步的，惯性测量模块包括惯性测量单元，所述惯性测量单元包括陀螺仪、加速度计和磁力计，分别用以测量旋转角速度、加速度以及方向。

更进一步的，惯性测量单元测量过程中首先进行校准，之后采用马赫尼算法将陀螺仪、加速度计和磁力计测量和计算得到的数据融合，得到世界坐标系至IMU坐标系的转换矩阵。

进一步的，所述ArUco阵列为由多个ArUco码组成的阵列，包括多个大码和多个小码。

进一步的，视觉信息模块中，得到相机在世界坐标系中的位姿信息的具体过程为：

对相机拍摄的含有ArUco阵列的图像进行预处理；

通过四边形近似方法检测标记的角点；

根据标记的角点进行定位和识别；

根据角点在世界坐标系与相机坐标系中的对应关系，得到相机在世界坐标系中的位姿信息。

进一步的，数据融合模块包括预测模块和更新模块，预测模块用于利用融合定位算法根据机器人在世界坐标系中的位姿信息进行预测，更新模块用于利用IMU坐标系中的位姿信息和相机在世界坐标系中的位姿信息构建观测模型，利用观测模型对预测过程得到的预测结果进行迭代更新。

更进一步的，利用观测模型对预测过程得到的预测结果进行迭代更新的具体过程为：

构建观测矩阵，并根据预测模块得到的机器人当前时刻的先验估计协方差矩阵和噪声协方差矩阵计算卡尔曼增益；

根据卡尔曼增益和观测模型对预测模块得到的机器人当前时刻的位姿状态进行更新。

本发明第二方面提供了一种基于ArUco阵列的移动机器人室内定位方法，包括以下步骤：

获取机器人当前的运动状态，通过构建运动学模型计算机器人在世界坐标系中的位姿信息；

获取IMU坐标系中的位姿信息，并计算世界坐标系与IMU坐标系的转换关系；

获取相机拍摄的含有ArUco阵列的图像，通过对图像进行识别解析得到相机在世界坐标系中的位姿信息；

采用融合定位算法将机器人在世界坐标系中的位姿信息、IMU坐标系中的位姿信息和相机在世界坐标系中的位姿信息进行融合预测，得到机器人当前的位姿状态。

以上一个或多个技术方案存在以下有益效果：

本发明公开了一种基于ArUco阵列的移动机器人室内定位系统及方法，为了应对机器人位姿解算过程中不同移动距离的识别问题，本发明提出一种ArUco标记阵列的定位方法，在机器人长短距离移动过程中，都可以准确的获取机器人定位数据。为提高机器人的室内定位精度和鲁棒性，本发明使用动态方差融合定位算法(EKF)融合轮式里程计、IMU和视觉传感器的定位方法，有效地提高了室内定位的精度并减少了动态环境的影响。

本发明附加方面的优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

图1为本发明实施例一中移动机器人室内定位系统框架图；

图2为本发明实施例一中移动机器人系统坐标系示意图；

图3为本发明实施例一中移动机器人运动学模型示意图；

图4为本发明实施例一中马赫尼算法流程图；

图5为本发明实施例一中ArUco标记阵列示意图；

图6为本发明实施例一中坐标变换过程示意图；

图7为本发明实施例一中融合模块框架示意图；

图8为本发明实施例一中识别不同数量和尺寸的ArUco标记结果图；

图9为本发明实施例一中机器人移动路径图；

图10为本发明实施例一中EKF算法融合imu和轮式里程计数据计算机器人位置的实验结果图；

图11为本发明实施例一中EKF算法融合imu和轮式里程计数据和相机数据计算机器人位置的实验结果图；

图12为本发明实施例一中实验轨迹示意图。

具体实施方式

应该指出，以下详细说明都是示例性的，旨在对本发明提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合；

实施例一：

本发明实施例一提供了一种基于ArUco阵列的移动机器人室内定位系统，如图1所示，该室内定位系统集成了基于3种传感器，以确保机器人能够在室内进行精准定位。系统的运动学模型结合了磁编码器获取的轮速信息，并得出相对于出发点的定位数据。此外，通过安装在控制器电路板上的惯性传感器，能够解算出机器人的姿态角，以实现更加精准的运动控制。视觉传感器则安装在车体后侧，通过图像处理和位姿估计的算法，能够将识别计算结果发送到控制器，以进一步优化机器人的运动轨迹。

控制器使用EKF算法，融合三种传感器的定位数据。世界坐标系X_wY_wZ_w、标记坐标系X_mY_mZ_m、相机坐标系X_camY_camZ_cam、IMU坐标系X_imuY_imuZ_imu、机器人坐标系X_rY_rZ_r，如图2所示。其中世界坐标系与标记坐标系重合，机器人坐标系与IMU坐标系重合。

该系统具体包括驱动模块、惯性测量模块、视觉信息模块和数据融合模块。

驱动模块，用于获取机器人当前的运动状态，通过构建运动学模型计算机器人在世界坐标系中的位姿信息。

在一种具体的实施方式中，驱动模块包括磁编码器、电机和电机驱动，电机安装在机器人的车轮上，磁编码器安装在电机上，电机和电机驱动用于为机器人提供动力。

机器人装备了四个全向轮，使其可以在二维水平环境中实现全向移动。通过车轮电机上的编码器，利用M法获取单位时间内轮速，构建运动学模型，根据轮速计算机器人相对于出发点的定位数据从而得到机器人在世界坐标系中的位姿信息。

利用M法获取单位时间内轮速的具体过程为：在一定的时间周期Tc内，测量编码器输出的脉冲个数M1，得到编码器输出脉冲的频率。假设电机转动一圈可以产生Z个脉冲，用频率除以一圈的脉冲个数Z就得到单位时间内电机的转速，进而得到轮速。

如图3所示，[x,y,θ]^T为机器人在世界坐标系中的位姿，[x_cam,y_cam,θ_cam]^T为相机在世界坐标系中的位姿，为车轮角速度，i为车轮序号，l和b为车轮间距，r为车轮半径，a为相机距离机器人中心距离。

机器人的运动学模型表示为：

其中，v_x表示x方向的速度，v_y表示y方向的速度，ω表示角速度，

其中，T₁表示将车体坐标系下的水平、竖直、旋转速度，变换到全局坐标系下。T₂表示根据四个车轮的速度，解算出车体的速度。

机器人在第k个采样时刻上的姿态表示为q_k＝[x_k,y_k,θ_k]^T。x_k、y_k确定了机器人的中心位置，θ_k为机器人在世界坐标系XOY中的航向角。

其中，Δt表示传感器采样一次的时间间隔。

惯性测量模块，用于获取IMU坐标系中的位姿信息，并计算世界坐标系与IMU坐标系的转换关系。

在一种具体的实施方式中，惯性测量模块包括惯性测量单元，所述惯性测量单元包括陀螺仪、加速度计和磁力计，分别用以测量旋转角速度、加速度以及方向。其中，陀螺仪可以对物体的旋转角速度进行测量，通过对其积分，得到物体的横滚、俯仰和航向增量。加速度计能够测量物体在不同方向上的加速度，进行积分计算，可以获得物体的运动速度和位移。此外，磁力计可以感知磁北方向，从而得到物体的磁北航向信息。

陀螺仪具有良好的动态响应特性，但计算姿态时会产生累积误差。与此不同，磁力计和加速度计测量姿态时不会产生累积误差。然而，它们的动态响应性却不如陀螺仪。本实施例使用马赫尼(Mahony)算法，采用加速度计和磁力计的即时输出值对陀螺仪进行修正，可以达到优势互补的效果，进而提高测量精度和系统的动态性能。

具体的，惯性测量单元测量过程中首先进行校准：针对陀螺仪的校准，一般采用简单的方法：在上电后，让IMU处于静止状态一段时间，计算此时的零偏，每次采样时减去零偏即可。对于磁力计的校准，通常采用使安装有磁力计的载体做特定运动或者将载体转动到特定的角度的方式，利用磁力计在不同姿态下的磁场强度测量值，通过对测量值的分析进行磁力计的校准。

之后采用马赫尼算法将陀螺仪、加速度计和磁力计测量和计算得到的数据融合，得到世界坐标系至IMU坐标系的转换矩阵。

本实施例采用东北天坐标系，即IMU坐标系的x轴指向正东、y轴指向正北、z轴指向正上。Mahony算法的流程如图4所示。

其中，b系代表载体坐标系，n系为地理坐标系，为地理坐标系至载体坐标系之间的旋转矩阵，q_i为四元数，且有如下等式：

1.计算加速度计修正量

因为导航系N是北东地，机体系为前右下，如果加速度计坐标系与机体系一致，载体静止且水平时加速度计测量值应为[0,0,-1]，Z方向为负。利用C_n ^b矩阵将地球矢量转到机体系前右下，得到与实际加速度计输出求向量积。

2.计算磁力计修正量

磁力计的测量值经过C_b ⁿ转到地理系，得到[h_x,h_y,h_z]^T。因为航向不准，造成误差，所以转换后的磁力计测量值在北向和东向都有值。再将这个值转到机体系下，得到[w_x,w_y,w_z]^T。再与磁力计测量值求向量积。

3.将2个向量积求和后得到

4.根据pi调节，设置对陀螺测量值的修正量：

5.进行四元数的更新，并进行单位化得到

6.通过三角函数，计算出θ_imu

通过惯性测量模块，获得机器人角度估计值θ_{r_imu}。

θ_{r_imu}＝θ_imu (4)

视觉信息模块，用于获取相机拍摄的含有ArUco阵列的图像，通过对图像进行识别解析得到相机在世界坐标系中的位姿信息。

ArUco阵列为由多个ArUco码组成的阵列，包括多个大码和多个小码。每一个ArUco码都由一个黑色边框和内部编码组成，边框的四个角为标记的角点，内部编码包含标记的ID。在已知4个角点的三维坐标的情况下，结合相机的像素坐标，可得到单个ArUco码的位置和姿态。

在一种具体的实施方式中，通过对图像进行识别解析得到相机在世界坐标系中的位姿信息的具体过程为：

(1)对相机拍摄的含有ArUco阵列的图像进行预处理，预处理包括去除图像噪声、调整图像亮度、对比度、增强图像等操作。

(2)通过四边形近似方法检测标记的角点，并对角点按照顺时针排序。

使用单个标记，无法保证在近距离和远距离时都具有较好的定位精度。当距离较远时，定位精度下降。本实施例使用多个ArUco码组成的阵列，包括2个50cmx50cm的大码和5个14cmx14cm的小码，如图5所示。当距离标记较近时，相机检测到多个小标记，当距离较远时，相机检测到大标记和小标记，且大标记的图案在同一类中较易分辨。

在表1中，所呈现的数据是标记的四个角点在世界坐标系下的坐标值，单位为厘米。Id为2、8、9的角点坐标，可由轴对称关系得到。这些角点的顺序从标记左上角开始，按顺时针顺序排列。

表1.标记的交点坐标

ID＝1	ID＝5	ID＝6	ID＝7
				0，-53，87	0，-41，30	0，-24，30	0，-7，30
0，-3，87	0，-27，30	0，-10，30	0，7，30
				0，-3，37	0，-27，16	0，-10，16	0，7，16
0，-53，37	0，-41，16	0，-24，16	0，-7，16

(3)根据标记的角点进行定位和识别。将标记的每个子区域映射到一组位图，使用ArUco库中提供的字典来识别标记的ID，识别后根据标记的ID，确定标记是否有效并且与预期的标记相匹配。

(4)根据角点在世界坐标系与相机坐标系中的对应关系，得到相机在世界坐标系中的位姿信息。

具体的，通过多个标记分别计算出标记坐标系到相机坐标系的变换矩阵，再使用随机抽样一致(Ransac)算法，过滤掉误匹配的点对，从而提高求解的精度和鲁棒性。世界坐标系与标记坐标系重合，获得的变换矩阵等价于从世界坐标系到相机坐标系的旋转向量rvec和平移向量tvec，坐标变换关系如图6所示。利用罗德里格斯(Rodrigues)公式求解，将旋转向量转换为相关的旋转矩阵。由此，推导出了变换矩阵^camR_m。然而，在标记坐标系中获取相机的位置和姿态需要^mR_cam。

^camR_m＝Rodrigues(rvec)

其中，^mR_cam＝^cam R^-1 _m(5)

进而得到相机外参矩阵T，

经过坐标变换，得到相机在世界坐标系中的位置和姿态，进而推导出机器人的位姿。

需要特别说明的是，1.当旋转角度过大时，必须同时检测到2个以上的码再进行定位于角度的计算。2.当距离超过2.5m，且转角过大时，必须检测到一个大码和一个小码，再进行定位于角度的计算，认为数据有效。3.当相机离标记的距离超过2.5米，并且转角过大时，必须检测到一个大标记和一个小标记，方能认为数据有效，然后再进行计算以确定角度。

数据融合模块，如图7所示，用于将驱动模块、惯性测量模块和视觉信息模块中的位姿信息采用融合定位算法进行融合预测，得到机器人当前的位姿状态。数据融合模块包括预测模块和更新模块，预测模块用于利用融合定位算法根据机器人在世界坐标系中的位姿信息进行预测，更新模块用于利用IMU坐标系中的位姿信息和相机在世界坐标系中的位姿信息构建观测模型，利用观测模型对预测过程得到的预测结果进行迭代更新。

在一种具体的实施方式中，预测模块的预测过程为：

使用轮式里程计数据进行状态预测，驱动模块计算得到的机器人位姿状态为q_k＝[x_k,y_k,θ_k]^T。

控制系统状态方程如下

其中，w_k是k时刻的归一化过程噪声。u_k是系统控制向量,由外部对系统控制信号决定。

根据EKF算法，当前时刻机器人位姿状态的预测方程为：

其中，为预测得到的机器人位姿状态，Δt表示传感器采样一次的时间间隔。

当前时刻的先验估计协方差矩阵为：

其中，F_k-1代表上一时刻，由系统状态变换方程得到的状态转移矩阵,也叫预测矩阵。I₃代表3x3的单位矩阵。

其中，Q矩阵为3*3对角阵，表示系统模型中的过程噪声协方差矩阵。

更新模块的预测过程为：

在k时刻，从imu得到机器人姿态测量信息θ_{k,r_imu}，相机通过ArUco码计算得到机器人的位置信息[x_{k,r_cam},y_{k,_cam},θ_{k,r_cam}]^T。

若传感器数据都有效，则此时观测模型z_k为：

其中，v_k为k时刻的观测噪声。

构建观测矩阵，并根据预测模块得到的机器人当前时刻的先验估计协方差矩阵和噪声协方差矩阵计算卡尔曼增益。

具体的，卡尔曼增益K_k计算公式为：

观测矩阵H_k由IMU观测矩阵和相机观测矩阵组成，根据k时刻传感器数据是否更新以及是否有效，取值分为3种情况：

传感器噪声协方差矩阵R在机器人运动过程中起到关键作用。其默认值通常为R＝diag(0.1,0.1,0.1,0.1)，但在实际应用中，R矩阵需要实时更新以适应不同的运动状态和环境条件。具体而言，R矩阵的值等于传感器在时刻k之前的10次测量数据的方差。这种实时更新的方法能够更加准确地反映当前环境的变化，从而提高机器人的运动效率和精度。并且R的取值分类情况，也与H_k相同。

R＝diag(σ²(θ_{(k-10,k),r_imu}),σ²(x_{(k-10,k),r_cam}),σ²(y_{(k-10,k),r_cam}),σ²(θ_{(k-10,k),r_cam}))(17)

根据卡尔曼增益和观测模型对预测模块得到的机器人当前时刻的位姿状态进行更新。其公式为：

为验证基于EKF融合定位算法，在Webots中建立移动机器人仿真模型进行仿真。

步骤1，识别不同数量和尺寸的ArUco标记。

机器人从A点出发，沿着设定路径，到达B点。记录在下面4种情况下，通过识别ArUco标记计算出的机器人位置。

a.使用ID＝7的单个标记进行定位。

b.使用ID＝6～8的3个标记组成的阵列进行定位。

c.使用ID＝5～9的5个标记组成的阵列进行定位。

d.使用设计的ArUco阵列进行定位。

其结果如图8所示，从实验结果可以得出以下结论。单个小标记的定位精度差，距离标记2.5m以上时，定位数据偏差太大，不能再使用。多个小码的定位精度明显提高，当使用5个小码时，在距离标记3.5m处时，定位偏差在0.1m。使用大码和小码组合成矩阵的设计方式，在3.5m范围内，定位偏差在0.02m以内。

步骤2，使用EkF算法进行数据融合。

机器人沿A-B-C-D-E-F-G-A路径移动，如图9所示。

1.使用轮式里程计数据计算机器人的位置。并且使用EKF融合IMU的角度和轮式里程计数据。其中机器人的初始位置提前在程序中设定。实验结果如图10所示。根据实验结果可知，利用IMU所计算的yaw值来进行扩展卡尔曼滤波的角度更新，可有效消除轮式里程计的角度数据误差积累问题。然而，此方法并不能取得定位数据方面的改善。因此仍需进一步探究解决方案。

2.机器人首先在初始位置进行定位初始化，将相机定位数据作为机器人初始位置。机器人沿着设定轨迹运行3周，得到的轨迹图如下。在运行过程中的部分区域，相机视野中不再检测到标记，此时仅使用轮式里程计和IMU的数据进行融合定位。实验结果如图11所示。根据实验结果显示，图10中EKF算法计算位置图线可以看出，每运行一圈在x方向有大约0.08m的定位误差，并且随着运行圈数的增加而累积。在图11中，EKF算法计算位置图线在x方向，仅有0.05m范围内的小波动，且定位误差不会累积。因此，采用相机进行ArUco标记识别以获取定位数据，并借助扩展卡尔曼滤波进行位置更新，能够有效地消除轮式里程计在定位数据领域的累积误差，从而提高定位准确性。尤其值得注意的是，在相机数据无效的情况下，该方法仍能取得良好的定位效果。

为了验证该系统的有效性，搭建实验平台进行测试。在房屋四角放置直径20cm的红色圆，在机器人上放置直径15cm的红色与绿色圆，所有圆都在同一水平面上。房屋顶部固定一个竖直向下的相机，拍摄机器人运动过程，经过计算得到机器人位姿，以此作为真实值与其他数据比较。

由房屋顶部相机获取机器人位姿的流程如下：

Step1，经过颜色分割、形态学处理和Hough检测处理后，得到在像素坐标系中6个圆心坐标和半径值。

Step2，根据角落中的四个圆，在世界坐标系和像素坐标系中的坐标，计算透视变换矩阵T。

Step3，由机器人上红色和绿色圆心坐标，得到机器人上2个圆在世界坐标系中的位置(x_r,y_r)和(x_g,y_g)。

Step4，根据2个圆心坐标，计算得到机器人位置和航向角θ，并以此作为机器人室内定位数据真实值。

遥控机器人沿特定路线运行3圈，使用opencv实时处理安装在房屋顶部的相机视频，计算机器人位姿。同时，机器人通过ZigBee模块将EKF算法计算出的位置信息传输到电脑。图12为实验轨迹。

由轮式里程计估计出的位置和航向角，短时间内数据较为平稳。但是一段时间后，里程计数据存在累计误差，机器人运行5m后，定位误差会达到0.1m。使用相机进行定位，可以得到机器人全局定位信息。但是机器人在运动过程中，部分区域内相机不再朝向标记，当丢失或者重新检测到标记时，相机定位数据会出现暂时波动。设计的EKF数据融合算法，结合轮式里程计和视觉定位特点，定位误差在0.04m以内。

为了解决室内定位问题，本实施例采用ArUco人工标识、单目相机、IMU和轮式编码器方案。在定位算法上，采用了基于EKF的数据融合算法，以实现移动机器人的精确、鲁棒定位。通过实验表明，本实施例提出的动态方差EKF融合定位算法，相比于单个传感器的定位方法，可以有效改善移动机器人在定位过程中姿态偏离和定位误差累积的问题，有效降低了定位的随机误差，从而提高了定位精度和鲁棒性。

实施例二：

本发明实施例二提供了一种基于ArUco阵列的移动机器人室内定位方法，包括以下步骤：

获取机器人当前的运动状态，通过构建运动学模型计算机器人在世界坐标系中的位姿信息；

获取IMU坐标系中的位姿信息，并计算世界坐标系与IMU坐标系的转换关系；

获取相机拍摄的含有ArUco阵列的图像，通过对图像进行识别解析得到相机在世界坐标系中的位姿信息；

采用融合定位算法将机器人在世界坐标系中的位姿信息、IMU坐标系中的位姿信息和相机在世界坐标系中的位姿信息进行融合预测，得到机器人当前的位姿状态。

以上实施例二中涉及的各步骤与方法实施例一相对应，具体实施方式可参见实施例一的相关说明部分。

本领域技术人员应该明白，上述本发明的各模块或各步骤可以用通用的计算机装置来实现，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。本发明不限制于任何特定的硬件和软件的结合。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (10)

1.一种基于ArUco阵列的移动机器人室内定位系统，其特征在于，包括：

驱动模块，用于获取机器人当前的运动状态，通过构建运动学模型计算机器人在世界坐标系中的位姿信息；

惯性测量模块，用于获取IMU坐标系中的位姿信息，并计算世界坐标系与IMU坐标系的转换关系；

视觉信息模块，用于获取相机拍摄的含有ArUco阵列的图像，通过对图像进行识别解析得到相机在世界坐标系中的位姿信息；

数据融合模块，用于将驱动模块、惯性测量模块和视觉信息模块中的位姿信息采用融合定位算法进行融合预测，得到机器人当前的位姿状态。

2.如权利要求1所述的基于ArUco阵列的移动机器人室内定位系统，其特征在于，驱动模块包括磁编码器，利用编码器获得单位时间内的轮速，构建运动学模型，根据轮速计算机器人相对于出发点的定位数据从而得到机器人在世界坐标系中的位姿信息。

3.如权利要求2所述的基于ArUco阵列的移动机器人室内定位系统，其特征在于，驱动模块还包括电机和电机驱动，电机安装在机器人的车轮上，磁编码器安装在电机上，电机和电机驱动用于为机器人提供动力。

4.如权利要求1所述的基于ArUco阵列的移动机器人室内定位系统，其特征在于，惯性测量模块包括惯性测量单元，所述惯性测量单元包括陀螺仪、加速度计和磁力计，分别用以测量旋转角速度、加速度以及方向。

5.如权利要求4所述的基于ArUco阵列的移动机器人室内定位系统，其特征在于，惯性测量单元测量过程中首先进行校准，之后采用马赫尼算法将陀螺仪、加速度计和磁力计测量和计算得到的数据融合，得到世界坐标系至IMU坐标系的转换矩阵。

6.如权利要求1所述的基于ArUco阵列的移动机器人室内定位系统，其特征在于，所述ArUco阵列为由多个ArUco码组成的阵列，包括多个大码和多个小码。

7.如权利要求1所述的基于ArUco阵列的移动机器人室内定位系统，其特征在于，视觉信息模块中，得到相机在世界坐标系中的位姿信息的具体过程为：

对相机拍摄的含有ArUco阵列的图像进行预处理；

通过四边形近似方法检测标记的角点；

根据标记的角点进行定位和识别；

根据角点在世界坐标系与相机坐标系中的对应关系，得到相机在世界坐标系中的位姿信息。

8.如权利要求1所述的基于ArUco阵列的移动机器人室内定位系统，其特征在于，数据融合模块包括预测模块和更新模块，预测模块用于利用融合定位算法根据机器人在世界坐标系中的位姿信息进行预测，更新模块用于利用IMU坐标系中的位姿信息和相机在世界坐标系中的位姿信息构建观测模型，利用观测模型对预测过程得到的预测结果进行迭代更新。

9.如权利要求8所述的基于ArUco阵列的移动机器人室内定位系统，其特征在于，利用观测模型对预测过程得到的预测结果进行迭代更新的具体过程为：

构建观测矩阵，并根据预测模块得到的机器人当前时刻的先验估计协方差矩阵和噪声协方差矩阵计算卡尔曼增益；

根据卡尔曼增益和观测模型对预测模块得到的机器人当前时刻的位姿状态进行更新。

10.一种基于ArUco阵列的移动机器人室内定位方法，其特征在于，包括以下步骤：

获取机器人当前的运动状态，通过构建运动学模型计算机器人在世界坐标系中的位姿信息；

获取IMU坐标系中的位姿信息，并计算世界坐标系与IMU坐标系的转换关系；

获取相机拍摄的含有ArUco阵列的图像，通过对图像进行识别解析得到相机在世界坐标系中的位姿信息；

采用融合定位算法将机器人在世界坐标系中的位姿信息、IMU坐标系中的位姿信息和相机在世界坐标系中的位姿信息进行融合预测，得到机器人当前的位姿状态。