CN113188543B - 一种路径优化的方法、装置、电子设备及存储介质 - Google Patents
一种路径优化的方法、装置、电子设备及存储介质 Download PDFInfo
- Publication number
- CN113188543B CN113188543B CN202110439686.4A CN202110439686A CN113188543B CN 113188543 B CN113188543 B CN 113188543B CN 202110439686 A CN202110439686 A CN 202110439686A CN 113188543 B CN113188543 B CN 113188543B
- Authority
- CN
- China
- Prior art keywords
- path
- paths
- optimized
- movable
- target
- 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.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本申请提供一种路径优化的方法、装置、电子设备及存储介质，其中，路径优化的方法包括：获取环境地图，根据环境地图得到与N个可移动设备分别对应的初始规划路径，得到N条初始规划路径；根据N条初始规划路径的各起始位置点创建N条待优化路径；根据优化误差函数对N条待优化路径进行处理，得到N个可移动设备中至少一个可移动设备的目标规划路径，向至少一个可移动设备发送目标规划路径，能够实现准确规划多个可移动设备路径，其中，优化误差函数用于保证目标规划路径与相对应的初始规划路径的接近程度满足要求，且进一步保证各可移动设备在目标规划路径上的速度的改变频率满足要求。
Description
技术领域
本申请实施例涉及路径规划领域，具体涉及一种路径优化的方法、装置、电子设备及存储介质。
背景技术
相关技术中对于多机器人服务场景，多机器人之间的路径规划避让是一个必须处理的问题。多机器人的规划一般采用搜索的方法或者把其他机器作为运动障碍物动态避让，如果只采用动态障碍物实时避让处理则不能处理窄通道之类的拥塞场景，所以一般会采用搜索的方法，得到多机器人能够安全通行的路径。
因此，如何同时为多个可移动设备准确规划出高质量的可执行路径成为亟待解决的问题。
发明内容
本申请实施例提供一种路径优化的方法、装置、电子设备及存储介质，通过本申请的一些实施例至少能够根据可执行性和安全性调整与N个可移动设备对应的N条待优化路径而得到安全性和可执行性更高的目标规划路径，提升路径规划的效果。
第一方面，本申请实施例提供一种路径优化的方法，方法包括：获取环境地图，其中，环境地图包括N个可移动设备中各可移动设备的起始位置点和目标位置点，N为大于1的整数；根据环境地图得到与N个可移动设备分别对应的初始规划路径，得到N条初始规划路径；根据N条初始规划路径的各起始位置点创建N条待优化路径；根据优化误差函数对N条待优化路径进行处理，得到N个可移动设备中至少一个可移动设备的目标规划路径，其中，所述优化误差函数用于保证目标规划路径与相对应的初始规划路径的接近程度满足要求，且进一步保证各可移动设备在目标规划路径上的速度的改变频率满足要求；向至少一个可移动设备发送目标规划路径，以使至少一个可移动设备中的各可移动设备按照对应的目标规划路径从相应的起始位置点运动至目标位置点。
因此，本申请实施例通过包括限制频繁加减速目的优化误差函数对N条待优化路径进行优化处理，最终使得与目标规划路径对应的可移动设备在相应路径上的行驶更加平稳即不存在过多的加减速情况，进而提升了可移动设备行驶的安全性。
结合第一方面，在一种实施方式中，优化误差函数与各可移动设备在对应的待优化路径上的速度变化的频率相关；优化误差函数与各可移动设备的初始规划路径和待优化路径之间的偏移量相关。
本申请的一些实施例通过优化误差函数可以限制可移动设备在规划路径上的加减速频率，并使得最终的规划路径与初始规划路径(即采用传统A*方法算法搜索得到的路径)尽可能靠近，进而提升了最终得到的目标规划路径的平滑性和安全性。
结合第一方面，在一种实施方式中，优化误差函数的计算公式为：
其中，N表示所述可移动设备的总个数，Q表示所述N条待优化路径中最长待优化路径的路径点个数，Δu表示所述N条待优化路径中各优化路径包括的相邻的两个路径点之间的速度差，Δz表示所述N条待优化路径中各优化路径包括的与所述N条初始规划路径所对应的各路径点之间的距离差，A为第一常数矩阵，B为第二常数矩阵。
因此，本申请实施例通过优化误差函数，能够使待优化路径上的速度变化的频率尽可能小，并使得初始规划路径和待优化路径之间的偏移量尽可能小，从而能够使目标规划路径满足安全和平滑约束，从而使可移动设备安全平稳运行。
结合第一方面，在一种实施方式中，根据优化误差函数对N条待优化路径进行处理，得到N个可移动设备中至少一个可移动设备的目标规划路径，包括：根据N个可移动设备中各可移动设备的运动学属性特征和优化误差函数，确定至少一个可移动设备的目标规划路径。
结合第一方面，在一种实施方式中，运动学属性特征包括：运动学约束、速度极限和加速度极限。
通过对优化误差函数增加运动学属性特征的约束条件可以使得规划得到的路径能够被各可移动设备执行，差异化满足各可移动设备自身的运动属性要求，最终增加规划路径的可执行性。
结合第一方面，在一种实施方式中，根据优化误差函数对N条待优化路径进行处理，得到N个可移动设备中至少一个可移动设备的目标规划路径，包括：根据运动学属性特征、优化误差函数和任意两个可移动设备的安全运动距离，确定至少一个可移动设备的目标规划路径。
因此，本申请实施例通过对优化误差函数增加运动学属性特征约束，可以使得最终规划得到的目标规划路径不仅能够被各可移动设备执行增加可执行性；另一方面通过对优化误差函数增加安全距离约束(例如，两个路径点之间的路径小于两个机器人的半径之和)，能够有效解决窄通道之类的拥塞场景中可能存在的拥塞问题。
结合第一方面，在一种实施方式中，至少一个可移动设备的总个数为M，其中，M为大于或者等于1且小于或者等于N的整数；在根据优化误差函数对N条待优化路径进行处理，得到N个可移动设备中至少一个可移动设备的目标规划路径之后，方法还包括：调整P个可移动设备的工作状态为休眠状态，其中，P个可移动设备是N个可移动设备中除去M个可移动设备后剩余的可移动设备，P为大于或者等于1且小于或者等于N的整数；根据预设时间唤醒P个可移动设备；重新规划P个可移动设备的路径。
因此，本申请实施例通过对没有得到目标规划路径的可移动设备的工作状态的调整，能够节省这些可移动设备的功耗。
第二方面，本申请实施例提供一种路径优化的装置，包括：获取单元，被配置为获取环境地图，其中，环境地图包括N个可移动设备中各可移动设备的起始位置点和目标位置点，N为大于1的整数；第一规划单元，被配置为根据环境地图得到与N个可移动设备分别对应的初始规划路径，得到N条初始规划路径；待优化路径创建单元，被配置为根据N条初始规划路径的各起始位置点创建N条待优化路径；第二规划单元，被配置为根据优化误差函数对N条待优化路径进行处理，得到N个可移动设备中至少一个可移动设备的目标规划路径，其中，优化误差函数用于保证目标规划路径与相对应的初始规划路径的接近程度满足要求，且进一步保证各可移动设备在目标规划路径上的速度的改变频率满足要求；发送单元，被配置为向至少一个可移动设备发送目标规划路径，以使至少一个可移动设备中的各设备按照对应的目标规划路径从相应的起始位置点运动至目标位置点。
结合第二方面，在一种实施方式中，优化误差函数与各可移动设备在对应的待优化路径上的速度变化的频率相关；优化误差函数与各可移动设备的初始规划路径和待优化路径之间的偏移量相关。
结合第二方面，在一种实施方式中，优化误差函数的计算公式为：
其中，N表示所述可移动设备的总个数，Q表示所述N条待优化路径中最长待优化路径的路径点个数，Δu表示所述N条待优化路径中各优化路径包括的相邻的两个路径点之间的速度差，Δz表示所述N条待优化路径中各优化路径包括的与所述N条初始规划路径所对应的各路径点之间的距离差，A为第一常数矩阵，B为第二常数矩阵。
结合第二方面，在一种实施方式中，第二规划单元还被配置为：根据N个可移动设备中各可移动设备的运动学属性特征和优化误差函数，确定与所述至少一个可移动设备对应的目标规划路径。
结合第二方面，在一种实施方式中，运动学属性特征包括：运动学约束、速度极限和加速度极限。
结合第二方面，在一种实施方式中，第二规划单元还被配置为：根据运动学属性特征、优化误差函数和任意两个可移动设备的安全运动距离，确定至少一个可移动设备的目标规划路径。
结合第二方面，在一种实施方式中，至少一个可移动设备的总个数为M，其中，M为大于或者等于1且小于或者等于N的整数；待优化路径创建单元还被配置为调整P个可移动设备的工作状态为休眠状态，其中，P个可移动设备是N个可移动设备中除去M个可移动设备后剩余的可移动设备，P为大于或者等于1且小于或者等于N的整数；根据预设时间唤醒P个可移动设备；重新规划P个可移动设备的路径。
第三方面，本申请提供一种电子设备，包括：处理器、存储器和总线；处理器通过总线与存储器相连，存储器存储有计算机可读取指令，当计算机可读取指令由处理器执行时，用于实现如第一方面任一项所述的方法。
第四方面，本申请提供一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，该计算机程序被执行时实现如第一方面任一项所述的方法。
附图说明
图1为本申请实施例示出的一种路径优化的系统；
图2为本申请实施例示出的一种路径优化的方法的实施流程；
图3为本申请实施例示出的一种路径优化的方法的一种实施例；
图4为本申请实施例示出的一种路径优化的方法的另一种实施例；
图5为本申请实施例示出的一种路径优化的方法的具体实施例流程；
图6为本申请实施例示出的一种路径优化的装置；
图7为本申请实施例示出的一种电子设备。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例仅仅是本申请的一部分实施例，而不是全部实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对附图中提供的本申请的实施例的详情描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护范围。
下面结合附图详细描述本申请实施例中的方法步骤。
本申请实施例可以应用于N个可移动设备的路径规划的场景，例如，这些场景包括服务器为在狭窄通道工作的可移动设备进行路径规划的场景，例如，在狭窄通道的可移动设备需要根据优化误差函数和针对可移动设备的运动学属性特征，对初始规划路线进行优化，以适应狭窄通道的特殊工作环境。
以机器人为例示例性阐述相关技术的路径规划方法存在的问题。具体地，相关技术中，多机器人服务的场景中，一般将其他机器人作为运动障碍物动态避让，而采用动态障碍物实时避让处理并不能处理窄通道的拥塞场景，导致路径规划可执行性很差。
至少为了解决上述问题，本申请一些实施例提供的一种通过优化误差函数和多个约束条件对待优化路径进行处理，以提升得到的规划路径的平滑性和可执行性。在本申请的一些实施例中，通过优化误差函数和可移动设备的运动学属性特征对待优化路径进行处理，能够实现可移动设备能够狭窄通道安全平稳运行的方法，从而实现准确规划可移动设备路径。可以理解的是，本申请实施例的应用场景不限于此。例如，本申请的实施例也可以用于无人驾驶车领域的路径规划。
如图1所示，示例性的展示本申请实施例的一种路径优化的系统，包括：路径规划服务器110、第一可移动设备150、第二可移动设备140、第三可移动设备130和第四可移动设备120。
第一可移动设备生成第一初始规划路径发送到路径规划服务器，同理，第二可移动设备生成第二初始规划路径、第三可移动设备生成第三初始规划路径和第四可移动设备生成第四初始规划路径也都发送到路径规划服务器，路径规划服务器根据接收到的四条初始规划路径，由于一些原因，只计算得到与第一初始规划路径对应的第一目标规划路径，和与第三初始规划路径对应的第三目标规划路径。
需要说明的是，初始规划路径可以由可移动设备获取并发送到路径规划服务器，同时也可以由路径规划服务器进行获取。
如图2所示，由路径规划服务器执行的路径优化的方法包括：S110，获取环境地图；S120，根据环境地图得到与N个可移动设备分别对应的初始规划路径，得到N条初始规划路径；S130，根据N条初始规划路径的各起始位置点创建N条待优化路径；S140，根据优化误差函数对N条待优化路径进行处理，得到N个可移动设备中至少一个可移动设备的目标规划路径。
下面示例性阐述图2的各步骤。
S110，获取环境地图。
在一种实施方式中，环境地图包括N个可移动设备中各可移动设备的起始位置点和目标位置点，N为大于1的整数。
路径规划服务器获取N个可移动设备所在环境的环境地图，获取N个可移动设备中所有可移动设备的状态，将处于静止状态的可移动设备转换为静态障碍物标注在环境地图(栅格地图)上，搜索所有任务中可移动设备的起始位置点和目标位置点。例如，图1的路径规划服务器获取四个可移动设备所在环境的栅格地图，并通过获取的四个可移动设备的状态进行搜索获得环境地图。
需要说明是，可移动设备是任何可以按照规划路径进行移动的设备，可以是扫地机器人、送货机器人、引导机器人或者无人驾驶车等。N个可移动设备可以至少包括两个可移动设备、也可以是十个可移动设备，可移动设备所处的运动环境可以是办公场所、仓库、酒店等，本申请实施例不限于此。
下文示例性的阐述S120。
S120，根据环境地图得到与N个可移动设备分别对应的初始规划路径，得到N条初始规划路径。
路径规划服务器在获得N个可移动设备所在环境的环境地图后，基于现有的多设备路径碰撞搜索方法，搜索得到所有可移动设备的初始规划路径。以现有的路径搜索方法(例如，A*算法、基于冲突的多机路径搜索算法和基于安全间隔的多机路径规划)为例示例性阐述初始规划路径。
下面以A*算法示例性阐述初始路径的获取过程：通过A*算法得到所有可移动设备从起始位置点到目标位置点的第一初始规划路径；根据可移动设备的半径安装时序遍历得到第一初始规划路径中各路径之间的碰撞点，根据碰撞点生成碰撞树；再通过A*算法和碰撞树搜索得到第二初始规划路径，判断第二初始规划路径中各路径是否存在碰撞位置，若不存在碰撞位置将第二规划路径作为初始规划路径，若存在碰撞位置，重复上述过程直至生成的路径之间没有碰撞位置，得到N条初始规划路径。
下文示例性阐述S130的实施方式。
S130，根据N条初始规划路径的各起始位置点创建N条待优化路径。
在服务器得到与N个可移动设备相对应的N条初始规划路径后，以N条初始规划路径中的各起始位置点创建相对应的N条待优化路径。
如图3所示，在环境地图300中，N个可移动设备中第一可移动设备的第一初始规划路径L1中的路径点包括第一路径点1、第二路径点2、第三路径点3和第四路径点4。在一些实施例中，获取与第一初始规划路径L1对应的第一待优化路径M1的过程包括：在第一路径点1相临近的范围(小于设定阈值的范围)内设置4个路径点，由这4个路径点构成的路径为与第一初始规划路径对应的第一待优化路径M1，如图3所示第一待优化路径M1包括a点、b点、c点和d点。需要说明的是，在本申请的一些实施例中，各待优化路径包括的多个路径点都集中在起始位置点(即图3的第一路径点1)附近，经过S140多轮优化后，可以把第一待优化路径优化为如图4所示的目标规划路径D1。在本申请的另一些实施例中，各待优化路径包括的多个路径点都集中在目标位置点(即图3的第四路径点4)附近，经过S140多轮优化后，也可以把第一待优化路径优化为如图4所示的目标规划路径D1。
需要说明的是，N条待优化路径与N条初始规划路径的路径点数量一致。
下文示例性阐述S140的实施方式。
S140，根据优化误差函数对N条待优化路径进行处理，得到N个可移动设备中至少一个可移动设备的目标规划路径。
在一种实施方式中，优化误差函数用于保证目标规划路径与相对应的初始规划路径尽可能的接近(即接近程度满足要求)，且进一步保证各可移动设备在目标规划路径上的速度的改变频率尽可能的小(即速度代表频率满足要求)。
在一种实施方式中，优化误差函数与各可移动设备在对应的待优化路径上的速度变化的频率相关；优化误差函数与各可移动设备的初始规划路径和待优化路径之间的偏移量相关。也就是说，在本申请的一些实施例中通过优化误差函数可以保证各可移动设备在对应的待优化路径上的速度变化的频率最小，且可以保证各可移动设备对应的待优化路径与初始规划路径对应的各路径的偏移量最小。
使目标规划路径与相对应的初始规划路径的接近程度满足要求，保证各可移动设备在目标规划路径上的速度的改变频率满足要求。也就是说，通过优化误差函数使待优化路径上的路径点趋近于初始规划路径上路径点(即初始规划路径和待优化路径之间的偏移量较小)，同时对待优化路径上的速度变化率尽可能的小，以使得到的目标规划路径既与初始规划路径相近，又保证可移动设备能够以较小的速度变化率平稳的运行。
在一种实施方式中，优化误差函数的计算公式为：
其中，N表示可移动设备的总个数，Q表示N条待优化路径中最长待优化路径的路径点个数，Δu表示N条待优化路径中各优化路径包括的相邻的两个路径点之间的速度差，Δz表示N条待优化路径中各优化路径包括的与N条初始规划路径所对应的各路径点之间的距离差，A为第一常数矩阵，B为第二常数矩阵。
在一种实施方式中，S140包括：根据N个可移动设备中各可移动设备的运动学属性特征和优化误差函数，确定至少一个可移动设备的目标规划路径。运动学属性特征包括：运动学约束、速度极限和加速度极限。
上述优化误差函数可以保证目标规划路径满足趋近于初始规划路径和速度变化率平稳，但在可移动设备实际运动的过程中，还要满足可移动设备的运动学约束、速度极限和加速度极限，以使可移动设备能够在狭窄的道路中安全平稳的运行。
在一种实施方式中，S140包括：根据运动学属性特征、优化误差函数和任意两个可移动设备的安全运动距离，确定至少一个可移动设备的目标规划路径。
下面结合具体计算公式示例性阐述根据优化误差函数和各约束条件得到目标优化路径的过程。
作为一种实施例，使用上述优化误差函数对待优化路径进行优化，取待优化路径中相邻的两个路径点之间的速度差Δu，和待优化路径与初始规划路径所对应的路径点之间的偏移量Δz，以及第一常数矩阵A和第二常数矩阵B带入上述公式(1)，对待优化路径中路径点的速度变化和偏移量进行优化，得到路径点的位置和速度。其中，Δu＝(Δν，Δω)，Δν表示待优化路径中相邻的两个路径点之间的线速度差，Δω表示待优化路径中相邻的两个路径点之间的角速度差；第一常数矩阵A可以是正定矩阵
第二常数矩阵B可以是正定矩阵
a1、a2、a3、a4、b1、b2、b3、b4、b5、b6、b7、b8和b9可以是任意常数，其中，优化误差函数使速度变化率尽可能的小，可以使可移动设备平稳运行；通过尽量缩小待优化路径与初始规划路径的偏移量，可以使最终得到的目标规划路径不偏离初始规划路径。
因此，本申请实施例通过优化误差函数，能够使待优化路径上的速度变化的频率和初始规划路径和待优化路径之间的偏移量满足要求，从而能够使目标规划路径满足安全和平滑约束，从而使可移动设备安全平稳运行。
在使用优化误差函数对待优化路径进行优化的过程中，需要保证起始位置点和目标位置点不变；满足可移动设备的运动模型，即在待优化路径中，相邻的两个路径点的坐标满足可移动设备自身的运动规律，例如：假设在待优化路径中，相邻的两个路径点分别为a点和b点，a点为b点的前一个路径点，若a点的坐标为(x，y，θ)，u＝(ν，ω)，那么b点的坐标就应该为(x+νcosθdt，y+νsinθdt，θ+ωdt)；还需满足可移动设备的速度极限和加速度极限。
例如：可移动设备的速度极限为2m/s，那么在优化待优化路径点的过程中，就应使路径点的速度小于等于2m/s，同理可移动设备的加速度极限为2m/s2，那么就应使路径点的加速度小于等于2m/s2；还需满足任意两个可移动设备相近的两个路径点之间的距离小于等于两个可移动设备的半径之和，以使可移动设备在狭窄的通道中能够安全稳定工作。
通过上述约束条件与优化误差函数，对各待优化路径进行优化后，得到各目标优化路径，例如：如图4所示的目标优化路径D1，可以看出D1与初始规划路径L1中路径点相近，但由于D1需要满足上述约束条件与优化误差函数，有存在一定的差异，以使可移动设备能够平稳安全的运行，从而实现准确规划可移动设备的路径。
因此，本申请实施例通过可移动设备的运动属性特征对待优化路径进行约束，能够使目标规划路径满足可移动设备的运动属性，从而能够使可移动设备安全平稳运行。
在一种实施方式中至少一个可移动设备为M个可移动设备，至少一个可移动设备的总个数为M，其中，M为大于或者等于1且小于或者等于N的整数；在根据优化误差函数对N条待优化路径进行处理，得到N个可移动设备中至少一个可移动设备的目标规划路径之后，方法还包括：调整P个可移动设备的工作状态为休眠状态，其中，P个可移动设备是N个可移动设备中除去M个可移动设备后剩余的可移动设备，P为大于或者等于1且小于或者等于N的整数；根据预设时间唤醒P个可移动设备；重新规划P个可移动设备的路径。
在获取N个可移动设备对应的目标规划路径的过程中，作为一种实施例，N个可移动设备都生成了目标规划路径，那么N个可移动设备就对应生成了N个目标规划路径；作为另一种实施例，由于一些原因导致有部分可移动设备没有生成目标规划路径，也就是说，N个可移动设备对应生成了M个目标规划路径，同时M个目标规划路径对应M个可移动设备，服务器需要对N个可移动设备中除去M个可移动设备后剩余的P个可移动设备，进行休眠，在预设时间之后，唤醒P个可移动设备，再规划P个可移动设备的目标规划路径，以使能够保证大部分可移动设备顺利执行任务。
因此，本申请实施例通过对没有生成目标规划路径的可移动设备状态的调整，能够保证其他可移动设备的正常运转，从而保证被执行任务按时完成。
下文示例性阐述由服务器执行的S150的实施方式。
S150，向至少一个可移动设备发送目标规划路径。
在一种实施方式中，向至少一个可移动设备发送目标规划路径，以使至少一个可移动设备中的各可移动设备按照对应的目标规划路径从相应的起始位置点运动至目标位置点。
服务器在规划完成可移动设备的目标规划路径之后，将能够成功规划完成的至少一个可移动设备对应的目标规划路径，发送至可移动设备中，可移动设备可以按照对应的目标规划路径，从相应的起始位置点运动至目标位置点，完成需要执行的任务。
因此，本申请实施例通过优化误差函数和N条待优化路径，能够获得至少一个可移动设备的目标规划路径，以使可移动设备能够根据目标规划路径进行运动，从而能够使可移动设备在狭窄通道的特殊工作环境中安全工作，从而使目标规划路径准确安全。
上文描述了服务器执行的路径优化的方法，下文将描述路径优化的方法的具体实施例。
如图5所示，作为本申请多种场景中的一种，以可移动设备为机器人为例描述路径优化的具体实施例。
S510，创建机器人所在环境的环境地图。
服务器获取N个机器人所在环境的环境地图，获取N个机器人中所有机器人的状态，将空闲的机器人转换为静态障碍物标注在环境地图上，搜索所有任务中机器人的起始位置点和目标位置点。
S520，基于环境地图搜索初始规划路径。
服务器在获得N个机器人所在环境的环境地图后，基于现有的多机器人路径碰撞搜索方法，搜索得到所有机器人的初始规划路径，搜索方法简述如下：通过A*算法得到所有机器人从起始位置点到目标位置点的路径第一初始规划路径；根据机器人的半径安装时序遍历得到第一初始规划路径中各路径之间的碰撞点，根据碰撞点生成碰撞树；再通过A*算法和碰撞树搜索得到第二初始规划路径，判断第二初始规划路径中各路径是否存在碰撞位置，若不存在碰撞位置将第二规划路径作为初始规划路径，若存在碰撞位置，重复上述过程直至生成的路径之间没有碰撞位置，得到最终的N条初始规划路径。
S530，通过优化误差函数、运动学约束和初始规划路径对待优化路径进行优化，获得目标规划路径。
创建N个机器人所对应的N个待优化路径，N个待优化路径上的所有起始位置点都集中在所对应的初始规划路径的起始位置点附近。
在创建完成各可移动设备的待优化路径后，依据如下优化误差函数：
其中，N表示可移动设备的总个数，Q表示N条待优化路径中最长待优化路径的路径点个数，Δu表示N条待优化路径中各优化路径包括的相邻的两个路径点之间的速度差，Δz表示N条待优化路径中各优化路径包括的与N条初始规划路径所对应的各路径点之间的距离差，A为第一常数矩阵，B为第二常数矩阵。
将待优化路径中相邻的两个路径点之间的速度差Δu，Δu＝(Δν，Δω)，和待优化路径与初始规划路径所对应的路径点之间的偏移量Δz，以及第一常数矩阵
和第二常数矩阵
带入上述公式，得到各路径点的位置的速度。在通过上述优化误差函数对待优化路径进行优化的同时，还要满足约束条件：保证起始位置点和目标位置点不变；满足机器人的运动模型，即在待优化路径中，相邻的两个路径点的坐标满足相邻的两个路径点分别为a点和b点，a点为b点的前一个路径点，若a点的坐标为(x，y，θ)，u＝(ν，ω)，那么b点的坐标就应该为(x+νcosθdt，y+νsinθdt，θ+ωdt)；还需满足机器人的速度极限和加速度极限：路径点的速度小于等于2m/s，路径点的加速度小于等于2m/s2；还需满足任意两个机器人相近的两个路径点之间的距离小于等于两个机器人的半径之和，以使机器人在狭窄的通道中能够安全稳定工作。
S540，处理异常情况。
由于一些原因导致有部分机器人没有生成目标规划路径，也就是说，N个机器人对应生成了M个目标规划路径，同时M个目标规划路径对应M个机器人，服务器需要对N个机器人中除去M个机器人后剩余的P个机器人，进行休眠，在预设时间之后，唤醒P个机器人，再规划P个机器人的目标规划路径，以使能够保证大部分机器人顺利执行任务。
上文描述了一种路径优化的方法的具体实施例，下文将描述一种路径优化的装置。
如图6所示，本申请的一些实施例还提供一种路径优化的装置600，包括：获取单元610、第一规划单元620、待优化路径创建单元630、第二规划单元640和发送单元650。
在一种实施方式中，本申请实施例提供一种路径优化的装置，包括：获取单元，被配置为获取环境地图，其中，环境地图包括N个可移动设备中各可移动设备的起始位置点和目标位置点，N为大于1的整数；第一规划单元，被配置为根据环境地图得到与N个可移动设备分别对应的初始规划路径，得到N条初始规划路径；待优化路径创建单元，被配置为根据N条初始规划路径的各起始位置点创建N条待优化路径；第二规划单元，被配置为根据优化误差函数对N条待优化路径进行处理，得到N个可移动设备中至少一个可移动设备的目标规划路径，其中，优化误差函数用于保证目标规划路径与相对应的初始规划路径的接近程度满足要求，且进一步保证各可移动设备在目标规划路径上的速度的改变频率满足要求；发送单元，被配置为向至少一个可移动设备发送目标规划路径，以使至少一个可移动设备中的各设备按照对应的目标规划路径从相应的起始位置点运动至目标位置点。
在一种实施方式中，优化误差函数与各可移动设备在对应的待优化路径上的速度变化的频率相关；优化误差函数与各可移动设备的初始规划路径和待优化路径之间的偏移量相关。
在一种实施方式中，优化误差函数的计算公式为：
其中，N表示所述可移动设备的总个数，Q表示所述N条待优化路径中最长待优化路径的路径点个数，Δu表示所述N条待优化路径中各优化路径包括的相邻的两个路径点之间的速度差，Δz表示所述N条待优化路径中各优化路径包括的与所述N条初始规划路径所对应的各路径点之间的距离差，A为第一常数矩阵，B为第二常数矩阵。
在一种实施方式中，第二规划单元还被配置为：根据N个可移动设备中各可移动设备的运动学属性特征和优化误差函数，确定与所述至少一个可移动设备对应的目标规划路径。
在一种实施方式中，运动学属性特征包括：运动学约束、速度极限和加速度极限。
在一种实施方式中，第二规划单元还被配置为：根据运动学属性特征、优化误差函数和任意两个可移动设备的安全运动距离，确定至少一个可移动设备的目标规划路径。
在一种实施方式中，至少一个可移动设备的总个数为M，其中，M为大于或者等于1且小于或者等于N的整数；待优化路径创建单元还被配置为调整P个可移动设备的工作状态为休眠状态，其中，P个可移动设备是N个可移动设备中除去M个可移动设备后剩余的可移动设备，P为大于或者等于1且小于或者等于N的整数；根据预设时间唤醒P个可移动设备；重新规划P个可移动设备的路径。
在本申请实施例中，图6所示模块能够实现图1至图5方法实施例中的各个过程。图6中的各个模块的操作和/或功能，分别为了实现图1至图5中的方法实施例中的相应流程。具体可参见上述方法实施例中的描述，为避免重复，此处适当省略详细描述。
如图7所示，本申请实施例提供一种电子设备700，包括：处理器710、存储器720和总线730，所述处理器通过所述总线与所述存储器相连，所述存储器存储有计算机可读取指令，当所述计算机可读取指令由所述处理器执行时，用于实现如上述所有实施例中任一项所述的方法，具体可参见上述方法实施例中的描述，为避免重复，此处适当省略详细描述。
其中，总线用于实现这些组件直接的连接通信。其中，本申请实施例中处理器可以是一种集成电路芯片，具有信号的处理能力。上述的处理器可以是通用处理器，包括中央处理器(Central Processing Unit，简称CPU)、网络处理器(Network Processor，简称NP)等；还可以是数字信号处理器(DSP)、专用集成电路(ASIC)、现成可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本申请实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
存储器可以是，但不限于，随机存取存储器(Random Access Memory，RAM)，只读存储器(Read Only Memory，ROM)，可编程只读存储器(Programmable Read-Only Memory，PROM)，可擦除只读存储器(Erasable Programmable Read-Only Memory，EPROM)，电可擦除只读存储器(Electric Erasable Programmable Read-Only Memory，EEPROM)等。存储器中存储有计算机可读取指令，当所述计算机可读取指令由所述处理器执行时，可以执行上述实施例中所述的方法。
可以理解，图7所示的结构仅为示意，还可包括比图7中所示更多或者更少的组件，或者具有与图7所示不同的配置。图7中所示的各组件可以采用硬件、软件或其组合实现。
本申请实施例还提供一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，该计算机程序被服务器执行时实现上述所有实施方式中任一所述的方法，具体可参见上述方法实施例中的描述，为避免重复，此处适当省略详细描述。
以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。
以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应所述以权利要求的保护范围为准。
Claims (9)
1.一种路径优化的方法，其特征在于，所述方法包括：
获取环境地图，其中，所述环境地图包括N个可移动设备中各可移动设备的起始位置点和目标位置点，所述N为大于1的整数；
根据所述环境地图得到与所述N个可移动设备分别对应的初始规划路径，得到N条初始规划路径；
根据所述N条初始规划路径的各起始位置点创建N条待优化路径；
根据优化误差函数对所述N条待优化路径进行处理，得到所述N个可移动设备中至少一个可移动设备的目标规划路径，其中，所述优化误差函数用于保证所述目标规划路径与相对应的初始规划路径的接近程度满足要求，且进一步保证所述各可移动设备在所述目标规划路径上的速度的改变频率满足要求；
向所述至少一个可移动设备发送所述目标规划路径，以使所述至少一个可移动设备中的各可移动设备按照对应的所述目标规划路径从相应的起始位置点运动至目标位置点；
所述优化误差函数的计算公式为：
其中，N表示所述可移动设备的总个数，Q表示所述N条待优化路径中最长待优化路径的路径点个数，Δu表示所述N条待优化路径中各优化路径包括的相邻的两个路径点之间的速度差，Δz表示所述N条待优化路径中各优化路径包括的与所述N条初始规划路径所对应的各路径点之间的距离差，A为第一常数矩阵，B为第二常数矩阵。
2.根据权利要求1所述的方法，其特征在于，
所述优化误差函数与各可移动设备在对应的待优化路径上的速度变化的频率相关；
所述优化误差函数与所述各可移动设备的初始规划路径和待优化路径之间的偏移量相关。
3.根据权利要求2所述的方法，其特征在于，所述根据优化误差函数对所述N条待优化路径进行处理，得到所述N个可移动设备中至少一个可移动设备的目标规划路径，包括：
根据所述N个可移动设备中各可移动设备的运动学属性特征和所述优化误差函数，确定与所述至少一个可移动设备对应的目标规划路径。
4.根据权利要求1所述的方法，其特征在于，所述根据优化误差函数对所述N条待优化路径进行处理，得到所述N个可移动设备中至少一个可移动设备的目标规划路径，包括：
根据运动学属性特征、所述优化误差函数和任意两个可移动设备的安全运动距离，确定与所述至少一个可移动设备对应的目标规划路径。
5.根据权利要求3或4所述的方法，其特征在于，所述运动学属性特征包括：运动学约束、速度极限和加速度极限。
6.根据权利要求1所述的方法，其特征在于，所述至少一个可移动设备的总个数为M，其中，M为大于或者等于1且小于或者等于N的整数；
在所述根据优化误差函数对所述N条待优化路径进行处理，得到所述N个可移动设备中至少一个可移动设备的目标规划路径之后，所述方法还包括：
调整P个可移动设备的工作状态为休眠状态，其中，所述P个可移动设备是所述N个可移动设备中除去M个可移动设备后剩余的可移动设备，P为大于或者等于1且小于或者等于N的整数；
根据预设时间唤醒所述P个可移动设备；
重新规划所述P个可移动设备的路径。
7.一种路径优化的装置，其特征在于，所述装置包括：
获取单元，被配置为获取环境地图，其中，所述环境地图包括N个可移动设备中各可移动设备的起始位置点和目标位置点，所述N为大于1的整数；
第一规划单元，被配置为根据所述环境地图得到与所述N个可移动设备分别对应的初始规划路径，得到N条初始规划路径；
待优化路径创建单元，被配置为根据所述N条初始规划路径的各起始位置点创建N条待优化路径；
第二规划单元，被配置为根据优化误差函数对所述N条待优化路径进行处理，得到所述N个可移动设备中至少一个可移动设备的目标规划路径，其中，所述优化误差函数用于保证所述目标规划路径与相对应的初始规划路径的接近程度满足要求，且进一步保证所述各可移动设备在目标规划路径上的速度的改变频率满足要求；
发送单元，被配置为向所述至少一个可移动设备发送所述目标规划路径，以使所述至少一个可移动设备中的各设备按照对应的目标规划路径从相应的起始位置点运动至目标位置点；
所述优化误差函数的计算公式为：
其中，N表示所述可移动设备的总个数，Q表示所述N条待优化路径中最长待优化路径的路径点个数，Δu表示所述N条待优化路径中各优化路径包括的相邻的两个路径点之间的速度差，Δz表示所述N条待优化路径中各优化路径包括的与所述N条初始规划路径所对应的各路径点之间的距离差，A为第一常数矩阵，B为第二常数矩阵。
8.一种电子设备，其特征在于，包括：处理器、存储器和总线；
所述处理器通过所述总线与所述存储器相连，所述存储器存储有计算机可读取指令，当所述计算机可读取指令由所述处理器执行时，用于实现如权利要求1-6任一项所述方法。
9.一种计算机可读存储介质，其特征在于，该计算机可读存储介质上存储有计算机程序，该计算机程序被执行时实现如权利要求1-6任一项所述方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110439686.4A CN113188543B (zh) | 2021-04-22 | 2021-04-22 | 一种路径优化的方法、装置、电子设备及存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110439686.4A CN113188543B (zh) | 2021-04-22 | 2021-04-22 | 一种路径优化的方法、装置、电子设备及存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113188543A CN113188543A (zh) | 2021-07-30 |
| CN113188543B true CN113188543B (zh) | 2023-02-24 |
Family
ID=76978481
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110439686.4A Active CN113188543B (zh) | 2021-04-22 | 2021-04-22 | 一种路径优化的方法、装置、电子设备及存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113188543B (zh) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103217979A (zh) * | 2013-03-29 | 2013-07-24 | 无锡普智联科高新技术有限公司 | 基于缓冲区的多移动机器人路径冲突解决方法 |
| KR20160021161A (ko) * | 2016-02-01 | 2016-02-24 | 고려대학교 산학협력단 | 복수의 이동 로봇 간의 충돌 회피를 위한 경로 생성 방법 |
| CN109991977A (zh) * | 2019-03-04 | 2019-07-09 | 斯坦德机器人（深圳）有限公司 | 机器人的路径规划方法及装置 |
| CN111340286A (zh) * | 2020-02-24 | 2020-06-26 | 广东博智林机器人有限公司 | 机器人调度方法、装置、电子设备以及存储介质 |
| CN111552296A (zh) * | 2020-05-14 | 2020-08-18 | 宁波智能装备研究院有限公司 | 一种基于弯曲柱坐标系的局部平滑轨迹规划方法 |
| CN112631173A (zh) * | 2020-12-11 | 2021-04-09 | 中国人民解放军国防科技大学 | 脑控无人平台协同控制系统 |
-
2021
- 2021-04-22 CN CN202110439686.4A patent/CN113188543B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103217979A (zh) * | 2013-03-29 | 2013-07-24 | 无锡普智联科高新技术有限公司 | 基于缓冲区的多移动机器人路径冲突解决方法 |
| KR20160021161A (ko) * | 2016-02-01 | 2016-02-24 | 고려대학교 산학협력단 | 복수의 이동 로봇 간의 충돌 회피를 위한 경로 생성 방법 |
| CN109991977A (zh) * | 2019-03-04 | 2019-07-09 | 斯坦德机器人（深圳）有限公司 | 机器人的路径规划方法及装置 |
| CN111340286A (zh) * | 2020-02-24 | 2020-06-26 | 广东博智林机器人有限公司 | 机器人调度方法、装置、电子设备以及存储介质 |
| CN111552296A (zh) * | 2020-05-14 | 2020-08-18 | 宁波智能装备研究院有限公司 | 一种基于弯曲柱坐标系的局部平滑轨迹规划方法 |
| CN112631173A (zh) * | 2020-12-11 | 2021-04-09 | 中国人民解放军国防科技大学 | 脑控无人平台协同控制系统 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113188543A (zh) | 2021-07-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US10564647B2 (en) | Method and apparatus for determining a desired trajectory for a vehicle | |
| CN113715814B (zh) | 碰撞检测方法、装置、电子设备、介质及自动驾驶车辆 | |
| CN108205318B (zh) | 机器人轨迹规划方法及装置 | |
| EP1733287B1 (en) | System and method for adaptive path planning | |
| CN109213155B (zh) | 用于多机器人相互避让的调度方法、装置及服务器 | |
| CN111142883B (zh) | 基于sdn架构的车辆计算任务卸载方法 | |
| KR102531197B1 (ko) | 무인 비행체의 최적 경로 생성 방법 및 장치 | |
| CN109916421B (zh) | 路径规划方法及装置 | |
| CN113165670A (zh) | 一种智能驾驶方法、装置、存储介质及计算机程序 | |
| CN112393732B (zh) | 无人机避障方法、装置、可读存储介质及电子设备 | |
| Zhang et al. | A trajectory optimization-based intersection coordination framework for cooperative autonomous vehicles | |
| KR20210139204A (ko) | 차량 궤적 계획 방법, 장치, 전자 기기, 저장 매체 및 컴퓨터 프로그램 | |
| Sabattini et al. | Closed-curve path tracking for decentralized systems of multiple mobile robots | |
| CN111813112A (zh) | 一种车辆轨迹点确定方法、装置、车辆及存储介质 | |
| CN111142567A (zh) | 一种无人机系统中的无人机目标位置交换方法及装置 | |
| CN114771551A (zh) | 自动驾驶车辆轨迹规划方法、装置和自动驾驶车辆 | |
| CN113188543B (zh) | 一种路径优化的方法、装置、电子设备及存储介质 | |
| Wang et al. | A universal trajectory planning method for automated lane-changing and overtaking maneuvers | |
| CN110370270B (zh) | 一种防止机器人碰撞的方法和装置 | |
| Van Koevering et al. | Provable probabilistic safety and feasibility-assured control for autonomous vehicles using exponential control barrier functions | |
| CN105651276A (zh) | 一种基于动态线路规划仓库路径的系统 | |
| CN113361174A (zh) | 基于stp模型的大型无人机碰撞概率计算方法 | |
| Xiao et al. | Overcoming Occlusions: Perception Task-Oriented Information Sharing in Connected and Autonomous Vehicles | |
| CN114475652A (zh) | 一种车辆运动规划方法、装置、设备和介质 | |
| CN112577503B (zh) | 车辆起点区域的路径规划方法、装置、设备 |
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 | ||
| CB02 | Change of applicant information | ||
| CB02 | Change of applicant information |
Address after: Room 702, 7 / F, 67 North Fourth Ring Road West, Haidian District, Beijing Applicant after: Beijing Yunji Technology Co.,Ltd. Address before: Room 702, 7 / F, 67 North Fourth Ring Road West, Haidian District, Beijing Applicant before: BEIJING YUNJI TECHNOLOGY Co.,Ltd. |
|
| GR01 | Patent grant | ||
| GR01 | Patent grant |