✅作者简介:热爱科研的Matlab仿真开发者,修心和技术同步精进,
代码获取、论文复现及科研仿真合作可私信。
🍎个人主页:Matlab科研工作室
🍊个人信条:格物致知。
更多Matlab完整代码及仿真定制内容点击👇
🔥 内容介绍
摘要
本文提出了一种基于快速搜索树算法RRT的六自由度机械臂动态路径规划方法。该方法利用RRT算法的快速搜索能力,能够快速生成机械臂从起始位置到目标位置的路径。同时,该方法考虑了机械臂的运动学和动力学约束,能够生成可行的机械臂运动轨迹。此外,该方法还具有鲁棒性,能够应对环境中的不确定性。
1. 引言
机械臂是一种广泛应用于工业生产、医疗手术、机器人等领域的自动化设备。机械臂的路径规划是机械臂控制的关键技术之一,其目的是生成机械臂从起始位置到目标位置的运动轨迹。机械臂的路径规划算法有很多种,其中快速搜索树算法RRT是一种非常有效的算法。
RRT算法是一种基于随机采样的路径规划算法。该算法首先在工作空间中随机生成一个点,然后从该点出发,向目标位置方向随机生成一个点。如果新生成的点与已有的路径没有碰撞,则将该点添加到路径中。如此重复,直到路径到达目标位置。
2. RRT算法原理
RRT算法的基本原理如下:
-
初始化:在工作空间中随机生成一个点作为起始点,并将其添加到路径中。
-
扩展:从路径的最后一个点出发,向目标位置方向随机生成一个点。如果新生成的点与已有的路径没有碰撞,则将该点添加到路径中。
-
重接:检查路径中是否有任何点与目标位置更近。如果有,则将路径从该点重新连接到目标位置。
-
重复:重复步骤2和步骤3,直到路径到达目标位置。
3. 基于RRT算法的六自由度机械臂动态路径规划方法
本文提出的基于RRT算法的六自由度机械臂动态路径规划方法,主要包括以下几个步骤:
-
建立机械臂的运动学和动力学模型。
-
将机械臂的运动学和动力学模型离散化为一组状态方程。
-
将状态方程转换为RRT算法的输入。
-
运行RRT算法,生成机械臂从起始位置到目标位置的路径。
-
将生成的路径转换为机械臂的运动轨迹。
📣 部分代码
function [path_new,T_new] = path_opt(path,map,T)
%% input: oringinal path obtained from RRT(n*6, struct)
% output: optimized path(n*6,struct)
path_new = prun(path,map);
[path_new,T_new] = expand(path_new,map,T);
end
%% prun the path
function path_new = prun(path,map)
%% input: oringinal path obtained from RRT(n*6, struct)
% output: pruned path(n*6,struct)
path_new = path(1);
[n,m] = size(path);
i = 1;
%if we only have two sample points:
if n<3
path_new = path;
else
%we have 3 or more sample points:
while i<n
for j= i+2:n %following nodes
q = path(i);
q_ = path(j);
if j == n % we have reached the very end of the pruning process
path_new = [path_new;path(n-1);path(n)];
return
end
if DetCol(q,q_,map)
% if the connection line contains no collision, ignore it.
% else, add the former node into the path
path_new = [path_new;path(j-1)];
i = j-1;
break
end
end
end
end
end
%% insert necessary nodes to make the path roughly evenly distributed
function [path_new,T_new] = expand(prunned_path,map,T)
T_new = T;
[n,m]=size(prunned_path);
path_new = prunned_path(1);
insert.val = [];
insert.parent = [];
insert.flag = 0;
max_length = 1;
for i=1:n-1
length = norm(prunned_path(i).val-prunned_path(i+1).val);
if length > max_length
% if the length of connection link is bigger than max_length, add a mid point
insert.val = (prunned_path(i).val+prunned_path(i+1).val)/2;
for j=1:50
% if the mid point contains collision, try some random nodes
insert.val = prunned_path(i).val+(prunned_path(i+1).val-prunned_path(i).val)*randi([30,70])/100;
DetCol(insert,prunned_path(i),map)
if ~DetCol(insert,prunned_path(i),map)
break
end
end
if j<50
% add inter-points into the path and tree, set the correct parents to
% the new node and the following node
insert.parent = prunned_path(i);
insert.flag = 1;
prunned_path(i+1).parent = insert;
path_new = [path_new;insert];
T_new = [T_new;insert];
end
end
path_new = [path_new;prunned_path(i+1)];
end
end
⛳️ 运行结果
4. 仿真实验
为了验证本文提出的方法的有效性,我们进行了仿真实验。仿真实验中,我们使用了一个六自由度机械臂模型,该模型包括6个关节和1个末端执行器。机械臂的起始位置和目标位置分别为:
起始位置:x = 0, y = 0, z = 0, α = 0, β = 0, γ = 0
目标位置:x = 1, y = 1, z = 1, α = π/2, β = π/2, γ = π/2
仿真实验结果表明,本文提出的方法能够快速生成机械臂从起始位置到目标位置的路径,并且生成的路径是可行的和鲁棒的。
5. 结论
本文提出了一种基于快速搜索树算法RRT的六自由度机械臂动态路径规划方法。该方法利用RRT算法的快速搜索能力,能够快速生成机械臂从起始位置到目标位置的路径。同时,该方法考虑了机械臂的运动学和动力学约束,能够生成可行的机械臂运动轨迹。此外,该方法还具有鲁棒性,能够应对环境中的不确定性。仿真实验结果表明,本文提出的方法是有效的。
🔗 参考文献
[1]张宁.基于双目视觉的未知环境下机械臂避障研究[D].济南大学,2019.