首页 > 测试测量 > 技术文章 > 基于里程计/地磁/GPS的导航系统设计

基于里程计/地磁/GPS的导航系统设计

录入：edatop.com 点击：

　　摘要：导航是机器人技术中的核心问题。针对单一导航系统的不足，设计了一种基于里程计/地磁/GPS的移动机器人的组合导航系统。通过建立各导航系统的误差模型，并对误差进行分析，利用卡尔曼滤波器对导航系统的误差进行估计，得到系统误差的最优估计。最后利用间接法滤波输出对导航系统进行校正。通过MATLAB进行仿真实验，证明该导航系统能够有效地降低导航误差，提高了移动机器人的导航定位精度，而且当某一导航系统接收不到信号或者坏掉时，系统仍然可以保持高精度的导航。

　　关键词：组合导航；卡尔曼滤波；移动机器人；里程计

0引言

　　移动机器人导航技术的关键在于导航与定位的信息检测和路径规划等多种传感器信息的融合。机器人只有通过自身的传感器感知周围的环境信息和自身的位置、姿态，才能在有障碍物的环境中无碰撞地完成面向目标点的任务。目前广泛应用于移动机器人的导航与定位信息检测的传感器有里程计、陀螺仪、GPS、地磁传感器、超声波传感器等［1］。

　　里程计是一种位置传感器，可以用来测量车轮的速度信息，从而计算出机器人的位置和姿态，但是存在累计误差；GPS具有高精度、全球性、全天候等特点，但其信号容易受到干扰，常作为辅助导航系统；地磁传感器可以测出地球自生的磁场信息，从而计算出机器人与地理北的夹角，但是它易受周围磁场影响。

　　由此可见，每一种导航系统都有自身的独特性和局限性，很难独立地去完成导航任务，因此本文设计了一种以FPGA为导航计算机，基于里程计/地磁传感器/GPS的组合导航系统，实现移动机器人的导航定位，提高导航精度。

1移动机器人的导航定位

　　1.1光电编码器测量机器人位置及航向角

　　光电编码器又称光电码盘，是一种测量速度和位置的装置。假设光电码盘编码原盘上的光栅总数为N，电动机通过K倍的减速器驱动车轮，车轮的直径为D，在采样时间Ts内，光电码盘输出的脉冲增量为Q，则在单位采样时间内车轮的增量ΔS为：

　　假设左、右两个驱动轮之间的距离为w，单位采样时间内左、右两轮的位置增量分别为ΔSL和ΔSR，机器人从t－1时刻St－1=(xt－1,yt－1,θt－1)运动到t时刻的St=(xt,yt,θt)，则单位采样时间内位姿增量为：

　　光电编码器位置及测量航向角原理图如图1所示。

　　机器人在短时间内的运动可以近似看作是直线运动。直线模型具体描述如下：

　　1.2三轴地磁传感器测量机器人航向角

　　地磁传感器用来测量地球周围磁场，通过各个坐标系上磁敏感轴测出的地磁分量值，再经过计算和误差校正，可以得到载体与地磁北的夹角，再加上磁偏角即可得到载体准确的航向角［2］。

　　假设移动机器人运动在水平的路面上，这时就不用考虑Z轴上的地磁分量。三轴传感器的X敏感轴和Y敏感轴分别沿机器人的横轴和纵轴方向。假设此时磁敏感轴的X轴和Y轴测得的地磁场强度分别为α和β。由图2三轴地磁传感器测量航向角原理图，根据三角关系可求出此时的磁航角θ，由tanθ=α/β可求出θ=arctan(α/β)。

　　1.3GPS导航

　　GPS主要由GPS卫星星座、地面监控和用户接受设备三部分组成。GPS通过观测信号传播时间来计算出卫星与用户之间的距离，再通过导航电文里卫星星历、时钟改正等信息可以反推出目标位置在WGS－84坐标系下的三维坐标。设用户坐标为(Xu,Yu,Zu)，第i颗卫星的坐标为(Xsi,Ysi,Zsi)，可以求出用户到第i颗卫星的真实距离Ri为［3］：

　　在实际应用中，由于各种误差的存在，接收机无法测出用户与卫星之间的真实距离，只能测出包含各种误差在内的“伪距”。用户与第i颗卫星之间的伪距Di可以表示为：

　　Di=Ri+δI(t)+δT(t)+c(tu－tsi)(5)

　　电离层和对流层产生的误差可以通过双频传输和误差补偿模型来补偿，卫星的坐标、时钟偏差和延迟误差可以从导航电文中获得。则上述方程可写为：

　　ρi=Ri+ctu(6)

　　式中，ρi=Di－δI(t)－δT(t)；i的范围是1~4，代表不同的4颗卫星。

　　从上述原理中可以看到测量一颗卫星可以得到一个方程，上式中有四个未知数，所以至少得测量四颗卫星的距离才可以求出用户坐标。

　　由于GPS采用的是WGS－84坐标系，因此在计算时需要对坐标进行转换。坐标转换修正后的P(X1，Y1)点的平面坐标的公式［4］为：

2基于FPGA的移动机器人导航系统

　　在本文中，移动机器人作为导航系统的载体，需要承载着各种传感器和FPGA控制板、无线数据传输装置、电池等物品。移动机器人导航系统硬件结构主要可分四个部分：（1）主控单元DE2 Board开发板，负责与各个传感器进行通信、整个系统的信息处理以及对移动机器人运动的控制等；（2）系统导航单元，由GPS、地磁传感器、光电编码器和无线数据传输模块组成；（3）避障单元，由超声波测距和红外线测距共同组成，实现对障碍物的检测和避让；（4）硬件接口板单元，负责提供各种不同的电源电压以及各个传感器和DE2开发板之间的硬件接口。系统的硬件结构图如图3所示。　

3导航系统卡尔曼滤波器的设计

　　本文使用卡尔曼滤波器对系统误差进行估计，得到系统误差的最优估计，再把这一估计值从实际测得的位置和方位角中减去，从而得到机器人此时此刻的位置和方位角等参量［5］。

　　3.1里程计的误差模型

　　在平坦路面下移动机器人的运动学模型：

　　本设计中光电编码器安装在移动机器人的左右两个驱动轮上，构成了里程计导航系统，基于里程计系统的航位推算公式为：

　　里程计的误差可分为系统误差和非系统误差两种。系统误差是由移动机器人的驱动轮安装位置的误差以及左、右两轮半径不同以及两轮之间的距离测量不准确造成的。非系统误差主要是由车轮打滑造成的，但它对里程计造成的误差是随机产生的，可以当做零均值白噪声来处理［6］。考虑到系统误差因素的影响，可得出：

　　X^(k)、Y^(k)、θ^(k)为里程计观测到移动机器人在平面坐标系下的位置坐标和航向角信息； δW(k)为移动机器人左、右两轮之间测量的距离误差；δSL(k)、δSR(k)为移动机器人左、右两轮上里程计测量的误差。联立式(9)~(11)可得出里程计系统的误差模型：

　　3.2GPS和地磁传感器的误差模型

　　本文根据GPS导航系统原理，直接给出GPS的误差模型：

　　其中，φg、λg分别为GPS导航电文中的纬度、经度；φ、λ分别为系统真实的纬度、经度；δφg、δλg分别为系统的纬度、经度误差；mpx、mpy可看作为GPS观测信息中的系统白噪声。地磁传感器测出的航向角只是移动机器人与地磁北的夹角，地磁北与地理北之间存在着一定的夹角，称为磁偏角。为了获得移动机器人与地理北的航向角信息，需要加上磁偏角。由此得出地磁传感器测航向角的误差模型为：

　　θEC=θ+δθEC+εEC(14)

　　其中，θEC是地磁传感器经过处理后最终输出的航向角信息；δθEC为地磁北与地理北之间磁偏角的误差；εEC是受到周围环境中随机干扰磁场影响产生的误差，可以看作系统白噪声来处理。

　　33里程计/GPS/地磁传感器的卡尔曼滤波器设计

　　卡尔曼滤波器实际上是一种线性最小方差估计意义上的最优估计算法，本文将对移动机器人的位置误差δX(k)、δY(k)和航向角误差δθ(k)进行滤波，实现位置误差δX^、δY^以及方位角误差δθ^的最优估计。本设计采用如图4所示的方案对系统进行滤波。

　　其中，对里程计和GPS输出的位置误差信息进行滤波输出，作为导航系统的位置误差估计；对里程计和地磁传感器输出的航向角误差进行滤波输出，作为导航系统的航向角误差估计。系统的状态变量包括：里程计的位置误差和航向角误差δX(k)、δY(k)、δθ(k)，GPS信号的位置误差δφg、δλg，地磁传感器输出的航向角误差δθEC以及移动机器人测量时的结构误差δSL(k)、δSR(k)、δW(k)。系统的状态变量、状态方程以及状态矩阵分别为：

　　由于GPS和地磁传感器测量的是绝对量，它们的误差不随时间积累，综合式(11)、(13)和(14)，可得到系统的观测矩阵为：

　　C=［00000011－1］(18)

　　B为系统的误差系数矩阵，可将它设置为单位矩阵，ω(k)和ν(k)分别为系统噪声和观测噪声，它们都满足零均值白噪声的正态分布。E［ωk］=E［νk］=0，E［ωk ,ωTk］=Qk，E［νk,νTk］= Rk，Qk和Rk分别为系统噪声和观测噪声的协方差矩阵，它们互不相关。根据以上信息构建系统的卡尔曼滤波器如下：

　　（1）状态变量预测：

　　通过上式可以对移动机器人的位置误差以及航向角误差δX、δY、δθ这三个参数做出最优估计，再通过公式(11)可以计算出移动机器人的位置和航向角X、Y、θ这三个量，从而实现移动机器人精确的导航。

4仿真结果

　　为了对本设计中组合导航性能和卡尔曼滤波器的准确性进行验证，采用MATLAB对系统进行仿真。结合本文的实际情况，将移动机器人左右两轮半径的测量误差δSL(k)、δSR(k)和它们之间距离的测量误差δW(k)均设置为01 cm，导航计算机程序执行的时间周期应该与系统的采样周期相同，将它们设置为02 s，左、右两轮在一个采样周期里面行驶的距离UL、UR分别设置为597 cm和602 cm，仿真的时长设置为60 s。仿真结果如图5~图7所示。