基于动力学模型预测控制的Stewart型轮足腿控制方法

汪首坤， 刘大江， 郭非， 岳彬凯， 陈志华， 王军政

(北京理工大学 自动化学院，北京 100081)

Stewart平台是一种并联机构，具有精度高、刚性好的优点.将Stewart平台应用于机器人，可以使其具有姿态控制精度高、承重能力强的特点.如图1所示轮足机器人,它的每条轮足腿均包含一个倒置的Stewart平台.改变Stewart平台的位姿可以使轮足腿的位姿发生变化从而改变机器人的位姿.

Stewart平台是一种包含6个作动器的并联机构，具有非线性和强耦合的特点.其运动控制方法可分为作动器独立控制以及平台整体控制.作动器独立控制方法不需要考虑平台整体的动力学特性，每个作动器采用相同的控制器且独立运行.虽然控制器设计简单易实现，但无法保证Stewart平台在不同位姿条件下的控制性能.而平台整体控制方法由于考虑了平台动力学特性并对作动器进行整体协调控制，所以可以保证不同位姿状态下的控制性能.基于平台整体控制的控制器设计上有不同方法：哈尔滨工业大学的刘磊等[1]通过动力学模型建立了多变量鲁棒控制器，将其用于Stewart平台的隔振方面，可以有效抑制微扰动.北京理工大学的郝仁剑等[2]利用力矩解耦的方法构建动力学模型，采用自适应鲁棒控制实现了平台的鲁棒控制；哈尔滨工业大学的郭洪波[3]提出基于期望补偿的非线性自适应控制器，可以克服动力学模型的不确定性对系统性能的影响.

图1 Stewart型轮足腿机器人

模型预测控制(MPC)广泛应用于多变量、带约束的控制问题，它通过预测模型得到被控制对象的优化控制量，因而在Stewart平台这种并联机构的控制方法上具有研究价值.

1 Stewart型轮足腿控制原理

图2为Stewart型轮足腿的控制框图,通过速度位置双闭环实现对位姿输入的跟踪.内环速度环采用MPC控制器,外环位置环采用比例控制器+前馈控制.对于速度环，MPC控制器根据目标速度、作动器的实际速度和预测模型完成作动器速度控制.对于位置环，由3个位移量和3个姿态角构成位姿输入向量经过运动学逆解得到6个作动器的目标长度.目标长度与从积分器输出的作动器实际长度进行比较，其偏差经过比例控制器并叠加前馈信号输入到MPC控制器.Stewart型轮足腿的作动器速度经过积分器输出作动器的长度，再经过运动学正解[4]得到Stewart型轮足腿的位姿输出，实现位姿输出对位姿输入的跟踪.相对于单闭环控制，采用双闭环控制可以降低MPC控制器预测模型中的状态向量的维数，从而降低在高采样率实验条件下对硬件运算、存储能力的需求.

图2 Stewart平台控制框图

2 Stewart平台动力学模型

图3为简化后的负载受力示意图(不考虑作动器质量、惯量)，通过对负载进行力和力矩的分析可以得到平台动力学模型.其中C为负载的质心，其位置矢量为RC.f1～f6为6个作动器产生的推力,作动器受到的摩擦力相对于负载的惯性力可视为未建模参数忽略不计.ρ1～ρ6是质心C到作动器推力作用点的径矢(为了简明起见，图中只标注了f1和ρ1).连体坐标系B以质心C为原点并跟随负载一起运动，惯性坐标系N以基座圆心作为原点.

首先，在惯性坐标系N中对质心进行受力分析，可以得到负载质心的牛顿运动方程

(1)

在连体坐标系B中对负载进行力矩分析可以得到如下欧拉方程[5]

(2)

图3 Stewart平台受力图

为了将在连体坐标系中的向量坐标表示为惯性坐标系中的坐标，引入旋转矩阵R

RRT=I3×3

式中：c和s分别表示三角函数余弦和正弦；φ，θ和ψ分别为负载在惯性坐标系中的横滚、俯仰及航向姿态角.如下关系式反映了负载角速度，径矢以及推力向量在连体坐标系和惯性坐标系中坐标表示之间关系

(3)

对式(2)进行变量替换得到惯性坐标系下的欧拉方程

(4)

可将式(4)中的向量积⊗运算替换为矩阵运算[6]

(5)

(6)

牛顿运动方程可以写为

(7)

(8)

ωN(k+1)=(I3×3-TsRJ-1RTΩNRJRT)ωN(k)+

(9)

将式(8)与式(9)合并

(10)

式中：

(11)

式(10)可化为

(12)

xp(k+1)=Ax kxp(k)+Bu ku(k)+Bvkv(k)

(13)

输出方程可以写为

yp(k)=xp(k)

(14)

3 模型预测控制器

Stewart平台采用模型预测控制器(MPC控制器)进行速度闭环控制，控制流程见图2中的速度环闭环控制部分.目标向量yr(k)是Stewart平台作动器的目标速度，MPC控制器对平台的状态向量xp(k)进行估计得到控制器的状态向量xc(k)，再利用平台的预测模型得到Stewart平台预测时域p内的输出向量y(k)，y(k+1)，……，y(k+p-1)，然后通过使优化目标函数最小化和约束条件求解得到最优化控制向量u(k)，最后该控制向量输入到被控对象Stewart平台实现输出向量yp(k)对目标向量yr(k)的跟踪.

3.1 预测模型

由已知动力学模型式(13)、(14)得到式(15)状态观测器，其中xc(k)从xp(k)的值估计得到；Axk、Buk及Bvk在每个控制周期更新一次，并在预测时域p内保持不变

(15)

预测模型的第i步预测状态如下

xc(k+i+1|k)=Ax kxc(k+i|k)+

Bu ku(k+i|k)+Bv kv(k),0≤i≤p-1

(16)

预测模型的第i步预测输出如下

y(k+i|k)=xc(k+i|k),0≤i≤p-1

(17)

3.2 目标函数及其约束

通过设定目标函数并使其最小化从而得到最优控制.目标函数可以写为如下表达式[7]

(18)

式中：yr(k+i|k)为从当前时间k起始的预测时域p内的目标值，是在预测时域p内的目标轨迹；控制时域m满足m

Δuk=[Δu(k|k)TΔu(k+1|k)T…

Δu(k+m-1|k)T]T

(19)

式中Δu(k+i|k)=u(k+i|k)-u(k+i-1|k)为两个时刻控制向量之差.式(18)右侧的前一项反映了对跟踪目标性能的需求，后一项反映了对抑制控制量变化的需求.此外，通过加入松弛因子可以保证在违反约束条件情况下最小化目标函数得到可行解，松弛因子ε>0，ρ为其权重.

在实际系统中，往往需要对控制量和被控制对象的输出进行约束.其中被控对象输出量约束为

ymin(k+i|k)-εvmin≤y(k+i|k)≤

ymax(k+i|k)+εvmax, 0≤i≤p-1

(20)

式中：ymax(k+i|k)和ymin(k+i|k)为被控制对象输出的最大、最小值；vmax>0和vmin>0为软化约束向量.

控制量约束为

umin(k+i|k)≤u(k+i|k)≤

umax(k+i|k), 0≤i≤m-1

(21)

式中umax(k+i|k)和umin(k+i|k)为控制量的最大、最小值.

控制量变化约束为

Δumin(k+i|k)≤Δu(k+i|k)≤

Δumax(k+i|k), 0≤i≤m-1

(22)

式中Δumax(k+i|k)和Δumin(k+i|k)为控制量变化的最大、最小值.

3.3 目标函数最小化求解

式(18)可以进一步写成如下形式

x(k)TKx)Δuk+const

(23)

式中：KΔ u，Kyr，Kv，Ku，Kx均为常数矩阵；const为常数标量；u(k-1)为k前一时刻的控制量.J(ε,Δuk)写为式(23)形式，最小化问题便转化为求解带有约束的二次规划(quadratic programming ，QP)问题，可以计算出控制时域内的最优控制增量序列

(24)

将此控制序列的第一个元素与u(k-1)相加得到k时刻优化控制量

u(k)=u(k-1)+Δu*(k|k)

(25)

4 仿真与控制器参数整定

根据图2的控制框图在Matlab/Simulink建立控制模块图，如图4所示.式(13)(14)包含在的动力学模型模块中，MPC控制器完成预测输出和控制量的优化求解.控制量输入到Stewart平台Adams模型，同时模型作动器的长度和速度测量值反馈回控制器及其他模块.阶跃信号用于速度环MPC控制器参数的整定；正弦信号用于位置环比例控制器参数的整定.

图4 Simulink中的控制模块图

Stewart平台的Adams模型的主要参数如表1所示,将摩擦因数和预紧力等未建模参数也考虑进去，可以评估MPC控制器的鲁棒性.仿真和控制器参数整定按照先速度环后位置环的顺序进行，采样时间Ts设定为0.5 ms.

表1 Adams模型主要参数

4.1 速度环仿真及控制器参数整定

首先对速度环的MPC控制器参数进行整定，主要的整定参数为预测时域p和控制时域m.将幅值为0.15 m/s的阶跃信号输入到MPC控制器，控制量的约束范围为±700 N，比例因子为12 000.通过比较不同p和不同m条件下的阶跃响应确定它们的优化值.

从图5可以得知：预测时域p的变化对阶跃响应的影响较小，而控制时域m的变化对阶跃响应的影响较大.表2总结了m变化的阶跃响应.

图5 p，m变化条件下的阶跃响应

表2 m变化的阶跃响应

对表2分析可知：随着m的增大，阶跃响应上升时间和稳态相对误差均减小.考虑到增大m会导致计算量增大，故MPC控制器的控制时域取中间值即m=2，同时MPC控制器对未建模的摩擦力显示出良好的鲁棒性.

4.2 位置环仿真及控制器参数整定

对位置环比例控制器参数进行整定.将频率2 Hz，幅值为0.15 rad的正弦波作为俯仰轴姿态的输入参考信号，逐渐增大比例控制器增益且输出不发生振荡.不同增益下的正弦波响应如图6所示.

图6 m=2,p=5的正弦波响应

由图6可知，当位置环控制器增益为0.12，正弦波响应的幅值误差为0.000 4 rad，延时为0.5 ms，满足姿态跟踪精度及动态特性的要求.

5 实验与分析

图7为实验装置图，开发计算机对控制算法编译后下发给目标计算机.目标计算机运行控制算法并将控制量输入到伺服驱动器控制电动缸运动，同时通过编码器接口读取电动缸的位置速度信息.

图7 实验装置图

首先进行姿态跟踪实验,选择正弦波信号作为俯仰轴的姿态输入.频率分别为1,2和4 Hz，幅值为0.087 3 rad.MPC控制器和作为对比的PI控制器(参数设置:P=25，I=0.1,)实验曲线如图8所示.跟踪性能如表3所示，在频率4 Hz条件下，MPC控制器优势更为明显.

图8 MPC控制器与PI控制器姿态跟踪对比

表3 姿态跟踪性能对比

图9 MPC控制器与PI控制器位移跟踪对比

6 结 论

根据Stewart型轮足腿多变量、带约束的控制特点，采用MPC控制器作为速度环控制器，对轮足腿进行速度位置双闭环控制.通过Stewart平台动力学模型建立MPC控制器的预测模型并对目标函数最小化求解得到控制量，再利用Adams 和Matlab联合仿真对MPC控制器参数进行整定,最后通过实验验证了MPC控制器能够提高Stewart型轮足腿的跟踪精度、动态特性，其性能优于PI控制器并对未建模的摩擦力具有良好的鲁棒性.