[发明专利]基于正态分布和雅可比矩阵的同心管机器人运动规划方法

说明书

一种基于正态分布RRT*和雅可比矩阵的同心管机器人运动规划方法，根据同心管机器人参数及正运动学模型计算并绘制其工作空间；根据解剖结构，生成手术工作环境点云；利用基于正态分布RRT*算法生成同心管机器人最优工作路径；使用基于雅可比的逆运动学计算出路径上每个点对应的驱动空间输入量，使用输入量作为当前步的积分初值，更快速的计算出了每个路径点对应的输入量q。与现有技术相比，本发明能够在较短时间内生成一条符合手术和同心管形状要求的最优路径。

技术领域

本发明涉及手术机器人领域，具体为一种基于正态分布RRT*和雅可比矩阵的同心管机器人运动规划方法。

背景技术

近些年来，随着医疗水平的稳步发展，产生了各种能够在手术中发挥关键作用的机器人。相比于一般的刚性器械，同心管这一类连续型机器人灵活弯曲，并沿着术前规划的非线性路径到达指定病灶点的能力开始受到广泛关注。但是，此类机器人的关节自由度趋于无限大，并且其在自由空间中的形状十分复杂，从而导致其运动规划的复杂程度显著上升。

同心管的建模设计大都建立在分段常曲率和刚度主宰的假设上，并且忽略了剪切应变以及摩擦等难以量化的信息。在此基础上使用静力学平衡法和能量最小原则建立的同心管正运动学模型已能达到较高的精度，而在路径规划方面，普遍适用于高维空间的采样路径规划算法受到了人们的广泛关注，如PRM和RRT。

现在仍有许多问题阻碍着同心管机器人系统的进一步发展：同心管的原材料镍钛合金管加工难度很大，且会随着时间推移发生老化和变形，无法满足运动学模型的理想化假设要求以及手术的高精度要求。

发明内容

为了克服现有技术的不足，为了在一定程度上提高同心管系统的整体运行能力，本发明提出了一种基于正态分布RRT*和雅可比矩阵的同心管机器人运动规划方法，能够提高机器人在复杂手术环境下的工作效率。

本发明所采用的技术方案是：

一种基于正态分布RRT*和雅可比矩阵的同心管机器人运动规划方法，包括以下步骤：

1)对于几何参数和力学参数已知的同心管机器人，预计算其正运动学的解，绘制机器人的工作空间，建立驱动输入与末端位姿一一对应的数据表

2)通过安装在同心管机器人末端的内窥镜系统生成手术解剖结构的点云，并将相互距离小于同心管截面直径d的点用一个最小球体包裹住，设此方法生成的障碍物球体共有N个，且第i个障碍物的半径为R_i；

3)使用基于正态分布的RRT*算法进行同心管机器人的路径规划：从数据表中选取出位于病灶点附近且不在障碍物内的位置x_target，并根据其对应的输入，利用正运动学模型绘制出机器人在工作空间中的整体形状Shape，该形状对应的点集为S0，因为同心管的建模满足刚度主宰的假设，故若S0中的任意一点都不在障碍物内，则同心管在该手术环境中的最优工作路径即为S0；若存在一段路径被第i个障碍物截断，则需要对该段进行新的路径规划；

4)根据步骤3)实现了障碍物段的最优路径规划，得到路径S2，进而得到同心管机器人在工作空间中的最佳路径S＝S0-S1+S2。

5)为了使机器人能够沿着路径S到达病灶点，使用基于雅可比矩阵J的逆运动学逐步计算出路径上每一个点x所对应的机器人驱动输入量q；

6)使用步骤5)计算得到的输入量作为当前步的积分初值，更快速的计算出了每个路径点对应的输入量q。

进一步，所述步骤3)的过程如下：