CN113296514B - 一种基于稀疏带状结构的局部路径优化方法及系统 - Google Patents
一种基于稀疏带状结构的局部路径优化方法及系统 Download PDFInfo
- Publication number
- CN113296514B CN113296514B CN202110566018.8A CN202110566018A CN113296514B CN 113296514 B CN113296514 B CN 113296514B CN 202110566018 A CN202110566018 A CN 202110566018A CN 113296514 B CN113296514 B CN 113296514B
- Authority
- CN
- China
- Prior art keywords
- robot
- local
- grid map
- path
- obstacle avoidance
- 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 116
- 238000005457 optimization Methods 0.000 title claims abstract description 61
- 230000004888 barrier function Effects 0.000 claims abstract description 15
- 230000006870 function Effects 0.000 claims description 49
- 239000011159 matrix material Substances 0.000 claims description 27
- 238000005070 sampling Methods 0.000 claims description 16
- 238000004590 computer program Methods 0.000 claims description 9
- 238000009499 grossing Methods 0.000 claims description 6
- 238000010276 construction Methods 0.000 claims description 4
- 238000006073 displacement reaction Methods 0.000 claims description 4
- 238000009825 accumulation Methods 0.000 claims description 2
- 239000000126 substance Substances 0.000 claims description 2
- 230000009286 beneficial effect Effects 0.000 abstract description 3
- 230000033001 locomotion Effects 0.000 description 22
- 238000004422 calculation algorithm Methods 0.000 description 13
- 238000004364 calculation method Methods 0.000 description 9
- 230000008569 process Effects 0.000 description 8
- 238000002474 experimental method Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 6
- 238000011156 evaluation Methods 0.000 description 5
- 238000012360 testing method Methods 0.000 description 5
- 238000004088 simulation Methods 0.000 description 4
- 230000003068 static effect Effects 0.000 description 4
- 238000012795 verification Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 2
- 238000000354 decomposition reaction Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000009826 distribution Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000008030 elimination Effects 0.000 description 2
- 238000003379 elimination reaction Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 230000036961 partial effect Effects 0.000 description 2
- 235000006140 Raphanus sativus var sativus Nutrition 0.000 description 1
- 240000001970 Raphanus sativus var. sativus Species 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000006835 compression Effects 0.000 description 1
- 238000007906 compression Methods 0.000 description 1
- 238000002939 conjugate gradient method Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000012216 screening Methods 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000036962 time dependent Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000000844 transformation Methods 0.000 description 1
- 238000009827 uniform distribution Methods 0.000 description 1
Images
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/0219—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory ensuring the processing of the whole working surface
-
- 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)
- Manipulator (AREA)
Abstract
本发明公开了一种基于稀疏带状结构的机器人局部路径优化方法及系统，所述方法包括：获取室内环境数据，并构建占据栅格地图；将占据栅格地图转换为欧式距离栅格地图，根据欧氏距离地图插值根据欧氏距离地图插值得到机器人与最近障碍物的距离函数，结合给定机器人到障碍物的安全距离，得到避障约束条件；在以机器人当前位置为中心的局部区域内，以局部路径的平滑性和避障为约束条件，求解局部最优路径。本发明通过将占据栅格地图转换为欧式距离栅格地图，根据欧氏距离栅格地图插值得到机器人到最近障碍物的连续可微的距离函数，克服了占据栅格地图离散化问题，并天然地计算得到梯度信息从而构建避障约束条件，有利于局部路径优化准确性的提高。
Description
技术领域
本发明属于机器人自主导航技术领域，尤其涉及一种基于稀疏带状结 构的局部路径优化方法及系统。
背景技术
本部分的陈述仅仅是提供了与本公开相关的背景技术信息，不必然构 成在先技术。
随着科学技术的发展，移动机器人在家庭服务、仓储物流、救灾救援 等方面得到了广泛的应用。运动规划是移动机器人实现自主导航的关键技 术之一，得到了学者们的广泛关注和研究。出于计算效率的考虑，通常将 移动机器人运动规划问题分解为全局规划和局部规划来处理，前者基于已 知或部分未知的先验地图为机器人规划一条粗略的全局路径，后者根据实 时的传感信息为机器人提供安全、灵活、高效的运动命令。在实际导航应用中，局部规划对机器人的运动质量起主导作用。
移动机器人局部规划方法可分为基于采样的方法和基于数值优化的方 法。基于采样的局部规划通常根据机器人当前状态生成一组候选的轨迹， 然后根据评价函数筛选出一条最优的轨迹。评价函数通常会综合考虑和全 局规划结果的偏差、到障碍物的距离等因素。根据采样空间的不同，又可 将基于采样的局部规划方法分为基于速度空间采样的方法和基于状态空间 采样的方法。基于速度空间采样的典型代表有动态窗口法(DynamicWindow Approach，DWA)和速度避障法(Velocity Obstacle，VO)，这类方法能够 保证轨迹的可行性。2008年，美国卡内基梅隆大学的Howard等人研究发 现，相比于基于速度空间采样的方法，基于状态空间采样的方法具有轨迹 分布均匀、对初始参数不敏感等优点。基于状态空间采样的典型方法包括 模型预测控制法(Model Predictive Control，MPC)和几何轨线法。模型预 测控制法由Howard等人提出，给定初始状态、目标状态和预测控制模型，结合牛顿迭代法和有限差分法求解控制输入，得到分布均匀的候选路径。 然而，模型预测控制法在规划过程中并没有考虑到环境障碍物信息，因此 会耗费大量时间用于生成不可行轨迹。几何轨线法是解耦的规划方法，首 先利用几何轨线规划出可行的参考路径，然后沿给定路径进行速度规划， 计算复杂度低。中国科学院沈阳自动化研究所的何玉庆研究员团队提出基 于四阶贝塞尔曲线的局部路径生成方法，根据始末状态约束对贝塞尔曲线 进行参数化，把局部路径构造问题转化为约束下的参数最优化问题，保证 路径曲率连续有界。为降低在线生成局部路径的计算负担，南开大学张雪 波教授团队提出执行层路径规划方法，离线对机器人初始曲率和目标状态 进行密集采样，然后根据采样的初始曲率和目标状态利用文献中的方法生 成五阶贝塞尔曲线，并存储在查找表中。在线规划的时候，根据机器人当 前曲率从查找表中检索出相应的一簇候选路径，再根据评价函数对候选路 径进行评价和筛选。这种离线生成路径、在线评价的局部路径规划方法计 算效率非常高，但离线生成的局部路径的终点是离散固定的，因此规划的 灵活性受限。此外，所有基于采样的局部规划方法本质上都受制于评价函 数的设计和各项权重的设置。
基于数值优化的局部规划方法通常综合考虑局部路径的安全性、平滑 性、最优性和机器人自身的运动学、动力学模型等约束，将局部规划问题 建模为最优化问题，通过迭代计算优化目标的局部极小值来获得局部最优 的路径或轨迹。典型的基于数值优化的局部规划方法有
等人提出 的TEB(Timed Elastic Band)。TEB将轨迹规划问题建模为非线性最小二 乘问题，将避障、非完整约束等作为惩罚项加入优化目标中。通过调整路径点和路径点间的时间间隔，来得到安全、高效的局部轨迹。此外，由于 优化变量中考虑了时间分量，使得速度、加速度等和时间相关的约束也可 以天然地包含在优化目标中。然而，这种同时考虑避障、运动学和动力学 约束的耦合式轨迹优化问题，存在着如下两大局限性：
第一，软约束框架下，速度、加速度约束未必能满足；第二，约束之 间如时间最优和避障是相互冲突的，导致运动效率、安全性等在一次优化 过程中难以取得较好的效果。因此，一些学者也提出了解耦式的局部规划， 先优化局部路径，再沿着给定路径进行速度规划，典型方法为Dolgov等人 提出的Hybrid A*。该方法先用图搜索规划出一条粗糙的全局路径，然后考 虑避障、最小转弯半径、平滑性等约束，采用共轭梯度法对局部路径进行平滑。然而，Hybrid A*中简单粗暴地采用K-D树来关联机器人和最近障碍 物，计算负荷较大。此外，Hybrid A*并没有进一步探讨路径优化问题本身 的一些数学特性，没能充分高效地求解优化问题。
总结文献可以得知，现有的针对部分未知、动态变化的非结构化室内 环境下的移动机器人局部规划方法仍然存在诸多缺陷，影响移动机器人的 实际导航应用。
发明内容
为克服上述现有技术的不足，本发明提供了一种基于稀疏带状结构的 机器人局部路径优化方法及系统，结合欧式距离栅格地图，克服了栅格地 图离散化问题，同时便于构建避障约束条件，保障了优化后局部路径的实 时性，机器人能够灵活避障。
为实现上述目的，本发明的一个或多个实施例提供了如下技术方案：
一种基于稀疏带状结构的机器人局部路径优化方法，包括以下步骤：
获取室内环境数据，并构建占据栅格地图；
将占据栅格地图转换为欧式距离栅格地图，根据欧氏距离地图插值得 到机器人与最近障碍物的距离函数，结合给定机器人到障碍物的安全距离， 得到避障约束条件；
在以机器人当前位置为中心的局部区域内，以局部路径的平滑性和避 障为约束条件，求解局部最优路径。
进一步地，将占据栅格地图转换为欧式距离栅格地图具体为：
将被障碍物占据的栅格值记为0，计算每个栅格的中心到最近障碍物栅 格中心的欧式距离，得到欧式距离栅格地图。
进一步地，所述避障约束条件采用避障项代价函数表示：
其中，ds为给定机器人到障碍物的安全距离，
为机器人与最近障 碍物的距离函数的梯度，xi＝(xi,yi)T表示机器人位置的世界坐标。
进一步地，局部路径的平滑性约束条件采用平滑项函数表示：
其中，Δxi＝xi-xi-1,2≤i≤N表示xi-1指向xi的位移矢量，
xi＝(xi,yi)T,1≤i≤N是以机器人当前位置为起点，以一定的间隔在局部区域内 的全局路径上进行采样，得到的N个全局坐标系下表示的路径点，其中， 全局路径是假设已经通过全局路径规划得到的一条无碰撞的路径。
进一步地，求解局部最优路径具体为：求解以平滑项函数和避障项代 价函数加权组合构建的目标函数。
进一步地，基于LM法对目标函数进行求解，得到平滑项函数和避障 项代价函数对应的海塞矩阵，累加得到稀疏带状的海塞矩阵，结合追赶法 求解得到局部最优路径。
一个或多个实施例提供了一种基于稀疏带状结构的机器人局部路径优 化系统，包括：
占据栅格地图构建模块，被配置为：获取室内环境数据，并构建占据 栅格地图；
欧氏距离栅格地图构建模块，被配置为：将占据栅格地图转换为欧式 距离栅格地图；
避障约束条件求解模块，被配置为：根据欧氏距离地图插值得到机器 人与最近障碍物的距离函数，结合给定机器人到障碍物的安全距离，得到 避障约束条件；
局部路径优化模块，被配置为：在以机器人当前位置为中心的局部区 域内，以局部路径的平滑性和避障为约束条件，求解局部最优路径。
一个或多个实施例提供了一种电子设备，包括存储器、处理器及存储 在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序 时实现所述的局部路径优化方法。
一个或多个实施例提供了一种计算机可读存储介质，其上存储有计算 机程序，该程序被处理器执行时实现所述的局部路径优化方法。
一个或多个实施例提供了一种机器人，其上包括存储器、处理器及存 储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理 器执行所述程序时实现所述的局部路径优化方法。
以上一个或多个技术方案存在以下有益效果：
以上一个或多个技术方案提出的局部路径优化方法，将占据栅格地图 转换为欧式距离栅格地图，然后根据欧氏距离栅格地图，基于双线性插值 法高效地计算了机器人到最近障碍物的连续可微的距离函数，克服了占据 栅格地图离散化问题，并天然地计算得到梯度信息从而构建避障约束条件， 有利于局部路径优化准确性的提高；
通过以局部路径平滑和避障为约束条件构建多项加权目标函数，充分 挖掘了所设计优化问题背后的稀疏带状结构，结合LM算法和追赶法进行 求解，相比于传统的基于稀疏矩阵分解的方法，所提出的方法不需要对稀 疏矩阵进行压缩存储操作，效率更高，提高了机器人局部路径优化过程的 实时性；
实验证明，基于上述方法得到的局部路径能够顺利地通过狭窄通道、 避开动态障碍物，以及穿过人群。
附图说明
构成本发明的一部分的说明书附图用来提供对本发明的进一步理解， 本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不 当限定。
图1为本发明一个或多个实施例中一种基于稀疏带状结构的机器人局 部路径优化方法流程图；
图2为双线性插值法示意图；
图3为整个优化问题对应的海塞矩阵的构造过程示意图，其中，上半 部分对应平滑项，下半部分对应避障项；
图4为在Intel实验室数据集上采用Karto SLAM构建的占据栅格地图；
图5(a)-图5(c)展示预先用A*算法在Intel实验室地图中规划出来的3 条全局路径；
图6(a)和图6(b)为图5(a)红圈区域对应的部分三维模型；
图7(a)和图7(b)分别展示文献1和本发明一个或多个实施例中局 部规划算法在Intel仿真场景中的规划结果；
图8为实验平台示意图；
图9(a)和图9(b)为室内实验场景示意图；
图10(a)-图10(b)分别展示文献1和本发明一个或多个实施例中局 部规划算法在实验室真实场景中的规划结果；
图11为室内导航实验的大致路线示意图。其中蓝色矩形表示箱子，箭 头表示行人运动的方向；
图12(a)-图12(d)为机器人避障示意图，其中图12(a)和图12(c) 示意机器人避开静态障碍物，图12(b)和图12(d)示意机器人避开动态障 碍物；
图13(a)-图13(d)为机器人穿过静态人群的过程示意图。
具体实施方式
应该指出，以下详细说明都是示例性的，旨在对本发明提供进一步的 说明。除非另有指明，本文使用的所有技术和科学术语具有与本发明所属 技术领域的普通技术人员通常理解的相同含义。
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非 意图限制根据本发明的示例性实施方式。如在这里所使用的，除非上下文 另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的 是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步 骤、操作、器件、组件和/或它们的组合。
在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组 合。
实施例一
本实施例公开了一种基于稀疏带状结构的机器人局部路径优化方法， 如图1所示，包括以下步骤：
步骤1：获取室内环境数据，并构建占据栅格地图；
所述室内环境数据可通过激光扫描仪器、图像获取装置等现有方式获 取，在此不做限定。获取室内环境数据后，首先进行室内障碍物识别和提 取，然后基于障碍物位置构建占据栅格地图。
步骤2：将占据栅格地图转换为欧式距离栅格地图，根据欧氏距离地图 插值得到机器人与最近障碍物的距离函数，结合给定机器人到障碍物的安 全距离，得到避障约束条件；
通常，采用SLAM(Simultaneous Localization and Mapping，同步定位 与地图构建)技术得到的点云地图或者占据栅格地图是难以直接应用于导 航规划任务的，因为这样的尺度地图未能刻画构形空间(Configuration space，C-space)到环境障碍物的距离关系。因此，本实施例进行了距离变 换，将占据栅格地图转换为欧式距离栅格地图。
欧式距离栅格地图中的每个栅格记录当前栅格中心到最近障碍物栅格 中心的欧式距离，被障碍物占据的栅格的值即为0。将欧式距离栅格地图记 为离散函数fd(P)，其中自变量P表示栅格中心的世界坐标。为近似计算连 续空间下机器人到最近障碍物的距离函数，本实施例采用双线性插值法对 欧式距离栅格地图进行插值，从而既克服了栅格地图的离散化问题，又能 够天然地计算梯度信息。
具体地，如图2所示，Q＝(xi,yi)T表示机器人位置的世界坐标， P00＝(x0,y0)T，P10＝(x1,y0)T，P01＝(x0,y1)T和P11＝(x1,y1)T表示到机器人位置最近 的四个栅格中心的世界坐标。根据双线性插值计算公式可得机器人到最近 障碍物的距离函数：
以及距离函数的梯度
最后，给定机器人到障碍物的安全距离ds，计算避障项代价函数如下
步骤3：在以机器人当前位置为中心的局部区域内，以局部路径的平滑 性和避障约束为条件，求解局部最优路径。
具体地，所述步骤3包括：
假设已经通过全局路径规划算法得到一条无碰撞的全局路径，根据机 器人的传感范围以及计算实时性需求，以机器人当前位置为中心确定一个 矩形的局部窗口，然后以机器人当前位置为起点，以一定的间隔在局部窗 口内的全局路径上进行采样，得到N个全局坐标系下表示的路径点
其中xi＝(xi,yi)T,1≤i≤N。在此基础上，综合考虑了局部路 径平滑性和避障等约束，定义了如式(4)所示的多项加权的优化目标函数：
其中Δxi＝xi-xi-1,2≤i≤N表示xi-1指向xi的位移矢量，fo(xi)为避障代价 函数，ωs和ωo分别表示平滑项和避障项对应的权重。通过最小化这一目标 函数得到局部最优解x*
所述优化目标函数的求解方法包括：
(1)线性化
针对式(5)这样一个典型的非线性最小二乘问题，本实施例采用LM算 法进行求解。为方便后续的推导，令
将式(6)代入式(4)，得
先考察平滑项子函数Si-1,i,i+1(x)。假设现在有一个较好的初值
在
附 近对Si-1,i,i+1(x)泰勒展开并保留其线性部分，得
其中
为Si-1,i,i+1(x)在
处的雅可比矩阵。则
考虑所有的平滑项子函数，可得
同理可得避障项
其中
为Oi(x)在
处的雅可比矩阵。则整个目标函数
其中，
目标是最小化
即求
的极小值。因此，直接对
求导，得
HΔx＝-b. (15)
整个求解过程就是计算式(15)得到增量Δx，并将增量叠加到本轮迭代的 初值
直至收敛
而LM算法在式(15)的基础上通过引入松弛因子λ来控制收敛速度
(H+λI)Δx＝-b. (17)
注意到，平滑项和避障项中的每一个子函数均仅和局部若干个连续变 量相关。因此，在计算子函数对应的雅可比矩阵的时候，与这些变量无关 的偏导数全为0。以平滑项子函数为例
Si-1,i,i+1(x)＝Δxi+1-Δxi＝xi+1-2xi+xi-1, (18)
其在迭代初值
处的雅可比矩阵为
海塞矩阵为
即单独一平滑项子函数Si-1,i,i+1(x)对应的海塞矩阵
仅为整个优化 问题的海塞矩阵H贡献一个6×6的对角块。因此，如图3所示，把优化目标 中的所有子函数对应的海塞矩阵累加在一起，最终得到的整个优化问题的 海塞矩阵H是一个维数为2N×2N、带宽为5的稀疏带状矩阵。
(2)追赶法求解
为方便后续说明，将式(17)记为
Ax＝b, (21)
其中A＝H+λI。根据LM算法可得矩阵A为对称正定(symmetric positive-definite)阵。又因为矩阵A为带状矩阵，因此可用LU分解将矩阵A 分解为一单位下三角矩阵L和一上三角矩阵U的乘积
A＝LU. (22)
将式(22)代入式(21)，得
Ax＝LUx＝b. (23)
因为L为单位下三角矩阵，U为上三角矩阵，可用追赶法求解式(23)。
第一阶段，将Ux视为整体
y＝Ux, (24)
通过前向消元计算得到y
Ly＝b. (25)
在此基础上，第二阶段采用回代计算得到x
Ux＝y. (26)
追赶法本质上也是高斯消元法，只是利用了稀疏带状矩阵非零元素分 布的特点，达到提高运算速度的目的。此外，追赶法的计算效率通常比常 规的稀疏矩阵分解来得快，因为它避免了稀疏矩阵分解的压缩存储操作。
实施例二
基于上述局部路径优化方法，本实施例提供了一种基于稀疏带状结构 的机器人局部路径优化系统，包括：
占据栅格地图构建模块，被配置为：获取室内环境数据，并构建占据 栅格地图；
欧氏距离栅格地图构建模块，被配置为：将占据栅格地图转换为欧式 距离栅格地图；
避障约束条件求解模块，被配置为：根据欧氏距离地图插值得到机器 人与最近障碍物的距离函数，结合给定机器人到障碍物的安全距离，得到 避障约束条件；
局部路径优化模块，被配置为：在以机器人当前位置为中心的局部区 域内，以局部路径的平滑性和避障为约束条件，求解局部最优路径。
实施例三
基于上述局部路径优化方法，本实施例提供了一种电子设备。所述设 备包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程 序，所述处理器执行所述程序时实现以下步骤：
获取室内环境数据，并构建占据栅格地图；
将占据栅格地图转换为欧式距离栅格地图，根据欧氏距离地图插值得 到机器人与最近障碍物的距离函数，结合给定机器人到障碍物的安全距离， 得到避障约束条件；
在以机器人当前位置为中心的局部区域内，以局部路径的平滑性和避 障为约束条件，求解局部最优路径。
实施例四
本实施例的目的是提供一种计算机可读存储介质。其上存储有计算机 程序，该程序被处理器执行时执行以下步骤：
获取室内环境数据，并构建占据栅格地图；
将占据栅格地图转换为欧式距离栅格地图，根据欧氏距离地图插值得 到机器人与最近障碍物的距离函数，结合给定机器人到障碍物的安全距离， 得到避障约束条件；
在以机器人当前位置为中心的局部区域内，以局部路径的平滑性和避 障为约束条件，求解局部最优路径。
实施例五
本实施例的目的是提供一种机器人。包括存储器、处理器及存储在存 储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行 所述程序时实现以下步骤：
获取室内环境数据，并构建占据栅格地图；
将占据栅格地图转换为欧式距离栅格地图，根据欧氏距离地图插值得 到机器人与最近障碍物的距离函数，结合给定机器人到障碍物的安全距离， 得到避障约束条件；
在以机器人当前位置为中心的局部区域内，以局部路径的平滑性和避 障为约束条件，求解局部最优路径。
以上实施例二至五中涉及的各步骤与方法实施例一相对应，具体实施 方式可参见实施例一的相关说明部分。术语“计算机可读存储介质”应该理解 为包括一个或多个指令集的单个介质或多个介质；还应当被理解为包括任 何介质，所述任何介质能够存储、编码或承载用于由处理器执行的指令集 并使处理器执行本发明中的任一方法。
本领域技术人员应该明白，上述本发明的各模块或各步骤可以用通用 的计算机装置来实现，可选地，它们可以用计算装置可执行的程序代码来 实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它 们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成 单个集成电路模块来实现。本发明不限制于任何特定的硬件和软件的结合。 实验验证：
为验证上述基于稀疏带状结构的局部路径优化方法，我们分别在虚拟 仿真环境和实验室实际环境中开展验证工作，并和文献1基于五阶贝塞尔 曲线的状态空间采样的局部路径规划方法(Zhang X,Wang J,Fang Y,et al. Multilevel humanlike motionplanning for mobile robots in complex indoor environments[J].IEEETransactions on Automation Science and Engineering, 2019,16(3):1244-1258)进行对比。为保证对比的公平性，全局规划均采用 文献2(Likhachev M,Ferguson D.Planninglong dynamically feasible maneuvers for autonomous vehicles[J].TheInternational Journal of Robotics Research,2009,28(8):933-945)中的基于运动基元的图搜索算法，速度规划 采用文献1中的沿给定路径的速度可调且完备的轨迹规划算法，控制器采 用文献3(Siciliano B,Sciavicco L,Villani L,et al.Robotics:modelling, planning and control[M].Springer Science&Business Media,2010)中的轨迹 跟踪控制算法。所有的算法均采用C/C++编程实现，并在Ubuntu 16.04操 作系统中运行。
仿真对比结果
为了让仿真更加真实，我们先通过真实数据集构建二维栅格地图，在 此基础上搭建仿真环境。如图4所示，我们从Radish数据仓库中获取经典 的SLAM数据集Intel实验室数据集，并采用开源的二维激光SLAM算法Karto SLAM构建占据栅格地图。环境的尺寸大小约为30m×30m。
(1)灵活性对比
为对局部路径规划方法的灵活性进行对比，我们让机器人沿着如附图 5(a)所示的全局路径运动。这是一个非常有挑战性的场景。第一，如图6(a) 所示机器人需要在拐角处转一个U型弯，这个过程需要局部规划方法提供 灵活的运动指引；第二，在经过拐角后机器人需要经过狭窄的折线型通道 才能到达目标点，这一过程需要局部规划方法提供安全的运动命令。总而 言之，这个导航任务对局部规划方法的灵活性和安全性提出了巨大的挑战。
在本次测试中，文献1中的方法引导机器人缓慢地转过拐角。如图7(a) 所示，在拐角处文献1中的方法根据评价函数选取前向运动的局部路径， 而不是曲率更大但能引导机器人顺利到达目标点的左转的局部路径。最后， 机器人运动到死胡同，所有的候选路径都不可行，局部规划方法失效。与 之相反，如图7(b)所示本发明提出的局部路径优化方法引导机器人平滑地 通过拐角和狭窄通道。从这个测试中可以看出，相比于文献1中的方法，所提出的局部路径规划方法具有更高的灵活性。
(2)运动效率对比
为对局部规划方法的运动效率进行对比，我们让机器人沿着如图5(b) 和图5(c)所示的全局路径运动。在机器人运动过程中，文献1中的方法是将 一段又一段的五阶贝塞尔曲线拼接起来，因此得到的局部路径是不光滑的。 与之相反，本发明提出的局部路径优化方法考虑了路径平滑性，因此能够 为机器人提供更加光滑的局部路径。路径的平滑性直接体现在运动效率上。 在本次测试中，本发明提出的局部规划方法分别用49.78s和50.59s引导机器 人到达目标点，而文献1中的方法分别用时54.43s和55.35s，这也验证了所 提出的局部路径规划方法具有更高的运动效率。
(3)实验对比结果
我们采用如图8所示的实验平台对局部规划方法进行实验验证。移动 平台采用差分移动机器人Pioneer 3-DX，上面搭载了一个二维激光雷达 Hokuyo UTM-30LX。激光雷达的扫描范围为-135°～135°，角度分辨率为0.25°， 有效测量范围为0.1m～30m。机器人的最大速度为1.2m/s。考虑到室内导航 的安全性，实验过程中将速度上限设置为0.7m/s。
为对局部规划方法的灵活性、平滑性和运动效率进行对比，我们选取 如图9(a)和图9(b)所示的实验室场景进行实验验证。在这个测试中， 机器人需要先沿着走廊运动，然后拐一个急转弯并避开门口摆放的箱子进 入右侧的房间。这样的场景需要局部规划方法提供安全、灵活的运动指引。
在本次测试中，本发明提出的局部路径优化方法展现了出色的平滑性 和灵活性。尤其是当机器人靠近门并需要拐一个右急转弯时，所提出的方 法为机器人提供了平滑的局部路径，如图10(b)所示。与之相反，文献1中 的方法引导机器人缓慢地拐进实验室。如图10(a)所示，文献1中离线存储 的查找表中包含的局部路径的端点是固定的，因此在线规划时局部规划的 目标难以根据场景灵活调整，限制了规划的灵活性。我们重复进行多组实验，并记录实验结果如表1所示。文献1中的方法平均耗时25.76s引导机器 人到达目标点，而本发明所提出的方法平均耗时19.86s，运动效率提高约 22.87％。
表1实验室环境中局部规划运动效率结果
| 文献1 | 本发明 | 效率提升 | |
| 1 | 25.614s | 19.813s | 22.65％ |
| 2 | 25.711s | 19.842s | 22.83％ |
| 3 | 25.872s | 19.921s | 23.00％ |
| 4 | 25.836s | 19.909s | 22.94％ |
| 5 | 25.743s | 19.834s | 22.95％ |
(4)导航实验结果
最后，我们在一个大小为92.9×26.5m2的实验室环境中进行导航实验，以 此对本发明提出的局部规划方法进行综合评估。图11展示了机器人的大致 运动路线。为了让实验过程更具挑战性，我们在走廊摆放两个箱子作为静 态障碍物，并以此构造狭窄通道。此外，走廊还有行人走动。整个实验情 景综合考察局部路径规划方法应对狭窄空间和动态障碍物的能力。
在上述导航实验中，本发明提出的局部规划方法引导机器人耗时62.83s 到达目标点，总行程为44.2m。我们列举了下述一些导航实验结果来验证所 提出方法的核心特性：
图12(a)和图12(c)展示了带有静态障碍物的实验场景。在本发明提 出的局部规划方法的引导下，机器人避开了箱子，顺利地通过了狭窄的通 道。
图12(b)和图12(d)展示了带有动态障碍物的实验场景。基于本发明 提出的局部规划方法提供的高效的运动指引，机器人实现了快速的重新规 划，成功地避开了迎面走来的行人。
图13(a)-图13(d)展示了机器人在本发明提出的局部规划方法的指 导下顺利穿过人群。这种具有挑战性的场景需要局部规划方法提供灵活、 安全和平滑的运动命令。
以上一个或多个实施例具有以下技术效果：
本发明提出了一种基于稀疏带状结构的局部路径优化方法，所提出的 方法结合欧式距离栅格地图和双线性插值法，高效地计算了机器人到最近 障碍物的距离函数，克服了栅格地图离散化问题，并天然地计算得到梯度 信息。此外，本发明充分挖掘了所设计优化问题背后的稀疏带状结构，结 合LM算法和追赶法进行求解。相比于传统的基于稀疏矩阵分解的方法， 所提出的方法不需要对稀疏矩阵进行压缩存储操作，效率更高。
上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本 发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案 的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或 变形仍在本发明的保护范围以内。
Claims (10)
1.一种基于稀疏带状结构的机器人局部路径优化方法，其特征在于，包括以下步骤：
获取室内环境数据，并构建占据栅格地图；
将占据栅格地图转换为欧式距离栅格地图，根据欧氏距离栅格地图，基于双线性插值法计算机器人到最近障碍物的连续可微的距离函数，结合给定机器人到障碍物的安全距离，得到避障约束条件；
在以机器人当前位置为中心的局部区域内，以局部路径的平滑性和避障为约束条件，求解局部最优路径；
综合考虑局部路径平滑性和避障约束，定义多项加权的优化目标函数为：
其中，参数向量
为全局坐标系下的路径点序列，N为路径点个数，Δxi＝xi-xi-1,2≤i≤N表示xi-1指向xi的位移矢量，fo(xi)为避障代价函数，ωs和ωo分别表示平滑项和避障项对应的权重；通过最小化上述目标函数得到局部最优解x*
记平滑项子函数Si-1,i,i+1(x)＝Δxi+1-Δxi，避障项子函数Oi(x)＝fo(xi)，权重系数矩阵Ωs＝ωs·I2×2和Ωo＝ωo·I1×1；为求解上述优化问题，现假设有一较好的初值
在该初值附近分别对平滑项子函数Si-1,i,i+1(x)和避障项子函数Oi(x)泰勒展开，并保留线性部分：
其中，
和
分别是Si-1,i,i+1(x)和Oi(x)在
处的雅可比矩阵，则上述目标函数转换为：
其中，Hs和Ho分别是平滑项和避障项对应的海塞矩阵，H为整个优化问题的海塞矩阵，具体公式为：
2.如权利要求1所述的基于稀疏带状结构的机器人局部路径优化方法，其特征在于，将占据栅格地图转换为欧式距离栅格地图具体为：
将被障碍物占据的栅格值记为0，计算每个栅格的中心到最近障碍物栅格中心的欧式距离，得到欧式距离栅格地图。
3.如权利要求1所述的基于稀疏带状结构的机器人局部路径优化方法，其特征在于，所述避障约束条件采用避障项代价函数表示：
其中，ds为给定机器人到障碍物的安全距离，
为机器人与最近障碍物的距离函数的梯度，xi＝(xi,yi)T表示机器人位置的世界坐标。
4.如权利要求3所述的基于稀疏带状结构的机器人局部路径优化方法，其特征在于，局部路径的平滑性约束条件采用平滑项函数表示：
其中，Δxi＝xi-xi-1,2≤i≤N表示xi-1指向xi的位移矢量，
xi＝(xi,yi)T,1≤i≤N是以机器人当前位置为起点，以一定的间隔在局部区域内的全局路径上进行采样，得到的N个全局坐标系下表示的路径点，其中，全局路径是假设已经通过全局路径规划得到的一条无碰撞的路径。
5.如权利要求4所述的基于稀疏带状结构的机器人局部路径优化方法，其特征在于，求解局部最优路径具体为：求解以平滑项函数和避障项代价函数加权组合构建的目标函数。
6.如权利要求5所述的基于稀疏带状结构的机器人局部路径优化方法，其特征在于，基于LM法对目标函数进行求解，得到平滑项函数和避障项代价函数对应的海塞矩阵，累加得到稀疏带状的海塞矩阵，结合追赶法求解得到局部最优路径。
7.一种基于稀疏带状结构的机器人局部路径优化系统，其特征在于，采用如权利要求1-6任一项所述的一种基于稀疏带状结构的机器人局部路径优化方法，包括：
占据栅格地图构建模块，被配置为：获取室内环境数据，并构建占据栅格地图；
欧氏距离栅格地图构建模块，被配置为：将占据栅格地图转换为欧式距离栅格地图；
避障约束条件求解模块，被配置为：根据欧氏距离栅格地图，基于双线性插值法计算机器人到最近障碍物的连续可微的距离函数，结合给定机器人到障碍物的安全距离，得到避障约束条件；
局部路径优化模块，被配置为：在以机器人当前位置为中心的局部区域内，以局部路径的平滑性和避障为约束条件，求解局部最优路径。
8.一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-6任一项所述的局部路径优化方法。
9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-6任一项所述的局部路径优化方法。
10.一种机器人，其特征在于，其上包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-6任一项所述的局部路径优化方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110566018.8A CN113296514B (zh) | 2021-05-24 | 2021-05-24 | 一种基于稀疏带状结构的局部路径优化方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110566018.8A CN113296514B (zh) | 2021-05-24 | 2021-05-24 | 一种基于稀疏带状结构的局部路径优化方法及系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113296514A CN113296514A (zh) | 2021-08-24 |
| CN113296514B true CN113296514B (zh) | 2022-09-27 |
Family
ID=77324314
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110566018.8A Active CN113296514B (zh) | 2021-05-24 | 2021-05-24 | 一种基于稀疏带状结构的局部路径优化方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113296514B (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113778099A (zh) * | 2021-09-16 | 2021-12-10 | 浙江大学湖州研究院 | 基于NDT算法和Hybrid A*算法的无人船路径规划方法 |
| CN114237256B (zh) * | 2021-12-20 | 2023-07-04 | 东北大学 | 一种适用于欠驱动机器人的三维路径规划与导航方法 |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111258316A (zh) * | 2020-01-20 | 2020-06-09 | 浙江工业大学 | 一种动态环境下趋势感知的机器人轨迹规划方法 |
| CN112509056A (zh) * | 2020-11-30 | 2021-03-16 | 中国人民解放军32181部队 | 一种动态战场环境实时路径规划系统及方法 |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP6559535B2 (ja) * | 2015-10-22 | 2019-08-14 | 株式会社東芝 | 障害物マップ生成装置、その方法、及び、そのプログラム |
| CN108958238B (zh) * | 2018-06-01 | 2021-05-07 | 哈尔滨理工大学 | 一种基于协变代价函数的机器人点到区路径规划方法 |
| CN111380532B (zh) * | 2018-12-29 | 2022-06-28 | 深圳市优必选科技有限公司 | 路径规划方法、装置、终端及计算机存储介质 |
| CN110455274B (zh) * | 2019-08-02 | 2021-07-06 | 中科新松有限公司 | 基于倒角距离形状匹配的agv初始定位方法及定位系统 |
| CN112327931B (zh) * | 2020-12-01 | 2022-11-29 | 天津基点科技有限公司 | 一种基于sdf地图的无人机三维路径快速规划方法 |
-
2021
- 2021-05-24 CN CN202110566018.8A patent/CN113296514B/zh active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111258316A (zh) * | 2020-01-20 | 2020-06-09 | 浙江工业大学 | 一种动态环境下趋势感知的机器人轨迹规划方法 |
| CN112509056A (zh) * | 2020-11-30 | 2021-03-16 | 中国人民解放军32181部队 | 一种动态战场环境实时路径规划系统及方法 |
Non-Patent Citations (4)
| Title |
|---|
| Efficient Trajectory Optimization using a Sparse Model;Christoph R¨osmann;《ECMR》;20130927;全文 * |
| Optimization-Based Collision Avoidance;Xiaojing Zhang;《IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY》;20200409;全文 * |
| 基于传感器融合的 SLAM 与路径规划算法研究;姜晗;《中国优秀硕博士论文全文数据库》;20210115;全文 * |
| 非结构场景下移动机器人自主导航关键技术研究;程传奇;《中国优秀硕博士论文全文数据库》;20190715;全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113296514A (zh) | 2021-08-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Qureshi et al. | Motion planning networks | |
| Guechi et al. | PDC control design for non-holonomic wheeled mobile robots with delayed outputs | |
| CN113296514B (zh) | 一种基于稀疏带状结构的局部路径优化方法及系统 | |
| Maurović et al. | Explicit model predictive control for trajectory tracking with mobile robots | |
| Fareh et al. | Investigating reduced path planning strategy for differential wheeled mobile robot | |
| Khalaji et al. | Lyapunov-based formation control of underwater robots | |
| Poonganam et al. | Reactive navigation under non-parametric uncertainty through hilbert space embedding of probabilistic velocity obstacles | |
| Miller et al. | Optimal planning for target localization and coverage using range sensing | |
| Mansouri et al. | A unified nmpc scheme for mavs navigation with 3d collision avoidance under position uncertainty | |
| Mohamed et al. | Autonomous navigation of agvs in unknown cluttered environments: log-mppi control strategy | |
| Huh et al. | Cost-to-go function generating networks for high dimensional motion planning | |
| Ahmed et al. | Space deformation based path planning for Mobile Robots | |
| Chehelgami et al. | Safe deep learning-based global path planning using a fast collision-free path generator | |
| Pshikhopov et al. | Trajectory planning algorithms in two-dimensional environment with obstacles | |
| Zhang et al. | A safety planning and control architecture applied to a quadrotor autopilot | |
| Asgharivaskasi et al. | Active mapping via gradient ascent optimization of shannon mutual information over continuous se (3) trajectories | |
| Nfaileh et al. | Formation control of multiple wheeled mobile robots based on model predictive control | |
| Nayak et al. | A heuristic-guided dynamical multi-rover motion planning framework for planetary surface missions | |
| Helble et al. | 3-d path planning and target trajectory prediction for the oxford aerial tracking system | |
| Jacinto et al. | Navigation of autonomous vehicles using reinforcement learning with generalized advantage estimation | |
| Ferrolho et al. | Residual force polytope: Admissible task-space forces of dynamic trajectories | |
| Quang et al. | Design a nonlinear MPC controller for autonomous mobile robot navigation system based on ROS | |
| CN114815899A (zh) | 基于3d激光雷达传感器的无人机三维空间路径规划方法 | |
| Zarei et al. | Experimental study on optimal motion planning of wheeled mobile robot using convex optimization and receding horizon concept | |
| Sartori et al. | CNN-based path planning on a map |
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 |