球形机器人轨迹跟踪算法

汪 磊，战 强

（北京航空航天大学 机械工程及自动化学院，北京 100191）

0 引言

与传统的轮式机器人相比，球形机器人有其固有的优势：滚动中的球自然会选择阻力最小的路径，因此球形可以令机器人在不同障碍或地形中运动［1］。在过去的几十年里提出过的一些典型球形机器人模型存在很多阻碍球形机器人大规模投入实用的问题，其中最主要的就是运动控制的复杂性，目前仍然没有有效的球形机器人控制方法。本文对BHQ－1球形机器人的轨迹跟踪问题［2］进行了探讨，着眼于结构如BHQ－1的球形机器人轨迹跟踪控制的解决方法［3］。

1 球形机器人模型

1．1 物理系统描述

球形机器人BHQ－1的机械结构如图1所示，它由电机1和2、空心轴3、重块4、摄像头5和一副球壳6组成。两个电机的转动可使机器人重心改变然后产生一个重力转矩驱动机器人运动，因此可以通过控制两个电机来控制球形机器人的运动。

图1 BHQ－1球形机器人结构

1．2 运动学模型

当球形移动机器人在平面上运动时，球壳的运动模型可简化为如图2所示。其中ΣO｛X，Y，Z｝为惯性坐标系与地面固联，Σb｛Xb，Yb，Zb｝为球形移动机器人的载体坐标系，其中Ob与机器人球壳的中心固联，球壳的半径为R，球壳与地面的接触点Pc在惯性坐标下的坐标为（xc，yc），φ、β、ψ分别为绕 Xb、Yb、Zb旋转的角度，采用ZYX 欧拉角表示为（ψ，β，φ），τφ、τβ分别为绕Xb、Yb旋转的剪切应力。球形机器人的运动学方程可写成：

图2 球形机器人运动学模型示意图

可见球形机器人系统运动学方程是一个2输入5输出的一阶非线性方程，并且速度项不可积分，针对轮式机器人提出的非完整性约束系统控制方法无法直接应用于球形移动机器人。

2 轨迹跟踪算法设计

2．1 动态轨迹跟踪

机器人的轨迹跟踪与路径跟随不同，轨迹跟踪的理想轨迹是一条与时间呈一定关系的几何曲线f（xr（t），yr（t），θr（t））。

机器人轨迹跟踪的误差模型如图3所示，机器人的位姿坐标为（x，y，θ），跟随目标的位姿坐标为（xr，yr，θr），得到的误差模型可写成：

其中：de为目标参考点位置与机器人位置的绝对距离差；θe为目标参考点至机器人连线与此时航向角的差。

图3 轨迹跟踪误差模型示意图

位置跟踪控制器的期望输入位移差为0，当目标点与机器人的位移差增大，则PID控制输出足够大的前进速度指令用于追赶目标点以缩小位移差。设ud为位置跟踪控制器的输出值：

其中：kp、kd、ki分别为比例、微分和积分系数。

航向跟踪控制器与位置跟踪控制器类似，通过航向误差信号θe，由控制器输出角速度控制指令用于修正机器人的航向角，从而使机器人与目标点航向保持一致。设uθ为航向跟踪控制器的输出值：针对不同的输入变量在同一个参数的作用下并不能输出最优的控制策略，尤其针对不同轨迹的跟踪将表现出不同的控制性能。普通的PID控制器并不能有效地抑制外界扰动，当扰动过大甚至可能造成控制失效。为此对上述提出的轨迹跟踪算法提出如下的改进方式［4］。

2．2 模糊自适应控制方法

为了提高算法的自适应性及鲁棒性，采用模糊数学方法将规则条件、控制策略用模糊集合表示，并将这些模糊规则及策略存储在嵌入式控制系统中，同时控制器根据外界实际的输入变量运用模糊推理在线对PID的参数进行自适应整定，根据不同轨迹的输入变量自适应调节PID参数。自适应模糊PID控制器以误差e和误差的变化率ec作为输入，可以实现在线根据不同e和ec情况对PID参数自整定［5］。

对于模糊控制器，误差信号与微分信号的输入效果十分重要。为了更好地获得一个信号的最佳逼近同时获取该信号的微分，本课题采用非线性跟踪微分器（NTD）来实现［6］。跟踪微分器的动态系统可描述为［7］：

由于PID控制器是一个参数敏感的控制方式，其

其中：r为跟踪速度因子，决定了跟踪效果的快慢，当r值较大时跟踪速度越快；T 为采样周期；fst［x1（k），x2（k），v（k），r，T］为用于跟踪信号v（k）的二阶导数，其定义如下：

其中：v（t）为输入信号；输出信号x1为v（t）的跟踪信号；x2为v（t）的微分信号。

为了更好地实现嵌入式系统，将其进行离散化处理，得到：

其中：δ为跟踪信号v（k）的变化量；

y（k）＝x1（k）－v（k）＋Tx2（k）。

根据各参数修正的模糊规则进行在线推理，得到了相应的模糊输出子集，由于被控对象只能接收一个精确的控制量，无法接收这种模糊控制量，因此必须经过清晰化处理将其转换为精确量，本设计采用加权平均法进行去模糊处理。处理后的控制变量乘以一个量化因子α后作用在相应的PID参数上，即模糊自适应PID控制器的参数可表示为：

其中：Kp、Ki、Kd分别为去模糊处理后的修正比例、积分、微分系数；Kp0、Ki0、Kd0分别为上述系数的初始值。

3 仿真研究

为了验证设计算法的可行性，以BHQ－1球形机器人为模型对象，针对式（1）球形机器人运动学模型，对直线轨迹下的轨迹跟踪问题进行了仿真，球形机器人原始位姿为（x＝0，y＝0，ψ＝－π／2，β＝0，φ＝0）。目标跟踪轨迹为x＝0．2t，y＝0，即目标点跟踪速度为0．2m／s。普通和变参数PID控制器的仿真曲线分别如图4、图5所示。

图4 普通PID控制器的直线轨迹跟踪仿真曲线

采用了模糊自适应PID控制器后，机器人能够迅速地跟踪上目标路径，调节时间小于1s，并且系统超调微小。

4 结论

虽然传统的动态轨迹跟踪算法对于非完整移动机器人具有良好的跟踪性能，但仿真结果表明，这并不适合2自由度摆球形机器人。采用经典PID控制策略虽然能够有效地跟踪上目标曲线，但是由于各个PID参数固定，针对不同路径的跟踪表现出的性能不尽相同。而模糊自适应PID控制器能够根据机器人与目标位置的误差信号实时调整PID参数，使机器人的位姿得到最优的调整。

图5 变参数PID控制器的直线轨迹跟踪仿真曲线

［1］ Armour Ｒ Ｈ，Vincent Ｊ Ｆ Ｖ．Rolling in nature and robotics：a review［J］．Journal of Bionic Engineering，2006，3：195－208．

［2］ Zhan Q，Liu Z，Cai Y．A back－stepping based trajectory tracking controller for a non－chained nonholonomic spherical robot［J］．Chinese Journal of Aeronautics，2008，21：472－480．

［3］ Chen M，Zhan Q，Liu Z．Optimal trajectory planning based on Hamiltonian function of a spherical mobile robot［J］．Chinese High Technology Letters，2008，14：289－293．

［4］ 曹承志．微型计算机控制新技术［M］．北京：机械工业出版社，2000．

［5］ 孙增圻，邓志东，张再兴．智能控制理论与技术［M］．北京：清华大学出版社；南宁：广西科学技术出版社，2006．

［6］ 韩京清，王伟．非线性跟踪—微分器［J］．系统科学与数学，1994，14（2）：177－183．

［7］ 解永清，郑恩让．带有微分－跟踪器的无模型自适应控制研究［J］．电气自动化，2009，31（2）：16－19．