基于径向基神经网络补偿卡尔曼滤波估计误差的视觉伺服运动控制MATLAB仿真程序

clc
clear all
%close all

cam = CentralCamera('default');
% P = mkgrid( 2, 0.18, 'pose', SE3(0,0,1) );%在Z=1.6m的平面画一个中心在（0,0,1.6），边长为0.5m的正方形
 P=[-0.1,-0.1,0.1,0.1;-0.075,0.075,0.075,-0.075;1.6,1.6,1.6,1.6];
% figure(23)
%  scatter3(P(3,:),P(2,:),P(1,:));%以点的形式在三维空间中画出P点实际位置
% hold on
 P_C_0=[0 0 1.3 0 0 0];
T_C0=transl(P_C_0(1),P_C_0(2),P_C_0(3))*trotx(P_C_0(4))*troty(P_C_0(5))*trotz(P_C_0(6));

%%%%%%%%%%期望图像特征%%%%%%%%%
p0 = cam.plot(P, 'pose', T_C0);%期望图像特征
p0_y=[p0(:,1);p0(:,2);p0(:,3);p0(:,4)];
%%%%%%%%%%期望图像特征%%%%%%%%%
%%%%%在figure20中绘制特征运动图

figure(20);
scatter(p0(1,:),p0(2,:),'.');
axis([0 1024 0 1024]);
hold on
%%%%%%%
T_x=0.4;
T_y=0.2;
T_z=0.4;
R_x=10*pi/180;
R_y=10*pi/180;
R_z=10*pi/180;

m=numel(p0);%计算图像特征矩阵元素个数
%p_c=[T_x;T_y;T_z;R_x;R_y;R_z];
 P_C=[T_x T_y T_z R_x R_y R_z];

 Error_P_C=P_C_0-P_C;
 Error_P_C_y=transpose( Error_P_C);
 Error_P_C_y_s(:,1)= Error_P_C_y;
n=length(P_C);%计算末端位姿自由度数
T_C=transl(T_x,T_y,T_z)*trotx(R_x)*troty(R_y)*trotz(R_z);%T_C表示相机当前初始位姿
p_c = cam.plot(P, 'pose', T_C);%计算当前初始位姿下图像特征
p_c0=p_c;

p_c_y=[p_c(:,1);p_c(:,2);p_c(:,3);p_c(:,4)];%将初始图像特征p_c转为列向量p_c_y
p_c0_y=p_c_y;
Error_f_0=p0_y-p_c_y;
Error_f_y=Error_f_0;
Error_f_s(:,1)=Error_f_0;%存储初始图像特征误差

%%%%%%%通过卡尔曼滤波进行X估计，从而求解图像雅克比矩阵J%%%%%%%%

%%初始化；计算初始试探运动的第一步，图像空间四个特征点位姿
% for st=1:5
%     
% delta_P= Error_P_C/100;%初始相机位姿各方向均匀移动一步delta_P
% delta_P_y=transpose(delta_P);
% delta_P_s(:,st)=delta_P;%存储相机位姿变化量
% P_C=P_C-delta_P;%更新相机位姿
% %P_C_s(st,:)=P_C;%存储相机位姿
% T_C1=transl(P_C(1),P_C(2),P_C(3))*trotx(P_C(4))*troty(P_C(5))*trotz(P_C(6));%计算当前相机位姿变化矩阵
% 
% %%%%%%%%伺服初始当前图像特征%%%%%%%%
% p1 = cam.plot(P, 'pose', T_C1);%计算当前位姿下的图像特征
% p1_y=[p1(:,1);p1(:,2);p1(:,3);p1(:,4)];%将当前图像特征转为列形式
% p1_y_s(:,st)=p1_y;%存储当前图像特征，以列形式存储
% 
% %%%%%%%%伺服初始当前图像特征%%%%%%%%
% delta_p=p1_y-p_c_y;%因相机初始移动一步造成的图像空间特征运动量delta_p214
% delta_p_s(:,st)=delta_p;%存储当前图像与上一步图像特征误差
% p_c_y=p1_y;%更新当前图像特征，作为下次一步基础图像特征
% 
% %delta_f=[delta_p(:,1);delta_p(:,2);delta_p(:,3);delta_p(:,4)];%把图像特征变化量delta_p转换为列向量delta_f
% P_n=pinv(delta_P_s);%计算delta_P的广义逆矩阵
% end
% J0=delta_p_s*pinv(delta_P_s);%计算初始雅克比矩阵，并用于推导出初始状态向量x（0）；J0~=delta_f*(delta_q)^(-1)

%基于模型的雅克比矩阵求解
J0 = cam.visjac_p( p_c, Error_P_C(3) )/1;%除3是为了让初始雅克比矩阵不那么准确，以方便判断算法的鲁棒性
J=J0 ;
%初始化X：将用模型计算的初始图像雅克比矩阵J0转换为状态向量X0
X0=transpose([J0(1,:),J0(2,:) ,J0(3,:), J0(4,:), J0(5,:),J0(6,:),J0(7,:),J0(8,:)]);%求初始X（0）=（f1-f0）*(p1-p0)^(-1)
X_s(1:48,1)=X0;%将X（k）存储到X_s中
 %delta_P_y=transpose(delta_P);%计算delta_q
X=X0;
X_d_s(:,1)=X0;

W_noise=normrnd(0.1,0.5,m*n,1);
q=W_noise;
V_noise=normrnd(0.01,0.5,m,1);
r=V_noise;
%初始化估计协方差P，用U代替P，避免符号重复
U=(10^0)*eye(m*n);%初始状态估计误差协方差P(0)，P越大，则K越大，就意味着越相信测量值，X（k）=X^+K(Z-HX)
%U=U;%为避免符号重复，用U代表估计误差协方差P

b=0.65;%遗忘因子
R=0.5*eye(m);%测量噪声初始方差阵
Q=0.05*eye(m*n);%状态噪声方差
p_c_a=p_c_y;%初始化图像特征
P_C=transpose(P_C);

%%%%%%通过RBF神经网络对Error_X进行修正
load('C:\Users\taoli\Documents\MATLAB\Robot\RBF_input_output', 'Error_K_s', 'Error_X_s', 'Error_Z_s', 'Error_X_d');
input=[Error_K_s;Error_X_s;Error_Z_s];
output=Error_X_d;
%[net,tr]=newrb(input,output,0,1,MN,DF);
net=newrbe(input,output,1);
%%%清空神经网络训练的样本数据，避免与迭代重复
clear Error_K_s Error_X_s Error_Z_s Error_X_d
%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for j=1:1000
  if max(abs(Error_f_y))>2

%%%%计算当前位姿的速度
t=0.05;
lambda=1;
V_max=2;
v=lambda*pinv(J)*Error_f_y;%基于当前特征误差及雅克比矩阵求解当前末端速度v
%%%%%限制最高速度不超过机器人的极限速度
v_l=sqrt((v(1)^2)+(v(2)^2)+(v(3)^2));
v_l_s(j)=v_l;
if max(abs(sqrt((v(1)^2)+(v(2)^2)+(v(3)^2))))>=V_max
    v=(V_max/sqrt((v(1)^2)+(v(2)^2)+(v(3)^2)))*v;
end
v_s(:,j)=v;

%计算当前速度下的图像变化量Z
delta_P=v*t;%计算当前速度在一个控制节拍T=0.05s内，末端位姿的变化量
P_C=P_C+delta_P;%更新预期速度运动后的下一状态相机位姿
%P_C_s(1:6,j)=P_C;
P_C_s(:,j)=P_C;
T_C=transl(P_C(1),P_C(2),P_C(3))*trotx(P_C(4))*troty(P_C(5))*trotz(P_C(6));%T_C表示相机当前初始位姿
p_noise=rand(2,4) ;
p_c = cam.plot(P, 'pose', T_C)+0*p_noise;%计算当前初始位姿下图像特征，3*p_noise表示图像识别过程中的误差，均值为3个像素
%p_c=p_c+normrnd(10,1,2,4)/j;

%%%更新期望的雅克比矩阵
 depth=abs(1.6-P_C(3));
 J_d=cam.visjac_p(p_c,depth);
 X_d=[J_d(:,1);J_d(:,2);J_d(:,3);J_d(:,4);J_d(:,5);J_d(:,6)];
 X_d_s(:,j+1)=X_d;
 Error_X_d_s(:,j)= X_d_s(:,j+1)-X_d_s(:,j);

figure(20);
% scatter(p_c(1,:),p_c(2,:),'.');
axis([0 1024 0 1024]);
hold on

p_c_y=[p_c(:,1);p_c(:,2);p_c(:,3);p_c(:,4)];%将当前计算得到的图像特征坐标转换为列向量
p_c_y_s(:,j)=p_c_y;%存储当前图像特征
Error_f=p0-p_c;
Error_f_y=[Error_f(:,1);Error_f(:,2);Error_f(:,3);Error_f(:,4)];
Error_f_s(:,j)=Error_f_y;

delta_f_new=p_c_y-p_c_a;%计算当前图像特征p与前一步图像特征差值p_a
V_noise=normrnd(0.1,0.5,m,1);%设定观测噪声normrnd(MU,SIGNA,m,n)均值为MU，方差SIGNA，m行n列高斯噪声
Z=delta_f_new+1*V_noise;%计算观测矩阵Z
Z_s(:,j)=Z;
p_c_a=p_c_y;%更新当前图像特征作为下一步的前向图像特征

%%%%计算观测转移矩阵H
for i=1:8
H(i,((i-1)*6+1):(i*6))=transpose(delta_P);%设定初始观测转移矩阵
end

% %进行状态更新X_a
X_a=X;

%计算观测估计值Z_est
Z_est=H*X_a;
Error_Z=Z-Z_est;
Error_Z_s(:,j)=Error_Z;
U_a=U+Q;
%%%计算卡尔曼滤波增益K
K=U_a*transpose(H)*pinv(H*U_a*transpose(H)+R)/1000;%卡尔曼滤波增益K
%%%计算增益误差
K_y=[K(:,1);K(:,2);K(:,3);K(:,4);K(:,5);K(:,6);K(:,7);K(:,8)];
if j==1
    K_a_y=K_y;
end
Rrror_K=K_y-K_a_y;
Error_K_s(:,j)=Rrror_K;

%%计算当前状态实际估计值X
X=X_a+K*(Z-Z_est);%估计出状态变量X(k),原则上应该靠近X初始值，即信任预测值更多，因此P（0）应设置小的初始值
% if j>1
% Error_E_e=sim(net,input(:,j-1));
% Error_E_e_s(:,j)=Error_E_e;
% X=X+Error_E_e;%%%利用RBF神经网络对KF滤波的状态估计误差进行补偿修正
% end
W_noise=normrnd(0.1,0.5,m*n,1);
X_s(1:48,j+1)=X;
%计算并存储估计误差Error_X
Error_X=X_s(1:48,j+1)-X_s(1:48,j);
Error_X_s(:,j)=Error_X;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%期望状态变量的输出，作为神经网络的输出值
Error_X_d(:,j)=X_s(1:48,j+1)- X_d_s(:,j+1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Error_X_s(:,j)= Error_X_s(:,j)-Error_X_d_s(:,j);
%%更新估计协方差P
 U=(eye()-K*H)*U_a*transpose((eye()-K*H))+K*R*transpose(K);%更新状态估计误差P（k），用于下一轮估计运算
% d_a=b;%%%遗忘因子
% q=(1-d_a)*q+d_a*(X_s(1:48,j+1)-X_s(1:48,j));
% r=(1-d_a)*r+d_a*(Z-H*X_a);
% Q=(1-b)*q+b*(K*Z*transpose(Z)*transpose(K)+U-U_a);%更新状态估计协方差矩阵
% R=(1-b)*r+b*(Z*transpose(Z)-H*U_a*transpose(H));%更新观测协方差矩阵

%基于估计的X(k)重构雅克比矩阵J（k）
%%%计算J（k）
% J_noise=normrnd(0.001,0.005,m*n,1);
% T_x=transpose(X0+J_noise);
% 
T_x=transpose(X);
% T_x=transpose(X);
J=[T_x(1:6);T_x(((2-1)*6+1):(2*6));T_x(((3-1)*6+1):(3*6));...
    T_x(((4-1)*6+1):(4*6));T_x(((5-1)*6+1):(5*6));T_x(((6-1)*6+1):(6*6));...
    T_x(((7-1)*6+1):(7*6));T_x(((8-1)*6+1):(8*6))];%将X(k)转换为J（k）

input(:,j)=[Error_K_s(:,j);Error_X_s(:,j);Error_Z_s(:,j)];

  end
%   pause(0.01)
end

% save('C:\Users\taoli\Documents\MATLAB\Robot\RBF_input_output', 'Error_K_s', 'Error_X_s', 'Error_Z_s', 'Error_X_d');
%%%%%%通过RBF神经网络对Error_X进行修正
% input=[Error_K_s;Error_X_s;Error_Z_s];
% output=Error_X_d;
% %[net,tr]=newrb(input,output,0,1,MN,DF);
% net=newrbe(input,output,1);

% input_1=[Error_K_s(:,1);Error_X_s(:,1);Error_Z_s(:,1)];
% ty=sim(net,input_1);
% delata_Error_X=Error_X_d(:,1)-ty

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%计算结果可视化分析%%%%%%%%%%%%%%%%
r=40;
figure(21)
[a,b]=size(Error_f_s);
step=[1:b]/r;
plot(step,Error_f_s(1,:),'r','DisplayName','P1_x特征误差')
hold on
plot(step,Error_f_s(2,:),'b','DisplayName','P1_y特征误差')
hold on
plot(step,Error_f_s(3,:),'Color','r','LineStyle','--','DisplayName','P2_x特征误差')
hold on
plot(step,Error_f_s(4,:),'b','LineStyle','--','DisplayName','P2_y特征误差')
hold on
plot(step,Error_f_s(5,:),'r','LineStyle',':','DisplayName','P3_x特征误差')
hold on
plot(step,Error_f_s(6,:),'b','LineStyle',':','DisplayName','P3_y特征误差')
hold on
plot(step,Error_f_s(7,:),'r','LineStyle','-.','DisplayName','P4_x特征误差')
hold on
plot(step,Error_f_s(8,:),'b','LineStyle','-.','DisplayName','P4_y特征误差')
hold on
title('图像特征误差')
xlabel('时间t/s')
ylabel('图像特征误差Error f')
grid on
legend('show','Location','best')
set(gcf,'Color',[1,1,1]);
%%%%%%

figure(22);
subplot(211)
[c,d]=size(v_s);
step=[1:d]/r;
plot(step,v_s(1,:),'r','LineStyle','-','DisplayName','末端运动线速度V_x')
hold on
plot(step,v_s(2,:),'g','LineStyle','--','DisplayName','末端运动线速度V_y')
hold on
plot(step,v_s(3,:),'Color','b','LineStyle','-.','DisplayName','末端运动线速度V_z')
hold on
title('相机笛卡尔空间运动线速度')
xlabel('时间t/s')
ylabel('末端相机线速度Camera_V')
set(gcf,'Color',[1,1,1]);
legend('show','Location','best')
grid on
%%%%%

subplot(212)
[c,d]=size(v_s);
step=[1:d]/r;
plot(step,v_s(4,:),'c','LineStyle','-','DisplayName','末端运动角速度W_x')
hold on
plot(step,v_s(5,:),'m','LineStyle','--','DisplayName','末端运动角速度W_y')
hold on
plot(step,v_s(6,:),'k','LineStyle','-.','DisplayName','末端运动角速度W_z')
hold on
title('相机笛卡尔空间运动角速度')
xlabel('时间t/s')
ylabel('末端相机角速度Camera_W')
set(gcf,'Color',[1,1,1]);
legend('show','Location','best')
grid on
%%%%%%

figure(20)
p_c_y_s(:,1)=p_c0_y;
[e,f]=size(p_c_y_s);
step=1:f;
plot(p_c_y_s(1,:),p_c_y_s(2,:),'-','LineWidth',1);
hold on
plot(p_c_y_s(3,:),p_c_y_s(4,:),'-');
hold on
plot(p_c_y_s(5,:),p_c_y_s(6,:),'-');
hold on
plot(p_c_y_s(7,:),p_c_y_s(8,:),'-');
hold on
% plot(p_c_y_s(3,i),p_c_y_s(4,i),'b')
% hold on
% plot(p_c_y_s(5,i),p_c_y_s(6,i),'Color','r','LineStyle','--')
% hold on
% plot(p_c_y_s(7,i),p_c_y_s(8,i),'b','LineStyle','--')
% hold on
title('图像空间特征点运动轨迹')
xlabel('u/pixel')
ylabel('v/pixel')
set(gcf,'Color',[1,1,1]);
axis([0 1024 0 1024]);
grid on
%%%%画起始点和目标点

for i=1:4
plot(p0(1,i),p0(2,i),'-pr','MarkerSize',10,'MarkerFaceColor','r');
hold on
plot(p_c0(1,i),p_c0(2,i),'-ob','MarkerSize',5,'MarkerFaceColor','b');
end
%%%将期望特征点点连成方形
% x1=2;y1=2;%A point 
% x2=4;y2=2;%B point 
% x3=4;y3=5;%C point 
% x4=2;y4=5;%D point 

line([p0(1,1) p0(1,2)],[p0(2,1) p0(2,2)],'Color','red','LineStyle' ,'--','lineWidth',1); 
line([p0(1,2) p0(1,3)],[p0(2,2) p0(2,3)],'Color','red','LineStyle' ,'--','lineWidth',1); 
line([p0(1,3) p0(1,4)],[p0(2,3) p0(2,4)],'Color','red','LineStyle' ,'--','lineWidth',1); 
line([p0(1,4) p0(1,1)],[p0(2,4) p0(2,1)],'Color','red','LineStyle' ,'--','lineWidth',1);
%%%%%%%%%将起始点连成方形
line([p_c0(1,1) p_c0(1,2)],[p_c0(2,1) p_c0(2,2)],'Color','b','LineStyle' ,'--','lineWidth',1); 
line([p_c0(1,2) p_c0(1,3)],[p_c0(2,2) p_c0(2,3)],'Color','b','LineStyle' ,'--','lineWidth',1); 
line([p_c0(1,3) p_c0(1,4)],[p_c0(2,3) p_c0(2,4)],'Color','b','LineStyle' ,'--','lineWidth',1); 
line([p_c0(1,4) p_c0(1,1)],[p_c0(2,4) p_c0(2,1)],'Color','b','LineStyle' ,'--','lineWidth',1);

%%%%%%

%%%%画三维空间相机路径%%%%%
figure(23)
% surf(X,Y,Z)
% RECTANGLE('Curvature'、[x y]、
plot3(P_C_s(3,:),P_C_s(2,:),P_C_s(1,:)+0.977,'DisplayName','末端运动轨迹');
hold on
plot3(P_C_s(3,1),P_C_s(2,1),P_C_s(1,1)+0.977,'ob','MarkerFaceColor','b','DisplayName','末端起始位姿');
hold on
plot3(P_C_s(3,end),P_C_s(2,end),P_C_s(1,end)+0.977,'pr','MarkerFaceColor','r','DisplayName','末端期望位姿');
hold on
legend('show','Location','best')
% p0 = [P_C_s(3,1) P_C_s(2,1) P_C_s(1,1)];
% p1 = [P_C_s(6,1) P_C_s(5,1) P_C_s(4,1)];
% vectarrow(p0,p1)
%%%%%相机空间运动轨迹在各平面内的投影
plot3(P_C_s(3,:),P_C_s(2,:),(P_C_s(1,:)+0.977)*0,'--','MarkerSize',1,'DisplayName','X-Y面轨迹投影');
hold on
plot3(P_C_s(3,:),P_C_s(2,:)*0+0.6,(P_C_s(1,:)+0.977),':','MarkerSize',1,'DisplayName','X-Z面轨迹投影');
hold on
plot3(P_C_s(3,:)*0,P_C_s(2,:),(P_C_s(1,:)+0.977),'-.','MarkerSize',1,'DisplayName','Y-Z面轨迹投影');
hold on

title('机器人基坐标系下末端相机运动轨迹')
xlabel('X_w/m')
ylabel('Y_w/m')
zlabel('Z_w/m')
grid on
legend('show','Location','best')
axis([0 1.6 -0.8 0.8 0 1.6]);
set(gcf,'Color',[1,1,1]);
% %%%%%绘制空间矩形
A=[1.6;0-0.1;0.977-0.075];
B=[1.6;0-0.1;0.977+0.075];
C=[1.6;0+0.1;0.977+0.075];
D=[1.6;0+0.1;0.977-0.075];
P = [B,A;C,D];
X = P([1,4],:);
Y = P([2,5],:);
Z = P([3,6],:);
h = surf(X,Y,Z,'DisplayName','油箱盖目标');
legend('show','Location','best')
hold on;
set(h,'FaceColor','b');