% 軌跡推測;?得到?xt:?機器人向前運動后的預測位姿;?traj:?當前時刻?到?預測時刻之間的軌跡 ?

[xt,traj]=GenerateTrajectory(x,vt,ot,evalParam(4),model);??%evalParam(4),前向模擬時間;??

%?各評價函數的計算??

heading=CalcHeadingEval(xt,goal);??

dist=CalcDistEval(xt,ob,R);??

vel=abs(vt);??

%?制動距離的計算??

stopDist=CalcBreakingDist(vel,model);??

if?dist>stopDist?%???

evalDB=[evalDB;[vt?ot?heading?dist?vel]];??

trajDB=[trajDB;traj];??

end??

end??

end??

?

function?EvalDB=NormalizeEval(EvalDB)??

%?評價函數正則化??

if?sum(EvalDB(:,3))~=0??

EvalDB(:,3)=EvalDB(:,3)/sum(EvalDB(:,3));??

end??

if?sum(EvalDB(:,4))~=0??

EvalDB(:,4)=EvalDB(:,4)/sum(EvalDB(:,4));??

end??

if?sum(EvalDB(:,5))~=0??

EvalDB(:,5)=EvalDB(:,5)/sum(EvalDB(:,5));??

end??

?

function?[x,traj]=GenerateTrajectory(x,vt,ot,evaldt,model)??

%?軌跡生成函數??

%?evaldt：前向模擬時間;?vt、ot當前速度和角速度;???

global?dt;??

time=0;??

u=[vt;ot];%?輸入值??

traj=x;%?機器人軌跡??

while?time<=evaldt??

time=time+dt;%?時間更新??

x=f(x,u);%?運動更新??

traj=[traj?x];??

end??

?

function?stopDist=CalcBreakingDist(vel,model)??

%?根據運動學模型計算制動距離,這個制動距離并沒有考慮旋轉速度，不精確吧！??！??

global?dt;??

stopDist=0;??

while?vel>0??

stopDist=stopDist+vel*dt;%?制動距離的計算??

vel=vel-model(3)*dt;%???

end??

?

function?dist=CalcDistEval(x,ob,R)??

%?障礙物距離評價函數??

?

dist=100;??

for?io=1:length(ob(:,1))??

disttmp=norm(ob(io,:)-x(1:2)')-R;%僷僗偺埵抲偲忈奞暔偲偺僲儖儉岆嵎傪寁嶼??

if?dist>disttmp%?離障礙物最小的距離??

dist=disttmp;??

end??

end??

?

%?障礙物距離評價限定一個最大值，如果不設定，一旦一條軌跡沒有障礙物，將太占比重??

if?dist>=2*R??

dist=2*R;??

end??

?

function?heading=CalcHeadingEval(x,goal)??

%?heading的評價函數計算??

?

theta=toDegree(x(3));%?機器人朝向??