柔性关节机械臂最优控制方法研究

崔士鹏， 孙永军， 刘伊威， 刘宏

(哈尔滨工业大学 机器人技术与系统国家重点实验室，哈尔滨 150001)

0 引 言

随着机器人应用在智能制造、医疗康复、助老助残服务、教育娱乐等领域的不断扩展，以实现人机共融为设计目标的柔性关节机械臂近年来备受世界各国研究人员的关注。目前，柔性关节机械臂主要包括2类：基于串联弹性驱动器(series elastic actuator,SEA)的柔性关节机械臂和基于可变刚度驱动器(variable stiffness actuator,VSA)的柔性关节机械臂。针对柔性关节机械臂的位置控制、轨迹跟踪控制和阻抗/导纳控制，学者们已经开展了大量的研究工作。但这些传统的控制方法无法利用柔性关节机械臂的固有柔顺性使机械臂的运动性能达到最佳。此外，对于每个关节具有多个驱动单元的VSA机械臂来说，传统的控制方法更是无法确定如何进行驱动单元的控制分配以实现机械臂在执行任务时表现出最优的位置、力或阻抗特性。以目标函数为驱动的最优控制方法为解决上述问题提供了基本思路。

Özparpucu M.C[1]、Chen L[2]等人由极大值原理计算出使SEA在固定时间点处达到最大连杆速度的电机端控制力矩的解析解；针对VSA的速度最优[3-4]、能耗最优[5]控制问题，Garabini M、Haddadin S等人通过打靶法得到了原最优控制问题的数值近似解；张世杰[6]运用多重打靶法对高速重载码垛机器人能耗最优轨迹规划及运动控制问题进行数值求解，得到了机器人能耗最优轨迹；Patel A[7]通过正交配置法将机械臂的最优轨迹规划问题转化为非线性规划(NLP)问题，以便利用成熟的NLP算法进行求解；胡友忠[8]提出了一种给定运动时间的机械臂能量最优轨迹规划方法，通过动态规划算法在时间轴上进行搜索得到了能量最优轨迹；针对存在耦合交联不确定性的可重构机械臂系统分散控制问题，刘富[9]等人提出一种基于自适应动态规划的能耗保代价最优控制方法；Pham Q[10]等人采用基于极大值原理的数值积分法，解决了存在驱动饱和约束下的冗余机器人时间最优轨迹规划问题；此外，Kim J[11]、Machmudah A[12]等人基于粒子群优化算法研究了机械臂的最优轨迹规划问题。

分析上述国内外研究现状可以发现，现有关于机器人最优控制方法的研究主要是围绕刚性机器人或柔性关节展开的，关于柔性关节机械臂最优控制方法的研究成果较少，柔性关节机械臂通常会受到控制量、状态量，甚至是控制量/状态量复杂耦合形式下的不等式约束的限制。但目前关于机器人最优控制方法的研究大多不考虑不等式约束或仅考虑状态量不等式约束，针对柔性关节机械臂所受约束的有效处理方法以及可处理任意不等式约束的机器人最优控制方法的相关研究基本没有。本文将可有效处理约束的障碍函数法从数学规划领域引入到最优控制领域，提出可处理具有任意不等式约束最优控制问题的改进型ILQR算法；设计柔性关节机械臂所受约束的有效处理方案，将柔性关节机械臂最优控制系统的软约束转化为控制量上下界约束和系统硬约束，减少系统软约束的数量，建立易于改进型ILQR算法求解的数学模型。通过仿真实验验证所提出的柔性关节机械臂约束处理方案和改进型ILQR算法的可行性和有效性。

1 最优控制问题描述

据柔性关节机械臂的实际情况，本文仅考虑结束时间T固定、带不等式约束的最优控制问题，其数学描述如下：

(1)

(2)

gj(x(t),u(t))≤0,j=1,2,…,G；

(3)

x(0)=x0；

(4)

0≤t≤T。

(5)

其中，x(t)∈Rn为状态向量，u(t)∈Rm为控制向量，被控对象由状态方程(2)描述，其初始状态为x0。J是最优控制的目标函数，其中φ和c分别为目标函数的终值项和积分项。值得注意的是，系统受到的不等式约束由式(3)表示，它既可以表示关于控制量的不等式约束，又可以表示关于状态量的不等式约束，还可以表示它们的复杂组合，j为不等式约束的数量。

最优控制问题式(1)～ 式(5)可描述为：在系统初始状态式(4)下，从满足约束条件式(2)和式(3)的可行控制集中找到最优控制量u*(t)，使目标函数式(1)达到最小。

2 改进型ILQR算法研究

2.1 基于障碍函数法的改进型ILQR算法

由于具有高阶数、多维度、强耦合、非线性等特性，柔性关节机械臂的最优控制方法研究一直是机器人技术领域的一个难点。由Li和Todorov[13-14]提出的ILQR算法可有效处理复杂非线性系统的最优控制问题。作为微分动态规划(differential dynamic programming，DDP)算法的简化形式，ILQR算法的计算效率更高，并已经成功应用于VSA机械臂的最优控制中[15-16]。但只能处理无约束最优控制问题或仅带控制量上下界约束最优控制问题的不足，限制了ILQR算法的应用范围。

为解决带任意不等式约束的最优控制问题，本节将可有效处理约束的障碍函数法同ILQR算法相结合，提出了可处理任意不等式约束最优控制问题的改进型ILQR算法。该算法主要由内环迭代和外环迭代2个部分组成，其伪代码如下：

result:最优控制量u*和对应的最优状态量x*

repeat://外环迭代

通过式(6)构建障碍函数并得到增广目标函数(7)

repeat://内环迭代

ILQR算法求解无约束最优控制序列u(i)

untilconvergence

u(i+1)←u(i)

untilconvergence

(6)

将状态量不等式约束和控制量/状态量耦合不等式约束作为积分形式的障碍项，增广到原最优控制问题的目标函数(1)中，得到增广目标函数，即

(7)

2.2 收敛性证明

为方便证明改进型ILQR算法的收敛性，首先将式(1)描述的最优控制问题转化为如下离散时间最优化问题1，即：

(8)

定理1 采用改进型ILQR算法求解问题1，状态量轨迹和控制量轨迹{x,u}将收敛于局部最优解{x*,u*}，即满足如下KKT条件：

(9)

(10)

(11)

(12)

Step1：

由2.1节的内容可知，改进型ILQR算法中的外环迭代通过障碍函数法将不等式约束添加至目标函数中，则问题1转化为如下形式，即：

(13)

式(13)可改写为无约束最优化问题2，即：

(14)

Step2：

定义如下Hamiltonian函数，即

(15)

则问题2的Lagrangian函数可表示为

(16)

根据式(16)的Lagrangian函数的微分，即

(17)

通过将式(17)中各项系数设置为零，可得到问题2的KKT条件：

(18)

由2.1节的内容可知，改进型ILQR算法通过内环迭代求解问题2。由于ILQR算法是DDP算法的简化形式，因此，ILQR算法计算的最优解满足Pontryagin极大值原理，即：

(19)

Step3：

由问题2的平稳性条件可得到

(20)

(21)

(22)

(23)

(24)

通过比较可以发现，除了互补松弛条件(24)以外，式(21)～式(24)和问题1的KKT条件完全相同。因此，式(21)～式(24)可视为问题1的变形KKT条件。由2.1节的内容可知，bj随着改进型ILQR算法的外环迭代逐渐增大至无穷，式(24)可表示为

(25)

则问题2的KKT条件最终转变为问题1的KKT条件。

综上所述，改进型ILQR算法通过障碍项将问题1转化为问题2；问题2由内环ILQR算法求解，且求解的状态量和控制量满足KKT条件；问题2的KKT条件是问题1的KKT条件的变形形式，且随着改进型ILQR算法的外环迭代转变为问题1的KKT条件。因此，改进型ILQR算法求解的最优状态量和控制量满足问题1的KKT条件，即改进型ILQR算法处理问题1时收敛。

3 柔性关节机械臂约束处理方法研究

3.1 柔性关节机械臂动力学模型建立

柔性关节机械臂动力学方程可表示为：

(26)

当柔性关节机械臂为SEA机械臂时，m=n；当柔性关节机械臂为VSA机械臂时，m=2n。根据刚度定义，柔性关节机械臂的刚度可以表示为

(27)

通常柔性关节机械臂会受到2类不等式约束的限制，分别为电机端运动轨迹约束和弹性元件变形约束，具体如下：

1)电机端运动轨迹约束。电机端运动轨迹会受到运动范围和运动速度的限制，其容许集Θ可表示为

(28)

2)弹性元件变形约束。由于弹性元件只能在一定范围内变形，柔性关节机械臂还会受到关于弹性元件变形的不等式约束的限制。弹性元件变形容许集Φ可表示为

Φ={φ∈Rm:φm(θ)≤φ(q,θ)≤φM(θ)}。

(29)

其中，φ为弹性元件变形量，如：弹性元件长度；φM、φm分别为弹性元件变形量的上下界，它们同电机端状态量有关。

3.2 电机端闭环控制及约束处理方法

式(26)所表达的电机端动力学模型可改写为

(30)

采用PD控制对电机端位置进行控制，即

(31)

(32)

其中：

风影一直离群索居，几乎不与村民们来往，独来独往的他看上去总是一副心事重重的样子。红琴的面孔倒是逐渐丰润起来，那种知足常乐的样子，乍一看根本看不出她过去有什么伤痛的经历与未来有什么美好的梦想。有时候，他会朝山坡上那个迷蒙的地方看，有时候风和日丽，有时候云雾缭绕，他一看就是大半天。红琴见了就开玩笑，和尚，你的灵魂是不是还丢在那间破庙里，如果你还想出家去当和尚，那就尽管去，这回你要是想受戒，就尽管让那老和尚拿香火烫，我绝不拦你。风影有些恼火，但也没有发作，只在心底里骂了一声鬼女子，便悄无声息地走开了。

(33)

β2=B-1kp；

(34)

(35)

(36)

(37)

(38)

通过电机端PD控制，实现了柔性关节机械臂系统控制输入由电机端力矩到电机端期望位置的转变，并且电机端位置θ可无超调地跟踪控制输入θd。

由以上分析可以看出，采用本节所述电机端闭环控制方法，可将电机端运动轨迹约束转化为控制量上下界约束，并通过β嵌入到电机端动力学模型当中，从而达到减少系统约束数量的目的，方便应用改进型ILQR算法求解。

3.3 柔性关节机械臂最优控制问题求解

定义向量

(39)

其中：x∈R2(n+m)为柔性关节机械臂的状态向量；u∈Rm为柔性关节机械臂的控制向量。则柔性关节机械臂可由状态方程表示为

(40)

由3.2节的内容可知，电机端运动轨迹约束可通过对角矩阵β和控制量约束θm≤u≤θM嵌入到柔性关节机械臂电机端动力学模型中。此外，有些柔性关节机械臂还会受到弹性元件变形不等式约束的限制，弹性元件变形不等式约束采用状态向量可表示为φm(x3)≤φ(x1,x3)≤φM(x3)。

4 仿真实验

4.1 仿真设计

图1 柔性关节机械臂结构简图Fig.1 Schematic diagram of flexible joint manipulator

图2 掷球任务简图Fig.2 Schematic diagram of throwing task

(41)

机械臂掷球任务的目标函数可表示为

(42)

(43)

4.2 仿真结果

1) 由MACCEPA构成的二连杆机械臂。

MACCEPA二连杆机械臂和MACCEPA样机分别如图3(a)和图3(b)所示。

机械臂的电机端位置向量可表示为θ=[θ1,θ2,θ3,θ4]T，由MACCEPA结构原理图3(c)可知：MACCEPA的主电机控制电机端位置θ1和θ2；刚度调节电机控制线性弹簧的预紧，对应的电机端位置为θ3和θ4；连杆端弹性力矩向量τq和二连杆机械臂刚度矩阵K可表示为：

图3 可变刚度柔性关节机械臂Fig.3 Variable stiffness joint manipulator

(44)

其中ki为线性弹簧的刚度。式(44)中其它参数具有如下关系式：

(45)

式中：lsi、l0i分别为拉伸后的线性弹簧长度和弹簧原长；Bi、Ci和ri为MACCEPA的几何尺寸，MACCEPA二连杆机械臂模型参数见表1。

表1 MACCEPA二连杆机械臂模型参数

采用改进型ILQR算法求解使目标函数(42)最小，即二连杆机械臂掷球最远的机械臂各状态量轨迹。此外，通过采用将刚度调节电机控制量设置为常值(u=[u1(t),u2(t),u3,u4]T)的对比实验来研究主动刚度调节在掷球任务中的作用。此时，二连杆机械臂无法主动调节关节刚度，VSA机械臂退化为SEA关节机械臂。2种情况的仿真结果如图4所示。

由频闪观测图4(a)和图4(b)可知，VSA机械臂和SEA机械臂掷球的距离分别为9.69 m和7.67 m；无论是VSA机械臂和SEA机械臂，在掷球的过程中都有一个明显的回摆动作。值得注意的是，人类在进行诸如击打、跳跃等爆发性运动时也会采用类似的“收缩-伸展循环”策略。因此，本文提出的最优控制方法在柔性关节机械臂掷球任务中的数值预测符合生物运动规律。

图4 MACCEPA二连杆机械臂最优控制仿真结果Fig.4 Simulation results of optimal control for MACCEPA two-link manipulator

VSA机械臂和SEA机械臂位置信息和关节刚度变化情况分别由图4(c)至图4(h)中的黑色曲线和蓝色曲线表示。从图4中可以看出，在机械臂投掷小球的过程中，电机端运动轨迹始终满足约束条件。由图4(e)和图4(f)可知，VSA机械臂的刚度调节电机位置可由最优控制进行周期性调节，而SEA机械臂电机位置不受最优控制调节。因此，如图4(g)和4(h)所示，VSA机械臂可在刚度调节范围内主动调节关节刚度，而SEA机械臂的关节刚度始终处于刚度边界值。由于可主动调节刚度，VSA机械臂的连杆端可同电机端有效“解耦”，在更大的范围内更加自由地运动。这使得VSA机械臂较SEA机械臂具有更佳的任务表现，即掷球距离更远。

相比于传统的机械臂，柔性关节机械臂可利用弹性元件储存并释放能量来提高机械臂的运动性能。接下来，将从功率和能量的角度进一步解释MACCEPA二连杆机械臂是如何利用固有柔顺性来实现小球更远的投掷。在柔性关节机械臂执行投掷任务的过程中，电机的输入功率为

(46)

(47)

MACCEPA二连杆机械臂的能量流如图5所示。

图5 MACCEPA二连杆机械臂的能量流Fig.5 Power flow of MACCEPA two-link manipulator

从图5(a)至图5(d)可以看出，无论是VSA机械臂还是SEA机械臂，在改进型ILQR算法的控制下，机械臂都处于功率放大和能量储存模式的能量流循环之中，且在机械臂释放小球前，存在显著的输出功率放大过程。VSA机械臂在执行掷球任务时，不仅可由主电机输入能量，还可通过刚度调节电机在调节刚度的过程中输入部分能量。这使得VSA机械臂的输出能量大于SEA机械臂的输出能量(如图5(f)所示)，从而使VSA机械臂释放的小球的速度大于SEA机械臂释放的小球的速度(如图5(e)所示)，最终导致VSA机械臂掷球的距离更远。由于传统机械臂没有弹性元件，机械臂的输入功率同输出功率相等，即pin=pout，因此传统机械臂无法实现功率放大，也就无法像柔性关节机械臂一样利用固有柔顺性来提高机械臂的运动性能。

2)由FSJ构成的二连杆机械臂。

FSJ样机及其模型图如图6(a)和6(b)所示，FSJ二连杆机械臂电机端位置向量同样可表示为θ=[θ1,θ2,θ3,θ4]T。由文献[18]可知，FSJ二连杆机械臂连杆端弹性力矩向量τq可表示为

图6 FSJ关节Fig.6 DLR floating spring joint

(48)

其中：τmax=10N·m；τchar=12；φmax=15°。FSJ二连杆机械臂模型参数见表2。

表2 FSJ二连杆机械臂模型参数

采用改进型ILQR算法求解使FSJ二连杆机械臂掷球最远的机械臂各状态量轨迹，仿真结果如图7所示。由频闪观测图7(a)可知，VSA机械臂的最远掷球距离为9.42 m。根据图7(b)和6(c)，并结合刚度调节电机端位置图7(h)和7(i)可知，机械臂关节状态量满足弹性元件变形约束条件。主电机端运动轨迹如图7(d)至7(g)所示，可以看出，无论是位置曲线还是速度曲线都位于相应的约束范围内。以上仿真结果表明，改进型ILQR算法可处理具有电机端运动轨迹约束和弹性元件变形约束的柔性关节机械臂最优控制问题。

图7 FSJ二连杆机械臂最优控制仿真结果Fig.7 Simulation results of optimal control for FSJ two-link manipulator

5 结 论

本文将数学规划领域中的障碍函数法引入到ILQR算法当中，提出了可处理具有任意不等式约束最优控制问题的改进型ILQR算法，通过KKT条件系统地证明了算法的收敛性；选取合适的PD控制参数对电机端进行控制，将柔性关节机械臂电机端动力学模型转化为二阶临界阻尼系统，实现了电机端运动轨迹约束转化为控制量上下界约束，并通过二阶临界阻尼系统的系统参数嵌入到电机端动力学模型当中，从而达到减少系统软约束数量、方便应用改进型ILQR算法对系统最优控制问题进行求解的目的；通过MACCEPA二连杆机械臂仿真实验验证了本文提出的改进型ILQR算法的有效性，并通过能量流解释了柔性关节机械臂利用固有柔顺性达到最佳运动性能的机理。最后，通过FSJ二连杆机械臂掷球仿真实验验证了所提出的改进型ILQR算法的普遍适用性。

本文从理论上证明了所提出的最优控制方法的可行性，使用该控制方法对真实的柔性关节机械臂进行最优控制将作为日后工作的重点。