CN109375158A - 基于UGO Fusion的移动机器人定位方法 - Google Patents

Abstract

本发明公开了基于UGO Fusion的移动机器人定位方法。为方便起见以下简称该算法为UGO Fusion定位算法。该算法基于扩展卡尔曼滤波(Extend Kalman Filter,EKF)。解决了单一定位算法精度不高，单一UWB定位算法无法准确的估计机器人朝向问题。该算法包括室内超宽带定位系统搭建模块、基于飞行时间测距模块、三边定位模块、移动机器人建立模型模块、超宽带、陀螺仪、里程计数据融合模块。物理实验结果表明，应用本发明提出的UGO Fusion定位算法有效提高了移动机器人的定位精度，同时得到移动机器人准确的偏航角。可为移动机器人的自主导航提供精确的位姿。

Description

基于UGO Fusion的移动机器人定位方法

技术领域

本发明属于移动机器人定位方法，可以为教学实验中的移动机器人提供精确的位置信息，验证各种导航方法的正确性。

背景技术

最近几年，随着人工智能火热迅猛地发展，机器人再一次受到前所未有的关注，移动机器人自主导航问题受到众多高校，研究所的研究。而其中移动机器人的定位是机器人自主导航非常关键的一步。

移动机器人要完成自主导航，首先需要知晓其自身处于环境中的位置，然后才能完成地图构建或路径规划等后续工作，定位是移动机器人的一项最基础的能力。目前移动机器人定位方式按照机器人所使用传感器和定位技术的不同可以分为惯性导航定位、地图匹配定位和基于信标的定位方式。单一的定位方法会存在某方面的不足，比如惯性导航定位需要给定初始的位置和姿态，在机器人的移动过程中会产生累计误差；而地图匹配定位方式适用于静态环境，对于动态环境的定位容错率低；大多数基于信标的定位精度低，抗多径能力差，基本无法满足室内移动机器人对于定位精度的要求。

为了解决单一定位方式定位精度低等问题本发明提出了融合UWB，陀螺仪，里程计多传感器信息的室内移动机器人定位方法。实验结果表明，应用本发明提出的UGO Fusion定位方法有效提高了移动机器人的定位精度。可以为移动机器人的自主导航提供准确的位置信息。

发明内容

本发明基于超宽带(UWB)，陀螺仪(Gyroscope)，里程计(Odometer)多传感器的信息进行融合的思想提出室内移动机器人基于UGO Fusion的定位方法。UGO Fusion定位方法第一步采用英国DeacWave公司低成本的DWM1000测距模块为超宽带无线收发模块搭建室内定位系统。第二步基于飞行时间算法测定定位系统中基站与标签间的距离。第三步基于上一步测定的距离利用三边定位算法解算机器人位置。第四步根据给定的机器人运动模型建立运动方程观测方程。第五步基于扩展卡尔曼滤波算法对超宽带，里程计，陀螺仪传感器数据进行融合，得到机器人的准确位姿。为了验证UGO Fusion方法进行机器人位姿估计的可靠性，设置了两种固定轨迹：真实轨迹1和真实轨迹2，真实轨迹1为方形轨迹(SquareTrajectory，ST)，真实轨迹2为“S”型轨迹(“S”type Trajectory，STT)。该方法对超宽带(UWB)，陀螺仪(Gyroscope)，里程计(Odometer)多传感器的信息进行融合。解决了单一UWB定位历史数据离散，无法准确的估计机器人朝向问题。实验结果表明，应用本发明提出的UGO定位方法有效提高了移动机器人的定位精度，同时解决了移动机器人获取可靠偏航角难的问题。基于该方法搭建的定位系统可以作为移动机器人导航的定位系统。

本发明采用的技术方案为基于UGO Fusion的移动机器人定位方法，该方法具体包括以下步骤：

第一步、超宽带的室内定位系统搭建模块参照图1，设置三个参考节点于室内固定位置，三个参考点位置已知x₁y₁，x₂y₂，x₃y₃。移动节点置于移动机器人上(x，y)。为名称统一将参考节点称为锚节点(Anchor)，移动节点称为标签(Tag)，超宽带(UWB)模块采用英国DeacWave公司低成本的DWM1000测距模块为超宽带无线收发模块，兼容IEEE802.15.4-2011协议，该模块集成了天线、RF电路、电源管理和时钟电路，官方的测距精度为10cm，该模块支持标准的SPI通信，可以与微控制器进行通信。

第二步、基于飞行时间(TOF)测距模块参照图2，通过测定UWB脉冲信号从标签节点到锚节点的信号往返时间从而确定其距离，采样频率为50HZ。标签节点以一定时间间隔启动测距过程，而锚节点则始终保持监听状态并在收到测距信号后回应并计算节点间的距离。

T_SP为标签节点发送信号时间戳，T_RP为锚节点接收信号时间戳，经过T_ARSP时间段延迟后，T_SR为锚节点发送信号时间戳，T_RR为标签节点接收锚节点回应信号时间戳，经过T_TRSP时间段延迟后，T_SF为标签节点第二次发送信号时间戳。T_RF为锚节点第二次接收到信号时间戳。

对于标签节点，信号输出的往返延迟时间为：

T_TRT＝T_RR-T_SP

锚节点的延误时间：

T_ART＝T_RF-T_SR

标签节点的响应时间为：

T_TRSP＝T_SF-T_RR

锚节点的响应时间为：

T_ARSP＝T_SR-T_RP

往返延误时间中除了信号在空气中的飞行时间还包括了节点从接收到发送之间的响应时间。为了消除两边终端设备时钟差异造成的影响，将两端往返行程的时间取平均，再除以2得到单程的时间，得单程的飞行时间为：

得两点直接的距离r：

r＝c*T_TOF

其中c为光速。

第三步、三边定位模块，通过TOF测定标签节点到锚节点的距离，通过三边定位法得到标签位置。三个已知坐标锚节点能对标签节点进行定位。假设标签节点和锚节点在同一平面上标签节点，标签节点位置计算方程的公式如下：

该方程用线性方程组表示：

第四步、移动机器人建立模型模块参照图3，XYZ坐标系为导航坐标系，X_rY_rZ_r坐标系为机器人载体坐标系。超宽带定位系统的锚节点固定在室内特定位置，标签节点固定在移动机器人上。通过UWB标签节点输出的信息获取移动机器人的位置信息，通过陀螺仪陀(螺仪型号为STMicroelectronics生产的L3G4200D，测量范围为±250°/s，采样频率为50Hz。)获得其姿态，里程计采用高精度增量编码器(11.7ticks/mm，25718.16ticks/revolution)，采样频率为50Hz，可获得机器人线速度。根据欧拉角微分方程，可以得到姿态角与角速度关系为为机器人载体坐标系相对于惯性坐标系的角速度在机器人载体坐标系中的投影。轮式机器人的线速度通过编码器获得，编码器的线数为P即轮子转一圈编码器的输出的脉冲数，轮径为D，通过左右的编码器的脉冲频率f_L和f_R算出左右轮子的线速度分别为V_L和V_R，进而得出机器人速度V。

在二维平面中运动机器人的状态方程，通过标签点获得机器人在导航坐标系中的位置坐标(x，y)，编码器获得机器人的线速度v，陀螺仪获得机器人的的偏航角速度用向量描述机器人k时刻系统的状态。系统的运动方程是：

机器人室内运动模型中，采样周期为T，机器人在k-1时刻所在的估计位置在点p_k-1处，后验估计状态为假设机器人沿着弧s在k时刻运动到位置为p_k处，在p_k处的先验估计状态为则弧长弧s对应的圆心为o，圆心角角度圆弧的半径为直线长度该直线与x轴夹角为推导得到系统的状态方程为：

式中为k-1时刻的后验估计，为k时刻的先验估计，u_k-1为k-1时刻的控制量，w_k-1为k-1时刻的过程噪声w_k-1～(0，Q_K)，假设机器匀速前进则控制量为u_k＝0。

系统的测量方程是：

Z_K＝HX_K+μ_k

式中μ_k为测量噪声，μ_k～N(0，R_K)，R_K为k时刻的观测噪声协方差矩阵。由于多径效应、障碍物遮挡等原因。观测值会有粗大误差值。剔除粗大误差值设置一个安全阈值距离D，计算预测值与真实UWB观测值之间的距离d_k根据系统所需精度而定。如果d_k＞D则视为粗大误差将此刻的观测值剔除，此值不参与位姿更新过程。

第五步基于扩展卡尔曼滤波算法对超宽带，陀螺仪，里程计传感器信息进行融合，上面的到的状态方程为非线性方程，需要依照EKF的方式对其进行线性化和离散化处理，得到k时刻的过程雅可比矩阵如下：

扩展卡尔曼时间更新过程为：

式中，为K时刻先验误差协方差，P_K-1为K-1时刻协方差。

测量更新过程：

式中，K_K是K时刻的卡尔曼增益。

通过时间更新过程和测量更新过程不断修正机器人该时刻的位姿，得到相对可靠地机器人位置和偏航角。

附图说明

图1基于超宽带(UWB)室内定位系统搭建图。

图2 TOF测距原理图。

图3移动机器人运动模型图。

图4方形轨迹。

图5 UWB测量值(方形)。

图6陀螺仪里程计积分轨迹(方形)。

图7 UGO Fusion轨迹(方形)。

图8偏航角误差对比曲线(方形)。

图9 S型轨迹。

图10 UWB测量值(S型)。

图11陀螺仪里程计积分轨迹(S型)。

图12 UGO Fusion轨迹(S型)。

图13偏航角误差对比曲线(S型)。

具体实施方式

下面对本发明的实施例作详细说明，本实施例在本发明技术方案为前提下进行实施，给出了方形轨迹详细的实施方式和具体的操作过程，S型轨迹同理但本发明的保护范围不限于下述的实施例。

实施例

第一步中超宽带的室内定位系统搭建模块，采用四个英国DeacWave公司低成本的DWM1000测距模块，根据图1设置三个UWB模块为参考节点置于室内固定位置，三个参考节点位置由人工精准量出可得坐标(x₁y₁)(x₂y₂)(x₃y₃)。移动节点置于移动机器人上(x，y)待求。为名称统一将参考节点称为锚节点(Anchor)，移动节点称为标签(Tag).

第二步中基于飞行时间(TOF)测距模块，根据图4的已知方形轨迹推动机器人沿着方形轨迹匀速行进机器人的起点为(2.63，1.96)，沿着路径1、2、3、4到达终点(2.63，1.96)位置。利用如下公式可以实时得出三个锚节点对标签节点的距离r₁r₂r₃。

第三步中三边定位模块，通过TOF可以实时测定标签节点到锚节点的实时距离r₁r₂r₃，在此基础上通过三边定位法可以得到标签位置。三个已知坐标锚节点能对标签节点进行定位。标签节点和锚节点在同一平面上标签节点，依据标签节点位置计算公式：可以实时得出标签(机器人)节点的位置(x，y)如图5可以看出该值含有噪声，数据离散程度较高，但UWB测量值基本在真实轨迹附近跳动，说明UWB可以为移动机器人提供相对可靠的位置，但是无法提供给机器人可靠的偏航角。

第四步移动机器人建立模型模块图2，图1中XYZ坐标系为导航坐标系，X_rY_rZ_r坐标系为机器人载体坐标系。沿着方形轨迹匀速推动机器人过程中通过陀螺仪陀(螺仪型号为STMicroelectronics生产的L3G4200D，测量范围为±250°/s，采样频率为50Hz。)实时可获得其姿态，里程计采用高精度增量编码器(11.7ticks/mm，25718.16ticks/revolution)，采样频率为50Hz，实时可获得机器人线速度。安全阈值D设置为30cm。机器人的惯导系统为陀螺仪和里程计进行积分(Gyroscopes and Odometer Integration，GOI)后得到的运动轨迹，通过陀螺仪和里程计积分得到的轨迹如图6所示。从图中可以看出，短时间内积分得到的路径相对准确，随着时间的累计，误差越来越大，积分得到的路径越来越偏移真实轨迹。

第五步依据EKF对超宽带，陀螺仪，里程计数据进行融合。可得图6基于UGO Fusion定位方法的机器人运动轨迹。图中可以看出，使用UGO_Fusion得到的估计位置与实际轨迹比较接近，有效提高了机器人的定位精度。

移动机器人从路径1到路径4的偏航角依次为根据图8可看出随着时间的增加，陀螺仪积分得到的偏航角误差越来越大，而使用UGO_Fusion估计的偏航角误差基本维持在0参考线附近。

实施效果

依据以上步骤，本发明设计了方形，S型两种轨迹实验。实验结果如图4-图13所示。将UGO Fusion方法与直接对陀螺仪，里程计积分全方位对比，实验结果表明应用本发明提出的UGO Fusion定位方法有效提高了移动机器人的定位精度，同时得到移动机器人准确的偏航角。可为移动机器人的自主导航提供精确的位姿。

Claims (1)

1.基于UGO Fusion的移动机器人定位方法，其特征在于：该方法具体包括以下步骤：

第一步、超宽带的室内定位系统搭建模块，设置三个参考节点于室内固定位置，三个参考点位置已知x₁y₁，x₂y₂，x₃y₃；移动节点置于移动机器人上(x，y)；将参考节点称为锚节点，移动节点称为标签，超宽带模块为超宽带无线收发模块，集成了天线、RF电路、电源管理和时钟电路，官方的测距精度为10cm，该超宽带模块支持标准的SPI通信，与微控制器进行通信；

第二步、基于飞行时间测距模块，通过测定UWB脉冲信号从标签节点到锚节点的信号往返时间从而确定其距离，采样频率为50HZ；标签节点以一定时间间隔启动测距过程，而锚节点则始终保持监听状态并在收到测距信号后回应并计算节点间的距离；

T_SP为标签节点发送信号时间戳，T_RP为锚节点接收信号时间戳，经过T_ARSP时间段延迟后，T_SR为锚节点发送信号时间戳，T_RR为标签节点接收锚节点回应信号时间戳，经过T_TRSP时间段延迟后，T_SF为标签节点第二次发送信号时间戳；T_RF为锚节点第二次接收到信号时间戳；

对于标签节点，信号输出的往返延迟时间为：

T_TRT＝T_RR-T_SP

锚节点的延误时间：

T_ART＝T_RF-T_SR

标签节点的响应时间为：

T_TRSP＝T_SF-T_RR

锚节点的响应时间为：

T_ARSP＝T_SR-T_RP

往返延误时间中除了信号在空气中的飞行时间还包括了节点从接收到发送之间的响应时间；为了消除两边终端设备时钟差异造成的影响，将两端往返行程的时间取平均，再除以2得到单程的时间，得单程的飞行时间为：

得两点直接的距离r：

r＝c*T_TOF

其中c为光速；

第三步、三边定位模块，通过TOF测定标签节点到锚节点的距离，通过三边定位法得到标签位置；三个已知坐标锚节点能对标签节点进行定位；假设标签节点和锚节点在同一平面上标签节点，标签节点位置计算方程的公式如下：

该方程用线性方程组表示：

第四步、移动机器人建立模型模块，XYZ坐标系为导航坐标系，X_rY_rZ_r坐标系为机器人载体坐标系；超宽带定位系统的锚节点固定在室内特定位置，标签节点固定在移动机器人上；通过UWB标签节点输出的信息获取移动机器人的位置信息，通过陀螺仪陀获得姿态，里程计采用高精度增量编码器，采样频率为50Hz，获得机器人线速度；根据欧拉角微分方程，得到姿态角与角速度关系为 为机器人载体坐标系相对于惯性坐标系的角速度在机器人载体坐标系中的投影；轮式机器人的线速度通过编码器获得，编码器的线数为P即轮子转一圈编码器的输出的脉冲数，轮径为D，通过左右的编码器的脉冲频率f_L和f_R算出左右轮子的线速度分别为V_L和V_R，进而得出机器人速度V；

在二维平面中运动机器人的状态方程，通过标签点获得机器人在导航坐标系中的位置坐标(x，y)，编码器获得机器人的线速度v，陀螺仪获得机器人的的偏航角速度用向量描述机器人k时刻系统的状态；系统的运动方程是：

机器人室内运动模型中，采样周期为T，机器人在k-1时刻所在的估计位置在点p_k-1处，后验估计状态为假设机器人沿着弧s在k时刻运动到位置为p_k处，在p_k处的先验估计状态为则弧长弧s对应的圆心为o，圆心角角度圆弧的半径为 该直线与x轴夹角为推导得到系统的状态方程为：

式中为k-1时刻的后验估计， 为k时刻的先验估计，u_k-1为k-1时刻的控制量，w_k-1为k-1时刻的过程噪声w_k-1～(0，Q_K)，假设机器匀速前进则控制量为u_k＝0；

系统的测量方程是：

Z_K＝HX_K+μ_k

式中μ_k为测量噪声，μ_k～N(0，R_K)，R_K为k时刻的观测噪声协方差矩阵；由于多径效应、障碍物遮挡等原因；观测值会有粗大误差值；剔除粗大误差值设置一个安全阈值距离D，计算预测值与真实UWB观测值之间的距离d_k根据系统所需精度而定；如果d_k＞D则视为粗大误差将此刻的观测值剔除，此值不参与位姿更新过程；

第五步基于扩展卡尔曼滤波算法对超宽带，陀螺仪，里程计传感器信息进行融合，得到的状态方程为非线性方程，需要依照EKF的方式对其进行线性化和离散化处理，得到k时刻的过程雅可比矩阵如下：

扩展卡尔曼时间更新过程为：

式中，为K时刻先验误差协方差，P_K-1为K-1时刻协方差；

测量更新过程：

式中，K_K是K时刻的卡尔曼增益；

通过时间更新过程和测量更新过程不断修正机器人该时刻的位姿，得到相对可靠地机器人位置和偏航角。