CN113433936B - 移动设备绕障方法、装置、移动设备及存储介质 - Google Patents
移动设备绕障方法、装置、移动设备及存储介质 Download PDFInfo
- Publication number
- CN113433936B CN113433936B CN202110616400.5A CN202110616400A CN113433936B CN 113433936 B CN113433936 B CN 113433936B CN 202110616400 A CN202110616400 A CN 202110616400A CN 113433936 B CN113433936 B CN 113433936B
- Authority
- CN
- China
- Prior art keywords
- control point
- obstacle
- path
- distance
- constraint function
- 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
- 238000000034 method Methods 0.000 title claims abstract description 97
- 238000003860 storage Methods 0.000 title claims abstract description 14
- 238000005457 optimization Methods 0.000 claims abstract description 44
- 238000005070 sampling Methods 0.000 claims abstract description 39
- 230000006870 function Effects 0.000 claims description 161
- 230000008569 process Effects 0.000 claims description 38
- 230000001133 acceleration Effects 0.000 claims description 37
- 238000012804 iterative process Methods 0.000 claims description 24
- 238000004590 computer program Methods 0.000 claims description 13
- 238000004422 calculation algorithm Methods 0.000 claims description 12
- 238000010845 search algorithm Methods 0.000 claims description 7
- 230000036461 convulsion Effects 0.000 claims description 4
- 230000008859 change Effects 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 9
- 238000010276 construction Methods 0.000 description 5
- 230000008901 benefit Effects 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000012795 verification Methods 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000009499 grossing Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
Classifications
-
- 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/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
- G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
-
- 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/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- 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/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- 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/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Optics & Photonics (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供了一种移动设备绕障方法、装置、移动设备及存储介质,该方法包括:基于路径规划指令,确定从当前位置到所述避障位置的初始路径,所述避障位置位于全局路径上;对所述初始路径进行采样,并将路径采样点作为待拟合曲线的控制点的初始位置;基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行优化;根据优化得到的控制点的位置,确定所述当前位置到所述避障位置的局部目标路径。本发明将控制点的位置作为优化变量,相对于基于优化的方法,减少了优化参数,减少了求解耗时,从而可以在低成本的计算单元上满足实时性的需求。
Description
技术领域
本发明涉及控制技术领域,特别是涉及一种移动设备绕障方法、装置、移动设备及存储介质。
背景技术
对于某一类移动设备,通常是沿着设定的路径执行相应的任务,如仓储机器人、酒店服务机器人等,但是当行进路径上存在临时障碍物时,移动设备就会停在障碍物前等待障碍物离开。一种更合适的做法是,在设定的路径周围进行绕障,当障碍物挡住行进道路时,移动设备可以灵活地绕过障碍物并且不与障碍物发生碰撞。
现有技术中,可以采用传统的路径搜索算法(如A*,Hybrid A*),或者基于采样的算法(如动态窗口法DWA,快速搜索随机树RRT)来实现绕障,但此类方法是在离散空间进行的,而移动设备的控制空间和轨迹空间都是连续的,为了保证实时性,一般在比较粗粒度的分辨率下实现,因此规划出的轨迹并不是最优的。此外,也可以采用基于优化的方法(如时间弹性带算法TEB)来实现绕障,但该算法将轨迹参数化为离散的点集,因此待优化的变量维数较大,无法在低成本的计算单元上满足实时性的需求。
发明内容
鉴于上述问题,提出了本发明实施例以便提供一种克服上述问题或者至少部分地解决上述问题的一种移动设备绕障方法、装置、移动设备及存储介质。
依据本发明实施例的第一方面,提供了一种移动设备绕障方法,包括:
基于路径规划指令,确定从当前位置到所述避障位置的初始路径,所述避障位置位于全局路径上;
对所述初始路径进行采样,并将路径采样点作为待拟合曲线的控制点的初始位置;
基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行优化;
根据优化得到的控制点的位置,确定所述当前位置到所述避障位置的局部目标路径。
依据本发明实施例的第二方面,提供了一种移动设备绕障装置,包括:
初始路径确定模块,用于基于路径规划指令,确定从当前位置到所述避障位置的初始路径,所述避障位置位于全局路径上;
初始路径采样模块,用于对所述初始路径进行采样,并将路径采样点作为待拟合曲线的控制点的初始位置;
控制点优化模块,用于基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行优化;
绕障路径确定模块,用于根据优化得到的控制点的位置,确定所述当前位置到所述避障位置的局部目标路径。
依据本发明实施例的第三方面,提供了一种移动设备,包括:处理器、存储器及存储在所述存储器上并可在所述处理器上运行的计算机程序,所述计算机程序被所述处理器执行时实现如第一方面中所述的移动设备绕障方法。
依据本发明实施例的第四方面,提供了一种计算机可读存储介质,所述计算机可读存储介质上存储有计算机程序,所述计算机程序被处理器执行时实现如第一方面所述的移动设备绕障方法。
本发明实施例提供的移动设备绕障方法、装置、移动设备及存储介质,通过基于路径规划指令确定从当前位置到避障位置的初始路径,对初始路径进行采样,将路径采样点作为待拟合曲线的控制点的初始位置,基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行优化,根据优化得到的控制点的位置确定当前位置到避障位置的局部目标路径,由于通过对控制点的位置进行优化来获取更优的解,是在连续空间的求解,规划出的轨迹最优,而且将控制点的位置作为优化变量,相对于基于优化的方法,减少了优化参数,减少了求解耗时,从而可以在低成本的计算单元上满足实时性的需求。
上述说明仅是本发明技术方案的概述,为了能够更清楚了解本发明的技术手段,而可依照说明书的内容予以实施,并且为了让本发明的上述和其它目的、特征和优点能够更明显易懂,以下特举本发明的具体实施方式。
附图说明
通过阅读下文优选实施方式的详细描述,各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出优选实施方式的目的,而并不认为是对本发明的限制。
图1是本发明实施例提供的一种移动设备绕障方法的步骤流程图;
图2是本发明实施例中的确定避障位置的示意图;
图3是本发明实施例提供的一种移动设备绕障装置的结构框图。
具体实施方式
下面将参照附图更详细地描述本发明的示例性实施例。虽然附图中显示了本发明的示例性实施例,然而应当理解,可以以各种形式实现本发明而不应被这里阐述的实施例所限制。相反,提供这些实施例是为了能够更透彻地理解本发明,并且能够将本发明的范围完整的传达给本领域的技术人员。
图1是本发明实施例提供的一种移动设备绕障方法的步骤流程图,如图1所示,该方法可以包括:
步骤101,基于路径规划指令,确定从当前位置到避障位置的初始路径,所述避障位置位于全局路径上。
其中,全局路径是移动设备(如机器人)执行某项任务时从初始的起点到目标终点的全局的路径。在移动设备行进过程中,全局路径中的某个位置有可能被障碍物遮挡,这就需要进行局部绕障。
在移动设备沿着上一路径规划周期的规划轨迹行进时,如果将要与障碍物发生碰撞,或者移动设备偏离全局路径太远,则需要重新规划路径,确定接收到路径规划指令。基于路径规划指令,首先确定局部规划的终点,即避障位置,可以将全局路径上距离当前位置第一距离的一点,作为局部规划的避障位置,以根据当前位置和避障位置获得一条没有碰撞的绕障轨迹。例如,如图2所示,移动设备1在全局路径2上行进时,将会与第一障碍物3和第二障碍物4碰撞,这时确定全局路径2上距离当前位置第一距离的一点G’,作为局部规划的避障位置,以绕过第一障碍物3和第二障碍物4。其中,第一距离可以根据需要设定,例如可以是8米。
在确定避障位置后,可以通过路径搜索算法获得一个初始解,即可以基于导航地图,通过路径搜索算法搜索一条从当前位置到避障位置的初始路径。搜索初始路径时,有可能会搜索失败,这时可以按照全局路径进行行进,并在到达下一路径规划周期时再进行规划。其中,路径搜索算法可以为A*算法或者Hybrid A*算法。A*搜索是在离散的栅格地图上进行的,每个格子的边长一般是5cm。Hybrid A*是在离散的状态空间上进行的,比如方向盘分别为-10,0,10度并前进1米。
在本发明的一个实施例中,所述方法还包括:
在到达路径规划周期时,若当前位置偏离所述全局路径的距离大于第一距离阈值,或者,上一路径规划周期的规划轨迹在行进方向上第一距离内有障碍物且距离障碍物的距离大于或等于第二距离阈值,或者,上一路径规划周期的规划轨迹在行进方向上所述第一距离内有障碍物且预测得到的临时终点与障碍物的距离不小于第三距离阈值,则确定接收到路径规划指令。
移动设备中可以包括地图模块、移动设备位姿的定位模块、全局规划模块、绕障模块和控制模块,地图模块根据定位模块输出的移动设备前时刻的位姿、激光点云数据,对概率栅格地图进行更新,概率栅格地图用于表示每个栅格中存在障碍物的概率,根据概率栅格地图的概率可以获得指定位置的占据状态,占据状态可以包括存在障碍物、空闲或未知;地图模块再将概率栅格地图进行欧式距离转换(Euclidean Distance Transform,EDT)获得导航地图,导航地图中,每个栅格内的值表示该位置与最近障碍物的距离。绕障模块用于执行本发明实施例中的移动设备绕障方法。控制模块用于控制移动设备按照规划的轨迹行进。
移动设备按照路径规划周期来对路径进行局部规划,在到达路径规划周期时,基于导航地图,判断当前位置偏离全局路径的距离是否大于第一距离阈值,如果是,则可能是由于控制等原因导致移动设备偏离全局路径太远,需要重新规划路径,确定接收到路径规划指令,如果不是,继续判断上一路径规划周期的规划轨迹是否将要与障碍物发生碰撞,如果不会与障碍物发生碰撞,即在上一路径规划周期的规划轨迹的行进方向上第一距离内没有有障碍物,则不需要重新规划路径,如果将要与障碍物发生碰撞,即在上一路径规划周期的规划轨迹的行进方向上第一距离内有障碍物,则还需要判断行进方向上移动设备与障碍物的距离是否大于或等于第二距离阈值,或者预测得到的避障位置与障碍物的距离是否大于或等于第三距离阈值,如果在上一路径规划周期的规划轨迹在行进方向上第一距离内有障碍物的情况下,行进方向上移动设备与障碍物的距离是否大于或等于第二距离阈值,或者预测得到的避障位置与障碍物的距离是否大于或等于第三距离阈值,则确定需要重新规划路径,即接收到路径规划指令,如果在上一路径规划周期的规划轨迹在行进方向上第一距离内有障碍物的情况下,行进方向上移动设备与障碍物的距离小于第二距离阈值,或者避障位置与障碍物的距离小于第三距离阈值,则确定不需要重新规划路径,因为移动设备距离障碍物较近或者避障位置距离障碍物较近,即使重新规划也会失败,所以这时不需要重新规划路径,按照上一周期的规划轨迹行进距离障碍物较近时可以进行停障。
在本发明的一个实施例中,所述基于路径规划指令,确定从当前位置到所述避障位置的初始路径,包括:确定距离所述全局路径第二距离内的区域为搜索区域;在所述搜索区域内,通过路径搜索算法搜索从所述当前位置到所述避障位置的初始路径。
由于需要在全局路径附近进行绕障,首先需要构造搜索区域,即将全局路径第二预设距离内的区域作为搜索区域,如图2中两条虚线内的区域。在搜索区域内以当前位置为起点,通过路径搜索算法搜索一条到避障位置的路径,作为初始路径。其中,路径搜索算法可以采用Hybrid A*算法。通过在搜索区域内搜索初始路径,可以避免出现初始路径偏离全局路径太远的情况。
在本发明的一个实施例中,所述方法还包括:若所述搜索区域被障碍物完全遮挡,则在通过路径搜索算法搜索从所述当前位置到所述避障位置的初始路径时,确定搜索失败。
若在行进方向上,搜索区域被障碍物完全遮挡,即行进路径被大型障碍物遮挡,则通过路径搜索算法搜索初始路径时会搜索失败,因为障碍物较大无法在搜索区域内实现绕障,这时不需要进行绕障,可以由停障模块负责停障。
步骤102,对所述初始路径进行采样,并将路径采样点作为待拟合曲线的控制点的初始位置。
可以按照距离间隔或者时间间隔,对初始路径进行采样,将得到的路径采样点作为待拟合曲线的控制点的初始位置。其中,所述待拟合曲线可以是B样条曲线。
在本发明的一个实施例中,对所述初始路径进行采样,包括:计算在所述初始路径上行进所需的时间;按照指定时间间隔,对所述初始路径进行采样,得到所述初始路径上的路径采样点。
由于在非线性优化步骤中,优化变量是待拟合曲线的控制点,因此需要先将初始路径进行曲线的拟合,这时预估移动设备在初始路径上行进所需的时间,可以采用梯形规划预估得到,按照指定时间间隔对所述时间进行采样,得到多个时间采样点,预估移动设备在行进过程中到达每个时间采样点时移动设备所到达的路径采样点,得到时间采样点在初始路径上所对应的路径采样点。通过基于指定时间间隔来对初始路径进行采样获得路径采样点,将得到的路径采样点作为待拟合曲线的控制点的初始位置,可以获得合适的初始控制点,提高待拟合曲线拟合的速度。
待拟合曲线可以是B样条曲线,采用B样条曲线的优点有3点:第一,优化变量减少,采用B样条曲线的方式参数化,优化变量为n+1个控制点,优化变量数目为2*(n+1),通常控制点的数量不会太多,对于8米的路径20~30个控制点即可,而基于离散点的优化,通常需要更多的点数(如100个,此时平均间隔为8cm),优化参数越多,问题的规模越大,求解越耗时,从而B样条曲线相对于离散点的优化可以减少优化参数,提高求解速度;第二,B样条具有可微性,可以求得曲线上任意一点的位置、速度和加速度,而离散点的优化方式只能通过有限差分获得,时间间隔越大越不精确;第三,B样条曲线具有凸包性质,可以简化边界约束的表示。
步骤103,基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行优化。
其中,所述速度参量可以是加加速度,所述加加速度也可以称为急动度或力变率,是加速度的变化率。
基于初始位置以及待拟合曲线,对控制点的位置进行优化,以使得控制点与障碍物的距离较小,保证移动设备的安全性,并使得移动设备在控制点处的速度参量较小,保证待拟合曲线的平滑性。
在得到待拟合的待拟合曲线的初始位置后,通过最小二乘法拟合一条准均匀的B样条曲线:
其中,n+1为控制点的数量,Qi为第i个控制点的位置,p为B样条曲线的次数,Ni,p(t)为p次B样条曲线基函数。
以目标函数和约束条件构建非线性优化问题,将控制点的初始位置作为非线性优化问题的输入,对控制点的位置进行优化,以使得由控制点确定的B样条曲线能够较为平滑并且安全。需要注意的是,在求解时需要增大起点(即当前位置)和终点(即避障位置)的权重,保证该B样条曲线能穿过起点和终点。在求解时,优化变量为:[Qp,Qp+1,…,Qn-p],两端的控制点不参与优化,这是为了保证起点和终点的位置等于设定的值。在求解该非线性优化问题时,可能会求解失败,这时可以等待下一路径规划周期再进行路径规划。
在本发明的一个实施例中,基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行优化,包括:基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行迭代调整,直至控制点与障碍物的距离和移动设备在控制点处的速度参量收敛。
在基于初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行迭代调整,来对控制点的位置进行优化,具体的,将初始位置代入待拟合曲线的方程,得到初始位置对应的待拟合曲线,确定初始位置对应的待拟合曲线中的当前位置与避障位置之间的轨迹,在该轨迹中确定控制点与障碍物的距离以及移动设备在控制点处的速度参量,基于控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行调整,使得控制点与障碍物的距离以及移动设备在控制点处的速度参量减小,迭代执行上述将控制点的位置代入待拟合曲线的方程,得到控制点的位置对应的待拟合曲线,并对控制点的位置进行调整的操作,直至控制点与障碍物的距离和移动设备在控制点处的速度参量达到最小值,即控制点与障碍物的距离和移动设备在控制点处的速度参量收敛,结束迭代调整,得到优化后控制点的位置。
在本发明的一个实施例中,基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行迭代调整,直至控制点与障碍物的距离和移动设备在控制点处的速度参量收敛,包括:基于所述初始位置和控制点与障碍物的距离,构建安全性约束函数;基于所述初始位置和移动设备在控制点处的速度参量,构建平滑项约束函数;根据所述安全性约束函数、所述平滑项约束函数,对控制点的位置进行迭代调整,直至所述安全性约束函数和所述平滑项约束函数收敛。
通过安全性约束函数保证优化后的轨迹上没有碰撞,通过平滑项约束函数保证优化后的轨迹平滑,在构建安全性约束函数和平滑项约束函数后,可以对控制点的位置进行迭代调整,即将控制点的位置代入待拟合曲线的方程,得到从当前位置到避障位置的轨迹,基于该路径来确定安全性约束函数值和平滑项约束函数值,基于安全性约束函数值和平滑项约束函数值,对控制点的位置进行调整,使得安全性约束函数值和平滑项约束函数值减小,迭代执行上述将控制点的位置代入待拟合曲线的方程以及对控制点的位置进行调整的操作,直至安全性约束函数值和平滑项约束函数值达到最小,即安全性约束函数值和平滑项约束函数值收敛,结束迭代过程。
在本发明的一个实施例中,所述基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行迭代调整,直至控制点与障碍物的距离和移动设备在控制点处的速度参量收敛,还包括:基于所述初始位置、移动设备在迭代过程中待拟合曲线上的速度以及移动设备在迭代过程中待拟合曲线上的加速度,构建速度约束函数;根据所述速度约束函数,对控制点的位置进行迭代调整,直至所述速度约束函数收敛。
在对控制点的位置进行迭代调整的过程中,还可以保证在迭代过程中的轨迹上的速度满足要求,这时可以基于初始位置、移动设备在迭代过程中待拟合曲线上的速度以及移动设备在迭代过程中待拟合曲线上的加速度,构建速度约束函数,并基于安全性约束函数、平滑项约束函数和速度约束函数,对控制点的位置进行迭代调整,直至安全性约束函数、平滑项约束函数和速度约束函数收敛,结束迭代操作,这样可以保证移动设备的安全性、轨迹的平滑性以及在优化后的轨迹上的速度和加速度满足要求。
在本发明的一个实施例中,所述基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行迭代调整,直至控制点与障碍物的距离和移动设备在控制点处的速度参量收敛,还包括:基于所述初始位置和迭代过程中待拟合曲线的曲率,构建曲率约束函数;基于所述初始位置和迭代过程中待拟合曲线的起点角度和终点角度,构建角度约束函数;根据所述曲率约束函数、角度约束函数,对控制点的位置进行迭代调整,直至所述曲率约束函数、角度约束函数收敛。
在对控制点的位置进行迭代调整的过程中,还可以保证在迭代过程中的轨迹上的曲率和角度满足要求,这时可以基于初始位置和迭代过程中待拟合曲线的曲率,构建曲率约束函数函数,并基于初始位置和迭代过程中待拟合曲线的起点角度和终点角度构建角度约束函数,并基于安全性约束函数、平滑项约束函数、速度约束函数、曲率约束函数和角度约束函数,对控制点的位置进行迭代调整,直至安全性约束函数、平滑项约束函数、速度约束函数、曲率约束函数和角度约束函数收敛,结束迭代操作,这样可以保证移动设备的安全性、轨迹的平滑性、在优化后的轨迹上的速度和加速度满足要求、曲率和角度满足要求。
在对控制点的位置进行优化的过程中,优化参数除了控制点与障碍物的距离以及移动设备在控制点处的速度参量外,还可以包括移动设备在优化后轨迹上的速度、移动设备在优化后轨迹上的加速度、优化后轨迹的曲率、优化后轨迹的起点角度和终点角度,从而可以在保证安全和平滑的基础上,使得移动设备在优化后的轨迹上行进时速度、加速度、曲率、起点角度和终点角度均能满足要求,进一步提高安全性。
在本发明的一个实施例中,基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行迭代调整,直至控制点与障碍物的距离和移动设备在控制点处的速度参量收敛,包括:对所述安全性约束函数、平滑项约束函数、速度约束函数、加速度约束函数、曲率约束函数和角度约束函数进行加权,确定所述目标函数;基于所述目标函数,对控制点的位置进行迭代调整,直至所述目标函数收敛。
其中,所述目标函数表示如下:
ftotal=ω1Fs+ω2Fc+ω3(Fv+Fa)+ω4Fk+ω5Fθ
其中,ftotal为目标函数,Fs为平滑项约束函数,Fc为安全性约束函数,Fv为速度约束函数,Fa为加速度约束函数,Fk为曲率约束函数,Fθ为角度约束函数,ω1为平滑项约束函数的权重,ω2为安全性约束函数的权重,ω3为速度约束函数和加速度约束函数的权重,ω4为曲率约束函数的权重,ω5为角度约束函数的权重。
通过平滑项约束函数保证优化后的轨迹平滑,通过安全性约束函数保证优化后的轨迹上没有碰撞,通过速度约束函数和加速度约束函数,保证优化后的轨迹上的速度和加速度在设定范围内,通过曲率约束函数保证优化后的轨迹的曲率在设定范围内,通过角度约束函数的约束保证优化后的轨迹的起点和终点的方向尽可能不变。在对控制点的位置进行优化时,确定使得目标函数的值最小时的控制点的位置。
在上述目标函数中,所述平滑项约束函数表示如下:
Fs(i)=((Qi+3-2Qi+2+Qi+1)-(Qi+2-2Qi+1+Qi))2
其中,Qi表示第i个控制点,Fs(i)表示在第i个控制点处的速度参量的平方和;
所述安全性约束函数表示如下:
其中,Fc(d(Qi))表示控制点与障碍物的距离的平方和,d(Qi)为第i个控制点与障碍物的距离,dthr表示发生碰撞时移动设备与障碍物的最小距离;
所述速度约束函数表示如下:
其中,Fv(vμ)表示速度约束函数,vμ表示迭代过程中待拟合曲线上的速度,vmax表示速度阈值;
所述加速度约束函数表示如下:
其中,Fa(aμ)表示加速度约束函数,aμ表示迭代过程中待拟合曲线上的加速度,amax表示加速度阈值。
平滑项约束函数通过速度参量的平方和来表示。安全性约束函数通过控制点与障碍物的距离的平方和来表示,d(Qi)可以从导航地图中直接获得。在速度约束函数和加速度约束函数中,速度和加速度可以由控制点差分获得。曲率约束函数用于约束优化后的轨迹的曲率在设定曲率范围内,曲率可以由控制点计算得到。角度约束函数用于约束优化后的轨迹的起点和终点的方向尽可能不变,起点角度和终点角度可以由控制点计算得到。
在构建上述非线性优化问题后,可以采用开源第三方库ipopt或nlopt对上述控制点的位置进行优化求解。
在本发明的一个实施例中,基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行优化,包括:
基于所述初始位置、控制点与障碍物的距离、移动设备在控制点处的速度参量和迭代过程中待拟合曲线的参数,对控制点的位置进行迭代调整,所述迭代过程中待拟合曲线的参数包括所述移动设备在迭代过程中待拟合曲线上的速度、所移动设备在迭代过程中待拟合曲线上的加速度、迭代过程中待拟合曲线的曲率、迭代过程中待拟合曲线的起点角度和终点角度中的一个或多个。
在基于初始位置、控制点与障碍物的距离、移动设备在控制点处的速度参量和迭代过程中待拟合曲线的参数,对控制点的位置进行迭代调整,来对控制点的位置进行优化,具体的,将初始位置代入待拟合曲线的方程,得到初始位置对应的待拟合曲线,确定初始位置对应的待拟合曲线中的当前位置与避障位置之间的轨迹,在该轨迹中确定控制点与障碍物的距离、移动设备在控制点处的速度参量和迭代过程中待拟合曲线的参数,基于控制点与障碍物的距离、移动设备在控制点处的速度参量和迭代过程中待拟合曲线的参数,对控制点的位置进行调整,使得控制点与障碍物的距离、移动设备在控制点处的速度参量减小和迭代过程中待拟合曲线的参数减小,迭代执行上述将控制点的位置代入待拟合曲线的方程,得到控制点的位置对应的待拟合曲线,并对控制点的位置进行调整的操作,直至控制点与障碍物的距离、移动设备在控制点处的速度参量和迭代过程中待拟合曲线的参数达到最小值,即控制点与障碍物的距离、移动设备在控制点处的速度参量和迭代过程中待拟合曲线的参数收敛,结束迭代调整,得到优化后控制点的位置。所述迭代过程中待拟合曲线的参数包括所述移动设备在迭代过程中待拟合曲线上的速度、所移动设备在迭代过程中待拟合曲线上的加速度、迭代过程中待拟合曲线的曲率、迭代过程中待拟合曲线的起点角度和终点角度中的一个或多个,这样可以保证优化后的轨迹满足要求,提高安全性。
步骤104,根据优化得到的控制点的位置,确定所述当前位置到所述避障位置的局部目标路径。
在对控制点的位置进行优化后,将优化得到的控制点的位置代入待拟合曲线方程,得到待拟合曲线,并从该待拟合曲线中截取从当前位置到避障位置的曲线段,得到从当前位置到避障位置的局部目标路径。如待拟合曲线为B样本曲线时,将优化后的控制点的位置代入B样条曲线方程,可以得到B样条曲线,从B样条曲线中截取从当前位置到避障位置的曲线段,得到从当前位置到避障位置的局部目标路径。
其中,所述根据优化得到的控制点的位置,确定所述当前位置到所述避障位置的局部目标路径,包括:
根据优化得到的控制点的位置,按照如下B样条曲线确定所述当前位置到所述避障位置的局部目标路径:
其中,n+1为控制点的数量,Qi为第i个控制点的位置,p为B样条曲线的次数,Ni,p(t)为p次B样条曲线基函数。
本发明实施例中,可以采用三次B样条曲线来进行求解,即p为3。
本实施例提供的移动设备绕障方法,通过基于路径规划指令确定从当前位置到避障位置的初始路径,对初始路径进行采样,将路径采样点作为待拟合曲线的控制点的初始位置,基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行优化,根据优化得到的控制点的位置确定当前位置到避障位置的局部目标路径,由于通过对控制点的位置进行优化来获取更优的解,是在连续空间的求解,规划出的轨迹最优,而且将控制点的位置作为优化变量,相对于基于优化的方法,减少了优化参数,减少了求解耗时,从而可以在低成本的计算单元上满足实时性的需求。
在上述技术方案的基础上,在所述确定所述当前位置到所述避障位置的局部目标路径之后,还包括:
若移动设备在沿着所述局部目标路径行进时不会与障碍物发生碰撞,且所述局部目标路径满足角度约束条件和曲率约束条件,则输出所述局部目标路径;
否则,输出所述全局路径。
其中,角度约束条件是预先设置的起点角度和终点角度需满足的条件,曲率约束条件是预先设置的运动轨迹的曲率需满足的条件。
在确定当前位置到避障位置的局部目标路径后,对该局部目标路径进行校验,校验若移动设备沿着局部目标路径行进时是否会与障碍物发生碰撞,并且校验局部目标路径是否满足角度约束条件和曲率约束条件,若移动设备沿着局部目标路径行进时不会与障碍物发生碰撞,而且局部目标路径满足角度约束条件和曲率约束条件,则可以将局部目标路径输出至控制模块,由控制模块控制移动设备按照局部目标路径行进,以绕过障碍物行进。经过校验,若移动设备沿着局部目标路径行进时会与障碍物发生碰撞,或者局部目标路径不满足角度约束条件和曲率约束条件,则输出全局路径至控制模块,由控制模块控制机器人按照全局路径行进。通过路径校验,保证移动设备沿着较优的路径行进,可以保证移动设备的安全性。
在上述技术方案的基础上,所述方法还包括:
若在所述全局路径上行进时与障碍物的距离小于第四距离阈值,则停止行进。
在上述绕障轨迹规划失败或者距离障碍物较近无法进行路径重新规划时,移动设备需要在全局路径上行进,这时移动设备在行进的过程中,实时确定移动设备与障碍物的距离,如果距离小于第四距离阈值,即移动设备即将要与障碍物发生碰撞,则停止行进,可以避免与障碍物发生碰撞。
需要说明的是,对于方法实施例,为了简单描述,故将其都表述为一系列的动作组合,但是本领域技术人员应该知悉,本发明实施例并不受所描述的动作顺序的限制,因为依据本发明实施例,某些步骤可以采用其他顺序或者同时进行。其次,本领域技术人员也应该知悉,说明书中所描述的实施例均属于优选实施例,所涉及的动作并不一定是本发明实施例所必须的。
图3是本发明实施例提供的一种移动设备绕障装置的结构框图,如图3所示,该移动设备绕障装置可以包括:
初始路径确定模块301,用于基于路径规划指令,确定从当前位置到所述避障位置的初始路径,所述避障位置位于全局路径上;
初始路径采样模块302,用于对所述初始路径进行采样,并将路径采样点作为待拟合曲线的控制点的初始位置;
控制点优化模块303,用于基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行优化;
绕障路径确定模块304,用于根据优化得到的控制点的位置,确定所述当前位置到所述避障位置的局部目标路径。
可选的,所述控制点优化模块具体用于:
基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行迭代调整,直至控制点与障碍物的距离和移动设备在控制点处的速度参量收敛。
可选的,所述控制点优化模块包括:
安全性约束构建单元,用于基于所述初始位置和控制点与障碍物的距离,构建安全性约束函数;
平滑项约束构建单元,用于基于所述初始位置和移动设备在控制点处的速度参量,构建平滑项约束函数;
迭代调整单元,用于根据所述安全性约束函数、所述平滑项约束函数,对控制点的位置进行迭代调整,直至所述安全性约束函数和所述平滑项约束函数收敛。
可选的,所述控制点优化模块还包括:
速度约束构建单元,用于基于所述初始位置、移动设备在迭代过程中待拟合曲线上的速度以及移动设备在迭代过程中待拟合曲线上的加速度,构建速度约束函数;
所述迭代调整单元,还用于根据所述速度约束函数,对控制点的位置进行迭代调整,直至所述速度约束函数收敛。
可选的,所述控制点优化模块还包括:
曲率约束构建单元,用于基于所述初始位置和迭代过程中待拟合曲线的曲率,构建曲率约束函数;
角度约束构建单元,用于基于所述初始位置和迭代过程中待拟合曲线的起点角度和终点角度,构建角度约束函数;
所述迭代调整单元,还用于根据所述曲率约束函数、角度约束函数,对控制点的位置进行迭代调整,直至所述曲率约束函数、角度约束函数收敛。
可选的,所述控制点优化模块包括:
目标函数确定单元,用于对所述安全性约束函数、平滑项约束函数、速度约束函数、加速度约束函数、曲率约束函数和角度约束函数进行加权,确定所述目标函数;
迭代优化单元,用于基于所述目标函数,对控制点的位置进行迭代调整,直至所述目标函数收敛。
可选的,所述平滑项约束函数表示如下:
Fs(i)=((Qi+3-2Qi+2+Qi+1)-(Qi+2-2Qi+1+Qi))2
其中,Qi表示第i个控制点,Fs(i)表示在第i个控制点处的速度参量的平方和;
所述安全性约束函数表示如下:
其中,Fc(d(Qi))表示控制点与障碍物的距离的平方和,d(Qi)为第i个控制点与障碍物的距离,dthr表示发生碰撞时移动设备与障碍物的最小距离;
所述速度约束函数表示如下:
其中,Fv(vμ)表示速度约束函数,vμ表示迭代过程中待拟合曲线上的速度,vmax表示速度阈值;
所述加速度约束函数表示如下:
其中,Fa(aμ)表示加速度约束函数,aμ表示迭代过程中待拟合曲线上的加速度,amax表示加速度阈值。
可选的,所述控制点优化模块具体用于:
基于所述初始位置、控制点与障碍物的距离、移动设备在控制点处的速度参量和迭代过程中待拟合曲线的参数,对控制点的位置进行迭代调整,所述迭代过程中待拟合曲线的参数包括所述移动设备在迭代过程中待拟合曲线上的速度、所移动设备在迭代过程中待拟合曲线上的加速度、迭代过程中待拟合曲线的曲率、迭代过程中待拟合曲线的起点角度和终点角度中的一个或多个。
可选的,所述初始路径采样模块包括:
时间预估单元,用于计算在所述初始路径上行进所需的时间;
采样单元,用于按照指定时间间隔,对所述初始路径进行采样,得到所述初始路径上的路径采样点。
可选的,所述装置还包括:
重新规划判断模块,用于在到达路径规划周期时,若当前位置偏离所述全局路径的距离大于第一距离阈值,或者,上一路径规划周期的规划轨迹在行进方向上第一距离内有障碍物且距离障碍物的距离大于或等于第二距离阈值,或者,上一路径规划周期的规划轨迹在行进方向上所述第一距离内有障碍物且预测得到的临时终点与障碍物的距离大于或等于第三距离阈值,则确定接收到路径规划指令。
可选的,所述初始路径确定模块包括:
搜索区域确定单元,用于确定距离所述全局路径第二距离内的区域为搜索区域;
初始路径搜索单元,用于在所述搜索区域内,通过路径搜索算法搜索从所述当前位置到所述避障位置的初始路径。
可选的,所述装置还包括:
搜索失败确定模块,用于若所述搜索区域被障碍物完全遮挡,则在通过路径搜索算法搜索从所述当前位置到所述避障位置的初始路径时,确定搜索失败。
可选的,所述绕障路径确定模块具体用于:
根据优化得到的控制点的位置,按照如下B样条曲线确定所述当前位置到所述避障位置的局部目标路径:
其中,n+1为控制点的数量,Qi为第i个控制点的位置,p为B样条曲线的次数,Ni,p(t)为p次B样条曲线基函数。
可选的,所述装置还包括:
路径输出模块,用于若移动设备在沿着所述局部目标路径行进时不会与障碍物发生碰撞,且所述局部目标路径满足角度约束条件和曲率约束条件,则输出所述局部目标路径;否则,输出所述全局路径。
可选的,所述装置还包括:
停障模块,用于若在所述全局路径上行进时与障碍物的距离小于第四距离阈值,则停止行进。
本实施例提供的移动设备绕障装置,通过基于路径规划指令确定从当前位置到避障位置的初始路径,对初始路径进行采样,将路径采样点作为待拟合曲线的控制点的初始位置,基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行优化,根据优化得到的控制点的位置确定当前位置到避障位置的局部目标路径,由于通过优化控制点的位置来获取更优的解,是在连续空间的求解,规划出的轨迹最优,而且将控制点的位置作为优化变量,相对于基于优化的方法,减少了优化参数,减少了求解耗时,从而可以在低成本的计算单元上满足实时性的需求。
对于装置实施例而言,由于其与方法实施例基本相似,所以描述的比较简单,相关之处参见方法实施例的部分说明即可。
进一步地,根据本发明的一个实施例,提供了一种移动设备,所述移动设备包括:处理器、存储器及存储在所述存储器上并可在所述处理器上运行的计算机程序,所述计算机程序被所述处理器执行时实现前述实施例的移动设备绕障方法。
根据本发明的一个实施例,还提供了一种计算机可读存储介质,所述计算机可读存储介质包括但不限于磁盘存储器、CD-ROM、光学存储器等,所述计算机可读存储介质上存储有计算机程序,所述计算机程序被处理器执行时实现前述实施例的移动设备绕障方法。
本说明书中的各个实施例均采用递进的方式描述,每个实施例重点说明的都是与其他实施例的不同之处,各个实施例之间相同相似的部分互相参见即可。
本领域内的技术人员应明白,本发明实施例的实施例可提供为方法、装置、或计算机程序产品。因此,本发明实施例可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且,本发明实施例可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本发明实施例是参照根据本发明实施例的方法、终端设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理终端设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理终端设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理终端设备以特定方式工作的计算机可读存储器中,使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品,该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理终端设备上,使得在计算机或其他可编程终端设备上执行一系列操作步骤以产生计算机实现的处理,从而在计算机或其他可编程终端设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
尽管已描述了本发明实施例的优选实施例,但本领域内的技术人员一旦得知了基本创造性概念,则可对这些实施例做出另外的变更和修改。所以,所附权利要求意欲解释为包括优选实施例以及落入本发明实施例范围的所有变更和修改。
最后,还需要说明的是,在本文中,诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来,而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且,术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、方法、物品或者终端设备不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、方法、物品或者终端设备所固有的要素。在没有更多限制的情况下,由语句“包括一个……”限定的要素,并不排除在包括所述要素的过程、方法、物品或者终端设备中还存在另外的相同要素。
以上对本发明所提供的一种移动设备绕障方法、装置、移动设备及存储介质,进行了详细介绍,本文中应用了具体个例对本发明的原理及实施方式进行了阐述,以上实施例的说明只是用于帮助理解本发明的方法及其核心思想;同时,对于本领域的一般技术人员,依据本发明的思想,在具体实施方式及应用范围上均会有改变之处,综上所述,本说明书内容不应理解为对本发明的限制。
Claims (18)
1.一种移动设备绕障方法,其特征在于,包括:
基于路径规划指令,确定从当前位置到避障位置的初始路径,所述避障位置位于全局路径上;
对所述初始路径进行采样,并将路径采样点作为待拟合曲线的控制点的初始位置,其中,所述待拟合曲线是B样条曲线;
基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行优化,其中,所述速度参量是加加速度,表示加速度的变化率,将所述控制点的位置作为优化变量;
根据优化得到的控制点的位置,确定所述当前位置到所述避障位置的局部目标路径。
2.根据权利要求1所述的方法,其特征在于,基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行优化,包括:
基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行迭代调整,直至控制点与障碍物的距离和移动设备在控制点处的速度参量收敛。
3.根据权利要求2所述的方法,其特征在于,基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行迭代调整,直至控制点与障碍物的距离和移动设备在控制点处的速度参量收敛,包括:
基于所述初始位置和控制点与障碍物的距离,构建安全性约束函数;
基于所述初始位置和移动设备在控制点处的速度参量,构建平滑项约束函数;
根据所述安全性约束函数、所述平滑项约束函数,对控制点的位置进行迭代调整,直至所述安全性约束函数和所述平滑项约束函数收敛。
4.根据权利要求3所述的方法,其特征在于,所述基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行迭代调整,直至控制点与障碍物的距离和移动设备在控制点处的速度参量收敛,还包括:
基于所述初始位置、移动设备在迭代过程中待拟合曲线上的速度以及移动设备在迭代过程中待拟合曲线上的加速度,构建速度约束函数;
根据所述速度约束函数,对控制点的位置进行迭代调整,直至所述速度约束函数收敛。
5.根据权利要求4所述的方法,其特征在于,所述基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行迭代调整,直至控制点与障碍物的距离和移动设备在控制点处的速度参量收敛,还包括:
基于所述初始位置和迭代过程中待拟合曲线的曲率,构建曲率约束函数;
基于所述初始位置和迭代过程中待拟合曲线的起点角度和终点角度,构建角度约束函数;
根据所述曲率约束函数、角度约束函数,对控制点的位置进行迭代调整,直至所述曲率约束函数、角度约束函数收敛。
6.根据权利要求5所述的方法,其特征在于,基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行迭代调整,直至控制点与障碍物的距离和移动设备在控制点处的速度参量收敛,包括:
对所述安全性约束函数、平滑项约束函数、速度约束函数、加速度约束函数、曲率约束函数和角度约束函数进行加权,确定目标函数;
基于所述目标函数,对控制点的位置进行迭代调整,直至所述目标函数收敛。
7.根据权利要求6所述的方法,其特征在于,所述平滑项约束函数表示如下:
Fs(i)=(Qi+3-2Qi+2+i+1)-(Qi+2-2Qi+1+i))2
其中,Qi表示第i个控制点,Fs(i)表示在第i个控制点处的速度参量的平方和;
所述安全性约束函数表示如下:
其中,Fc(d(Qi))表示控制点与障碍物的距离的平方和,d(Qi)为第i个控制点与障碍物的距离,dthr表示发生碰撞时移动设备与障碍物的最小距离;
所述速度约束函数表示如下:
其中,Fv(vμ)表示速度约束函数,vμ表示迭代过程中待拟合曲线上的速度,vmax表示速度阈值;
所述加速度约束函数表示如下:
其中,Fa(aμ)表示加速度约束函数,aμ表示迭代过程中待拟合曲线上的加速度,amax表示加速度阈值。
8.根据权利要求1-7任一项所述的方法,其特征在于,基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行优化,包括:
基于所述初始位置、控制点与障碍物的距离、移动设备在控制点处的速度参量和迭代过程中待拟合曲线的参数,对控制点的位置进行迭代调整,所述迭代过程中待拟合曲线的参数包括所述移动设备在迭代过程中待拟合曲线上的速度、所移动设备在迭代过程中待拟合曲线上的加速度、迭代过程中待拟合曲线的曲率、迭代过程中待拟合曲线的起点角度和终点角度中的一个或多个。
9.根据权利要求1-8任一项所述的方法,其特征在于,对所述初始路径进行采样,包括:
计算在所述初始路径上行进所需的时间;
按照指定时间间隔,对所述初始路径进行采样,得到所述初始路径上的路径采样点。
10.根据权利要求1-8任一项所述的方法,其特征在于,还包括:
在到达路径规划周期时,若当前位置偏离所述全局路径的距离大于第一距离阈值,或者,上一路径规划周期的规划轨迹在行进方向上第一距离内有障碍物且距离障碍物的距离大于或等于第二距离阈值,或者,上一路径规划周期的规划轨迹在行进方向上所述第一距离内有障碍物且预测得到的临时终点与障碍物的距离大于或等于第三距离阈值,则确定接收到路径规划指令。
11.根据权利要求1-8任一项所述的方法,其特征在于,所述基于路径规划指令,确定从当前位置到所述避障位置的初始路径,包括:
确定距离所述全局路径第二距离内的区域为搜索区域;
在所述搜索区域内,通过路径搜索算法搜索从所述当前位置到所述避障位置的初始路径。
12.根据权利要求11所述的方法,其特征在于,还包括:
若所述搜索区域被障碍物完全遮挡,则在通过路径搜索算法搜索从所述当前位置到所述避障位置的初始路径时,确定搜索失败。
13.根据权利要求1-8任一项所述的方法,其特征在于,所述根据优化得到的控制点的位置,确定所述当前位置到所述避障位置的局部目标路径,包括:
根据优化得到的控制点的位置,按照如下B样条曲线确定所述当前位置到所述避障位置的局部目标路径:
其中,n+1为控制点的数量,Qi为第i个控制点的位置,p为B样条曲线的次数,Ni,(t)为p次B样条曲线基函数。
14.根据权利要求1-8任一项所述的方法,其特征在于,在所述确定所述当前位置到所述避障位置的局部目标路径之后,还包括:
若移动设备在沿着所述局部目标路径行进时不会与障碍物发生碰撞,且所述局部目标路径满足角度约束条件和曲率约束条件,则输出所述局部目标路径;
否则,输出所述全局路径。
15.根据权利要求1-8任一项所述的方法,其特征在于,还包括:
若在所述全局路径上行进时与障碍物的距离小于第四距离阈值,则停止行进。
16.一种移动设备绕障装置,其特征在于,包括:
初始路径确定模块,用于基于路径规划指令,确定从当前位置到避障位置的初始路径,所述避障位置位于全局路径上;
初始路径采样模块,用于对所述初始路径进行采样,并将路径采样点作为待拟合曲线的控制点的初始位置,其中,所述待拟合曲线是B样条曲线;
控制点优化模块,用于基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量,对控制点的位置进行优化,其中,所述速度参量是加加速度,表示加速度的变化率,将所述控制点的位置作为优化变量;
绕障路径确定模块,用于根据优化得到的控制点的位置,确定所述当前位置到所述避障位置的局部目标路径。
17.一种移动设备,其特征在于,包括:处理器、存储器及存储在所述存储器上并可在所述处理器上运行的计算机程序,所述计算机程序被所述处理器执行时实现如权利要求1-15任一项所述的移动设备绕障方法。
18.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质上存储有计算机程序,所述计算机程序被处理器执行时实现如权利要求1-15任一项所述的移动设备绕障方法。
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110616400.5A CN113433936B (zh) | 2021-06-02 | 2021-06-02 | 移动设备绕障方法、装置、移动设备及存储介质 |
PCT/CN2022/088575 WO2022252869A1 (zh) | 2021-06-02 | 2022-04-22 | 移动设备绕障方法、移动设备及存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110616400.5A CN113433936B (zh) | 2021-06-02 | 2021-06-02 | 移动设备绕障方法、装置、移动设备及存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113433936A CN113433936A (zh) | 2021-09-24 |
CN113433936B true CN113433936B (zh) | 2023-07-25 |
Family
ID=77803555
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110616400.5A Active CN113433936B (zh) | 2021-06-02 | 2021-06-02 | 移动设备绕障方法、装置、移动设备及存储介质 |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN113433936B (zh) |
WO (1) | WO2022252869A1 (zh) |
Families Citing this family (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113433936B (zh) * | 2021-06-02 | 2023-07-25 | 北京迈格威科技有限公司 | 移动设备绕障方法、装置、移动设备及存储介质 |
CN113868818B (zh) * | 2021-10-13 | 2024-04-02 | 桂林电子科技大学 | 一种区域数据点栅格化区块大小确定方法及系统 |
CN115268457A (zh) * | 2022-08-10 | 2022-11-01 | 美的集团(上海)有限公司 | 避障机器人及其控制方法、控制装置和可读存储介质 |
CN116858254A (zh) * | 2023-09-01 | 2023-10-10 | 青岛中德智能技术研究院 | 单舵轮agv路径规划方法 |
CN117472066B (zh) * | 2023-12-27 | 2024-03-26 | 成都流体动力创新中心 | 一种航向角速度局部最优的避障控制方法 |
CN118081743B (zh) * | 2024-02-29 | 2024-08-02 | 哈尔滨工业大学 | 一种考虑多维度轨迹样本的机械臂多约束轨迹规划方法 |
CN118464028B (zh) * | 2024-07-15 | 2024-09-06 | 龙门实验室 | 一种基于分治策略的农机路径规划方法 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109542106A (zh) * | 2019-01-04 | 2019-03-29 | 电子科技大学 | 一种移动机器人多约束条件下的路径规划方法 |
CN109799822A (zh) * | 2019-01-30 | 2019-05-24 | 中国石油大学(华东) | 移动机器人全局平滑路径规划方法 |
Family Cites Families (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR102552719B1 (ko) * | 2015-12-14 | 2023-07-06 | 현대모비스 주식회사 | 주행경로 자동생성방법 및 장치 |
CN106681335A (zh) * | 2017-01-22 | 2017-05-17 | 无锡卡尔曼导航技术有限公司 | 用于农机无人驾驶的避障路径规划及其控制方法 |
CN110388920A (zh) * | 2018-04-15 | 2019-10-29 | 张舒怡 | 机器人的路径规划 |
CN110347151B (zh) * | 2019-05-31 | 2022-07-12 | 河南科技大学 | 一种融合Bezier优化遗传算法的机器人路径规划方法 |
CN110488802B (zh) * | 2019-08-21 | 2020-05-12 | 清华大学 | 一种网联环境下的自动驾驶车辆动态行为决策方法 |
CN112506176B (zh) * | 2019-08-26 | 2024-05-28 | 上海汽车集团股份有限公司 | 路径规划方法及装置 |
CN110865642A (zh) * | 2019-11-06 | 2020-03-06 | 天津大学 | 一种基于移动机器人的路径规划方法 |
CN111694364A (zh) * | 2020-06-30 | 2020-09-22 | 山东交通学院 | 一种应用于智能车路径规划的基于改进蚁群算法与动态窗口法的混合算法 |
CN111679678B (zh) * | 2020-06-30 | 2022-04-08 | 安徽海博智能科技有限责任公司 | 一种横纵向分离的轨迹规划方法、系统及计算机设备 |
CN111813117B (zh) * | 2020-07-09 | 2023-09-01 | 北京布科思科技有限公司 | 一种机器人巡线优先导航方法、装置和设备 |
CN112352530B (zh) * | 2020-10-27 | 2022-05-31 | 懿力创新(厦门)科技有限公司 | 一种自动除草机器人的工作路径优化方法 |
CN112432648A (zh) * | 2020-11-17 | 2021-03-02 | 中山大学 | 一种移动型机器人安全运动轨迹的实时规划方法 |
CN112462785B (zh) * | 2020-12-04 | 2022-06-03 | 厦门大学 | 一种移动机器人路径规划方法、装置及存储介质 |
CN112857385B (zh) * | 2021-01-18 | 2022-09-27 | 北京理工大学 | 一种基于非均匀栅格模型的快速无人车局部路径规划方法 |
CN113433936B (zh) * | 2021-06-02 | 2023-07-25 | 北京迈格威科技有限公司 | 移动设备绕障方法、装置、移动设备及存储介质 |
-
2021
- 2021-06-02 CN CN202110616400.5A patent/CN113433936B/zh active Active
-
2022
- 2022-04-22 WO PCT/CN2022/088575 patent/WO2022252869A1/zh unknown
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109542106A (zh) * | 2019-01-04 | 2019-03-29 | 电子科技大学 | 一种移动机器人多约束条件下的路径规划方法 |
CN109799822A (zh) * | 2019-01-30 | 2019-05-24 | 中国石油大学(华东) | 移动机器人全局平滑路径规划方法 |
Also Published As
Publication number | Publication date |
---|---|
WO2022252869A1 (zh) | 2022-12-08 |
CN113433936A (zh) | 2021-09-24 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113433936B (zh) | 移动设备绕障方法、装置、移动设备及存储介质 | |
Stahl et al. | Multilayer graph-based trajectory planning for race vehicles in dynamic scenarios | |
Petrich et al. | Map-based long term motion prediction for vehicles in traffic environments | |
Brunner et al. | Repetitive learning model predictive control: An autonomous racing example | |
Gu et al. | Focused trajectory planning for autonomous on-road driving | |
CN110134140B (zh) | 一种环境信息未知连续状态下基于势函数奖赏dqn的无人机路径规划方法 | |
JP6634100B2 (ja) | 走行軌道決定装置及び自動運転装置 | |
JP2021526478A (ja) | 車両の制御システム、車両を制御する方法、及び非一時的コンピュータ可読メモリ | |
KR101585504B1 (ko) | 자율 이동 차량의 경로 생성 방법 및 경로 생성 장치 | |
CN105573323A (zh) | 自动驾驶轨迹生成方法及装置 | |
CN110244721A (zh) | 自动驾驶控制方法、装置、设备及存储介质 | |
CN111142542A (zh) | 一种基于vfh*局部路径规划方法的全向移动机器人自主导航系统 | |
CN115683145A (zh) | 一种基于轨迹预测的自动驾驶安全避障方法 | |
CN114771551A (zh) | 自动驾驶车辆轨迹规划方法、装置和自动驾驶车辆 | |
Butyrev et al. | Deep reinforcement learning for motion planning of mobile robots | |
CN113467476A (zh) | 考虑转角约束的无碰撞检测快速随机树全局路径规划方法 | |
JP7235060B2 (ja) | 経路計画装置、経路計画方法、及びプログラム | |
Lakhal et al. | Safe and adaptive autonomous navigation under uncertainty based on sequential waypoints and reachability analysis | |
Xiong et al. | Surrounding vehicle trajectory prediction and dynamic speed planning for autonomous vehicle in cut-in scenarios | |
US11614750B2 (en) | Self-propelled device, self-propelling method, and recording medium | |
CN116700288A (zh) | 一种基于自适应模型预测控制的移动机器人局部轨迹规划方法及系统 | |
CN115061470B (zh) | 适用狭窄空间的无人车改进teb导航方法 | |
Zhang | A hybrid reactive navigation strategy for a non-holonomic mobile robot in cluttered environments | |
CN113532457B (zh) | 机器人路径导航方法、系统、设备及存储介质 | |
Wang et al. | Safe Reinforcement Learning for Automated Vehicles via Online Reachability Analysis |
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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |