matlab里期望轨迹作为输入,【matlab有偿编程】机器人避障路径规划，最优轨迹寻优源代码程序.pdf...

【matlab有偿编程】机器人避障路径规划，最优轨迹寻优源代码程序

%SVM 神经网络求解机械手逆运动学

%SVM 神经网络求解机械手逆运动学

clc

clear

close all

load Test1 p q t r %q 是角度6 行，p 是位姿

%

% 产生训练样本与测试样本

%p=p([2,4,6],:);%只看前4 个角度

p=p(1:end-1,:,:); %将最后一行去掉

q=q';

%j=length(q);

for i=1:200

pp(:,i)=reshape(p(:,:,i),12,1);%将原来的单个三维轨迹矩阵变换成一列，使得输入成为一个 12

行

%qqq(:,i)=RPY(qq(:,i));%转换成RPY 表示方式，将输入变成6 个

end

X=pp(:,1:200)';

Xt=pp(:,151:200)';

Y=q(:,1:200)';

Yt=q(:,151:200)';

%

% 参数设置

type = 'f';

kernel = 'RBF_kernel';

gam =100 ;%602572453.6492; % Regularization parameter 正则化参数。原先是

100

sig2=0.01;%329.5513877866; % Kernel parameter 核参数 (bandwidth in the case of the

'RBF_kernel'(0.01)

% igam=10;

% isig2=0.01;

% costfun = 'rcrossvalidate';

% costfun_args = {X,Y,10};

% optfun = 'gridsearch';

%[gam,sig2,cost] = tunelssvm({X,Y,type,igam,isig2,kernel}); % 模型参数优化 ,每一个输

出都对应一组gam、sig2.

model = initlssvm(X,Y,type,gam,sig2,kernel); % 模型初始化

%

% 交叉验证优化参数

% costfun = 'rcrossvalidate';

% costfun_args = {X,Y,10};

% optfun = 'gridsearch';

% model = tunelssvm(model,[],optfun,{},costfun,costfun_args); % 模型参数优化

%

% 训练与测试

model = trainlssvm(model); % 训练

Yd= simlssvm(model,Xt); % 回归

%

% 结果作图

%plot(1:length(Yt),Yt(:,1),'r+:',1:length(Yd),Yd(:,1),'bo:')

figure(1)

plot(Yt(:,4),'g','LineWidth',2);

hold on;

plot(Yd(:,4),'b','LineWidth',2);

grid on;

xlabel('测试样本数');ylabel('角度');

legend('期望值','求解值')

figure(2)

error=Yt-Yd;

plot(error)

title('误差曲线');grid on;

xlabel('测试样本数');ylabel('误差/rad');

%末端轨迹比较图

a1=fkine(r,Yd*pi/180);

figure(2)

plot3(squeeze(a1(1,4,:)),squeeze(a1(2,4,:)),squeeze(a1(3,4,:)),'r');

hold on;

a1=fkine(r,Yt*pi/180);

figure(2)

plot3(squeeze(a1(1,4,:)),squeeze(a1(2,4,:)),squeeze(a1(3,4,:)),'r');

hold on;

plot3(squeeze(p(1,4,:)),squeeze(p(2,4,:)),squeeze(p(3,4,:)),'b');%轨迹的三维图

title('机械手末端轨迹跟踪效果');

legend('输出的轨迹','期望的轨迹');

grid on;

% plot3(squeeze(p(1,4,151:201)),squeeze(p(2,4,151:201)),squeeze(