CN110202574A - 基于环境刚度估计的机器人自适应混合阻抗/导纳控制方法 - Google Patents

Abstract

本发明公开了机器人领域一种基于环境刚度估计的机器人自适应混合阻抗/导纳控制方法，通过建立由环境刚度估计和自适应率组成的自适应混合阻抗/导纳控制方法，能克服阻抗控制针对环境刚度变化时阻抗性能下降的缺点，具有应用范围广、鲁棒性强等优点，能够确保机器人与环境交互时期望的柔顺性、本发明可用于机器人系统的主动柔顺控制。

Description

基于环境刚度估计的机器人自适应混合阻抗/导纳控制方法

技术领域

本发明涉及机器人主动柔顺控制方法，尤其是机器人阻抗控制，具体为一种刚性机器人的自适应混合阻抗/导纳控制方法。

背景技术

Neville Hogan于上世纪80年代提出了“阻抗控制”的概念，其核心是调整机器人的机械阻抗以得到期望的阻抗特性，目标是建立了被控对象位置和所受外力之间的动态关系，为解决自由空间中的位置控制和接触空间中的柔顺控制提供了统一的框架。

作为一种主控柔顺控制的实现方法，阻抗控制的目标是实现和调整机器人运动与接触外力之间的特定动力学关系，通常称为阻抗关系。通过设计期望的阻抗关系，机器人在进行接触性作业时具有柔顺交互功能，在搬运、磨削、装配、辅助康复、辅助手术等领域具有重要的应用价值。

严格上区分，阻抗控制就其实现方式来说可分为阻抗控制和导纳控制。在交互过程中分别应用阻抗控制和导纳控制的系统具有相反的稳定性和性能表现：阻抗控制下的机器人系统与高刚度环境具有稳定的动态交互，但由于摩擦和其他模型误差，控制器在自由空间中具有较差的位置跟踪精度；导纳控制对于非接触任务中提供较好的位置精度，但在与高刚度环境的动态交互期间系统可能会不稳定。

不同于单独改善阻抗控制器或导纳控制器性能的思路，Christian Ott基于切换系统的概念，提出了一种统一的阻抗控制和导纳控制控制策略。控制器结合了阻抗和导纳控制的优点，通过在阻抗和导纳控制之间快速切换实现与较广范围环境交互的优异性能表现。Christian Ott在文章中指出切换信号的占空比决定了阻抗和导纳控制对混合系统总响应的贡献，通过设置合适的占空比可以使混合系统在不同环境下都有较优的性能表现，混合控制器具有提供最佳性能的潜力。为了建立针对于环境刚度的占空比自适应律，Francesco Cavenago提出了一种基于前馈神经网络的自适应策略，利用遗传算法作为神经网络的训练优化方法，并针对两自由度机器人验证了算法的有效性。然而，该策略的复杂性可能引起实际应用中的问题，且其对其他混合阻抗和导纳控制器的可扩展性有限。

发明内容

本发明的目的是提出一种控制算法能克服阻抗控制和导纳控制针对环境刚度变化时阻抗性能下降的缺点，具有应用范围广、鲁棒性强等优点，能够确保机器人与环境交互时期望的柔顺性。

本实现上述目的，本发明采用如下技术方案：

一种基于环境刚度估计的机器人自适应混合阻抗/导纳控制方法，包括以下步骤：

步骤1，建立机器人控制的混合系统；

式中τ_hyb为机器人关节空间的混合控制输出，τ_imp为机器人关节空间的阻抗控制输出，τ_adm为机器人关节空间的导纳控制输出，为雅可比矩阵，F_imp为机器人操作空间下的阻抗控制器输出，F_adm为机器人操作空间下的位置控制器输出，t₀为初始时刻，t为控制过程中的时间变量，δ为阻抗/导纳控制信号的切换周期，n_s∈[0,1]为阻抗/导纳控制信号切换的占空比，k为一个整数，取值为0,1,...,N，其中N为全部期望的切换周期数；

步骤2，基于递推最小二乘法估计环境刚度；

步骤3，建立混合系统阻抗性能评价函数，获得与接触环境刚度对应的机器人性能最优的阻抗/导纳切换的占空比，并建立占空比对于接触环境刚度的自适应率f(k_e)；

式中α_i为取自n_s取值集合中的实数，A_i为环境刚度取值区间，m表示A_i的个数，k_e为环境刚度，为A_i的指示函数，取值为1(当k_e∈A_i)或0(当)；

步骤4，将步骤3选择的最优占空比带入步骤1中的机器人控制的混合系统。

进一步的，所述步骤1中，首先在机器人操作空间，针对被控对象分别设计阻抗和导纳控制器；

机器人动力学模型的操作空间形式为：

其中x为描述机器人末端运动的实际位置坐标向量，Λ(x)为操作空间内机器人的惯性矩阵，为操作空间内机器人的科里奥利/离心矩阵，F_g为操作空间内机器人的等效重力向量，F_τ为操作空间内机器人的驱动力向量，F_ext为操作空间内机器人受到的外力向量；为x的二阶导数；为x的一阶导数；

设计阻抗控制器为：

式中F_imp为机器人操作空间下的阻抗控制器输出，为机器人操作空间阻抗控制期望惯性的正定对角矩阵的逆矩阵，K_d为机器人操作空间阻抗控制的阻尼的正定对角矩阵，D_d为机器人操作空间阻抗控制的刚度的正定对角矩阵，x_d为机器人末端的期望位置，为机器人末端位置x距x_d的位置偏差；为x_d的一阶导数；为x_d的二阶导数；为的一阶导数；

得到机器人末端受力和末端位置误差之间的期望动力学：

设计导纳控制器为：

式中F_adm为机器人操作空间下的位置控制器输出，M_d为机器人操作空间阻抗控制期望惯性的正定对角矩阵，K_p和K_v为正定对角矩阵，分别表示机器人位置控制器的比例和微分系数，x_i为导纳控制器的输出，为机器人末端位置x_i距x_d的位置偏差；为的一阶导数；为的二阶导数。

得到机器人操作空间下的位置控制器为：

通过在阻抗控制和导纳控制之间来回连续切换，获得机器人控制的混合系统。

进一步的，所述步骤2中，针对机器人与环境接触力的离散时域表达：

式中下标k表示瞬时时刻k，为回归变量向量，θ_k＝k_ek为系统参数向量，n_k代表建模误差和环境噪声，x_k为机器人末端执行器在t＝kT采样时刻的位置，T为采样周期。

定义力估计误差为：

式中F_k为接触力，为估计接触力，为估计环境参数。

利用递归最小二乘法得到环境参数的更新方程:

式中L_k+1为k+1时刻的自适应增益系数，P_k为k时刻的参数估计协方差矩阵，λ∈(0,1]为遗忘因子，其取值为：

其中λ_max和λ_min分别为遗忘因子的最大和最小值，γ为调节混合系统误差和响应速度的设计参数。

进一步的，所述步骤3中，基于混合系统响应的代价函数建立其阻抗性能评价函数：

式中T_sim为总测试时间，x_ref为理想的机器人阻抗控制响应轨迹，其满足如下方程

式中，K_e为环境刚度向量，为x_ref的二阶导数，为x_ref的一阶导数；

通过测试得到对应于不同环境刚度的混合系统切换最优占空比，并利用阶跃函数来表示最优占空比于环境刚度之间的对应关系，建立占空比的自适应律f(k_e)。

进一步的，为获得机器人动力学模型的操作空间形式，首先建立串联机器人关节坐标表示的动力学模型：

其中q＝(q₁,q₂)^T为关节转角，M(q)为惯性矩阵，为科里奥利/离心矩阵，g(q)为重力矩向量，τ和τ_ext分别为控制力矩和外力矩向量；矩阵M(q)、和g(q)分别为

其中

h＝-m₂l₁l_c2sinq₂，

g₁(q)＝g[(m₁l_c1+m₂l₁)cosq₁+m₂l_c2cos(q₁+q₂)]，g₂(q)＝gm₂l_c2cos(q₁+q₂)，m₁和m₂分别表示各连杆质量，l₁和l₂分别表示各连杆长度，l_c1和l_c2分别表示连杆质心与前一关节之间的距离，I₁和I₂分别表示各连杆关于质心的转动惯量，g为重力加速度；

系统动力学方程的操作空间形式改写为：

有益效果：与现有技术相比，本发明提供的基于环境刚度估计的机器人自适应混合阻抗/导纳控制方法能克服阻抗控制针对环境刚度变化时阻抗性能下降的缺点，具有应用范围广、鲁棒性强等优点，能够确保机器人与环境交互时期望的柔顺性、本发明可用于机器人系统的主动柔顺控制。

附图说明

图1是自适应混合阻抗/导纳控制实现的结构示意图；

图2是两连杆刚性机器人交互任务示意图；

图3是对应于不同环境刚度的最优占空比值图；

图4是机器人末端期望位置和对应的理想阻抗轨迹曲线；

图5是接触环境刚度估计曲线；

图6是针对时变环境系统响应误差曲线。

具体实施方式

下面结合附图对本发明进一步说明：

如图1所示的一种基于环境刚度估计的自适应混合阻抗/导纳控制方法，包括以下步骤：

步骤1，建立机器人控制的混合系统。

首先建立两连杆串联机器人关节坐标表示的动力学模型：

其中q＝(q₁,q₂)^T为关节转角，M(q)为惯性矩阵，为科里奥利/离心矩阵，g(q)为重力矩向量，τ和τ_ext分别为控制力矩和外力矩向量。矩阵M(q)、和g(q)分别为

其中 h＝-m₂l₁l_c2sinq₂，g₁(q)＝g[(m₁l_c1+m₂l₁)cosq₁+m₂l_c2cos(q₁+q₂)]，g₂(q)＝gm₂l_c2cos(q₁+q₂)，m₁和m₂分别表示各连杆质量，l₁和l₂分别表示各连杆长度，l_c1和l_c2分别表示连杆质心与前一关节之间的距离，I₁和I₂分别表示各连杆关于质心的转动惯量，g为重力加速度。

系统动力学方程的操作空间形式可改写为：

式中Λ(x)和分别为笛卡尔坐标下的惯性矩阵和科里奥利/离心矩阵，具有如下形式

Λ(x)＝J(q)^-TM(q)J(q)^-1 (3)

其中q＝f^-1(x)且 为机器人正向动力学函数，为其雅可比矩阵，F_g＝J(q)^-Tg(q)，F_ext＝J(q)^-Tτ_ext，F_τ＝J(q)^-Tτ。

作为本发明的进一步改进，所述步骤1中设计阻抗控制器为：

关节控制力矩为：

τ_imp＝J(q)^TF_imp

同时设计导纳控制器为：

关节控制力矩为：

τ_adm＝J(q)^TF_adm (7)

然后，通过在阻抗控制和导纳控制之间来回连续切换，设计混合阻抗/导纳控制系统为：

式中τ_imp为机器人关节空间的阻抗控制输出，τ_adm为机器人关节空间的导纳控制输出，为雅可比矩阵，F_imp为机器人操作空间下的阻抗控制器输出，F_adm为机器人操作空间下的位置控制器输出，t₀为初始时刻，t为控制过程中的时间变量，δ为阻抗/导纳控制信号的切换周期，n_s∈[0,1]为阻抗/导纳控制信号切换的占空比，k为一个整数，取值为0,1,...,N，其中N为全部期望的切换周期数。

步骤2，基于递推最小二乘法估计环境刚度。

针对机器人与环境接触力的离散时域表达：

式中下标k表示瞬时时刻k，为回归变量向量，θ_k＝k_ek为系统参数向量，n_k代表建模误差和环境噪声，x_k为机器人末端执行器在t＝kT采样时刻的位置，T为采样周期。

定义力估计误差为：

式中F_k为接触力，为估计接触力，为估计环境参数。

利用递归最小二乘法得到环境参数的更新方程:

式中L_k+1为k+1时刻的自适应增益系数，P_k为k时刻的参数估计协方差矩阵，λ∈(0,1]为遗忘因子，其取值为：

其中λ_max和λ_min分别为遗忘因子的最大和最小值，γ为调节混合系统误差和响应速度的设计参数。

利用递归最小二乘法建立环境刚度估计算法，分别设置λ_max、λ_min和γ的值为0.99，0.75和1。对于时变刚度环境，设置机器人末端期望轨迹如图4中x_d所示，其估计结果如图5所示。

步骤3，建立混合系统阻抗性能评价函数，获得与接触环境刚度对应的机器人性能最优的阻抗/导纳切换的占空比，并建立占空比对于接触环境刚度的自适应率f(k_e)；其中，基于混合系统响应的代价函数建立其阻抗性能评价函数：

式中T_sim为总测试时间，x_ref为理想的机器人阻抗控制响应轨迹，其满足如下方程

式中，K_e为环境刚度向量，为x_ref的二阶导数，为x_ref的一阶导数；

通过测试得到一系列对应于不同环境刚度的混合系统切换最优占空比，并利用阶跃函数来表示最优占空比于环境刚度之间的对应关系，建立占空比的自适应律：

式中α_i为取自n_s取值集合中的实数，A_i为环境刚度取值区间，m表示A_i的个数，k_e为环境刚度，为A_i的指示函数，取值为1(当k_e∈A_i)或0(当)；

δ＝0.02s。

为了阐述混合系统的性能表现，在本实施方式中，在MATLAB的Simulink中搭建仿真模型，考虑关节内实际存在的摩擦力矩，建立虚拟机器人动力学模型：

式中τ_f为关节的摩擦力矩，关节i中的摩擦力矩为：

其中为第i个关节角度的导数，c_v和τ_c分别为粘性摩擦系数和库伦摩擦力。

设置机器人虚拟样机参数为m₁＝1.0kg，m₂＝1.0kg，l₁＝0.5m，l₂＝0.5m，l_c1＝0.25m，l_c2＝0.25m，c_v＝0.05Nms/rad，τ_c＝0.05Nm，控制器设计使用的估计参数为控制器的参数设置为 机器人末端期望轨迹x_d是从(0.5,0)m到(0.8,0)m的周期阶跃信号，进行不同环境刚度下的系统响应仿真。在仿真开始时，假设末端执行器已经与环境发生接触，即x＝x_d＝(0.5,0)m，如图2所示。

分别设置不同环境刚度进行仿真，得到对应于不同环境刚度的最优占空比值，得到如图3所示的占空比值，其中横轴为环境刚度取值k_e，纵轴为占空比值n_s。图3中分散的数值用f(k_e)函数表示。

步骤4，将步骤3选择的最优占空比带入步骤1中的机器人控制的混合系统。从而获得本发明需要获得的对机器人自适应混合控制的最佳性能。

为了验证本发明提供的自适应混合控制方法相对于现有技术未采用自适应混合控制手段的方法具有控制性能优势，采用以下对比试验进行验证：

利用递归最小二乘法建立环境刚度估计算法，分别设置λ_max、λ_min和γ的值为0.99，0.75和1。对于时变刚度环境，设置机器人末端期望轨迹如图4中x_d所示，其估计结果如图5所示。

分别测试系统在导纳控制、阻抗控制、具有固定占空比(n_s＝0.5)的混合控制和自适应混合控制下的性能，其阻抗响应误差对比如图6所示，在整个仿真期间，自适应混合系统的响应误差始终能够保持较低的水平，表明自适应混合控制对于各个环境刚度都具有最佳性能，达到了满意的控制效果。

本发明不局限于上述实施例,在本公开的技术方案的基础上,本领域的技术人员根据所公开的技术内容,不需要创造性的劳动就可以对其中的一些技术特征作出一些替换和变形,这些替换和变形均在本发明的保护范围内。

Claims (5)

1.一种基于环境刚度估计的机器人自适应混合阻抗/导纳控制方法，其特征在于，包括以下步骤：

步骤1，建立机器人控制的混合系统；

式中τ_hyb为机器人关节空间的混合控制输出，τ_imp为机器人关节空间的阻抗控制输出，τ_adm为机器人关节空间的导纳控制输出，为雅可比矩阵，F_imp为机器人操作空间下的阻抗控制器输出，F_adm为机器人操作空间下的位置控制器输出，t₀为初始时刻，t为控制过程中的时间变量，δ为阻抗/导纳控制信号的切换周期，n_s∈[0,1]为阻抗/导纳控制信号切换的占空比，k为一个整数，取值为0,1,...,N，其中N为全部期望的切换周期数；

步骤2，基于递推最小二乘法估计环境刚度；

步骤3，建立混合系统阻抗性能评价函数，获得与接触环境刚度对应的机器人性能最优的阻抗/导纳切换的占空比，并建立占空比对于接触环境刚度的自适应率f(k_e)；

式中α_i为取自n_s取值集合中的实数，A_i为环境刚度取值区间，m表示A_i的个数，k_e为环境刚度，为A_i的指示函数，取值为1(当k_e∈A_i)或0(当)；

步骤4，将步骤3选择的最优占空比带入步骤1中的机器人控制的混合系统。

2.根据权利要求1所述的基于环境刚度估计的机器人自适应混合阻抗/导纳控制方法，其特征在于，所述步骤1中，首先在机器人操作空间，针对被控对象分别设计阻抗和导纳控制器；

机器人动力学模型的操作空间形式为：

其中x为描述机器人末端运动的实际位置坐标向量，Λ(x)为操作空间内机器人的惯性矩阵，为操作空间内机器人的科里奥利/离心矩阵，F_g为操作空间内机器人的等效重力向量，F_τ为操作空间内机器人的驱动力向量，F_ext为操作空间内机器人受到的外力向量；为x的二阶导数；为x的一阶导数；

设计阻抗控制器为：

式中F_imp为机器人操作空间下的阻抗控制器输出，为机器人操作空间阻抗控制期望惯性的正定对角矩阵的逆矩阵，K_d为机器人操作空间阻抗控制的阻尼的正定对角矩阵，D_d为机器人操作空间阻抗控制的刚度的正定对角矩阵，x_d为机器人末端的期望位置，为机器人末端位置x距x_d的位置偏差；为x_d的一阶导数；为x_d的二阶导数；为的一阶导数；

得到机器人末端受力和末端位置误差之间的期望动力学：

设计导纳控制器为：

式中F_adm为机器人操作空间下的位置控制器输出，M_d为机器人操作空间阻抗控制期望惯性的正定对角矩阵，K_p和K_v为正定对角矩阵，分别表示机器人位置控制器的比例和微分系数，x_i为导纳控制器的输出，为机器人末端位置x_i距x_d的位置偏差；为的一阶导数；为的二阶导数；

得到机器人操作空间下的位置控制器为：

通过在阻抗控制和导纳控制之间来回连续切换，获得机器人控制的混合系统。

3.根据权利要求1所述的基于环境刚度估计的机器人自适应混合阻抗/导纳控制方法，其特征在于，所述步骤2中，针对机器人与环境接触力的离散时域表达：

式中下标k表示瞬时时刻k，为回归变量向量，θ_k＝k_ek为系统参数向量，n_k代表建模误差和环境噪声，x_k为机器人末端执行器在t＝kT采样时刻的位置，T为采样周期；

定义力估计误差为：

式中F_k为接触力，为估计接触力，为估计环境参数；

利用递归最小二乘法得到环境参数的更新方程:

式中L_k+1为k+1时刻的自适应增益系数，P_k为k时刻的参数估计协方差矩阵，λ∈(0,1]为遗忘因子，其取值为：

其中λ_max和λ_min分别为遗忘因子的最大和最小值，γ为调节混合系统误差和响应速度的设计参数。

4.根据权利要求1所述的基于环境刚度估计的机器人自适应混合阻抗/导纳控制方法，其特征在于，所述步骤3中，基于混合系统响应的代价函数建立其阻抗性能评价函数：

式中T_sim为总测试时间，x_ref为理想的机器人阻抗控制响应轨迹，其满足如下方程

式中，K_e为环境刚度向量，为x_ref的二阶导数，为x_ref的一阶导数；

通过测试得到对应于不同环境刚度的混合系统切换最优占空比，并利用阶跃函数来表示最优占空比于环境刚度之间的对应关系，建立占空比的自适应律f(k_e)。

5.根据权利要求2所述的基于环境刚度估计的机器人自适应混合阻抗/导纳控制方法，其特征在于，为获得机器人动力学模型的操作空间形式，首先建立串联机器人关节坐标表示的动力学模型：

其中q＝(q₁,q₂)^T为关节转角，M(q)为惯性矩阵，为科里奥利/离心矩阵，g(q)为重力矩向量，τ和τ_ext分别为控制力矩和外力矩向量；矩阵M(q)、和g(q)分别为

其中

h＝-m₂l₁l_c2sinq₂，g₁(q)＝g[(m₁l_c1+m₂l₁)cosq₁+m₂l_c2cos(q₁+q₂)]，g₂(q)＝gm₂l_c2cos(q₁+q₂)，m₁和m₂分别表示各连杆质量，l₁和l₂分别表示各连杆长度，l_c1和l_c2分别表示连杆质心与前一关节之间的距离，I₁和I₂分别表示各连杆关于质心的转动惯量，g为重力加速度；

系统动力学方程的操作空间形式改写为：