CN116276995B - 一种具有零空间避障能力的基于阻抗控制的人机协同方法 - Google Patents

Abstract

一种具有零空间避障能力的基于阻抗控制的人机协同方法，包括：(1)指数坐标六自由度阻抗控制，(2)阻抗控制模型应用于人机协同的安全控制方法及(3)零空间避障。通过阻抗控制的弹簧‑阻尼‑刚度模型构建机器人运动过程中位置与力的相对关系，实现人手对于机械臂的拖动效果和虚拟约束效果。通过实时感知安全作用力，以避免在协同操作时由于惯性过大导致损伤。通过对七自由度机械臂零空间避障控制的研究，使机械臂可在末端位姿不变的情况下随人的作用力改变其空间构型，避免与周围操作人员发生干涉，以提高机器人对环境的适应性。

Description

一种具有零空间避障能力的基于阻抗控制的人机协同方法

技术领域

本发明涉及人机协同机器人技术领域，尤其涉及一种具有零空间避障能力的基于虚拟六维弹簧的人机协同方法。

背景技术

机器人辅助或代替人进行复杂操作已成为重要趋势。如骨科手术机器人因其高精度的定位和稳定性，在创伤骨科、关节、脊柱和颌面外科都有广泛的应用，但机器人辅助骨科安全操作仍是骨科机器人技术不断追求的目标。又如工业装配机器人代替工人进行重复沉重的装配工作，但目前其应用仅限于纯机械环境，且由于仅由位置控制而无法实现对于加工误差的适应性(如拧螺丝的过程)。因此在人机协同交互过程中，若机器人只具备位置运动能力，而无相关接触力信息，则无法保障交互中操作人员和患者的安全性。将力信息融入机器人控制系统，实现机器人柔顺控制成为重要的研究方向。

柔顺控制分为被动柔顺控制与主动柔顺控制，从控制效果上看好像有一个弹簧连接在机器人末端和与其交互的物体上，从而调控机器人和环境的交互作用力。被动柔顺控制指通过改变机构的弹性(如在末端加入弹簧等)方式实现柔顺；而主动柔顺指通过控制策略实现机器人对力的主动响应，虚拟出一个弹簧-质点-阻尼的效果。被动柔顺方式难以实现精确控制，并不适用于位置较高要求的机器人(例如骨科机器人)的控制。关于主动柔顺控制，主要包括导纳控制、阻抗控制和力位混合控制。其中阻抗控制更适用于与刚性环境接触的情况，因此本发明采用六自由度阻抗控制技术，通过虚拟出参数可调的六维弹簧，实现人机协同控制方法。通过引入机器人零空间避障技术，提高人机协同操作的临床适用性和安全性。

发明内容

本发明首先进行了指数坐标的六自由度阻抗控制模型建立，这种模型能够应用于机器人(例如，骨科机器人)人机交互过程中的安全领域，提出了一种基于阻抗控制的人机协同方法，通过引入零空间避障研究，进一步提高机器人人机协同的适应性。即通过阻抗控制的弹簧-阻尼-刚度模型构建机器人运动过程中位置与力的相对关系，实现人手对于机械臂的拖动效果和虚拟约束效果。同时通过实时感知安全作用力，以避免在协同操作时由于惯性过大导致损伤。通过对七自由度机械臂零空间避障控制的研究，使机械臂可在末端位姿不变的情况下随人的作用力改变其空间构型，避免与周围操作人员发生干涉，以提高机器人对环境的适应性。

本发明的目的在于提供一种具有零空间避障能力的基于阻抗控制的人机协同方法，所述具有零空间避障能力的基于阻抗控制的人机协同方法包括：(1)指数坐标六自由度阻抗控制，(2)阻抗控制模型应用于人机协同的安全控制方法及(3)零空间避障。

根据本发明的一种实施方式，例如，(1)指数坐标六自由度阻抗控制包括：

阻抗控制是输入位置输出力的控制系统，通过阻抗控制器将机器人实际位姿X与规划位姿X_d的差值X_e转换为机器人控制力矩τ_imp，与环境作用力τ_en相加得机器人控制外部力矩τ_ext，作用于机器人以控制其运动；

其中六自由度阻抗控制模型为质量-弹簧-阻尼模型；

质量-弹簧-阻尼模型公式表示为：

其中，X_e为实际位姿与规划位姿之差；对于六自由度阻抗控制，将位置和姿态解耦讨论，以旋转矩阵R_d,R分别代表规划姿态和实际姿态，t_d,t分别代表规划位置和实际位置，其中R,t由机械臂的正运动学求得；由于外力的存在使得规划位姿与实际位姿存在目标差值即r_e,p_e；

解耦后的六自由度阻抗控制公式为：

其中M_r,B_r,K_r,M_p,B_p,K_p为相应控制参数，F_r,F_p为与r_e,p_e相同坐标系下的阻抗控制输出力矩和力，分别由角度误差和位置误差产生；

选用指数坐标定义r_e,p_e，即：

p_e＝R_d ^-1(t-t_d)

其中e^[·]代表矩阵的指数运算，r_e,p_e均定义在规划坐标系{T_d}下；等式两边求导得一阶时间导数与/>

如图3所示，ω,ω_d,v,v_d分别表示当前末端角速度，规划角速度，线速度及规划线速度在空间坐标系{s}下的表达；A(·)为计算符，计算公式为：

且[·]表示斜对称矩阵，p¹表示向量p中第一个元素，以此类推；

机器人的动力学方程，即

其中q为机器人关节角，M(q)为惯性矩阵，为科氏力/向心力矩阵，g(q)为重力力矩，τ_ext为外部施加力矩；

当选取上述指数坐标定义误差量时，将关节空间动力学方程转换到误差空间，定义误差：

由求导链式法则得X_e(q,t)对时间的一阶及二阶导数为：

定义

为随体坐标系{b}的雅克比矩阵；

将上式代入关节动力学可得

其中

Λ(q,t)＝M(q)J_x(q,t)^-1

根据阻抗控制公式可得外力F_ext

代入求得阻抗控制机器人所需力矩τ_imp；至此即完成六自由度指数坐标阻抗控制的控制逻辑推导。

根据本发明的一种实施方式，例如，(2)人机协同截断的安全控制方法包括：

通过阻抗控制构建作用力与机器人位置间的相对关系，实现人与机器人的协同交互，即选取合适参数的情况下机器人将随人的作用力进行移动，以实现人机协同的过程；

除实现人机协同拖动功能外，在切削截断过程中，为防止操作人员截至下端点时由于突然截断惯性过大导致非必要损伤，选定一个安全位置为阻抗零点，通过上述阻抗控制实现质量-弹簧-阻尼的反馈力，即操作人员操作机器人离虚拟零点越远，受到的回拉力越大，以保证操作过程中的安全性；

其中相关控制参数M,D,K根据实际体验进行调节，以获得最大的舒适度；同时虚拟零点随操作的进行不断调整，在保证不影响正常切削的情况下实现对安全性的进一步保证。

根据本发明的一种实施方式，例如，(3)零空间避障包括：

冗余机械臂本身的自由度大于末端位姿自由度，多出来的冗余自由度产生零空间，通过将关节阻抗向零空间映射产生零空间阻抗，从而实现零空间内的自由拖动效果。

根据本发明的一种实施方式，例如，(3)零空间避障进一步包括：

关节阻抗控制公式：

在关节阻抗中选用弹簧阻尼模型，忽略阻抗控制中的质量项；其中B_q,K_q为对应阻尼及刚度系数，q为当前机械臂关节角，q₀为虚拟零点关节角位置；

将关节扭矩映射至零空间，映射系数P由下式求得：

其中I为单位矩阵，J_x(q,t)与M(q)为(1)中定义的雅克比矩阵与惯性矩阵；

零空间阻抗控制：

τ_null＝Pτ_q

将此前算得末端笛卡尔空间的控制力矩τ_imp与零空间阻抗控制力矩τ_null相加：

τ＝τ_imp+τ_null

以控制机械臂关节扭矩。

附图说明

图1为阻抗控制示意图。

图2为质量-弹簧-阻尼模型示意图。

图3为规划位姿与实际位姿存在目标差值即r_e,p_e的相关坐标系表达。

图4为机器人人机协同控制效果图。

图5为人机协同安全控制示意图。

图6为零空间避障效果图。

图7为总体控制流程示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。但本领域技术人员知晓，本发明并不局限于附图和以下实施例。

本发明的实施例提供一种具有零空间避障能力的基于阻抗控制的人机协同方法，主要包括：(1)指数坐标六自由度阻抗控制，(2)阻抗控制模型应用于人机协同的安全控制方法及(3)零空间避障三个方面。

(1)指数坐标六自由度阻抗控制

阻抗控制是输入位置输出力的控制系统，如图1所示，通过阻抗控制器将机器人实际位姿X与规划位姿X_d的差值X_e转换为机器人控制力矩τ_imp，与环境作用力τ_en相加得机器人控制外部力矩τ_ext，作用于机器人以控制其运动。

其中六自由度阻抗控制模型为质量-弹簧-阻尼模型，如图2所示。

质量-弹簧-阻尼模型公式表示为：

其中，X_e为实际位姿与规划位姿之差。对于六自由度阻抗控制，将位置和姿态解耦讨论，以旋转矩阵R_d,R分别代表规划姿态和实际姿态，t_d,t分别代表规划位置和实际位置，其中R,t由机械臂的正运动学求得。由于外力的存在使得规划位姿与实际位姿存在目标差值即r_e,p_e，如图3所示。

解耦后的六自由度阻抗控制公式为：

其中M_r,B_r,K_r,M_p,B_p,K_p为相应控制参数，F_r,F_p为与r_e,p_e相同坐标系下的阻抗控制输出力矩和力，分别由角度误差和位置误差产生。

选用指数坐标定义r_e,p_e，即：

p_e＝R_d ^-1(t-t_d)

其中e^[·]代表矩阵的指数运算，r_e,p_e均定义在规划坐标系{T_d}下。等式两边求导得一阶时间导数与/>

如图3所示，ω,ω_d,v,v_d分别表示当前末端角速度，规划角速度，线速度及规划线速度在空间坐标系{s}下的表达。A(·)为计算符，计算公式为：

且[·]表示斜对称矩阵，p¹表示向量p中第一个元素，以此类推。

机器人的动力学方程，即

其中q为机器人关节角，M(q)为惯性矩阵，为科氏力/向心力矩阵，g(q)为重力力矩，τ_ext为外部施加力矩(无环境作用力时即为阻抗控制力矩)。

当选取上述指数坐标定义误差量时，将关节空间动力学方程转换到误差空间，定义误差：

由求导链式法则可得X_e(q,t)对时间的一阶及二阶导数为：

定义

为随体坐标系{b}的雅克比矩阵。

将上式代入关节动力学可得

其中

Λ(q,t)＝M(q)J_x(q,t)^-1

根据阻抗控制公式可得外力F_ext

代入求得阻抗控制机器人所需力矩τ_imp。至此即完成六自由度指数坐标阻抗控制的控制逻辑推导。

(2)人机协同截断的安全控制方法

为保证操作过程的安全性，以防止在实际应用中由于接触物的突然截断等情况导致惯性过大造成损伤，因此利用(1)中笛卡尔空间阻抗控制设计了人机协同截断安全控制方法。

首先通过阻抗控制构建作用力与机器人位置间的相对关系，实现人与机器人的协同交互，即选取合适参数的情况下机器人将随人的作用力进行移动，以实现人机协同的过程，实现效果如图4。

除实现人机协同拖动功能外，在如切削截断等过程中，为防止操作人员截至下端点时由于突然截断惯性过大导致非必要损伤，设计选定一个安全位置为阻抗零点，通过上述阻抗控制实现质量-弹簧-阻尼的反馈力，即操作人员操作机器人离虚拟零点越远，受到的回拉力越大，以保证操作过程中的安全性。其中相关控制参数M,D,K可根据实际体验进行调节，以获得最大的舒适度。同时虚拟零点可随操作的进行不断调整，在保证不影响正常切削的情况下实现对安全性的进一步保证。从使用效果上看，在操作过程中像是有一个虚拟的六维弹簧在约束着切削的路径，使工具无法偏离设计路径，同时沿着截断方向，有个回拽的拉力，防止在截断时发生过冲，实现效果如图5所示。

(3)零空间避障

在实际机器人操作过程中，容易出现由于机械臂构型导致的挤占空间或与操作者“打架”等情况，因此，为了进一步提高人机协同的舒适性，针对提出了基于冗余机械臂的零空间避障方法，效果如图6所示。即在保证末端手术器械位姿不变的情况下，机械臂的构型随人手推动产生相应改变，可极大地提高人机交互的舒适性和操作者的体验感。

具体实现方法如下：

由于冗余机械臂本身的自由度(如7自由度机械臂)大于末端位姿自由度(6自由度)，多出来的冗余自由度产生零空间(即在此空间内的运动不影响末端位姿)。因此可通过将关节阻抗向零空间映射产生零空间阻抗，从而实现零空间内的自由拖动效果。

关节阻抗控制公式：

在关节阻抗中选用弹簧阻尼模型，忽略前文阻抗控制中的质量项。其中B_q,K_q为对应阻尼及刚度系数，q为当前机械臂关节角，q₀为虚拟零点关节角位置。

将关节扭矩映射至零空间，映射系数P由下式求得：

其中I为单位矩阵，J_x(q,t)与M(q)为(1)中定义的雅克比矩阵与惯性矩阵。

零空间阻抗控制：

τ_null＝Pτ_q

前文算得末端笛卡尔空间的控制力矩τ_imp与零空间阻抗控制力矩τ_null相加：

τ＝τ_imp+τ_null

以控制机械臂关节扭矩，即实现了本发明实施例所述的具有零空间避障能力的基于阻抗控制的人机协同方法，控制流程如图7所示。

Claims (1)

1.一种具有零空间避障能力的基于阻抗控制的人机协同方法，其特征在于，所述具有零空间避障能力的基于阻抗控制的人机协同方法包括：(1)指数坐标六自由度阻抗控制，(2)阻抗控制模型应用于人机协同的安全控制方法及(3)零空间避障；

(1)指数坐标六自由度阻抗控制包括：

阻抗控制是输入位置输出力的控制系统，通过阻抗控制器将机器人实际位姿X与规划位姿X_d的差值X_e转换为机器人控制力矩τ_imp，与环境作用力τ_en相加得机器人控制外部力矩τ_ext，作用于机器人以控制其运动；

其中六自由度阻抗控制模型为质量-弹簧-阻尼模型；

质量-弹簧-阻尼模型公式表示为：

其中，X_e为实际位姿与规划位姿之差；对于六自由度阻抗控制，将位置和姿态解耦讨论，以旋转矩阵R_d,R分别代表规划姿态和实际姿态，t_d,t分别代表规划位置和实际位置，其中R,t由机械臂的正运动学求得；由于外力的存在使得规划位姿与实际位姿存在目标差值即r_e,p_e；

解耦后的六自由度阻抗控制公式为：

其中M_r,B_r,K_r,M_p,B_p,K_p为相应控制参数，F_r,F_p为与r_e,p_e相同坐标系下的阻抗控制输出力矩和力，分别由角度误差和位置误差产生；

选用指数坐标定义r_e,p_e，即：

p_e＝R_d ^-1(t-t_d)

其中e^[·]代表矩阵的指数运算，r_e,p_e均定义在规划坐标系{T_d}下；等式两边求导得一阶时间导数与/>

ω,ω_d,v,v_d分别表示当前末端角速度，规划角速度，线速度及规划线速度在空间坐标系{s}下的表达；A(·)为计算符，计算公式为：

且[·]表示斜对称矩阵，p¹表示向量p中第一个元素，以此类推；

机器人的动力学方程，即

其中q为机器人关节角，M(q)为惯性矩阵，为科氏力/向心力矩阵，g(q)为重力力矩，τ_ext为外部施加力矩；

当选取上述指数坐标定义误差量时，将关节空间动力学方程转换到误差空间，定义误差：

由求导链式法则得X_e(q,t)对时间的一阶及二阶导数为：

定义

为随体坐标系{b}的雅克比矩阵；

将上式代入关节动力学可得

其中

Λ(q,t)＝M(q)J_x(q,t)^-1

根据阻抗控制公式可得外力F_ext

代入求得阻抗控制机器人所需力矩τ_imp；至此即完成六自由度指数坐标阻抗控制的控制逻辑推导；

(2)阻抗控制模型应用于人机协同的安全控制方法包括：

通过阻抗控制构建作用力与机器人位置间的相对关系，实现人与机器人的协同交互，即选取合适参数的情况下机器人将随人的作用力进行移动，以实现人机协同的过程；

除实现人机协同拖动功能外，在切削截断过程中，为防止操作人员截至下端点时由于突然截断惯性过大导致非必要损伤，选定一个安全位置为阻抗零点，通过上述阻抗控制实现质量-弹簧-阻尼的反馈力，即操作人员操作机器人离虚拟零点越远，受到的回拉力越大，以保证操作过程中的安全性；

其中相关控制参数M,D,K根据实际体验进行调节，以获得最大的舒适度；同时虚拟零点随操作的进行不断调整，在保证不影响正常切削的情况下实现对安全性的进一步保证；

(3)零空间避障包括：

冗余机械臂本身的自由度大于末端位姿自由度，多出来的冗余自由度产生零空间，通过将关节阻抗向零空间映射产生零空间阻抗，从而实现零空间内的自由拖动效果；

零空间避障进一步包括：

关节阻抗控制公式：

在关节阻抗中选用弹簧阻尼模型，忽略阻抗控制中的质量项；其中B_q,K_q为对应阻尼及刚度系数，q为当前机械臂关节角，q₀为虚拟零点关节角位置；

将关节扭矩映射至零空间，映射系数P由下式求得：

其中I为单位矩阵，J_x(q,t)与M(q)为(1)中定义的雅克比矩阵与惯性矩阵；

零空间阻抗控制：

τ_null＝Pτ_q

将此前算得末端笛卡尔空间的控制力矩τ_imp与零空间阻抗控制力矩τ_null相加：

τ＝τ_imp+τ_null

以控制机械臂关节扭矩。