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

轮式移动机器人航向跟踪预估控制算法
龚建伟 黄文宇 陆际联
(北京理工大学机器人中心，北京 100081)
摘 要：本文提出了一种轮式移动机器人航向跟踪预估控制算法，航向预估量根据机器人前轮偏角和纵向速度实时得出，预估量与机器人实际航向之和作为控制反馈航向.仿真和实验时用PID控制器和航向预估算法结合进行航向跟踪，结果表明该算法与常规PID算法相比，对机器人纵向速度适应范围较宽，能有效地改善控制器的动态特性，表现出了较好的自适应能力.
关键词：轮式移动机器人；自主车；侧向控制；航向跟踪；预估控制
1 引言 Introduction
轮式移动机器人是一个具有大延迟、高度非线性的复杂系统，建立精确的数学模型十分困难，在进行航向跟踪控制时，参数的变化对系统模型影响较大，其中纵向速度的变化影响最为明显.轮式移动机器人航向跟踪一般控制方法是把期望航向与机器人实测航向之差作为控制器输入偏差，控制器输出控制量为机器人的前轮偏角.轮式移动机器人的航向与其纵向速度、横向速度、前轮偏角、机器人绕其重心的转动惯量、重心位置、前后轮侧偏系数以及实际道路情况等诸多因素有关，在常规控制方法中，只考虑了期望航向与实际航向的偏差，而未能包含其它因素的影响，因此难以达到满意的控制效果，当系统参数特别是某些敏感参数发生变化时，就必须重新设定控制器参数.例如，我们用常规PID控制器进行航向跟踪实验，在某一纵向速度下整定好PID控制参数，当纵向速度发生很小变化时，必须重新整定PID参数，否则控制性能变坏，超调较大，甚至出现振荡.表现在路径跟踪实验中，则是在该速度下能较好地完成弯道或急弯等路径跟踪任务，而速度变化后，跟踪误差变大或出现大幅度振荡.因此，在轮式机器人航向跟踪控制中，控制方法应该能对纵向速度等影响因素有一定的自适应能力，航向跟踪预估控制方法就是在这一背景下提出的.
2 二自由度轮式移动机器人动力学模型 Two Degrees of Freedom Dynamic Model for Wheeled Mobile Robot
当横向加速度和横摆角速度较小时，常采用经简化的二自由度轮式移动机器人动力学模型1,2，其微分方程如下：
（1）
（2）
其中：Izz：轮式机器人绕重心的转动惯量(kgm2)； Ms：轮式机器人质量(kg)；Cf、Cr：分别为前、后轮侧偏系数(N/rad)；U：轮式机器人纵向速度(m/s)；v：轮式机器人横向速度(m/s)；a、b：前后车轴到重心的距离(m)；r：轮式移动机器人横摆角速度.
将轮式移动机器人转向机构视为一个惯性环节，则有：
(3)
其中：τ：惯性时间常数；δd：期望前轮偏角.
在航向跟踪控制过程中，可以令道路曲率为零，即不计实际路径的影响，航向变化率即为机器人的横摆角速度3，即：
(4)
其中：为轮式移动机器人航向与期望航向的偏差角.
(1)、(2)、(3)、(4)式联立即可得出以横摆角速度、横向速度、航向偏差角、前轮偏角即X=（r,v,,δf）为状态变量的系统状态空间表达式.
3 航向预估算法原理 Heading Prediction Algorithm
在实际的航向控制过程中，控制器根据期望航向与采样航向得到航向偏差，再计算控制量，而当执行机构执行这一控制量时，要经过一个采样周期，这时机器人的实际航向已经改变，即控制量执行时已有一个采样周期的滞后，而且，采样周期一定时，机器人纵向速度或其它影响因素不同，航向的变化量也不一样.
航向预估算法的基本思想是预测机器人航向变化趋势，并将其计入控制偏差，这样航向变化趋势就可以影响控制器的输出，即轮式移动机器人的前轮偏角.本文介绍的是一种在简化模型基础上提出的预估算法，预估模型原理如图1所示，图1中：R为机器人绕运动中心点O运动半径(m)，δf为前轮偏角(rad)，B为轴距(m).
由式(4)可知，只要求出轮式机器人的横摆角速度，就能得到航向偏差角变化率，航向偏差角变化率与采样周期的乘积即机器人在一个采样周期内的航向变化量.若设控制器采样周期为Tc (s)，纵向速度以前轮速度近似，则轮式移动机器人航向在一个控制周期的变化量(rad)可以近似地计算为：
(5)
其中: (6)
· ·             称为轮式移动机器人在一个控制周期内的航向变化预估量，以下简称航向预估量.在控制算法中，把轮式移动机器人当前航向与航向预估量之和作为航向反馈量，期望航向与航向反馈量的差值作为控制器的输入偏差，航向预估控制框图如图2所示.
4 PID控制算法 PID Control Algorithm
在仿真和实验中，航向预估控制方法和与之对比的常规控制方法的控制器均采用增量PID算法，且两者的比例、积分和微分系数一样.增量PID算法如式(7)所示：

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

图1 二自由度轮式移动机器人航向预估模型 图2 航向跟踪预估控制框图
Fig.1 Two degrees of freedom model for heading prediction Fig.2 Heading prediction control
5 仿真结果 Simulation Results
仿真实验在MATLAB/SIMULINK图形仿真环境下进行.将初始航向偏差设置为20º(0.35rad)，进行航向跟踪阶跃响应实验.与实际系统一致，轮式机器人前轮偏角最大设为35º，因此控制器最大输出绝对位置控制量为0.611rad,控制器采样周期为0.064s,每一控制周期输出增量最大为0.0224rad.仿真时系统参数选取如下：Izz：8890kg.m2；Ms：3000kg；Cf、Cr：分别为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)

图5 速度6m/s时有预估量航向阶跃响应曲线 图6 速度6m/s时无预估量航向阶跃响应曲线
Fig.5 Step response of prediction method (u=6m/s) Fig.6 Step response of general PID (u=6m/s)
从仿真结果可以看出，纵向速度为4m/s时有、无航向预估量时轮式移动机器人航向阶跃响应曲线基本相同，但当速度变为6m/s时，航向预估控制方法的阶跃响应曲线较为理想，而用常规控制方法则出现了较大的超调，纵向速度更高时，两种方法的差别更明显.仿真结果说明有航向预估的PID控制器适应的速度范围较宽，控制系统的鲁棒性比常规控制方法强.
6 实验结果 Experimental Results
实验平台为一辆奔驰厢式货车改装而成的轮式移动机器人，方向盘、油门踏板和制动踏板均由交流伺服电机通过减速器用钢丝绳牵引，机器人航向由惯性导航系统给出，航向精度为0.1º，纵向速度由速度控制系统进行控制，实验时能稳定在±0.2m/s以内,与仿真一样采用2组纵向速度进行：4m/s和6m/s.实验在平坦开阔的场地进行，实验时所用PID参数与仿真实验相同.
实验一 航向阶跃实验 Exp.1 Heading step reponse
开始由轮式移动机器人跟踪一给定航向，达到期望速度且航向稳定后，给出20º（0.35rad）的阶跃偏差.纵向速度为4m/s时，有、无航向预估量的航向阶跃响应曲线如图7和图8所示，纵向速度为6m/s有、无航向预估量的阶跃响应曲线如图9和图10所示.
实验结果与仿真结果基本一致，纵向速度为4m/s时，两种方法的阶跃响应曲线基本相同，但预估控制响应典线较为平滑，纵向速度为6m/s时，预估控制方法控制效果与速度为4m/s时差别不大，但常规控制方法阶跃响应典线则出现了明显的振荡.同时，从图7和图8的直线跟踪阶段曲线（即阶跃响应之前的曲线）可以看出，纵向速度为4m/s时，航向预估控制方法的跟踪典线较为平滑，速度为6m/s时也有同样的效果.

图7速度为4m/s时有预估量航向阶跃响应曲线 图8 速度4m/s时无预估量航向阶跃响应曲线
Fig.7 Step response of prediction method (u=4m/s) Fig.8 Step response of general PID (u=4m/s)

图9 速度6m/s时有预估量航向阶跃响应曲线 图10 速度6m/s时无预估量航向阶跃响应曲线
Fig.9 Step response of prediction method (u=6m/s) Fig.10 Step response of general PID (u=6m/s)
实验二 航向连续跟踪实验 Exp.2 Heading continually following
实验过程中，每5个控制周期(5X0.064s)期望航向减少1º，减少20º后期望航向保持不变.设计本实验的目的是模拟弯道跟踪.实验时纵向速度保持在6m/s,有、无预估量的航向响应曲线分别如图11、图12所示.

图11 纵向速度6m/s时有预估量航向连续跟踪响应曲线 图12 纵向速度6m/s时无预估量航向连续跟踪响应曲线
Fig.11 Step response of prediction method (u=6m/s) Fig.12 Step response of general PID (u=6m/s)
从图中可以看出，航向预估控制方法的响应曲线较为平滑，且无超调，能很好地完成连续跟踪任务，而常规控制方法则出现了大幅振荡，且曲线不平滑.
7 结论 Conclusion
本文提出一种轮式移动机器人航向跟踪预估控制算法，根据机器人前轮偏角和纵向速度来计算航向的变化量作为航向预估量，并将航向预估量与机器人实际航向之和作为控制的反馈航向，航向预估量对机器人航向变化趋势作了预测.由于在航向预估量中考虑了机器人纵向速度和实时前轮偏角的影响，因而控制器对机器人纵向速度变化的适应范围较宽.算法简单实用，实时性非常好.仿真实验和实车实验中，用航向预估算法和PID控制算法结合进行轮式移动机器人的航向跟踪实验，结果表明这种算法有效地改善了控制性能，适应的速度范围较常规控制方法要宽，对系统参数变化的敏感性较低，减少了控制器对移动机器人模型的依赖，提高了控制器的鲁棒性.