脚轮式全向移动平台的运动控制设计与仿真

张天宇， 彭忆强,2,3， 黄晓蓉,3， 孙树磊,2,3， 何灼馀,3

(1. 西华大学汽车与交通学院，四川 成都 610039; 2. 汽车测控与安全四川省重点实验室，四川 成都 610039;3. 四川省新能源汽车智能控制与仿真测试技术工程研究中心，四川 成都 610039)

0 引 言

脚轮式全向移动平台在物流搬运、室内服务和外星探测等领域有着广泛的应用，具有重要的研究价值[1]。脚轮可分为双电机脚轮和单电机脚轮。双电机脚轮中的两个电机分别用于控制脚轮的转速和转向角，该结构在移动过程中会产生耦合运动，导致全向移动平台属于冗余系统、控制策略设计复杂且其运动精度低。电机脚轮具有机械结构简单、性价比高以及其全驱动特性可有效降低控制策略设计难度等优点；因此，本文的研究对象为装配有3个独立驱动的单电机脚轮式全向移动平台，主要分析其运动学模型、动力学模型以及控制策略。

运动学模型主要用于获得跟踪或到达目标的稳定运动控制规律，国内外学者采用几何方法、D-H坐标变换法和轮心建模法等对轮式全向移动平台运动学模型进行了大量研究。针对麦克纳姆轮全向移动平台，曾志雄[2]等采用几何方法得到了运动学模型，并分析了其在狭窄过道的运动特性；叶长龙[3]等采用轮心建模法，建立具有偏心轮机构的运动学模型，以获得最佳的结构设计参数，并对其运动轨迹误差进行了分析。针对脚轮式全向移动平台，Bruno[4]等首次用D-H坐标变换法，建立了单个脚轮的运动学模型；Ahmed[5]等采用矩阵求逆、偏导计算、雅可比矩阵等方式，建立了整体运动学模型。然而上述方法对于复杂多刚体平台建模过程繁琐，同时存在难以准确建立坐标系和确定其参数等问题。

轮式全向移动平台的运动学模型中速度雅可比矩阵存在不可逆情况，会出现奇异位形问题，导致平台自由度降低和灵活性变差[6]。针对脚轮式全向移动平台奇异位形问题，Jea H C[7]等将两个脚轮作为驱动轮，另一个脚轮作为转向轮，从而摆脱奇异位形。Ahmed[5]等将每个脚轮转速和转向角速度进行耦合，利用双轮差速原理摆脱奇异位形。以上两种方法不能有效识别平台是否处于奇异位形，导致平台运动效率过低。Kim D[8]等采用单电机和双电机脚轮搭配的工作方式摆脱奇异位形，但是该方法使平台成为冗余驱动系统，增大了控制策略设计难度。

轮式全向移动平台要达到快速且高精度的速度跟踪，必须建立平台的动力学模型，以运动学和动力学模型为基础设计控制策略。针对脚轮式全向移动平台，Ahmed[5]等采用PID控制器对脚轮转速进行控制，实现了参考速度的跟踪；Shi C S[9]等采用PID控制器实现了平台点到点跟踪控制。PID控制器其本质是一个线性控制器，只能在系统的某个平衡点或其邻域内有效，对复杂非线性平台的控制效果不佳。针对其他轮式移动平台，Soudbakhsh D[10]等采用线性最优法和滑模变结构控制法对平台进行轨迹跟踪控制；倪洪杰[11]等利用预演信息设计预演控制策略，调节平台驱动电机电压使其快速跟踪参考轨迹；孙棣华[12]等构造出一种自适应反步控制器，实现外部环境参数变化时平台良好的路径跟踪功能。上述设计的策略主要是基于现代控制理论，所设计方法均要求系统具有精确的数学模型，然而平台动力学模型中存在较多非线性项，证明系统稳定性时计算量大，导致无法获得控制率。

针对上述问题，本文采用正交分解法建立运动学模型，引入耦合因子动态调节平台避免奇异位形，采用拉格朗日方法建立平台的动力学模型且去掉向心力和科式力引起的力矩项得到其简化模型，设计基于逆动力学的双闭环模糊PID控制策略，仿真结果表明平台能快速准确地跟踪参考速度。

1 平台运动学建模

1.1 平台结构设计

图1表示平台结构，包括3个脚轮和1个车身。图2表示每个脚轮具有3个自由度，分别是绕轮心A的角速度、绕转向轴B的转向角速度、绕轮地接触点C旋转的角速度。脚轮的位置由结构参数、、、和时变参数、确定。其中为转向点与平台中点连线长度，为转向点与平台中点连线相对于X轴的夹角，为转向点与轮心的偏置距离，为脚轮半径，为脚轮绕轮心旋转角度，为脚轮绕转向轴旋转角度。平台运动过程中满足如下假设：

图1 平台结构示意图

图2 脚轮结构示意图

1）平台运动保持水平，转向轴垂直于水平面；

2）平台各部件为刚体；

3）脚轮与地面没有相对滑动，满足纯滚动条件。

1.2 运动学建模

表1 脚轮1速度正交分解表

由纯滚动约束条件可知，平行方向上的速度分量必须由脚轮的转速抵消平衡，可得滚动约束；垂直方向上的速度分量必须由脚轮转向角速度抵消平衡，可得滑动约束，分别表示如下：

同理可得脚轮2和脚轮3的滚动约束和滑动约束，将3个脚轮的滚动约束和滑动约束进行耦合可得：

1.3 奇异位形分析

对逆运动学模型式（4）进行分析：1）当θs1=θs2= θs3=kπ时（k=1，2，3, ······,下同），平台无法在X轴方向运动；2）当 θs1= θs2= θs3= π/2+kπ时，平台无法在Y方向运动；3）当 θs1= θs2= θs3= π/4+kπ时，平台无法在与X轴夹角为的方向上运动。因此，当3个脚轮的初始转向角度相互平行时，平台在垂直于轮轴的方向上将丧失一个自由度，失去全向移动能力，平台处于奇异位形。

针对该问题，本文采用耦合逆运动学方法解决奇异位形问题，基本思路是将两个脚轮角速度进行耦合，虚拟驱动第3个脚轮转向运动。由式（5）可得脚轮3转向角正运动学模型：

由转向角逆运动学模型式（4）可得：

将式（9）、（10）和（11）耦合得到脚轮 1和脚轮2的耦合运动学模型。同理可得脚轮1和脚轮3、脚轮2和脚轮3的耦合运动学模型，写成矩阵如下式所示：

其中：

由上述过程可知每个脚轮参与了两次耦合，为了消除多次耦合产生的误差，并进一步提高计算效率，需要判定平台是否处于奇异位形，从而决定式（12）中的耦合项是否参与运算。假设平台期望速度为、，当前实际速度为、，定义当前实际速度和最终期望速度夹角Ψ =|δ−β|，式中δ=arctan表示当前速度方向和X轴正向的夹角；β=arctan(vyr/vxr)，表示期望速度方向和X轴正向的夹角。在运动过程中平台实际速度、和夹角时刻变化，而期望速度、的夹角保持固定，所以是关于实际速度、的函数，且进一步分析可知：当Ψ =π/2时，此时平台完全处于奇异位形，不具有全向移动能力，需要耦合项参与运算，当或者时，平台完全远离奇异位形，具有全向移动能力，不需要耦合项参与运算。为了满足以上要求，设置动态耦合调节因子：

2 平台动力学模型

建立平台动力学模型可用牛顿欧拉方程或拉格朗日法。相较于牛顿欧拉方程，拉格朗日法只需要知道平台的速度而不涉及内力，更简洁高效。所以，本文采用拉格朗日法建立平台动力学模型。已知平台的线速度和角速度，可得拉格朗日算子，拉格朗日算子对驱动变量求偏导，即可得到拉格朗日动力学方程，建模流程如图3所示。

图3 平台动力学建模流程

2.1 转角动力学模型

对上述转角动力学模型式（18）进行Matlab/Simulink仿真分析。为了保证结果的有效性和广泛性，取任意随机速度作为参考速度，得到3个脚轮力矩如图4所示。分析可知，第二部分力矩要远小于第一部分力矩，其原因是在运动过程中，为了保持稳定性和高效性，平台转向过程较平稳，脚轮转向角速度很小，因此第二部分力矩可以忽略，以消除转向角速度对平台的影响，且不会改变计算结果，简化后的表达如下式所示：

图4 力矩对比图

2.2 转向角动力学模型

对于脚轮转向角动力学建模，令广义变量q=θsi，平台总能量在式（17）基础上应加上脚轮转向所产生的能量，表达式如下：

对正运动学模型式（6）进行求导，可得平台的加速度，表达式如下：

3 控制器设计

3.1 基于逆动力学的双闭环模糊PID控制器设计

对于速度控制器而言，经典控制方法是PID控制，但是PID控制的3个参数的整定是一个关键的问题。针对精确的数学模型，采用不同的参数直接影响被控信号的超调量和调节时间等。在实际的工程应用中常采用试凑的方法来整定参数，这为设计带来了诸多不便[13]，而模糊PID控制器能够根据一定的模糊规则对PID参数进行优化，克服PID控制器无法动态调参的缺点。为了实现对平台在非线性及不确定性条件下的速度精确跟踪问题，根据模糊PID控制原理提出平台仿真控制策略如图5所示。当指定平台的期望速度后，由式（2）和（3）逆运动学模型可得脚轮当前转速和转向角度；经模糊PID控制器2调节后得到平台的驱动力矩，然后输入给式（19）、（22）和（24）的逆动力学模型；最后由平台得到实际速度，经过反馈后和期望速度共同输入给式（13）得到动态耦合因子值 ，动态调节运动学模型中耦合项的比重。上述仿真控制系统中包含两个控制环：外环实现精确的全局速度跟踪，内环稳定控制脚轮转速。

图5 基于逆动力学的双闭环模糊PID控制策略结构框图

3.2 输入/输出变量的模糊处理

选取Matlab/Simulink调试的PID参数作为模糊PID控制器的初始参数，控制器1初始参数：kp=0.1，ki=0.8，kd=–1.9，控制器 2初始参数：kp=1.1，ki=0.6，kd=0.3。速度误差E和速度误差变化率Ec的基本论域为[–0.2，0.2]，修正参数、、的基本论域为[–5，5]。速度误差E及变化率Ec作为其输入，通过模糊推理确定PID控制器3个参数。根据文献[14]提出的模糊控制规则主要思想，结合平台的控制要求，通过查阅相关资料[15]，模糊规则选取应当遵循以下原则：

1）当误差和误差率越大，参数kp应当逐渐增大，ki和kd应当逐渐减小；当系统达到零点时，kp应当为最大，ki和kd应当为最小；

2）当速度误差绝对值较大时，为了提高系统响应速度，应该适当增大kp值；如果全向移动平台在完成特殊运动，如圆弧运动、转弯运动等，应当适当减小ki值，避免出现超调现象；

3）当速度误差和误差率大小适中时，为了保证响应速度的同时防止出现严重的超调现象，应当减小kp值，合理设置ki和kd值；

4）当速度误差绝对值较小时，为了提高移动平台的稳态性能，应当增大kp和ki的值；同时为了避免在初始值附近出现震荡，应当增大kd值；

根据以上规则得到3个修正参数的模糊规则表，如表2、表3所示。

表2 模糊控制器1控制参数修正量

表3 模糊控制器2控制参数修正量

4 仿真分析

4.1 运动学和动力学仿真分析

为了验证所提动态耦合因子方法摆脱奇异位形的有效性，根据运动学和动力学模型，当平台在时刻处于奇异位形的工况时，进行Matlab/Simulink仿真，平台的结构参数如表4所示。设置平台初始位姿为（单位：rad，下同）；指定平台的参考速度为（单位：线速度m/s、角速度rad/s，下同）。

表4 全向移动平台结构参数和几何参数

图6是耦合因子取最大值0.5得到的脚轮转速；图7是采用动态耦合因子得到的脚轮转速。对比可知：1）当平台处于奇异位形时，脚轮3保持不动，脚轮1和脚轮2迅速反向转动，带动平台顺时针扭动一个微小角度，此时平台等效于双轮差速模型。当平台摆脱奇异位形时，脚轮3开始转动，并迅速和脚轮1、脚轮2达到稳定转速，带动平台实现期望运动。2）当采用动态耦合因子后，系统能够根据当前位形和奇异位形实时调整耦合项的比重，更合理地控制3个脚轮的转速，使系统的动态性能指标发生显著变化，见表5。

表5 动态性能指标对比结果

图6 m =0.5时转速曲线

图7 实时变化时转速曲线

图8 动态耦合因子变化曲线

4.2 基于逆动力学的双闭环模糊PID控制仿真分析

为了验证所提控制策略的正确性和有效性，本小节将其与文献[7]设计的PID控制策略进行比较分析；对于平台是否处于奇异位形，仿真设置了两种工况，所得分析结果如图9～ 图16所示。

图9 工况一横向速度对比图

图10 工况一纵向速度对比图

图11 工况一自转速度对比图

图12表示平台的运动轨迹，从图可知，所提策略得到的最大纵向位移偏移量是–0.003 m，而文献[7]的最大偏移量是–0.024 m；同时，当系统达到稳定状态时，文献[7]得到的实际轨迹与期望轨迹始终存在–0.005 m左右的误差，而所提策略能使平台实际轨迹迅速收敛到期望轨迹且误差保持在–0.0001 m 左右。

图12 工况一平台质心的运动轨迹对比图

图13 工况二横向速度对比图

图14 工况二纵向速度对比图

图15 工况二自转速度对比图

图16表示平台的运动轨迹，从图可知，所提策略得到的最大偏移量是0.002 m，而文献[7]的最大偏移量是0.3 m；平台逐渐收敛于稳定状态时，文献[7]得到的实际轨迹在PID控制作用下出现了偏差纠正过度情况，误差从–0.07 m一直增加到0.3 m，且呈现继续增加趋势；而本文所提策略能使平台实际轨迹迅速收敛到期望轨迹并一直保持在0.0005 m的误差范围。

图16 工况二运动轨迹对比图

5 结束语

本文对脚轮式全向移动平台运动控制问题进行了研究。采用简洁高效的正交分解法，根据纯滚动约束条件得到了平台的运动学模型。针对运动学模型速度雅可比矩阵不可逆导致自由度降低的奇异位形问题，通过在运动学模型的基础上增加由转角运动学和转向角运动学耦合得到的耦合项，引入耦合因子动态调节耦合项的比重。采用拉格朗日法建立平台的动力学模型并对其进行相应简化；基于上述模型，提出了基于逆动力学的双闭环模糊PID控制策略。

通过Matlab仿真得到的结果表明：设计的动态耦合项能使平台快速且有效摆脱奇异位形，恢复全向移动能力；对动力学模型分析可知平台在运动过程中转向角速度小、转向过程平稳，可以忽略其对平台的影响，简化后的动力学模型消除了转向角速度对平台能耗的影响，表明所提简化方法是正确且有效的；同时，所提控制方法可实现平台精确快速的跟踪参考速度，具有良好的稳定性和鲁棒性。上述研究可为脚轮式全向移动平台下一步实际搭建工作提供理论支撑。