[发明专利]一种主从式机器人的离合控制方法

权利要求书

1.一种主从式机器人的离合控制方法，其特征在于：包括步骤：

(1)根据主操作手和从臂器械的姿态可达空间划分主操作手的工作区、过渡区和离合区；将从臂器械的姿态可达空间设为工作区，将主操作手的姿态可达空间内除从臂器械的姿态可达空间以外的区域中靠近工作区的部分区域设为过渡区，远离工作区的部分区域设为离合区；

(2)实时获取所述主操作手姿态，在所述主操作手离开所述工作区时断开主从连接，在所述主操作手进入所述工作区时匹配主从连接；

其中，在所述主操作手从所述工作区进入所述过渡区时断开主从连接，并通过第一阻抗控制器规划出所述主操作手到达离合区的最短路径和目标姿态并沿着最短路径形成力矩通道；

在所述主操作手从所述离合区进入所述过渡区时，通过第二阻抗控制器规划出所述主操作手运动到离开工作区时的原始姿态的最短路径并沿着最短路径形成力矩通道。

2.根据权利要求1所述的主从式机器人的离合控制方法，其特征在于：当操作者识别得到主操作手的位姿处于病态位姿时，拖动主操作手从工作区向所述离合区运动，并在所述主操作手进入所述离合区后调整所述主操作手的姿态。

3.根据权利要求1所述的主从式机器人的离合控制方法，其特征在于：所述第一阻抗控制器规划出所述主操作手到达离合区的最短路径和目标姿态具体为：

所述主操作手处于所述工作区与所述过渡区的边界时，记录当前所述主操作手各关节的位置；据此通过正向运动学计算得到当前主操作手的姿态矩阵R₀；

根据当前的主操作手姿态矩阵R₀的轴角形式计算得到所述离合区内距离当前的主操作手姿态矩阵R₀最近的目标姿态矩阵R₁对应的轴角形式，通过逆推计算得到目标姿态矩阵R₁；

将当前的主操作手姿态矩阵R₀和目标姿态矩阵R₁分别表示为四元数形式，对四元数在当前姿态和目标姿态之间以主操作手末端运动形成的球面进行线性插值得到最短路径。

4.根据权利要求1所述的主从式机器人的离合控制方法，其特征在于：所述第二阻抗控制器规划出所述主操作手运动到离开工作区时的原始姿态的最短路径具体为：

所述主操作手处于所述离合区与所述过渡区的边界时，记录当前所述主操作手各关节的位置；据此通过正向运动学计算得到主操作手的当前姿态矩阵R₂；

将主操作手的当前姿态矩阵R₂和所述主操作手离开工作区时的原始姿态矩阵R₀分别表示为四元数形式，对四元数在当前姿态和原始姿态之间以主操作手末端运动形成的球面进行线性插值得到最短路径。

5.根据权利要求1所述的主从式机器人的离合控制方法，其特征在于：操作者在所述过渡区拖动主操作手进入离合区或工作区的过程中若偏离对应的所述最短路径，则根据相应的阻抗控制器计算得到的对应的最短路径计算得到纠正力F＝k*sign(q_t-q_a)*|q_t-q_a|，使得主操作手沿着计算得到的最短路径运动；其中，k表示调参系数，q_a表示t时刻偏离对应的最短路径后的当前位置，q_t表示对应t时刻在对应的最短路径上的目标位置。

6.根据权利要求1所述的主从式机器人的离合控制方法，其特征在于：所述主操作手和从臂器械的姿态可达空间分别表示为M_w和I_w，则所述工作区为W_z＝M_w∩I_w，所述过渡区为T_z＝(M_w+I_w)/2-W_z，所述离合区为C_z＝M_w-T_z。

7.根据权利要求1所述的主从式机器人的离合控制方法，其特征在于：所述主操作手和从臂器械的姿态可达空间根据主操作手和从臂器械的各个姿态关节的关节限位通过正运动学计算获得。