CN113282077B - 基于正态分布和雅可比矩阵的同心管机器人运动规划方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明涉及手术机器人领域，具体为一种基于正态分布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)的过程如下：

3.1)首先，根据S1设定所需规划的起始位置x_init，目标位置x_goal，迭代次数 n，步长p以及碰撞风险评估指标：dis(x_i0,x₁)≥R_i+λr₀，其中r₀为同心管机器人的截面半径，λ为安全性常数，越大代表安全性要求越高，x_i0为第i个障碍物的中心位置坐标，x₁为工作空间中任意节点的位置坐标；

3.2)基于正态随机分布在自由空间中产生一个点x_rand，且正态分布的均值μ接近于目标位置x_goal，标准差σ＝10；

3.3)在搜索树T中找到与x_rand最近的节点x_nearest，并沿着x_nearest指向x_rand的方向，以步长p产生一个新节点x_new；

3.4)以x_new为中心，r_i为搜索半径，在搜索树T中找到所有潜在的父节点x_pp，以更新x_new。先对找到的所有潜在的父节点x_pp进行碰撞风险评估，剔除不符合风险指标的节点，最终组成潜在的父节点集合E_pp；

3.5)先不考虑碰撞检测，将E_pp中的每一个点分别作为父节点与x_new相连，计算从x_init到x_new的新路径长度，若该路径长度小于原路径长度，则进行碰撞检测。若检测失败，则考虑下一个潜在的父节点；若检测通过，则删除掉搜索树T 中多余的边，并将该节点作为新的父节点；

3.6)遍历所有潜在的父节点，得到更新完成后的树T；

3.7)步骤3.2)～3.6)为基于正态分布RRT*搜索最优路径的一次迭代，当达到最大迭代次数n后，找到一条渐近最优的路径。

本发明的技术构思为：首先根据设计好的同心管机器人，预计算出其工作空间，然后根据解剖结构产生机器人运动空间的三维点云，确定路径规划的起始位置和目标位置，使用基于正态分布的RRT*方法规划出一条符合风险评估要求的最优路径，最后使用基于雅可比的逆运动学计算出路径上每个点所对应的驱动输入。

本发明的有益效果为：通过以上方案，可以设计出符合特定解剖结构的同心管机器人工作路径。

附图说明

图1为一种基于正态分布RRT*和雅可比矩阵的同心管机器人运动规划方法流程图；

图2为同心管机器人的工作空间示意图；

图3为本发明的避障算法得到的路径结果示意图，用作下文分析。

具体实施方式

下面结合附图对本发明作进一步描述。

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

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

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

3)使用基于正态分布的RRT*算法进行同心管机器人的路径规划：从数据表

中选取出位于病灶点附近且不在障碍物内的位置x_target，并根据其对应的输入，利用正运动学模型绘制出机器人在工作空间中的整体形状Shape，该形状对应的点集为S0，因为同心管的建模满足刚度主宰的假设，故若S0中的任意一点都不在障碍物内，则同心管在该手术环境中的最优工作路径即为S0；若存在一段路径

被第i个障碍物截断，则需要对该段进行新的路径规划，过程如下：

3.1)首先，根据S1设定所需规划的起始位置x_init，目标位置x_goal，迭代次数n，步长p以及碰撞风险评估指标：dis(x_i0,x₁)≥R_i+λr₀，其中r₀为同心管机器人的截面半径，λ为安全性常数，越大代表安全性要求越高，x_i0为第i个障碍物的中心位置坐标，x₁为工作空间中任意节点的位置坐标；

3.2)基于正态随机分布在自由空间中产生一个点x_rand，且正态分布的均值μ接近于目标位置x_goal，标准差σ＝10；

3.3)在搜索树T中找到与x_rand最近的节点x_nearest，并沿着x_nearest指向x_rand的方向，以步长p产生一个新节点x_new；

3.4)以x_new为中心，r_i为搜索半径，在搜索树T中找到所有潜在的父节点x_pp，以更新x_new。先对找到的所有潜在的父节点x_pp进行碰撞风险评估，剔除不符合风险指标的节点，最终组成潜在的父节点集合；

3.5)先不考虑碰撞检测，将E_pp中的每一个点分别作为父节点与x_new相连，计算从x_init到x_new的新路径长度，若该路径长度小于原路径长度，则进行碰撞检测。若检测失败，则考虑下一个潜在的父节点；若检测通过，则删除掉搜索树T中多余的边，并将该节点作为新的父节点；

3.6)遍历所有潜在的父节点，得到更新完成后的树T；

3.7)步骤3.2)～3.6)为基于正态分布RRT*搜索最优路径的一次迭代，当达到最大迭代次数n后，找到一条渐近最优的路径。

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

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

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

本实施例以使用内窥镜产生的真实解剖结构点云为手术工作环境，并在 MATLAB内进行相同比例的手术环境重建，以进行同心管机器人的路径规划仿真，一种基于正态分布RRT*和雅可比矩阵的同心管机器人运动规划方法，包括以下步骤：

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

2)在MATLAB内进行相同手术环境的三维重建；

3)使用基于正态分布的RRT*算法进行同心管机器人的路径规划：从数据表

中选取出位于病灶点附近且不在障碍物内的位置x_target，并根据其对应的输入，利用正运动学模型绘制出机器人在工作空间中的整体形状Shape，该形状对应的点集为S0，因为同心管的建模满足刚度主宰的假设，故若S0中的任意一点都不在障碍物内，则同心管在该手术环境中的最优工作路径即为S0；若存在一段路径

被第i个障碍物截断，则需要对该段进行新的路径规划；

3.1)首先，根据S1设定所需规划的起始位置x_init，目标位置x_goal，迭代次数n，步长p以及碰撞风险评估指标：dis(x_i0,x₁)≥R_i+λr₀，其中r₀为同心管机器人的截面半径，λ为安全性常数，越大代表安全性要求越高，x_i0为第i个障碍物的中心位置坐标，x₁为工作空间中任意节点的位置坐标；

3.2)本发明基于正态随机分布在自由空间中产生一个点x_rand，且正态分布的均值μ接近于目标位置x_goal，标准差σ＝10；

3.3)在搜索树T中找到与x_rand最近的节点x_nearest，并沿着x_nearest指向x_rand的方向，以步长p产生一个新节点x_new；

3.4)以x_new为中心，r_i为搜索半径，在搜索树T中找到所有潜在的父节点x_pp，以更新x_new。先对找到的所有潜在的父节点x_pp进行碰撞风险评估，剔除不符合风险指标的节点，最终组成潜在的父节点集合E_pp；

3.5)先不考虑碰撞检测，将E_pp中的每一个点分别作为父节点与x_new相连，计算从x_init到x_new的新路径长度，若该路径长度小于原路径长度，则进行碰撞检测。若检测失败，则考虑下一个潜在的父节点；若检测通过，则删除掉搜索树T中多余的边，并将该节点作为新的父节点；

3.6)遍历所有潜在的父节点，得到更新完成后的树T；

3.7)步骤3.2)～3.6)为基于正态分布RRT*搜索最优路径的一次迭代，当达到最大迭代次数n后，找到一条渐近最优的路径；

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

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

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

以MATLAB R2018b仿真软件为实施例，运用以上方法能够在短时间内得到基于正态分布RRT*算法的同心管机器人工作路径，并且能够在避障的前提下使路径最大限度保持同心管在工作空间中的形状。

以上阐述的是本发明给出的一个实施例表现出来的优良效果，显然本发明不仅适合上述实施例，在不偏离本发明基本精神及不超出本发明实质内容所涉及内容的前提下可对其做种种变化加以实施。

Claims (2)

1.一种基于正态分布和雅可比矩阵的同心管机器人运动规划方法，其特征在于，所述方法包括以下步骤：

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。

2.如权利要求1所述的基于正态分布和雅可比矩阵的同心管机器人运动规划方法，其特征在于，所述步骤3)的过程如下：

3.1)首先，根据S1设定所需规划的起始位置x_init，目标位置x_goal，迭代次数n，步长p以及碰撞风险评估指标：dis(x_i0,x₁)≥R_i+λr₀，其中r₀为同心管机器人的截面半径，λ为安全性常数，越大代表安全性要求越高，x_i0为第i个障碍物的中心位置坐标，x₁为工作空间中任意节点的位置坐标；

3.2)基于正态随机分布在自由空间中产生一个点x_rand，且正态分布的均值μ接近于目标位置x_goal，标准差σ＝10；

3.3)在搜索树T中找到与x_rand最近的节点x_nearest，并沿着x_nearest指向x_rand的方向，以步长p产生一个新节点x_new；

3.4)以x_new为中心，r_i为搜索半径，在搜索树T中找到所有潜在的父节点x_pp，以更新x_new，先对找到的所有潜在的父节点x_pp进行碰撞风险评估，剔除不符合风险指标的节点，最终组成潜在的父节点集合E_pp；

3.5)先不考虑碰撞检测，将E_pp中的每一个点分别作为父节点与x_new相连，计算从x_init到x_new的新路径长度，若该路径长度小于原路径长度，则进行碰撞检测；若检测失败，则考虑下一个潜在的父节点；若检测通过，则删除掉搜索树T中多余的边，并将该节点作为新的父节点；

3.6)遍历所有潜在的父节点，得到更新完成后的树T；

3.7)步骤3.2)～3.6)为基于正态分布RRT*搜索最优路径的一次迭代，当达到最大迭代次数n后，找到一条渐近最优的路径。

Images