CN112269317B - 一种基于扩展卡尔曼滤波器的双边遥操作控制方法 - Google Patents

Abstract

本发明提供一种基于扩展卡尔曼滤波器的双边遥操作控制方法，包括如下步骤：考虑***不确定性和噪声信息，基于拉格朗日方程，建立双边遥操作***模型；针对双边遥操作模型中存在的不确定扰动，设计干扰观测器；将干扰观测器的输出作为双边遥操作***状态变量的一部分，得到***扩张状态方程；设计扩展卡尔曼滤波器去除过程噪声和测量噪声，得到***状态和扰动的准确估计；设计自适应二阶滑模有限时间控制器，来抵消不确定性扰动对***的影响以及消除***抖震，使从端机器人能够快速准确跟踪主端机器人。

Description

一种基于扩展卡尔曼滤波器的双边遥操作控制方法

技术领域：

本发明属于双边遥操作控制领域，具体涉及一种基于扩展卡尔曼滤波器的双边遥操作控制方法。

背景技术：

随着网络技术的快速发展，在医疗机器人手术、外太空操作、水下探索等多种场景下，双边遥操作***的研究逐渐受到关注。通过医疗机器人可以在很远的距离内进行安全、准确的微创手术。通过远程机器人***实现了外太空作业的地面跟踪和钻孔定位任务。同时，将水下遥控操作***应用于危险环境下的深水探测。双边遥操作***擅长于在人类难以接近的复杂、危险的环境中工作，***中存在动态扰动，未知摩擦时延等不确定性干扰因素。另外，在实际工程应用中，由于角度传感器或速度传感器的带宽有限，不可避免地会产生过程噪声和测量噪声等噪声信息。不确定性干扰因素以及噪声信息严重影响***稳定性和透明性，给精确的位置控制带来了巨大的挑战。因此，设计一个控制算法处理这些不确定性干扰因素和噪声信息显得尤为重要。

干扰观测器可以对***中的不确定扰动进行观测，具有很强的抗干扰能力，因此能够被很好的应用于双边遥操作***的位置精度控制。本发明采用一种基于扩展卡尔曼滤波器的双边遥操作控制方法，设计基于干扰观测器的扩展卡尔曼滤波器不仅能够观测***状态和扰动，而且可以减小噪声影响，设计自适应二阶有限时间控制器，保证***的快速跟踪性能，同时采用二阶滑模控制算法消除了传统滑模方法中出现的抖振现象。

发明内容：

本发明的目的在于提供一种基于扩展卡尔曼滤波器的双边遥操作控制方法，来处理双边遥操作***中存在的不确定性干扰因素和噪声扰动问题，提高***的稳定性和透明性，弥补现有研究方法中对***噪声影响处理较少的缺点。本发明通过干扰观测器观测扰动，并将其扩张为新的***状态，利用扩展卡尔曼滤波器对扩张状态方程进行滤波，得到更加精确的***状态和扰动的估计，再对***扰动进行补偿，设计自适应有限时间控制器，实现主从机器人的快速无抖振位置跟踪。

本发明技术方案如下：

一种基于扩展卡尔曼滤波器的双边遥操作控制方法，包括如下步骤：

步骤一：考虑***不确定性和噪声信息，基于拉格朗日方程，建立双边遥操作***模型；

步骤二：在步骤一的基础上，针对双边遥操作模型中存在的不确定扰动，设计干扰观测器；

步骤三：在步骤二的基础上，将干扰观测器的输出作为双边遥操作***状态变量的一部分，得到***扩张状态方程；

步骤四：在步骤三的基础上，设计扩展卡尔曼滤波器去除过程噪声和测量噪声，得到***状态和扰动的准确估计；

步骤五：在步骤四的基础上，设计自适应二阶滑模有限时间控制器，来抵消不确定性扰动对***的影响以及消除***抖震，使从端机器人能够快速准确跟踪主端机器人。

在步骤一中，考虑***不确定性干扰因素和噪声信息，根据拉格朗日方程，建立双边遥操作***模型为：

其中，下标m和s分别表示双边遥操作***的主端机器人和从端机器人，让i＝m,s,q_i(t)∈Rⁿ表示关节角位移矩阵，表示关节角速度矩阵,/>表示关节角加速度矩阵，/>为正定对称惯性矩阵，/>为哥氏力和离心力矩阵，g_i(q_i(t))∈Rⁿ表示重力矩阵,N_i(t)∈Rⁿ为***不确定性扰动，τ_i(t)∈Rⁿ为主从端机器人控制器提供的输入力矩，F_m(t)∈Rⁿ和F_s(t)∈Rⁿ分别为人为操作力转矩和环境力转矩，ω_i(t)为过程噪声；

考虑到双边遥操作***模型中包含动态不确定性结构参数，存在

其中M_i(q_i(t))和C_i(q_i(t))表示标称动态部分，ΔM_i(q_i(t))和ΔC_i(q_i(t))表示动态不确定性部分，把***不确定性结构参数和不确定性扰动作为***总集扰动，用D_i(t)表示

因此，双边遥操作***模型重写为

***测量方程为

y_m(t)＝q_m(t)+η_m(t)

y_s(t)＝q_s(t)+η_s(t)

其中y_i(t)为***测量输出,η_i(t)为测量噪声；

在步骤二中，设计干扰观测器来估计未知***总集扰动D_i(t)，该干扰观测器不需要测量加速度

其中是干扰观测器增益矩阵，C∈Rⁿ是一个常数可逆矩阵，是D_i(t)的估计值，D_i(t)可以通过如下***得到

进一步的，在步骤三中，定义双边遥操作***状态方程为/>

为简单起见，将以下变量引入为

为了消除噪声干扰，提出基于干扰观测的扩展卡尔曼滤波器，同时将干扰观测器的输出作为新的***状态变量，因此，***扩张状态方程为

其中变量F(t)和G(t)表示如下

进一步的，在步骤四中，利用Ito随机微分方程，将双边遥操作***建模为

dX(t)＝f[X(t),x(t)]dt+G(t)dω_i(t)

dy(t)＝h[X(t)]dt+D(t)dη_i(t)

其中，非线性函数f(·)和h(·)是连续微分非线性函数，通过下面方程的一阶展开式得到

是由矩阵随机过程的线性部分α(t)，β(t)和非线性部分组成，其中

***的输出y_i(t)由***测量方程表示，因此，h[X(t)]＝Hx(t)，其中H＝[I 0]^T为状态观测矩阵；对该非线性***，进行扩展卡尔曼滤波策略的结构如下

状态估计的微分方程

滤波器增益：

K(t}＝P(t)β^T(t)R^-1(t)

黎卡提微分方程：

dP(t)＝[α(t)P(t)+P(t)α^T(t)+Q(t)-P(t)β^T(f)R^-1(t)β(t)P(t)]dt

其中Q(t)和R(t)是时变对称正定矩阵，一般是选择***模型中扰动噪声项的协方差

Q(t)＝G(t)G^T(t)

R(t)＝D(t)D^T(t)

进一步的，在步骤五中，主、从机器人位置误差函数为

e_m(t)＝x_1m(t-T_m)-x_1s(t)

e_s(t)＝x_1s(t-T_s)-x_1m(t)

其中T_m和T_s分别表示主端机器人到从端机器人以及从端机器人到主端机器人的时间延迟，主、从机器人速度误差函数为

根据***状态方程，主、从机器人加速度误差函数为

选取线性滑模面ε＝diag(ε₁₁,…,ε_nn)是一个正定矩阵，为了保证线性滑模面s在有限时间内收敛到零，设计一种新的非奇异的终端滑模面

其中，K_j＝diag(k_j1,…,k_jn)，j＝1,2是正定矩阵，θ(s_i)＝[θ(s_i1),…,θ(s_in)]^T

其中λ是一个足够小的正常数,a和b是正奇数且满足 和/>对非奇异终端滑模面求时间的导数，可得

其中表示为

自适应律设计为

因此，设计自适应二阶滑模有限时间控制器τ,，主从端机器人控制器提供的输入力矩τ_i(t)如下，由等效控制器参数u_eq和辅助控制器参数u_au组成

其中，当时可以求得/>

其中κ＝diag(κ₁₁,…,k_nn)是正定矩阵。

本发明与现有技术相比有如下优点：

1、双边遥操作***是复杂的网络化非线性***，并处于复杂的工作环境中，不确定感染因素较多，工程实践中噪声信息对***稳定性和透明性的影响是不可避免的。现有双边遥操作***处理噪声信息的研究较少，因此，本发明考虑了不确定性扰动和噪声信息，基于拉格朗日方程建立新的双边遥操作动力学模型；

2、本发明不用增加昂贵的加速度传感器，能对双边遥操作***实现高精度位置跟踪控制；

3、本发明通过干扰观测器观测扰动，并将其扩张为新的***状态，利用扩展卡尔曼滤波器对扩张状态方程进行滤波，得到更加精确的***状态和扰动的估计，再对***扰动进行补偿。与传统观测器相比，它同时具有观测状态、扰动和去除噪声的功能，增强了***鲁棒性；

4、本发明采用自适应二阶滑模有限时间控制算法，它的控制器结构简单，参数整定方便，鲁棒性强，可以保证***的快速跟踪性能，同时消除了传统滑模控制方法中常出现的抖振现象。

附图说明：

图1为双边遥操作***结构框图；

图2为基于扩展卡尔曼滤波器的双边遥操作控制算法原理框图；

图3为扩展卡尔曼滤波器估计扰动误差曲线图；

图4为双边遥操作***主从机械臂位置跟踪曲线图。

具体实施方式：

为使本发明的目的更加明确，技术方案更加清晰，以下结合附图和具体实施例对本发明进行详细说明。

图1所示为本发明所述的双边遥操作***结构框图，其主要由操作者，主端机器人，网络传输通道，从端机器人和工作环境五部分组成。在双边遥操作实际应用***中，由于需要传感器测量位置信息，需要网络通道进行主从端信息交互，因此***中存在着噪声信息，同时由于工作环境的复杂性，***中不可避免的存在不确定干扰因素，如摩擦力，外部扰动等。

图2为本发明的控制原理框图，表明了本发明所述的一种基于扩展卡尔曼滤波器的双边遥操作控制算法。

下面结合图2、图3和图4对本发明所述控制算法进行详细描述，具体内容包括以下步骤：

步骤一，考虑***不确定性干扰因素和噪声信息，根据拉格朗日方程，建立双边遥操作***模型为：

其中，下标m和s分别表示双边遥操作***的主端机器人和从端机器人，让i＝m,s,q_i(t)∈Rⁿ表示关节角位移矩阵，表示关节角速度矩阵,/>表示关节角加速度矩阵，/>为正定对称惯性矩阵，/>为哥氏力和离心力矩阵，g_i(q_i(t))∈Rⁿ表示重力矩阵,N_i(t)∈Rⁿ为***不确定性扰动，τ_i(t)∈Rⁿ为主从端机器人控制器提供的控制输入力矩，F_m(t)∈Rⁿ和F_s(t)∈Rⁿ分别为人为操作力转矩和环境力转矩，ω_i(t)为过程噪声。

考虑到双边遥操作***模型中包含动态不确定性结构参数，存在

其中M_i(q_i(t))和C_i(q_i(t))表示标称动态部分，ΔM_i(q_i(t))和ΔC_i(q_i(t))表示动态不确定性部分，把***不确定性结构参数和不确定性扰动作为***总集扰动，用D_i(t)表示

因此，双边遥操作***模型可以改写为

***测量方程为

y_m(t)＝q_m(t)+η_m(t)

y_s(t)＝q_s(t)+η_s(t)

其中y_i(t)为***测量输出,η_i(t)为测量噪声。

步骤二，设计干扰观测器来估计未知***总集扰动D_i(t)，该干扰观测器不需要测量加速度

其中是干扰观测器增益矩阵，C∈Rⁿ是一个常数可逆矩阵，是D_i(t)的估计值，D_i(t)可以通过如下***得到

步骤三，写出***扩张状态方程，定义则双边遥操作***状态方程为/>

为简单起见，将以下变量引入为

为了消除噪声干扰，提出基于干扰观测的扩展卡尔曼滤波器，同时将干扰的输出作为新的***状态变量，因此，***扩张状态方程为

其中变量F(t)和G(t)表示如下

步骤四，设计扩展卡尔曼滤波器，利用Ito随机微分方程，进一步的将双边遥操作***建模为

dX(t)＝f[X(t),x(t)]dt+G(t)dω_i(t)

dy(t)＝h[X(t)]dt+D(t)dη_i(t)

其中，非线性函数f(·)和h(·)是连续微分非线性函数，通过下面方程的一阶展开式得到

是由矩阵随机过程的线性部分α(t)，β(t)和非线性部分组成，其中

***的输出y_i(t)由***测量方程表示，因此，h[X(t)]＝Hx(t)，其中H＝[I 0]^T为状态观测矩阵；对该非线性***，进行扩展卡尔曼滤波策略的结构如下

状态估计的微分方程

滤波器增益：

K(t}＝P(t)β^T(t)R^-1(t)

黎卡提微分方程：

dP(t)＝[α(t)P(t)+P(t)α^T(t)+Q(t)-P(t)β^T(f)R^-1(t)β(t)P(t)]dt

其中Q(t)和R(t)是时变对称正定矩阵，一般是选择***模型中扰动噪声项的协方差

Q(t)＝G(t)G^T(t)

R(t)＝D(t)D^T(t)

步骤五，设计自适应二阶有限时间滑模控制方案，主、从机器人位置误差函数为

e_m(t)＝x_1m(t-T_m)-x_1s(t)

e_s(t)＝x_1s(t-T_s)-x_1m(t)

其中T_m和T_s分别表示主端机器人到从端机器人以及从端机器人到主端机器人的时间延迟，主、从机器人速度误差函数为

根据***状态方程，主、从机器人加速度误差函数为

选取线性滑模面ε＝diag(γ₁₁,…,γ_nn)是一个正定矩阵，为了保证线性滑模面s在有限时间内收敛到零，设计一种新的非奇异的终端滑模面

其中，K_j＝diag(k_j1,…,k_jn)，j＝1,2是正定矩阵，θ(s_i)＝[θ(s_i1),…,θ(s_in)]^T

其中λ是一个足够小的正常数,a和b是正奇数且满足 和/>对非奇异终端滑模面求时间的导数，可得

其中表示为

自适应律设计为

设计自适应二阶滑模有限时间控制器τ，τ由等效控制器u_eq和辅助控制器u_au组成

其中，当时可以求得/>

其中κ＝diag(κ₁₁,…,κ_nn)是正定矩阵。

下面给出自适应二阶滑模有限时间控制器的稳定性证明。

根据本发明所述的双边遥操作***选择如下李雅普诺夫方程：

其中定义自适应错误为对求导得到进一步整理可以得到/>

其中根据假设我们可以知道，/>因此，根据李雅普诺夫稳定性判据，自适应二阶滑模面σ＝0可以在有限的时间内建立。另一方面，一旦σ达到零，它将在此后保持零。因此，滑动变量s将在有限时间内沿自适应二阶滑模面快速收敛到零。当/>双边遥操作***的跟踪误差e_i(t)可以沿滑模面渐进收敛到零。

实施例

为了验证上述理论的可行性，在MATLAB下进行仿真实验，仿真实验验证了基于扩展卡尔曼滤波器的双边遥操作控制方法的效果。按照图2所示原理图在MATLAB中搭建双边遥操作***仿真框架，定义主从机器人为2自由度的主从机械臂，其参数矩阵为：

其中，具体参数选择为：

仿真中主从机械臂间的通信时延为0.45s，为保持***器快速稳定的跟踪性能，控制器参数选则如下：

图3为扩展卡尔曼滤波器估计扰动误差曲线图，可以看出在噪声存在情况下，对扰动的估计误差很小，保持在10^-4数量级。图4为双边遥操作***主从机械臂位置跟踪曲线图，可以看出从机械臂可以很好的跟踪主机械臂的运动轨迹。通过仿真可以验证，本发明所述的基于扩展卡尔曼滤波器的双边遥操作控制方法能够在噪声和不确定干扰因素下实现双边遥操作***的快速稳定跟踪。

本发明未尽事宜为公知技术。

上述实施例只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人士能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡根据本发明精神实质所作的等效变化或修饰，都应涵盖在本发明的保护范围之内。

Claims (1)

1.一种基于扩展卡尔曼滤波器的双边遥操作控制方法，包括如下步骤：

步骤一：考虑***不确定性和噪声信息，基于拉格朗日方程，建立双边遥操作***模型，方法如下：

其中，下标m和s分别表示双边遥操作***的主端机器人和从端机器人，让i＝m,s,q_i(t)∈Rⁿ表示关节角位移矩阵，表示关节角速度矩阵,/>表示关节角加速度矩阵，/>为正定对称惯性矩阵，/>为哥氏力和离心力矩阵，g_i(q_i(t))∈Rⁿ表示重力矩阵,N_i(t)∈Rⁿ为***不确定性扰动，τ_i(t)∈Rⁿ为主从端机器人控制器提供的输入力矩，F_m(t)∈Rⁿ和F_s(t)∈Rⁿ分别为人为操作力转矩和环境力转矩，ω_i(t)为过程噪声；

考虑到双边遥操作***模型中包含动态不确定性结构参数，存在

其中M_i(q_i(t))和C_i(q_i(t))表示标称动态部分，ΔM_i(q_i(t))和ΔC_i(q_i(t))表示动态不确定性部分，把***不确定性结构参数和不确定性扰动作为***总集扰动，用D_i(t)表示

因此，双边遥操作***模型重写为

***测量方程为

y_m(t)＝q_m(t)+η_m(t)

y_s(t)＝q_s(t)+η_s(t)

其中y_i(t)为***测量输出,η_i(t)为测量噪声；

步骤二：在步骤一的基础上，针对双边遥操作模型中存在的不确定扰动，设计干扰观测器，方法如下：

设计干扰观测器来估计未知***总集扰动D_i(t)，该干扰观测器不需要测量加速度

其中是干扰观测器增益矩阵，C∈Rⁿ是一个常数可逆矩阵，/>是D_i(t)的估计值，D_i(t)通过如下***得到

步骤三：在步骤二的基础上，将干扰观测器的输出作为双边遥操作***状态变量的一部分，得到***扩张状态方程，方法如下：

定义双边遥操作***状态方程为

将以下变量引入为

为消除噪声干扰，提出基于干扰观测的扩展卡尔曼滤波器，同时将干扰观测器的输出作为新的***状态变量，因此，***扩张状态方程为

其中变量F(t)和G(t)表示如下

步骤四：在步骤三的基础上，设计扩展卡尔曼滤波器去除过程噪声和测量噪声，得到***状态和扰动的准确估计，方法如下：

利用Ito随机微分方程，将双边遥操作***建模为

dX(t)＝f[X(t)，x(t)]dt+G(t)dω_i(t)

dy(t)＝h[X(t)]dt+D(t)dη_i(t)

其中，非线性函数f(·)和h(·)是连续微分非线性函数，通过下面方程的一阶展开式得到

是由矩阵随机过程的线性部分α(t)，β(t)和非线性部分 组成，其中

***的输出y_i(t)由***测量方程表示，因此，h[X(t)]＝Hx(t)，其中H＝[I 0]^T为状态观测矩阵；对该非线性***，进行扩展卡尔曼滤波策略的结构如下

状态估计的微分方程

滤波器增益：

K(t}＝P(t)β^T(t)R^-1(t)

黎卡提微分方程：

dP(t)＝[α(t)P(t)+P(t)α^T(t)+Q(t)-P(t)β^T(f)R^-1(t)β(t)P(t)]dt

其中Q(t)和R(t)是时变对称正定矩阵，一般是选择***模型中扰动噪声项的协方差

Q(t)＝G(t)G^T(t)

R(t)＝D(t)D^T(t)；

步骤五：在步骤四的基础上，设计自适应二阶滑模有限时间控制器，来抵消不确定性扰动对***的影响以及消除***抖震，使从端机器人能够快速准确跟踪主端机器人，方法如下：

主、从机器人位置误差函数为

e_m(t)＝x_1m(t-T_m)-x_1s(t)

e_s(t)＝x_1s(t-T_s)-x_1m(t)

其中T_m和T_s分别表示主端机器人到从端机器人以及从端机器人到主端机器人的时间延迟，主、从机器人速度误差函数为

根据***状态方程，主、从机器人加速度误差函数为

选取线性滑模面ε＝diag(ε₁₁，...，ε_nn)是一个正定矩阵，为了保证线性滑模面s在有限时间内收敛到零，设计一种新的非奇异的终端滑模面

其中，K_j＝diag(k_j1，...，k_jn)，j＝1，2是正定矩阵，θ(s_i)＝[θ(s_i1)，...，θ(s_in)]^T

其中λ是一个足够小的正常数，a和b是正奇数且满足和/>对非奇异终端滑模面求时间的导数，可得

其中表示为

自适应律设计为

设计自适应二阶滑模有限时间控制器τ,，主从端机器人控制器提供的输入力矩τ_i(t)如下，由等效控制器参数u_eq和辅助控制器参数u_au组成：

其中，当时求得/>

其中v＝diag(κ₁₁,…,κ_nn)是正定矩阵。