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

权利要求书

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的无偏估计。