Skip to content

Commit

Permalink
添加实验代码 (#2)
Browse files Browse the repository at this point in the history
  • Loading branch information
@345ljh
345ljh authored Nov 25, 2024
1 parent c326eae commit 130e323
Show file tree
Hide file tree
Showing 6 changed files with 573 additions and 0 deletions.
Binary file added labs/2024-ljh/210320111-吕家昊.docx
View file
Binary file not shown.
102 changes: 102 additions & 0 deletions labs/2024-ljh/实验1/main.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,102 @@

addpath(genpath('robotcore'))
clear;
clc;

%% 定义参考路径
% %参考路径1
path = [2.00 1.00;
1.25 1.75;
2.00 5.00;
5.25 8.25;
7.25 8.75;
11.75 10.75;
12.00 10.00];
%
% % 参考路径2
% pathx = (0:pi/50:3*pi);
% pathy = 10*exp(-0.5*pathx).*sin(0.5*pi*pathx);

% 参考路径3
% pathx = 0:0.5:15;
% pathy = sqrt(pathx) + sin(pathx);
% path = [pathx', pathy'];

%% 相关参数定义
H = 1; % 车辆轴距，单位：m
goalRadius = 0.05; % 当车辆距离目标点小于此值时结束跟踪
sampleTime = 0.1; % 仿真时间，单位s
vizRate = rateControl(10); % 控制频率

% 设置机器人初始状态
robotInitialLocation = path(1,:); % 初始位置
initialOrientation = 0; % 初始航向角
robotCurrentPose = [robotInitialLocation initialOrientation]'; % [x; y; theta];
v0 = 0; % 初始线速度
w = 0; % 初始角速度

robotGoal = path(end,:); % 确定目标点
vr = 2; % 目标线速度
distanceToGoal = norm(robotInitialLocation - robotGoal); % 计算当前位置与目标点的距离
v = v0; % 初始速度
yaw = initialOrientation; % 初始航向角

% 确定车辆框架尺寸以最接近代表车辆
frameSize = H/0.5;

con = 0.4; % 视距常数
k = 0.4; % 倍率
% 轨迹预处理, 使得相邻轨迹点之间的距离不小于con
x_tmp = [];
y_tmp = [];
for i=1:size(path,1)-1
add_points = floor(sqrt((path(i,1) - path(i+1,1)) ^ 2 + (path(i,2) - path(i+1,2)) ^ 2) / con) + 2;
x_tmp = [x_tmp, linspace(path(i,1), path(i+1,1), add_points)];
y_tmp = [y_tmp, linspace(path(i,2), path(i+1,2), add_points)];
end
path = [x_tmp, path(size(path,1),1); y_tmp, path(size(path,1),2)]';

while( distanceToGoal > goalRadius )
% 更新轨迹跟踪图
hold off

% 绘制参考轨迹
plot(path(:,1), path(:,2),"k--d")
hold all

% 计算控制器的输出，将其输入到机器人中
[v, w] = pure_pursuit_control(robotCurrentPose, path, v, vr, k, con); % 机器人系下v与w

% 使用控制输入得到机器人的速度
vel = derivative(robotCurrentPose, v, w, sampleTime);

% 更新当前姿态
robotCurrentPose = robotCurrentPose + vel*sampleTime;

% 重新计算与目标点的距离
distanceToGoal = norm(robotCurrentPose(1:2) - robotGoal(:));

% 绘制机器人的运行轨迹
% plotTrVec = [robotCurrentPose(1:2); 0];
% plotRot = axang2quat([0 0 1 robotCurrentPose(3)]);
% plotTransforms(plotTrVec', plotRot, "MeshFilePath", "groundvehicle.stl", "Parent", gca, "View","2D", "FrameSize", frameSize);
% light;

scatter(robotCurrentPose(1), robotCurrentPose(2), 20, [0.5,0,0], 'filled');
plot([robotCurrentPose(1); robotCurrentPose(1) + 1 * cos(robotCurrentPose(3))], [robotCurrentPose(2); robotCurrentPose(2) + 1 * sin(robotCurrentPose(3))], 'color', [0.5,0,0]);

% 设置图的横纵坐标
xlim([-5 15]);
ylim([-5 15]);

waitfor(vizRate); % 暂停代码执行，知道计时器vizRate出发，vizRate为采样频率

if distanceToGoal<0.1 % 到达目标后停止
break
end
end





39 changes: 39 additions & 0 deletions labs/2024-ljh/实验1/pure_pursuit_control.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
function [v, w] = pure_pursuit_control(robotCurrentPose, path, v, vr, k, con)
% 寻找离小车最近的点
now_index = 0;
now_dist = 999999;
for i=1:size(path,1)
if sqrt((path(i,1) - robotCurrentPose(1)) ^ 2 + (path(i,2) - robotCurrentPose(2)) ^ 2) < now_dist
now_dist = sqrt((path(i,1) - robotCurrentPose(1)) ^ 2 + (path(i,2) - robotCurrentPose(2)) ^ 2);
now_index = i;
end
end
% 选取目标点
tar_index = size(path,1);
vis = v * k + con; % 预瞄距离
for i=now_index+1:size(path,1)
if sqrt((path(i,1) - robotCurrentPose(1)) ^ 2 + (path(i,2) - robotCurrentPose(2)) ^ 2) > vis
tar_index = i;
break
end
end
scatter(path(tar_index,1), path(tar_index,2), 20, 'b', 'filled'); % 绘制目标
% 计算相对位置
if tar_index ~= size(path,1)
point_pose = atan2(path(tar_index+1,2) - path(tar_index,2), path(tar_index+1,1) - path(tar_index,1));
else
point_pose = atan2(path(tar_index,2) - path(tar_index-1,2), path(tar_index,1) - path(tar_index-1,1));
end
delPose = [path(tar_index, 1:2)'; point_pose] - robotCurrentPose;
delPose = [ cos(robotCurrentPose(3)), sin(robotCurrentPose(3)), 0;
-sin(robotCurrentPose(3)), cos(robotCurrentPose(3)), 0;
0, 0, 1] * delPose;
if delPose(3) > pi
delPose(3) = delPose(3) - 2*pi;
elseif delPose(3) < -pi
delPose(3) = delPose(3) + 2*pi;
end
delPose
v = v + 0.3*(vr - v); % 使用单P控制器实现线速度的控制
w = 2 * v * delPose(2) / (delPose(1)^2 + delPose(2)^2);
end
171 changes: 171 additions & 0 deletions labs/2024-ljh/实验2/AStarGrid.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,171 @@
function [route,numExpanded,I,J] = AStarGrid (input_map, start_coords, dest_coords)
% 在一个网格上运行 A* 算法
% Inputs :
% input_map : 逻辑数组，其中自由空间单元格为 false 或 0
% 障碍物为 true 或 1
% start_coords and dest_coords : 起点和终点单元格的坐标
% 第一个元素是行，第二个是列
% Output :
% route : 包含沿从起点到终点的最短路径的单元格的线性索引的数组
% 如果没有路径，则为空数组。这是一个一维向量。
% numExpanded: 记得同时返回你在搜索过程中扩展的总节点数
% 不要将目标节点计算为扩展节点

% 用一个map矩阵来表示每个点的状态
% 设置用于显示的颜色映射
% 1 - white - 空白单元格
% 2 - black - 障碍物
% 3 - red - 相当于CLOSED列表的作用
% 4 - blue - 相当于OPEN列表的作用
% 5 - green - 起点
% 6 - yellow - 目的地

cmap = [1 1 1; ... 白色，表示空闲的格子
0 0 0; ... 黑色，表示障碍物
1 0 0; ... 红色，表示已访问的格子
0 0 1; ... 蓝色，表示正在考虑的格子
0 1 0; ... 绿色，表示起点
1 1 0; ... 黄色，表示终点
0.5 0.5 0.5]; %灰色，表示路径上的格子
colormap(cmap);

% 是否将每次迭代地图进行可视化展示
drawMapEveryTime = true;
[nrows, ncols] = size(input_map);

% map - 一个二维数组，用于跟踪每个格子的状态
map = zeros(nrows,ncols);

map(~input_map) = 1; % 标记空闲格子
map(input_map) = 2; % 标记障碍物格子

% 产生起点和终点的线性索引
start_node = sub2ind(size(map), start_coords(1), start_coords(2));
dest_node = sub2ind(size(map), dest_coords(1), dest_coords(2));
map(start_node) = 5; % 标记起点
map(dest_node) = 6; % 标记终点

parent = zeros(nrows,ncols); % 用来记录每个节点的父节点
[X, Y] = meshgrid (1:ncols, 1:nrows); % 函数可以将输入的向量扩展为二维网格

xd = dest_coords(1); % 目标节点的 x 坐标
yd = dest_coords(2); % 目标节点的 y 坐标

% 用曼哈顿距离作为启发式函数
% H = abs(X - xd) + abs(Y - yd);
% 用欧氏距离距离作为启发式函数
H = sqrt((X - xd).*(X-xd) + (Y - yd).*(Y-yd));
H = H';

% 初始化成本评价函数，将代价设置为正无穷表示还没探索过这些节点
f = Inf(nrows, ncols);
g = Inf(nrows, ncols);

g(start_node) = 0;
f(start_node) = H(start_node);

% 记录已经被探索的节点数
numExpanded = 0;

% Main Loop
while true
if (drawMapEveryTime)
map1=imrotate(map,90);
image(0.5, 0.5, map1);
axis image; % 设置坐标轴的宽高比为1，使地图绘制时不发生形变
drawnow; % 立即更新图形窗口
end

% 寻找f最小的点，并返回最小值min_f和对应的索引current
[min_f, current] = min(f(:) + abs(map(:) - 4) * 10000); % 只会访问open(map=4)的点

% 判断条件是否满足其中之一，说明已找最短路径或无法到达终点
% 跳出循环，结束路径搜索
if ((current == dest_node) || isinf(min_f))
break;
end

% 更新地图
map(current) = 3; % 将当前节点标记为已访问状态
numExpanded = numExpanded + 1;

% 计算当前节点的行列坐标
[i, j] = ind2sub(size(f), current);

% *********************************************************************
% 两行星号*之间编写代码，实现以下功能：
% 访问当前节点的每个相邻节点
% 根据访问结果更新地图、f和g和父节点表
% 访问节点
if i > 1
if map(i-1, j) == 1 || map(i-1, j) == 6
map(i-1, j) = 4;
parent(i-1, j) = sub2ind(size(map), i, j);
g(i-1, j) = g(i, j) + 0.1;
end
end
if j > 1
if map(i, j-1) == 1 || map(i, j-1) == 6
map(i, j-1) = 4;
parent(i, j-1) = sub2ind(size(map), i, j);
g(i, j-1) = g(i, j) + 0.1;
end
end
if i < nrows
if map(i+1, j) == 1 || map(i+1, j) == 6
map(i+1, j) = 4;
parent(i+1, j) = sub2ind(size(map), i, j);
g(i+1, j) = g(i, j) + 0.1;
end
end
if j < ncols
if map(i, j+1) == 1 || map(i, j+1) == 6
map(i, j+1) = 4;
parent(i, j+1) = sub2ind(size(map), i, j);
g(i, j+1) = g(i, j) + 0.1;
end
end
f = g + H;
%*********************************************************************
end

%% 构建从起点到终点的路径，并进行可视化

if (isinf(f(dest_node))) % 判断是否存在路径
route = [];
else
% *********************************************************************
% 两行星号*之间编写代码，实现以下功能：
% 构建从终点到起点回溯路径
route = [];
I = [];
J = [];
now = dest_node;
while now ~= 0 % 循环在起点停止
route = [route now];
I = [I X(now)];
J = [J Y(now)];
now = parent(now);
end
numExpanded = numExpanded - 1; % 长度去除终点
I = I - 0.5;
J = J - 0.5;

% 重绘起点与终点
map(start_node) = 5;
map(dest_node) = 6;

% *********************************************************************

% 用于可视化地图和路径的代码片段
for k = 2:length(route) - 1
map(route(k)) = 7;
pause(0.04);
map1 = imrotate(map,90);
image(0.5, 0.5, map1);
grid on;
axis image;
end

end
end
Loading

0 comments on commit 130e323

Please sign in to comment.