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

孙 华，李 伟，吴亚明

(哈尔滨工程大学 自动化学院，黑龙江 哈尔滨 150001)

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

孙 华，李 伟，吴亚明

(哈尔滨工程大学 自动化学院，黑龙江 哈尔滨 150001)

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

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

0 引言

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

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

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

1 移动机器人的导航定位

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

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

(1)

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

(2)

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

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

(3)

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

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

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

图2 三轴地磁传感器测量航向角原理图

1.3 GPS导航

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

(4)

在实际应用中，由于各种误差的存在，接收机无法测出用户与卫星之间的真实距离，只能测出包含各种误差在内的“伪距”。用户与第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]为：

(7)

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

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

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

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

3.1 里程计的误差模型

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

(8)

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

(9)

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

(10)

(11)

(12)

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

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

(13)

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

θEC=θ+δθEC+εEC

(14)

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

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

图4 移动机器人导航系统滤波方案

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

(15)

(16)

(17)

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

(18)

(1)状态变量预测：

(19)

(2)协方差阵预测：

(20)

(3)卡尔曼滤波增益：

Kk+1=Pk+1/kCT[CPk+1/kCT+R]-1

(21)

(4)状态更新：

(22)

(5)协方差矩阵更新：

(23)

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

4 仿真结果

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

5 结论

图5 东向位置误差估计

图6 北向位置误差估计

图7 航向角误差估计

本文设计的基于里程计/地磁/GPS的组合导航系统，结合卡尔曼滤波对系统的位置误差和航向角误差进行估计。通过MATLAB仿真验证，该系统能够有效地减小移动机器人导航系统的误差，提高系统导航精度；并且在GPS信号丢失时，在一定的时间内同样可以实现高精度的导航功能，该系统的各方面性能都优于单一导航系统。

[1] 李磊,叶涛,谭民.移动机器人技术研究现状与来[J].机器人,2002,24(5):475-480.

[2] 徐德,邹伟.室内移动式服务机器人的感知、定位与控制[M].北京：科学出版社，2008.

[3] 于金霞,蔡自兴,邹小兵,等.非平坦地形下移动机器人航迹推算系统设计[J].河南理工大学学报,2005,24(3):210-216.

[4] 徐卫明,赵俊生. GPS测量坐标转换实用性问题的分析[J]. 测绘工程， 2000,9(2): 10-15.

[5] 张国良,曾静.组合导航原理与技术[M].西安:西安交通大学出版社,2008.

[6] 常青,郑平方,柳重堪,等.车载GPS/DR组合导航系统数据融合算法研究[J].通信学报，2000,21(2): 42-47.

Navigation system design based on odometer/geomagnetic /GPS

Sun Hua, Li Wei, Wu Yaming

(College of Automation,Harbin Engineering University,Harbin 150001,China)

Navigation is the core problem in robotics. Aiming at the disadvantages of single navigation system, a mobile robot’s combined navigation system based on odometer/geomagnetic/GPS is designed. Through establishing the error model of the navigation systems and analyzing the error, using the Calman filter to estimate the error of the navigation system, the system’s optimal estimation is cbtained. Finally, the navigation system is corrected by the indirect method. The simulation experiment is carried out by MATLAB. The results show that the navigation system can effectively reduce the navigation error and improve the mobile robot’s navigation and positioning precision. And when a navigation system can’t receive the signal or bad, the system can still maintain high precision navigation.

combined navigation; Calman filter; mobile robot; odometer

TN967.1

A

1674-7720(2016)01-0081-04

孙华，李伟，吴亚明．基于里程计/地磁/GPS的导航系统设计[J] .微型机与应用，2016，35(1)：81-84，87.

2015-09-13)

孙华(1963-)，女，博士，教授，硕士研究生导师，主要研究方向：机器人导航定位、路径规划以及智能控制。

李伟(1991-)，通讯作者，男，在读硕士研究生，主要研究方向：机器人导航与路径规划。E-mail：179994303@qq.com。

吴亚明(1991-)，男，在读硕士研究生，主要研究方向：控制工程与科学。