采用PSO算法对运动学冗余机械臂求逆解的Matlab程序满足关节限位约束

%%%%%%%%%%%%%%%%%%采用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