CN114237256A - 一种适用于欠驱动机器人的三维路径规划与导航方法 - Google Patents

一种适用于欠驱动机器人的三维路径规划与导航方法 Download PDF

Info

Publication number
CN114237256A
CN114237256A CN202111561505.1A CN202111561505A CN114237256A CN 114237256 A CN114237256 A CN 114237256A CN 202111561505 A CN202111561505 A CN 202111561505A CN 114237256 A CN114237256 A CN 114237256A
Authority
CN
China
Prior art keywords
cost
path
robot
track
obstacle
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.)
Granted
Application number
CN202111561505.1A
Other languages
English (en)
Other versions
CN114237256B (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.)
Northeastern University China
Original Assignee
Northeastern University China
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 Northeastern University China filed Critical Northeastern University China
Priority to CN202111561505.1A priority Critical patent/CN114237256B/zh
Publication of CN114237256A publication Critical patent/CN114237256A/zh
Application granted granted Critical
Publication of CN114237256B publication Critical patent/CN114237256B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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 or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • 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
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

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)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Electromagnetism (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Manipulator (AREA)

Abstract

本发明提供一种适用于欠驱动机器人的三维路径规划与导航方法，涉及机器人导航技术领域。本发明包括全局路径平滑算法、局部绕障规划算法及三维避障跟踪控制算法三个部分。适用于低矮、悬挂以及动态等障碍物的复杂三维环境的室内外自主导航方法，克服存储三维环境信息具有较大资源开销的同时，能够灵活控制绕障路径相对参考路径横向偏离距离，且绕障完成后能够返回至全局路径的局部绕障模块，该方法可满足机器人的动力学及运动学约束进行避障跟踪控制，适用于多种运动模型的欠驱动机器人方法的自主导航。

Description

一种适用于欠驱动机器人的三维路径规划与导航方法
技术领域
本发明涉及机器人导航技术领域，尤其涉及一种适用于欠驱动机器人的三维路径规划与导航方法。
背景技术
近年来，随着科学技术的飞速发展，移动机器人技术在实体工业、农业、医疗、卫生、服务、军事和航空航天领域均得到了广泛的应用，但机器人技术复杂，综合性强，仍有很多难点亟待突破。在移动机器人的诸多技术中，路径规划是移动机器人导航系统的重要组成部分,其任务是根据先验的全局地图信息以及自身传感器实时感知的动态环境信息，综合给定的路径评价指标，寻找一条从起始状态(包括位置和姿态)到达目标状态(包括位置和姿态)的无碰撞路径。在实际应用中，愈发复杂的三维环境将会直接影响控制器路径规划的效果，通常所面临的具有未知性、非结构化和动态性等特点的工作环境给机器人的避障规划带来了巨大挑战。此外，移动机器人的运动学和动力学约束使机器人的运动规划问题维度上升，难以快速求解，无法满足实时性的需求。对于上述复杂工作环境，移动机器人需要高度自主化、鲁棒、可靠地应对各种突发情况，从而保证机器人依据全局路径安全稳定地运行。
目前，根据不同的分类标准，路径规划技术可分为以下四类。根据是否具有先验信息，可分为全局路径规划和局部路径规划；根据地图维度信息，可分为2D、2.5D、3D及更高维度地图下的路径规划；根据地图表示方法，可分为基于可视图、栅格地图、特征地图及状态空间的路径规划算法；根据搜索方式，可分为基于图搜索、采样及人工智能的路径规划算法。上述算法存在着诸多不足，对于低维空间环境而言，基于图搜索的规划算法效率较高，且一般情况下均能保证算法的完整性。但是对于高维环境而言，基于图搜索的规划算法所需要的计算资源和存储资源都十分巨大，这也是机器人系统中所欠缺的必要资源，同时算法实时性无法达到机器人系统的要求。由于在高维复杂环境中，前端规划器给定的全局路径往往存在较多动态及静态障碍物，在表征障碍物信息时，其存储资源将会以指数级递增，导致规划算法所需要的计算复杂度和空间复杂度都将倍增，对移动机器人的机载计算资源和存储资源都有较高的需求。
综上，移动机器人的路径规划系统承担着机器人在已知或部分已知的环境中导航避障的功能。路径规划系统的输入包含机器人在全局地图中的实时位置信息、机器人自身传感器感知的环境信息和机器人移动所需要的路径信息，并在上述信息的基础上，通过感知模块、规划模块与控制模块，形成机器人底层控制所需要的速度输出。机器人路径规划系统主要服务于欠驱动机器人的室内外清洁、巡检、安保等工作，而欠驱动机器人的工作环境通常具有未知性、非结构化和动态性等特点，对机器人的路径规划、跟踪控制以及动态避障功能要求更加严格。因此，设计适用于室内外环境的三维导航系统是欠驱动机器人导航功能实现的基础，如何采用更小的能量消耗和更低的算法复杂度，获取动态及静态环境信息同时考虑动力学与运动学约束实现移动机器人的自主导航，是该项发明的主要目标。
现有路径平滑算法采用特殊曲线或多项式插值对路径平滑。多项式插值的方法容易计算，但是高阶曲线的系数难以控制，计算资源消耗也随之增高，同时有可能出现龙格现象，造成插值函数剧烈波动。参数化曲线存在显著缺点，其曲率是参数的复杂函数，对于高阶曲线考虑曲率有界约束的连续性路径平滑带来了很大挑战。对于三维环境表示方法主要有点云、数字高程模型、八叉树等等，上述表示形式虽然可以完全表征三维环境信息的同时，存在内存开销过大的问题。常用的路径规划算法均采用栅格地图进行地图表示，点云与数字高程模型不能被直接运用到移动机器人路径规划中，八叉树地图作为三维栅格地图的一种改进，在一定程度上解决了存储开销大的问题，同时也带来了计算复杂度高的问题。目前，移动机器人平台的存储与计算能力较弱，较高的计算复杂度和较大的存储开销导致算法不稳定性的提升。而二维代价地图表示方法具有较小的存储开销，但是它不能完整的表示三维环境的全部特征，具有局限性。模型预测控制(ModelPredictive Control，MPC)与线性二次型调节器(Linear Quadratic Regulator，LQR)是一种基于模型的闭环优化控制策略，其具有控制效果好、鲁棒性强等优点。在复杂的环境中，要考虑机器人运动学和动力学约束、时域约束及障碍物约束等，将导致优化求解过程耗时引发资源存储问题。传统动态窗口法(DynamicWindowApproach，DWA)算法是一种反应式规划，避障实时性较高，但存在不满足转向机构约束、容易陷入局部最优和没考虑三维环境信息等缺陷。
发明内容
针对现有技术的不足，本发明提供一种适用于欠驱动机器人的三维路径规划与导航方法。
一种适用于欠驱动机器人的三维路径规划与导航方法，包括以下步骤：
步骤1：基于先验的全局地图采用任务调度器对全局路径进行满足二阶几何连续性的三阶贝塞尔曲线平滑；通过解析法获得的高效路径平滑算法，通过任务调度器提供的有序路径点插值生成满足曲率上限约束的连续曲率路径；
步骤1.1：对欠驱动机器人运动进行分析，确定移动机器人的最小转弯半径，计算最大曲率kmax，以及该最大曲率约束的路径平滑长度dk，如下式所示：
Figure BDA0003420711760000031
式中，c1＝7.2364，
Figure BDA0003420711760000032
kmax指定曲率的上限值，dk表示满足最大曲率约束的路径平滑长度，β表示向量夹角；
步骤1.2：对于给定的先验全局地图，通过任务调度器提供有序路径点，若在机器人操作系统的三维可视化工具上显示箭头，两个相邻路径点的角度差大于等于设定阈值，则路径点提供完毕，若两个相邻路径点的角度差小于设定阈值，则在Rviz上继续提供路径点，直到满足上述条件，路径点提供完毕；
步骤1.3：对有序路径点通过满足二阶几何连续性的三阶贝塞尔曲线拟合并进行插值，生成满足机器人曲率连续性和有界性约束的路径；
步骤2：利用滚动时域规划器提出一种图搜索和共轭梯度平滑相结合的方法生成绕障路径；以全局路径作为依据，在平滑全局路径上截取参考路径，并根据局部动态地图上的障碍物分布对机器人在该段路径上的碰撞情况进行检测，若检测到障碍物，则依据代价函数在参考路径的邻近区域adj(ζ)构建节点图，并利用词典搜索的方法进行搜索，得到一条安全平滑的绕障路径；
步骤2.1：根据移动机器人当前状态xc和最大规划距离droll从全局路径ζ中截取参考路径σR，其中，全局路径ζ由参考路径σR与剩余的原始路径σζ组成，即ζ＝σR∪σζ；
步骤2.2：当全局路径ζ未执行至最终目标点，通过欠驱动机器人搭载的三维雷达与二维激光传感器的感知数据S获取实时障碍物分布，基于移动机器人的当前状态xc检查截取的参考路径σR的可行性；
步骤2.3：若检测到障碍物，滚动时域规划器根据参考路径σR和感知数据S构建横向距离dspan上的局部搜索节点图G＝(V,E)，其中，dspan是V中采样节点与全局路径ζ的最大横向距离，V是节点集，E是边集，通过使用固定密度进行采样得到节点集V，每个节点与相邻八个节点连接，用来显示节点图G，执行词典搜索算法，搜索成功则输出更新后的参考路径σR，即绕障路径σG，搜索失败则输出机器人当前状态xc。若未检测到障碍物，无碰撞风险则不进行重规划；
所述滚动规划过程中定义三种类型的代价指标，分别是风险代价、航向代价和距离代价：
其中所述风险代价沿路径σ累积，定义为第一级代价，风险代价C1(σ)公式如下所示：
Figure BDA0003420711760000033
式中，σ(s)是构型空间C中自由空间Cfree的有限长连续函数，s是区间[0,1]中任意值，σ(0)是机器人的当前状态xinit，σ(1)是机器人的目标状态xgoal，函数Risk()评估单个机器人状态下的碰撞风险，R(x)为x与距离x最近的障碍物之间距离的倒数；如果R(x)大于风险阈值Thrisk，则激活Risk()函数；
所述航向代价为第二级代价，为惩罚机器人和全局路径ζ之间的航向差异，航向代价C2(σ)公式如下所示：
Figure BDA0003420711760000041
式中，函数Heading(x)为x和全局路径ζ上与x最近路径段的航向之间的航向差；定义航向差异阈值Thhead，当误差H(x)大于Thhead时，Heading(x)返回非零。添加此代价可确保生成的路径在遵守的航向的同时相对平滑。
所述距离代价为第三级代价，随着机器人行驶距离增加而增加，距离代价函数C3(σ)公式如下所示：
Figure BDA0003420711760000042
式中，函数Distance(x)是机器人行驶的距离。
步骤2.4：使用词典搜索算法根据节点图G、初始状态xinit、目标状态xgoal作为输入，初始化节点图中每个节点的三级非负代价值为无穷，使用迪克斯特拉Dijkstra算法执行图搜索得到最优路径时，xinit被指定为图中最接近机器人当前状态xc的状态、xgoal是节点图G在发生滚动规划时收敛到全局路径ζ的状态；
所述词典搜索算法在每次迭代中，最小代价计算函数FindMinCostk()会返回具有最小代价值的状态集合Xmin并与节点图G提供的节点之间共用最小的第k级代价。若Xmin包含多个节点状态，则需要检查集合中节点的较低层级代价，直到集合Xmin包含单个节点xi，并检查其相邻节点。此外，如果存在边eij，可以使用节点xi来降低与相邻节点xj相关联的代价。若通过节点xi从初始节点xinit节点xj的第k级代价低于当前代价xj·ck，则通过选择节点xi作为其新的父节点，来更新节点xj的代价。否则，节点xi从初始节点xinit节点xj的第k级代价与当前代价xj·ck相关联，词典搜索算法将继续评估xi在xj处的第k+1级代价。为了减少末级代价关联的可能性，假设最低层级代价K在所有路径上都是正的。最终搜索的绕障路径σG，采用共轭梯度优化方法对绕障路径σG进行平滑，得到曲率连续的平滑的最优可行路径。
步骤3：采用避障跟踪控制，在滚动时域规划器提供的安全平滑路径、激光点云信息及机器人实时位姿基础上，构建实时障碍物栅格地图并在该地图上创建速度场，基于速度场与预锚点，采用快速行进法生成目标势场，根据目标势场生成方向梯度场，进而构建轨迹评价函数，依据轨迹评价函数建立改进的FMM-DWA算法满足机器人运动学及动力学模型的最优安全轨迹，执行最优轨迹对应的机器人控制指令；
步骤3.1：根据步骤2.2中的感知数据S，先对点云数据进行降采样滤波处理，根据机器人大小选取一个Box空间内的点云，将Box空间内的点云投影到一个平面，构建一个二维栅格地图，并对地图障碍物添加膨胀层；
所述Box空间以移动机器人为中心，实时更新并滚动重建的环境信息；
步骤3.2：根据实时障碍物栅格地图构建的速度场提供的网格传播速度，采用快速行进法FMM生成目标势场，然后根据目标势场进行梯度求导生成方向梯度场；
步骤3.2.1：当计算到达时间T时，将障碍物所在区域的网格传播速度W(i,j)设置为0，同时，根据分段函数设置靠近障碍物区域的传播速度W(i,j)，包含栅格地图中每个点速度的速度场。计算障碍物区域的传播速度W(i,j)，公式如下所示：
Figure BDA0003420711760000051
式中，α是障碍物影响因子，d(i,j)是点(i,j)到栅格地图上最近障碍物的距离，Thd是设定阈值；
步骤3.2.2：采用快速行进法FMM按设定的传播速度扩展搜索空间，获得目标势场；
Figure BDA0003420711760000052
式中，(i,j)为由栅格表示的规划空间中第i行和第j列的栅格，T(i,j)为从起点到达点(i,j)的时间，W(i,j)是界面在点(i,j)处的局部传播速度；通过离散化梯度T(i,j)在空间中每点(i,j)求解程函方程；
简化到达点(i,j)的时间表示方式：
T1＝min(T(i-1,j),T(i+1,j)),T2＝min(T(i,j-1),T(i,j+1))
式中，T1是从起点至第j列的第i-1行与第i+1行栅格中的最小到达时间，T2是从起点至第i行的第j-1行与第j+1行栅格中的最小到达时间；
进而得到程函方程的解如下公式所示：
Figure BDA0003420711760000061
步骤3.2.3：对目标势场进行微分，产生方向梯度场；
步骤3.3：基于实时障碍物地图、目标势场和方向梯度场构造轨迹评价函数，其评价指标构建分为运动有效性评价、航向有效性评价、曲率连续性评价、朝向有效性评价，根据步骤3.2对目标位置的构建方法，将快速行进算法FMM建立的目标势场及方向梯度场提供的目标信息嵌入DWA的轨迹评价函数的评价指标中，计算FMM-DWA算法中的轨迹评价指标；
步骤3.3.1：构造运动有效性函数，如下式所示：
Figure BDA0003420711760000062
式中，goal_cost是轨迹有效性代价，∑T(xe,ye)是所有轨迹的终点到达时间T(xe,ye)的代数和，β是机器人航向的影响因素，θe为轨迹的端点方向，θr(xe,ye)为方向梯度场提供的参考方向；
步骤3.3.2：进行航向有效性评价，如下式所示：
Figure BDA0003420711760000063
式中，heading_cost是航向有效性代价，α是障碍物影响因子，(xe,ye)为机器人轨迹终点位置坐标，d(xs,ys)是轨迹端点与最近障碍物之间的距离，T(xs,ys)是轨迹起点的到达时间；
步骤3.3.3：进行曲率连续性评价，如下式所示；
Figure BDA0003420711760000064
式中，curvature_cost是曲率连续性代价，v是机器人的线速度，w是机器人的角速度，k是轨迹的曲率，kset[i]是所有轨迹中第i条轨迹曲率，klast是上一周期选择的最优轨迹的曲率。
步骤3.3.4：进行朝向有效性评价，对机器人采样轨迹的起始点方向与该位置对应的参考方向比较；如下式所示：
Figure BDA0003420711760000065
式中，direction_cost是朝向有效性代价，Vset[i]表示所有采样轨迹中第i条轨迹的速度，Vsum表示所有采样轨迹速度绝对值的和。θs表示机器人当前位置方向，θr(xs,ys)表示机器人当前位置对应在梯度场中的参考方向。
步骤3.3.5：使用加权goal_cost、加权heading_cost、加权curvature_cost和加权direction_cost之和total_cost，评估每个轨迹的代价，从所有采样轨迹中选择最优轨迹，计算FMM-DWA算法中的轨迹评价指标，如下式所示：
total_cost＝σ*goal_cost+λ*heading_cost+γ*curvature_cost+κ*direction_cost
式中，σ、λ、γ和κ分别是运动有效性评价、航向有效性评价、曲率相似度评价和朝向有效性评价的权重；采用运动有效性代价函数、航向有效性代价函数、曲率相似度代价函数和朝向有效性代价函数的加权和作为总体代价函数total_cost，根据总体代价函数的函数值来评价欠驱动机器人向目标点移动时轨迹生成的优劣性，并通过最小化总体代价函数判断采样轨迹中的最优轨迹；
步骤3.4：采用FMM-DWA算法对速度空间(v,w)中满足最大允许曲率约束的若干组速度进行采样，其中v是线速度，w是角速度，模拟机器人在设定时间内的运动轨迹；采用轨迹评价函数选择最优轨迹，根据最优轨迹生成速度指令对机器人进行控制。
采用上述技术方案所产生的有益效果在于：
本发明提出了一种适用于欠驱动机器人的三维路径规划与导航方法，具有以下有益效果:
1、全局路径规划子系统的路径平滑器提供贝塞尔曲线优化后的路径，该路径满足曲率连续性及曲率有界性约束，从而提高了机器人的运动平滑性。
2、局部绕障规划子系统提出了一种词典优化结合图搜索的方法实现滚动时域规划器，其根据三维激光点云构建的局部动态地图，实时检测截取全局路径上的参考段路径邻域是否存在障碍物的情况，若存在障碍物则执行绕障规划算法，则生成一条平滑、安全的路径的同时节省内存资源损耗，保证移动机器人安全稳定的运行。该算法解决了绕障规划面临的挑战建模最小化代价的问题，能够通过对多目标优化处理代价间的分级排序，在不调整任何参数的情况下，快速求解并获得可行路径。在实机测试中，机器人以1.5m的速度运行时，采用最远纵向距离时10m，最大横向距离是1m，局部规划过程中，图构建耗时与图搜索耗时都小于10ms，满足静态环境或低动态环境的需要。
3、避障跟踪控制子系统提出了一种改进的DWA算法，FMM-DWA，该算法适用于多种欠驱动模型机器人，解决传统DWA陷入局部最优问题同时考虑三维环境信息，能够避免低矮障碍物和悬空障碍物检测不到或者检测不完全的问题。
附图说明
图1为本发明中适用于欠驱动机器人的三维路径规划与导航方法结构图；
图2为本发明中适用于欠驱动机器人的三维路径规划与导航方法流程图；
图3为本发明中满足二阶几何连续性的三阶贝塞尔曲线平滑方法示意图；
图4为本发明中先验全局地图与全局平滑路径示意图；
图5为本发明中滚动时域规划器中在参考路径σR上的最大横向距离dspan上生成的采样节点集合V的示意图；
图6为本发明中滚动时域规划器中生成的节点图G与绕障路径σG示意图；
图7为本发明中跟踪控制模块提供的传播速度场示意图；
图8为本发明中跟踪控制模块提供的快速行进算法FMM算法建立的目标势场示意图；
图9为本发明中跟踪控制模块提供的方向梯度场示意图；
图10为本发明实施例中滚动时域规划器伪代码图；
图11为本发明实施例中词典搜索算法伪代码图。
具体实施方式
下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。
一种适用于欠驱动机器人的三维路径规划与导航方法，如图1、图2所示，包括以下步骤：
步骤1：基于先验的全局地图采用任务调度器对全局路径进行满足二阶几何连续性的三阶贝塞尔曲线平滑；通过解析法获得的高效路径平滑算法，通过任务调度器提供的有序路径点插值生成满足曲率上限约束的连续曲率路径；
步骤1.1：欠驱动机器人是指独立控制输入量少于系统自由度的一类机器人，具有成本低、能耗低等众多优点。对欠驱动机器人运动进行分析，确定移动机器人的最小转弯半径，计算最大曲率kmax，以及该最大曲率约束的路径平滑长度dk，如下式所示：
Figure BDA0003420711760000081
式中，c1＝7.2364，
Figure BDA0003420711760000082
kmax指定曲率的上限值，dk表示满足最大曲率约束的路径平滑长度，β表示
Figure BDA0003420711760000083
和
Figure BDA0003420711760000084
矢量之间的向量夹角；一般地，设置dk＝d1＝d2，β＝γ/2，如图3所示。
步骤1.2：如图4所示，对于给定的先验全局地图，通过任务调度器提供有序路径点，若在机器人操作系统(RobotOperatingSystem，ROS)的三维可视化工具(3Dvisualization tool for ROS，Rviz)上显示箭头，两个相邻路径点的角度差大于等于设定阈值，则路径点提供完毕，若两个相邻路径点的角度差小于设定阈值，则在Rviz上继续提供路径点，直到满足上述条件，路径点提供完毕；
步骤1.3：对有序路径点通过满足二阶几何连续性的三阶贝塞尔曲线拟合并进行插值，生成满足机器人曲率连续性和有界性约束的路径；
步骤2：利用滚动时域规划器提出一种图搜索和共轭梯度平滑相结合的方法生成绕障路径；以全局路径作为依据，在平滑全局路径上截取参考路径，并根据局部动态地图上的障碍物分布对机器人在该段路径上的碰撞情况进行检测，若检测到障碍物，则依据代价函数在参考路径的邻近区域adj(ζ)构建节点图，并利用词典搜索的方法进行搜索，得到一条安全平滑的绕障路径；
步骤2.1：根据移动机器人当前状态xc和最大规划距离droll从全局路径ζ中截取参考路径σR，其中，全局路径ζ由参考路径σR与剩余的原始路径σζ组成，即ζ＝σR∪σζ；
步骤2.2：当全局路径ζ未执行至最终目标点，通过欠驱动机器人搭载的三维雷达与二维激光传感器的感知数据S获取实时障碍物分布，基于移动机器人的当前状态xc检查截取的参考路径σR的可行性；
步骤2.3：若检测到障碍物，滚动时域规划器根据参考路径σR和感知数据S构建横向距离dspan上的局部搜索节点图G＝(V,E)，其中，dspan是V中采样节点与全局路径ζ的最大横向距离，V是节点集，E是边集，通过使用固定密度0.1m进行采样得到节点集V，每个节点与相邻八个节点连接，用来直观的显示节点图G，如图5所示。执行词典搜索算法，搜索成功则输出更新后的参考路径σR，即绕障路径σG，如图6所示，搜索失败则输出机器人当前状态xc。若未检测到障碍物，无碰撞风险则不进行重规划，从而减少了计算资源消耗。滚动时域规划器伪代码如图10所示。
所述滚动规划过程中定义三种类型的代价指标。这三种代价按层次排列，分别是风险代价、航向代价和距离代价。
其中所述风险代价沿路径σ累积，定义为第一级代价，风险代价C1(σ)公式如下所示：
Figure BDA0003420711760000091
式中，σ(s)是构型空间C中自由空间Cfree的有限长连续函数，s是区间[0,1]中任意值，σ(0)是机器人的当前状态xinit，σ(1)是机器人的目标状态xgoal，函数Risk()评估单个机器人状态下的碰撞风险，R(x)为x与距离x最近的障碍物之间距离的倒数；如果R(x)大于风险阈值Thrisk，则激活Risk()函数，使用该代价函数的目的是在动静态障碍物附近建立危险区，保证规划的路径与障碍物之间有一个安全距离。将风险代价最小化可使机器人在这些区域的行驶距离最小化。
所述航向代价为第二级代价，为惩罚机器人和全局路径ζ之间的航向差异，航向代价C2(σ)公式如下所示：
Figure BDA0003420711760000101
式中，函数Heading(x)为x和全局路径ζ上与x最近路径段的航向之间的航向差；为了避免极端控制的强度，定义航向差异阈值Thhead，当误差H(x)大于Thhead时，Heading(x)返回非零。添加此代价可确保生成的路径在遵守的航向的同时相对平滑。
所述距离代价为第三级代价，随着机器人行驶距离增加而增加，距离代价函数C3(σ)公式如下所示：
Figure BDA0003420711760000102
式中，函数Distance(x)是机器人行驶的距离。
这三个代价的定义旨在支持我们获取一条安全、平滑且距离最优的路径。风险代价能够将环境中的行人或车辆等动态障碍物对机器人的干扰降至最低，以确保机器人运行安全。航向代价有助于提供平稳的驾驶，同时最大限度地减少控制工作。距离代价有助于机器人选择最短轨迹，减少能量消耗；
步骤2.4：使用词典搜索算法根据节点图G、初始状态xinit、目标状态xgoal作为输入，初始化节点图中每个节点的三级非负代价值为无穷，使用迪克斯特拉Dijkstra算法执行图搜索得到最优路径时，xinit被指定为图中最接近机器人当前状态xc的状态、xgoal是节点图G在发生滚动规划时收敛到全局路径ζ的状态；
所述词典搜索算法在每次迭代中，最小代价计算函数FindMinCostk()会返回具有最小代价值的状态集合Xmin并与节点图G提供的节点之间共用最小的第k级代价。若Xmin包含多个节点状态，则需要检查集合中节点的较低层级代价，直到集合Xmin包含单个节点xi，并检查其相邻节点。此外，如果存在边eij，可以使用节点xi来降低与相邻节点xj相关联的代价。若通过节点xi从初始节点xinit节点xj的第k级代价低于当前代价xj·ck，则通过选择节点xi作为其新的父节点，来更新节点xj的代价。否则，节点xi从初始节点xinit节点xj的第k级代价与当前代价xj·ck相关联，词典搜索算法将继续评估xi在xj处的第k+1级代价。为了减少末级代价关联的可能性，假设最低层级代价K在所有路径上都是正的。最终搜索的绕障路径σG，采用共轭梯度优化方法对绕障路径σG进行平滑，得到曲率连续的平滑的最优可行路径。词典搜索算法伪代码如图11所示；
步骤3：采用避障跟踪控制，在滚动时域规划器提供的安全平滑路径、激光点云信息及机器人实时位姿基础上，构建实时障碍物栅格地图并在该地图上创建速度场，基于速度场与预锚点，采用快速行进法(Fast March Method,FMM)生成目标势场，根据目标势场生成方向梯度场，进而构建轨迹评价函数，依据轨迹评价函数建立改进的DWA算法(FastMarch Method-Dynamic Window Algorithm，FMM-DWA)满足机器人运动学及动力学模型的最优安全轨迹，执行最优轨迹对应的机器人控制指令；
步骤3.1：根据步骤2.2中的感知数据S，先对点云数据进行降采样滤波处理，根据机器人大小选取一个Box空间内的点云，将Box空间内的点云投影到一个平面，构建一个二维栅格地图，并对地图障碍物添加膨胀层；
所述Box空间以移动机器人为中心，实时更新并滚动重建的环境信息；
步骤3.2：根据实时障碍物栅格地图构建的速度场提供的网格传播速度，采用快速行进法FMM生成目标势场，然后根据目标势场进行梯度求导生成方向梯度场；
步骤3.2.1：当计算到达时间T时，将障碍物所在区域的网格传播速度W(i,j)设置为0，同时，根据分段函数设置靠近障碍物区域的传播速度W(i,j)，包含栅格地图中每个点速度的速度场，如图7所示。计算障碍物区域的传播速度W(i,j)，公式如下所示：
Figure BDA0003420711760000111
式中，α是障碍物影响因子，d(i,j)是点(i,j)到栅格地图上最近障碍物的距离，Thd是设定阈值；
步骤3.2.2：快速行进法FMM是由数值方法求解程函方程(EikonalEquation)的粘性解，来解决界面的传播问题。程函方程如下公式所示。采用快速行进法FMM按设定的传播速度扩展搜索空间，获得目标势场，如图8所示；
Figure BDA0003420711760000112
式中，(i,j)为由栅格表示的规划空间中第i行和第j列的栅格，T(i,j)为从起点到达点(i,j)的时间，W(i,j)是界面在点(i,j)处的局部传播速度；通过离散化梯度T(i,j)在空间中每点(i,j)求解程函方程；
简化到达点(i,j)的时间表示方式：
T1＝min(T(i-1,j),T(i+1,j)),T2＝min(T(i,j-1),T(i,j+1))
式中，T1是从起点至第j列的第i-1行与第i+1行栅格中的最小到达时间，T2是从起点至第i行的第j-1行与第j+1行栅格中的最小到达时间；
进而得到程函方程的解如下公式所示：
Figure BDA0003420711760000121
步骤3.2.3：对目标势场进行微分，产生方向梯度场，如图9所示，其为移动机器人的运动提供了参考方向；
步骤3.3：基于实时障碍物地图、目标势场和方向梯度场构造轨迹评价函数，其评价指标构建分为运动有效性评价、航向有效性评价、曲率连续性评价、朝向有效性评价，根据步骤3.2对目标位置的构建方法，将快速行进算法FMM建立的目标势场及方向梯度场提供的目标信息嵌入DWA的轨迹评价函数的评价指标中，计算FMM-DWA算法中的轨迹评价指标，从而解决传统DWA容易陷入局部最优的缺陷；
步骤3.3.1：运动有效性评价是为了评估从当前位置移动到目标的有效性，构造运动有效性函数，如下式所示：
Figure BDA0003420711760000122
式中，goal_cost是轨迹有效性代价，用于评估轨迹是否移动到T值较低的位置。∑T(xe,ye)是所有轨迹的终点到达时间T(xe,ye)的代数和，β是机器人航向的影响因素，θe为轨迹的端点方向，θr(xe,ye)为方向梯度场提供的参考方向，当轨迹的端点方向θe和之间的差值增大时，goal_cost将随之增大，从而选择更接近参考方向的轨迹。
步骤3.3.2：进行航向有效性评价，解决机器人陷入局部最优的问题，使机器人在接近障碍物时能够快速调整航向避开前方障碍物，特别是在距离障碍物太近时机器人能够向后移动。如下式所示：
Figure BDA0003420711760000123
式中，heading_cost是航向有效性代价，α是障碍物影响因子，用来评定障碍物对于规划的路径的影响，(xe,ye)为机器人轨迹终点位置坐标，d(xs,ys)是轨迹端点与最近障碍物之间的距离，T(xs,ys)是轨迹起点的到达时间，当T(xs,ys)较小时，heading_cost迅速增加，将使机器人更倾向于选择与梯度场提供的参考方向相近的轨迹。当d(xs,ys)值较小时，机器人会迅速将航向调整到参考方向，避免距离障碍物距离过近。
步骤3.3.3：进行曲率连续性评价，降低移动机器人的转向电机响应频率影响，从而降低跃度的变化，满足机器人稳定运行时的舒适性，如下式所示；
Figure BDA0003420711760000124
式中，curvature_cost是曲率连续性代价，v是机器人的线速度，w是机器人的角速度，k是轨迹的曲率，kset[i]是所有轨迹中第i条轨迹曲率，klast是上一周期选择的最优轨迹的曲率。当kset[i]与klast两差的绝对值越小，curvature_cost值越小，机器人舵轮需要转动的幅度就越小，机器人运行就越平稳。∑|kset[i]-klast|表示当前规划出的所有轨迹与上一时刻的最优轨迹曲率偏差和。
步骤3.3.4：进行朝向有效性评价，对机器人采样轨迹的起始点方向与该位置对应的参考方向比较；当机器人当前位置方向与梯度场提供的参考方向存在很大差异时，朝向有效性评价会倾向选择后退的轨迹。此外，考虑机器人掉头效率及稳定性，该评价函数还引入机器人行驶速度作为动态系数。在机器人掉头过程中，机器人优先选择最大后退速度。如下式所示：
Figure BDA0003420711760000131
式中，direction_cost是朝向有效性代价，Vset[i]表示所有采样轨迹中第i条轨迹的速度，Vsum表示所有采样轨迹速度绝对值的和。θs表示机器人当前位置方向，θr(xs,ys)表示机器人当前位置对应在梯度场中的参考方向。
步骤3.3.5：使用加权goal_cost、加权heading_cost、加权curvature_cost和加权direction_cost之和total_cost，评估每个轨迹的代价，从所有采样轨迹中选择最优轨迹，计算FMM-DWA算法中的轨迹评价指标，如下式所示：
total_cost＝σ*goal_cost+λ*heading_cost+γ*curvature_cost+κ*dirsction_cost
式中，σ、λ、γ和κ分别是运动有效性评价、航向有效性评价、曲率相似度评价和朝向有效性评价的权重；采用运动有效性代价函数、航向有效性代价函数、曲率相似度代价函数和朝向有效性代价函数的加权和作为总体代价函数total_cost，根据总体代价函数的函数值来评价欠驱动机器人向目标点移动时轨迹生成的优劣性，并通过最小化总体代价函数判断采样轨迹中的最优轨迹；
步骤3.4：采用FMM-DWA算法对速度空间(v,w)中满足最大允许曲率约束的若干组速度进行采样，其中v是线速度，w是角速度，模拟机器人在设定时间内的运动轨迹；采用轨迹评价函数选择最优轨迹，根据最优轨迹生成速度指令对机器人进行控制。
本实施例中，针对机器人运动学模型的约束，实现了满足曲率连续且满足最大曲率约束的多项式插值的路径平滑方法，根据二维及三维激光传感器感知到的环境信息对栅格地图进行更新，并引入词典优化与图搜索相结合的方法实现滚动时域规划器，避障跟踪控制在局部绕障规划模块提供的安全平滑路径、降采样后的激光点云信息及机器人实时位姿基础上，通过构建实时障碍物地图、目标势场及方向梯度场来实现实时避障。此外，在不同运动模型的机构约束条件下(机器人最小转弯半径约束)，对欠驱动机器人进行实验，通过机器人运行过程中根据执行指令生成的动作曲率，分析得出系统输出的速度指令满足机器人的最大允许曲率约束，验证了本发明改进的DWA方法能够满足欠驱动机器人的运动学模型。通过在仿真环境及多种运动模型机器人平台上实机测试，验证了本发明提出的三维自主导航方法的有效性、实时性与鲁棒性。
以上描述仅为本公开的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本公开的实施例中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离上述发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本公开的实施例中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。

Claims (8)

1.一种适用于欠驱动机器人的三维路径规划与导航方法，其特征在于，包括以下步骤：
步骤1：基于先验的全局地图采用任务调度器对全局路径进行满足二阶几何连续性的三阶贝塞尔曲线平滑；通过解析法获得的高效路径平滑算法，通过任务调度器提供的有序路径点插值生成满足曲率上限约束的连续曲率路径；
步骤2：利用滚动时域规划器提出一种图搜索和共轭梯度平滑相结合的方法生成绕障路径；以全局路径作为依据，在平滑全局路径上截取参考路径，并根据局部动态地图上的障碍物分布对机器人在该段路径上的碰撞情况进行检测，若检测到障碍物，则依据代价函数在参考路径的邻近区域adj(ζ)构建节点图，并利用词典搜索的方法进行搜索，得到一条安全平滑的绕障路径；
步骤3：采用避障跟踪控制，在滚动时域规划器提供的安全平滑路径、激光点云信息及机器人实时位姿基础上，构建实时障碍物栅格地图并在该地图上创建速度场，基于速度场与预锚点，采用快速行进法生成目标势场，根据目标势场生成方向梯度场，进而构建轨迹评价函数，依据轨迹评价函数建立改进的FMM-DWA算法满足机器人运动学及动力学模型的最优安全轨迹，执行最优轨迹对应的机器人控制指令。
2.根据权利要求1所述的一种适用于欠驱动机器人的三维路径规划与导航方法，其特征在于，所述步骤1具体包括以下步骤：
步骤1.1：对欠驱动机器人运动进行分析，确定移动机器人的最小转弯半径，计算最大曲率kmax，以及该最大曲率约束的路径平滑长度dk，如下式所示：
Figure FDA0003420711750000011
式中，c1＝7.2364，
Figure FDA0003420711750000012
kmax指定曲率的上限值，dk表示满足最大曲率约束的路径平滑长度，β表示向量夹角；
步骤1.2：对于给定的先验全局地图，通过任务调度器提供有序路径点，若在机器人操作系统的三维可视化工具上显示箭头，两个相邻路径点的角度差大于等于设定阈值，则路径点提供完毕，若两个相邻路径点的角度差小于设定阈值，则在Rviz上继续提供路径点，直到满足上述条件，路径点提供完毕；
步骤1.3：对有序路径点通过满足二阶几何连续性的三阶贝塞尔曲线拟合并进行插值，生成满足机器人曲率连续性和有界性约束的路径。
3.根据权利要求1所述的一种适用于欠驱动机器人的三维路径规划与导航方法，其特征在于，所述步骤2具体包括以下步骤：
步骤2.1：根据移动机器人当前状态xc和最大规划距离droll从全局路径ζ中截取参考路径σR，其中，全局路径ζ由参考路径σR与剩余的原始路径σζ组成，即ζ＝σR∪σζ；
步骤2.2：当全局路径ζ未执行至最终目标点，通过欠驱动机器人搭载的三维雷达与二维激光传感器的感知数据S获取实时障碍物分布，基于移动机器人的当前状态xc检查截取的参考路径σR的可行性；
步骤2.3：若检测到障碍物，使用滚动时域规划器根据参考路径σR和感知数据S构建横向距离dspan上的局部搜索节点图G＝(V，E)，其中，dspan是V中采样节点与全局路径ζ的最大横向距离，V是节点集，E是边集，通过使用固定密度进行采样得到节点集V，每个节点与相邻八个节点连接，用来显示节点图G，执行词典搜索算法，搜索成功则输出更新后的参考路径σR，即绕障路径σG，搜索失败则输出机器人当前状态xc，若未检测到障碍物，无碰撞风险则不进行重规划；
步骤2.4：使用词典搜索算法根据节点图G、初始状态xinit、目标状态xgoal作为输入，初始化节点图中每个节点的三级非负代价值为无穷，使用迪克斯特拉Dijkstra算法执行图搜索得到最优路径时，xinit被指定为图中最接近机器人当前状态xc的状态、xgoal是节点图G在发生滚动规划时收敛到全局路径ζ的状态。
4.根据权利要求3所述的一种适用于欠驱动机器人的三维路径规划与导航方法，其特征在于，步骤2.3中所述滚动时域规划器在进行滚动规划过程中定义三种类型的代价指标，分别是风险代价、航向代价和距离代价：
其中所述风险代价沿路径σ累积，定义为第一级代价，风险代价C1(σ)公式如下所示：
Figure FDA0003420711750000021
式中，σ(s)是构型空间C中自由空间Cfree的有限长连续函数，s是区间[0，1]中任意值，σ(0)是机器人的当前状态xinit，σ(1)是机器人的目标状态xgoal，函数Risk()评估单个机器人状态下的碰撞风险，R(x)为x与距离x最近的障碍物之间距离的倒数；如果R(x)大于风险阈值Thrisk，则激活Risk()函数；
所述航向代价为第二级代价，为惩罚机器人和全局路径ζ之间的航向差异，航向代价C2(σ)公式如下所示：
Figure FDA0003420711750000022
式中，函数Heading(x)为x和全局路径ζ上与x最近路径段的航向之间的航向差；定义航向差异阈值Thhead，当误差H(x)大于Thhead时，Heading(x)返回非零，添加此代价确保生成的路径在遵守的航向的同时相对平滑；
所述距离代价为第三级代价，随着机器人行驶距离增加而增加，距离代价函数C3(σ)公式如下所示：
Figure FDA0003420711750000031
式中，函数Distance(x)是机器人行驶的距离。
5.根据权利要求3所述的一种适用于欠驱动机器人的三维路径规划与导航方法，其特征在于，步骤2.4中所述词典搜索算法在每次迭代中，最小代价计算函数FindMinCostk()会返回具有最小代价值的状态集合Xmin并与节点图G提供的节点之间共用最小的第k级代价；若Xmin包含多个节点状态，则需要检查集合中节点的较低层级代价，直到集合Xmin包含单个节点xi，并检查其相邻节点；此外，如果存在边eij，使用节点xi来降低与相邻节点xj相关联的代价；若通过节点xi从初始节点xinit节点xj的第k级代价低于当前代价xj·ck，则通过选择节点xi作为其新的父节点，来更新节点xj的代价；否则，节点xi从初始节点xinit节点xj的第k级代价与当前代价xj·ck相关联，词典搜索算法将继续评估xi在xj处的第k+1级代价；为了减少末级代价关联的可能性，假设最低层级代价K在所有路径上都是正的；最终搜索的绕障路径σG，采用共轭梯度优化方法对绕障路径σG进行平滑，得到曲率连续的平滑的最优可行路径。
6.根据权利要求1所述的一种适用于欠驱动机器人的三维路径规划与导航方法，其特征在于，所述步骤3具体包括以下步骤：
步骤3.1：根据步骤2.2中的感知数据S，先对点云数据进行降采样滤波处理，根据机器人大小选取一个Box空间内的点云，将Box空间内的点云投影到一个平面，构建一个二维栅格地图，并对地图障碍物添加膨胀层；
所述Box空间以移动机器人为中心，实时更新并滚动重建的环境信息；
步骤3.2：根据实时障碍物栅格地图构建的速度场提供的网格传播速度，采用快速行进法FMM生成目标势场，然后根据目标势场进行梯度求导生成方向梯度场；
步骤3.3：基于实时障碍物地图、目标势场和方向梯度场构造轨迹评价函数，其评价指标构建分为运动有效性评价、航向有效性评价、曲率连续性评价、朝向有效性评价，根据步骤3.2对目标位置的构建方法，将快速行进算法FMM建立的目标势场及方向梯度场提供的目标信息嵌入DWA的轨迹评价函数的评价指标中，计算FMM-DWA算法中的轨迹评价指标；
步骤3.4：采用FMM-DWA算法对速度空间(v，w)中满足最大允许曲率约束的若干组速度进行采样，其中v是线速度，w是角速度，模拟机器人在设定时间内的运动轨迹；采用轨迹评价函数选择最优轨迹，根据最优轨迹生成速度指令对机器人进行控制。
7.根据权利要求6所述的一种适用于欠驱动机器人的三维路径规划与导航方法，其特征在于，所述步骤3.2具体包括以下步骤：
步骤3.2.1：当计算到达时间T时，将障碍物所在区域的网格传播速度W(i，j)设置为0，同时，根据分段函数设置靠近障碍物区域的传播速度W(i，j)，包含栅格地图中每个点速度的速度场；计算障碍物区域的传播速度W(i，j)，公式如下所示：
Figure FDA0003420711750000041
式中，α是障碍物影响因子，d(i，j)是点(i，j)到栅格地图上最近障碍物的距离，Thd是设定阈值；
步骤3.2.2：采用快速行进法FMM按设定的传播速度扩展搜索空间，获得目标势场；
Figure FDA0003420711750000042
式中，(i，j)为由栅格表示的规划空间中第i行和第j列的栅格，T(i，j)为从起点到达点(i，j)的时间，W(i，j)是界面在点(i，j)处的局部传播速度；通过离散化梯度T(i，j)在空间中每点(i，j)求解程函方程；
简化到达点(i，j)的时间表示方式：
T1＝min(T(i-1，j)，T(i+1，j))，T2＝min(T(i，j-1)，T(i，j+1))
式中，T1是从起点至第j列的第i-1行与第i+1行栅格中的最小到达时间，T2是从起点至第i行的第j-1行与第j+1行栅格中的最小到达时间；
进而得到程函方程的解如下公式所示：
Figure FDA0003420711750000043
步骤3.2.3：对目标势场进行微分，产生方向梯度场。
8.根据权利要求6所述的一种适用于欠驱动机器人的三维路径规划与导航方法，其特征在于，所述步骤3.3具体包括以下步骤：
步骤3.3.1：构造运动有效性函数，如下式所示：
Figure FDA0003420711750000044
式中，goal_cost是轨迹有效性代价，∑T(xe，ye)是所有轨迹的终点到达时间T(xe，ye)的代数和，β是机器人航向的影响因素，θe为轨迹的端点方向，θr(xe，ye)为方向梯度场提供的参考方向；
步骤3.3.2：进行航向有效性评价，如下式所示：
Figure FDA0003420711750000051
式中，heading_cost是航向有效性代价，α是障碍物影响因子，(xe，ye)为机器人轨迹终点位置坐标，d(xs，ys)是轨迹端点与最近障碍物之间的距离，T(xs，ys)是轨迹起点的到达时间；
步骤3.3.3：进行曲率连续性评价，如下式所示；
Figure FDA0003420711750000052
式中，curvature_cost是曲率连续性代价，v是机器人的线速度，w是机器人的角速度，k是轨迹的曲率，kset[i]是所有轨迹中第i条轨迹曲率，klast是上一周期选择的最优轨迹的曲率；
步骤3.3.4：进行朝向有效性评价，对机器人采样轨迹的起始点方向与该位置对应的参考方向比较；如下式所示：
Figure FDA0003420711750000053
式中，direction_cost是朝向有效性代价，Vset[i]表示所有采样轨迹中第i条轨迹的速度，Vsum表示所有采样轨迹速度绝对值的和；θs表示机器人当前位置方向，θr(xs，ys)表示机器人当前位置对应在梯度场中的参考方向；
步骤3.3.5：使用加权goal_cost、加权heading_cost、加权curvature_cost和加权direction_cost之和total_cost，评估每个轨迹的代价，从所有采样轨迹中选择最优轨迹，计算FMM-DWA算法中的轨迹评价指标，如下式所示：
total_cost＝σ*goal_cost+λ*heading_cost+γ*curvature_cost+κ*direction_cost
式中，σ、λ、γ和κ分别是运动有效性评价、航向有效性评价、曲率相似度评价和朝向有效性评价的权重；采用运动有效性代价函数、航向有效性代价函数、曲率相似度代价函数和朝向有效性代价函数的加权和作为总体代价函数total_cost，根据总体代价函数的函数值来评价欠驱动机器人向目标点移动时轨迹生成的优劣性，并通过最小化总体代价函数判断采样轨迹中的最优轨迹。
CN202111561505.1A 2021-12-20 2021-12-20 一种适用于欠驱动机器人的三维路径规划与导航方法 Active CN114237256B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111561505.1A CN114237256B (zh) 2021-12-20 2021-12-20 一种适用于欠驱动机器人的三维路径规划与导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111561505.1A CN114237256B (zh) 2021-12-20 2021-12-20 一种适用于欠驱动机器人的三维路径规划与导航方法

Publications (2)

Publication Number Publication Date
CN114237256A true CN114237256A (zh) 2022-03-25
CN114237256B CN114237256B (zh) 2023-07-04

Family

ID=80759233

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111561505.1A Active CN114237256B (zh) 2021-12-20 2021-12-20 一种适用于欠驱动机器人的三维路径规划与导航方法

Country Status (1)

Country Link
CN (1) CN114237256B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115171235A (zh) * 2022-06-28 2022-10-11 广州土圭垚信息科技有限公司 一种蓝牙信标综合巡检方法及巡检系统
CN117369480A (zh) * 2023-12-05 2024-01-09 北京理工大学 一种室内复杂环境下轮腿机器人路径规划方法与系统

Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102501250A (zh) * 2011-10-26 2012-06-20 东北大学 一种欠驱动机械臂控制装置及控制方法
CN108897312A (zh) * 2018-05-14 2018-11-27 南开大学 多无人飞行器对大规模环境的持续监控路径规划方法
CA3076498A1 (en) * 2017-09-22 2019-03-28 Locus Robotics Corp. Dynamic window approach using optimal reciprocal collision avoidance cost-critic
CN109947119A (zh) * 2019-04-23 2019-06-28 东北大学 一种基于多传感器融合的移动机器人自主跟随系统及方法
WO2019190395A1 (en) * 2018-03-28 2019-10-03 Agency For Science, Technology And Research Method and system for returning a displaced autonomous mobile robot to its navigational path
CN111123903A (zh) * 2018-10-30 2020-05-08 武汉理工大学 一种基于圆形轨迹单元的无人艇避障方法
CN111399506A (zh) * 2020-03-13 2020-07-10 大连海事大学 一种基于动力学约束的全局-局部混合无人船路径规划方法
US20200278686A1 (en) * 2019-02-28 2020-09-03 University Of South Carolina Iterative Feedback Motion Planning
CN112304318A (zh) * 2020-11-10 2021-02-02 河北工业大学 一种虚实耦合约束环境下的机器人自主导航方法
CN112327885A (zh) * 2020-12-01 2021-02-05 大连海事大学 一种无人船自适应全局-局部混合路径规划的方法
CN112378408A (zh) * 2020-11-26 2021-02-19 重庆大学 一种实现轮式移动机器人实时避障的路径规划方法
CN112631294A (zh) * 2020-12-16 2021-04-09 上海应用技术大学 一种移动机器人智能路径规划方法
CN113050646A (zh) * 2021-03-22 2021-06-29 西安工业大学 一种用于室内移动机器人动态环境路径规划方法
CN113110424A (zh) * 2021-03-26 2021-07-13 大连海事大学 一种基于海图信息的无人艇避碰方法
CN113296514A (zh) * 2021-05-24 2021-08-24 南开大学 一种基于稀疏带状结构的局部路径优化方法及系统

Patent Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102501250A (zh) * 2011-10-26 2012-06-20 东北大学 一种欠驱动机械臂控制装置及控制方法
CA3076498A1 (en) * 2017-09-22 2019-03-28 Locus Robotics Corp. Dynamic window approach using optimal reciprocal collision avoidance cost-critic
WO2019190395A1 (en) * 2018-03-28 2019-10-03 Agency For Science, Technology And Research Method and system for returning a displaced autonomous mobile robot to its navigational path
CN108897312A (zh) * 2018-05-14 2018-11-27 南开大学 多无人飞行器对大规模环境的持续监控路径规划方法
CN111123903A (zh) * 2018-10-30 2020-05-08 武汉理工大学 一种基于圆形轨迹单元的无人艇避障方法
US20200278686A1 (en) * 2019-02-28 2020-09-03 University Of South Carolina Iterative Feedback Motion Planning
CN109947119A (zh) * 2019-04-23 2019-06-28 东北大学 一种基于多传感器融合的移动机器人自主跟随系统及方法
CN111399506A (zh) * 2020-03-13 2020-07-10 大连海事大学 一种基于动力学约束的全局-局部混合无人船路径规划方法
CN112304318A (zh) * 2020-11-10 2021-02-02 河北工业大学 一种虚实耦合约束环境下的机器人自主导航方法
CN112378408A (zh) * 2020-11-26 2021-02-19 重庆大学 一种实现轮式移动机器人实时避障的路径规划方法
CN112327885A (zh) * 2020-12-01 2021-02-05 大连海事大学 一种无人船自适应全局-局部混合路径规划的方法
CN112631294A (zh) * 2020-12-16 2021-04-09 上海应用技术大学 一种移动机器人智能路径规划方法
CN113050646A (zh) * 2021-03-22 2021-06-29 西安工业大学 一种用于室内移动机器人动态环境路径规划方法
CN113110424A (zh) * 2021-03-26 2021-07-13 大连海事大学 一种基于海图信息的无人艇避碰方法
CN113296514A (zh) * 2021-05-24 2021-08-24 南开大学 一种基于稀疏带状结构的局部路径优化方法及系统

Non-Patent Citations (7)

* Cited by examiner, † Cited by third party
Title
JIN, D., FANG, Z., & ZENG, J: "A robust autonomous following method for mobile robots in dynamic environments", 《IEEE ACCESS》, no. 8, pages 150311 - 150325, XP011806873, DOI: 10.1109/ACCESS.2020.3016472 *
MAHMOUDZADEH, S.; POWERS, D.M.W.; YAZDANI, A.M.; SAMMUT, K.; ATYABI, A: "Efficient AUV path planning in time-variant underwater environment using differential evolution algorithm", 《J. MAR. SCI. APPL》, no. 17, pages 585 *
TAHIR Z, QURESHI A H, AYAZ Y AND NAWAZ: "Potentially guided bidirectionalized RRT* for fast optimal path planning in cluttered environments", 《ROBOTICS AND AUTONOMOUS SYSTEMS》, no. 108, pages 13 *
包加桐;姚小梅;缪露;唐鸿儒;宋爱国;: "基于云平台地图的侦察机器人室外自主导航", 《控制工程》, no. 06, pages 25 - 30 *
方正;佟国峰;徐心和;: "一种鲁棒高效的移动机器人定位方法", 《自动化学报》, vol. 33, no. 01, pages 49 - 52 *
方正等: "自主导航：赋予移动机器人智能感知与运动的能力", 《智慧机器人》, pages 69 - 72 *
齐尧;徐友春;李华;王任栋;: "一种基于改进混合A~*的智能车路径规划算法", 《军事交通学院学报》, no. 08, pages 88 - 93 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115171235A (zh) * 2022-06-28 2022-10-11 广州土圭垚信息科技有限公司 一种蓝牙信标综合巡检方法及巡检系统
CN117369480A (zh) * 2023-12-05 2024-01-09 北京理工大学 一种室内复杂环境下轮腿机器人路径规划方法与系统
CN117369480B (zh) * 2023-12-05 2024-02-23 北京理工大学 一种室内复杂环境下轮腿机器人路径规划方法与系统

Also Published As

Publication number Publication date
CN114237256B (zh) 2023-07-04

Similar Documents

Publication Publication Date Title
Stahl et al. Multilayer graph-based trajectory planning for race vehicles in dynamic scenarios
US11970161B2 (en) Apparatus, method and article to facilitate motion planning of an autonomous vehicle in an environment having dynamic objects
CN113031583B (zh) 一种结构化道路避障方法
EP3623759B1 (en) A computer-implemented method and a system for defining a path for a vehicle within an environment with obstacles
Scherer et al. River mapping from a flying robot: state estimation, river detection, and obstacle mapping
CN114237256B (zh) 一种适用于欠驱动机器人的三维路径规划与导航方法
Artunedo et al. Motion planning approach considering localization uncertainty
JP2007317165A (ja) 自律移動ロボットの動作計画方法、自律移動ロボットの動作計画方法を利用した自律移動ロボットの制御方法、自律移動ロボットの動作計画装置、自律移動ロボットの動作計画プログラム及びその記録媒体、自律移動ロボットの制御プログラム
Xiong et al. Application improvement of A* algorithm in intelligent vehicle trajectory planning
CN112577506A (zh) 一种自动驾驶局部路径规划方法和系统
Mouhagir et al. A markov decision process-based approach for trajectory planning with clothoid tentacles
Hegedüs et al. Motion planning for highly automated road vehicles with a hybrid approach using nonlinear optimization and artificial neural networks
Vallon et al. Data-driven strategies for hierarchical predictive control in unknown environments
Li et al. Adaptive sampling-based motion planning with a non-conservatively defensive strategy for autonomous driving
Velagić et al. Mobile robot motion framework based on enhanced robust panel method
Kulathunga et al. Optimization-based trajectory tracking approach for multi-rotor aerial vehicles in unknown environments
Xiong et al. Surrounding vehicle trajectory prediction and dynamic speed planning for autonomous vehicle in cut-in scenarios
Park et al. Path generation algorithm based on crash point prediction for lane changing of autonomous vehicles
CN117109620A (zh) 基于采样的车辆行为与环境交互的自动驾驶路径规划方法
Kabtoul et al. Proactive and smooth maneuvering for navigation around pedestrians
Du et al. Path planning for autonomous vehicles in complicated environments
Akmandor et al. Reactive navigation framework for mobile robots by heuristically evaluated pre-sampled trajectories
CN112286211A (zh) 一种不规则布局车间的环境建模及agv路径规划方法
Wang et al. Dynamic path planning algorithm for autonomous vehicles in cluttered environments
CN111857112A (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
GR01 Patent grant
GR01 Patent grant