引言

机器人是制造业皇冠顶端的"明珠"，其研发、制造、应用是衡量国家科技创新和高端制造业水平的重要标志. 目前机器人已在制造业、服务业、医疗与康复、国防和太空等各领域得到广泛应用^[1-3]. 机器人的设计、研发与制造涉及力学、机械、自动化和计算机等多学科交叉和技术综合，特别是多体系统动力学与控制是机器人研发的重要理论基础.

针对特种机器人以及机器人智能装备在有限时间内完成规定操作任务需求，需要研究机器人的快速、精确、有限时间控制问题^[4-6]，具体来讲主要分为点位控制和轨迹跟踪控制.点位控制要求机器人的末端执行机构从当前状态准确移动到指定状态；轨迹跟踪控制要求机器人的末端执行机构严格按照目标轨迹运动.特别是机器人的轨迹跟踪控制问题，国内外学者进行了大量研究工作.Galicki^[7-8]针对SCARA类型工业机器人操作臂的跟踪控制问题，采用终端滑模控制方法设计有限时间收敛的跟踪控制器. Kim等^[9]和Korayem等^[10]分别针对带有旋翼的空间机器人以及绳系机器人，采用状态相关Riccati方程方法设计轨迹跟踪控制器.Scaglia等^[11]针对地面移动机器人的轨迹跟踪问题，设计了基于线性差值的轨迹跟踪控制器.进一步，陈罡等^[12]针对地面移动机器人模型参数存在未知、有界干扰情况，推出一个鲁棒自适应轨迹跟踪控制方法.杨俊友等^[13]针对康复训练机器人，通过反推控制算法设计了轨迹跟踪控制器，等等. 上述针对工业机器人、地面移动机器人以及空间机器人的轨迹跟踪控制问题等，主要采用动力学基本原理建立特定机器人的动力学模型，且数学描述以常微分方程(ordinary differential equations,ODEs)为主.所以对于不同机器人需要找到完全独立坐标描述方式并采用复杂的符号推导运算建立动力学模型，而对于更加复杂的机器人系统存在建模困难，各种针对特定机器人建立的轨迹跟踪控制方法难以推广.

事实上，从动力学分析角度来讲，机器人的本质就是一个多体系统，且采用多体系统动力学建模方法能够对各种机器人进行统一建模^[14-15]. 特别是采用绝对坐标描述方法对机器人进行多体动力学建模将得到微分代数方程(differential algebraic equations,DAEs). DAEs的求解相对ODEs要困难得多，目前不断有新的数值方法提出并用于求解DAEs^[16-17].因此，基于DAEs进行多体系统的轨迹跟踪控制仍是一个难点，例如：Antos 等^[18]为了避免直接基于DAEs进行轨迹跟踪控制的困难，将多体系统的DAEs进行降阶与简化并得到相应的ODEs，然后基于ODEs进行轨迹跟踪LQR控制器设计，最后通过DAEs来验证轨迹跟踪控制效果.Liu等^[19]采用绝对节点坐标方法建立刚柔耦合多体系统动力学模型，采用经典PID控制方法实现对铰的运动轨迹跟踪.Bottasso等^[20] 和Buskens 等^[21]将连续受控DAEs在时域内进行离散，并将其转化为每一个时间步内的非线性参数优化问题，进而实现多体系统的轨迹跟踪控制目的，不过大量的在线非线性参数优化仍是一个不小的计算负担.

本文主要针对基于DAEs进行轨迹跟踪控制存在的困难，开展多体系统轨迹跟踪控制的保辛数值方法研究工作. 本课题组在前期研究工作中，分别针对不受控DAEs提出保辛数值方法^[22-23]，以及基于ODEs提出非线性轨迹优化的保辛数值方法^[24-25]，在这些方面进行了大量研究工作^[26]. 因此，本文采用瞬时最优控制理论实现对目标轨迹的跟踪，进一步在最优控制输入的计算过程中，采用保辛数值方法对DAEs进行求解，最后通过数值仿真验证本文瞬时最优控制保辛方法在多体系统轨迹跟踪方面的有效性.

1 多体系统微分代数方程的保辛方法 1.1 多体系统动力学模型

对于多体动力学问题，采用绝对坐标方法建模将获得多体动力学系统的微分代数方程组^[27-28].考虑有n个广义自由度，s个完整约束的多体系统，则其DAEs方程通常表达如下

$M\ddot{q}(t)+\Phi _{q}^{\text{T}}(q,t)\lambda ={{F}_{w}}(q,\dot{q},t)$

(1)

$\Phi (q,t)=\mathbf{0}$

(2)

其中，M为$n\times n$维的系统广义质量矩阵，${ \varPhi} ({q},t)$为系统约束方程，${ q}(t)$,$\ddot{ q}(t)$和${ F}_w ({ q},\dot { q},t)$依次为$n\times 1$维的系统广义坐标矢量、广义加速度矢量和广义外力，λ为$s\times 1$维的Lagrange乘子矢量，${ \varPhi} _{ q} $为$s\times n$维的约束雅可比矩阵.

1.2 微分代数方程的保辛方法

针对上述微分代数方程的数值求解，本文采用保辛方法^[26]，在数值求解微分代数方程的过程中不仅保辛而且保持能量守恒.

首先将二阶动力学微分方程转化到状态空间并采用一阶常微分方程描述，令

$z={{({{q}^{\text{T}}},{{p}^{\text{T}}})}^{\text{T}}}$

(3)

其中

$p=M\dot{q}$

(4)

则可以将二阶微分方程(1)转化为如下与之等效的状态方程

$\left[ \begin{array}{*{35}{l}} {\dot{q}} \\ {\dot{p}} \\ \end{array} \right]=\left[ \begin{matrix} {{M}^{-1}}p \\ -\Phi _{q}^{\text{T}}\lambda +{{F}_{w}} \\ \end{matrix} \right]$

(5)

可简记为

$\dot{z}=f(z,\lambda )$

(6)

然后，将连续时间坐标离散并得到一系列的离散时刻点$t_0 ,t_1 ,\cdots,t_k ,t_{k + 1},\cdots$，根据保辛方法，假定在任意积分区间$(t_k ,t_{k + 1} )$内，将Lagrange乘子矢量λ视为参数变量，其在该时间段$(t_k ,t_{k + 1} )$内恒定不变，均记为${ \lambda}_k $.进一步采用欧拉中点差分格式，一阶微分方程(6)可以被离散为

${{z}_{k+1}}={{z}_{k}}+h\cdot f(\frac{{{z}_{k}}+{{z}_{k+1}}}{2},{{\lambda }_{k}})$

(7)

其中，h为时间积分步长，${ z}_k $和${ z}_{k + 1} $分别为t_k和t_{k + 1}时刻的状态向量.

再将式(4)与式(5)代入上述离散格式(7)，可得如下离散形式的状态方程

$\left[ \begin{matrix} \frac{{{q}_{k+1}}-{{q}_{k}}}{h} \\ \frac{M{{{\dot{q}}}_{k+1}}-M{{{\dot{q}}}_{k}}}{h} \\ \end{matrix} \right]=\left[ \begin{matrix} \frac{{{{\dot{q}}}_{k+1}}+{{{\dot{q}}}_{k}}}{2} \\ \text{ }{{\Psi }_{1}}+{{\Psi }_{2}} \\ \end{matrix} \right]$

(8)

其中

$\left. \begin{align} & {{\Psi }_{1}}=-\Psi _{q}^{\text{T}}(\frac{{{q}_{k+1}}+{{q}_{k}}}{2})\cdot {{\lambda }_{k}} \\ & {{\Psi }_{2}}={{F}_{w}}(\frac{{{q}_{k+1}}+{{q}_{k}}}{2},\frac{{{q}_{k+1}}-{{q}_{k}}}{h},{{t}_{k+1}}) \\ \end{align} \right\}$

(9)

另外，根据保辛方法的思想，使约束方程在每个离散时间节点上严格满足，得到约束方程的离散形式如下

$\Psi ({{q}_{k+1}},{{t}_{k+1}})=\mathbf{0}$

(10)

将式(8)和式(10)展开改写为如下标准非线性方程组

其中

$f={{\left( f_{1}^{\text{T}},\ f_{2}^{\text{T}},\ f_{3}^{\text{T}} \right)}^{\text{T}}}=\mathbf{0}$

(12)

$\begin{align} & {{f}_{2}}=M({{{\dot{q}}}_{k+1}}-{{{\dot{q}}}_{k}})- \\ & h{{F}_{w}}(\frac{{{q}_{k+1}}+{{q}_{k}}}{2},\frac{{{q}_{k+1}}-{{q}_{k}}}{h},{{t}_{k+1}})+ \\ & h\Psi _{q}^{\text{T}}(\frac{{{q}_{k+1}}+{{q}_{k}}}{2})\cdot {{\lambda }_{k}} \\ \end{align}$

(13)

${{f}_{\text{3}}}\text{ =}\Psi \left( {{q}_{k+1}},{{t}_{k+1}} \right)$

(14)

上述非线性方程组(11)即为不受控多体系统动力学DAEs方程的离散格式. 显然，方程(11)为一组关于${q}_{k + 1} ,{ \lambda}_k $和$\dot{ q}_{k + 1} $共2n +s个未知数的非线性代数方程，对于该非线性代数方程组，通常采用已经成熟的Newton-Raphson迭代方法进行求解.至此，采用保辛方法已经完成不受控多体系统DAEs方程的数值求解.本文将进一步基于上述保辛方法提出多体动力学系统轨迹跟踪问题的瞬时最优控制.

2 多体动力系统轨迹跟踪的瞬时最优控制 2.1 瞬时最优控制理论

瞬时最优控制算法(instantaneous optimal control,IOC)^[29-30]，属于最优控制算法之一，其算法思想是在每个瞬时构造目标函数，在状态方程与约束方程的约束下，求目标函数的极小值，故其数学模型为

$\left. \begin{array}{*{35}{l}} \text{find} & {{u}_{\text{c}}}(t) \\ \text{min}. & J={{z}^{\text{T}}}(t)Qz(t)+u_{\text{c}}^{\text{T}}(t)R{{u}_{\text{c}}}(t) \\ \text{s}.\text{t}. & \dot{z}=f(z,{{u}_{\text{c}}}) \\ \end{array} \right\}$

(15)

其中，${ u}_{\rm c} (t)$为控制向量，${ z}(t)$为状态向量，$J$为瞬时二次性能指标，Q为半正定权矩阵，R为正定权矩阵.本文将基于瞬时最优控制理论，并联合微分代数方程的保辛方法提出多体动力学轨迹跟踪控制方法.

2.2 基于微分代数方程保辛方法的瞬时最优控制

为实现多体动力学系统对轨迹的跟踪控制，考虑主动控制作用，并将多体系统的动力学方程转化为受控微分代数方程如下

$M\ddot{q}(t)+\Psi _{q}^{\text{T}}(q,t)\lambda ={{F}_{w}}(q,\dot{q},t)+Bu(t)$

(16)

其中，B为$n\times m$维控制位置矩阵，${ u}(t)$为$m\times1$维的控制向量. 同样，在每个积分区间$(t_k ,t_{k + 1})$内，假定控制输入为常值${ u}_{k + 1}$，则该受控微分动力学系统经过保辛方法离散后为

$\hat{f}=f+\left[ \begin{matrix} \mathbf{0} \\ -hB{{u}_{k+1}} \\ \mathbf{0} \\ \end{matrix} \right]=\mathbf{0}$

(17)

对于非线性代数方程组(17)仍然可以采用Newton-Raphson迭代方法求解.

首先，引入连续受控系统的状态变量

$x={{\left( {{q}^{\text{T}}},\ {{\lambda }^{\text{T}}},\ {{{\dot{q}}}^{\text{T}}} \right)}^{\text{T}}}$

(18)

则，在离散t_k和t_{k + 1}时刻有

$\left. \begin{align} & {{x}_{k}}={{\left( q_{k}^{\text{T}},\ \lambda _{k}^{\text{T}},\ \dot{q}_{k}^{\text{T}} \right)}^{\text{T}}} \\ & {{x}_{k+1}}={{\left( q_{k+1}^{\text{T}},\lambda _{k}^{\text{T}},\ \dot{q}_{k+1}^{\text{T}} \right)}^{\text{T}}} \\ \end{align} \right\}$

(19)

从而非线性代数方程组(17)的Newton-Raphson迭代格式为

${{x}_{k+1}}={{x}_{k}}-\hat{f}_{x}^{-1}({{x}_{k}})\cdot \hat{f}({{x}_{k}})$

(20)

进一步将非线性代数方程组(20)展开可得

$\begin{align} & {{x}_{k+1}}={{x}_{k}}-f_{x}^{-1}({{x}_{k}})\cdot f({{x}_{k}})- \\ & f_{x}^{-1}({{x}_{k}})\cdot \left[ \begin{matrix} \mathbf{0} \\ -hB{{u}_{k+1}} \\ \mathbf{0} \\ \end{matrix} \right] \\ \end{align}$

(21)

至此，方程(21)中状态变量已经被转化为关于控制输入的显式函数.

然后，引入多体系统动力学的目标跟踪轨迹，并假定被跟踪的目标轨迹函数具有如下表达式

$\tilde{y}(t)=G(t)$

(22)

其中，${ G}(t)$ 为被跟踪的已知目标轨迹函数.

而对上述受控多体动力学系统，给出瞬时最优控制二次型性能指标为

$J={{({{y}_{k+1}}-{{\tilde{y}}_{k+1}})}^{\text{T}}}Q({{y}_{k+1}}-{{\tilde{y}}_{k+1}})+u_{k+1}^{\text{T}}R{{u}_{k+1}}$

(23)

其中，y_{k + 1}为受控多体动力学系统在t_{k + 1}时刻的实际输出量，$\tilde{ y}_{k + 1} $为被跟踪目标轨迹在t_{k + 1}时刻的数值. 并且受控多体动力学系统在t_{k + 1}时刻的实际输出y_{k + 1}可根据多体动力学系统的真实状态获得，即

${{y}_{k+1}}=\hat{C}{{x}_{k+1}}$

(24)

其中，$\hat{ C}$为$r\times n$维输出矩阵，r为控制输出变量的个数. x_{k + 1}可根据Newton-Raphson迭代格式(21)获得. 此时瞬时最优控制二次型性能指标(23)只是关于未知控制变量u_{k + 1}的函数，因此使性能指标取极小值可得

$\frac{\partial J}{\partial {{u}_{k+1}}}=\mathbf{0}$

(25)

由此可得瞬时最优控制输入为

${{u}_{k+1}}={{(\zeta _{2}^{\text{T}}{{\hat{C}}^{\text{T}}}Q\hat{C}{{\zeta }_{2}}+R)}^{-1}}\zeta _{2}^{\text{T}}{{\hat{C}}^{\text{T}}}Q(\hat{C}{{\zeta }_{1}}-{{\tilde{y}}_{k+1}})$

(26)

其中，变量${ \zeta}_1 $和${ \zeta}_2 $依次为

${{\zeta }_{1}}={{x}_{k}}-f_{x}^{-1}({{x}_{k}})\cdot f({{x}_{k}})$

(27)

${{\zeta }_{2}}=-h\Gamma B$

(28)

式(28)中，Γ为$(2n + s)\times n$维矩阵，取自矩阵${ f}_x^{ - 1} (x_k )$的第n + 1至2n列.

最后，将Newton-Raphson前后两次迭代的状态变量 x的相对误差作为收敛判据，完成在每个时间步$(t_k,t_{k + 1} )$内的轨迹跟踪控制求解，即

$\left\| {{x}_{k+1}}-{{x}_{k}} \right\|/\left\| {{x}_{k+1}} \right\|<\varepsilon $

(29)

其中，ε为收敛误差.

至此，将多体动力学系统的轨迹跟踪瞬时最优控制算法总结如下：

(1) 形成多体动力学系统的受控微分代数方程DAEs(式(16))，并设定被跟踪的目标轨迹函数$\tilde{ y}(t)$(式(22));

(2) 采用保辛方法,将DAEs方程在连续时间坐标上进行离散，得到非线性代数方程组(式(21))；

(3) 将t_k时刻的状态量x_k作为Newton-Raphson方法迭代的初值，代入式(26)获得瞬时最优控制u_{k + 1}，再将瞬时最优控制u_{k + 1}代入式(21)，更新当前状态变量x_k;

(4) 将x_k和x_k代入收敛判据公式(29)，若收敛，则进入下一时间步，若不收敛，则重复步骤(3)，直至收敛.

3 数值算例与讨论

如图 1所示，两长度均为L=1 m，质量均为m =2 kg的均质杆组成的平面双摆. 杆A左端通过铰接方式固定，其右端与杆B的左端铰接，而杆B的右端自由. 双摆初始状态竖直悬挂，在初始角速度$\dot {\theta }_a = 20$ rad/s,$\dot{\theta }_b = - 20$ rad/s作用下，双摆在平面内自由摆动(取重力加速度$g_r = 9.806 65$ m/s$^2$). 积分步长为h= 0.001，仿真时间为30 s. 本文针对上述双摆问题，分别进行无控与受控两种情况研究.

(1) 在无控情况下，讨论保辛方法的有效性. 图 2、图 3和图 4依次为杆A质心的运动轨迹，杆A位移约束相对误差和系统能量相对误差.

根据模型图 1可知，杆A质心的轨迹应在半径为0.5 m的圆弧上，图 2的求解结果与实际运动情况一致，说明本文算法求解是正确的. 而图 3给出的位移约束相对误差量级为10 ^{- 16}，很好地满足了约束条件，说明该算法求解的精度高.进一步，图 4给出双摆系统不受控情况下，系统能量相对误差量级为10^{- 5}，这再次说明该算法具有保能量特性.因此，在无控情况下，保辛算法具有既保约束又保能量优点.

(2) 在有控情况下，验证瞬时最优控制的有效性. 在两铰处实施主动控制，并分别设计主动控制扭矩M₁,M₂ 使杆B质心绕以(0,1.5)为圆心，半径为0.5 m的圆，作周期$T = 2\pi $的圆周运动，其目标轨迹函数$\tilde{y}$如下

$\left. \begin{array}{*{35}{l}} \tilde{x}=0.5\sin t \\ \tilde{y}=1+0.5\cos t \\ \end{array} \right\}$

(30)

图 5为杆B质心在有、无控制情况的轨迹对比，图 6(a)与图 6(b)依次为图 5在第一个周期$(0\tilde{\ }T)$和第二个周期(T, 2T)跟踪目标轨迹(图 4(b)区域 A)的局部放大图.

根据图 5可知，在无控制情况下，杆B质心在做杂乱无章运动，其轨迹与目标轨迹并无关联.但在受控情况下，由图 6(a)知，在第一个周期内，受控轨迹偏离目标轨迹小于1%.由图 6(b)知，在受控轨迹基本稳定后，即在第二个周期，受控轨迹偏离目标轨迹小于0.1%.随时间增加，受控轨迹将严格绕指定的有向目标轨迹做周期运动，这充分说明瞬时最优控制在轨迹跟踪方面能够获得满意效果.

进一步，为了更清楚地表现杆B质心在受控情况下的周期往复运动，图 7(a)和图 7(b)依次给出杆B质心的x坐标和y坐标随时间的变化曲线.

根据图 7(a)和图 7(b)可知，无控情况下，杆B质心水平方向和竖直方向的轨迹均呈现杂乱无章运动.但在受控情况下，杆B质心水平方向轨迹坐标始终保持在区间[-0.5,0.5]内，而竖直方向轨迹坐标始终保持在区间[0.5,1.5]内，两者随时间的推移都在做周期运动，且稳定后始终与目标轨迹重合.

最后，控制力矩M₁ ,M₂ 随时间变化分别如图 8(a)和图 8(b)所示.根据图 8(a)和图 8(b)可知，在起始较短时间内，需要相对较大的控制力迫使杆B质心跟踪目标轨迹运动，受控系统稳定后则跟踪目标轨迹做有向周期往复运动所需要的控制力矩相对较小，其中控制力矩M₁大约在区间 [- 39.3 N⋅m,28.4 N⋅m] 内，而控制力矩M₂则大约在区间[-10.0 N⋅m,12.6 N⋅m] 内.另外，当受控双摆轨迹跟踪控制稳定后，控制力矩也呈现出明显的周期性变化.

4 结论

本文针对机器人轨迹跟踪控制需求，基于微分代数方程开展瞬时最优轨迹跟踪控制研究. 本文所提出的瞬时最优控制保辛方法，将连续时间域离散化并将多体系统的微分代数方程转化为非线性代数方程组. 在此基础上，进一步基于瞬时最优控制理论，有效地实现了多体系统对目标轨迹的跟踪控制. 特别是，本文所提出的轨迹跟踪控制方法建立在一般微分代数方程基础上，因而适用于更为复杂的机器人多体系统轨迹跟踪控制，从而能够有助于解决机械领域、航天领域和机器人领域等众多实际工程问题.