基于扩展状态观测与补偿的空间机器人自抗扰协调控制方法与流程

本发明属于航天器姿态控制领域，涉及一种基于干扰补偿的空间机器人自抗扰协调控制方法，具体涉及一种基于扩展状态观测与补偿的空间机器人自抗扰协调控制方法。

背景技术：

空间机器人在在轨服务任务中扮演着关键的角色，如在轨维修、燃料加注、装配等。但不同于基座固定的地面机器人，空间机器人基座姿态会受到来自机械臂运动以及和目标接触所产生的扰动的影响，为了满足空间机器人工作过程中保持对地通信和太阳帆板指向等任务需要，在满足机械臂运动的同时保持空间机器人基座载体稳定的协调控制是十分必要的。再者，由于实际系统中存在的强非线性、多输入外力矩、参数不确定性以及干扰的影响，具有鲁棒性的协调控制方法也是应该考虑的问题。

yoshida等提出了零反作用机动的思想，按照该思想规划机械臂的运动，对基座姿态不产生任何影响，但对于非运动学冗余的机械臂来说，产生的规划路径非常有限。所以应采用主动姿态控制和机械臂运动控制结合的方式进行协调控制。papadopoulos等提出转移雅克比矩阵形式对基座姿态和机械臂运动进行协调控制，但并未考虑到星载计算机和姿态执行机构的限制条件。m.oda等提出通过估计机械臂运动对基座的干扰力矩，通过姿态执行机构对干扰力矩进行补偿的方法。但未考虑到干扰和参数不确定性的存在，该方法并没有较好的鲁棒性。

技术实现要素：

为了克服上述现有技术存在的缺陷，本发明的目的在于提供一种基于扩展状态观测与补偿的空间机器人自抗扰协调控制方法，以解决星载计算机能力受限以及存在干扰和参数不确定情形下，机械臂运动同时能够保证空间机器人基座的姿态稳定的协调控制问题。该方法通过扩展状态观测器对基座干扰力矩进行补偿，进而通过自抗扰控制技术以实现空间机器人的协调控制。

本发明是通过以下技术方案来实现：

本发明公开的一种基于扩展状态观测与补偿的空间机器人自抗扰协调控制方法，包括以下步骤：

步骤一：对带有反作用飞轮的空间机器人的动力学方程进行建模仿真；

步骤二：实时估计机械臂运动相对空间机器人基座产生的干扰角动量；

步骤三：利用跟踪微分器估计机械臂运动相对空间机器人基座产生的干扰力矩；

步骤四：利用扩展状态观测器补偿环境干扰力矩；

步骤五：设计自抗干扰协调控制器进行自抗扰协调控制。

优选地，步骤一所述建模仿真，具体如下：

其中，为广义坐标，φm＝(φ1,φ2,…,φn)^t表示机械臂的关节角，φs＝(α,β,γ)^t表示空间机器人基体的姿态；τ＝[τ1,...,τn,τw1,τw2,τw3]^t，τi,i＝1,2,...n代表关节力矩，τw1,τw2,τw3分别代表绕着滚转轴、俯仰轴及偏航轴的飞轮力矩；

矩阵a为转动惯量矩阵；

矩阵b[(n+3)×(n+3)]为非线性项；

a＝hn；

n为坐标变换矩阵

各分块矩阵表示如下：

jtk＝[z1×(rk-p1),z2×(rk-p2),...,zk×(rk-pk),0,...,0]∈r^3×n

jrk＝[z1,z2,...,0,...0]

根据基体、机械臂运动和飞轮系统角动量守恒，得到：

hw＝iw；

由角动量守恒，得到：

式中，mk为第k个臂杆的质量，m表示空间机器人总质量，zk为第k个臂杆转轴的方向矢量，rk-pm为第k个臂杆质心相对于第m个臂杆末端的位置矢量，iw表示飞轮的转动惯量矩阵，ik表示第k个臂杆的惯性张量矩阵，i0表示空间机器人基座惯性张量矩阵，为空间机器人基座质心相对于空间机器人系统质心位置矢量的斜对称矩阵。

优选地，步骤二所述实时估计机械臂运动相对空间机器人基座产生的干扰角动量，采用以下近似方法进行计算：

上述近似计算公式，不仅包含了机械臂相对基座运动部分的角动量，也包含了机械臂相对惯性空间运动部分的角动量以及基座相对惯性空间转动的角动量

优选地，步骤三所述利用跟踪微分器估计机械臂运动相对基座产生的干扰力矩，是将步骤二估计出来的干扰角动量信号传输到舵机上，通过微分环节得到补偿干扰力矩，再传输到飞轮上；

具体采用跟踪微分器的离散形式，通过输入估计的干扰角动量，得到补偿的干扰力矩：

设离散系统为：

对上式计算快速控制最优综合函数，得到：

式中，

h为积分步长，r为待定参数；

有离散系统，即快速离散跟踪微分器：

x1(t)是在限制下最快的跟踪输入信号v(t)；

x1(t)充分接近v(t)时，有做v(t)的近似微分。

优选地，步骤四所述的利用扩展状态观测器补偿环境干扰力矩，是将空间机器人模型表示成一个二阶系统：

反应实际情况考虑系统的不确定性：

假设其中代表矩阵a的标称值，δa代表不确定性；则定义一个虚拟的力矩tc*，其与实际力矩tc之间关系为

其中h[e(t)]未知，代表系统的不确定性，且每一部分都已假定有界||hi||≤himax,i＝1,2,...,n+3；

对于二阶系统：

其中，f(x1,x2,t)未知，取新的状态x3＝f(x1,x2,t)，并设则g(x1,x2,t)也为未知函数；

设计扩张状态观测器，使其状态z1,z2,z3分别观测原系统的状态x1,x2,x3，取ε1＝z1-y；则有：

式中，

β01,β02,β03为扩张状态观测器针对不同系统的可调参数。

优选地，步骤五所述设计自抗扰协调控制器，是采用传统pid控制方法与自抗扰控制方法结合，减小星载计算机运算负荷；

u＝-(kds+kp)θs；

其中，θs为空间机器人基座姿态角，kd和kp为pid控制参数

与现有技术相比，本发明具有以下有益的技术效果：

本发明公开的基于扩展状态观测与补偿的空间机器人自抗扰协调控制方法，首先，从空间机器人动力学角度出发，对带有反作用飞轮的空间机器人的动力学方程进行建模仿真，实时估计机械臂运动相对基座产生的干扰角动量，然后利用跟踪微分器估计机械臂运动相对基座产生的干扰力矩，再利用扩展状态观测器补偿环境干扰力矩，最后设计自抗干扰协调控制器进行自抗扰协调控制。该方法适用情形广，无论机械臂是冗余情形还是非冗余情形，均可实现星臂的协调控制；同时，单独设计空间机器人基座姿态控制器，可以减轻星载计算机的运算负荷。该自抗扰协调控制方法针对空间存在各种干扰力矩(太阳光压、重力梯度力矩等)情形下，使得系统具备一定的鲁棒性。通过本发明的控制方法，能够满足空间机器人工作过程中保持对地通信畅通和太阳帆板指向等任务需要。

附图说明

图1为基于扩展状态观测与补偿的空间机器人自抗扰协调控制框图；

图2为自抗扰控制系统框图；

图3为空间机器人系统各坐标系示意图；

图4为空间机器人载体三轴姿态；

图5为空间机器人关节角误差结果图。

具体实施方式

下面结合具体的实施例对本发明做进一步的详细说明，所述是对本发明的解释而不是限定。

本发明公开的基于扩展状态观测与补偿的空间机器人自抗扰协调控制方法，包括以下步骤：

步骤一、对带有反作用飞轮的空间机器人的动力学方程进行建模：

其中，为广义坐标，φm＝(φ1,φ2,…,φn)^t表示机械臂的关节角，φs＝(α,β,γ)^t表示空间机器人基体的姿态；

τ＝[τ1,...,τn,τw1,τw2,τw3]^t，τi,i＝1,2,...n代表关节力矩，τw1,τw2,τw3分别代表绕着滚转轴、俯仰轴及偏航轴的飞轮力矩；

矩阵b[(n+3)×(n+3)]为非线性项，包括哥氏力等；

矩阵a为转动惯量矩阵；

a＝hn；

n为坐标变换矩阵

各分块矩阵表示如下：

jtk＝[z1×(rk-p1),z2×(rk-p2),...,zk×(rk-pk),0,...,0]∈r^3×n

jrk＝[z1,z2,...,0,...0]

根据基体、机械臂运动和飞轮系统角动量守恒，得到：

hw＝iw；

由角动量守恒，得到：

式中mk为第k个臂杆的质量，m表示空间机器人总质量，zk为第k个臂杆转轴的方向矢量，rk-pm为第k个臂杆质心相对于第m个臂杆末端的位置矢量，iw表示飞轮的转动惯量矩阵，ik表示第k个臂杆的惯性张量矩阵，i0表示空间机器人基座惯性张量矩阵，为空间机器人基座质心相对于空间机器人系统质心位置矢量的斜对称矩阵。

步骤二、实时估计机械臂运动相对基座产生的干扰角动量：

由于空间机器人基座上面只安装有位置、速度的传感器，所以在工程上面将机械臂运动产生的干扰作用表示为角动量的形式更合适。以飞轮为代表的姿态控制执行机构，是通过吸收角动量来产生控制作用，因此发给飞轮的指令以角动量的形式更为方便。

考虑精确计算导致计算量过大，增加星载计算机的工作负荷，所以采用以下近似方法进行计算：

上述近似计算公式，不仅包含了机械臂相对基座运动部分的角动量，也包含了机械臂相对惯性空间运动部分的角动量以及基座相对惯性空间转动的角动量

步骤三、利用跟踪微分器估计机械臂运动相对基座产生的干扰力矩：

将估计出来的干扰角动量信号传输到舵机上后，要通过微分环节得到补偿干扰力矩，再传输到飞轮上。在此步骤采用跟踪微分器的离散形式，通过输入估计的干扰角动量，得到补偿的干扰力矩。

设离散系统为：

直接对上式求“快速控制最优综合函数”，得到：

式中，

h为积分步长，r为待定参数；(注：方程中的其他字母均是中间变量，使方程形式简洁，并无实际意义)。

有离散系统，即快速离散跟踪微分器：

x1(t)是在限制下最快的跟踪输入信号v(t)；

x1(t)充分接近v(t)时，有做v(t)的近似微分。

步骤四、利用扩展状态观测器补偿环境力矩：

上述空间机器人模型可以表示成为一个二阶系统：

为了反应实际情况，应考虑系统的不确定性(例如矩阵a和b都不是精确已知的)。假设其中代表矩阵a的标称值，δa代表不确定性。则定义一个虚拟的力矩tc^*，其与实际力矩tc之间关系为

其中h[e(t)]未知，代表系统的不确定性，且每一部分都已假定有界||hi||≤himax,i＝1,2,...,n+3；

对于二阶系统：

其中，f(x1,x2,t)未知，取新的状态x3＝f(x1,x2,t)，并设则g(x1,x2,t)也为未知函数；

设计扩张状态观测器，使其状态z1,z2,z3分别观测原系统的状态x1,x2,x3，取ε1＝z1-y；则有：

式中，

β01,β02,β03为扩张状态观测器针对不同系统的可调参数。

步骤五、设计自抗扰协调控制器：

采用工程上面传统的pid控制方法与自抗扰控制技术结合，既能够使得自抗扰控制技术发挥其优势，又不用引入复杂控制律，减小星载计算机运算负荷。

u＝-(kds+kp)θs；

其中，θs为空间机器人基座姿态角，kd和kp为pid控制参数。

实施例

结合图1、图2、图3说明本实施例，通过图1可知，控制律包含三个部分，步骤一到步骤三：通过建立空间机器人数学模型和角动量表达式来估计机械臂运动的角动量，并通过跟踪微分器估计出机械臂运动产生的干扰力矩进而作为补偿控制力矩；步骤四：通过扩展状态观测器补偿由于存在不确定性产生的干扰力矩；步骤五：通过期望姿态命令与实时姿态信息的差值从而推导出pid控制律。

通过图3可知，空间机器人由三自由度转动副机械臂和卫星基座组成，机械臂的运动将使空间机器人基座偏离滚转、俯仰和偏航轴。空间机器人的质量参数如表1所示。

表1

控制器的目的是使得机械臂和基座姿态都稳定到预设的期望值。

假定：

机械臂关节运动的初始值为：φm0＝[50,65,-105deg]^t；期望关节角为φmd＝[60,60,-120deg]^t；

基座姿态角初始值为：φs0＝[-10,5,5deg]^t；期望姿态角为：φsd＝[0,0,0deg]^t。

矩阵a和b均有10％的不确定性。外部的扰动力均为谐波信号。

所设计的基于扩展状态观测与补偿的空间机器人自抗扰协调控制方法的实际效果图如图4、图5所示。可以看出，姿态角和关节角均平稳收敛到期望值，由于基座姿态控制系统补偿了机械臂运动产生的干扰力矩和环境干扰力矩，基座姿态较关节角更快的收敛到期望值。通过该控制器可以实现空间机器人机械臂运动和姿态运动的协调控制，由于该控制方法简单可行，能够应用在工程实际中。

综上所述，本发明提供基于扩展状态观测与补偿的空间机器人自抗扰协调控制方法，能够解决星载计算机能力受限以及存在干扰和参数不确定情形下，机械臂运动同时能够保证空间机器人基座的姿态稳定的协调控制问题。该方法通过扩展状态观测器对基座干扰力矩进行补偿，进而通过自抗扰控制技术以实现空间机器人的协调控制。