轮式移动机器人航向跟踪预估控制算法

轮式移动机器人航向跟踪预估控制算法

龚建伟 黄文宇 陆际联

(北京理工大学机器人中心，北京 100081)

摘 要：本文提出了一种轮式移动机器人航向跟踪预估控制算法，航向预估量根据机器人前轮偏角和纵向速度实时得出，预估量与机器人实际航向之和作为控制反馈航向.仿真和实验时用PID 控制器和航向预估算法结合进行航向跟踪，结果表明该算法与常规PID 算法相比，对机器人纵向速度适应范围较宽，能有效地改善控制器的动态特性，表现出了较好的自适应能力.

关键词：轮式移动机器人；自主车；侧向控制；航向跟踪；预估控制

1 引言 Introduction

轮式移动机器人是一个具有大延迟、高度非线性的复杂系统，建立精确的数学模型十分困难，在进行航向跟踪控制时，参数的变化对系统模型影响较大，其中纵向速度的变化影响最为明显.轮式移动机器人航向跟踪一般控制方法是把期望航向与机器人实测航向之差作为控制器输入偏差，控制器输出控制量为机器人的前轮偏角.轮式移动机器人的航向与其纵向速度、横向速度、前轮偏角、机器人绕其重心的转动惯量、重心位置、前后轮侧偏系数以及实际道路情况等诸多因素有关，在常规控制方法中，只考虑了期望航向与实际航向的偏差，而未能包含其它因素的影响，因此难以达到满意的控制效果，当系统参数特别是某些敏感参数发生变化时，就必须重新设定控制器参数.例如，我们用常规PID 控制器进行航向跟踪实验，在某一纵向速度下整定好PID 控制参数，当纵向速度发生很小变化时，必须重新整定PID 参数，否则控制性能变坏，超调较大，甚至出现振荡.表现在路径跟踪实验中，则是在该速度下能较好地完成弯道或急弯等路径跟踪任务，而速度变化后，跟踪误差变大或出现大幅度振荡.因此，在轮式机器人航向跟踪控制中，控制方法应该能对纵向速度等影响因素有一定的自适应能力，航向跟踪预估控制方法就是在这一背景下提出的.

2 二自由度轮式移动机器人动力学模型 T wo Degrees of Freedom Dynamic Model for Wheeled Mobile Robot

当横向加速度和横摆角速度较小时，常采用经简化的二自由度轮式移动机器人动力学模型1,2，其微分方程如下：

r f r f r f zz C aC v U bC aC r U C b C a r I 2)

(2)(222=-+++ （1）

f f r f r f s s C v U C C r U bC aC U M v M δ2)(2)(2=++⎥⎦⎤⎢⎣⎡-++ （2）

其中：I zz ：轮式机器人绕重心的转动惯量(kgm 2)； M s ：轮式机器人质量(kg)；C f 、C r ：分别为前、后轮侧偏系数(N/rad)；U ：轮式机器人纵向速度(m/s)；v ：轮式机器人横向速度(m/s)；a 、b ：前后车轴到重心的距离(m)；r ：轮式移动机器人横摆角速度.

将轮式移动机器人转向机构视为一个惯性环节，则有：

d f f δδδτ=+ (3)

其中：τ：惯性时间常数；δd ：期望前轮偏角.

在航向跟踪控制过程中，可以令道路曲率为零，即不计实际路径的影响，航向变化率即为机器人的横摆角速度3，即：

r =ϕ

(4) 其中：ϕ为轮式移动机器人航向与期望航向的偏差角.

(1)、(2)、(3)、(4)式联立即可得出以横摆角速度、横向速度、航向偏差角、前轮偏角即X=（r ,v ,ϕ,δf ）为状态变量的系统状态空间表达式.

3 航向预估算法原理 Heading Prediction Algorithm

在实际的航向控制过程中，控制器根据期望航向与采样航向得到航向偏差，再计算控制量，而当执行机构执行这一控制量时，要经过一个采样周期，这时机器人的实际航向已经改

变，即控制量执行时已有一个采样周期的滞后，而且，采样周期一定时，机器人纵向速度或其它影响因素不同，航向的变化量也不一样.

航向预估算法的基本思想是预测机器人航向变化趋势，并将其计入控制偏差，这样航向变化趋势就可以影响控制器的输出，即轮式移动机器人的前轮偏角.本文介绍的是一种在简化模型基础上提出的预估算法，预估模型原理如图1所示，图1中：R 为机器人绕运动中心点O 运动半径(m)，δf 为前轮偏角(rad)，B 为轴距(m).

由式(4)可知，只要求出轮式机器人的横摆角速度，就能得到航向偏差角变化率，航向偏差角变化率与采样周期的乘积即机器人在一个采样周期内的航向变化量.若设控制器采样周期为T c (s)，纵向速度以前轮速度近似，则轮式移动机器人航向在一个控制周期的变化量h ∆(rad)可以近似地计算为：

R UT h c /=∆ (5)

其中: f B R δsin /= (6)

∙ ∙ 称为轮式移动机器人在一个控制周期内的航向变化预估量，以下简称航向预估量.在控制算法中，把轮式移动机器人当前航向与航向预估量之和作为航向反馈量，期望航向与航向反馈量的差值作为控制器的输入偏差，航向预估控制框图如图2所示. 4 PID 控制算法 PID Control Algorithm

在仿真和实验中，航向预估控制方法和与之对比的常规控制方法的控制器均采用增量PID 算法，且两者的比例、积分和微分系数一样.增量PID 算法如式(7)所示：

)1()(--=∆k u k u u

)]

2()1(2)([)()]1()([-+--++--=k e k e k e K k e K k e k e K d i p (7)

其中：K p 、K i 、K d 分别为比例、积分和微分放大系数，u (k )表示第k 个采样时刻的控制量,e (k )表示第k 个采样时刻的航向输入偏差.

图1 二自由度轮式移动机器人航向预估模型 图2 航向跟踪预估控制框图

Fig.1 Two degrees of freedom model f or heading predi ction Fig.2 Heading prediction control 5 仿真结果 Simulation Results

仿真实验在MATLAB/SIMULINK 图形仿真环境下进行.将初始航向偏差设置为20º(0.35rad)，进行航向跟踪阶跃响应实验.与实际系统一致，轮式机器人前轮偏角最大设为35º，因此控制器最大输出绝对位置控制量为0.611rad,控制器采样周期为0.064s,每一控制周期输出增量最大为0.0224rad.仿真时系统参数选取如下：I zz ：8890kg .m 2；M s ：3000kg ；C f 、C r ：分别为48000N/rad 和42000N/rad ；a 、b ：分别为1.56m 和2.0m ，τ：0.5s .仿真时取2组纵向速度：4m/s 和6m/s ，2组纵向速度下的PID 控制系数相同，比例、积分和微分系数分别取为0.8、0.025和1.0.纵向速度为4m/s 有、无航向预估量的航向阶跃响应曲线如图3和图4所示，纵向速度为6m/s 有、无航向预估量航向阶跃响应曲线如图5和图6所示.

图3 速度4m/s 时有预估量航向阶跃响应曲线 图4 速度4m/s 时无预估量航向阶跃响应曲线

Fig.3 Step response of prediction method (u=4m/s) Fig.4 Step response of general PID (u=4m/s)