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

权利要求书

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后，找到一条渐近最优的路径。