CN110722557A - 一种平台-机械臂一体化控制方法 - Google Patents

Abstract

本发明公开了一种平台‑机械臂一体化控制方法，应用于空间机械臂对非共基座目标的精确捕获任务，其结合平台‑机械臂组合体的动力学模型，基于平台测量信息及机械臂关节角反馈；通过路径规划补偿以降低周期运算带来的固有延时问题，另外结合平台‑机械臂组合体动力学方程，求解平台与机械臂动力学耦合项在的诸多非线性项，并通过开环前馈补偿的形式实现平台与机械臂动力学解耦，结合闭环反馈修正，通过前馈补偿+闭环反馈的控制方式实现平台与机械臂的一体化控制，降低组合体动力学中非线性耦合项等对控制精度的影响，以实现抓捕精度的提高。本发明对相对运动变化缓慢的目标进行抓捕时效果更佳。

Description

一种平台-机械臂一体化控制方法

技术领域

本发明涉及空间安全与维护技术，具体涉及一种基于平台-机械臂一体化控制方法。

背景技术

随着航天技术应用领域的飞速拓展，空间在轨服务受到越来越多的重视，其中机械臂在在轨维护领域发挥了不可或缺的作用，其中不乏一些基于机械臂的精细操作任务。对于机械臂与操作目标非共平台安装的操作任务来讲，除去对机械臂本身的精确控制外，还要实现对平台自身的高精度位姿保持控制，而平台-机械臂组合体动力学具有高度的动态非线性耦合特性，且在机械臂质量惯量占比较大的情况下，这一特性尤为突出。所以如何实现机械臂及平台的精确控制，是实现空间非共平台机械臂精细操作任务的关键。

目前机械臂与平台的一体化控制技术，多集中于装有多臂系统的协调控制领域，通过多臂的协调规划保证平台的位姿变化。部分研究针对冗余机械臂进行无扰规划，通过减小对平台的扰动体现一体化控制。还有研究单纯对平台进行机械臂扰动的补偿或单纯进行机械臂的补偿规划，无法有效利用机械臂与平台之间的信息交互，实现平台与机械臂的一体化工作。

为了提高基于机械臂抓捕非共基座目标捕获任务的抓捕精度，研发一种平台-机械臂一体化控制方法实为必要。

发明内容

本发明的目的在于提供一种平台-机械臂一体化控制方法，根据相对测量设备的测量信息，结合组合体动力学耦合特性进行分析，在进行独立解耦控制的基础上充分利用信息交互进行耦合项补偿以同时提升平台与机械臂的控制精度。

为了达到上述目的，本发明通过以下技术方案实现：

一种平台-机械臂一体化控制方法，包含以下过程：

S1、对机械臂末端与目标的相对位置、相对姿态进行差分滤波，预判机械臂本周期运动过程中相对位置与相互姿态的变化量，并在逆运动学求解中加以补偿，得到本周期的期望关节角后用以进行关节的路径规划；

S2、基于机械臂的关节角信息，进行差分滤波得到关节角速度信息，再进行二次差分滤波得到机械臂的关节角加速度信息；根据平台的惯性角速度，差分滤波得到平台的惯性角加速度信息，结合平台-机械臂组合体的动力学方程，求解平台轨道动力学、平台姿态动力学及机械臂动力学的非线性耦合项；

S3、将步骤S2求得的各耦合项作为前馈量引入控制，以实现平台与机械臂的动力学解耦；同时将机械臂系统和平台系统分别作为独立的二阶系统进行反馈控制项操作，并结合前馈项和反馈项，实现平台-机械臂组合体的一体化解耦补偿控制。

优选地，所述步骤S1中，进一步包含：

通过手眼相机或双目相机测量获知当前机械臂末端与目标的相对位置、相对姿态r_k、

分别通过与上周期相对位置、相对姿态r_k-1、

进行差分滤波后，得到当前机械臂末端与目标的相对位置变化率、相对姿态变化率

有：

式中，T_s为手眼相机或双目相机测量更新周期；

设定机械臂两次路径规划之间的时间间隔为T_q，预判本次路径规划中的相对位置变化量与相对姿态变化量分别为：

结合当前时刻测量得到的机械臂末端与目标的相对位置、相对姿态r_k、

为了在时间间隔T_q后实现对目标点的捕获，设定目标抓捕点到机械臂末端的相对位姿矩阵为：

在机械臂确定后，根据各关节角信息以及机械臂正运动学方程，求得机械臂末端到基座坐标系的相对位姿矩阵⁶A₀(q_k)，q_k为当前关节角；

进一步，求得目标到机械臂基座的期望相对位姿矩阵：

每周期可得到明确的位姿转换矩阵

阵，并令：

则通过矩阵各元素对应，求得使机械臂末端与目标相对位姿趋于一致的期望关节角q_d。

优选地，所述步骤S2中，进一步包含：

通过平台测量得到平台的加速度

惯性角速度ω，将平台的角速度ω进行差分滤波得到平台的角加速度

以及将机械臂实时反馈的关节角q进行差分滤波得到关节角速度

且将关节角速度

进行二次差分滤波得到关节角加速度

采用的平台-机械臂一体化动力学方程为如下形式：

式中，m为系统质量，包含机械臂及平台质量；H_s(q)为系统相对于平台质心系原点的静矩；H_q1(q)为机械臂各杆相对自身连杆系的静矩；I_bt(q)为系统相对平台质心系的转动惯量；H_q2(q)为机械臂各杆相对平台质心系的耦合惯量矩阵；

为平台平动方程的耦合非线性项；

为平台转动方程的耦合非线性项；

为机械臂转动方程的耦合非线性项；F_b为平台轨控控制力；T_b为平台姿控控制力矩；T_q为关节控制力矩；

获取的平台轨控前馈项、平台姿控前馈项、机械臂各关节控制力矩的前馈项分别如下：

优选地，所述步骤S3中，进一步包含：

将所述步骤S2求得的各前馈项引入控制系统，得到各控制输出为：

F_b＝F_bd+F_bc

T_b＝T_bd+T_bc

T_q＝T_qd+T_qc

其中，F_bc、T_bc、T_qc为反馈控制项，描述为如下形式：

式中，K_op、K_od、K_oi、K_ap、K_ad、K_ai、K_qp、K_qd、K_qi为正控制参数；Δr_b为平台位置控制量；Δθ为平台姿态控制量；Δq为关节角控制量。

与现有技术相比，本发明的有益效果在于：本发明的平台-机械臂一体化控制方法，充分利用平台测量信息及机械臂自身的关节角信息反馈，一方面通过路径规划补偿以降低周期运算带来的固有延时问题；另一方面采用简单常规的解耦控制算法，在此基础上实现了平台平动、转动控制及机械臂关节转动控制耦合项及非线性项的解算补偿，同时提高了平台、机械臂的控制精度，本发明取得了平台-机械臂高精度控制新思路以及可以扩展到协同抓捕等有益效果。

附图说明

图1为本发明的平台-机械臂一体化控制方法原理示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明以安装在飞行器平台上的六自由度冗余机械臂为例，进行本发明的具体实施方式描述。本发明的一体化控制策略体现在两个层面：一是机械臂抓捕控制层面，基于手眼相机、双目相机等设备的测量信息，预判机械臂末端与目标抓捕点的相对运动，在路径规划中进行补偿，已降低周期运算时间带来的固有时延问题；二是卫星平台的稳定控制层面，通过解耦机械臂与平台动力学，推导得到机械臂对飞行器平台的反作用力和力矩作为平台控制的前馈项，平台通过“前馈补偿+闭环修正”的方式提高平台控制精度。

如图1所示，本发明提供一种平台-机械臂一体化控制方法，包括如下的步骤：

步骤S1、首先对机械臂末端与目标之间的相对位置、相对速度(也称为相对姿态)进行差分滤波，根据机械臂的轨迹规划周期预判机械臂本周期运动过程中相对位姿(即相对位置、相对速度)的变化量，并在逆运动学求解中加以补偿，得到机械臂的本周期的期望关节角后，进行关节的路径规划设计。

步骤S2、根据机械臂实时反馈的关节角信息，进行差分滤波得到关节角速度信息，再进行二次差分滤波得到机械臂的关节角加速度信息；根据平台的惯性角速度，进行差分滤波得到平台的惯性角加速度信息；结合平台与组合体的动力学方程，分别求解平台轨道动力学、平台姿态动力学及机械臂动力学的非线性耦合项。

步骤S3、将步骤S2求得的诸多耦合项作为前馈量引入控制，以实现平台与机械臂的动力学解耦；同时将机械臂系统和平台系统分别作为独立的二阶系统进行反馈控制项设计。同时结合“前馈项”和“反馈项”，实现“平台-机械臂”组合体的一体化解耦补偿控制。

所述步骤S1中的通过路径规划补偿，可以降低周期运算带来的固有延时问题，具体如下：

通过手眼相机及平台相机等测量手段，可以得到当前机械臂末端(第6关节坐标系)与目标的相对位置、相对姿态r_k、

分别通过与上周期相对位姿(相对位置、相对姿态)r_k-1、

数据差分滤波后，可近似得到当前机械臂末端与目标的相对位置变化率、相对姿态变化率

有：

其中，T_s手眼相机或双目相机测量更新周期。

考虑机械臂两次路径规划之间的时间间隔为T_q，可预判本次路径规划中的相对位置变化量与相对姿态变化量分别为：

结合当前时刻测量得到的机械臂末端与目标的相对位姿r_k、

可知若要在T_q后实现对目标点的捕获，目标抓捕点到机械臂末端(第6关节坐标系)的相对位姿矩阵为：

同时在机械臂确定后，根据各关节角信息，可以根据现有技术中成熟的机械臂正运动学方程，求得机械臂末端(第6关节坐标系)到基座坐标系(0)的相对位姿矩阵⁶A₀(q_k)，q_k为当前关节角。既而可求得目标到机械臂基座的期望相对位姿矩阵：

在测量信息明确的情况下，每周期可得到明确的位姿转换矩阵

阵。令

通过矩阵各元素对应，可求得使机械臂末端与目标相对位姿趋于一致的期望关节角q_d，此为成熟算法，本处不再赘述。

所述步骤S2中，所述的求解平台轨道动力学、平台姿态动力学及机械臂动力学的非线性耦合项，进一步包括如下步骤：

通过平台测量得到平台的加速度

惯性角速度ω；

将平台的角速度ω进行差分滤波后可得平台的角加速度

以及将机械臂实时反馈的关节角q进行差分滤波后可得关节角速度

且将关节角速度

进行二次差分滤波得到关节角加速度

现有技术中成熟的平台-机械臂一体化动力学方程有如下形式：

其中，m为系统质量，包含机械臂及平台质量；H_s(q)为系统相对于平台质心系原点的静矩；H_q1(q)为机械臂各杆相对自身连杆系的静矩；I_bt(q)为系统相对平台质心系的转动惯量；H_q2(q)为机械臂各杆相对平台质心系的耦合惯量矩阵；为平台平动方程的耦合非线性项；

为平台转动方程的耦合非线性项；

为机械臂转动方程的耦合非线性项；F_b为平台轨控控制力；T_b为平台姿控控制力矩；T_q为关节控制力矩。

结合“平台-机械臂”组合体的动力学方程，可以得到平台轨控前馈项、平台姿控前馈项、机械臂各关节控制力矩的前馈项：

所述步骤S3中，进一步包括：

步骤S2求得各前馈项后，引入控制系统，得到各控制输出为：

F_b＝F_bd+F_bc

T_b＝T_bd+T_bc

T_q＝T_qd+T_qc

其中，F_bc、T_bc、T_qc为反馈控制项，采用常规的PID控制策略，可描述为如下形式：

式中，K_op、K_od、K_oi、K_ap、K_ad、K_ai、K_qp、K_qd、K_qi为正控制参数；Δr_b为平台位置控制量；Δθ为平台姿态控制量；Δq为关节角控制量。

通过该控制算法作用后，结合组合体动力学，有：

整个组合体系统解耦成三个独立的二阶控制系统模块。

综上所述，本发明的平台-机械臂一体化控制方法可应用于空间机械臂对非共基座目标的精确捕获任务。对于安装有机械臂的空间卫星平台来讲，机械臂的动力学与平台的动力学具有较强的耦合特性，单独控制机械臂运动或单独控制平台运动都会对彼此产生较强的干扰，如平台运动的角加速度、角速度、加速度特性，会引起机械臂关节动力学中非线性项的复杂变化，机械臂各关节的运动也会对平台的控制产生较大的干扰。同时，平台的控制精度会影响机械臂末端与抓捕目标之间相对位姿测量，继而影响机械臂的路径规划，影响最终抓捕精度。本发明结合平台-机械臂组合体的动力学模型，基于平台测量信息及机械臂关节角反馈，设计了一种平台-机械臂一体化控制策略，一方面通过路径规划补偿以降低周期运算带来的固有延时问题，另一方面结合“平台-机械臂”组合体动力学方程，求解平台与机械臂动力学耦合项在的诸多非线性项，并通过开环前馈补偿的形式实现平台与机械臂动力学解耦，结合闭环反馈修正，以“前馈补偿+闭环反馈”的控制方式实现平台与机械臂的一体化控制，降低组合体动力学中非线性耦合项等对控制精度的影响，以实现抓捕精度的提高。该方法对相对运动变化缓慢的目标进行抓捕时效果更佳。

尽管本发明的内容已经通过上述优选实施例作了详细介绍，但应当认识到上述的描述不应被认为是对本发明的限制。在本领域技术人员阅读了上述内容后，对于本发明的多种修改和替代都将是显而易见的。因此，本发明的保护范围应由所附的权利要求来限定。

Claims (4)

1.一种平台-机械臂一体化控制方法，其特征在于，包含以下过程：

S1、对机械臂末端与目标的相对位置、相对姿态进行差分滤波，预判机械臂本周期运动过程中相对位置与相互姿态的变化量，并在逆运动学求解中加以补偿，得到本周期的期望关节角后用以进行关节的路径规划；

S2、基于机械臂的关节角信息，进行差分滤波得到关节角速度信息，再进行二次差分滤波得到机械臂的关节角加速度信息；根据平台的惯性角速度，差分滤波得到平台的惯性角加速度信息，结合平台-机械臂组合体的动力学方程，求解平台轨道动力学、平台姿态动力学及机械臂动力学的非线性耦合项；

S3、将步骤S2求得的各耦合项作为前馈量引入控制，以实现平台与机械臂的动力学解耦；同时将机械臂系统和平台系统分别作为独立的二阶系统进行反馈控制项操作，并结合前馈项和反馈项，实现平台-机械臂组合体的一体化解耦补偿控制。

2.如权利要求1所述的平台-机械臂一体化控制方法，其特征在于，所述步骤S1中，进一步包含：

通过手眼相机或双目相机测量获知当前机械臂末端与目标的相对位置、相对姿态r_k、θ_k，分别通过与上周期相对位置、相对姿态r_k-1、θ_k-1进行差分滤波后，得到当前机械臂末端与目标的相对位置变化率、相对姿态变化率

有：

式中，T_s为手眼相机或双目相机测量更新周期；

设定机械臂两次路径规划之间的时间间隔为T_q，预判本次路径规划中的相对位置变化量与相对姿态变化量分别为：

结合当前时刻测量得到的机械臂末端与目标的相对位置、相对姿态r_k、θ_k，为了在时间间隔T_q后实现对目标点的捕获，设定目标抓捕点到机械臂末端的相对位姿矩阵为：

在机械臂确定后，根据各关节角信息以及机械臂正运动学方程，求得机械臂末端到基座坐标系的相对位姿矩阵⁶A₀(q_k)，q_k为当前关节角；

进一步，求得目标到机械臂基座的期望相对位姿矩阵：

每周期可得到明确的位姿转换矩阵

阵，并令：

则通过矩阵各元素对应，求得使机械臂末端与目标相对位姿趋于一致的期望关节角q_d。

3.如权利要求2所述的平台-机械臂一体化控制方法，其特征在于，所述步骤S2中，进一步包含：

通过平台测量得到平台的加速度惯性角速度ω，将平台的角速度ω进行差分滤波得到平台的角加速度

以及将机械臂实时反馈的关节角q进行差分滤波得到关节角速度

且将关节角速度

进行二次差分滤波得到关节角加速度

采用的平台-机械臂一体化动力学方程为如下形式：

式中，m为系统质量，包含机械臂及平台质量；H_s(q)为系统相对于平台质心系原点的静矩；H_q1(q)为机械臂各杆相对自身连杆系的静矩；I_bt(q)为系统相对平台质心系的转动惯量；H_q2(q)为机械臂各杆相对平台质心系的耦合惯量矩阵；

为平台平动方程的耦合非线性项；

为平台转动方程的耦合非线性项；

为机械臂转动方程的耦合非线性项；F_b为平台轨控控制力；T_b为平台姿控控制力矩；T_q为关节控制力矩；获取的平台轨控前馈项、平台姿控前馈项、机械臂各关节控制力矩的前馈项分别如下：

4.如权利要求1所述的平台-机械臂一体化控制方法，其特征在于，所述步骤S3中，进一步包含：

将所述步骤S2求得的各前馈项引入控制系统，得到各控制输出为：

F_b＝F_bd+F_bc

T_b＝T_bd+T_bc

T_q＝T_qd+T_qc

其中，F_bc、T_bc、T_qc为反馈控制项，描述为如下形式：

式中，K_op、K_od、K_oi、K_ap、K_ad、K_ai、K_qp、K_qd、K_qi为正控制参数；Δr_b为平台位置控制量；Δθ为平台姿态控制量；Δq为关节角控制量。

Images