4. Kinodynamic Path Finding

Introduction

Kinodynamic : Kinematic + Dynamic:

The kinodynamic planning problem is to synthesize a robot motion subject to simultaneous kinematic constraints, such as avoiding obstacles, and dynamics constraints, such as modulus bounds on velocity, acceleration, and force. A kinodynamic solution is a mapping from time to generalized forces or accelerations. (From Kinodynamic Motion Planning)

• Differentially constrained
• Up to force (acceleration)

Reason: Straight-line connections between pairs of states are typically not valid trajectories due to the system’s differential constraints.

• Coarse-to-fine process
• Trajectory only optimizes locally
• Infeasible path means nothing to nonholonomic system

Examples:

• Unicycle model:

$$\left(\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right)=\left(\begin{array}{c}\cos \theta \\ \sin \theta \\ 0\end{array}\right)\cdot v+\left(\begin{array}{l}0\\ 0\\ 1\end{array}\right)\cdot \omega$$

Constraints:

$$\begin{array}{rl}& |v|⩽v_{max}\\ & |\omega |⩽{\omega }_{max}\end{array}$$

• Differential drive model:

$$\left(\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right)=\left(\begin{array}{c}\frac{r}{2}(u_l+u_r)\cos \theta \\ \frac{r}{2}(u_l+u_r)\sin \theta \\ \frac{r}{L}(u_r-u_l)\end{array}\right)$$

Constraints:

$$\begin{array}{rl}& |u_l|⩽u_{l,max}\\ & |u_r|⩽u_{r,max}\\ & v=\frac{r}{2}(u_l+u_r)\\ & \omega =\frac{r}{L}(u_r-u_l)\end{array}$$

Figure 1: Differential drive model.

• Simplified car model:

$$\left(\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right)=\left(\begin{array}{c}v\cos \theta \\ v\sin \theta \\ \frac{r}{L}\tan \varphi \end{array}\right)$$

Constraints:

$$\begin{array}{rlr}& |v|⩽v_{max},|\varphi |⩽{\varphi }_{max}<\frac{\pi }{2}& \text{Simple car model}\\ & |v|\in \{-v_{max},v_{max}\},|\varphi |⩽{\varphi }_{max}<\frac{\pi }{2}& \text{Reeds \& Shepp’s car}\\ & |v|=v_{max},|\varphi |⩽{\varphi }_{max}<\frac{\pi }{2}& \text{Dubin’s car}\end{array}$$

Figure 2: Simplified car model.

State Lattice Planning

Basic idea

TODO: chap 2

• Recall the search-based path finding method in Chapter 2
• For planning, how to build a graph?
• Is this graph doable for our real robot?

Assume the robot, a mass point, is not satisfactory any more. We now require a graph with feasible motion connections. We manually create (build) a graph with all edges executable by the robot.

• Forward direction: discrete (sample) in control space
• Reverse direction: discrete (sample) in state space

This is the basic motivation for all kinodyanmic planning. State lattice planning is the most straight-forward one.

TODO: chap 2, 3

• In chapter 2, we discretize the control space (4 connections for 4-neighbor, 8 connections for 8-neighbor). We assume the robot can move in 4 / 8 directions.
• In chapter 3, we discretize the state space (position) and assume the robot can move in any direction.

For a robot model: $\dot{s}=f(s,u)$ The robot is differentially driven. We have an initial state $s_0$ of the robot. Generate feasible local motions by:

• Sample in control space: select a $u$, fix a time duration $T$, forward simulate the system (numerical integration).
  • Forward simulation
  • Fixed $u,T$
  • No mission guidance,
  • East to implement
  • Low planning efficiency
• Sample in state space: select a $s_f$, find the connection (a trajectory) between $s_0$ and $s_f$
  • Backward calculation
  • Need calculate $u,T$
  • Good mission guidance
  • Hard to implement
  • High planning efficiency

Sampling in control space

• State:

$$\mathit{s}=\left(\begin{array}{c}x\\ y\\ z\\ \dot{x}\\ \dot{y}\\ \dot{z}\end{array}\right)$$

• Input:

$$\mathit{u}=\left(\begin{array}{c}\ddot{x}\\ \ddot{y}\\ \ddot{z}\end{array}\right)$$

• System equation:

$$\dot{\mathit{s}}=\mathbf{A}\mathit{s}+\mathbf{B}\mathit{u},\mathit{s}(0)={\mathit{s}}_0.$$

where

$$\mathbf{A}=\left[\begin{array}{cc}\mathbf{0}& {\mathbf{I}}_3\\ \mathbf{0}& \mathbf{0}\end{array}\right],\mathbf{B}=\left[\begin{array}{c}\mathbf{0}\\ {\mathbf{I}}_3\end{array}\right].$$

Specially, $\mathbf{A},\mathbf{B}$ can be expanded to more general cases, such as:

$$\mathbf{A}=\left[\begin{array}{ccccc}\mathbf{0}& {\mathbf{I}}_3& \mathbf{0}& \cdots & \mathbf{0}\\ \mathbf{0}& \mathbf{0}& {\mathbf{I}}_3& \cdots & \mathbf{0}\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ \mathbf{0}& \cdots & \cdots & \mathbf{0}& {\mathbf{I}}_3\\ \mathbf{0}& \cdots & \cdots & \mathbf{0}& \mathbf{0}\end{array}\right],\mathbf{B}=\left[\begin{array}{c}\mathbf{0}\\ \mathbf{0}\\ ⋮\\ \mathbf{0}\\ {\mathbf{I}}_3\end{array}\right].$$

• Several-order integrator
• $\mathbf{A}$ nilpotent

The solution is:

$$\begin{array}{rl}\mathit{s}(t)& =e^{\mathbf{A}t}{\mathit{s}}_0+\left[{\int }_0^t{e}^{\mathbf{A}(t-\tau )}\mathbf{B}\mathrm{d}\tau \right]{\mathit{u}}_m.\\ & \triangleq F(t){\mathit{s}}_0+G(t){\mathit{u}}_m\end{array}$$

where $F(t)=e^{\mathbf{A}t}$ is the state transition matrix, critical to the integration.

If matrix $\mathbf{A}\in {\mathbb{R}}^{n\times n}$ is nilpotent, i.e., ${\mathbf{A}}^n=\mathbf{0}$, then $e^{\mathbf{A}t}$ has a closed-form expression in the form of an $(n-1)$ degree matrix polynomial in $t$.

Note:

• During searching, the graph can be built when necessary.
• Create nodes (state) and edges (motion primitive) when nodes are newly discovered.
• Save computational time/space.

For every $s\in \mathcal{S}$ from the search tree:

• Pick a control vector $u$
• Integrate the equation over short duration
• Add collision-free motions to the search tree

For simplified car model with state $s=(x,y,\theta {)}^{\mathrm{\top }}$ and control $u=(v,\varphi {)}^{\mathrm{\top }}$:

• Select a $s\in \mathcal{S}$
• Pick $v,\varphi ,\tau$
• Integrate motion from $s$
• Add result if collision-free

Sampling in state space

Build a lattice graph:

• Given an origin.
• for 8 neighbor nodes around the origin, feasible paths are found.
• extend outward to 24(=25-1) neighbors.
• complete lattice.

Comparison

Figure 3: Comparison of sampling methods.

Boundary Value Problem (BVP)

• BVP is the basis of state sampled lattice planning.
• No general solution. Case by case design.
• Often evolve complicated numerical optimization.

BVP: design a trajectory $x(t)$ such that:

$$x(0)=a,x(T)=b$$

With $5^{\text{th}}$ order polynomial trajectory:

$$x(t)=c_5{t}^5+c_4{t}^4+c_3{t}^3+c_2{t}^2+c_{1}t+c_0.$$

Boundary conditions:

	Position	Velocity	Acceleration
$t=0$	$a$	$0$	$0$
$t=T$	$b$	$0$	$0$

Solve:

$$\left[\begin{array}{c}a\\ b\\ 0\\ 0\\ 0\\ 0\end{array}\right]=\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 1\\ T^5& T^4& T^3& T^2& T& 1\\ 0& 0& 0& 0& 1& 0\\ 5T^4& 4T^3& 3T^2& 2T& 1& 0\\ 0& 0& 0& 2& 0& 0\\ 20T^3& 12T^2& 6T& 2& 0& 0\end{array}\right]\left[\begin{array}{c}{c}_5\\ c_4\\ c_3\\ c_2\\ c_1\\ c_0\end{array}\right].$$

Optimal BVP (OBVP)^[1]

Fix final state

Modelling:

• Objective: minimize the integral of squared jerk:

$$J_{\mathrm{\Sigma }}=\sum _{k=1}^3{J}_k,J_k=\frac{1}{T}{\int }_0^T{j}_k^2(t)\mathrm{d}t.$$

• State: ${\mathit{s}}_k=(p_k,v_k,a_k)$. Input: $u_k=j_k$
• System model: ${\dot{\mathit{s}}}_k={\mathit{f}}_s({\mathit{s}}_k,u_k)=(v_k,a_k,j_k)$ / $\dot{\mathit{s}}={\mathit{f}}_s(\mathit{s},u)=(v,a,j)$

Solving:

• By Pontryagin's Minimum Principle, we first introduce the costate: $\mathit{\lambda }=({\lambda }_1,{\lambda }_2,{\lambda }_3)$
• Define the Hamiltonian function:

$$\begin{array}{rl}H(\mathit{s},u,\mathit{\lambda })& =\frac{1}{T}{j}^2+{\mathit{\lambda }}^{\mathrm{\top }}{\mathit{f}}_s(\mathit{s},u)\\ & =\frac{1}{T}{j}^2+{\lambda }_{1}v+{\lambda }_{2}a+{\lambda }_{3}j\end{array}$$

Generally:

$$J=h(s(T))+{\int }_0^{T}g(\mathit{s}(t),u(t))\mathrm{d}t$$

where $s(T)$ is the final state and $g(\mathit{s}(t),u(t))$ is the transition cost.

Write the Hamiltonian and costate:

$$\begin{array}{rl}& H(\mathit{s},u,\mathit{\lambda })=g(\mathit{s},u)+{\mathit{\lambda }}^{\mathrm{\top }}{\mathit{f}}_s(\mathit{s},u)\\ & \mathit{\lambda }={({\lambda }_1,{\lambda }_2,\cdots ,{\lambda }_n)}^{\mathrm{\top }}\end{array}$$

Suppose the optimal input is $u^{\ast }$ and the optimal state is $s^{\ast }$, then the necessary conditions for optimality are:

$$\dot{\mathit{\lambda }}=-\mathrm{\nabla }{H}_{\mathit{s}}({\mathit{s}}^{\ast },u^{\ast },\mathit{\lambda })$$

with the boundary condition of:

$$\mathit{\lambda }(T)=-\mathrm{\nabla }h({\mathit{s}}^{\ast }(T))$$

and the optimal input $u^{\ast }$ satisfies:

$$u^{\ast }=\arg \underset{u}{min}H({\mathit{s}}^{\ast },u,\mathit{\lambda })$$

$$\dot{\mathit{\lambda }}=-{\mathrm{\nabla }}_{\mathit{s}}H({\mathit{s}}^{\ast },u^{\ast },\mathit{\lambda })={\left(\begin{array}{ccc}0& -{\lambda }_1& -{\lambda }_2\end{array}\right)}^{\mathrm{\top }}$$

The costate is solved as:

$$\mathit{\lambda }=\frac{1}{T}\left[\begin{array}{c}-2\alpha \\ 2\alpha t+2\beta \\ -\alpha t^2-2\beta t-2\gamma \end{array}\right]$$

(For later convenience)

$$\begin{array}{rl}& \dot{\mathit{\lambda }}=\left[\begin{array}{c}0\\ -{\lambda }_1\\ -{\lambda }_2\end{array}\right]=\left[\begin{array}{ccc}0& 0& 0\\ -1& 0& 0\\ 0& -1& 0\end{array}\right]\mathit{\lambda }\triangleq \mathbf{C}\mathit{\lambda }\\ \Rightarrow & \mathit{\lambda }=e^{\mathbf{C}t}\mathit{\lambda }(0),\mathit{\lambda }(0)\triangleq 2/T\left[\begin{array}{ccc}-\alpha & \beta & -\gamma \end{array}\right]\\ & {\mathbf{C}}^2=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 1& 0& 0\end{array}\right],{\mathbf{C}}^3=\mathbf{0},\cdots \\ \Rightarrow & \mathit{\lambda }=\left[\begin{array}{ccc}1& 0& 0\\ -t& 1& 0\\ t^2/2& -t& 1\end{array}\right]\mathit{\lambda }(0)\end{array}$$

The optimal input is solved as:

$$\begin{array}{rl}{u}^{\ast }(t)& =j^{\ast }(t)=\arg \underset{j(t)}{min}H({\mathit{s}}^{\ast }(t),j(t),\mathit{\lambda }(t))\\ & =\frac{1}{2}\alpha t^2+\beta t+\gamma .\end{array}$$

The optimal state trajectory is solved as:

$$\begin{array}{rl}{\mathit{s}}^{\ast }(t)& =\left[\begin{array}{c}{p}^{\ast }(t)\\ v^{\ast }(t)\\ a^{\ast }(t)\end{array}\right]\\ & =\left[\begin{array}{c}\frac{1}{120}\alpha t^5+\frac{1}{24}\beta t^4+\frac{1}{6}\gamma t^3+\frac{1}{2}{a}_0{t}^2+v_{0}t+p_0\\ \frac{1}{24}\alpha t^4+\frac{1}{6}\beta t^3+\frac{1}{2}\gamma t^2+a_{0}t+v_0\\ \frac{1}{6}\alpha t^3+\frac{1}{2}\beta t^2+\gamma t+a_0\end{array}\right]\end{array}$$

with initial state: $\mathit{s}(0)=(p_0,v_0,a_0)$. This is obtained by jerk-acceleration-velocity-position integration.

The remaining unknowns $\alpha ,\beta ,\gamma$ are solved for as a function of the desired end state variable components $s_k$. Let $s(T)={(p_f,v_f,a_f)}^{\mathrm{\top }}$, then the unknowns $\alpha ,\beta ,\gamma$ are isolated by reordering the above equation as:

$$\left[\begin{array}{c}{p}_f\\ v_f\\ a_f\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{120}{T}^5& \frac{1}{24}{T}^4& \frac{1}{6}{T}^3\\ \frac{1}{24}{T}^4& \frac{1}{6}{T}^3& \frac{1}{2}{T}^2\\ \frac{1}{6}{T}^3& \frac{1}{2}{T}^2& T\end{array}\right]\left[\begin{array}{c}\alpha \\ \beta \\ \gamma \end{array}\right]+\left[\begin{array}{c}\frac{1}{2}{a}_0{T}^2+v_{0}T+p_0\\ a_{0}T+v_0\\ a_0\end{array}\right].$$

Solving for the unknown coefficients yields

$$\begin{array}{rl}\left[\begin{array}{c}\alpha \\ \beta \\ \gamma \end{array}\right]& ={\left[\begin{array}{ccc}\frac{1}{120}{T}^5& \frac{1}{24}{T}^4& \frac{1}{6}{T}^3\\ \frac{1}{24}{T}^4& \frac{1}{6}{T}^3& \frac{1}{2}{T}^2\\ \frac{1}{6}{T}^3& \frac{1}{2}{T}^2& T\end{array}\right]}^{-1}\left[\begin{array}{c}\mathrm{\Delta }p\\ \mathrm{\Delta }v\\ \mathrm{\Delta }a\end{array}\right]\\ & =\frac{1}{T^5}\left[\begin{array}{ccc}720& -360T& 60T^2\\ -360T& 168T^2& -24T^3\\ 60T^2& -24T^3& 3T^4\end{array}\right]\left[\begin{array}{c}\mathrm{\Delta }p\\ \mathrm{\Delta }v\\ \mathrm{\Delta }a\end{array}\right]\end{array}$$

where

$$\left[\begin{array}{c}\mathrm{\Delta }p\\ \mathrm{\Delta }v\\ \mathrm{\Delta }a\end{array}\right]=\left[\begin{array}{c}{p}_f\\ v_f\\ a_f\end{array}\right]-\left[\begin{array}{c}\frac{1}{2}{a}_0{T}^2+v_{0}T+p_0\\ a_{0}T+v_0\\ a_0\end{array}\right].$$

And the cost is:

$$\begin{array}{rl}{J}_{\mathrm{\Sigma }}& =\frac{1}{T}{\int }_0^T{j}^{\ast 2}(t)\mathrm{d}t=\frac{1}{T}{\int }_0^T{(\frac{1}{2}\alpha t^2+\beta t+\gamma )}^2\mathrm{d}t.\\ & =\frac{1}{T}{\int }_0^T(\frac{1}{4}{\alpha }^2{t}^4+\alpha \beta t^3+(\frac{1}{2}\alpha \gamma +{\beta }^2)t^2+2\beta \gamma t+{\gamma }^2)\mathrm{d}t\\ & =\frac{1}{20}{\alpha }^2{T}^4+\frac{1}{4}\alpha \beta T^3+(\frac{1}{6}\alpha \gamma +\frac{1}{3}{\beta }^2)T^2+\beta \gamma T+{\gamma }^2\end{array}$$

• Similar solution can also be found when $s(T)$ is partially defined
• Same solving process holds for $J_k={\int }_0^T{j}^{\ast 2}(t)\mathrm{d}t+T$

The boundary condition: $\mathit{\lambda }(t)=-\mathrm{\nabla }h({\mathit{s}}^{\ast }(t))$.

• For fixed final state problem:

$$h(s(T))=\{\begin{array}{ll}0,& s=s(T)\\ +\mathrm{\infty },& \text{otherwise}\end{array}$$

Not differentiable. So we discard this condition, and use given $s(T)$ to directly solve for unknown variables

• For (partially)-free final state problem:

$$\text{Given }{s}_i(T),i\in I$$

We have boundary condition for other costate:

$${\lambda }_j(T)=\frac{\partial }{\partial {\mathit{s}}_j}h(s^{\ast }(T)),j\ne i.$$

Free v & a (Homework)

The Hamiltonian function is shown in the last section:

$$H(\mathit{s},u,\mathit{\lambda })=\frac{1}{T}{j}^2+{\lambda }_{1}v+{\lambda }_{2}a+{\lambda }_{3}j$$

By utilizing Pontryagin's Minimum Principle, we have (Also shown in the last section):

$$\dot{\mathit{\lambda }}=-{\mathrm{\nabla }}_{\mathit{s}}H({\mathit{s}}^{\ast },u^{\ast },\mathit{\lambda })\Rightarrow \mathit{\lambda }(t)=\frac{1}{T}\left[\begin{array}{c}-2\alpha \\ -2\alpha t-2\beta \\ \alpha t^2+2\beta t+2\gamma \end{array}\right]$$

With boundary condition for $\mathit{\lambda }(T)$ with free $v(T),a(T)$, thus $h$ doesn't relate to $v(T),a(T)$, we have:

$${\lambda }_2(T)={\lambda }_3(T)=0$$

Thus, we have:

$$\{\begin{array}{r}\beta =-\alpha T\\ \gamma =\frac{1}{2}\alpha T^2\end{array}$$

and

$$\mathit{\lambda }(t)=\frac{1}{T}\left[\begin{array}{c}-2\alpha \\ -2\alpha t-2\beta \\ \alpha t^2+2\beta t+2\gamma \end{array}\right]=\frac{1}{T}\left[\begin{array}{c}-2\alpha \\ 2\alpha (t-T)\\ -\alpha (t^2-2Tt+T^2)\end{array}\right].$$

The optimal input is solved as:

$$\begin{array}{rl}{u}^{\ast }(t)& =j^{\ast }(t)=\arg \underset{j(t)}{min}H({\mathit{s}}^{\ast }(t),j(t),\mathit{\lambda }(t))\\ & =-\frac{{\lambda }_{3}T}{2}=\frac{1}{2}\alpha t^2-\alpha Tt+\frac{1}{2}\alpha T^2.\end{array}$$

By integrating the optimal input, we have the optimal state trajectory as:

$$\begin{array}{rl}{\mathit{s}}^{\ast }(t)& =\left[\begin{array}{c}{p}^{\ast }(t)\\ v^{\ast }(t)\\ a^{\ast }(t)\end{array}\right]\\ & =\left[\begin{array}{c}\frac{1}{120}\alpha t^5-\frac{1}{24}\alpha T{t}^4+\frac{1}{12}\alpha T^2{t}^3+\frac{1}{2}{a}_0{t}^2+v_{0}t+p_0\\ \frac{1}{24}\alpha t^4-\frac{1}{6}\alpha T{t}^3+\frac{1}{4}\alpha T^2{t}^2+a_{0}t+v_0\\ \frac{1}{6}\alpha t^3-\frac{1}{2}\alpha T{t}^2+\frac{1}{2}\alpha T^{2}t+a_0\end{array}\right].\end{array}$$

With known $p(T)=p_f$, we can solve for $\alpha$ as:

$$\begin{array}{rl}& p_f=\frac{1}{120}\alpha T^5-\frac{1}{24}\alpha T^5+\frac{1}{12}\alpha T^5+\frac{1}{2}{a}_0{T}^2+v_{0}T+p_0\\ \Rightarrow & \alpha =\frac{20}{T^5}(p_f-p_0-v_{0}T-\frac{1}{2}{a}_0{T}^2).\end{array}$$

Thus:

$$J_{\mathrm{\Sigma }}=\frac{1}{T}{\int }_0^T{j}^{\ast 2}(t)\mathrm{d}t=\frac{20}{T^6}{(p_f-p_0-v_{0}T-\frac{1}{2}{a}_0{T}^2)}^2.$$

It shows that $J$ is a function of $T$. By optimizing $T$, we can further reduce the cost. With differentiating $J$ with respect to $T$, and setting $\frac{\mathrm{d}J}{\mathrm{d}T}=0$, we have:

$$(p_f-p_0-v_{0}T-\frac{1}{2}{a}_0{T}^2)(a_0{T}^2+4v_{0}T-6p_f+6p_0)=0$$

Thus, the optimal $T$ is:

$$T^{\ast }=\frac{-v_0+\sqrt{v_0^2+2a_0(p_f-p_0)}}{a_0}\text{ or }{T}^{\ast }=\frac{-2v_0+\sqrt{4v_0^2+6a_0(p_f-p_0)}}{a_0}.$$

And $T^{\ast }\in {\mathbb{R}}_{⩾0}$ and pick the one with smaller cost.

TBC

---

1. Mueller M W, Hehn M, D'Andrea R. A computationally efficient motion primitive for quadrocopter trajectory generation[J]. IEEE transactions on robotics, 2015, 31(6): 1294-1310. ↩︎