CN116817913A - 利用转弯惩罚因子和孪生路网改进的路径规划新方法 - Google Patents

Abstract

本发明提供一种利用转弯惩罚因子和孪生路网改进的路径规划新方法，从而解决现有技术中存在的室内复杂环境下路径生成速度慢、转弯次数不受控、路径规划难以应对复杂室内环境等问题。本发明通过增加转弯惩罚因子完善启发函数，显著控制了规划路径的转弯次数。通过叠加孪生路网信息改进了搜索函数，从地图平台路径规划的层面有效解决了障碍物对路径生成的影响。相比于传统方法，本发明提高了算法运行效率、降低了转弯次数，能够根据孪生路网信息的变化更新路径，实现了室内复杂环境下的路径动态规划。可通过地图平台发布路径规划服务。有助于降低智能机器人、自动驾驶设备的控制负担，从而提升了室内复杂环境下导航路径规划的服务效率。

Description

利用转弯惩罚因子和孪生路网改进的路径规划新方法

技术领域

本发明涉及室内路径规划技术领域，尤其涉及一种利用转弯惩罚因子和孪生路网改进的路径规划新方法。

背景技术

随着人类生产和日常消费活动范围的不断扩张，路径规划及其导航应用场景已从室外路径确定环境，扩展至室内复杂变化环境。面对开放、动态变化的室内环境，地图平台如何提供有效可行、简单易用的规划路径，提升路径搜索效率，降低路径转弯次数，成为室内路径规划的研究热点。

人类活动中，每天在室内活动时间约占70％～80％。作为支撑室内活动的重要服务，室内路径规划服务引起了越来越多的关注。室内路径规划是室内导航服务的主要内容。面对日益复杂的室内环境，如何利用室内地图改进路径规划算法，支撑地图服务平台规划全局路径成为室内位置服务的研究热点之一。路径规划是指在地图或空间环境中寻找从起点到目标点的最优路径或最短路径的过程。

地图建模和路径搜索算法是路径规划中两个重要问题。地图建模是指将三维世界中的实际场景转化为计算机可以识别的数据格式，常用的方法有可视图法、栅格法、拓扑法等。路径搜索算法是指根据地图模型，在搜索范围内找到从起点到目标点的最优路径或最短路径。常见的路径规划算法有Dijkstra算法、A*算法、人工势场法、RRT(rapidly-exploring random trees)算法、粒子群算法、遗传算法等。

路径规划算法中，Dijkstra算法是一种基于贪心策略的最短路径算法，通过逐步扩展搜索空间来寻找从起点到目标点的最短路径。A*算法是一种启发式搜索算法，在Dijkstra算法的基础上叠加了启发式函数，提高了算法运行效率。RRT算法是先由随机采样生成一棵覆盖整个搜索空间的树，然后在树上寻找最短路径。粒子群算法和遗传算法都是优化算法，能够在搜索空间中寻找问题的最优解。通过使用跳点算法来改进A*算法，可以提高A*算法的计算效率。但改进后路径转折点较多，车辆使用含有较多转折点的规划路径时相关控制负担将显著增加。还有一种通过融合动态窗口法和改进的A*算法，提出的全局动态路径规划算法。但该算法计算复杂度随空间维数呈指数增长，难以适用于大空间场景下的实时路径规划。

发明内容

本发明的目的在于提供一种利用转弯惩罚因子和孪生路网改进的路径规划新方法，从而解决现有技术中存在的室内复杂环境下路径生成速度慢、转弯次数不受控、路径规划难以应对复杂室内环境等前述问题。

相比于其他路径规划算法，A*算法具有高效性、最优性等优点。但A*算法仍可进一步完善启发函数和搜索函数，从而充分顾及障碍物影响、控制规划路径转弯次数等。本发明结合上述因素提出了一种改进的路径规划方法。通过增加转弯惩罚因子完善启发函数，显著控制了规划路径的转弯次数。通过叠加孪生路网信息改进了搜索函数，从地图平台路径规划的层面有效解决了障碍物对路径生成的影响。

为了实现上述目的，本发明采用的技术方案如下：

一种利用转弯惩罚因子和孪生路网改进的路径规划新方法，包括以下步骤：

S1,将起始点和目标点在参考坐标系下的坐标转换为局部坐标系下的坐标；根据起始点和目标点在局部坐标系下的坐标和网格边长，计算起始点和目标点的网格编码；将单个网格作为一个节点，从而得到起始节点和目标节点；若当前节点为父节点，则与当前节点临近和可达的节点为子节点；路径搜索过程中每个节点应存放父节点的网格编码、当前节点的网格编码、起始节点到当前节点实际代价、当前节点到目标节点的估计代价，从而可根据当前节点迭代回溯父节点得到路径节点序列；所述起始节点的父节点为空；

S2，创建搜索节点集合openList、扩展节点集合closeList、临近节点集合adjacentList；所述openList用于存放进入搜索范围的节点；所述closeList用于存放每一步从所述openList中优选得到的代价函数值最小节点；所述adjacentList用于存放与当前节点临近和可达的子节点；

S3，设置所述openList、所述closeList、所述adjacentList为空；将起始节点加入所述openList；

S4，利用代价函数FP(n)，计算所述openList中所有节点的代价函数值；选出代价值最小的节点lowestF；

S5，将所述lowestF节点作为扩展节点从所述openList移入所述closeList；将所述lowestF节点从所述openList中删除；设置所述adjacentList为空；

S6，若所述lowestF节点为目标节点，则利用所述lowestF节点和所述closeList，通过迭代回溯父节点生成路径，结束搜索；若所述lowestF节点不是目标节点，则继续下一步；

S7，以所述lowestF节点为父节点，搜索临近的子节点；按照子节点网格编码，检索对应网格的路网语义信息表；根据用户身份，从对应路网语义信息表中读取子节点的通行状态；

S8，根据所有子节点的通行状态，按照邻域搜索函数处理子节点，将临近可达子节点存入所述adjacentList；并记录子节点与父节点关系；

S9，计算所述adjacentList中每个节点的G(n)和HP(n)；

S10，判断所述adjacentList中每个节点是否在所述openList中已存在；若不存在，则将该节点加入所述openList中；若已存在，且该节点当前的G(n)更小，则用该节点代替所述openList中已存在节点；

S11，判断所述openList集合是否为空；若所述openList为空，则说明路径不存在，结束搜索；否则，返回至所述步骤S4。

优选的，路径规划结果为节点序列。

优选的，在进行动态路网规划时，可根据用户实时位置坐标，计算用户所在的网格编码，得到用户当前节点；若路网中被设置了障碍物，可利用设置障碍物的坐标得到障碍物位置处的网格编码，按照障碍物网格编码修改对应网格语义信息表中的路网状态；从路径规划结果中截取用户当前节点至目标节点的剩余节点序列；判断剩余节点序列中是否包含障碍物网格编码，若不包含，则继续使用原规划路径；否则，从用户当前节点重新规划路径。

优选的，步骤S4中所述的代价函数FP(n)是改进的代价函数FP(n)，在获取所述改进的代价函数FP(n)过程中引入转弯惩罚因子。

优选的，所述改进的代价函数FP(n)的获取方法具体为：

首先判断当前节点到下一节点是否发生转弯：

式中，(x_current,y_current)，(x_previous,y_previous)，(x_next,y_next)分别为当前节点及其父节点和子节点的平面坐标，所述子节点是指由当前节点拓展出来的节点；(dx₁,dy₁)和(dx₂,dy₂)分别表示父节点到当前节点和当前节点到其子节点的向量，s₁和s₂分别表示这两个向量的模；cos A是判断当前节点是否发生拐弯的函数值，其物理意义为两个向量夹角的余弦值，取值范围为cos A∈[-1,1]；如果|cos A -1|≤10^-8，表明两个向量夹角的余弦值非常接近于0，两个向量夹角约为0，说明该节点处没有发生转弯，否则，表明两个向量夹角大于零，即该节点处发生转弯；

判断完当前节点是否转弯后，构建转弯惩罚因子p：

式中，G(next)表示起始节点到next节点的实际代价，H₀表示next节点到目标节点的初始代价，可由两个节点的欧几里得距离计算得到：

式中，(x_next,y_next)，(x_goal,y_goal)分别表示子节点next和目标节点goal的平面坐标；将惩罚因子应用于其子节点next的估计代价H(next)计算，得到改进后的启发函数：

HP(next)＝H₀×p (6)

若将next节点记为n，利用改进后的启发函数更新代价函数，得到改进的代价函数：

FP(n)＝G(n)+HP(n) (7)。

优选的，节点next的转弯惩罚因子由两段距离计算得到；第一段距离为当前节点到起始节点的实际距离，第二段距离为该当前节点到目标节点的欧几里得距离；若当前节点与下一节点之间未发生转弯，惩罚因子值为1，启发函数保持不变；若当前节点与下一节点之间发生转弯，惩罚因子值范围为p∈(1,2]，启发函数值将成倍增加；较大的启发函数值可引导算法朝着转弯次数较少的方向搜索路径节点。

优选的，若当前节点越靠近起始节点，转弯惩罚因子越大；在路径规划的起始阶段，改进后的启发函数将促使路径搜索尽可能沿着直线方向朝目标点推进，从而有力降低转弯次数；较少的转弯次数，将显著压缩路径节点，而较少的路径节点进一步降低了路径搜索计算量；较小的计算量，可更加节省路径搜索耗时，提高路径搜索效率。

优选的，若当前节点越靠近目标点，惩罚因子值逐渐向1回归，惩罚效应逐渐变弱；在路径规划的收尾阶段，当前节点应尽可能朝着目标点闭合；路径规划收尾阶段的首要目标为路线闭合至目标点，在该阶段转弯次数惩罚因子应适当服从于路线闭合目标；此时，适当减弱转弯惩罚因子放大效应，促使路线搜索朝着目标点尽快闭合。

优选的，步骤S8中所述的邻域搜索函数是改进的邻域搜索函数，所述改进的邻域搜索函数处理流程为：1)在原邻域搜索函数的基础上增加上、下、左、右四个方向节点的障碍物判断；2)若当前节点向上为障碍物时，无论当前节点左上、右上节点是否可以到达，都不再将其放入临近可达节点集合；3)以类似2)的方式处理其他方向障碍物情况；以避免路径搜索从障碍物中间穿过。

优选的，将在参考坐标系下的坐标转换为局部坐标系下的坐标并构建孪生路网的具体过程为：

首先建立局部坐标系，局部坐标系的原点为地面网格西南角点，沿网格线向北为x轴，向东为y轴，以国际单位制m为单位长度；该局部坐标系与参考坐标系相互转化的公式为：

式中，l为局部坐标系，r为参考坐标系，(X^l,Y^l)为某点的局部坐标系坐标，(X^r,Y^r)为该点对应的参考坐标系坐标，为局部坐标系原点在参考坐标系下的坐标，/>为两个坐标系的旋转角。

本发明的有益效果是：

本发明的利用转弯惩罚因子和孪生路网改进的路径规划新方法。首先，设计转弯惩罚因子改进了A*算法的启发式函数，克服了传统A*算法路径规划效率低、路径转弯次数不可控等问题，有效减少了路径中的转弯点，提高了路径生成效率。其次，设计了孪生路网改进了A*算法的邻域搜索函数。改进后新方法叠加了实时孪生路网信息，进一步增强了路径规划的适应能力，弥补了传统A*算法难以进行动态路径规划的不足。新方法能够根据环境信息的变化及时更新路径，提高了路径生成结果的可用性。

相比于传统A*算法，本发明提出的路径规划新方法，提高了算法运行效率、降低了转弯次数，能够根据孪生路网信息的变化更新路径，实现了室内复杂环境下的路径动态规划。改进后的A*算法可通过地图平台发布路径规划服务。新方法生成的路径转弯次数更少，有助于降低智能机器人、自动驾驶设备的控制负担，从而提升了室内复杂环境下导航路径规划的服务效率。

附图说明

图1是A*算法4邻域搜索示意图；

图2是A*算法8邻域搜索示意图；

图3是路网网格语义信息表；

图4是本发明路径规划方法的流程示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施方式仅仅用以解释本发明，并不用于限定本发明。

1.1传统A*算法

A*算法是一种启发式算法，利用启发式信息来估计每个节点到目标节点的代价，并根据这些估计代价确定搜索顺序，是一种高效的路径规划算法。A*算法使用两个函数来估计每个节点的代价：一个是从起始节点到当前节点的实际代价(通常是距离或时间)，另一个是从当前节点到目标节点的估计代价(通常是估算距离或时间)。其代价函数定义为：

F(n)＝G(n)+H(n) (1)

式中：n表示当前节点，F(n)表示从起始节点经当前节点n到达目标节点的估价函数，G(n)表示起始节点到当前节点n的实际代价，H(n)表示当前节点n到目标节点的估计代价。

A*算法的计算效率取决于启发式函数H(n)，如果H(n)不准确会导致搜索结果不够优化或搜索时间过长。A*算法通常采用曼哈顿距离或欧几里得距离作为启发式函数，由于本发明采用八邻域搜索路径，启发式函数采用欧几里得距离更接近其真实距离，路径规划结果更优，所以本发明采用欧几里得距离作为启发式函数，其公式为：

式中，(x_n,y_n)，(x_g,y_g)分别为当前节点和目标节点的平面坐标。

1.2改进的A*算法

1.2.1改进A*算法启发函数

传统A*算法在路径规划过程中会出现很多拐点，不符合实际应用中人员车辆路径规划的需求。本发明对传统A*算法转弯次数较多的现象进行优化处理，通过预判断是否转弯并设计转弯惩罚函数，引导算法朝着直线和顺畅方向搜索道路。具体方法为：

首先判断当前节点到下一节点是否发生转弯：

式中，(x_current,y_current)，(x_previous,y_previous)，(x_next,y_next)分别为当前节点及其父节点和子节点的平面坐标。子节点指从当前节点临接可达节点。(dx₁,dy₁)和(dx₂,dy₂)分别表示父节点到当前节点和当前节点到其子节点的向量，s₁和s₂分别表示这两个向量的模。cosA是判断当前节点是否发生拐弯的函数值，其物理意义为两个向量夹角的余弦值，取值范围为cos A∈[-1,1]。如果|cos A-1|≤10^-8，表明两个向量夹角的余弦值非常接近于0，两个向量夹角约为0，说明该节点处没有发生转弯，否则，表明两个向量夹角大于零，即该节点处发生转弯。

判断完当前节点是否转弯后，构建转弯惩罚因子p：

式中，G(next)表示起始节点到next节点的实际代价，H₀表示next节点到目标节点的初始代价，可由两个节点的欧几里得距离计算得到：

式中，(x_next,y_next)，(x_goal,y_goal)分别表示子节点next和目标节点goal的平面坐标。将惩罚因子应用于其子节点next的估计代价H(next)计算，得到改进后的启发函数：

HP(next)＝H₀×p (6)

若将next节点记为n，利用改进后的启发函数更新代价函数，得到改进的代价函数：

FP(n)＝G(n)+HP(n) (7)

可进一步分析转弯惩罚因子的作用机理。(4)式显示，节点next的转弯惩罚因子由两段距离计算得到。第一段距离为当前节点到起始节点的实际距离，第二段距离为该当前节点到目标节点的欧几里得距离。若当前节点与下一节点之间未发生转弯，惩罚因子值为1，启发函数保持不变。若当前节点与下一节点之间发生转弯，惩罚因子值范围为p∈(1,2]，启发函数值将成倍增加。较大的启发函数值可引导算法朝着转弯次数较少的方向搜索路径节点。

若当前节点越靠近起始节点，转弯惩罚因子越大。因此，在路径规划的起始阶段，改进后的启发函数将促使路径搜索尽可能沿着直线方向朝目标点推进，从而有力降低转弯次数。较少的转弯次数，将显著压缩路径节点，而较少的路径节点进一步降低了路径搜索计算量。较小的计算量，可更加节省路径搜索耗时，提高路径搜索效率。

若当前节点越靠近目标点，惩罚因子值逐渐向1回归，惩罚效应逐渐变弱。在路径规划的收尾阶段，当前节点应尽可能朝着目标点闭合。由于路径规划收尾阶段的首要目标为路线闭合至目标点，因此在该阶段转弯惩罚因子应适当服从于路线闭合目标。此时，需要适当减弱转弯惩罚因子放大效应，促使路线搜索朝着目标点尽快闭合。(4)式恰好具备了在路线规划收尾阶段降低惩罚效应的性能，从而避免了在路线收尾阶段过度压减转弯次数导致算法沿直线搜索错过目标点又重新掉头回来的问题。最终，附加转弯惩罚因子的改进启发函数通过自适应调整惩罚因子不仅满足了路径搜索各阶段的不同目标，而且确保了算法在路径规划方面的全局优化效果。

1.2.2改进A*算法邻域搜索函数

如图1、2所示，传统A*算法一般分为4邻域搜索和8邻域搜索，四邻域搜索算法转折点较多，不符合实际情况路径规划的需求。而8邻域搜索算法在室内复杂环境中寻找最优路径时，可能出现从顶点临接的障碍物中穿过的情况，导致搜索路径不实用。

现有邻域搜索函数的处理流程如下：(1)从当前节点向上、下、左、右、左上、右上、左下、右下八个方向搜索可以到达的节点。(2)如果该节点可以到达，则将该节点存储到临近可达节点集合。

当前节点的正上和正右节点为角连接节点。若当前节点的正上和正右节点存在障碍物时，现有邻域搜索函数可能会选择右上节点作为临近可达节点。此时，可能出现规划路径穿越2个角连接障碍物的现象。针对上述现象改进邻域搜索函数，得到优化的邻域搜索函数处理流程：(1)在原邻域搜索函数的基础上增加上、下、左、右四个方向节点的障碍物判断。(2)若当前节点向上为障碍物时，无论当前节点左上、右上节点是否可以到达，都不再将其放入临近可达节点集合。(3)其他方向障碍物处理情况类似(2)。A*算法邻域搜索函数经过改进后，可有效克服路径搜索从障碍物中间穿过的问题。

本发明通过优化A*算法邻域搜索函数，不仅保障了规划路径在实际场景中的可应用性，也提高了路径规划的准确性。借鉴数字孪生技术的优势，可将障碍物对室内路网的影响通过节点通行状态表征。建立孪生路网，实时动态更新和反馈室内各区域的通行状态，支撑优化后的邻域搜索函数进行路径节点搜索，从而在室内地图平台上实现顾及障碍物影响的动态路径规划。通过构建孪生路网模型，在室内地图平台上运行改进后的A*算法，将进一步增强规划路径的可用性和有效性，形成叠加孪生路网信息的路径规划方法如下节。

2改进A*算法并叠加孪生路网信息的路径规划新方法

A*算法目前仅利用静态路网信息进行路径规划。对于复杂的室内环境，路网的各段状态并非固定不变，路网的表现形态也存在多样性和灵活性。因此，改进后的A*算法将结合室内路径规划的特点，叠加孪生路网信息以满足室内复杂环境下的路径规划的需求。为此，本发明设计了孪生路网模型，利用改进后的A*算法叠加了孪生路网信息，形成了路径规划新方法。在室内路网环境下，根据路网各段的实时状态，驱动改进的A*算法实现动态路径规划，提高了A*路径规划算法对复杂多变环境的适应能力。

2.1孪生路网构建

数字孪生地图是一种将真实世界中的物理实体与虚拟世界中的数学模型相结合的数字地图，它将真实世界中各种物理系统的实时状态和特征反映到虚拟世界中，并为用户提供可视化、交互式的应用接口，以支持各种决策和应用场景。数字孪生地图需要依靠物联网、云计算、大数据等技术，将各种传感器和监测设备采集的物理系统的实时数据上传到云端进行处理和分析。然后，将处理后的数据与数字孪生模型相融合，构建出一个实时反映物理世界的虚拟模型。

孪生路网是孪生地图的一个重要组成部分，人员车辆的定位导航服务均需要用到孪生路网。首先建立局部坐标系，局部坐标系的原点为地面网格西南角点，沿网格线向北为x轴，向东为y轴，以国际单位制m为单位长度。该局部坐标系与参考坐标系相互转化的公式为：

式中，l为局部坐标系，r为参考坐标系，(X^l,Y^l)为某点的局部坐标系坐标，(X^r,Y^r)为该点对应的参考坐标系坐标，为局部坐标系原点在参考坐标系下的坐标，/>为两个坐标系的旋转角。

然后在局部坐标系中基于预设网格边长s，将地面划分为多个网格，得到地面的路网网格，并为每个路网网格建立对应的语义信息表，如附图3所示。由路网网格及其语义信息表，建立孪生路网模型。

2.2孪生路网信息叠加

如附图4所示，利用改进后的A*算法，叠加孪生路网信息，实现室内复杂环境路径规划的基本流程为：

(1)利用(9)式将起始点和目标点在参考坐标系下的坐标转换为局部坐标系下坐标。根据起始点和目标点在局部坐标系下的坐标和网格边长，计算起始点和目标点的网格编码。将单个网格作为一个节点，从而得到起始节点和目标节点。若当前节点为父节点，则与当前节点临近和可达的节点为子节点。路径搜索过程中每个节点应存放父节点的网格编码、当前节点的网格编码、起始节点到当前节点实际代价、当前节点到目标节点的估计代价等，从而可根据当前节点迭代回溯父节点得到路径节点序列。起始节点的父节点为空。

(2)创建搜索节点集合openList、扩展节点集合closeList、临近节点集合adjacentList。搜索节点集合用于存放进入搜索范围的节点。扩展节点集合用于存放每一步从搜索节点集合中优选得到的代价函数值最小节点。临近节点集合用于存放与当前节点临近和可达的子节点。

(3)设置openList、closeList、adjacentList为空。将起始节点加入openList。

(4)利用改进的代价函数FP(n)，计算openList中所有节点的代价函数值。选出代价值最小的节点lowestF。

(5)将lowestF节点作为扩展节点从openList移入closeList。将lowestF节点从openList中删除。设置adjacentList为空。

(6)若lowestF节点为目标节点，则利用lowestF节点和closeList，通过迭代回溯父节点生成路径，结束搜索。若lowestF节点不是目标节点，则继续下一步。

(7)以lowestF节点为父节点，搜索临近的子节点。按照子节点网格编码，检索对应网格的路网语义信息表。根据用户身份，从对应路网语义信息表中读取子节点的通行状态。

(8)根据所有子节点的通行状态，按照改进的邻域搜索函数处理子节点，将临近可达子节点存入adjacentList。并记录子节点与父节点关系。

(9)计算adjacentList中每个节点的G(n)和HP(n)。

(10)判断adjacentList中每个节点是否在openList已存在。若不存在，则将该节点加入openList中；若已存在，且该节点当前的G(n)更小，则用该节点代替openList已存在节点。

(11)判断openList集合是否为空。若openList为空，则说明路径不存在，结束搜索；否则，返回至(4)。

路径规划结果为节点序列。在应用新方法进行动态路网规划时，可根据用户实时位置坐标，计算用户所在的网格编码，得到用户当前节点。若路网中被设置了障碍物，可利用设置障碍物的坐标得到障碍物位置处的网格编码，按照障碍物网格编码修改对应网格语义信息表中的路网状态。从路径规划结果中截取用户当前节点至目标节点的剩余节点序列。判断剩余节点序列中是否包含障碍物网格编码，若不包含，则继续使用原规划路径；否则，从用户当前节点重新规划路径。

通过采用本发明公开的上述技术方案，得到了如下有益的效果：

随着人类生产和日常消费活动范围的不断扩张，路径规划及其导航应用场景已从室外路径确定环境，扩展至室内复杂变化环境。面对开放、动态变化的室内环境，地图平台如何提供有效可行、简单易用的规划路径，提升路径搜索效率，降低路径转弯次数，成为室内路径规划的热点领域。本发明针对传统A*算法在室内复杂环境下路径生成速度慢、转弯次数不受控、路径规划难以应对动态障碍等问题，提出了一种改进的路径规划方法。首先，设计转弯惩罚因子改进了A*算法的启发式函数，克服了传统A*算法路径规划效率低、路径转弯次数不可控等问题，有效减少了路径中的转弯点，提高了路径生成效率。其次，设计了孪生路网改进了A*算法的邻域搜索函数。改进后的新方法叠加了实时孪生路网信息，进一步增强了路径规划的适应能力，弥补了传统A*算法难以进行动态路径规划的不足。新方法能够根据环境信息的变化及时更新路径，提高了路径生成结果的可用性。

相比于传统A*算法，文中提出的路径规划新方法，提高了算法运行效率、降低了转弯次数，能够根据孪生路网信息的变化更新路径，实现了室内复杂环境下的路径动态规划。改进后的A*算法可通过地图平台发布路径规划服务。新方法生成的路径转弯次数更少，有助于降低智能机器人、自动驾驶设备的控制负担，从而提升了室内复杂环境下导航路径规划的服务效率。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视本发明的保护范围。

Claims (10)

1.一种利用转弯惩罚因子和孪生路网改进的路径规划新方法，其特征在于，包括以下步骤：

S1,将起始点和目标点在参考坐标系下的坐标转换为局部坐标系下的坐标；根据起始点和目标点在局部坐标系下的坐标和网格边长，计算起始点和目标点的网格编码；将单个网格作为一个节点，从而得到起始节点和目标节点；若当前节点为父节点，则与当前节点临近和可达的节点为子节点；路径搜索过程中每个节点应存放父节点的网格编码、当前节点的网格编码、起始节点到当前节点实际代价、当前节点到目标节点的估计代价，从而可根据当前节点迭代回溯父节点得到路径节点序列；所述起始节点的父节点为空；

S2，创建搜索节点集合openList、扩展节点集合closeList、临近节点集合adjacentList；所述openList用于存放进入搜索范围的节点；所述closeList用于存放每一步从所述openList中优选得到的代价函数值最小节点；所述adjacentList用于存放与当前节点临近和可达的子节点；

S3，设置所述openList、所述closeList、所述adjacentList为空；将起始节点加入所述openList；

S4，利用代价函数FP(n)，计算所述openList中所有节点的代价函数值；选出代价值最小的节点lowestF；

S5，将所述lowestF节点作为扩展节点从所述openList移入所述closeList；将所述lowestF节点从所述openList中删除；设置所述adjacentList为空；

S6，若所述lowestF节点为目标节点，则利用所述lowestF节点和所述closeList，通过迭代回溯父节点生成路径，结束搜索；若所述lowestF节点不是目标节点，则继续下一步；

S7，以所述lowestF节点为父节点，搜索临近的子节点；按照子节点网格编码，检索对应网格的路网语义信息表；根据用户身份，从对应路网语义信息表中读取子节点的通行状态；

S8，根据所有子节点的通行状态，按照邻域搜索函数处理子节点，将临近可达子节点存入所述adjacentList；并记录子节点与父节点关系；

S9，计算所述adjacentList中每个节点的G(n)和HP(n)；

S10，判断所述adjacentList中每个节点是否在所述openList中已存在；若不存在，则将该节点加入所述openList中；若已存在，且该节点当前的G(n)更小，则用该节点代替所述openList中已存在节点；

S11，判断所述openList集合是否为空；若所述openList为空，则说明路径不存在，结束搜索；否则，返回至所述步骤S4。

2.根据权利要求1所述的利用转弯惩罚因子和孪生路网改进的路径规划新方法，其特征在于，路径规划结果为节点序列。

3.根据权利要求1所述的利用转弯惩罚因子和孪生路网改进的路径规划新方法，其特征在于，在进行动态路网规划时，可根据用户实时位置坐标，计算用户所在的网格编码，得到用户当前节点；若路网中被设置了障碍物，可利用设置障碍物的坐标得到障碍物位置处的网格编码，按照障碍物网格编码修改对应网格语义信息表中的路网状态；从路径规划结果中截取用户当前节点至目标节点的剩余节点序列；判断剩余节点序列中是否包含障碍物网格编码，若不包含，则继续使用原规划路径；否则，从用户当前节点重新规划路径。

4.根据权利要求1所述的利用转弯惩罚因子和孪生路网改进的路径规划新方法，其特征在于，步骤S4中所述的代价函数FP(n)是改进的代价函数FP(n)，在获取所述改进的代价函数FP(n)过程中引入转弯惩罚因子。

5.根据权利要求4所述的利用转弯惩罚因子和孪生路网改进的路径规划新方法，其特征在于，所述改进的代价函数FP(n)的获取方法具体为：

首先判断当前节点到下一节点是否发生转弯：

式中，(x_current,y_current)，(x_previous,y_previous)，(x_next,y_next)分别为当前节点及其父节点和子节点的平面坐标，所述子节点是指由当前节点拓展出来的节点；(dx₁,dy₁)和(dx₂,dy₂)分别表示父节点到当前节点和当前节点到其子节点的向量，s₁和s₂分别表示这两个向量的模；cosA是判断当前节点是否发生拐弯的函数值，其物理意义为两个向量夹角的余弦值，取值范围为cosA∈[-1,1]；如果|cosA-1|≤10^-8，表明两个向量夹角的余弦值非常接近于0，两个向量夹角约为0，说明该节点处没有发生转弯，否则，表明两个向量夹角大于零，即该节点处发生转弯；

判断完当前节点是否转弯后，构建转弯惩罚因子p：

式中，G(next)表示起始节点到next节点的实际代价，H₀表示next节点到目标节点的初始代价，可由两个节点的欧几里得距离计算得到：

式中，(x_next,y_next)，(x_goal,y_goal)分别表示子节点next和目标节点goal的平面坐标；将惩罚因子应用于其子节点next的估计代价H(next)计算，得到改进后的启发函数：

HP(next)＝H₀×p (6)

若将next节点记为n，利用改进后的启发函数更新代价函数，得到改进的代价函数：

FP(n)＝G(n)+HP(n) (7)。

6.根据权利要求5所述的利用转弯惩罚因子和孪生路网改进的路径规划新方法，其特征在于，节点next的转弯惩罚因子由两段距离计算得到；第一段距离为当前节点到起始节点的实际距离，第二段距离为该当前节点到目标节点的欧几里得距离；若当前节点与下一节点之间未发生转弯，惩罚因子值为1，启发函数保持不变；若当前节点与下一节点之间发生转弯，惩罚因子值范围为p∈(1,2]，启发函数值将成倍增加；较大的启发函数值可引导算法朝着转弯次数较少的方向搜索路径节点。

7.根据权利要求5所述的利用转弯惩罚因子和孪生路网改进的路径规划新方法，其特征在于，若当前节点越靠近起始节点，转弯惩罚因子越大；在路径规划的起始阶段，改进后的启发函数将促使路径搜索尽可能沿着直线方向朝目标点推进，从而有力降低转弯次数；较少的转弯次数，将显著压缩路径节点，而较少的路径节点进一步降低了路径搜索计算量；较小的计算量，可更加节省路径搜索耗时，提高路径搜索效率。

8.根据权利要求5所述的利用转弯惩罚因子和孪生路网改进的路径规划新方法，其特征在于，若当前节点越靠近目标点，惩罚因子值逐渐向1回归，惩罚效应逐渐变弱；在路径规划的收尾阶段，当前节点应尽可能朝着目标点闭合；路径规划收尾阶段的首要目标为路线闭合至目标点，在该阶段转弯次数惩罚因子应适当服从于路线闭合目标；此时，适当减弱转弯惩罚因子放大效应，促使路线搜索朝着目标点尽快闭合。

9.根据权利要求1所述的利用转弯惩罚因子和孪生路网改进的路径规划新方法，其特征在于，步骤S8中所述的邻域搜索函数是改进的邻域搜索函数，所述改进的邻域搜索函数处理流程为：1)在原邻域搜索函数的基础上增加上、下、左、右四个方向节点的障碍物判断；2)若当前节点向上为障碍物时，无论当前节点左上、右上节点是否可以到达，都不再将其放入临近可达节点集合；3)以类似2)的方式处理其他方向障碍物情况；以避免路径搜索从障碍物中间穿过。

10.根据权利要求1所述的利用转弯惩罚因子和孪生路网改进的路径规划新方法，其特征在于，将在参考坐标系下的坐标转换为局部坐标系下的坐标并构建孪生路网的具体过程为：

首先建立局部坐标系，局部坐标系的原点为地面网格西南角点，沿网格线向北为x轴，向东为y轴，以国际单位制m为单位长度；该局部坐标系与参考坐标系相互转化的公式为：

式中，l为局部坐标系，r为参考坐标系，(X^l,Y^l)为某点的局部坐标系坐标，(X^r,Y^r)为该点对应的参考坐标系坐标，为局部坐标系原点在参考坐标系下的坐标，/>为两个坐标系的旋转角。