%%%%%%%%%%%%%%%%%%采用PSO算法對運動學冗余機械臂求一組最優(yōu)逆解%%%%%%%%%%%%%%%%%%%??
%該程序?qū)σ粋€具有四自由度的機械臂做位置控制，由操作空間中的位置坐標，反解出關(guān)節(jié)空間中的各個關(guān)節(jié)角度
tic???%該函數(shù)表示計時開始??

%------初始格式化--------------------------------------------------??
clear?all;??
clc;??
format?long;??

%------給定初始化條件----------------------------------------------??
c1=1.4962;?????????????%加速常數(shù)即學習因子1??
c2=1.4962;?????????????%加速常數(shù)即學習因子2??
w=0.7298;??????????????%慣性權(quán)重??
MaxDT=500;?????????????%最大迭代次數(shù)，迭代次數(shù)也可以根據(jù)適應度函數(shù)值的精度是否滿足要求來定?

D=4;???????????????????%搜索空間維數(shù)（一個機械臂的關(guān)節(jié)變量的個數(shù)為4）??
N=40;??????????????????%群體個體數(shù)目??
eps=10^（-7）;???????????%設(shè)置精度（在已知最小值時候用）??
alpha=10^（-5）;

%機械臂參數(shù)（D-H參數(shù)）
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