Energy-optimal trajectory planning for car-like robots

Abstract

When a battery-powered robot needs to operate for a long period of time, optimizing its energy consumption becomes critical. Driving motors are a major source of power consumption for mobile robots. In this paper, we study the problem of finding optimal paths and velocity profiles for car-like robots so as to minimize the energy consumed during motion. We start with an established model for energy consumption of DC motors. We first study the problem of finding the energy optimal velocity profiles, given a path for the robot. We present closed form solutions for the unconstrained case and for the case where there is a bound on maximum velocity. We then study a general problem of finding an energy optimal path along with a velocity profile, given a starting and goal position and orientation for the robot. Along the path, the instantaneous velocity of the robot may be bounded as a function of its turning radius, which in turn affects the energy consumption. Unlike minimum length paths, minimum energy paths may contain circular segments of varying radii. We show how to efficiently construct a graph which generalizes Dubins’ paths by including segments with arbitrary radii. Our algorithm uses the closed-form solution for the optimal velocity profiles as a subroutine to find the minimum energy trajectories, up to a fine discretization. We investigate the structure of energy-optimal paths and highlight instances where these paths deviate from the minimum length Dubins’ curves. In addition, we present a calibration method to find energy model parameters. Finally, we present results from experiments conducted on a custom-built robot for following optimal velocity profiles.

Appendices

Appendix 1: Proof: unconstrained solution

The state transition equation can be written as,

$$\begin{aligned} \dot{\mathbf {X}}(t) = \left[ \begin{array}{l} \dot{x}(t) \\ \dot{v}(t)\end{array}\right] = \left[ \begin{array}{l} v(t) \\ a(t) \end{array}\right] \end{aligned}$$

(25)

The objective is to find a velocity profile $v^*(t)$ which minimizes the total energy required for motion given by the following cost functional,

$$\begin{aligned} J = \int \limits _{0}^{t_{f}} \Big [ c_1 a^2(t) + c_2 v^2(t) + c_3 v(t) + c_4 \Big ] dt, \end{aligned}$$

(26)

where the final time $t_f$ is kept a free variable. The initial boundary conditions are given as,

$$\begin{aligned} x(0) = 0,\quad v(0) = 0, \end{aligned}$$

(27)

and the final boundary conditions are given as,

$$\begin{aligned} x(t_f) = D,\quad v(t_f) = 0. \end{aligned}$$

(28)

1.1 Hamiltonian

The hamiltonian $H(\mathbf {X},\varvec{\lambda },u,t)$ is defined as,

$$\begin{aligned} H(\mathbf {X},\varvec{\lambda },u,t) = J + \lambda _1(t) \dot{x}(t) + \lambda _2(t) \dot{v(t)}, \end{aligned}$$

(29)

where $\lambda _1(t)$ and $\lambda _2(t)$ are the Lagrange multipliers, also called the co-state variables which include the state transition equations as a constraint to the objective.

When there is no bound on the maximum velocity, the Hamiltonian for this problem can be obtained using Eqs. 25 and 26 as,

$$\begin{aligned} H(\mathbf {X}(t), a(t), \varvec{\lambda }(t),t)&= c_1 a^2(t) + c_2 v^2(t) + c_3 v(t) \nonumber \\&+\, c_4 + \lambda _1(t) v(t) + \lambda _2(t) a(t) \end{aligned}$$

(30)

where the acceleration $a(t)$ is the control.

The three necessary conditions for $a^*(t)$ to optimize the Hamiltonian [15] for all time $t \in [0, t_f]$ are given as,

$$\begin{aligned} \dot{\mathbf {X}}^*(t) = \frac{\partial H}{\partial \varvec{\lambda }}, \;\;\; \varvec{\dot{\lambda }}^*(t) = -\frac{\partial H}{\partial \mathbf {X}},\;\;\; 0 = \frac{\partial H}{\partial a} \end{aligned}$$

(31)

By substituting we get,

$$\begin{aligned} \dot{x}(t)&= v(t),\\ \dot{v}(t)&= a(t),\\ \dot{\lambda _1}(t)&= 0,\\ \dot{\lambda _2}(t)&= 2c_2v(t)+c_3+\lambda _1,\\ \lambda _2(t)&= -2c_1a(t), \\ \therefore \lambda _2(t)&= -2c_1\dot{v}(t). \end{aligned}$$

Using the last two equations, we can write,

$$\begin{aligned} -2c_1\ddot{v}(t)&= 2c_2v(t)+c_3+\lambda _1, 2c_1\ddot{v}(t) \\&+\, 2c_2v(t)+c_3+\lambda _1 = 0. \end{aligned}$$

We can solve for this second order differential equation to yield,

$$\begin{aligned} v^*(t) = s_1 e^{k t} + s_2 e^{-k t} - \left( \frac{c_3 + s_3}{2 c_1}\right) \end{aligned}$$

(32)

where $k = \sqrt{\frac{c_2}{c_1}}$ and $s_1 - s_4$ are constants and $\lambda _1 = s_3$.

Applying the state transition equations, we can get the optimal control and states given as,

$$\begin{aligned} a^*(t)&= k s_1 e^{k t} - k s_2 e^{-k t} \end{aligned}$$

(33)

$$\begin{aligned} x^*(t)&= \frac{s_1 e^{k t}}{k} - \frac{s_2 e^{-k t}}{k} - \left( \frac{c_3 + s_3}{2 c_1}\right) t + s_4. \end{aligned}$$

(34)

We can solve for $s_1-s_4$ in terms of the final time $t_f$ by substituting the boundary conditions given in Eqs. 27 and 28 for $v^*(t)$ and $x^*(t)$. We obtain,

$$\begin{aligned} s_1&= - \frac{D k}{kt_f + e^{kt_f}(kt_f-2) + 2}, \nonumber \\ s_2&= s_1 e^{kt_f},\nonumber \\ s_3&= 2c_1(s_1 + s_2) - c_3, \nonumber \\ s_4&= - \frac{s_1-s_2}{k}. \end{aligned}$$

(35)

By substituting in Eqs. 33–34 we obtain,

$$\begin{aligned} a^*(t)&= D\left( \frac{c_2}{c_1}\right) \left( \frac{e^{k(t_f-t)} - e^{kt}}{kt_f + e^{kt_f}(kt_f-2) + 2}\right) , \nonumber \\ v^*(t)&= D\sqrt{\frac{c_2}{c_1}} \left( \frac{(1+e^{kt_f} - (e^{k(t_f-t)}+e^{kt}))}{kt_f + e^{kt_f}(kt_f-2) + 2}\right) , \nonumber \\ x^*(t)&= D\left( \! \frac{(e^{k(t_f-t)}-e^{kt}) - (e^{kt_f}-1) + kt(e^{kt_f}+1)}{kt_f + e^{kt_f}(kt_f-2) + 2} \!\right) \!. \end{aligned}$$

(36)

Since the final time is free, it can be solved for using the additional boundary condition (known as the transversality condition) given by,

$$\begin{aligned} H(\mathbf {X^*}(t_f), a^*(t_f), \varvec{\lambda }^*(t_f),t_f) = 0. \end{aligned}$$

(37)

Substituting Eqs. 33–34 and 35 above results in,

$$\begin{aligned} \left( D\frac{c_2}{c_1}+2\right) \left( 1-e^{kt_f}\right) + \sqrt{\frac{c_4}{c_1}}kt_f\left( 1+e^{kt_f}\right) = 0, \end{aligned}$$

(38)

which is an equation in single variable $t_f$ and can be solved using existing solvers. (We used MATLAB’s solve function). Alternatively, if the final time is fixed, we can directly substitute this given value in Eq. 36 to find $v^*(t)$.

Appendix 2: Proof for Lemma 1

Proof

Consider any velocity profile $v(t)$ shown in Fig. 19. Let $D$ and $E$ be the total distance covered and energy consumed by $v(t)$. This profile crosses $\sqrt{\frac{c_4}{c_2}}$ between times $[t_1,t_2]$ and $[t_3, t_4]$. Let $d_{12}$ and $d_{34}$ be the distances covered by $v(t)$ in these sections. The total energy consumption of $v(t)$ is given by,

$$\begin{aligned} E = E_{01} + E_{12} + E_{23} + E_{34} + E_{45}, \end{aligned}$$

(39)

where $E_{ij}$ refers to the energy consumption to cover the distance $d_{ij}$.

Fig. 19

Sections of this velocity profile crossing ($v_c=\sqrt{\frac{c_4}{c_2}}$) between $[t_1,t_2]$ and $[t_3,t_4]$ can be replaced by constant velocity ($v_c$) sections resulting in a velocity profile that consumes lesser energy to travel the same distance

We construct another velocity profile $v'(t)$ by replacing the sections $[t_1,t_2]$ and $[t_3, t_4]$ by constant velocity $v_c = \sqrt{\frac{c_4}{c_2}}$ sections for time $\frac{d_{12}}{v_c}$ and $\frac{d_{34}}{v_c}$ respectively. The total distance traveled by $v'(t)$ is $D$, same as $v(t)$. The total energy consumption of $v'(t)$ is given by,

$$\begin{aligned} E' = E_{01} + E_{12}' + E_{23} + E_{34}'+E_{45}, \end{aligned}$$

(40)

since $v'(t)$ is the same as $v(t)$ everywhere except $t\in [t_1,t_2]$ and $t\in [t_3,t_4]$.

We now show that $E' \le E$ by proving both $E_{12}' \le E_{12}$ and $E_{34}' \le E_{34}$. This result can then be generalized to velocity profiles with any number of crossing sections in either directions.

First, consider the energy consumption $E_{12}$ for $v(t)$,

$$\begin{aligned} E_{12} = \int _{t_{1}}^{t_{2}} \Big [ c_1 a^2(t) + c_2 v^2(t) + c_4 \Big ] dt + c_3d_{12}. \end{aligned}$$

(41)

Now, let us consider $E_{12}'$. The time taken in this case would be $t_c =\frac{d_{12}}{v_{c}}$. The energy consumption is,

$$\begin{aligned} E_{12}'&= \int _{t_{1}}^{t_{c}} \Big [ c_1 a^2(t) + c_2 v^2(t) + c_3 v(t) + c_4 \Big ] dt, \nonumber \\&= c_2 v_{c} d_{12} + c_3 d_{12} + c_4 \frac{d_{12}}{v_{c}}. \end{aligned}$$

(42)

The distance $d_{12}$ can also be written as,

$$\begin{aligned} d_{12} = \int _{t_{1}}^{t_{2}} \left[ v(t) dt \right] . \end{aligned}$$

(43)

Substituting Eq. 43 in 42, we obtain,

$$\begin{aligned} E_{12}' = c_2 \int _{t_1}^{t_2} v_c v(t) dt + c_3 d_{12} + c_4 \int _{t_1}^{t_2} \frac{v(t)}{v_c} dt. \end{aligned}$$

(44)

Using Eqs. 41 and 44, we can write,

$$\begin{aligned}&\!\!\!E_{12} - E_{12}'= c_1 \int _{t_1}^{t_2} a^2(t) dt \\&\qquad \qquad \qquad + \frac{c_2}{v_c}\int _{t_1}^{t_2} \left[ v_c(t) - v(t) \right] \left[ \frac{c_4}{c_2} - v(t) v_c\right] dt\\&\!\!\!\therefore E_{12} - E_{12}' \ge 0, \end{aligned}$$

since $v(t) \le v_{c} \le \sqrt{\frac{c_4}{c_2}}$. For the section between $t_3$ and $t_4$, we can show that $E_{34}-E_{34}' \ge 0$.

In general we can replace any number of such sections crossing $\sqrt{\frac{c_4}{c_2}}$ to yield another velocity profile with lower energy covering the same distance moving at $\sqrt{\frac{c_4}{c_2}}$. Hence, once the velocity profile hits $\sqrt{\frac{c_4}{c_2}}$, there is no reason to deviate from this value except at the boundary (initial and final conditions). $\square$

Appendix 3: Proof: constrained solution

We begin by writing the velocity constraint in the form of state inequality $\bar{S} = (v(t) - v_m) \le 0$. The state inequality $\bar{S}$ is converted into a control equality and interior point constraint by differentiating $\bar{S}$ once leading to,

$$\begin{aligned}&\bar{S}^{(1)} = \dot{v}(t) = u. \nonumber \\&v(t_1) = v_m \end{aligned}$$

(45)

Along the unconstrained arc, the state transition is governed by Eq. 25. On the constrained arc, the state transition is given by,

$$\begin{aligned} \dot{\mathbf {X}}(t) = \left[ \begin{array}{l} \dot{x}(t) \\ \dot{v}(t)\end{array}\right] = \left[ \begin{array}{l} v_m \\ 0 \end{array}\right] \end{aligned}$$

(46)

The Hamiltonian is augmented with the control equality constraint in $[t_1, t_f-t_2]$ and is given by,

$$\begin{aligned} \hat{H}&= c_1 a^2(t) + c_2 v^2(t) + c_3 v(t) + c_4 \nonumber \\&+\, \lambda _1(t) v(t) + \lambda _2(t) a(t) + \mu (t) a(t) \end{aligned}$$

(47)

where $\mu$ is the slack variable associated with the control constraint. In the interval $[0, t_1]$ and $[t_f-t_2, t_f]$, the Hamiltonian is given by,

$$\begin{aligned} H&= c_1 a^2(t) + c_2 v^2(t) + c_3 v(t) \nonumber \\&+\, c_4 + \lambda _1(t) v(t) + \lambda _2(t) a(t) \end{aligned}$$

(48)

The interior point constraint is given by,

$$\begin{aligned} G = \xi (t) (v(t) - v_m). \end{aligned}$$

(49)

1.1 $0 \le t \le t_1$

Using the necessary condition $\dot{\lambda } = - \frac{\partial H}{\partial x}$ we get,

$$\begin{aligned} \dot{\lambda _1}&= 0,\\ \therefore \quad \lambda _1&= s_3. \end{aligned}$$

and,

$$\begin{aligned} \dot{\lambda _2}&= -\frac{\partial H}{\partial x},\end{aligned}$$

(50)

$$\begin{aligned} \therefore \quad \dot{\lambda _2}&= -\left[ 2c_2 v(t) + c_3 + s_3\right] . \end{aligned}$$

(51)

Applying the third necessary condition, $0 = \frac{\partial H}{\partial a}$ we get,

$$\begin{aligned} 0&= 2c_1a(t) + \lambda _2(t),\\ \therefore \quad \lambda _2(t)&= -2c_1 a(t). \end{aligned}$$

Differentiating the above equation we get,

$$\begin{aligned} \dot{\lambda _2(t)} = -2c_1 \ddot{v}(t). \end{aligned}$$

From Eq. 51 we can write,

$$\begin{aligned}&2c_1\ddot{v(t)} = 2c_2 v(t) + c_3 + s_3,\\&\quad \therefore \quad \ddot{v}(t) - \frac{c_2}{c_1} v(t) - \frac{c_3+s_3}{2c_1} = 0. \end{aligned}$$

The solution for the above differential equation is given as,

$$\begin{aligned} v^*(t)&= s_1 e^{kt} + s_2 e^{-kt} - \frac{c_3+s_3}{2c_1},\\ a^*(t)&= s_1 k e^{kt} - s_2 k e^{-kt},\\ x^*(t)&= \frac{s_1}{k} e^{kt} - \frac{s_2}{k} e^{-kt} - \frac{c_3+s_3}{2c_1}t + s_4,\\ \lambda _1^*(t)&= s_3,\\ \lambda _2^*(t)&= -2c_1 a^*(t). \end{aligned}$$

Using initial conditions $x(0) = 0$ and $v(0) = v_0$, we get,

$$\begin{aligned} s_4&= - \frac{s_1-s_2}{k},\\ \frac{c_3 + s_3}{2c_1}&= s_1 + s_2 - v_0. \end{aligned}$$

Putting these together we get,

$$\begin{aligned} v^*(t)&= s_1 e^{kt} + s_2 e^{-kt} - (s_1+s_2 - v_0),\\ a^*(t)&= s_1 k e^{kt} - s_2 k e^{-kt},\\ x^*(t)&= \frac{s_1}{k} e^{kt} - \frac{s_2}{k} e^{-kt} - (s_1+s_2-v_0)t - \frac{s_1-s_2}{k},\\ \lambda _1^*(t)&= 2c_1{s_1+s_2-v_0} - c_3,\\ \lambda _2^*(t)&= -2c_1 a^*(t), \end{aligned}$$

where $s_1$ and $s_2$ are two constants left to be evaluated.

1.2 $t_f-t_2 \le t \le t_f$

In this section, the system is governed by the same state equation as in the interval $0 \le t \le t_1$. Hence, we get a similar form for the optimal state and control given by,

$$\begin{aligned} v^*(t)&= s_1' e^{-kt_f} e^{kt} + s_2' e^{kt_f} e^{-kt} - \frac{c_3+s_3'}{2c_1},\\ a^*(t)&= s_1' k e^{-kt_f} e^{kt} - s_2' k e^{kt_f} e^{-kt},\\ x^*(t)&= \frac{s_1'}{k} e^{-kt_f} e^{kt} - \frac{s_2'}{k} e^{kt_f} e^{-kt} - \frac{c_3+s_3'}{2c_1}t + s_4',\\ \lambda _1^*(t)&= s_3',\\ \lambda _2^*(t)&= -2c_1 a^*(t). \end{aligned}$$

where $s_1'\ldots s_4'$ are the new constants to be solved for. Using the final condition, $x(t_f) = D$, we get

$$\begin{aligned} D&= \frac{s_1'}{k} - \frac{s_2'}{k} - \frac{c_3+s_3'}{2c_1}t_f+s_4',\\ \therefore \quad s_4'&= D - \left[ \frac{s_1'}{k} - \frac{s_2'}{k} - \frac{c_3+s_3'}{2c_1}t_f \right] . \end{aligned}$$

Using the second final condition, $v(t_f) = v_f$ we get,

$$\begin{aligned} v_f&= s_1' + s_2' - \frac{c_3+s_3'}{2c_1},\\ \therefore \quad s_3'&= 2c_1(s_1'+s_2'-v_f) - c_3. \end{aligned}$$

The equations can then be written as,

$$\begin{aligned} v^*(t)&= s_1' e^{-k(t_f-t)} + s_2' e^{k(t_f-t)} - (s_1' + s_2' - v_f),\\ a^*(t)&= s_1' k e^{-k(t_f-t)} - s_2' k e^{k(t_f-t)},\\ \lambda _1^*(t)&= 2c_1(s_1'+s_2'-v_f) - c_3,\\ \lambda _2^*(t)&= -2c_1 a^*(t). \end{aligned}$$

1.3 Corner conditions

We can now use the corner conditions to determine the unknown constants $s_1, s_2, s_1', s_2'$. The corner conditions state that $\lambda ((t_f-t_2)^+) = \lambda ((t_f-t_2)^-)$ and $v((t_f-t_2)^+) = v((t_f-t_2)^-)$ and $\mu ((t_f-t_2)^+) = 0$,

$$\begin{aligned}&H(t_2^+) = H(t_2^-),\\&\therefore \quad a(t_f-t_2) = 0,\\&\therefore \quad s_2' = s_1' e^{-2kt_2}. \end{aligned}$$

Using the other corner condition $v((t_f-t_2)^+) = v((t_f-t_2)^-)$ we have,

$$\begin{aligned}&v(t_f-t_2) = v_m,\\&\quad \therefore \quad s_1' e^{-kt_2} + s_1' e^{-kt_2} - (s_1' + s_1' e^{-2kt_2} - v_f) = v_m,\\&\quad \therefore \quad s_1' = -\frac{v_m - v_f}{(e^{-kt_2}-1)^2}. \end{aligned}$$

Using similar arguments at the other corner $t=t_1$ we get the final form for the optimal velocity profile as,

$$\begin{aligned} v^*(t) = {\left\{ \begin{array}{ll} s_1 \left( e^{kt} + e^{k(2t_1-t)} -(1+e^{2kt_1})\right) + v_0, \\ 0 \le t \le t_1 \\ v_m, t_1 \le t \le t_f - t_2 \\ s_2 \left( e^{-k(t_f-t-2t_2)} + e^{k(t_f-t)} - (1 + e^{2kt_2}) \right) + v_f,\\ t_f-t_2 \le t \le t_f. \\ \end{array}\right. } \end{aligned}$$

where,

$$\begin{aligned}&s_1 = -\frac{(v_m - v_0)}{(e^{kt_1} - 1)^2},\nonumber \\&s_2 = -\frac{v_m - v_f}{(e^{kt_2} - 1)^2},\nonumber \end{aligned}$$

Using the transversality condition $H(t_f) = 0$, we can determine the times $t_1$ and $t_2$ as,

$$\begin{aligned} t_1 = \frac{1}{k}\ln&\left( \frac{c_4 + c_2v_m^2 - 2c_2v_0v_m}{c_4-c_2v_m^2} + \right. \nonumber \\&\quad \left. \frac{2(c_2v_m(c_4-c_2v_0v_m)(v_m-v_0))^{\frac{1}{2}}}{c_4-c_2v_m^2}\right) , \nonumber \\ t_2 = \frac{1}{k}\ln&\left( \frac{c_4 + c_2v_m^2 - ca_2v_fv_m}{c_4-c_2v_m^2} + \right. \nonumber \\&\quad \left. \frac{2(c_2v_m(c_4-c_2v_fv_m)(v_m-v_f))^{\frac{1}{2}}}{c_4-c_2v_m^2}\right) . \end{aligned}$$

(52)

and the final time can then be calculated by using the total distance to travel and the distances traveled in the two exponential curves. It is easy to see that if $v_0$ or $v_f$ is equal to $v_m$, then $t_1 =0$ or $t_2 = 0$ respectively.

Appendix 4: Proof for Lemma 2

Proof

Consider any velocity profile consisting of a $\mathbf {C}-\mathbf {U}{-}\mathbf {C}$ sequence covering distance $D$. We can replace this ${\mathbf {C}{-}\mathbf {U}{-}\mathbf {C}}$ sequence with a single $\mathbf {C}$ section, so that the resulting velocity profile covers the same distance and consumes energy less than the original profile.

Let $v(t)$ be any velocity profile that contains a ${\mathbf {C}{-}\mathbf {U}{-}\mathbf {C}}$ sequence. That is, ${v(t) = v_m(t)}$ for ${t_0 \le t \le t_1}$ and ${t_2 {\le } t {\le } t_3}$ and ${v(t) < v_m}$ between ${t_1 \le t \le t_2}$. The energy consumption of this profile for traveling a distance ${D = d_{01} + d_{12} + d_{23}}$ is ${E = E_{01} + E_{12} + E_{23}}$, where $E_{ij}$ is the energy spent in traveling $d_{ij}$ for $v(t)$. We construct another velocity profile that is identical to $v(t)$ in ${[t_0,t_1]}$ and ${[t_2,t_3]}$ but covers the section $d_{12}$ at ${v(t) = v_m}$. The energy consumption for this new profile differs only in the $d_{12}$ section.

By following a process similar to that in Lemma 1, we can show that ${E_{12}' \le E_{12}}$ leading to ${E' \le E}$. Hence, any ${\mathbf {C}{-}\mathbf {U}{-}\mathbf {C}}$ sequence can be replaced by a single $\mathbf {C}$ segment to reduce the energy consumption. Hence, the optimal velocity profile will never consist of a ${\mathbf {C}{-}\mathbf {U}{-}\mathbf {C}}$ sequence. $\square$

Appendix 5: Proof of energy model for non-zero initial and final velocities

Lemma 3

Let $\mathcal {\tau }\$be a path with $N$ segments starting and returning to rest, i.e., $v_0(1) = 0$ and $v_f(N) = 0$. Let $E_{14}(i)$ and $E_{16}(i)$ be the minimum energy obtained for the $i^{th}$ segment using Eqs. 5 and 4 respectively. Then $\sum _i E_{14}(i) = \sum _i E_{16}(i)$.

Proof

First, consider the energy obtained using Eq. 4. For each segment, we have

$$\begin{aligned} E_{16}(i)&= \int _{0}^{t_{f}} \Big [ c_1a^2(t) + c_2v^2(t) + c_3v(t) \\&\quad + c_4 + c_5a(t) + c_6v(t)a(t) \Big ]dt\\&= \int _0^{t_f} \Big [ c_1a^2(t) + c_2v^2(t) + c_3v(t) + c_4\Big ]dt\\&\quad + \int _0^{t_f}\Big [c_5a(t) + c_6v(t)a(t) \Big ]dt\\&= E_{16}^{14}(i) + E_{16}^{56}(i). \end{aligned}$$

Now we have,

$$\begin{aligned} \sum _i E_{16}(i)&= \sum _i E_{16}^{14}(i) + \sum _i E_{16}^{56}(i)\\&= \sum _i E_{16}^{14}(i)\\&= \sum _i E_{14}(i). \end{aligned}$$

The second statement follows since $\sum _i E_{16}^{56}(i)=0$ when $v_0(0) = v_f(N) = 0$ as given by [19]. That is, the net effect of $c_5$ and $c_6$ is zero when the robot starts and returns to rest. $\square$