机械臂路径规划：全局与局部的艺术与实践
在机器人技术领域，机械臂的应用日益广泛，从工业制造到医疗手术，再到家庭服务，它们的身影无处不在。而机械臂能否高效、安全地完成任务，很大程度上取决于其路径规划能力。路径规划的目标是为机械臂找到一条从起始点到目标点的无碰撞、最优（或次优）路径。
机械臂的路径规划算法通常分为两大类：全局路径规划和局部路径规划。
1. 全局路径规划
全局路径规划算法在已知环境中进行，需要掌握环境的完整信息（如障碍物的位置、大小、形状等）。全局规划算法的目标是找到一条从起点到终点的全局最优路径。
1.1 A* 算法
A* (A-star) 算法是一种广泛使用的启发式搜索算法，它在图形中寻找最短路径。A* 算法维护两个列表：OPEN 列表（待探索的节点）和 CLOSED 列表（已探索的节点）。
算法步骤：
- 将起始节点加入 OPEN 列表，并计算其代价函数 f(n) = g(n) + h(n)。
- g(n)：从起始节点到当前节点 n 的实际代价。
- h(n)：从当前节点 n 到目标节点的估计代价（启发式函数）。
- 从 OPEN 列表中选择 f(n) 值最小的节点 n。
- 如果节点 n 是目标节点，则找到路径，算法结束。
- 将节点 n 从 OPEN 列表移至 CLOSED 列表。
- 扩展节点 n 的所有邻居节点：
- 如果邻居节点不在 OPEN 列表或 CLOSED 列表中，计算其 f(n) 值，并将其加入 OPEN 列表，设置其父节点为 n。
- 如果邻居节点已在 OPEN 列表或 CLOSED 列表中，比较其新的 f(n) 值与旧的 f(n) 值，如果新的 f(n) 值更小，则更新其 f(n) 值和父节点。
- 重复步骤 2-5，直到找到目标节点或 OPEN 列表为空。
Python 代码示例 (简化版，二维平面)：
import heapq
class Node:
def __init__(self, x, y, g=0, h=0, parent=None):
self.x = x
self.y = y
self.g = g
self.h = h
self.f = g + h
self.parent = parent
def __lt__(self, other):
return self.f < other.f
def astar(start, goal, grid):
"""
A* 算法实现
:param start: 起始点坐标 (x, y)
:param goal: 目标点坐标 (x, y)
:param grid: 二维网格，0 表示可通过，1 表示障碍物
:return: 路径列表（如果找到），否则返回 None
"""
open_list = []
closed_list = set()
start_node = Node(start[0], start[1], 0, heuristic(start, goal))
heapq.heappush(open_list, start_node)
while open_list:
current_node = heapq.heappop(open_list)
if (current_node.x, current_node.y) == goal:
return reconstruct_path(current_node)
closed_list.add((current_node.x, current_node.y))
neighbors = get_neighbors(current_node, grid)
for neighbor_x, neighbor_y in neighbors:
if (neighbor_x, neighbor_y) in closed_list or grid[neighbor_y][neighbor_x] == 1:
continue
new_g = current_node.g + 1 # 假设移动代价为 1
neighbor_node = find_node(open_list, neighbor_x, neighbor_y)
if neighbor_node is None:
neighbor_node = Node(neighbor_x, neighbor_y, new_g, heuristic((neighbor_x, neighbor_y), goal), current_node)
heapq.heappush(open_list, neighbor_node)
elif new_g < neighbor_node.g:
neighbor_node.g = new_g
neighbor_node.h = heuristic((neighbor_x, neighbor_y), goal)
neighbor_node.f = neighbor_node.g + neighbor_node.h
neighbor_node.parent = current_node
heapq.heapify(open_list) # 重新堆化
return None
def heuristic(a, b):
"""
计算两点之间的曼哈顿距离
"""
return abs(a[0] - b[0]) + abs(a[1] - b[1])
def get_neighbors(node, grid):
"""
获取当前节点的邻居节点
"""
neighbors = []
for dx, dy in [(0, 1), (1, 0), (0, -1), (-1, 0)]: # 上下左右四个方向
x, y = node.x + dx, node.y + dy
if 0 <= x < len(grid[0]) and 0 <= y < len(grid):
neighbors.append((x, y))
return neighbors
def find_node(open_list, x, y):
"""
在open_list中查找指定坐标的节点
"""
for node in open_list:
if node.x == x and node.y == y:
return node
return None
def reconstruct_path(node):
"""
从目标节点回溯到起始节点，构建路径
"""
path = []
while node:
path.append((node.x, node.y))
node = node.parent
return path[::-1] # 反转路径
# 示例用法
grid = [
[0, 0, 0, 0, 0],
[0, 1, 1, 1, 0],
[0, 0, 0, 0, 0],
[0, 1, 0, 1, 0],
[0, 0, 0, 0, 0],
]
start = (0, 0)
goal = (4, 4)
path = astar(start, goal, grid)
if path:
print("找到路径:", path)
else:
print("未找到路径")
代码解释：
Node类：表示搜索中的节点，包含位置、代价、父节点等信息。astar函数：A* 算法的主函数。heuristic函数：计算启发式代价（这里使用曼哈顿距离）。get_neighbors函数：获取当前节点的合法邻居节点。reconstruct_path函数：从目标节点回溯，构建路径。
1.2 RRT 算法
快速探索随机树（RRT）算法是一种基于采样的路径规划算法，它通过在配置空间中随机采样点来构建一棵搜索树。
算法步骤：
- 将起始点作为树的根节点。
- 在配置空间中随机采样一个点 q_rand。
- 在树中找到距离 q_rand 最近的节点 q_near。
- 从 q_near 向 q_rand 方向扩展一个步长，得到新节点 q_new。
- 检查 q_new 是否与障碍物碰撞：
- 如果无碰撞，将 q_new 加入树中，并将其父节点设置为 q_near。
- 如果有碰撞，放弃 q_new。
- 检查 q_new 是否接近目标点：
- 如果接近，尝试将目标点连接到树中。
- 如果不接近，继续采样。
- 重复步骤 2-6，直到找到路径或达到最大迭代次数。
Python 代码示例 (简化版，二维平面)：
import random
import math
class Node:
def __init__(self, x, y, parent=None):
self.x = x
self.y = y
self.parent = parent
def rrt(start, goal, obstacle_list, x_range, y_range, expand_dis=1.0, goal_sample_rate=5, max_iter=500):
"""
RRT 算法实现
:param start: 起始点坐标 (x, y)
:param goal: 目标点坐标 (x, y)
:param obstacle_list: 障碍物列表，每个障碍物为 (x, y, radius)
:param x_range: x 坐标范围 (min, max)
:param y_range: y 坐标范围 (min, max)
:param expand_dis: 扩展步长
:param goal_sample_rate: 采样到目标点的概率（百分比）
:param max_iter: 最大迭代次数
:return: 路径列表（如果找到），否则返回 None
"""
node_list = [Node(start[0], start[1])]
for i in range(max_iter):
if random.randint(0, 100) > goal_sample_rate:
rnd_x = random.uniform(x_range[0], x_range[1])
rnd_y = random.uniform(y_range[0], y_range[1])
rnd_node = Node(rnd_x, rnd_y)
else:
rnd_node = Node(goal[0], goal[1])
nearest_ind = get_nearest_node_index(node_list, rnd_node)
nearest_node = node_list[nearest_ind]
new_node = steer(nearest_node, rnd_node, expand_dis)
if is_collision_free(new_node, obstacle_list):
node_list.append(new_node)
if is_near_goal(new_node, goal, expand_dis):
final_node = steer(new_node, Node(goal[0], goal[1]), expand_dis)
if is_collision_free(final_node, obstacle_list):
return generate_final_course(final_node)
return None
def get_nearest_node_index(node_list, rnd_node):
"""
获取距离随机节点最近的树节点的索引
"""
dlist = [(node.x - rnd_node.x) ** 2 + (node.y - rnd_node.y) ** 2 for node in node_list]
minind = dlist.index(min(dlist))
return minind
def steer(from_node, to_node, extend_length=float("inf")):
"""
从 from_node 向 to_node 扩展
"""
new_node = Node(from_node.x, from_node.y, parent=from_node)
d, theta = calculate_distance_and_angle(new_node, to_node)
if extend_length > d:
extend_length = d
new_node.x += extend_length * math.cos(theta)
new_node.y += extend_length * math.sin(theta)
return new_node
def calculate_distance_and_angle(from_node, to_node):
"""
计算两点之间的距离和角度
"""
dx = to_node.x - from_node.x
dy = to_node.y - from_node.y
d = math.sqrt(dx ** 2 + dy ** 2)
theta = math.atan2(dy, dx)
return d, theta
def is_collision_free(node, obstacle_list):
"""
检查节点是否与障碍物碰撞
"""
for (ox, oy, size) in obstacle_list:
dx = ox - node.x
dy = oy - node.y
d = math.sqrt(dx ** 2 + dy ** 2)
if d <= size:
return False # 碰撞
return True # 无碰撞
def is_near_goal(node, goal, expand_dis):
"""
检查节点是否接近目标点
"""
d, _ = calculate_distance_and_angle(node, Node(goal[0], goal[1]))
return d <= expand_dis
def generate_final_course(node):
"""
从目标节点回溯到起始节点，构建路径
"""
path = [[node.x, node.y]]
while node.parent is not None:
node = node.parent
path.append([node.x, node.y])
return path[::-1]
# 示例用法
start = (0, 0)
goal = (10, 10)
obstacle_list = [
(3, 3, 1.5),
(7, 7, 2),
(5, 8, 1)
]
x_range = (-2, 12)
y_range = (-2, 12)
path = rrt(start, goal, obstacle_list, x_range, y_range)
if path:
print("找到路径:", path)
else:
print("未找到路径")
代码解释：
Node类：表示树中的节点，包含位置和父节点信息。rrt函数：RRT 算法的主函数。get_nearest_node_index函数：找到树中距离采样点最近的节点。steer函数：从一个节点向另一个节点扩展。calculate_distance_and_angle函数：计算两点间的距离和角度。is_collision_free函数：检查节点是否与障碍物碰撞。is_near_goal函数：检查节点是否接近目标点。generate_final_course函数：从目标节点回溯，构建路径。
1.3 其他全局路径规划算法
除了 A* 和 RRT，还有许多其他的全局路径规划算法，例如：
- Dijkstra 算法： 一种基于图形的最短路径算法，适用于非负权重的图。
- PRM (Probabilistic Roadmap) 算法： 一种基于采样的算法，构建一个无碰撞路径的概率路线图。
- 人工势场法： 将目标点设置为吸引势，障碍物设置为排斥势，机械臂在势场力的作用下移动。
2. 局部路径规划
局部路径规划算法通常用于处理动态环境或未知环境。它只关注机械臂周围的局部环境信息，根据传感器实时获取的数据进行路径规划。
2.1 DWA (Dynamic Window Approach) 算法
动态窗口法（DWA）是一种常用的局部路径规划算法，它在速度空间中进行采样，并根据一系列评价函数选择最优速度。
算法步骤：
- 速度采样： 在机器人的速度空间（v, ω）中进行采样，其中 v 是线速度，ω 是角速度。由于机器人加速度的限制，采样窗口是动态的。
- 轨迹预测： 根据采样的速度，预测机器人在一段时间内的运动轨迹。
- 轨迹评价： 根据一系列评价函数对预测的轨迹进行评估，选择最优轨迹。评价函数通常包括：
- 目标导向： 轨迹与目标点的接近程度。
- 障碍物距离： 轨迹与障碍物的距离。
- 速度： 轨迹的速度大小。
- 执行最优速度： 执行最优轨迹对应的速度。
Python 代码示例 (简化版)：
import math
import numpy as np
class Config:
"""
DWA 算法的配置参数
"""
def __init__(self):
# 机器人参数
self.max_speed = 1.0 # 最大线速度 [m/s]
self.min_speed = -0.5 # 最小线速度 [m/s]
self.max_yawrate = 40.0 * math.pi / 180.0 # 最大角速度 [rad/s]
self.max_accel = 0.2 # 最大线加速度 [m/ss]
self.max_dyawrate = 40.0 * math.pi / 180.0 # 最大角加速度 [rad/ss]
self.v_reso = 0.01 # 线速度分辨率 [m/s]
self.yawrate_reso = 0.1 * math.pi / 180.0 # 角速度分辨率 [rad/s]
self.dt = 0.1 # 时间间隔 [s]
self.predict_time = 3.0 # 预测时间 [s]
self.to_goal_cost_gain = 1.0 # 目标导向代价增益
self.speed_cost_gain = 0.1 # 速度代价增益
self.obstacle_cost_gain = 1.0 # 障碍物代价增益
self.robot_radius = 0.5 # 机器人半径 [m]
def motion(x, u, dt):
"""
运动模型
:param x: 机器人状态 [x, y, yaw, v, w]
:param u: 控制输入 [v, w]
:param dt: 时间间隔
:return: 新的机器人状态
"""
x[0] += u[0] * math.cos(x[2]) * dt
x[1] += u[0] * math.sin(x[2]) * dt
x[2] += u[1] * dt
x[3] = u[0]
x[4] = u[1]
return x
def calc_dynamic_window(x, config):
"""
计算动态窗口
:param x: 机器人当前状态
:param config: 配置参数
:return: 动态窗口 [v_min, v_max, w_min, w_max]
"""
# 机器人能够达到的最大最小速度
vs = [config.min_speed, config.max_speed,
-config.max_yawrate, config.max_yawrate]
# 一个时间间隔内，机器人能够改变的最大最小速度
vd = [x[3] - config.max_accel * config.dt,
x[3] + config.max_accel * config.dt,
x[4] - config.max_dyawrate * config.dt,
x[4] + config.max_dyawrate * config.dt]
# 最终的动态窗口
dw = [max(vs[0], vd[0]), min(vs[1], vd[1]),
max(vs[2], vd[2]), min(vs[3], vd[3])]
return dw
def calc_trajectory(x_init, v, w, config):
"""
轨迹推算
:param x_init: 机器人初始状态
:param v: 线速度
:param w: 角速度
:param config: 配置参数
:return: 轨迹列表
"""
x = np.array(x_init)
trajectory = [x]
time = 0
while time <= config.predict_time:
x = motion(x, [v, w], config.dt)
trajectory.append(x)
time += config.dt
return np.array(trajectory)
def calc_to_goal_cost(trajectory, goal):
"""
计算目标导向代价
:param trajectory: 轨迹
:param goal: 目标点
:return: 目标导向代价
"""
dx = goal[0] - trajectory[-1, 0]
dy = goal[1] - trajectory[-1, 1]
dist = math.sqrt(dx ** 2 + dy ** 2)
return dist
def calc_obstacle_cost(trajectory, ob, config):
"""
计算障碍物代价
:param trajectory: 轨迹
:param ob: 障碍物列表
:param config: 配置参数
:return: 障碍物代价
"""
min_dist = float("inf")
for i in range(len(trajectory)):
for j in range(len(ob)):
ox = ob[j][0]
oy = ob[j][1]
dx = trajectory[i, 0] - ox
dy = trajectory[i, 1] - oy
dist = math.sqrt(dx ** 2 + dy ** 2)
if dist <= config.robot_radius:
return float("inf") # 碰撞
if dist < min_dist:
min_dist = dist
return 1.0 / min_dist
def calc_final_input(x, u, dw, config, goal, ob):
"""
计算最终输入
:param x: 机器人当前状态
:param u: 当前控制输入
:param dw: 动态窗口
:param config: 配置参数
:param goal: 目标点
:param ob: 障碍物列表
:return: 最佳控制输入和对应的轨迹
"""
x_init = x[:]
min_cost = float("inf")
best_u = [0.0, 0.0]
best_trajectory = np.array([x])
# 遍历速度空间
for v in np.arange(dw[0], dw[1], config.v_reso):
for w in np.arange(dw[2], dw[3], config.yawrate_reso):
trajectory = calc_trajectory(x_init, v, w, config)
# 计算各项代价
to_goal_cost = config.to_goal_cost_gain * calc_to_goal_cost(trajectory, goal)
speed_cost = config.speed_cost_gain * (config.max_speed - trajectory[-1, 3])
ob_cost = config.obstacle_cost_gain * calc_obstacle_cost(trajectory, ob, config)
final_cost = to_goal_cost + speed_cost + ob_cost
# 更新最优解
if final_cost < min_cost:
min_cost = final_cost
best_u = [v, w]
best_trajectory = trajectory
return best_u, best_trajectory
def dwa_control(x, config, goal, ob):
"""
DWA 控制
:param x: 机器人当前状态
:param config: 配置参数
:param goal: 目标点
:param ob: 障碍物列表
:return: 控制输入和预测轨迹
"""
dw = calc_dynamic_window(x, config)
u, trajectory = calc_final_input(x, [0.0, 0.0], dw, config, goal, ob)
return u, trajectory
# 示例用法
# 机器人初始状态 [x, y, yaw, v, w]
x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0])
# 目标点
goal = np.array([10.0, 10.0])
# 障碍物列表
ob = np.array([[5.0, 5.0],
[3.0, 6.0],
[7.0, 2.0]])
config = Config()
# 模拟
trajectory = [x]
for i in range(100):
u, predicted_trajectory = dwa_control(x, config, goal, ob)
x = motion(x, u, config.dt)
trajectory.append(x)
# 可视化 (此处省略，可以使用 matplotlib 等库进行绘制)
# ...
if math.sqrt((x[0] - goal[0]) ** 2 + (x[1] - goal[1]) ** 2) <= config.robot_radius:
print("到达目标点")
break
print("轨迹:", trajectory)
代码解释：
Config类：存储 DWA 算法的配置参数。motion函数：根据运动学模型更新机器人状态。calc_dynamic_window函数：计算动态窗口。calc_trajectory函数：根据给定的速度推算轨迹。calc_to_goal_cost函数：计算目标导向代价。calc_obstacle_cost函数：计算障碍物代价。calc_final_input函数：计算最终的控制输入（速度）。dwa_control函数：DWA 控制的主函数。
2.2 其他局部路径规划算法
- 向量场直方图 (VFH)： 一种基于栅格地图的局部规划算法，通过构建直方图来表示障碍物的分布。
- 时间弹性带 (TEB)： 一种优化算法，通过调整路径和速度来避开障碍物并优化时间。
3. 总结
机械臂的路径规划是一个复杂的问题，涉及到环境建模、搜索算法、优化等多个方面。全局路径规划算法和局部路径规划算法各有优缺点，适用于不同的场景。
- 全局路径规划：
- 优点：能够找到全局最优路径。
- 缺点：需要完整的环境信息，计算量大，难以应对动态环境。
- 局部路径规划：
- 优点：能够应对动态环境和未知环境，计算量小，实时性好。
- 缺点：容易陷入局部最优，可能无法找到全局最优路径。
在实际应用中，通常会将全局路径规划和局部路径规划结合起来使用，例如：
- 使用全局路径规划算法（如 A* 或 RRT）在静态环境中规划出一条全局路径。
- 使用局部路径规划算法（如 DWA）根据传感器实时获取的信息，沿着全局路径进行局部调整，避开动态障碍物。
希望这篇博客能够帮助你深入理解机械臂路径规划算法的原理和实现。
扫一扫
9509
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
