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

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

背景技术：

1、随着航天事业的发展和时间的推移，各类航天任务所产生的大型失效卫星(例如燃料耗尽的通信卫星、遥感卫星等)占据了大量的轨道空间。此类卫星自身价值高、且具有大型挠性附件，对其进行延寿或回收既符合可持续发展理念，同时又是目前空间碎片清理的研究热点与难点。

2、使用机械臂捕获此类失效卫星并开展延寿操作是近年航天领域的研究热点。值得注意的是，利用机械臂捕获大挠性失效卫星后形成的组合体由多个刚体、柔体铰接组成，构型复杂，且模型维数高、参数不确定性强，不利于控制设计。此外，回收失效卫星的过程中，其挠性太阳帆板会因拖拽而发生振动，若不加以主动控制，可能导致组合体的耦合振动甚至失稳，进而导致抓捕任务失败。

技术实现思路

1、本发明的主要目的是提供一种针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法，通过“状态观测器+有限时间控制”组合的双闭环控制策略实现多自由度机械臂各关节的轨迹跟踪控制，从而实现对大挠性组合体的稳定回收控制。

2、本发明的目的是通过下述技术方案实现的。

3、本发明公开的针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法，包括以下步骤：

4、步骤一：建立机械臂轨迹跟踪的控制方程。对大挠性组合体采用基于kane方程的自动组集算法建立显式动力学方程，推导得到机械臂的动力学方程，从而建立机械臂轨迹跟踪的控制方程，便于步骤二设计控制器。

5、步骤1.1：采用基于kane方程的自动组集算法建立大挠性组合体的动力学方程为

6、

7、其中：是系统总体的质量矩阵；u＝[u1t u2t … unt]t，是系统广义速率依次排列组成的列阵；fa是系统的广义主动力，包括外界施加的力和力矩，以及弹性体的广义内力项；是系统广义惯性力的非线性项。

8、步骤1.2：根据建立的系统动力学方程(1)，组合体中机械臂的动力学方程可以写为

9、

10、其中，θa为机械臂各关节转角列阵，ma为机械臂标称质量矩阵，但其中不考虑机械臂弹性位移和弹性转角的影响，是各机械臂转角θa的函数，为已知量；δma为质量阵不确定项，主要由机械臂挠性形变、目标星的质量不确定性引起；da为系统的内外扰动之和，包括环境力矩的扰动、服务星星本体运动的扰动、目标星挠性振动的扰动、机械臂挠性振动的扰动以及系统中的未知非线性耦合量；ta为各关节控制力矩构成的控制力矩向量，为系统的控制输入。

11、步骤1.3：根据式(2)，机械臂轨迹跟踪的控制方程进一步变换为

12、

13、其中，ua为控制量；aa(t)为方程中总的不确定量，包括系统惯量不确定性导致的控制输入误差以及卫星星体姿态受到的总扰动对机械臂关节运动的影响；且有

14、ua＝ba0ta

15、aa(t)＝δbata+bada

16、ba＝(ma+δma)-1

17、

18、δba＝ba-ba0

19、经如上处理后，控制方程(3)则成为有关转角向量θa的二阶微分方程，而ua为待设计控制量。轨迹跟踪控制即是要设计控制力矩向量ta，使得

20、

21、其中，θar和分别为期望的机械臂关节角和角速度，由机械臂轨迹规划给出。轨迹规划除给出各关节的期望转角θar和期望角速度外，还给出各关节的期望角加速度期望角加速度已知量对轨迹跟踪控制十分重要。

22、步骤二：基于步骤一建立的机械臂轨迹跟踪控制方程，设计扩张状态观测器，利用对系统状态的观测实现扰动估计，从而为控制补偿提供基础；进而设计有限时间控制器，提高控制系统对未知扰动的鲁棒性，加快姿态稳定的收敛速度；针对回收过程中系统挠性附件会产生的振动，加入滤波器进行动态解耦。通过采用“状态观测器+有限时间姿态控制器”组合的双闭环控制策略进行轨迹跟踪，提高机械臂的跟踪速度和跟踪精度，实现机械臂回收过程的稳定跟踪控制。

23、所述“状态观测器+有限时间姿态控制器”组合的双闭环控制策略，内环以机械臂各关节转角的角速度为输入，利用二阶扩张状态观测器输出不确定量的估计值，所述不确定量在外环反馈控制器输出控制量中加以补偿，能够提高轨迹跟踪控制的精度。

24、步骤2.1：设计扩张状态观测器跟踪机械臂各关节的状态，采用内环补偿加外环反馈的双闭环控制，对实时观测得到的总扰动量进行快速估计和补偿。

25、步骤一建立的控制方程(3)是一个不包含非线性项的一阶系统，系统量测量为因此采用二阶扩张状态观测器对系统状态量和方程中的总不确定量aa(t)进行估计

26、

27、其中，βa1＞0，βa2＞0，0＜αa0＜1，δa0＞0均为待设计参数；ea0，za1，za2及均为列矢量，其维度与机械臂系统的自由度相等；za1和za2分别跟踪系统状态量和方程中的总不确定量aa(t)；fal(ea0,αa0,δa0)为非线性函数，定义为

28、

29、其中，

30、

31、其中，sgn(·)表示符号函数。

32、定义状态跟踪误差为和ea2＝za2-aa(t)，其中易见ea0＝ea1；则对于式(5)设计的扩张状态观测器，其跟踪的状态误差方程为

33、

34、只要参数βa1、βa2、αa0及δa0选取合适，且满足二阶扩张状态观测器的稳定性条件

35、

36、则式(5)所示扩张状态观测器的稳态误差将以有限时间收敛到

37、

38、其中，wa为的界，即和分别反应状态观测器对系统状态量和方程中的总不确定量aa(t)的最大估计误差。分析表明，只要βa2足够大，和将足够小，此时扩张状态观测器的状态za1和za2将分别实时估计出系统状态量和方程中的总不确定量aa(t)。

39、利用估计值za2，对机械臂控制方程(3)实施如下动态反馈补偿律

40、ua＝ua0-za2 (11)

41、则控制方程(3)将成为如下二阶线性系统

42、

43、当和足够小时，不确定量aa(t)的估计误差对机械臂动力学的影响可以忽略不计，因此控制方程进一步简化为

44、

45、该方程(13)为步骤2.2滑模面的设计提供基础。

46、步骤2.2：在步骤2.1设计的扩张状态观测器基础上，通过有限时间控制律设计快速终端滑模面和有限时间控制器，提高系统姿态稳定的收敛速度。

47、首先定义机械臂关节轨迹跟踪误差状态变量为

48、

49、则机械臂关节轨迹跟踪误差动力学方程的状态空间形式为

50、

51、针对步骤2.1简化的控制方程(13)，设计如下快速终端滑模面

52、

53、其中，βa＝d[βa1,βa2,βa3]＞0；0＜αa＜1；。则滑模面的导数有

54、

55、基于滑模面(16)，设计系统的有限时间控制器为

56、

57、其中，ksa1＝d[ksa1,1,ksa1,2,ksa1,3]＞0。

58、下面分析模型(15)在所设计滑模面(16)和控制器(18)作用下的稳定性，稳定性分析过程如下：

59、引入有限时间控制系统稳定性判据：对于系统f(0)＝0，若存在定义于原点邻域u0上的正定连续函数v(x)以及实数a＞0、b＞0、0＜p＜1，满足则该系统是有限时间稳定的。

60、首先对滑模面进行稳定性分析，考虑lyapunov函数

61、

62、对其求时间导数，并带入式(15)-(18)，得到

63、

64、其中，为参数矩阵ksa1对角线上的最小元素。则根据有限时间稳定判据，滑模面(16)是有限时间稳定的，即系统状态能在有限时间内收敛至sa＝0。

65、下面分析系统状态收敛到sa＝0之后的稳定性。考虑lyapunov函数

66、

67、对其求时间导数，由于当sa＝0时有则

68、

69、其中，βa，min为参数矩阵βa对角线上的最小元素。根据有限时间稳定判据，系统状态到达sa＝0后，将进一步在有限时间内收敛至{x1＝0，x2＝0}，即机械臂关节轨迹跟踪误差将在有限时间内收敛至零。

70、模型(3)将在基于状态观测器(5)、动态反馈补偿律(11)、滑模面(16)和控制器(18)的共同作用下，在有限时间内收敛至

71、步骤2.3：在步骤2.1和2.2设计的组合控制律的基础上，构建一阶线性滤波器进行动态解耦，避免由于挠性附件的振动造成姿控系统中不必要的能量消耗。

72、通过滤波解决控制作用激励模态振动。扩张状态观测器本身已具有对测量信号滤波的功能，故只需对反馈补偿律(11)的输出进行滤波。设计一阶线性滤波器为

73、

74、其中βta＞0，根据低阶模态频率选取适当值。

75、实际的机械臂各关节控制力矩指令则为

76、

77、其中，如前所述，ma为机械臂标称质量矩阵，是各机械臂转角θa的函数，需要根据各机械臂转角θa的测量值进行实时计算。

78、步骤2.4：基于步骤2.1所述扩张状态观测器、步骤2.2所述快速终端滑模面和有限时间控制器、步骤2.3所述滤波器，通过采用“状态观测器+有限时间姿态控制器”组合的双闭环控制策略进行轨迹跟踪，提高机械臂的跟踪速度和跟踪精度，实现机械臂回收过程的稳定跟踪控制。

79、基于步骤一建立的机械臂轨迹跟踪控制方程，设计扩张状态观测器，利用对系统状态的观测实现扰动估计，从而为控制补偿提供基础；进而设计有限时间控制器，提高控制系统对未知扰动的鲁棒性，加快姿态稳定的收敛速度；针对回收过程中系统挠性附件会产生的振动，加入滤波器进行动态解耦。通过采用“状态观测器+有限时间姿态控制器”组合的双闭环控制策略进行轨迹跟踪，提高机械臂的跟踪速度和跟踪精度，实现机械臂回收过程的稳定跟踪控制。

80、有益效果：

81、1、本发明公开的针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法，通过设计扩张状态观测器观测机械臂各关节的状态，对实时的扰动进行快速估计和补偿，进而设计快速终端滑模面和有限时间控制器，提高控制系统对未知扰动的鲁棒性，加快姿态稳定的收敛速度，并针对系统挠性附件振动会产生的能量消耗问题，构建一阶线性滤波器进行动态解耦，实现对大挠性组合体的稳定回收控制。

82、2、本发明公开的针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法，采用“状态观测器+有限时间姿态控制器”组合的双闭环控制策略。内环以机械臂各关节转角的角速度为输入，利用二阶扩张状态观测器，输出不确定量的估计值，所述不确定量在外环反馈控制器输出控制量中加以补偿，能够提高轨迹跟踪控制的速度和精度，实现机械臂回收过程的稳定跟踪控制。

83、3、本发明公开的针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法，在设计的扩张状态观测器基础上，对机械臂系统的七个通道均采用有限时间控制律设计稳定性的有限时间姿态控制器，加快系统姿态稳定的收敛速度。对每个通道均采用有限时间控制律，较之传统的pd控制器，收敛速度快、抗干扰和鲁棒性能好。

84、4、本发明公开的针对多自由度机械臂回收失效卫星的轨迹跟踪控制方法，采用基于kane方程的自动组集算法建立大挠性组合体的动力学方程，进而得到机械臂系统的动力学方程，从而建立机械臂轨迹跟踪的控制方程。kane方程可以避免繁琐的求导运算，得到的动力学方程形式简单，物理意义明确，便于控制器的设计，适用于复杂的航天器系统建模。