CN106945040A - 一种用于单自由度柔性末端的机械臂控制算法 - Google Patents

Abstract

本发明公开了一种用于单自由度柔性末端的机械臂控制算法，该算法首先建立世界坐标系、接触力测量坐标系和运动坐标系，整个系统的柔顺度为机械臂的柔顺度、工具末端的柔顺度以及环境的柔顺度之和；通过系统柔顺度来建立起机械臂末端运动速度和接触力的关系，由所需控制的接触力f得到机械臂末端运动速度，通过逆运动学求解完成机械臂各个关节角的转动速度并控制机械臂运动，再通过PD控制器使接触力达到稳定值。本发明结合了传统的被动柔顺控制和主动柔顺控制的优点，从而使机械臂完成与环境之间的良好交互控制。

Description

一种用于单自由度柔性末端的机械臂控制算法

技术领域

本发明涉及一种用于单自由度柔性末端的机械臂控制算法，属于机械臂结构和控制技术领域。

背景技术

机械臂被广泛使用在机械制造、航空航天、医药、原子能等领域，对自动化生产起到了重大作用。机械臂在运动中可以分为自由运动和受力运动，自由运动一般应用在喷漆、焊接、码垛等工作中，在这样的环境中机械臂不直接和外界环境接触，所以在控制上只需要考虑路径规划问题，目前该方面的研究应用较为成熟。对于受力运动即机械臂在运动的过程中和环境发生交互作用，需要和外界进行接触来完成指定的功能，像打磨、钻孔、抛光等，最简单的莫过于抓取，在受力运动中，机械臂不但要进行精确的位置控制，而且需恰当地控制接触力大小，否则难以完成任务甚至损坏器物。这种能对外界力做出运动调整的控制方式称为柔顺控制，极大地拓展了机械臂的安全性与应用领域。

机械臂与外界环境接触力的控制体现出机械臂具有一定的柔顺性。为了使机器人对外界具有柔顺性，现有的控制方法可以分为两种：被动柔顺控制和主动柔顺控制。被动柔顺即利用能够储存或吸收能量的机械器件把机械臂与作用环境分隔开，如弹簧、阻尼等具有弹性的机构，使机器人与环境相接触时对外部的作用力产生一定的自然顺从能力。最具有代表性的被动柔顺装置是美国一个实验室设计的一种称为远程中心柔顺的无源机械装置，它是一个由六根弹簧构成的能顺从空间六个自由度的柔顺手腕，但它不能使机器人本身产生对力的反应。另外被动柔顺控制由于弹性构件的变形导致难以精确控制位置，因而其一般应用在要求比较低的场合。

主动柔顺控制是把力完整的体现在控制系统中，应用控制算法来抵消位置和力双向误差，如阻抗控制、刚度控制、力/位混合控制等，但往往需要机械臂开放力矩控制，这是目前市场上的机械臂难以做到的，而且力矩控制需要从底层控制器上进行算法编程，大大地增加了控制算法的复杂程度。

发明内容

有鉴于此，本发明提供了一种用于单自由度柔性末端的机械臂控制算法，通过速度控制的方式来实现机械臂和环境之间的力交互控制，简化了算法的复杂性；通过在机械臂末端安装力传感器的方式对接触力进行测量，从而完成主动柔顺控制；通过在工具和力传感器之间增加单自由度的柔性连接来提高位置控制精度，即结合了传统的被动柔顺控制和主动柔顺控制的优点，从而使机械臂完成与环境之间的良好交互控制。

一种用于单自由度柔性末端的机械臂控制算法，该算法基于单自由度柔性末端的机械臂，机械臂末端固定连接力传感器，力传感器与轴承套固定连接，工具通过直线轴承安装在轴承套中，工具只能沿直线轴承的轴向滑动，弹簧安装在轴承套内部的空腔内，弹簧的两端分别与工具和力传感器抵触；

该算法首先建立世界坐标系、接触力测量坐标系和运动坐标系，通过柔顺度来建立起机械臂末端运动速度和接触力的关系，整体的柔顺度为机械臂的柔顺度、工具末端的柔顺度以及环境的柔顺度之和；由所需控制的接触力得到机械臂末端运动速度，通过逆运动学求解完成机械臂各个关节角的转动速度并控制机械臂运动，再通过PD控制器使接触力达到稳定值。

进一步地，所述控制算法的计算步骤如下：

第一步：建立坐标系

机械臂的基座上建立世界坐标系{W}，在力传感器测量端面上建立接触力测量坐标系{S}，此外，再建立一个运动坐标系{C}，运动坐标系{C}以工具末端和环境的接触点为原点，工具的轴线方向为Z轴；

第二步：求解机械臂末端与环境接触点在世界坐标系{W}下的速度

机械臂末端与环境的接触点在世界坐标系{W}中的输出位置为y，通过运动学关系可以表示为：

y＝f(q,x_u) (1)

其中q为机械臂的关节角，x_u为整个系统的未能控制的自由度所产生的位移，包括环境接触的变形量以及弹簧的变形量；交互环境限定为刚性环境，环境的变形量可以忽略不计，因此整个系统未能控制的自由度所产生的位移即为弹簧的变形量δx：

δx＝f_s/k_s (2)

其中k_s为弹簧的刚度，f_s为施加在弹簧上的力由传感器测量得到，由于工具仅沿着{S}坐标系的Z轴方向运动，因此坐标系{C}相对于{S}的齐次变换矩阵为：

其中L为弹簧不受力时工具末端到坐标系{S}原点的距离，因此从末端接触点到世界坐标的正运动学变换可以表示为：

其中为坐标系{S}相对于世界坐标系{W}的变换矩阵；在基于速度的控制方法中，计算目标速度的大小：

其中

K_s＝diag(0,0,-1/k_s,0,0,0) (6)

J_q为机械臂的雅可比矩阵，将关节速度映射到世界坐标系下的笛卡尔速度；为从坐标系{S2}到{W}的速度转换矩阵；

第三步：求解机械臂在世界坐标系{W}下的柔顺度

在使用基于速度控制的方法时，力不能被直接控制，必须转化成位移，因此力和速度的关系表示为：

其中C_y为在世界坐标系下的柔顺度之和，f为施加在机械臂末端上的力，为机械臂末端输出的速度；

为了控制具有一定柔顺性的机械臂，只需要考虑静力学，从而机械臂的驱动模型可以表示为：

τ_q＝K_q(q_d-q) (8)

其中τ_q为关节输出力矩，K_q为控制系统的增益矩阵，q为关节角，即关节力矩的大小和关节的运动速度成正比；另外，机械臂的关节空间和末端输出的速度以及力的转换关系有：

J_q ^Tf＝τ_q (9)

将式(9)、(10)代入式(8)整理可得到：

对比式(7)可以得到机械臂在世界坐标系{W}下的柔顺度为：

C_r＝J_qK_q ^-1J_q ^T (12)

第四步，求解工具末端以及环境在世界坐标系{W}下的柔顺度

为了计算出整体的柔顺度，在此还需要考虑弹簧的变形，由于限定环境为刚性，因此，环境的柔顺度为0，工具和环境的接触为点接触，从而可以得到接触力f和弹簧所受到的力f_s之间的关系为：

f_s＝fcos(θ) (13)

其中θ为接触力和工具轴线方向的夹角，将式(12)和(13)带入式(5)可以得到末端接触点的速度在世界坐标系下为：

因为控制的目标是为了给环境施加一个静态的力，即代入式(14)，

可得

由于整体的柔顺度为机械臂和工具的柔顺度之和，因此，工具的柔顺度表示为：

第五步：通过PD控制器使接触力达到稳定值

通过柔顺度的计算从而可得到力和速度的关系，另外为了控制接触力能达到一个稳定的值，在此采用PD控制器，传统的PD控制器为：

其中为控制输出量(速度)，e为控制输入量和反馈量的差，K_p,K_d∈R^6×6为对角矩阵，分别为比例控制系数和微分控制系数；当控制的输入量为力时，可通过式(7)建立起速度和力的关系，结合PD控制器，从而可以得到：

其中f_c为工具和环境之间的接触力反馈，f_{c_d}为需要达到的接触力；由于反馈的线性化，因此可以找到合适的矩阵来使整个闭环系统达到稳定的状态。

有益效果：

1、本发明克服了现有柔顺控制存在的一些缺点，通过速度控制的方式来实现机械臂和环境之间的力交互控制，简化了算法的复杂性，降低了对机械臂的硬件配置要求，有利于人机协作机械臂的推广。

2、本发明结合了传统的被动柔顺控制和主动柔顺控制的优点，通过主动柔顺控制的方式补偿了被动柔顺机构位置误差大的问题，从而使机械臂完成与环境之间的良好力交互控制。

附图说明

图1为本发明机械臂末端的结构示意图；

图2为本发明机械臂控制场景示意图。

其中，1-机械臂末端、2-力传感器、3-直线轴承、4-工具、5-弹簧、6-轴承套。

具体实施方式

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

本发明提供了一种用于单自由度柔性末端的机械臂控制算法，该算法基于单自由度柔性末端的机械臂，如附图1所示，机械臂末端1固定连接力传感器2，力传感器2与轴承套6固定连接，工具4通过直线轴承3安装在轴承套6中，工具4只能沿直线轴承3的轴向滑动，弹簧5安装在轴承套6内部的空腔内，弹簧5的两端分别与工具4和力传感器2抵触。

机械臂的控制场景如附图2所示，机械臂的基座上建立世界坐标系{W}，在力传感器测量端面上建立接触力测量坐标系{S}，此外，再建立一个运动坐标系{C}，运动坐标系{C}以工具末端和环境的接触点为原点，工具的轴线方向为Z轴。

通过整体柔顺度来建立起机械臂末端运动速度和接触力的关系，整体的柔顺度C_y为机械臂的柔顺度C_r，工具末端的柔顺度C_t以及环境的柔顺度C_e之和，由所需控制的接触力f得到机械臂末端运动速度通过逆运动学求解完成机械臂各个关节角的转动速度并控制机械臂运动，再通过PD控制器使接触力达到稳定值。

本发明的控制算法的计算步骤如下：

第一步：建立坐标系

机械臂的基座上建立世界坐标系{W}，在力传感器测量端面上建立接触力测量坐标系{S}，此外，再建立一个运动坐标系{C}，运动坐标系{C}以工具末端和环境的接触点为原点，工具的轴线方向为Z轴；

第二步：求解机械臂末端与环境接触点在世界坐标系{W}下的速度

机械臂末端与环境的接触点在世界坐标系{W}中的输出位置为y，通过运动学关系可以表示为：

y＝f(q,x_u) (1)

其中q为机械臂的关节角，x_u为整个系统的未能控制的自由度所产生的位移，包括环境接触的变形量以及弹簧的变形量；交互环境限定为刚性环境，环境的变形量可以忽略不计，因此整个系统未能控制的自由度所产生的位移即为弹簧的变形量δx：

δx＝f_s/k_s (2)

其中k_s为弹簧的刚度，f_s为施加在弹簧上的力由传感器测量得到，由于工具仅沿着{S}坐标系的Z轴方向运动，因此坐标系{C}相对于{S}的齐次变换矩阵为：

其中L为弹簧不受力时工具末端到坐标系{S}原点的距离，因此从末端接触点到世界坐标的正运动学变换可以表示为：

其中为坐标系{S}相对于世界坐标系{W}的变换矩阵；在基于速度的控制方法中，计算目标速度的大小：

其中

K_s＝diag(0,0,-1/k_s,0,0,0) (6)

J_q为机械臂的雅可比矩阵，将关节速度映射到世界坐标系下的笛卡尔速度；为从坐标系{S2}到{W}的速度转换矩阵；

第三步：求解机械臂在世界坐标系{W}下的柔顺度

在使用基于速度控制的方法时，力不能被直接控制，必须转化成位移，因此力和速度的关系表示为：

其中C_y为在世界坐标系下的柔顺度之和，f为施加在机械臂末端上的力，为机械臂末端输出的速度；

为了控制具有一定柔顺性的机械臂，只需要考虑静力学，从而机械臂的驱动模型可以表示为：

τ_q＝K_q(q_d-q) (8)

其中τ_q为关节输出力矩，K_q为控制系统的增益矩阵，q为关节角，即关节力矩的大小和关节的运动速度成正比；另外，机械臂的关节空间和末端输出的速度以及力的转换关系有：

J_q ^Tf＝τ_q (9)

将式(9)、(10)代入式(8)整理可得到：

对比式(7)可以得到机械臂在世界坐标系{W}下的柔顺度为：

C_r＝J_qK_q ^-1J_q ^T (12)

第四步，求解工具末端以及环境在世界坐标系{W}下的柔顺度

为了计算出整体的柔顺度，在此还需要考虑弹簧的变形，由于限定环境为刚性，因此，环境的柔顺度为0，工具和环境的接触为点接触，从而可以得到接触力f和弹簧所受到的力f_s之间的关系为：

f_s＝fcos(θ) (13)

其中θ为接触力和工具轴线方向的夹角，将式(12)和(13)带入式(5)可以得到末端接触点的速度在世界坐标系下为：

因为控制的目标是为了给环境施加一个静态的力，即代入式(14)，

可得

由于整体的柔顺度为机械臂和工具的柔顺度之和，因此，工具的柔顺度表示为：

第五步：通过PD控制器使接触力达到稳定值

通过柔顺度的计算从而可得到力和速度的关系，另外为了控制接触力能达到一个稳定的值，在此采用PD控制器，传统的PD控制器为：

其中为控制输出量(速度)，e为控制输入量和反馈量的差，K_p,K_d∈R^6×6为对角矩阵，分别为比例控制系数和微分控制系数；当控制的输入量为力时，可通过式(7)建立起速度和力的关系，结合PD控制器，从而可以得到：

其中f_c为工具和环境之间的接触力反馈，f_{c_d}为需要达到的接触力；由于反馈的线性化，因此可以找到合适的矩阵来使整个闭环系统达到稳定的状态。

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

Claims (7)

1.一种用于单自由度柔性末端的机械臂控制算法，其特征在于，该算法基于单自由度柔性末端的机械臂，机械臂末端固定连接力传感器，力传感器与轴承套固定连接，工具通过直线轴承安装在轴承套中，工具只能沿直线轴承的轴向滑动，弹簧安装在轴承套内部的空腔内，弹簧的两端分别与工具和力传感器抵触；

该算法首先建立世界坐标系、接触力测量坐标系和运动坐标系，通过整体柔顺度来建立起机械臂末端运动速度和接触力的关系，整体的柔顺度为机械臂的柔顺度、工具末端的柔顺度以及环境的柔顺度之和；由所需控制的接触力f得到机械臂末端运动速度，通过逆运动学求解完成机械臂各个关节角的转动速度并控制机械臂运动，再通过PD控制器使接触力达到稳定值。

2.如权利要求1所述的一种用于单自由度柔性末端的机械臂控制算法，其特征在于，所述控制算法的计算步骤如下：

第一步：建立坐标系；

第二步：求解机械臂末端与环境接触点在世界坐标系{W}下的速度

第三步：求解机械臂在世界坐标系{W}下的柔顺度；

第四步：求解工具末端以及环境在世界坐标系{W}下的柔顺度；

第五步：通过PD控制器使接触力达到稳定值。

3.如权利要求2所述的一种用于单自由度柔性末端的机械臂控制算法，其特征在于，所述第一步中坐标系的建立过程为：机械臂的基座上建立世界坐标系{W}，在力传感器测量端面上建立接触力测量坐标系{S}，此外，再建立一个运动坐标系{C}，运动坐标系{C}以工具末端和环境的接触点为原点，工具的轴线方向为Z轴。

4.如权利要求2或3所述的一种用于单自由度柔性末端的机械臂控制算法，其特征在于，所述第二步中的求解过程为：机械臂末端与环境的接触点在世界坐标系{W}中的输出位置为y，通过运动学关系可以表示为：

y＝f(q,x_u) (1)

其中q为机械臂的关节角，x_u为整个系统的未能控制的自由度所产生的位移，包括环境接触的变形量以及弹簧的变形量；交互环境限定为刚性环境，环境的变形量可以忽略不计，因此整个系统未能控制的自由度所产生的位移即为弹簧的变形量δx：

δx＝f_s/k_s (2)

其中k_s为弹簧的刚度，f_s为施加在弹簧上的力由传感器测量得到，由于工具仅沿着{S}坐标系的Z轴方向运动，因此坐标系{C}相对于{S}的齐次变换矩阵为：

$T_C^S=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& L-\mathrm{\δ}x\\ 0& 0& 0& 0\\ 0& 0& 0& 1\end{array}\right]---\left(3\right)$

其中L为弹簧不受力时工具末端到坐标系{S}原点的距离，因此从末端接触点到世界坐标的正运动学变换可以表示为：

$T=T_S^{W}T_C^S---\left(4\right)$

其中为坐标系{S}相对于世界坐标系{W}的变换矩阵；在基于速度的控制方法中，计算目标速度的大小：

$\stackrel{\·}{y}=J_q\stackrel{\·}{q}+{T_S^W}_v{K}_s{\stackrel{\·}{f}}_s---\left(5\right)$

其中

K_s＝diag(0,0,-1/k_s,0,0,0) (6)

J_q为机械臂的雅可比矩阵，将关节速度映射到世界坐标系下的笛卡尔速度；

为从坐标系{S2}到{W}的速度转换矩阵。

5.如权利要求2或4所述的一种用于单自由度柔性末端的机械臂控制算法，其特征在于，所述第三步中的求解过程为：

在使用基于速度控制的方法时，力不能被直接控制，必须转化成位移，因此力和速度的关系表示为：

$\stackrel{\·}{y}=C_{y}f---\left(7\right)$

其中C_y为在世界坐标系下整体的柔顺度，f为施加在机械臂末端上的力，为机械臂末端输出的速度；

为了控制具有一定柔顺性的机械臂，只需要考虑静力学，从而机械臂的驱动模型可以表示为：

τ_q＝K_q(q_d-q) (8)

其中τ_q为关节输出力矩，K_q为控制系统的增益矩阵，q为关节角，即关节力矩的大小和关节的运动速度成正比；另外，机械臂的关节空间和末端输出的速度以及力的转换关系有：

J_q ^Tf＝τ_q (9)

$J_q\stackrel{\·}{q}=\stackrel{\·}{y}---\left(10\right)$

将式(9)、(10)代入式(8)整理可得到：

$\stackrel{\·}{y}={J_q}^{-1}{K_q}^{-1}{J_q}^{T}f---\left(11\right)$

对比式(7)可以得到机械臂在世界坐标系{W}下的柔顺度为：

C_r＝J_qK_q ^-1J_q ^T (12)。

6.如权利要求2或5所述的一种用于单自由度柔性末端的机械臂控制算法，其特征在于，所述第四步中的求解过程为：

由于限定环境为刚性，因此，环境的柔顺度为0，工具和环境的接触为点接触，从而可以得到接触力f和弹簧所受到的力f_s之间的关系为：

f_s＝fcos(θ) (13)

其中θ为接触力和工具轴线方向的夹角，将式(12)和(13)带入式(5)可以得到末端接触点的速度在世界坐标系下为：

$\stackrel{\·}{y}=\[C_r-\stackrel{\·}{\mathrm{\θ}}\sin \left(\mathrm{\θ}\right){T_S^W}_v{K}_s\]f+{T_S^W}_v{K}_s\stackrel{\·}{f}cos\left(\mathrm{\θ}\right)---\left(14\right)$

因为控制的目标是为了给环境施加一个静态的力，即代入式(14)，可得

由于整体的柔顺度为机械臂和工具的柔顺度之和，因此，工具的柔顺度表示为：

$C_t=-\stackrel{\·}{\mathrm{\θ}}sin\left(\mathrm{\θ}\right){T_{S2}^W}_v{K}_s---\left(16\right).$

7.如权利要求2或6所述的一种用于单自由度柔性末端的机械臂控制算法，其特征在于，所述第五步的实现过程为：

通过柔顺度的计算从而可得到力和速度的关系，另外为了控制接触力能达到一个稳定的值，在此采用PD控制器，传统的PD控制器为：

$\stackrel{\·}{y}=K_{p}e+K_d{e}^{\′}---\left(17\right)$

其中为控制输出量(速度)，e为控制输入量和反馈量的差，K_p,K_d∈R^6×6为对角矩阵，分别为比例控制系数和微分控制系数；当控制的输入量为力时，可通过式(7)建立起速度和力的关系，结合PD控制器，从而可以得到：

$\stackrel{\·}{y}=K_p{C}_y(f_{c\_d}-f_c)+K_d{C}_y{(f_{c\_d}-f_t)}^{\′}---\left(18\right)$

其中f_c为工具和环境之间的接触力反馈，f_{c_d}为需要达到的接触力。