一种半柔性机械臂系统的控制方法与流程

本发明属于机器人控制领域，主要涉及一种半柔性机械臂系统的控制方法。

背景技术：

随着上世纪60年代第一台工业机器人问世以来，机器人的应用已逐渐渗透到航天、医疗、军事，甚至日常生活及娱乐教育等各个领域。起初制造机器人更多是把它作为一种自动化装置，服务于制造业。随着理论和技术的日益成熟，人们对机器人提出了越来越高的要求，越来越多的场合需要机器人或者机械臂为我们服务。

机械臂是机器人最主要的执行机构。在当下热门的无人车上加入机械臂可以协助或者帮助人类完成许多任务，比如自动加油、自动清障等。通常机械臂的工作空间在设计时可能无法满足复杂多变的环境，因此有必要给机械臂配备工具，以提高它的工作范围，进而满足实际需求。

机械臂的主体是刚性的，但附加的工具可能是带有柔性的连杆(有一定的柔性更符合实际情况)。当工具末端带有负载(如汽油枪、带清理的障碍物等)，柔性连杆末端在进行运动时会出现变形。在对机械臂进行控制时，比如轨迹跟踪，柔性变形会对运动学和动力学模型产生很大影响。简单地把工具的影响当成扰动不能很好地解决问题，而是需要把工具当成整个机器人系统的一部分。这样就扩展了本身的机械臂，组成了一个刚性主体、柔性附件的半柔性机械臂系统。由于工具是柔性的，所以需要应用柔性机械臂中柔性连杆或者柔性关节的动力学建模的相关方法。半柔性机械臂和柔性机械臂是都是一个刚柔耦合的非线性系统，系统的动力学特性和控制特性相互耦合。

柔性连杆实际上一个无穷多自由度的柔性系统，它的建模方法主要有假设模态法、有限元法、集总参数法等。但是这些方法的特点都是结合弹性力学，建立一系列的偏微分方程去描述弹性变形，然后按照拉格朗日或者牛顿-欧拉法的思路去建立柔性机械臂的动力学模型，然而这种建模和控制过程很复杂，对于半柔性机械臂来说，其主体是刚性的，仅针对其中一个柔性连杆采用如此复杂的建模和控制，是没有必要的，也不能满足实际需求。

技术实现要素：

有鉴于此，本发明提供了一种半柔性机械臂系统的控制方法，针对半柔性机械臂系统，把连杆柔性变形建模成随机干扰，利用递推牛顿-欧拉动力学算法建立半柔性机械臂的动力学模型，针对所建立的动力学模型采用双闭环运动控制方法进行运动控制，能够适应半柔性机械臂的控制需求。

为了达到上述目的，本发明的技术方案为：一种半柔性机械臂系统的控制方法，半柔性机械臂系统由刚性机械臂和附加的柔性连杆组成，刚性机械臂是由一系列的刚性连杆和关节构成，其中关节为转动关节或者移动关节；方法包括如下步骤：

步骤1、刚性机械臂共有n+1个刚性连杆，编号从A₀到A_n，相邻刚性连杆之间通过关节连接，共有n个关节，则关节编号从B₁到B_n；附加的柔性连杆为R，R与刚性连杆A_n通过关节B_n+1连接；

步骤2、刚性机械臂采用改进型的DH参数方法建立得到{0}，{1}，{2}，…，{n-1}，{n}共n+1个刚性连杆的固连坐标系，并计算得到n+1个刚性连杆的连杆参数；附加的柔性连杆为R的三个坐标轴分别规定为：采用改进型的DH参数方法定义关节B_n的轴线作为Z轴，然后把关节B_n+1到柔性连杆R的末端的连线所在直线作为X轴，再根据右手坐标系的原则确定Y轴；

步骤3、刚性机械臂按照原有的连杆参数规定不变；对于柔性连杆R，将柔性连杆R视作刚性连杆，进行连杆参数的计算，其中连杆参数中的柔性连杆R的长度为从关节B_n+1到柔性连杆R的末端的线段长度L_n+1，L_n+1为可变值，其他参数不变；

步骤4、根据连杆参数确定刚性连杆A₀到A_n以及柔性连杆R之间相邻连杆的坐标系变换公式；

步骤5、用递推牛顿-欧拉动力学算法建立半柔性机械臂系统的动力学方程，即建立关节力矩与关节角、关节速度、关节加速度之间的关系；

步骤6、在上述动力学方程的基础上，依据如下方法对半柔性机械臂系统进行控制：

step601、先假设半柔性机械臂系统中连杆均为刚性连杆，根据预先给出的笛卡尔空间中期望的运动轨迹规划出关节空间中的期望轨迹，用关节角度qd、速度和加速度表达；

step602、根据step601中计算得到的关节空间中的期望轨迹采用鲁棒自适应PD控制算法，计算得到关节力矩τ为

其中前两项为PD控制部分，e和分别表示关节角度和关节角速度的跟踪误差，K_p和K_v分别是PD控制中的位置和速度反馈对应的比例系数矩阵，为补偿动力学模型的自适应控制项，t_d表示补偿干扰的鲁棒控制项；

step603、将step602控制后的关节空间的实际轨迹映射到笛卡尔空间，将实际轨迹的笛卡尔空间映射值与笛卡尔空间中期望的运动轨迹进行误差计算，并依据计算得到的误差为规划出关节空间中的期望轨迹增加一个调整值，返回step602，并重复step602和step603，直至误差满足精度要求。

进一步地，步骤5中，关节力矩表示为τ，q＝[θ₁ … θ_n θ_n+1]^T表示前n个关节的角度θ₁～θ_n和第n+1个关节的角度测量值θ_n+1组成的关节向量，其中θ_n+1为柔性连杆R变形前测得的关节角度；表示前n个关节的角度θ₁～θ_n和虚拟关节角度组成的关节角向量，为柔性连杆变形后的虚拟关节角度；则半柔性机械臂系统的动力学方程具体如下：

其中，为关节力矩，M⁰为机械臂变形前相应的惯性矩阵、C⁰为机械臂变形前相应的离心力和哥氏力矩阵、G⁰为机械臂变形前相应的重力项，和分别为q的二阶和一阶导数，ω为扰动，ΔC为机械臂变形前相应的离心力和哥氏力矩阵变化量，ΔG为机械臂变形前后相应的重力项变化量，为关节角加速度向量变化量，为关节角向量速度变化量。

进一步地，step603中以笛卡尔空间的轨迹误差的平方和作为目标函数R，关节空间的期望节轨迹作为控制变量，求出目标函数R对控制变量的各个分量的负梯度方向分别为则调整值为：其中μ为设定的学习步长。

有益效果：

本发明提供的控制方法，针对半柔性机械臂系统，在合理假设的前提下，在连杆附加坐标系特殊规定的基础上建立半柔性机械臂的运动学模型，把连杆柔性变形建模成随机干扰，利用递推牛顿-欧拉动力学算法建立半柔性机械臂的动力学模型，针对所建立的动力学模型采用双闭环运动控制方法，其中以step602～step603作为外环，采用学习控制，通过期望的位置坐标，这是整个半柔性机械臂系统需要实现的跟踪目标，得到期望的关节轨迹；以step602作为内环，采用关节轨迹控制，通过外环的控制量即期望的关节轨迹，计算得到关节力矩。本发明方法针对主体为刚性，具有一个柔性连杆的半柔性机械臂，适应性强、效果好。

附图说明

图1是由刚性机械臂和柔性连杆组成的半柔性机械臂系统图，代表一种半柔性机械臂系统的通用结构；

图2是一种2自由度半柔性机械臂的系统简图。2自由度半柔性机械臂系统位于竖直平面内，其中刚性主体部分含有1个关节，基座连杆省略不画，因此共标出一个刚性连杆加上一个柔性连杆，共2个自由度；

图3是图2所示的2自由度半柔性机械臂的运动学模型示意图。其中标明了机械臂的参数；

图4是半柔性机械臂的运动控制系统框图；

图5是半柔性机械臂的运动控制流程图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

一种半柔性机械臂系统的控制方法，半柔性机械臂系统由刚性机械臂和附加的柔性连杆组成，刚性机械臂是由一系列的刚性连杆和转动关节或者移动关节构成；建模方法包括如下步骤：

步骤1、刚性机械臂共有n+1个刚性连杆，编号从A₀到A_n，相邻刚性连杆之间通过关节连接，共有n个关节，则关节编号从B₁到B_n；附加的柔性连杆为R，R与A_n通过关节B_n+1连接；

步骤2、刚性机械臂采用改进型的DH参数方法建立得到{0}，{1}，{2}，…，{n-1}，{n}共n+1个刚性连杆的固连坐标系，并计算得到n+1个刚性连杆的连杆参数；附加的柔性连杆为R的三个坐标轴分别规定为：采用改进型的DH参数方法定义关节B_n的轴线作为Z轴，然后把关节B_n+1到柔性连杆R的末端的连线所在直线作为X轴，再根据右手坐标系的原则确定Y轴，

步骤3、刚性机械臂按照原有的连杆参数规定不变；对于柔性连杆R，将柔性连杆R视作刚性连杆，进行连杆参数的计算，其中连杆参数中的柔性连杆R的长度为从关节B_n+1到柔性连杆R的末端的线段长度L_n+1，L_n+1为可变值，其他参数不变；

L_n+1＝l_n+1+Δl_n+1

步骤4、根据连杆参数确定刚性连杆A₀到A_n以及柔性连杆R之间相邻连杆的坐标系变换公式；

步骤5、用递推牛顿-欧拉动力学算法建立半柔性机械臂系统的动力学方程，即建立关节力矩与关节角、关节速度和关节加速度之间的关系；关节力矩表示为τ，q＝[θ₁ … θ_nθ_n+1]^T表示前n个关节的角度θ₁～θ_n和第n+1个关节的角度测量值θ_n+1组成的关节向量，其中θ_n+1为柔性连杆R变形前测得的关节角度；表示前n个关节的角度θ₁～θ_n和虚拟关节角度组成的关节角向量，为柔性连杆变形后的虚拟关节角度；则半柔性机械臂系统的动力学方程具体如下：

其中，τ为关节力矩，M⁰为机械臂变形前相应的惯性矩阵、C⁰为机械臂变形前相应的离心力和哥氏力矩阵、G⁰为机械臂变形前相应的重力项，和分别为q的二阶和一阶导数，ω为扰动，ΔM为机械臂变形前后相应的惯性矩阵变化量，ΔC为机械臂变形前后相应的离心力和哥氏力矩阵变化量，ΔG为机械臂变形前后相应的重力项变化量，为关节角向量变化量，为关节加速度向量变化量，为关节速度向量变化量。

步骤6、在上述动力学方程的基础上，依据如下方法对半柔性机械臂系统进行控制：

step601、先假设半柔性机械臂系统中连杆均为刚性连杆，根据预先给出的笛卡尔空间中期望的运动轨迹规划出关节空间中的期望轨迹，用关节角度q_d、速度和加速度表达；

step602、根据step601中计算得到的关节空间中的期望轨迹采用鲁棒自适应PD控制对半柔性机械臂系统实施控制，获得关节力矩τ，依据τ对半柔性机械臂系统进行控制；

用q_d和分别表示期望关节角和期望关节角速度。内环的关节空间控制器输出的控制量为关节力矩，采用鲁棒自适应PD控制算法，总表达式为

其中前两项为PD控制部分，e,分别表示关节角度和关节角速度的跟踪误差，K_p和K_v分别是PD控制中的位置和速度反馈对应的比例系数矩阵，为补偿动力学模型的自适应控制项，t_d表示补偿干扰的鲁棒控制项。

step603、将step602控制后的关节空间的实际轨迹映射到笛卡尔空间，将实际轨迹的笛卡尔空间映射值与笛卡尔空间中期望的运动轨迹进行误差计算，并依据计算得到的误差为规划出关节空间中的期望轨迹增加一个调整值，返回step602，直至误差满足精度要求。

步骤6中以笛卡尔空间的轨迹误差的平方和作为目标函数，关节空间的期望节轨迹作为控制变量，求出目标函数R对控制变量的各个分量的负梯度方向分别为则调整值为：其中μ为设定的学习步长。

实施例、

图1是由刚性机械臂和柔性连杆组成的半柔性机械臂系统图，代表一种通用的结构。

本发明提供的一种半柔性机械臂的运动学和动力学建模方法，具体实施步骤如下：

步骤1.连杆和关节编号的规定。如图2所示，2自由度半柔性机械臂共有两个刚性连杆，O₀基座定义为第0号连杆(图2中省略)，O₁到O₂的连线是第1号刚性连杆，O₁表示第1个关节，O₂表示第2个关节，是第2号连杆，是柔性的。这样图2所示的半柔性机械臂系统共有2个自由度。

步骤2.对连杆附加坐标系的规定。为了建立运动学模型，需要给出半柔性机械臂系统连杆附加坐标系的规定。如图3所示，刚性主体部分采用改进型的DH参数方法:和O₀Y₀分别为基座连杆的坐标系{0}的X轴和Y轴，Z₀轴为过O₀的直线，且正方向指向纸面外；和O₁Y₁分别为第1个连杆的固连坐标系{1}的X轴和Y轴，Z₁轴为过O₁的直线，且正方向指向纸面外；附加在末端的柔性连杆的坐标系{2}的坐标轴分别规定为：Z₂定义为O₂的直线，且正方向指向纸外；把关节原点O₂到末端B的连线所在直线规定为X轴，这样就类似于定义了一个虚拟的连杆；再根据右手坐标系的原则,即由X轴逆时针旋转90度得到Y轴。由于整个机械臂系统分布在竖直平面，且仅有两个自由度，因此Z轴在图3中被省略掉了。

步骤3.连杆参数的规定。刚性主体部分按照原有的连杆参数规定不变。如图3所示，连杆参数的规定如下：第1个刚性连杆的长度为l₁，质量为m₁，关节角度为θ₁；第2个柔性连杆，原长为l₂，定义虚拟连杆的长度为L₂，由于是柔性连杆，可认为连杆质量和负载质量相比可以忽略，这样质量几乎集中在末端，记为m₂，可测的关节角度为θ₂，虚拟关节角为无法直接测得。根据定义得到下面的表达式，

L₂＝l₂+Δl₂

其中，Δl₂和Δθ₂反映了柔性变形导致的偏差。

步骤4.运动学模型的建立。对连杆附加坐标系和连杆参数的规定考虑了柔性变形的影响，把带有柔性连杆的机械臂等效成了一种特殊的刚性机械臂。相邻连杆坐标系{0}和{1}、{1}和{2}的变换公式由下面两个式子给出：

这样根据连杆坐标系和相应的连杆参数，就可以按照一般刚性机械臂的正向运动学计算思路来确定一个坐标系{2}相对于坐标系{0}的变换矩阵。最后得到在笛卡尔空间中的机械臂末端位置，即负载质心的位置由下面的式子给出：

步骤5.动力学模型的建立。图4所示的2自由度半柔性机械臂系统处于竖直平面内。本发明采用递推牛顿-欧拉动力学算法建立半柔性机械臂的动力学模型，即建立关节力矩、关节角、关节速度和关节加速度之间的关系。

用下面的式子简写正余弦函数:

c₁＝cosθ₁,s₁＝sinθ₁,

记τ₁、τ₂分别为第1个关节和第2个关节的驱动力矩，g表示重力加速度。利用递推牛顿-欧拉动力学算法得到整个机械臂系统的解析形式的动力学方程如下：

其中，

M₂₁＝M₁₂

C₂₂＝0

G₂＝m₂L₂gc₁₂

步骤6.分离干扰。为了更好的体现随机干扰对机械臂系统的影响，下面把干扰分离出来。将式1带入动力学方程得到

相应的各系数的扰动变化量可以分离出来。惯性矩阵各个分量的变化量为

ΔM₂₁＝ΔM₁₂

离心力和哥氏力矩阵各分量的变化量为

ΔC₂₂＝0

重力项的变化量为

ΔG₁＝m₂g[Δl₂cos(θ₁+θ₂+Δθ₂)-l₂sin(θ₁+θ₂)sinΔθ₂]

ΔG₂＝ΔG₁

记q＝[θ₁ θ₂]^T，分别表示关节角向量、关节角速度向量和关节角加速度向量，则分离出柔性干扰后的机械臂动力学方程如下：

其中，M⁰,C⁰,D⁰是机械臂变形前相应的动力学参数，扰动表示为

分离干扰后的运动学方程为：

X＝X⁰+ΔX

Y＝Y⁰+ΔY

其中，

X⁰＝l₁cosθ₁+l₂cos(θ₁+θ₂)

Y⁰＝l₁sinθ₁+l₂sin(θ₁+θ₂)

ΔX＝Δl₂cos(θ₁+θ₂)cosΔθ₂-(l₂+Δl₂)sin(θ₁+θ₂)sinΔθ₂

ΔY＝Δl₂sin(θ₁+θ₂)cosΔθ₂+(l₂+Δl₂)cos(θ₁+θ₂)sinΔθ₂

根据前面建立的运动学和动力学模型设计控制系统。

如图4所示，本发明的运动控制系统的内环采用鲁棒自适应PD控制，实现关节角跟踪，由线性PD反馈项、补偿动力学模型的自适应控制项和补偿干扰的鲁棒控制项三部分组成。

本发明的运动控制系统的外环采用基于梯度下降法的开环迭代学习控制算法，具体的控制流程如图5所示。

外环控制的目标是跟踪笛卡尔空间的期望轨迹，定义笛卡尔空间的误差向量为：

E＝[ΔX ΔY]^T＝[X-X^* Y-Y^*]^T

则一次运动过程的累积误差平方和为

当机械臂的实际关节轨迹跟踪上期望轨迹时，由于柔性干扰的存在，机械臂末端的轨迹变化并没有跟踪上期望的轨迹。为了在关节空间中寻找到合适的期望轨迹q_d以减少R的值到足够小的，这里采用最优化方法中的梯度下降的思想，把目标函数R对控制变量——期望轨迹q_d的负梯度方向作为调整期望关节角的方向，通过数次迭代学习可以使笛卡尔空间中的期望轨迹达到一定的精度要求。

记第k次和k+1次的期望关节轨迹分别为q^*(k)和q^*(k+1)，Δq^*(k+1)为第k+1次相对于第k次期望关节轨迹的调整量，则

q^*(k+1)＝q^*(k)+Δq^*(k+1)

现在需要确定Δq^*(k+1)，先求出目标函数R对控制变量的各个分量的负梯度方向分别为：

这样就可以得到学习控制算法的表达式如下：

其中μ为学习步长。

综上，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。