CN103399493B - 可重构机械臂传感器故障实时诊断和容错系统及其方法 - Google Patents

Abstract

可重构机械臂传感器故障实时诊断和容错系统及其方法涉及智能机械与机器人控制技术领域，该系统包括分散主控模块、故障检测模块、信号重构模块、主动切换装置、执行器及传感器信号采集子系统；其中，分散主控模块提供系统正常运行时控制规律；故障检测模块通过分散滑模观测器采集传感器故障信号，并判断何种传感器何时发生故障；信号重构模块通过非故障传感器信号重构故障传感器信号；主动切换装置关闭故障传感器反馈通道，将重构信号反馈分散主控模块，实现可重构模块化机械臂传感器故障的实时检测与容错。本发明基于控制对象模块化特点，单个模块故障不影响其他模块正常运行，有效改善了故障系统的控制精度，使控制结果更准确合理，更具实用性。

Description

可重构机械臂传感器故障实时诊断和容错系统及其方法

技术领域

本发明涉及智能机械与机器人控制技术领域，尤其涉及一种可重构机械臂传感器故障实时诊断和容错系统及其方法。

背景技术

可重构机械臂是一种由一组具有标准接口的可互换的关节模块和连杆模块组成的，并能够通过增减模块数量、变换几何构形装配成适应不同任务环境的机械臂。与传统机械臂相比，可重构机械臂适应的工作范围更加广泛并且具有低成本、易维护、易修改、便携等优点。可重构机械臂技术的发展扩展了机械臂在工业、军事和航空航天等很多领域的应用空间。由于可重构机械臂长时间地工作在未知、恶劣的环境中，故障会不可避免地发生在执行器、传感器及其他部件上，并且操作人员不能直接进行干预，因此对系统的可靠性和安全性提出了更高的要求。

因此，克服不确定自由度可重构机械臂系统控制器设计复杂，以及故障诊断与容错系统强非线性、高维度、强耦合的技术缺陷，提供一种可重构机械臂传感器故障实时诊断和容错系统及其方法势在必行。

发明内容

为了解决不确定自由度可重构机械臂系统控制器设计复杂，以及故障诊断与容错系统强非线性、高维度、强耦合的技术问题，本发明提供一种可重构机械臂传感器故障实时诊断和容错系统及其方法，并且该系统避免了关节传感器故障影响其他正常关节运行的现象。该系统充分利用模块关节自身信息，使得各子系统独立进行控制，并将传感器故障隔离在子系统内部，并在故障模块内实现主动容错，使得控制系统简单、效率高、实时性好，能够达到满意的容错效果，满足实际需要。

本发明基于可重构机械臂系统最重要的属性之一-模块化属性，将可重构机械臂系统的每个广义关节考虑成一个子系统，针对各子系统设计基于计算力矩法(CTC)的自适应模糊分散控制器，并作为主控制器封装成分散主控模块。本发明采用基于T-S模型的模糊逻辑控制方法，其是由Takagi和Sugeno提出的一种典型的模糊动态模型。这类模型采用的模糊规则：其前提部是依据系统输入、输出间是否存在局部线性关系来进行划分；而其结论部是由多项式线性方程来表达，从而构成各条规则间的线性组合，使非线性系统的全局输出具有良好的线性描述特性。模糊控制理论不仅可以解决复杂且难以建立精确数学模型的对象或系统的控制问题，也是处理时变和不确定性问题的一种有效方法。其控制规则是基于启发式的知识及语言型决策规则，有利于增强控制系统的自适应性、自学习和自调整的能力。模糊控制从受控对象的定性认识出发，用语言变量代替代数变量，比较容易用于知识表达和建立语言型控制规则，描述前提部和结论部之间的模糊关系，同时其鲁棒性强，可有效减少系统干扰和参数变化对控制精度的影响。

故障诊断是检测系统是否发生故障，并确定故障的种类和发生的部位及故障发生的时间。基于观测器技术的故障诊断方法是一种状态估计方法，其基本思想是通过被控过程状态与可测变量比较构成残差序列，从中将故障检测出来。滑模控制技术具有相应速度快，对参数变化和干扰不敏感以及易于物理实现等优点，常用于非线性动力学控制中。本发明结合分散观测器与滑模控制技术实现可重构机械臂子系统的传感器故障诊断。

当可重构机械臂系统发生传感器故障后，容错控制技术可保证动态系统仍然可以稳定运行，并达到可以接受的性能指标，提高系统运行的可靠性与安全性。基于解析冗余技术的容错控制主要分为被动容错控制与主动容错控制，其中被动容错控制采用固定的控制器来确保闭环系统保持稳定，虽然不需要在线故障信息，即不需要FDD子模块，但其只对特定的故障不敏感；主动容错控制是指在故障发生后，根据所期望的特性重新设计控制系统，一般需要FDD子模块检测故障。本发明通过信号重构模块利用非故障传感器信号重构故障信号，同时主动切换装置在关闭故障传感器反馈通道后，立即切换相应的重构信号送回至主控模块，实现主动容错控制。

本发明解决技术问题所采取的技术方案如下：

可重构机械臂传感器故障实时诊断和容错系统包括分散主控模块、故障检测模块、信号重构模块、主动切换装置、执行器及传感器信号采集子系统，所述传感器信号采集子系统包括位置传感器和速度传感器；所述分散主控模块与执行器连接，其用于利用模糊逻辑系统可任意精度拟合非线性函数的特性，采用模糊逻辑控制系统对子系统非线性项进行估计，并采用自适应调节器补偿系统估计误差；所述故障检测模块与传感器信号采集子系统连接，其用于先通过引入一阶滤波器将子系统传感器故障转化为伪执行器故障，再采用分散滑模观测器技术对子系统传感器故障进行检测，同时判断故障发生时间；所述信号重构模块与故障检测模块连接，其用于在所述故障检测模块判断出子系统位置传感器发生故障时，利用数字积分器对速度传感器信号进行积分，重构出位置信号；在所述故障检测模块判断出子系统速度传感器发生故障时，利用微分跟踪器重构出速度信号；所述主动切换装置分别与信号重构模块、故障检测模块、分散主控模块连接，其用于按照制定的主动切换策略，将故障信号通道关闭，开启相应的重构信号通道，将重构信号作为反馈送至分散主控模块实现可重构机械臂传感器故障的实时诊断与容错控制；所述执行器和传感器信号采集子系统，用于为系统运行提供动力支持和反馈信号。

可重构机械臂传感器故障实时诊断和容错方法包括如下步骤：

第一步，分散主控模块设计：当系统无传感器故障时，针对式(5)所示子系统动力学状态空间表达式，设计如式(10)-(12)所示的分散主控模块，使得关节模块能够精确跟踪期望轨迹；此分散主控模块包括CTC控制器、模糊控制系统和自适应调节器；

$S_i:\left\{\begin{array}{c}{\stackrel{\·}{x}}_i=A_i{x}_i+B_i[f_i(q_i,{\stackrel{\·}{q}}_i)+g_i\left(q_i\right)u_i+h_i(q,\stackrel{\·}{q},\stackrel{\·\·}{q})]\\ y_i=C_i{x}_i\end{array}\right.---\left(5\right)$

u_i＝u_i0+u_ic(10)

$u_{i0}=M_i\left(q_i\right)({\stackrel{\·\·}{q}}_{\mathrm{id}}-k_{\mathrm{iv}}{\stackrel{\·}{e}}_i-k_{\mathrm{ip}}{e}_i)+C_i(q_i,{\stackrel{\·}{q}}_i){\stackrel{\·}{q}}_i+G_i\left(q_i\right)---\left(11\right)$

$u_{\mathrm{ic}}=-M_i(\mathrm{sgn}\left(x_i^T{P}_i{B}_i\right){\hat{p}}_i\left(\right|x_i^T{P}_i{B}_i|,{\hat{W}}_{\mathrm{ip}})+\mathrm{sgn}\left(x_i^T{P}_i{B}_i\right){\widehat{\mathrm{\η}}}_i)---\left(12\right)$

式中，x_i是子系统S_i的状态向量，y_i是子系统S_i的输出，且 $A_i=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]$ $B_i=\left[\begin{array}{c}0\\ 1\end{array}\right]$ $C_i=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],$ $f_i(q_i,{\stackrel{\·}{q}}_i)=M_i^{-1}\left(q_i\right)[-C_i(q_i,{\stackrel{\·}{q}}_i){\stackrel{\·}{q}}_i-G_i\left(q_i\right)],$ $g_i\left(q_i\right)=M_i^{-1}\left(q_i\right),$ $h_i(q,\stackrel{\·}{q},\stackrel{\·\·}{q})=-M_i^{-1}\left(q_i\right)Z_i(q,\stackrel{\·}{q},\stackrel{\·\·}{q}),$ q∈Rⁿ为关节位置向量，M(q)∈R^n×n为惯性矩阵，为哥氏力和离心力项，G(q)∈Rⁿ为重力项；u_i0是计算力矩控制律，u_ic是鲁棒自适应模糊控制律，用来补偿系统不确定性.k_iv,k_ip是微分和比例常数， $x_i={\left[\begin{array}{cc}{e}_i& {\stackrel{\·}{e}}_i\end{array}\right]}^T$ 定义为跟踪误差向量， $B_i={\left[\begin{array}{cc}0& 1\end{array}\right]}^T.{\widehat{\mathrm{\η}}}_i$ 是可调参数；

第二步，故障检测模块设计：通过引入如式(20)的一阶滤波器将故障子系统方程扩展为式(22)，设计分散滑模观测器如式(23)，由此可判断出何种传感器何时发生故障：

${\stackrel{\·}{z}}_{\mathrm{ai}}=A_{\mathrm{ai}}{z}_{\mathrm{ai}}+B_{\mathrm{ai}}{y}_i---\left(20\right)$

${\stackrel{\‾}{S}}_{\mathrm{if}}:\left\{\begin{array}{c}{\stackrel{\·}{z}}_i=A_i{z}_i+B_i[f_i(q_i,{\stackrel{\·}{q}}_i)+g_i\left(q_i\right)u_i+h_i(q,\stackrel{\·}{q,}\stackrel{\·\·}{q})]\\ {\stackrel{\·}{z}}_{\mathrm{ai}}=A_{\mathrm{ai}}{z}_{\mathrm{ai}}+B_{\mathrm{ai}}(C_i{x}_i+D_i{f}_{\mathrm{si}})\\ y_a=z_{\mathrm{ai}}\end{array}\right.---\left(22\right)$

${\hat{S}}_{\mathrm{if}}:\left\{\begin{array}{c}{\stackrel{\·}{\hat{z}}}_i=A_i{\hat{z}}_i+B_i[{\hat{f}}_i(q_i,{\stackrel{\·}{q}}_i)+{\hat{g}}_i\left(q_i\right)u_i+{\hat{p}}_i\left(\right|x_i^T{P}_i{B}_i|,{\hat{W}}_{\mathrm{ip}})]\\ {\stackrel{\·}{\hat{z}}}_{\mathrm{ai}}=A_{\mathrm{ai}}{z}_{\mathrm{ai}}+B_{\mathrm{ai}}(C_i{x}_i+v_i)\\ {\hat{y}}_a={\hat{z}}_{\mathrm{ai}}\end{array}\right.---\left(23\right)$

其中， $z_{\mathrm{ai}}=\left[\begin{array}{c}{z}_{\mathrm{ai}1}\\ z_{\mathrm{ai}2}\end{array}\right],$ $A_{\mathrm{ai}}=\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right],$ $B_{\mathrm{ai}}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],$ 是的估计， ${\tilde{W}}_{\mathrm{ip}}^T=W_{\mathrm{ip}}^T-{\hat{W}}_{\mathrm{ip}}^T$ 是权值估计误差，是可调参数向量， 和由模糊逻辑系统估计得到；

第三步，信号重构模块设计：根据第二步判断出的故障传感器类型及故障发生时间，利用冗余传感器信息对故障传感器信号进行重构，即当位置传感器发生故障时，采用基于四阶变步长Runge-Kutta算法的积分器进行位置信号重构；当速度传感器发生故障时，采用如式(34)所示的数值微分器对速度信号进行重构；

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ω}}}_{i1}\left(t\right)={\mathrm{\ω}}_{i2}\left(t\right)-{\mathrm{\λ}}_{i1}\sqrt{|{\mathrm{\ω}}_{i1}\left(t\right)-x_{i1}\left(t\right)|}\\ \×\mathrm{sgn}({\mathrm{\ω}}_{i1}\left(t\right)-x_{i1}\left(t\right))\\ {\stackrel{\·}{\mathrm{\ω}}}_{i2}\left(t\right)=-{\mathrm{\λ}}_{i2}\mathrm{sgn}({\mathrm{\ω}}_{i1}\left(t\right)-x_{i1}\left(t\right))\\ {\hat{x}}_{i2}\left(t\right)={\widehat{\stackrel{\·}{x}}}_{i1}\left(t\right)={\mathrm{\ω}}_{i2}\left(t\right)\end{array}\right.---\left(34\right)$

其中，为由位置传感器信号重构得到的速度信号，ω_i1,ω_i2为辅助变量，λ_i1,λ_i2为设计参数；

第四步，主动切换装置设计：基于第二步和第三步中所判断出的故障类型及重构信号，当判断出未发生传感器故障时，主动切换装置不进行信号切换，采用第一步中所设计的分散主控模块进行控制；当判断出位置传感器故障时，主动切换装置用位置重构信号取代位置传感器信号进行反馈控制；当判断出速度传感器故障时，主动切换装置用速度重构信号取代速度传感器信号进行反馈控制，从而实现反馈信号的主动切换，使发生传感器故障的可重构机械臂关节模块仍能精确跟踪期望轨迹，实现故障系统的容错控制，进而完成可重构机械臂传感器故障的实时诊断及容错方法。

本发明的有益效果如下：

(1)由于采用了分散控制方式，可只利用关节模块自身信息进行独立控制，避免了不同自由度可重构机械臂控制系统复杂的问题；

(2)本发明通过分散滑模观测器对各子系统传感器故障进行检测，将故障隔离在子系统内，使其故障不影响其他正常工作模块关节的运行；

(3)本发明将传感器故障信号通过引入一阶滤波器转化为“伪执行器”故障，采用滑模观测器的输出代替相应的故障信号实现主动容错；

(4)不论系统位置传感器还是速度传感器发生故障，本发明的方法都可以有效地实现主动容错；

(5)本发明利用位置传感器和速度传感器在功能上的冗余关系，结合解析冗余技术，不需要故障发生后更换硬件等措施实现容错，提高容错系统准确性、容错效率及实时性，成本低且更有实用性。

附图说明

图1是本发明可重构机械臂系统结构示意图；

图2是本发明可重构机械臂传感器故障实时诊断和容错系统原理图；

图3是本发明可重构机械臂传感器故障实时诊断和容错方法流程图。

具体实施方式

下面结合附图和实施例对本发明做进一步详细说明。

如图1所示，本发明基于控制对象—n自由度可重构机械臂系统特有的模块化属性，从其动力学模型中分离出关节模块的局部变量，将其分解为n个相互交联的子系统集合，然后设计子系统控制器使之稳定，并抑制子系统间的关联作用。其优点是控制系统复杂性从系统级变为分散级，使得控制系统结构、算法都大为简化，容错能力增强。分散控制器设计的关键问题是如何处理由机械结构频繁重构或增减模块导致的关节运动学、动力学的变化以及各个子系统之间的耦合关联。

如图2所示，本发明的可重构机械臂传感器故障实时诊断和容错系统是将分散主控模块的输出力矩信号τ_i送至执行器，进而将执行器力矩信号u_i提供给可重构机械臂子系统模型，将其实际位置输出q_i与速度输出和分散滑模观测器模块相应输出比较，将比较值送至故障检测模块，检测传感器是否发生故障。当传感器未发生故障，主动切换装置不发生切换，完成控制过程；当检测到位置或速度传感器发生故障时，信号重构模块重构相应故障信号送给主动切换装置，关闭故障信号通道，同时将此重构信号作为反馈信号进行控制，实现可重构机械臂传感器故障的实时诊断与容错控制。

本发明可重构机械臂传感器故障实时诊断和容错系统实现中关键处理方法及过程如下：

1.基于模块化属性的故障子系统模型

根据Newton-Euler方程得到的n自由度可重构机械臂的动力学模型为

$M\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})\stackrel{\·}{q}+G\left(q\right)=u---\left(1\right)$

其中，q∈Rⁿ为关节位置向量，M(q)∈R^n×n为惯性矩阵，为哥氏力和离心力项，G(q)∈Rⁿ为重力项，u∈Rⁿ为关节力矩向量。将式(1)模型分解为

$\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{M}_{\mathrm{ij}}\left(q\right){\stackrel{\·\·}{q}}_j+\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{C}_{\mathrm{ij}}(q,\stackrel{\·}{q}){\stackrel{\·}{q}}_j+{\stackrel{\‾}{G}}_i\left(q\right)=u_i---\left(2\right)$

其中q_i，和u_i分别为向量q，G(q)和u的第i个分量，M_ij(q)和分别为矩阵M(q)和的第ij个分量。

将可重构机械臂的每个关节考虑为一个子系统，从式(2)分离出局部变量则子系统的动力学模型可以描述为

$M_i\left(q_i\right){\stackrel{\·\·}{q}}_i+C_i(q_i,{\stackrel{\·}{q}}_i){\stackrel{\·}{q}}_i+G_i\left(q_i\right)+Z_i(q,\stackrel{\·}{q}\stackrel{\·\·}{,q})=u_i---\left(3\right)$

$Z_i(q,\stackrel{\·}{q},\stackrel{\·\·}{q})=\{\underset{j=1,j\≠i}{\overset{n}{\mathrm{\Σ}}}{M}_{\mathrm{ij}}\left(q\right){\stackrel{\·\·}{q}}_j+[M_{\mathrm{ii}}\left(q\right)-M_i\left(q_i\right)\left]{\stackrel{\·\·}{q}}_i\right\}+---\left(4\right)$

$\{\underset{j=1,j\≠i}{\overset{n}{\mathrm{\Σ}}}{C}_{\mathrm{ij}}(q,\stackrel{\·}{q}){\stackrel{\·}{q}}_j+[C_{\mathrm{ii}}(q,\stackrel{\·}{q})-C_i(q_i,{\stackrel{\·}{q}}_i)\left]{\stackrel{\·}{q}}_i\right\}+[{\stackrel{\‾}{G}}_i\left(q\right)-G_i\left(q_i\right)]$

其中，为子系统交联项。

设 $x_i={\left[\begin{array}{cc}{x}_{i1}& x_{i2}\end{array}\right]}^T={\left[\begin{array}{cc}{q}_i& {\stackrel{\·}{q}}_i\end{array}\right]}^T(i=\mathrm{1,2},...,n),$ 将式(3)表示为状态空间方程形式

$S_i:\left\{\begin{array}{c}{\stackrel{\·}{x}}_i=A_i{x}_i+B_i[f_i(q_i,{\stackrel{\·}{q}}_i)+g_i\left(q_i\right)u_i+h_i(q,\stackrel{\·}{q},\stackrel{\·\·}{q})]\\ y_i=C_i{x}_i\end{array}\right.---\left(5\right)$

其中x_i是子系统S_i的状态向量，y_i是子系统S_i的输出，且

$A_i=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]$ $B_i=\left[\begin{array}{c}0\\ 1\end{array}\right]$ $C_i=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$

$f_i(q_i,{\stackrel{\·}{q}}_i)=M_i^{-1}\left(q_i\right)[-C_i(q_i,{\stackrel{\·}{q}}_i){\stackrel{\·}{q}}_i-G_i\left(q_i\right)]$

$g_i\left(q_i\right)=M_i^{-1}\left(q_i\right)---\left(6\right)$

$h_i(q,\stackrel{\·}{q},\stackrel{\·\·}{q})=-M_i^{-1}\left(q_i\right)Z_i(q,\stackrel{\·}{q},\stackrel{\·\·}{q})$

当子系统位置传感器或速度传感器发生故障时，故障子系统的动力学模型为

$S_{\mathrm{if}}:\left\{\begin{array}{c}{\stackrel{\·}{x}}_i=A_i{x}_i+B_i[f_i(q_i,{\stackrel{\·}{q}}_i)+g_i\left(q_i\right)u_i+h_i(q,\stackrel{\·}{q},\stackrel{\·\·}{q})]\\ y_i=C_i{x}_i+D_i{f}_{\mathrm{si}}\end{array}\right.---\left(7\right)$

其中 $D_i=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$ 为第i个关节模块传感器故障分布矩阵。 $f_{\mathrm{si}}=\left[\begin{array}{c}{f}_{\mathrm{si}1}\\ f_{\mathrm{si}2}\end{array}\right]$ 为第i个关节模块的传感器故障函数矩阵，且满足||f_si(t)||≤ρ_i(t)，其中ρ_i(t)为已知连续函数。假设f_si(t)未知，且满足如表1所示条件：

表1传感器故障类型

故障类型	fsi1	fsi2
			无故障	0	0
位置传感器故障	非0	0
			速度传感器故障	0	非0
多传感器故障	非0	非0

因此，f_si(·)在t∈R⁺有定义。

2.可重构机械臂传感器故障实时诊断及容错系统

2.1基于CTC的自适应模糊分散主控制器设计

首先利用李代数方法进行非线性变换，将关节模块传感器故障转化为“伪执行器”故障，则对于子系统(5)-(6)

${\stackrel{\·}{z}}_{i1}=\frac{\∂x_{i1}}{\∂x_i}{x}_{i2}=L_{x_{i2}}{x}_{i2}=z_{i2}$

${\stackrel{\·}{z}}_{i2}=\frac{\∂z_{i2}}{\∂x_i}(f_i(q_i,{\stackrel{\·}{q}}_i)+g_i\left(q_i\right)u_i+h_i(q,\stackrel{\·}{q},\stackrel{\·\·}{q}))---\left(8\right)$

$=L_{f_i(q_i,{\stackrel{\·}{q}}_i)+g_i\left(q_i\right)u_i+h_i(q,\stackrel{\·}{q},\stackrel{\·\·}{q})}^2{z}_{i2}$

$=f_i(q_i,{\stackrel{\·}{q}}_i)+g_i\left(q_i\right)u_i+h_i(q,\stackrel{\·}{q},\stackrel{\·\·}{q})$

因此，上式可表达为

${\stackrel{\·}{z}}_i=A_i{z}_i+B_i[f_i(q_i,{\stackrel{\·}{q}}_i)+g_i\left(q_i\right)u_i+h_i(q,\stackrel{\·}{q},\stackrel{\·\·}{q})]---\left(9\right)$

下面针对传感器正常工作模式下的可重构机械臂进行基于CTC的自适应模糊分散控制器设计.

假设1：期望轨迹和有界

针对可重构机械臂子系统动力学模型(3)，设计基于CTC的分散自适应模糊控制器如下

u_i＝u_i0+u_ic(10)

$u_{i0}=M_i\left(q_i\right)({\stackrel{\·\·}{q}}_{\mathrm{id}}-k_{\mathrm{iv}}{\stackrel{\·}{e}}_i-k_{\mathrm{ip}}{e}_i)+C_i(q_i,{\stackrel{\·}{q}}_i){\stackrel{\·}{q}}_i+G_i\left(q_i\right)---\left(11\right)$

$u_{\mathrm{ic}}=-M_i(\mathrm{sgn}\left(x_i^T{P}_i{B}_i\right){\hat{p}}_i\left(\right|x_i^T{P}_i{B}_i|,{\hat{W}}_{\mathrm{ip}})+\mathrm{sgn}\left(x_i^T{P}_i{B}_i\right){\widehat{\mathrm{\η}}}_i)---\left(12\right)$

其中u_i0是计算力矩控制律，u_ic是鲁棒自适应模糊控制律，用来补偿系统不确定性.k_iv,k_ip是微分和比例常数， $x_i={\left[\begin{array}{cc}{e}_i& {\stackrel{\·}{e}}_i\end{array}\right]}^T$ 定义为跟踪误差向量， $B_i={\left[\begin{array}{cc}0& 1\end{array}\right]}^T.{\widehat{\mathrm{\η}}}_i$ 是可调参数，并可利用如式(13)的自适应更新律更新

${\stackrel{\stackrel{\·}{^}}{\mathrm{\η}}}_i={\mathrm{\Γ}}_{\mathrm{i\η}}^{-1}\left|x_i^T{P}_i{B}_i\right|---\left(13\right)$

其中Γ_iη为正常数.

假设2：交联项有界且满足

$\left|h_i(q,\stackrel{\·}{q},\stackrel{\·\·}{q})\right|\≤\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{d}_{\mathrm{ij}}{E}_j---\left(14\right)$

其中 $d_{\mathrm{ij}}\≥0,E_j=1+\left|x_j^T{P}_j{B}_j\right|+{\left|x_j^T{P}_j{B}_j\right|}^2,$ 并定义 $p_i\left(\right|x_i^T{P}_i{B}_i\left|\right)=n\underset{\mathrm{ij}}{\max }\left\{d_{\mathrm{ij}}\right\}{E}_i.$

定义估计误差 $w_i=p_i\left(\right|x_i^T{P}_i{B}_i\left|\right)-W_{\mathrm{ip}}^T{\hat{Z}}_{\mathrm{ip}}\left(\right|x_i^T{P}_i{B}_i\left|\right),$ 且满足w_i≤η_i.

利用模糊逻辑系统补偿子系统间耦合关联项影响，表示如下

${\hat{p}}_i\left(\right|x_i^T{P}_i{B}_i|,{\hat{W}}_{\mathrm{ip}})={\hat{W}}_{\mathrm{ip}}^T{\hat{Z}}_{\mathrm{ip}}\left(\right|x_i^T{P}_i{B}_i\left|\right)---\left(15\right)$

其中是的估计，是权值估计误差.是模糊基函数向量，是可调参数向量，可由如式(16)的自适应更新律更新

${\stackrel{\·}{\hat{W}}}_{\mathrm{ip}}={\mathrm{\η}}_{\mathrm{ip}}\left|x_i^T{P}_i{B}_i\right|{\hat{Z}}_{\mathrm{ip}}\left(\right|x_i^T{P}_i{B}_i\left|\right)---\left(16\right)$

其中η_ip为正常数，且P_i是如下Riccati方程的解，

$A_i^T{P}_i+P_i{A}_i+P_i{B}_i{B}_i^T{P}_i=-Q_i---\left(17\right)$

其中Q_i∈R^2×2是对称正定矩阵.

比较式(3)和式(10)，可知

${\stackrel{\·\·}{e}}_i=-k_{\mathrm{iv}}{\stackrel{\·}{e}}_i-k_{\mathrm{ip}}{e}_i+M_i^{-1}(u_{\mathrm{ic}}-Z_i)---\left(18\right)$

注意到 $x_i={\left[\begin{array}{cc}{e}_i& {\stackrel{\·}{e}}_i\end{array}\right]}^T,$ 并定义为交联项，则上式为

${\stackrel{\·}{x}}_i=A_i{x}_i+B_i{\tilde{h}}_i---\left(19\right)$

其中 $A_i=\left[\begin{array}{cc}0& 1\\ -k_{\mathrm{ip}}& -k_{\mathrm{iv}}\end{array}\right],$ $B_i=\left[\begin{array}{c}0\\ 1\end{array}\right],$ ${\tilde{h}}_i=M_i^{-1}(u_{\mathrm{ic}}-Z_i).$

2.2基于分散滑模观测器的故障检测模块

首先设计分散滑模观测器检测可重构机械臂子系统各传感器是否发生故障。

引入一阶滤波器

${\stackrel{\·}{z}}_{\mathrm{ai}}=A_{\mathrm{ai}}{z}_{\mathrm{ai}}+B_{\mathrm{ai}}{y}_i---\left(20\right)$

其中 $z_{\mathrm{ai}}=\left[\begin{array}{c}{z}_{\mathrm{ai}1}\\ z_{\mathrm{ai}2}\end{array}\right].$

令 $A_{\mathrm{ai}}=\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right],$ $B_{\mathrm{ai}}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$ 则

${\stackrel{\·}{z}}_{\mathrm{ai}}=A_{\mathrm{ai}}{z}_{\mathrm{ai}}+B_{\mathrm{ai}}{y}_i=A_{\mathrm{ai}}{z}_{\mathrm{ai}}+B_{\mathrm{ai}}(C_i{x}_i+D_i{f}_{\mathrm{si}})---\left(21\right)$

则扩展后的子系统动力学方程为

${\stackrel{\‾}{S}}_{\mathrm{if}}:\left\{\begin{array}{c}{\stackrel{\·}{z}}_i=A_i{z}_i+B_i[f_i(q_i,{\stackrel{\·}{q}}_i)+g_i\left(q_i\right)u_i+h_i(q,\stackrel{\·}{q,}\stackrel{\·\·}{q})]\\ {\stackrel{\·}{z}}_{\mathrm{ai}}=A_{\mathrm{ai}}{z}_{\mathrm{ai}}+B_{\mathrm{ai}}(C_i{x}_i+D_i{f}_{\mathrm{si}})\\ y_a=z_{\mathrm{ai}}\end{array}\right.---\left(22\right)$

分散滑模观测器设计为

${\hat{S}}_{\mathrm{if}}:\left\{\begin{array}{c}{\stackrel{\·}{\hat{z}}}_i=A_i{\hat{z}}_i+B_i[{\hat{f}}_i(q_i,{\stackrel{\·}{q}}_i)+{\hat{g}}_i\left(q_i\right)u_i+{\hat{p}}_i\left(\right|x_i^T{P}_i{B}_i|,{\hat{W}}_{\mathrm{ip}})]\\ {\stackrel{\·}{\hat{z}}}_{\mathrm{ai}}=A_{\mathrm{ai}}{z}_{\mathrm{ai}}+B_{\mathrm{ai}}(C_i{x}_i+v_i)\\ {\hat{y}}_a={\hat{z}}_{\mathrm{ai}}\end{array}\right.---\left(23\right)$

其中

$v_i=-k_i\frac{y_a-{\hat{y}}_a}{\left|\right|y_a-{\hat{y}}_a\left|\right|}---\left(24\right)$

和由模糊逻辑系统估计得到。

令 $\left\{\begin{array}{c}{e}_i=z_i-{\hat{z}}_i\\ e_{\mathrm{ai}}=z_{\mathrm{ai}}-{\hat{z}}_{\mathrm{ai}}\end{array}\right.---\left(25\right)$

因此可知子系统误差方程

$\left\{\begin{array}{c}{\stackrel{\·}{e}}_i=A_i{e}_i+B_i[f_i-{\hat{f}}_i+(g_i-{\hat{g}}_i)u_i+h_i-{\hat{p}}_i]\\ {\stackrel{\·}{e}}_{\mathrm{ai}}=A_{\mathrm{ai}}{e}_{\mathrm{ai}}+B_{\mathrm{ai}}(C_i{e}_i+D_i{f}_{\mathrm{si}}-v_i)\end{array}\right.---\left(26\right)$

假设3：估计误差的范数满足||w_i||≤L_i||e_i||，其中L_i为已知正常数。

假设4：子系统观测误差e_i有界，即sup{||e_i||}≤b_i，其中b_i为已知小正常数。

定义滑模面

s_i＝{col(e_i,e_ai)|e_ai＝0}(27)

定义：若令k_i满足式(28)，且假设3和假设4成立，则子系统误差(26)可到达式(31)的滑模面。

k_i≥b_i+ρ_i+η_i(28)

其中为η_i＞0常数.

在滑动运动中，在有限时间内发生如下式的滑模动态

$\left\{\begin{array}{c}{e}_{\mathrm{ai}}=0\\ {\stackrel{\·}{e}}_{\mathrm{ai}}=0\end{array}\right.---\left(29\right)$

因此通过式(26)，得到

C_ie_i+D_if_si-v_ieq＝0(30)

其中v_ieq是滑模控制的等效输出误差，其等效于式(24)所示的非连续函数v_i。

由式(30)，可以得到

$f_{\mathrm{si}}=D_i^{-1}(v_{\mathrm{ieq}}-C_i{e}_i)---\left(31\right)$

考虑式(24)所示的函数v_i，当选择一个合适的正常数σ_i时，等效输出误差v_ieq可由以下以任意精度进行估计，

$v_{\mathrm{i\σ}}=k_i\frac{z_{\mathrm{ai}}-{\hat{z}}_{\mathrm{ai}}}{\left|\right|z_{\mathrm{ai}}-{\hat{z}}_{\mathrm{ai}}\left|\right|+{\mathrm{\σ}}_i}---\left(32\right)$

其中k_i满足式(28)所示条件。

2.3可重构机械臂传感器故障实时诊断及容错系统

根据可重构机械臂系统的模块化属性，采用分散控制思想进行容错控制律设计，此方法可将故障隔离在子系统中，故障模块控制性能的下降不影响非故障子系统的正常运行。同时，作为子系统反馈控制信号的位置信号和速度信号在功能上是冗余关系，因此可利用解析余度的思想，设计传感器故障容错控制系统，即当检测出位置传感器发生故障，采用速度信号的数值积分信号代替故障的位置信号，同理，当检测出速度传感器发生故障，采用位置信号的数值微分信号代替速度信号，实现基于信号重构的主动分散容错控制。

下面，分别对位置传感器故障和速度传感器故障信号重构方法进行详细设计。

2.3.1位置传感器故障信号重构

变步长的Runge-Kutta法是一种常用的常微分方程解法，其可根据实际问题的具体情况合理选择每一步的步长，平衡了局部截断误差和误差累积过大之间的矛盾，精确性高.本文选择基于四阶变步长Runge-Kutta算法的积分器进行数值积分运算.

标准的四阶Runge-Kutta公式如(33)

$\left\{\begin{array}{c}{y}_{n+1}=x_n+\frac{1}{6}(k_1+2k_2+2k_3+k_4)\\ k_1=f(x_n,y_n)\\ k_2=f(x_n+\frac{h}{2},y_n+\frac{h}{2}{k}_1)\\ k_3=f(x_n+\frac{h}{2},y_n+\frac{h}{2}{k}_2)\\ k_4=f(x_n+h,y_n+h{k}_3)\end{array}\right.---\left(33\right)$

式中，n为计算次数，且n＝0,1,2,…。

变步长的Runge-Kutta方法的计算步骤如下：

设误差上限为ε，下限为M＞1，步长最大值为h₀.从x_h出发进行计算，步长为h.

(1)用步长h和Runge-Kutta公式计算用步长计算两步得并计算Δ；

(2)若Δ＞ε，说明步长过大，应将h折半，返回(1)重新计算；

(3)若说明步长过小，在下一步将h放大，但不超过h₀。

2.3.2速度传感器故障信号重构

数值微分器可直接由位置传感器信号重构得到速度传感器信号，本文选用如(34)式的微分跟踪器代替故障速度传感器信号实现主动容错控制.

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ω}}}_{i1}\left(t\right)={\mathrm{\ω}}_{i2}\left(t\right)-{\mathrm{\λ}}_{i1}\sqrt{|{\mathrm{\ω}}_{i1}\left(t\right)-x_{i1}\left(t\right)|}\\ \×\mathrm{sgn}({\mathrm{\ω}}_{i1}\left(t\right)-x_{i1}\left(t\right))\\ {\stackrel{\·}{\mathrm{\ω}}}_{i2}\left(t\right)=-{\mathrm{\λ}}_{i2}\mathrm{sgn}({\mathrm{\ω}}_{i1}\left(t\right)-x_{i1}\left(t\right))\\ {\hat{x}}_{i2}\left(t\right)={\widehat{\stackrel{\·}{x}}}_{i1}\left(t\right)={\mathrm{\ω}}_{i2}\left(t\right)\end{array}\right.---\left(34\right)$

其中，为由位置传感器信号重构得到的速度信号，ω_i1,ω_i2为辅助变量，λ_i1,λ_i2为设计参数.

2.3.3主动分散容错控制方案

主动分散容错控制系统分为故障检测、信号重构、取代控制以及反馈控制四个部分.其主动分散容错控制策略为：当实时检测出位置传感器发生故障，立即采用式(33)中的重构信号取代位置传感器信号进行反馈控制，同理，当子系统速度传感器发生故障时，立即采用如式(34)所重构的速度信号进行反馈控制，以达到主动容错控制的目的。

3.可重构机械臂传感器故障实时诊断和容错系统软件设计

本发明可重构机械臂传感器故障实时诊断和容错系统软件采用Matlab语言编写，生成.m文件。程序设计思想及实现过程如图3所示，程序开始后进行初始化过程，然后将主控器指令送给执行器，继续进入子系统动力学模型实现无故障状态的控制过程。然后进行传感器故障判断，若判断出故障，则进行相应的故障传感器信号重构，再利用切换装置将相应的重构信号替换故障信号，进而进行数据存储，若判断无故障发生，则直接进行数据存储，然后再进行运行时间判断，若未到达运行时间，则转至主控器继续运行，若达到运行时间，则输出结果结束。

分散主控模块采用控制器如式(10)-(12)，其中自适应更新律如式(13)、(16)，控制器、观测器及微分跟踪器各参数设置如下：k_ip＝400，k_iv＝80，Q_i＝diag(10,10)，η_ip＝1,Γ_iη＝1，σ_i＝0.0003，k_i＝2，给定误差阈值e_i＝0.01，λ_i1＝8，λ_i2＝8。

通过点击“Run”按钮，系统即可运行，并实时诊断系统是否发生传感器故障，其中故障检测部分由分散滑模观测器提供信息。当系统未发生故障时，采用分散主控模块，主动切换装置提供传感器信号作为控制器反馈信号，实现正常工作模式下的可重构机械臂轨迹跟踪控制；当故障检测模块诊断出传感器存在故障时，即当位置传感器发生故障时，采用速度信号的数值积分重构位置信号，当速度传感器发生故障时，则采用微分跟踪器结合位置信息重构速度信号，同时利用主动切换装置将故障传感器信号通道关闭，并将相应的重构信号反馈至主控模块，实现主动容错。结果可采用word、excel或图表形式保存。

Claims (2)

1.可重构机械臂传感器故障实时诊断和容错系统，其特征在于，该系统包括分散主控模块、故障检测模块、信号重构模块、主动切换装置、执行器及传感器信号采集子系统，所述传感器信号采集子系统包括位置传感器和速度传感器；所述分散主控模块与执行器连接，其用于利用模糊逻辑系统可任意精度拟合非线性函数的特性，采用模糊逻辑控制系统对子系统非线性项进行估计，并采用自适应调节器补偿系统估计误差；所述故障检测模块与传感器信号采集子系统连接，其用于先通过引入一阶滤波器将子系统传感器故障转化为伪执行器故障，再采用分散滑模观测器技术对子系统传感器故障进行检测，同时判断故障发生时间；所述信号重构模块与故障检测模块连接，其用于在所述故障检测模块判断出子系统位置传感器发生故障时，利用数字积分器对速度传感器信号进行积分，重构出位置信号；在所述故障检测模块判断出子系统速度传感器发生故障时，利用微分跟踪器重构出速度信号；所述主动切换装置分别与信号重构模块、故障检测模块、分散主控模块连接，其用于按照制定的主动切换策略，将故障信号通道关闭，开启相应的重构信号通道，将重构信号作为反馈送至分散主控模块实现可重构机械臂传感器故障的实时诊断与容错控制；所述执行器和传感器信号采集子系统，用于为系统运行提供动力支持和反馈信号。

2.可重构机械臂传感器故障实时诊断和容错方法，其特征在于，该方法包括如下步骤：

第一步，分散主控模块设计：当系统无传感器故障时，针对式(5)所示子系统动力学状态空间表达式，设计如式(10)-(12)所示的分散主控模块，使得关节模块能够精确跟踪期望轨迹；此分散主控模块包括CTC控制器、模糊控制系统和自适应调节器；

$S_i:\left\{\begin{array}{c}{\stackrel{\·}{x}}_i=A_i{x}_i+B_i\[f_i(q_i,{\stackrel{\·}{q}}_i)+g_i\left(q_i\right)u_i+h_i(q,\stackrel{\·}{q},\stackrel{\·\·}{q})\]\\ y_i=C_i{x}_i\end{array}\right.---\left(5\right)$

u_i＝u_i0+u_ic(10)

$u_{i0}=M_i\left(q_i\right)({\stackrel{\·\·}{q}}_{id}-k_{iv}{\stackrel{\·}{e}}_i-k_{ip}{e}_i)+C_i(q_i,{\stackrel{\·}{q}}_i){\stackrel{\·}{q}}_i+G_i\left(q_i\right)---\left(11\right)$

$u_{ic}=-M_i\left(\mathrm{sgn}\right(x_i^T{P}_i{B}_i){\hat{p}}_i\left(\right|x_i^T{P}_i{B}_i|,{\hat{W}}_{ip})+\mathrm{sgn}\left(x_i^T{P}_i{B}_i\right){\widehat{\mathrm{\η}}}_i)---\left(12\right)$

式中，x_i是子系统S_i的状态向量，y_i是子系统S_i的输出，且 $\begin{array}{cccc}{A}_i=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]& B_i=\left[\begin{array}{c}0\\ 1\end{array}\right]& C_i=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],& f(q_i,{\stackrel{\·}{q}}_i)=M_i^{-1}\left(q_i\right)\[-C_i(q_i,{\stackrel{\·}{q}}_i){\stackrel{\·}{q}}_i-G_i\left(q_i\right)\]\end{array},$ $g_i\left(q_i\right)=M_i^{-1}\left(q_i\right),h_i(q,\stackrel{\·}{q},\stackrel{\·\·}{q})=-M_i^{-1}\left(q_i\right)Z_i(q,\stackrel{\·}{q},\stackrel{\·\·}{q}),$ q∈Rⁿ为关节位置向量，M(q)∈R^n×n为惯性矩阵，为哥氏力和离心力项，G(q)∈Rⁿ为重力项；u_i0是计算力矩控制律，u_ic是鲁棒自适应模糊控制律，用来补偿系统不确定性；k_iv,k_ip是微分和比例常数， $x_i={\left[\begin{array}{cc}{e}_i& {\stackrel{\·}{e}}_i\end{array}\right]}^T$ 定义为跟踪误差向量，是可调参数；M_i(q_i)为第i个子系统的惯性矩阵分量；为第i个子系统的哥氏力和离心力矩阵分量；G_i(q_i)为第i个子系统的重力矩阵分量；为模糊逻辑系统权值的转置；为子系统交联项；P_i是如下Riccati方程的解， $A_i^T{P}_i+P_i{A}_i+P_i{B}_i{B}_i^T{P}_i=-Q_i,$ 其中Q_i∈R^2×2是对称正定矩阵；

第二步，故障检测模块设计：通过引入如式(20)的一阶滤波器将故障子系统方程扩展为式(22)，设计分散滑模观测器如式(23)，由此可判断出何种传感器何时发生故障：

${\stackrel{\·}{z}}_{ai}=A_{ai}{z}_{ai}+B_{ai}{y}_i---\left(20\right)$

${\stackrel{\‾}{S}}_{if}:\left\{\begin{array}{c}{\stackrel{\·}{z}}_i=A_i{z}_i+B_i\[f_i(q_i,{\stackrel{\·}{q}}_i)+g_i\left(q_i\right)u_i+h_i(q,\stackrel{\·}{q},\stackrel{\·\·}{q})\]\\ {\stackrel{\·}{z}}_{ai}=A_{ai}{z}_{ai}+B_{ai}(C_i{x}_i+D_i{f}_{si})\\ y_a=z_{ai}\end{array}\right.---\left(22\right)$

${\hat{S}}_{if}:\left\{\begin{array}{c}{\stackrel{\·}{\hat{z}}}_i=A_i{\hat{z}}_i+B_i\[{\hat{f}}_i(q_i,{\stackrel{\·}{q}}_i)+{\hat{g}}_i\left(q_i\right)u_i+{\hat{p}}_i\left(\right|x_i^T{P}_i{B}_i|,{\hat{W}}_{ip})\]\\ {\stackrel{\·}{\hat{z}}}_{ai}=A_{ai}{\hat{z}}_{ai}+B_{ai}(C_i{x}_i+v_i)\\ {\hat{y}}_a={\hat{z}}_{ai}\end{array}\right.---\left(23\right)$

其中， $z_{ai}=\left[\begin{array}{c}{z}_{ai1}\\ z_{ai2}\end{array}\right],A_{ai}=\left[\begin{array}{cc}-1& 0\\ 0& -1\end{array}\right],B_{ai}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],$ 是的估计， ${\tilde{W}}_{ip}^T=W_{ip}^T-{\hat{W}}_{ip}^T$ 是权值估计误差，是可调参数向量， 和由模糊逻辑系统估计得到；表示扩展后的子系统动力学方程；z_i表示李代数变换后的系统变量；y_a为扩展后子系统输出量；表示设计的分散滑模观测器；

第三步，信号重构模块设计：根据第二步判断出的故障传感器类型及故障发生时间，利用冗余传感器信息对故障传感器信号进行重构，即当位置传感器发生故障时，采用基于四阶变步长Runge-Kutta算法的积分器进行位置信号重构；当速度传感器发生故障时，采用如式(34)所示的数值微分器对速度信号进行重构；

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ω}}}_{i1}\left(t\right)={\mathrm{\ω}}_{i2}\left(t\right)-{\mathrm{\λ}}_{i1}\sqrt{|{\mathrm{\ω}}_{i1}\left(t\right)-x_{i1}\left(t\right)|}\\ \×\mathrm{sgn}\left({\mathrm{\ω}}_{i1}\right(t)-x_{i1}(t\left)\right)\\ {\stackrel{\·}{\mathrm{\ω}}}_{i2}\left(t\right)=-{\mathrm{\λ}}_{\mathrm{i2}}\mathrm{sgn}\left({\mathrm{\ω}}_{i1}\right(t)-x_{i1}(t\left)\right)\\ {\hat{x}}_{i2}\left(t\right)={\widehat{\stackrel{\·}{x}}}_{i1}\left(t\right)={\mathrm{\ω}}_{i2}\left(t\right)\end{array}\right.---\left(34\right)$

其中，为由位置传感器信号重构得到的速度信号，ω_i1,ω_i2为辅助变量，λ_i1,λ_i2为设计参数，x_i1表示位置传感器信号；

第四步，主动切换装置设计：基于第二步和第三步中所判断出的故障类型及重构信号，当判断出未发生传感器故障时，主动切换装置不进行信号切换，采用第一步中所设计的分散主控模块进行控制；当判断出位置传感器故障时，主动切换装置用位置重构信号取代位置传感器信号进行反馈控制；当判断出速度传感器故障时，主动切换装置用速度重构信号取代速度传感器信号进行反馈控制，从而实现反馈信号的主动切换，使发生传感器故障的可重构机械臂关节模块仍能精确跟踪期望轨迹，实现故障系统的容错控制，进而完成可重构机械臂传感器故障的实时诊断及容错方法。