机械臂避障与轨迹规划仿真（更新中）
2023-04-18 16:05:29 时间
本篇目录
在本篇内容中，将使用MATLAB机器人工具箱，对机械臂进行避障仿真。主要内容包括使用图搜索算法生成路径，获取路径后，对生成的路径进行插值，从而获取轨迹。（水平有限，还请各位大佬指正）
关键词： RRT、关节空间插值、多项式、闭式求解、QP 求解器、minium jerk、minium snap、笛卡尔空间插值、B样条、四元数多点插值Squad
一、机械臂模型及参数
此处采用的机械臂为puma560，puma560为6自由度的机械臂，本文使用 MDH方法建模，MDH模型如图1.2所示：
DH参数表如图1.3所示，图中单位为rad和mm：
代码如下：
% @autor: fuqb
% @data: 2022-09-17
% @function: PlotRobot.m
function robot=PlotRobot()
%建立机器人模型
% theta d a alpha offset
L1=Link([0 0 0 0 0 ],'modified'); %连杆的D-H参数
L2=Link([0 149.09 0 -pi/2 0 ],'modified');
L3=Link([0 0 431.8 0 0 ],'modified');
L4=Link([0 433.07 20.32 -pi/2 0 ],'modified');
L5=Link([0 0 0 pi/2 0 ],'modified');
L6=Link([0 0 0 -pi/2 0 ],'modified');
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','puma560','base' , ...
transl(0, 0, 0.62)* trotz(0)); %连接连杆，机器人取名puma560
robot.display();
q=[10*pi/180 0*pi/180 0*pi/180 0*pi/180 0*pi/180 0*pi/180 ];
robot.plot(q);
robot.teach();
% robot.fkine(q)
hold on
% T=TransformMatrix(10*pi/180 ,0, 0 ,0)*TransformMatrix(-10*pi/180 ,149.09, 0, -pi/2)*TransformMatrix(-10*pi/180, 0, 431.8, 0)*...
% TransformMatrix(10*pi/180, 433.07, 20.32, -pi/2)*TransformMatrix(10*pi/180, 0, 0, pi/2)*TransformMatrix(10*pi/180, 0, 0 ,-pi/2);
% [T,T01,T02,T03,T04,T05,T06]=RobotFkine(q,false);
end
二、路径生成
本文路径生成算法采用RRT算法。
2.1 RRT算法简介
RRT算法全称为快速扩展随机树算法（Rapidly Exploring Random Tree）,此处RRT算法参考B站IR艾若机器人的路径规划、轨迹优化等系列课程，算法流程如图2.1所示：
对于机械臂而言，其可以在笛卡尔空间或者关节空间搜索，相对笛卡尔空间而言，关节空间中搜索更为简单直接，因此本篇采用 关节空间中搜索的方法。
2.2 初始化
1、使用运动学逆解计算目标关节角度；
2、初始化随机生成树，随机数的1-6列存放关节角度，第7列存放cost，第8列存放节点parent；
3、设置关节空间搜索步长；
4、设置机械臂各连杆距离障碍物最近阈值；
5、设置最大迭代步数；
如代码所示：
targetPos=[0 0 1;
0 -1 0;
1 0 0]; %targetPos is the posture when manipulator touch balls
targetPos = [targetPos,[ballsOrigin(index,:)]';
[0 0 0 1]];
targetJointPos = robot.ikine(targetPos) %target joint pos
%%
nodeList=[initJointPos,0,0]; %nodeList:col 1-6 is jointpos,col 7 is cost,col 8 is node parent
%%
angleStepSize=0.2*pi/180; %joint space search step length is 1°
threhold=10; %distance between robot and obstacle threhold is 10mm
%%
maxLoopCount=15000;
2.3 Sample、Steer、Near
产生新节点的方法如图2.2所示，x_new为新节点关节角度，x_rand为随机采样点，x_near为最近的节点，x_init为初始节点关节角度，x_target为目标节点关节角度，kp为权重。引入kp后括号内项目的是为了加快搜索速度，提高效率，有点像人工势场法中的引力。具体做法如代码所示：
function newNode=SampleNearAndSteer(nodeList,angleStepSize,initJointPos,targetJointPos)
Kp = 1.5;
randNode = rand(1,6).*2*pi - pi;
[row,~]=size(nodeList);
len = zeros(row,1);
for i=1:row
len(i) = sqrt((nodeList(i,1:6)-randNode)*(nodeList(i,1:6)-randNode)');
end
[~,index]=min(len,[],1);
nearNode = nodeList(index,1:6);
randOrientation = (randNode-nearNode)/ sqrt((randNode-nearNode)*(randNode-nearNode)');
searchOrientation=(targetJointPos-initJointPos)/ sqrt((targetJointPos-initJointPos)*(targetJointPos-initJointPos)');
newNode = nearNode+angleStepSize*(randOrientation+Kp*searchOrientation);
cost = sqrt((randOrientation+Kp*searchOrientation)*(randOrientation+Kp*searchOrientation)');
newNode = [newNode,cost,index];
end
2.5 碰撞检测
碰撞检测算法将在第三部分详细介绍。
2.6 退出循环条件
退出循环条件有两个，第一个是超出循环次数，第二是搜索到终点附近，且与终点连接无碰撞，具体做法如代码所示：
isJointsPosNear = abs((targetJointPos-nearNode(1:6)))<...
angleStepSize*10*[2 2 2 20 15 15];
isNear = sum(isJointsPosNear);
noCollision1=CheckPathCollision(newNode,targetJointPos,threhold,...
ballsOrigin,ballsRadius,cylindersInfo,index); %check path collision
if (isNear==6)&&noCollision1
disp('find path successfully.')
[n,~]=size(nodeList);
nodeList=[nodeList;[targetJointPos,0,n]];
isFind = true;
return;
end
2.7 RRT算法整体代码
整体代码如下：
% @autor: fuqb
% @function: RRT
% @param: robot,initJointPos,origin,radius,index
% @robot: puma560
% @initJointPos: robot current joint pos
% @origin: balls origin (obstacle)
% @radius: obstacle radius
% @index: target ball index
function [nodeList,isFind]=RRT(robot,initJointPos,ballsOrigin,ballsRadius,cylindersInfo,index)
isFind = false;
targetPos=[0 0 1;
0 -1 0;
1 0 0]; %targetPos is the posture when manipulator touch balls
targetPos = [targetPos,[ballsOrigin(index,:)]';
[0 0 0 1]];
targetJointPos = robot.ikine(targetPos) %target joint pos
%%
nodeList=[initJointPos,0,0]; %nodeList:col 1-6 is jointpos,col 7 is cost,col 8 is node parent
%%
angleStepSize=0.2*pi/180; %joint space search step length is 1°
threhold=10; %distance between robot and obstacle threhold is 10mm
%%
maxLoopCount=15000;
%%
% loop to grow RRTs
for i=1:maxLoopCount
newNode=SampleNearAndSteer(nodeList,angleStepSize,...
initJointPos,targetJointPos); %generate new node
nearNode = nodeList(newNode(8),:); %get nearest node
noCollision=CheckPathCollision(newNode,nearNode,threhold,ballsOrigin,...
ballsRadius,cylindersInfo,index); %check path collision
if noCollision
nodeList=[nodeList;newNode];
else
continue;
end
isJointsPosNear = abs((targetJointPos-nearNode(1:6)))<...
angleStepSize*10*[2 2 2 20 15 15];
isNear = sum(isJointsPosNear);
noCollision1=CheckPathCollision(newNode,targetJointPos,threhold,...
ballsOrigin,ballsRadius,cylindersInfo,index); %check path collision
if (isNear==6)&&noCollision1
disp('find path successfully.')
[n,~]=size(nodeList);
nodeList=[nodeList;[targetJointPos,0,n]];
isFind = true;
return;
end
end
isFind = false;
disp('failed to find path.')
[row1,~]=size(nodeList);
len = zeros(row1,1);
for i=1:row1
len(i) = 100*(nodeList(i,1:3)-targetJointPos(1:3))*...
(nodeList(i,1:3)-targetJointPos(1:3))'+...
(nodeList(i,4:6)-targetJointPos(4:6))*(nodeList(i,4:6)-targetJointPos(4:6))';
end
[~,index]=min(len,[],1)
nearestNode = nodeList(index,1:6)
abs(targetJointPos - nearestNode)*180/pi
end
三、碰撞检测算法简介
本文采用的碰撞检测流程如图3.1所示，本文机械臂各个连杆采用圆柱进行包围，障碍物采用球包围。
3.1 机械臂连杆圆柱体包围及障碍物球包围
机械臂共使用8个圆柱体进行包围，具体做法如代码所示，障碍物如图3.2所示：
cylindersInfo=[150 120 100 90 70 70 60 60; %cylinder radius
100 90 435 90 435 90 90 90; %cylinder axis end
-100 -120 0 -90 0 -90 -90 -90; %cylinder axis start
1 2 2 3 3 4 5 6; %cylinder subordinate to frame i
3 3 1 3 2 3 3 3]; %cylinder axis orientation,1 means axis x,2 means axis y,3 means axis z
3.2 路径离散化
将路径分为n份，再分别对每个点进行碰撞检测；
% @autor: fuqb
% @function: CheckCollision
% @param: newNode,nearNode,threhold,robot,origin,radius
% @index: target ball index
% @notes:this function is designed to check path collison between newNode
% and nearNode.
function noCollision=CheckPathCollision(newNode,nearNode,threhold,ballsOrigin,ballsRadius,cylindersInfo,index)
n=10; %path interpolation count
initPos=newNode(1:6);
targetPos=nearNode(1:6);
% tic
for i=0:n
pos = (targetPos-initPos)*i/n+initPos;
noCollision=CheckLinksCollision(pos,threhold,ballsOrigin,ballsRadius,cylindersInfo,index);
end
% toc
end
3.3 机械臂当前位姿碰撞检测
如3.1中所示，已经对路径进行了离散，对机械臂当前位姿进行碰撞检测。为方便各个连杆与球进行碰撞检测，将各个球球心坐标臂变换至各个连杆坐标系。
% @autor: fuqb
% @function: CheckLinksCollision
% @param: q,threhold,robot,origin,radius
% @q:current joints pos
% @notes:this function is designed to check manipulator links collision with
% balls,manipulator joints pos is given.
function noCollision=CheckLinksCollision(q,threhold,ballsOrigin,ballsRadius,cylindersInfo,index)
% using SAT algorithm to check collision.
% The manipulator link is surrounded by cylinders
% obstacle is surrounded by balls
% links and balls project on the plane
noCollision=true;
[~,T01,T02,T03,T04,T05,T06]=RobotFkine(q,false);
o=zeros(6,4);
[row,~]=size(ballsOrigin);
for i=1:row
if(i~=index)
o(1,:)=T01[ballsOrigin(i,:),1]'; %ball origin in Coordinate 1
o(2,:)=T02[ballsOrigin(i,:),1]'; %ball origin in Coordinate 2
o(3,:)=T03[ballsOrigin(i,:),1]'; %ball origin in Coordinate 3
o(4,:)=T04[ballsOrigin(i,:),1]'; %ball origin in Coordinate 4
o(5,:)=T05[ballsOrigin(i,:),1]'; %ball origin in Coordinate 5
o(6,:)=T06[ballsOrigin(i,:),1]'; %ball origin in Coordinate 6
o1=o(:,1:3);
for j=1:8
temp=CheckSingleLinkCollision(j,cylindersInfo,threhold,o1,ballsRadius(i));
noCollision = noCollision&temp;
end
end
end
end
3.4 机械臂单个连杆碰撞检测
在机械臂参数已知及姿态已知的情况下，将障碍物球心变换到各个关节坐标系中，分别在各个坐标系中计算点到线段的距离（此线段为圆柱的中心线，其必定与关节坐标系的x,y或者z轴重合），如果距离大于圆柱与球半径之和，那么不发生碰撞，此种方法可以大大减少计算量。机械臂单个连杆碰撞检测方法如代码所示。
% @autor: fuqb
% @function: CheckSingleLinkCollision
% @param: idx,cylinderInfo,threhold,origin,ballRadius
% @idx:cylinder index
% @origin:ball origin in frame cylinderInfo(4,idx)
% @notes:this function is designed to check single cylinder collision
% with single ball
function noCollision=CheckSingleLinkCollision(idx,cylinderInfo,threhold,origin,ballRadius)
cylinderRadius = cylinderInfo(1,idx);
cylinderAxisStart = cylinderInfo(3,idx);
cylinderAxisEnd = cylinderInfo(2,idx);
cylinderCoordinate = cylinderInfo(4,idx);
cylinderAxisOrientation = cylinderInfo(5,idx);
o=origin(cylinderCoordinate,:);
axiss=1:3;
axiss(cylinderAxisOrientation) =[];
if o(cylinderAxisOrientation)<cylinderAxisStart
distance = sqrt((o(cylinderAxisOrientation)-cylinderAxisStart)^2+(sqrt(o(axiss(1))^2+o(axiss(2))^2)-cylinderRadius)^2);
elseif o(cylinderAxisOrientation)>cylinderAxisEnd
distance = sqrt((o(cylinderAxisOrientation)-cylinderAxisEnd)^2+(sqrt(o(axiss(1))^2+o(axiss(2))^2)-cylinderRadius)^2);
else
distance = sqrt(o(axiss(1))^2+o(axiss(2))^2)-cylinderRadius;
end
if distance>ballRadius+threhold
noCollision = true;
else
noCollision = false;
end
end
四、轨迹规划
RRT算法返回了一系列离散点，需要对这些离散点进行插值。可以在关节空间中进行插值，也可以在笛卡尔空间中分别对位置和姿态进行插值。本文首先在关节空间中使用多项式进行插值。
4.1 关节空间多项式插值（闭式求解 minium jerk）
% @autor: fuqb
% @function: miniumJerkTrajectoryGeneration,closed form solution
% @param: pieceNum,initialPVA,terminalPVA,intermediatePositions,timeAllocatorVector
% @initialPVA:initial pos,vel,acc
% @terminalPVA:terminal pos,vel,acc
% @intermediatePositions:intermediate pos
% @timeAllocatorVector:traj segments duration
function traj=miniumJerkTrajectoryGeneration(pieceNum,initialPVA,...
terminalPVA,intermediatePositions,timeAllocatorVector,controlPeriod)
% M*c=b
b = zeros(6*pieceNum,6);
b(1:3,:) = initialPVA;
b(4:6,:) = terminalPVA;
b(7:6+pieceNum-1,:) = intermediatePositions(1:end,:);
M = zeros(6*pieceNum,6*pieceNum);
%%
%The starting point condition
M(1,1:6) = CalculateMiniumJerkPolynomialCoefficients(0,timeAllocatorVector(1),true);
M(2,1:6) = CalculateMiniumJerkPolynomialCoefficients(1,timeAllocatorVector(1),true);
M(3,1:6) = CalculateMiniumJerkPolynomialCoefficients(2,timeAllocatorVector(1),true);
%%
%%The terminal point condition
M(4,end-5:end) = CalculateMiniumJerkPolynomialCoefficients(0,timeAllocatorVector(end),false);
M(5,end-5:end) = CalculateMiniumJerkPolynomialCoefficients(1,timeAllocatorVector(end),false);
M(6,end-5:end) = CalculateMiniumJerkPolynomialCoefficients(2,timeAllocatorVector(end),false);
for i = 1:pieceNum-1
%%
% Midpoint condition
M(6+i,(6*(i-1)+1):6*i) = CalculateMiniumJerkPolynomialCoefficients(0,timeAllocatorVector(i),false);
%%
% Continuity conditions
%pos continuous conditions
M(6+pieceNum+(i-1)*5,(6*(i-1)+1):6*i) = ...
CalculateMiniumJerkPolynomialCoefficients(0,timeAllocatorVector(i),false);
M(6+pieceNum+(i-1)*5,(6*i+1):6*(i+1)) = ...
-CalculateMiniumJerkPolynomialCoefficients(0,timeAllocatorVector(i+1),true);
%vel continuous conditions
M(6+pieceNum+(i-1)*5+1,(6*(i-1)+1):6*i) = ...
CalculateMiniumJerkPolynomialCoefficients(1,timeAllocatorVector(i),false);
M(6+pieceNum+(i-1)*5+1,(6*i+1):6*(i+1)) = ...
-CalculateMiniumJerkPolynomialCoefficients(1,timeAllocatorVector(i+1),true);
%acc continuous conditions
M(6+pieceNum+(i-1)*5+2,(6*(i-1)+1):6*i) = ...
CalculateMiniumJerkPolynomialCoefficients(2,timeAllocatorVector(i),false);
M(6+pieceNum+(i-1)*5+2,(6*i+1):6*(i+1)) = ...
-CalculateMiniumJerkPolynomialCoefficients(2,timeAllocatorVector(i+1),true);
%jerk continuous conditions
M(6+pieceNum+(i-1)*5+3,(6*(i-1)+1):6*i) = ...
CalculateMiniumJerkPolynomialCoefficients(3,timeAllocatorVector(i),false);
M(6+pieceNum+(i-1)*5+3,(6*i+1):6*(i+1)) = ...
-CalculateMiniumJerkPolynomialCoefficients(3,timeAllocatorVector(i+1),true);
%snap continuous conditions
M(6+pieceNum+(i-1)*5+4,(6*(i-1)+1):6*i) = ...
CalculateMiniumJerkPolynomialCoefficients(4,timeAllocatorVector(i),false);
M(6+pieceNum+(i-1)*5+4,(6*i+1):6*(i+1)) = ...
-CalculateMiniumJerkPolynomialCoefficients(4,timeAllocatorVector(i+1),true);
end
coefficientMatrix =pinv(M)*b;
traj=GetTrajInJointSpace(controlPeriod,coefficientMatrix,timeAllocatorVector);
end
function traj=GetTrajInJointSpace(controlPeriod,coefficientMatrix,timeAllocatorVector)
trajDuration = sum(timeAllocatorVector);
t = 0:controlPeriod:trajDuration;
n = length(t);
traj = zeros(n,6);
currentTime = 0;
k = 1;
for i = 1:n
for j = 1:6
traj(i,j) = CalculatePolynomialValue(coefficientMatrix((k-1)*6+1:k*6,j)',currentTime,5);
end
currentTime = currentTime + controlPeriod;
if currentTime >=timeAllocatorVector(k)
currentTime = currentTime - timeAllocatorVector(k);
k = k + 1;
end
end
end
4.2 关节空间多项式插值（QP求解器 minium snap）
% @autor: fuqb
% @function: TrajectoryGenerationUsingQPSolver,solution that using QP solver.
% @param: pieceNum,initialPVA,terminalPVA,intermediatePositions,timeAllocatorVector
% @initialPVA:initial pos,vel,acc
% @terminalPVA:terminal pos,vel,acc
% @intermediatePositions:intermediate pos
% @timeAllocatorVector:traj segments duration
function traj = TrajectoryGenerationUsingQPSolver(pieceNum,initialPVA,...
terminalPVA,intermediatePositions,timeAllocatorVector,controlPeriod)
r = 4;
n = 2*r-1;
[row,~] = size(timeAllocatorVector);
time = zeros(row+1,1);
sum =0;
for i = 1:row
sum = sum + timeAllocatorVector(i);
time(i+1) = sum;
end
H=[];
for i=1:pieceNum
Qi=GetPartQ(n,r,time(i),time(i+1));
H=blkdiag(H,Qi);
end
f = zeros(pieceNum*(n+1),1);
p = zeros(pieceNum*(n+1),6);
[Aeq,beq]=GenerateEquationCoefficients(pieceNum,initialPVA,...
terminalPVA,intermediatePositions,timeAllocatorVector,n);
for i = 1:6
be = beq(:,i);
p(:,i) = quadprog(H,f,[],[],Aeq,be);
end
traj=GetTrajInJointSpace1(controlPeriod,p,timeAllocatorVector);
end
4.3 效果展示
在这里插入图片描述
(待续。。。。)
相关文章
- 【技术种草】cdn+轻量服务器+hugo=让博客“云原生”一下
- CLB运维&运营最佳实践 ---访问日志大洞察
- vnc方式登陆服务器
- 轻松学排序算法：眼睛直观感受几种常用排序算法
- 十二个经典的大数据项目
- 为什么使用 CDN 内容分发网络？
- 大数据——大数据默认端口号列表
- Weld 1.1.5.Final，JSR-299 的框架
- JavaFX 2012：彻底开源
- 提升as3程序性能的十大要点
- 通过凸面几何学进行独立于边际的在线多类学习
- 利用行动影响的规律性和部分已知的模型进行离线强化学习
- ModelLight：基于模型的交通信号控制的元强化学习
- 浅谈Visual Source Safe项目分支
- 基于先验知识的递归卡尔曼滤波的代理人联合状态和输入估计
- 结合网络结构和非线性恢复来提高声誉评估的性能
- 最佳实践丨云开发CloudBase多环境管理实践
- TimeVAE：用于生成多变量时间序列的变异自动编码器
- 具有线性阈值激活的神经网络：结构和算法
- 内网渗透之横向移动 -- 从域外向域内进行密码喷洒攻击