基于A*算法的四旋翼城市物流无人机三维路径规划MATLAB实现-平芜编程栈

使用MATLAB完成基于A*算法的无人机三维路径规划代码（单独环境创建代码80，打包所有规划及改进代码280），背景是四旋翼的城市物流无人机。做了拐弯次数改进，程序运行计时，路线远离障碍物等改进，垂直升降等，

在当今城市物流领域，四旋翼无人机因其灵活性和适应性，正逐渐崭露头角。而高效的路径规划对于其执行任务的效率和安全性至关重要。本文将分享如何使用MATLAB实现基于A*算法的四旋翼城市物流无人机三维路径规划，并详细介绍其中所做的改进。

一、环境创建代码（30分部分）

在进行路径规划前，我们需要构建一个模拟的三维环境，这个环境包含障碍物、起始点和目标点等信息。以下是简单的环境创建代码示例：

% 定义环境尺寸 env_size = [100 100 50]; % 长、宽、高 % 初始化障碍物矩阵，0表示无障碍物，1表示有障碍物 obstacle_map = zeros(env_size); % 添加一些简单障碍物 obstacle_map(30:40, 40:50, 20:30) = 1; % 定义起始点和目标点 start_point = [10 10 10]; goal_point = [90 90 40];

代码分析

定义环境尺寸：我们通过env_size变量设定了三维空间的长、宽、高，这里设置为100x100x50的空间。这决定了无人机活动的范围。
初始化障碍物矩阵：obstacle_map是一个与环境尺寸相同的三维矩阵，初始化为全0，表示整个空间初始时无障碍物。
添加障碍物：通过对矩阵特定区域赋值为1，来模拟一些障碍物的存在。这里在坐标(30 - 40, 40 - 50, 20 - 30)区域设置了障碍物。
定义起始点和目标点：明确了无人机路径规划的起点startpoint和终点goalpoint，它们是三维空间中的具体坐标。

二、A*算法核心路径规划及改进代码（80分部分）

A*算法基本框架

function [path, cost] = a_star_search(obstacle_map, start_point, goal_point) % 定义启发函数，这里采用曼哈顿距离 heuristic = @(x1, y1, z1, x2, y2, z2) abs(x1 - x2) + abs(y1 - y2) + abs(z1 - z2); % 初始化开放列表和关闭列表 open_list = []; closed_list = []; % 将起始点加入开放列表 start_node.cost = 0; start_node.heuristic = heuristic(start_point(1), start_point(2), start_point(3), goal_point(1), goal_point(2), goal_point(3)); start_node.total_cost = start_node.cost + start_node.heuristic; start_node.position = start_point; start_node.parent = []; open_list = [open_list; start_node]; while ~isempty(open_list) % 找到开放列表中总代价最小的节点 [~, min_index] = min([open_list.total_cost]); current_node = open_list(min_index); open_list(min_index) = []; % 如果当前节点是目标节点，回溯路径 if all(current_node.position == goal_point) path = []; while ~isempty(current_node.parent) path = [current_node.position; path]; current_node = current_node.parent; end path = [start_point; path]; cost = current_node.cost; return; end % 将当前节点加入关闭列表 closed_list = [closed_list; current_node]; % 生成邻居节点 neighbors = generate_neighbors(current_node.position, obstacle_map); for i = 1:size(neighbors, 1) neighbor = neighbors(i, :); if any(all(neighbor == [closed_list.position]), 2) continue; end neighbor_node.cost = current_node.cost + 1; neighbor_node.heuristic = heuristic(neighbor(1), neighbor(2), neighbor(3), goal_point(1), goal_point(2), goal_point(3)); neighbor_node.total_cost = neighbor_node.cost + neighbor_node.heuristic; neighbor_node.position = neighbor; neighbor_node.parent = current_node; if ~any(all(neighbor == [open_list.position]), 2) open_list = [open_list; neighbor_node]; else existing_index = find(all(neighbor == [open_list.position]), 1); if neighbor_node.cost < open_list(existing_index).cost open_list(existing_index) = neighbor_node; end end end end path = []; cost = Inf; end

改进部分

拐弯次数改进：在实际飞行中，过多的拐弯会消耗更多能量并降低飞行效率。我们可以在生成邻居节点时，对方向变化进行记录和限制。

function [path, cost] = a_star_search_with_turns(obstacle_map, start_point, goal_point) % 新增：记录方向 previous_direction = [0 0 0]; turn_count = 0; % 定义启发函数，这里采用曼哈顿距离 heuristic = @(x1, y1, z1, x2, y2, z2) abs(x1 - x2) + abs(y1 - y2) + abs(z1 - z2); % 初始化开放列表和关闭列表 open_list = []; closed_list = []; % 将起始点加入开放列表 start_node.cost = 0; start_node.heuristic = heuristic(start_point(1), start_point(2), start_point(3), goal_point(1), goal_point(2), goal_point(3)); start_node.total_cost = start_node.cost + start_node.heuristic; start_node.position = start_point; start_node.parent = []; open_list = [open_list; start_node]; while ~isempty(open_list) % 找到开放列表中总代价最小的节点 [~, min_index] = min([open_list.total_cost]); current_node = open_list(min_index); open_list(min_index) = []; % 如果当前节点是目标节点，回溯路径 if all(current_node.position == goal_point) path = []; while ~isempty(current_node.parent) path = [current_node.position; path]; current_node = current_node.parent; end path = [start_point; path]; cost = current_node.cost; return; end % 将当前节点加入关闭列表 closed_list = [closed_list; current_node]; % 生成邻居节点 neighbors = generate_neighbors(current_node.position, obstacle_map); for i = 1:size(neighbors, 1) neighbor = neighbors(i, :); if any(all(neighbor == [closed_list.position]), 2) continue; end new_direction = neighbor - current_node.position; if ~all(new_direction == previous_direction) turn_count = turn_count + 1; end previous_direction = new_direction; % 新增：对拐弯次数进行限制，比如最多允许5次拐弯 if turn_count > 5 continue; end neighbor_node.cost = current_node.cost + 1; neighbor_node.heuristic = heuristic(neighbor(1), neighbor(2), neighbor(3), goal_point(1), goal_point(2), goal_point(3)); neighbor_node.total_cost = neighbor_node.cost + neighbor_node.heuristic; neighbor_node.position = neighbor; neighbor_node.parent = current_node; if ~any(all(neighbor == [open_list.position]), 2) open_list = [open_list; neighbor_node]; else existing_index = find(all(neighbor == [open_list.position]), 1); if neighbor_node.cost < open_list(existing_index).cost open_list(existing_index) = neighbor_node; end end end end path = []; cost = Inf; end

代码分析 - A*算法基本框架

启发函数：使用曼哈顿距离作为启发函数，用于估计当前节点到目标节点的距离。曼哈顿距离计算简单且能在一定程度上引导搜索方向。
开放列表和关闭列表：开放列表存储待评估的节点，关闭列表存储已评估过的节点，避免重复访问。
节点处理：从开放列表中选取总代价最小的节点进行扩展，生成邻居节点并计算其代价。如果邻居节点已在关闭列表中则跳过，否则根据情况更新开放列表。

代码分析 - 拐弯次数改进

记录方向和拐弯次数：新增了previousdirection变量记录上一个方向，turncount记录拐弯次数。每次生成邻居节点时，对比当前方向与上一个方向，若不同则拐弯次数加1。
拐弯次数限制：设定了一个阈值（这里为5次），当拐弯次数超过该阈值时，跳过该邻居节点，以此减少路径中的拐弯次数。

程序运行计时：使用MATLAB的tic和toc函数来记录程序运行时间。

tic [path, cost] = a_star_search_with_turns(obstacle_map, start_point, goal_point); time_taken = toc; fprintf('程序运行时间: %f 秒\n', time_taken);

代码分析 - 程序运行计时

tic函数标记计时开始，在执行完路径规划函数后，使用toc函数获取从tic开始到当前的时间间隔，即程序运行时间，并通过fprintf输出。

路线远离障碍物：在计算节点代价时，可以增加一个与障碍物距离相关的惩罚项，使得路径尽量远离障碍物。

function [path, cost] = a_star_search_with_obstacle_avoidance(obstacle_map, start_point, goal_point) % 定义启发函数，这里采用曼哈顿距离 heuristic = @(x1, y1, z1, x2, y2, z2) abs(x1 - x2) + abs(y1 - y2) + abs(z1 - z2); % 初始化开放列表和关闭列表 open_list = []; closed_list = []; % 将起始点加入开放列表 start_node.cost = 0; start_node.heuristic = heuristic(start_point(1), start_point(2), start_point(3), goal_point(1), goal_point(2), goal_point(3)); start_node.total_cost = start_node.cost + start_node.heuristic; start_node.position = start_point; start_node.parent = []; open_list = [open_list; start_node]; while ~isempty(open_list) % 找到开放列表中总代价最小的节点 [~, min_index] = min([open_list.total_cost]); current_node = open_list(min_index); open_list(min_index) = []; % 如果当前节点是目标节点，回溯路径 if all(current_node.position == goal_point) path = []; while ~isempty(current_node.parent) path = [current_node.position; path]; current_node = current_node.parent; end path = [start_point; path]; cost = current_node.cost; return; end % 将当前节点加入关闭列表 closed_list = [closed_list; current_node]; % 生成邻居节点 neighbors = generate_neighbors(current_node.position, obstacle_map); for i = 1:size(neighbors, 1) neighbor = neighbors(i, :); if any(all(neighbor == [closed_list.position]), 2) continue; end % 新增：计算与障碍物的距离惩罚项 dist_to_obstacle = calculate_distance_to_obstacle(neighbor, obstacle_map); penalty = 1 / dist_to_obstacle; neighbor_node.cost = current_node.cost + 1 + penalty; neighbor_node.heuristic = heuristic(neighbor(1), neighbor(2), neighbor(3), goal_point(1), goal_point(2), goal_point(3)); neighbor_node.total_cost = neighbor_node.cost + neighbor_node.heuristic; neighbor_node.position = neighbor; neighbor_node.parent = current_node; if ~any(all(neighbor == [open_list.position]), 2) open_list = [open_list; neighbor_node]; else existing_index = find(all(neighbor == [open_list.position]), 1); if neighbor_node.cost < open_list(existing_index).cost open_list(existing_index) = neighbor_node; end end end end path = []; cost = Inf; end function dist = calculate_distance_to_obstacle(point, obstacle_map) % 简单计算与最近障碍物的距离 dist = Inf; for i = 1:size(obstacle_map, 1) for j = 1:size(obstacle_map, 2) for k = 1:size(obstacle_map, 3) if obstacle_map(i, j, k) == 1 current_dist = norm(point - [i j k]); if current_dist < dist dist = current_dist; end end end end end end

代码分析 - 路线远离障碍物

距离惩罚项：新增了calculatedistanceto_obstacle函数来计算节点与最近障碍物的距离。在计算邻居节点代价时，加入了一个与距离成反比的惩罚项penalty，使得路径更倾向于远离障碍物的节点。

垂直升降：在邻居节点生成时，特别考虑垂直方向的移动，以适应四旋翼无人机垂直起降的特点。

function neighbors = generate_neighbors(position, obstacle_map) neighbors = []; directions = [-1 0 0; 1 0 0; 0 -1 0; 0 1 0; 0 0 -1; 0 0 1; -1 -1 0; -1 1 0; 1 -1 0; 1 1 0; -1 0 -1; -1 0 1; 1 0 -1; 1 0 1; 0 -1 -1; 0 -1 1; 0 1 -1; 0 1 1; -1 -1 -1; -1 -1 1; -1 1 -1; -1 1 1; 1 -1 -1; 1 -1 1; 1 1 -1; 1 1 1]; for i = 1:size(directions, 1) new_position = position + directions(i, :); if new_position(1) >= 1 && new_position(1) <= size(obstacle_map, 1) &&... new_position(2) >= 1 && new_position(2) <= size(obstacle_map, 2) &&... new_position(3) >= 1 && new_position(3) <= size(obstacle_map, 3) &&... obstacle_map(new_position(1), new_position(2), new_position(3)) == 0 neighbors = [neighbors; new_position]; end end % 特别考虑垂直升降，增加垂直方向移动权重 vertical_directions = [0 0 -1; 0 0 1]; for i = 1:size(vertical_directions, 1) new_position = position + vertical_directions(i, :); if new_position(1) >= 1 && new_position(1) <= size(obstacle_map, 1) &&... new_position(2) >= 1 && new_position(2) <= size(obstacle_map, 2) &&... new_position(3) >= 1 && new_position(3) <= size(obstacle_map, 3) &&... obstacle_map(new_position(1), new_position(2), new_position(3)) == 0 neighbors = [neighbors; new_position]; end end