[发明专利]基于UGO Fusion的移动机器人定位方法

摘要

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

主权项

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得到单程的时间，得单程的飞行时间为：得两点直接的距离r：r＝c*TTOF其中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；系统的测量方程是：ZK＝HXK+μ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时刻协方差；测量更新过程：式中，KK是K时刻的卡尔曼增益；通过时间更新过程和测量更新过程不断修正机器人该时刻的位姿，得到相对可靠地机器人位置和偏航角。