0 Introduction to the column
With a full set of C++/Python/Matlab code Essential for course design, graduation design, and innovation competitions! Detailed introduction to global planning (graph search, sampling method, intelligent algorithms, etc.); local planning (DWA, APF, etc.); curve optimization (Bessel curves, B spline curves, etc.).
Details:
Motion Planning in Autonomous Driving, with dozens of planning algorithms.
1 Dynamic obstacle modeling
One of the ultimate goals of indoor mobile robotics research is to build robots that can safely perform tasks in hazardous and densely populated environments. For example, service robots assisting humans in indoor office environments should be able to react quickly to unexpected changes and accomplish their tasks in a variety of external situations. However, most motion planners rely on accurate, static modeling of the environment, so they tend to stop working if humans or other unpredictable obstacles block their path. In order to build autonomous mobile robots, we need to build systems that can sense the environment, react to unpredictable situations, and plan dynamically to accomplish tasks
Dynamic Window Approach (DWA)It has emerged as a widely used algorithm in the field of robot path planning and navigation, with a background of applications that can be traced back to driverless vehicles, autonomous mobile robots, and other intelligent systems that need to move efficiently in complex environments.DWA achieves fast path planning and collision avoidance decision making by modeling the robot’s motion state and environmental information as a dynamic window. The algorithm utilizes heuristics to select the optimal action to avoid collisions with obstacles and reach the goal point as quickly as possible by evaluating the possible combinations of speed and direction for the current robot state.
2 DWA Fundamentals
2.1 Sampling window
DWA is concerned about a problem
How to reactively avoid collisions with static/dynamic obstacles?
DWA is specifically designed to handle constraints imposed by finite velocities and accelerations that are derived directly from the kinematic model of the mobile robot. In short, DWA only periodically considers a short time interval in the computation of the next control command in order to avoid the huge complexity associated with dealing with general motion planning problems. During such a time interval, a two-dimensional search space, including translational and rotational velocities, is obtained by approximating the trajectory as circular curvature. This search space is reduced to a legal velocity that allows the robot to stop safely. The velocities are further constrained due to finite acceleration: the robot only considers velocities that are reachable in the next time interval. These velocities form a dynamic window, which is centered around the robot’s current velocity in velocity space.
In DWA, construct a sampling window as follows
{
w
i
n
(
v
)
=
[
max
{
v
min
,
v
−
a
v
d
t
}
,
min
{
v
max
,
v
+
a
v
d
t
}
]
w
i
n
(
ω
)
=
[
max
{
ω
min
,
ω
−
a
ω
d
t
}
,
min
{
ω
max
,
ω
+
a
ω
d
t
}
]
\begin{cases} win\left( v \right) =\left[ \max \left\{ v_{\min},v-a_v\mathrm{d}t \right\} , \min \left\{ v_{\max}, v+a_v\mathrm{d}t \right\} \right]\\ win\left( \omega \right) =\left[ \max \left\{ \omega _{\min},\omega -a_{\omega}\mathrm{d}t \right\} , \min \left\{ \omega _{\max}, \omega +a_{\omega}\mathrm{d}t \right\} \right]\\\end{cases}
{win(v)=[max{vmin,v−avdt},min{vmax,v+avdt}]win(ω)=[max{ωmin,ω−aωdt},min{ωmax,ω+aωdt}]
2.2 Evaluation function
Then sample binary point pairs within the window
(
v
,
ω
)
\left( v,\omega \right)
(v,ω)Setting the simulation time
t
t
t, which simulates the robot starting from the current position with
(
v
,
ω
)
\left( v,\omega \right)
(v,ω)Constant velocity travel trajectory and evaluate the trajectory
G
(
v
,
ω
)
=
α
⋅
h
e
a
d
i
n
g
(
v
,
ω
)
+
β
⋅
d
i
s
t
(
v
,
ω
)
+
γ
⋅
v
e
l
o
c
i
t
y
(
v
,
ω
)
G\left( v,\omega \right) =\alpha \cdot \mathrm{heading}\left( v,\omega \right) +\beta \cdot \mathrm{dist}\left( v,\omega \right) +\gamma \cdot \mathrm{velocity}\left( v,\omega \right)
G(v,ω)=α⋅heading(v,ω)+β⋅dist(v,ω)+γ⋅velocity(v,ω)
where the direction angle evaluation function
h
e
a
d
i
n
g
(
v
,
ω
)
=
π
−
∣
θ
g
o
a
l
−
θ
∗
∣
\mathrm{heading}\left( v,\omega \right) =\pi -\left| \theta _{\mathrm{goal}}-\theta ^* \right|
heading(v,ω)=π−∣θgoal−θ∗∣
included among these
θ
g
o
a
l
\theta _{\mathrm{goal}}
θgoalis the distance between the robot’s end of the simulated trajectory and the end point of the line connected to the
x
x
xThe angle between the axes of the
θ
∗
\theta ^*
θ∗is the robot’s attitude angle at the end of the simulated trajectory, aiming at selecting the trajectory with a small difference between the target angle and the pointing angle, so that the robot’s forward direction is aligned with the end point. Obstacle evaluation function
d
i
s
t
(
v
,
ω
)
=
{
d
0
,
d
>
d
0
d
,
d
s
a
f
e
⩽
d
⩽
d
0
0
,
d
<
d
s
a
f
e
\mathrm{dist}\left( v,\omega \right) =\begin{cases} d_0\,\,, d>d_0\\ d\,\, , d_{\mathrm{safe}}\leqslant d\leqslant d_0\\ 0 , d<d_{\mathrm{safe}}\\\end{cases}
dist(v,ω)=⎩
⎨
⎧d0,d>d0d,dsafe⩽d⩽d00,d<dsafe
aiming at selecting trajectories that are farther away from obstacles to avoid collisions during robot operation.
d
d
dIndicates the closest distance of the predicted trajectory to the obstacle;
d
0
d_0
d0is the maximum value of the obstacle score, and if the predicted trajectory and obstacle spacing exceeds
d
0
d_0
d0Then the track is considered safe;
d
s
a
f
e
d_{\mathrm{safe}}
dsafeis the safe distance between the robot and the obstacle, if the predicted trajectory and obstacle distance are less than
d
s
a
f
e
d_{\mathrm{safe}}
dsafethen the trajectory is considered to be in collision. The velocity evaluation function
v
e
l
o
c
i
t
y
(
v
,
ω
)
=
∣
v
∣
\mathrm{velocity}\left( v,\omega \right) =\left| v \right|
velocity(v,ω)=∣v∣
The aim is to select the trajectory with larger linear velocity so that the robot moves as fast as possible. In summary, the physical meaning of the evaluation function is to make the robot move towards the target with a faster speed and perform autonomous obstacle avoidance, and the evaluation function and its weight coefficients can be changed according to the needs in practical applications
3 DWA Algorithm Flow
The DWA algorithm flow is shown below
4 Simulation Realization
4.1 ROS C++ Implementation
Constructing Dynamic Windows
generator_.initialise(pos,
vel,
goal,
&limits,
vsamples_);
Inside this function the sampling window is constructed
max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_period_);
max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_period_);
max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_period_);
min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_period_);
min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_period_);
min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_period_);
and then through the iterator
VelocityIterator x_it(min_vel[0], max_vel[0], vsamples[0]);
VelocityIterator y_it(min_vel[1], max_vel[1], vsamples[1]);
VelocityIterator th_it(min_vel[2], max_vel[2], vsamples[2]);
Iterate through all possible combinations of speeds and deposit them into the
sample_params_in which the optimal trajectory is generated
std::vector<base_local_planner::Trajectory> all_explored;
scored_sampling_planner_.findBestTrajectory(result_traj_, &all_explored);
in
findBestTrajectoryThere are two main steps in:
- Trajectory Generation
gen_->nextTrajectory(loop_traj);
Here.gen_will be passed through the constructedsample_params_Iterate through all possible combinations of speeds by theirgenerateTrajectory()The function generates the trajectory return
- Trajectory evaluation
loop_traj_cost = scoreTrajectory(loop_traj, best_traj_cost);
Here the cost function stack is actually called to evaluate the trajectories one by one
for (std::vector<TrajectoryCostFunction *>::iterator score_function = critics_.begin(); score_function != critics_.end(); ++score_function)
{
TrajectoryCostFunction *score_function_p = *score_function;
double cost = score_function_p->scoreTrajectory(traj);
...
}
And these cost functions can be customized, they have already been updated in the previous step.
Finally, just choose the least costly trajectory to return to.
4.2 Python implementation
The core code is shown below
def evaluation(self, vr):
v_start, v_end, w_start, w_end = vr
v = np.linspace(v_start, v_end, num=int((v_end - v_start) / self.robot.V_RESOLUTION)).tolist()
w = np.linspace(w_start, w_end, num=int((w_end - w_start) / self.robot.W_RESOLUTION)).tolist()
eval_win, traj_win = [], []
for v_, w_ in product(v, w):
# trajectory prediction, consistent of poses
traj = self.generateTraj(v_, w_)
end_state = traj[-1].squeeze().tolist()
# heading evaluation
theta = self.angle((end_state[0], end_state[1]), self.goal)
heading = np.pi - abs(end_state[2] - theta)
# obstacle evaluation
dist_vector = np.array(tuple(self.obstacles)) - np.array([end_state[0], end_state[1]])
dist_vector = np.sqrt(np.sum(dist_vector**2, axis=1))
distance = np.min(dist_vector)
if distance > self.eval_param["R"]:
distance = self.eval_param["R"]
# velocity evaluation
velocity = abs(v_)
# braking evaluation
dist_stop = v_ * v_ / (2 * self.robot.V_ACC)
# collision check
if distance > dist_stop and distance >= 1:
eval_win.append((v_, w_, heading, distance, velocity))
traj_win.append(traj)
...
# evaluation
factor = np.array([[1, 0, 0],
[0, 1, 0],
[0, 0, self.eval_param["heading"]],
[0, 0, self.eval_param["distance"]],
[0, 0, self.eval_param["velocity"]]])
return eval_win @ factor, traj_win
4.3 Matlab Implementation
The core code is shown below
function [eval_win, traj_win] = evaluation(robot, vr, goal, obstacle, kinematic, eval_param)
eval_win = []; traj_win = [];
for v = vr(1):kinematic.V_RESOLUTION:vr(2)
for w=vr(3):kinematic.W_RESOLUTION:vr(4)
% trajectory prediction, consistent of poses
[robot_star, traj] = generate_traj(robot, v, w, eval_param(4), eval_param(5));
% heading evaluation
theta = angle([robot_star.x, robot_star.y], goal(1:2));
heading = pi - abs(robot_star.theta - theta);
% obstacle evaluation
dist_vector = dist(obstacle, [robot_star.x; robot_star.y]);
distance = min(dist_vector);
if distance > eval_param(6)
distance = eval_param(6);
end
% velocity evaluation
velocity = abs(v);
% braking evaluation
dist_stop = v * v / (2 * kinematic.V_ACC);
% collision check
if distance > dist_stop && distance >= 1
eval_win = [eval_win; [v w heading distance velocity]];
traj_win = [traj_win; traj];
end
end
end
% normalization
...
eval_win = [eval_win(:, 1:2), eval_win(:, 3:5) * eval_param(1:3)'];
end
Contact the blogger’s business card below for the full project code!
