CN108972553A - 一种基于粒子滤波算法的空间机械臂故障检测方法 - Google Patents

Abstract

本发明公开了一种基于粒子滤波算法的空间机械臂故障检测方法。包括：基于空间机械臂动力学解析模型设计非线性干扰观测器，在此基础上选取李雅普诺夫函数证明其收敛；然后设计滑模变结构控制器对干扰观测器所得结果进行补偿，得到空间机械臂的关节角度；由于传感器测量值与关节角实际值之间存在噪声，使用粒子滤波算法对传感器测量结果进行评估，计算空间机械臂达到目标关节角度的概率，结合干扰观测器曲线和计算得到的概率值，综合评价故障的发生。与其他故障检测方法相比，本发明所提出的故障检测方法可以避免故障阈值的设定，提高故障诊断的准确率。

Description

一种基于粒子滤波算法的空间机械臂故障检测方法

【技术领域】

本发明属于自动化控制技术领域，涉及干扰观测器和滑模变结构控制，具体是一种基于粒子滤波算法的空间机械臂故障检测方法。

【背景技术】

由于其所处工作环境恶劣，加之宇宙环境的干扰，空间机械臂关节故障时有发生。未能及时准确的判断出故障关节，调整机械臂控制策略将导致其任务失败，甚至损坏机械臂自身。

机械臂的故障检测方法大抵可以分为两类：基于知识的故障诊断方法根据系统在正常工作和故障状况下的历史数据，构成先验知识数据库，再根据一些智能推理规则推断系统是否发生了故障。然而大量的数据运算超越了星载计算机的能力范围；另一种是基于解析模型的故障诊断方法，其基本思想是将正常工作状态时数学模型的输出值和传感器测量输出值进行比较并产生一个残差信号。故障阈值的设定影响了故障诊断的准确率，但故障阈值的大小是一个很难选择的问题。

【发明内容】

有鉴于此，鉴于上述的故障诊断方法所存在的问题，本发明提出一种基于粒子滤波算法的空间机械臂故障诊断方法，包括：

第一步：基于空间机械臂动力学模型设计非线性干扰观测器，在此基础上选取李雅普诺夫函数证明其收敛；；

第二步：设计滑模变结构控制器对干扰观测器的结果进行补偿，得到空间机械臂的关节角度；

第三步：使用粒子滤波算法中对传感器测量结果进行评估计算空间机械臂达到目标关节角的概率，结合干扰观测器曲线和计算得到的概率值，综合评价故障的发生。

进一步的，第一步中设计干扰观测器对空间机械臂系统进行检测的具体方法是：

建立空间机械臂动力学模型如下所示：

其中，M(θ)∈R^3×3为机械臂的惯性矩阵；代表其离心力和科里奥利力；θ∈R³，τ∈R³分别代表机械臂关节角度、角速度以及控制输入力矩；d∈R³为外界空间干扰及关节电机故障。

观测器的基本思想是用估计输出和实际输出的差值对估计值进行修正，即：

因此干扰观测器设计为：

其中，d为干扰矢量实际值，为干扰矢量估计值，z为辅助参数向量，为辅助参数向量关于时间的导数，θ为关节角度，为关节角速度，L(θ)、分别是关于关节角度和关节角速度的增益矩阵，二者定义如下：

所述X为可逆矩阵，通过李雅普诺夫函数得到的线性矩阵不等式(7)求得。

令上述表达式构成了非线性干扰观测器。实际工程中一般很难获得干扰d的微分先验知识，且相对于观测器的动态特性，干扰的变化是缓慢的，取

针对本发明提出的空间机械臂系统，设计李雅普诺夫函数为

其中表示观测器干扰观测误差。

将带入式(2)并化简，可得

从而得到

将式(4)代入式(3)并化简得到

构造不等式

其中Γ>0为对称正定矩阵，则存在Γ的导数，使得

可见，本文所用干扰观测器指数收敛，收敛精度取决于参数Γ值，参数值越大，收敛速度越快，精度越高；

由不等式可见，式中含有非线性项，必须转化为线性矩阵不等式才能求解，令Y＝X^-1，将Y^T和Y分别乘以不等式左右，得：

由于则则上式成立的充分条件为：

Y^T+Y-ζI-Y^TΓY≥0

其中ζ为一个正的常数，I为n阶单位矩阵，根据Schur定理，上式等价于

因此该不等式的求解是否有效取决于ζ和Γ的值，其值越小，越容易得到有效的解。

进一步的，第二步中设计滑模变结构控制器对干扰观测器的结果进行补偿，得到空间机械臂的关节角度的具体方法如下：

设空间机械臂关节理想角度为θ_aim，关节角的实际值为θ，取跟踪误差e＝θ-θ_aim，定义滑模函数s为：

其中λ为n维对角矩阵，λ_i>0,i＝1,2…n。显然，当控制系统状态保持在滑模面上时，关节角跟踪误差趋近于零。

为了实现在控制率中不含有加速度项，同时将观测器输出反馈到滑模控制器，引入变量则

从而使得中不包含加速度项。

在控制率设计中，为确保在存在干扰的前提下，系统轨迹依旧能靠近滑模表面，应满足滑模函数与滑模函数的微分之积小于等于零，即将其展开可得：

因此滑模控制器设计为：

其中，α为元素均为正数的对角矩阵，sgn为符号函数。

进一步的，第三步中使用粒子滤波算法中对传感器测量结果进行评估计算空间机械臂达到目标关节角的概率，对比干扰观测器曲线和计算得到的机械臂使用可靠度，综合评价故障发生的具体方法如下：

受限于机械臂各组成部件的加工工艺、装配工艺以及使用环境中的各类干扰，传感器测量值与关节角实际值之间存在噪声，根据传感器测量值得到关节角实际值的后验概率分布θ_i|y_i，然后基于粒子滤波算法计算符合空间机械臂任务精度的概率；

根据空间机械臂使用可靠性控制系统状态空间模型：

其中，θ_i为第i个控制周期机械臂关节角的实际值；y_i为第i个控制周期传感器的测量值；x_i为第i个控制周期的控制策略。可知要计算空间机械臂的使用可靠度则需要获得θ_i的后验概率分布θ_i|y_i。采用如下的正态分布模型作为空间机械臂关节角度传感器观测误差波动：

v_i～N(μ₀,Λ₀)

其中，μ₀为预设的空间机械臂关节角测量误差的期望值，Λ₀为预设的空间机械臂关节角测量误差的方差；

采用和两个集合来对θ_i的分布进行描述。其中是与θ_i相同形式的一组样本数据，即粒子，N_s为粒子数量；是粒子对应的权值，表示粒子与θ_i的相关程度；

粒子滤波算法源于蒙特卡洛思想，即以某事件出现的频率来指代该事件的概率。基于粒子滤波算法实现空间机械臂使用可靠度计算分为以下5个步骤：

1)初始状态：随机产生N_s个粒子，依次计算每个粒子z_j到目标初始角度的距离，其中j＝1,2…N_s，然后得到所有粒子的几何中心位置；

2)预测阶段：根据状态转移方程，每一个粒子得到一个预测粒子，求取每个粒子与测量位置相差的距离dist，并根据传感器测量的先验概率密度求得粒子权重ω_i，之后将权值归一化；

3)重采样：重要性采样法引入一个已知的、容易采样的重要性概率密度函数从中生成采样粒子，利用这些随机样本的加权和来逼近后验滤波概率密度根据粒子权重对粒子进行筛选，筛选过程中，去掉权重较小的粒子，复制权值较大的粒子；计算所有粒子几何中心位置，得到与实际值的位置误差；

4)计算到达目标角度的概率：根据空间机械臂任务精度要求，计算精度范围内粒子占总粒子个数的比例，空间机械臂的使用可靠度计算方法如下：

其中，P{θ_i1≤θ_i≤θ_i2}为到达关节目标位置的概率,θ_i1≤θ_i≤θ_i2为预设的精度范围，θ_i1为精度范围的下限值，θ_i2为精度范围的上限值；

滤波：将重采样后的粒子带入状态转移方程得到新的预测粒子，即步骤2)；

计算所得的结果为空间机械臂在每个控制周期达到目标角度的概率，当关节角不符合精度范围且干扰观测曲线出现异常时，即可综合定位故障发生的关节和时间；当干扰观测曲线未出现明显异常，但使用可靠度持续较低时，可判定为关节角传感器故障，所述方法可在一定力矩干扰的情况下使机械臂继续可靠运行。

由以上技术方案可以看出，本发明实施例解决了故障阈值难以设定的问题，并提高了故障诊断的准确率，并且使得空间机械臂在一定力矩干扰的情况下继续可靠运行。

【附图说明】

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。

图1表示本发明实例的设计流程图。

图2表示本发明实施例所提出的基于干扰补偿的控制系统框图。

图3表示本发明实例提出的基于粒子滤波算法计算空间机械臂达到目标角度概率的流程图。

图4表示干扰补偿后的角度跟踪曲线。

图5表示故障时第二关节角度跟踪曲线。

图6表示故障时第二关节角速度跟踪曲线。

图7表示故障时干扰观测器的观测结果。

图8表示故障时力矩输出曲线。

图9表示整个过程达到目标角度的概率。

【具体实施方式】

为了更好地理解本发明的技术方案，下面结合附图对本发明实施例进行详细描述。

应当明确，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

基于粒子滤波算法的空间机械臂故障检测方法具体包括步骤如下：

1.基于空间机械臂动力学模型设计非线性干扰观测器，在此基础上选取李雅普诺夫函数证明其收敛；2.设计滑模变结构控制器对干扰观测器的结果进行补偿，得到空间机械臂的关节角度；3.使用粒子滤波算法中对传感器测量结果进行评估，计算空间机械臂达到目标关节角的概率，结合干扰观测器曲线和计算得到的概率值，综合评价故障的发生。

本发明实施例给出一种基于粒子滤波算法的空间机械臂故障检测方法，请参考图1，其为本发明实施例所提出的基于粒子滤波算法的空间机械臂故障检测方法的流程示意图，如图1所示，该方法包括以下步骤：

步骤101，基于空间机械臂动力学模型设计非线性干扰观测器，在此基础上选取李雅普诺夫函数证明其收敛；

具体的，空间机械臂动力学模型如下所示：

其中，M(θ)∈R^3×3为机械臂的惯性矩阵；代表其离心力和科里奥利力；θ∈R³，τ∈R³分别代表机械臂关节角度、角速度以及控制输入力矩；d∈R³为空间机械臂力矩干扰及关节电机故障。

观测器的基本思想是用估计输出和实际输出的差值对估计值进行修正，即：

因此干扰观测器设计为：

其中，d为干扰实际值，为干扰估计值，z为辅助参数向量，为其关于时间的导数，θ为关节角度，为关节角速度，L(θ)、分别是关于关节角度和关节角速度的增益矩阵，二者定义如下：

所述X为可逆矩阵，通过李雅普诺夫函数所得到的线性矩阵不等式(7)求得。

实际工程中一般很难获得干扰d的微分先验知识，且相对于观测器的动态特性，干扰的变化是缓慢的，取

针对本发明提出的空间机械臂系统，设计李雅普诺夫函数为

其中表示观测器干扰观测误差。

将带入观测器表达式，化简式(2)，可得

从而得到

将式(4)代入李雅普诺夫函数并化简得到

构造不等式

其中Γ>0为对称正定矩阵，则存在Γ的导数，使得

可见，本文所用干扰观测器指数收敛，收敛精度取决于参数Γ值，参数值越大，收敛速度越快，精度越高；

由不等式可见，式中含有非线性项，必须转化为线性矩阵不等式才能求解，令Y＝X^-1，将Y^T和Y分别乘以不等式左右，得：

由于则则上式成立的充分条件为：

Y^T+Y-ζI-Y^TΓY≥0

根据Schur定理，上式等价于

因此该不等式的求解是否有效取决于ζ和Γ的值，其值越小，越容易得到有效的解。

步骤102，设计滑模变结构控制器对干扰观测器的结果进行补偿，得到空间机械臂的关节角度，其流程框图如图2所示。

具体的，设空间机械臂关节理想角度为θ_aim，关节角的实际值为θ，取跟踪误差e＝θ-θ_aim，定义滑模函数s为：

其中λ为3维对角矩阵，为保证s为Hurwitz多项式，则λ_i>0,i＝1,2,3。显然，当控制系统状态保持在滑模面上时，关节角跟踪误差趋近于零。

为了实现在控制率中不含有加速度项，同时将观测器输出反馈到滑模控制器，引入变量则

从而使得中不包含加速度项。

在控制率设计中，为确保在存在干扰的前提下，系统轨迹依旧能靠近滑模表面，应满足滑模函数与滑模函数的微分之积小于等于零，即将其展开可得：

因此滑模控制器设计为：

其中，α为元素均为正数的对角矩阵。

步骤103，使用粒子滤波算法中对传感器测量结果进行评估，计算空间机械臂达到目标关节角的概率，结合干扰观测器曲线和计算得到的概率值，综合评价故障的发生。流程框图如图3所示；

具体的，受限于机械臂各组成部件的加工工艺、装配工艺以及使用环境中的各类干扰，传感器测量值与关节角实际值之间存在噪声，根据传感器测量值得到关节角实际值的后验概率分布θ_i|y_i，然后基于粒子滤波算法计算符合空间机械臂任务精度的概率；

根据空间机械臂使用可靠性控制系统状态空间模型：

其中，θ_i为第i个控制周期机械臂关节角的实际值；y_i为第i个控制周期传感器的测量值；x_i为第i个控制周期的控制策略。可知要计算空间机械臂的使用可靠度则需要获得θ_i的后验概率分布θ_i|y_i。采用如下的正态分布模型作为空间机械臂关节角度传感器观测误差波动：

v_i～N(μ₀,Λ₀)

其中，μ₀为预设的空间机械臂关节角测量误差的期望值，Λ₀为预设的空间机械臂关节角测量误差的方差；

采用和两个集合来对θ_i的分布进行描述。其中是与θ_i相同形式的一组样本数据，即粒子，N_s为粒子数量；是粒子对应的权值，表示粒子与θ_i的相关程度；

粒子滤波算法源于蒙特卡洛思想，即以某事件出现的频率来指代该事件的概率。基于粒子滤波算法实现空间机械臂使用可靠度计算分为以下5个步骤：

1)初始状态：随机产生N_s个粒子，依次计算每个粒子z_j到目标初始角度的距离，其中j＝1,2…N_s，然后得到所有粒子的几何中心位置；

2)预测阶段：根据状态转移方程，每一个粒子得到一个预测粒子，求取每个粒子与测量位置相差的距离dist，并根据传感器测量的先验概率密度求得粒子权重ω_i，之后将权值归一化；

3)重采样：重要性采样法引入一个已知的、容易采样的重要性概率密度函数从中生成采样粒子，利用这些随机样本的加权和来逼近后验滤波概率密度根据粒子权重对粒子进行筛选，筛选过程中，去掉权重较小的粒子，复制权值较大的粒子；计算所有粒子几何中心位置，得到与实际值的位置误差；

4)计算到达目标角度的概率：根据空间机械臂任务精度要求，计算精度范围内粒子占总粒子个数的比例，空间机械臂的使用可靠度计算方法如下：

其中，P{θ_i1≤θ_i≤θ_i2}为到达关节目标位置的概率，θ_i1≤θ_i≤θ_i2为预设的精度范围，θ_i1为精度范围的下限值，θ_i2为精度范围的上限值；

滤波：将重采样后的粒子带入状态转移方程得到新的预测粒子，即步骤2)；

具体实施中，其动力学参数如下表所示：

i	mi(Kg)	Li(m)	li(m)	Ji(Kg*m2)
					1	1	1	1	0.083
2	1	1	1	0.083
					3	1	1	1	0.083

干扰模型取k₁＝2,k₂＝2,k₃＝2。关节1、关节2、关节3的理想轨迹分别为θ_aim1＝sin t、θ_aim2＝sin t、θ_aim3＝0.5*sin t，单位为弧度。文中所述干扰观测器无需加速度信号，干扰d的观测初始值取[0,0,0]。仿真时间设置为10s。图4表示空间机械臂的角度跟踪。在t＝2.9s时对机械臂第二个关节模拟一个力矩故障，并假设电机最大输出力矩为20N。图5表示故障时第二关节的角度跟踪，图6表示故障时第二关节角速度跟踪，图7表示第二个关节的系统干扰和故障，图8表示第二个关节的力矩输出。由于传感器测量误差的存在，为机械臂关节输出角度叠加一个v_i～N(0,0.01)的正态分布误差，并设置机械臂关节角的精度允许范围为0.017弧度，即1°。通过粒子滤波算法得到机械臂在每个状态点上在精度范围内的粒子个数如下表所示：

时间(s)	0	0.5	1	1.5	2	2.5	3	3.5	4	4.5
											粒子数	2	90	98	98	99	97	63	35	70	69
时间(s)	5	5.5	6	6.5	7	7.5	8	8.5	9	9.5
											粒子数	89	97	98	97	96	99	95	96	98	98

图9表示每个控制周期空间机械臂达到目标角度的概率。

由仿真结果可以看出，本发明所提方法可以很快检测到故障的发生及其波形。

以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。

Claims (4)

1.一种基于粒子滤波算法的空间机械臂故障检测方法，其特征在于，所述方法步骤包括：

(1)基于空间机械臂动力学模型设计非线性干扰观测器，在此基础上选取李雅普诺夫函数证明其收敛；

(2)设计滑模变结构控制器对干扰观测器的结果进行补偿，得到空间机械臂的关节角度；

(3)使用粒子滤波算法中对传感器测量结果进行评估，计算空间机械臂达到目标关节角的概率，结合干扰观测器曲线和计算得到的概率值，综合评价故障的发生。

2.根据权利要求1所述的方法，其特征在于，基于空间机械臂动力学模型设计非线性干扰观测器，在此基础上选取李雅普诺夫函数证明其收敛；

使用空间机械臂力矩干扰估计值和力矩干扰实际输出值的差值对力矩干扰估计值进行修正，因此干扰观测器设计为：

其中，d为干扰矢量实际值，为干扰矢量估计值，z为辅助参数向量，为辅助参数向量关于时间的导数，θ为关节角度向量，为关节角速度向量，L(θ)、分别是关于关节角度和关节角速度的增益矩阵，二者定义如下：

X为可逆矩阵，通过李雅普诺夫函数所得到的线性矩阵不等式(11)求得；所述关节力矩干扰矢量d可由空间机械臂在关节空间的动力学基本方程求得，M^-1(θ)为动力学惯性矩阵的逆矩阵，所述空间机械臂在关节空间的动力学基本方程如下：

式(3)中，θ表示关节角序列，为n维列向量，由空间机械臂的任务规划和路径规划算法得到；M(θ)∈R^n×n为关节空间中的惯性矩阵；为哥氏力和离心力矩阵；τ＝(τ_1,τ₂,…,τ_n)^T为关节力矩矢量；

令表达式(1)、(2)、(3)构成了非线性干扰观测器，实际工程中一般很难获得干扰矢量d的微分先验知识，且相对于观测器的动态特性，干扰的变化是缓慢的，因此假设

针对所提出的空间机械臂系统，设计李雅普诺夫函数为

其中表示观测器干扰观测误差，X^T为式(2)中可逆矩阵X的转置矩阵；

化简上述观测器表达式，可得

从而得到

将上述推导结果带入李雅普诺夫函数得到

构造不等式

其中Γ>0为对称正定矩阵，则存在Γ的导数，使得

可见，所用干扰观测器指数收敛，收敛精度取决于参数Γ值，参数值越大，收敛速度越快，精度越高；

由不等式(8)可见，式中含有非线性项，必须转化为线性矩阵不等式才能求解，令Y＝X^-1，将Y^T和Y分别乘以不等式左右，得：

由于则则上式成立的充分条件为：

Y^T+Y-ζI-Y^TΓY≥0 (10)

其中I为n维单位矩阵，ζ为一个正的常数，根据Schur定理，式(10)等价于

因此该不等式的求解是否有效取决于ζ和Γ的值，其值越小，越容易得到有效解，不等式的解即可得到式(2)中的可逆矩阵X。

3.根据权利要求1所述的方法，其特征在于，设计滑模变结构控制器对干扰观测器的结果进行补偿，得到空间机械臂的关节角度；

设空间机械臂关节理想角度为θ_aim，关节角的实际值为θ，取跟踪误差e＝θ-θ_aim，定义滑模函数s为：

其中λ为n维对角矩阵，λ_i>0,i＝1,2…n，为e的关于时间的导数；显然，当控制系统状态保持在滑模面上时，关节角跟踪误差趋近于零；

为了实现在控制率中不含有加速度项，同时将观测器输出反馈到滑模控制器，引入变量则

从而使得中不包含加速度项；

在控制率设计中，为确保在存在干扰时，系统轨迹依旧能靠近滑模表面，应满足滑模函数与滑模函数的微分之积小于等于零，即将其展开可得：

因此滑模控制器设计为：

其中，α为元素均为正数的对角矩阵，sgn为符号函数；

通过在控制率中加入对力矩干扰的补偿来降低切换增益，从而抑制抖振。

4.根据权利要求1所述的方法，其特征在于，使用粒子滤波算法中对传感器测量结果进行评估，计算空间机械臂达到目标关节角的概率；

根据空间机械臂使用可靠性控制系统状态空间模型：

其中，θ_i为第i个控制周期机械臂关节角的实际值；y_i为第i个控制周期传感器的测量值；x_i为第i个控制周期的控制策略，可知要计算空间机械臂的使用可靠度则需要获得θ_i的后验概率分布θ_i|y_i，采用如下的正态分布模型作为空间机械臂关节角度传感器观测误差波动：

v_i～N(μ₀,Λ₀)

其中，μ₀为预设的空间机械臂关节角测量误差的期望值，Λ₀为预设的空间机械臂关节角测量误差的方差；

采用和两个集合来对θ_i的分布进行描述；其中是与θ_i相同形式的一组样本数据，即粒子，N_s为粒子数量；是粒子对应的权值，表示粒子与θ_i的相关程度；

粒子滤波算法源于蒙特卡洛思想，即以某事件出现的频率来指代该事件的概率，基于粒子滤波算法实现空间机械臂使用可靠度计算分为以下5个步骤：

1)初始状态：随机产生N_s个粒子，依次计算每个粒子z_j到目标初始角度的距离，其中j＝1,2…N_s，然后得到所有粒子的几何中心位置；

2)预测阶段：根据状态转移方程，每一个粒子得到一个预测粒子，求取每个粒子与测量位置相差的距离dist，并根据传感器测量的先验概率密度求得粒子权重ω_i，之后将权值归一化；

3)重采样：重要性采样法引入一个已知的、容易采样的重要性概率密度函数从中生成采样粒子，利用这些随机样本的加权和来逼近后验滤波概率密度根据粒子权重对粒子进行筛选，筛选过程中，去掉权重较小的粒子，复制权值较大的粒子；计算所有粒子几何中心位置，得到与实际值的位置误差；

4)计算到达目标角度的概率：根据空间机械臂任务精度要求，计算精度范围内粒子占总粒子个数的比例，空间机械臂到达目标角度概率计算方法如下：

其中，P{θ_i1≤θ_i≤θ_i2}为到达关节目标位置的概率，θ_i1≤θ_i≤θ_i2为预设的精度范围，θ_i1为精度范围的下限值，θ_i2为精度范围的上限值；

5)滤波：将重采样后的粒子带入状态转移方程得到新的预测粒子，即步骤2)；

计算所得的结果为空间机械臂在每个控制周期达到目标角度的概率，当关节角不符合精度范围且干扰观测曲线出现异常时，即可综合定位故障发生的关节和时间；当干扰观测曲线未出现明显异常，但使用可靠度持续较低时，可判定为关节角传感器故障，所述方法可在一定力矩干扰的情况下使机械臂继续可靠运行。