运动规划实战案例 | 图解基于状态晶格(State Lattice)的路径规划(附ROS C++/Python仿真)

目录

• 1 控制采样 vs 状态采样
• 2 State Lattice路径规划
• • 2.1 算法流程
  • 2.2 Lattice运动基元生成
  • 2.3 几何代价函数
  • 2.4 运动学约束启发式
  • 3 算法仿真
  • • 3.1 ROS C++仿真
    • 3.2 Python仿真

      1 控制采样 vs 状态采样

      控制采样的技术路线源自经典的运动学建模思想。这种方法将机器人的控制指令空间进行离散化，预设一组基础运动模式（如固定转向角、恒定速度等），通过前向积分生成候选路径。以差速驱动机器人为例，算法可能预设

      • 左转30度
      • 直行
      • 右转30度

        三种基础控制指令，在规划时将这些指令按不同时长组合，形成扇形展开的候选路径集，如下图(a)所示。这种方法的优势在于物理可解释性强，易于求解。但其局限性同样显著：当环境障碍复杂时，由于缺乏目标导向，规划效率较低

        

        状态采样则直接在目标状态空间（如位置、朝向）中离散化采样点，通过最优控制或数值优化反向求解连接当前状态与目标状态的可行轨迹，如上图(b)所示。例如在自动驾驶场景中，算法可能在车辆前方50米处均匀采样多个目标位姿，再通过多项式曲线或回旋曲线连接起点与终点。这种方法的优势在于解空间覆盖度高，能够生成形态多样的候选路径，特别适合结构化道路中需要精确贴合车道线的场景。但代价是计算复杂度显著增加，反向轨迹求解可能涉及大量迭代优化，实时性面临挑战。

        两种方法在工程实践中的博弈，折射出路径规划领域的核心矛盾——解空间完备性与计算效率的平衡。本节介绍的状态晶格State Lattice路径规划就属于状态空间采样类的方法，下面详细阐述

        2 State Lattice路径规划

        2.1 算法流程

        先宏观地展示算法流程

        

        其中的核心概念总结如下：

        • ​​状态晶格(State Lattice)​​ 将机器人的状态空间（位置、方向等）离散化为一系列相连的状态点，形成网格状结构
        • ​​开节点表(Open List)​​ 存储待评估探索的节点集合，按照代价排序
        • ​​闭节点表(Closed List) 存储已经评估处理过的节点集合
        • ​​节点(Node) 表示状态空间中的一个点，包含位置、方向、代价等信息
        • ​​运动基元(Motion Primitive)​​ 预定义的合法移动方式，确保路径满足动力学约束

          可以看到，State Lattice同样遵守A*算法框架，可以对比：

          • 路径规划 | 图解A*、Dijkstra、GBFS算法的异同(附C++/Python/Matlab仿真)
          • 路径规划 | 详解混合A*算法Hybrid A*(附ROS C++/Python/Matlab仿真)

            不同在于，State Lattice规划器在状态空间采样一系列节点生成运动基元，而A*或混合A*算法则是在控制空间采样。那么，状态空间运动基元是如何生成的呢？

            2.2 Lattice运动基元生成

            设机器人状态空间为

            s = [ x , y , θ ] T s=[x,y,\theta]^T s=[x,y,θ]T

            如下图所示，机器人用绿色矩形框表示，在圆周上等距离地采样一系列点作为状态采样点 [ x i , y i , θ i ] [x_i,y_i, \theta_i] [xi​,yi​,θi​]，问题转变为已知起始状态 [ x 0 , y 0 , θ 0 ] [x_0,y_0,\theta_0] [x0​,y0​,θ0​]和各个终止状态 [ x i , y i , θ i ] [x_i,y_i, \theta_i] [xi​,yi​,θi​]，如何生成一条连接两个状态的运动学可行路径的问题，即如何生成下图所示的蓝色与红色路径

            

            求解的方法有很多，例如转化为两点边值问题、最优控制优化问题等，但为了简明起见，本节介绍一种解析几何的方法。如下图所示，设首末方向向量交点为 P P P，首末端点分别为 A A A、 B B B，不失一般性假定 ∣ P A ∣ ≥ ∣ P B ∣ |PA|≥|PB| ∣PA∣≥∣PB∣，则在线段 P A PA PA上从 P P P出发截取 ∣ P B ∣ |PB| ∣PB∣长度的子线段 P C PC PC，以 B B B和 C C C为两个端点构造圆弧，产生由 A C AC AC和 C B ⏠ \overgroup{CB} CB 组成的单线段单圆弧轨迹；特别地，当 ∣ P A ∣ = ∣ P B ∣ \left| PA \right|=\left| PB \right| ∣PA∣=∣PB∣时 ∣ A C ∣ = 0 \left| AC \right|=0 ∣AC∣=0，退化为单圆弧轨迹

            

            2.3 几何代价函数

            基于上述几何关系可以定义基本代价函数

            C a d j u s t = { C b a s e    i f 直线运动 C b a s e ⋅ P n o n s t r a i g h t i f 同向转弯 C b a s e ⋅ ( P n o n s t r a i g h t + P c h a n g e ) i f 转向切换 C_{\mathrm{adjust}}=\begin{cases} C_{\mathrm{base}}\,\,& \mathrm{if} \text{直线运动}\\ C_{\mathrm{base}}\cdot P_{\mathrm{nonstraight}}& \mathrm{if} \text{同向转弯}\\ C_{\mathrm{base}}\cdot \left( P_{\mathrm{nonstraight}}+P_{\mathrm{change}} \right)& \mathrm{if} \text{转向切换}\\\end{cases} Cadjust​=⎩ ⎨ ⎧​Cbase​Cbase​⋅Pnonstraight​Cbase​⋅(Pnonstraight​+Pchange​)​if直线运动if同向转弯if转向切换​

            其中 C b a s e = L p r i m ⋅ P t r a v e l C_{\mathrm{base}}=L_{\mathrm{prim}}\cdot P_{\mathrm{travel}} Cbase​=Lprim​⋅Ptravel​， L p r i m L_{\mathrm{prim}} Lprim​是运动基元路径长度。为了考虑纯转向和反向运动，进一步修正代价函数为

            C = { P r o t a t e    i f L p r i m pose().y(), motion_table_.getAngleFromBin(current_node->pose().theta())); // Current node exists in closed list if (current_node->is_visited()) { continue; } iterations++; // 2) Mark Nbest as visited current_node->visited(); // 2.1) Use an analytic expansion (if available) to generate a path NodeLattice::NodePtr expansion_result = tryAnalyticExpansion(current_node, goal_node); if (expansion_result != nullptr) { current_node = expansion_result; } // 3) Goal found if (current_node == goal_node) { return backtracePath(current_node, path); } // 4) Expand neighbors of Nbest not visited neighbors.clear(); getNeighbors(current_node, neighbors); for (auto neighbor_iterator = neighbors.begin(); neighbor_iterator != neighbors.end(); ++neighbor_iterator) { neighbor = *neighbor_iterator; // 4.1) Compute the cost to go to this node double g_cost = current_node->accumulated_cost() + current_node->getTraversalCost(neighbor, motion_table_); // 4.2) If this is a lower cost than prior, we set this as the new cost and new approach if (g_cost accumulated_cost()) { neighbor->setAccumulatedCost(g_cost); neighbor->parent = current_node; // 4.3) Add to queue with heuristic cost addToQueue(g_cost + search_info_.lamda_h * getHeuristicCost(neighbor, goal_node), neighbor); } } } return false; }

            

            3.2 Python仿真

            核心代码如下所示

            def plan(self, start: Point3d, goal: Point3d) -> Tuple[List[Point3d], List[Dict]]:
            	"""
            	State Lattice motion plan function
            	"""
            	start_node = Point3d(start.x(), start.y(), self.motion_table.getOrientationBin(start.theta()))
            	goal_node = Point3d(goal.x(), goal.y(), self.motion_table.getOrientationBin(goal.theta()))
            	self.start = self.addToGraph(self.getIndex(start_node))
            	self.goal = self.addToGraph(self.getIndex(goal_node))
            	self.obstacle_htable = self.precomputeObstacleHeuristic(goal)
            	
            	# 0) Add starting point to the open set
            	self.queue.clear();
            	heapq.heappush(self.queue, QueueNode(0.0, self.start))
            	self.start.setAccumulatedCost(0.0)
            	
            	# main loop
            	iterations = 0
            	path, expand = [], []
            	while iterations  
             
            完整工程代码请联系下方博主名片获取