Algorithm 2	Improved-DWA (Local Planning with Hard/Soft Safety and Multi-Objective)
Input	$\mathrm{Input}: \mathrm{Current} \mathrm{state} x=(x,y,\theta ,v,\omega )$, local goal, static map, predicted dynamic obstacles, DWA parameters.
Output	$\mathrm{Control} u^{\ast }=\left(v^{\ast },{\varphi }^{\ast }\right), \mathrm{best} \mathrm{trajectory} \mathrm{t}\mathrm{r}\mathrm{a}\mathrm{j}{j}^{\ast }$.
1	$\mathrm{Build} \mathrm{dynamic} \mathrm{window}: v\in \left[\mathrm{m}\mathrm{a}\mathrm{x}\left(v_{\mathrm{m}\mathrm{i}\mathrm{n}},v_k+a_{\mathrm{m}\mathrm{i}\mathrm{n}}\Delta t\right),\mathrm{m}\mathrm{i}\mathrm{n}\left(v_{\mathrm{m}\mathrm{a}\mathrm{x}},v_k+a_{\mathrm{m}\mathrm{a}\mathrm{x}}\Delta t\right)\right];\varphi \in \left[\mathrm{m}\mathrm{a}\mathrm{x}\left({\varphi }_{\mathrm{m}\mathrm{i}\mathrm{n}},{\varphi }_k+{\dot{\varphi }}_{\mathrm{m}\mathrm{i}\mathrm{n}}\Delta t\right),\mathrm{m}\mathrm{i}\mathrm{n}\left({\varphi }_{\mathrm{m}\mathrm{a}\mathrm{x}},{\varphi }_k+{\dot{\varphi }}_{\mathrm{m}\mathrm{a}\mathrm{x}}\Delta t\right)\right]$.
2	$\mathrm{Prune} \mathrm{samples} \mathrm{with} \left|\varphi \right|>{\varphi }_{\mathrm{m}\mathrm{a}\mathrm{x}} \mathrm{or} v<v_{\epsilon }$;
3	$\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}\mathrm{S}\mathrm{c}\mathrm{o}\mathrm{r}\mathrm{e}\leftarrow -\infty ; u^{\ast }\leftarrow \varnothing ; {traj}^{\ast }\leftarrow \varnothing ;$
4	$\mathrm{For} \mathrm{each} \mathrm{sample} (v,\varphi )$ in dynamic window do
5	$\hspace{1em}\hspace{1em}\mathrm{Predict} N_p$-step trajectory $\mathsf{\Gamma }={\left\{\left(x_{\mathcal{l}},y_{\mathcal{l}},{\theta }_{\mathcal{l}}\right)\right\}}_{\mathcal{l}=1}^{N_p}$.
6	$\hspace{1em}\hspace{1em}{d}_{\mathrm{m}\mathrm{i}\mathrm{n}}\leftarrow \underset{\mathcal{l}}{\mathrm{m}\mathrm{i}\mathrm{n}} \underset{o}{\mathrm{m}\mathrm{i}\mathrm{n}} ‖\left(x_{\mathcal{l}},y_{\mathcal{l}}\right)-\left(x_o,y_o\right)‖$ over static + predicted obstacles.
7	$\mathrm{If} d_{\mathrm{m}\mathrm{i}\mathrm{n}}<D$ then continue.
8	$\mathrm{Compute} \mathrm{desired} \mathrm{heading} {\theta }_{\mathrm{des}}=\mathrm{a}\mathrm{t}\mathrm{a}\mathrm{n}2\left(y_g-y_{N_p},x_g-x_{N_p}\right)$.
9	$\mathit{Heading}={\displaystyle \frac{1+cos\left({\theta }_{N_p}-{\theta }_{\mathrm{d}\mathrm{e}\mathrm{s}}\right)}{2}}$.
10	$\mathit{Smooth}={\displaystyle \frac{1}{N_p-1}}{\sum }_{\mathcal{l}-1}^{N_p-1} {\displaystyle \frac{1+cos\left({\theta }_{\mathcal{l}+1}-{\theta }_{\mathcal{l}}\right)}{2}}$
11	${Clear}^{\ast }=K\left({\displaystyle \frac{1}{1+exp\left(c\left(d_{min}-d_{lim}\right)\right)}}-{\displaystyle \frac{1}{2}}\right)$
12	$\mathit{Velocity}={\displaystyle \frac{v-v_{\mathrm{m}\mathrm{i}\mathrm{n}}}{v_{\mathrm{m}\mathrm{a}\mathrm{x}}-v_{\mathrm{m}\mathrm{i}\mathrm{n}}}}$.
13	$\mathit{RelAngle}={\displaystyle \frac{1+cos\left({\theta }_{\mathrm{des}}-{\theta }_k\right)}{2}}$.
14	$J=\alpha \mathit{Heading}+\beta \mathit{Smooth}+\delta {Clear}^{\ast }+\gamma \mathit{Velocity}+\mu \mathit{RelAngle}$.
15	$\mathrm{If} J>\mathrm{bestScore} \mathrm{then} \mathrm{update} \mathrm{bestScore}, u^{\ast }=(v,\varphi ),tra{j}^{\ast }=\mathsf{\Gamma }$.
16	$\mathrm{Return} u^{\ast },tra{j}^{\ast }$.