% 軌跡推測;?得到?xt:?機(jī)器人向前運(yùn)動(dòng)后的預(yù)測位姿;?traj:?當(dāng)前時(shí)刻?到?預(yù)測時(shí)刻之間的軌跡 ?

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

%?各評(píng)價(jià)函數(shù)的計(jì)算??

heading=CalcHeadingEval(xt,goal);??

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

vel=abs(vt);??

%?制動(dòng)距離的計(jì)算??

stopDist=CalcBreakingDist(vel,model);??

if?dist>stopDist?%???

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

trajDB=[trajDB;traj];??

end??

end??

end??

?

function?EvalDB=NormalizeEval(EvalDB)??

%?評(píng)價(jià)函數(shù)正則化??

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)??

%?軌跡生成函數(shù)??

%?evaldt：前向模擬時(shí)間;?vt、ot當(dāng)前速度和角速度;???

global?dt;??

time=0;??

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

traj=x;%?機(jī)器人軌跡??

while?time<=evaldt??

time=time+dt;%?時(shí)間更新??

x=f(x,u);%?運(yùn)動(dòng)更新??

traj=[traj?x];??

end??

?

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

%?根據(jù)運(yùn)動(dòng)學(xué)模型計(jì)算制動(dòng)距離,這個(gè)制動(dòng)距離并沒有考慮旋轉(zhuǎn)速度，不精確吧?。。??

global?dt;??

stopDist=0;??

while?vel>0??

stopDist=stopDist+vel*dt;%?制動(dòng)距離的計(jì)算??

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

end??

?

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

%?障礙物距離評(píng)價(jià)函數(shù)??

?

dist=100;??

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

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

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

dist=disttmp;??

end??

end??

?

%?障礙物距離評(píng)價(jià)限定一個(gè)最大值，如果不設(shè)定，一旦一條軌跡沒有障礙物，將太占比重??

if?dist>=2*R??

dist=2*R;??

end??

?

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

%?heading的評(píng)價(jià)函數(shù)計(jì)算??

?

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