针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法

1.本发明涉及一种针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法，尤其涉及一种面向回收大挠性失效卫星过程中多自由度机械臂的轨迹跟踪控制方法，属于航空航天技术领域。


背景技术：

2.随着航天事业的发展和时间的推移，各类航天任务所产生的大型失效卫星(例如燃料耗尽的通信卫星、遥感卫星等)占据了大量的轨道空间。此类卫星自身价值高、且具有大型挠性附件，对其进行延寿或回收既符合可持续发展理念，同时又是目前空间碎片清理的研究热点与难点。
3.使用机械臂捕获此类失效卫星并开展延寿操作是近年航天领域的研究热点。值得注意的是，利用机械臂捕获大挠性失效卫星后形成的组合体由多个刚体、柔体铰接组成，构型复杂，且模型维数高、参数不确定性强，不利于控制设计。此外，回收失效卫星的过程中，其挠性太阳帆板会因拖拽而发生振动，若不加以主动控制，可能导致组合体的耦合振动甚至失稳，进而导致抓捕任务失败。


技术实现要素：

4.本发明的主要目的是提供一种针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法，通过“状态观测器+有限时间控制”组合的双闭环控制策略实现多自由度机械臂各关节的轨迹跟踪控制，从而实现对大挠性组合体的稳定回收控制。
5.本发明的目的是通过下述技术方案实现的。
6.本发明公开的针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法，包括以下步骤：
7.步骤一：建立机械臂轨迹跟踪的控制方程。对大挠性组合体采用基于kane方程的自动组集算法建立显式动力学方程，推导得到机械臂的动力学方程，从而建立机械臂轨迹跟踪的控制方程，便于步骤二设计控制器。
8.步骤1.1：采用基于kane方程的自动组集算法建立大挠性组合体的动力学方程为
[0009][0010]
其中：是系统总体的质量矩阵；u＝[u
1t u
2t
ꢀ…ꢀunt
]
t
，是系统广义速率依次排列组成的列阵；fa是系统的广义主动力，包括外界施加的力和力矩，以及弹性体的广义内力项；是系统广义惯性力的非线性项。
[0011]
步骤1.2：根据建立的系统动力学方程(1)，组合体中机械臂的动力学方程可以写为
[0012][0013]
其中，θa为机械臂各关节转角列阵，ma为机械臂标称质量矩阵，但其中不考虑机械臂弹性位移和弹性转角的影响，是各机械臂转角θa的函数，为已知量；δma为质量阵不确定项，主要由机械臂挠性形变、目标星的质量不确定性引起；da为系统的内外扰动之和，包括环境力矩的扰动、服务星星本体运动的扰动、目标星挠性振动的扰动、机械臂挠性振动的扰动以及系统中的未知非线性耦合量；ta为各关节控制力矩构成的控制力矩向量，为系统的控制输入。
[0014]
步骤1.3：根据式(2)，机械臂轨迹跟踪的控制方程进一步变换为
[0015][0016]
其中，ua为控制量；aa(t)为方程中总的不确定量，包括系统惯量不确定性导致的控制输入误差以及卫星星体姿态受到的总扰动对机械臂关节运动的影响；且有
[0017]
ua＝b
a0
ta[0018]aa
(t)＝δbata+bada[0019]
ba＝(ma+δma)-1
[0020][0021]
δba＝b
a-b
a0
[0022]
经如上处理后，控制方程(3)则成为有关转角向量θa的二阶微分方程，而ua为待设计控制量。轨迹跟踪控制即是要设计控制力矩向量ta，使得
[0023][0024]
其中，θ
ar
和分别为期望的机械臂关节角和角速度，由机械臂轨迹规划给出。轨迹规划除给出各关节的期望转角θ
ar
和期望角速度外，还给出各关节的期望角加速度期望角加速度已知量对轨迹跟踪控制十分重要。
[0025]
步骤二：基于步骤一建立的机械臂轨迹跟踪控制方程，设计扩张状态观测器，利用对系统状态的观测实现扰动估计，从而为控制补偿提供基础；进而设计有限时间控制器，提高控制系统对未知扰动的鲁棒性，加快姿态稳定的收敛速度；针对回收过程中系统挠性附件会产生的振动，加入滤波器进行动态解耦。通过采用“状态观测器+有限时间姿态控制器”组合的双闭环控制策略进行轨迹跟踪，提高机械臂的跟踪速度和跟踪精度，实现机械臂回收过程的稳定跟踪控制。
[0026]
所述“状态观测器+有限时间姿态控制器”组合的双闭环控制策略，内环以机械臂各关节转角的角速度为输入，利用二阶扩张状态观测器输出不确定量的估计值，所述不确定量在外环反馈控制器输出控制量中加以补偿，能够提高轨迹跟踪控制的精度。
[0027]
步骤2.1：设计扩张状态观测器跟踪机械臂各关节的状态，采用内环补偿加外环反馈的双闭环控制，对实时观测得到的总扰动量进行快速估计和补偿。
[0028]
步骤一建立的控制方程(3)是一个不包含非线性项的一阶系统，系统量测量为因此采用二阶扩张状态观测器对系统状态量和方程中的总不确定量aa(t)进行估计
[0029][0030]
其中，β
a1
＞0，β
a2
＞0，0＜α
a0
＜1，δ
a0
＞0均为待设计参数；e
a0
，z
a1
，z
a2
及均为列矢量，其维度与机械臂系统的自由度相等；z
a1
和z
a2
分别跟踪系统状态量和方程中的总不确定量aa(t)；fal(e
a0
,α
a0
,δ
a0
)为非线性函数，定义为
[0031][0032]
其中，
[0033][0034]
其中，sgn(
·
)表示符号函数。
[0035]
定义状态跟踪误差为和e
a2
＝z
a2-aa(t)，其中易见e
a0
＝e
a1
；则对于式(5)设计的扩张状态观测器，其跟踪的状态误差方程为
[0036][0037]
只要参数β
a1
、β
a2
、α
a0
及δ
a0
选取合适，且满足二阶扩张状态观测器的稳定性条件
[0038][0039]
则式(5)所示扩张状态观测器的稳态误差将以有限时间收敛到
[0040][0041]
其中，wa为的界，即即和分别反应状态观测器对系统状态量和方程中的总不确定量aa(t)的最大估计误差。分析表明，只要β
a2
足够大，和将足够小，此时扩张状态观测器的状态z
a1
和z
a2
将分别实时估计出系统状态量和方程中的总不确定量aa(t)。
[0042]
利用估计值z
a2
，对机械臂控制方程(3)实施如下动态反馈补偿律
[0043]
ua＝u
a0-z
a2
(11)
[0044]
则控制方程(3)将成为如下二阶线性系统
[0045][0046]
当和足够小时，不确定量aa(t)的估计误差对机械臂动力学的影响可以忽略
不计，因此控制方程进一步简化为
[0047][0048]
该方程(13)为步骤2.2滑模面的设计提供基础。
[0049]
步骤2.2：在步骤2.1设计的扩张状态观测器基础上，通过有限时间控制律设计快速终端滑模面和有限时间控制器，提高系统姿态稳定的收敛速度。
[0050]
首先定义机械臂关节轨迹跟踪误差状态变量为
[0051][0052]
则机械臂关节轨迹跟踪误差动力学方程的状态空间形式为
[0053][0054]
针对步骤2.1简化的控制方程(13)，设计如下快速终端滑模面
[0055][0056]
其中，βa＝d[β
a1
,β
a2
,β
a3
]＞0；0＜αa＜1；。则滑模面的导数有
[0057][0058]
基于滑模面(16)，设计系统的有限时间控制器为
[0059][0060]
其中，k
sa1
＝d[k
sa1,1
,k
sa1,2
,k
sa1,3
]＞0。
[0061]
下面分析模型(15)在所设计滑模面(16)和控制器(18)作用下的稳定性，稳定性分析过程如下：
[0062]
引入有限时间控制系统稳定性判据：对于系统f(0)＝0，若存在定义于原点邻域u0上的正定连续函数v(x)以及实数a＞0、b＞0、0＜p＜1，满足则该系统是有限时间稳定的。
[0063]
首先对滑模面进行稳定性分析，考虑lyapunov函数
[0064][0065]
对其求时间导数，并带入式(15)-(18)，得到
[0066][0067]
其中，为参数矩阵k
sa1
对角线上的最小元素。则根据有限时间稳定判据，滑模面(16)是有限时间稳定的，即系统状态能在有限时间内收敛至sa＝0。
[0068]
下面分析系统状态收敛到sa＝0之后的稳定性。考虑lyapunov函数
[0069][0070]
对其求时间导数，由于当sa＝0时有则
[0071][0072]
其中，β
a，min
为参数矩阵βa对角线上的最小元素。根据有限时间稳定判据，系统状态
到达sa＝0后，将进一步在有限时间内收敛至{x1＝0，x2＝0}，即机械臂关节轨迹跟踪误差将在有限时间内收敛至零。
[0073]
模型(3)将在基于状态观测器(5)、动态反馈补偿律(11)、滑模面(16)和控制器(18)的共同作用下，在有限时间内收敛至
[0074]
步骤2.3：在步骤2.1和2.2设计的组合控制律的基础上，构建一阶线性滤波器进行动态解耦，避免由于挠性附件的振动造成姿控系统中不必要的能量消耗。
[0075]
通过滤波解决控制作用激励模态振动。扩张状态观测器本身已具有对测量信号滤波的功能，故只需对反馈补偿律(11)的输出进行滤波。设计一阶线性滤波器为
[0076][0077]
其中β
ta
＞0，根据低阶模态频率选取适当值。
[0078]
实际的机械臂各关节控制力矩指令则为
[0079][0080]
其中，如前所述，ma为机械臂标称质量矩阵，是各机械臂转角θa的函数，需要根据各机械臂转角θa的测量值进行实时计算。
[0081]
步骤2.4：基于步骤2.1所述扩张状态观测器、步骤2.2所述快速终端滑模面和有限时间控制器、步骤2.3所述滤波器，通过采用“状态观测器+有限时间姿态控制器”组合的双闭环控制策略进行轨迹跟踪，提高机械臂的跟踪速度和跟踪精度，实现机械臂回收过程的稳定跟踪控制。
[0082]
基于步骤一建立的机械臂轨迹跟踪控制方程，设计扩张状态观测器，利用对系统状态的观测实现扰动估计，从而为控制补偿提供基础；进而设计有限时间控制器，提高控制系统对未知扰动的鲁棒性，加快姿态稳定的收敛速度；针对回收过程中系统挠性附件会产生的振动，加入滤波器进行动态解耦。通过采用“状态观测器+有限时间姿态控制器”组合的双闭环控制策略进行轨迹跟踪，提高机械臂的跟踪速度和跟踪精度，实现机械臂回收过程的稳定跟踪控制。
[0083]
有益效果：
[0084]
1、本发明公开的针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法，通过设计扩张状态观测器观测机械臂各关节的状态，对实时的扰动进行快速估计和补偿，进而设计快速终端滑模面和有限时间控制器，提高控制系统对未知扰动的鲁棒性，加快姿态稳定的收敛速度，并针对系统挠性附件振动会产生的能量消耗问题，构建一阶线性滤波器进行动态解耦，实现对大挠性组合体的稳定回收控制。
[0085]
2、本发明公开的针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法，采用“状态观测器+有限时间姿态控制器”组合的双闭环控制策略。内环以机械臂各关节转角的角速度为输入，利用二阶扩张状态观测器，输出不确定量的估计值，所述不确定量在外环反馈控制器输出控制量中加以补偿，能够提高轨迹跟踪控制的速度和精度，实现机械臂回收过程的稳定跟踪控制。
[0086]
3、本发明公开的针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法，在设计的扩张状态观测器基础上，对机械臂系统的七个通道均采用有限时间控制律设计稳定性的有限时间姿态控制器，加快系统姿态稳定的收敛速度。对每个通道均采用有限时间控制律，
较之传统的pd控制器，收敛速度快、抗干扰和鲁棒性能好。
[0087]
4、本发明公开的针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法，采用基于kane方程的自动组集算法建立大挠性组合体的动力学方程，进而得到机械臂系统的动力学方程，从而建立机械臂轨迹跟踪的控制方程。kane方程可以避免繁琐的求导运算，得到的动力学方程形式简单，物理意义明确，便于控制器的设计，适用于复杂的航天器系统建模。
附图说明
[0088]
图1为本发明实施例中捕获后大挠性组合体的构型图。
[0089]
其中：a——服务星；b——失效卫星；10、13——刚性机械臂；11、12——柔性机械臂；21、31——服务星的柔性帆板；41、51——失效卫星的柔性帆板。
[0090]
图2为本发明公开的一种对多自由度机械臂回收失效卫星的轨迹跟踪控制方法的总流程图。
[0091]
图3为本发明公开的一种对多自由度机械臂回收失效卫星的轨迹跟踪控制方法的结构框图。
[0092]
图4为本发明实施例中fta控制器作用下的机械臂关节转角、角速度控制误差曲线图，其中图4(a)为关节转角控制误差曲线图，图4(b)为关节角速度控制误差曲线图。
[0093]
图5为本发明实施例中pda控制器作用下的机械臂关节转角、角速度控制误差曲线图，其中图5(a)为关节转角控制误差曲线图，图5(b)为关节角速度控制误差曲线图。
具体实施方式
[0094]
下面结合实施例和附图对本发明公开的一种针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法作详细说明。
[0095]
本实例所采用的仿真模型为图1所示的大挠性组合体模型，由棱长2m的卫星本体及其上两块帆板、7自由度机械臂、棱长2m的目标星体及其上两块帆板组成。其中服务星、目标星体和较短机械臂为刚体，本体与目标附带帆板和较长机械臂视为柔体。
[0096]
组合体系统各单体质量和几何参数如下：
[0097]
服务星质量mb＝1000kg，主轴转动惯量jb＝d[2000/3,2000/3,2000/3]kg
·
m2，其中算子d[x]返回以矢量x的分量为对角元的方阵；目标星体质量m
bt
＝1500kg，主轴转动惯量j
bt
＝d[1000,1000,1000]kg
·
m2；帆板质量m
p
＝37.6546kg，几何参数取宽为b
p
＝1.72m，长为h
p
＝6.795m，厚为d
p
＝0.002m；考虑机械臂质量参数时，需要考虑铰接对系统质量参数的影响，假设机械臂的三处铰接质量均为m
joint
＝1kg。机械臂的内径rm＝0.03m，外径rm＝0.05m，材料密度ρ＝7850kg/m3，长度尺寸参数分别为l
10
＝0.2m，l
11
＝1m，l
12
＝1.2m，l
13
＝0.3m。
[0098]
如图2所示，本实施例公开的针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法的总流程图，具体实现步骤如下：
[0099]
步骤一：建立机械臂轨迹跟踪控制方程。对大挠性组合体采用基于kane方程的自动组集算法建立显式动力学方程，推导得到机械臂的动力学方程，从而建立机械臂轨迹跟踪的控制方程，便于步骤二进行控制器的设计。
[0100]
步骤1.1：采用基于kane方程的自动组集算法建立大挠性组合体的动力学方程为
[0101]
[0102]
其中：是系统总体的质量矩阵；u＝[u
1t u
2t
ꢀ…ꢀunt
]
t
，是系统广义速率依次排列组成的列阵，本实施例中的广义速率及初始值在表1中给出；fa是系统的广义主动力，包括外界施加的力和力矩，以及弹性体的广义内力项；是系统广义惯性力的非线性项。
[0103]
表1系统广义速率初始值
[0104][0105][0106]
步骤1.2：根据建立的系统动力学方程(25)，组合体中机械臂的动力学方程可以写为
[0107][0108]
其中，θa为机械臂各关节转角列阵，这里ma为机械臂标称质量矩阵，但其中不考虑机械臂弹性位移和弹性转角的影响，是各机械臂转角θa的函数，为已知量；δma为质量阵不确定项，主要由机械臂挠性形变、目标星的质量不确定性等因素引起；da为系统的内外扰动之和，包括环境力矩的扰动、服务星星本体运动的扰动、目标星挠性振动的扰动、机械臂挠性振动的扰动以及系统中的未知非线性耦合量等；ta为各关节控制力矩构成的控制力矩向量，为系统的控制输入。
[0109]
步骤1.3：根据动力学方程(26)，机械臂轨迹跟踪的控制方程可进一步写为
[0110][0111]
其中，ua为控制量；aa(t)为方程中总的不确定量，包括系统惯量不确定性导致的控
制输入误差以及卫星星体姿态受到的总扰动等因素机械臂关节运动的影响；且有
[0112]
ua＝b
a0
ta[0113]aa
(t)＝δbata+bada[0114]
ba＝(ma+δma)-1
[0115][0116]
δba＝b
a-b
a0
[0117]
步骤二：基于步骤一建立的机械臂轨迹跟踪控制方程，设计扩张状态观测器，利用对系统状态的观测实现扰动估计，从而为控制补偿提供基础；进而设计有限时间控制器，提高控制系统对未知扰动的鲁棒性，加快姿态稳定的收敛速度；针对回收过程中系统挠性附件会产生的振动，加入滤波器进行动态解耦。通过采用“状态观测器+有限时间姿态控制器”组合的双闭环控制策略进行轨迹跟踪，提高机械臂的跟踪速度和跟踪精度，实现机械臂回收过程的稳定跟踪控制。
[0118]
步骤2.1：设计扩张状态观测器跟踪机械臂各关节的状态，采用内环补偿加外环反馈的双闭环控制，对实时观测得到的总扰动量进行快速估计和补偿。
[0119]
步骤一建立的控制方程(27)是一个不包含非线性项的一阶系统，系统量测量为因此采用二阶扩张状态观测器对系统状态量和方程中的总不确定量aa(t)进行估计，
[0120][0121]
其中，β
a1
＞0，β
a2
＞0，0＜α
a0
＜1，δ
a0
＞0均为待设计参数；e
a0
，z
a1
，z
a2
及均为列矢量，其维度与机械臂系统的自由度相等；z
a1
和z
a2
分别跟踪系统状态量和方程中的总不确定量aa(t)。
[0122]
定义状态跟踪误差为和e
a2
＝z
a2-aa(t)，只要参数β
a1
、β
a2
、α
a0
及δ
a0
满足则式(28)所示扩张状态观测器的稳态误差将以有限时间收敛到
[0123][0124]
其中，wa为的界，即即和分别反应状态观测器对系统状态量和方程中的总不确定量aa(t)的最大估计误差。可以看出，只要β
a2
足够大，和将足够小，此时扩张状态观测器的状态z
a1
和z
a2
将分别实时估计出系统状态量和方程中的总不确定量aa(t)。
[0125]
利用估计值z
a2
，对机械臂控制方程(27)实施如下动态反馈补偿律，
[0126]
ua＝u
a0-z
a2
(30)
[0127]
则控制方程将成为如下二阶线性系统，
[0128][0129]
当和足够小时，不确定量aa(t)的估计误差对机械臂动力学的影响可以忽略不计，因此控制方程可进一步简化为
[0130][0131]
式(32)为步骤2.2滑模面的设计提供基础。
[0132]
步骤2.2：在步骤2.1设计的扩张状态观测器基础上，通过有限时间控制律设计快速终端滑模面和有限时间控制器，提高系统姿态稳定的收敛速度。
[0133]
首先定义机械臂关节轨迹跟踪误差状态变量为
[0134][0135]
则机械臂关节轨迹跟踪误差动力学方程的状态空间形式为
[0136][0137]
针对步骤2.1简化的控制方程(32)，设计如下快速终端滑模面
[0138][0139]
其中，βa＝d[β
a1
,β
a2
,β
a3
]＞0；0＜αa＜1；。则滑模面的导数有
[0140][0141]
基于滑模面(35)可设计系统的有限时间控制器为
[0142][0143]
其中，k
sa1
＝d[k
sa1,1
,k
sa1,2
,k
sa1,3
]＞0。
[0144]
下面分析模型(34)在所设计滑模面(35)和控制器(37)作用下的稳定性。稳定性分析如下：
[0145]
首先对滑模面进行稳定性分析，考虑lyapunov函数
[0146][0147]
对其求时间导数，并带入式(34)-(37)，得到
[0148][0149]
其中，为参数矩阵k
sa1
对角线上的最小元素。则根据有限时间稳定判据，滑模面(35)是有限时间稳定的，即系统状态能在有限时间内收敛至sa＝0。
[0150]
下面分析系统状态收敛到sa＝0之后的稳定性。考虑lyapunov函数
[0151][0152]
对其求时间导数，由于当sa＝0时有则
[0153][0154]
其中，β
a，min
为参数矩阵βa对角线上的最小元素。根据有限时间稳定判据，系统状态到达sa＝0后，将进一步在有限时间内收敛至{x1＝0，x2＝0}，即机械臂关节轨迹跟踪误差将
在有限时间内收敛至零。
[0155]
至此可知，模型(27)将在基于状态观测器(28)、动态反馈补偿律(30)、滑模面(35)和控制器(37)的共同作用下，在有限时间内收敛至
[0156]
步骤2.3：在步骤2.1和2.2设计的组合控制律的基础上，设计一阶线性滤波器进行动态解耦，避免由于挠性附件的振动造成姿控系统中不必要的能量消耗。
[0157]
解决控制作用激励模态振动的现实办法是进行滤波。扩张状态观测器本身已具有对测量信号滤波的功能，故只需对反馈补偿律的输出进行滤波。设计一阶线性滤波器为
[0158][0159]
其中β
ta
＞0，可根据低阶模态频率选取适当值。
[0160]
实际的机械臂各关节控制力矩指令则为
[0161][0162]
其中，如前所述，ma为机械臂标称质量矩阵，是各机械臂转角θa的函数，需要根据各机械臂转角θa的测量值进行实时计算。
[0163]
结合图3的针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法结构框图说明本实施例的实施方式，控制方法与实现方法与发明内容相同。
[0164]
为了对比控制效果，将设计的“状态观测器+有限时间轨迹跟踪控制器”双闭环控制策略(以下简称fta控制器)与“状态观测器+pd姿态控制器”双闭环控制策略(以下简称pda控制器)作对比。其中pd姿态控制器选取为
[0165][0166]
其中，k
ap
＝d[k
ap,1
,k
ap,2
,k
ap,3
]＞0；k
ad
＝d[k
ad,1
,k
ad,2
,k
ad,3
]＞0。
[0167]
系统标称总惯量矩阵和机械臂标称质量矩阵分别取为系统实际总惯量矩阵和机械臂实际质量矩阵的80％。初始时，服务星星体三轴姿态角为[3；-3；2]
°
，姿态角速度为[0；0；0]
°
/s，控制目标为实现姿态镇定控制任务。
[0168]
机械臂轨迹跟踪控制参数选取如表2所示，其中算子d[x]返回以矢量x的分量为对角元的方阵。
[0169]
表2轨迹跟踪控制参数
[0170][0171][0172]
经过15s，结果表明，本实施例中，两种基于“状态观测器+轨迹跟踪控制器”组合的双闭环控制策略均能实现大挠性组合体的稳定回收控制。其中，图4表明，fta控制器实现的机械臂关节转角跟踪精度优于0.03deg，关节角速度跟踪精度优于0.3deg/s；图5表明，pda控制器实现的机械臂关节转角跟踪精度仅为0.07deg，关节角速度跟踪精度约为0.03deg/
s。对比图4与图5可知，fta控制器比pda控制器具有更高的关节转角跟踪精度，从整体误差曲线对比分析可知，fta控制器具有更好的轨迹跟踪精度和响应速度，也明显呈现出了有限时间控制方法的特性。对比结果表明，本发明公开的一种针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法更能实现对大挠性组合体的稳定回收控制。
[0173]
以上所述的具体描述，对发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

技术特征：
1.针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法，其特征在于：包括以下步骤，步骤一：建立机械臂轨迹跟踪的控制方程；对大挠性组合体采用基于kane方程的自动组集算法建立显式动力学方程，推导得到机械臂的动力学方程，从而建立机械臂轨迹跟踪的控制方程，便于步骤二设计控制器；步骤二：基于步骤一建立的机械臂轨迹跟踪控制方程，设计扩张状态观测器，利用对系统状态的观测实现扰动估计，从而为控制补偿提供基础；设计有限时间控制器，提高控制系统对未知扰动的鲁棒性，加快姿态稳定的收敛速度；针对回收过程中系统挠性附件会产生的振动，加入滤波器进行动态解耦；通过采用“状态观测器+有限时间姿态控制器”组合的双闭环控制策略进行轨迹跟踪，提高机械臂的跟踪速度和跟踪精度，实现机械臂回收过程的稳定跟踪控制；所述“状态观测器+有限时间姿态控制器”组合的双闭环控制策略，内环以机械臂各关节转角的角速度为输入，利用二阶扩张状态观测器输出不确定量的估计值，所述不确定量在外环反馈控制器输出控制量中加以补偿，能够提高轨迹跟踪控制的精度。2.如权利要求1所述的针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法，其特征在于：步骤一实现方法为，步骤1.1：采用基于kane方程的自动组集算法建立大挠性组合体的动力学方程为其中：是系统总体的质量矩阵；u＝[u
1t
u
2t
u
nt
]
t
，是系统广义速率依次排列组成的列阵；f
a
是系统的广义主动力，包括外界施加的力和力矩，以及弹性体的广义内力项；是系统广义惯性力的非线性项；步骤1.2：根据建立的系统动力学方程(1)，组合体中机械臂的动力学方程可以写为其中，θ
a
为机械臂各关节转角列阵，m
a
为机械臂标称质量矩阵，但其中不考虑机械臂弹性位移和弹性转角的影响，是各机械臂转角θ
a
的函数，为已知量；δm
a
为质量阵不确定项，主要由机械臂挠性形变、目标星的质量不确定性引起；d
a
为系统的内外扰动之和，包括环境力矩的扰动、服务星星本体运动的扰动、目标星挠性振动的扰动、机械臂挠性振动的扰动以及系统中的未知非线性耦合量；t
a
为各关节控制力矩构成的控制力矩向量，为系统的控制输入；步骤1.3：根据式(2)，机械臂轨迹跟踪的控制方程进一步变换为其中，u
a
为控制量；a
a
(t)为方程中总的不确定量，包括系统惯量不确定性导致的控制输入误差以及卫星星体姿态受到的总扰动对机械臂关节运动的影响；且有u
a
＝b
a0
t
a
a
a
(t)＝δb
a
t
a
+b
a
d
a
b
a
＝(m
a
+δm
a
)-1
δb
a
＝b
a-b
a0
经如上处理后，控制方程(3)则成为有关转角向量θ
a
的二阶微分方程，而u
a
为待设计控制量；轨迹跟踪控制即是要设计控制力矩向量t
a
，使得其中，θ
ar
和分别为期望的机械臂关节角和角速度，由机械臂轨迹规划给出；轨迹规划除给出各关节的期望转角θ
ar
和期望角速度外，还给出各关节的期望角加速度3.如权利要求2所述的针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法，其特征在于：步骤二实现方法为，步骤2.1：设计扩张状态观测器跟踪机械臂各关节的状态，采用内环补偿加外环反馈的双闭环控制，对实时观测得到的总扰动量进行快速估计和补偿；步骤2.2：在步骤2.1设计的扩张状态观测器基础上，通过有限时间控制律设计快速终端滑模面和有限时间控制器，提高系统姿态稳定的收敛速度；步骤2.3：在步骤2.1和2.2设计的组合控制律的基础上，构建一阶线性滤波器进行动态解耦，避免由于挠性附件的振动造成姿控系统中不必要的能量消耗；步骤2.4：基于步骤2.1所述扩张状态观测器、步骤2.2所述快速终端滑模面和有限时间控制器、步骤2.3所述滤波器，通过采用“状态观测器+有限时间姿态控制器”组合的双闭环控制策略进行轨迹跟踪，提高机械臂的跟踪速度和跟踪精度，实现机械臂回收过程的稳定跟踪控制。4.如权利要求3所述的针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法，其特征在于：步骤2.1实现方法为，步骤一建立的控制方程(3)是一个不包含非线性项的一阶系统，系统量测量为因此采用二阶扩张状态观测器对系统状态量和方程中的总不确定量a
a
(t)进行估计其中，β
a1
＞0，β
a2
＞0，0＜α
a0
＜1，δ
a0
＞0均为待设计参数；e
a0
，z
a1
，z
a2
及均为列矢量，其维度与机械臂系统的自由度相等；z
a1
和z
a2
分别跟踪系统状态量和方程中的总不确定量a
a
(t)；fal(e
a0
,α
a0
,δ
a0
)为非线性函数，定义为其中，
其中，sgn(
·
)表示符号函数；定义状态跟踪误差为和e
a2
＝z
a2-a
a
(t)，其中易见e
a0
＝e
a1
；则对于式(5)设计的扩张状态观测器，其跟踪的状态误差方程为只要参数β
a1
、β
a2
、α
a0
及δ
a0
选取合适，且满足二阶扩张状态观测器的稳定性条件则式(5)所示扩张状态观测器的稳态误差将以有限时间收敛到其中，w
a
为的界，即即和分别反应状态观测器对系统状态量和方程中的总不确定量a
a
(t)的最大估计误差；分析表明，只要β
a2
足够大，和将足够小，此时扩张状态观测器的状态z
a1
和z
a2
将分别实时估计出系统状态量和方程中的总不确定量a
a
(t)；利用估计值z
a2
，对机械臂控制方程(3)实施如下动态反馈补偿律u
a
＝u
a0-z
a2
(11)则控制方程(3)将成为如下二阶线性系统当和足够小时，不确定量a
a
(t)的估计误差对机械臂动力学的影响可以忽略不计，因此控制方程进一步简化为该方程(13)为步骤2.2滑模面的设计提供基础。5.如权利要求4所述的针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法，其特征在于：步骤2.2实现方法为，首先定义机械臂关节轨迹跟踪误差状态变量为则机械臂关节轨迹跟踪误差动力学方程的状态空间形式为
针对步骤2.1简化的控制方程(13)，设计如下快速终端滑模面其中，β
a
＝d[β
a1
,β
a2
,β
a3
]＞0；0＜α
a
＜1；；则滑模面的导数有基于滑模面(16)，设计系统的有限时间控制器为其中，k
sa1
＝d[k
sa1,1
,k
sa1,2
,k
sa1,3
]＞0；下面分析模型(15)在所设计滑模面(16)和控制器(18)作用下的稳定性，稳定性分析过程如下：引入有限时间控制系统稳定性判据：对于系统f(0)＝0，若存在定义于原点邻域u0上的正定连续函数v(x)以及实数a＞0、b＞0、0＜p＜1，满足则该系统是有限时间稳定的；首先对滑模面进行稳定性分析，考虑lyapunov函数对其求时间导数，并带入式(15)-(18)，得到其中，为参数矩阵k
sa1
对角线上的最小元素；则根据有限时间稳定判据，滑模面(16)是有限时间稳定的，即系统状态能在有限时间内收敛至s
a
＝0；下面分析系统状态收敛到s
a
＝0之后的稳定性；考虑lyapunov函数对其求时间导数，由于当s
a
＝0时有则其中，β
a，min
为参数矩阵β
a
对角线上的最小元素；根据有限时间稳定判据，系统状态到达s
a
＝0后，将进一步在有限时间内收敛至{x1＝0，x2＝0}，即机械臂关节轨迹跟踪误差将在有限时间内收敛至零；模型(3)将在基于状态观测器(5)、动态反馈补偿律(11)、滑模面(16)和控制器(18)的共同作用下，在有限时间内收敛至6.如权利要求5所述的针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法，其特征在于：步骤2.3实现方法为，通过滤波解决控制作用激励模态振动；扩张状态观测器本身已具有对测量信号滤波的功能，故只需对反馈补偿律(11)的输出进行滤波；设计一阶线性滤波器为其中β
ta
＞0，根据低阶模态频率选取适当值；
实际的机械臂各关节控制力矩指令则为其中，如前所述，m
a
为机械臂标称质量矩阵，是各机械臂转角θ
a
的函数，根据各机械臂转角θ
a
的测量值进行实时计算。

技术总结
本发明公开的针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法，属于航空航天技术领域。本发明实现方法为：建立机械臂轨迹跟踪的控制方程；对大挠性组合体采用基于Kane方程的自动组集算法建立显式动力学方程，推导得到机械臂的动力学方程，建立机械臂轨迹跟踪的控制方程；基于所述控制方程，设计扩张状态观测器，利用对系统状态的观测实现扰动估计，设计有限时间控制器，提高控制系统对未知扰动鲁棒性，加快姿态稳定的收敛速度；针对回收过程中系统挠性附件产生的振动，加入滤波器进行动态解耦；采用“状态观测器+有限时间姿态控制器”组合的双闭环控制策略进行轨迹跟踪，提高机械臂的跟踪速度和跟踪精度，实现机械臂回收过程的稳定跟踪控制。的稳定跟踪控制。的稳定跟踪控制。


技术研发人员：贺子轩 胡权
受保护的技术使用者：北京理工大学
技术研发日：2023.05.15
技术公布日：2023/8/9