CN112549030B - 一种基于抗干扰滤波的空间机械臂柔性关节位姿估计方法 - Google Patents

Abstract

本发明涉及一种基于抗干扰滤波的空间机械臂柔性关节位姿估计方法，首先，考虑空间机械臂系统受到的外部扰动力矩、关节摩擦力矩、执行机构噪声及传感器噪声，建立柔性关节滤波模型；其次，根据关节编码器的量测数据对外部扰动和关节摩擦进行估计，计算干扰估计增益以保证其在最小方差意义下的无偏估计；最后，将干扰估计与扩展卡尔曼滤波相结合，计算滤波增益矩阵以保证状态估计误差在最小方差意义下的最优性，最终实现各关节角度、角速度的准确估计。本发明依托干扰估计和前馈补偿技术改进传统的扩展卡尔曼滤波方法，实现复杂干扰下空间机械臂关节状态的精确估计，为高精度在轨作业任务中空间机械臂的末端定位及运动控制提供支持。

Description

一种基于抗干扰滤波的空间机械臂柔性关节位姿估计方法

技术领域

本发明涉及一种基于抗干扰滤波的空间机械臂柔性关节位姿估计方法，空间在轨作业任务中空间机械臂系统技术领域。

背景技术

近年来，随着空间技术的飞速发展，特别是空间站、航天飞机等航天器的成功应用，各航天大国对于深空探测、科学实验、博弈对抗等航天任务的需求也不断提升。空间机械臂作为在轨支持、服务的一项关键执行机构对于顺利完成日益精密化与多样化的航天器空间任务至关重要。为了满足空间机械臂系统高精度的在轨任务需求，必须对其航天器基座、串联多关节进行准确的位姿量测，否则，可能导致机械臂难以快速对准工作点，进而影响空间机械臂的在轨任务品质。然而，在实际系统中，由于谐波转速装置产生的关节柔性，仅依靠关节电机端位置传感器无法反映关节的实际转动，需要在滤波模型中考虑柔性影响；同时，在轨作业的空间机械臂系统将面临更加复杂的工作环境，不可避免地受到未知环境扰动、关节摩擦、执行机构噪声以及传感器噪声等的严重影响，因此，为实现干扰情形下高精度的空间机械臂柔性关节系统位姿测量，亟需设计一种具备抗干扰能力和满足高精度要求的柔性关节状态滤波方法。

近年来，针对空间机械臂系统的关节角测量问题，很多学者从不同角度进行了大量研究。从滤波模型角度讲，许多基于空间机械臂的动力学设计的滤波方法在建模过程中没有充分考虑空间机械臂由谐波减速器引起的关节柔性问题。从状态估计方法角度讲，现阶段研究较为广泛的滤波方法有卡尔曼滤波、鲁棒滤波以及粒子滤波等。卡尔曼滤波给出了线性和高斯条件下滤波问题的最优解，鲁棒滤波主要解决噪声特性未知或模型参数不确定时的估计问题，粒子滤波作为近年来的热门研究方向，优势是适用于非线性和非高斯估计问题。然而，针对实际空间机械臂关节系统在状态量测中受到外部扰动、关节摩擦等未知特性干扰的影响问题，现有方法很少考虑对其进行直接有效的估计和前馈补偿，可能导致关节状态估计精度受限。例如，专利申请号201810883670.0中基于空间机械臂动力学模型的关节角粒子滤波方法，忽略了系统在轨面临的未知特性干扰的影响；专利申请号201910717833.2中基于LSO与UDE结合的鲁棒控制回路未考虑空间机械臂执行机构噪声和传感器噪声对状态量测精度的影响。因此，要设计理想的关节抗干扰滤波器，需要建立考虑关节柔性和未知特性干扰的滤波模型，借助关节传感器量测数据对干扰进行估计、补偿，提升空间机械臂系统的抗干扰能力和估计精度。

发明内容

本发明的技术解决问题是：针对现有空间机械臂系统未能充分考虑关节柔性和未知特性干扰的影响，导致关节角和角速度测量精度下降的问题，克服传统卡尔曼滤波方法的不足，提出一种基于抗干扰滤波的空间机械臂柔性关节位姿估计方法，在考虑关节柔性的前提下，基于柔性关节动力学建立关节滤波模型，通过对未知特性的关节摩擦及外部干扰的快速估计和前馈补偿，改进关节状态滤波器的抗干扰能力，提高关节角度和角速度估计的精度，进而满足空间机械臂柔性关节系统对抗干扰能力和高精度测量的要求。

本发明的技术解决方案为：一种基于抗干扰滤波的空间机械臂柔性关节位姿估计方法，其特征在于包括以下步骤：首先，考虑空间机械臂系统柔性关节所受到的外部扰动力矩、关节摩擦力矩、执行机构噪声以及传感器噪声，基于柔性关节动力学建立关节滤波模型并进行线性化、离散化处理；其次，根据关节编码器的量测数据对外部扰动和关节摩擦进行估计，借助高斯马尔可夫定理计算增益以保证干扰在最小方差意义下的无偏估计；最后，将干扰估计与扩展卡尔曼滤波相结合，计算滤波增益矩阵以保证状态估计误差在最小方差意义下的最优性，最终实现各关节角度、角速度的准确估计。上述环节的具体设计步骤如下：

第一步，考虑空间机械臂系统受到的外部扰动、关节摩擦、执行机构噪声及传感器噪声，并基于柔性关节动力学建立干扰情形下关节滤波模型。

空间机械臂关节是安装谐波减速装置的柔性结构，同时受到外部扰动力矩、关节摩擦力矩、执行机构噪声以及传感器噪声多种干扰，针对此问题，建立空间机械臂柔性关节动力学模型如下：

式中，q_l和q_m分别为柔性关节负载端和电机端的转动角，n为谐波减速比，M(q_l)、J_m分别为负载端和电机端的转动惯量，均为正定矩阵，

为离心力与科氏力项，k、D分别为关节的刚度系数和结构变形阻尼系数，τ、τ_m分别为关节输出力矩、电机控制力矩，τ_f、τ_ext分别表示关节电机端的摩擦力矩和负载端的外部扰动力矩。

本发明在关节负载端使用光电编码器对关节角位移信号进行采集与处理，直接提供关节转角及转速等信息，在电机端则使用双通道旋转变压器来直接反馈电机转子的位置及转速信息，将关节位置传感器的量测噪声统一看作高斯白噪声。

设状态量

干扰量d＝[τ_ext ^T τ_f ^T]^T，量测输出z，则根据空间机械臂柔性关节动力学，关节滤波模型可整理如下：

其中，w和v分别为连杆和电机的内部噪声，均为高斯白噪声，具有已知的协方差矩阵Q＝E[ww^T]和R＝E[vv^T]；f(x,τ_m)为滤波模型的非线性函数，干扰的参数矩阵

量测参数H＝I，I为单位矩阵。

参考扩展卡尔曼滤波方法，将系统状态方程进行线性化、离散化处理，最终得到如下滤波方程：

其中，A_k＝I+F(t_k)△T，t_k为传感器量测时间点，△T为传感器量测的时间间隔，

H_k＝I，参数阵F可表示如下：

其中

易得(A_k,H_k)是可观测的，并且初始状态x₀与w_k、v_k均无关。

第二步，针对滤波模型中具有未知特性的干扰量d_k，根据关节编码器的量测数据对其进行快速估计，并借助高斯马尔可夫定理计算增益以保证干扰在最小方差意义下的无偏估计。

在干扰估计过程中，由于系统所受干扰特性未知，没有确定的模型描述等先验信息，仅利用新的量测信息和当前时刻的量测估计之间的差即“新息”来计算干扰估计值

即：

其中

为上一时刻状态估计值，M_k∈R^m×p为待设计的干扰估计增益。

设量测值的新息

代入上式得：

其中，

则有：

假设

是无偏的，即E[e_k]＝0。若要求

无偏，即

需要满足如下条件：

M_kH_kG_k-1＝I_m

设干扰估计误差

则：

设一步预测协方差P_k|k-1＝A_kP_k-1A_k ^T+Q_k-1，P_k-1为上一时刻状态估计协方差，则e_k的方差阵

为：

在此基础上，设计干扰估计增益M_k为：

其中F_k＝H_kG_k-1，

则

为d_k-1的无偏最小方差估计，干扰估计的误差协方差由

确定。

第三步，基于干扰估计值

采用改进的卡尔曼滤波方法进行时间更新、量测更新，实现各关节角度、角速度的精确估计，计算滤波增益矩阵以保证状态估计误差在最小方差意义下的最优性。

基于干扰估计值

时间更新和量测更新过程如下：

其中K_k∈R^n×p为待设计的滤波增益。

设系统状态的一步预测误差

则：

其中A_k|k-1＝(I-G_k-1M_kH_k)A_k-1，w_k|k-1＝(I-G_k-1M_kH_k)w_k-1-G_k-1M_kv_k。

系统状态的一步预测协方差P^x _k|k-1可表示为：

其中Q_k|k-1＝E[w_k|k-1w_k|k-1 ^T]。

设状态估计误差

则：

设中间量S_k ^*＝E[w_k|k-1v_k ^T]＝-G_k-1M_kR_k，则状态估计协方差P_k|k可表示为：

其中，

V_k ^*＝P^x _k|k-1H_k ^T+S_k ^*＝A_k|k-1P_k-1|k-1A_k|k-1 ^TH_k ^T+Q_k|k-1H_k ^T+S_k ^*。

在此基础上，设r＝rank[R_k ^*]，α_k∈R^r×p满足α_kR_k ^*α_k ^T满秩，则滤波增益矩阵K_k设计如下：

K_k＝(P^x _k|k-1H_k ^T+S_k ^*)α_k ^T(α_kR_k ^*α_k ^T)^-1α_k

在此增益下，系统状态估计协方差P^x _k|k为：

即可得

为最小方差意义下x_k的无偏估计。至此，抗干扰滤波器给出了对未知特性的多源干扰和状态的最小方差意义下的无偏估计。

本发明与现有技术相比的优点在于：

(1)本发明充分考虑了在轨作业的空间机械臂系统的关节柔性以及所受到的外部扰动力矩、关节摩擦力矩、执行机构噪声以及传感器噪声等干扰，并基于关节动力学建立干扰情形下柔性关节滤波模型；

(2)本发明利用关节编码器的输出数据对未知特性的干扰进行快速估计和前馈补偿，借助高斯马尔科夫定理计算估计增益以保证干扰估计在最小方差意义下的最优性，从而改进了空间机械臂系统的抗干扰能力与量测精度。

(3)本发明在传统卡尔曼滤波方法的基础上加入了干扰估计环节，既继承了卡尔曼滤波方法的实用性，又增强了空间机械臂系统关节状态估计的稳定性和可靠性，为后续的末端定位和关节运动控制提供支持。

附图说明

图1为所发明的基于抗干扰滤波的空间机械臂柔性关节位姿估计方法的实施流程图；

图2为基于抗干扰滤波的空间机械臂柔性关节位姿估计回路结构框图；

图3为基于抗干扰滤波的关节位姿估计方法下多源干扰及关节角度估计效果图，(a)多源干扰估计效果-外部干扰，(b)多源干扰估计效果-右为摩擦干扰，(c)关节角度估计效果。

具体实施方式

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

如图1所示，本发明提供了一种空间机械臂柔性关节系统抗干扰滤波方法，针对含有外部扰动力矩、关节摩擦力矩、执行机构噪声以及传感器噪声多源干扰的柔性关节空间机械臂系统，所提方法充分考虑了空间机械臂的关节柔性和多种干扰类型，建立多源干扰情形下的柔性关节滤波模型，依托干扰估计补偿技术改进传统的扩展卡尔曼滤波方法，实现复杂干扰下空间机械臂关节状态的精确估计，为高精度在轨作业任务中空间机械臂系统的末端定位及运动控制提供支持。

本发明具体设计和实施流程如下：

1、考虑空间机械臂系统受到的外部扰动、关节摩擦、执行机构噪声及传感器噪声，并基于柔性关节动力学建立干扰情形下关节滤波模型如下：

式中，q_l和q_m分别为柔性关节负载端和电机端的转动角，n为谐波减速比，M(q_l)、J_m分别为负载端和电机端的转动惯量，均为正定矩阵，

为离心力与科氏力项，k、D分别为关节的刚度系数和结构变形阻尼系数，τ、τ_m分别为关节输出力矩、电机控制力矩，τ_f、τ_ext分别表示关节电机端的摩擦力矩和负载端的外部扰动力矩。

本发明在关节负载端使用光电编码器对关节角位移信号进行采集与处理，直接提供关节转角及转速等信息，在电机端则使用双通道旋转变压器来直接反馈电机转子的位置及转速信息，将关节位置传感器的量测噪声统一看作高斯白噪声。

设状态量

干扰量d＝[τ_ext ^T τ_f ^T]^T，量测输出z，则空间机械臂关节滤波模型可整理如下：

其中，w和v分别为连杆和电机的内部噪声，均为高斯白噪声，具有已知的协方差矩阵Q＝E[ww^T]和R＝E[vv^T]；f(x,τ_m)为滤波模型的非线性函数，干扰的参数矩阵

量测参数H＝I，I为单位矩阵。

参考扩展卡尔曼滤波方法，将系统状态方程进行线性化、离散化处理，最终得到如下滤波方程：

其中，A_k＝I+F(t_k)△T，t_k为传感器量测时间点，△T为传感器量测的时间间隔，

参数阵F可表示如下：

其中

易得(A_k,H_k)是可观测的，并且初始状态x₀与w_k、v_k均无关。

2、针对滤波模型中具有未知特性的干扰d_k，根据关节编码器的量测数据对其进行快速估计，并借助高斯马尔可夫定理计算增益以保证干扰在最小方差意义下的无偏估计。干扰估计设计如下：

其中

为上一时刻状态估计值，M_k∈R^m×p为待设计的干扰估计增益。

设量测值的新息

可得

其中

则有：

假设

是无偏的，即E[e_k]＝0。若要求

无偏，即

需要满足如下条件：

M_kH_kG_k-1＝I_m

设干扰估计误差

则：

已知一步预测协方差P_k|k-1＝A_kP_k-1A_k ^T+Q_k-1，P_k-1为上一时刻状态估计协方差，则e_k的方差阵

为：

在此基础上，设计干扰估计增益M_k为：

其中F_k＝H_kG_k-1，

则

为d_k-1的无偏最小方差估计，干扰估计的误差协方差由

确定。

3、基于干扰估计值

采用改进的卡尔曼滤波方法进行时间更新、量测更新，实现各关节角度、角速度的精确估计，计算滤波增益矩阵以保证状态估计误差在最小方差意义下的最优性。

基于干扰估计值

时间更新和量测更新过程如下：

其中K_k∈R^n×p为待设计的滤波增益。

设系统状态的一步预测误差

则：

其中A_k|k-1＝(I-G_k-1M_kH_k)A_k-1，w_k|k-1＝(I-G_k-1M_kH_k)w_k-1-G_k-1M_kv_k。

系统状态的一步预测协方差P^x _k|k-1可表示为：

设状态估计误差

则：

设中间量S_k ^*＝E[w_k|k-1v_k ^T]＝-G_k-1M_kR_k，则状态估计协方差P_k|k可表示为：

其中，

V_k ^*＝P^x _k|k-1H_k ^T+S_k ^*＝A_k|k-1P_k-1|k-1A_k|k-1 ^TH_k ^T+Q_k|k-1H_k ^T+S_k ^*。

在此基础上，设r＝rank[R_k ^*]，α_k∈R^r×p满足α_kR_k ^*α_k ^T满秩，则滤波增益矩阵K_k设计如下：

K_k＝(P^x _k|k-1H_k ^T+S_k ^*)α_k ^T(α_kR_k ^*α_k ^T)^-1α_k

在此增益下，系统状态估计协方差P^x _k|k为：

即可得

为最小方差意义下x_k的无偏估计。

如图2所示，在空间机械臂系统中，设计干扰估计环节结合关节传感器量测数据对柔性关节滤波模型中外部干扰、关节摩擦进行快速估计，将干扰估计与扩展卡尔曼滤波相结合，所得关节角度/角速度估计

应用于后续控制器设计，构成完整的柔性关节空间机械臂系统回路。

图3中的(a)和(b)图分别为外部扰动、关节摩擦的干扰估计效果图，可以看出外部扰动估计误差迅速稳定在±0.05Nm范围内，关节摩擦估计误差趋近于零；图3中的(c)为柔性关节负载端角度的估计效果图，可以看出关节负载端角度估计曲线光滑，且未偏离实际角度曲线，说明所设计滤波器可以抵消多源干扰对关节角度/角速度估计的影响，估计效果良好。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (5)

1.一种基于抗干扰滤波的空间机械臂柔性关节位姿估计方法，其特征在于，包括以下步骤：

第一步，根据空间机械臂系统柔性关节受到的外部扰动力矩、关节摩擦力矩、执行机构噪声以及传感器噪声，基于柔性关节动力学建立关节滤波模型；

第二步，根据关节编码器的量测数据对关节滤波模型中外部扰动和关节摩擦进行估计，基于高斯马尔可夫定理计算干扰估计增益，得到在最小方差意义下的无偏干扰估计值；

第三步，基于第二步对关节滤波模型进行干扰补偿，并与扩展卡尔曼滤波相结合，计算滤波增益矩阵以保证估计误差在最小方差意义下的最优性，实现各关节角度、角速度的准确估计；

所述第一步，建立关节滤波模型具体实现如下；

设状态量

干扰量d＝[τ_ext ^T τ_f ^T]^T，传感器量测输出z，其中q_l、q_m分别为柔性关节负载端和电机端的转动角度，τ_f、τ_ext分别为关节电机端的摩擦力矩和负载端的外部扰动力矩；

基于空间机械臂柔性关节动力学建立柔性关节滤波模型，并进行线性化、离散化处理如下：

其中，w_k和v_k分别为执行机构噪声和传感器噪声，均为高斯白噪声，具有已知的协方差矩阵Q_k＝E[w_kw_k ^T]和R_k＝E[v_kv_k ^T]；A_k＝I+F(t_k)△T，

量测参数阵H_k＝I，I为单位矩阵，t_k为传感器量测时间点，△T为传感器量测的时间间隔，

n为谐波减速比，M(q_l)、J_m分别为负载端和电机端的转动惯量，

为离心力与科氏力项，k、D分别为关节的刚度系数和结构变形阻尼系数。

2.根据权利要求1所述的基于抗干扰滤波的空间机械臂柔性关节位姿估计方法，其特征在于：所述第二步，针对柔性关节滤波模型中具有未知特性的干扰量d_k，根据关节编码器的量测数据对其进行快速估计，并借助高斯马尔可夫定理计算估计增益以保证干扰在最小方差意义下的无偏估计；

仅利用新的量测信息和当前时刻的量测估计之间的差计算干扰估计值

即：

其中

为上一时刻状态估计值，M_k为待设计的干扰估计增益。

3.根据权利要求2所述的基于抗干扰滤波的空间机械臂柔性关节位姿估计方法，其特征在于：所述干扰估计增益M_k设计如下：

假设

无偏，若要求

无偏，即

需要满足如下条件：

M_kH_kG_k-1＝I_m

在此基础上，设P_k-1为上一时刻状态估计协方差，设计干扰估计增益M_k为：

其中F_k＝H_kG_k-1，

P_k|k-1＝A_kP_k-1A_k ^T+Q_k-1，则

为d_k-1的无偏最小方差估计，干扰估计的误差协方差由

确定。

4.根据权利要求1所述的基于抗干扰滤波的空间机械臂柔性关节位姿估计方法，其特征在于：所述第三步，基于干扰估计值

采用改进的卡尔曼滤波方法进行时间更新、量测更新，计算滤波增益矩阵以保证关节角度和角速度估计误差在最小方差意义下的最优性，实现各关节角度、角速度的准确估计；

基于干扰估计值

时间更新和量测更新过程如下：

其中K_k为待设计的滤波增益矩阵。

5.根据权利要求4所述的基于抗干扰滤波的空间机械臂柔性关节位姿估计方法，其特征在于：所述滤波增益矩阵K_k设计如下：

设S_k ^*＝E[w_k|k-1v_k ^T]＝-G_k-1M_kR_k，

r＝rank[R_k*]，α_k满足α_kR_k ^*α_k ^T满秩，则滤波增益矩阵K_k设计如下：

K_k＝(P^x _k|k-1H_k ^T+S_k ^*)α_k ^T(α_kR_k ^*α_k ^T)^-1α_k

其中，P^x _k|k-1＝(I_n-G_k-1M_kH_k)P_k|k-1(I_n-G_k-1M_kH_k)^T+G_k-1M_kR_kM_k ^TG_k-1 ^T为当前时刻状态的一步预测协方差，在此增益下，状态估计协方差P_k|k为：

P_k|k＝P^x _k|k-1-(P^x _k|k-1H_k ^T+S_k ^*)α_k ^T(α_kR_k ^*α_k ^T)^-1α_k(P^x _k|k-1H_k ^T+S_k ^*)^T

即得

为最小方差意义下x_k的无偏估计。

Images