CN115167425A - 一种四足机器人的地图构建与路径规划方法 - Google Patents

Abstract

本发明公开了一种四足机器人的地图构建与路径规划方法，主要由四足机器人几何模型、地图构建器、四足机器人运动学模型、路径规划器实现。本发明基于四足机器人的几何特性，构建一种同时考虑四足机器人的位置和航向角信息的地图。本发明将待规划的位置和航向角在三维栅格地图中表示，将二维平面的导航任务转化为了三维空间搜索问题，提升了路径规划的效率；将四足机器人的几何外形通过合理简化，结合地图构建的过程，使路径规划结果不过于保守，同时保证安全性；将四足机器人的运动学模型和运动能力结合，构造独特的运动学约束，基于运动学模型和约束的A*算法，能充分利用信息并规划出平滑的路径。

Description

一种四足机器人的地图构建与路径规划方法

技术领域

本发明属于机器人领域，尤其涉及一种四足机器人的地图构建与路径规划方法。

背景技术

对移动机器人而言，自主导航是常见的任务需求。机器人需要根据传感器信息构建描述环境的地图，在地图中设定起点和终点，并在地图中进行路径规划。当环境较为复杂或机器人运动特性较独特时，环境建图和路径规划的设计会很大程度影响自主导航完成的质量。轮式机器人在移动机器人中占据较大比例，目前已有许多成熟方法。地图构建方法有占用栅格图、八叉树地图、欧式距离场图等，路径规划方法有Dijkstra、A*、D*、RRT等方法。

近年来随着腿足机器人领域相关研究的进步，四足机器人技术愈发成熟。相比于轮式机器人，四足机器人能适应更复杂的地形，具备更灵活的运动能力，因此逐渐承担起场景巡检等任务。但由于四足机器人与轮式机器人结构特性、运动模型均存在不同，用于轮式机器人的常规方法并不能很好适配于四足机器人。

目前，专利(CN108646765A)提出了一种基于改进A*算法的四足机器人路径规划方法及系统。该方法包括构建环境栅格地图，按照四足机器人中心与机体边缘的最短距离对障碍物的栅格进行初始膨胀。路径规划端利用改进A*算法确定出从初始栅格点到目标栅格点的通行代价值，判定从初始栅格点到目标栅格点的通行代价值最小时所对应的路径为最优路径。专利(CN114022824A)提出的一种面向狭窄环境的四足机器人运动规划方法。该方法主要步骤为：根据传感器获取的环境信息，构造通行性代价地图；利用外部全局规划器确定局部目标点，再利用四足机器人的质点模型在代价地图上使用A*算法规划一条粗略的最优无碰撞路径；结合四足机器人的运动动力学模型进行轨迹优化，生成有效运动轨迹。

上述方法针对四足机器人的特性作出了方法改进，但仍存在一定问题。地图构建方法存在的主要问题是在对障碍物边缘进行膨胀时，质点模型会导致最终规划结果过于保守或不够安全的问题，同时往往无法规划机器人的航向角。路径规划方法存在的主要问题是未考虑四足机器人的运动特性，即四足机器人尽管具有全向移动能力，但各向能力存在差异。

发明内容

针对当前方法地图构建端未充分考虑四足机器人的几何特性，路径规划端未有效结合运动学模型，导致自主导航任务效率低的问题，本发明提供一种四足机器人的地图构建与路径规划方法。本发明基于四足机器人的几何特性，构建一种同时考虑四足机器人的位置和航向角信息的地图；同时，结合运动特性的A*算法规划一条安全、平滑、高效的路径。

本发明的目的是通过以下技术方案来实现的：一种四足机器人的地图构建与路径规划方法，主要由四足机器人几何模型、地图构建器、四足机器人运动学模型、路径规划器实现；地图构建器接收到路径规划任务后，会结合四足机器人几何模型和三维点云，构建用于规划的三维栅格地图，并将此地图输出至路径规划器；路径规划器将基于四足机器人运动学模型，并结合运动学约束的A*搜索算法，构建同时包含四足机器人位置信息和航向角信息的全局路径，并进行输出。

进一步地，四足机器人几何模型，由一个三维空间中的凸包来等效表示，所述凸包为柱体。

进一步地，地图构建器结合四足机器人几何模型和三维点云，构建三维栅格地图，包括：

(2.1)地图构建器接收三维点云信息后，筛选在四足机器人凸包的柱体高度范围内的点云；将高度筛选后的这部分点云，再次统一高度构成二维点云，将其视为同一个平面的障碍物；

(2.2)地图构建器进行三维栅格地图构建；该三维栅格地图的x、y方向与世界坐标系方向一致，z方向用于表示四足机器人在世界坐标系中的航向角ψ；三维栅格地图中的可行区域表示，四足机器人机体坐标系原点在这些区域时，四足机器人对应的位置和航向角是安全的，这就将四足机器人的路径规划问题转为点到点的路径规划问题；对于一个障碍物，地图构建器计算不同航向角下该障碍物等效的障碍区域，再在地图的z方向进行叠加，就能得到该障碍物在机器人各航向角下所形成的障碍区域；地图构建器遍历地图中的障碍物，将所有等效障碍区域所在的栅格标为不可通行，就完成了具有位置和航向角信息的地图构建。

进一步地，步骤(2.2)包括以下步骤：

(2.2.1)地图构建器预先设定z方向的地图边界为[0,z_max]，通过z方向的任意值z₀计算对应的航向角ψ：

(2.2.2)地图构建器通过航向角ψ，计算四足机器人机体坐标系相对于世界坐标系的旋转矩阵

(2.2.3)将旋转矩阵

作用于表示机器人几何外形凸包的横截形状，就能得到横截形状按航向角ψ旋转后的俯视形状，此形状等效表示了四足机器人沿该航向角在世界坐标系中俯视外形；

(2.2.4)对于某个航向角的机器人俯视形状，地图构建器将其与障碍物形状作闵可夫斯基和，就能得到该航向角下障碍物等效的障碍区域；

(2.2.5)如公式(1)所述，栅格地图中z方向值与航向角ψ存在折算关系，因此障碍区域表示了四足机器人在此位置和航向角下，会与障碍物发生碰撞；地图构建器对地图中每个栅格进行遍历，就能得到表示全局地图中任意位置和任意航向角是否可行的三维栅格地图。

进一步地，步骤(2.2.4)包括：地图构建器获取根据凸包高度构造的二维点云，将二维点云的每一点，与步骤(2.2.3)得到的旋转后的横截形状，作闵可夫斯基和，获得新的点集合；闵可夫斯基和定义如下：

R＝{p+q|p∈P,q∈Q} (3)

其中，集合R表示作闵可夫斯基和后得到的新点集；集合P表示二维点云，元素p表示P中的某一点；集合Q表示组成当前航向角下横截形状的点集，元素q表示Q中的某一点；

地图构建器根据式(3)得到的集合R中的点的数据，按照点的x、y、z方向的值，将这些点所在的栅格标为障碍区域。

进一步地，四足机器人运动学模型，包括：

(3.1)按状态空间模型的形式，构造四足机器人的运动学模型：

其中，运动学模型包含了四足机器人状态变量S和输入变量[V_bx V_by ω]^T；状态变量S包含四足机器人在世界坐标系中x方向的位置X_w、机器人在世界坐标系中y方向的位置Y_w、机器人在世界坐标系中的航向角ψ；输入变量包含V_bx、V_by和ω，其中V_bx表示四足机器人在机体坐标系中x方向的速度，V_by表示机体坐标系中y方向的速度，ω表示机体坐标系中自转的角速度；

(3.2)根据步骤(3.1)构建的运动学模型中的输入变量，设定四足机器人的运动学约束：

-c≤ω≤c，c>0 (7)

((V_bx)²+(V_by)²)·ω²≤d，d>0 (8)

其中，a表示四足机器人在机体坐标系中x方向的最大前向速度，b表示四足机器人在机体坐标系中y方向的最大侧向速度；因此式(6)对机器人的线速度约束形成了一个椭圆；c表示四足机器人在机体坐标系中的最大自转角速度，d表示四足机器人为了不翻倒而对向心加速度的大小作出的约束。

进一步地，路径规划器，接收地图构建器构建的三维栅格地图，从起点所在的栅格规划出一条路径，到达终点所在的栅格，包括：

(4.1)将四足机器人转化为栅格中的质点，在栅格地图中任意一个可行的栅格，其A*算法的代价表示为：

f(n)＝g(n)+h(n) (9)

g(n+1)＝g(n)+cost_e

其中，f(n)是从起点栅格经由当前栅格n到达终点栅格的最小代价的估计；g(n)是从起点栅格到当前栅格n的最小累计代价；cost_e表示从栅格n到达栅格n+1所花费的代价；h(n)是从当前栅格n到终点栅格的路径的最小估计代价；

(4.2)当三维栅格地图中表示四足机器人当前位置和航向角的点，所处高度为z₀'时，计算其当时的航向角为：

(4.3)当四足机器人从一个栅格沿八个可行的方向中的任意一个方向，到达邻居栅格时，这个行进方向与航向角之间会存在偏角

其中，i表示八个行进方向；

(4.4)在最大线速度为椭圆特性的运动学约束下，计算偏角

对应行进方向最大的可行速度|v_n|为：

(4.5)用两个栅格中心点之间的距离l，除以这个最大可行速度，便得到了从当前栅格n到达栅格n+1所花费的代价cost_e；

(4.6)结合运动学约束的A*算法，在地图构建器构建出的地图中，重复步骤(4.1)～步骤(4.5)，不断更新每个栅格的f(n)，最后搜索到目标位置和航向角所在的栅格，将路径输出。

进一步地，步骤(4.1)中，选用当前栅格n到达终点栅格的直线距离除以四足机器人的最大线速度，作为h(n)。

本发明的有益效果是：本发明充分结合四足机器人的几何外形和运动学特性，从而规划出一条高效、安全、平滑的全局路径。包括以下特点：

(1)将待规划的位置和航向角在三维栅格地图中表示，将二维平面的导航任务转化为了三维空间搜索问题，提升了路径规划的效率；

(2)将四足机器人的几何外形通过合理简化，结合地图构建的过程，使路径规划结果不过于保守，同时保证安全性；

(3)将四足机器人的运动学模型和运动能力结合，构造独特的运动学约束，基于运动学模型和约束的A*算法，能充分利用信息并规划出平滑的路径。

附图说明

图1为本发明四足机器人的地图构建与路径规划方法的示意图；

图2为本发明地图构建器的工作原理示意图；

图3为本发明四足机器人运动学模型示意图；

图4为本发明四足机器人速度约束示意图；

图5为本发明计算栅格代价示意图；

图6为本发明路径规划器输出路径示意图。

具体实施方式

下面结合附图对本发明作进一步描述。

如图1所示，本发明一种四足机器人的地图构建与路径规划方法，接收四足机器人导航任务，主要通过四足机器人几何模型、四足机器人地图构建器、四足机器人运动学模型以及四足机器人路径规划器等实现。地图构建器接收到导航规划任务后，会结合四足机器人的几何模型和传感器获取的三维点云，构建用于规划的三维栅格地图，并将此地图输出至路径规划器；路径规划器将基于四足机器人运动学模型，并结合运动学约束的A*搜索算法，构建同时包含四足机器人位置信息和航向角信息的全局路径，并进行输出。

具体地，包括以下步骤：

(1)构建四足机器人几何模型，包括：

四足机器人由各种复杂机械结构组成，在导航任务中，需要将四足机器人，用一个合理的几何模型进行简化。本发明考虑用一个三维空间中的凸包，来等效表示四足机器人的几何外观。该凸包是三维空间中的一个柱体，柱体高度为四足机器人正常站立时机体最高点至地面的距离，柱体横截面的形状为八边形，是四足机器人正常站立时俯视形状的较紧凸包。从而，用一个三维空间中的凸包表示了四足机器人在地图构建时的几何外形。

(2)构造四足机器人地图构建器，利用步骤(1)中表示四足机器人几何模型的凸包，和传感器获得的点云信息，构建三维栅格地图。

(2.1)地图构建器接收三维点云信息后，首先根据表示四足机器人凸包的柱体高度，对三维点云进行数据筛选，具体为：只保留在柱体高度范围内的点云。对于二维平面的路径规划问题，在柱体高度范围内的点云都应视为障碍区域，而高于这个范围的障碍物则不会对路径规划产生影响。因此可将高度筛选后的这部分点云，再次统一高度构成二维点云，将其视为同一个平面的障碍物。

(2.2)地图构建器进行三维栅格地图构建。该三维栅格地图的x、y方向与世界坐标系方向一致，z方向用于表示四足机器人在世界坐标系中的航向角ψ。三维栅格地图中的可行区域表示，四足机器人机体坐标系原点在这些区域时，四足机器人对应的位置和航向角是安全的，这就将四足机器人的路径规划问题转为点到点的路径规划问题。如图2所示，对于一个障碍物，地图构建器计算不同航向角下该障碍物等效的障碍区域，再在地图的z方向进行叠加，就能得到该障碍物在机器人各航向角下所形成的障碍区域。地图构建器按照上述方法遍历地图中的障碍物，将所有等效障碍区域所在的栅格标为不可通行，就完成了具有位置和航向角信息的地图构建。包括以下步骤：

(2.2.1)地图构建器预先设定z方向的地图边界为[0,z_max]，通过z方向的任意值z₀计算对应的航向角ψ：

(2.2.2)地图构建器通过航向角ψ，计算四足机器人机体坐标系相对于世界坐标系的旋转矩阵

(2.2.3)将旋转矩阵

作用于表示机器人几何外形凸包的横截形状，就能得到横截形状按航向角ψ旋转后的俯视形状，此形状等效表示了四足机器人沿该航向角在世界坐标系中俯视外形。

(2.2.4)对于某个航向角的机器人俯视形状，地图构建器将其与障碍物形状作闵可夫斯基和，就能得到该航向角下障碍物等效的障碍区域。

具体地，地图构建器获取根据凸包高度构造的二维点云，将二维点云的每一点，与步骤(2.2.3)得到的旋转后的横截形状，作闵可夫斯基和，获得新的点集合。闵可夫斯基和定义如下：

R＝{p+q|p∈P,q∈Q} (3)

其中，集合R表示作闵可夫斯基和后得到的新点集；集合P表示二维点云，元素p表示P中的某一点；集合Q表示组成当前航向角下横截形状的点集，元素q表示Q中的某一点。

地图构建器根据式(3)得到的集合R中的点的数据，按照点的x、y、z方向的值，将这些点所在的栅格标为障碍区域。

(2.2.5)如公式(1)所述，栅格地图中z方向值与航向角ψ存在折算关系，因此障碍区域表示了四足机器人在此位置和航向角下，会与障碍物发生碰撞。地图构建器对地图中每个栅格进行遍历，就能得到表示全局地图中任意位置和任意航向角是否可行的三维栅格地图。

本发明将四足机器人的几何外形通过合理简化，结合进地图构建的过程，使规划结果在保证安全性的同时不过分保守。本发明创造性地将待规划的位置和航向角在三维栅格地图中表示，将二维平面的导航任务转化为了三维空间搜索问题，提升了规划的效率和质量。

(3)如图3所示，构建四足机器人运动学模型，包括：

(3.1)按状态空间模型的形式，构造四足机器人的运动学模型：

其中，运动学模型包含了四足机器人状态变量S和输入变量[V_bx V_by ω]^T。状态变量S包含四足机器人在世界坐标系中x方向的位置X_w、机器人在世界坐标系中y方向的位置Y_w、机器人在世界坐标系中的航向角ψ。输入变量包含V_bx、V_by和ω，其中V_bx表示四足机器人在机体坐标系中x方向的速度，V_by表示机体坐标系中y方向的速度，ω表示机体坐标系中自转的角速度。

(3.2)如图4所示，根据步骤(3.1)构建的运动学模型中的输入变量，设定四足机器人的运动学约束：

-c≤ω≤c，c>0 (7)

((V_bx)²+(V_by)²)·ω²≤d，d>0 (8)

其中，a表示四足机器人在机体坐标系中x方向的最大前向速度，b表示四足机器人在机体坐标系中y方向的最大侧向速度；因此式(6)对机器人的线速度约束形成了一个椭圆。c表示四足机器人在机体坐标系中的最大自转角速度，d表示四足机器人为了不翻倒而对向心加速度的大小作出的约束。

(4)构建四足机器人路径规划器，接收步骤(2)构建的三维栅格地图，从起点所在的栅格规划出一条路径，到达终点所在的栅格。包括：

(4.1)将四足机器人转化为栅格中的质点，在栅格地图中任意一个可行的栅格，其A*算法的代价表示为：

f(n)＝g(n)+h(n) (9)

其中，f(n)是从起点栅格经由当前栅格n到达终点栅格的最小代价的估计。

g(n)是从起点栅格到当前栅格n的最小累计代价。对于一个新的栅格n+1，g(n+1)可表示为g(n)+cost_e。其中，cost_e表示从栅格n到达栅格n+1所花费的代价。设定起点的代价g(0)＝0，便能在路径搜索过程中不断计算g(n)。

h(n)是从当前栅格n到终点栅格的路径的最小估计代价，h^*(n)是从当前栅格n到终点栅格的路径的最小实际代价。只要估计的代价h(n)比实际的代价h^*(n)小，就能保证A*算法的最优性。因此选用当前栅格n到达终点栅格的直线距离除以四足机器人的最大线速度，作为h(n)：

其中，P_goal表示终点栅格在三维栅格地图中的位置，P_n表示当前栅格n在栅格地图中的位置，P_goal-P_n表示两个栅格的直线距离，V_max表示机器人的最大线速度。

(4.2)当三维栅格地图中表示四足机器人当前位置和航向角的点，所处高度为z₀'时，可以计算其当时的航向角为：

(4.3)如图5所示，栅格地图的x、y方向和世界坐标系的x、y方向一致，因此当四足机器人从一个栅格沿八个可行的方向中的任意一个方向，到达邻居栅格时，这个行进方向与航向角之间会存在偏角

可以计算这个偏角

为：

其中，序号i表示了八个行进方向，0号表示世界坐标系x轴正方向，1号表示与世界坐标系x轴正方向呈45°角的方向，2号表示世界坐标系y轴正方向，3号表示与世界坐标系y轴正方向呈45°角的方向，4号表示世界坐标系x轴负方向，5号表示与世界坐标系x轴负方向呈45°角的方向，6号表示世界坐标系y轴负方向，7号表示与世界坐标系y轴负方向呈45°角的方向。

(4.4)在最大线速度为椭圆特性的运动学约束下，可以由步骤(4.3)得到的偏角

计算该偏角

对应行进方向最大的可行速度|v_n|为：

(4.5)用两个栅格中心点之间的距离l，除以这个最大可行速度，便得到了从当前栅格n到达栅格n+1所花费的代价cost_e：

cost_e＝l/|v_n| (14)。

(4.6)结合运动学约束的A*算法，在步骤(2)构建出的地图中，重复上述步骤(4.1)～步骤(4.5)的搜索流程，不断更新每个栅格的f(n)，最后搜索到目标位置和航向角所在的栅格，将路径输出，便完成了四足机器人导航任务的路径规划。图6展示了一条实施例的输出路径，路径由连接的栅格组成，栅格所在的x、y位置表示了该路径预期的机器人位置，栅格所在的z位置可通过式(1)折算为航向角。

本发明通过四足机器人的运动学模型和实际运动能力，构造了独特的运动学约束，基于运动学模型和约束的A*算法，能充分利用这些信息，在构建出的三维栅格地图中进行规划。

上述实施例用来解释说明本发明，而不是对本发明进行限制，在本发明的精神和权利要求的保护范围内，对本发明做出的任何修改和改变，都落入本发明的保护范围。

Claims (8)

1.一种四足机器人的地图构建与路径规划方法，其特征在于，主要由四足机器人几何模型、地图构建器、四足机器人运动学模型、路径规划器实现；地图构建器接收到路径规划任务后，会结合四足机器人几何模型和三维点云，构建用于规划的三维栅格地图，并将此地图输出至路径规划器；路径规划器将基于四足机器人运动学模型，并结合运动学约束的A*搜索算法，构建同时包含四足机器人位置信息和航向角信息的全局路径，并进行输出。

2.如权利要求1所述四足机器人的地图构建与路径规划方法，其特征在于，四足机器人几何模型，由一个三维空间中的凸包来等效表示，所述凸包为柱体。

3.如权利要求2所述四足机器人的地图构建与路径规划方法，其特征在于，地图构建器结合四足机器人几何模型和三维点云，构建三维栅格地图，包括：

(2.1)地图构建器接收三维点云信息后，筛选在四足机器人凸包的柱体高度范围内的点云；将高度筛选后的这部分点云，再次统一高度构成二维点云，将其视为同一个平面的障碍物；

(2.2)地图构建器进行三维栅格地图构建；该三维栅格地图的x、y方向与世界坐标系方向一致，z方向用于表示四足机器人在世界坐标系中的航向角ψ；三维栅格地图中的可行区域表示，四足机器人机体坐标系原点在这些区域时，四足机器人对应的位置和航向角是安全的，这就将四足机器人的路径规划问题转为点到点的路径规划问题；对于一个障碍物，地图构建器计算不同航向角下该障碍物等效的障碍区域，再在地图的z方向进行叠加，就能得到该障碍物在机器人各航向角下所形成的障碍区域；地图构建器遍历地图中的障碍物，将所有等效障碍区域所在的栅格标为不可通行，就完成了具有位置和航向角信息的地图构建。

4.如权利要求3所述四足机器人的地图构建与路径规划方法，其特征在于，步骤(2.2)包括以下步骤：

(2.2.1)地图构建器预先设定z方向的地图边界为[0,z_max]，通过z方向的任意值z₀计算对应的航向角ψ：

(2.2.2)地图构建器通过航向角ψ，计算四足机器人机体坐标系相对于世界坐标系的旋转矩阵

(2.2.3)将旋转矩阵

作用于表示机器人几何外形凸包的横截形状，就能得到横截形状按航向角ψ旋转后的俯视形状，此形状等效表示了四足机器人沿该航向角在世界坐标系中俯视外形；

(2.2.4)对于某个航向角的机器人俯视形状，地图构建器将其与障碍物形状作闵可夫斯基和，就能得到该航向角下障碍物等效的障碍区域；

(2.2.5)地图构建器对地图中每个栅格进行遍历，就能得到表示全局地图中任意位置和任意航向角是否可行的三维栅格地图。

5.如权利要求4所述四足机器人的地图构建与路径规划方法，其特征在于，步骤(2.2.4)包括：地图构建器获取根据凸包高度构造的二维点云，将二维点云的每一点，与步骤(2.2.3)得到的旋转后的横截形状，作闵可夫斯基和，获得新的点集合；闵可夫斯基和定义如下：

R＝{p+q|p∈P,q∈Q} (3)

其中，集合R表示作闵可夫斯基和后得到的新点集；集合P表示二维点云，元素p表示P中的某一点；集合Q表示组成当前航向角下横截形状的点集，元素q表示Q中的某一点；

地图构建器根据式(3)得到的集合R中的点的数据，按照点的x、y、z方向的值，将这些点所在的栅格标为障碍区域。

6.如权利要求1所述四足机器人的地图构建与路径规划方法，其特征在于，四足机器人运动学模型，包括：

(3.1)按状态空间模型的形式，构造四足机器人的运动学模型：

其中，运动学模型包含了四足机器人状态变量S和输入变量[V_bx V_by ω]^T；状态变量S包含四足机器人在世界坐标系中x方向的位置X_w、机器人在世界坐标系中y方向的位置Y_w、机器人在世界坐标系中的航向角ψ；输入变量包含V_bx、V_by和ω，其中V_bx表示四足机器人在机体坐标系中x方向的速度，V_by表示机体坐标系中y方向的速度，ω表示机体坐标系中自转的角速度；

(3.2)根据步骤(3.1)构建的运动学模型中的输入变量，设定四足机器人的运动学约束：

-c≤ω≤c，c>0 (7)

((V_bx)²+(V_by)²)·ω²≤d，d>0 (8)

其中，a表示四足机器人在机体坐标系中x方向的最大前向速度，b表示四足机器人在机体坐标系中y方向的最大侧向速度；因此式(6)对机器人的线速度约束形成了一个椭圆；c表示四足机器人在机体坐标系中的最大自转角速度，d表示四足机器人为了不翻倒而对向心加速度的大小作出的约束。

7.如权利要求1所述四足机器人的地图构建与路径规划方法，其特征在于，路径规划器，接收地图构建器构建的三维栅格地图，从起点所在的栅格规划出一条路径，到达终点所在的栅格，包括：

(4.1)将四足机器人转化为栅格中的质点，在栅格地图中任意一个可行的栅格，其A*算法的代价表示为：

f(n)＝g(n)+h(n) (9)

g(n+1)＝g(n)+cost_e

其中，f(n)是从起点栅格经由当前栅格n到达终点栅格的最小代价的估计；g(n)是从起点栅格到当前栅格n的最小累计代价；cost_e表示从栅格n到达栅格n+1所花费的代价；h(n)是从当前栅格n到终点栅格的路径的最小估计代价；

(4.2)当三维栅格地图中表示四足机器人当前位置和航向角的点，所处高度为z₀'时，计算其当时的航向角为：

(4.3)当四足机器人从一个栅格沿八个可行的方向中的任意一个方向，到达邻居栅格时，这个行进方向与航向角之间会存在偏角

其中，i表示八个行进方向；

(4.4)在最大线速度为椭圆特性的运动学约束下，计算偏角

对应行进方向最大的可行速度|v_n|为：

(4.5)用两个栅格中心点之间的距离l，除以这个最大可行速度，便得到了从当前栅格n到达栅格n+1所花费的代价cost_e；

(4.6)结合运动学约束的A*算法，在地图构建器构建出的地图中，重复步骤(4.1)～步骤(4.5)，不断更新每个栅格的f(n)，最后搜索到目标位置和航向角所在的栅格，将路径输出。

8.如权利要求7所述四足机器人的地图构建与路径规划方法，其特征在于，步骤(4.1)中，选用当前栅格n到达终点栅格的直线距离除以四足机器人的最大线速度，作为h(n)。

Images