CN114654470B - 一种基于自抗扰控制策略的上肢外骨骼系统协同随动控制方法 - Google Patents

Abstract

本发明公开一种基于自抗扰控制策略的上肢外骨骼系统协同随动控制方法，包括步骤S1、在上肢外骨骼系统的人机交互点安装多维力传感器，检测操作者对多维力传感器的人机相互作用力，构成上肢外骨骼系统的阻抗控制模型；步骤S2、通过阻抗控制模型获取操作手末端的期望位置，由得到的期望位置计算出上肢外骨骼系统中肩关节的期望关节角度和肘关节的期望关节角度；步骤S3、通过自抗扰控制器估计上肢外骨骼系统的状态和干扰，将估计的干扰作为上肢外骨骼系统的负反馈，并将干扰消除，能够估计出上肢外骨骼系统模型中耦合项和各种不确定因素引起的总扰动并加以补偿，针对上肢外骨骼系统中不确定因素具有较强鲁棒性。

Description

一种基于自抗扰控制策略的上肢外骨骼系统协同随动控制 方法

技术领域

本发明属于上肢外骨骼系统协同随动控制技术领域，具体涉及一种基于自抗扰控制策略的上肢外骨骼系统协同随动控制方法。

背景技术

随着人口老龄化问题的日益严峻与社会对于康复医疗问题的重视程度的增加，可穿戴的外骨骼机器人已经成为了当前研究的重点与热点，在抢险救灾、医疗康复、助力助行、国防军事等多个领域发挥重要作用。

传统的机器人通常是操作手末端的位置和姿态来实现机器人的控制，在整个控制过程中，需要知道操作手与目标位置，并通过逆运动学解算关节角度，对角度进行轨迹规划，计算出角速度和角加速度。最后，将相关角度信息带入动力学方程解算出关节所需的力矩，通过执行器驱动机器人的运动，从而实现机器人的位置控制。与传统机器人的控制不同，上肢外骨骼是人机一体系统，需要操作者实时做出运动决策，外骨骼系统只需负责执行相应的动作。上肢外骨骼系统控制目的是跟随操作者的运动，且给操作者提供部分助力。

操作者在穿戴上肢外骨骼系统完成搬运托举的过程中，外骨骼系统在给操作者提供助力的同时，还需要跟随操作者手部运动，且不能阻碍操作者手部的运动。要达到良好的随动控制效果，选择适合的控制策略与控制方法具有重要的意义。传统的机械臂使用力/位置控制、PD控制等方式，这些控制方式无法有效的控制机械臂与环境的交互。在产生偏差时，会由于缺乏柔顺性使机械臂在移动旋转等过程中超过期望值。尤其是PD控制在与不同环境切换过程会造成系统不稳定。

发明内容

根据现有技术的不足，本发明的目的是提供一种基于自抗扰控制策略的上肢外骨骼系统协同随动控制方法

为了解决上述技术问题，本发明采用的技术方案为：

一种基于自抗扰控制策略的上肢外骨骼系统协同随动控制方法，包括：

步骤S1、在上肢外骨骼系统的人机交互点安装多维力传感器，检测操作者对多维力传感器的人机相互作用力，构成上肢外骨骼系统的阻抗控制模型；

步骤S2、通过阻抗控制模型获取操作手末端的期望位置，由得到的期望位置计算出上肢外骨骼系统中肩关节的期望关节角度和肘关节的期望关节角度；

步骤S3、通过自抗扰控制器估计上肢外骨骼系统的状态和干扰，将估计的干扰作为上肢外骨骼系统的负反馈，通过自抗扰控制器将干扰消除。

进一步地，所述步骤S1中，将上肢外骨骼系统与人的相互作用力表示为惯性、阻尼与刚度的线性组合：

其中，F_hm为多维力传感器检测到的人机相互作用力，e是上肢外骨骼系统的期望位置和上肢外骨骼系统实际位置之间的差值，M为惯性系数，B为阻尼系数，K为刚度系数；

对阻抗控制模型建模，考虑上肢外骨骼系统中人机接口的柔性特征，采用弹簧模型进行阻抗控制模型建模，人机接口动力学表达式如下：

F_hm＝K(p_h-p_m)

其中，K为刚度系数，p_h为操作者手部的位置，即上肢外骨骼系统期望位置，p_m为上肢外骨骼系统的末端位置，F_hm为多维力传感器检测到的人机相互作用力。

进一步地，所述步骤S2中，上肢外骨骼系统包括第一机械臂、第二机械臂、肩关节、肘关节和握把，第一机械臂一端与肩关节固定，另一端与和肘关节可转动的连接，第二机械臂一端与肘关节固定，另一端与握把固定，人机交互点设在握把上，操作者手部握住握把，通过上肢外骨骼系统的运动学方程建立上肢外骨骼系统期望位置与肩关节角度和肘关节角度的关系，：

p_h＝[l_s sin(q_ds)+l_esin(q_ds+q_de),-l_s cos(q_ds)-l_ecos(q_ds+q_de),0]^T

其中，p_h为操作者手部的位置，即上肢外骨骼系统期望位置，q_ds为肩关节的期望关节角度，q_de为肘关节的期望关节角度，l_s为第一机械臂的长度，l_e为第二机械臂的长度。

进一步地，所述自抗扰控制器包括跟踪微分器、扩张状态观测器、线性状态误差补偿和扰动补偿，获取操作者对多维力传感器的人机相互作用力，通过多维力传感器得到给定输入信号，通过跟踪微分器对给定输入信号的微分进行提取，确定适合上肢外骨骼系统中给定输入信号的过渡过程，使得上肢外骨骼系统跟踪过渡过程对应的输出信号；所述扩张状态观测器将上肢外骨骼系统的自身不确定性、外部及环境因素的干扰，转化成含有未知扰动的积分串联环节，将估计的干扰作为上肢外骨骼系统的负反馈，通过线性状态误差补偿和扰动补偿进行实时补充，消除干扰。

进一步地，通过跟踪过渡过程对应的输出信号，在噪声较大的情况下，将给定输入信号v输入到跟踪微分器，得到平滑的输入信号：

其中，v₁，v₂是输入信号的跟踪信号；k₁，k₂为跟踪信号系数。

进一步地，在步骤S3中，上肢外骨骼系统设有二阶系统：

其中，x₁、x₂是上肢外骨骼系统的状态变量，y是上肢外骨骼系统的输出，u为上肢外骨骼系统的控制量，f(t)表示上肢外骨骼系统的不确定因素，b为控制量系数；

针对上肢外骨骼系统设计二阶线性的扩张状态观测器的形式如下；

其中，z₁，z₂是上肢外骨骼系统内部状态变量的实时估计值，z₃是上肢外骨骼系统不确定因素的实时估计值，参数β₁、β₂、β₃是扩张状态观测器的系数，起到误差校正作用，b₀为控制量系数。

进一步地，采用误差反馈线性组合方式也能取得良好的控制效果，为了避免对给定输入信号进行微分且使闭环传递函数成为不包括零点的二阶传递函数，状态误差反馈律可采用PD参数形式：

其中，e＝v₁-z₁为扩张状态观测器的观测误差，k_p、k_d是控制器增益，自抗扰控制是通过扩张状态观测器的观测量对控制信号进行扰动补偿，控制表达式为：

其中，b₀为控制量系数，u为上肢外骨骼系统的控制量，z₃是上肢外骨骼系统不确定因素的实时估计值。

进一步地，还包括驱动系统控制器，所述驱动系统控制器采用PID控制方法对驱动系统进行控制，驱动系统为驱动上肢外骨骼系统运动的系统，在PID控制方法中，设置比例单元P、积分单元I和微分单元D，并设置K_p、K_i和K_d实现上肢外骨骼系统的实际位置与上肢外骨骼系统的期望位置一致，通过PID控制方法获取输出驱动系统的电流：

其中，U为输出到驱动系统的电流，e是上肢外骨骼系统的期望位置和上肢外骨骼系统实际位置之间的差值，t为时间，K_p、K_i、K_d分别是比例单元、积分单元和微分单元的增益；

上肢外骨骼系统的期望位置和上肢外骨骼系统实际位置之间的差值的计算公式为：

e(t)＝q(t)-q_d(t)

其中，q_d是上肢外骨骼系统期望关节角度，q为上肢外骨骼系统实际关节角度；

获取驱动系统的输出力矩T，驱动上肢外骨骼系统运动，动力学方程可表示为：

其中，T_s是肩关节的输出力矩，T_e是肘关节的输出力矩，M，C，G分别是上肢外骨骼系统的惯性矩阵，向心力矩阵，重力矩阵。

进一步地，所述上肢外骨骼系统是一个多输入多输出系统，因此上肢外骨骼系统表达式可表示为：

其中，T_s是肩关节的输出力矩，T_e是肘关节的输出力矩，

是角速度，

肩关节的角速度，

肘关节的角速度，T_d是外部干扰，T_ds是肩关节的外部干扰，T_de是肘关节的外部干扰，M、C和G是惯性系数、向心力矢量和重力矢量；

其中，

当惯性矩阵恒为正，可以得到以下方程：

其中，τ＝M^-1(q)T，

f是系统的总干扰，包括系统误差，外部干扰和耦合等不确定因素，f_s是肩关节的的总干扰，f_e是肘关节的的总干扰，经过解耦后，上肢外骨骼系统构成的多输入多输出系统可以分解为两个通道的单输入单输出系统，使得自抗扰控制器分别控制肩关节和肘关节，上式可表示为：

肩关节状态扩张状态观测器可表示为：

其中，β₁₁，β₁₂，β₁₃是肩关节的扩张状态观测器增益；

肘关节的扩张状态观测器可表示为：

其中，β₂₁，β₂₂，β₂₃是肘关节扩张状态观测器的增益；

设扩张状态观测器的极点在-ω₁，-ω₂，扩张状态观测器的极点可根据多项式进行配置：

其中，ω₁，ω₂是肩关节扩张状态观测器的带宽和肘关节扩张状态观测器的带宽，扩张状态观测器增益可以表示为：

β₁₁＝3ω₁，

β₂₁＝3ω₁，

肩关节的反馈控制器和肘关节的反馈控制器可表达为：

其中，e₁，e₂分别是肩关节的扩张状态观测误差和肘关节的扩张状态观测误差，k₁₁，k₁₂是肩关节的反馈控制器的增益，k₂₁，k₂₂是肘关节的反馈控制器的增益，的反馈控制器的增益为

k₁₂＝2ω₁，

k₂₂＝2ω₂；

自抗扰控制策略可表达如下：

其中，

是上肢外骨骼系统估计的干扰，

进一步地，扩张状态观测器的带宽在所述扩张观测器的性能及所述扩张观测器对噪声的容纳程度之间取一个平衡值。

与现有技术相比，本发明具有以下优点和有益效果：

本发明所述的一种基于自抗扰控制策略的上肢外骨骼系统协同随动控制方法，特别设计基于自抗扰控制策略的上肢外骨骼系统协同随动控制，在上肢外骨骼系统的人机交互点处安装多维力传感器，测量人机之间相互作用力，并构成上肢外骨骼系统的阻抗控制模型，预先识别操作者的运动意图，在不直接控制机械臂末端与环境接触力的情况下，提高了控制的柔顺性。基于自抗扰控制策略，对各种不确定因素引起的总扰动加以补偿，保证良好的控制性能，自抗扰控制策略优化了其他控制方法中的干扰问题，具有较强鲁棒性。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：

图1为本发明的实施流程图。

图2为多维力传感器在人机交互点的安装位置。

图3为自抗扰控制系统结构。

图4为基于阻抗控制策略的控制框图。

图5为自抗扰控制器在外骨骼系统中的应用。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。

一种基于自抗扰控制策略的上肢外骨骼系统协同随动控制方法，其特征在于，包括：

步骤S1、在上肢外骨骼系统的人机交互点安装多维力传感器，检测操作者对多维力传感器的人机相互作用力，构成上肢外骨骼系统的阻抗控制模型；

步骤S2、通过阻抗控制模型获取操作手末端的期望位置，由得到的期望位置计算出上肢外骨骼系统中肩关节和肘关节的角度；

步骤S3、通过自抗扰控制器估计上肢外骨骼系统的状态和干扰，将估计的干扰作为上肢外骨骼系统的负反馈，通过自抗扰控制器将干扰消除。

本发明通过多维力传感器直接测量出人机作用力来识别操作者的运动意图，并构成外骨骼系统的阻抗控制模型。根据多维力传感器获取的信号，通过阻抗控制模型计算出操作手末端的期望位置。由于上肢外骨骼系统模型具有非线性的特点且存在许多干扰，因此采用自抗扰控制器将内部干扰与外部的干扰当成系统的总干扰，通过自抗扰控制器本身的结构特点将干扰消除，使上肢外骨骼系统有更好的跟踪效果。

本发明所述的一种基于自抗扰控制策略的上肢外骨骼系统协同随动控制方法，特别设计基于自抗扰控制(Active Disturbance Rejection Controller，ADRC)策略的上肢外骨骼系统协同随动控制，在上肢外骨骼系统的人机交互点处安装多维力传感器，测量人机之间相互作用力，并构成上肢外骨骼系统的阻抗控制模型，预先识别操作者的运动意图。基于自抗扰控制策略不需要外骨骼精确的数学建模，能够对各种不确定因素引起的总扰动并加以补偿，简化了控制对象，保证良好的控制性能，该控制方法优于传统的PID控制方法，针对外骨骼系统中不确定因素具有较强鲁棒性。

所述步骤S1中，对阻抗控制模型建模，考虑上肢外骨骼系统中人机接口的柔性特征，采用弹簧模型进行阻抗控制模型建模，人机接口动力学表达式如下：

F_hm＝K(p_h-p_m)

其中，K为刚度系数，p_h为操作者手部的位置，即上肢外骨骼系统期望位置，p_m为上肢外骨骼系统的末端位置，F_hm为多维力传感器检测到的人机相互作用力；

根据多维力传感器检测到的人机相互作用力F_hm和上肢外骨骼系统的末端位置p_m获取操作者手部的位置p_h。

具体地，p_h＝[p_hx,p_hy,p_hz]^T∈R^3×1，[P_hx，P_hy，P_hz]为操作者手部的坐标位置，p_m＝[p_mx,p_my,p_mz]^T∈R^3×1，[P_mx，P_my，P_mz]为上肢外骨骼系统的末端坐标位置，F_hm＝[F_x,F_y,T_z]^T∈R^3×1。

所述步骤S2中，上肢外骨骼系统包括第一机械臂、第二机械臂、肩关节、肘关节和握把，第一机械臂一端与肩关节固定，另一端与和肘关节可转动的连接，第二机械臂一端与肘关节固定，另一端与握把固定，人机交互点设在握把上，操作者手部握住握把，通过上肢外骨骼系统的运动学方程建立上肢外骨骼系统期望位置与肩关节角度和肘关节角度的关系，：

p_h＝[l_s sin(q_ds)+l_e sin(q_ds+q_de),-l_s cos(q_ds)-l_e cos(q_ds+q_de),0]^T

其中，q_ds为肩关节的期望关节角度，q_de为肘关节的期望关节角度，l_s为第一机械臂的长度，l_e为第二机械臂的长度。

在步骤S3中，通过自抗扰控制器获取上肢外骨骼系统的状态和干扰，所述自抗扰控制器包括跟踪微分器、扩张状态观测器、线性状态误差补偿和扰动补偿。

获取操作者对多维力传感器的人机相互作用力，通过多维力传感器得到给定输入信号，通过跟踪微分器对给定输入信号的微分进行提取，确定适合上肢外骨骼系统中给定输入信号的过渡过程，使得上肢外骨骼系统跟踪过渡过程对应的输出信号；所述扩张状态观测器将上肢外骨骼系统的自身不确定性、外部及环境因素的干扰，转化成含有未知扰动的积分串联环节，将估计的干扰作为上肢外骨骼系统的负反馈，通过线性状态误差补偿和扰动补偿进行实时补充，消除干扰。

具体地，通过跟踪过渡过程对应的输出信号，在噪声较大的情况下，将给定输入信号v输入到跟踪微分器，得到平滑的输入信号：

其中，v₁，v₂是输入信号的跟踪信号；k₁，k₂为跟踪信号系数。

在步骤S3中，上肢外骨骼系统设有二阶系统：

其中，b为控制量系数，x₁、x₂是上肢外骨骼系统的状态变量，y是上肢外骨骼系统的输出，u为上肢外骨骼系统的控制量，f(t)表示上肢外骨骼系统的不确定因素；

针对上肢外骨骼系统设计二阶线性的扩张状态观测器的形式如下；

其中，b₀为控制量系数，z₁，z₂是上肢外骨骼系统内部状态变量的实时估计值，z₃是上肢外骨骼系统不确定因素的实时估计值，参数β₁、β₂、β₃是扩张状态观测器的系数，起到误差校正作用，采用误差反馈线性组合方式也能取得良好的控制效果。

为了避免对给定输入信号进行微分且使闭环传递函数成为不包括零点的二阶传递函数，状态误差反馈律可采用PD参数形式：

其中，e为扩张状态观测器的观测误差，e＝v₁-z₁，k_p、k_d是控制器增益，自抗扰控制是通过扩张状态观测器的观测量对控制信号进行扰动补偿，控制表达式为：

其中，b₀为控制量系数，u₀上肢外骨骼系统的初值，u为上肢外骨骼系统的控制量，z₃是上肢外骨骼系统不确定因素的实时估计值。

通过线性扩展状态观测器(Linear Extended State Observer，LESO)估计出外骨骼系统模型中耦合项和各种不确定因素引起的总扰动并加以补偿，使复杂的上肢外骨骼系统变为积分器串联型系统，简化了控制对象，保证良好的控制性能。

其中，由于扩张状态观测器的性能受观测器带宽影响较大，扩张状态观测器的带宽较大能够较好的实现干扰估计，但如果扩张状态观测器的带宽变得过大，那么对噪声就会较为敏感，从而降低观测器的控制性能，因此，本发明中，扩张状态观测器的带宽在所述扩张观测器的性能及所述扩张观测器对噪声的容纳程度之间取一个平衡值。

具体地，所述扩张状态观测器的带宽是所述自抗扰控制器带宽的2至10倍，所述自抗扰控制器带宽不小于采样频率的5至10倍。

增加反馈控制可以使控制结果更加符合期望结果，本发明给出的一种基于自抗扰控制策略的上肢外骨骼系统协同随动控制方法还包括驱动系统控制器，在本发明中驱动系统控制器采用PID控制方法对驱动系统进行控制，驱动系统为驱动上肢外骨骼系统运动的系统，在PID控制方法中，设置比例单元P、积分单元I和微分单元D，且设置K_p、K_i和K_d实现上肢外骨骼系统的实际位置与上肢外骨骼系统的期望位置一致，通过PID控制方法获取输出驱动系统的电流：

其中，U为输出到驱动系统的电流，e是上肢外骨骼系统的期望位置和上肢外骨骼系统实际位置之间的差值，t为时间，K_p、K_i、K_d分别是比例单元、积分单元和微分单元的增益；

上肢外骨骼系统的期望位置和上肢外骨骼系统实际位置之间的差值的计算公式为：

e(t)＝q(t)-q_d(t)

其中，q_d是上肢外骨骼系统期望关节角度，q为上肢外骨骼系统实际关节角度；

获取驱动系统的输出力矩T，驱动上肢外骨骼系统运动，动力学方程可表示为：

其中，M，C，G分别是上肢外骨骼系统的惯性矩阵，向心力矩阵，重力矩阵；

由于本发明用于上肢外骨骼系统，一般上肢外骨骼系统包括第一机械臂、第二机械臂、肩关节、肘关节和握把，第一机械臂设在肩关节和肘关节之间，第二机械臂设在肘关节和握把间，操作手握住握把，握把上设置人机交互点。

因此驱动系统的输出力矩T＝[T_s，T_e]∈R^2×1，T_s和T_e分别是肩关节的输出力矩和肘关节的输出力矩。

本发明中，采用液压作为驱动力，通过设置第一液压杆和第二液压杆，分别驱动肩关节和肘关节转动，通过向第一液压杆和第二液压杆输出驱动系统的电流U，控制第一液压杆和第二液压杆运动。

在工业机器人中，通常将机器人与环境之间的作用力建模为一个阻抗模型，包括质量、弹簧、阻尼三个部分还有采用非线性模型来描述环境的，但非线性模型复杂度较高，不利于控制器的设计。因此本发明将人当作外部环境，上肢外骨骼系统与人之间的相互作用力等同于工业机器人中机器人与环境之间的相互作用力。上肢外骨骼系统与人的相互作用力可以表示为惯性、阻尼与刚度的线性组合。而阻抗控制不直接控制机械臂末端与环境接触力，通过分析“上肢外骨骼系统与人的动态关系”，将力控制和位置控制综合起来考虑，实现力控制和位置控制。同时在面对上肢外骨骼系统模型非线性的特点和液压系统的干扰的特性，使用自抗扰控制策略将消除，使上肢外骨骼系统有更好的跟踪效果，在步骤S1中，建立上肢外骨骼系统与人之间的阻抗模型，将上肢外骨骼系统与人的相互作用力表示为惯性、阻尼与刚度的线性组合：

其中，F_hm为多维力传感器检测到的人机相互作用力，e为上肢外骨骼系统与人之间的位置误差，M为惯性系数，B为阻尼系数，K为刚度系数。

本发明设置阻抗控制模型，不直接控制第一机械臂和第二机械臂与环境接触力，提高了控制的柔顺性。

实际的上肢外骨骼系统非常复杂，需要近似考虑。同时常规的建模会使上肢外骨骼系统中的第一机械臂和第二机械臂柔性不足，这里考虑增加上肢外骨骼系统的柔性特性，所以采用弹簧建模。

在此只考虑人机接口的柔性特征，采用弹簧模型对其建模，人机接口动力学表达式如下：

F_hm＝K(p_h-p_m)

其中，K为刚度系数，p_h＝[p_hx,p_hy,p_hz]^T∈R^3×1操作者手部的位置，即上肢外骨骼系统期望位置，p_m＝[p_mx,p_my,p_mz]^T∈R^3×1为外上肢外骨骼系统的末端实际位置，F_hm＝[F_x,F_y,T_z]^T∈R^3×1为多维力传感器检测到的人机相互作用力。由此可计算出期望的位置如下式：

p_h＝p_m+F_hm/K

通过上肢外骨骼系统的运动学方程建立位置坐标与关节角度的关系，表达式如下：

p_h＝[l_s sin(q_ds)+l_e sin(q_ds+q_de),-l_s cos(q_ds)-l_e cos(q_ds+q_de),0]^T

其中，q_ds为肩关节的期望关节角度，q_de为肘关节的期望关节角度，l_s为第一机械臂的长度，l_e为第二机械臂的长度。

计算出肘关节的期望关节角度q_de和肩关节的期望关节角度q_ds。

上肢外骨骼系统是一个多输入多输出(Multi Input Multi Output，MIMO)系统，因此上肢外骨骼系统表达式可表示为：

其中，T＝[T_s,T_e]^T∈R^2×1是肩关节和肘关节的控制力矩，

是肩关节和肘关节的角速度，T_d＝[T_ds,T_de]^T∈R^2×1是肩关节和肘关节的外部干扰，M、C和G是惯性系数、向心力矢量和重力矢量；

其中，

当惯性矩阵恒为正，可以得到以下方程：

其中，τ＝M^-1(q)T，

f＝[f_s,f_e]^T是系统的总干扰，包括系统误差，外部干扰和耦合等不确定因素。经过解耦后，上肢外骨骼系统构成的多输入多输出系统可以分解为两个通道的单输入单输出系统，使得自抗扰控制器分别控制肩关节和肘关节，上式可表示为：

肩关节状态观测器可表示为：

其中，β₁₁，β₁₂，β₁₃是肩关节的观测器增益。肘关节的状态观测器可表示为：

其中，β₂₁，β₂₂，β₂₃是肘关节观测器的增益。设观测器的极点在-ω₁，-ω₂，观测器的极点可根据多项式进行配置：

其中，ω₁，ω₂是肩关节扩张状态观测器的带宽和肘关节扩张状态观测器的带宽，观测器增益可以表示为：

β₁₁＝3ω₁，

β₂₁＝3ω₁，

按照选择扩张状态观测器带宽的准则：选择扩张状态观测器带宽应足够高，满足系统设计要求；

肩关节和肘关节反馈控制器可表达为：

其中，e₁，e₂分别是肩关节和肘关节观测器误差。k₁₁，k₁₂是肩关节控制器的增益，k₂₁，k₂₂是肘关节控制器的增益。控制器的增益为

k₁₂＝2ω₁，

k₂₂＝2ω₂。

自抗扰控制策略可表达如下：

其中，

是上肢外骨骼系统估计的干扰，

自抗扰控制器在上肢外骨骼系统中的应用如图4所示，有两个独立的扩张观测器估计肩关节和肘关节的干扰。z₁，z₂，z₃是肩关节扩展状态观测器的输出，z₄，z₅，z₆是肘关节扩展状态观测器的输出；q_ds和q_de是肩关节的期望关节角度和肘关节的期望关节角度；q_s和q_e是肩关节的实际关节角度和肘关节的实际关节角度；U_os和U_oe是肩关节反馈控制器和肘关节反馈控制器的输出；U_s和U_e是肩关节和肘关节自抗扰控制器的输出。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (5)

1.一种基于自抗扰控制策略的上肢外骨骼系统协同随动控制方法，其特征在于，包括：

步骤S1、在上肢外骨骼系统的人机交互点安装多维力传感器，检测操作者对多维力传感器的人机相互作用力，将上肢外骨骼系统与人的相互作用力表示为惯性、阻尼与刚度的线性组合：

其中，F_hm为多维力传感器检测到的人机相互作用力，e是上肢外骨骼系统的期望位置和上肢外骨骼系统实际位置之间的差值，M为惯性系数，B为阻尼系数，K为刚度系数；

构成上肢外骨骼系统的阻抗控制模型，对阻抗控制模型建模，考虑上肢外骨骼系统中人机接口的柔性特征，采用弹簧模型进行阻抗控制模型建模，人机接口动力学表达式如下：

F_hm＝K(p_h-p_m)

其中，K为刚度系数，p_h为操作者手部的位置，即上肢外骨骼系统期望位置，p_m为上肢外骨骼系统的末端位置，F_hm为多维力传感器检测到的人机相互作用力；

步骤S2、通过阻抗控制模型获取操作手末端的期望位置，由得到的期望位置计算出上肢外骨骼系统中肩关节的期望关节角度和肘关节的期望关节角度；

步骤S3、通过自抗扰控制器估计上肢外骨骼系统的状态和干扰，将估计的干扰作为上肢外骨骼系统的负反馈，通过自抗扰控制器将干扰消除；

所述自抗扰控制器包括跟踪微分器、扩张状态观测器、线性状态误差补偿和扰动补偿，获取操作者对多维力传感器的人机相互作用力，通过多维力传感器得到给定输入信号，通过跟踪微分器对给定输入信号的微分进行提取，确定适合上肢外骨骼系统中给定输入信号的过渡过程，使得上肢外骨骼系统跟踪过渡过程对应的输出信号；所述扩张状态观测器将上肢外骨骼系统的自身不确定性、外部及环境因素的干扰，转化成含有未知扰动的积分串联环节，将估计的干扰作为上肢外骨骼系统的负反馈，通过线性状态误差补偿和扰动补偿进行实时补充，消除干扰；

通过跟踪过渡过程对应的输出信号，在噪声较大的情况下，将给定输入信号v输入到跟踪微分器，得到平滑的输入信号：

其中，v₁，v₂是输入信号的跟踪信号；k₁，k₂为跟踪信号系数；

上肢外骨骼系统设有二阶系统：

其中，x₁、x₂是上肢外骨骼系统的状态变量，y是上肢外骨骼系统的输出，u为上肢外骨骼系统的控制量，f(t)表示上肢外骨骼系统的不确定因素，b为控制量系数；

针对上肢外骨骼系统设计二阶线性的扩张状态观测器的形式如下；

其中，Z₁，Z₂是上肢外骨骼系统内部状态变量的实时估计值，Z₃是上肢外骨骼系统不确定因素的实时估计值，参数β₁、β₂、β₃是扩张状态观测器的系数，起到误差校正作用，b₀为控制量系数；

采用误差反馈线性组合方式也能取得良好的控制效果，为了避免对给定输入信号进行微分且使闭环传递函数成为不包括零点的二阶传递函数，状态误差反馈律可采用PD参数形式：

其中，e＝v₁-z₁为扩张状态观测器的观测误差，k_p、k_d是控制器增益，自抗扰控制是通过扩张状态观测器的观测量对控制信号进行扰动补偿，控制表达式为：

其中，b₀为控制量系数，u₀上肢外骨骼系统的初值，u为上肢外骨骼系统的控制量，z₃是上肢外骨骼系统不确定因素的实时估计值。

2.根据权利要求1所述的一种基于自抗扰控制策略的上肢外骨骼系统协同随动控制方法，其特征在于：所述步骤S2中，上肢外骨骼系统包括第一机械臂、第二机械臂、肩关节、肘关节和握把，第一机械臂一端与肩关节固定，另一端与和肘关节可转动的连接，第二机械臂一端与肘关节固定，另一端与握把固定，人机交互点设在握把上，操作者手部握住握把，通过上肢外骨骼系统的运动学方程建立上肢外骨骼系统期望位置与肩关节角度和肘关节角度的关系：

p_h＝[l_ssin(q_ds)+l_esin(q_ds+q_de),-l_scos(q_ds)-l_ecos(q_ds+q_de),0]^T

其中，p_h为操作者手部的位置，即上肢外骨骼系统期望位置，q_ds为肩关节的期望关节角度，q_de为肘关节的期望关节角度，l_s为第一机械臂的长度，l_e为第二机械臂的长度。

3.根据权利要求1所述的基于自抗扰控制策略的上肢外骨骼系统协同随动控制方法，其特征在于：还包括驱动系统控制器，所述驱动系统控制器采用PID控制方法对驱动系统进行控制，驱动系统为驱动上肢外骨骼系统运动的系统，在PID控制方法中，设置比例单元P、积分单元I和微分单元D，并设置K_p、K_i和K_d实现上肢外骨骼系统的实际位置与上肢外骨骼系统的期望位置一致，通过PID控制方法获取输出驱动系统的电流：

其中，U为输出到驱动系统的电流，e是上肢外骨骼系统的期望位置和上肢外骨骼系统实际位置之间的差值，t为时间，K_p、K_i、K_d分别是比例单元、积分单元和微分单元的增益；

上肢外骨骼系统的期望位置和上肢外骨骼系统实际位置之间的差值的计算公式为：

e(t)＝q(t)-q_d(t)

其中，q_d是上肢外骨骼系统期望关节角度，q为上肢外骨骼系统实际关节角度；

获取驱动系统的输出力矩T，驱动上肢外骨骼系统运动，动力学方程可表示为：

其中，T_s是肩关节的输出力矩，T_e是肘关节的输出力矩，M，C，G分别是上肢外骨骼系统的惯性矩阵，向心力矩阵，重力矩阵。

4.根据权利要求1所述的基于自抗扰控制策略的上肢外骨骼系统协同随动控制方法，其特征在于：所述上肢外骨骼系统是一个多输入多输出系统，因此上肢外骨骼系统表达式可表示为：

其中，T_s是肩关节的输出力矩，T_e是肘关节的输出力矩，

是角速度，

肩关节的角速度，

肘关节的角速度，T_d是外部干扰，T_ds是肩关节的外部干扰，T_de是肘关节的外部干扰，M、C和G是惯性系数、向心力矢量和重力矢量；

其中，

当惯性矩阵恒为正，可以得到以下方程：

其中，τ＝M^-1(q)T，

f是系统的总干扰，包括系统误差，外部干扰和耦合等不确定因素，f_s是肩关节的的总干扰，f_e是肘关节的的总干扰，λ为所有误差总和，ΔC为向心力矢量的误差量，ΔG为重力矢量的误差量，经过解耦后，上肢外骨骼系统构成的多输入多输出系统可以分解为两个通道的单输入单输出系统，使得自抗扰控制器分别控制肩关节和肘关节，上式可表示为：

肩关节状态扩张状态观测器可表示为：

其中，z₁，z₂，z₃是肩关节扩展状态观测器的输出，β₁₁，β₁₂，β₁₃是肩关节的扩张状态观测器增益；

肘关节的扩张状态观测器可表示为：

其中，z₄，z₅，z₆是肘关节扩展状态观测器的输出，β₂₁，β₂₂，β₂₃是肘关节扩张状态观测器的增益；

设扩张状态观测器的极点在-ω₁，-ω₂，扩张状态观测器的极点可根据多项式进行配置：

其中，ω₁，ω₂是肩关节扩张状态观测器的带宽和肘关节扩张状态观测器的带宽，扩张状态观测器增益可以表示为：

β₁₁＝3ω₁，

β₂₁＝3ω₁，

肩关节的反馈控制器和肘关节的反馈控制器可表达为：

其中，e₁，e₂分别是肩关节的扩张状态观测误差和肘关节的扩张状态观测误差，k₁₁，k₁₂是肩关节的反馈控制器的增益，k₂₁，k₂₂是肘关节的反馈控制器的增益，的反馈控制器的增益为

k₁₂＝2ω₁，

k₂₂＝2ω₂；

自抗扰控制策略可表达如下：

其中，

是上肢外骨骼系统估计的干扰，

5.根据权利要求1所述的基于自抗扰控制策略的上肢外骨骼系统协同随动控制方法，其特征在于：扩张状态观测器的带宽在所述扩张观测器的性能及所述扩张观测器对噪声的容纳程度之间取一个平衡值。

Images