CN116135159A - 三维路径规划方法、装置、设备和存储介质 - Google Patents
三维路径规划方法、装置、设备和存储介质 Download PDFInfo
- Publication number
- CN116135159A CN116135159A CN202111359931.7A CN202111359931A CN116135159A CN 116135159 A CN116135159 A CN 116135159A CN 202111359931 A CN202111359931 A CN 202111359931A CN 116135159 A CN116135159 A CN 116135159A
- Authority
- CN
- China
- Prior art keywords
- node
- path
- puncture
- target
- search
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 122
- 238000004422 calculation algorithm Methods 0.000 claims description 98
- 230000006870 function Effects 0.000 claims description 41
- 230000035515 penetration Effects 0.000 claims description 19
- 230000033001 locomotion Effects 0.000 claims description 14
- 238000004590 computer program Methods 0.000 claims description 6
- 230000008569 process Effects 0.000 description 25
- 238000010586 diagram Methods 0.000 description 18
- 230000001360 synchronised effect Effects 0.000 description 9
- 238000001514 detection method Methods 0.000 description 8
- 238000003780 insertion Methods 0.000 description 7
- 230000037431 insertion Effects 0.000 description 7
- 210000003491 skin Anatomy 0.000 description 7
- 230000005291 magnetic effect Effects 0.000 description 6
- 230000003287 optical effect Effects 0.000 description 4
- 238000005070 sampling Methods 0.000 description 4
- 230000003068 static effect Effects 0.000 description 4
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 3
- 241000208199 Buxus sempervirens Species 0.000 description 3
- 230000008901 benefit Effects 0.000 description 3
- 238000006073 displacement reaction Methods 0.000 description 3
- 210000000056 organ Anatomy 0.000 description 3
- 210000001519 tissue Anatomy 0.000 description 3
- 230000004888 barrier function Effects 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 235000019800 disodium phosphate Nutrition 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000001788 irregular Effects 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 201000004569 Blindness Diseases 0.000 description 1
- 210000003484 anatomy Anatomy 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 210000002615 epidermis Anatomy 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000005294 ferromagnetic effect Effects 0.000 description 1
- 238000012804 iterative process Methods 0.000 description 1
- 230000009191 jumping Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
Images
Classifications
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B17/00—Surgical instruments, devices or methods, e.g. tourniquets
- A61B17/34—Trocars; Puncturing needles
- A61B17/3403—Needle locating or guiding means
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
- A61B34/10—Computer-aided planning, simulation or modelling of surgical operations
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/12—Target-seeking control
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
- A61B34/10—Computer-aided planning, simulation or modelling of surgical operations
- A61B2034/101—Computer-aided simulation of surgical operations
- A61B2034/102—Modelling of surgical devices, implants or prosthesis
Landscapes
- Health & Medical Sciences (AREA)
- Engineering & Computer Science (AREA)
- Life Sciences & Earth Sciences (AREA)
- Surgery (AREA)
- Animal Behavior & Ethology (AREA)
- Veterinary Medicine (AREA)
- Biomedical Technology (AREA)
- Heart & Thoracic Surgery (AREA)
- Medical Informatics (AREA)
- Molecular Biology (AREA)
- Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
- General Health & Medical Sciences (AREA)
- Public Health (AREA)
- Pathology (AREA)
- Robotics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Abstract
本发明公开一种三维路径规划方法、装置、设备和存储介质，应用于医学行针轨迹规划技术领域。方法包括：获得硬针的穿刺目标位置和刺入位置，根据刺入位置确定目标区域，以及根据穿刺目标位置和刺入位置确定初始区域；根据穿刺目标位置和初始区域规划初始搜索路径，获得初始搜索节点；对初始搜索节点进行扩展确定状态节点对应的搜索区域；基于初始搜索路径和搜索区域规划目标搜索路径；判断目标搜索路径是否到达目标区域，获得判断结果；在判断结果表明所述目标搜索路径到达目标区域的情况下，将目标搜索路径作为硬针穿刺的三维路径规划，本发明采用的方案能大大提高穿刺路径的成功率，减少对人体造成的伤害。
Description
技术领域
本发明涉及医学行针轨迹规划技术领域，特别涉及一种三维路径规划方法、装置、设备和存储介质。
背景技术
目前穿刺过程的路径都是外科医生凭借自己的穿刺经验及人体解剖学观察手动计划的近似穿刺数据，在穿刺过程中，不断使用扫描仪扫描，通过反复实验调整对穿刺针位置和角度重新定位，这样重复多次最终到达目标位置。此过程需要对患者进行反复扫描，此外，穿刺针会长时间留在人体内部，如果穿刺不成功，必须再次穿刺，对人体造成很大伤害。因此，设计和规划准确的穿刺路径对穿刺手术意义重大。如何解决该问题，目前尚无有效解决方案。
发明内容
有鉴于此，本发明实施例期望提供一种三维路径规划方法、装置、设备和存储介质。
本发明实施例的技术实施例是这样实现的：
本发明实施例提供一种三维路径规划方法，应用于包括硬针穿刺的场景，所述方法包括：
获得所述硬针的穿刺目标位置和刺入位置，根据所述刺入位置确定目标区域，以及根据所述穿刺目标位置和所述刺入位置确定初始区域；
根据所述穿刺目标位置和所述初始区域规划初始搜索路径，获得初始搜索节点；对所述初始搜索节点进行扩展确定状态节点对应的搜索区域；
基于所述初始搜索路径和所述搜索区域规划目标搜索路径；
判断所述目标搜索路径是否到达所述目标区域，获得判断结果；在所述判断结果表明所述目标搜索路径到达所述目标区域的情况下，将所述目标搜索路径作为硬针穿刺的三维路径规划。
在上述方案中，所述方法还包括：
在所述判断结果表明所述目标搜索路径没有到达所述目标区域的情况下，将所述状态节点按预设方向进行扩展确定新节点对应的扩展区域；所述预设方向表征所述状态节点朝向所述目标区域的方向；
根据所述目标搜索路径和所述扩展区域规划新搜索路径；
判断所述新搜索路径是否到达所述目标区域，在所述新搜索路径到达所述目标区域的情况下，将所述新搜索路径作为硬针穿刺的三维路径规划。
在上述方案中，所述根据所述穿刺目标位置和所述刺入位置确定初始区域，包括：
获得初始穿刺角度和初始穿刺步长；
根据所述穿刺目标位置、所述刺入位置、所述初始穿刺角度和所述初始穿刺步长确定圆锥体；
将所述圆锥体的底部区域作为所述初始区域。
在上述方案中，所述根据所述穿刺目标位置和所述初始区域规划初始搜索路径，获得初始搜索节点，包括：
将所述穿刺目标位置作为扩展树的根节点；
基于所述根节点和所述初始区域通过搜素随机树RRT算法规划可行路径；
根据A星算法确定所述可行路径中的最优路径；
判断所述最优路径是否成功避开障碍物；在所述最优路径成功避开障碍物的情况下，将所述最优路径作为初始搜索路径；
将所述初始搜索路径在所述初始区域中的端点作为初始搜索节点。
在上述方案中，所述对所述初始搜索节点进行扩展确定状态节点对应的搜索区域，包括：
根据搜素随机树RRT算法对所述初始搜索节点进行扩展，获得多个随机状态节点；
基于所述初始搜索节点和所述多个随机状态节点中的每个随机状态节点确定多个状态节点；
根据A星算法确定所述多个状态节点中的最佳状态节点；在所述初始搜索节点和所述最佳状态节点之间的运动直线是无碰撞路径的情况下，根据所述初始搜索节点的位置信息和所述最佳状态节点的位置信息确定第一穿刺步长；
获得硬针穿刺角度的约束范围；基于所述初始搜索节点的硬针穿刺姿态信息和所述约束范围确定所述最佳状态节点处所述硬针的第一穿刺角度；
根据所述初始搜索节点、所述最佳状态节点、所述第一穿刺步长和所述第一穿刺角度确定第一圆锥体；
将所述第一圆锥体的底部区域作为所述搜索区域。
在上述方案中，所述方法还包括：
在所述初始搜索节点和所述最佳状态节点之间的运动直线不是无碰撞路径的情况下，获得第一状态节点；所述第一状态节点为所述多个状态节点中除所述最佳状态节点以外的任一状态节点；
根据所述第一状态节点的位置信息和所述最佳状态节点的位置信息确定所述第一状态节点的第二穿刺步长；
基于所述初始搜索节点的硬针穿刺姿态信息和所述约束范围更新所述第一状态节点，确定更新后第一状态节点处所述硬针的第二穿刺角度；
根据所述更新后第一状态节点、所述第二穿刺步长和所述第二穿刺角度确定第二圆锥体；
将所述第二圆锥体的底部区域作为所述搜索区域。
在上述方案中，所述基于所述初始搜索路径和所述搜索区域规划目标搜索路径，包括：
基于所述初始搜索路径和所述搜索区域确定在所述搜索区域中的目标节点；
根据所述初始搜索路径和所述目标节点规划目标搜索路径。
在上述方案中，所述基于所述初始搜索路径和所述搜索区域确定在所述搜索区域中的目标节点，包括：
采用A星算法的估价函数估算所述搜索区域中每个状态节点与所述初始搜索路径的路径估计值；
确定所述路径估计值的最小值对应的状态节点为目标节点。
本发明实施例提供一种三维路径规划装置，应用于包括硬针穿刺的场景，所述装置包括：获得单元、确定单元、规划单元和判断单元，其中：
所述获得单元，用于获得所述硬针的穿刺目标位置和刺入位置，根据所述刺入位置确定目标区域，以及根据所述穿刺目标位置和所述刺入位置确定初始区域；
所述确定单元，用于根据所述穿刺目标位置和所述初始区域规划初始搜索路径，获得初始搜索节点；对所述初始搜索节点进行扩展确定状态节点对应的搜索区域；
所述规划单元，用于基于所述初始搜索路径和所述搜索区域规划目标搜索路径；
所述判断单元，用于判断所述目标搜索路径是否到达所述目标区域，获得判断结果；在所述判断结果表明所述目标搜索路径到达所述目标区域的情况下，将所述目标搜索路径作为硬针穿刺的三维路径规划。
在上述方案中，所述判断单元，还用于在所述判断结果表明所述目标搜索路径没有到达所述目标区域的情况下，将所述状态节点按预设方向进行扩展确定新节点对应的扩展区域；所述预设方向表征所述状态节点朝向所述目标区域的方向；根据所述目标搜索路径和所述扩展区域规划新搜索路径；判断所述新搜索路径是否到达所述目标区域，在所述新搜索路径到达所述目标区域的情况下，将所述新搜索路径作为硬针穿刺的三维路径规划。
在上述方案中，所述获得单元，还用于获得初始穿刺角度和初始穿刺步长；根据所述穿刺目标位置、所述刺入位置、所述初始穿刺角度和所述初始穿刺步长确定圆锥体；将所述圆锥体的底部区域作为所述初始区域。
在上述方案中，所述获得单元，还用于将所述穿刺目标位置作为扩展树的根节点；基于所述根节点和所述初始区域通过搜素随机树RRT算法规划可行路径；根据A星算法确定所述可行路径中的最优路径；判断所述最优路径是否成功避开障碍物；在所述最优路径成功避开障碍物的情况下，将所述最优路径作为初始搜索路径；将所述初始搜索路径在所述初始区域中的端点作为初始搜索节点。
在上述方案中，所述确定单元，还用于根据搜素随机树RRT算法对所述初始搜索节点进行扩展，获得多个随机状态节点；基于所述初始搜索节点和所述多个随机状态节点中的每个随机状态节点确定多个状态节点；根据A星算法确定所述多个状态节点中的最佳状态节点；在所述初始搜索节点和所述最佳状态节点之间的运动直线是无碰撞路径的情况下，根据所述初始搜索节点的位置信息和所述最佳状态节点的位置信息确定第一穿刺步长；获得硬针穿刺角度的约束范围；基于所述初始搜索节点的硬针穿刺姿态信息和所述约束范围确定所述最佳状态节点处所述硬针的第一穿刺角度；根据所述初始搜索节点、所述最佳状态节点、所述第一穿刺步长和所述第一穿刺角度确定第一圆锥体；将所述第一圆锥体的底部区域作为所述搜索区域。
在上述方案中，所述获得单元，还用于在所述初始搜索节点和所述最佳状态节点之间的运动直线不是无碰撞路径的情况下，获得第一状态节点；所述第一状态节点为所述多个状态节点中除所述最佳状态节点以外的任一状态节点；
所述确定单元，还用于根据所述第一状态节点的位置信息和所述最佳状态节点的位置信息确定所述第一状态节点的第二穿刺步长；基于所述初始搜索节点的硬针穿刺姿态信息和所述约束范围更新所述第一状态节点，确定更新后第一状态节点处所述硬针的第二穿刺角度；根据所述更新后第一状态节点、所述第二穿刺步长和所述第二穿刺角度确定第二圆锥体；将所述第二圆锥体的底部区域作为所述搜索区域。
在上述方案中，所述规划单元，还用于基于所述初始搜索路径和所述搜索区域确定在所述搜索区域中的目标节点；根据所述初始搜索路径和所述目标节点规划目标搜索路径。
在上述方案中，所述规划单元，还用于采用A星算法的估价函数估算所述搜索区域中每个状态节点与所述初始搜索路径的路径估计值；确定所述路径估计值的最小值对应的状态节点为目标节点。
本发明实施例提供一种设备，包括存储器和处理器，所述存储器存储有可在处理器上运行的程序，所述处理器执行所述程序时实现上述所述方法的任一步骤。
本发明实施例提供一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现上述所述方法的任一步骤。
本发明实施例提供的三维路径规划方法、装置、设备和存储介质，其中，通过根据所述穿刺目标位置和所述初始区域规划初始搜索路径，获得初始搜索节点；对所述初始搜索节点进行扩展确定状态节点对应的搜索区域；基于所述初始搜索路径和所述搜索区域规划目标搜索路径；判断所述目标搜索路径是否到达所述目标区域，获得判断结果；在所述判断结果表明所述目标搜索路径到达所述目标区域的情况下，将所述目标搜索路径作为硬针穿刺的三维路径规划。采用本发明实施例的技术方案，通过设计和规划准确的穿刺路径，大大提高穿刺路径的成功率，减少对人体造成的伤害。
附图说明
图1为本发明实施例三维路径规划方法实现流程示意图；
图2为本发明实施例三维路径规划方法中穿刺目标位置和刺入位置的示意图；
图3为本发明实施例三维路径规划方法中初始区域的示意图；
图4为本发明实施例三维路径规划方法中规划流程示意图；
图5为本发明实施例三维路径规划方法中26领域A*算法的示意图；
图6为本发明实施例三维路径规划方法中改进的RRT算法示意图；
图7为本发明实施例三维路径规划方法中硬针穿刺的一种应用场景示意图；
图8为本发明实施例三维路径规划装置的组成结构示意图；
图9为本发明实施例中三维路径规划设备的一种硬件实体结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对发明的具体技术方案做进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。
本实施例提出一种三维路径规划方法，图1为本发明实施例三维路径规划方法实现流程示意图，如图1所示，该方法包括：
步骤S101：获得所述硬针的穿刺目标位置和刺入位置，根据所述刺入位置确定目标区域，以及根据所述穿刺目标位置和所述刺入位置确定初始区域。
需要说明的是，获得所述硬针的穿刺目标位置可以为获得患者所述硬针的穿刺目标位置，在实际应用中，所述穿刺目标位置可以为穿刺目标靶点位置，简称穿刺目标靶点或目标点；作为一种示例所述穿刺目标靶点可以记为T。获得所述硬针的刺入位置可以为将所述硬针的穿刺目标位置作为已知条件，刺入位置依据所述硬针可达区域逆向计算求解出来，获得所述硬针的刺入位置的两个临界点，在所述两个临界点以及两个临界点之间的任意位置均可以为刺入位置；作为一种示例，所述刺入位置可以简称刺入点或可穿刺点，所述两个临界点可以分别记为point1和point2。为了方便理解，这里示例说明，图2为本发明实施例三维路径规划方法中穿刺目标位置和刺入位置的示意图；对于穿刺目标靶点T，穿刺针尖能够穿刺的两个临界点是point1和point2，如果穿刺点不在point1和piont2之间，将无法到达穿刺目标靶点T。
根据所述刺入位置确定目标区域可以为将所述刺入位置为中心，预设长度为半径，形成曲面区域，将所述曲面区域作为目标区域。在实际应用中，可以假设目标区域为Areagoal，Areagoal是皮肤上以一可穿刺点为中心，某长度为半径范围内的不规则曲面区域。
根据所述穿刺目标位置和所述刺入位置确定初始区域可以为以所述穿刺目标位置为顶点，以所述穿刺目标位置和所述刺入位置之间的连线中心线，绘制出一个高度为预设长度，顶角为预设角度的圆锥体，该圆锥体的底部作为初始区域。其中，所述预设长度和所述预设角度分别可以根据实际情况进行确定，在此不做限定。作为一种示例，所述预设长度可以为一个步长，记为h，h可以根据实际穿刺手术中空间网格单位确定；所述预设角度可以记为2θ，θ可以在10度内。
步骤S102：根据所述穿刺目标位置和所述初始区域规划初始搜索路径，获得初始搜索节点；对所述初始搜索节点进行扩展确定状态节点对应的搜索区域。
这里，根据所述穿刺目标位置和所述初始区域规划初始搜索路径可以为通过预设算法在所述穿刺目标位置和所述初始区域之间规划初始搜索路径；所述预设算法可以根据实际情况进行确定，在此不做限定，作为一种示例，所述预设算法可以为搜素随机树RRT算法和A星算法。在实际应用中，所述初始搜索路径可以称为第一段工作空间规划路径，可以记为L1。获得初始搜索节点可以为将所述初始搜索路径在所述初始区域中的端点称为初始搜索节点；所述初始搜索节点可以记为P11。在实际应用中，所述初始搜索节点也可以称为第一段穿刺路径末节点。
对所述初始搜索节点进行扩展确定状态节点对应的搜索区域可以为根据改进的搜素随机树RRT算法对所述初始搜索节点进行扩展，确定状态节点对应的搜索区域。
步骤S103：基于所述初始搜索路径和所述搜索区域规划目标搜索路径。
需要说明的是，基于所述初始搜索路径和所述搜索区域规划目标搜索路径可以为基于所述初始搜索路径和所述搜索区域确定在所述搜索区域中的目标节点；根据所述初始搜索路径和所述目标节点规划目标搜索路径。在实际应用中，所述初始搜索路径可以称为第一段工作空间规划路径；所述目标节点可以为第二个工作区间的节点；根据所述初始搜索路径和所述目标节点规划目标搜索路径可以为根据所述第一段工作空间规划路径和所述目标节点规划第二段工作空间规划路径，基于所述第一段工作空间规划路径和所述第二段工作空间规划路径确定目标搜索路径。
步骤S104：判断所述目标搜索路径是否到达所述目标区域，获得判断结果；在所述判断结果表明所述目标搜索路径到达所述目标区域的情况下，将所述目标搜索路径作为硬针穿刺的三维路径规划。
这里，判断所述目标搜索路径是否到达所述目标区域，获得判断结果可以为根据所述目标搜索路径获得所述目标搜索路径的终点，确定所述目标搜索路径的终点与所述刺入位置的距离；根据所述目标区域获得预设阈值；判断所述距离是否小于等于所述预设阈值，在所述距离小于等于所述预设阈值的情况下，获得所述目标搜索路径到达所述目标区域判断结果；在所述距离大于所述预设阈值的情况下，获得所述目标搜索路径没有到达所述目标区域判断结果。在实际应用中，所述目标搜索路径的终点也可以称为穿刺规划路径的终点；所述刺入位置也可以称为目标终点；所述目标搜索路径的终点与所述刺入位置的距离可以记为d；所述预设阈值可以根据实际情况进行确定，在此不做限定，作为一种示例，所述预设阈值可以为设定好的阈值，可以记为ε。判断所述目标搜索路径是否到达所述目标区域，获得判断结果可以理解为确定当前计划的穿刺路线有没有到达目标区域(穿刺针进针区域)，可以通过计算穿刺规划路径的终点和目标终点的距离d，和设定好的阈值ε作比较，来判断规划路径有没有到达目标中心。如果d值比ε值小，则当前搜索的穿刺路径成功抵达目标区域，穿刺路径规划完成，即获得当前搜索的穿刺路径成功抵达目标区域的判断结果。如果d大于ε，则当前规划的穿刺针穿刺轨迹没有到达目标区域，即获得当前搜索的穿刺路径没有成功抵达目标区域的判断结果。
在所述判断结果表明所述目标搜索路径到达所述目标区域的情况下，将所述目标搜索路径作为硬针穿刺的三维路径规划可以理解为当前搜索的穿刺路径成功抵达目标区域，穿刺路径规划完成，所述目标搜索路径即为硬针穿刺的三维路径规划。
本发明实施例提供的三维路径规划方法，通过设计和规划准确的穿刺路径，大大提高穿刺路径的成功率，减少对人体造成的伤害。
在本发明一种可选实施例中，所述方法还包括：在所述判断结果表明所述目标搜索路径没有到达所述目标区域的情况下，将所述状态节点按预设方向进行扩展确定新节点对应的扩展区域；所述预设方向表征所述状态节点朝向所述目标区域的方向；根据所述目标搜索路径和所述扩展区域规划新搜索路径；判断所述新搜索路径是否到达所述目标区域，在所述新搜索路径到达所述目标区域的情况下，将所述新搜索路径作为硬针穿刺的三维路径规划。
在本实施例中，所述预设方向表征所述状态节点朝向所述目标区域的方向可以理解为引导所述状态节点朝向所述目标区域的方向，在实际应用中，所述预设方向可以通过方向引导函数来实现，所述方向引导函数可以理解为在考虑下一节点的选择因素中加入与节点扩展方向和路径搜索长度相关的方向引导函数，所述方向引导函数可以记为
在所述判断结果表明所述目标搜索路径没有到达所述目标区域的情况下，将所述状态节点按预设方向进行扩展确定新节点对应的扩展区域可以为在所述判断结果表明所述目标搜索路径没有到达所述目标区域的情况下，将所述状态节点通过方向引导函数进行扩展确定新节点对应的扩展区域。
在本发明一种可选实施例中，所述根据所述穿刺目标位置和所述刺入位置确定初始区域，包括：获得初始穿刺角度和初始穿刺步长；根据所述穿刺目标位置、所述刺入位置、所述初始穿刺角度和所述初始穿刺步长确定圆锥体；将所述圆锥体的底部区域作为所述初始区域。
本实施例中，所述初始穿刺角度可以记为θ，所述θ的值可以根据实际情况进行确定，在此不做限定，作为一种示例，所述θ的值可以在10度内。所述初始穿刺步长可以记为h，所述h的值可以根据实际情况进行确定，在此不做限定，作为一种示例，h可以根据实际穿刺手术中空间网格单位确定。
根据所述穿刺目标位置、所述刺入位置、所述初始穿刺角度和所述初始穿刺步长确定圆锥体可以为以所述穿刺目标位置为顶点，以所述穿刺目标位置和所述刺入位置之间的连线中心线，绘制出一个高度为所述初始穿刺步长，顶角为2倍所述初始穿刺角度的圆锥体。
将所述圆锥体的底部区域作为所述初始区域可以为将所述圆锥体的底部的圆锥面作为所述初始区域。在实际应用中，所述初始区域也可以理解为扩展树首个搜索节点区域。
为了方便理解，图3为本发明实施例三维路径规划方法中初始区域的示意图，如图3所示，P1点为穿刺目标位置，P2点为刺入位置，2倍的初始穿刺角度为圆锥体的顶角，初始区域为圆锥体的底部区域。
在本发明一种可选实施例中，所述根据所述穿刺目标位置和所述初始区域规划初始搜索路径，获得初始搜索节点，包括：将所述穿刺目标位置作为扩展树的根节点；基于所述根节点和所述初始区域通过搜素随机树RRT算法规划可行路径；根据A星算法确定所述可行路径中的最优路径；判断所述最优路径是否成功避开障碍物；在所述最优路径成功避开障碍物的情况下，将所述最优路径作为初始搜索路径；将所述初始搜索路径在所述初始区域中的端点作为初始搜索节点。
本实施例中，基于所述根节点和所述初始区域通过搜素随机树RRT算法规划可行路径可以为以所述根节点通过搜素随机树RRT算法进行搜索，当搜索树扩展至所述初始区域中的某点后进行规划，获得可行路径。
根据A星算法确定所述可行路径中的最优路径可以为根据A星算法的估价函数确定所述可行路径中的最优路径。其中，所述估价函数可以根据实际情况进行确定，在此不做限定，作为一种示例，所述估价函数可以为f(n)＝g(n)+h(n)，f(n)表示节点n到目标点的总代价，g(n)表示沿着根节点至当前路径节点n的实际代价，h(n)为节点n到目标点的路径的预估代价。在具体穿刺针任务中，f(n)是节点n的从起点(穿刺靶点)到终结点(皮肤上的穿刺进针区域)的估价函数，g(n)表示工作环境中从根节点到当前n节点的代价值，h(n)是从n节点到终止点的最优路线的预估成本。
判断所述最优路径是否成功避开障碍物可以为通过碰撞检测算法判断所述最优路径是否成功避开障碍物。所述碰撞检测算法可以根据实际情况进行确定，在此不做限定，作为一种示例，所述碰撞检测算法可以为OBBTree包围盒树检测算法；所述障碍物可以根据实际情况进行确定，在此不做限定，作为一种示例，所述障碍物可以为重要器官组织。
为了方便理解，这里示例说明，图4为本发明实施例三维路径规划方法中规划流程示意图，可以结合图4进行理解；通过某状态节点在某时刻的位置及状态可以确定穿刺针在工作空间的可达区域，接着使用改进的RRT算法和A星算法尝试寻求最优路径，并使用碰撞检测算法(本文使用OBBTree包围盒树检测算法)判定路径是否与障碍物(如重要器官组织)发生碰撞。若当前规划路径成功避开障碍物，则判断该节点是否到达目标区域，若没有，则将该节点添加到扩展搜索树，否则，结束路径规划流程；若当前规划穿刺路径与障碍物发生碰撞，则重新规划次优路径，并判断该路径是否避开障碍物，重复以上步骤，直到流程结束。
在本发明一种可选实施例中，所述对所述初始搜索节点进行扩展确定状态节点对应的搜索区域，包括：根据搜素随机树RRT算法对所述初始搜索节点进行扩展，获得多个随机状态节点；基于所述初始搜索节点和所述多个随机状态节点中的每个随机状态节点确定多个状态节点；根据A星算法确定所述多个状态节点中的最佳状态节点；在所述初始搜索节点和所述最佳状态节点之间的运动直线是无碰撞路径的情况下，根据所述初始搜索节点的位置信息和所述最佳状态节点的位置信息确定第一穿刺步长；获得硬针穿刺角度的约束范围；基于所述初始搜索节点的硬针穿刺姿态信息和所述约束范围确定所述最佳状态节点处所述硬针的第一穿刺角度；根据所述初始搜索节点、所述最佳状态节点、所述第一穿刺步长和所述第一穿刺角度确定第一圆锥体；将所述第一圆锥体的底部区域作为所述搜索区域。
本实施例中，根据搜素随机树RRT算法对所述初始搜索节点进行扩展，获得多个随机状态节点可以为根据搜素随机树RRT算法对所述初始搜索节点进行随机扩展，获得多个随机状态节点。在实际应用中，所述初始搜索节点可以记为(xp,yp)；所述随机状态节点可以记为(xr,yr)。
基于所述初始搜索节点和所述多个随机状态节点中的每个随机状态节点确定多个状态节点可以为基于所述初始搜索节点和所述多个随机状态节点中的每个随机状态节点通过预设公式确定多个状态节点；其中，所述状态节点可以记为dn(xn，yn)；所述预设公式可以根据实际情况进行确定，在此不做限定。作为一种示例，所述预设公式可以参照以下公式(1)：
式(1)中，xn，yn为扩展节点横纵坐标，h为穿刺针单步探索步长。
根据A星算法确定所述多个状态节点中的最佳状态节点可以为根据改进的A星算法选择适当的估价函数确定所述多个状态节点中的最佳状态节点。其中，所述估价函数可以为f(n)＝g(n)+h(n)，f(n)表示节点n到目标点的总代价，g(n)表示沿着根节点至当前路径节点n的实际代价，h(n)为节点n到目标点的路径的预估代价。在具体穿刺针任务中，f(n)是节点n的从起点(穿刺靶点)到终结点(皮肤上的穿刺进针区域)的估价函数，g(n)表示工作环境中从根节点到当前n节点的代价值，h(n)是从n节点到终止点的最优路线的预估成本。
在实际应用中，为了减少扩展方式的随机性，使得树的生长朝着目标点方向快速收敛，在RRT算法的基础上，借鉴了A*算法思想中寻找最优路径的思想，在构建扩展树的过程中，减少扩展树搜索节点数，减少树生长的随机性，使得规划的穿刺路径接近最短路径。
根据所述初始搜索节点的位置信息和所述最佳状态节点的位置信息确定第一穿刺步长可以参照如下公式(2)、(3)：
hi＝hmin+(hmax-hmin)si (2)
式(2)中，hmin和hmax表示最大步长值和最小步长值。式(3)中，ni表示第i个状态节点的位置，nb表示最佳状态节点的位置，si表示第i个状态节点的位置与最佳状态节点的位置的误差因子。dmax表示最佳状态节点与其余状态节点的最大距离值，由上一步迭代结果更新本次迭代步长。
获得硬针穿刺角度的约束范围可以为获得硬针穿刺角度的约束系数和约束函数；其中，所述约束系数和所述约束函数可以根据实际情况进行确定，在此不做限定，作为一种示例，所述约束系数可以记为ω，所述约束函数可以记为a(n)。
基于所述初始搜索节点的硬针穿刺姿态信息和所述约束范围确定所述最佳状态节点处所述硬针的第一穿刺角度可以为根据所述初始搜索节点的硬针穿刺姿态信息在所述约束范围确定所述最佳状态节点处所述硬针的第一穿刺角度。其中，所述姿态信息可以包括位置信息和状态信息；作为一种示例，所述状态信息可以为上一时刻穿刺针朝向角，可以记为θn-1；所述约束范围可以为穿刺角度约束函数，例如，a(n)＝ω×(θn-θn-1)，a(n)越小，偏转角度越小，全局路径的转折点就越少，确定a(n)为最小值的情况下的θn即为第一穿刺角度。
根据所述初始搜索节点、所述最佳状态节点、所述第一穿刺步长和所述第一穿刺角度确定第一圆锥体可以为将所述初始搜索节点作为顶点，所述初始搜索节点与所述最佳状态节点的连线作为中心线，绘制出高度为所述第一穿刺步长，顶角为2倍所述第一穿刺角度的圆锥体。
将所述第一圆锥体的底部区域作为所述搜索区域可以理解为将所述第一圆锥体的底面区域作为所述搜索区域。
在本发明一种可选实施例中，所述方法还包括：在所述初始搜索节点和所述最佳状态节点之间的运动直线不是无碰撞路径的情况下，获得第一状态节点；所述第一状态节点为所述多个状态节点中除所述最佳状态节点以外的任一状态节点；根据所述第一状态节点的位置信息和所述最佳状态节点的位置信息确定所述第一状态节点的第二穿刺步长；基于所述初始搜索节点的硬针穿刺姿态信息和所述约束范围更新所述第一状态节点，确定更新后第一状态节点处所述硬针的第二穿刺角度；根据所述更新后第一状态节点、所述第二穿刺步长和所述第二穿刺角度确定第二圆锥体；将所述第二圆锥体的底部区域作为所述搜索区域。
本实施例中，在所述初始搜索节点和所述最佳状态节点之间的运动直线不是无碰撞路径的情况下，获得第一状态节点可以为在所述初始搜索节点和所述最佳状态节点之间的运动直线不是无碰撞路径的情况下，获得某时刻扩展状态节点。为了方便理解，所述某时刻扩展状态节点可以记为dn(xn,yn)，可以参照前面公式(1)，所述第一状态节点为所述多个状态节点中除所述最佳状态节点以外的任一状态节点。
根据所述第一状态节点的位置信息和所述最佳状态节点的位置信息确定所述第一状态节点的第二穿刺步长可以为根据所述第一状态节点的位置信息和所述最佳状态节点的位置信息确定所述第一状态节点与所述最佳状态节点的误差因子，基于所述误差因子确定所述第一状态节点的第二穿刺步长。为了方便理解，这里示例说明，所述第一状态节点的位置信息可以记为ni，所述最佳状态节点的位置信息可以记为nb，所述误差因子可以记为si；所述第二穿刺步长可以记为hi；所述第二穿刺步长的确定过程可以参照前面公式(2)、(3)。
基于所述初始搜索节点的硬针穿刺姿态信息和所述约束范围更新所述第一状态节点，确定更新后第一状态节点处所述硬针的第二穿刺角度可以为基于所述初始搜索节点的硬针穿刺姿态信息和所述约束范围修改所述第一状态节点，确定修改后第一状态节点处所述硬针的第二穿刺角度。为了方便理解，所述第二穿刺角度可以记为θn。
根据所述更新后第一状态节点、所述第二穿刺步长和所述第二穿刺角度确定第二圆锥体可以为将所述初始搜索节点作为顶点，以所述初始搜索节点与所述更新后第一状态节点的连线为中心线，2倍所述第二穿刺角度为顶角，高度为所述第二穿刺步长的圆锥体。为了方便理解，所述更新后第一状态节点可以记为d'n。
为了方便理解，这里示例说明，式(1)中，xr,yr为该迭代过程中生成的随机点dr的坐标，可以采用向量表示，该向量表示可以参照如下公式(4)：
式(4)中，dp,n为dp到dn的位移，dp,r为dp到dr的位移，为了减少穿刺针状态节点dn对随机点dr的依赖，引入向量cr对状态节点dn进行修改，使得穿刺路径满足下一步迭代要求。具体的计算过程可以参照如下公式(5)、(6)、(7)、(8)、(9)：
式(5)中，dn,f为dn到df的位移，dp,r为dp到dr的位移，m为dn到df之间的曼哈顿距离；式(6)中，t为调节系数,ω是角度约束系数；式(8)中，d'n为修改后的状态节点。
a(n)＝ω×(θn-θn-1) (9)
a(n)(式(9))是穿刺角度约束函数，θn当前穿刺针朝向角，θn-1是上一时刻穿刺针朝向角，a(n)越小，偏转角度越小，全局路径的转折点就越少。
通过对状态节点生长进行修改，使快速搜索随机树算法很好的应用于穿刺的路径规划中，同时在RRT算法基础上，在规划路径时加入改进A*寻路思想，提高搜索效率，使得规划的路径是与最优路径接近的次优路径。在实际穿刺过程中，提出一种区域控制函数Td(x,y)，该区域控制函数可以参照如下公式(10)：
(x-xf)2+(y-yf)2≤(R)2 (10)
式(10)中，xf和yf表示控制区域中心坐标，R为底面半径。
通过估计多次随机采样点到目标区域(目标点)的代价，每次选择最优状态节点添加到随机扩展树上，即由式(8)得到的d'n(xn,yn)满足式(10)时，即(x'-xf)2+(y'-yf)2≤(R)2成立，则该步迭代成功，节点可以添加进来。然后进行下一工作空间穿刺路径规划(否则重新由随机点dr生成式(1))，直到最终到达目标区域(目标点)；同时本发明对扩展方向进行控制改进，针对RRT算法中搜索方向的随机性导致搜索容易偏移目标区域(目标点)的问题，考虑到实际穿刺情况，为了使规划的路径更符合实际穿刺的需求，扩展树节点的搜索方向要控制在一定的角度之内，不能出现很大的迂回，并且需要避免出现局部极小值的问题。
在本发明一种可选实施例中，所述基于所述初始搜索路径和所述搜索区域规划目标搜索路径，包括：基于所述初始搜索路径和所述搜索区域确定在所述搜索区域中的目标节点；根据所述初始搜索路径和所述目标节点规划目标搜索路径。
本实施例中，根据所述初始搜索路径和所述目标节点规划目标搜索路径可以为将所述初始搜索路径的末节点与所述目标节点进行连线，确定所述初始搜索路径的末节点与所述目标节点的路径，进而确定所述初始搜索路径的起始节点至所述目标节点的目标搜索路径。
在本发明一种可选实施例中，所述基于所述初始搜索路径和所述搜索区域确定在所述搜索区域中的目标节点，包括：采用A星算法的估价函数估算所述搜索区域中每个状态节点与所述初始搜索路径的路径估计值；确定所述路径估计值的最小值对应的状态节点为目标节点。
本实施例中，所述估价函数估算可以根据实际情况进行确定，作为一种示例，所述估价函数估算可以为f(n)＝g(n)+h(n)，f(n)表示节点n到目标点的总代价，g(n)表示沿着根节点至当前路径节点n的实际代价，h(n)为节点n到目标点的路径的预估代价。在具体穿刺针任务中，f(n)是节点n的从起点(穿刺靶点)到终结点(皮肤上的穿刺进针区域)的估价函数，g(n)表示工作环境中从根节点到当前n节点的代价值，h(n)是从n节点到终止点的最优路线的预估成本。
确定所述路径估计值的最小值对应的状态节点为目标节点可以为从所述路径估计值中选出所述路径估计值的最小值，将所述最小值对应的状态节点为目标节点。
本发明实施例提供的三维路径规划方法，通过改进的快速搜索随机树算法的思想与穿刺路径规划应用情形相结合，在RRT算法原理和RRT算法的构建方式基础上，修改状态节点的生长方式和控制扩展方向，加入改进A*寻路思想，并在搜索过程中避免局部极小值。
为了方便理解，这里示例一种三维路径规划方法为一种基于改进RRT算法和A*算法的三维路径规划方法，
快速搜索随机树(RRT:Rapid-exploration Random Tree)算法是一种比较常用的应用在机器人上的路径规划的一种方法，主要解决如何在有障碍的空间中自主探索一条从给定的起点到适当终点的运动路径，从而在运动的过程中，通过规划出的全局优化路径，无碰、安全地绕过障碍物。RRT算法能够搜索出从根节点到终结点的避开障碍物的可行路径。但基本的RRT算法没有考虑穿刺实际情形，搜索的路径不符合穿刺针的穿刺轨迹。RRT算法在搜索点的过程中对于采样点的选择有着很大的随机性和盲目性，使得直接使用该算法求解规划的路径和最优路径相比有着较大的差距，并且该算法直接求解所得的搜索结果不能应用于穿刺针的操作过程，还有必要将获得的搜索结果做优化处理得到接近直线穿刺路线。针对RRT算法搜索的随机性和盲从性，对基本算法进行改进以满足穿刺的实际需求。
本发明将改进的快速搜索随机树算法的思想与穿刺路径规划应用情形相结合，在RRT算法原理和RRT算法的构建方式基础上，修改状态节点的生长方式和控制扩展方向，加入改进A*寻路思想，并在搜索过程中避免局部极小值。其中，由RRT算法原理参照前面公式(1)至公式(10)，在此不再赘述。
通过估计多次随机采样点到目标区域(目标点)的代价，每次选择最优状态节点添加到随机扩展树上，即由前面公式(8)得到的d′n(xn，yn)满足前面公式(10)时，即(x′-xf)2+(y′-yf)2≤(R)2成立，则该步迭代成功，节点可以添加进来。然后进行下一工作空间穿刺路径规划(否则重新由随机点dr生成式(1))，直到最终到达目标区域(目标点)；同时本发明对扩展方向进行控制改进，针对RRT算法中搜索方向的随机性导致搜索容易偏移目标区域(目标点)的问题，考虑到实际穿刺情况，为了使规划的路径更符合实际穿刺的需求，扩展树节点的搜索方向要控制在一定的角度之内，不能出现很大的迂回，并且需要避免出现局部极小值的问题。
穿刺针三维轨迹规划过程如下：
目前穿刺手术无法确定穿刺区域，只能凭借医生经验实现穿刺工作。穿刺针三维轨迹规划也包括表皮刺入点位置的选择，针尖刺入点的选择可以看作是穿刺针针尖可到达范围的逆向求解问题。目标靶点位置作为已知条件，针尖刺入点的区域可以依据穿刺针可达区域逆向计算求解出来。如图2所示，对于穿刺目标靶点T，穿刺针尖能够穿刺的两个临界点是point1和point2，如果穿刺点不在point1和piont2之间，将无法到达目标点T。
穿刺针整体路径规划任务主要有以下四个步骤。
1、确定首个搜索节点区域。将整个搜索工作区域离散成三维网格空间，假设目标区域为Areagoal，Areagoal是皮肤上以一可穿刺点为中心，某长度为半径范围内的不规则曲面区域。然后开始搜索并获取三维工作区间上的穿刺路径信息。以穿刺目标靶点为扩展搜索树的根节点进行搜索，当搜索树扩展至某点后进行规划，规划流程图如图4所示。穿刺路径的首个状态节点的选取是在穿刺终止点和根节点之间直线θ(10度)度内，距离目标点为一个步长h的大小(h根据实际穿刺手术中空间网格单位确定)。如图3所示，以目标靶点p1为顶点，以靶点和穿刺起始点p2之间的连线为中心线，绘制出一个高度为一步长,顶角为2θ的圆锥体，这个锥体底部区域(浅色)内空间就是扩展树首个搜索节点区域。如图4所示，若每个工作空间节点都能够成功被添加进来(即式(8)满足(x′-xf)2+(y′-yf)2≤(R)2)，则可以直接到达目标区域或目标点(皮肤上的穿刺初始进针点区域)，若此刻最终没有搜寻到最佳节点，则舍弃当前节点，从当前树节点的父节点开始继续规划，直至规划出可行的穿刺路径，则完成从初始点到目标区域的规划。
在图4中，通过某状态节点在某时刻的位置及状态可以确定穿刺针在工作空间的可达区域，接着使用改进的RRT算法和A星算法尝试寻求最优路径，并使用碰撞检测算法(本文使用OBBTree包围盒树检测算法)判定路径是否与障碍物(如重要器官组织)发生碰撞。若当前规划路径成功避开障碍物，则判断该节点是否到达目标区域，若没有，则将该节点添加到扩展搜索树，否则，结束路径规划流程；若当前规划穿刺路径与障碍物发生碰撞，则重新规划次优路径，并判断该路径是否避开障碍物，重复以上步骤，直到流程结束。
2、规划出第一个工作空间的搜索路径。在本文中，为了减少扩展方式的随机性，使得树的生长朝着目标点方向快速收敛，在RRT算法的基础上，借鉴了A*算法思想中寻找最优路径的思想，在构建扩展树的过程中，减少扩展树搜索节点数，减少树生长的随机性，使得规划的穿刺路径接近最短路径。A*算法是一种典型的启发式算法，该算法通过选择适当的估价函数在实际应用中搜索最优路径。A*算法的估价函数为f(n)＝g(n)+h(n)，f(n)表示节点n到目标点的总代价，g(n)表示沿着根节点至当前路径节点n的实际代价，h(n)为节点n到目标点的路径的预估代价。在具体穿刺针任务中，f(n)是节点n的从起点(穿刺靶点)到终结点(皮肤上的穿刺进针区域)的估价函数，g(n)表示工作环境中从根节点到当前n节点的代价值，h(n)是从n节点到终止点的最优路线的预估成本。
下面介绍搜索空间中引入A*寻路思想，通过对Open表存储和节点搜索方向进行优化，提高算法效率，求解最短路径。由估价函数f(n)可以看出h(n)的选取决定着最优路径求解的结果，为了更好的应用A*算法，首先需要增加搜索方向，传统的A*算法8邻域扩展只能上下左右移动，不能得到最佳路径。三维工作环境中为了更好地规划路径，将搜索空间扩展至26个邻域，在搜索过程中把待考察未访问过的节点加入扩展列表Open表中(如下步骤4)，并使用小根堆存储的方式对节点f值排序，降低遍历节点时间代价，加快收敛，具体过程如下：
(1)建立Open表和Close表，将障碍点放入Close表中，起始点纳入Open表中，检查输入邻接点，确定其在工作环境范围内，使用小根堆存储结构；
(2)对第一层邻域点(1-8邻域)进行判断，检查邻接点是否在Close表内或是否为障碍点，若不是障碍点，则可以对其进行扩展。对Open表进行分治，若Open表长度不为0，采用小根堆存储分治法找出最小代价节点M，移出Open中该节点并插入到close表中，并根据情况更新邻接点的父节点；若Open表中没有此点，使用open_insert方法将此邻接点添加到Open表。
(3)对第二层(9-26邻域)所有节点进行判断，判断该点是否为障碍点，若不是障碍点，则检查从起点开始到此邻接点是否途经障碍点，若有障碍点，则舍弃该邻接点，若途经点区域为自由区，则检查Open表中是否存在该邻接点，若Open表长度不为0，对Open表进行分治，采用小根堆存储分治法找出最小代价节点M；若Open表中没有此点，使用open_insert方法将此邻接点添加到Open表；
(4)对邻接点进行逐层判断(以输入点为中心的两层所有邻接点)，判断待扩展节点x的26邻接点能否加入扩展节点Open表中，即将途经非障碍点或自由区域点纳入扩展节点，确保以输入点为中心的26邻域被覆盖；
(5)通过小根堆存储分治法将Open表中f估值最小的节点作为最优节点，并纳入Close表中；
(6)若最小代价节点M是目标节点，表明寻路成功，从最佳节点回溯到原始节点，退出，若不是，跳到第(2)步。
通过对A*算法进行改进，使得每次扩展节点过程中可选邻域由原来的8个扩增至26个，增加了更多的连续搜索方向，同时通过小根堆存储分治法提高工作效率。在算法设计中，子函数open_insert()表示将可选取的待扩展结点插入Open表中，min_f()表示Open表中值最小的节点，Delete()表示将Open表中的节点移除，并纳入Close表中,T(M)表示节点M所处的状态，b(N)＝M表示M节点的前向指针为N节点。为了方便理解，图5为本发明实施例三维路径规划方法中26领域A*算法的示意图，通过输入初始输入点和目标区域，便可以输出从初始点到终点的规划路径。
在三维搜索空间中，在使用RRT算法规划搜索路径时，增加新的叶节点xnew的方式中引入改进的A*算法(即扩展26邻域和Open表分治查找最小代价节点)，对于每个新增节点的选择，通过计算每个叶节点到下一状态节点的估价值作为选取依据。在扩展树生长的时候，通过随机采样点xrand可以计算出一系列临时候选节点xtemp并存入Open表，根据公式(11)来计算。
其中，xparent是当前扩展树中最后一次加入的新叶节点，即xtemp的父节点。将到下一状态节点代价估值最小的节点加入到Close表中。对于状态节点n,若由式(8)得到的d'n(xn,yn)满足式(10)时，则将该节点添加到扩展树，进行下一空间路径规划。
3、确定当前计划的穿刺路线有没有到达目标区域(穿刺针进针区域)。通过计算穿刺规划路径的终点和目标终点的距离d，和设定好的阈值ε作比较，来判断规划路径有没有到达目标中心。如果d值比ε值小，则当前搜索的穿刺路径成功抵达目标区域，穿刺路径规划完成。如果d大于ε，则当前规划的穿刺针穿刺轨迹没有到达目标区域，转到第4步。
4、在经过第3步过程之后，如果当前穿刺规划路径还未抵达目标范围空间，则需要进行扩展树路径搜索并确定是否达到终点，通过加入方向引导函数
避免局部极小值，具体的步骤如下(a、b、c、d)。
a、继续搜索下一段穿刺路径。和首个路径状态节点选取及路径搜索方法相似，首先要确定下一个扩展搜索树状态节点的区域空间，即第一段路径末节点和目标点(目标区域)连线θ度范围内的区域(图3)为下一段穿刺路径节点的选取范围。
b、避免局部极小值问题。在路径搜索过程中容易出现局部极小值的情况，为了解决局部最小值问题，在考虑下一节点的选择因素中加入与节点扩展方向和路径搜索长度相关的方向引导函数
(公式(14))，在考虑扩展区域和路径长度的同时，总是选择转角最小的路径到达下一节点。在RRT算法中引入该函数使得在扩展树生长时，总是朝着目标区域方向，按照式(12)和式(13)，在选取新节点b的时候总是选择离下一状态节点最近的路径搜索节点。
式(14)
中，/>
为方向引导函数，λ为函数影响系数，且0≤λ≤1/>
是关于角度和距离的函数，反映了选择下一节点行进方向的矢量相关程度。
θa和θb为上一节点和下一节点的转向角度，θi∈[-π，π]，lac为边集合中的一边。
在本文中，如果不等式
成立，则按照式(12)把最大可能的节点b值选入随机树构造的解；否则，按式(13)选择下一节点b。这样规划的路径在绕开障碍物的同时也会避免出现局部极小值的问题，在规划的路径朝着目标方向搜索的时候，会尽量朝着未探索过的空旷区域延申。为了方便理解，具体的算法步骤如图6所示，图6为本发明实施例三维路径规划方法中改进的RRT算法示意图；其中返回值Success表示到达目标区域xgoal，Failure表示寻找新的叶节点失败。c、下一个工作空间的路线搜索。此空间规划的方法和上一个搜索空间的规划方式相一致。
d、确定当前规划的穿刺路径有没有到达穿刺目标区域(穿刺进针区域)。如果没有到达目标区域，则返回上面步骤a再重新规划路径。若已抵达目标区域，则穿刺路径规划任务结束。
为了方便理解，这里示例出本发明实施例三维路径规划方法中硬针穿刺的一种应用场景示意图，图7为本发明实施例三维路径规划方法中硬针穿刺的一种应用场景示意图，如图7所示。需要规划一条从T点开始，至目标P点某个阈值区间位置的最佳穿刺路径，从p1点开始规划，L1是第一段工作空间规划路径，L2是第二段工作空间规划路径。
第一个工作区间在起始点和目标点连线角度θ(初始10度)以内，距起始节点一个步长h(h根据实际穿刺手术中空间网格单位确定)的距离，绘制一个顶角是2θ度的锥体，锥体底面是第一段穿刺路径末节点的选取范围；
第二个工作区间的节点的穿刺范围和第一个工作区间类似，第一段节点的末节点作为第二个工作区间的起始节点开始规划路径，根据RRT扩展算法计算一系列随机状态节点xrand，若起始点和状态节点的运动直线是无碰撞的，则可根据计算的角度θ(公式(9))和步长h(公式(2))绘制一个顶角是2θ度，运动直线方向步长为h高度的椎体。当第i个节点位置越接近最佳位置时，步长越小，当第i个节点位置离最佳位置较远时，步长越大。要尽量保持公式(9)的值小，值越小，路径偏转代价越小。
在每个工作空间内，根据RRT计算选取扩展节点，公式(1)是扩展节点坐标，通过区域控制函数Td(x,y)，节点表达式满足式(10)，则将节点添加到扩展树中，进行下一空间路径搜索。
在扩展树生长的时候引入改进的A*算法，增加新的叶节点xnew的方式中引入改进的A*算法(即扩展26邻域和Open表分治查找最小代价节点)，通过随机采样点xrand可以计算出一系列临时候选节点xtemp并存入Open表(公式(11))，将到下一状态节点代价估值最小的节点加入到Close表中。对于状态节点xtemp,若由式(8)得到的d'n(xn,yn)满足式(10)时，则可以被添加到扩展树。
若添加的节点xnew到达目标区域，则规划完成。
若没有到达目标区域，对扩展树进行回溯，避免局部极小值问题。
在考虑下一节点的选择因素中加入与节点扩展方向和路径搜索长度相关的方向引导函数
(公式(14))，在考虑扩展区域和路径长度的同时，总是选择转角最小的路径到达下一节点。在RRT算法中引入该函数使得在扩展树生长时，总是朝着目标区域方向，按照式(12)和式(13)，在选取新节点b的时候总是选择离下一状态节点最近的路径搜索节点。如果不等式/>
成立，则按照式(12)把最大可能的节点b值选入随机树构造的解；否则，按式(13)选择下一节点b。直至到达目标区域。
采用本发明实施例的技术方案，具备以下几方面的技术效果：第一，提出了一种具有引导式的RRT路径规划算法，并在搜索过程中解决了局部极小值问题。第二，首次将该改进算法应用于硬针穿刺路径规划中。第三，在RRT算法基础上引入A*算法思想并进行改进，能实现目标区域快速收敛，减少搜索数量，降低搜索随机性，使得规划的结果接近最短穿刺路径。
本实施例提出一种三维路径规划装置，图8为本发明实施例三维路径规划装置的组成结构示意图，如图8所示，应用于包括硬针穿刺的场景，所述装置200包括：获得单元201、确定单元202、规划单元203和判断单元204，其中：
所述获得单元201，用于获得所述硬针的穿刺目标位置和刺入位置，根据所述刺入位置确定目标区域，以及根据所述穿刺目标位置和所述刺入位置确定初始区域；
所述确定单元202，用于根据所述穿刺目标位置和所述初始区域规划初始搜索路径，获得初始搜索节点；对所述初始搜索节点进行扩展确定状态节点对应的搜索区域；
所述规划单元203，用于基于所述初始搜索路径和所述搜索区域规划目标搜索路径；
所述判断单元204，用于判断所述目标搜索路径是否到达所述目标区域，获得判断结果；在所述判断结果表明所述目标搜索路径到达所述目标区域的情况下，将所述目标搜索路径作为硬针穿刺的三维路径规划。
在其他的实施例中，所述判断单元204，还用于在所述判断结果表明所述目标搜索路径没有到达所述目标区域的情况下，将所述状态节点按预设方向进行扩展确定新节点对应的扩展区域；所述预设方向表征所述状态节点朝向所述目标区域的方向；根据所述目标搜索路径和所述扩展区域规划新搜索路径；判断所述新搜索路径是否到达所述目标区域，在所述新搜索路径到达所述目标区域的情况下，将所述新搜索路径作为硬针穿刺的三维路径规划。
在其他的实施例中，所述获得单元201，还用于获得初始穿刺角度和初始穿刺步长；根据所述穿刺目标位置、所述刺入位置、所述初始穿刺角度和所述初始穿刺步长确定圆锥体；将所述圆锥体的底部区域作为所述初始区域。
在其他的实施例中，所述获得单元201，还用于将所述穿刺目标位置作为扩展树的根节点；基于所述根节点和所述初始区域通过搜素随机树RRT算法规划可行路径；根据A星算法确定所述可行路径中的最优路径；判断所述最优路径是否成功避开障碍物；在所述最优路径成功避开障碍物的情况下，将所述最优路径作为初始搜索路径；将所述初始搜索路径在所述初始区域中的端点作为初始搜索节点。
在其他的实施例中，所述确定单元202，还用于根据搜素随机树RRT算法对所述初始搜索节点进行扩展，获得多个随机状态节点；基于所述初始搜索节点和所述多个随机状态节点中的每个随机状态节点确定多个状态节点；根据A星算法确定所述多个状态节点中的最佳状态节点；在所述初始搜索节点和所述最佳状态节点之间的运动直线是无碰撞路径的情况下，根据所述初始搜索节点的位置信息和所述最佳状态节点的位置信息确定第一穿刺步长；获得硬针穿刺角度的约束范围；基于所述初始搜索节点的硬针穿刺姿态信息和所述约束范围确定所述最佳状态节点处所述硬针的第一穿刺角度；根据所述初始搜索节点、所述最佳状态节点、所述第一穿刺步长和所述第一穿刺角度确定第一圆锥体；将所述第一圆锥体的底部区域作为所述搜索区域。
在其他的实施例中，所述获得单元201，还用于在所述初始搜索节点和所述最佳状态节点之间的运动直线不是无碰撞路径的情况下，获得第一状态节点；所述第一状态节点为所述多个状态节点中除所述最佳状态节点以外的任一状态节点；
所述确定单元202，还用于根据所述第一状态节点的位置信息和所述最佳状态节点的位置信息确定所述第一状态节点的第二穿刺步长；基于所述初始搜索节点的硬针穿刺姿态信息和所述约束范围更新所述第一状态节点，确定更新后第一状态节点处所述硬针的第二穿刺角度；根据所述更新后第一状态节点、所述第二穿刺步长和所述第二穿刺角度确定第二圆锥体；将所述第二圆锥体的底部区域作为所述搜索区域。
在其他的实施例中，所述规划单元203，还用于基于所述初始搜索路径和所述搜索区域确定在所述搜索区域中的目标节点；根据所述初始搜索路径和所述目标节点规划目标搜索路径。
在其他的实施例中，所述规划单元203，还用于采用A星算法的估价函数估算所述搜索区域中每个状态节点与所述初始搜索路径的路径估计值；确定所述路径估计值的最小值对应的状态节点为目标节点。
以上装置实施例的描述，与上述方法实施例的描述是类似的，具有同方法实施例相似的有益效果。对于本发明装置实施例中未披露的技术细节，请参照本发明方法实施例的描述而理解。
需要说明的是，本发明实施例中，如果以软件功能模块的形式实现上述的三维路径规划方法，并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实施例的技术实施例本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台控制服务器(可以是个人计算机、服务器、或者网络服务器等)执行本发明各个实施例所述方法的全部或部分。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read Only Memory，ROM)、磁碟或者光盘等各种可以存储程序代码的介质。这样，本发明实施例不限制于任何特定的硬件和软件结合。
对应地，本发明实施例提供一种三维路径规划设备，包括存储器和处理器，所述存储器存储有可在处理器上运行的计算机程序，所述处理器执行所述程序时实现上述实施例提供的控制方法中的步骤。
对应地，本发明实施例提供一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现上述实施例提供的控制方法中的步骤。
这里需要指出的是：以上存储介质和设备实施例的描述，与上述方法实施例的描述是类似的，具有同方法实施例相似的有益效果。对于本发明存储介质和设备实施例中未披露的技术细节，请参照本发明方法实施例的描述而理解。
需要说明的是，图9为本发明实施例中三维路径规划设备的一种硬件实体结构示意图，如图9所示，该三维路径规划设备300的硬件实体包括：处理器301和存储器303，可选地，所述三维路径规划设备300还可以包括通信接口302。
可以理解，存储器303可以是易失性存储器或非易失性存储器，也可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是只读存储器(ROM，Read Only Memory)、可编程只读存储器(PROM，Programmable Read-Only Memory)、可擦除可编程只读存储器(EPROM，Erasable Programmable Read-Only Memory)、电可擦除可编程只读存储器(EEPROM，Electrically Erasable Programmable Read-Only Memory)、磁性随机存取存储器(FRAM，ferromagnetic random access memory)、快闪存储器(Flash Memory)、磁表面存储器、光盘、或只读光盘(CD-ROM，Compact Disc Read-Only Memory)；磁表面存储器可以是磁盘存储器或磁带存储器。易失性存储器可以是随机存取存储器(RAM，Random AccessMemory)，其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的RAM可用，例如静态随机存取存储器(SRAM，Static Random Access Memory)、同步静态随机存取存储器(SSRAM，Synchronous Static Random Access Memory)、动态随机存取存储器(DRAM，Dynamic Random Access Memory)、同步动态随机存取存储器(SDRAM，SynchronousDynamic Random Access Memory)、双倍数据速率同步动态随机存取存储器(DDRSDRAM，Double Data Rate Synchronous Dynamic Random Access Memory)、增强型同步动态随机存取存储器(ESDRAM，Enhanced Synchronous Dynamic Random Access Memory)、同步连接动态随机存取存储器(SLDRAM，SyncLink Dynamic Random Access Memory)、直接内存总线随机存取存储器(DRRAM，Direct Rambus Random Access Memory)。本发明实施例描述的存储器303旨在包括但不限于这些和任意其它适合类型的存储器。
上述本发明实施例揭示的方法可以应用于处理器301中，或者由处理器301实现。处理器301可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器301中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器301可以是通用处理器、数字信号处理器(DSP，Digital Signal Processor)，或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。处理器301可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者任何常规的处理器等。结合本发明实施例所公开的方法的步骤，可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于存储介质中，该存储介质位于存储器303，处理器301读取存储器303中的信息，结合其硬件完成前述方法的步骤。
在示例性实施例中，设备可以被一个或多个应用专用集成电路(ASIC，Application Specific Integrated Circuit)、DSP、可编程逻辑器件(PLD，ProgrammableLogic Device)、复杂可编程逻辑器件(CPLD，Complex Programmable Logic Device)、现场可编程门阵列(FPGA，Field-Programmable Gate Array)、通用处理器、控制器、微控制器(MCU，Micro Controller Unit)、微处理器(Microprocessor)、或其他电子元件实现，用于执行前述方法。
在本发明所提供的几个实施例中，应该理解到，所揭露的方法和装置，可以通过其他的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，如：多个单元或组件可以结合，或可以集成到另一个观测量，或一些特征可以忽略，或不执行。另外，所显示或讨论的各组成部分相互之间的通信连接可以是通过一些接口，设备或单元的间接耦合或通信连接，可以是电性的、机械的或其他形式的。
上述作为分离部件说明的单元可以是、或也可以不是物理上分开的，作为单元显示的部件可以是、或也可以不是物理单元，即可以位于一个地方，也可以分布到多个网络单元上；可以根据实际的需要选择其中的部分或全部单元来实现本实施例的目的。
本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于计算机可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：移动存储设备、只读存储器(ROM，Read-Only Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
或者，本发明实施例上述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实施例的技术实施例本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台设备(可以是个人计算机、服务器、或者网络设备等)执行本发明各个实施例所述方法的全部或部分。而前述的存储介质包括：移动存储设备、ROM、磁碟或者光盘等各种可以存储程序代码的介质。
本发明是实例中记载的三维路径规划方法、装置、设备和存储介质只以本发明所述实施例为例，但不仅限于此，只要涉及到该三维路径规划方法、装置、设备和存储介质均在本发明的保护范围。
应理解，说明书通篇中提到的“一个实施例”或“一实施例”意味着与实施例有关的特定特征、结构或特性包括在本发明的至少一个实施例中。因此，在整个说明书各处出现的“在一个实施例中”或“在一实施例中”未必一定指相同的实施例。此外，这些特定的特征、结构或特性可以任意适合的方式结合在一个或多个实施例中。应理解，在本发明的各种实施例中，上述各过程的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。
需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者装置中还存在另外的相同要素。
本发明所提供的几个方法实施例中所揭露的方法，在不冲突的情况下可以任意组合，得到新的方法实施例。
本发明所提供的几个产品实施例中所揭露的特征，在不冲突的情况下可以任意组合，得到新的产品实施例。
本发明所提供的几个方法或设备实施例中所揭露的特征，在不冲突的情况下可以任意组合，得到新的方法实施例或设备实施例。
以上所述，仅为本发明的实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。
Claims (11)
1.一种三维路径规划方法，其特征在于，应用于包括硬针穿刺的场景，所述方法包括：
获得所述硬针的穿刺目标位置和刺入位置，根据所述刺入位置确定目标区域，以及根据所述穿刺目标位置和所述刺入位置确定初始区域；
根据所述穿刺目标位置和所述初始区域规划初始搜索路径，获得初始搜索节点；对所述初始搜索节点进行扩展确定状态节点对应的搜索区域；
基于所述初始搜索路径和所述搜索区域规划目标搜索路径；
判断所述目标搜索路径是否到达所述目标区域，获得判断结果；在所述判断结果表明所述目标搜索路径到达所述目标区域的情况下，将所述目标搜索路径作为硬针穿刺的三维路径规划。
2.根据权利要求1所述的方法，其特征在于，所述方法还包括：
在所述判断结果表明所述目标搜索路径没有到达所述目标区域的情况下，将所述状态节点按预设方向进行扩展确定新节点对应的扩展区域；所述预设方向表征所述状态节点朝向所述目标区域的方向；
根据所述目标搜索路径和所述扩展区域规划新搜索路径；
判断所述新搜索路径是否到达所述目标区域，在所述新搜索路径到达所述目标区域的情况下，将所述新搜索路径作为硬针穿刺的三维路径规划。
3.根据权利要求1所述的方法，其特征在于，所述根据所述穿刺目标位置和所述刺入位置确定初始区域，包括：
获得初始穿刺角度和初始穿刺步长；
根据所述穿刺目标位置、所述刺入位置、所述初始穿刺角度和所述初始穿刺步长确定圆锥体；
将所述圆锥体的底部区域作为所述初始区域。
4.根据权利要求3所述的方法，其特征在于，所述根据所述穿刺目标位置和所述初始区域规划初始搜索路径，获得初始搜索节点，包括：
将所述穿刺目标位置作为扩展树的根节点；
基于所述根节点和所述初始区域通过搜素随机树RRT算法规划可行路径；
根据A星算法确定所述可行路径中的最优路径；
判断所述最优路径是否成功避开障碍物；在所述最优路径成功避开障碍物的情况下，将所述最优路径作为初始搜索路径；
将所述初始搜索路径在所述初始区域中的端点作为初始搜索节点。
5.根据权利要求1所述的方法，其特征在于，所述对所述初始搜索节点进行扩展确定状态节点对应的搜索区域，包括：
根据搜素随机树RRT算法对所述初始搜索节点进行扩展，获得多个随机状态节点；
基于所述初始搜索节点和所述多个随机状态节点中的每个随机状态节点确定多个状态节点；
根据A星算法确定所述多个状态节点中的最佳状态节点；在所述初始搜索节点和所述最佳状态节点之间的运动直线是无碰撞路径的情况下，根据所述初始搜索节点的位置信息和所述最佳状态节点的位置信息确定第一穿刺步长；
获得硬针穿刺角度的约束范围；基于所述初始搜索节点的硬针穿刺姿态信息和所述约束范围确定所述最佳状态节点处所述硬针的第一穿刺角度；
根据所述初始搜索节点、所述最佳状态节点、所述第一穿刺步长和所述第一穿刺角度确定第一圆锥体；
将所述第一圆锥体的底部区域作为所述搜索区域。
6.根据权利要求5所述的方法，其特征在于，所述方法还包括：
在所述初始搜索节点和所述最佳状态节点之间的运动直线不是无碰撞路径的情况下，获得第一状态节点；所述第一状态节点为所述多个状态节点中除所述最佳状态节点以外的任一状态节点；
根据所述第一状态节点的位置信息和所述最佳状态节点的位置信息确定所述第一状态节点的第二穿刺步长；
基于所述初始搜索节点的硬针穿刺姿态信息和所述约束范围更新所述第一状态节点，确定更新后第一状态节点处所述硬针的第二穿刺角度；
根据所述更新后第一状态节点、所述第二穿刺步长和所述第二穿刺角度确定第二圆锥体；
将所述第二圆锥体的底部区域作为所述搜索区域。
7.根据权利要求1所述的方法，其特征在于，所述基于所述初始搜索路径和所述搜索区域规划目标搜索路径，包括：
基于所述初始搜索路径和所述搜索区域确定在所述搜索区域中的目标节点；
根据所述初始搜索路径和所述目标节点规划目标搜索路径。
8.根据权利要求7所述的方法，其特征在于，所述基于所述初始搜索路径和所述搜索区域确定在所述搜索区域中的目标节点，包括：
采用A星算法的估价函数估算所述搜索区域中每个状态节点与所述初始搜索路径的路径估计值；
确定所述路径估计值的最小值对应的状态节点为目标节点。
9.一种三维路径规划装置，其特征在于，应用于包括硬针穿刺的场景，所述装置包括：获得单元、确定单元、规划单元和判断单元，其中：
所述获得单元，用于获得所述硬针的穿刺目标位置和刺入位置，根据所述刺入位置确定目标区域，以及根据所述穿刺目标位置和所述刺入位置确定初始区域；
所述确定单元，用于根据所述穿刺目标位置和所述初始区域规划初始搜索路径，获得初始搜索节点；对所述初始搜索节点进行扩展确定状态节点对应的搜索区域；
所述规划单元，用于基于所述初始搜索路径和所述搜索区域规划目标搜索路径；
所述判断单元，用于判断所述目标搜索路径是否到达所述目标区域，获得判断结果；在所述判断结果表明所述目标搜索路径到达所述目标区域的情况下，将所述目标搜索路径作为硬针穿刺的三维路径规划。
10.一种三维路径规划设备，包括存储器和处理器，所述存储器存储有可在处理器上运行的程序，其特征在于，所述处理器执行所述程序时实现权利要求1至8任一项所述方法中的步骤。
11.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至8任一项所述方法中的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111359931.7A CN116135159A (zh) | 2021-11-17 | 2021-11-17 | 三维路径规划方法、装置、设备和存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111359931.7A CN116135159A (zh) | 2021-11-17 | 2021-11-17 | 三维路径规划方法、装置、设备和存储介质 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116135159A true CN116135159A (zh) | 2023-05-19 |
Family
ID=86332617
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111359931.7A Pending CN116135159A (zh) | 2021-11-17 | 2021-11-17 | 三维路径规划方法、装置、设备和存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116135159A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117357249A (zh) * | 2023-12-05 | 2024-01-09 | 深圳惟德精准医疗科技有限公司 | 针对目标器官的穿刺路径规划方法及相关产品 |
-
2021
- 2021-11-17 CN CN202111359931.7A patent/CN116135159A/zh active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117357249A (zh) * | 2023-12-05 | 2024-01-09 | 深圳惟德精准医疗科技有限公司 | 针对目标器官的穿刺路径规划方法及相关产品 |
| CN117357249B (zh) * | 2023-12-05 | 2024-02-20 | 深圳惟德精准医疗科技有限公司 | 针对目标器官的穿刺路径规划方法及相关产品 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US10365110B2 (en) | Method and system for determining a path of an object for moving from a starting state to an end state set avoiding one or more obstacles | |
| WO2022148336A1 (zh) | 手术机器人及其引导手术臂移动的方法、控制装置 | |
| JP5442993B2 (ja) | ３ｄ器具経路計画立案、シミュレーションおよび制御システム | |
| JP5060619B2 (ja) | モーション計画方法、モーション計画システム及び記録媒体 | |
| CN106774347A (zh) | 室内动态环境下的机器人路径规划方法、装置和机器人 | |
| CN111678523B (zh) | 一种基于star算法优化的快速bi_rrt避障轨迹规划方法 | |
| CN111650941B (zh) | 基于RRT-Connect改进的医用移动机器人路径规划方法 | |
| CN109579854B (zh) | 基于快速扩展随机树的无人车避障方法 | |
| Pinzi et al. | The Adaptive Hermite Fractal Tree (AHFT): a novel surgical 3D path planning approach with curvature and heading constraints | |
| Torres et al. | Interactive-rate motion planning for concentric tube robots | |
| US11919170B2 (en) | Fast method for robot path planning with obstacle avoidance | |
| CN113156956B (zh) | 机器人的导航方法、芯片及机器人 | |
| Favaro et al. | Automatic optimized 3D path planner for steerable catheters with heuristic search and uncertainty tolerance | |
| CN116135159A (zh) | 三维路径规划方法、装置、设备和存储介质 | |
| CN113359746A (zh) | 基于改进双向RRT和Dijkstra融合算法的路径规划方法和装置 | |
| Caborni et al. | Risk-based path planning for a steerable flexible probe for neurosurgical intervention | |
| Fauser et al. | Planning nonlinear access paths for temporal bone surgery | |
| CN114494602A (zh) | 碰撞检测方法、系统、计算机设备和存储介质 | |
| Wu et al. | Motion planning of continuum tubular robots based on centerlines extracted from statistical atlas | |
| CN114427866A (zh) | 路径规划方法、电子设备以及存储介质 | |
| CN112338916A (zh) | 基于快速扩展随机树的机械臂避障路径规划方法及系统 | |
| CN110840534B (zh) | 穿刺速度规划方法、装置、穿刺设备以及计算机存储介质 | |
| Aghdam et al. | A novel path planner for steerable bevel-tip needles to reach multiple targets with obstacles | |
| CN109977455B (zh) | 一种适用于带地形障碍三维空间的蚁群优化路径构建方法 | |
| Jiang et al. | Learning humanoid reaching tasks in dynamic environments |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |