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

权利要求书

1.一种手术机械臂自动定位寻路方法，其特征在于，包括：

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

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

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

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

2.根据权利要求1所述的手术机械臂自动定位寻路方法，其特征在于，还包括：

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

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

3.根据权利要求2所述的手术机械臂自动定位寻路方法，其特征在于，所述机械臂包括前端工具，所述当前位姿数据包括所述前端工具的空间坐标以及机械臂围绕所述前端工具旋转的欧拉角；

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

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

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

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

4.根据权利要求3所述的手术机械臂自动定位寻路方法，其特征在于，所述根据所述手术区域的位置数据计算所述机械臂在所述位置所有候选的目标位姿数据包括：

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

5.根据权利要求2所述的手术机械臂自动定位寻路方法，其特征在于，所述差值计算的公式为：

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}|

式中，q_{1_1}至q_{1_4}为所述当前位姿数据的四元数数据，q_{n_1}至q_{n_4}为所述所有候选位姿数据的四元数数据，Err为所述差值。

6.根据权利要求1所述的手术机械臂自动定位寻路方法，其特征在于，所述雅克比矩阵公式为：

q＝J(q)^-1*V_e

式中q为所述机械臂的关节速度，V_e是所述机械臂的末端速度，j(q)^-1表示从关节速度向末端速度映射的雅克比矩阵式的逆。

7.根据权利要求1所述的手术机械臂自动定位寻路方法，其特征在于，所述最短路径为所述机械臂的前端工具的空间坐标到所述手术区域的空间坐标连成的直线；

所述最短路径的计算公式为：

p(λ)＝(1-λ)*p₁+λ*p₂

式中p1为所述机械臂的前端工具的空间坐标，p2为所述手术区域的空间坐标，λ为斜率参数。