CN116560360A - 面向复杂动态场景的医用护理机器人实时动态路径规划方法及系统 - Google Patents

Abstract

本发明公开了一种面向复杂动态场景的医用护理机器人实时动态路径规划方法及系统，方法包括：S1、载入一张地图，并对其进行二值化处理；S2、对自由空间X_free中拟合的多边形进行地图凸多边形重构；S3、当机器人起点被确定后，利用改进的实时探索式搜索树路径规划方法，对地图进行路径搜索；S4、在终点确定以后，机器人以某一速度沿着路径朝终点走去，并使探索式搜索树搜索的树根节点随着机器人移动；S5、针对机器人行进过程中目标点改变的情况，从树的根节点进行重布线；S6、针对机器人行进过程中出现障碍物的情况，首先需要阻塞探索式搜索树中被障碍物占据的节点，以此来避免机器人碰撞障碍物。

Description

面向复杂动态场景的医用护理机器人实时动态路径规划方法 及系统

技术领域

本发明属于机器人路径规划技术领域，特别是涉及一种面向复杂动态场景的医用护理机器人实时动态路径规划方法及系统。

背景技术

路径规划是机器人研究领域的热门话题之一，它的目标是为机器人在无障碍空间中找到连接起始状态和目标状态的连续轨迹。路径规划算法在服务机器人导航、自动检测系统、工业自动化、自动驾驶汽车和机器人手术等多个领域具有广泛应用。

路径规划算法按采样方法分类，主要可以分为基于图搜索、基于采样两种。另外，规划出来的路径往往不能直接使用，需要进行轨迹优化，轨迹优化包括轨迹表示方法、轨迹优化目标和轨迹约束方法等方面。

基于图搜索的路径规划方法主要包括Dijkstra算法和A*算法等。但是，由于机器人所处的环境是连续且时变的，因此这些方法通常无法直接应用于路径规划中，需要对地图进行离散化表达。由于空间离散化方法的不同，这些算法的效果很大程度上取决于其离散化的精度。通常情况下，为了提高求解的精度，必须牺牲求解效率，而且随着问题规模的增加，计算成本呈指数增长。

基于采样的方法主要有快速随机搜索树算法(Rapidly Exploring RandomTree)、PRM算法、DWA算法，这类算法的优点在于其不用对地图进行离散化的表达，并且针对复杂问题(例如高维问题等)也可以快速找到可行解。但由于其采样方法往往具有一些随机性，其得到的解是渐进最优的。基于采样的方法被广泛应用于指导采样过程，但这些方法的泛化能力和可解释性较差，不能保证它们在所有情况下都能安全稳定地工作，如果因此发生不可预测的错误，将造成灾难性的后果。

综上，上述算法必须权衡搜索路径的优劣和搜索时间的长短，当允许改变目标点和添加障碍物时，如何利用好之前采样的信息，快速重新规划路线，是一个非常具有挑战性的问题。除此之外，路径搜索过程中，会花费大量时间和内存去搜索一些不必要的路径，如何区分并避免同类路径的搜索是一个急需解决的问题。

在医疗护理环境下，病房场景基本固定不变，如病床位置、柜子位置等，即地图环境基本不变，但有许多动态障碍物的出现，如病人、家属、医生的走动，针对以上场景，本发明提出了一种面向复杂动态场景的医用护理机器人实时动态路径规划方法及系统。

发明内容

为了克服现有技术中的缺陷，本发明提供一种面向复杂动态场景的医用护理机器人实时动态路径规划方法及系统，主要依靠地图凸多边形重构方法和实时探索式搜索树方法，其中地图凸多边形重构主要避免了路径规划过程中同类型路径的搜索，并使用了一种实时探索式搜索树动态路径规划方法，在改变目标点和添加障碍物时，不丢弃先前搜索的路径，实现快速重新规划路线。

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

一种面向复杂动态场景的医用护理机器人实时动态路径规划方法，其特征在于，所述方法具体包括以下步骤：

S1、载入一张地图，并对其进行二值化处理，其中黑色部分为不可通行区域，白色部分为可通行区域，并将其称为自由空间X_free，之后将地图进行多边形拟合，提取地图中的边界信息与角点信息；

S2、对自由空间X_free中拟合的多边形进行地图凸多边形重构，将自由空间X_free分割为多个凸多边形的集合，由于凸多边形的性质，凸多边形内部的路径是同伦道路，即可以视为相似的路径，对路径的搜索可以直接在凸多边形的边界上进行，从而避免重复路径搜索，提升搜索效率；

S3、当机器人起点被确定后，开始路径规划，利用改进的实时探索式搜索树路径规划方法，对地图进行路径搜索；具体包括：

S31、优先搜索凸多边形的每一条切割线的中点，可快速得到起点到每一个凸多边形的路径，因此，无论终点在哪，只需要判断终点在地图上的哪一个凸多边形中，就可以快速得到一条次优路径；

S32、在遍历完所有凸多边形的每一条切割线后，再随机重复采样凸多边形的割线上的点，来优化路径的长度；

S4、在终点确定以后，机器人以某一速度沿着路径朝终点走去，并使探索式搜索树搜索的树根节点随着机器人移动，从而不丢弃先前采样的路径，但也不必等待树完全构建，因此搜索树扩展和机器人移动是交错的；

S5、针对机器人行进过程中目标点改变的情况，由于探索式搜索树搜索的根节点随机器人移动，并且从树的根节点进行重布线，可以利用先前采样的路径来快速得到一个次优解，并在后续树的采样过程中不断优化路径长度；

S6、针对机器人行进过程中出现障碍物的情况，首先需要阻塞探索式搜索树中被障碍物占据的节点，以此来避免机器人碰撞障碍物；然后需要判断出障碍物在哪一些凸多边形中，在后续的探索式搜索树采样过程中，只需要对障碍物所占据的凸多边形内部进行采样，由于采样被限制在障碍物所在凸多边形内部，极大提升了路径搜索效率，并更好地实现避障。

进一步的，在步骤S1中，在提取到的角点信息中，规定最外侧一层角点按逆时针排列，内侧一层角点按顺时针排列，其排列方式由右手定则来判断自由空间在哪一侧，即右手大拇指向上，四指指向为向量方向，此时手心方向为自由空间；

进一步的，在步骤S2中，所述地图凸多边形重构的过程包括：

S21:选取凹点；

对自由空间进行腐蚀膨胀操作来消除噪点，之后进行多边形拟合，多边形拟合的结果中包含各个多边形的顶点，按顺序遍历多边形拟合结果中的各个顶点，其中多边形外侧一层的顶点按逆时针排列，多边形内侧的顶点按顺时针排列，通过计算顶点两条边的叉积的方法来判断点是否为凹点，即角度大于180度的点；

具体的，设当前点为P_now,P_now的前一个点为P_end，后一个点为P_start，向量v₁为向量v₂为/>由向量叉积的性质可知，当v₁×v₂>0时，P_end在向量v₁的左侧；由右手定则判断得到的排列顺序可得，自由空间在v₁的右侧，即P_now的角度大于180度，为凹点；

S22:计算凹点的可视点；

在获得地图中的所有凹点以后，对每个凹点计算可视点，即凹点可以通过一条直线到达的点，并且不会触碰到障碍物和其他剖分线，具体方法为：

遍历地图上所有的边界，首先设当前点需要计算可视点的凹点为V_p，遍历边界的起始点为L_S，终点为L_E,当前遍历到的点为P,向量向量/>向量如果点P被阻挡，则点P在向量v₃和向量v₄之间，即在v₃右侧v₄左侧，或者在v₃左侧v₄右侧，(v₃×v₄)*(v₅×v₄)≤0；

当满足上述条件后，如果点P和点V_p不在边界点的同一侧，则点P

被阻挡，设如果(v₆×v₇)*(v₈×v₇)≤0，则P和点V_p不在边界点的同一侧；

如果点P同时满足(v₃×v₄)*(v₅×v₄)≤0和(v₆×v₇)*(v₈×v₇)≤0的情况，则P被阻挡，否则P为可视点；

S23：选取凹点的最优可视点；

在所有可视点中，计算最优的可视点，最优的定义为，可视点要离凹点的角平分线尽量近，同时需要满足离凹点尽量靠近并且不和现有的剖分线相交；具体方法为：

设凹点为V_p，可视点为P₁，计算凹点的角平分线为v₉，v₉以V_p为起点，并将其单位化，v₁₀是以V_p为起点，P₁为终点的单位化向量，即

定义最优可视点的目标函数如公式1：

需要选取目标函数最小的可视点，其中需要优先选取离角平分线v₉最近的点，所以在为负值，确保/>范围内的优先级比/>高；其次，为了考虑距离的因素，计算可视点和凹点的距离为L,在目标函数中加入距离因素，最后的目标函数如公式2：

选取凹点的最优可视点,连接凹点与最优可视点，将凹点切割为凸点，并将凹点与可视点连接的过程可视为一次地图凸多边形重构，连接线称为剖分线，如果凹点在一次剖分后角度仍然大于180度，则再次剖分，直到没有凹点为止；在完成一次剖分后，还需要检查连接凹点的最优可视点是否为凹点，如果是且被剖分为了凸点，遍历凹点时可以直接跳过；

判断剖分后角度是否大于180度的方法具体为，设凹点为V_p，凹点前一个点为P_end，后一个点为P_start，可视点为P₁，向量v₁₁为向量v₁₂为/>向量v₁₃为/>只有当(v₁₁×v₁₃)≤0，(v₁₂×v₁₃)≥0时才没有大于180度的角；

对地图上每一个凹点进行剖分后，即可将自由空间分割为多个凸多边形的集合。

进一步的，在步骤S2中，对于凸多边形内部的路径是同伦道路，可以视为相似的路径，具体方法为：

设X是拓扑空间，X中的所有道路记作P(X),所有以x₀为起点，x₁为终点的道路记作P(X；x₀,x₁)，所有以x₀为基点的回路记作Ω(X,x₀)；设f,g∈P(X；x₀,x₁),如果存在连续映射F:(I²→X),使得F(s,0)＝f(s),F(s,1)＝g(s),F(0,t)＝x₀,F(1,t)＝x₁，则称F为从f到g的道路同伦，记作即这两条道路之间可以连续地变化过去，是同类型的路径。

进一步的，在步骤S5中，探索式搜索树的根节点会随着机器人移动，其中节点的移动和搜索树的更新是相互交替的；在一次迭代中，根节点首先随着机器人移动，之后进行树的搜索，树的搜索包括树的扩张和重布线，树的扩张为由剖分线引导的节点采样；树的重布线分为从根节点重布线和随机重布线，从根节点重布线可以利用原先采样的信息来很好地解决目标点改变的情况与障碍物的躲避，随机重布线用于路径的优化。

进一步的，所述树的扩张的过程中，需要查找搜索树中离采样点最近的点，采用动态参考点树的数据结构，参考点树选择数据集中的一个点v作为“枢轴”，并将其他点与它的距离存储在一个半径为μ内的区域内，然后再以同样的方式处理这个区域的子集，形成一棵二叉树，这样就得到了一个参考点树,其计算公式如公式3所示：

其中S₁、S₂为两颗子树，d(s,v)为点s和v的距离计算公式，这里采用欧式距离。

进一步的，在步骤S6中，随着障碍物的增加，在新添加的障碍物周围的采样概率应该增加，而旧障碍物周围的采样概率应该减少，具体实现的方法为：

定义一个特殊的采样概率函数，设置障碍物数组为Obs＝[obs1,obs2,…,obsn],其中的障碍物按先后出现的顺序排列，第一个出现的障碍物为obs1，第n个出现的障碍物为obsn；按出现顺序定义一个概率权重数组，第一个出现障碍物权重为1，第n个出现的障碍物权重为n，权重数组为weights＝[1,2,…,n]，计算概率时，第i个障碍物被选中的概率如公式4所示：

其中sum(weights)为对weights数组中的值进行求和。

一种用于实现面向复杂动态场景的医用护理机器人实时动态路径规划方法的系统，其特征在于，包括：环境感知模块、位置定位模块、数据分析模块、数据记录模块以及数据传输模块；

环境感知模块，用于获取智能医疗护理机器人周围的环境数据，包括地图建立、语义信息、静态障碍物与动态障碍物信息；

位置定位模块，用于获取医疗护理机器人的高精度定位信息，包括里程计信息、IMU信息、RTK信息；

数据分析模块，用于医疗护理机器人周围复杂环境的场景识别，建立基于规则的决策输出信息，得到当前场景判断的结果；

数据记录模块，用于对传感器数据以及分析结果初始化信息进行记录；

数据传输模块，用于将分析所得数据和记录数据传输到主控制器，包括全局地图信息、语义信息、障碍物信息、里程计信息、IMU信息、RTK信息、场景判断信息。

与现有技术相比，本发明的有益效果体现在：

1、本发明通过一种地图凸多边形重构方法，利用在剖分线上进行采样，减少了探索式搜索树扩张过程中，对于重复路径的搜索；对于出现的动态障碍物，通过将采样点限制在障碍物所占据的凸多边形中，极大提升了避障效果。

2、在机器人移动过程中，探索式搜索树的根节点会随着机器人移动，并从根节点开始重布线，机器人移动和重布线是交替进行的，因此可以充分利用之前探索的路径实现实时路径规划，并且可以躲避动态障碍物。

附图说明

图1为本发明实施例一中提供的一种面向复杂动态场景的医用护理机器人实时动态路径规划方法及系统的原理框图。

图2a为本发明实施例一中的地图二值化效果图。

图2b为本发明实施例一中的地图多边形拟合效果图。

图2c为本发明实施例一中的地图拟合效果图。

图2d为图2c中A的地图拟合效果细节图。

图3为本发明实施例一中的多边形拟合后的角点排列顺序图。

图4为本发明实施例一中的凹点判断方法图。

图5为本发明实施例一中的提取地图凹点结果图。

图6为本发明实施例一中的判断是否被阻挡条件一原理图。

图7为本发明实施例一中的判断是否被阻挡条件二原理图。

图8为本发明实施例一中的一个凹点的所有可视点提取结果图。

图9为本发明实施例一中的凹点角平分线与可视点关系示意图。

图10为本发明实施例一中的可视点选取目标函数图。

图11为本发明实施例一中的单次地图凸多边形重构结果图。

图12为本发明实施例一中的判断剖分后是否为凹点原理图。

图13为本发明实施例一中的地图凸多边形重构结果图。

图14为本发明实施例一中的同伦路径原理图。

图15a为本发明实施例一中的在剖分线中点采样效果图一。

图15b为本发明实施例一中的在剖分线中点采样效果图二。

图16为本发明实施例一中的在剖分线上采样效果图。

图17a为本发明实施例一中的参考点树原理图一。

图17b为本发明实施例一中的参考点树原理图二。

图18a为本发明实施例一中的第一次目标变化后路径规划效果图。

图18b为本发明实施例一中的第二次目标变化后路径规划效果图。

图19a为本发明实施例一中的在障碍物附近采样效果图。

图19b为图19a中B处的在障碍物附近采样效果细节图。

图20a为本发明实施例一中的躲避障碍物效果对比图一。

图20b为本发明实施例一中的躲避障碍物效果对比图二。

图20c为图20a中C处的躲避障碍物效果对比细节图。

图20d为图20b中D处的躲避障碍物效果对比细节图。

图21a为本发明实施例一中的躲避动态障碍物效果对比图一

图21b为本发明实施例一中的躲避动态障碍物效果对比图二。

图21c为图21a中E处的躲避动态障碍物效果对比细节图。

图21d为图21b中F处的躲避动态障碍物效果对比细节图。

图22为本发明实施例一中的采样点转向躲避障碍物原理图。

图23为本发明实施例二中的环境感知与定位建图流程图。

具体实施方式

以下结合附图对本发明实施例的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明实施例，并不用于限制本发明实施例。

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

下面将参考附图并结合示例性实施例来详细说明本发明。

实施例一

如图1所示，本实施例的一种面向复杂动态场景的医用护理机器人实时动态路径规划方法及系统，主要通过地图凸多边形重构的方法，将自由空间分割成多个凸多边形的集合，每个凸多边形内部的路径可以视为同类路径，基于探索式搜索树的实时路径规划可以在凸多边形的剖分线上进行采样，减少了重复路径的搜索，加快了路径搜索效率。并且针对机器人行进过程中目标点变化和出现动态障碍物的情况，探索式搜索树通过从根节点重布线和随机重布线来动态更新探索式搜索树的采样树，充分利用之前的采样信息来实现快速实时动态路径规划。

探索式搜索树算法的基本思想是从起始点开始，在自由空间中随机采样节点，并使用最近邻搜索找到最接近该节点的树节点。然后，通过连接操作将这两个节点连接起来，并进行可行性检查。如果连接的路径是可行的，则将新的节点添加到树中，并重复上述步骤，直到达到目标状态。

本实施例的一种面向复杂动态场景的医用护理机器人实时动态路径规划方法，其主要包括如下步骤：

步骤一：载入一张地图，进行腐蚀膨胀操作来消除噪点，之后其进行二值化处理，其效果如图2a所示，其中黑色部分不可通行区域，白色部分为可通行区域，将其称为自由空间X_free，之后将地图进行多边形拟合，其效果如图2b所示，拟合后的对比如图2c所示，拟合效果细节如图2d所示，主要是为了提取地图中的边界信息与角点信息。

在提取到的角点信息中，规定最外侧一层角点按逆时针排列，内侧一层按顺时针排列，其排列方式如图3所示，图中最外侧角点用圆形表示，内侧角点用方形表示，外侧角点按逆时针排列，内侧角点按顺时针排列，由此可由右手定则来判断自由空间在哪一侧，具体方法为，右手大拇指向上，四指指向为向量方向，此时手心方向为自由空间。

步骤二：对自由空间中拟合的多边形进行地图凸多边形重构，将自由空间X_free分割为多个凸多边形的集合，由于凸多边形的性质，凸多边形内部的路径是同伦道路，即可以视为相似的路径，因此对路径的搜索可以直接在凸多边形的边界上进行，从而避免重复路径搜索，提升搜索效率。

所述地图凸多边形重构过程包括：

步骤2.1，选取凹点；

按顺序遍历多边形拟合结果中的各个顶点，其中多边形外侧一层的顶点按逆时针排列，多边形内侧按顺时针排列。通过计算顶点两条边的叉积的方法来判断点是否为凹点，即角度大于180度的点。具体方法如图4，设当前点为P_now,P_now的前一个点为P_end，后一个点为P_start，向量v₁为向量v₂为/>由向量叉积的性质可知，当v₁×v₂>0时，P_end在向量v₁的左侧；由步骤一中点的排列顺序可得，自由空间在v₁的右侧，即P_now的角度大于180度，为凹点，如图5是在地图上找到的所有凹点，用圆形表示。

步骤2.2，计算凹点的可视点；

在获得地图中的所有凸点以后，对每个凸点计算可视点，即凸点可以通过一条直线到达的点，并且不会触碰到障碍物和其他剖分线，具体方法为：

遍历地图上所有的边界，首先设当前点需要计算可视点的凹点为V_p，遍历边界的起始点为L_S终点为L_E,当前遍历到的点为P,向量向量/>向量如果点P被阻挡，则点P在向量v₃和向量v₄之间，即在v₃右侧v₄左侧，或者在v₃左侧v₄右侧，(v₃×v₄)*(v₅×v₄)≤0，情况如图6所示,细斜线部分为P点可能范围。

当满足上述条件后，如果点P和点V_p不在边界点的同一侧，则点P被阻挡，设如果(v₆×v₇)*(v₈×v₇)≤0，则P和点V_p不在边界点的同一侧，情况如图7所示,细斜线部分为P点可能范围。

如果点P同时满足图6和图7的情况，则P被阻挡，否则P为可视点，如图8所示，正方形为某一个凹点，圆形为凹点的所有可视点。

步骤2.3，选取凹点的最优可视点；

在所有可视点中，计算最优的可视点，最优的定义为，可视点要离凹点的角平分线尽量近，同时需要满足离凹点尽量靠近并且不和现有的剖分线相交。具体方法为：

设凹点为V_p，可视点为P₁，计算凹点的角平分线为v₉，v₉以V_p为起点，并将其单位化，v₁₀是以V_p为起点，P₁为终点的单位化向量，即两向量如图9所示。

定义最优可视点的目标函数如公式1：

其函数图像如图10所示，需要选取目标函数最小的可视点，其中需要优先选取离角平分线v₉最近的点，所以在为负值，确保/>范围内的优先级比高；其次，为了考虑距离的因素，计算可视点和凹点的距离为L,在目标函数中加入距离因素，最后的目标函数如公式2：

选取凹点的最优可视点,如图11为某一个凹点的最优可视点，图中的正方形为凸点，圆形为可视点，空心圆形为最优可视点，连接凹点与最优可视点，将凹点切割为凸点。并将凹点与可视点连接的过程可视为一次地图凸多边形重构，连接线称为剖分线，如果凹点在一次剖分后角度仍然大于180度，则再次剖分，直到没有凹点为止。在完成一次剖分后，还需要检查连接凹点的最优可视点是否为凹点，如果是且被剖分为了凸点，遍历凹点时可以直接跳过。

判断剖分后角度是否大于180度的方法具体为，设凹点为V_p，凹点前一个点为P_end，后一个点为P_start，可视点为P₁，向量v₁₁为向量v₁₂为/>向量v₁₃为/>只有当(v₁₁×v₁₃)≤0，(v₁₂×v₁₃)≥0时才没有大于180度的角，具体情况如图12所示。

对地图上每一个凹点进行剖分后，即可将自由空间分割为多个凸多边形的集合，剖分后的结果如图13，图中细一号的线为剖分线，

值得注意的是，剖分过程中，凹点选取的顺序，会影响剖分的结果，但不会影响后续的路径规划，因为最后都将自由空间转换为了凸多边形的集合，只是凸多边形有差异，凸多边形内部的路径是同伦道路，可以视为相似的路径，不会影响后续的路径规划。

对于凸多边形内部的路径是同伦道路，可以视为相似的路径。设X是拓扑空间，X中的所有道路记作P(X),所有以x₀为起点，x₁为终点的道路记作P(X；x₀,x₁)，所有以x₀为基点的回路记作Ω(X,x₀).设f,g∈P(X；x₀,x₁),如果存在连续映射F:(I²→X),使得F(s,0)＝f(s),F(s,1)＝g(s),F(0,t)＝x₀,F(1,t)＝x₁。则称F为从f到g的道路同伦，记作即这两条道路之间可以连续地变化过去，是同类型的路径，如图14所示，F(s,t₀)代表f(s)和g(s)同伦的路径，可视为相似路径。

步骤三：当机器人起点被确定后，开始路径规划，利用改进的实时探索式搜索树方法，对地图进行路径搜索。优先搜索凸多边形的每一条切割线的中点，可快速得到起点到每一个凸多边形的路径，如图15a和图15b所示，起点设置在右上角，浅色线段为剖分线，深色线段为探索式搜索树的采样树，在路径规划的起始阶段，探索式搜索树的采样点在剖分线的中点，能快速搜索整个地图。之后设置终点，无论终点在哪，只需要判断终点在地图上的哪一个凸多边形中，就可以快速得到一条次优路径。在遍历完所有凸多边形的每一条割线后，再随机重复采样凸多边形的割线上的点，来优化路径的长度，如图16所示，在采集完所有的剖分线的中点之后，开始随机在剖分线上的任意位置采样。

步骤四：在终点确定以后，机器人以某一速度，沿着路径朝终点走去，并使探索式搜索树搜索的树根节点随着机器人移动，从而不丢弃先前采样的路径，但也不必等待树完全构建，因为树扩展和机器人的移动是交错的。

探索式搜索树的根节点会随着机器人移动，其中节点的移动和搜索树的更新是相互交替的。在一次迭代中，根节点首先朝着机器人移动，之后进行树的搜索，树的搜索包括树的扩张和重布线，树的扩张为由剖分线引导的节点采样，树的重布线分为从根节点重布线和随机重布线，其中设置剖分线引导的节点采样时间为0.15s，从根节点进行重布线时间为0.1s，随机节点重布线时间为0.03s。从根节点重布线可以利用原先采样的信息来很好地解决目标点改变的情况与障碍物的躲避，随机重布线用于路径的优化。

探索式搜索树扩张的过程中，需要查找搜索树中离采样点最近的点，为了加快搜索效率，采用动态参考点树的数据结构，参考点树选择数据集中的一个点v作为“枢轴”，并将其他点与它的距离存储在一个半径为μ内的区域内，其原理图如图17a。然后再以同样的方式处理这个区域的子集，形成一棵二叉树，这样就得到了一个参考点树,其计算公式如公式3：

其中S₁、S₂为两颗子树，d(s,v)为点s和v的距离计算公式，这里采用欧式距离，其原理图如图17b。

为了适应需要频繁添加和删除数据点的应用场景，维护多颗参考点树，先将元素添加到池中，如果池的大小达到了最小树的大小，则使用池中的元素构建一棵参考点树。然后，合并大小相等的树，以保证最多有log(log(n))棵树，并且最大的树大约有个节点,其中n为数据点的数量，查询复杂度接近O(log n)。

步骤五：针对机器人行进过程中目标点改变的情况，由于探索式搜索树搜索的根节点随机器人移动，并且从树的根节点进行重布线，可以利用先前采样的路径来快速得到一个次优解，并在后续树的采样过程中不断优化路径长度如图18a所示，图中大圆形为终点，粗线段为规划出来的路线，第一次设置终点时，根据之前搜索的树快速规划出了一条路径，但很明显其结果是次优的，但在之后的过程中，终点位置改变，根据之前搜索的结果，这一次规划的的路径明显有了提升，如图18b所示。

步骤六：针对机器人行进过程中出现障碍物的情况，首先需要阻塞探索式搜索树中被障碍物占据的节点，以此来避免机器人碰撞障碍物。然后需要判断出障碍物在哪一些凸多边形中，在后续的探索式搜索树采样过程中，需要对障碍物所占据的凸多边形内部进行采样。如图19a所示，由于采样别限制在的障碍物所在凸多边形内部，极大提升了路径搜索效率，并更好地实现避障，其细节如图19b，图中的圆形为一个障碍物，其占据了两个凸多边形，之后的采样点快速集中在了这两个凸多边形中，并且并没有在其他凸多边形中浪费采样机会。如图20a和图20b所示，是路径规划系统实现的避障效果图，图中浅色圆形为终点，深色圆形为障碍物，加粗线条为规划出来的路径，细线条为探索式搜索树，当规划出来的路径被障碍物阻塞时，由于搜索树的重布线和节点的采样，路径规划系统快速规划出来的一条次优路径，图20c和图20d是避障细节图。

随着障碍物的增加，在新添加的障碍物周围的采样概率应该增加，而旧障碍物周围的采样概率应该减少，具体实现的方法为定义一个特殊的采样概率函数，设置障碍物数组为Obs＝[obs1,obs2,…,obsn],其中的障碍物按先后出现的顺序排列，第一个出现的障碍物为obs1，第n个出现的障碍物为obsn。按出现顺序定义一个概率权重数组，第一个出现障碍物权重为1，第n个出现的障碍物权重为n，权重数组为weights＝[1,2,…,n]，计算概率时，第i个障碍物被选中的概率如公式2：

其中sum(weights)为对weights数组中的值进行求和。

对于障碍物会运动情况，由于采样树从根节点进行重布线，并且会在障碍物周围进行采样，路径规划系统可以实现快速躲避动态障碍物，并且不断优化路径，其效果如图21a图21b所示，在障碍物阻塞路径之后，路径规划系统快速重新规划出了一条路线，并且在障碍物周围的凸多边形区域进行采样，使得当障碍物移动之后，路径规划系统可以实现躲避动态障碍物，同时保证路径长度不会过长，其效果细节如图21c和图21d所示。

采样过程中，如果采样点和探索式搜索树上的临近节点之间有障碍物阻挡，可以对采样点进行转向来避开障碍物，以提升搜索效率，具体过程为，检测和临近点之间是否有障碍物，如果有，则将采样点重新设置为，临近点和采样点连线上没有障碍物的区域,如图22所示，x_nearest为探索式搜索树中离采样点x_rand最近的节点，但之间有一个障碍物，所以进行转向，新的采样点为x_nearest和障碍物之间的，某一个节点。

实施例二

图23是为本发明实施例二提供的一种用于实现面向复杂动态场景的医用护理机器人实时动态路径规划方法的系统结构示意图，路径规划的实施是以良好的环境感知和定位建图为基础的。该系统包括：环境感知模块、位置定位模块、数据分析模块、数据记录模块、数据传输模块。

环境感知模块，用于获取智能医疗护理机器人周围的环境数据，包括，地图建立、语义信息、静态障碍物与动态障碍物信息等；

位置定位模块，用于获取医疗护理机器人的高精度定位信息，包括里程计信息、IMU信息、RTK信息；

数据分析模块，用于医疗护理机器人周围复杂环境的场景识别，建立基于规则的决策输出信息，得到当前场景判断的结果；

数据记录模块，用于对传感器数据以及分析结果初始化信息进行记录；

数据传输模块，用于将分析所得数据和记录数据传输到主控制器，包括全局地图信息、语义信息、障碍物信息、里程计信息、IMU信息、RTK信息、场景判断信息。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种面向复杂动态场景的医用护理机器人实时动态路径规划方法，其特征在于，所述方法具体包括以下步骤：

S1、载入一张地图，并对其进行二值化处理，其中黑色部分为不可通行区域，白色部分为可通行区域，并将其称为自由空间X_free，之后将地图进行多边形拟合，提取地图中的边界信息与角点信息；

S2、对自由空间X_free中拟合的多边形进行地图凸多边形重构，将自由空间X_free分割为多个凸多边形的集合，由于凸多边形的性质，凸多边形内部的路径是同伦道路，即可以视为相似的路径，对路径的搜索可以直接在凸多边形的边界上进行，从而避免重复路径搜索，提升搜索效率；

S3、当机器人起点被确定后，开始路径规划，利用改进的实时探索式搜索树路径规划方法，对地图进行路径搜索；具体包括：

S31、优先搜索凸多边形的每一条切割线的中点，可快速得到起点到每一个凸多边形的路径，因此，无论终点在哪，只需要判断终点在地图上的哪一个凸多边形中，就可以快速得到一条次优路径；

S32、在遍历完所有凸多边形的每一条切割线后，再随机重复采样凸多边形的割线上的点，来优化路径的长度；

S4、在终点确定以后，机器人以某一速度沿着路径朝终点走去，并使探索式搜索树搜索的树根节点随着机器人移动，从而不丢弃先前采样的路径，但也不必等待树完全构建，因此搜索树扩展和机器人移动是交错的；

S5、针对机器人行进过程中目标点改变的情况，由于探索式搜索树搜索的根节点随机器人移动，并且从树的根节点进行重布线，可以利用先前采样的路径来快速得到一个次优解，并在后续树的采样过程中不断优化路径长度；

S6、针对机器人行进过程中出现障碍物的情况，首先需要阻塞探索式搜索树中被障碍物占据的节点，以此来避免机器人碰撞障碍物；然后需要判断出障碍物在哪一些凸多边形中，在后续的探索式搜索树采样过程中，只需要对障碍物所占据的凸多边形内部进行采样，由于采样被限制在障碍物所在凸多边形内部，极大提升了路径搜索效率，并更好地实现避障。

2.如权利要求1所述的一种面向复杂动态场景的医用护理机器人实时动态路径规划方法，其特征在于，在步骤S1中，在提取到的角点信息中，规定最外侧一层角点按逆时针排列，内侧一层角点按顺时针排列，其排列方式由右手定则来判断自由空间在哪一侧，即右手大拇指向上，四指指向为向量方向，此时手心方向为自由空间。

3.如权利要求1所述的一种面向复杂动态场景的医用护理机器人实时动态路径规划方法，其特征在于，在步骤S2中，所述地图凸多边形重构的过程包括：

S21:选取凹点；

对自由空间进行腐蚀膨胀操作来消除噪点，之后进行多边形拟合，多边形拟合的结果中包含各个多边形的顶点，按顺序遍历多边形拟合结果中的各个顶点，其中多边形外侧一层的顶点按逆时针排列，多边形内侧的顶点按顺时针排列，通过计算顶点两条边的叉积的方法来判断点是否为凹点，即角度大于180度的点；

具体的，设当前点为P_now,P_now的前一个点为P_end，后一个点为P_start，向量v₁为向量v₂为/>由向量叉积的性质可知，当v₁×v₂>0时，P_end在向量v₁的左侧；由右手定则判断得到的排列顺序可得，自由空间在v₁的右侧，即P_now的角度大于180度，为凹点；

S22:计算凹点的可视点；

在获得地图中的所有凹点以后，对每个凹点计算可视点，即凹点可以通过一条直线到达的点，并且不会触碰到障碍物和其他剖分线，具体方法为：

遍历地图上所有的边界，首先设当前点需要计算可视点的凹点为V_p，遍历边界的起始点为L_S，终点为L_E,当前遍历到的点为P,向量向量/>向量/>如果点P被阻挡，则点P在向量v₃和向量v₄之间，即在v₃右侧v₄左侧，或者在v₃左侧v₄右侧，(v₃×v₄)*(v₅×v₄)≤0；

当满足上述条件后，如果点P和点V_p不在边界点的同一侧，则点P被阻挡，设如果(v₆×v₇)*(v₈×v₇)≤0，则P和点V_p不在边界点的同一侧；

如果点P同时满足(v₃×v₄)*(v₅×v₄)≤0和(v₆×v₇)*(v₈×v₇)≤0的情况，则P被阻挡，否则P为可视点；

S23：选取凹点的最优可视点；

在所有可视点中，计算最优的可视点，最优的定义为，可视点要离凹点的角平分线尽量近，同时需要满足离凹点尽量靠近并且不和现有的剖分线相交；具体方法为：

设凹点为V_p，可视点为P₁，计算凹点的角平分线为v₉，v₉以V_p为起点，并将其单位化，v₁₀是以V_p为起点，P₁为终点的单位化向量，即

定义最优可视点的目标函数如公式1：

需要选取目标函数最小的可视点，其中需要优先选取离角平分线v₉最近的点，所以在为负值，确保/>范围内的优先级比/>高；其次，为了考虑距离的因素，计算可视点和凹点的距离为L,在目标函数中加入距离因素，最后的目标函数如公式2：

选取凹点的最优可视点,连接凹点与最优可视点，将凹点切割为凸点，并将凹点与可视点连接的过程可视为一次地图凸多边形重构，连接线称为剖分线，如果凹点在一次剖分后角度仍然大于180度，则再次剖分，直到没有凹点为止；在完成一次剖分后，还需要检查连接凹点的最优可视点是否为凹点，如果是且被剖分为了凸点，遍历凹点时可以直接跳过；

判断剖分后角度是否大于180度的方法具体为，设凹点为V_p，凹点前一个点为P_end，后一个点为P_start，可视点为P₁，向量v₁₁为向量v₁₂为/>向量v₁₃为/>只有当(v₁₁×v₁₃)≤0，(v₁₂×v₁₃)≥0时才没有大于180度的角；

对地图上每一个凹点进行剖分后，即可将自由空间分割为多个凸多边形的集合。

4.如权利要求1所述的一种面向复杂动态场景的医用护理机器人实时动态路径规划方法，其特征在于，在步骤S2中，对于凸多边形内部的路径是同伦道路，可以视为相似的路径，具体方法为：

设X是拓扑空间，X中的所有道路记作P(X),所有以x₀为起点，x₁为终点的道路记作P(X；x₀,x₁)，所有以x₀为基点的回路记作Ω(X,x₀)；设f,g∈P(X；x₀,x₁),如果存在连续映射F:(I²→X),使得F(s,0)＝f(s),F(s,1)＝g(s),F(0,t)＝x₀,F(1,t)＝x₁，则称F为从f到g的道路同伦，记作即这两条道路之间可以连续地变化过去，是同类型的路径。

5.如权利要求1所述的一种面向复杂动态场景的医用护理机器人实时动态路径规划方法，其特征在于，在步骤S5中，探索式搜索树的根节点会随着机器人移动，其中节点的移动和搜索树的更新是相互交替的；在一次迭代中，根节点首先随着机器人移动，之后进行树的搜索，树的搜索包括树的扩张和重布线，树的扩张为由剖分线引导的节点采样；树的重布线分为从根节点重布线和随机重布线，从根节点重布线可以利用原先采样的信息来很好地解决目标点改变的情况与障碍物的躲避，随机重布线用于路径的优化。

6.如权利要求5所述的一种面向复杂动态场景的医用护理机器人实时动态路径规划方法，其特征在于，所述树的扩张的过程中，需要查找搜索树中离采样点最近的点，采用动态参考点树的数据结构，参考点树选择数据集中的一个点v作为“枢轴”，并将其他点与它的距离存储在一个半径为μ内的区域内，然后再以同样的方式处理这个区域的子集，形成一棵二叉树，这样就得到了一个参考点树,其计算公式如公式3所示：

其中S₁、S₂为两颗子树，d(s,v)为点s和v的距离计算公式，这里采用欧式距离。

7.如权利要求1所述的一种面向复杂动态场景的医用护理机器人实时动态路径规划方法，其特征在于，在步骤S6中，随着障碍物的增加，在新添加的障碍物周围的采样概率应该增加，而旧障碍物周围的采样概率应该减少，具体实现的方法为：

定义一个特殊的采样概率函数，设置障碍物数组为Obs＝[obs1,obs2,…,obsn],其中的障碍物按先后出现的顺序排列，第一个出现的障碍物为obs1，第n个出现的障碍物为obsn；按出现顺序定义一个概率权重数组，第一个出现障碍物权重为1，第n个出现的障碍物权重为n，权重数组为weights＝[1,2,…,n]，计算概率时，第i个障碍物被选中的概率如公式4所示：

其中sum(weights)为对weights数组中的值进行求和。

8.一种用于实现面向复杂动态场景的医用护理机器人实时动态路径规划方法的系统，其特征在于，包括：环境感知模块、位置定位模块、数据分析模块、数据记录模块以及数据传输模块；

环境感知模块，用于获取智能医疗护理机器人周围的环境数据，包括地图建立、语义信息、静态障碍物与动态障碍物信息；

位置定位模块，用于获取医疗护理机器人的高精度定位信息，包括里程计信息、IMU信息、RTK信息；

数据分析模块，用于医疗护理机器人周围复杂环境的场景识别，建立基于规则的决策输出信息，得到当前场景判断的结果；

数据记录模块，用于对传感器数据以及分析结果初始化信息进行记录；

数据传输模块，用于将分析所得数据和记录数据传输到主控制器，包括全局地图信息、语义信息、障碍物信息、里程计信息、IMU信息、RTK信息、场景判断信息。