然而单纯 的惯性导 航有个 缺点 , 就 是随时 间增 长 , 误差
不断积累 。 那么肯定对于长时间 无须停机 校正的系统 而言 ,
单独依靠惯性 导航系统是不够的 , 所以 作为辅助系 统―磁感
应器修正子系 统就被 应用 进来 。 该 系统的 作用 就是 每隔一
定的时间 , 或按照一定的规律对惯性导航系统进行修正 。
ABSTRACT:T his paper presen ts an integrated naviga tion m e thod o f whee led mobile robo t which is driven by front whee .l The inertia l naviga tion, gy ro scope, m agnet senso r and m ileme ter are used to m easure naviga tion info rm ation. Gy ro scope and m ilem e ter a re am ended by m agnet sensor in fo rm ation every o the r defin ite d istance. Itm akesm obile robo t run accura te ly and stably fo r a long time. A ccum ula ted e rro r of the sing le ine rtia l nav ig ation w ith tim e is e lim inated. M oreove r, the ab ility o f an ti - jamm ing to outside env ironm en t is strong. Expe rim en t re su lts prove the m e thod is feasible and effec tive. It can be used on line and comb ined w ith o ther naviga tion senso rs info rma tion w ith strong expandab ility. KEYWO RDS:M ob ile robo t;Integ ra ted nav ig ation;G yroscope;D ata fusion
表示 。
— 85 —
α = sin-1 (d ω/v)
·xg = v cosαcosθ
(1)
y·g = v cosαsinθ
θ· =ω
式 (1) 中 , d 代表移动机器 人的 前轮到 后轮 轴之间 的距
离 。θ为车体的航向 , (xg , yg ) 为参考点的坐标 。 对式 (1) 离散化可得式 (2)。 θ(k +1) =θ(k) +Ts ω(k)
状态估计 :
X^k +1 /k =Υk +1, k X^k
(5)
X^k +1 =X^k +1 /k +K k +1(Z k +1 - Hk +1X^k +1 /k ) 滤波增益 :
(6)
K k +1
=P k +1
HT
/k k +1
(H k +1P k +1
HT
/k k +1
+R k +1)- 1
或 K k +1
第 22卷 第 7期 文章编号 :1006 - 9348(2005)07 -0085 - 03
计 算 机 仿 真
2005年 7月
轮式移动机器人组合导航方法及试验研究
沈猛 , 徐德民 , 李俊 , 辛廷慧
(西北工业大学航海学院 , 陕西 西安 710072)
摘要 :该文提出了以惯性导航为基础 , 磁感应器修正的移动机器人组合导航方法 。 该方法以陀螺仪 、磁感应器和里 程计作为 导航信息的检测器件 , 每隔一定的距离 , 利用磁感应器检测到的信息对陀螺仪和里程计进行修正 , 使得移动机器人 能够精确 定位 、长时间稳定运行 。 一方面 , 消除了纯惯性 导航随时 间增长累 积的误差 ;另一方面 , 对 外界环境 有较强的 抗干扰 能力 。 试验结果验证了该组合导航方法是有效 、可行的 , 适于在线实时应用, 能融合其它导航传感器信息 , 具有较强可扩展性 。 关键词 :移动机器人 ;组合导航 ;陀螺仪 ;数据融合 中图分类号 :TP24 文献标识码 :B
或 P k +1
=(I
-
Kk +1H k +1)P k +1 /k
(9=
P
-1 k +1
/k
+
HTk +1
R
-1 k +1
Hk
+1(9″)
将以上的 式 (5) ～ (9), 按 (5)(8)(7)(9)(6)(7) 的顺
序就时间周期 循环就可以了 。
对于 X^0 初值 , 可以在试验 时设定 ;P 0 可以设 的大一 点 , 比如 P0 = 105 I。
α(k +1) = sin- 1[ d ω(k) /v(k)]
(2)
xg (k +1) =xg(k) +T s v(k) co sα(k)co sθ(k)
yg (k +1) =yg(k) +T s v(k) co sα(k) sinθ(k) 但 是对式 (2)中的 v(k)与 ω(k)的实际测量存在一定的
1 引言
移动机器人是现代物流系 统中的关键设 备之一 , 它以电 池为动力 , 进行非接 触导航 , 根 据实 际需要 可配 备不同 的移 载机构 , 完成相应的操作任务 。 移动机 器人常用的 导航方法 有电磁导航 、直接坐标 导航 、惯 性导航 、超 声波 导航 [ 1] 、激光 导航和图像识 别导航 [ 2] 、地图 匹配导 航 [ 3, 4] 等 [ 5, 6, 8, 7, 8, 9, 10, 11] , 这些导航各有优 缺点 。 单一的 导航 方法很 难满 足越来 越高 的导航性能要求 , 所以一个有效的途径 就是采取组 合导航技 术 [ 12] 。 文献 [ 13] 提出了 用 G PS和里 程计的 车辆 组合导 航 , 即使 G PS信号丢失 150秒 , 在该 时间段 里 , 系统 依靠里 程计 仍能有效导航 , 而此时的导航精度不 高 。 本文提出 了一种基 于以陀螺仪和里程计为基本检 测器件 , 磁感应器修 正的组合 导航 , 所设计的组合导航精度 经过试 验验证 精度可 达到 2 ～ 3 cm 。
— 86 —
系统的状 态方程可以表示为 :
X k +1 =Υk +1, k X k +ΓW k 式 (3) 中移动机器人的状态为
(3)
X = [ x y Vgx Vgy] T , 状 态 转 移 矩 阵 Υ =
1 0 Ts 0
00
0 1 0 T s , 系统噪声驱动矩阵 Γ = 0 0 , 系 统噪声
存在一定的近似过程 , 所以 还可以 利用 实验来 获得 。现假设
D(Vgx) =Rgx;D (Vgy) =Rgy 。除此之外还有 系统噪声 (Q)的 求解 , 在本文中系统噪 声取 的是移 动机 器人的 加速 度 , 可以
通过对动力系 统的分析得到 。
基于上述 讨论 , 采用卡尔曼滤波器 :
状态一步 预测 :
收稿日期 :2004 - 03 - 18
2 移动 机器 人运 动学 模型
在本 文 中 研究 前 轮 驱
动并 转向 的三 轮 移动 机 器 人 , 其模型如图 1所示 。
对于 本 导 航系 统 的 移 动机器人来说 , 可以控 制的 量有两个 :一个是能够 精确 控制的量 , 即驱动轮与 车体
正方向的车 轮偏角 α, 另 外
=P
k
+1
H
T k +1
R -1 K +1
一步预测 均方误差 :
(7)
P k +1 /k
=Υk
+1,
k
P
k
ΥT k +1,
k
+ΓkQk ΓTk
估计均方 误差 :
(8)
P k +1 =(I - K k +1H k +1 )P k +1 /k ( I - Kk +1Hk +1)T +K k +1R k +1KTk +1 (9)
图 2 导航系统示意图
4 导航算法设计
导航算法的设计 以惯性导航为基础的组 合导航中 , 应用 卡尔曼滤波 , 将所获得 的信息 对运 动状 态做出 最佳 估计 , 使 系统性能更加可靠 。
量测量可以看作 卡尔曼滤波器的输入 , 估计值 可以看作 输出 。在本文中 , 驱动轮的速率 v 和转动角速率 ω作为 输入 , 而移动机器人的运动 状态 (包括速度和位置 ) 作为输出 。
R esearch on theM ethod and E xper mi ent of the Integrated Nav igation forW heeled M ob ile Robot
SHEN M eng, XU De - m in, L I Jun, X IN T ing - hui
(Co lleg e ofM arine, N or thw estern P o ly techn ica l Un ive rsity, X i'A n Shanx i 710072, China)
磁感应器能够测 量出离它一定距离的 磁钉的相对 位置 , 但测量 距离很有 限 ;目 前所利用的 M S20 磁感应器 能够测量 的范围是一个 8cm ×6cm 的一个矩形区域 。 只有当磁钉位于 这个区域时 它 才可 以测 量 出磁 钉与 磁 感应 器的 相 对位 置 。 利用这一点 , 我们可 以事先 将磁 钉安装 在固 定的地 方 , 并事 先确定出它 的位 置 , 当 装有 磁 感应 器的 移 动机 器人 靠 近它 时 , 便可以利用磁钉 的位置 计算 出移 动机器 人的 准确位 置 。 而且 , 还可以利用此 信息对 导航 参数进 行校 正 , 其中包 括对 陀螺漂移 , 以及由环境因素引起的导航偏差 。