一种基于MRAC与PID的冗余自由度连续臂控制方法

技术领域

本发明涉及自动控制技术领域，尤其涉及一种基于MRAC与PID的冗余自由度连续臂控制方法。

背景技术

传统刚性臂因其结构与材料特性，具有较高的精确性与可操控性。但随着机器人的应用环境日益复杂，由工业拓展到医疗、勘探、服务等各行业，传统刚性机械臂暴露出来的人机交互安全性差、自由度低、对复杂环境适应性差等特性，局限了刚性机械臂的使用场景。而软体臂使用软体材料或柔性材料加工制作得到，由于自身材料的特性，具备传统刚性机器人无法具备的柔性、更高的灵活性、无损的操作性和人机交互安全性，且在受到一定程度的拉伸或挤压的情况下，仍能较保持较好的工作性能，从而使其更合适应用于服务型机器人。基于这一现象，现有技术中提出融合了刚性臂与软体臂特点的刚柔耦合连续臂。

对于机器人的建模与控制算法而言，传统的刚性机器人在工作空间中任意点的位姿可以由连杆长度和关节角确定，建立比较精确的运动学与动力学模型，并在此基础上实现快速、准确、稳定的控制，研究大多采用常规PID控制方法。但仅使用常规PID控制方法，不适用于具有大量自由度的刚柔耦合连续臂，并且因为柔性材料弹性变形大，具有较强的非线性，存在建模精度低、振动抑制困难的问题，且末端负载以及软体材料参数的时变性，对控制系统的鲁棒性也提出较高要求。另外，连续臂具有强耦合性，所以整体难以得到良好的控制结果。上述原因导致传统控制算法很难适应非线性系统的运动控制要求，因此在连续体机器人的控制方法研究上仍然需要进一步改进。

发明内容

针对现有技术的不足，本发明提出一种基于MRAC与PID的冗余自由度连续臂控制方法，能够在满足连续臂实时性与运动快速性的情况下，有效降低连续臂各段之间的耦合效应与运动到目标位置后的振动情况，并且可以降低对模型的精确度要求，且无需建立完整的多自由度连续臂的动力学模型，具有实时性良好、响应时间快、运动精度高、鲁棒性良好的特点。

具体技术方案如下：

一种基于MRAC与PID的冗余自由度连续臂控制方法，包括以下步骤：

S1：根据气动连续臂的结构和关节参数，对气动连续臂进行运动学、静力学与动力学建模；所述气动连续臂分为多段，每一段均有两个自由度，且每一段包括使用万向节结构的刚性骨架和气压控制的气动肌肉；

S2：将气动连续臂的关节角度置零，初始化气动肌肉各气道与控制参数，并设置目标角度；

S3：根据所述目标角度进行关节角度轨迹规划，并通过角度解耦，得到与各PID控制一一对应的目标角度；

S4：根据气动连续臂实时的关节角度数据，计算当前角度与目标角度的差值，结合气动连续臂的负载，进行控制方案优化调整，并控制气动连续臂到达目标位置；所述控制方案优化调整具体为：若当前角度与目标角度的差值小于最小阈值，则对气动连续臂进行PID控制；若当前角度与目标角度的差值大于最大阈值，或当前角度与目标角度的差值在最小阈值与最大阈值之间但负载大于单段气动连续臂的自重，则对气动连续臂进行MRAC与PID联合控制；若当前角度与目标角度的差值在最小阈值与最大阈值之间，且负载小于单段气动连续臂的自重，则基于静力学模型得到的静力学初值对气动连续臂进行PID控制。

进一步地，所述S1中，采用分段分析法建立气动连续臂的运动学模型，具体操作如下：

将所述气动连续臂等效为n个相连的长方体结构，远离物体的气动连续臂为首端，靠近物体的气动连续臂为末端；每个长方体结构的中心点为万向节中心，令该中心点为O点，万向节中心能进行两个方向的旋转，旋转角度分别为α

建立第i段气动连续臂的运动学模型，表达式如下：

式中，T

从而得到气动连续臂末端靠近物体的平面中点位置l表达式如下：

将气动连续臂末端的位置精度作为主要考量，设计目标函数如下：

f(r，p，Θ)＝λf(r，p)+(1-k)g(Θ)

λ＝0.5e

f(r，p)＝min(ω

θ

Θ＝[θ

式中，k为对气动连续臂末端的位置精度的考量系数；r为末端姿态误差，p为末端位置误差，Θ为气动连续臂整臂的关节角度向量，θ为单节的关节角度向量，θ

建立适应度函数表达式如下：

在已知目标点的情况下，通过遗传算法得到逆解，进而得到各关节的期望角度α

进一步地，所述S1中，建立静力学模型，通过目标角度得到各气动肌肉所需的气压值，具体操作如下：

对万向节中心取矩，表达式如下：

式中，M

由于在静态平衡状态下时，有

∑M

∑M

从而解得充气侧的两个气压值P

进一步地，所述S1中，建立动力学模型，通过建立各关节角加速度与气动肌肉的气压值的关系，具体操作如下：

将动力学模型分为刚性骨架的动力学模型和对气动肌肉的等效力矩模型；

所述刚性骨架的动力学模型的表达式如下：

Θ＝[θ

式中，τ为关节所需力矩，τ

所述对气动肌肉的等效力矩模型的表达式如下：

P＝Fp(τ,Θ)

式中，P为气动肌肉的气压向量；

联立刚性骨架的动力学模型和对气动肌肉的等效力矩模型，得到各关节角加速度与气动肌肉的气压值的关系，并可整理为状态空间方程的形式：

y

式中，x

进一步地，所述S3具体包括以下子步骤：

(3.1)进行关节角度轨迹规划，其表达式如下：

θ(t)＝a

式中，a

已知气动连续臂起点零时刻的关节空间参数为θ

θ(0)＝θ

θ(t

从而得到待定系数a

a

a

(3.2)进行角度解耦，使气动肌肉与万向节中心错开45°分布，表达式如下：

其中，α

进一步地，所述S4中，对气动连续臂进行MRAC与PID联合控制，具体操作如下：

对远离物体的首端或首两端气动连续臂进行基于动力学模型的角加速度控制，使首端或首两端气动连续臂到达目标位置时，角速度与角加速度为0，这一过程通过动力学前馈PD控制模块、基于动力学模型的MRAC控制模块实现；下部其余节的气动连续臂中的气动肌肉平缓增加气压至静力学初值，接近目标位置附近再进行PID控制；所述静力学初值为通过静力学模型建立的关节角度与各气动肌肉的气压值的关系得到的气动肌肉的气压值；

基于动力学模型的MRAC控制模块，采用基于状态变量的Lyapunov-MRAC控制，使用自适应律对状态反馈控制器F和前馈控制器K进行在线调节；选取Lyapunov函数为：

式中，Q、P

得到用于调节被控对象输出与参考模型输出之间的误差，使被控对象的动态特性与参考模型一致的自适应律，表达式如下：

式中，

在基于动力学模型的MRAC控制模块的上层加入动力学前馈PD控制模块，动力学前馈的方法考虑了连续臂动力学中惯性项、离心力和哥氏力项、重力项对系统的影响，并通过关节力矩等效模块P＝Fp(τ,Θ)输出得到气压值，并将其作为基于动力学模型的MRAC控制模块的参考输入；动力学前馈PD控制律表达式如下：

P＝Fp(τ,Θ)

Θ＝[θ

θ

式中，τ为关节所需力矩，K

进一步地，所述S4中，当前角度与目标角度的差值的最小阈值为5°，最大阈值为15°。

一种刚柔耦合连续臂，用于实现所述的基于MRAC与PID的冗余自由度连续臂控制方法，包括：多段式的气动连续臂、固定支架、控制器、气阀装置；多段式的气动连续臂的一端固连在固定支架上，并在两者连接处安装有压力传感器，用于测量气动连续臂的负载变化，该段气动连续臂为首端连续臂；多段式的气动连续臂的另一端固连有真空吸盘，该段气动连续臂为末端连续臂；

每一段气动连续臂均包括气动肌肉和刚性骨架，刚性骨架采用万向节结构，沿气动连续臂的轴线周向均匀分布，使每段气动连续臂均能实现两个方向的转动，即每段气动连续臂具有两个自由度；在刚性骨架的转轴处安装霍尔传感器，用于测量关节角度；气动肌肉采用柔性材料制作，布置在刚性骨架的外周；

控制器用于在确定目标位置后，计算各气动肌肉所需的气压值；气阀装置用于根据控制器的计算结果实现对气压的精确控制，从而使气动连续臂做弯曲运动并到达目标位置。

本发明的有益效果是：

(1)本发明引入MRAC控制来实现对刚柔耦合连续臂的运动控制，可以降低对模型的精确度要求，克服了传统的控制方法对于模型不确定性的不足，不需要精确的数学模型即可完成控制。

(2)本发明设计了基于动力学模型与静力学模型的MRAC与PID的融合控制方法，根据不同情况合理选择控制方案，降低了运算量，提高了实时性，兼具灵活性和鲁棒性。

附图说明

图1是本发明所用的刚柔耦合连续臂结构示意图。

图2是本发明基于MRAC与PID的冗余自由度连续臂控制方法的流程图。

图3是本发明气动连续臂的结构和受力分析简图，其中，(a)为气动连续臂的等效模型图，(b)为气动连续臂中单段的受力分析简图。

图4是本发明根据气动连续臂状态进行控制模式选择的流程框图。

图5是本发明的动力学前馈PD控制模块、基于动力学模型的MRAC控制模块结构图。

图中，气动肌肉1、刚性骨架2、霍尔传感器3、压力传感器4、真空吸盘5、控制器6、气阀装置7。

具体实施方式

下面根据附图和优选实施例详细描述本发明，本发明的目的和效果将变得更加明白，以下结合附图和实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

如图1所示，本发明设计了一种刚柔耦合连续臂，包括：四段式的气动连续臂、固定支架、控制器6、气阀装置7。四段式的气动连续臂的一端固连在固定支架上，并在两者连接处安装有压力传感器4，用于测量气动连续臂的负载变化，该段气动连续臂为首端连续臂；另一端固连有真空吸盘5，该段气动连续臂为末端连续臂。

每一段气动连续臂均包括气动肌肉1和刚性骨架2，刚性骨架2采用万向节结构，沿气动连续臂的轴线周向均匀分布，使每段气动连续臂均可实现两个方向的转动，即每段气动连续臂具有两个自由度，整个气动连续臂则具有8个自由度。在刚性骨架2的转轴处安装霍尔传感器3，用于测量关节角度。气动肌肉1布置在刚性骨架2的外周，在本实施例中，为增大关节的弯曲效果，气动肌肉1与霍尔传感器3错开45°分布。

控制器6用于在确定目标位置后，计算各气动肌肉1所需的气压值，气阀装置7用于根据控制器6的计算结果实现对气压的精确控制，从而使四段式的气动连续臂做弯曲运动并到达目标位置。

因气动连续臂采用气动肌肉1驱动，而气动肌肉1采用硅胶材料制作，受到材料弹性以及编制网套摩擦等因素的影响，其真实物理模型很复杂。若采用无模型控制，如PID等，因其未考虑连续臂动力学中惯性项、离心力和哥氏力项、重力项对系统的影响，各关节运动耦合效应严重，控制效果差；若只采用简化的模型，则因建模困难导致简化模型与真实模型差异很大，从而影响其控制效果。另外，若对整臂进行MRAC控制则需要建立8自由度的气动连续臂的动力学模型，模型将十分复杂，在实际性能表征实验中发现，气动连续臂的首端两节是振动的主要来源，因此本发明提出了一种基于MRAC与PID的冗余自由度连续臂控制方法，如图2所示，具体步骤如下：

S1：根据气动连续臂的具体结构和关节参数，对气动连续臂进行运动学、静力学与动力学建模，具体包括如下子步骤：

(1.1)根据所应用的气动连续臂各个关节的类型、具体尺寸以及它们之间的连接方式，通过分段分析法建立气动连续臂的运动学模型。

由于气动连续臂为四段式的，得到其等效模型图如图3中的(a)所示，每一段气动连续臂均可等效为四个长方体，其中平面0靠近固定支架，平面4靠近真空吸盘5。如图3的(b)所示，对于单段气动连续臂，其中心点为万向节中心，令该中心点为O点，O点映射到长方体的上表面为G

建立第i段气动连续臂的运动学模型，表达式如下：

式中，T

因各段气动连续臂的结构相同，易知末端，即平面4中点的位置，该位置可通过基坐标系下的位置矢量l表示为：

在正运动学方程中，共有12个方程，其中只有6个方程是独立的。由于存在冗余自由度，机械臂存在无穷多组运动学逆解。为了获得唯一的逆解，需要附加约束条件，通过附加合适的约束，可以获得符合约束的最优解。

对于超冗余机械臂逆运动学求解问题，气动连续臂末端的位置精度应为首要考量，需保证其到达预期位置与姿态时的误差最小，因此目标函数建立为：

f(r，p)＝min(ω

式中，r为末端姿态误差，p为末端位置误差，ω

同时，适应度函数的建立采用“运动柔顺”原则，即气动连续臂从当前位姿点到期望的目标点之间，关节角度变化量最小，同时，从能量与运动平稳性出发，应采用“多动下臂，少动上臂”的原则，基于“运动柔顺”和“多动下臂，少动上臂”原则，建立目标函数为：

θ

Θ＝[θ

式中，Ε为气动连续臂整臂的关节角度向量，θ为单节的关节角度向量，θ

结合上述两种考量，且气动连续臂末端的位置精度为主要考量，设计目标函数为：

f(r，p，Θ)＝λf(r，p)+(1-k)g(Θ)

λ＝0.5e

式中，k为对气动连续臂末端的位置精度的考量系数。

通过系数λ，使在求解初期，末端位姿误差较大时，同时考虑气动连续臂末端的位置精度与“运动柔顺”原则，而随着末端位姿误差减小，λ趋向于1，着重考虑末端位姿误差。

综上，建立适应度函数为：

在已知目标点的情况下，通过遗传算法即可得逆解，得到各关节的期望角度α

(1.2)建立静力学模型，建立该模型的目的是通过目标角度得到各气动肌肉1所需的气压值。

对万向节中心取矩，重力忽略不计，表达式如下：

式中，M

充气状态下的各气动肌肉1的肌肉收缩力为Fs＝Fs(P,L)，其中P为肌肉气压值，L为肌肉长度，肌肉长度根据运动学模型得到的各关节的期望角度α

由于在静态平衡状态下时，有

∑M

∑M

从而解得充气侧的两个气压值P

(1.3)建立动力学模型，建立该模型的目的是建立各关节角加速度与气动肌肉1的气压值的关系。

前面提到，气动连续臂的首端两节是振动的主要来源，且对整臂进行动力学建模将十分复杂，因此利用牛顿欧拉法进行首端两节连续臂的动力学建模，将末端两节连续臂对首端两节连续臂的影响视为接触外力，并为简化运算，将其分为刚性骨架2的动力学建模和对气动肌肉1的等效力矩建模。

整理动力学方程，可得带接触的刚性骨架2的动力学模型的表达式如下：

式中，τ为关节所需力矩，σ

根据刚性骨架2的动力学建模的表达式计算得到关节所需力矩τ与时间的表达式，再通过静力学模型建立过程中的对万向节中心取矩的方法，得到τ与气动肌肉1的气压P的关系，从而完成对气动肌肉1的等效力矩建模：

σ(P,θ)＝σ

整理方程可得：

P＝Fp(τ,Θ)

式中，P为气动肌肉1的气压向量。

联立刚性骨架的动力学模型和对气动肌肉的等效力矩模型，即可得各关节角加速度与气动肌肉1的气压值和接触力τ

式中，x

S2：在上位机进行刚柔耦合连续臂的关节角度置零、气动肌肉1各气道与控制参数初始化的操作，再设置目标位置，并通过运动学逆解由气动连续臂末端位姿得到各关节目标角度。

S3：根据目标角度进行关节角度轨迹规划，并通过角度解耦，得到与各PID控制所一一对应的解耦角度。具体包括以下子步骤：

(3.1)对于连续臂的振动抑制问题，规划关节角加速度是直接有效的方法，本发明采用基于五次多项式插值的关节角度轨迹规划，该五次多项式具有如下形式：

θ(t)＝a

式中，a

若已知气动连续臂起点零时刻的关节空间参数为θ

θ(0)＝θ

θ(t

从而得到待定系数a

a

a

(3.2)从结构出发，为增大关节的弯曲效果，气动肌肉1与霍尔传感器3错开45°分布，为使得一对气动肌肉1的控制有一一对应的角度，需进行角度解耦，即解算得到由霍尔传感器3实际测得角度绕z轴旋转45°后的角度，表达式如下：

其中，α

S4：根据压力传感器4读取负载重量信息，并使用霍尔传感器3实时采集关节角度数据，通过当前角度与目标角度的差值，进行控制方案优化调整，并控制气动连续臂到达目标位置。具体通过如下操作实现：

根据对气动连续臂进行性能实验可知，在气动连续臂摆动角度越大，且摆动速度越快时，上两节气动连续臂(即从首端连续臂开始的两段连续臂)因惯性所产生的振动是整臂振动的根源。如在此情况下，仅采用PID控制则会造成气动连续臂各段之间运动耦合明显、振荡明显且持久。

采用如图4所示的控制策略，将目标位置与当前位置的距离与负载重量作为考量因素，进行控制模式选择。其中目标位置与当前位置的距离，根据当前角度与目标角度的差值进行划分，如下表1所示将差值小于5°的记为较近，差值在5°到15°之间的记为中等，差值大于15°的记为较远。将小于单关节自重的负载重量记为较小，大于单关节自重的记为较大。

表1

当目标位置与当前位置的距离较小时，惯性带来的振动效应并不明显，因此对整臂采用PID控制，降低运算量与控制复杂度，提高系统响应与运动实时性。

当目标位置与当前位置的距离中等且负载较小时，将通过静力学模型建立的关节角度与各气动肌肉1的气压值的关系得到的气动肌肉1的气压值作为静力学初值，随后由于模型误差与环境干扰等因素，气动连续臂并不能准确到达目标位置，但会在目标位置附近，到达目标位置后，再采用PID进行控制。

当目标位置与当前位置的距离较大时，或是目标位置与当前位置的距离中等但负载较大时，气动连续臂因惯性所产生的振动效应十分明显，且振动主要由上两节产生，因此通过对最上一节(即首端连续臂)或两节进行基于动力学模型的角加速度控制，使最上一节或两节的气动连续臂到达目标位置时，角速度与角加速度为0，这一过程采用模型参考自适应控制(Model Reference Adaptive Control，以下简称MRAC)；下部其余节的气动连续臂中的气动肌肉1平缓增加气压至静力学初值，接近目标位置附近再进行PID，可有效降低气动连续臂的运动耦合效应，对气动连续臂的运动振动进行有效抑制。

由以上控制策略可知，控制器6中包括以下控制子模块：单对气动肌肉1的PID控制模块、角度解耦模块、静力学初值计算模块、轨迹规划模块、动力学前馈PD控制模块、基于动力学模型的MRAC控制模块。单对气动肌肉1的PID控制模块用于对气动肌肉1的气压值进行PID控制，角度解耦模块用于实现步骤(3.2)中的角度解耦计算；静力学初值计算模块用于通过静力学模型建立的关节角度与各气动肌肉1的气压值的关系，得到气动肌肉1的气压值，并将其作为静力学初值；轨迹规划模块用于根据到达终点时关节角速度与角加速度为0的条件进行气动连续臂的轨迹规划。

基于动力学模型的MRAC控制模块，采用基于状态变量的Lyapunov-MRAC控制，使用自适应律对状态反馈控制器F和前馈控制器K进行在线调节，在理论上可实现参考模型与被控对象的完全匹配，使连续臂的运动响应过程与参考模型输出一致，达成期望的控制性能。

选取Lyapunov函数为：

式中，Q、P

经推导可得自适应律为：

式中，

如图5所示，MRAC算法的主要思想是在控制器(本实施例中采用动力学前馈PD)和被控对象组成的闭环回路外，再建立一个由参考模型和自适应机构组成的附加调节回路。基于动力学模型的MRAC控制模块对其性能指标的要求完全通过参考模型来表达，即参考模型的输出就是该模块的理想输出。当运行时，由于被控对象参数或特性变化导致被控对象输出不能跟踪参考模型的输出，此时被控对象输出与参考模型输出的误差由自适应律进行调节，目的是使被控对象的动态特性与参考模型一致。在基于动力学模型的MRAC控制模块的上层加入动力学前馈PD控制模块，动力学前馈的方法考虑了连续臂动力学中惯性项、离心力和哥氏力项、重力项对系统的影响，并通过关节力矩等效模块P＝Fp(τ,Θ)输出得到气压值，并将其作为基于动力学模型的MRAC控制模块的参考输入，通过将动力学前馈PD算法和MRAC算法这两种控制算法结合，可以有效降低对模型精度的要求，大大提高控制效果。

动力学前馈PD控制律表示为：

P＝Fp(τ,Θ)

式中，K

本发明通过针对四段式、8自由度刚柔耦合连续臂的运动特性，设计了一种基于MRAC与PID的冗余自由度连续臂控制方法，在满足连续臂实时性与运动快速性的情况下，可以有效降低连续臂各段之间的耦合效应，以及运动到目标位置后的振动情况。

本领域普通技术人员可以理解，以上所述仅为发明的优选实例而已，并不用于限制发明，尽管参照前述实例对发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实例记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在发明的精神和原则之内，所做的修改、等同替换等均应包含在发明的保护范围之内。