CN109062032A - 一种基于近似动态逆的机器人pid变阻抗控制方法 - Google Patents

Abstract

本发明公开了机器人控制领域内的一种基于近似动态逆的机器人PID变阻抗控制方法，包括以下步骤：步骤1，建立5‑杆并联机器人的拉格朗日动态模型；步骤2，构建期望的二阶弹簧‑阻尼变阻抗动态模型；步骤3，设计参考轨迹，将变阻抗控制问题转化为参考轨迹的特殊跟踪问题；步骤4，基于近似动态逆，设计PID变阻抗控制；步骤5，根据控制仿真效果对控制参数进行调整；简单可靠，易于实现，克服机器人变阻抗控制的不足，以使变阻抗误差收敛到0，可确保变阻抗误差的收敛、变阻抗控制的稳定性和变阻抗动态的实现，从而确保机器人‑环境交互的柔顺性与可靠性，本发明可以用于机器人系统的阻抗控制。

Description

一种基于近似动态逆的机器人PID变阻抗控制方法

技术领域

本发明涉及一种机器人柔顺控制方法，特别涉及一种机器人PID变阻抗控制方法。

背景技术

阻抗控制是最重要的柔顺控制方法之一，其目的是在期望轨迹和交互力之间建立一个弹簧-阻尼动态关系。该控制可以为机器人进行接触性作业时提供柔顺交互，在机器人搬运、磨削、装配、辅助康复、辅助手术等领域具有重要的应用价值。为得到期望的阻抗动态，在机器人阻抗控制方面已经取得了很多重要的成果，但已有的阻抗控制存在如下不足：1)主要考虑常阻抗控制设计；2)不能保证阻抗误差的收敛。事实上，当机器人与环境进行接触时，由于环境的不同以及交互力的变化，环境的等效刚度、阻尼随着时间而变化。所以，在机器人阻抗控制的应用中，需要实时调整阻抗模型的参数以适应环境变化。而阻抗误差收敛到0或者0的一个小领域是至关重要的，它可以保证阻抗控制的稳定性和期望阻抗动态的实现。因此，如何为机器人设计阻抗误差收敛的变阻抗控制具有重要的理论价值和实际意义。

Kronander和Billard首次且唯一提出了变阻抗矩阵的约束条件以保证变阻抗控制的稳定性。但是，该变阻抗控制存在如下不足：一方面，该变阻抗控制只能适用于机器人模型完全可知的情况，而在机器人动力学模型中，系统的不确定性和干扰总是存在的；另一方面，该变阻抗矩阵的约束条件只能保证期望的阻抗动态模型当交互力为0时渐近收敛，但不能保证其指数收敛，也就不能保证期望的阻抗动态模型中的机器人状态有界。

发明内容

本发明的目的是提供一种基于近似动态逆的机器人PID变阻抗控制方法，简单可靠，易于实现，克服机器人变阻抗控制的不足，以使变阻抗误差收敛到0，可确保变阻抗误差的收敛、变阻抗控制的稳定性和变阻抗动态的实现，从而确保机器人-环境交互的柔顺性与可靠性。

为了实现上述目的，本发明提供了一种基于近似动态逆的机器人PID变阻抗控制方法，包括以下步骤：

步骤1，建立5-杆并联机器人的拉格朗日动态模型；

步骤2，构建期望的二阶弹簧-阻尼变阻抗动态模型；

步骤3，设计参考轨迹，将变阻抗控制问题转化为参考轨迹的特殊跟踪问题；

步骤4，基于近似动态逆，设计PID变阻抗控制；

步骤5，根据控制仿真效果对控制参数进行调整。

与现有技术相比，本发明的有益效果在于，通过设计基于近似动态逆的机器人变阻抗控制，提出变刚度和变阻尼矩阵的约束条件，保证期望的二阶弹簧-阻尼动态模型的稳定性；设计一参考轨迹，将阻抗控制设计问题转化为参考轨迹的特殊跟踪问题；基于近似动态逆设计PID变阻抗控制，使得参考轨迹跟踪误差及其一阶二阶导数收敛到零，进而保证变阻抗误差收敛到零且实现期望的变阻抗动态；简单可靠，易于实现，克服机器人变阻抗控制的不足，以使变阻抗误差收敛到0，可确保变阻抗误差的收敛、变阻抗控制的稳定性和变阻抗动态的实现，从而确保机器人-环境交互的柔顺性与可靠性，本发明可以用于机器人系统的阻抗控制。

作为本发明的进一步改进，所述步骤1中5-杆并联机器人的拉格朗日动态模型为式一：

其中：q＝[q₁ q₂]^T∈R²为机器人位置，q₁和q₂分别表示杆1和杆2的角度；为机器人的速度；为机器人的加速度；M(q)∈R^2×2表示机器人的惯性矩阵；表示机器人的哥氏力矩和向心力矩；G(q)∈R²表示重力矩；表示机器人的摩擦力；τ∈R²表示控制输入；τ_e＝J^T(q)f_e∈R²表示关节空间机器人-环境交互力，J(q)表示雅可比矩阵，f_e＝[f_e1 f_e2]^T∈R²表示末端P处的交互力；

矩阵和J(q)表示为：

其中：d₁₁(q)＝m₁l² _c1+m₃l² _c3+m₄l₁ ²+I₁+I₃，d₁₂(q)＝d₂₁(q)＝(m₃l₂l_c3+m₄l₁l_c4)cos(q₂-q₁)，d₂₂(q)＝m₂l² _c2+m₃l₂ ²+m₄l_c ² ₄+I₂+I₄，h＝-(m₃l₂l_c3+m₄l₁l_c4)sin(q₂-q₁)，l₁,l₂,l₃,l₄分别表示各关节长度，l_c1,l_c2,l_c3,l_c4表示各关节到对应质心的距离，I₁,I₂,I₄,I₅表示各关节惯性矩，m₁,m₂,m₃,m₄分别表示各关节质量；摩擦力表示为式二：

其中，γ_i∈R,i＝1,2...6为未知正常数，tanh表示双曲正切函数，用式(2)描述机器人摩擦力具有以下优点：

可表示黏性力；可表示静摩擦力；可表示库伦摩擦力；具有Stribeck效应。

作为本发明的进一步改进，所述步骤2中的二阶弹簧-阻尼变阻抗动态模型为式三：

其中：M_d，B_d(t)和K_d(t)分别表示期望的惯性矩阵，期望的变阻尼矩阵，和期望的变刚度矩阵，标记

根据线性时变系统理论，提出关于参数M_d，B_d(t)和K_d(t)的约束条件：i)M_d，B_d(t)和K_d(t)为有界的对角正定矩阵；ii)存在正数β₁使得存在正数β₂,β₃使得其中，

所述约束条件可以保证：i)在式三中，当交互力τ_e＝0时，以及收敛到0；ii)给定有界的交互力τ_e，由式二所得到的信号q，以及有界。

作为本发明的进一步改进，所述步骤3中，设计参考轨迹q_r∈R²为信号通过如下滤波器的输出式四：

那么，阻抗误差可以表示为式五：

其中，e₁＝q_r-q为参考轨迹跟踪误差，使得e₁，和收敛到0的问题将阻抗控制设计问题转化为设计控制τ使得参考轨迹跟踪误差e₁＝q_r-q及其一阶、二阶导数收敛到0，进而保证变阻抗误差收敛到0和变阻抗控制的稳定性。

作为本发明的进一步改进，所述步骤4中，设计PID变阻抗控制式六：

其中，ε为较小的正控制参数，对角正定矩阵K_I,K_P∈R^2×2为控制参数，使得参考轨迹跟踪误差e₁＝q_r-q及其一阶、二阶导数收敛到0。

作为本发明的进一步改进，通过MATLAB仿真后，如果控制效果不能满足要求，则返回步骤4继续调节控制参数，直到控制效果达到要求；如果控制效果达到要求，则控制设计结束。

附图说明

图1是本发明实施步骤流程图。

图2是5杆-并联机器人的结构图。

图3是机器人变阻抗误差变化图。

图4是机器人变阻抗控制输入图。

具体实施方式

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

如图1-4所示的一种基于近似动态逆的机器人PID变阻抗控制方法，包括以下步骤：步骤1，建立5-杆并联机器人的拉格朗日动态模型：

其中：q＝[q₁ q₂]^T∈R²为机器人位置，q₁和q₂分别表示杆1和杆2的角度；为机器人的速度；为机器人的加速度；M(q)∈R^2×2表示机器人的惯性矩阵；表示机器人的哥氏力矩和向心力矩；G(q)∈R²表示重力矩；表示机器人的摩擦力；τ∈R²表示控制输入；τ_e＝J^T(q)f_e∈R²表示关节空间机器人-环境交互力，J(q)表示雅可比矩阵，f_e＝[f_e1 f_e2]^T∈R²表示末端P处的交互力；

矩阵M(q)，和J(q)表示为：

其中：d₁₁(q)＝m₁l² _c1+m₃l² _c3+m₄l₁ ²+I₁+I₃，d₁₂(q)＝d₂₁(q)＝(m₃l₂l_c3+m₄l₁l_c4)cos(q₂-q₁)，h＝-(m₃l₂l_c3+m₄l₁l_c4)sin(q₂-q₁)，l₁,l₂,l₃,l₄分别表示各关节长度，l_c1,l_c2,l_c3,l_c4表示各关节到对应质心的距离，I₁,I₂,I₄,I₅表示各关节惯性矩，m₁,m₂,m₃,m₄分别表示各关节质量；摩擦力表示为式二：

其中，γ_i∈R,i＝1,2...6为未知正常数，tanh表示双曲正切函数，用式(2)描述机器人摩擦力具有以下优点：

可表示黏性力；可表示静摩擦力；可表示库伦摩擦力；具有Stribeck效应。

步骤2，构建期望的二阶弹簧-阻尼变阻抗动态模型：

其中：M_d，B_d(t)和K_d(t)分别表示期望的惯性矩阵，期望的变阻尼矩阵，和期望的变刚度矩阵，标记

根据线性时变系统理论，提出关于参数M_d，B_d(t)和K_d(t)的约束条件：i)M_d，B_d(t)和K_d(t)为有界的对角正定矩阵；ii)存在正数β₁使得存在正数β₂,β₃使得其中，

所述约束条件可以保证：i)在式三中，当交互力τ_e＝0时，q_d-q，以及收敛到0；ii)给定有界的交互力τ_e，由式二所得到的信号q，以及有界。

步骤3，设计参考轨迹，将变阻抗控制问题转化为参考轨迹的特殊跟踪问题：

设计参考轨迹q_r∈R²为信号通过如下滤波器的输出式四：

那么，阻抗误差可以表示为式五：

其中，e₁＝q_r-q为参考轨迹跟踪误差，使得e₁，和收敛到0的问题将阻抗控制设计问题转化为设计控制τ使得参考轨迹跟踪误差e₁＝q_r-q及其一阶、二阶导数收敛到0，进而保证变阻抗误差收敛到0和变阻抗控制的稳定性。

步骤4，基于近似动态逆，设计PID变阻抗控制：

设计PID变阻抗控制式六：

其中，ε为较小的正控制参数，对角正定矩阵K_I,K_P∈R^2×2为控制参数，使得参考轨迹跟踪误差e₁＝q_r-q及其一阶、二阶导数收敛到0。

步骤5，根据控制仿真效果对控制参数进行调整：

通过MATLAB仿真后，如果控制效果不能满足要求，则返回步骤4继续调节控制参数，直到控制效果达到要求；如果控制效果达到要求，则控制设计结束。

设计时，步骤1：该并联机器人的拉格朗日模型为：

其中：q＝[q₁ q₂]^T∈R²为机器人位置，q₁和q₂分别表示杆1和杆2的角度；为机器人的速度；为机器人的加速度；M(q)∈R^2×2表示机器人的惯性矩阵；表示机器人的哥氏力矩和向心力矩；G(q)∈R²表示重力矩；表示机器人的摩擦力；τ∈R²表示控制输入；τ_e＝J^T(q)f_e∈R²表示关节空间机器人-环境交互力，J(q)表示雅可比矩阵，f_e＝[f_e1 f_e2]^T∈R²表示末端P处的交互力。矩阵M(q)，和J(q)表示为

其中：d₁₁(q)＝m₁l² _c1+m₃l² _c3+m₄l₁ ²+I₁+I₃，d₁₂(q)＝d₂₁(q)＝(m₃l₂l_c3+m₄l₁l_c4)cos(q₂-q₁)，h＝-(m₃l₂l_c3+m₄l₁l_c4)sin(q₂-q₁)，各个关节长度分别为：l₁＝0.2m,l₂＝l₄＝0.4m,l₃＝0.5m，各关节到对应关节质心距离分别为：l_c1＝0.1m,l_c2＝l_c4＝0.2m,l_c3＝0.25m，各关节惯性矩为：I₁＝0.1kg.m²,I₂＝I₄＝0.2kg.m²,I₃＝0.3kg.m²，各关节质量分别为：m₁＝0.5kg,m₂＝m₄＝1kg,m₃＝1.5kg。

摩擦力表示为：

其中：γ₁＝0.2,γ₂＝2,γ₃＝10,γ₄＝20,γ₅＝0.3。

步骤2：设期望的阻抗动态模型为：

其中：M_d＝I,B_d(t)＝10I,K_d(t)＝(15+sint)I，交互力f_e＝[f_e1,f_e2]^T为

步骤3：将信号通过如下滤波器

可得参考轨迹q_r∈R²。那么，阻抗误差可以表示为：

其中，e₁＝q_r-q为参考轨迹跟踪误差。所以，设计变阻抗控制使得e₁，和收敛到0，可保证变阻抗误差e收敛到0。因此，变阻抗控制设计问题转化为设计控制使得e₁，和收敛到0的问题。

步骤4：基于近似动态逆设计如下变阻抗控制：

其中，ε为较小的正控制参数，对角正定矩阵K_I,K_P∈R^2×2为控制参数。

步骤5：通过仿真和调参发现,当控制参数为k_I＝2I,k_P＝0.5I,ε＝0.1，机器人参考轨迹跟踪误差及其一阶二阶导数收敛到0点的一个小邻域，阻抗误差e收敛到0点一个小邻域，达到了满意的控制效果。

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

Claims (6)

1.一种基于近似动态逆的机器人PID变阻抗控制方法，其特征在于，包括以下步骤：

步骤1，建立5-杆并联机器人的拉格朗日动态模型；

步骤2，构建期望的二阶弹簧-阻尼变阻抗动态模型；

步骤3，设计参考轨迹，将变阻抗控制问题转化为参考轨迹的特殊跟踪问题；

步骤4，基于近似动态逆，设计PID变阻抗控制；

步骤5，根据控制仿真效果对控制参数进行调整。

2.根据权利要求1所述的一种基于近似动态逆的机器人PID变阻抗控制方法，其特征在于，所述步骤1中5-杆并联机器人的拉格朗日动态模型为式一：

其中：q＝[q₁ q₂]^T∈R²为机器人位置，q₁和q₂分别表示杆1和杆2的角度；为机器人的速度；为机器人的加速度；M(q)∈R^2×2表示机器人的惯性矩阵；表示机器人的哥氏力矩和向心力矩；G(q)∈R²表示重力矩；表示机器人的摩擦力；τ∈R²表示控制输入；τ_e＝J^T(q)f_e∈R²表示关节空间机器人-环境交互力，J(q)表示雅可比矩阵，f_e＝[f_e1 f_e2]^T∈R²表示末端P处的交互力；

矩阵M(q)，和J(q)表示为：

其中：d₁₁(q)＝m₁l² _c1+m₃l² _c3+m₄l₁ ²+I₁+I₃，d₁₂(q)＝d₂₁(q)＝(m₃l₂l_c3+m₄l₁l_c4)cos(q₂-q₁)，d₂₂(q)＝m₂l² _c2+m₃l₂ ²+m₄l_c ² ₄+I₂+I₄，h＝-(m₃l₂l_c3+m₄l₁l_c4)sin(q₂-q₁)，l₁,l₂,l₃,l₄分别表示各关节长度，l_c1,l_c2,l_c3,l_c4表示各关节到对应质心的距离，I₁,I₂,I₄,I₅表示各关节惯性矩，m₁,m₂,m₃,m₄分别表示各关节质量；摩擦力表示为式二：

其中，γ_i∈R,i＝1,2...6为未知正常数，tanh表示双曲正切函数，用式(2)描述机器人摩擦力具有以下优点：

可表示黏性力；可表示静摩擦力；可表示库伦摩擦力；具有Stribeck效应。

3.根据权利要求2所述的一种基于近似动态逆的机器人PID变阻抗控制方法，其特征在于，所述步骤2中的二阶弹簧-阻尼变阻抗动态模型为式三：

其中：M_d，B_d(t)和K_d(t)分别表示期望的惯性矩阵，期望的变阻尼矩阵，和期望的变刚度矩阵，标记

根据线性时变系统理论，提出关于参数M_d，B_d(t)和K_d(t)的约束条件：i)M_d，B_d(t)和K_d(t)为有界的对角正定矩阵；ii)存在正数β₁使得iii)存在正数β₂,β₃使得其中，

所述约束条件可以保证：i)在式三中，当交互力τ_e＝0时，q_d-q，以及收敛到0；ii)给定有界的交互力τ_e，由式二所得到的信号q，以及有界。

4.根据权利要求3所述的一种基于近似动态逆的机器人PID变阻抗控制方法，其特征在于，所述步骤3中，设计参考轨迹q_r∈R²为信号通过如下滤波器的输出式四：

那么，阻抗误差可以表示为式五：

其中，e₁＝q_r-q为参考轨迹跟踪误差，使得e₁，和收敛到0的问题将阻抗控制设计问题转化为设计控制τ使得参考轨迹跟踪误差e₁＝q_r-q及其一阶、二阶导数收敛到0，进而保证变阻抗误差收敛到0和变阻抗控制的稳定性。

5.根据权利要求4所述的一种基于近似动态逆的机器人PID变阻抗控制方法，其特征在于，所述步骤4中，设计PID变阻抗控制式六：

其中，ε为较小的正控制参数，对角正定矩阵K_I,K_P∈R^2×2为控制参数，使得参考轨迹跟踪误差e₁＝q_r-q及其一阶、二阶导数收敛到0。

6.根据权利要求1-5任意一项所述的一种基于近似动态逆的机器人PID变阻抗控制方法，其特征在于，通过MATLAB仿真后，如果控制效果不能满足要求，则返回步骤4继续调节控制参数，直到控制效果达到要求；如果控制效果达到要求，则控制设计结束。