# Odometric navigation with matching of landscape features

Michel Parent, Edern Ollivier

► To cite this version:

Michel Parent, Edern Ollivier. Odometric navigation with matching of landscape features. ICARCV 2002: The Seventh International Conference on Control, Automation, Robotics and Vision, IEEE, Dec 2002, Singapore, Singapore. pp.757-762, 10.1109/ICARCV.2002.1238517. hal-01915083

HAL Id: hal-01915083

https://inria.hal.science/hal-01915083

Submitted on 7 Nov 2018

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_1.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# Odometric Navigation with Matching of Landscape Features

Edern Ollivier

Michel Parent

INRIA/IMARA

BP105

F-78153 Le Chesnay Cedex

France

Tel. 33-1 39 63 55 93

Fax: 33-1 39 63 54 91

e-mail: Edern.Ollivier@inria.fr,

Michel.Parent@inria.fr

**Abstract**— This paper presents a technique to localize and guide outdoors vehicles by both vehicle odometry and landmark localization. In this case, we use reflective poles spaced out about 50 m apart and located at the front of the vehicle. The LADAR detects distances, angles, speed, and the position of obstacles within 20 m. Odometry is performed by a steering sensor and encoder attached at the vehicle differential. Using this information, the embedded system matches the vehicle's computed position with the location of the poles. Finally, passenger safety is ensured by an anti-collision subsystem thanks to the LADAR sensor.

*Index Terms*—Automated vehicles, Robotics, Intelligent Transportation, Cybercars, PRT, Public Transport, CTS.

## 1. INTRODUCTION

Most European cities face numerous challenges associated with the use of private vehicles. Problems include road congestion, energy expenditure, noise and pollution, all of which degrade the quality of urban life. Historical city centres are facing severe problems: traditional commerce declines, moves to the periphery, and they become less attractive to tourists.

### PROJECT BACKGROUND

The CTS (Cybernetic Transportation System) concept started with car-sharing: a fleet of individual vehicles shared among a relatively large number of users, offers the possibility of using a car for some time or having a car available at both ends of a train trip. These systems are increasingly popular in Switzerland and Germany. They work well in specific areas where the demand is properly structured, but have yet to offer a door to door service: currently, the vehicles are only available at a few locations and have to be returned. Modern fleet management technologies have recently improved the service, e.g. allowing short trips without the need to return the

car to its point of origin. Such systems are called station-car systems and have been developed since the mid-90's. For example, Praxitèle and Liselec Projects in France, City-Car in Switzerland, IntelliShare in the USA, Crayon in Japan (Fig. 1), just to name a few.

<figure><img src="image_2.png" /><figcaption>Fig. 1: Crayon station car system</figcaption></figure>

Specific vehicles, well adapted to city driving, have generally been used for these systems: small size, convenient, energy efficient, quiet, often based on electric power. They even compete with public transportation in terms of energy consumption on per passenger-km basis. However, these systems have not generally proven that they can compete economically, one reason being limited vehicles availability in too few locations, thus limiting the number of potential customers (see European Project Utopia).

Cybercars is a car-sharing concept that can dramatically improve upon exist fleet management systems. Here, vehicles move by themselves in response to the demand of passengers or the need to move goods.
## 2. WHAT IS A CYBERCAR?

Cybercars are road vehicles (Fig. 2) with fully automated driving capabilities. A fleet of such vehicles forms a transportation system (CTS), for passengers or goods, on a network of roads with on-demand and door-to-door capability. The fleet of cars is under control of a central management system in order to meet particular demands in a particular environment. At the initial stages, cybercars are designed for short trips at low speed in an urban environment or in private grounds.

In the long term, cybercars could also run autonomously at high speed on dedicated tracks. With the development of the CTS infrastructures, private cars with fully autonomous driving capabilities could also be allowed on these infrastructures while maintaining their manual mode on standard roads.

<figure><img src="image_3.png" /><figcaption>Fig. 2: Frog cybercar in Schiphol</figcaption></figure>

The advantages of autonomous driving capabilities and the new transportation systems, based on environmental friendly vehicles, are numerous.

- First, they provide reduction of congestion, and better traffic flow, air quality and energy conservation.

- Second, in automatic mode, the systems are very safe. In fact, there is no need for a driver's license, so anyone can use it, including the elderly and person with handicaps.

- Third, the cars can be moved easily from one location to another, using fully autonomous driving platoon formations with a single driver.

- Fourth, the cars can drive autonomously to a remote parking area when not needed, hence leaving valuable urban space free for pedestrians and cyclists.

- Fifth, the concept and technologies are also appropriate for delivery of goods in city centers and even for garbage collection: the same infrastructure could be used by specifically adapted vehicles with delivery (or collection) "boxes".

- Finally, flexible design will make it possible to optimize the overall system performance, taking into account the

needs and requirements of the private consumer, the system operator and the public (e.g. municipality), permitting the system to operate in different modes at different times of the day, week and year.

Several companies and research organizations have been involved, during the last years, in the development of these new vehicles (Fig. 3). Most of the actors have established a partnership to launch the CyberCars project.

<figure><img src="image_4.png" /><figcaption>Fig. 3: CyCab from INRIA/Robosoft</figcaption></figure>

## 3. AUTOMATED GUIDED VEHICLE

The testing vehicle is an outdoor electrical car made by Yamaha based on a golf car frame, equipped with sensors and an embedded PC. Three driving modes are available: manual, wire-guided and automatic. For this experiment, we use the vehicle in the automatic mode i.e. speed and steering controls are managed by our algorithms. The following are brief descriptions of the sensors:

### 3.1. Encoder

One low-cost encoder in quadrature is attached to the differential of the vehicle. It directly measures mean rotation of the two rear wheels.

### 3.2. Steering sensor

One potentiometer is attached to the steering shaft.

### 3.3. Gyrometer

One low-cost gyrometer [1] measures the heading rotation speed at the frequency of 30 Hz.

### 3.4. GPS

One low-cost GPS receiver [2] provides the geographical coordinates at the frequency of 1 Hz.

### 3.5. LADAR

The LADAR, a laser scanner [3], transmits a pulsed laser beam which uses a rotating mirror to scan an area in front of the vehicle. Within this area of 20 m x 20 m, distances and angles to poles are measured such that position and speed are available to the vehicle computer via a CAN bus at the frequency of 10 Hz.
The distance measurement resolution of the LADAR is 3.9 mm with an accuracy of +/- 5 cm. The angle increment is 0.25° so that one reflective pole of Ø 5 cm can be detected from up to a distance of 12 m.

## 4. VEHICLE POSITION

The vehicle position is computed by fusion of odometric, gyroscopic and GPS observations via Kalman Filtering.

### 4.1. Odometric model

The vehicle posture is classically defined as follows: position x and y, coordinates in a local reference frame (O, x₀, y₀), and ψ, the heading of vehicle (Fig. 4). The localized mobile frame (M, x, y) is attached to the middle of the rear wheels. φ represents the steering angle and v the speed vector in the model. The point G is the instantaneous rotation center.

<figure><img src="image_5.png" /><figcaption>Fig. 4: Kinematics model</figcaption></figure>

The equations of movement of the vehicle at the point M in the reference frame are:

$$ 
\left\{ 
\begin{aligned}
\frac{dx}{dt} &= v \cos \psi \\
\frac{dy}{dt} &= v \sin \psi \\
\frac{d\psi}{dt} &= \frac{v}{\rho} = v \frac{\tan \varphi}{d}
\end{aligned}
\right.
\qquad (1) $$

Here, we assume zero wheel slip and perfect planar rolling surface conditions. Given these assumptions we have the following discrete model:

$$ 
\begin{bmatrix} x_k \\ y_k \\ \psi_k \end{bmatrix} = \begin{bmatrix} x_{k-1} \\ y_{k-1} \\ \psi_{k-1} \end{bmatrix} + \begin{bmatrix} \Delta l \cos \psi_{k-1} \\ \Delta l \sin \psi_{k-1} \\ \Delta \psi \end{bmatrix} + \alpha_{k-1} \quad (2) $$

Where, Δl represents the variation of distance and Δψ the elementary variation of heading between two sampling instants. The state noise is α.

$$ \Delta \psi = \Delta l \frac{\tan \varphi_k}{d} \qquad (3) $$

The integration of the odometric data, Δl from the encoder and φk from the steering sensor, is processed at the frequency of 30 Hz.

### 4.2. Heading estimation

The heading of vehicle ψ is updated by measures from GPS and gyrometer. A Kalman filter [4] is applying with the following equations of model and observation:

$$ \begin{bmatrix} \psi_k \\ \frac{d\psi}{dt} \end{bmatrix} = \begin{bmatrix} \psi_{k-1} + \Delta\psi \\ \frac{\Delta\psi}{\Delta t} \end{bmatrix} + \beta_{k-1} \quad (4) $$

$$ \begin{bmatrix} \psi_k \\ \frac{d\psi}{dt} \end{bmatrix} = \begin{bmatrix} \psi_{gyro k} \\ \omega_{gyro k} \end{bmatrix} + \chi_k \qquad (5) $$

ωgyro represents the heading rotation speed given by the gyrometer. The state noise is β and measure noise χ, both represented by centered and independent white noises with known variances.

### 4.3. Position estimation

We now describe the method used to generate a "best" estimate of the vehicle position. Essentially, GPS and LADAR correct the prediction of the vehicle position given by the odometric model (2) using a Kalman Filter with the state and command vectors given as:

$$ 
\begin{align} 
X_k &= \begin{bmatrix} \delta x_k \\ \delta y_k \\ \delta \psi_k \end{bmatrix} = \begin{bmatrix} x_k - x_k^0 \\ y_k - y_k^0 \\ \psi_k - \psi_k^0 \end{bmatrix} \tag{6} \\ 
U_k &= \begin{bmatrix} \Delta l \\ \Delta \psi \end{bmatrix} \notag 
\end{align} $$
$X_k$ represents the perturbation on the last estimated state $X^0_k$. The linearization of (2) by a first order Taylor series approximation makes usable the standard Kalman formalism.

$$X_k = \begin{bmatrix} 1 & 0 & -\Delta t \sin \psi_{k-1}^0 \\ 0 & 1 & \Delta t \cos \psi_{k-1}^0 \\ 0 & 0 & 1 \end{bmatrix} X_{k-1} + \delta_{k-1} \quad (7)$$

Previous equation forms the Kalman equation of model. The state noise is $\delta$, represented by centered and independent white noise with known variance.

"If the problem is sufficiently observable, then the deviations between the estimated trajectory and the actual trajectory will remain sufficiently small that the linearization assumption is valid [5][6]" [7].

### 4.3.1. GPS observations

The GPS provides dead-reckoning feedback by measuring the vehicle position, designated as $x_{gps}$, $y_{gps}$ and $\psi_{gps}$, at the frequency of 1 Hz:

$$\begin{bmatrix} x \\ y \\ \psi \end{bmatrix} = \begin{bmatrix} x_{gps} \\ y_{gps} \\ \psi_{gps} \end{bmatrix} + \varepsilon \quad (8)$$

The measure noise is $\varepsilon$, represented by centered and independent white noise with known variance.

### 4.3.2. LADAR observations

As stated earlier, the LADAR provides dead-reckoning feedback by measuring the vehicle location with respect to the reflective poles spaced throughout the vehicle course.

Poles are easily identified by the LADAR up to a range of 12 m away at the frequency of 10 Hz. The LADAR detects the nearest pole (point A, Fig. 4) and tracks its positions until the vehicle passes the pole. The measurement provides the position and orientation of pole in the vehicle frame, here designated by $X_A$, $Y_A$ and $\Psi_A$.

Secondly, the position and orientation of pole in the global frame is known, designated as $x_A$, $y_A$ and $\psi_A$. The vehicle position is given by the formula:

$$\begin{bmatrix} x \\ y \\ \psi \end{bmatrix} = \begin{bmatrix} x_A \\ y_A \\ \psi_A \end{bmatrix} - \begin{bmatrix} \cos \psi & \sin \psi & 0 \\ -\sin \psi & \cos \psi & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X_A \\ Y_A \\ \Psi_A \end{bmatrix} + \gamma \quad (9)$$

The measure noise is $\gamma$, represented by centered and independent white noise with known variance. The vehicle updates its position if a pole is detected by the LADAR and identified in the GIS database (Geographical Information System) of vehicle.

## 5. VEHICLE GUIDANCE

A central management system sends the path to follow, a list of positions and orientations, to the vehicle via a Wave-LAN network. Then, the vehicle guidance is ensured by the target planning and the trajectory following subsystems (Fig. 5) as described below.

### 5.1. Target planning

A target position, designated by $x_t$, $y_t$ and $\psi_t$ in the global frame, is computed at the frequency of 10 Hz depending on the path and the nearest obstacle.

Obstacle positions are detected by the LADAR and projected onto the global coordinate system. Based upon this data and the specified minimum distance of collision, vehicle speed is reduced until the obstacle has cleared the path.

### 5.2. Trajectory following

#### 5.2.1. High level

Driving commands are computed depending on the distance between the vehicle position and the target position. The error, designated by $e_x$, $e_y$ and $e_\psi$, is equal to the coordinates of target position in the vehicle frame:

$$\begin{bmatrix} e_x \\ e_y \\ e_\psi \end{bmatrix} = \begin{bmatrix} \cos \psi & \sin \psi & 0 \\ -\sin \psi & \cos \psi & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x_t - x \\ y_t - y \\ \psi_t - \psi \end{bmatrix} \quad (10)$$

Then, we apply the following law of command modified from [8]:

$$\begin{bmatrix} \frac{d\varphi_c}{dt} \\ \varphi_c \end{bmatrix} = \nu_{\max} \begin{bmatrix} K_{xy} \frac{e_x}{e_x} + K_{\psi} \sin e_\psi \\ \frac{e_x}{\cos e_\psi} \end{bmatrix} \quad (11)$$

Here, $\nu_c$ and $\varphi_c$ represent the commands of speed and steering angle. $\nu_{\max}$ is the maximum speed of the vehicle associated with the target position. $K_{xy}$ and $K_{\psi}$ are constant parameters tuned to obtain an optimal trajectory following.

#### 5.2.2. Low level

PI controllers are used to control the speed and the steering angle of vehicle. At $k^{th}$ time iteration,

$$\varphi_s = K_p (\varphi_c - \varphi)_k + K_I \sum_{i \le k} (\varphi_c - \varphi)_i \quad (12)$$

$\varphi_s$ represents the control of steering angle updated at the frequency of 30 Hz. $K_p$ and $K_I$ are proportional and integral gains tuned to obtain the lateral control of vehicle.
## 6. IMPLEMENTATION

To guarantee safe and reliable performance, we have implemented our guided transportation system with RTAI Linux [9]. Besides the well-known benefits of strict timing constraints, RTAI also provides numerous features that aid our development. In our case, we make extensive use of operating modules, communicating asynchronously with mailboxes to achieve the tasks of detection, navigation, and controls. This is schematically shown Fig. 5.

<figure><img src="image_6.png" /><figcaption>Fig. 5: Software architecture</figcaption></figure>

## 7. CONCLUSION

In conclusion, we have presented a navigation technique for outdoor low-speed vehicles which improves the odometric navigation with matching of landscape features.

<figure><img src="image_7.png" /><figcaption>Fig. 6: Experimental result by trajectory following {circles} with matching of poles {crosses}</figcaption></figure>
## ACKNOWLEDGMENT

The CyberCars project is funded by the European Commission Programme IST (Information Society Technologies) and the CyberMove project is funded by the EC Program EESD (Energy, Environment and Sustainable Development).

## REFERENCES

[1] Piezoelectric vibratory Gyroscope "Data sheet of Gyrostar ENV-05F-03", muRata, April 2000.

[2] Lassen-SK8 GPS Board for embedded applications "System designer reference manual", Trimble, August 1997.

[3] IBEO LADAR digital A 3P "Operating manual 1.1.0", November 1999.

[4] Welch G., Bichop G. "An Introduction to the Kalman Filter". University of North Carolina. December 1995.

[5] Grewal M. S., Andrews A. P. "Application of Kalman Filtering to GPS, INS, & Navigation (Notes)". Kalman Filtering Associates, Anaheim, CA 2000.

[6] Itô K. "Lectures on Stochastic Processes". Tata Institute of Fundamental Research, Bombay 2000.

[7] Grewal M. S., Andrews A. P. "Kalman Filtering, Theory and Practice Using MATLAB" p.175 p.179. John Wiley & Sons, Inc. 2001.

[8] Kanayama Y., Kimura Y., Myazaki F., Noguchi T. "A stable tracking control method for a non-holonomic mobile robot". IEEE Int. Workshop on Intelligent Robots and Systems, vol. 2, p. 1236-1241, Osaka, 1991.

[9] Bianchi E., Dozio L., Mantegaza P. "RTAI a Hard Real Time support for LINUX". DIAPM.

Parent Michel, Texier Pierre-Yves. "A Public Transport System Based on Light Electric Cars". Fourth International Conference on Automated People Movers. Irving, USA. March 1993.

Parent Michel, Dumontet François, Texier Pierre-Yves and Laurent Fabien. "Design and Implementation of a Public Transportation System Based on Self-Service Electric Cars". IFAC/IFORS Congress. Tianjin, China. August 1994.

Parent Michel, Fauconnier Sylvain. "Design of an Electric Vehicle Specific for Urban Transport". Congrès EVT'95. Paris, November 1995.

Parent Michel, Daviet Pascal. "Automated urban vehicles : towards a dual mode PRT (Personnal Rapid Transit)". IEEE Int. Conf. on Robotics and Automation, Minneapolis, Minnesota, April 22-28, 1996.

Parent Michel, Fauconnier Sylvain, Lisowski Leszek. "PRAXI: A Small Electric Vehicle Specific for Urban Transport". Electric Vehicle Symposium EVS'13. Osaka, Japan. October 1996.

Parent Michel. "Automated Public Vehicles: A First Step Towards the Automated Highway". 4th World Congress on Intelligent Transport Systems. Berlin, 21-24 October 1997.

Parent Michel, Bleijs Cyriacus. "The CyCab: an Electric Vehicle Specifically Designed for Car-Free Cities". Electric Vehicle Symposium EVS'15. Bruxelles. October 1998.

Gualino David, Parent Michel, Uchanski Michael. "Autonomous Lateral Control of a Vehicle Using a Linear CCD Camera". IEEE Conference on Intelligent Vehicles. Stuttgart 1998.

Massot Marie-Hélène, Allouche Jean-François, Benéjam Evelyne, Parent Michel. "Praxitèle: Preliminary Results from the Saint-Quentin Experiment". TRB'99. Washington. January 1999 and Transportation Research Record 1666. Paper n° 99-1457, pp.125-132.

Laugier C., Fraichard T., Garnier P., Paromtchik I.E., Scheuer A. "Sensor-Based Control Architecture for a Car-Like Vehicle". Autonomous robots. Vol.6, n° 2. Mai 1999.

Laugier C., Paromtchik I.E., Parent M. "Developing Autonomous Maneuvering Capabilities for Future Cars". International Conference on Intelligent Transportation systems. Pp.68-73. Tokyo, Japan. October 1999.

Langheim J., Divko R., Tango F., Artis JP, Wahl M., Lavergne Ch., Lages U., Ludwig S., Parent M., Tarel J.-Ph., Buchanan A., Steux B. "Sensing of Car Environment at Low Speed Driving". Conference ITS. Turin, 6-9 November 2000.