士大夫地方大师傅

最新推荐文章于 2024-09-16 15:57:34 发布
守护种花家
最新推荐文章于 2024-09-16 15:57:34 发布
阅读量143
点赞数 3
文章标签： java 前端算法
本文链接：https://blog.csdn.net/qq_38587963/article/details/141938897
版权
State_1
entry:
Obj_Fus_State = single(0);
Time_Obj_Same = 0;
during:
Obj_Fus_State = single(0);
if  Tgt_Dtct == 1 && abs(LatDistance) < 1.2 ...
        && Tgt_Id == Tgt_Id_Delay
    Time_Obj_Same = Time_Obj_Same + 0.02;
else
    Time_Obj_Same = 0;
end




[      Time_Obj_Same > 0.3 ...
 && Tgt_Dtct == 1 ...
 && Tgt_Id == Tgt_Id_Delay ...
 && Flag_BicCar == 0 ]


[     (g_ACC_Fus_ObjectSelectorFlag == paramACC_Selector_Cipv || g_ACC_Fus_ObjectSelectorFlag == paramACC_Selector_CutIn)  ...
 && Tgt_Dtct == 1  &&  Tgt_Id == Tgt_Id_Delay...
 && Tgt_Dtct_2 == 1 && min(Tgt_LngRate_2, VehSpd/3.6) - min(Tgt_LngRate, VehSpd/3.6) > 0.2 ...
 && Tgt_LngRng_2 - Tgt_LngRng > 1 &&  Tgt_LngRng_2 - Tgt_LngRng < max(2.5*VehSpd/3.6,15) ...
 && Th_real < 4 && Flag_BicCar == 0  ]




State_1a
entry:
Obj_Fus_State = single(0);
during:
Obj_Fus_State = single(0);




State_3
entry:
Obj_Fus_State = single(1);
during:
Obj_Fus_State = single(1);



[ Tgt_Id ~= Tgt_Id_Delay ...
|| Tgt_Dtct ~= 1 || Flag_BicCar ~= 0]



[ Tgt_Dtct_2 ~= 1 || Th_real > 4.5 || Flag_BicCar ~= 0 ]



[Tgt_Dtct ~= 1 || Tgt_Id ~= Tgt_Id_Delay || Flag_BicCar ~= 0]


[     abs(LatDistance) > 1.4 ...
&& Flag_BicCar == 0  ]


[  abs(LatDistance) < 1.1]



[  Tgt_Dtct ~= 1 || Tgt_Dtct_2 ~= 1 || Th_real > 4.5 || Tgt_Id ~= Tgt_Id_Delay...
|| min(Tgt_LngRate_2, VehSpd/3.6) - min(Tgt_LngRate, VehSpd/3.6) <= 0 || Flag_BicCar ~= 0  ]


State_4
entry:
Obj_Fus_State = single(2);
during:
Obj_Fus_State = single(2);




















function [Vdes_2,Vdes]= fcn(isDetected_2, Tgt_LngRng_2, Tgt_LngRate_2, Tgt_Spd_F_2, Th_Des_2, VehSpd, paramACC_StoppingDistance_o, paramACC_ESC_RespTime, ACC_SpdDt, Tgt_Spd_F, isDetected, distance_error_o, Tgt_LngRng, paramACC_MaxSpeedLimit_Out)
%计算出两个目标期望跟车速度
distance_error_o_2 = Tgt_LngRng_2 - Th_Des_2*VehSpd/3.6 - paramACC_StoppingDistance_o - max(Tgt_LngRate_2, 0) * paramACC_ESC_RespTime;%期望距离误差 = 相对距离 - 期望跟车距离 - 延迟距离
if isDetected == 1
    if  distance_error_o > 0
            %%%%%%%%%%%%%%%%
            %计算系数ratio_1和ratio_2，两个系数分别与自车车速和两车相对速度有关
            if VehSpd > paramACC_MaxSpeedLimit_Out  %% 功能未开启退出使能的最高车速   single((135-0.25)/1.04); (135,无穷大)
                ratio_1 = single(0.8);
            elseif VehSpd> 60  && VehSpd < paramACC_MaxSpeedLimit_Out  %(60,135)
                ratio_1 = 1 - 0.2*(VehSpd - 60)/(paramACC_MaxSpeedLimit_Out - 60) ;
            else%(0,60)
                ratio_1 = 1 + 0.2*(1 - VehSpd/60);
            end
            
            if VehSpd >= Tgt_Spd_F
                if VehSpd - Tgt_Spd_F >= 10
                    ratio_2 = single(0.7);
                else
                    ratio_2 = 1 - 0.3 * (VehSpd - Tgt_Spd_F)/10;
                end
            else
                ratio_2 = single(1);
            end
            %%%%%%%%%%%%%%%%
            Vdes_Derro_Limit_m = distance_error_o * VehSpd / Tgt_LngRng;
            if distance_error_o <= 5%(负无穷大,5]
                Vdes_Derro_Limit = min(single(2) * ratio_1 * ratio_2, Vdes_Derro_Limit_m);
            elseif distance_error_o <= 10 && distance_error_o > 5%(5,10]
                Vdes_Derro_Limit = min((2 + 3* (distance_error_o-5)/(10-5)) * ratio_1 * ratio_2, Vdes_Derro_Limit_m);
            elseif distance_error_o <= 30 && distance_error_o > 10%(10,30]
                Vdes_Derro_Limit = min((5 + 10* (distance_error_o-10)/(30-10)) * ratio_1 * ratio_2, Vdes_Derro_Limit_m);
            elseif distance_error_o <= 70 && distance_error_o > 30%(30,70]
                Vdes_Derro_Limit = min((15 + 10* (distance_error_o-30)/(70-30)) * ratio_1 * ratio_2, Vdes_Derro_Limit_m);
            else%(70,正无穷大)
                Vdes_Derro_Limit = min(single(25) * ratio_1 * ratio_2, Vdes_Derro_Limit_m);
            end
            
            Vdes_Follow_1 = max((Tgt_Spd_F + min(distance_error_o * VehSpd / Tgt_LngRng,  Vdes_Derro_Limit)),ACC_SpdDt);
            Vdes_Follow_2 = max((Tgt_Spd_F + distance_error_o * VehSpd / Tgt_LngRng) ,ACC_SpdDt);%理想跟车速度，一般理论上公式是这样子的
            if Tgt_Spd_F > 40%(40,正无穷大)
                Vdes = Vdes_Follow_1;
            elseif Tgt_Spd_F > 20 && Tgt_Spd_F <= 40%(20,40]
                Vdes = (Tgt_Spd_F-20)/20* Vdes_Follow_1 + (1 -(Tgt_Spd_F-20)/20)*Vdes_Follow_2;
            else%(负无穷大20]
                Vdes = Vdes_Follow_2;
            end
    else
    Vdes = max((Tgt_Spd_F + distance_error_o * VehSpd / Tgt_LngRng) ,ACC_SpdDt);
    end
else
    Vdes = VehSpd;
end




if isDetected_2 == 1
    if distance_error_o_2 > 0
        if VehSpd > paramACC_MaxSpeedLimit_Out
            ratio_1 = single(0.8);
        elseif VehSpd> 60  && VehSpd < paramACC_MaxSpeedLimit_Out
            ratio_1 = 1 - 0.2*(VehSpd - 60)/(paramACC_MaxSpeedLimit_Out - 60) ;
        else
            ratio_1 = 1 + 0.2*(1 - VehSpd/60);
        end
        
        if VehSpd >= Tgt_Spd_F_2
            if VehSpd - Tgt_Spd_F_2 >= 10
                ratio_2 = single(0.7);
            else
                ratio_2 = 1 - 0.3 * (VehSpd - Tgt_Spd_F_2)/10;
            end
        else
            ratio_2 = single(1);
        end
        Vdes_Derro_Limit_m = distance_error_o_2 * VehSpd / Tgt_LngRng;
        if distance_error_o_2 <= 5
            Vdes_Derro_Limit = min(single(2) * ratio_1 * ratio_2, Vdes_Derro_Limit_m);
        elseif distance_error_o_2 <= 10 && distance_error_o_2 > 5
            Vdes_Derro_Limit = min((2 + 3* (distance_error_o_2-5)/(10-5)) * ratio_1 * ratio_2, Vdes_Derro_Limit_m);
        elseif distance_error_o_2 <= 30 && distance_error_o_2 > 10
            Vdes_Derro_Limit = min((5 + 10* (distance_error_o_2-10)/(30-10)) * ratio_1 * ratio_2, Vdes_Derro_Limit_m);
        elseif distance_error_o_2 <= 70 && distance_error_o_2 > 30
            Vdes_Derro_Limit = min((15 + 10* (distance_error_o_2-30)/(70-30)) * ratio_1 * ratio_2, Vdes_Derro_Limit_m);
        else
            Vdes_Derro_Limit = min(single(25) * ratio_1 * ratio_2, Vdes_Derro_Limit_m);
        end
        Vdes_Follow_1 = max((Tgt_Spd_F_2 + min(distance_error_o_2 * VehSpd / Tgt_LngRng,  Vdes_Derro_Limit)),ACC_SpdDt);
        Vdes_Follow_2 = max((Tgt_Spd_F_2 + distance_error_o_2 * VehSpd / Tgt_LngRng) ,ACC_SpdDt);
        if Tgt_Spd_F_2 > 40
            Vdes_2 = Vdes_Follow_1;
        elseif Tgt_Spd_F_2 > 20 && Tgt_Spd_F_2 <= 40
            Vdes_2 = (Tgt_Spd_F_2-20)/20* Vdes_Follow_1 + (1 -(Tgt_Spd_F_2-20)/20)*Vdes_Follow_2;
            
        else
            Vdes_2 = Vdes_Follow_2;
        end
    else
        Vdes_2 = max(Tgt_Spd_F_2 + distance_error_o_2 * VehSpd / Tgt_LngRng_2, ACC_SpdDt);
    end
else
    Vdes_2 = VehSpd;
end

































State_0
entry:
Flag_BicCar = single(0);
Long_BicCar_First = single(-1);
during:
Flag_BicCar = single(0);
Long_BicCar_First = single(-1);






















[     isDetected == 1 ...
&& fusionId == fusionId_Delay ...
&& (type == paramACC_TgtCar || type == paramACC_TgtBus || type == paramACC_TgtTruck)  ...
&& isDetected_2 == 1 ...
&& fusionId_2 == fusionId_2_Delay ...
&& (type_2 == paramACC_TgtBicycle ||  type_2 == paramACC_TgtPed) ...
&& (longDistance_2 - longDistance > 1.5) ...
&& longDistance_2 - longDistance < max(2 * VehSpd/3.6, 15) ...
&& Tgt_Spd_F - Tgt_Spd_F_2 > 2 ...
&& Vdes_2 - VehSpd < -1 ...
&& Vdes - Vdes_2 > 2]


[  isDetected == 0 ...
|| fusionId ~= fusionId_Delay ...
|| (type ~= paramACC_TgtCar && type ~= paramACC_TgtBus && type ~= paramACC_TgtTruck)  ...
|| isDetected_2 == 0 ...
|| fusionId_2 ~= fusionId_2_Delay ...
|| (type_2 ~= paramACC_TgtBicycle &&  type_2 ~= paramACC_TgtPed) ...
|| longDistance_2 <= longDistance ...
|| longDistance_2 - longDistance > max(3 * VehSpd/3.6,  20) ...
|| Tgt_Spd_F - Tgt_Spd_F_2 <= 0 ...
|| Vdes_2 - VehSpd >= 1 ]





















[    isDetected == 1 ...
&& fusionId == fusionId_Delay ...
&& (type == paramACC_TgtBicycle ||  type_2 == paramACC_TgtPed) ...
&& isDetected_2 == 1 ...
&& fusionId_2 == fusionId_2_Delay ...
&& (type_2 == paramACC_TgtCar || type == paramACC_TgtBus || type == paramACC_TgtTruck)  ...
&& longDistance_2 - longDistance > 1.5 ...
&& longDistance_2 - longDistance < max(1.5 * VehSpd/3.6, 10) ...
&& Vdes_2 - VehSpd < -1 ]




[    isDetected == 0 ...
|| fusionId ~= fusionId_Delay ...
|| (type ~= paramACC_TgtBicycle &&  type_2 ~= paramACC_TgtPed) ...
|| isDetected_2 == 0 ...
|| fusionId_2 ~= fusionId_2_Delay ...
|| (type_2 ~= paramACC_TgtCar && type ~= paramACC_TgtBus && type ~= paramACC_TgtTruck)  ...
|| longDistance_2 <= longDistance ...
|| longDistance_2 - longDistance > max(3 * VehSpd/3.6, 15) ...
|| Vdes_2 - VehSpd >= 1 ]



State_1
entry:
Flag_BicCar = single(1);
Long_BicCar_First = longDistance_2 - longDistance;
during:
Flag_BicCar = single(1);



State_2
entry:
Flag_BicCar = single(2);
Long_BicCar_First = single(-1);
during:
Flag_BicCar = single(2);