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

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

【技术实现步骤摘要】
基于UGOFusion的移动机器人定位方法
本专利技术属于移动机器人定位方法，可以为教学实验中的移动机器人提供精确的位置信息，验证各种导航方法的正确性。
技术介绍
最近几年，随着人工智能火热迅猛地发展，机器人再一次受到前所未有的关注，移动机器人自主导航问题受到众多高校，研究所的研究。而其中移动机器人的定位是机器人自主导航非常关键的一步。移动机器人要完成自主导航，首先需要知晓其自身处于环境中的位置，然后才能完成地图构建或路径规划等后续工作，定位是移动机器人的一项最基础的能力。目前移动机器人定位方式按照机器人所使用传感器和定位技术的不同可以分为惯性导航定位、地图匹配定位和基于信标的定位方式。单一的定位方法会存在某方面的不足，比如惯性导航定位需要给定初始的位置和姿态，在机器人的移动过程中会产生累计误差；而地图匹配定位方式适用于静态环境，对于动态环境的定位容错率低；大多数基于信标的定位精度低，抗多径能力差，基本无法满足室内移动机器人对于定位精度的要求。为了解决单一定位方式定位精度低等问题本专利技术提出了融合UWB，陀螺仪，里程计多传感器信息的室内移动机器人定位方法。实验结果表明，应用本专利技术提出的UGOFusion定位方法有效提高了移动机器人的定位精度。可以为移动机器人的自主导航提供准确的位置信息。
技术实现思路
本专利技术基于超宽带(UWB)，陀螺仪(Gyroscope)，里程计(Odometer)多传感器的信息进行融合的思想提出室内移动机器人基于UGOFusion的定位方法。UGOFusion定位方法第一步采用英国DeacWave公司低成本的DWM1000测距模块为超宽带无线收发模块搭建室内定位系统。第二步基于飞行时间算法测定定位系统中基站与标签间的距离。第三步基于上一步测定的距离利用三边定位算法解算机器人位置。第四步根据给定的机器人运动模型建立运动方程观测方程。第五步基于扩展卡尔曼滤波算法对超宽带，里程计，陀螺仪传感器数据进行融合，得到机器人的准确位姿。为了验证UGOFusion方法进行机器人位姿估计的可靠性，设置了两种固定轨迹：真实轨迹1和真实轨迹2，真实轨迹1为方形轨迹(SquareTrajectory，ST)，真实轨迹2为“S”型轨迹(“S”typeTrajectory，STT)。该方法对超宽带(UWB)，陀螺仪(Gyroscope)，里程计(Odometer)多传感器的信息进行融合。解决了单一UWB定位历史数据离散，无法准确的估计机器人朝向问题。实验结果表明，应用本专利技术提出的UGO定位方法有效提高了移动机器人的定位精度，同时解决了移动机器人获取可靠偏航角难的问题。基于该方法搭建的定位系统可以作为移动机器人导航的定位系统。本专利技术采用的技术方案为基于UGOFusion的移动机器人定位方法，该方法具体包括以下步骤：第一步、超宽带的室内定位系统搭建模块参照图1，设置三个参考节点于室内固定位置，三个参考点位置已知x1y1，x2y2，x3y3。移动节点置于移动机器人上(x，y)。为名称统一将参考节点称为锚节点(Anchor)，移动节点称为标签(Tag)，超宽带(UWB)模块采用英国DeacWave公司低成本的DWM1000测距模块为超宽带无线收发模块，兼容IEEE802.15.4-2011协议，该模块集成了天线、RF电路、电源管理和时钟电路，官方的测距精度为10cm，该模块支持标准的SPI通信，可以与微控制器进行通信。第二步、基于飞行时间(TOF)测距模块参照图2，通过测定UWB脉冲信号从标签节点到锚节点的信号往返时间从而确定其距离，采样频率为50HZ。标签节点以一定时间间隔启动测距过程，而锚节点则始终保持监听状态并在收到测距信号后回应并计算节点间的距离。TSP为标签节点发送信号时间戳，TRP为锚节点接收信号时间戳，经过TARSP时间段延迟后，TSR为锚节点发送信号时间戳，TRR为标签节点接收锚节点回应信号时间戳，经过TTRSP时间段延迟后，TSF为标签节点第二次发送信号时间戳。TRF为锚节点第二次接收到信号时间戳。对于标签节点，信号输出的往返延迟时间为：TTRT＝TRR-TSP锚节点的延误时间：TART＝TRF-TSR标签节点的响应时间为：TTRSP＝TSF-TRR锚节点的响应时间为：TARSP＝TSR-TRP往返延误时间中除了信号在空气中的飞行时间还包括了节点从接收到发送之间的响应时间。为了消除两边终端设备时钟差异造成的影响，将两端往返行程的时间取平均，再除以2得到单程的时间，得单程的飞行时间为：得两点直接的距离r：r＝c*TTOF其中c为光速。第三步、三边定位模块，通过TOF测定标签节点到锚节点的距离，通过三边定位法得到标签位置。三个已知坐标锚节点能对标签节点进行定位。假设标签节点和锚节点在同一平面上标签节点，标签节点位置计算方程的公式如下：该方程用线性方程组表示：第四步、移动机器人建立模型模块参照图3，XYZ坐标系为导航坐标系，XrYrZr坐标系为机器人载体坐标系。超宽带定位系统的锚节点固定在室内特定位置，标签节点固定在移动机器人上。通过UWB标签节点输出的信息获取移动机器人的位置信息，通过陀螺仪陀(螺仪型号为STMicroelectronics生产的L3G4200D，测量范围为±250°/s，采样频率为50Hz。)获得其姿态，里程计采用高精度增量编码器(11.7ticks/mm，25718.16ticks/revolution)，采样频率为50Hz，可获得机器人线速度。根据欧拉角微分方程，可以得到姿态角与角速度关系为为机器人载体坐标系相对于惯性坐标系的角速度在机器人载体坐标系中的投影。轮式机器人的线速度通过编码器获得，编码器的线数为P即轮子转一圈编码器的输出的脉冲数，轮径为D，通过左右的编码器的脉冲频率fL和fR算出左右轮子的线速度分别为VL和VR，进而得出机器人速度V。在二维平面中运动机器人的状态方程，通过标签点获得机器人在导航坐标系中的位置坐标(x，y)，编码器获得机器人的线速度v，陀螺仪获得机器人的的偏航角速度用向量描述机器人k时刻系统的状态。系统的运动方程是：机器人室内运动模型中，采样周期为T，机器人在k-1时刻所在的估计位置在点pk-1处，后验估计状态为假设机器人沿着弧s在k时刻运动到位置为pk处，在pk处的先验估计状态为则弧长弧s对应的圆心为o，圆心角角度圆弧的半径为直线长度该直线与x轴夹角为推导得到系统的状态方程为：式中为k-1时刻的后验估计，为k时刻的先验估计，uk-1为k-1时刻的控制量，wk-1为k-1时刻的过程噪声wk-1～(0，QK)，假设机器匀速前进则控制量为uk＝0。系统的测量方程是：ZK＝HXK+μk式中μk为测量噪声，μk～N(0，RK)，RK为k时刻的观测噪声协方差矩阵。由于多径效应、障碍物遮挡等原因。观测值会有粗大误差值。剔除粗大误差值设置一个安全阈值距离D，计算预测值与真实UWB观测值之间的距离dk根据系统所需精度而定。如果dk＞D则视为粗大误差将此刻的观测值剔除，此值不参与位姿更新过程。第五步基于扩展卡尔曼滤波算法对超宽带，陀螺仪，里程计传感器信息进行融合，上面的到的状态方程为非线性方程，需要依照EKF的方式对其进行线性化和离散化处理，得到k时刻的过程雅可比矩阵本文档来自技高网...

【技术保护点】
1.基于UGO Fusion的移动机器人定位方法，其特征在于：该方法具体包括以下步骤：第一步、超宽带的室内定位系统搭建模块，设置三个参考节点于室内固定位置，三个参考点位置已知x1y1，x2y2，x3y3；移动节点置于移动机器人上(x，y)；将参考节点称为锚节点，移动节点称为标签，超宽带模块为超宽带无线收发模块，集成了天线、RF电路、电源管理和时钟电路，官方的测距精度为10cm，该超宽带模块支持标准的SPI通信，与微控制器进行通信；第二步、基于飞行时间测距模块，通过测定UWB脉冲信号从标签节点到锚节点的信号往返时间从而确定其距离，采样频率为50HZ；标签节点以一定时间间隔启动测距过程，而锚节点则始终保持监听状态并在收到测距信号后回应并计算节点间的距离；TSP为标签节点发送信号时间戳，TRP为锚节点接收信号时间戳，经过TARSP时间段延迟后，TSR为锚节点发送信号时间戳，TRR为标签节点接收锚节点回应信号时间戳，经过TTRSP时间段延迟后，TSF为标签节点第二次发送信号时间戳；TRF为锚节点第二次接收到信号时间戳；对于标签节点，信号输出的往返延迟时间为：TTRT＝TRR‑TSP锚节点的延误时间：TART＝TRF‑TSR标签节点的响应时间为：TTRSP＝TSF‑TRR锚节点的响应时间为：TARSP＝TSR‑TRP往返延误时间中除了信号在空气中的飞行时间还包括了节点从接收到发送之间的响应时间；为了消除两边终端设备时钟差异造成的影响，将两端往返行程的时间取平均，再除以2得到单程的时间，得单程的飞行时间为：...

【技术特征摘要】
1.基于UGOFusion的移动机器人定位方法，其特征在于：该方法具体包括以下步骤：第一步、超宽带的室内定位系统搭建模块，设置三个参考节点于室内固定位置，三个参考点位置已知x1y1，x2y2，x3y3；移动节点置于移动机器人上(x，y)；将参考节点称为锚节点，移动节点称为标签，超宽带模块为超宽带无线收发模块，集成了天线、RF电路、电源管理和时钟电路，官方的测距精度为10cm，该超宽带模块支持标准的SPI通信，与微控制器进行通信；第二步、基于飞行时间测距模块，通过测定UWB脉冲信号从标签节点到锚节点的信号往返时间从而确定其距离，采样频率为50HZ；标签节点以一定时间间隔启动测距过程，而锚节点则始终保持监听状态并在收到测距信号后回应并计算节点间的距离；TSP为标签节点发送信号时间戳，TRP为锚节点接收信号时间戳，经过TARSP时间段延迟后，TSR为锚节点发送信号时间戳，TRR为标签节点接收锚节点回应信号时间戳，经过TTRSP时间段延迟后，TSF为标签节点第二次发送信号时间戳；TRF为锚节点第二次接收到信号时间戳；对于标签节点，信号输出的往返延迟时间为：TTRT＝TRR-TSP锚节点的延误时间：TART＝TRF-TSR标签节点的响应时间为：TTRSP＝TSF-TRR锚节点的响应时间为：TARSP＝TSR-TRP往返延误时间中除了信号在空气中的飞行时间还包括了节点从接收到发送之间的响应时间；为了消除两边终端设备时钟差异造成的影响，将两端往返行程的时间取平均，再除以2得到单程的时间，得单程的飞行时间为：得两点直接的距离r：r＝c*TTOF其中c为光速；第三步、三边定位模块，通过TOF测定标签节点到锚节点的距离，通过三边定位法得到标签位置；三个已知坐标锚节点能对标签节点进行定位；假设标签节点和锚节点在同一平面上标签节点，标签节点位置计算方程的公式如下：该方程用线性方程组表示：第四步、移动机器人建立模型模块，XYZ坐标系为导航坐标系，XrYrZr坐标系为机器人载体坐标系；超宽带定位系统的锚节点固定在室内特定位置，标签节点固定在移动机器人上；通过UWB标签节点输出的信息...

【专利技术属性】
技术研发人员：阮晓钢，刘少达，朱晓庆，任顶奇，武悦，李昂，
申请(专利权)人：北京工业大学，
类型：发明
国别省市：北京,11