前言

机械臂路径规划算法是机器人运动控制的核心技术之一，主要目的是在复杂环境中为机械臂规划出安全、高效的运动轨迹。根据算法原理的不同，主要可分为以下三类：

  • 传统路径规划算法
  • 智能路径规划算法
  • 基于采样的路径规划算法

这节笔记主要讲述传统路径规划算法有哪些，对这些算法进行基本描述与总结

一、基于图搜索的算法

通过将环境建模为图结构（如栅格、路标点）进行路径搜索，追求全局最优或次优解。

配置空间(Configuration Space)这个概念。在实际环境，也就是机器人的工作空间(Workspace)中，机器人是有形状和大小的，这不利于进行运动规划。要将工作空间转换到配置空间中，即将机器人转化为一个质点，同时将障碍物按照机器人的体积进行膨胀，如下图：

img

**1. 广度优先搜索（**Breadth First Search,BFS）

  • 原理：逐层遍历所有相邻节点，直到找到目标节点。
  • 特点：
    • 保证找到最短路径，但效率低。
    • 适用于小规模栅格地图。

为了实现波状推进搜索特性，BFS采用**队列(Queue)作为openlist的数据结构。队列是一种先进先出(FIFO)**的容器，如下图

img

过程如下，首先创建一个队列作为容器，将节点a加入队列

img

接着将节点a弹出队列，将节点a周围没有访问过的节点加入队列

img

按照上面的流程不断地弹出、扩展节点，直到找到节点i为止，完整流程如下图：

img

从终点回溯，i的父节点为f，f的父节点为e，e的父节点为a，这样就可以得到a到i的最短路径为：a->e->f->i，如下

img

显而易见，相较于DFS，BFS中使用了大量的入队、出队操作，耗时增加，但是能保证找到最优路径。

2. 深度优先搜索（Depth First Search,DFS）

  • 原理：沿分支尽可能深入搜索，回溯后继续其他分支。
  • 特点：
    • 不保证最优解，可能陷入死循环，但内存占用低。
    • 适用于路径存在性验证。

DFS能够快速地找到一条路径，是一种以时间换空间的方法。将其应用到二维地图的路径规划中，如下图，很显然找到的路径并不是移动机器人运动规划所需要的最优路径

深度优先，顾名思义即深度越大的节点会被优先扩展。在DFS中，使用**栈(Stack)**数据结构来实现上述特性。

栈是一种**后进先出(LIFO)**的容器，如下图

img

以在下面的无权图中找到从节点a到节点i的路径为例，说明一下DFS算法的工作流程

img

按照上节的图搜索算法的基本流程进行搜索，过程如下：

img

从i回溯得到路径：a->b->c->g->i，如下：

img

3. Dijkstra算法(狄克斯特拉算法)

  • 原理：从起点出发，逐步扩展到距离最短的未访问节点，更新邻节点代价。
  • 特点：
    • 全局最优，但计算量大。
    • 适用于静态环境全局规划（如仓储机器人）。

以下图为例，计算起点a到终点i的最短路径，箭头上的数值表示两个节点间的距离

img

首先扩展第一个节点，计算其余节点与第一个节点的距离，用橙色标出已经扩展的节点，未扩展的节点仍用绿色标出，其中圆中的数值表示该节点的代价函数，字母则表示该节点没有直接到达此时已扩展节点的路径。从未扩展的节点(绿色节点)中选择代价函数最小的节点进行拓展，并更新其余节点的代价函数，如下图

img

重复进行上面的步骤，直到所有节点都已扩展。

img

最后标出起点到终点的最短路径

img

将Dijkstra算法应用到二维地图路径规划中，如下图，可以看到Dijkstra算法能够得到最优路径，但是它的速度和BFS是一样的，采取的都是稳扎稳打、波状前进的方式，导致速度较慢。

4. A*算法

  • 原理：结合Dijkstra的实际代价和启发式估计，优化搜索方向。
  • 特点：
    • 启发函数需满足可采纳性（如曼哈顿距离）。
    • 效率高于Dijkstra，广泛应用于游戏和无人机路径规划。
  • 变种：
    • D*（动态A）*：支持动态环境下的增量式重规划。
    • Weighted A*：牺牲最优性以提升速度（f(n)=g(n)+w⋅h(n)f(n)=g(n)+w⋅h(n)）。

将A*算法应用到二维地图路径规划中，如下图：

5. 其他图搜索算法

  • Floyd-Warshall算法（多源最短路径–弗洛伊德）：计算所有节点对之间的最短路径，适用于多目标规划。Floyd算法又称为插点法，是一种利用动态规划的思想寻找给定的加权图中多源点之间最短路径的算法，与Dijkstra算法类似。该算法名称以创始人之一、1978年图灵奖获得者、斯坦福大学计算机科学系教授罗伯特·弗洛伊德命名。
  • Bellman-Ford算法：支持负权边的最短路径搜索，复杂度 O(n⋅m)O(n⋅m)（mm为边数）。
引用站外链接
鬼木士
路径规划 | 图搜索算法：DFS、BFS、GBFS、Dijkstra、A*

二、基于栅格的规划算法

将环境离散化为栅格单元，通过栅格状态（自由/障碍）规划路径。

1. 波前规划法（Wavefront Planning）

  • 原理：从目标点向外扩散“波前”，标记各栅格到目标的距离，反向生成路径。
  • 特点：
    • 适合全局静态环境，路径平滑但计算量大。
    • 常用于扫地机器人全覆盖路径规划。

2. 动态窗口法（Dynamic Window Approach, DWA）

  • 原理：在速度空间内采样可行轨迹，选择最优局部路径（兼顾避障和速度）。
  • 特点：适合移动机器人实时避障，但依赖高精度传感器。

DWA算法主要包括3个步骤：速度采样、轨迹预测（推算）、轨迹评价。

Python实现

完整程序请移步github仓库。
本次代码的参数配置和画图代码参考了AtsushiSakai/PythonRobotics 。

参数配置
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
import numpy as np
import matplotlib.pyplot as plt
import copy
from celluloid import Camera  # 保存动图时用，pip install celluloid
import math

class Config:
    """
    simulation parameter class
    """
def __init__(self):
    # robot parameter
    # 线速度边界
    self.v_max = 1.0  # [m/s]
    self.v_min = -0.5  # [m/s]
    # 角速度边界
    self.w_max = 40.0 * math.pi / 180.0  # [rad/s]
    self.w_min = -40.0 * math.pi / 180.0  # [rad/s]
    # 线加速度和角加速度最大值
    self.a_vmax = 0.2  # [m/ss]
    self.a_wmax = 40.0 * math.pi / 180.0  # [rad/ss]
    # 采样分辨率 
    self.v_sample = 0.01  # [m/s]
    self.w_sample = 0.1 * math.pi / 180.0  # [rad/s]
    # 离散时间间隔
    self.dt = 0.1  # [s] Time tick for motion prediction
    # 轨迹推算时间长度
    self.predict_time = 3.0  # [s]
    # 轨迹评价函数系数
    self.alpha = 0.15
    self.beta = 1.0
    self.gamma = 1.0

    # Also used to check if goal is reached in both types
    self.robot_radius = 1.0  # [m] for collision check
    
    self.judge_distance = 10 # 若与障碍物的最小距离大于阈值（例如这里设置的阈值为robot_radius+0.2）,则设为一个较大的常值

    # 障碍物位置 [x(m) y(m), ....]
    self.ob = np.array([[-1, -1],
                [0, 2],
                [4.0, 2.0],
                [5.0, 4.0],
                [5.0, 5.0],
                [5.0, 6.0],
                [5.0, 9.0],
                [8.0, 9.0],
                [7.0, 9.0],
                [8.0, 10.0],
                [9.0, 11.0],
                [12.0, 13.0],
                [12.0, 12.0],
                [15.0, 15.0],
                [13.0, 13.0]
                ])
    # 目标点位置
    self.target = np.array([10,10])

值得注意的是，这边障碍物只给了位置，并没有给定大小，因为这边相当于把障碍物的大小合并到了机器人本体大小上，也即代码中的robot_radius上。

机器人运动学模型

主要用于轨迹推算。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
def KinematicModel(state,control,dt):
  """机器人运动学模型

  Args:
      state (_type_): 状态量---x,y,yaw,v,w
      control (_type_): 控制量---v,w,线速度和角速度
      dt (_type_): 离散时间

  Returns:
      _type_: 下一步的状态
  """
  state[0] += control[0] * math.cos(state[2]) * dt
  state[1] += control[0] * math.sin(state[2]) * dt
  state[2] += control[1] * dt
  state[3] = control[0]
  state[4] = control[1]

  return state

在这里顺便保存了线速度和角速度作为状态分量，便于后面轨迹计算。

DWA算法类实现
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
class DWA:
    def __init__(self,config) -> None:
        """初始化

        Args:
            config (_type_): 参数类
        """
        self.dt=config.dt
        self.v_min=config.v_min
        self.w_min=config.w_min
        self.v_max=config.v_max
        self.w_max=config.w_max
        self.predict_time = config.predict_time
        self.a_vmax = config.a_vmax
        self.a_wmax = config.a_wmax
        self.v_sample = config.v_sample # 线速度采样分辨率
        self.w_sample = config.w_sample # 角速度采样分辨率
        self.alpha = config.alpha
        self.beta = config.beta
        self.gamma = config.gamma
        self.radius = config.robot_radius
        self.judge_distance = config.judge_distance

    def dwa_control(self,state,goal,obstacle):
        """滚动窗口算法入口

        Args:
            state (_type_): 机器人当前状态--[x,y,yaw,v,w]
            goal (_type_): 目标点位置，[x,y]

            obstacle (_type_): 障碍物位置，dim:[num_ob,2]

        Returns:
            _type_: 控制量、轨迹（便于绘画）
        """
        control,trajectory = self.trajectory_evaluation(state,goal,obstacle)
        return control,trajectory


    def cal_dynamic_window_vel(self,v,w,state,obstacle):
        """速度采样,得到速度空间窗口

        Args:
            v (_type_): 当前时刻线速度
            w (_type_): 当前时刻角速度
            state (_type_): 当前机器人状态
            obstacle (_type_): 障碍物位置
        Returns:
            [v_low,v_high,w_low,w_high]: 最终采样后的速度空间
        """
        Vm = self.__cal_vel_limit()
        Vd = self.__cal_accel_limit(v,w)
        Va = self.__cal_obstacle_limit(state,obstacle)
        a = max([Vm[0],Vd[0],Va[0]])
        b = min([Vm[1],Vd[1],Va[1]])
        c = max([Vm[2], Vd[2],Va[2]])
        d = min([Vm[3], Vd[3],Va[3]])
        return [a,b,c,d]

    def __cal_vel_limit(self):
        """计算速度边界限制Vm

        Returns:
            _type_: 速度边界限制后的速度空间Vm
        """
        return [self.v_min,self.v_max,self.w_min,self.w_max]
    
    def __cal_accel_limit(self,v,w):
        """计算加速度限制Vd

        Args:
            v (_type_): 当前时刻线速度
            w (_type_): 当前时刻角速度
        Returns: 
            _type_:考虑加速度时的速度空间Vd
        """
        v_low = v-self.a_vmax*self.dt
        v_high = v+self.a_vmax*self.dt
        w_low = w-self.a_wmax*self.dt
        w_high = w+self.a_wmax*self.dt
        return [v_low, v_high,w_low, w_high]
    
    def __cal_obstacle_limit(self,state,obstacle):
        """环境障碍物限制Va

        Args:
            state (_type_): 当前机器人状态
            obstacle (_type_): 障碍物位置

        Returns:
            _type_: 某一时刻移动机器人不与周围障碍物发生碰撞的速度空间Va
        """
        v_low=self.v_min
        v_high = np.sqrt(2*self._dist(state,obstacle)*self.a_vmax)
        w_low =self.w_min
        w_high = np.sqrt(2*self._dist(state,obstacle)*self.a_wmax)
        return [v_low,v_high,w_low,w_high]

    def trajectory_predict(self,state_init, v,w):
        """轨迹推算

        Args:
            state_init (_type_): 当前状态---x,y,yaw,v,w
            v (_type_): 当前时刻线速度
            w (_type_): 当前时刻线速度

        Returns:
            _type_: _description_
        """
        state = np.array(state_init)
        trajectory = state
        time = 0
        # 在预测时间段内
        while time <= self.predict_time:
            x = KinematicModel(state, [v,w], self.dt) # 运动学模型
            trajectory = np.vstack((trajectory, x))
            time += self.dt

        return trajectory

    def trajectory_evaluation(self,state,goal,obstacle):
        """轨迹评价函数,评价越高，轨迹越优

        Args:
            state (_type_): 当前状态---x,y,yaw,v,w
            dynamic_window_vel (_type_): 采样的速度空间窗口---[v_low,v_high,w_low,w_high]
            goal (_type_): 目标点位置，[x,y]
            obstacle (_type_): 障碍物位置，dim:[num_ob,2]

        Returns:
            _type_: 最优控制量、最优轨迹
        """
        G_max = -float('inf') # 最优评价
        trajectory_opt = state # 最优轨迹
        control_opt = [0.,0.] # 最优控制
        dynamic_window_vel = self.cal_dynamic_window_vel(state[3], state[4],state,obstacle) # 第1步--计算速度空间
        
        # sum_heading,sum_dist,sum_vel = 0,0,0 # 统计全部采样轨迹的各个评价之和，便于评价的归一化
        # # 在本次实验中，不进行归一化也可实现该有的效果。
        # for v in np.arange(dynamic_window_vel[0],dynamic_window_vel[1],self.v_sample):
        #     for w in np.arange(dynamic_window_vel[2], dynamic_window_vel[3], self.w_sample):   
        #         trajectory = self.trajectory_predict(state, v, w)  

        #         heading_eval = self.alpha*self.__heading(trajectory,goal)
        #         dist_eval = self.beta*self.__dist(trajectory,obstacle)
        #         vel_eval = self.gamma*self.__velocity(trajectory)
        #         sum_vel+=vel_eval
        #         sum_dist+=dist_eval
        #         sum_heading +=heading_eval

        sum_heading,sum_dist,sum_vel = 1,1,1 # 不进行归一化
        # 在速度空间中按照预先设定的分辨率采样
        for v in np.arange(dynamic_window_vel[0],dynamic_window_vel[1],self.v_sample):
            for w in np.arange(dynamic_window_vel[2], dynamic_window_vel[3], self.w_sample):

                trajectory = self.trajectory_predict(state, v, w)  # 第2步--轨迹推算

                heading_eval = self.alpha*self.__heading(trajectory,goal)/sum_heading
                dist_eval = self.beta*self.__dist(trajectory,obstacle)/sum_dist
                vel_eval = self.gamma*self.__velocity(trajectory)/sum_vel
                G = heading_eval+dist_eval+vel_eval # 第3步--轨迹评价

                if G_max<=G:
                    G_max = G
                    trajectory_opt = trajectory
                    control_opt = [v,w]

        return control_opt, trajectory_opt

    def _dist(self,state,obstacle):
        """计算当前移动机器人距离障碍物最近的几何距离

        Args:
            state (_type_): 当前机器人状态
            obstacle (_type_): 障碍物位置

        Returns:
            _type_: 移动机器人距离障碍物最近的几何距离
        """
        ox = obstacle[:, 0]
        oy = obstacle[:, 1]
        dx = state[0,None] - ox[:, None]
        dy = state[1,None] - oy[:, None]
        r = np.hypot(dx, dy)
        return np.min(r)

    def __dist(self,trajectory,obstacle):
        """距离评价函数
        表示当前速度下对应模拟轨迹与障碍物之间的最近距离；
        如果没有障碍物或者最近距离大于设定的阈值，那么就将其值设为一个较大的常数值。
        Args:
            trajectory (_type_): 轨迹，dim:[n,5]
            
            obstacle (_type_): 障碍物位置，dim:[num_ob,2]

        Returns:
            _type_: _description_
        """
        ox = obstacle[:, 0]
        oy = obstacle[:, 1]
        dx = trajectory[:, 0] - ox[:, None]
        dy = trajectory[:, 1] - oy[:, None]
        r = np.hypot(dx, dy)
        return np.min(r) if np.array(r <self.radius+0.2).any() else self.judge_distance

    def __heading(self,trajectory, goal):
        """方位角评价函数
        评估在当前采样速度下产生的轨迹终点位置方向与目标点连线的夹角的误差

        Args:
            trajectory (_type_): 轨迹，dim:[n,5]
            goal (_type_): 目标点位置[x,y]

        Returns:
            _type_: 方位角评价数值
        """
        dx = goal[0] - trajectory[-1, 0]
        dy = goal[1] - trajectory[-1, 1]
        error_angle = math.atan2(dy, dx)
        cost_angle = error_angle - trajectory[-1, 2]
        cost = math.pi-abs(cost_angle)

        return cost

    def __velocity(self,trajectory):
        """速度评价函数， 表示当前的速度大小，可以用模拟轨迹末端位置的线速度的大小来表示

        Args:
            trajectory (_type_): 轨迹，dim:[n,5]

        Returns:
            _type_: 速度评价
        """
        return trajectory[-1,3]


从后面的实验效果上看，这边没有进行归一化也是可以的。

画图

主要用于机器人和方向箭头的绘制。

1
2
3
4
5
6
7
8
9
10
11
12
def plot_arrow(x, y, yaw, length=0.5, width=0.1):  # pragma: no cover
    plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
              head_length=width, head_width=width)
    plt.plot(x, y)


def plot_robot(x, y, yaw, config):  # pragma: no cover
        circle = plt.Circle((x, y), config.robot_radius, color="b")
        plt.gcf().gca().add_artist(circle)
        out_x, out_y = (np.array([x, y]) +
                        np.array([np.cos(yaw), np.sin(yaw)]) * config.robot_radius)
        plt.plot([x, out_x], [y, out_y], "-k")
主函数
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
def main(config):
    # initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
    x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0])
    # goal position [x(m), y(m)]
    goal = config.target

    # input [forward speed, yaw_rate]

    trajectory = np.array(x)
    ob = config.ob
    dwa = DWA(config)
    fig=plt.figure(1)
    camera = Camera(fig)
    while True:
        u, predicted_trajectory = dwa.dwa_control(x,goal, ob)

        x = KinematicModel(x, u, config.dt)  # simulate robot
        trajectory = np.vstack((trajectory, x))  # store state history
        plt.cla()
        # for stopping simulation with the esc key.
        plt.gcf().canvas.mpl_connect(
            'key_release_event',
            lambda event: [exit(0) if event.key == 'escape' else None])
        plt.plot(predicted_trajectory[:, 0], predicted_trajectory[:, 1], "-g")
        plt.plot(x[0], x[1], "xr")
        plt.plot(goal[0], goal[1], "xb")
        plt.plot(ob[:, 0], ob[:, 1], "ok")
        plot_robot(x[0], x[1], x[2], config)
        plot_arrow(x[0], x[1], x[2])
        plt.axis("equal")
        plt.grid(True)
        plt.pause(0.001)

        # check reaching goal
        dist_to_goal = math.hypot(x[0] - goal[0], x[1] - goal[1])
        if dist_to_goal <= config.robot_radius:
            print("Goal!!")
            break
        # camera.snap()
        # print(x)
        # print(u)

    print("Done")
    plt.plot(trajectory[:, 0], trajectory[:, 1], "-r")
    plt.pause(0.001)
    # camera.snap()
    # animation = camera.animate()
    # animation.save('trajectory.gif')
    plt.show()


main(Config())

运行主函数，即可得到下图所示的效果：

在这里插入图片描述

引用站外链接
CHH3213
局部路径规划算法——DWA算法

三、人工势场法（Artificial Potential Field, APF）

  • 原理：目标点产生引力，障碍物产生斥力，合力引导机器人运动。
  • 改进方法：
    • 虚拟力场法：引入旋转力解决局部极小问题。
    • 势场+随机扰动：通过随机扰动逃离局部极小（如U型陷阱）。
  • 应用：机械臂末端实时避障、无人机动态路径调整。

四、基于动态规划的算法

通过递推公式求解多阶段决策问题，适用于离散状态空间。

1. 值迭代（Value Iteration）

  • 原理：迭代更新各状态的最优值函数，最终回溯路径。
  • 特点：
    • 适合马尔可夫决策过程（MDP），但计算复杂度高。

2. 策略迭代（Policy Iteration）

  • 原理：交替优化策略和值函数，收敛速度快于值迭代。

五、基于几何的规划算法

利用几何学原理直接生成路径，适用于结构化环境。

1. 可见图法（Visibility Graph）

  • 原理：连接障碍物顶点与起点/目标点，构建可见边网络，搜索最短路径。
  • 特点：
    • 路径为折线段，全局最优，但计算复杂度高（O(n3)O(n3)）。
    • 适用于多边形障碍物环境（如室内导航）。

六、传统算法的改进与混合方法

1. Hybrid A*

  • 原理：结合连续状态空间和离散搜索，解决车辆运动学约束问题。
  • 应用：自动驾驶汽车的路径规划。

2. APF + 图搜索

  • 原理：先用A*生成全局路径，再用APF实时避障。
  • 优势：兼顾全局最优性和动态适应性。

3. D* Lite

  • 原理：增量式重规划算法，高效处理动态环境变化。
  • 应用：火星探测车、战场机器人