CN116048091B - 一种考虑位姿估计不确定性的机器人轨迹规划方法和装置 - Google Patents

一种考虑位姿估计不确定性的机器人轨迹规划方法和装置 Download PDF

Info

Publication number
CN116048091B
CN116048091B CN202310112220.2A CN202310112220A CN116048091B CN 116048091 B CN116048091 B CN 116048091B CN 202310112220 A CN202310112220 A CN 202310112220A CN 116048091 B CN116048091 B CN 116048091B
Authority
CN
China
Prior art keywords
robot
node
potential field
uncertainty
collision probability
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
Application number
CN202310112220.2A
Other languages
English (en)
Other versions
CN116048091A (zh
Inventor
孟杰
蒋立泉
李丽坤
胡航宇
肖扬
朱晓杏
蒋立宇
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Wuhan Suye Technology Co ltd
Original Assignee
Wuhan Suye Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Wuhan Suye Technology Co ltd filed Critical Wuhan Suye Technology Co ltd
Priority to CN202310112220.2A priority Critical patent/CN116048091B/zh
Publication of CN116048091A publication Critical patent/CN116048091A/zh
Application granted granted Critical
Publication of CN116048091B publication Critical patent/CN116048091B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本申请公开了一种考虑位姿估计不确定性的机器人轨迹规划方法和装置，包括：获取环境栅格地图、机器人的位姿数据和所述机器人的运动模型；其中，所述环境栅格地图包含动态障碍物信息；根据所述环境栅格地图和所述机器人的位姿数据，得到考虑位姿估计不确定性的所述机器人的碰撞概率和综合势场；根据碰撞概率和综合势场对RRT算法进行改进，得到改进的RRT算法；利用改进的RRT算法和所述机器人的运动模型，确定所述机器人的安全规划轨迹。本发明将机器人位姿估计不确定性在轨迹规划过程中加以考虑，为动态场景下的轨迹规划提供位姿估计不确定性约束，通过对RRT算法进行优化，提高了机器人轨迹在线规划的安全性和实时性。

Description

一种考虑位姿估计不确定性的机器人轨迹规划方法和装置
技术领域
本发明涉及移动机器人技术领域，具体涉及一种考虑位姿估计不确定性的机器人轨迹规划方法、装置和计算机可读存储介质。
背景技术
移动机器人具有在环境中改变位姿的能力，实际应用中，机器人应当沿着规划的轨迹高效、安全地运行。由于机器人多应用于复杂的场景，动态的障碍物和狭窄的运动空间都使得机器人的位姿估计会出现波动，轨迹跟踪时无法获得准确的参考，轨迹规划的实时性和安全性面临巨大的挑战。
目前，在动态环境中对自主机器人进行全局轨迹规划和避障的方法大多数都忽略了机器人位姿估计的不确定性。在实际情况中，位姿估计精度可能会受到动态环境的影响而急剧下降，如果轨迹规划方法忽略了位姿估计的不确定性，那么机器人将很容易偏离规划的轨迹，遇到危险的碰撞或需要耗费额外的时间对轨迹进行重新规划，无法满足轨迹规划的实时性和安全性要求。同时，现有技术通常采用快速扩展随机树(RRT)建立规划的模型约束，通过曲率滤波获得可行的轨迹。RRT方法虽然比较实时高效，但为了提高操作的安全性，大多方法会增加搜索范围以满足规划的约束，增加的搜索范围以及额外的不确定性约束无疑会降低操作的效率。
因此，需要提出一种考虑位姿估计不确定性的机器人轨迹规划方法和装置，解决现有技术中由于忽略了位姿估计的不确定性，从而导致在包含动态障碍物的场景下机器人沿规划轨迹移动时经常发生碰撞，且轨迹规划效率低，存在无法满足实际所需的安全性和实时性的问题。
发明内容
有鉴于此，有必要提供一种考虑位姿估计不确定性的机器人轨迹规划方法和装置，解决现有技术存在的由于忽略了位姿估计的不确定性，导致在包含动态障碍物的场景下，机器人沿规划轨迹移动时经常发生碰撞，轨迹规划无法满足实际所需的安全性和实时性的技术问题。
为了解决上述问题，本发明提供一种考虑位姿估计不确定性的机器人轨迹规划方法，包括：
获取环境栅格地图、机器人的位姿数据和所述机器人的运动模型；其中，所述环境栅格地图包含动态障碍物信息；
根据所述环境栅格地图和所述机器人的位姿数据，得到考虑位姿估计不确定性的所述机器人的碰撞概率和综合势场；
根据所述碰撞概率和综合势场对RRT算法进行改进，得到改进的RRT算法；
利用所述改进的RRT算法和所述机器人的运动模型，确定所述机器人的安全规划轨迹。
进一步的，根据所述环境栅格地图和所述机器人的位姿数据，得到考虑位姿估计不确定性的所述机器人的碰撞概率，包括：
根据所述机器人的位姿数据，基于贝叶斯优化算法得到所述机器人的位置估计概率分布图；
对所述位置估计概率分布图进行栅格划分，得到基于栅格划分的位置分布图；
将所述基于栅格划分的位置分布图投影到所述环境栅格地图中，得到考虑位姿估计不确定性的所述机器人的碰撞概率。
进一步的，根据所述环境栅格地图和所述机器人的位姿数据，得到考虑位姿估计不确定性的所述机器人的综合势场，包括：
根据所述环境栅格地图和所述机器人的位姿数据，确定障碍物产生的排斥力和位姿估计不确定性产生的排斥力；
根据所述障碍物产生的排斥力和位姿估计不确定产生的排斥力，得到所述机器人的斥力势场；
根据预设偏移量和所述机器人的位姿数据，确定所述机器人的合力势场；
根据所述机器人的斥力势场和合力势场，得到所述机器人的综合势场。
进一步的，根据所述碰撞概率和综合势场对RRT算法进行改进，得到改进的RRT算法，包括：
根据所述机器人的综合势场，调整RRT算法中各个扩展节点的权重，得到初始搜索空间；
根据所述碰撞概率和预设扩展规则，对RRT算法中的节点进行识别和过滤。
进一步的，根据所述碰撞概率和预设扩展规则，对RRT算法中的节点进行识别，包括：
对于任一节点，根据所述节点的碰撞概率、所述节点的扩展次数和当前已检测到的碰撞节点个数，确定所述节点的停止扩展概率；
当所述节点的停止扩展概率大于预设终止阈值时，确定所述节点为边界节点，并将所述边界节点设置为不可扩展节点。
进一步的，根据所述碰撞概率和预设扩展规则，对RRT算法中的节点进行过滤，包括：
对于随机抽样点所获取的新节点，若所述新节点与最近的节点之间的距离小于所述新节点与父节点之间的距离，则对所述新节点进行过滤。
进一步的，根据所述碰撞概率和预设扩展规则，对RRT算法中的节点进行过滤，还包括：
对于任一节点，以所述节点为圆心、预设长度阈值为半径构成的圆形区域内为不允许扩展节点的范围。
进一步的，所述机器人为四轮差动机器人；在所述机器人的运动模型中，机器人的状态方程表示为：
其中，χ(t)为t时刻机器人的状态变量，u(t)为控制输入变量。
本发明还提供一种考虑位姿估计不确定性的机器人轨迹规划装置，包括：
数据获取模块，用于获取环境栅格地图、机器人的位姿数据和所述机器人的运动模型；其中，所述环境栅格地图包含动态障碍物信息；
数据处理模块，用于根据所述环境栅格地图和所述机器人的位姿数据，得到考虑位姿估计不确定性的所述机器人的碰撞概率和综合势场；
算法优化模块，用于根据所述碰撞概率和综合势场对RRT算法进行改进，得到改进的RRT算法；
轨迹生成模块，用于利用改进的RRT算法和所述机器人的运动模型，确定所述机器人的安全规划轨迹。
本发明还提供一种计算机可读存储介质，所述计算机可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时，实现上述技术方案任一所述的一种考虑位姿估计不确定性的机器人轨迹规划方法。
与现有技术相比，本发明的有益效果包括：首先，获取环境栅格地图、机器人的位姿数据和机器人的运动模型；其次，根据栅格地图和机器人的位姿数据得到考虑机器人位姿估计不确定性的碰撞概率和综合势场；最后，根据所述碰撞概率和综合势场对RRT算法进行改进，并用改进的RRT算法和机器人的运动模型，确定机器人的安全规划轨迹。本发明的方法将位姿估计不确定性在轨迹规划过程中加以考虑，根据栅格地图和机器人的位姿数据得到了考虑机器人位姿估计不准确性的碰撞概率，为轨迹规划提供位姿估计不确定性约束；通过构建考虑位姿估计不确定性的综合势场，实现动态场景下的安全轨迹规划；为避免空间节点的过度搜索，根据碰撞概率和综合势场对RRT算法的优化，提高了轨迹在线规划的收敛速度和可靠性，能够提高机器人轨迹规划的安全性和实时性，有较高的实用价值。
附图说明
图1为本发明提供的一种考虑位姿估计不确定性的安全轨迹规划方法一实施例的流程示意图；
图2为本发明提供的四轮差速机器人的运动模型一实施例的示意图；
图3为本发明提供的环境栅格地图和基于栅格划分的位置分布图一实施例的示意图；
图4为本发明提供的边界节点识别一实施例的示意图；
图5为本发明提供的最近点过滤一实施例的示意图；
图6为本发明提供的预设范围节点过滤一实施例的示意图；
图7为本发明提供的一种考虑位姿估计不确定性的安全轨迹规划装置一实施例的结构示意图。
具体实施方式
下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理，并非用于限定本发明的范围。
在实施例描述之前，首先对本申请的相关术语进行解释。
RRT算法：Rapidly-Exploring Random Trees，快速搜索随机树法。RRT算法是一种基于采样的算法，从起始点进行扩展，在空间中通过随机函数进行采样得到随机点，利用节点扩展策略得到新节点，通过碰撞检测判断是否将新节点加入随机树中，经多次探索和扩展最终将目标点加入随机树中得到规划轨迹。
下面对本申请的发明构思进行说明。
现有技术中，自主机器人在动态环境中的轨迹规划方法大多数都是在假定可以忽略位姿估计不确定性的基础上进行开发的，例如：采用A*算法与自适应窗口法相结合的轨迹规划方法等等。但实际应用中，机器人位姿估计的准确性对安全导航至关重要，如果规划算法忽略了位姿估计的不准确性，那么机器人将很容易偏离规划的轨迹，导致遇到危险的碰撞或需要耗费更多时间对路线进行重新规划。此外，轨迹规划大多采用具有概率完备性的快速扩展随机树(RRT)方法，RRT方法本身虽然高效，但在实际应用中，为了提高操作的安全性，大多数方法会增加搜索范围，增加的搜索范围无疑会使计算效率降低。
本发明提出了一种考虑位姿估计不确定性的机器人轨迹规划方法和装置，在机器人轨迹规划中对位姿估计不确定性加以考虑，在考虑位姿估计不确定性的基础上得到了机器人的碰撞概率和综合势场，以实现在动态场景下的安全轨迹规划；此外，本发明还对RRT算法进行了优化，避免空间节点的过度搜寻，提高轨迹在线规划过程的收敛速度和可靠性。
本发明实施例提供了一种考虑位姿估计不确定性的机器人轨迹规划方法，如图1所示，图1是所述考虑位姿估计不确定性的安全轨迹规划方法的流程示意图，包括：
步骤S101：获取环境栅格地图、机器人的位姿数据和所述机器人的运动模型；其中，所述环境栅格地图包含动态障碍物信息；
步骤S102：根据所述环境栅格地图和所述机器人的位姿数据，得到考虑位姿估计不确定性的所述机器人的碰撞概率和综合势场；
步骤S103：根据所述碰撞概率和综合势场对RRT算法进行改进，得到改进的RRT算法；
步骤S104：利用所述改进的RRT算法和所述机器人的运动模型，确定所述机器人的安全规划轨迹。
本实施例提供的考虑位姿估计不确定性的安全轨迹规划方法，首先，获取环境栅格地图、机器人的位姿数据和机器人的运动模型；其次，根据栅格地图和机器人的位姿数据得到考虑机器人位姿估计不确定性的碰撞概率和综合势场；最后，根据所述碰撞概率和综合势场对RRT算法进行改进，并用改进的RRT算法和机器人的运动模型，确定机器人的安全规划轨迹。本实施例的方法将位姿估计不确定性在轨迹规划过程中加以考虑，根据栅格地图和机器人的位姿数据得到了考虑机器人位姿估计不准确性的碰撞概率，为轨迹规划提供位姿估计不确定性约束；通过构建考虑位姿估计不确定性的综合势场，实现动态场景下的安全轨迹规划；为避免空间节点的过度搜索，根据碰撞概率和综合势场，提高了轨迹在线规划的收敛速度和可靠性，能够提高机器人轨迹规划的安全性和实时性，有较高的实用价值。
作为优选的实施例，在步骤S101中，所述机器人为四轮差动机器人；在所述机器人的运动模型中，机器人的状态方程表示为：
其中，χ(t)为t时刻机器人的状态变量，u(t)为控制输入变量。
作为一个具体的实施例，四轮差动机器人在全局坐标系(OgXgYg)中的模型如图2所示，将四轮差动机器人的状态变量定义为χ(t)＝(x,y,θ)T，u(t)∈U是系统控制输入变量。
那么系统的状态方程可以写为：
在轨迹规划过程中，考虑到移动机器人的移动，通过迭代可以得到时间t+Δt的位置：
在运行过程中，任何转弯半径都可以通过调整车轮的速度来实现。因此，通过给定的控制变量和运行时间约束，可以计算出下一时间的新位置。
预设半径为R的运行空间，将S∈R3定义为整个运行空间，四轮差动机器人的可行区域为Sfree∈S，则运行空间的障碍区域为Sob∈S-Sfree。
为了保证规划的可行性，在轨迹规划时，轨迹的起点需要满足pinit∈Sfree，终点需要满足pgoal∈Sfree。
作为优选的实施例，在步骤S102中，根据所述环境栅格地图和所述机器人的位姿数据，得到考虑位姿估计不确定性的所述机器人的碰撞概率，包括：
根据所述机器人的位姿数据，基于贝叶斯优化算法得到所述机器人的位置估计概率分布图；
对所述位置估计概率分布图进行栅格划分，得到基于栅格划分的位置分布图；
将所述基于栅格划分的位置分布图投影到所述环境栅格地图中，得到考虑位姿估计不确定性的所述机器人的碰撞概率。
作为一个具体的实施例，如图3所示，图3下方的地图为所述环境栅格地图，若获取的机器人位姿数据为：当前机器人处于所述环境栅格地图的Pi处，则考虑到位姿估计不确定性，机器人在Pi处附近出现的概率可通过贝叶斯优化算法等算法，得到位姿估计不确定性的表示图，即：位置估计概率分布图。图3上方为对所述位置估计概率分布图进行栅格划分后的基于栅格划分的位置分布图，该分布图显示了静态环境中Pi处的位姿估计不确定性，黑色表示机器人出现的概率低，白色表示机器人出现的概率高。这里需要说明的是，在实际应用中，通常是用不同的颜色对栅格进行着色，用于区分机器人在该栅格出现的概率。
将基于栅格划分的位置分布图投影到环境栅格地图中，机器人在Pi处的碰撞概率Proi可表示为：
其中，表示机器人在/>处发生碰撞的概率；/>为位置分布图上机器人的位置；/>表示将位置分布图上的位置投影到环境栅格地图中，环境栅格地图中对应于位置分布图上/>的网格；Nrp,i为位置分布图上栅格的总数；TH为网格有障碍物的预设占用概率阈值。如果任一网格的占用概率低于阈值，碰撞就不会发生。反之亦然，存在的碰撞概率为/>
通过上述计算过程，可以得到所选轨迹点的定量位姿估计不确定性碰撞概率，从而对后续的轨迹规划提供支持。
在动态环境中，定位不确定将导致四轮差动机器人跟踪发生较大的偏移，甚至导致碰撞。因此，有必要通过构建在当前节点处估计的定位不确定性来构建规划势场，以确保四轮差动机器人的运行安全，在此基础上，结合RRT算法和获得的位姿估计不确定性来规划安全的运行轨迹。
作为优选的实施例，根据所述环境栅格地图和所述机器人的位姿数据，得到考虑位姿估计不确定性的所述机器人的综合势场，包括：
根据所述环境栅格地图和所述机器人的位姿数据，确定障碍物产生的排斥力和位姿估计不确定性产生的排斥力；
根据所述障碍物产生的排斥力和位姿估计不确定产生的排斥力，得到所述机器人的斥力势场；
根据预设偏移量和所述机器人的位姿数据，确定所述机器人的合力势场；
根据所述机器人的斥力势场和合力势场，得到所述机器人的综合势场。
作为一个具体的实施例，考虑当前节点的位置，为了避免RRT算法中节点增长的无序性导致自主机器人的轨迹规划陷入局部最小，通过增加偏移量，建立合力势场：
FT＝Fgoal+Foff (4)
其中，Fgoal表示在势场范围内获得的引力场；Foff表示机器人行驶过程中为避免引力势场落入局部最小值的偏移力。
Fgoal和Foff可表示为：
其中，kT是引力场系数；pcurrentpgoal当前点到目标点的距离矢量；Dgoal预设引力势场的范围；ε和koff分别是小偏移和偏移系数；(x0,y0)是原点坐标；θoffset是偏移角；ζ表示偏移的最大斜率；(xgoal,ygoal)是目标点的坐标。
为了保证自主机器人与障碍物之间的安全距离，防止在运行中发生碰撞，需要利用位姿估计的不确定性构建斥力势场。斥力势场的构建包括两部分：第一部分是原始地图中的障碍物和机器人感知模块观察到的动态障碍物；另一部分是由位姿估计不确定性产生的。
斥力势场FR的数学表达式为：
FR＝Fobs+Floc (7)
其中，Fobs表示障碍物产生的排斥力；Floc表示位姿估计不确定性产生的排斥力。
Fobs和Floc表示为：
其中，kR表示距离场系数；pobspcur指障碍物到当前点的方向系数；是障碍物与当前点的距离；Ds是器人的制动距离；/>是行驶方向与障碍物到当前点的角度；ξ指调整系数；func(t)是位姿估计不确定度带来的参数权重；kunc是不确定度系数；a1和a2是方向权重；Sobs是障碍物分布值；Proi是扩展节点i的位姿估计不确定度值；/>表示不确定点与当前点的距离；ψ是调整系数，Dp是位姿估计不确定度的势场距离。
综合上述合力势场和斥力势场，结合RRT的动态编程方法，则单个节点上的力可表示为：
FP＝FT+FR (10)
其中，FP为合力势场，FR为斥力势场。
考虑到具有位姿估计不确定性的势场构建，通过FP的值大小可以引导在障碍物区域附近均匀扩展的节点pfree，从而使扩展节点向远离障碍物的方向扩展。通过调整权重参数，加快RRT规划算法的收敛速度。
作为优选的实施例，根据所述碰撞概率和综合势场对RRT算法进行改进，得到改进的RRT算法，包括：
根据所述机器人的综合势场，调整RRT算法中各个扩展节点的权重，得到初始搜索空间；
根据所述碰撞概率和预设扩展规则，对RRT算法中的节点进行识别和过滤。
作为一个具体的实施例，利用RRT的概率完备性，可以实现整个空间的搜索，为了加快规划效率，保证规划过程的顺利进行，将搜索到的节点进行离线拟合，以减少搜索点集。
首先，在初始阶段利用建立的不确定势场函数，使用RRT获取初始搜索空间的树集合为后续规划提供数据，保证运行过程的安全性和稳定性。初始搜索树Tinit被作为规划空间样本。
其次，利用离线规划获取的从初始点到目标点的轨迹，对边界点集进行处理，提取初始规划阶段的可行点，减少碰撞检测时间，提高规划效率。注意到，在密闭空间边界节点将会导致频繁的碰撞检测，并且难以扩展，降低了扩展效率。因此，设置初始节点为边界节点概率设为PO。在统计过程中，初始值PO接近0，这意味着当前节点不是边界节点，并且有很高的扩展概率。
作为优选的实施例，根据所述碰撞概率和预设扩展规则，对RRT算法中的节点进行识别，包括：
对于任一节点，根据所述节点的碰撞概率、所述节点的扩展次数和当前已检测到的碰撞节点个数，确定所述节点的停止扩展概率；
当所述节点的停止扩展概率大于预设终止阈值时，确定所述节点为边界节点，并将所述边界节点设置为不可扩展节点。
作为一个具体的实施例，由公式(3)确定定位不确定性估计值Proi，并根据Proi计算当前节点N的概率pn，可以加速碰撞节点的计算速度。
假设节点N已经被扩展了m次，并且现有已检测到的碰撞节点有mo，则pn的值可以改写为(1-Proi)(mo+1)/(m+1)。
将节点扩展的终止阈值定义为Pm，当pn>Pm时，表示该节点为边界节点，可以将该节点设置为不可扩展节点。
进一步的，通过对空间中边界节点的统计，可以实现障碍物边界的拟合。
如图4所示，图4显示了边界节点的搜索方法。图4中，Obstacle表示障碍物，Pinit为初始节点，Pparent为父节点，Pnew生成的新节点，Pnode为已确认的可行节点，Ppath为规划轨迹，Ptarget为目标节点。可以看出，图4中的Pnew为位于障碍物区域，或父节点与新节点的连线会穿过障碍物，因此需要舍弃。通过对轨迹节点的离散规划和回溯，得到地图场景中的障碍点集，通过设置节点碰撞概率较小的节点进行扩展，可以使规划的轨迹离障碍物更远，便于实现安全规划。
作为优选的实施例，根据所述碰撞概率和预设扩展规则，对RRT算法中的节点进行过滤，包括：
对于随机抽样点所获取的新节点，若所述新节点与最近的节点之间的距离小于所述新节点与父节点之间的距离，则对所述新节点进行过滤。
在节点扩展过程中进行空间节点过滤，对同一区域的节点进行过滤，可以减少节点数量，实现轨迹的平滑和提高迭代速度。
作为一个具体的实施例，如图5所示，本实施例采用最近点过滤，即：如果由随机抽样点获取的新节点与最近的节点pnew之间的距离小于pnew与父节点pparent之间的距离，节点pnew可以被过滤，图5中，由于节点pfilter与最近的节点之间的距离小于pfilter与父节点pparent之间的距离，因此需要将节点pfilter进行舍弃。
作为优选的实施例，根据所述碰撞概率和预设扩展规则，对RRT算法中的节点进行过滤，还包括：
对于任一节点，以所述节点为圆心、预设长度阈值为半径构成的圆形区域内为不允许扩展节点的范围。
作为一个具体的实施例，如图6所示，进行距离过滤稀疏节点。在节点树生长过程中，每个节点附近rfilter内的范围里不允许生成新的节点，图6中，过滤节点pfilter落入以pnew为圆心，半径为rfilter的圆形范围内，因此需要进行舍去。这种节点过滤机制能有效防止空间的过度搜索，有效减少节点、降低空间的节点密度，特别是在接近目标点时，少量的节点可以加快规划速度。
从上述的分析可以看出，本实施例所使用的改进后的RRT算法，克服了传统RRT算法存在的搜索节点无序扩展、位置波动大，碰撞检测和最近节点的搜索时间长、效率低的问题。通过对边界节点进行识别、对最近点和稀疏节点进行过滤，减少了节点搜索量，从而避免了过度的空间搜索。同时，在规划过程中引入了势场法，通过位姿估计的不确定性动态调整标准阈值，实现了轨迹的安全规划。因此，本实施例的方法能够在保证安全的前提下，提高轨迹规划的速度。
本发明实施例还提供一种考虑位姿估计不确定性的机器人轨迹规划装置，如图7所示，所述考虑位姿估计不确定性的机器人轨迹规划装置700包括：
数据获取模块701，用于获取环境栅格地图、机器人的位姿数据和所述机器人的运动模型；其中，所述环境栅格地图包含动态障碍物信息；
数据处理模块702，用于根据所述环境栅格地图和所述机器人的位姿数据，得到考虑位姿估计不确定性的所述机器人的碰撞概率和综合势场；
算法优化模块703，用于根据所述碰撞概率和综合势场对RRT算法进行改进，得到改进的RRT算法；
轨迹生成模块704，用于利用改进的RRT算法和所述机器人的运动模型，确定所述机器人的安全规划轨迹。
本实施例还提供一种计算机可读存储介质，所述计算机可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时，实现上述技术方案任一所述的考虑位姿估计不确定性的机器人轨迹规划方法。
根据本发明上述实施例提供的计算机可读存储介质和计算设备，可以参照根据本发明实现如上所述的一种考虑位姿估计不确定性的机器人轨迹规划方法具体描述的内容实现，并具有与如上所述的一种考虑位姿估计不确定性的机器人轨迹规划方法类似的有益效果，在此不再赘述。
本发明公开的一种考虑位姿估计不确定性的机器人轨迹规划方法和装置，首先，获取环境栅格地图、机器人的位姿数据和机器人的运动模型；其次，根据栅格地图和机器人的位姿数据得到考虑机器人位姿估计不确定性的碰撞概率和综合势场；最后，根据所述碰撞概率和综合势场对RRT算法进行改进，并用改进的RRT算法和机器人的运动模型，确定机器人的安全规划轨迹。
本发明的方法将位姿估计不确定性在轨迹规划过程中加以考虑，根据栅格地图和机器人的位姿数据得到了考虑机器人位姿估计不准确性的碰撞概率，为轨迹规划提供位姿估计不确定性约束；通过构建考虑位姿估计不确定性的综合势场，实现动态场景下的安全轨迹规划；为避免空间节点的过度搜索，根据所述碰撞概率和综合势场对RRT算法进行了优化，提高了轨迹在线规划的收敛速度和可靠性，能够提高机器人轨迹规划的安全性和实时性，有较高的实用价值。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims (3)

1.一种考虑位姿估计不确定性的机器人轨迹规划方法，其特征在于，包括：
获取环境栅格地图、机器人的位姿数据和所述机器人的运动模型；其中，所述环境栅格地图包含动态障碍物信息；
根据所述环境栅格地图和所述机器人的位姿数据，得到考虑位姿估计不确定性的所述机器人的碰撞概率和综合势场；
根据所述碰撞概率和综合势场对RRT算法进行改进，得到改进的RRT算法；
利用所述改进的RRT算法和所述机器人的运动模型，确定所述机器人的安全规划轨迹；
根据所述环境栅格地图和所述机器人的位姿数据，得到考虑位姿估计不确定性的所述机器人的碰撞概率，包括：
根据所述机器人的位姿数据，基于贝叶斯优化算法得到所述机器人的位置估计概率分布图；
对所述位置估计概率分布图进行栅格划分，得到基于栅格划分的位置分布图；
将所述基于栅格划分的位置分布图投影到所述环境栅格地图中，得到考虑位姿估计不确定性的所述机器人的碰撞概率；
根据所述环境栅格地图和所述机器人的位姿数据，得到考虑位姿估计不确定性的所述机器人的综合势场，包括：
根据所述环境栅格地图和所述机器人的位姿数据，确定障碍物产生的排斥力和位姿估计不确定性产生的排斥力；
根据所述障碍物产生的排斥力和位姿估计不确定产生的排斥力，得到所述机器人的斥力势场；
根据预设偏移量和所述机器人的位姿数据，确定所述机器人的合力势场；
根据所述机器人的斥力势场和合力势场，得到所述机器人的综合势场；
根据所述碰撞概率和综合势场对RRT算法进行改进，得到改进的RRT算法，包括：
根据所述机器人的综合势场，调整RRT算法中各个扩展节点的权重，得到初始搜索空间；
根据所述碰撞概率和预设扩展规则，对RRT算法中的节点进行识别和过滤；
根据所述碰撞概率和预设扩展规则，对RRT算法中的节点进行识别，包括：
对于任一节点，根据所述节点的碰撞概率、所述节点的扩展次数和当前已检测到的碰撞节点个数，确定所述节点的停止扩展概率；
当所述节点的停止扩展概率大于预设终止阈值时，确定所述节点为边界节点，并将所述边界节点设置为不可扩展节点；
根据所述碰撞概率和预设扩展规则，对RRT算法中的节点进行过滤，包括：
对于随机抽样点所获取的新节点，若所述新节点与最近的节点之间的距离小于所述新节点与父节点之间的距离，则对所述新节点进行过滤；
根据所述碰撞概率和预设扩展规则，对RRT算法中的节点进行过滤，还包括：
对于任一节点，以所述节点为圆心、预设长度阈值为半径构成的圆形区域内为不允许扩展节点的范围；
所述机器人为四轮差动机器人；在所述机器人的运动模型中，机器人的状态方程表示为：
其中， 为/>时刻机器人的状态变量，/>为控制输入变量。
2.一种考虑位姿估计不确定性的机器人轨迹规划装置，其特征在于，包括：
数据获取模块，用于获取环境栅格地图、机器人的位姿数据和所述机器人的运动模型；其中，所述环境栅格地图包含动态障碍物信息；
数据处理模块，用于根据所述环境栅格地图和所述机器人的位姿数据，得到考虑位姿估计不确定性的所述机器人的碰撞概率和综合势场；
算法优化模块，用于根据所述碰撞概率和综合势场对RRT算法进行改进，得到改进的RRT算法；
轨迹生成模块，用于利用改进的RRT算法和所述机器人的运动模型，确定所述机器人的安全规划轨迹；
根据所述环境栅格地图和所述机器人的位姿数据，得到考虑位姿估计不确定性的所述机器人的碰撞概率，包括：
根据所述机器人的位姿数据，基于贝叶斯优化算法得到所述机器人的位置估计概率分布图；
对所述位置估计概率分布图进行栅格划分，得到基于栅格划分的位置分布图；
将所述基于栅格划分的位置分布图投影到所述环境栅格地图中，得到考虑位姿估计不确定性的所述机器人的碰撞概率；
根据所述环境栅格地图和所述机器人的位姿数据，得到考虑位姿估计不确定性的所述机器人的综合势场，包括：
根据所述环境栅格地图和所述机器人的位姿数据，确定障碍物产生的排斥力和位姿估计不确定性产生的排斥力；
根据所述障碍物产生的排斥力和位姿估计不确定产生的排斥力，得到所述机器人的斥力势场；
根据预设偏移量和所述机器人的位姿数据，确定所述机器人的合力势场；
根据所述机器人的斥力势场和合力势场，得到所述机器人的综合势场；
根据所述碰撞概率和综合势场对RRT算法进行改进，得到改进的RRT算法，包括：
根据所述机器人的综合势场，调整RRT算法中各个扩展节点的权重，得到初始搜索空间；
根据所述碰撞概率和预设扩展规则，对RRT算法中的节点进行识别和过滤；
根据所述碰撞概率和预设扩展规则，对RRT算法中的节点进行识别，包括：
对于任一节点，根据所述节点的碰撞概率、所述节点的扩展次数和当前已检测到的碰撞节点个数，确定所述节点的停止扩展概率；
当所述节点的停止扩展概率大于预设终止阈值时，确定所述节点为边界节点，并将所述边界节点设置为不可扩展节点；
根据所述碰撞概率和预设扩展规则，对RRT算法中的节点进行过滤，包括：
对于随机抽样点所获取的新节点，若所述新节点与最近的节点之间的距离小于所述新节点与父节点之间的距离，则对所述新节点进行过滤；
根据所述碰撞概率和预设扩展规则，对RRT算法中的节点进行过滤，还包括：
对于任一节点，以所述节点为圆心、预设长度阈值为半径构成的圆形区域内为不允许扩展节点的范围；
所述机器人为四轮差动机器人；在所述机器人的运动模型中，机器人的状态方程表示为：
其中， 为/>时刻机器人的状态变量，/>为控制输入变量。
3.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时，实现如权利要求1所述的一种考虑位姿估计不确定性的机器人轨迹规划方法。
CN202310112220.2A 2023-02-09 2023-02-09 一种考虑位姿估计不确定性的机器人轨迹规划方法和装置 Active CN116048091B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310112220.2A CN116048091B (zh) 2023-02-09 2023-02-09 一种考虑位姿估计不确定性的机器人轨迹规划方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310112220.2A CN116048091B (zh) 2023-02-09 2023-02-09 一种考虑位姿估计不确定性的机器人轨迹规划方法和装置

Publications (2)

Publication Number Publication Date
CN116048091A CN116048091A (zh) 2023-05-02
CN116048091B true CN116048091B (zh) 2023-09-15

Family

ID=86113294

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310112220.2A Active CN116048091B (zh) 2023-02-09 2023-02-09 一种考虑位姿估计不确定性的机器人轨迹规划方法和装置

Country Status (1)

Country Link
CN (1) CN116048091B (zh)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109634301A (zh) * 2018-12-05 2019-04-16 浙江大学 一种考虑视场角限制以及运动不确定性的结合记忆的旋翼飞行器高速飞行避障方法
CN109669481A (zh) * 2019-01-24 2019-04-23 中国人民解放军国防科技大学 基于等碰撞概率面法的航天器安全接近控制方法
CN110111880A (zh) * 2019-04-22 2019-08-09 北京航空航天大学 柔性针的基于障碍分级的人工势场路径规划方法及装置
CN113492882A (zh) * 2021-07-14 2021-10-12 清华大学 一种实现车辆控制的方法、装置、计算机存储介质及终端
CN114005280A (zh) * 2021-11-17 2022-02-01 同济大学 一种基于不确定性估计的车辆轨迹预测方法
CN114399526A (zh) * 2021-11-30 2022-04-26 江苏清微智能科技有限公司 位姿确定方法、装置以及存储介质
WO2023283186A1 (en) * 2021-07-06 2023-01-12 The Regents Of The University Of Michigan Two-wheeled, self-balancing robot

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101807484B1 (ko) * 2012-10-29 2017-12-11 한국전자통신연구원 객체 및 시스템 특성에 기반한 확률 분포 지도 작성 장치 및 그 방법

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109634301A (zh) * 2018-12-05 2019-04-16 浙江大学 一种考虑视场角限制以及运动不确定性的结合记忆的旋翼飞行器高速飞行避障方法
CN109669481A (zh) * 2019-01-24 2019-04-23 中国人民解放军国防科技大学 基于等碰撞概率面法的航天器安全接近控制方法
CN110111880A (zh) * 2019-04-22 2019-08-09 北京航空航天大学 柔性针的基于障碍分级的人工势场路径规划方法及装置
WO2023283186A1 (en) * 2021-07-06 2023-01-12 The Regents Of The University Of Michigan Two-wheeled, self-balancing robot
CN113492882A (zh) * 2021-07-14 2021-10-12 清华大学 一种实现车辆控制的方法、装置、计算机存储介质及终端
CN114005280A (zh) * 2021-11-17 2022-02-01 同济大学 一种基于不确定性估计的车辆轨迹预测方法
CN114399526A (zh) * 2021-11-30 2022-04-26 江苏清微智能科技有限公司 位姿确定方法、装置以及存储介质

Also Published As

Publication number Publication date
CN116048091A (zh) 2023-05-02

Similar Documents

Publication Publication Date Title
CN108698595B (zh) 用于控制车辆运动的方法和车辆的控制系统
Fulgenzi et al. Dynamic obstacle avoidance in uncertain environment combining PVOs and occupancy grid
Akametalu et al. Reachability-based safe learning with Gaussian processes
Min et al. RNN-based path prediction of obstacle vehicles with deep ensemble
Fulgenzi et al. Probabilistic navigation in dynamic environment using rapidly-exploring random trees and gaussian processes
EP3863904A2 (en) Autonomous vehicle planning
WO2017144350A1 (en) Method for motion planning for autonomous moving objects
Fulgenzi et al. Probabilistic motion planning among moving obstacles following typical motion patterns
Alzugaray et al. Short-term UAV path-planning with monocular-inertial SLAM in the loop
EP4046058A1 (en) Prediction and planning for mobile robots
Miller et al. Optimal planning for target localization and coverage using range sensing
Mohamed et al. Autonomous navigation of agvs in unknown cluttered environments: log-mppi control strategy
Akhtar et al. The interacting multiple model smooth variable structure filter for trajectory prediction
Choi et al. Hybrid map-based SLAM with Rao-Blackwellized particle filters
Xiong et al. Surrounding vehicle trajectory prediction and dynamic speed planning for autonomous vehicle in cut-in scenarios
Berntorp et al. Particle filtering for online motion planning with task specifications
Ortiz-Arroyo A hybrid 3D path planning method for UAVs
CN114326810A (zh) 一种无人机在复杂动态环境下的避障方法
Miura et al. Adaptive robot speed control by considering map and motion uncertainty
CN116048091B (zh) 一种考虑位姿估计不确定性的机器人轨迹规划方法和装置
Tay et al. The Bayesian occupation filter
US20230288886A1 (en) Model-Based Control with Uncertain Motion Model
Dekkata et al. Improved Model Predictive Control System Design and Implementation for Unmanned Ground Vehicles
Fulgenzi et al. Probabilistic rapidly-exploring random trees for autonomous navigation among moving obstacles
Neuman et al. Anytime policy planning in large dynamic environments with interactive uncertainty

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