mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-13 10:37:54 -05:00
keep implementing
This commit is contained in:
Atsushi Sakai
@@ -1,12 +1,268 @@
|
||||
"""
|
||||
|
||||
author: Atsushi Sakai
|
||||
Mobile robot motion planning sample with Dynamic Window Approach
|
||||
|
||||
author: Atsushi Sakai (@Atsushi_twi)
|
||||
|
||||
|
||||
bstacleR=0.5;%衝突判定用の障害物の半径
|
||||
global dt; dt=0.1;%刻み時間[s]
|
||||
|
||||
%ロボットの力学モデル
|
||||
%[最高速度[m/s],最高回頭速度[rad/s],最高加減速度[m/ss],最高加減回頭速度[rad/ss],
|
||||
% 速度解像度[m/s],回頭速度解像度[rad/s]]
|
||||
Kinematic=[1.0,toRadian(20.0),0.2,toRadian(50.0),0.01,toRadian(1)];
|
||||
|
||||
%評価関数のパラメータ [heading,dist,velocity,predictDT]
|
||||
evalParam=[0.1,0.2,0.1,3.0];
|
||||
area=[-1 11 -1 11];%シミュレーションエリアの広さ [xmin xmax ymin ymax]
|
||||
|
||||
%シミュレーション結果
|
||||
result.x=[];
|
||||
tic;
|
||||
%movcount=0;
|
||||
% Main loop
|
||||
for i=1:5000
|
||||
%DWAによる入力値の計算
|
||||
[u,traj]=DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR);
|
||||
x=f(x,u);%運動モデルによる移動
|
||||
|
||||
%シミュレーション結果の保存
|
||||
result.x=[result.x; x'];
|
||||
|
||||
%ゴール判定
|
||||
if norm(x(1:2)-goal')<0.5
|
||||
disp('Arrive Goal!!');break;
|
||||
end
|
||||
|
||||
%====Animation====
|
||||
hold off;
|
||||
ArrowLength=0.5;%矢印の長さ
|
||||
%ロボット
|
||||
quiver(x(1),x(2),ArrowLength*cos(x(3)),ArrowLength*sin(x(3)),'ok');hold on;
|
||||
plot(result.x(:,1),result.x(:,2),'-b');hold on;
|
||||
plot(goal(1),goal(2),'*r');hold on;
|
||||
plot(obstacle(:,1),obstacle(:,2),'*k');hold on;
|
||||
%探索軌跡表示
|
||||
if ~isempty(traj)
|
||||
for it=1:length(traj(:,1))/5
|
||||
ind=1+(it-1)*5;
|
||||
plot(traj(ind,:),traj(ind+1,:),'-g');hold on;
|
||||
end
|
||||
end
|
||||
axis(area);
|
||||
grid on;
|
||||
drawnow;
|
||||
%movcount=movcount+1;
|
||||
%mov(movcount) = getframe(gcf);% アニメーションのフレームをゲットする
|
||||
end
|
||||
figure(2)
|
||||
plot(result.x(:,4));
|
||||
toc
|
||||
%movie2avi(mov,'movie.avi');
|
||||
|
||||
|
||||
function [u,trajDB]=DynamicWindowApproach(x,model,goal,evalParam,ob,R)
|
||||
%DWAによる入力値の計算をする関数
|
||||
|
||||
%Dynamic Window[vmin,vmax,ωmin,ωmax]の作成
|
||||
Vr=CalcDynamicWindow(x,model);
|
||||
%評価関数の計算
|
||||
[evalDB,trajDB]=Evaluation(x,Vr,goal,ob,R,model,evalParam);
|
||||
|
||||
if isempty(evalDB)
|
||||
disp('no path to goal!!');
|
||||
u=[0;0];return;
|
||||
end
|
||||
|
||||
%各評価関数の正規化
|
||||
evalDB=NormalizeEval(evalDB);
|
||||
|
||||
%最終評価値の計算
|
||||
feval=[];
|
||||
for id=1:length(evalDB(:,1))
|
||||
feval=[feval;evalParam(1:3)*evalDB(id,3:5)'];
|
||||
end
|
||||
evalDB=[evalDB feval];
|
||||
|
||||
[maxv,ind]=max(feval);%最も評価値が大きい入力値のインデックスを計算
|
||||
u=evalDB(ind,1:2)';%評価値が高い入力値を返す
|
||||
|
||||
function [evalDB,trajDB]=Evaluation(x,Vr,goal,ob,R,model,evalParam)
|
||||
%各パスに対して評価値を計算する関数
|
||||
evalDB=[];
|
||||
trajDB=[];
|
||||
|
||||
for vt=Vr(1):model(5):Vr(2)
|
||||
for ot=Vr(3):model(6):Vr(4)
|
||||
%軌跡の推定
|
||||
[xt,traj]=GenerateTrajectory(x,vt,ot,evalParam(4),model);
|
||||
%各評価関数の計算
|
||||
heading=CalcHeadingEval(xt,goal);
|
||||
dist=CalcDistEval(xt,ob,R);
|
||||
vel=abs(vt);
|
||||
|
||||
evalDB=[evalDB;[vt ot heading dist vel]];
|
||||
trajDB=[trajDB;traj];
|
||||
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)
|
||||
%軌跡データを作成する関数
|
||||
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=2;
|
||||
for io=1:length(ob(:,1))
|
||||
disttmp=norm(ob(io,:)-x(1:2)')-R;%パスの位置と障害物とのノルム誤差を計算
|
||||
if dist>disttmp%最小値を見つける
|
||||
dist=disttmp;
|
||||
end
|
||||
end
|
||||
|
||||
function heading=CalcHeadingEval(x,goal)
|
||||
%headingの評価関数を計算する関数
|
||||
|
||||
theta=toDegree(x(3));%ロボットの方位
|
||||
goalTheta=toDegree(atan2(goal(2)-x(2),goal(1)-x(1)));%ゴールの方位
|
||||
|
||||
if goalTheta>theta
|
||||
targetTheta=goalTheta-theta;%ゴールまでの方位差分[deg]
|
||||
else
|
||||
targetTheta=theta-goalTheta;%ゴールまでの方位差分[deg]
|
||||
end
|
||||
|
||||
heading=180-targetTheta;
|
||||
|
||||
function Vr=CalcDynamicWindow(x,model)
|
||||
%モデルと現在の状態からDyamicWindowを計算
|
||||
global dt;
|
||||
%車両モデルによるWindow
|
||||
Vs=[0 model(1) -model(2) model(2)];
|
||||
|
||||
%運動モデルによるWindow
|
||||
Vd=[x(4)-model(3)*dt x(4)+model(3)*dt x(5)-model(4)*dt x(5)+model(4)*dt];
|
||||
|
||||
%最終的なDynamic Windowの計算
|
||||
Vtmp=[Vs;Vd];
|
||||
Vr=[max(Vtmp(:,1)) min(Vtmp(:,2)) max(Vtmp(:,3)) min(Vtmp(:,4))];
|
||||
%[vmin,vmax,ωmin,ωmax]
|
||||
|
||||
function x = f(x, u)
|
||||
% Motion Model
|
||||
global dt;
|
||||
|
||||
F = [1 0 0 0 0
|
||||
0 1 0 0 0
|
||||
0 0 1 0 0
|
||||
0 0 0 0 0
|
||||
0 0 0 0 0];
|
||||
|
||||
B = [dt*cos(x(3)) 0
|
||||
dt*sin(x(3)) 0
|
||||
0 dt
|
||||
1 0
|
||||
0 1];
|
||||
|
||||
x= F*x+B*u;
|
||||
|
||||
function radian = toRadian(degree)
|
||||
% degree to radian
|
||||
radian = degree/180*pi;
|
||||
|
||||
function degree = toDegree(radian)
|
||||
% radian to degree
|
||||
degree = radian/pi*180;
|
||||
"""
|
||||
|
||||
import math
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
|
||||
def plot_arrow(x, y, yaw, length=1.0, width=0.5):
|
||||
plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
|
||||
head_length=width, head_width=width)
|
||||
plt.plot(x, y)
|
||||
|
||||
|
||||
def motion(x, u, dt):
|
||||
|
||||
x[0] += u[0] * math.cos(x[2]) * dt
|
||||
x[1] += u[0] * math.sin(x[2]) * dt
|
||||
x[2] += u[1] * dt
|
||||
|
||||
return x
|
||||
|
||||
|
||||
def main():
|
||||
print(__file__ + " start!!")
|
||||
|
||||
# initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
|
||||
x = np.array([0.0, 0.0, math.pi / 4.0])
|
||||
# goal position [x(m), y(m)]
|
||||
goal = np.array([10, 10])
|
||||
# obstacles [x(m) y(m), ....]
|
||||
ob = np.matrix([[0, 2],
|
||||
[4.0, 2.0],
|
||||
[5.0, 4.0],
|
||||
[5.0, 5.0],
|
||||
[5.0, 6.0],
|
||||
[5.0, 9.0],
|
||||
[8.0, 8.0],
|
||||
[8.0, 9.0],
|
||||
[7.0, 9.0]
|
||||
]) # 4 4
|
||||
|
||||
dt = 0.1
|
||||
u = np.array([1.0, 0.1])
|
||||
|
||||
for i in range(100):
|
||||
|
||||
x = motion(x, u, dt)
|
||||
|
||||
plt.cla()
|
||||
plt.plot(x[0], x[1], "xr")
|
||||
plt.plot(goal[0], goal[1], "xb")
|
||||
plt.plot(ob[:, 0], ob[:, 1], "ok")
|
||||
plot_arrow(x[0], x[1], x[2])
|
||||
plt.axis("equal")
|
||||
plt.grid(True)
|
||||
plt.pause(0.0001)
|
||||
|
||||
plt.show()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
||||
Reference in New Issue
Block a user