人机混合增强智能——人在环路的漂移扩散决策模型HDDM

%% Complied by Ezekiel Mok according to the paper of HDM behavior model
% single robot about the task-move_to_goal and obstacle_avoidance
% movtogoal任务有跟踪曲线
% 20210324
clear
clc
close all
%% Set up working field
figure('position', [300, 50, 500, 500]);
axis equal;
axis square
hold on
%% START Obstacle End Point
num=3;% 智能体数量
P_START=[1 8;5 6;4 1];
P_OBS=[10.2 12.3;8.8 6.4;13.8 6.4;8.8 8.8;17.823 13.956;17.823 11.556;];%[8 11;8.8 8.6;10 4;8.8 6.6]
% 画曲线[17.8230000000000,12.7560000000000]
Point_num = 2000;
ts = 14/Point_num;
count = 1;
store = zeros(Point_num,2);
store1 = zeros(Point_num,2);
store2 = zeros(Point_num,2 );
 for count = 1:1:Point_num
 t=ts*(count-1);
store(count,:) =   [1.4*t+2; 0.8*t+8];
store1(count,:) =  [1.4*t+6; 0.8*t+6];
store2(count,:) =  [1.4*t+5; 0.8*t+1];%需要机器人跟踪的任务函数，如何设计跟踪
 end
P_END=[store(Point_num,1) store(Point_num,2);store1(Point_num,1) store1(Point_num,2);store2(Point_num,1) store2(Point_num,2)];
%% task variable definite
x_goal = zeros(1,num);
y_goal = zeros(1,num);
x_obs = zeros(1,length(P_OBS));
y_obs =zeros(1,length(P_OBS));
vxobs = zeros(1,num);%存储离障碍物的x距离
vyobs = zeros(1,num);%存储离障碍物的y距离
x_dot = zeros(1,num);
y_dot = zeros(1,num);%实时位置，初始定义为起点
landa1 = zeros(1,num);
landa2 = zeros(1,num);%定义任务增益
landa3 = zeros(1,num);
vxend = zeros(1,num);%指向goal的x向量
vyend = zeros(1,num);%指向goal的y向量
dend = zeros(1,num);%定义距离终点的距离
%%
Jacob1 = zeros(2,2);%关于任务变量2的雅克比矩阵
Jacob2 = zeros(2,2*num);%关于任务变量2的雅克比矩阵
N_2 = zeros(2,2*num);
Jacob3 = zeros(num,2);%关于任务变量3的雅克比矩阵
N_3 = zeros(2,2*num);
Pseudo_Jacob3  = zeros(2,num);
%%
v_task1 = zeros(2,num);%关于任务变量2的雅克比矩阵';%任务2的速度向量
v_task2 = zeros(2,num);%关于任务变量2的雅克比矩阵';%任务2的速度向量
v_task3 = zeros(2,num);%任务3的速度向量
V_desired = zeros(2,num);%总任务输出的速度
d_obs = zeros(1,num);%与障碍物的安全距离
D_obs_p = zeros(1,num);%定义当前位置与障碍物的距离
%%
for i=1:1:num
x_goal(i)=P_END(i,1);
y_goal(i)=P_END(i,2);
vxobs(i) = 0;%存储离障碍物的x距离
vyobs(i) = 0;%存储离障碍物的y距离
x_dot(i)=P_START(i,1);
y_dot(i)=P_START(i,2);%实时位置，初始定义为起点
landa1(i)=16;
landa2(i)=4;%定义任务增益
landa3(i)=15;
d_obs(i)=1.5;%与障碍物的安全距离
end
for i=1:1:length(P_OBS)
x_obs(i) =P_OBS(i,1);
y_obs(i) =P_OBS(i,2);
end
 %% 仿真运行
j=1;
deta_PG=zeros(2,num);
temp_jacob=zeros(2,2);
R_Vec = zeros(2,num);
t = 0;%初始化时间参数t
Plot_datax = zeros(num,Point_num);
Plot_datay = zeros(num,Point_num);
time_point = zeros(1,Point_num);
tracking_error =zeros(num,Point_num);
Task_Swith = zeros(num,Point_num);%记录切换顺序
Agent_Obs= zeros(num,Point_num);%记录切换顺序
item = zeros(1,Point_num);
Pobs_emg = 0;
human_in1 = 0;
human_in2 = 0;
state_wait1 = 0;
state_wait2 = 0;
remark1 = 0;
remark2 = 0;
remark3 = 0;
remark4 = 0;
is_end1 = 0;
is_end2 = 0;
is_end3 = 0;
is_end4 = 0;
 %% Navigation Contoller 
 %End location attraction move_to_goal task
 for count = 1:1:Point_num
 t=ts*(count-1);
 time_point(count) = t;
 E=wgn(1,1,1);
 e=E/(1*40);% 为什么要加个高斯噪声
 P_rd =  [[1.4*t+2  ;0.8*t+8];
          [1.4*t+6;0.8*t+6];
          [1.4*t+5; 0.8*t+1]];%需要机器人跟踪的任务函数，如何设计跟踪
 %% Navigation Contoller 
 %% End location attraction move_to_goal task
 for i = 1:1:num
 x_goal(i) = P_rd(2*i-1);%时刻更新导航目标
 y_goal(i) = P_rd(2*i);
 tracking_error(i,count)=sqrt((P_rd(2*i-1)-x_dot(i))^2+(P_rd(2*i)-y_dot(i))^2);
 vxend(i) = x_goal(i) - x_dot(i);
 vyend(i) = y_goal(i) - y_dot(i);
 dend(i) = sqrt(vxend(i)^2 + vyend(i)^2)+e;%控制量偏差的范数
 deta_PG(:,i)=[vxend(i),vyend(i)]';%运动学偏差，作为控制量
 Jacob2(1:2,2*i-1:2*i) = eye(2,2);%move_to_goal任务的雅克比矩阵为单位阵，移动任务的雅克比矩阵
 p_task = [1.4,0.8;
            1.4,0.8;
            1.4,0.8];%任务函数导数
 v_task2(:,i) = Jacob2(1:2,2*i-1:2*i)*( p_task(i,:)' + landa2(i).* deta_PG(:,i));%实时计算move_to_goal的速度向量，闭环运动学
 %% obstacle aviodance task

 D_obs_p(i)=sqrt( vxobs(i)^2+vyobs(i)^2)+e;
 R_Vec(:,i)=[vxobs(i)/D_obs_p(i); vyobs(i)/D_obs_p(i)]; 
 Jacob3(i,:)= R_Vec(:,i)';%避障任务的雅克比矩阵
 Pseudo_Jacob3(:,i)=pinv(Jacob3(i,:)); %求避障任务的右伪逆%处理数组的时候得注意
 v_task3(:,i)=Pseudo_Jacob3(:,i)*landa3(i).*(d_obs(i)-D_obs_p(i));
 vxobs(i) = x_dot(i) - x_obs(i);
 vyobs(i) = y_dot(i) - y_obs(i);
 if i == 2
     dvod1x = x_dot(2) - x_obs(2);
     dvod1y = y_dot(2) - y_obs(2);
     dvod2x = x_dot(2) - x_obs(4);
     dvod2y = y_dot(2) - y_obs(4);
     dvod3x = x_dot(2) - x_obs(5);
     dvod3y = y_dot(2) - y_obs(5);
     dvod4x = x_dot(2) - x_obs(6);
     dvod4y = y_dot(2) - y_obs(6);
     dvod1 = sqrt(dvod1x^2 + dvod1y^2);
     dvod2 = sqrt(dvod2x^2 + dvod2y^2);
     dvod3 = sqrt(dvod3x^2 + dvod3y^2);
     dvod4 = sqrt(dvod4x^2 + dvod4y^2);
     Judge1 = [dvod1;dvod2;dvod3;dvod4];
     if human_in1 == 0
         if dvod1 - dvod2 <0.001&&dvod1<1.505 &&dvod2<1.505%不能决策
             v_task2(:,2) = [0 0]';
             v_task3(:,2) =[0 0]';
             %获取当下时间触发干预任务
              get_humanx1 = x_dot(2);
              get_humany1 = y_dot(2);
              if remark1 == 0
                 remark1 = count;
              end
              remark3 = count;
         end
     end
     if human_in2 == 0
          if dvod3 - dvod4 <0.001&&dvod3<1.505 &&dvod4<1.505 %不能决策
             v_task2(:,2) = [0 0]';
             v_task3(:,2) =[0 0]';
             get_humanx2 = x_dot(2);
             get_humany2 = y_dot(2);
            if remark2 == 0
               remark2 = count;
            end
            remark4 = count;
          end 
     end
     if dvod1 - dvod2 <0.001&&dvod1<1.505 &&dvod2<1.505%记录首次大于的干预点
          state_wait1 = 1;
         if tracking_error(2,count) >4%%漂移扩散阈值
             human_in1 = 1;
         end
     elseif dvod1>1.505 &&dvod2>1.505
         state_wait1 = 0;
         human_in1 = 0;
     end   
     if dvod3 - dvod4 <0.001&&dvod3<1.505&&dvod4<1.505%记录首次大于的干预点
         state_wait2 = 1;
         if tracking_error(2,count) >4%漂移扩散阈值
            human_in2 = 1;
         end
    elseif dvod3>1.505&&dvod4>1.505
         human_in2 = 0;
         state_wait2 = 0;
    end   
     [item1,item2] = find(Judge1 == min(min(Judge1)));
     %% 遇到局部极值点的情况
     if item1 == 1 
         vxobs(2) = x_dot(2) - x_obs(item1 + 1);
         vyobs(2) = y_dot(2) - y_obs(item1 + 1);
     else
         vxobs(2) = x_dot(2) - x_obs(item1 + 2);
         vyobs(2) = y_dot(2) - y_obs(item1 + 2);
     end
 end
 Agent_Obs(i,count) = (vxobs(i))^2 + (vyobs(i))^2;
 %% 人为干预的任务描述
     v_task1 = [1.4 0]';%实时计算move_to_goal的速度向量
     Jacob1 =eye(2,2);
 %% 利用NSB对任务进行综合
 N_1 = eye(2,2) - pinv(Jacob1)*Jacob1;
 N_2(1:2,2*i-1:2*i)=eye(2,2)-pinv(Jacob2(1:2,2*i-1:2*i))*Jacob2(1:2,2*i-1:2*i);%任务2的零空间
 N_3(1:2,2*i-1:2*i)=eye(2,2)-Pseudo_Jacob3(:,i)*( Jacob3(i,:)); %任务3的零空间
 %N_human(1:2,2*i-1:2*i)=eye(2,2) 
 % 优先级仲裁函数，当未到达障碍物警戒空间时，movetogoal任务优先级>obstacle>avoidance
 % 当到达障碍物警戒空间时，movetogoal任务优先级>obstacle>avoidance
 % 与指向目标向量与指向障碍向量正切时候movetogoal任务优先级>obstacle>avoidance
 % 加一个人的任务
 % 人的任务定义
 if D_obs_p(i) > d_obs(i)
   V_desired(:,i)=v_task2(:,i) +  N_2(1:2,2*i-1:2*i)*v_task3(:,i); 
   Task_Swith(i,count) = 0.5;
 else 
   V_desired(:,i)=v_task3(:,i) +  N_3(1:2,2*i-1:2*i)*v_task2(:,i);
   Task_Swith(i,count) = 1.0;
 end
if  i == 2
     if  state_wait1 == 0
             if D_obs_p(i) > d_obs(i)
               V_desired(:,i)=v_task2(:,i) +  N_2(1:2,2*i-1:2*i)*v_task3(:,i); 
               Task_Swith(i,count) = 0.5;
             else 
               V_desired(:,i)=v_task3(:,i) +  N_3(1:2,2*i-1:2*i)*v_task2(:,i);
               Task_Swith(i,count) = 1.0;
             end
     end
     if state_wait2 == 0%等待任务
            if D_obs_p(i) > d_obs(i)
               V_desired(:,i)=v_task2(:,i) +  N_2(1:2,2*i-1:2*i)*v_task3(:,i); 
               Task_Swith(i,count) = 0.5;
             else 
               V_desired(:,i)=v_task3(:,i) +  N_3(1:2,2*i-1:2*i)*v_task2(:,i);
               Task_Swith(i,count) = 1.0;
            end
     end
     if  state_wait1 == 1
            Task_Swith(i,count) = 1.5;
            V_desired(:,i) = [0, 0]';
     end
     if  state_wait2 == 1
            Task_Swith(i,count) = 1.5;
            V_desired(:,i) = [0, 0]';
     end
end
 if human_in1 == 1
     V_desired(:,2)=v_task1 + N_1*(v_task3(:,2) +  N_3(1:2,2*2-1:2*2)*v_task2(:,2));
     Task_Swith(2,count) = 2.0;
     if is_end1 == 0
        is_end1 = count;%%获知干预点结束点坐标
     end
     is_end3 = count;
 end
 
 if human_in2 == 1
     V_desired(:,2)=v_task1 + N_1*(v_task3(:,2) +  N_3(1:2,2*2-1:2*2)*v_task2(:,2));
     Task_Swith(2,count) = 2.0;
     if is_end2 == 0
        is_end2 = count;%%获知干预点结束点坐标
     end
     is_end4 = count;
 end
 x_dot(i) = x_dot(i) + V_desired(1,i).*ts;%任务变量相当于速度对时间进行积分
 y_dot(i) = y_dot(i) + V_desired(2,i).*ts;%任务变量相当于速度对时间进行积分
 %% 动画绘制
 Plot_datax(i,j) =  x_dot(i); 
 Plot_datay(i,j) =  y_dot(i);
 end
 j=j+1;
%% approach the goal point，stop and break
 end
 human_inter_end1 = [Plot_datax(2,is_end3 ),Plot_datay(2,is_end3)];
 human_inter_end2 = [Plot_datax(2,is_end4 ),Plot_datay(2,is_end4)];
 %human_inter_end1 = [Plot_datax(2,remark1 + is_end1-1),Plot_datay(2,remark1 + is_end1-1)];
%  human_inter_end2 = [Plot_datax(2,remark2 + is_end2),Plot_datay(2,remark2 + is_end2)];
% trajectory plot
figure(1)
plot ( Plot_datax(1,1:j-1), Plot_datay(1,1:j-1),'-b',Plot_datax(2,1:j-1), Plot_datay(2,1:j-1),'-g',Plot_datax(3,1:j-1), Plot_datay(3,1:j-1),'-y','linewidth', 1.5);
hold on  %'bd'菱形
%预绘制
plot(P_START(1,1),P_START(1,2),'ms','markersize',5, 'linewidth', 2);
plot(P_START(2,1),P_START(2,2),'ms','markersize',5, 'linewidth', 2);
plot(P_START(3,1),P_START(3,2),'ms','markersize',5, 'linewidth', 2);
for i=1:3
    plot(P_END(i,1),P_END(i,2),'ms','markersize',5, 'linewidth', 2);
end
plot(P_OBS(1,1),P_OBS(1,2),'o', 'markersize',5, 'linewidth', 2);
plot(P_OBS(2,1),P_OBS(2,2),'o', 'markersize',5, 'linewidth', 2);
plot(P_OBS(3,1),P_OBS(3,2),'o', 'markersize',5, 'linewidth', 2);
plot(P_OBS(4,1),P_OBS(4,2),'bp', 'markersize',5, 'linewidth', 2);
plot(P_OBS(5,1),P_OBS(5,2),'bp', 'markersize',5, 'linewidth', 2);
plot(P_OBS(6,1),P_OBS(6,2),'o', 'markersize',5, 'linewidth', 2);
hold on
plot(get_humanx1,get_humany1,'ms',get_humanx2, get_humany2,'ms')
plot(store(:,1),store(:,2),'--',store1(:,1),store1(:,2),'--',store2(:,1),store2(:,2),'--')
hold on
plot(human_inter_end1(1), human_inter_end1(2),'ms', human_inter_end2(1), human_inter_end2(2),'ms');
hold on
% legend('Robot 1','Robot 2','Robot 3','Location','NorthEast');
axis([0,26,0,26])
xlabel('x')
ylabel('y')
grid on
grid minor
for k = 1:10:Point_num
     for i = 1:1:3
        plot(Plot_datax(i,k), Plot_datay(i,k), 'ms','markersize',3.5, 'linewidth', 1);
        hold on
        drawnow
     end
   %pause(0.001)
end
figure('position', [300, 50, 500, 500]);
linebar =4.*ones(Point_num,1);
plot (time_point, linebar, time_point,tracking_error(1,:),'-b',time_point,tracking_error(2,:),'-g',time_point,tracking_error(3,:),'-y','linewidth',1.5);
xlabel('t')
ylabel('tracking_error')
%legend('Robot 1','Robot 2','Robot 3');
%set(gca,'XTickLabel',{'0','2','4','6','8','10','12','14','8'})
grid on
grid minor
figure(3)
linebar1 =2.*ones(Point_num,1);
linebar2 =0.*ones(Point_num,1);
linebar3 =1.*ones(Point_num,1);
subplot(3,1,1)
plot (time_point,Task_Swith(1,:),'-b','linewidth',1.5);
axis([0,time_point(Point_num),0,2.5])
grid on
grid minor
subplot(3,1,2)
plot (time_point,Task_Swith(2,:),'-g','linewidth',1.5);
axis([0,time_point(Point_num),0,2.5])
grid on
grid minor
subplot(3,1,3)
plot (time_point,Task_Swith(3,:),'-y','linewidth',1.5);
axis([0,time_point(Point_num),0,2.5])
grid on
grid minor
figure(6)
plot (time_point,Agent_Obs(1,:),'-b',time_point,Agent_Obs(2,:),'-y',time_point,Agent_Obs(3,:),'-g','linewidth',0.5);
% 跟踪误差还有人为干预任务的设计
grid on
grid minor
% %% 引力与斥力定义
density = 0.4;
Grid_X = 0:density:25;
Grid_Y = 0:density:25;
Field_re = zeros(length(Grid_X),length(Grid_Y));
Field_at = zeros(length(Grid_X),length(Grid_Y));
Obs_distance = zeros(1,length(P_OBS));
Y_re = zeros(1,length(P_OBS));
Basic_Z = ones(length(Grid_X), length(Grid_Y));%环境网格化
P0 = 1.5;
a = 100; %斥力影响因素
%%
Goal = P_END(2,:); %目标
 for k1 = 1: length(Grid_Y)
     
      Y_c = Grid_Y(k1);%遍历网格 
     for k2 =1:length(Grid_X)
         X_c = Grid_X(k2);
         for k3 = 1:1:length(P_OBS)
             Obs_distance(k3) = sqrt((P_OBS(k3,1)-X_c)^2 +(P_OBS(k3,2)-Y_c)^2);         
             if Obs_distance(k3) <= P0
                Y_re(k3) = 0.5*a*(1/Obs_distance(k3)  - 1/P0 )^2;%基本斥力场公式   
             else
                 Y_re(k3) =0;
             end
         end
         Field_re(k1,k2) =sum(Y_re);
         if  isinf(Field_re(k1,k2))==1|| Field_re(k1,k2) > 30 %为显示效果做的限制处理
              Field_re(k1,k2) = 30;
         end
     end
 end
SUM = Field_re;
figure(4)
surf(Grid_X,Grid_Y,SUM) %总力场
axis([0,25,0,25,0,100])
% figure(5)
% mesh(Grid_X,Grid_Y,SUM) %总力场
xlabel('x','Fontsize',14);
ylabel('y','Fontsize',14);
zlabel('z','Fontsize',14);
%% 绘制切换任务图，从txt里面导出图