See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/311757314

Expressive navigation and Local Path-Planning of independent steering

autonomous systems

Conference Paper · October 2016

DOI: 10.1109/IROS.2016.7759697

CITATIONS READS
11 616

2 authors:

Horatiu George Todoran Markus Bader

TU Wien TU Wien
9 PUBLICATIONS 35 CITATIONS 19 PUBLICATIONS 112 CITATIONS

SEE PROFILE SEE PROFILE

All content following this page was uploaded by Horatiu George Todoran on 28 June 2018.

The user has requested enhancement of the downloaded file.

 Expressive Navigation and Local Path-Planning of
Independent Steering Autonomous Systems
George Todoran1 , Markus Bader1

Abstract— This paper presents a novel Local-Path Planning

approach for an independent four-wheel steering (I4WS) mobile
base. The approach smoothly drives and rotates the platform
while still following a given path and avoiding obstacles. I4WS
vehicles are omnidirectional vehicles, similar to shopping carts,
if one neglects the time needed to steer the wheels. However,
due to the danger of mechanical parts breaking while actuating attention point
the wheels, they are commonly controlled in a stop-and-go
fashion, where the vehicle stops to turn its wheels perpendicular
to a desired instantaneous center of curvature (ICC) before
starting to move again. The approach overcomes this limita-
tion and ensures an ICC-based kinematic constraint during
continuous motion using a flat-input controller running at
100 Hz. Therefore, the trajectory of the ICC is both predictable
and suitable for model predictive control (MPC). The MPC
implemented generates optimized collision-free trajectories of
up to several meters ahead at 10 Hz, given a set of points
along a path and laser contour readings. Furthermore, the
planner realized here is able to deal with additional constraints
such as the vehicle’s view direction to focus on attention points Fig. 1. Left: the robot Blue; Right: navigation while focusing on an
while driving. Experimental results highlight the capabilities attention point
of the approach on a simulated robot, using GazeboSim, and
demonstrate its applicability for the field of service robotics. collocated control and synchronization of the wheels’ motors
being required in order to make use of their increased ma-
I. INTRODUCTION
neuverability. Furthermore, nonlinear kinematic and dynamic
Many fields of the application of mobile robotics bene- constraints of such platforms are inherently present.
fit greatly from holonomic motion capabilities. True holo- The work presented here focuses on a short-term naviga-
nomicity represents the capability of the system to undergo tion solution for such platforms, aiming to push the limits
an arbitrary trajectory (position and orientation) from any of achievable platform motions and behaviours. Especially
starting configuration. This property allows wheeled mobile in the field of assistive robotics, expressive, natural motions
agents to navigate effectively in cluttered environments such ease the process of human-robot interaction and increase
as work-spaces shared with humans or work-spaces restricted agent effectiveness. Advancements in research regarding
with respect to the agent size (e.g. forklifts operating in robot attention provide further motivation for a navigation
indoor warehouses). As true holonomic platforms using omni approach that is capable of different (safe) behaviours. For
or mecanum wheels tend to be avoided for operation in example, Fig. 1 – right illustrates an I4WS platform navigat-
unstructured environments (due to their generally required ing under an interest point focusing behaviour, representing
wheel maintenance), the typical design used for holonomic one of the expressive capabilities of our approach.
motions makes use of independent steering systems. Targeting wide contexts of operation and tasks of such a
Fig. 1 – left illustrates Blue, an assistive robot possessing mobile platform, the key points of Local Path-Planning are:
an independent four-wheel steering (I4WS) mobile base and
represents the motivation for this work. Its mobile platform is • unified motion parametrization
classified as an independent steering system, possessing eight • safety by design through constraint satisfaction (dynam-
actuators (two for each wheel) and the additional constraint ics, route deviation, collision avoidance)
of limited wheel orientation (approximately 270◦ ). • encapsulation from higher-level robotics tasks (such as
Despite their physical robustness and maintenance bene- human-robot interaction, grasping, etc.)
fits, independent steering systems impose other difficulties, • robust trajectory-altering set of parameters (such as min-
imum distances-to-obstacle, chassis orientation, maxi-
*The research leading to these results has received funding from mum distance to path, time optimal, energy optimal,
the Austrian Research Promotion Agency (FFG) according to grant
agreement No. 852708 (MPCv1), No. 855409 (AutonomousFleet) and maximum velocity, etc.)
No. 854865 (TransportBuddy). Taking into account those considerations, the navigation
1 George Todoran and Markus Bader are with the Institute of Computer
Aided Automation, Vienna University of Technology, Vienna 1040, Austria module proposed here addresses the low-level actuator con-
[george.todoran],[markus.bader]@tuwien.ac.at trol as well as local trajectory planning and following, using
flatness-based control as well as sampling and optimization-
based trajectory generation. Route Map Localization IMU +
Depth actuator Plant
This paper is organized as follows: Section II presents sns. sns.
the state of the art on the topics of independent steering
platforms and their control, Local Path-Planning and tra-
jectory sampling and optimization in navigation contexts.
Section III introduces the proposed approach and provides Local Path-Planner Flat Controller
an overview of the underlying modules and their interaction.
The main points of the approach are further investigated in Fig. 2. Overview Diagram
the Sections IV – V. Section VI presents simulated results of
sampling [5] can be performed for increased efficiency. How-
the navigation module applied to the robot Blue. Conclusions
ever, this requires pre-computation of trajectory primitives,
are drawn in the Section VII.
dependent upon platform parameters and defined costs. An
alternative to sampling towards an optimum is optimiza-
II. STATE OF THE ART
tion. A general description of trajectory optimization in the
Considering the desired holonomic characteristics, designs context of autonomous navigation is done by T. Howard
that possess quasi-holonomic motion capabilities are typi- in his PhD. thesis [6]. Being accurate and able to cope
cally classified as independent steering platforms. In such a well with nonlinear constraints, optimization is also used
design, each of the actuated wheels possesses two manners in autonomous vehicle driving [7]. The work proposed here
of actuation freedom: wheel rotation and wheel orientation is largely related to optimization. Although previous cycle
relative to the platform base. trajectory is usually a good initialization for optimization,
However, such designs are intrinsically over-actuated, as control space DWA is performed only at irregular cycles.
they do not possess the hard mechanical constraints that III. APPROACH
guarantee the geometrical validity of an instantaneous center
Data-flow, the required modules and their interdependence
of curvature (ICC, depicted in Fig. 3), relying rather on
for the proposed Local-Path Planning approach is depicted
electronic control to achieve geometrically proper configu-
in Fig. 2. The Local Path-Planner, based on route, map,
rations. In general, the holonomic definition of such designs
localization and sensor information computes, in every cycle,
is loosened to quasi-holonomicity due to the singularities
an optimal sequence of parametrized control commands
that are present when the ICC is in the vicinity of a wheel
with respect to user-defined costs. The refresh rate of the
[1], as well as due to additional design constraints (such as
planner is designed to be around 10 Hz. Based on the
use of limited rotation motors for wheel orientation). Also,
(time-stamped) sequence of control commands, the low-level
the geometrical topology of the wheels causes nonlinearities
controller computes the necessary actuator inputs of the plant
in actuator trajectories as well as in trajectory dynamics
at a frequency of approximately 100 Hz. The plant in this
constraints.
case is represented by the I4WS platform and is equipped
Low-level control of such platforms can be done using with actuator sensors (e.g. velocity measurements for wheel
various approaches. Use of discrete motion models that em- revolutions and angular measurements for wheels rotations)
ulate other mobile platforms can be used, the control problem and an inertial measurement unit (IMU), which are used in
thereby becoming less general [2] and allowing algorithmic the feedback loop by the flat controller. Additionally, the flat
reasoning of the motion model in use. At the other end of controller requires localization information. The localization
the spectrum, control can be obtained through optimization module is a topic of research in itself and is beyond the
of individual actuator states [3]. However, the large input- scope of this paper. It is further considered a black box that
space as well as the increased temporal frequency of the delivers sufficiently accurate information. The Local Path-
optimization points makes it infeasible regarding the higher- Planner requires localization information, depth information
level path-planning problem of obstacle avoidance, route- for obstacle avoidance (with the possible addition of a-priori
following and long-range (to the scale of several m for indoor maps) as well as a route. The focus of this paper lies on
applications). Another approach exploits the differentially the approach, design and implementation of the Local-Path
flat character of the system, as presented in [4]. The approach Planner and the flat controller modules, being sensitive to
proposed here makes use as well of a flat controller; however, their interdependency, requirements and inter-connectivity
it is parametrized in the local frame of the platform and with the other modules that constitute autonomous naviga-
used only for low-level control, the task of navigation being tion.
designated to an optimization-based Local Path-Planner.
Local Path-Planning is typically performed by sampling IV. LOW-LEVEL CONTROL
the control input space (also known as the Dynamic Window Starting with the geometrical topology and constraints
Approach – DWA) and by applying the best trajectory with of an I4WS system, this section presents considerations
respect to cost. However, this method becomes inaccurate when developing the low-level control as well as possible
and computationally expensive when the evaluated temporal parametrizations and motion models used to control and
horizon becomes large. An alternative method of state-space simulate the agent motion.
A. Geometrical Considerations
Fig. 3 illustrates the local frame of an I4WS with its ICC
at an arbitrary location. It is worth noting that for control ~vc icc
y rc
and wheel synchronization, the ICC should be considered,
in general, to be a local-frame quantity due to present ~viw αc
geometrical constraints. In this way, its location is tractable x
and parametrization is substantially easier as opposed to
when considered in a global frame. αwi
Assuming that no drifting motion is desired, wheel ori-
entations are constrained to perpendicular to the ICC and ωwi
their revolution has to be consistent with the angular velocity Fig. 3. Geometric model
caused by the ICC. Thus, the instantaneous kinematic state
of an IWS can be described by three abstract parameters αwi (t) and ωwi (t) result from geometrical considerations
p(t), for example: vc – velocity of the body center and the and are presented in Eq. 1 and 2 (xwi and yiw represent the
position of the ICC represented in polar coordinates (rc and wheel position coordinates in the local frame, rw the wheels’
αc ). However, such a parametrization leads to singularity in radius).
the case of parallel wheel placement (rc → ∞) and therefore Assuming independence in the wheels’ orientation actua-
rc is substituted with ρc = 1/rc . In order to be able to perform tion and modelling the actuators with viscous friction (pa-
changes to the ICC from one side of the base to the other, rameters µα and µω ), the wheel orientation torque becomes
ρc is allowed to be negative. (Iα – inertial mass of a wheel):
The orientation αwi (t) and rotation ωwi (t) of wheel i result
from geometrical considerations and are presented in Eq. 1 ταi (t) = Iα α̈wi (t) + µα α̇wi (t) (4)
and 2, with xwi and yiw – the wheel position coordinates in
the local frame, rw – the wheel radius. In the following, for Regarding the wheel rotation actuators, their intercon-
notation simplification, the time dependency of vc , ρc and nection cannot be neglected. Imposing force and torque
αc is assumed but not denoted. equilibrium in the body (Eq. 5 – 8), the problem becomes
solving the undetermined system (eq. 9) for computing the
sin(αc )/ρc − yiw
 
required torque of the wheels (mb , Ib – platform mass and
αwi (t) = arctan (1)
cos(αc )/ρc − xwi inertial mass respectively).

−vc
q T
τω0 (t)= τω0 0 (t) τω1 0 (t) τω2 0 (t) τω3 0 (t)

ωwi (t) = 1 + (xwi 2 + yiw 2 )ρc2 − 2ρc (xwi cos(αc ) + yiw sin(αc )) (5)
rw
cos(αwi (t))
 
(2)
Note that these equations hold for other kinematic ai (t)= sin(αwi (t))  (6)
parametrizations as well as for other mobile platforms (such xw sin(αw (t)) − yiw cos(αwi (t))
i i
 
as differential drives or Ackermann drives) through trivial A(t)= a0 (t) a1 (t) a2 (t) a3 (t) (7)
variable substitution. 
m v cos(αc )

d  b c
B. Feed-forward & State-feedback Control b(t)=−rw mb vc sin(αc )  (8)
dt
Ib ωc
Even though it suffices to model the actuators as linear,
the topology of a I4WS presents unavoidable nonlinearities
A(t)τω0 (t) = b(t) (9)
as well as overactuation. Thus, the control scheme employed
uses feed-forward control by making use of the differentially With similar modelling of the actuator, the rotational
flat parameter set p(t) and applies control allocation to cope actuator torque then becomes:
with overactuation. State-feedback is then performed on the
actuators in the form of a linear quadratic regulator (LQG, τωi (t) = τωi 0 (t) + µω ωwi (t) (10)
[8]) with a Kalman-filter based state observer (through the
actuator sensors and an inertial measurement unit – IMU). C. Parametrizations and Motion Models
Keeping in mind the quasi-linearity present in many ac- Due to the nonlinear nature of the geometrical configura-
tuator designs with the motor-shaft torque, the wheel state tion of an IWS, several parametrizations of its kinematic state
required for the controller is thus defined as: are possible and advisable depending on further application.
For example, the parametrization presented in Section IV
T has the advantage of remaining well defined when the
xiw (t) = αwi (t) α̇wi (t) ταi (t) ωwi (t) τωi (t)

(3)
base velocity tends toward 0, simplifying initial conditions
with ταi (t)
and τωi (t)
representing the feed-forward actu- and parametrization switches in the higher-level modules.
ator torques that control the orientation and the respective However, the trade-off lies on the fact that a singularity
rotation of each wheel. is present when performing pure rotation. An alternative
 too expensive to evaluate, together with their reduced accu-
racy. This motivates the main approach towards Local Path-
Planning to be based on optimization i.e. MPC. However,
as described in the following, trajectory sampling still has
a role in the planner in order to provide an initial estimate
of the trajectory when optimization reaches a local-minima.
As many of the approach choices are made while being
sensitive to the optimization requirements and bottlenecks,
the optimization problem will be discussed first, followed
ωb ωc by key elements of the Local Path-Planner.
wheel geometry ICC induced ICC A. The optimization problem
Fig. 4. Global Frame considerations; the vehicles body can rotate as well. Theoretically, finding an optimal local trajectory with
respect to a cost and constraints represents a dynamical
would be to use p = [vc , ωb , αc ], (ωb = −vc /rc ) and avoid
optimization problem [9], as presented in canonical form:
the previous singularity. However, this parametrization is not Z t1
defined at vc → 0. This can be exploited by the fact that min J(u(·)) = ϕ(x(t1 ),t1 ) + l(x(t), u(t),t)dt
when the platform stands still, the wheels do not have to u(·) t0
be synchronized in order to achieve a different initial state subject to: ẋ = f(x(t), u(t),t), x(t0 ) = x0
and thus can rotate at full speed. However, this generally ψ(x(t1 ),t1 ) = 0
complicates the control scheme.
h(x(t), u(t),t)  0, ∀t ∈ [t0 ,t1 ] (12)
Other parametrizations can be used by exploiting global
frame characteristics of the kinematics. More intuitively, with the cost functional J(u(·)), the terminal cost
consider the two situations presented in Fig. 4. On the left, ϕ(x(t1 ),t1 ), the Lagrange density function l(x(t), u(t),t) and
the global frame motion of the platform with a fixed wheel the state transition function f(x(t), u(t),t). In practice, the
geometry ICC is shown. The same trajectory can be achieved problem is discretized, resulting in static optimization over
by modifying αc while the wheels stay parallel. This creates finite space:
an induced ICC and the total trajectory motion curvature can min f (x, u) (13)
u∈U
be interpreted as a superposition of the two: ωtra j = ωb + ωc .
subject to g(x, u)=0 (14)
However, the orientation of the platform is influenced only
by ωb . This motivates the parametrization p = [vc , ωtra j , ωb ] h(x, u)0 (15)
due to the parameter decoupling between trajectory and with the cost function f , equality constraints g and in-
orientation of the platform, a fact that is further exploited in equality constraints h.
the Local Path-Planner in order to achieve trajectories with This introduces another dimension to p, its arc
the platform orientation following a focal point. parametrization (Eq. 17). A popular choice for this parameter
The differential equation that models the kinematics of is time and is usually assumed constant (e.g. time between
the platform is presented in Eq. 11, where all variables control-points). As the optimization problem requires at
can be time-dependent. According to the desired accuracy, least the numerical evaluation of gradients, our approach
discretization is achieved through various methods (e.g. Euler has opted towards reduction of the number of parametrized
or Runge-Kutta). control points by optimizing the trajectory arc-length pa-
˙ 
rameter. Even though most of the approaches take time as

xb vc cos(θb + αc )
 yb   vc sin(θb + αc )  a parametrization of choice [9], a considerable portion of
the optimization problem for navigation relies on spatial
   
 θb  =  ωb  (11)
information. By choosing sparse arc-length parametrization
   
 rc   −vc /ωb 
αc ωc (i.e. distance along trajectory), optimization convergence
speed is significantly (∼ 100%) increased.
Considering that the optimization problem requires numer- Considering the piecewise continuous differential charac-
ical evaluation of gradients, the model numerical stability is ter of the system required in the optimization problem of the
of importance. It is worthy to note that discretized motion Local Path-Planner, the kinematics of the parametrized state
models based on circular considerations are not advised, as suffice a constant parameter velocity model (Eq. 16). Even
they present numerical instability when ω → 0. though higher order splines represent an expressive method
Having had an analysis of the I4WS platform and its low- of parametrizing the control points, due to their difficulty
level control, the next Section will discuss the considerations of arc-length computation, trapezoidal interpolation is used
of trajectory planning using MPC. (Fig. 5 – left).
V. LOCAL PATH-PLANNING pi (t) = pi0 + ṗi t, t − bound by piarc (16)
Especially due to the increase of the control parameter p  T
space to 3, sole DWA-based sampling approaches become u = p0 p1 p2 parc , parc ∈ {∆t, ∆s} (17)
 ωb (t)
equal distance intervals
– initial state
– end state vb (t)
– control point knot
– temporal evaluation point t
0
– arc length evaluation point equal time intervals

Fig. 5. Trajectory parametrization

B. Trajectory simulation and evaluation • actuator dynamics (temporal, nonlinear for IWS)
As most of the motion models used in navigation are • base dynamics (temporal, varies with parametrization)
nonlinear, evaluation of trajectories implies numerical simu- • maximum prediction time (affine)

lation. Because of this, trajectory simulation (which is used • arc-parametrization non-negativity (affine)

in most of the path-planning approaches [10]) represents a Having already introduced motion-models and costs, algo-
considerable bottle-neck. Thus, a critical point to developing rithm 1 presents the pseudo-code of the trajectory simulator
an efficient path planner revolves in efficiently computing with on-line evaluation. Starting from the initial state x0 ,
and evaluating trajectories as well as balancing desired run- the simulation computes the time interval until the next
time versus simulation accuracy. evaluation point and advances according to the used motion
Due to the model kinematics nature, the typical constraints model (implemented in sim step(x, u[], dt) ). Depending on
are nonlinear and in general finding a piecewise closed-form the type of the evaluation point, the cost/constraint evaluator
approximation of them is unfeasible. Because of this, a re- computes and stores the afferent data.
laxed assumption of sufficiently small evaluation intervals is Especially in the case of non-linear constraints, the number
typically used. Fig. 5 – right illustrates a simulated trajectory of evaluations per constraint type has to be sufficiently
and its relevant evaluation points. As costs and constraints large (otherwise one might be unable to evaluate a high
present in the problem have dynamic as well as spatial nature constraint violation). An alternative for reduced optimization
(e.g. dynamic motor constraints or distance to obstacles), two constraint space is to perform soft-min of the constraint costs
evaluation lattices are used, one at constant time intervals over a larger interval (e.g. between two parametric knots)
and one at constant arc-length. This way, together with and provide only one entry per interval in the optimization
an analysis of the constraint nature, constraint satisfaction formulation. Note that the piece-wise character of the used
becomes tractable. Good definitions of cost-functions influ- parametric functions can furthermore motivate placing cost
ence heavily the characteristics of the generated trajectories evaluation and constraints at their knots (one might identify
and can lead to unexpected local minima if not chosen piecewise convex formulation of costs/constraints).
carefully. For example, penalizing distance to the end-point When reaching the end of simulation, the partial precom-
might seem intuitive, however it will get stuck at sharp puted costs/constraints are finalized. Note that such an ap-
corners of the route boundary. A better approach is to weight proach is not only parallelizable with respect to the simulated
the distance along path (or traveled distance), especially trajectories but also allows immediate interruption in case
for configurations with relatively small route margins. To of constraint violation, action that is used for speedup and
summarize, the main costs used in this approach due to the efficient pruning of trajectories sampling, as future described
varied characteristics and non-intrusion (they do not typically in Subsection V-C.
form local-minima between themselves) are:
• distance to route (integral/power cost)
Algorithm 1 simulate tra jectory(x0 , u[])
• traveled distance (integral/power cost) 1: t ← 0, x ← x0 , evaluator.init()
• base orientation (integral/power cost) 2: while t < tend do
• trajectory time or average velocity (end cost) 3: (dt, eval type) ← f ind next step(u[],t)
• distance to desired end-pose (end cost, small weight) 4: x ← sim step(x, u[], dt), t ← t + dt
Most of the constraints in this approach are inequality 5: switch (eval type)
constraints (the internal kinematic equality constraints are 6: case dt gitter: evaluator.evaluate@partial time(x,t)
not needed due to the lower-level parametrization). The only 7: case ds gitter: evaluator.evaluate@partial dist(x,t)
equality constraint enforces zero base velocity at the end of 8: case ui end: evaluator.evaluate last u interval(x,t)
the optimized trajectory. This way, the platform is guaranteed 9: case sim end: evaluator. f inish evaluation(x,t)
to not collide with obstacles even when moving at high 10: end switch
speeds. As previously mentioned, the inequality constraints 11: end while
are of different natures:
• maximum distance to route (spatial, nonlinear) Room for computational cost improvement is present also
• minimum distance to obstacles (spatial, nonlinear) in the computation of objective function and constraints
 local map initialization initial pose of
gradients, by beginning the simulation and evaluation only the Planner
from the modified region.
t
C. Trajectory sampling
Considering that optimization typically requires an initial depth sensor localization cycle end
estimate, the planner design includes sampling as a pre- data predicted cycle end
liminary step to optimization. This is performed to cope
with the prowess of optimization-based approaches to local- Fig. 6. Pose prediction during one cycle of the planner
minima. Even though during most of the cycles, the previous
the run-time of the optimizer. As piece-wise continuity and
trajectory is sufficient as a starting solution, sampling can
differentiability is required in nonlinear optimization, it is
also provide a layer of redundancy by sampling at every cycle
also advised to perform interpolation on the maps data (e.g.
until a safe trajectory to a platform stop is found. However,
bi-linear).
total space sampling is infeasible especially for I4WS as the
By imposing the route maximum deviation constraint, a
parameter space is cubic.
big part of the obstacle data becomes redundant and is
In a similar fashion with the traditional DWA approach,
excluded from all the other computations (no distance fields
the parameter space is sampled. However, non-expressive
are created for obstacles far away from the route).
parameter combinations are to be avoided as most of the
computation time is reserved for optimization. Another ben- E. Temporal synchronization
efit of the parametrization p = [vc , ωtra j , ωb ] lies on the Especially in asynchronous communication systems such
fact that if not taking into account constraints, only two as Robotics Operating System (ROS), proper data syn-
parameters are influencing the base trajectory, effectively chronisation is vital and must be accounted for. Fig. 6
reducing the sample space for emergency situations. presents the general temporal sequence of data acquisition
D. Preprocessing of external data and processing. In principle, given the previous sequence
of control commands sent by the planner to the low-level
As previously presented in Section III, the Local Path- controller, the pose of the agent is temporally synchronized
Planner requires external data in the form of a route, envi- with various data. Initially, the time delay between the depth
ronment information from a depth-sensor (and additionally sensor measurement and the localization measurement is
static information from a map) as well as the localization compensated by simulating the platform state to the time-
information of the agent. The methods of obtaining a global stamp of the sensor data. Notice that this can be also done
route as well as the localization information are a field of in reversed time.
research themselves and are beyond the purpose of this paper. Regarding the communication with the low-level con-
Regarding localization, the proposed Path-Planner assumes troller, an important point represents the usage of
it is sufficiently accurate but not necessarily temporally parametrized control commands arrays. This way, the planner
synchronized with the rest of the data (except being time- sends to the low-level controller its computed controls for
stamped). the next n - seconds, enabling non-deterministic cycle times
Most on-line global route planners return the shortest to be feasible as well as potentially increased safety with
euclidean path to the goal. Keeping in mind the dynamics respect to planner run-time malfunction.
and constraints of mobile agents as well as their desired As the planner cannot influence any platform physical
trajectory behaviours, accurate following of such paths be- state until its cycle is over, its pose is predicted from the
comes contradictory. As a solution, an additional parameter beginning to that temporal point (which varies typically
is defined, which specifies the maximum allowed deviation between 0.1s - 1s). Even though the optimizer is inherently
from the route. This addition not only increases the search- iterative, in many situations it is preferred to overshoot the
for-optimum space but also allows for a dramatic reduction of desired cycle time for achieving optimization convergence. In
the required vertices of the route graph (envision an A* algo- this sense, assuming that the time interval of an optimization
rithm generated route diagonally on a coarse grid). Especially cycle is relatively small with respect to the platform motion,
for mapped areas, image processing techniques for space additional correction through forward prediction of the initial
skeletization using line-of-sight heuristics/techniques could pose before each optimization iteration can be achieved.
be implemented for efficient and flexible route planning [11].
The route is thus consisted of line-connected consecutive VI. EXPERIMENTAL RESULTS
points of arbitrary distance, the desired end-pose being After an introduction to the implementation specifics, the
represented by the last point and its orientation. modelling and simulation of the robot ”Blue” is being intro-
As we are considering (sufficiently) planar surface nav- duced. Simulation results are then presented, showcasing the
igation, the depth sensor information as well as the map quality of the developed low-level control scheme as well as
is converted to a 2D representation. In practice, distance the capabilities of the proposed Local Path-Planner.
fields of the relevant spatial information are generated and The implementation of the presented work has been done
stored in separate layered maps. This way, data evaluation in C++, making use of GazeboSim as simulation environment
can be done in constant time, a critical factor in the terms of and the Robotics Operating System (ROS). Considering the
 0.1 m 1.5
1

x[m]
0.5 Motion-model
0 Measured
0 0.5 1 1.5 2 2.5 3 3.5
y[m]
Fig. 9. Global frame trajectory of the platform
ICC trajectory Even though the results in the local frame are accurate,
Fig. 7. Left: Simulated base of robot Blue, Right: ICC trajectory in local the global-frame motion of the platform compared with
frame of the platform the motion-model used in the Local Path-Planning module
is of importance. The presented motion model has been
development of a low-level controller, increased attention has
discretized using trapeze interpolation and evaluated every
to be payed to the simulated system. Fig. 7 – left illustrates
10 Hz. As it can be seen in Fig. 9, the motion-model used
the simulation of ”Blue”s mobile base, making use of CAx
by the Local Path-Planner accurately predicts the motion that
models of the components as specified by the designer, to-
is eventually undertaken by the agent up to several meters.
gether with component inertial information. General purpose
surface friction parameters have been applied accordingly. The Local Path-Planner has been implemented in C++,
Without loss of generality, the actuators are modeled taking being dependant only on the low-level controller, ROS (for
into account damping and the interface has been designed communication purposes), OpenCV as well as the optimiza-
to apply shaft torques as input. As 3D physics engines tion library Optizelle. The motivation behind the choice
implemented in Gazebo (such as ODE, Bullet or Dart) of the optimization library is its efficiency, State of the
perform typically poor in over-constrained closed kinematic Art implemented algorithms as well as matrix-free design.
chains (such as a I4WS) when force control is used, increased In practice, the optimization problem is solved using the
simulation accuracy is achieved through parameter-tuning of Interior-Point Method with SR1 Hessian estimation, Krylov
the engine (increased iteration count as well as relaxation sub-problem solver and affine line search for the inequality
of the stiffness of the formed contact joints). Additionally, constraints (through problem reformulation). The planner
quantisation noise is modelled for the angular sensors of the typically runs stable at a frequency of 10 Hz on single core
actuators as well as Gaussian noise of the depth sensor. of an Intel i7 processor. However, as discussed, the approach
The developed feed-forward control-scheme, together with is designed to be robust at lower cycle-frequencies as well.
the LQG feed-back has been implemented as a ROS-nodelet Fig. 10 presents some of the behaviours that can be
and has been tested at a frequency of 100 Hz, using sim- obtained by the Local Path-Planner using only three control
ulation ground-truth for validation. For simplicity of data points. The active route for this instance is represented
interpretation, the parametrization p = [vc ρc αc ] is used. by the green dotted line and its afferent distance-field is
Fig. 7 – right presents the local view of the platform as it created. The route far away is not considered locally and
performs a sequence of control commands. For visualization is in this instant inactive (blue). In the presented scenarios,
purposes, the entire parameter trajectory has been computed the parametrization p = [vc ωbase ωc ] has been used as it
initially; however, each cycle computation is performed on- performs superior to other parametrizations when attention-
line during normal operation. Desired versus applied param- focus points are targeted. The agent has an initial velocity of
eter and wheel velocity trajectories are illustrated in Fig. 8 – 0.1 m/s and for all trajectories, the end-constraint of 0 base
left, showcasing the accuracy of the proposed controller. In velocity is imposed. Furthermore, wheel actuator orientation,
Fig. 8 – right, the temporal evolution of the orientation as velocity and acceleration as well as platform base linear and
well as rotation speed of the top-right wheel is illustrated. As lateral acceleration constraints have been enforced. Fig. 10
expected, when the ICC revolutes at a small radius relative a) presents the scenario where route accuracy is desired.
to a wheel (big values of ρc ), the nonlinear character of the As the number of control points is significantly low, high
wheel motion becomes significant. path-following accuracy implies a reduction of the trajectory
1 length. Fig. 10 – b) presents the generated trajectory with
vc [ ms ]

0.6
αwi [rad]

0.5 0.4
0.2
dominant costs towards time-optimality. Notice that the ob-
0

2
0 stacle represents the only active spatial constraint. Fig. 10 –
αc [rad] ρc [ m1 ]

−0.2
1
0
c) and – d) showcase the same scenario when an attention-
−1
−2
0 focus point (to the left and right respectively) is targeted. The
s ]
ωwi [ rad

0.6
−10 increased duration of the generated trajectory d) is mostly
0.4
0.2
−20 due to the internal actuator constraints of such motion. Lastly,
0
0 2 4 6 8
−30
0 2 4 6 8
Fig. 10 – e) illustrates the situation where maximum path
t[s] t[s] deviation is reduced. Notice here the sensed obstacle distance
Fig. 8. Desired (red) vs. Measured (blue) data: left - ICC parameters, right
field kernelization further away from the path, as the path
- orientation and velocity a wheel constraint makes that region automatically infeasible.
 1m

sensed
obstacle

attention
points

a) close follow b) fast follow c) attention point d) attention point e) reduced path width
t = 4.76s, s = 3.31m t = 4.822s, s = 5.851m t = 8.432s, s = 6.121m t = 10.06s, s = 5.913m t = 4.726s, s = 5.431m
Fig. 10. Generated trajectories of the Local Path-Planner under different dominating weights

VII. CONCLUSIONS R EFERENCES

This work presented a two-layer control framework for an [1] C. P. Connette, C. Parlitz, M. Hagele, and A. Verl, “Singularity
I4WS platform capable to smoothly control a vehicle along avoidance for over-actuated, pseudo-omnidirectional, wheeled mobile
robots,” in Robotics and Automation, 2009. ICRA ’09. IEEE Interna-
a given path while enforcing constrains of various natures, tional Conference on, May 2009, pp. 4124–4130.
including platform body orientation. [2] C.-J. Lin, S.-M. Hsiao, Y.-H. Wang, C.-H. Yeh, C.-F. Huang, and
Initially, an overview of the addressed modules within the T. H. S. Li, “Design and implementation of a 4ws4wd mobile robot and
its control applications,” in System Science and Engineering (ICSSE),
autonomous navigation context has been presented. After- 2013 International Conference on, July 2013, pp. 235–240.
wards, local and global frame analysis of I4WS kinematics [3] P. Shen, Y. Fang, and X. Zhang, “Trajectory planning of omnidirec-
and dynamics has been done in order to identify beneficial tional mobile robots with active casters: Multi-motor coordination and
singularity avoidance,” in Cyber Technology in Automation, Control,
local frame parametrizations of I4WS motion. To this end, a and Intelligent Systems (CYBER), 2015 IEEE International Conference
flatness-based low-level control has been designed. on, June 2015, pp. 151–156.
In the second part of this paper, Local-Path Planning [4] S. Y. Jiang and K. T. Song, “Differential flatness-based motion control
of a steer-and-drive omnidirectional mobile robot,” in Mechatronics
within the context of autonomous navigation has been ad- and Automation (ICMA), 2013 IEEE International Conference on, Aug
dressed. The proposed optimization-based approach is able 2013, pp. 1167–1172.
to deal with various constraints of different natures (spa- [5] T. Howard, C. Green, D. Ferguson , and A. Kelly , “State space
sampling of feasible motions for high-performance mobile robot nav-
tial, temporal, dynamic) and showcases different motion igation in complex environments,” Journal of Field Robotics, vol. 25,
behaviours, intended to be of use to higher-level robotics no. 6-7, pp. 325–345, June 2008.
tasks while maintaining simplicity of interaction (through [6] T. Howard, “Adaptive model-predictive motion planning for naviga-
tion in complex environments,” Ph.D. dissertation, Robotics Institute,
a small set of parameters). Moreover, by careful design Carnegie Mellon University, Pittsburgh, PA, August 2009.
that targets bottle-necks, physics-engine simulations prove [7] W. Xu, J. Wei, J. M. Dolan, H. Zhao, and H. Zha, “A real-time motion
the capabilities and efficiency of the approach, constituting planner with trajectory optimization for autonomous vehicles,” in
Robotics and Automation (ICRA), 2012 IEEE International Conference
together with the low-level controller a Local Path-Planning on, May 2012, pp. 2061–2067.
Framework for I4WS systems. [8] P. Dorato, V. Cerone, and C. Abdallah, Linear Quadratic Control: An
On the research side, further investigation is intended on Introduction. Melbourne, FL, USA: Krieger Publishing Co., Inc.,
2000.
the topic of behaviour control on higher level modules. [9] B. Chachuat, Nonlinear and Dynamic Optimization: From
A generalized approach towards active use of expressive Theory to Practice - IC-32: Spring Term 2009, ser.
navigation will address many inconsistencies and difficulties, Polycopiés de l’EPFL. EPFL, 2009. [Online]. Available:
https://books.google.at/books?id= JOHYgEACAAJ
especially in the field of assistive robotics. Also, the mixture [10] M. B. Markus Suchi and M. Vincze, “Meta-Heuristic search strategies
of optimization-based Path-Planning and layered maps with for Local Path-Planning to find collision free trajectories,” in Pro-
fast queries motivates further investigation of multi-agent ceedings of the Austrian Robotics Workshop (ARW-14), May 2014,
pp. 36–41.
planning, potentially providing an elegant solution especially [11] Q. Wang, M. Wulfmeier, and B. Wagner, “Voronoi-based heuristic for
in the context where agents posses inter-communication [12]. nonholonomic search-based path planning,” in Intelligent Autonomous
On the application side, the model of the robot is planned Systems 13, ser. Advances in Intelligent Systems and Computing,
E. Menegatti, N. Michael, K. Berns, and H. Yamaguchi, Eds. Springer
to be published to the community, providing a carefully International Publishing, 2016, vol. 302, pp. 445–458.
designed model of I4WS for further development on related [12] M. Bader, A. Richtsfeld, M. Suchi, G. Todoran, W. Holl, and
topics. Last but not least, the presented work is to be M. Vincze, “Balancing Centralised Control with Vehicle Autonomy
in AGV Systems for Industrial Acceptance,” in Proceeding of the
implemented in the robot Blue, enabling extensive testing in Eleventh International Conference on Autonomic and Autonomous
demanding operating conditions towards smoother and more Systems (ICAS 2015), 2015.
natural Human-Robot interaction.

View publication stats