1 简介
2 部分代码
%%%%%%%%%%%%%%%%%%采用PSO算法对运动学冗余机械臂求一组最优逆解%%%%%%%%%%%%%%%%%%%
%该程序对一个具有四自由度的机械臂做位置控制,由操作空间中的位置坐标,反解出关节空间中的各个关节角度
tic %该函数表示计时开始
%------初始格式化--------------------------------------------------
clear all;
clc;
format long;
%------给定初始化条件----------------------------------------------
c1=1.4962; %加速常数即学习因子1
c2=1.4962; %加速常数即学习因子2
w=0.7298; %惯性权重
MaxDT=500; %最大迭代次数,迭代次数也可以根据适应度函数值的精度是否满足要求来定
D=4; %搜索空间维数(一个机械臂的关节变量的个数为4)
N=40; %群体个体数目
eps=10^(-7); %设置精度(在已知最小值时候用)
alpha=10^(-5);
%机械臂参数(D-H参数)
l=0.085; %琴竹的长度单位m 0.26 0.85
a1=0.175;
d2=0.082;
a3=0.38;
a4=0.26;
%定义目标点的空间位置
p_f=[-0.0516,-0.4006,-0.4135]; %%% -0.6 -0.35 0.52 -0.52
%机械臂各关节的初始角度
theta1=-pi/2;
theta2=pi/2;
theta3=-pi/2;
theta4=0;
q0=[theta1,theta2,theta3,theta4]';
%------初始化种群个体的位置和速度------------
k=0.5; %求取粒子速度系数 vmax=k*xmax 根据各关节的限位范围确定搜索空间
%各关节的最大限位和最小限位
x_min(1)=-2.62;
x_max(1)=-0.52;
x_min(2)=0.52;
x_max(2)=2.62;
x_min(3)=-2.35;
x_max(3)=-0.79;
x_min(4)=-1;
x_max(4)=1;
%各关节速度的上下限
v_min(1)=x_max(1)*(-k);
v_max(1)=x_max(1)*k;
v_min(2)=x_max(1)*(-k);
v_max(2)=x_max(1)*k;
v_min(3)=x_max(1)*(-k);
v_max(3)=x_max(1)*k;
v_min(4)=x_max(1)*(-k);
v_max(4)=x_max(1)*k;
for i=1:N
x(i,1)=rand(1)*(x_min(1)-x_max(1))+x_max(1); %产生一个服从正态分布的随机数作为初始化位置
v(i,1)=rand(1)*(x_max(1)*(-k)-x_max(1)*k)+x_max(1)*k; %产生一个服从正态分布的随机数作为初始化速度
x(i,2)=rand(1)*(x_min(2)-x_max(2))+x_max(2);
v(i,2)=rand(1)*(x_max(2)*(-k)-x_max(2)*k)+x_max(2)*k;
x(i,3)=rand(1)*(x_min(3)-x_max(3))+x_max(3);
v(i,3)=rand(1)*(x_max(3)*(-k)-x_max(3)*k)+x_max(3)*k;
x(i,4)=rand(1)*(x_min(4)-x_max(4))+x_max(4);
v(i,4)=rand(1)*(x_max(4)*(-k)-x_max(4)*k)+x_max(4)*k;
end
%根据机械臂的正向运动学公式,计算机械臂末端点在操作空间中的位置
for i=1:N
p_e(i,1)=a1 - a4*(cos(x(i,1))*sin(x(i,2))*sin(x(i,3)) - cos(x(i,1))*cos(x(i,2))*cos(x(i,3))) - d2*sin(x(i,1)) - ...
l*(sin(x(i,1))*sin(x(i,4)) + cos(x(i,4))*(cos(x(i,1))*sin(x(i,2))*sin(x(i,3)) - cos(x(i,1))*cos(x(i,2))*cos(x(i,3)))) + a3*cos(x(i,1))*cos(x(i,2));
p_e(i,2)=d2*cos(x(i,1)) - a4*(sin(x(i,1))*sin(x(i,2))*sin(x(i,3)) - cos(x(i,2))*cos(x(i,3))*sin(x(i,1))) + ...
l*(cos(x(i,1))*sin(x(i,4)) - cos(x(i,4))*(sin(x(i,1))*sin(x(i,2))*sin(x(i,3)) - cos(x(i,2))*cos(x(i,3))*sin(x(i,1)))) + a3*cos(x(i,2))*sin(x(i,1));
p_e(i,3)=-a4*(cos(x(i,2))*sin(x(i,3)) + cos(x(i,3))*sin(x(i,2))) - a3*sin(x(i,2)) - l*cos(x(i,4))*(cos(x(i,2))*sin(x(i,3)) + cos(x(i,3))*sin(x(i,2)));
end
%------先计算各个粒子的适应度,并初始化个体最优位置y和全局最优位置Pg--------
%适应度函数是跟上一关节的状态做对比的,该种情况只做一个达点运动,即与初始角度值做比较
%此时计算p(i)使用的均为初始化的值
for i=1:N
%p(i)为适应度函数
p(i)=sqrt((p_f(1)-p_e(i,1))^2+(p_f(2)-p_e(i,2))^2+(p_f(3)-p_e(i,3))^2)+alpha*((1*(x(i,1)-q0(1)))^2+(0.5*(x(i,2)-q0(2)))^2+(0.5*(x(i,3)-q0(3)))^2+(0.1*(x(i,4)-q0(4)))^2);
y(i,:)=x(i,:); %初始化个体最优位置y为在时间步t=0时的粒子位置 y()各个个体的初始化最优位置
end
Pg=1;
%Pg_x为全局最优位置,最优位置包含了4个关节的角度值(初始假想最优值)
Pg_x=x(1,:);
%%%%%%%%%%更新全局最优位置%%%%%%%%%%%%%%%%
record(:,:,t)=x(:,:); %记录每一次学习过程中的中间位置值
%%%至此学习完了1代%%%%
Pbest(t)=fit(Pg); %保存每一代的群体最佳位置
end
toc %该函数表示计时结束
%输出最后的计算结果
disp('函数的全局最优位置为:')
for i=1:D
fprintf('x(%d)=%s\n',i,Pg_x(i));
end
%验证最后求得的最优解中各个关节角度是否均在限位以内
if Pg_x(1)>=x_min(1) & Pg_x(1)<=x_max(1) & Pg_x(2)>=x_min(2) & Pg_x(2)<=x_max(2) & Pg_x(3)>=x_min(3) & Pg_x(3)<=x_max(3) & Pg_x(4)>=x_min(4) & Pg_x(4)<=x_max(4)
fprintf('OK!\n');
end
fprintf('最后得到的优化极值为:%s\n',fit(Pg)); %应该观察fit(Pg)的数量级
p_e(Pg,:)
figure
plot(Pbest)
xlabel('迭代次数')
ylabel('适应度值')
3 仿真结果
4 参考文献
[1]石建平, 刘鹏, 陈冬云. 基于改进粒子群优化算法的冗余机械臂逆运动学求解[J]. 机械传动, 2021, 45(2):7.
博主简介:擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真,相关matlab代码问题可私信交流。
部分理论引用网络文献,若有侵权联系博主删除。