A Framework for Mobile Robots Localization and Mapping

João Castrojoao.de.castro@tecnico.ulisboa.pt

Instituto Superior Técnico, Universidade de Lisboa, Lisboa, Portugal

June 2020

Abstract

One of the most important features of a mobile robot is the capability to localize itself in anunmapped environment. To do it, it is necessary to produce a map of the environment while atthe same time localizing itself inside that map. This problem is called simultaneous localizationand mapping (SLAM). In this thesis, a framework was constructed using a mobile robot that wasinstrumented to give it capabilities of autonomous self-localization and mapping of the surroundingenvironment. The mobile robot is a remote-controlled (RC) small toy car equipped with a RaspberryPi, a laser scanner, odometry sensors, and markers to allow a positioning system to determine thecar’s pose. Three Concurrent Localization and Mapping (CLM) algorithms were implemented thatattempt to localize the car and produce a map of the surrounding environment. These make use ofthree registration algorithms and a model of the car, all of which were implemented in this thesis. ASimulink software suite was also developed that was used to implement one of the CLM algorithmsand a controller. This software suite was also extended to work with two other similar cars.Keywords: concurrent localization and mapping, registration algorithms, mobile robotics, instrumen-tation

1. Introduction

The localization of a body and simultaneous map-ping of the environment, usually called simultane-ous localization and mapping (SLAM), is a problemencountered in multiple domains, such as: surgicalrobots [1], modeling of real-world objects [2], self-driving cars [3], autonomous industrial robots [4],virtual reality headsets, and mobile robotics [5].The latter is the focus of this thesis, specificallysmall mobile robotics. An example of this field isthe autonomous home vacuum cleaners that needto produce a map of a room to clean it properly.

With the increasing importance of autonomousrobots this problem has been heavily researchedin the last decades and is already a mature sub-ject with several solutions, [6, 7]. There are alsocommercial products available using SLAM tech-nology including the previously mentioned virtualreality headsets and home vacuum cleaners. How-ever, there is still a lot of research being done toimprove the solutions available.

The main contribution of this thesis is a frame-work to support autonomous navigation and coop-eration among robots with ground truth validation.The robots used are a set of small-scale RC car mod-els equipped with multiple sensors. A diagram rep-resenting the framework built is shown in figure 1.Its components are the vehicle’s actuators, a posi-

tioning system that measures the vehicle’s positionand orientation, an encoder that measures the ve-hicle’s speed, an Inertial Measurement Unit (IMU)that measures the linear accelerations and angularvelocities and a laser scanner that produces pointclouds of the environment. Moreover, the system isdistributed, allowing several mobile robots to coex-ist in the same environment, paving the way for thedevelopment of cooperative or collaborative strate-gies among robots.

Three applications of the architecture are alsoproposed in the form of Concurrent Localizationand Mapping (CLM) algorithms. The SLAM prob-lem is solved in [8, 9] assuming that landmarks existin the environment, whose true location is assumedtime invariant. Concurrent Localization and Map-ping (CLM) algorithms however, do not require theexistence of specific landmarks, thus not requiringstructuring the environment.

2. Background

Most CLM algorithms require the use of a registra-tion algorithm. In this thesis, the registration al-gorithm used is the Normal Iterative Closest Point(NICP), introduced in [10] and described in section2.2. The map construction algorithm used is intro-duced in [11] and described in section 2.3.

1

Positioning

Encoder

IMU

Laser Scanner

Actuators

Figure 1: Diagram of the architecture implemented.

2.1. Registration IntroductionIn registration algorithms, there are two pointclouds: the model cloud and the data cloud. Thesealgorithms try to find a coordinate transformationfor the data point cloud that makes it best fit themodel point cloud, meaning, a coordinate transfor-mation that makes all the points in the data pointcloud project onto their real-world location in themodel cloud reference frame. This means that iftwo points in each of the point clouds are sampledfrom the same point in the real world they shouldend up with the same coordinates after the datapoints are transformed by the coordinate transfor-mation. The coordinate transformation is modeledby two parameters, the rotation matrix R and thetranslation vector T , and is defined by the followingexpression:

pm = Rpd + T (1)

In this expression, the superscript denotes the ref-erence frame of the mathematical object, that is,pd is the vector of coordinates of the point p in thedata cloud reference frame. After the coordinatetransformation, the vector of coordinates pm thatdescribes the same point but in the model cloudreference frame is obtained.

In this thesis, the point clouds are in two dimen-sions, meaning, the previously mentionedR is a 2×2matrix described by one parameter, tθ, and T is a2×1 vector described by two parameters, tx and ty.The definition of R and T according to these param-eters can be found in equations 2 and 3 respectively.

R =

[cos tθ − sin tθsin tθ cos tθ

](2)

T =

[txty

](3)

The registration algorithms estimate the follow-ing vector of parameters:

t =

txtytθ

(4)

2.2. Normal Iterative Closest PointNormal Iterative Closest Point (NICP), introducedin [10] for the 3D case, is a variation of ICP that usessurface normals to improve its accuracy. It does soby trying to align the surface normals around cor-responding points and only penalizing the distancealong the surface normal, which results in surfacesbeing allowed to slip along themselves. The normalsare calculated from covariance matrices as shown in[12] and [13].

The estimation of the surface normal around apoint starts with choosing the nearby points overwhich the surface information is going to be calcu-lated. This means choosing the number of points,on each side, over which the surface normal is goingto be calculated, called the neighborhood radius.This value is different for each point and depends ontwo things: the sensor noise and the surface bound-aries. The larger the noise, the more points shouldbe considered to maintain the normal estimationaccuracy. The surface boundaries are crucial, sincecalculating normals with points from a different sur-face results in very large errors.

The covariance matrix is calculated over thepoints inside the neighborhood radius. It is calcu-lated using equation 5 in order to make it possibleto use integral images.

Ci =1

n

([∑pxpx

∑pxpy∑

pxpy∑pypy

]−

1

n

[∑px∑py

] [∑px

∑py]) (5)

Here, n is the number of points and px and pyare the x and y coordinates of each point. Thesecoordinates are summed over the points inside theneighborhood radius of the point i.

The eigendecomposition of matrix Ci is used toextract the surface normal, the surface curvatureand the coordinate transformation matrix, which isused to transform a vector from the reference frameformed by the surface normal and tangent vectorsto the point cloud’s reference frame.

The surface normal is the eigenvector correspond-ing to the eigenvalue with the smallest absolutevalue. The surface curvature is given by: γc =

|Dmin||Dmin|+|Dmax| where Dmin and Dmax are the small-

est and largest eigenvalue respectively. The co-ordinate transformation matrix, V , contains thetwo eigenvectors in its columns and, when leftmultiplied, transforms a vector’s coordinates fromthe normal tangential reference frame to the pointcloud’s reference frame.

Normal Iterative Closest Point (NICP), like Iter-ative Closest Point (ICP) described in detail in [14],is an iterative algorithm with the following generalsteps:

2

1. Build association pairs with one model pointand one data point.

2. Filter out outliers, that is, detect pairs of pointsthat are wrongly associated so they can be ig-nored in the next step.

3. Find a rotation matrix and a translation vectorthat best transforms the data points into thecorresponding model points.

4. Go back to the beginning unless a stopping cri-terion has been met.

The association method used consists in findingthe model point that has the smallest Euclidean dis-tance to the projection of each data point. The pro-jection of the data points is performed with equa-tion 1 where R and T are the ones calculated in theprevious iteration. The naive approach of calculat-ing the distance to all model points for each datapoint and choosing the smallest is very slow. Inorder to decrease the computational cost, a Delau-nay triangulation of the model point cloud is per-formed in the initialization of the registration algo-rithm and is then used to find the aforementionedclosest model point to every projected data point.

The filtering of outliers is performed by addinga weight wi to each pair of points, function of theEuclidean distance between the points of the pair.The weight functions implemented return a valuebetween 0 and 1 and are described in [15] wherethey are discussed in depth.

The rotation matrix and the translation vectorare found by minimizing the following objectivefunction:

W =

n∑i=0

wiei(T )TΩiei(T ) (6)

where:

ei =

(Rpi + T )− qi(atan

~ndi,y~ndi,x

+ tθ

)− atan ~n

mi,y

~nmi,x

(7)The Ωi matrix is subdivided into two matrices:

Ωi =

[Ωsi 00 Ωni

](8)

Here, Ωsi is a 2×2 matrix that scales the first twocomponents of the error vector to only penalize thedistance along the surface normal. Ωni is a scalarthat determines the importance of aligning the nor-mals. To calculate Ωsi , the eigenvectors matrix Vof the model point, together with a diagonal scalingmatrix S, is used in the following expression:

Ωsi = ViSiVTi (9)

The matrix S has two values, Sn and St, thatscale the normal and tangent components of theerror vector respectively. The position of Sn andSt in the diagonal depends on which eigenvectorcorresponds to the surface normal and which corre-sponds to the surface tangent. This is determinedfrom the matrix D of the eigendecomposition. Thevalue of Sn and St are the inverse of the correspond-ing eigenvalues Dmin and Dmax respectively. Thevalue of Ωni is set to one.

The solution to this minimization problem wasfound using the Levenberg–Marquardt algorithm.

2.3. Simultaneous Localization And Mapping

Simultaneous localization and mapping consists indetermining the position and orientation of a bodyin an unknown environment while at the same timeproducing a map of it. To do that, the trajectoryof the body is discretized into points in time, callednodes in this thesis, whose position and orientation,called pose, is determined. Since there is no pre-made map of the environment, the pose of each nodeis calculated relative to the first node.

The pose can be calculated from the addition ofthe relative poses between consecutive points, de-scribed in [16]. When the CLM is performed inthis way, small errors in the calculation of the rel-ative pose between two nodes inevitably grow intolarge errors in the pose estimates of the subsequentnodes, called dead-reckoning error. This results inthe CLM algorithm giving very different poses forthe body when it goes through the same place inthe real world multiple times.

To solve the problem of the dead-reckoning er-ror, it is possible to construct a network of relativeposes by directly computing the relative pose be-tween two distant nodes with similar poses, thatis, when the body goes through the same place inthe real world twice. This direct relative pose canbe computed by using a registration algorithm ontwo point clouds of the surrounding environmentproduced by a laser scanner, a depth camera or anormal camera. The network of relative poses canbe solved to find the best estimate of the currentand past absolute poses. Note that, in this solu-tion, the estimate of the absolute pose depends notonly on the pose relative to the previous node buton the pose relative to the subsequent nodes. Hu-mans locate themselves in a similar way, being ableto correct their previously estimated position whenthey arrive at a place they have been before. Thisconstruction and solution of the network of relativeposes is presented in [11] for the 2D case and in [17]for the 3D case.

Another way of solving this problem is by usingmeasurements from a positioning system. This datacan be combined with odometry data to produce an

3

estimate of the trajectory that is resilient to misseddata from the positioning system.

The CLM algorithm used, based on a networkof relative poses, is described in [11]. It is an op-timization algorithm that minimizes the followingcriterion:

W =∑{i,j}∈P

(tai − taj − trij

)TC−1ij

(tai − taj − trij

)(10)

Here, tai is a vector defining the absolute pose ofnode i and trij is the measured relative pose betweenthe nodes i and j, that is, the measured value of tai −taj . Cij is the covariance matrix of the relative posemeasurements and the summation is performed overthe set P that contains all pairs of connected nodes.

The CLM algorithm used, based on data from apositioning system, is composed of a Kalman filterwith the following equations:

xk|k−1 = Fxk−1|k−1 +Guk−1 (11)

Pk|k−1 = FPk−1|k−1FT +Q (12)

Sk = CPk|k−1CT +R (13)

Lk = Pk|k−1CTS−1k (14)

xk|k = xk|k−1 + Lk(ŷk − Cxk|k−1

)(15)

Pk|k = (I − LkC)Pk|k−1 (16)

In these equations, P , S and L are, respectively,the state variables covariance matrix, the innova-tion covariance matrix and the Kalman filter gain.The matrices Q and R are constants which repre-sent the process covariance and the outputs covari-ance. When there is no observation, that is, whenthe positioning system fails, the values of matrixL are set to zero since the innovation covarianceis infinite. The inputs vector contains the veloci-ties in earth’s reference frame which come from thebody’s state observer. The outputs vector containsthe position and orientation data from the position-ing system.

3. Implementation

In this section, the five parts of the architectureshown in figure 1 are described. This is followed bya description of the software implemented.

3.1. Actuators

The vehicle used in this thesis is a Desert Prerun-ner 1/18 Scale 4WD Truck from LaTrax [18]. Ithas four wheels, all-wheel drive and front steering.The equations for the model of the car, based on abicycle model, are in equations 17 to 21 illustratedby the diagram in figure 2.

X

Y

ψFRy FFy

δ

ab

FeFd

Figure 2: Diagram of the car.

ẍb = ẏbψ̇ +1

m

(Fe − Fd − FFy sin δ

)(17)

ÿb = −ẋbψ̇ + 1m

(FRy + F

Fy cos δ

)(18)

ψ̈ =1

I

(aFFy cos δ − bFRy

)(19)

ẋe = ẋb cosψ − ẏb sinψ (20)ẏe = ẏb cosψ + ẋb sinψ (21)

ẍb and ÿb are the linear accelerations in the car’sreference frame, ψ̈ is the angular acceleration of thecar around the vertical axis and ẋe and ẏe are thecar’s velocities in the earth’s frame of reference.

There are four constants: I, m, a and b. Theformer two, I and m, correspond, respectively, tothe car’s rotational inertia around the vertical axisand the car’s mass. The constant a is the distancebetween the front wheel axis and the car’s centerof mass, while b is the distance between the backwheel axis and the car’s center of mass. The car isapproximated to a rectangular box to calculate I,while the remaining three constants are measureddirectly from the car. Afterward, five variables haveto be characterized: Fe, Fd, F

Ry , F

Fy and δ which

are, respectively, the force produced by the engineon the wheels, the drag forces, the side force on therear tires, the side force on the front tires and thesteering angle of the front wheels.

There are two inputs to the car in the form ofPulse Width Modulation (PWM) signals. One sig-nal controls the driving motor that is connected toall four wheels while the other signal controls thesteering servo that turns the front steering wheels.Both PWM signals sent to the actuators have aduty cycle between 10% and 20%. To obtain a morepractical range, the model’s inputs are numbers be-tween −1 and 1 corresponding to 10% and 20% re-spectively. The inputs to the model are: ue andud, with values between −1 and 1, the former con-trols the engine force Fe and the latter controls thesteering angle δ. A value of zero for ue will stopthe driving motor, while a value of zero for ud willmake the car go roughly in a straight line.

3.2. Positioning SystemThe positioning system can measure, among otherthings, the position and orientation a body in three-

4

dimensional space. In this thesis, only the positionand orientation on the horizontal plane is used. Thethree degrees of freedom on the horizontal plane arethe positions x and y and the rotation around the zaxis, ψ. To characterize the noise, these three val-ues were measured with the car standing still. Thevalues of x and ψ, returned by the positioning sys-tem, are shown, with a fitted normal distribution,in figure 3. The values of y follow a distributionsimilar to the one that the values of x follow and,as such, are not included for brevity.

4.88075 4.8808 4.88085 4.8809 4.880950

2000

4000

6000

8000

10000

12000

14000

Experimental values

Fitted normal distribution

(a) x

-2.99 -2.9895 -2.989 -2.9885 -2.988 -2.9875 -2.987 -2.98650

100

200

300

400

500

600

700

800

900

Experimental values

Fitted normal distribution

(b) ψ

Figure 3: Values returned by the positioning systemwith the car standing still.

3.3. Encoder

An encoder was mounted into the car, measuringthe speed of rotation of the motor. The relation-ship between the car’s velocity and the number ofimpulses per second can be seen in figure 4a. Thedistribution of encoder error values is shown in fig-ure 4b.

0.8 1 1.2 1.4 1.6 1.8 21000

1200

1400

1600

1800

2000

2200

2400

2600

2800

3000

Experimental

Fitted model

(a) Relationship betweenencoder and velocity.

-0.3 -0.25 -0.2 -0.15 -0.1 -0.05 0 0.05 0.1 0.15

Speed Error (m/s)

0

1

2

3

4

5

6

7

8

9

10

Pro

babili

ty D

ensity F

unction

Distribution of encoder error values

Experimental values

Fitted normal distribution

(b) Encoder error distribu-tion.

Figure 4: Encoder characterization.

3.4. Inertial Measurement Unit

An Inertial Measurement Unit (IMU) was installedonto the car which measures the accelerations in thex and y direction and the angular velocity aroundthe vertical axis. These values, ẍb, ÿb and ψ̇, followa normal distribution as shown in figure 5 where thevalues of ÿb are not included due to their similarityto the values of ẍb. The small number of bins inthe first histogram is due to the limited resolutionof the accelerometer.

-0.09 -0.088 -0.086 -0.084 -0.082 -0.08 -0.078 -0.076 -0.0740

20

40

60

80

100

120

140

160

180

Experimental

Fitted normal distribution

(a) Acceleration along the xaxis, ẍ, in g’s.

-0.2 0 0.2 0.4 0.6 0.8 10

0.5

1

1.5

2

2.5

3

Experimental

Fitted normal distribution

(b) Angular velocity aroundthe vertical axis, ψ̇, in de-grees per second.

Figure 5: Histogram of IMU measurements.

3.5. Laser Scanner

In this thesis, the point clouds are produced bya laser scanner, a device with a rotating LIDAR(light detection and ranging) device that measuresthe distance to the nearest surface. By spinning theLIDAR, the laser scanner is able to collect samplesof multiple points in the environment. Not captur-ing the environment in one instant is a disadvantagewhen it is used in a moving body as is done in thisthesis. Using estimates from a state observer it ispossible to minimize this problem by correcting thepoint cloud to a single point in time. Regardless,every sample collected in one rotation of the LIDARis grouped into a scan that is processed as if theywere taken all at once.

The laser scanner used is the URG-04LX-UG01from Hokuyo [19]. It is a 2D scanner, meaning theLIDAR is only spinning around one axis. It has arange of around 5 m, collects 682 samples per rota-tion and rotates at 10 rotations per second. Since itis a 2D scanner it is used with the LIDAR rotatingaround the vertical axis, meaning that every LIDARsample is characterized by two values: the angle θaround the vertical axis in which the sample wastaken and the depth r of the nearest surface in thatdirection. The angle values of the samples takenin each rotation are constant and all the samplescollected with the same angle value can be groupedinto a ray whose depth value varies with time. Thetransformation of the LIDAR samples from a scaninto a point cloud is simply a transformation frompolar coordinates into cartesian ones.

Two aspects of the laser scanner noise were char-acterized: the variance and the bias. Multiple setsof scans were collected in different places with thelaser scanner standing still. The depth value of eachray follows a normal distribution with a standarddeviation constant across the whole depth range.

The bias is modeled by placing a planar surfaceroughly perpendicular to the laser scanner and atvarious distances. Since the surface is planar, thedepth values should follow equation 22 where rminis the depth of the point closest to the laser scan-

5

ner and β is the angle between the surface and thesurface normal to the closest point’s ray.

r (θ) =rmin

cos (θ − β)(22)

Multiple test runs were performed, with multiplescans each, and the average of each ray is taken.The previously mentioned planar surface is fittedto these ray averages. The difference between theray averages and the fitted surface is then recordedas the ray bias. To estimate how the bias varieswith depth these data points are put into bins andthe values in each bin are fitted to a normal dis-tribution. The standard deviation of each bin asa function of the mean value is plotted in figure 6.It shows that the standard deviation of the bias isproportional to the point’s depth.

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5

Depth (m)

0

0.002

0.004

0.006

0.008

0.01

0.012

0.014

0.016

Sta

nd

ard

de

via

tio

n o

f b

ias (

m)

Bias standard deviation as a function of depth

Bias of bins

Fitted linear model

Figure 6: Bias standard deviation as a function ofdepth r.

3.6. State ObserverThe state observer constructed is a Kalman filterwhose state variables are then used to estimate therelative pose between two consecutive samples, thatis, tmx , t

my and t

mθ . The general equation of the

Kalman filter used is the following:

xk|k−1 =Fxk−1|k−1 +Guk−1+

Lp(yk−1 − Cxk−1|k−1 −Duk−1

) (23)xk|k =xk|k−1 + Lc

(yk − Cxk|k−1 −Duk

)(24)

In the first equation, the matrix Lp corrects theprediction of the model given by the first part ofthe expression using the difference between the realsensor outputs and the predicted outputs. In thesecond equation, the prediction is corrected withthe new measurements from the sensors using thematrix Lc. The output vector contains the planaraccelerations, the angular velocity around the ver-tical axis and the forward velocity. The first threeoutputs come from the IMU while the fourth onecomes from the encoder.

The matrices Lp and Lc are computed by MAT-LAB and, besides the state-space matrices F , G, C,and D, they require the variance of each sensor andeach input for their computation. The variances of

the sensors used were the ones computed before insections 3.2 to 3.5. The variances for the inputswere chosen manually by minimizing the mean er-ror between the state variables estimation from theKalman filter and from the positioning system. Themanual tuning was performed because it is hard tomeasure and quantify the variance of the drivingmotor and the steering servo.

In order to estimate the coordinate transforma-tion between both samples, tm, equations 20 and21 are integrated assuming all three state variableschange linearly with time. Due to the possibilitythat the registration algorithm will converge to avalue away from the real one, a trust-region is con-structed from the state observer estimate of the rel-ative pose. If the value returned by the registrationalgorithm falls outside this trust-region, the resultis discarded and the relative pose from the stateobserver is used instead. The trust-region used hasthe shape of an ellipse for the translation part ofthe transformation and a scalar interval for the ori-entation part. The equations for the boolean valuesto be evaluated are the following:

B◦ =

(trx − tmxCex

)2+

(try − tmyCey

)2

10 20 30 40 50 60 70 80 90 100

0.8

0.9

1

1.1

1.2

1.3

1.4

Ground truth

Kalman filter

(a) Validation of the predic-tion of ẋb.

0 20 40 60 80 100 120

-0.25

-0.2

-0.15

-0.1

-0.05

0

0.05

0.1

0.15

0.2

Ground truth

Kalman filter

(b) Validation of the predic-tion of ẏb.

0 20 40 60 80 100 120

-3

-2.5

-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

Ground truth

Kalman filter

(c) Validation of the predic-tion of ψ̇.

Figure 7: Validation of the state observer.

The first piece of software, written in C++, isinstalled onto the car’s onboard Raspberry Pi. Itfocuses on the low level operations such as collect-ing data from the sensors and sending signals tothe actuators. To enable other computers to inter-act with this piece of software, a Wi-Fi network iscreated by the Raspberry Pi using a freely availablesoftware. Other computers then connect to this Wi-Fi network and send signals and receive data fromthe car’s C++ software.

The Python software written runs on a computerthat is connected to the car’s Raspberry Pi’s Wi-Finetwork and has four modules. One of the mod-ules receives, displays and logs the data sent by theC++ software. The other three modules interactwith each of the three C++ Controllers. The firstcontroller consists in directly controlling the car us-ing the laptop’s arrow keys. The second one is aproportional controller that uses the ψ estimatedby the State Observer module to maintain a ψ ref-erence that can be changed by the laptop’s arrowkeys. The third controller implemented is one thatuses the data from the Laser Scanner module totry to avoid obstacles. It locates the gap in thelaser scan point cloud that is closer to the direc-tion in front of the car and outputs the direction ofthat gap as a ψ reference. This ψ reference is thenpassed through a proportional controller similar tothe one described before.

Three MATLAB functions were written, each im-plements one of the registration algorithms stud-ied in this thesis: NDT, ICP and NICP. A MAT-LAB class was written that implements the stateobserver described in section 3.6. Another MAT-

LAB class was written that implements the firsttwo CLM algorithms described in section 2.3. Ascript was written that uses these pieces of softwareto perform offline CLM using the data logged bythe Python software.

Five S-functions were developed which implementthe functionality of the five C++ modules: Actua-tor, Encoder, IMU, Laser Scanner and PositioningSystem. An S-function is a type of function whichfollow a specific template and is used by a SimulinkS-function block to run code written in other lan-guages. The language used is C++ since it allowsthe reutilization of much of the code C++ code: theS-functions simply call the functions of the C++modules.

The positioning based CLM algorithm describedin section 2.3 is implemented as a Simulink modelthat runs in real-time which uses the previouslymentioned S-function blocks to interact with thecar’s sensors and actuators. A controller that makesthe car follow a trajectory defined by a series ofpoints was also developed.

4. Results

The results of the network-based and the position-ing system based CLM algorithms, described in sec-tion 2.3, are shown in section 4.1 and 4.2 respec-tively.

4.1. Network of Relative Poses

In this section, the network-based CLM algorithmdescribed in section 2.3 is tested. The result of thisalgorithm is shown in figures 8a and 9a. In fig-ures 8b and 9b, the relative poses connections usedfor the network-based CLM, are shown with blacklines connecting the nodes. A video showing theCLM algorithm constructing the map in figure 9ais available at: https://www.youtube.com/watch?v=D-ifL6qphI8.

(a) Map

0 5 10 15 20 25 30 35 40

(m)

25

30

35

40

45

50

55

60

(m)

(b) Connections

Figure 8: Network-based CLM inside an officebuilding.

The maps show the building plants in the back-ground, the trajectory of the car as a blue line andthe obstacles detected by the laser scanner in red.

7

https://www.youtube.com/watch?v=D-ifL6qphI8https://www.youtube.com/watch?v=D-ifL6qphI8

(a) Map0 5 10 15 20 25 30 35

-5

0

5

10

15

20

(b) Connections

Figure 9: Network-based CLM inside a residentialbuilding’s garage.

The environment in figure 8 is an office building inan university and its plant was obtained from theuniversity’s website. The environment in figure 9 isa garage in the basement of a residential buildingand its plant was constructed from measurementsperformed using a measuring tape.

The network-based CLM algorithm produces aconsistent map of the garage while producing largeerrors in the office building map. This is likelydue to the low accuracy of the registration of pointclouds along a corridor since all the normals pointin one direction. The connections in figure 8b showthat most of the closing loop connections in this net-work happen between nodes whose point clouds areof a corridor. By using the covariance matrix in 2.3,it is possible to ignore the relative pose along thelow accuracy direction of these point clouds whichwould improve the network solution when there arelong corridors in the environment. To do that, thecharacterization of the accuracy of the registrationalgorithm, which is shown in [20] to depend on thesurface normals, would have to performed.

4.2. Positioning System

The CLM algorithm based on positioning data, de-scribed in section 2.3, was used to estimate the tra-jectory of the car in real-time. The positioning sys-tem and state observer used are the ones describedin section 3.2 and 3.6 respectively. Because the po-sitioning system often fails to detect the car, thereis no complete ground truth trajectory. Regardless,the estimated trajectory of the car is compared withthe partial ground truth data available. Since thestate observer from section 3.6 works well, the CLMalgorithm takes time to drift away from the realtrajectory. Because of this, a manually controlledswitch used to further reduce the positioning dataavailable for the CLM algorithm was added to thereal-time Simulink model. Right after initializingthe Simulink model, with the car still standing still,a few data points from the positioning system aresent to the CLM algorithm so that its initial poseis the same as the one returned by the positioningsystem.

An experiment was performed where the car wasdriven using the controller mentioned in section 3.7with a discretized circular trajectory. Figures 10a,10b and 10c show the value of the absolute poseparameters over time and figure 10d shows the re-sulting trajectory.

0 20 40 60 80 100 120 140 160 1801

1.5

2

2.5

3

3.5

4

4.5

5

5.5

Ground truth

Estimate

Samples used

(a) Value of xe over time.

0 20 40 60 80 100 120 140 160 180-1.5

-1

-0.5

0

0.5

1

1.5

2

Ground truth

Estimate

Samples used

(b) Value of ye over time.

0 20 40 60 80 100 120 140 160 180-4

-3

-2

-1

0

1

2

3

4

Ground truth

Estimate

Samples used

(c) Value of ψ over time.

1 1.5 2 2.5 3 3.5 4 4.5 5 5.5-1.5

-1

-0.5

0

0.5

1

1.5

2

Ground truth

Position estimate

Samples used

(d) Final trajectory.

Figure 10: Results from the first experiment per-formed.

In these figures, both the trajectory estimatedby the CLM algorithm and the ground truth dataavailable can be seen. The positioning system sam-ples used by the CLM algorithm are representedby the black dots. It can be seen that, as expected,when there is no positioning data available, the poseestimates drift away from the real values. As soonas data from the positioning system is available itquickly corrects its estimate of the pose. The con-troller also performs well, driving the car in a circleaccording to the CLM algorithm’s estimate of thecar’s pose.

Another experiment was performed with thesame controller driving the car along an ellipticaltrajectory. The same behavior is observed with theCLM algorithm correctly estimating the pose of thecar when there is data available from the position-ing system and drifting away when there is none.Both the center of the ellipse and the orientationof its axes drift over time. Near the end of this ex-periment, the car was manually moved to a differentplace, while having no access to positioning data, tosee how the CLM algorithm would react. The al-gorithm got lost and, because of this, the controllercrashed the car into the surrounding environmentand the experiment was ended. As before, figures11a, 11b and 11c show the values of the absolutepose parameters over time and figure 11d shows thefinal trajectory.

8

0 50 100 150 200 250 300-3

-2

-1

0

1

2

3

4

5

6

7

Ground truth

Estimate

Samples used

(a) Value of xe over time.

0 50 100 150 200 250 300-3

-2

-1

0

1

2

3

Ground truth

Estimate

Samples used

(b) Value of ye over time.

0 50 100 150 200 250 300-4

-3

-2

-1

0

1

2

3

4

Ground truth

Estimate

Samples used

(c) Value of ψ over time.

-3 -2 -1 0 1 2 3 4 5 6 7-3

-2

-1

0

1

2

3

Ground truth

Position estimate

Samples used

(d) Final trajectory.

Figure 11: Results from the second experiment per-formed. The car’s pose is changed around 220 s.

Another experiment was conducted and a video,available at https://www.youtube.com/watch?v=87C1F8h0Q3U, was made from it. In conclusion, thealgorithm provides good results, producing an es-timate of the trajectory without using positioningdata, while being able to correct this estimate whenthere is data available. The reliability of the esti-mate of the trajectory when there is no positioningdata available is due to the Kalman filter devel-oped in section 3.6 which relies, in large part, onthe quality of the data from the sensors, especiallythe gyroscope and encoder.

5. Conclusions

The goal of this thesis was to develop a frameworkto support autonomous navigation and cooperationamong robots with ground truth validation, pavingthe way for the development of cooperative or col-laborative strategies among robots for cooperativemapping, formations and collaboration. For onerobot, three different Concurrent Localization andMapping (CLM) algorithms were implemented.

Three registration algorithms were studied andimplemented: NDT, ICP, and NICP. A few modifi-cations to the NICP registration algorithm and thenetwork-based CLM algorithms were performed.The three registration algorithms studied were com-pared and NICP was chosen as the most appropri-ate since it makes use of the surface normals which,not only improves accuracy but can be used to es-timate the accuracy of the registration. Besides,although it is considerably slower than ICP, it ismuch faster than NDT and, more importantly, fastenough for the hardware used.

Three CLM algorithms were also studied and im-

plemented: sequential, network-based and position-ing system based. The first two algorithms makeuse of the registration algorithms to estimate thetrajectory of the car while the last one uses a posi-tioning system to correct the dead-reckoning error.Some simulations of the first two algorithms wereperformed in an ideal environment, that is, withoutlong corridors, and the map obtained was similar tothe one in the original virtual environment.

The architecture was assembled and the car wasequipped with an encoder that measures the vehi-cle’s speed, an Inertial Measurement Unit (IMU)that measures the linear accelerations and angularvelocities and a laser scanner that produces pointclouds of the environment. A positioning systemwas also set up that was used both for deriving amodel of the car and for use in one of the CLM al-gorithms. The sensors used were also characterizedand a state observer was built which predicts thetrajectory of the car with great accuracy.

Multiple pieces of software were written includinglow-level software that interacts with the car’s hard-ware and MATLAB functions that perform CLM.Five Simulink blocks were also designed that canbe used to interface with the car’s hardware in aSimulink model.

Each of the CLM algorithms implemented wastested either online or using data collected fromreal-world experiments. The sequential CLM algo-rithm produces a map of the environment but withthe expected dead-reckoning error. The network-based CLM algorithm produces a consistent map,free of dead-reckoning error, but it is sensitive toregistration errors which happen often when theenvironment has long corridors. The positioningsystem CLM algorithm works well and is able toestimate the trajectory of the car with the dead-reckoning error being corrected when data is re-ceived from the positioning system.

In the future, work towards improving thenetwork-based CLM algorithm, discussed in section2.3, can be performed by developing an algorithmthat estimates the covariance matrix of the registra-tion algorithm such as the one in [20]. This can alsobe done by using the registration algorithm on pointclouds produced in a virtual environment and char-acterizing the error. The error estimation shouldhelp to minimize the registration errors that prop-agate into the network solution.

A SLAM solution or a controller that uses thefleet of cars, all at the same time, can also be de-veloped using the Simulink software suite.

Acknowledgements

This thesis was researched and written with thesupport and orientation provided by my supervi-sors, Prof. Carlos Baptista Cardeira and Prof.

9

https://www.youtube.com/watch?v=87C1F8h0Q3Uhttps://www.youtube.com/watch?v=87C1F8h0Q3U

Paulo Jorge Coelho Ramalho Oliveira, and the helpof the laboratory staff, Lúıs Jorge Bronze Raposeiroand Christo Camilo.

I would also like to thank my family and friendsfor all the support provided throughout my years ofstudy and the process of writing this thesis.

References[1] A. Guéziec, P. Kazanzides, B. Williamson, and

R. H. Taylor. Anatomy-based registration ofct-scan and intraoperative x-ray images forguiding a surgical robot. IEEE Transactionson Medical Imaging, 17(5):715–728, 1998.

[2] C. Dorai, G. Wang, A. K. Jain, and C. Mer-cer. Registration and integration of multipleobject views for 3d model construction. IEEETransactions on pattern analysis and machineintelligence, 20(1):83–89, 1998.

[3] P. Lamon, S. Kolski, and R. Siegwart. Thesmartter-a vehicle for fully autonomous navi-gation and mapping in outdoor environments.In Proceedings of CLAWAR, 2006.

[4] F. Tâche, F. Pomerleau, G. Caprari, R. Sieg-wart, M. Bosse, and R. Moser. Three-dimensional localization for the magnebike in-spection robot. Journal of Field Robotics, 28(2):180–203, 2011.

[5] S. Thrun, W. Burgard, and D. Fox. A proba-bilistic approach to concurrent mapping andlocalization for mobile robots. AutonomousRobots, 5(3-4):253–271, 1998.

[6] M. Montemerlo, S. Thrun, D. Koller, B. Weg-breit, et al. Fastslam: A factored solutionto the simultaneous localization and mappingproblem. Aaai/iaai, 593598, 2002.

[7] D. F. Wolf and G. S. Sukhatme. Mobile robotsimultaneous localization and mapping in dy-namic environments. Autonomous Robots, 19(1):53–65, 2005.

[8] H. Durrant-Whyte and T. Bailey. Simultane-ous localization and mapping: Part i. IEEErobotics & automation magazine, 13(2):99–110,2006.

[9] T. Bailey and H. Durrant-Whyte. Simultane-ous localization and mapping (slam): Part ii.IEEE robotics & automation magazine, 13(3):108–117, 2006.

[10] J. Serafin and G. Grisetti. Nicp: Dense nor-mal based point cloud registration. In 2015IEEE/RSJ International Conference on Intel-ligent Robots and Systems (IROS), pages 742–749. IEEE, 2015.

[11] F. Lu and E. Milios. Globally consistent rangescan alignment for environment mapping. Au-tonomous robots, 4(4):333–349, 1997.

[12] S. Holzer, R. B. Rusu, M. Dixon, S. Gedikli,and N. Navab. Adaptive neighborhood selec-tion for real-time surface normal estimationfrom organized point cloud data using inte-gral images. In 2012 IEEE/RSJ InternationalConference on Intelligent Robots and Systems,pages 2684–2689. IEEE, 2012.

[13] F. Porikli and O. Tuzel. Fast constructionof covariance matrices for arbitrary size im-age windows. In 2006 International Conferenceon Image Processing, pages 1581–1584. IEEE,2006.

[14] F. Pomerleau, F. Colas, and R. Siegwart. Areview of point cloud registration algorithmsfor mobile robotics. 2015.

[15] P. Bergström and O. Edlund. Robust registra-tion of point sets using iteratively reweightedleast squares. Computational Optimization andApplications, 58(3):543–561, 2014.

[16] P. Biber and W. Straßer. The normal dis-tributions transform: A new approach tolaser scan matching. In Proceedings 2003IEEE/RSJ International Conference on Intel-ligent Robots and Systems (IROS 2003)(Cat.No. 03CH37453), volume 3, pages 2743–2748.IEEE, 2003.

[17] D. Borrmann, J. Elseberg, K. Lingemann,A. Nüchter, and J. Hertzberg. Globallyconsistent 3d mapping with scan matching.Robotics and Autonomous Systems, 56(2):130–142, 2008.

[18] Latrax R© desert prerunner: 1/18-scale 4wdelectric truck — latrax. https://latrax.com/products/prerunner. Accessed: 2019-12-08.

[19] Hokuyo-usa :: Urg-04lx-ug01. https://www.hokuyo-usa.com/products/

scanning-laser-rangefinders/

urg-04lx-ug01. Accessed: 2019-12-08.

[20] B.-U. Lee, C.-M. Kim, and R.-H. Park. Anorientation reliability matrix for the iterativeclosest point algorithm. IEEE Transactions onPattern Analysis and Machine Intelligence, 22(10):1205–1208, 2000.

10

https://latrax.com/products/prerunnerhttps://latrax.com/products/prerunnerhttps://www.hokuyo-usa.com/products/scanning-laser-rangefinders/urg-04lx-ug01https://www.hokuyo-usa.com/products/scanning-laser-rangefinders/urg-04lx-ug01https://www.hokuyo-usa.com/products/scanning-laser-rangefinders/urg-04lx-ug01https://www.hokuyo-usa.com/products/scanning-laser-rangefinders/urg-04lx-ug01

1 Introduction2 Background2.1 Registration Introduction2.2 Normal Iterative Closest Point2.3 Simultaneous Localization And Mapping

3 Implementation3.1 Actuators3.2 Positioning System3.3 Encoder3.4 Inertial Measurement Unit3.5 Laser Scanner3.6 State Observer3.7 Software Implementation

4 Results4.1 Network of Relative Poses4.2 Positioning System

5 Conclusions