空间连续型机器人位姿与构型的能量整形控制

杨今朝，彭海军，周文雅，吴志刚

(1. 大连理工大学航空航天学院，大连 116024；2. 大连理工大学工程力学系，大连 116024；3. 大连理工大学工业装备结构分析国家重点实验室，大连 116024)

0 引 言

近年来，在轨组装和维护等航天任务日益增多。这些任务通常需要空间机器人的协作才能完成。目前，一些空间机器人系统已经成功用于在轨任务，例如：中国空间站机械臂、加拿大的“专用灵巧机械手”和美国的Robonaut 2机器人。这些机器人系统在复杂的空间任务中扮演了极其重要的角色。

空间机器人是一个高度耦合的动力学系统，机械臂运动会对基座位姿产生扰动。为了使空间机器人能够准确地完成预定任务，设计合理的控制器显得尤为重要。文献[4]针对目标抓捕任务，通过引入零反作用机动，消除了机械臂运动对基座姿态的扰动。并且设计了协调控制器，实现了对空间机器人的协调控制。为了处理基座与机械臂姿态耦合的问题，一种基于时延估计的无模型解耦控制方法被提出，实现了系统的协调运动。针对非合作目标的捕获，一种强化学习与比例微分控制相结合的控制方法被提出，实现了对基座姿态与机械臂运动的高精度控制。空间机器人捕获目标过程中，存在碰撞与冲击。文献[7]分析了碰撞对空间机器人的影响，提出一种控制力矩能量消耗少且对基座扰动小的镇定控制方法，实现了对目标捕获后组合体系统的镇定控制。为避免空间机器人的关节受到冲击而被破坏，一种弹簧阻尼装置被设计出来，在此基础上，通过自适应积分强化学习控制方法，实现了对失稳系统的镇定控制。由于碰撞可能造成设备的损坏，因此在捕获目标时，应实施柔顺性操作。文献[9]提出一种阻抗控制方法，在系统模型不确定和没有力传感器的情况下，实现了空间机器人与目标的柔性接触。此外，针对空间机器人轨迹规划和消旋控制等问题，已经开展了大量研究。

然而，上述研究工作均是针对由关节与连杆组成的空间机器人，这类机器人在非结构化环境中执行任务时，很难完成柔顺性操作，无法完全发挥其性能。为了解决这一难题，空间连续型机器人被提出。这种空间机器人由基座和连续型机械臂等部件组成。其中，连续型机械臂由柔性支撑梁、圆盘和连接器组成，在驱动线的作用下，可以发生连续的弯曲变形。因此，其可以通过狭小的空间执行勘测等任务，也可以通过缠绕的方式捕获不同形状尺寸的目标。与柔性关节空间机器人相比，空间连续型机器人的动力学模型维数更高、非线性更强、耦合度更高，这为控制器设计带来巨大挑战，引起了广泛关注。

目前，许多学者针对地面上的连续型机械臂控制问题开展了研究。文献[16-17]建立了闭环控制器，分别针对连续型机器人和软体机器人实现了机械臂末端的轨迹跟踪。为了减少跟踪控制中传感器的数量，一种基于自适应卡尔曼滤波器的无模型方法被提出，实验结果表明此方法具有良好的鲁棒性。文献[19]针对连续型机械臂提出一种协调变阻抗控制方法，实现了在位置-力耦合约束下的稳定操作。文献[20]将连续型机械臂简化为刚性连杆模型，基于能量整形方法设计了控制器，并提出一种干扰补偿策略，通过实验验证了控制器的有效性。文献[21]针对线驱动超冗余度机械臂设计了控制器，实现了桁架检测任务。文献[22]提出一种基于视觉的机械臂形状控制方法，避免了三维坐标不可测时难于实现形状控制的问题。然而，与地面上的固定端连续型机械臂相比，空间连续型机器人中的连续型机械臂安装在漂浮基座上，基座与连续型机械臂的运动高度耦合，精确控制更加困难，目前仍未有对空间连续型机器人控制问题的研究。

本文针对空间连续型机器人位置和姿态机动以及连续型机械臂的变形控制，基于连接与阻尼配置-无源性控制(IDA-PBC)方法设计了控制器。通过能量整形和阻尼注入分别得到系统期望的Hamilton函数和期望的阻尼，以此设计了反馈控制律。利用非线性干扰观测器估计系统受到的外界干扰，根据干扰的估计值设计控制方案对系统进行干扰补偿。通过数值仿真，验证了所设计控制器的有效性。

1 动力学模型与控制问题描述

本节给出了空间连续型机器人的动力学模型，并对研究的控制问题进行了描述。

1.1 动力学模型

本文研究的空间连续型机器人由基座和连续型机械臂组成，如图1所示。其主要通过缠绕的方式执行空间任务，例如捕获目标等。采用四元数描述基座的姿态。连续型机械臂由柔性支撑梁、圆盘和连接器组成。将柔性支撑梁等效为Euler-Bernoulli梁，圆盘和连接器等效为刚体。Σ为惯性坐标系；Σ为基座固连坐标系，其原点与基座质心重合，坐标轴与基座的主惯性轴重合。

图1 空间连续型机器人

先前的工作建立了空间连续型机器人的动力学模型，其可表示为：

(1)

式中：为广义坐标，为质量阵，包含离心项和Coriolis项，为势能函数，为约束方程，为Lagrange乘子，是系统输入矩阵，是系统输入，是干扰输入矩阵，是外界干扰。其中，广义坐标可表示为：

(2)

(3)

式(1)中的矩阵为空间连续型机器人的质量阵，矩阵可表示为：

(4)

式(1)中的势能函数只包含连续型机械臂的弹性势能，可表示为：

(5)

式中：是柔性支撑梁的单元长度，和分别是柔性支撑梁的弹性模量和剪切模量，是柔性支撑梁的横截面积，=diag(,,)是柔性支撑梁的转动惯量矩阵，∈[0, 1]是归一化的弧长坐标，e,是柔性支撑梁上第个单元的正应变，,1,,2和,3分别是柔性支撑梁上第个单元的扭率和2个方向上的弯曲曲率。

式(1)中的输入矩阵可表示为：

(6)

式中：为×阶的连续型机械臂输入矩阵，为驱动线的个数，由于连续型机械臂是利用有限元方法进行建模，且由数量较少的驱动线进行控制，因此的列数一般远小于行数；为基座固连坐标系Σ与惯性坐标系Σ之间的坐标变换矩阵，可表示为：

(7)

(8)

此外，可表示为：

(9)

式(1)中的为系统输入，可表示为：

(10)

式中：是基座控制力，在基座固连坐标系中表示；是基座控制力矩，在基座固连坐标系中表示；是连续型机械臂上各个驱动线的控制力。

式(1)中的输入矩阵可表示为：

(11)

式(1)中的为系统受到的干扰，假设干扰只作用于基座，可表示为：

(12)

式中：和分别为基座受到的干扰力和干扰力矩，均在全局坐标系中表示。

1.2 控制问题描述

2 控制器设计

IDA-PBC方法可以保持原先Hamilton系统的结构，控制器参数有明确的物理意义，具有很大的应用潜力。本节利用IDA-PBC方法对空间连续型机器人进行控制器设计。

2.1 Hamilton形式的动力学模型

空间连续型机器人的Hamilton函数可表示为动能与势能之和：

(13)

将空间连续型机器人动力学模型(1)中的第1式转化为Hamilton形式：

(14)

为了使系统以期望的动态性能到达期望的平衡点，可将空间连续型机器人原先的Hamilton函数(13)配置为期望的Hamilton函数：

(15)

=

(16)

式中：是质量阵的整形系数矩阵，为正定矩阵：

=diag(,,)

(17)

式中：,和分别是基座位置、基座姿态和连续型机械臂的能量整形系数矩阵，均为对角阵。

此时，原先的动力学模型(14)被配置为期望的动力学模型：

(18)

式中：是期望的阻尼矩阵。

由式(16)可知，存在如下关系：

(19)

将式(19)代入式(18)可得：

(20)

利用IDA-PBC方法可实现将式(14)配置为式(20)的形式，实现这一目标主要分为3个步骤，分别为：能量整形、阻尼注入和干扰补偿。IDA-PBC方法的控制输入可表示为：

=++

(21)

式中：为能量整形控制输入;为阻尼注入控制输入;为干扰补偿控制输入。关于这3个控制输入的计算过程如下。

2.2 能量整形

在对系统进行能量整形时，暂时不考虑原先动力学模型(14)中的干扰项，即考虑如下方程：

(22)

在能量整形控制输入的作用下，式(22)被配置为：

(23)

根据式(22)和式(23)可得：

(24)

式(24)中期望的Hamilton函数包含期望的势能,可根据控制目标来选择。为了控制空间连续型机器人飞行到期望的位置并且保持期望的姿态，同时控制连续型机械臂保持期望的变形，可选择为：

(-,d)d

(25)

式中：和分别是基座期望的位置和姿态；,d是的期望值，=[e,,1,2,3]；和分别是基座位置和姿态的权重矩阵，均为对角阵；=diag(,,,)是材料参数矩阵。

由式(10)可知，控制输入分为三部分控制输入。相应地，能量整形控制输入也可分为如下三部分：

(26)

式中：,和分别是能量整形控制输入部分中的基座控制力、基座控制力矩和连续型机械臂上各个驱动线上的控制力。其中，和均在基座固连坐标系中表示。

将式(6)和式(26)代入到式(24)中，可得：

=

(27)

=(2)

(28)

式中：

(29)

以及

(30)

对于，可通过物理意义计算得到。连续型机械臂的变形主要是由驱动线产生的弯矩引起的。因此，可以通过需要施加的弯矩求出驱动线上的控制力。对于任意一节连续型机械臂，假设末端圆盘位于第个节点，第个驱动线上的驱动力为c,es,。由图2可知，第个驱动线与末端圆盘的固定点在全局坐标系中的坐标可表示为：

图2 作用在末端圆盘上的驱动力

=+

(31)

式中：表示末端圆盘中心到驱动线固定点的位置，在末端圆盘固连坐标系中表示；为末端圆盘固连坐标系与惯性坐标系Σ之间的坐标变换矩阵，可表示为：

(32)

则第个驱动线对连续型机械臂末端产生的弯矩的广义力可表示为：

(33)

由于第个驱动线上的驱动力c,es,对应输入矩阵的第列，因此，输入矩阵可表示为：

(34)

式中：(:,)表示输入矩阵的第列；矩阵∂∂的作用是将广义力c,放入单位四元数在连续型机械臂广义坐标中的对应位置，由式(3)可知：

(35)

将式(26)和式(34)代入式(24)可得：

(36)

式中：

=[×3×4]

(37)

将式(27)、式(28)和式(36)代入式(26)，即可得到能量整形控制输入。

2.3 阻尼注入

在对系统进行阻尼注入时，将推导得到的能量整形控制输入，即式(26)，代入到控制输入中，即式(21)，并将控制输入代入到式(14)中，可得：

(38)

阻尼注入控制输入可选择为：

(39)

式中：为正定的权重矩阵，可表示为：

=diag(,,)

(40)

式中：和分别为基座线速度和角速度的阻尼注入系数矩阵，为连续型机械臂广义速度的阻尼注入系数矩阵，均为对角矩阵。

由于存在如下关系：

(41)

将式(19)中的第2式、式(39)和式(41)代入式(38)可得：

(42)

式中：

=

(43)

2.4 干扰补偿

考虑受到干扰的系统(42)，可通过设计干扰补偿控制律，抵消系统受到的干扰。然而，干扰通常难以测量。因此，需要设计干扰观测器对干扰进行估计。采用如下形式的非线性干扰观测器估计式(42)中的干扰：

(44)

式中：

(45)

()=∂()∂

(46)

由式(42)和式(45)可知存在如下关系：

(47)

另外，由式(46)可知：

(48)

干扰估计的误差可定义为：

(49)

进一步可得：

(50)

将式(44)、式(47)、式(48)和式(49)代入式(50)，可得：

(51)

(52)

因此，干扰补偿控制律可设计为：

(53)

3 稳定性分析

考虑到系统受到的外界干扰被控制律补偿，在式(21)中控制输入的作用下，原先的动力学方程(14)最终被配置为期望的动力学方程(20)。将式(19)中的第1式代入式(15)，可得到由广义坐标和广义动量表示的期望的Hamilton函数：

(54)

式(54)中的Hamilton函数代表系统的能量，是正定函数，选取其作为Lyapunov函数。将式(54)对时间求导可得：

(55)

将式(20)代入式(55)，整理可得：

(56)

由式(16)和式(54)可知：

(57)

由于约束方程=恒成立，因此下式成立：

(58)

将式(57)代入式(56)，并考虑到式(58)，可得式(56)中第二项为，即式(56)化简为：

(59)

4 仿真校验与讨论

为验证本文所设计控制器的有效性，本节对空间连续型机器人位置姿态机动和连续型机械臂构型控制进行了仿真分析。控制目标为空间连续型机器人机动到指定位置，并且保持期望的姿态，同时控制连续型机械臂实现期望的变形。本节考虑的空间连续型机器人由2个连续型机械臂组成，每个连续型机械臂有2个柔性支撑梁，每个柔性支撑梁上有11个圆盘和1个连接器。空间连续型机器人参数见表1，各项参数的定义可参考文献[14]。

表1 空间连续型机器人参数

4.1 位置姿态机动仿真

考虑空间连续型机器人受到干扰的情况下，位置姿态的机动过程以及连续型机械臂的变形控制。初始条件为：(0)=，(0)=[1 0 0 0]，连续型机械臂保持水平状态。空间连续型机器人初始时刻的构型如图3所示，初始时刻全局坐标系与基座固连坐标系重合。

基座受到的干扰=[]在全局坐标系中可表示为：

(60)

第1个和第2个连续型机械臂分别为安装在轴正向和负向的连续型机械臂，如图3所示。

图3 空间连续型机器人初始时刻的构型与部件编号

干扰观测器中配置的极点为：-[5 5 5 5 5 5]，其余控制器参数见表2。

表2 控制器参数

控制目标为：基座机动到=[1 1.5 2]的位置;基座的姿态绕固连坐标系中的轴旋转30°，即=[cos(π/12) sin(π/12) 0 0];每个连续型机械臂中第1节和第2节柔性支撑梁分别绕固连坐标系中的轴弯曲π/3和2π/3，即绕固连坐标系轴的曲率分别为0.748 m和1.496 m。

运动过程中，基座的位移与四元数的响应如图4所示。图4(a)中的水平直线分别表示,和方向上的位移期望值1.0 m、1.5 m和2.0 m。图4(b)中的水平直线分别表示四元数,,和的期望值cos(π/12)、sin(π/12)、0和0。由图4可以看出基座的位移与四元数随着时间快速稳定到期望值。利用干扰观测器准确地估计了干扰，在此基础上，实现了干扰补偿设计，因此基座的位移和姿态可以稳定在期望值。利用干扰观测器估计干扰，进而对干扰进行补偿，可以取得良好的控制效果。

图4 基座位移与四元数的响应

在第100 s时，两个连续型机械臂中柔性支撑梁的曲率如图5所示。曲率1、曲率2和曲率3分别表示柔性支撑梁绕固连坐标系中轴、轴和轴的曲率。由于每节柔性支撑梁的长度为1.4 m，因此弧长坐标由0 m至1.4 m表示第1节柔性支撑梁，弧长坐标由1.4 m至2.8 m表示第2节柔性支撑梁。本算例中的控制目标为每个连续型机械臂中，第1节的曲率2为0.748 m，第2节的曲率2为1.496 m，每节的曲率1和曲率3均为0。由图5可知，第1个连续型机械臂中，2节柔性支撑梁的曲率2均值分别为0.746 m和1.481 m，第2个连续型机械臂中，2节柔性支撑梁的曲率2均值分别为0.746 m和1.481 m。两个连续型机械臂在终端时刻的曲率基本一致，且与期望值的误差均小于0.015 m。因此，设计的控制律可以使连续型机械臂变形至期望的构型，这使得连续型机械臂可以用于完成预定的操作任务。

图5 在第100 s时，柔性支撑梁的曲率

空间连续型机器人机动到指定的位置，并且保持期望的姿态，所需要的基座控制力和控制力矩如图6所示，可以看出控制力和控制力矩随着时间逐渐趋于稳定。由于本算例施加的外界干扰中，有的为非消失干扰。因此，当空间连续型机器人机动到指定的位置和姿态时，仍需要控制力来补偿外界干扰，以此来保持系统的稳定。在空间连续型机器人机动的同时，连续型机械臂在控制力的作用下发生弯曲变形。因此，连续型机械臂会对基座的位置和姿态产生扰动。由于连续型机械臂主要发生绕固连坐标系中轴的弯曲变形，故其对基座的姿态扰动更加明显。由图6(b)可看出，为了实现基座姿态的控制目标，初始阶段需要不断调整基座的控制力矩，之后控制力矩逐渐趋于稳定。

图6 基座控制力和控制力矩

连续型机械臂与圆盘上穿孔的编号如图3所示，从第个穿孔穿过的线被定义为第个驱动线，圆盘上的轴和轴分别表示圆盘固连坐标系中的坐标轴，初始时刻分别与基座固连坐标系中的轴和轴方向一致。由图3可知，第1个驱动线和第3个驱动线控制连续型机械臂绕轴的弯曲变形，第2个驱动线和第4个驱动线控制连续型机械臂绕轴的弯曲变形。两个连续型机械臂期望的变形相同，因此控制力接近。图7给出第1个连续型机械臂上各个驱动线的控制力。图例中c,,,表示第个连续型机械臂的第节柔性支撑梁上第个驱动线的驱动力。期望的连续型机械臂变形为绕轴正向弯曲，这主要是通过对每节柔性支撑梁上第4个驱动线施加驱动力实现的。因此，每节柔性支撑梁上第4个驱动线上的驱动力显著大于其余3个驱动线上的驱动力。

图7 第1个连续型机械臂上的驱动力

空间连续型机器人两个连续型机械臂末端的坐标如图8所示。连续型机械臂在驱动力的作用下，逐渐稳定至期望的弯曲变形。因此，两个连续型机械臂末端的坐标在初始时会震荡，逐渐趋于稳定。在0至100 s的过程中，空间连续型机器人发生了平动和转动，以及连续型机械臂发生弯曲变形，运动过程较为复杂。

图8 连续型机械臂末端坐标

4.2 控制器参数对控制效果的影响

IDA-PBC控制器中包含能量整形系数和阻尼注入系数，由2.1节的理论可知这些系数会对空间连续型机器人的运动产生影响，本节针对这一问题进行讨论。空间连续型机器人的初始条件、受到的干扰、干扰观测器中配置的极点以及控制目标均与4.1节中的设置一致。

首先，讨论能量整形系数对空间连续型机器人运动的影响。当控制器参数中的基座能量整形系数矩阵设置为=2和=2时，基座的位移响应如图9所示。本算例中的能量整形系数矩阵和均为4.1节中算例的2倍，即本算例中期望的基座质量阵为实际的质量阵的2倍。因此，基座将以比4.1节中算例更慢的速度机动到期望的位置。由图9可看出，基座逐渐接近期望的位置。但与图4相比，图9中基座的机动速度明显较慢。直到100 s时，基座仍未机动到期望的位置。

图9 当βr=2I3和βa=2I4时，基座的位移响应

当控制器参数中的基座能量整形系数矩阵设置为=05和=05时，基座的位移响应如图10所示。本算例中的能量整形系数矩阵和均为4.1节中算例的0.5倍，即本算例中期望的基座质量阵为实际的质量阵的0.5倍。由图10可看出，基座最终稳定到期望的位置。但是在机动过程中，基座的位置出现了超调。这是因为基座位置的能量整形系数取为0.5，基座将以比4.1节中算例更快的速度机动到期望的位置。因此，机动过程中出现了超调。

图10 当βr=0.5I3和βa=0.5I4时，基座的位移响应

其次，讨论阻尼注入系数对空间连续型机器人运动的影响。当控制器参数中的阻尼注入系数矩阵设置为=50和=50，其余控制参数与4.1节算例参数一致时，基座的位移响应如图11所示。本算例中的阻尼注入系数矩阵和均为4.1节中算例的1/10。由图11可看出，空间连续型机器人在机动过程中，位移出现了明显的超调，且经过100 s后，仍未稳定到期望的位置。与4.1节中的算例相比，本算例的镇定速度显著减慢，这是由于本算例的阻尼注入系数仅为4.1节中阻尼注入系数的1/10，导致镇定速度较慢。

图11 当Kv=50I3和Kω=50I4时，基座的位移响应

以上算例表明：改变能量整形系数和阻尼注入系数会使空间连续型机器人以不同的动态性能运动。基于IDA-PBC方法设计的控制器，其参数有明确的物理意义。在调整控制器参数时，可根据其物理意义，选择合适的参数。根据空间任务的需要，可通过选择能量整形系数与阻尼注入系数，使空间连续型机器人以欠阻尼、临界阻尼或过阻尼的方式机动到期望的平衡点。

5 结 论

本文针对空间连续型机器人位姿机动、连续型机械臂变形控制以及系统受到外界干扰的问题，基于IDA-PBC方法设计了控制器，包含能量整形、阻尼注入和干扰补偿三部分。研究结果表明：所设计的控制器可以使系统机动到指定的位置和姿态，同时控制连续型机械臂变形到期望的构型。非线性干扰观测器可以准确地估计系统受到的干扰。根据干扰估计值对其进行补偿，使设计的控制器具有主动抗干扰能力，可以使系统在受到外界干扰的情况下，仍然稳定到期望的平衡点。通过选择能量整形系数与阻尼注入系数，可使系统以不同的动态性能机动到期望的平衡点，以满足空间任务的需要。