[发明专利]人机相互作用机械臂的控制方法

权利要求书

1.人机相互作用机械臂的控制方法，其特征在于它由以下步骤完成：

一、通过CAD建模准确计算出机械臂的运动学和动力学参数；

二、通过基于位置的轨迹规划给定机械臂的末端点笛卡尔期望位置x_d、给定机械臂末端的期望接触力F_cd和机械臂的阻抗参数；

三、机械臂控制器通过关节传感器实时采集关节的位置q、速度和加速度，并利用正运动学将关节的位置q、速度和加速度转换为末端点笛卡尔实际位置x、笛卡尔速度和加速度，且根据笛卡尔阻抗控制公式计算预接触力；所述的笛卡尔阻抗控制公式为：

Λdx~··+Ddx~·+Kdx~=Fext---(1)]]>

其中，是机械臂笛卡尔空间位置误差；x、x_d分别是末端点笛卡尔实际位置和末端点笛卡尔期望位置；Λ_d、D_d，和K_d为对称正定阵，分别为机械臂的期望惯性、阻尼和刚度；F_ext是外力分量；笛卡尔速度和加速度根据雅克比矩阵求得；

四、通过基于力跟踪的轨迹规划计算具有笛卡尔力反馈的实时轨迹规划x_pg；所述的实时轨迹规划过程如下：

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

f^=Λdx~··+Ddx~·+Kdx~---(2)]]>

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

∫TT+Δt|f^|dt≥∫TT+Δt|Fcd|dt---(3)]]>

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

Fcd=Λd(x··-x··pg|T+Δt)+Dd(x·-x·pg|T+Δt)]]>

+Kd(x-xpg|T+Δt)---(4)]]>

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

xpg=C1(f^)xd+C2(f^)(f^-Fcd)---(5)]]>

将公式(5)代入公式(4)中，可以表示为：

Fcd=Λd(x~··+C2(x··d-f^··))+Dd(x~·+C2(x·d-f^·))]]>

+Kd(x+C2(xd-f^+Fcd))]]>

则权重系数为

将公式(5)取代阻抗控制公式(1)的x_d可以推导出

五、机械臂控制器通过关节传感器实时采集关节扭矩τ，且通过笛卡尔阻抗控制计算出期望扭矩τ_r；所述的笛卡尔阻抗控制过程如下：

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

Λ(x)x··+μ(x,x·)x·+J(q)-Tg(q)=J(q)-Tτ+Fext---(8)]]>

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

Λ(x)＝J(q)^-T M(q)J(q)^-1        (9)

μ(x,x·)=J(q)-T(C(q,q·)-M(q)J(q)-1J·(q))J(q)-1---(10)]]>

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

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

Fτ=Λ(x)x··d+μ(x,x·)x·+J(q)-Tg(q)-]]>

Λ(x)Λd-1(Ddx~·+Kdx~)+(Λ(x)Λd-1-I)Fext---(11)]]>

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

Fτ=Λ(x)x··d+μ(x,x·)x·+J(q)-Tg(q)-Ddx~·-Kdx~;---(12)]]>

六、通过电机动态性补偿计算机械臂关节的输入扭矩τ_m；所述的电机动态补偿过程如下：

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

τr=J(q)T(Λ(x)x··d-Dd(x·-x·pg)-Kd(x-xpg))+g(q)---(13)]]>

考虑电机的动态性能

Bθ··+τr+τF=τm---(14)]]>

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

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

τm=Bθ··pg+τF+τr+kτp(τr-τ)---(15)]]>

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