[发明专利]手术机械臂自动定位寻路方法、装置、机器人及存储介质

说明书

本发明实施例公开了手术机械臂自动定位寻路方法、装置、机器人及存储介质，该方法包括：获取机械臂的当前位姿数据以及手术区域的位置数据；根据所述当前位姿数据和所述位置数据计算最短路径；构建对应的雅克比矩阵，并基于所述最短路径和当前位姿数据对所述雅克比矩阵求解，若所述雅克比矩阵为满秩，则求解得到所述机械臂的各个关节角度；根据所述各个关节角度控制所述机械臂到达所述手术区域。使得机械臂在确定了路径之后，可以在多个实现完成该路径的解中找到不会使得机械臂产生奇异位的解，保证了手术中患者的安全，提高了机械臂的安全系数。

技术领域

本发明涉及机械控制领域，尤其涉及一种手术机械臂自动定位寻路方法、装置、机器人及存储介质。

背景技术

在传统手术的牙科植牙过程中，医生手持牙机对植牙部位进行球钻打磨，先锋钻打孔，扩孔钻打孔，以及成型钻打孔等操作。期间需要多次打孔，打孔过程复杂，需要多次依靠医生经验定位患者部位。种植牙入门门槛高，需要经验丰富的医生进行手术。手术机器人自动定位可以解决上述问题。而市面上一般机器人定位方案，上位机规划一个机械臂能够到达的位置，让机械臂以一个姿态进行打钻，由于机械臂为6轴机械臂，在空间中有多个自由度，会存在多解的情况，所以到达的位置不一定为当前位置的最优解。因此可能会以奇异位的姿态到达位置，从而在手术中对患者造成危险。

发明内容

有鉴于此，本申请提供了一种手术机械臂自动定位寻路方法，包括：

获取机械臂的当前位姿数据以及手术区域的位置数据；

根据所述当前位姿数据和所述位置数据计算最短路径；

构建对应的雅克比矩阵，并基于所述最短路径和当前位姿数据对所述雅克比矩阵求解，若所述雅克比矩阵为满秩，则求解得到所述机械臂的各个关节角度；

根据所述各个关节角度控制所述机械臂到达所述手术区域。

进一步的，还包括：

若所述雅克比矩阵不为满秩，则根据所述手术区域的位置数据计算所述机械臂位于所述手术区域时的所有候选位姿数据，并将所有所述候选位姿数据和所述当前位姿数据进行差值计算；

将计算得到最小差值的候选位姿数据作为选取的位姿数据，根据所述选取的位姿数据和所述最短路径确定所述关节角度。

进一步的，所述机械臂包括前端工具，所述当前位姿数据包括所述前端工具的空间坐标以及机械臂围绕所述前端工具旋转的欧拉角；

将所有所述候选位姿数据和所述当前位姿数据进行差值计算包括：

根据所述手术区域的位置数据计算所述机械臂在所述位置所有候选位姿的四元数坐标，将所述当前位姿的四元数坐标与所述所有候选的目标位姿四元数坐标进行所述差值计算；

根据所述选取的位姿数据和所述最短路径确定所述关节角度包括：

将所述选取的位姿的四元数坐标转换成相应的欧拉角坐标，根据所述选取的位姿的欧拉角坐标和所述最短路径确定所述关节角度。

进一步的，所述根据所述手术区域的位置数据计算所述机械臂在所述位置所有候选的目标位姿数据包括：

根据所述手术区域确定所述前端工具的目标位置，每隔一度，记录所述机械臂顺时针或逆时针围绕所述前端工具在所述目标位置时所形成的所有位姿数据，将所述形成的所有位姿数据作为所述候选的目标位姿数据。

进一步的，所述差值计算的公式为：

Err＝|q_{1_1}-q_{n_1}|+|q_{1_2}-q_{n_2}|+|q_{1_3}-q_{n_3}|+|q_{1_4}-q_{n_4}|