The speed graph method: pseudo time optimal navigation among obstacles subject to uniform braking safety constraints

Abstract

This paper considers the synthesis of pseudo time optimal paths for a mobile robot navigating among obstacles subject to uniform braking safety constraints. The classical Brachistochrone problem studies the time optimal path of a particle moving in an obstacle free environment subject to a constant force field. By encoding the mobile robot’s braking safety constraint as a force field surrounding each obstacle, the paper generalizes the Brachistochrone problem into safe time optimal navigation of a mobile robot in environments populated by polygonal obstacles. Convexity of the safe travel time functional, a path dependent function, allows efficient construction of a speed graph for the environment. The speed graph consists of safe time optimal arcs computed as convex optimization problems in $O(n^3 \log (1/\epsilon ))$ total time, where n is the number of obstacle features in the environment and $\epsilon$ is the desired solution accuracy. Once the speed graph is constructed for a given environment, pseudo time optimal paths between any start and target robot positions can be computed along the speed graph in $O(n^2\log n)$ time. The results are illustrated with examples and described as a readily implementable procedure.

Appendices

Appendix 1: Time optimal path for a disc robot

Consider a disc robot of radius $\rho$ traveling near a point obstacle located at the origin of the (x, y) plane. The path traced by the robot’s center point is parameterized by polar coordinates, $\alpha (\theta )=(r(\theta )\cos \theta , r(\theta )\sin \theta )$ for $\theta \in [\theta _{S},\theta _{T}]$, with the endpoint conditions $r(\theta _{S})=r_{S}$ and $r(\theta _{T})=r_{T}$. The distance between the disc robot and the point obstacle is given by $r(\theta )-\rho$. Braking safety requires that $r(\theta )-\rho \ge d(\nu )$. Substituting $d(\nu )={\nu ^2}/{2 a_{max}}$ while maximizing the speed for travel time minimization gives the constraint: ${\nu (r) =\sqrt{2 a_{max} (r-\rho )}}$ for $r\ge \rho$. The safe travel time functional, $T(\alpha )=\int _{\theta _{S}}^{\theta _{T}} F\big (r(\theta ),r'(\theta )\big )d\theta$, has the integrant: ${F(r,r') = \sqrt{(r^2+r'^2)/2a_{max} (r-\rho )}}$ for $r\ge \rho$. Substituting for $F(r',r)$ in the Euler–Lagrange equation, then integrating with respect to $\theta$, gives the implicit differential equation:

$$\begin{aligned} c^2 \big ( r(\theta )-\rho \big )\big ( r^2(\theta ) + (r'(\theta ))^2 \big ) = r^4(\theta ) \end{aligned}$$

(14)

where c is yet to be determined. The analytic solution of (14) is a sum of two elliptic integrals (see Manor and Rimon 2013a). Rather then use elliptic integrals, a practical conservative approximation for the analytic solution of (14) is next described. First expand the point obstacle into a disc c-obstacle of radius $\rho$, while shrinking the robot to a point. Next, replace the portion of the c-obstacle between $\theta _{S}$ and $\theta _{T}$ by polygonal segments tangent to the c-obstacle as depicted in Fig. 11. The computation of the safe time optimal path in the polygon’s vicinity forms a convex optimization problem (Manor and Rimon 2013a).

Fig. 11

The safe time optimal path determined by the polygonal approximation, $\bar{\alpha }^*$, and the exact safe time optimal path, $\alpha ^*$, near a disc obstacle

Example

The c-obstacle depicted in Fig. 11 is approximated by three segments. The segments and their connecting vertices induce five proximity cells, with variable corner points $p(u_1),\ldots ,p(u_4)$. Convex optimization of the travel time functional ${T(\alpha (u_1,\ldots ,u_4))}$ gives the safe time optimal path $\bar{\alpha }^*$ depicted in Fig. 11. Note that $\bar{\alpha }^*$ closely approximates the exact time optimal path, $\alpha ^*$, with the quality of approximation improving as the number of polygonal segments increases.

Appendix 2: The mobile robot turning radius constraint

This appendix develops an expression for the robot’s minimal turning radius, $r_{min}(\nu )$, as a function of the robot’s speed $\nu$. We shall assume the common case of a four-wheeled mobile robot. However, the expression for $r_{min}(\nu )$ can be easily adapted to other mobile robots. The three main constraints affecting the mobile robot’s turning radius are as follows. First, the turning radius must respect the robot’s maximal steering angles. Second, the centrifugal force affecting the robot must not incur radial sliding of the robot wheels. Third, the lateral moment affecting the robot must be sufficiently low as to prevent tip-over during turning maneuvers. The three constraints are next discussed.

Fig. 12

a Top view of a four-wheel mobile robot turning geometry. b Rear view of a left turning mobile robot

The robot’s turning radius, denoted r, is the current radius of curvature of the curve traced by the robot’s center point. The robot’s inner turning radius, denoted $r_{in}$, is the current radius of curvature of the curve traced by the robot’s innermost wheel (Fig. 12a).

The kinematic turning radius constraint: Let $\gamma$ denote the robot’s front wheels average steering angle (Fig. 12a). This angle can vary within mechanical limits in the range $[-\gamma _{max},\gamma _{max}]$. The robot’s wheels are mounted at the vertices of a rectangular frame having length L and width W (Fig. 12a). Based on simple trigonometry, the average steering angle $\gamma$ is given by the expression: $\tan \gamma = L / ( \tfrac{W}{2}+ r_{in})$. The center point turning radius is given by $r = \sqrt{\L ^2/4 + \left( W/2 + r_{in} \right) ^{2}}$. Replacing $\tfrac{W}{2} + r_{in}$ by $L / \tan \gamma$ gives the turning radius:

$$\begin{aligned} r = \sqrt{\frac{L^2}{4} + \left( \frac{L}{\tan \gamma } \right) ^{2}} \qquad \gamma \in [-\gamma _{max}, \gamma _{max}]. \end{aligned}$$

Since r is monotonically decreasing in $\left| \gamma \right|$, the minimal feasible turning radius, denoted $r_{kin}$, is attained at $\gamma = \pm \gamma _{max}$. Thus $r \ge r_{kin}$, where ${r_{kin} = L \sqrt{1/4 + 1/\tan ^2(\gamma _{max})}}$.

The sliding constraint: Sliding occurs when the centrifugal force acting on the robot, $f_r$, exceeds the radial friction force acting at the ground contacts. The net centrifugal force is given by $f_r = m \nu ^{2}/r$, where m is the robot’s mass, $\nu$ is the robot’s speed, and r is the robot’s turning radius. The radial friction force acting between the wheels and the ground, denoted $f_s$, is given by $f_s =\mu f_N = \mu mg$, where $\mu$ is the coefficient of friction between the wheels and the ground, and $f_N = mg$ is the gravitational force acting downward on the robot. Sliding is prevented as long as $f_r \le f_s$. Substituting for $f_r$ and $f_s$, the minimal turning radius that prevents sliding, denoted $r_{slide}$, is given by $r \ge r_{slide}$, where $r_{slide}(\nu ) = \nu ^2/\mu g$.

The tip-over constraint: Let $\tau _{tip}$ denote the net moment affecting the robot about a horizontal axis passing through its outer wheels’ contacts with the ground. When $\tau _{tip}<0$, the robot will tip-over and loose ground contact at its inner wheels (Fig. 12b). The lateral moment generated on the robot by the centrifugal force $f_r$, denoted $\tau _r$, is given by $\tau _r = H \cdot f_r = -\tfrac{m H}{r} \nu ^2$, where H is the robot’s center of mass height above ground (Fig. 12b). The stabilizing moment generated on the robot by the gravitational force, denoted $\tau _g$, is given by $\tau _g = mg W/2$. Tip-over is prevented when $\tau _{tip} = \tau _r + \tau _g$ is positive semi-definite:

$$\begin{aligned} \tau _{tip}= \tau _g + \tau _r = mg\frac{W}{2} - \frac{m H}{r}\nu ^2 \ge 0. \end{aligned}$$

Hence, tip-over is prevented when $r \ge r_{tip}$, where $r_{tip}$ is given by $r_{tip}(\nu ) = (2 H/g W) \nu ^{2}$.

The robot’s minimal turning radius must satisfy all three constraints as summarized in the following lemma.

Lemma 7.1

The minimal turning radius of a four-wheel mobile robot moving with speed $\nu$, $r_{min}(\nu )$, is given by

$$\begin{aligned} r_{min}(\nu ) = \max \{ r_{kin}, r_{slide}(\nu ), r_{tip}(\nu ) \} \end{aligned}$$

where $r_{kin}$ is the minimal kinematically feasible turning radius, $r_{slide}(\nu )$ is the minimal sliding turning radius, and $r_{tip}(\nu )$ is the minimal tip-over turning radius.

Let us finally establish that the time optimal path which maintains a safe braking distance from the obstacles has a sufficiently flat curvature which prevents any sideways sliding.

Lemma 7.2

Let $\alpha (s):[0,1]\rightarrow \mathcal {F}$ be the time optimal path connecting $p_{S}$ to $p_{T}$ under the uniform braking safety constraint. The path’s radius of curvature is lower bounded by the minimal sliding turning radius, $r(s)\ge r_{slide}(\nu (s))$ for $s\in [0,1]$.

Proof

Let us use the robot’s x coordinate as the geometric parameter s, so that the robo’s time optimal path is given by $\alpha (x)= (x,y(x))$ for $x \in [x_{S},x_{T}]$. The path’s radius of curvature is given by

$$\begin{aligned} r(x) = \frac{\big (1+ (y'(x))^2\big )^{3/2}}{ \left| y''(x) \right| }. \end{aligned}$$

(15)

First consider the time optimal path near a wall segment, which satisfies the differential equation (7): $c^2 (1+(y'(x))^2) y(x) = 1$. Differentiation of both sides of this equation with respect to x gives: $2y(x)y''(x) + 1+(y'(x))^2=0$. Substituting for $y'(x)$ and $y''(x)$ in (15) gives: $r(x) = \tfrac{2}{c} \sqrt{y(x)}$. The robot’s speed along the time optimal path satisfies the equation $\nu = \sqrt{2 a_{max} y}$. Since $a_{max}= \mu g$, the minimal sliding turning radius along the time optimal path is given by $r_{slide}(\nu ) = \nu ^2 / \mu g = 2 y(x)$. Hence, the inequality $r(x) \ge r_{slide}(\nu (x))$ holds true when $\frac{2}{c}\sqrt{y(x)} \ge 2 y(x)$, or equivalently when $y(x)\le 1/c^2$. Since $1/2c^2$ represents the diameter of the rolling circle which generates the cycloid curve, $y(x) \le 1/2c^2 < 1/c^2$, which gives the inequality $r(x) \ge r_{slide}(\nu (x))$ for $x \in [x_{S},x_{T}]$.

Next consider the time optimal path near an obstacle vertex. The time optimal path forms a parabolic curve, whose parametrization in terms of the robot’s x coordinate is given by

$$\begin{aligned} y(x) =\frac{x^2 + y_0^2}{2y_0} \qquad x \in [x_{S},x_{T}] \end{aligned}$$

where $y_0$ is the minimal distance between the parabolic curve and the x axis aligned with the line l (Fig. 2). Based on (15), the path’s radius of curvature is given by $r(x)= y_0 \cdot \left( 1+\tfrac{x^2}{y_0^2}\right) ^{3/2}$. The parabolic curve is equidistant from the obstacle vertex and the line l. Hence, the robot’s speed along this time optimal path is given by $\nu (x) = \sqrt{2a_{max} y(x)}= \sqrt{a_{max} (x^2 + y_0^2)/y_0}$. Since $r_{slide}(\nu ) = \nu ^2 / \mu g$ and $a_{max}=\mu g$, the inequality $r(x)\ge r_{slide}(\nu (x))$ holds true since $y_0 \cdot (1+\tfrac{x^2}{y_0^2} )^{3/2} \ge y_0 \cdot (1 + \tfrac{x^2}{y_0^2})$, which again confirms the inequality $r(x) \ge r_{slide}(\nu (x))$ for $x \in [x_{S},x_{T}]$. $\square$