CN112405536B - 结合离线补偿和在线跟踪混合策略的高精度恒力控制方法 - Google Patents

Abstract

本发明公开了结合离线补偿和在线跟踪混合策略的高精度恒力控制方法，该方法为：首先在离线编程软件中导入工件模型和机器人模型进行单元建模，通过离线编程软件生成平滑且连续的运动参考轨迹序列；然后通过建立机器人关节刚度模型，在机器人关节刚度辨识的基础上对参考轨迹序列进行开环式的离线恒力补偿；最后在开环式的离线补偿的基础上，根据补偿后的参考轨迹序列进一步生成接触式作业中闭环式在线恒力控制的期望轨迹，进一步通过变刚度自适应导纳在线跟踪控制方法来实现接触式作业过程中的高精度恒力控制。本发明将离线补偿和在线跟踪策略结合起来实现机器人高精度恒力控制的功能，既包含开环式的位置补偿控制又结合了闭环式在线恒力跟踪控制。

Description

结合离线补偿和在线跟踪混合策略的高精度恒力控制方法

技术领域

本发明涉及工业机器人接触式作业领域，特别是涉及一种结合离线补偿和在线跟踪混合策略的高精度恒力控制方法。

背景技术

工业机器人的身影已遍布于人类的生活和生产过程中，重塑了生产体系并极大地解放了生产力。在接触式作业领域，例如打磨、装配、抛光等，由于传统技艺方法的桎梏，严重影响了生产效率且生产过程中的有害粉尘侵害了从业人员的身体。工业机器人生产线的引入完美地解决了上述问题，且随着机器人技术的不断发展，高精度恒力控制方法在机器人接触式作业领域已经得到了广泛的应用。

对于上述应用场景，核心问题是如何控制作业环境与机器人工具手末端的接触力大小。例如机器人在打磨工件的过程中，工具手与工件接触力的恒定可以降低工具手的磨损率并保证被打磨工件的连贯性，提高打磨精度。关于恒力控制技术方法的研究逐步走向成熟，现如今最具代表性的两种方法有力/位置混合控制和阻抗(导纳)控制。

力/位置混合控制的核心思想是通过正交原理将机器人的工作空间划分为力约束和位置约束两个相互正交的子空间，分而治之地进行力控制和位置控制。实际的工业生产中，很多复杂或多变的工作空间不可以正交分解成两个子空间，力/位置混合控制方法只适合应用于工作空间较为简单的任务需求。对于复杂且多变的工作空间，阻抗(导纳)控制方法也能够胜任。阻抗控制或导纳控制(两者在实现方式上有所不同)的核心就是将机器人末端力/位置控制等效为“弹簧-质量-阻尼器”模型，建立起系统间各部件的作用关系并由微分方程来描述。在实际接触式工作中，应用阻抗(导纳)控制方法的机器人末端工具手与外界的接触力大小由工具手的参考位置、环境刚度等因素来决定。该方法对环境信息的依赖性较强且是一种间接性质的力控制方法，然而在实际作业环境中，环境信息的精确获取是比较困难的，所以如何解决非精确环境信息下的力/位置控制是当前机器人阻抗控制的难点。

发明内容

发明目的：本发明的目的是为了克服已有技术的不足，提出了一种结合离线补偿和在线跟踪混合策略的高精度恒力控制方法，该方法基于机器人刚度模型和传统的导纳控制方法，提出了一种基于机器人关节刚度模型的离线恒力补偿控制方法和变刚度自适应导纳在线跟踪控制方法相结合的策略。在实际工作中，可以较好地解决非精确环境信息下的力/位置控制中难题，提高了工具手与环境间的接触位置和接触力大小的精度，从而提高了接触式作业生产中的效率和精度。

上述的目的通过以下技术方案实现：

一种结合离线补偿和在线跟踪混合策略的高精度恒力控制方法，该方法包括如下步骤：

S1.单元建模：根据所用机器人的型号选取离线编程软件，然后将接触式作业中的工件模型和机器人模型导入选定的离线编程软件中，即建立起机器人运动学模型并根据实际环境中工件和机器人的摆放位置进行布局，继而生成平滑且连续的运动参考轨迹序列；

S2.参考轨迹序列的开环式离线恒力补偿：首先建立机器人关节刚度模型，在此基础上提出基于机器人关节刚度模型的开环式离线恒力补偿方法并通过机器人关节刚度辨识实验所求出的机器人关节刚度矩阵K_θ和末端工具手的等效刚度系数k_t，以及给定的垂直于工件的期望力F_d的一半对参考轨迹序列中的机器人末端工具手位姿X_r进行开环式离线恒力补偿；然后对机器人接触式作业中的所有参考轨迹序列点所对应的机器人末端工具手的位姿X_r进行相应的开环式离线恒力补偿；

S3.闭环式在线恒力跟踪控制的实现：在对参考轨迹序列中的机器人末端工具手位姿X_r实现开环式离线恒力补偿功能的基础上，结合基于力反馈的变刚度自适应导纳控制方法，即在接触作业中，通过力传感器采集到的来自工件的作用力f与期望作用力f_d的差值作为变刚度自适应导纳控制器的反馈量来实现作业过程中的闭环式在线恒力跟踪功能。

所述的结合离线补偿和在线跟踪混合策略的高精度恒力控制方法，步骤S2的具体方法为：

S21.建立机器人关节刚度矩阵K_θ的表达式：

其中，k_θ1，…,k_θ6分别对应机器人关节1-6的等效弹簧系数、τ表示关节力矩、τ₁，…,τ₆分别对应于机器人关节1-6所受到的力矩、Δθ表示关节形变、Δθ₁，…,Δθ₆分别对应机器人关节1-6的形变量；

当机器人工具手末端进行接触式作业时，设机器人所受的来自工件的挤压力为F，F＝[F_x，F_y，F_z，T_x，T_y，T_z]，其中F_x，F_y，F_z表示机械臂的末端工具手在X,Y,Z方向上所受的力，T_x，T_y，T_z表示末端工具手在X,Y,Z方向上所受的力矩，J为机械臂的雅各比矩阵，J^T为J的转置；

然后可得关节力矩τ在工具手进行接触作业时的表达式如下：

τ＝J^TF (2)

由于六自由度串联式转动关节机械臂在接触式作业中通过挤压工件会使各个关节发生形变，每个关节形变经过层层传递的累计将导致机器人工具手末端出现位姿的偏差ΔX，其中ΔX＝[d_x，d_y，d_z，δ_x，δ_y，δ_z]，d_x，d_y，d_z表示工具手末端的位置偏差，δ_x，δ_y，δ_z表示工具手末端的姿态偏差；

接着可得偏差与各关节形变量关节如下所示：

ΔX＝JΔθ (3)

最后综合公式(1)、(2)和(3)可得机器人关节刚度模型：

S22.开环式离线恒力补偿：

在参考轨迹序列中，机器人工具手末端与工件处于临界接触状态，机器人末端工具手的位置和位姿分别用x_r和X_r表示，

其中机器人工具手末端位置和姿态的期望值分别由

和

r_zr表示；

在机器人接触式作业中，设机器人末端工具手与工件环境接触时产生的期望作用力为0.5F_d，由此所导致的机器人整体的垂直于工件方向上的末端形变量为ΔX_d，首先通过公式(5)计算得到ΔX_d：

ΔX_d＝ΔX_r+ΔX_t (5)

其中，

为机器人关节柔性引起的末端形变量，ΔX_t为末端工具手的形变量；

然后计算机器人末端工具手与工件环境接触时产生的期望作用力0.5F_d与末端工具手形变量ΔX_t之间的线性关系：

其中，k_t为末端工具手等效刚度；

最后由式(5)和(6)可以得到机械臂的末端工具手经过开环式离线恒力补偿后的位姿X_e：

S33.对机器人接触式作业中的所有参考轨迹序列点所对应的机器人末端工具手的位姿X_r进行相应的开环式离线恒力补偿，补偿之后的末端工具手的位置和位姿分别是x_e和X_e，以此来通过开环式的位置补偿控制来初步实现机器人与工件的接触式作业功能。

所述的结合离线补偿和在线跟踪混合策略的高精度恒力控制方法，步骤S3的具体方法为：

S31.在传统的导纳控制算法的基础上进行改进得到基于位置的导纳控制方法，进而得接触作业时机器人末端工具手与接触环境的作用力f和机器人末端工具手的位置x，具体方法如下：

传统导纳控制算法：

建立一个二阶微分方程来表示机器人工具手末端的力和位置之间的动态关系，即导纳控制模型：

然后对上式进行拉普拉斯变换,可更直观看出力与位置之间的转换关系：

(ms²+bs+k)E(s)＝F(s) (9)

E(s)＝X_d(s)-X_c(s)

在传统导纳控制的基础上对公式(8)中的f用机器人接触作业中的实际作用力与期望作用力的差值f-f_d来替代。

进一步得到改进的基于位置的导纳控制方法如下：

在接触式作业中，设机器人的内环中的位置控制器所具的良好跟踪能力可以保证机器人的末端执行器始终可以进行精确的柔顺控制，即x＝x_c；设接触环境为线性弹簧模型，其中环境刚度为k_e，环境位置为x_e，则得接触作业时机器人末端工具手与接触环境的作用力f和机器人末端工具手的位置x为：

S32.接触式作业中的期望轨迹生成，即将离线补偿和在线跟踪的策略结合起来实现机器人在接触式作业中的高精度恒力控制功能：

首先通过公式(11)可得到导纳控制器的输入量，即机器人末端工具手期望轨迹x_d的表达式如下：

其中机器人末端工具手的期望轨迹x_d由环境位置x_e、期望作用力f_d和环境刚度k_e所决定，其中x_e已通过前期的离线补偿环节所获取，期望作用力f_d是人为给定的常数，环境刚度k_e可通过在线估计获取，假设

为环境刚度k_e的估计值，

然后可得x_d与

关系的表达式：

最后综上可得：

由于接触作业时的工件一般是同一种材质的物体，故该工件的物体刚度系数大致为固定数值，通过离散化系统的数值积分计算法可得环境刚度的估计值

并通过式(12)来对已经经过离线补偿后的参考轨迹x_e进行更新以此来得到期望轨迹x_d；

S33.变刚度自适应导纳控制算法的实现，即实现闭环式在线恒力跟踪：

首先提出变刚度导纳控制方法，其微分方程如下所示：

其中，自适应刚度系数k(t)的适时调整由期望接触力控制，k_f和k_v分别表示力跟踪误差控制的比例系数与微分系数。

然后力的跟踪误差方程可由上式导出：

在实现开环式离线补偿功能的基础上，根据上述步骤，首先通过在线估计的方式获取环境刚度的估计值

通过式(13)计算机器人末端工具手的期望轨迹x_d；然后人为设定好目标刚度自适应调整率中的k_f和k_v；最后根据自适应调整率计算变刚度导纳控制器中的目标刚度系数k(t)和通过力传感器采集到的来自工件的作用力f与期望作用力f_d的差值作为变刚度自适应导纳控制器的反馈量来实现作业过程中的闭环式在线恒力跟踪功能。

有益效果：与现有技术相比，本发明具有如下有益效果：

本发明能够提供一种机器人接触式作业中所使用的通用性较强的高精度恒力控制算法，有效且实用地解决了接触式作业的参考轨迹规划问题和接触作业过程中的高精度恒力控制问题，提升了接触式作业领域的通用性且提高了接触式作业生产过程中的效率和精度，本发明具有较好的使用效果和应用前景。

附图说明

图1为：总体框架设计流程图；

图2为：机器人运动参考轨迹序列生成框图；

图3为：离线补偿控制框图；

图4为：导纳控制的物理建模示意图；

图5为：应用传统导纳控制方法的机器人控制框图；

图6为：应用变刚度自适应导纳控制方法的机器人控制框图。

具体实施方式

下面结合附图和具体实施方式对本发明做进一步说明。

图1为本专利所提出的结合离线补偿和在线跟踪混合策略的高精度恒力控制方法的总体流程图。其具体步骤如下：

1.机器人运动参考轨迹序列生成

(1)单元建模

本发明首先要根据所用机器人的型号选取合适的离线编程软件(例如ABB机器人与RobotStudio软件，KUKA机器人与KUKA.Sim软件等)，然后将接触式作业中的工件模型和机器人模型导入选定的离线编程软件中，继而根据机器人的各项参数和作业环境中机器人和工件的摆放位置分别进行运动学模型的建立和环境的布局，即实现单元建模功能。

(2)参考轨迹序列的生成

本发明在上述步骤完成之后，在离线编程软件中，根据接触式作业任务的需求和接触过程中的作业区域划分生成平滑且连续的机器人接触式作业的参考轨迹序列，有关机器人运动参考轨迹序列生成的过程如图2所示。

2.参考轨迹序列的开环式离线恒力补偿

(1)机器人关节刚度模型

本发明根据机器人工具手末端与外界环境进行接触式作业时机器人的各个关节会发生形变的原理，忽略此过程中连杆的变形，对机器人关节轴等效成线性弹簧进行建模，便可以建立机器人各个关节受力与关节形变量的关系，以接触式作业中所使用的六自由度工业机械臂为例。

首先可建立机器人关节刚度矩阵K_θ的表达式：

其中，τ表示关节力矩、Δθ表示关节形变、Δθ₁，…,Δθ₆分别对应机器人关节1-6的形变量。

当机器人工具手末端进行接触式作业时，设机器人所受的来自工件的挤压力为F(F＝[F_x，F_y，F_z，T_x，T_y，T_z]，其中F_x，F_y，F_z表示机械臂的末端工具手在X,Y,Z方向上所受的力，T_x，T_y，T_z表示末端工具手在X,Y,Z方向上所受的力矩)，J为机械臂的雅各比矩阵。

然后可得关节力矩τ在工具手进行接触作业时的表达式如下：

τ＝J^TF (2)

由于六自由度串联式转动关节机械臂在接触式作业中通过挤压工件会使各个关节发生形变，每个关节形变经过层层传递的累计将导致机器人工具手末端出现位姿的偏差ΔX(其中ΔX＝[d_x，d_y，d_z，δ_x，δ_y，δ_z]，d_x，d_y，d_z表示工具手末端的位置偏差，δ_x，δ_y，δ_z表示工具手末端的姿态偏差)，

接着可得偏差与各关节形变量关节如下所示：

ΔX＝JΔθ (3)

最后综合公式(1)、(2)和(3)可得机器人关节刚度模型：

(2)离线恒力补偿原理

本发明步骤一中所用离线编程软件生成的参考轨迹序列中的机器人末端工具手的位置和位姿分别是位姿为x_r和X_r，此时机器人工具手末端与工件处于临界接触状态(工具手与工件未产生相互作用力)时的位姿为X_r，

其中机器人工具手末端位置和姿态的期望值分别由

和

r_zr表示。

在机器人接触式作业中，由于机械臂末端工具手并非绝对刚体，在受力作用下会产生形变。因此，为了能够在给机器人一个垂直于工件方向的指令下压量ΔX_d时产生垂直于工件的期望作用力F_d的一半。

首先可以通过公式(5)计算得到ΔX_d：

ΔX_d＝ΔX_r+ΔX_t (5)

其中，

为机器人关节柔性引起的末端形变量，ΔX_t为末端工具手的形变量，设k_t为末端工具手等效刚度。

然后可得期望作用力F_d的二分之一与末端工具手形变量ΔX_t之间的线性关系:

最后由式(5)和(6)可以得到机械臂的末端工具手经过离线补偿后的位姿X_e：

根据上述的偏差补偿求解步骤可以对参考轨迹序列的机器人末端位姿进行补偿，整体的偏差补偿和控制框架如图3所示。通过机器人关节刚度辨识实验所求出的机器人关节刚度矩阵K_θ和末端工具手的等效刚度系数k_t，以及给定的垂直于工件的期望力F_d的一半结合机器人关节刚度模型的离线恒力补偿控制方法即公式(7)来对参考轨迹序列中的机器人末端工具手位姿X_r进行相应的偏差补偿，补偿之后的末端工具手的位置和位姿分别是x_e和X_e，以此来通过开环式的位置补偿控制来初步实现机器人与工件的接触式作业功能。

3.闭环式在线恒力跟踪控制的实现

(1)传统导纳控制算法

本发明实现接触式作业过程中的闭环式在线恒力跟踪功能的核心基础是导纳控制算法。阻抗控制或导纳控制(两者在实现方式上有所不同)的核心就是将机器人末端力/位置控制等效为“弹簧-质量-阻尼器”模型，建立起系统间各部件的作用关系并由微分方程来描述。

首先根据图4中的模型描述可以建立一个二阶微分方程来表示机器人工具手末端的力和位置之间的动态关系，即阻抗(导纳)控制模型：

然后对上式进行拉普拉斯变换,可更直观看出力与位置之间的转换关系：

(ms²+bs+k)E(s)＝F(s) (9)

E(s)＝X_d(s)-X_c(s)

其中导纳控制的传递函数为

是一种基于位置的控制方法。一些研究人员在传统导纳控制的基础上对公式(8)中的f用机器人接触作业中的实际作用力与期望作用力的差值f-f_d来替代。

进一步得到改进的基于位置的导纳控制方法如下：

在接触式作业中，假设机器人的内环中的位置控制器所具的良好跟踪能力可以保证机器人的末端执行器始终可以进行精确的柔顺控制(x＝x_c)；假设接触环境为线性弹簧模型，其中环境刚度为k_e，环境位置为x_e(即离线补偿之后的末端工具手的位置)，图5是上述基于位置的导纳控制方法的控制框图。

最后可得接触作业时机器人末端工具手与接触环境的作用力f和机器人末端工具手的位置x为：

f＝k_e(x-x_e) (11)

(2)变刚度自适应控制算法

1)接触式作业中的期望轨迹生成

本发明的该部分将离线补偿和在线跟踪的策略结合起来实现机器人在接触式作业中的高精度恒力控制功能。

首先通过公式(11)可得到导纳控制器的输入量，即机器人末端工具手期望轨迹x_d的表达式如下：

其中机器人末端工具手的期望轨迹x_d由环境位置x_e、期望作用力f_d和环境刚度k_e所决定，其中x_e已通过前期的离线补偿环节所获取，期望作用力f_d是人为给定的常数，环境刚度k_e可通过在线估计获取，假设

为环境刚度k_e的估计值，

然后可得x_d与

关系的表达式：

最后综上可得：

由于接触作业时的工件一般是同一种材质的物体，故该工件的物体刚度系数大致为固定数值，通过离散化系统的数值积分计算法可得环境刚度的估计值

并通过式(12)来对已经经过离线补偿后的参考轨迹x_e进行更新以此来得到期望轨迹x_d。

2)变刚度自适应导纳控制算法的实现

本发明的上一个步骤已经将离线补偿模块与在线跟踪控制相结合，然后本专利通过人体对力量的控制方式的研究，实现闭环式在线恒力跟踪。

首先提出变刚度导纳控制方法，其微分方程如下所示：

其中，自适应刚度系数k(t)的适时调整由期望接触力控制，k_f和k_v分别表示力跟踪误差控制的比例系数与微分系数。

然后力的跟踪误差方程可由上式导出：

稳态时，跟踪力误差为零且可通过李亚普诺夫函数证明本发明所提出的控制算法是稳定的。其具体的算法实现过程如图6所示。在实现开环式离线补偿功能的基础上，根据上述步骤，首先通过在线估计的方式获取环境刚度的估计值

通过式(13)计算机器人末端工具手的期望轨迹x_d；然后人为设定好目标刚度自适应调整率中的k_f和k_v；最后根据自适应调整率计算变刚度导纳控制器中的目标刚度系数k(t)和通过力传感器采集到的来自工件的作用力f与期望作用力f_d的差值作为变刚度自适应导纳控制器的反馈量来实现作业过程中的闭环式在线恒力跟踪功能。

本发明将离线补偿和在线跟踪策略结合起来进行机器人高精度恒力控制方法的设计，既包含开环式的位置补偿控制又结合了闭环式在线恒力跟踪控制。对于有关接触式作业的工业领域例如打磨具有良好的控制效果和精度保证。

上述具体实施方式只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人士能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡根据本发明精神实质所作的等效变化或修饰，都应涵盖在本发明的保护范围之内。

Claims (3)

1.一种结合离线补偿和在线跟踪混合策略的高精度恒力控制方法，其特征在于：该方法包括如下步骤：

S1.单元建模：根据所用机器人的型号选取离线编程软件，然后将接触式作业中的工件模型和机器人模型导入选定的离线编程软件中，即建立起机器人运动学模型并根据实际环境中工件和机器人的摆放位置进行布局，继而生成平滑且连续的运动参考轨迹序列；

S2.参考轨迹序列的开环式离线恒力补偿：首先建立机器人关节刚度模型，在此基础上提出基于机器人关节刚度模型的开环式离线恒力补偿方法并通过机器人关节刚度辨识实验所求出的机器人关节刚度矩阵K_θ和末端工具手的等效刚度系数k_t，以及给定的垂直于工件的期望力F_d的一半对参考轨迹序列中的机器人末端工具手位姿X_r进行开环式离线恒力补偿；然后对机器人接触式作业中的所有参考轨迹序列点所对应的机器人末端工具手的位姿X_r进行相应的开环式离线恒力补偿；

S3.闭环式在线恒力跟踪控制的实现：在对参考轨迹序列中的机器人末端工具手位姿X_r实现开环式离线恒力补偿功能的基础上，结合基于力反馈的变刚度自适应导纳控制方法，即在接触作业中，通过力传感器采集到的来自工件的作用力f与期望作用力f_d的差值作为变刚度自适应导纳控制器的反馈量来实现作业过程中的闭环式在线恒力跟踪功能。

2.根据权利要求1所述的结合离线补偿和在线跟踪混合策略的高精度恒力控制方法，其特征在于：步骤S2的具体方法为：

S21.建立机器人关节刚度矩阵K_θ的表达式：

其中，k_θ1，…,k_θ6分别对应机器人关节1-6的等效弹簧系数、τ表示关节力矩、τ₁，…,τ₆分别对应于机器人关节1-6所受到的力矩、Δθ表示关节形变、Δθ₁，…,Δθ₆分别对应机器人关节1-6的形变量；

当机器人工具手末端进行接触式作业时，设机器人所受的来自工件的挤压力为F，F＝[F_x，F_y，F_z，T_x，T_y，T_z]，其中F_x，F_y，F_z表示机械臂的末端工具手在X,Y,Z方向上所受的力，T_x，T_y，T_z表示末端工具手在X,Y,Z方向上所受的力矩，J为机械臂的雅各比矩阵，J^T为J的转置；

然后可得关节力矩τ在工具手进行接触作业时的表达式如下：

τ＝J^TF (2)

由于六自由度串联式转动关节机械臂在接触式作业中通过挤压工件会使各个关节发生形变，每个关节形变经过层层传递的累计将导致机器人工具手末端出现位姿的偏差ΔX，其中ΔX＝[d_x，d_y，d_z，δ_x，δ_y，δ_z]，d_x，d_y，d_z表示工具手末端的位置偏差，δ_x，δ_y，δ_z表示工具手末端的姿态偏差；

接着可得偏差与各关节形变量关节如下所示：

ΔX＝JΔθ (3)

最后综合公式(1)、(2)和(3)可得机器人关节刚度模型：

S22.开环式离线恒力补偿：

在参考轨迹序列中，机器人工具手末端与工件处于临界接触状态，机器人末端工具手的位置和位姿分别用x_r和X_r表示，

其中机器人工具手末端位置和姿态的期望值分别由

和

表示；

在机器人接触式作业中，设机器人末端工具手与工件环境接触时产生的期望作用力为0.5F_d，由此所导致的机器人整体的垂直于工件方向上的末端形变量为ΔX_d，首先通过公式(5)计算得到ΔX_d：

ΔX_d＝ΔX_r+ΔX_t (5)

其中，

为机器人关节柔性引起的末端形变量，ΔX_t为末端工具手的形变量；

然后计算机器人末端工具手与工件环境接触时产生的期望作用力0.5F_d与末端工具手形变量ΔX_t之间的线性关系：

其中，k_t为末端工具手等效刚度；

最后由式(5)和(6)可以得到机械臂的末端工具手经过开环式离线恒力补偿后的位姿X_e：

S23.对机器人接触式作业中的所有参考轨迹序列点所对应的机器人末端工具手的位姿X_r进行相应的开环式离线恒力补偿，补偿之后的末端工具手的位置和位姿分别是x_e和X_e，以此来通过开环式的位置补偿控制来初步实现机器人与工件的接触式作业功能。

3.根据权利要求1所述的结合离线补偿和在线跟踪混合策略的高精度恒力控制方法，其特征在于：步骤S3的具体方法为：

S31.在传统的导纳控制算法的基础上进行改进得到基于位置的导纳控制方法，进而得接触作业时机器人末端工具手与接触环境的作用力f和机器人末端工具手的位置x，具体方法如下：

传统导纳控制算法：

建立一个二阶微分方程来表示机器人工具手末端的力和位置之间的动态关系，即导纳控制模型：

然后对上式进行拉普拉斯变换,可更直观看出力与位置之间的转换关系：

(ms²+bs+k)E(s)＝F(s) (9)

E(s)＝X_d(s)-X_c(s)

其中，m为等效质量块质量，b为等效阻尼器的阻尼系数，k为等效线型弹簧的刚性系数；

在传统导纳控制的基础上对公式(8)中的f用机器人接触作业中的实际作用力与期望作用力的差值f-f_d来替代；

进一步得到改进的基于位置的导纳控制方法如下：

在接触式作业中，设机器人的内环中的位置控制器所具的良好跟踪能力可以保证机器人的末端执行器始终可以进行精确的柔顺控制，即x＝x_c；设接触环境为线性弹簧模型，其中环境刚度为k_e，环境位置为x_e，则得接触作业时机器人末端工具手与接触环境的作用力f和机器人末端工具手的位置x为：

S32.接触式作业中的期望轨迹生成，即将离线补偿和在线跟踪的策略结合起来实现机器人在接触式作业中的高精度恒力控制功能：

首先通过公式(11)可得到导纳控制器的输入量，即机器人末端工具手期望轨迹x_d的表达式如下：

其中机器人末端工具手的期望轨迹x_d由环境位置x_e、期望作用力f_d和环境刚度k_e所决定，其中x_e已通过前期的离线补偿环节所获取，期望作用力f_d是人为给定的常数，环境刚度k_e可通过在线估计获取，假设

为环境刚度k_e的估计值，

然后可得x_d与

关系的表达式：

最后综上可得：

由于接触作业时的工件一般是同一种材质的物体，故该工件的物体刚度系数大致为固定数值，通过离散化系统的数值积分计算法可得环境刚度的估计值

并通过式(12)来对已经经过离线补偿后的参考轨迹x_e进行更新以此来得到期望轨迹x_d；

S33.变刚度自适应导纳控制算法的实现，即实现闭环式在线恒力跟踪：

首先提出变刚度导纳控制方法，其微分方程如下所示：

其中，自适应刚度系数k(t)的适时调整由期望接触力控制，k_f和k_v分别表示力跟踪误差控制的比例系数与微分系数；

然后力的跟踪误差方程可由上式导出：

在实现开环式离线补偿功能的基础上，根据上述步骤，首先通过在线估计的方式获取环境刚度的估计值

通过式(13)计算机器人末端工具手的期望轨迹x_d；然后人为设定好目标刚度自适应调整率中的k_f和k_v；最后根据自适应调整率计算变刚度导纳控制器中的目标刚度系数k(t)和通过力传感器采集到的来自工件的作用力f与期望作用力f_d的差值作为变刚度自适应导纳控制器的反馈量来实现作业过程中的闭环式在线恒力跟踪功能。

Images