基于伪谱法的自由漂浮柔性空间机器人姿态运动规划与反馈控制

姚其家， 戈新生

(北京信息科技大学 机电工程学院， 北京 100192)

空间机器人可以取代宇航员进行部分舱外作业，是人类进行太空活动的重要工具之一[1-2]。空间机器人的工作模式可以分成自由漂浮模式，自由飞行模式和基座姿态受控模式三种。其中，自由漂浮模式是空间机器人最为常用的工作模式。该模式不仅可以节省燃料，延长空间机器人的在轨使用寿命，还可以避免在捕获过程中由于过控制引起的机械臂末端与目标之间的碰撞。对于自由漂浮空间机器人，由于机械臂与基座之间的强动力学耦合作用，可以通过机械臂的关节运动实现基座的姿态改变，从而使机械臂和基座同时到达期望姿态。此外，在许多实际操作中，还需要考虑机械臂柔性变形对系统姿态造成的影响。由于非完整约束与刚柔耦合项的存在，这类空间机器人系统的姿态控制问题要比一般的多体系统复杂很多。

国内外学者对空间机器人的动力学与控制问题进行了大量的研究工作。运动规划方法(包括遗传算法、粒子群算法和双向逼近方法等)[3-8]和主动控制方法(包括PID控制、模糊控制、模型预测控制和自适应控制等)[9-14]都在空间机器人的姿态最优控制和振动抑制等方面取得广泛应用。总体来说，对于空间机器人系统动力学与控制问题的研究已经非常深入，选取的模型较为复杂，考虑的问题也相当全面。近年来，伪谱法逐渐成为航天领域求解最优控制问题的一种重要方法，该方法也同样适用于求解空间机器人系统的姿态控制问题。戈新生等[15]利用Legendre伪谱法研究了自由漂浮空间机器人的姿态运动规划问题，在建模过程中将机械臂简化为刚体，没有考虑机械臂的柔性效应。Liao等[16]采用Gauss伪谱法和直接打靶法相结合的混合控制策略研究了柔性空间机器人的减振运动规划问题。需要指出的是，伪谱法从本质上讲是一种开环控制方法，参数不确定性和外界干扰都会对计算结果产生很大的影响。因此，本文在利用伪谱法规划柔性空间机器人关节运动的基础上，进一步设计了最优反馈控制律对伪谱法得到的参考轨迹进行修正。

针对初始状态不确定性对系统姿态造成的影响，在空间机器人系统动力学模型的基础上，本文提出一种基于间接伪谱法的运动规划与反馈控制相结合的姿态控制策略，通过规划机械臂的关节运动，使自由漂浮空间机器人系统从初始位置运动到指定姿态，并满足系统的能耗最小和控制输入受限。在运动规划方法的设计中，利用Legendre伪谱法将空间机械臂非完整运动规划问题转化为非线性规划问题，再利用序列二次规划(SQP)算法求解此非线性规划问题得到系统的优化运动轨迹和最优控制输入。在反馈控制方法的设计中，将状态方程在已知的参考轨迹上线性化处理，利用Pontryagin极大值原理将对参考轨迹的跟踪问题转化为两点边值问题，再利用Legendre伪谱法离散此两点边值问题，将其转化为线性代数方程组进行求解，该过程无需任何积分计算，具有很好的实时性。最后，文末给出仿真算例。

1 动力学建模

考虑一个作平面运动的自由漂浮柔性空间机器人系统如图1所示。该系统由刚性基座B0和n关节柔性机械臂各连杆Bi(i=1,2,…,n)组成，其中，各分体之间通过转动铰Oi(i=1,2,…,n)联结。以系统的总质心Oc为原点，建立惯性坐标系Σc。以基座B0的质心O0为原点，建立基座B0的连体坐标系Σ0。以各转动铰中心Oi(i=1,2,…,n)为原点，建立柔性连杆Bi的连体坐标系Σi(i=1,2,…,n)。设φ为基座B0相对惯性系的绝对姿态角，θi(i=1,2,…,n)为Oi铰邻接分体之间的关节转角。mi(i=0,1,…,n)表示分体Bi的质量。l0表示基座B0的质心O0与转动铰O1之间的距离，li(i=1,2,…,n)表示柔性连杆Bi的长度。这里，忽略柔性连杆微小变形对系统总质心位置造成的影响，假设系统总质心保持不动。

将柔性连杆视作Euler-Bernoulli梁，采用假设模态法对柔性连杆的弹性变形进行离散，即

(1)

图1 自由漂浮柔性空间机器人系统Fig.1 Free-floating flexible space robot system

忽略微弱的重力梯度影响，结合动量守恒定律和第二类Lagrange方程，得到图1所示的自由漂浮柔性空间机器人系统的动力学方程为[13-14]

(2)

按照系统的广义坐标对应的广义力是否为零，把广义坐标η划分为ηr=[θ1,θ2,…,θn]T和ηe=[φ,q1,q2,qn]T两部分，将式(2)按照相应的维数分解得到

(3)

(4)

式中：Kee为对应于坐标ηe的广义刚度矩阵。

在空间机器人系统姿态运动规划过程中，式(3)用于系统运动已知的情况下求解关节力矩。式(4)具有非完整约束方程形式，一方面，可以规划不同的机械臂关节运动来完成同样的操作任务；另一方面，系统质量矩阵的子矩阵Mre、Mee以及速度耦合项h2均与机械臂关节的运动规律有关，当关节运动规律改变时，Mre、Mee和h2也随之改变，机械臂的弹性变形也将发生变化。

2 运动规划方法

2.1 最优控制问题

(5)

根据最小能量原理，选择机械臂关节相对转动的耗散能作为系统的优化性能指标，相应的目标函数为

(6)

边界条件约束包括空间机器人系统运动过程初末端的位形约束和控制输入边界。

Θ(t0)=Θ0,Θ(tf)=Θf,

u(t0)=0,u(tf)=0

(7)

此外，在实际操作中还需要考虑空间机器人系统姿态角的范围约束和控制输入受限等不等式路径约束。

|θi(t)|≤θm,

|ui(t)|≤um

(8)

式中：θm>0和um>0分别表示姿态角和控制输入的上限。

将空间机器人系统非完整运动规划问题描述成最优控制问题，即：求解最优控制输入u(t)，系统从初始位形Θ0运动到终端位形Θf，使得性能指标(6)最小，并满足状态方程(5)、边界条件约束(7)和不等式路径约束(8)。

(9)

式中:Φ(·)和C(·)表示空间机器人系统的边界条件约束和不等式路径约束，分别对应式(7)和式(8)。式(9)为Bolza型最优控制问题的标准形式。

2.2 参考轨迹生成

对于式(9)所描述的最优控制问题，采用Legendre伪谱法将其在Legendre-Gauss-Lobatto(LGL)点处离散，转化为非线性规划问题进行求解。Fahroo等[17]证明了Legendre伪谱法的协态变量映射定理，使用该方法求得的解满足传统间接法的一阶最优必要条件，避免了直接法的不足之处。

空间机器人系统最优控制问题的时间区间为[t0,tf]，而Legendre伪谱法定义的时间区间为[-1,1]，因此对时间变量t作如下变换

(10)

最优控制问题(9)改写为如下形式

(11)

(12)

(13)

式中，Lagrange插值基函数Li(τ)定义为

(14)

状态变量由插值多项式近似表示后，其导数可以转化为对基函数的微分运算，即

(15)

式中，微分矩阵D∈R(K+1)×(K+1)由式(16)确定[17]

(16)

将式(15)代入状态方程，得到状态方程的离散表达式，

k=0,1,K

(17)

式中:为了表述方便，将X(τk)和U(τk)简写为Xk和Uk。

利用Gauss积分方法离散目标函数的积分项，得到目标函数的离散形式为

(18)

式中:μk(k=0,1,…,K)为Gauss-Lobatto权重，由式(19)确定

k=0,1,…,K

(19)

同理，将边界条件约束和不等式路径约束在LGL点处进行离散，

Φ(X0,t0,Xf,tf)=0

(20)

C(Xk,Uk,τk;t0,tf)≤0

(21)

经过上述过程，将空间机器人系统最优控制问题转化为非线性规划问题，即：通过求解离散控制变量U，使得性能指标(19)最小，并满足状态方程约束(17)、边界条件约束(20)和不等式路径约束(21)。对于离散得到的非线性规划问题，本文通过MATLAB工具箱SNOPT求解，该工具箱采用SQP算法，是求解这类参数寻优问题最有效的算法之一，具有很高的收敛效率和计算精度[18]。

这里需要指出的是，采用Legendre伪谱法求解式(11)描述的非线性规划问题，若选取的离散节点较少，则得到的结果精度较差，达不到要求。若离散节点选取较多，则设计变量的数目十分庞大，为SQP算法选取设计变量初值的过程变得相当困难。另外，如果变量初值选取不当，往往会使问题收敛不到可行解或陷入局部最小化而得不到全局最优解。针对这些问题，我们先选取较少的LGL节点K1，求解非线性规划问题，得到状态变量和控制变量的可行解，再利用更多的LGL节点K2离散原非线性规划问题，这些节点处状态变量和控制变量的初值通过对可行解线性插值获得，再利用SQP算法求解精度更高的空间机器人系统优化运动轨迹。该方法可以使程序更为有效地收敛。

3 反馈控制方法

3.1 轨迹跟踪问题的转化

考虑初始状态不确定性对系统姿态造成的影响，空间机器人的实际运动轨迹与利用运动规划方法得到的参考轨迹之间存在偏差。定义δx(t)∈Rm为状态变量实际值x与参考值x*之间的偏差量，δu(t)∈Rn为控制变量u的修正量，将对参考轨迹的跟踪问题转化成一个线性时变的姿态调节问题。对状态方程(5)线性化处理得到如下方程

(22)

式中：δx0为初始时刻实际状态量与参考状态量之间的差值。A(t)和B(t)分别为状态方程相对状态变量和控制变量的偏导数在参考轨迹上取值所构成的时变矩阵，表示为

(23)

空间机器人系统的姿态调节问题可以描述为：确定控制输入修正量δu(t)以及状态偏差量δx(t)，满足式(22)，并使得如下的二次型性能指标最小

δuT(t)R(t)δu(t)]

(24)

式中：Pf∈Rm×m表示半正定的末端状态加权矩阵，Q(t)∈Rm×m表示半正定的状态加权矩阵，R(t)∈Rn×n表示正定的控制加权矩阵。

根据Pontryagin极大值原理，引入系统的协态变量δλ(t)，构造如下Hamilton函数

δλT(t)[A(t)δx(t)+B(t)δu(t)]

(25)

根据最优控制的必要条件，得到如下正则方程

(26)

和边值条件

δx(t0)=δx0,δλ(tf)=Pfδx(tf)

(27)

δu(t)=-R-1(t)BT(t)δλ(t)

(28)

可以看出，通过求解式(26)和(27)所描述的两点边值问题，获得协态变量δλ(t)，即可进一步求得控制输入修正量δu(t)。

3.2 基于间接伪谱法的最优反馈控制律

对于式(26)和(27)所描述的两点边值问题，由于本文的动力学模型较为复杂，如果采用MATLAB的lqr函数等传统的对Riccati方程进行积分求解的方法，一方面要消耗大量的计算时间，另一方面还可能出现数值不稳定的情况。因此，本文利用间接Legendre伪谱法离散此两点边值问题，将其转化为一个线性方程组进行求解。

首先，通过时域变换(10)将式(26)和(27)所描述的两点边值问题的时间区间转换到[-1,1]上，表示为

(29)

同第2.2节一样，将状态偏差量δx(t)和协态变量δλ(t)用Lagrange插值多项式近似

(30)

(31)

对式(30)和(31)求导得到

(32)

(33)

式中：微分矩阵D的表达式由式(16)给出。将式(32)和(33)代入式(29)所描述的两点边值问题，得到其离散表达式为

k=0,1,…,K

(34)

k=0,1,…,K

(35)

式中：为了表述方便，将δx(τk)，δλ(τk)，A(τk)，B(τk)，R(τk)和Q(τk)分别简写为δxk，δλk，Ak，Bk，Rk和Qk。

(36)

(37)

式中，F、G、M、N∈Rm(K+1)×m(K+1)，具体表达式分别为[19]

(38)

(39)

(40)

(41)

式中：Em表示m×m阶单位矩阵，0m表示m×m阶零矩阵。这里，我们的目标是求解式(36)和(37)，并满足两点边值问题的边值条件(27)。综合式(27)、(36)和(37)并将其写成下列分块的形式

(42)

式中：S1=[0m,…,0m,Pf]∈Rm×m(K+1)，S2=[0m,…,0m,-Em]∈Rm×m(K+1)。这里，将V分为V=[V0,Ve]两部分，式(42)写成如下形式

V0δx0+VeZe=0

(43)

Ze=-VeV0δx0=Wδx0

(44)

式中:符号“”表示MATLAB的左除运算。进一步有

(45)

式中：将矩阵[Em,W]T重新分块为Wx∈Rm(K+1)×m和Wλ∈Rm(K+1)×m，使得

X=Wxδx0

(46)

Λ=Wλδx0

(47)

将式(47)代入式(28)，当初始时刻的状态偏差量δx0已知时，最优反馈控制律在离散节点处的值可以表示为

δu(τk)=-R-1(τk)BT(τk)Wλkδx0

(48)

式中：Wλk为Wλ对应τk时刻的子矩阵。通过计算式(46)、(47)和(48)得到两点边值问题的离散解，在此基础上，非离散节点处的控制输入修正量的值可以通过对离散节点处控制输入修正量插值获得。整个算法在执行过程中不需要任何积分计算，具有很强的实时性和在线计算能力。

另外需要指出的是，该闭环跟踪算法本质上讲是一类最优反馈控制，其稳定性与转化得到的两点边值问题的求解精度紧密相关[19-20]。选取更多的LGL节点离散式(29)中的状态偏差量和协态变量，则两点边值问题的求解精度越高，算法的稳定性越好。然而，随着离散节点的增多，两点边值问题的离散过程和最优反馈控制律的求解过程都将消耗更多的计算时间。因此，我们需要选取适当的离散节点个数，来同时兼顾闭环算法的稳定性和计算效率。

4 数值仿真

以平面运动的两杆自由漂浮柔性空间机器人的姿态控制问题作为仿真算例。设置空间机器人的运行时间t=20 s，系统的质量几何参数，如表1所示。

表1 空间机器人系统质量几何参数Tab.1 Mass and geometry parameters of the space robot system

给定系统的初始位形和终端目标位形分别为

Θ0=[0,-π/3,π/2]T,Θf=[-π/6,π/3,π/6]T

即空间机器人系统基座B0姿态φ由初始位置顺时针转动π/6到达-π/6位置，前杆B1姿态θ1由初始位置逆时针转动2π/3到达π/3位置，后杆B2姿态θ2由初始位置顺时针转动π/3到达π/6位置。

首先，采用运动规划方法生成空间机器人的参考运动轨迹。仿真在MATLAB平台上编程实现，求解可行解选取LGL节点个数K1=5，求解精度更高的数值解选取LGL节点个数K2=40，仿真结果如图2～5所示，其中，曲线上的标记代表LGL节点的分布情况。图2为空间机器人系统基座姿态φ以及机械臂关节角θ1和θ2的优化运动轨迹，运动曲线光滑平稳，满足空间机器人系统初末位置的位形约束和姿态角的范围约束。图3为空间机器人系统最优控制输入规律，初末位置边界值均为0，并且满足控制输入的范围约束。图4为空间机器人系统在运动过程中柔性机械臂中点处的一阶弹性变形情况，连杆的弹性变形在合理范围之内，且残余振动基本为0，符合工程实际。图5给出了自由漂浮空间机器人系统在不同时刻的运动投影，图中，三角形(Δ)标注的曲线表示机械臂末端点的运动轨迹。系统的总质心始终在原点保持不动，空间机器人能够有效地完成事先规划好的点对点操作任务。从仿真结果可以看出，以机械臂关节角速度作为控制输入信号，空间机器人系统姿态是可控的，且生成的优化运动轨迹能够满足系统的能耗最小和控制输入受限等要求。

图2 空间机器人系统优化运动轨迹Fig.2 Optimal trajectory of the space robot system

图3 空间机器人系统最优控制输入Fig.3 Optimal control inputs of the space robot system

图4 柔性机械臂连杆的弹性变形情况Fig.4 Elastic deformation of flexible manipulator links

图5 空间机器人系统总体运动过程Fig.5 Whole motion process of the space robot system

另外，这里考虑初始状态不确定性对系统姿态造成的影响，设定空间机器人系统在初始时刻存在外部扰动的范围如下

|Δφ(0)|≤π/30,|Δθ1(0)|≤π/15,

|Δθ2(0)|≤π/20

考虑正负两个极限情况为情况1和情况2，空间机器人的实际初始位形分别为Θ0=[π/30,-4π/15,11π/20]T(情况1)和Θ0=[-π/30,-2π/5,9π/20]T(情况2)。将图2的结果作为空间机器人姿态运动的参考轨迹，再利用反馈控制方法分别对情况1和情况2的扰动进行反馈控制，其中，加权矩阵Pf、Q和R均设计为对角矩阵，具体元素由BRYSON法则确定[21]。

初始状态量存在偏差情况下空间机器人系统的优化运动轨迹如图6所示，其中，实线表示不存在初始状态量偏差情况下空间机器人系统的参考运动轨迹，虚线和点划线分别表示反馈控制作用于系统后情况1和情况2的实际运动轨迹。这两种情况下终端状态量相对于参考轨迹的偏差值由表2给出。可以看出，在反馈控制律的作用下，空间机器人的实际运动轨迹逐渐偏向参考轨迹，一段时间后，实际运动轨迹和参考轨迹基本重合，终端状态量的偏差值在10-4rad量级，满足系统点对点操作的精度要求。仿真结果说明采用的反馈控制方法可以有效消除初始状态不确定性对系统姿态运动造成的影响，本文提出的运动规划与反馈控制相结合的姿态控制策略对于一定范围内的初始状态量偏差具有很好的鲁棒性。另外，两种极限情况下反馈控制律的解算时间均少于反馈周期，反馈控制律能够满足实时性的要求。

表2 初始状态量存在偏差情况下系统的终端状态量偏差Tab.2 Terminal state deviations subject to initial state errors

图6 初始状态量存在偏差情况下系统的运动轨迹Fig.6 Trajectory of the system subject to initial state errors

5 结 论

本文研究了自由漂浮柔性空间机器人系统的动力学建模与姿态控制问题，提出一种基于伪谱法的运动规划与反馈控制相结合的姿态控制策略。其中，利用伪谱法离散设计变量的思想在控制策略中两次应用。第一次是利用伪谱法离散状态变量和控制变量，将空间机器人姿态优化问题离散为非线性规划问题进行求解。第二次是利用伪谱法离散状态偏差量和协态变量，将轨迹跟踪问题转化得到的两点边值问题离散为线性方程组进行计算。通过数值仿真，系统优化运动轨迹光滑平稳，柔性机械臂连杆的全局振动和残余振动在合理范围之内，反馈控制有效消除系统初始状态量偏差对系统姿态造成的影响，空间机器人系统能够从初始位置运动到指定姿态，并满足系统的能耗最小和控制输入受限，仿真结果验证了该方法的有效性和鲁棒性。尽管本文工作是针对作平面运动的空间机器人系统姿态控制问题展开的，但提出的基于间接伪谱法的姿态控制策略也适用于作空间运动的空间机器人系统的姿态控制以及其他非完整系统的优化控制问题。