CN101332604A - 人机相互作用机械臂的控制方法 - Google Patents

Abstract

人机相互作用机械臂的控制方法，它涉及机械臂在未知环境工作过程中的一种安全控制方法，解决了现有机械手在与人类紧密接触工作时不能准确的对工作环境建模而造成对人类产生误伤害的问题。本发明机械臂控制器通过关节传感器实时采集关节位置并利用正运动学将关节位置q转换为笛卡尔位置x；计算具有笛卡尔力反馈的实时轨迹规划x_pg；机械臂控制器通过关节传感器实时采集关节扭矩τ，且通过笛卡尔阻抗控制计算出期望扭矩τ_r；通过电机动态性补偿计算机械臂关节的输入扭矩τ_m。本发明可有效的检测来自机械臂各个关节的力，当接触对象时，机械臂能够柔顺的接触；当碰撞发生时，机械臂能够保证各方向的接触力在期望力范围内，从而保证机械臂和操作对象的安全。

Description

人机相互作用机械臂的控制方法

技术领域

本发明涉及机械臂的一种控制方法。

背景技术

传统的工业机器人大多是在已建模的环境中重复相同的操作任务。如今，越来越多的机器人开始应用于医疗、家庭服务、空间探索等未精确建模环境中。相对于传统机器人，这些机器人并不需要很高的速度，但与人类和环境稳定、柔顺和安全的共存是其首要考虑的问题。

机械臂的安全操作从设计上可以分为三类。一类是结构设计，包括减轻机器人的惯性和重量并引进一些柔顺部件，例如柔性关节，弹性连杆和机械阻尼调节器等，这样机械臂在接触物体时能够保证一定的柔性，但并不能避免与环境发生碰撞而损坏对象，而过于柔性的设计，将使机械臂在跟踪效果和响应速度上不够理想；第二类是采用力矩传感器，如此就能快速检测到机器人本身的受力并控制其与环境的接触力，现行的机械臂大多利用力矩传感器检测到碰撞力后，通过紧急制动保护机器人，然而这种方法对于动态环境并不适宜，当人继续撞击机械臂时，将会对人产生伤害；第三类是采用实时的轨迹规划，传统的机械臂路径规划都是离线计算得到，没有融入当前机械臂的状态，这些规划对于已建模的环境适用，但对于未知环境容易引起机械臂与环境间的碰撞。

发明内容

本发明提供一种人机相互作用机械臂的控制方法，以解决现有机械手在与人类紧密接触工作时不能准确的对工作环境建模而造成对人类产生误伤害的问题。人机相互作用机械臂的控制方法由以下步骤完成：一、通过CAD建模准确计算出机械臂的运动学和动力学参数；二、通过基于位置的轨迹规划给定机械臂的末端期望轨迹x_d、给定机械臂末端的期望接触力F_cd和机械臂的阻抗参数；三、机械臂控制器通过关节传感器实时采集关节的位置q、速度

和加速度

并利用正运动学将关节的位置q、速度

和加速度

转换为笛卡尔位置x、速度

和加速度

且根据笛卡尔阻抗控制公式计算预接触力

四、通过基于力跟踪的轨迹规划计算具有笛卡尔力反馈的实时轨迹规划x_pg；五、机械臂控制器通过关节传感器实时采集关节扭矩τ，且通过笛卡尔阻抗控制计算出期望扭矩τ_r；六、通过电机动态性补偿计算机械臂关节的输入扭矩τ_m。

本发明具有以下有益效果：本发明可有效的检测来自机械臂各个关节的力，当接触对象时，机械臂能够柔顺的接触；当碰撞发生时，机械臂能够保证各方向的接触力在期望力范围内，从而保证机械臂和操作对象的安全，本发明的优点是适用于具有位置和力感知的串联机器人系统，控制结构简单，便于调节，控制精度高，可以进行人机交互的医疗、家庭作业以及深空探测等未知环境领域。

附图说明

图1是机械臂的控制框图，图2是四自由度机械臂在躲避障碍物时的X-Y坐标位置跟踪曲线图，其中实线代表机械臂的实际位置，虚线代表离线期望位置，点滑线代表实时规划的位置，点线代表障碍物，图3是四自由度机械臂在躲避障碍物时的X-Y方向受力曲线图，其中实线代表X方向的受力，虚线代表Y方向的受力。

具体实施方式

具体实施方式一：结合图1说明本实施方式，本实施方式由以下步骤完成：一、通过CAD建模准确计算出机械臂的运动学和动力学参数；二、通过基于位置的轨迹规划给定机械臂的末端期望轨迹x_d、给定机械臂末端的期望接触力F_cd和机械臂的阻抗参数；三、机械臂控制器通过关节传感器实时采集关节的位置q、速度

和加速度

并利用正运动学将关节的位置q、速度

和加速度

转换为笛卡尔位置x、速度

和加速度

且根据笛卡尔阻抗控制公式计算预接触力

四、通过基于力跟踪的轨迹规划计算具有笛卡尔力反馈的实时轨迹规划xpg；五、机械臂控制器通过关节传感器实时采集关节扭矩τ，且通过笛卡尔阻抗控制计算出期望扭矩τ_r；六、通过电机动态性补偿计算机械臂关节的输入扭矩τ_m。机器人运动碰到障碍即可柔顺的接触障碍物，并自动产生新的轨迹x_pg。

具体实施方式二：本实施方式与具体实施方式一的不同点是：本实施方式在步骤三中的笛卡尔阻抗控制公式为：

${\mathrm{\Λ}}_d\stackrel{\·\·}{\tilde{x}}+D_d\stackrel{\·}{\tilde{x}}+K_d\tilde{x}=F_{\mathrm{ext}}---\left(1\right)$

其中， $\tilde{x}=x-x_d$ 是机械臂笛卡尔空间位置误差；x、x_d是实际末端点位置和期望位置；A_d、D_d，和K_d为对称正定阵，分别为机械臂的期望惯性、阻尼和刚度；F_ext是外力分量；笛卡尔速度

和加速度

根据雅克比矩阵求得。

具体实施方式三：本实施方式与具体实施方式一或二的不同点是：本实施方式在步骤五中所述的笛卡尔阻抗控制过程如下：

依据拉格朗日方程，机械臂的位置x和关节扭矩τ之间的动态特性表示为公式(2)：

$\mathrm{\Λ}\left(x\right)\stackrel{\·\·}{x}+\mathrm{\μ}(x,\stackrel{\·}{x})\stackrel{\·}{x}+J{\left(q\right)}^{-T}g\left(q\right)=J{\left(q\right)}^{-T}\mathrm{\τ}+F_{\mathrm{ext}}---\left(2\right)$

其中，J(q)^-Tg(q)为机械臂的重力项；g(q)为关节的重力项；Λ(x)和可通过公式(3)和公式(4)得到；

Λ(x)＝J(q)^-TM(q)J(q)^-1              (3)

$\mathrm{\μ}(x,\stackrel{\·}{x})=J{\left(q\right)}^{-T}(C(q,\stackrel{\·}{q})-M\left(q\right)J{\left(q\right)}^{-1}\stackrel{\·}{J}\left(q\right))J{\left(q\right)}^{-1}---\left(4\right)$

其中，M(q)和

分别代表各关节的惯性矩阵和哥氏力与离心力合力的矩阵；

依据公式(2)和步骤三的笛卡尔阻抗控制公式(1)直接求得笛卡尔阻抗控制器公式，且控制输入F_τ＝J(q)^-Tτ求得如下

$F_{\mathrm{\τ}}=\mathrm{\Λ}\left(x\right){\stackrel{\·\·}{x}}_d+\mathrm{\μ}(x,\stackrel{\·}{x})\stackrel{\·}{x}+J{\left(q\right)}^{-T}g\left(q\right)-$ (5)

$\mathrm{\Λ}\left(x\right){{\mathrm{\Λ}}_d}^{-1}(D_d\stackrel{\·}{\tilde{x}}+K_d\tilde{x})+(\mathrm{\Λ}\left(x\right){{\mathrm{\Λ}}_d}^{-1}-I)F_{\mathrm{ext}}$

将期望惯性Λ_d与机械臂惯性Λ(x)设计成一致，由此上式可变为

$F_{\mathrm{\τ}}=\mathrm{\Λ}\left(x\right){\stackrel{\·\·}{x}}_d+\mathrm{\μ}(x,\stackrel{\·}{x})\stackrel{\·}{x}+J{\left(q\right)}^{-T}g\left(q\right)-D_d\stackrel{\·}{\tilde{x}}-K_d\tilde{x}---\left(6\right)$

如此，机械臂就具有柔顺特性。

具体实施方式四：本实施方式与具体实施方式三的不同点是：本实施方式在步骤四中所述的实时轨迹规划过程如下：

依据步骤三的笛卡尔阻抗控制公式，机械臂末端的预估接触力

可以表示成

$\hat{f}={\mathrm{\Λ}}_d\stackrel{\·\·}{\tilde{x}}+D_d\stackrel{\·}{\tilde{x}}+K_d\tilde{x}---\left(7\right)$

定义恒力值F_cd为期望碰撞力；在T时刻，经过一段时间Δt，满足公式8时表示碰撞发生，

${\∫}_T^{T+\mathrm{\Δt}}\left|\hat{f}\right|\mathrm{dt}\≥{\∫}_T^{T+\mathrm{\Δt}}\left|F_{\mathrm{cd}}\right|\mathrm{dt}---\left(8\right)$

此时，机械臂所受的力矩与F_cd相等，假定x_pg是机械臂末端重新规划的轨迹，则

$F_{\mathrm{cd}}={\mathrm{\Λ}}_d(\stackrel{\·\·}{x}-{\stackrel{\·\·}{x}}_{\mathrm{pg}}{|}_{T+\mathrm{\Δt}})+D_d(\stackrel{\·}{x}-{\stackrel{\·}{x}}_{\mathrm{pg}}{|}_{T+\mathrm{\Δt}})$ (9)

$+K_d(x-x_{\mathrm{pg}}{|}_{T+\mathrm{\Δt}})$

选定

和

分别是基于位置轨迹规划和基于力的运动规划权重系数，所规划的轨迹形式如下

$x_{\mathrm{pg}}=C_1\left(\hat{f}\right)x_d+C_2\left(\hat{f}\right)(\hat{f}-F_{\mathrm{cd}})---\left(10\right)$

将公式(10)代入公式(9)中，可以表示为：

$F_{\mathrm{cd}}={\mathrm{\Λ}}_d(\stackrel{\·\·}{\tilde{x}}+C_2({\stackrel{\·\·}{x}}_d-\stackrel{\·\·}{\hat{f}}))+D_d(\stackrel{\·}{\tilde{x}}+C_2({\stackrel{\·}{x}}_d-\stackrel{\·}{\hat{f}}))$

$+K_d(x+C_2(x_d-\hat{f}+F_{\mathrm{cd}}))$

则权重系数

为

$C_2\left(\hat{f}\right)=\left\{\begin{array}{cc}0& \left|\hat{f}\right|<F_{\mathrm{cd}}\\ \frac{F_{\mathrm{cd}}-\hat{f}}{K_d(x_d-\hat{f}+F_{\mathrm{cd}})+D_d({\stackrel{\&CenterDot;}{x}}_d-\stackrel{\&CenterDot;}{\hat{f}})+{\mathrm{\&Lambda;}}_d({\stackrel{\&CenterDot;\&CenterDot;}{x}}_d-\stackrel{\&CenterDot;\&CenterDot;}{\hat{f}})}& \mathrm{others}\end{array}\right.---\left(11\right)$

将公式(10)取代阻抗方程式(1)的x_d可以推导出

$F_{\mathrm{ext}}=\left\{\begin{array}{cc}{\mathrm{\&Lambda;}}_d\stackrel{\&CenterDot;\&CenterDot;}{\tilde{x}}+D_d\stackrel{\&CenterDot;}{\tilde{x}}+K_d\tilde{x}& \left|\hat{f}\right|<F_{\mathrm{cd}}\\ F_{\mathrm{cd}}& \mathrm{others}\end{array}\right..---\left(12\right)$

由此，我们就可以得知当预估力小于期望力时，机械臂表现的是二阶阻抗特性，当预估力达到期望力时，机械臂将以恒力接触碰撞物，与人手相似，根据操作对象的不同，我们可以调整期望接触力矩F_cd的大小，当碰撞发生时，机械臂能够根据期望力矩随意移动而不损害机械臂和操作对象。

具体实施方式五：本实施方式与具体实施方式四的不同点是：本实施方式中基于笛卡尔力反馈的轨迹规划满足以下几个条件：

a、当碰撞未发生时x_pg＝x_d，即 $C_1\left(\hat{f}\right)=1$ 且 $C_2\left(\hat{f}\right)=0;$

b、

和

只与预估力

有关，且随着

增大，

逐渐减小，

随之增大；

c、当时，为保证末端力仍然维持恒力值F_cd，则 $C_1\left(\hat{f}\right)\&RightArrow;0$ 且 $C_2\left(\hat{f}\right)\&RightArrow;1;$

d、 $C_1\left(\hat{f}\right)\&Element;\left(\mathrm{0,1}\right],$ $C_2\left(\hat{f}\right)\&Element;\left[\mathrm{1,0}\right),$ 且保证x_pg， 均连续；

e、 $C_1\left(\hat{f}\right)+C_2\left(\hat{f}\right)=1.$

具体实施方式六：本实施方式与具体实施方式四的不同点是：本实施方式在步骤六中所述的电机动态补偿过程如下：

将基于笛卡尔力反馈的轨迹规划公式(10)代入阻抗控制器公式(6)中，并根据F_τ＝J(q)^-Tτ可求得关节的所需力矩为

${\mathrm{\&tau;}}_r=J{\left(q\right)}^T(\mathrm{\&Lambda;}\left(x\right){\stackrel{\&CenterDot;\&CenterDot;}{x}}_d-D_d(\stackrel{\&CenterDot;}{x}-{\stackrel{\&CenterDot;}{x}}_{\mathrm{pg}})-K_d(x-x_{\mathrm{pg}}))+g\left(q\right)---\left(13\right)$

考虑电机的动态性能

$B\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}+{\mathrm{\&tau;}}_r+{\mathrm{\&tau;}}_F={\mathrm{\&tau;}}_m---\left(14\right)$

其中，θ代表除以关节减速比的电机角度；K和B分别代表关节刚性和电机的惯性；τ_m、τ_F分别是电机的输入扭矩和摩擦项，

将公式(13)代入公式(14)中可以得到机械臂关节电机的输入扭矩

${\mathrm{\&tau;}}_m={B\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}}_{\mathrm{pg}}+{\mathrm{\&tau;}}_F+{\mathrm{\&tau;}}_r+k_{\mathrm{\&tau;p}}({\mathrm{\&tau;}}_r-\mathrm{\&tau;})---\left(15\right)$

其中，k_τp是对角阵用来补偿关节的哥氏力、离心力以及关节间惯性的耦合。

这样就使得机械臂具有柔顺的接触性能，且能够根据实时路径规划控制机械臂末端的作用力在安全范围内，不对操作器本身和操作环境造成伤害。

具体实施方式七：本实施方式与具体实施方式一的不同点是：本实施方式在步骤一中所述的机械臂的运动学和动力学参数为关节位置、关节质量、电机的惯性和关节的惯性。

具体实施方式八：本实施方式与具体实施方式一的不同点是：本实施方式在步骤二中所述的阻抗参数为机械臂的刚度和阻尼。

具体实施方式九：本实施方式与具体实施方式一的不同点是：本实施方式在步骤三中所述的关节传感器由关节力矩传感器和关节位置传感器组成；基于关节力矩传感器实时测得关节所受力矩，通过关节位置传感器获取关节位置、速度和加速度信息，通过控制器解算，由机械臂关节电机执行运动。

实施例

结合图1、图2和图3说明本实施例，自由度机器人由四个模块化旋转关节构成，关节采用无刷直流电机驱动，谐波减速器(减速比1∶160)作为力矩传动。每个关节都安装有一个电位计和磁编码器用于测量关节和电机的角度；两个霍尔传感器用于测量电机相电流；八个应变片垂直的贴在力矩传感器上组成两个全桥，并将力矩传感器安置在谐波减速器的输出轴上用来测量关节扭矩。

机械臂的安全柔顺控制步骤为：

步骤一、通过CAD建模准确计算出机械臂的运动学和动力学参数，以四自由度机械臂为例，其D-H参数和控制器参数如表1所示：

表1机械臂参数

步骤二、给定机械臂的末端期望轨迹x_d、末端期望碰撞力F_cd、机械臂的阻抗参数。本例中给定末端期望轨迹是如图2所示，在X-Y平面半径为100mm的圆周运动，其阻抗参数和期望碰撞力如表2所示

表2机械臂阻抗参数和期望碰撞力

步骤三、机械臂控制器实时采集关节位置且利用正运动学将其转换为笛卡尔位置x，且根据笛卡尔阻抗控制公式(7)和期望末端轨迹x_d计算预估接触力

步骤四、根据预估接触力

和公式(10)、(11)计算具有笛卡尔力反馈的轨迹规划x_pg。

步骤五、机械臂控制器实时采集扭矩，根据笛卡尔阻抗控制公式(13)和步骤三中得到的机械臂的笛卡尔位置x，计算期望扭矩τ_r。

步骤六、考虑电机的动态性，根据公式(15)计算得到关节的输入扭矩τ_m。

步骤七、当机器人运动碰到障碍时，机械臂不需要切换控制模式，根据上述控制步骤，机器人能柔顺的接触障碍物，且根据预估接触力

自动产生新的轨迹x_pg，实时保证实际接触力小于期望力F_cd且能够继续工作。实时轨迹规划x_pg能够产生环绕障碍物的轨迹，机械臂沿着x_pg做阻抗运动x，从而绕过障碍物且保证末端的接触力在期望力F_cd范围内。

以四自由度机械臂的控制为例，柔顺安全控制方法的时域分析如表3所示，可以看出，响应较快，超调较小，且控制精度很高，能满足工程实际需求。

表3控制方法在四自由度机械臂中应用的时域分析

Claims (9)

1、人机相互作用机械臂的控制方法，其特征在于它由以下步骤完成：一、通过CAD建模准确计算出机械臂的运动学和动力学参数；二、通过基于位置的轨迹规划给定机械臂的末端期望轨迹x_d、给定机械臂末端的期望接触力F_cd和机械臂的阻抗参数；三、机械臂控制器通过关节传感器实时采集关节的位置q、速度和加速度

并利用正运动学将关节的位置q、速度和加速度

转换为笛卡尔位置x、速度和加速度

，且根据笛卡尔阻抗控制公式计算预接触力

四、通过基于力跟踪的轨迹规划计算具有笛卡尔力反馈的实时轨迹规划x_pg；五、机械臂控制器通过关节传感器实时采集关节扭矩τ，且通过笛卡尔阻抗控制计算出期望扭矩τ_r；六、通过电机动态性补偿计算机械臂关节的输入扭矩τ_m。

2、根据权利要求1所述的人机相互作用机械臂的控制方法，其特征在于在步骤三中所述的笛卡尔阻抗控制公式为：

${\mathrm{\&Lambda;}}_d\stackrel{\stackrel{\&CenterDot;\&CenterDot;}{~}}{x}+D_d\stackrel{\stackrel{\&CenterDot;}{~}}{x}+K_d\tilde{x}=F_{\mathrm{ext}}---\left(1\right)$

其中， $\tilde{x}=x-x_d$ 是机械臂笛卡尔空间位置误差；x、x_d是实际末端点位置和期望位置；Λ_d、D_d，和K_d为对称正定阵，分别为机械臂的期望惯性、阻尼和刚度；F_ext是外力分量；笛卡尔速度

和加速度根据雅克比矩阵求得。

3、根据权利要求2所述的人机相互作用机械臂的控制方法，其特征在于在步骤五中所述的笛卡尔阻抗控制过程如下：

依据拉格朗日方程，机械臂的位置x和关节扭矩τ之间的动态特性表示为公式(2)：

$\mathrm{\&Lambda;}\left(x\right)\stackrel{\&CenterDot;\&CenterDot;}{x}+\mathrm{\&mu;}(x,\stackrel{\&CenterDot;}{x})\stackrel{\&CenterDot;}{x}+J{\left(q\right)}^{-T}g\left(q\right)=J{\left(q\right)}^{-T}\mathrm{\&tau;}+F_{\mathrm{ext}}---\left(2\right)$

其中，J(q)^-Tg(q)为机械臂的重力项；g(q)为关节的重力项；Λ(x)和可通过公式(3)和公式(4)得到；

Λ(x)＝J(q)^-TM(q)J(q)^-1                   (3)

$\mathrm{\&mu;}(x,\stackrel{\&CenterDot;}{x})=J{\left(q\right)}^{-T}(C(q,\stackrel{\&CenterDot;}{q})-M\left(q\right)J{\left(q\right)}^{-1}\stackrel{\&CenterDot;}{J}\left(q\right))J{\left(q\right)}^{-1}---\left(4\right)$

其中，M(q)和分别代表各关节的惯性矩阵和哥氏力与离心力合力的矩阵；

依据公式(2)和步骤三的笛卡尔阻抗控制公式(1)直接求得笛卡尔阻抗控制器公式，且控制输入F_τ＝J(q)^-Tτ求得如下

$F_{\mathrm{\&tau;}}=\mathrm{\&Lambda;}\left(x\right){\stackrel{\&CenterDot;\&CenterDot;}{x}}_d+\mathrm{\&mu;}(x,\stackrel{\&CenterDot;}{x})\stackrel{\&CenterDot;}{x}+J{\left(q\right)}^{-T}g\left(q\right)-$

$\mathrm{\&Lambda;}\left(x\right){{\mathrm{\&Lambda;}}_d}^{-1}(D_d\stackrel{\stackrel{\&CenterDot;}{~}}{x}+K_d\tilde{x})+(\mathrm{\&Lambda;}\left(x\right){{\mathrm{\&Lambda;}}_d}^{-1}-I)F_{\mathrm{ext}}---\left(5\right)$

将期望惯性Λ_d与机械臂惯性Λ(x)设计成一致，由此笛卡尔阻抗控制器公式可变为

$F_{\mathrm{\&tau;}}=\mathrm{\&Lambda;}\left(x\right){\stackrel{\&CenterDot;\&CenterDot;}{x}}_d+\mathrm{\&mu;}(x,\stackrel{\&CenterDot;}{x})\stackrel{\&CenterDot;}{x}+J{\left(q\right)}^{-T}g\left(q\right)-D_d\stackrel{\stackrel{\&CenterDot;}{~}}{x}-K_d\tilde{x}.---\left(6\right)$

4、根据权利要求3所述的人机相互作用机械臂的控制方法，其特征在于在步骤四中所述的实时轨迹规划过程如下：

依据步骤三的笛卡尔阻抗控制公式，机械臂末端的预估接触力

可以表示成

$\hat{f}={\mathrm{\&Lambda;}}_d\stackrel{\stackrel{\&CenterDot;\&CenterDot;}{~}}{x}+D_d\stackrel{\stackrel{\&CenterDot;}{~}}{x}+K_d\tilde{x}---\left(7\right)$

定义恒力值F_cd为期望碰撞力；在T时刻，经过一段时间Δt，满足公式(8)时表示碰撞发生，

${\&Integral;}_T^{T+\mathrm{\&Delta;t}}\left|\hat{f}\right|\mathrm{dt}\&GreaterEqual;{\&Integral;}_T^{T+\mathrm{\&Delta;t}}\left|F_{\mathrm{cd}}\right|\mathrm{dt}---\left(8\right)$

此时，机械臂所受的力矩与F_cd相等，假定x_pg是机械臂末端重新规划的轨迹，则

$F_{\mathrm{cd}}={\mathrm{\&Lambda;}}_d(\stackrel{\&CenterDot;\&CenterDot;}{x}-{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{\mathrm{pg}}{|}_{T+\mathrm{\&Delta;t}})+D_d(\stackrel{\&CenterDot;}{x}-{\stackrel{\&CenterDot;}{x}}_{\mathrm{pg}}{|}_{T+\mathrm{\&Delta;t}})$

$+K_d(x-x_{\mathrm{pg}}{|}_{T+\mathrm{\&Delta;t}})---\left(9\right)$

选定

和

分别是基于位置轨迹规划和基于力的运动规划权重系数，所规划的轨迹形式如下

$x_{\mathrm{pg}}=C_1\left(\hat{f}\right)x_d+C_2\left(\hat{f}\right)(\hat{f}-F_{\mathrm{cd}})---\left(10\right)$

将公式(10)代入公式(9)中，可以表示为：

$F_{\mathrm{cd}}={\mathrm{\&Lambda;}}_d(\stackrel{\stackrel{\&CenterDot;\&CenterDot;}{~}}{x}+C_2({\stackrel{\&CenterDot;\&CenterDot;}{x}}_d-\stackrel{\stackrel{\&CenterDot;\&CenterDot;}{^}}{f}))+D_d(\stackrel{\stackrel{\&CenterDot;}{~}}{x}+C_2({\stackrel{\&CenterDot;}{x}}_d-\stackrel{\stackrel{\&CenterDot;}{^}}{f}))$

$+K_d(x+C_2(x_d-\hat{f}+F_{\mathrm{cd}}))$

则权重系数为

$C_2\left(\hat{f}\right)=\left\{\begin{array}{cc}0& \left|\hat{f}\right|<F_{\mathrm{cd}}\\ \frac{F_{\mathrm{cd}}-\hat{f}}{K_d(x_d-\hat{f}+F_{\mathrm{cd}})+D_d({\stackrel{\&CenterDot;}{x}}_d-\stackrel{\stackrel{\&CenterDot;}{^}}{f})+{\mathrm{\&Lambda;}}_d({\stackrel{\&CenterDot;\&CenterDot;}{x}}_d-\stackrel{\stackrel{\&CenterDot;\&CenterDot;}{^}}{f})}& \mathrm{others}\end{array}\right.---\left(11\right)$

将公式(10)取代阻抗方程式(1)的x_d可以推导出

$F_{\mathrm{ext}}=\left\{\begin{array}{cc}{\mathrm{\&Lambda;}}_d\stackrel{\stackrel{\&CenterDot;\&CenterDot;}{~}}{x}+D_d\stackrel{\stackrel{\&CenterDot;}{~}}{x}+K_d\tilde{x}& \left|\hat{f}\right|<F_{\mathrm{cd}}\\ F_{\mathrm{cd}}& \mathrm{others}\end{array}\right..---\left(12\right)$

5、根据权利要求4所述的人机相互作用机械臂的控制方法，其特征在于基于笛卡尔力反馈的轨迹规划满足以下几个条件：

a、未发生时x_pg＝x_d，即 $C_1\left(\hat{f}\right)=1$ 且 $C_2\left(\hat{f}\right)=0;$

b、

和

只与预估力

有关，且随着

增大，逐渐减小，

随之增大；

c、当 $\hat{f}\&RightArrow;\&infin;$ 时，为保证末端力仍然维持恒力值F_cd，则 $C_1\left(\hat{f}\right)\&RightArrow;0$ 且 $C_2\left(\hat{f}\right)\&RightArrow;1;$

d、 $C_1\left(\hat{f}\right)\&Element;\left(\mathrm{0,1}\right],C_2\left(\hat{f}\right)\&Element;\left[\mathrm{1,0}\right),$ 且保证x_pg，均连续；

e、 $C_1\left(\hat{f}\right)+C_2\left(\hat{f}\right)=1.$

6、根据权利要求4所述的人机相互作用机械臂的控制方法，其特征在于在步骤六中所述的电机动态补偿过程如下：

将基于笛卡尔力反馈的轨迹规划公式(10)代入阻抗控制器公式(6)中，并根据F_τ＝J(q)^-Tτ可求得关节的所需力矩为

${\mathrm{\&tau;}}_r=J{\left(q\right)}^T(\mathrm{\&Lambda;}\left(x\right){\stackrel{\&CenterDot;\&CenterDot;}{x}}_d-D_d(\stackrel{\&CenterDot;}{x}-{\stackrel{\&CenterDot;}{x}}_{\mathrm{pg}})-K_d(x-x_{\mathrm{pg}}))+g\left(q\right)---\left(13\right)$

考虑电机的动态性能

$B\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}+{\mathrm{\&tau;}}_r+{\mathrm{\&tau;}}_F={\mathrm{\&tau;}}_m---\left(14\right)$

其中，θ代表除以关节减速比的电机角度；K和B分别代表关节刚性和电机的惯性；τ_m、τ_F分别是电机的输入扭矩和摩擦项，

将公式(13)代入公式(14)中可以得到机械臂关节电机的输入扭矩

${\mathrm{\&tau;}}_m=B{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}}_{\mathrm{pg}}+{\mathrm{\&tau;}}_F+{\mathrm{\&tau;}}_r+k_{\mathrm{\&tau;p}}({\mathrm{\&tau;}}_r-\mathrm{\&tau;})---\left(15\right)$

其中，k_τp是对角阵用来补偿关节的哥氏力与离心力的合力以及关节间惯性的耦合。

7、根据权利要求1所述的人机相互作用机械臂的控制方法，其特征在于在步骤一中所述机械臂的运动学和动力学参数为关节位置、关节质量、电机的惯性和关节的惯性。

8、根据权利要求1所述的人机相互作用机械臂的控制方法，其特征在于在步骤二中所述的阻抗参数为机械臂的刚度和阻尼。

9、根据权利要求1所述的人机相互作用机械臂的控制方法，其特征在于在步骤三中所述的关节传感器由关节力矩传感器和关节位置传感器组成。

Images