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