Jiuchun Gao

Fabien Claveau

Anatol Pashkevich

Philippe Chevrel

Real-time motion planning for an autonomous mobile robot with wheelground adhesion constraint

Keywords: autonomous mobile robot, real-time motion planning, wheel-ground adhesion, timeoptimal trajectory, dynamic programming

The paper proposes a new real-time motion planning technique for an autonomous mobile robot that is able to find a collision-free path and a corresponding time-optimal trajectory while taking into account limits on the actuator capacities and constraints from the wheel-ground adhesion. The proposed technique includes two sub-modules. The first one, an optimal path planner, is based on the discretization of the robot workspace and dynamic programming principle. It allows finding the shortest path in the robot environment to avoid obstacles and reach the robot target. The second one, an optimal trajectory generator, operates with the discretized robot state-space and also employs dynamic programming techniques. It produces a time-optimal motion along the obtained path, which may include numerous regular and singular trajectory sections caused by the simultaneous application of actuator and wheel-ground adhesion constraints. To adapt this technique to real-time implementation, a moving window strategy is presented that allows regularly updating the robot motion profile in a dynamic environment. The advantages of the developed technique and its suitability for real-time control are illustrated by experimental studies implemented on a car-like mobile robot.

Introduction

At present, the autonomous mobile robot market is exploding, such types of robots are continuously improved and already widely used in numerous fields like aerospace, military, goods delivery, and medical services [START_REF] Ben-Ari | Robots and Their Applications[END_REF]. Their further development focuses on providing a higher degree of autonomy and greater mobility/speed, which motivates new developments in mobile robot motion planning and control because usual classical techniques can be hardly applied if the motion speed exceeds some physical limits. One of perspective research direction is related to the wheel-ground interaction whose modelling normally assumes rather high adhesion and excludes such phenomena as excessive slipping and skidding. Similar topics are also significant for self-driving cars whose mechanics is very close to four-wheel mobile robots.

Figure 1 General schematic for autonomous mobile robot control Generally, control of an autonomous mobile robot contains three basic layers as shown in Fig. 1, the main purpose of motion planning module is to provide the mobile robot with a proper and collision-free trajectory towards its destination while considering the constraints from the mobile robot kinematics/dynamics, manoeuvre capabilities in the presence of obstacles, and the environment boundaries. Motion planning for autonomous mobile robots can be divided into three hierarchical classes [START_REF] Gonzá Lez | A review of motion planning techniques for automated vehicles[END_REF]: (i) global planning, (ii) local planning, and (iii) trajectory generation. Global planning is more like routing, which is concerned with selecting the best global path from an origin to a destination. The local planning is a locally correcting phase that handles the dynamic obstacle avoidance while taking into account the vehicle kinematics, obstacles, road geometry, and traffic interactions in the case of the self-driving cars. And finally, the trajectory generation outputs a timevelocity profile corresponding to the previously obtained geometric path while considering some constraints from the robot kinematics and dynamics. In most of the related works, the main attention is paid to the path planning while the velocity profile generation, especially with the minimum-time objective, received less attention.

In literature, graph-based techniques are widely used to generate a global collisionfree path in robot workspace. It presents the robot workspace in the form of a 2D graph, which allows to present the desired robot motion as a shortest path on the graph connecting the initial and the final vertices. The most representative algorithms are Dijkstra and A*.

Together with a PID-based Path Follow module, they were adopted in [START_REF] Bacha | Odin: Team victortango's entry in the darpa urban challenge[END_REF][START_REF] Bohren | Little ben: The ben franklin racing team's entry in the 2007 DARPA urban challenge[END_REF] for the DARPA Urban Challenge 2007. Another known technique of this group, state lattice algorithm uses a discrete representation of the planning area with a grid of vehicle states [START_REF] Mcnaughton | Motion planning for autonomous driving with a conformal spatiotemporal lattice[END_REF][START_REF] Ziegler | Spatiotemporal state lattices for fast trajectory planning in dynamic on-road driving scenarios[END_REF]. But similar to the above two, the principal limitation of these techniques is the difficulty of taking into account constraints related to the robot kinematics/dynamics and wheel-ground interaction in real-time. Sampling-based approaches consist of randomly sampling the configuration space and searching for feasible connectivity inside it [START_REF] Elbanhawi | Sampling-Based Robot Motion Planning: A Review[END_REF]. The most widely used two are the Probabilistic Roadmap Method and the Rapidly-exploring Random Tree [START_REF] Kuwata | Real-Time Motion Planning With Applications to Autonomous Urban Driving[END_REF][START_REF] Mohammed | An efficient approach to path planning in 3D for Static and Dynamic Environments[END_REF]. However, this family usually produces non-smooth and jerky paths that should be avoided in time-optimal control of the autonomous mobile robot [START_REF] Gonzá Lez | A review of motion planning techniques for automated vehicles[END_REF]. Combinatorial planning techniques use a polyhedral representation of the environment as input and augments it with additional edges or vertices, such as Visibility Graph and Voronoi Diagram [START_REF] Motonaka | Obstacle avoidance using buffered voronoi cells based on local information from a laser range scanner[END_REF][START_REF] Xiong | Path planning of multiple autonomous marine vehicles for adaptive sampling using Voronoi-based ant colony optimization[END_REF][START_REF] Magid | Voronoi-based trajectory optimization for UGV path planning[END_REF][START_REF] Arnaoot | Visibility Graph-Based Path Planning Algorithm Safety Evaluation and Optimization[END_REF]. But this technique concentrates on geometric aspects mainly. Local path planning techniques are usually applied to avoid collisions in a dynamic environment [START_REF] Kunchev | Path Planning and Obstacle Avoidance for Autonomous Mobile Robots: A Review[END_REF]. The Potential Field Method and its modification Vector Field Histogram are commonly used because of the elegant mathematical analysis and simplicity [START_REF] Malone | Hybrid Dynamic Moving Obstacle Avoidance Using a Stochastic Reachable Set-Based Potential Field[END_REF][START_REF] Minguez | Motion Planning and Obstacle Avoidance[END_REF]. Despite numerous advantages, these field-based methods act as a fastest descent optimization procedure, it may get stuck at a local minimum [START_REF] Ge | Dynamic Motion Planning for Mobile Robots Using Potential Field Method[END_REF]. In contrast to field-based methods, the velocity-space techniques are derived directly from the mobile robot kinematics, allowing taking into account the constraints on the robot maximum velocity/acceleration [START_REF] Kim | Region with velocity constraints: map information and its usage for safe motion planning of a mobile robot in a public environment[END_REF]. A typical example is the Dynamic Window Approach (DWA) [START_REF] Molinos | Dynamic window based approaches for avoiding obstacles in moving[END_REF].

It generates the velocity commands directly, but still with difficulties in taking into account limitations arising from the wheel-ground interactions.

Trajectory generation is for producing the optimal motion along the obtained path.

Existing techniques mainly rely on the classical phase-plane methods came from control theory [START_REF] Kwakernaak | Linear optimal control systems[END_REF] in industrial robotics. For wheeled mobile robots, a time-optimal velocity profile should be produced under dynamic constraints that are due to the motor physical capacities and also must ensure the wheel-ground adhesion [START_REF] Weiguo | Optimal motion planning for a wheeled mobile robot[END_REF]. In the frame of the phase-plane method, this adhesion limits can be presented as a non-linear constraint imposed on the velocity and acceleration, which leads to multi-switching of the time-optimal control [START_REF] Shen | Complete and Time-Optimal Path-Constrained Trajectory Planning With Torque and Velocity Constraints: Theory and Applications[END_REF][START_REF] Petrinić | Time-optimal velocity planning along predefined path for static formations of mobile robots[END_REF].

Such multi-switching makes difficult for algorithmization in real-time and can be hardly implemented in mobile robot controllers. The wheel-ground adhesion constraint was partially taken into account in [START_REF] Consolini | An optimal complexity algorithm for minimum-time velocity planning[END_REF], where a phase-plane based algorithm was proposed and the maximum centripetal acceleration limit was considered. Another technique was presented in [START_REF] Kim | Cornering Trajectory Planning Avoiding Slip for Differential-Wheeled Mobile Robots[END_REF], where the cornering trajectory planning algorithm was developed and implemented for a differential-wheeled mobile robot. It takes into account constraints on the wheel maximum longitudinal force and lateral force in order to avoid sliding, similarly in [START_REF] Xiong | An optimized trajectory planner and motion controller framework for autonomous driving in unstructured environments[END_REF][START_REF] Diachuk | Motion Planning for Autonomous Vehicles Based on Sequential Optimization[END_REF]. However, the longitudinal force and lateral force were considered separately there instead of a resultant one within the friction circle. In [START_REF] Okuyama | Minimum-time trajectory planning for a differential drive mobile robot considering non-slipping constraints[END_REF], a simplified elliptical friction model was considered via limiting the angular acceleration to a predefined maximum value when generating a minimum-time velocity profile for a differential-wheeled mobile robot. It is also worth of mentioning some recent work based on NMPC [START_REF] Micheli | NMPC trajectory planner for urban autonomous driving[END_REF] that allows taking into account the Pacejka's tire model in urban driving conditions, however the high-curvature path and related wheel-ground adhesion issues was out of the authors' attention.

To our knowledge, few works combine global/local path planning with trajectory generation while taking into account the constraints arising from the mobile robot kinematics, dynamics, and wheel-ground interaction that are in the focus of the paper. It is an extension and integration of our previous works [START_REF] Gao | Optimal Motion Generation for Mobile Robot with Non-Skidding Constraint[END_REF][START_REF] Gao | Real Time Motion Generation for Mobile Robot[END_REF]. The main contribution of the paper is the development of a new real-time motion planning technique, implemented as two submodules. The first of them, the path planner is based on the dynamic programming principle applied to the discretized robot workspace. It allows finding the shortest path in the robot environment with multiple obstacles. Generally, the architecture design of the path planner depends on the application. In particular, for autonomous driving on the road, a hierarchical architecture including global path planning and local path planning is required. Whereas for goods transportation in small spaces, the global and local path planners can be merged into a general one, which is implemented in our experimental mobile robot. The second submodule, the trajectory generator operates with the discretized robot state-space and also employs dynamic programming technique. It produces a time-optimal motion along the obtained path, using the maximum capabilities of the actuators. The obtained fastest motion allows achieving higher efficiency in such applications as goods deliveries. An essential advantage of this work is that the wheel-ground adhesion condition is directly included in the trajectory generation sub-module. It is worth mentioning that the friction limits of the wheels are widely studied in the automotive field, and precise wheel dynamics models are used in vehicle dynamics control. However, there is no existing work considering this issue in the timeoptimal trajectory generation/planning for autonomous mobile robots. It should be also noted that in classical time-optimal control, the velocity and acceleration constraints are usually assumed to be independent yielding rather simple trajectories with two switching only. In contrast here, the velocity and acceleration constraints are coupled and corresponding timeoptimal trajectories include multiple switching points that can be hardly generated using conventional phase plane methods. But the developed technique is able to treat even such difficult cases in real-time.

The Problem of Real-Time Motion Planning

The problem of real-time motion planning here is divided into two sub-problems, such as (i) path planning and (ii) trajectory generation, as shown in Fig. 2. It should be mentioned, in our case, the mobile robot moves in a quite small indoor area, around 30 m 2 , and it is reasonable to merge the global planning and local planning into a general path planner in this work.

Figure 2 General schema of motion planning for autonomous mobile robot

Path Planning with Geometric/Kinematic Constraints

It is assumed that the path planner obtains the coordinates of the desired target location and the known obstacle locations from the indoor GPS system, as shown in Fig. 3a where k is the obstacle number. For the unknown obstacles, we are using the same notation but assume that the similar , where rob r is the equivalent radius of the robot. It should be mentioned that the unknown obstacles are detected by the LiDAR and also roughly approximated as circles for simplicity. Using the data from the LiDAR, a distance between the LiDAR center and the obstacle d, the obstacle width w, as well as an angle with respect to the LiDAR frame ϴ, can be obtained. Thus, an approximated circle with the radius w/2 can be located at the detected point whose distance to the LiDAR center is d.

Although the approximated circle may not completely cover the obstacle, in practice, no collision may happen as such circle and the locally planned path should be updated at each time step.

And finally, the collision constraint can be expressed via the distance from the path to the obstacle center and its increased radius

    22 ( ) ( ) ( ) ; 1, 2,3... G G k G G k k obs obs md x s x y s y r k              (4)
Therefore, the path planning problem for the autonomous mobile robot studied here is presented as finding the shortest path that describe the robot's capability to achieve desired velocities and accelerations during the motion. Besides, for fast motions of wheeled robots, there is an additional specific constraint arising from the wheel-ground interaction and excluding the wheel skidding when the robot enters the high curvature path segments with high speed. As known from the vehicle dynamics [START_REF] Jazar | Vehicle dynamics: theory and application[END_REF], the adhesion force acting between the wheel and the ground is limited by a certain value that depends on the road condition and should be inside of the so-called friction ellipse/circle, as shown in Fig. 5. The latter can be expressed in the form of the following

( ) ; ( ) d ( ) d tan( ) d ( ) d GG s GG G G k G G k k obs obs md G G G G G G G G tt G G G x s y s s s x s s y s s s c s L x s x y s y r x x y y x s x y s y y s s x s                        ,0 ss                     (5) (0)

Trajectory Generation with Dynamic Constraints

inequality 22 x y z F F F     (8)
where ,,

    2 2 2 2 2 2 d ( ) d ( ) d ( ) d ( ) s t t c t s t t g       (9)
where

  ( ) ( ) c t c s t 
is the path curvature.

Figure 5 Friction ellipse/circle

It should be mentioned that the non-skidding condition ( 9) was derived assuming that both the velocity and the acceleration are computed for the robot center of gravity, i.e. the robot is presented as a material point. However, this constraint can be easily generalized by considering the interaction with the ground for all wheels separately. It can be proved that for the four-wheel case, the non-skidding condition produce four constraints with similar structures

    2 2 2 2 2 2 d ( ) d ( ) d ( ) d ( ) ; 1, 2,3, 4 i i i s t t c t s t t g i         ( 10
)
where

1 2 2 1 ( ) 1 ( ( ) cos ( ) 0.5 ) c t c t t B L       , 1 2 2 2 ( ) 1 ( ( ) cos ( ) 0.5 ) c t c t t B L       , 1 1 3 ( ) [ ( ) cos ( ) 0.5 ] c t c t t B       , 11 4 ( ) [ ( ) cos ( ) 0.5 ] c t c t t B      and   ( ) arcsin ( ) t L c t   ,     d ( ) d d ( ) d ( ) i i s t t s t t R c t  ,   2 2 2 2 d ( ) d d ( ) d ( ) i i s t t s t t R c t    .
It should be also noted that for high-speed driving, full dynamics of the vehicle and wheel/tire usually should be considered in the motion control. However in this work, there are several simplifications. First of all, it is assumed that the robot drives on a 2D plane, so the pitch and roll motion are ignorable, and also the friction coefficient is almost constant because of unchanging road condition. For this reason, for planning the desired time-optimal trajectory under these assumptions, a kinematic vehicle model plus the tire dynamic model should be sufficient. For computational convenience, a simplified tire dynamic model based on TMeasy is used here. According to the generalized tire characteristics presented in [START_REF] Rill | TMeasy--A Handling Tire Model based on a three-dimensional slip approach[END_REF], there are three modes: 1) adhesion mode where the combined slip ratio

                               (11)
It is worth mentioning that the optimization problem includes some particularities compared to the classical time-optimal control problem for the second-order system that was intensively studied in the literature [START_REF] Bobrow | Time-optimal control of robotic manipulators along specified paths[END_REF], where the constraints were applied to the velocity and acceleration independently. In contrast, in our work, there is a non-linear adhesion constraint [START_REF] Mohammed | An efficient approach to path planning in 3D for Static and Dynamic Environments[END_REF] where the velocity and acceleration are coupled, which does not allow straightforwardly applying previously developed methods. Relevant techniques for the considered time-optimal trajectory generation in real-time are proposed in the following section.

Motion Planning Algorithms and the Real-Time Implementation

In order to solve the above-stated problems, a combinatorial optimization based methodology is proposed in this section. It includes the discretization of the robot workspace (for path planning) and state space (for trajectory generation). Further, to find the desired shortest path and time-optimal trajectory satisfying all static/dynamic constraints, the dynamic programming principle is applied to create relevant motion planning algorithms.

Shortest Path Search in Discretized Robot Workspace

To present the robot path planning problem in a discrete way, let us use the double presentation of the workspace, via the Cartesian coordinates ( , ) xy and the polar coordinates Further, assuming that the distance between the mobile robot and the target point is sequentially reducing, the desired optimal path can be presented as the following sequence of the nodes

k i k k m i i n           (12)
1 2 1 ,1 , 2 , 1 , { } { } ... { } { } nn k k k n k n       S S S S (14)
where { 1, 2,... } i k i n  are certain indices obtained from the optimization routine.

Corresponding distances between the subsequent nodes can be evaluated via the Cartesian coordinates as

1 1 1 , , 1 , , 1 , , 1 dist ( ,
) ii ii ii k i k i k i k i k i k i xx yy                        SS (15) where ,, ( , )
ii k i k i xy is the Cartesian coordinates of the i th path node, and

ii

k i k i xy  
is the potential coordinates of the (i+1) th path node. The latter allows us to present the objective function to be minimized as follows

1 1 , , 1 1 dist ( , ) min ii
i n k i k i k i D       SS (16)
Further, to take into account the constraint on the path curvature, all subsequences

1 1 , 1 , , 1
i i i k i k i k i      S S S
should be verified with respect to the inequality [START_REF] Gonzá Lez | A review of motion planning techniques for automated vehicles[END_REF], which in the frame of the adopted notations can be presented as

1 1
1 max curv [( , 1),( , ),( , 1)] sin

i i i k i k i k i L        (17)
where the curvature function curv [ , , ]  is easily obtained by the arc fitting of the following three points

11 , 1 , 1 { , } ii k i k i xy   , ,, { , } ii k i k i xy, 11 , 1 , 1 { , } ii k i k i
xy   . For computational convenience, the path curvature at the node , i ki S can be also expressed via the robot orientations

 , i ki ,    1 ,1 i ki at the segments 1 , 1 , [] i i k i k i    SS and 1 , , 1 [] ii k i k i    SS
respectively, which yields the revised approximated expression for the constraint (2) ) sin

11 1 , 1 , , , 1 max dist ( ,
i i i i k i k i k i k i L           SS (18) , , , 1 , , 1 atan 2( ,
)

i i i i i k i k i k i k i k i y y x x  , 1 , 1 , , 1 , atan 2( ,
)

i i i i i k i k i k i k i k i y y x x           .
The collision constraints can be taken into account via the verification of the validity of the inequality , dist( , )

i md k i j j r  SS (19)
where ˆj S represents the center of the j th obstacle and md j r is the modified obstacle radius.

Thus, the original robot path planning problem is converted to a combinatorial one, dealing with the shortest path search on the graph. In contrast to classical formulations, there are specific constraints here (feasible curvature and collision-free) applied to the graph nodes and triples of subsequent nodes, which make difficulties in applying known graph-based approaches. For this reason, a dynamic programming (DP) technique is used here to find the desired optimal path that satisfies all constraints imposed by the environment and geometry.

that is applied sequentially starting from the second layer, i.e.

i i i k i k i k i      S S S
extracted from the considered path. As the DP principle is applied to both path planning and trajectory generation, a more detailed explanation and pseudo-code for DP implementation are given in Subsection 3.2.

Time-Optimal Trajectory Generation in Discretized Robot State Space

For the problem of trajectory generation with time-optimal control structure, as discussed in Subsection 2.2, the conventional phase plane technique has the difficulty in including the nonlinear wheel-ground adhesion constraint [START_REF] Mohammed | An efficient approach to path planning in 3D for Static and Dynamic Environments[END_REF]. An alternative approach was first proposed in our previous work [START_REF] Gao | Optimal Motion Generation for Mobile Robot with Non-Skidding Constraint[END_REF][START_REF] Gao | Real Time Motion Generation for Mobile Robot[END_REF], based on the discrete dynamic programming (DP). Its main advantage is the universality allowing to take into account all considered constraints in a similar way and to generate optimal trajectories of complex structure including multiple switching points. In this paper, an extended and more complete DP-based algorithm is presented.

First

k i k i s v s k m    C (22)
Taking into account that the path points are naturally ordered in time as 01 ... ...

k k n    C C C ( 23
)
where the distance between subsequent nodes is corresponding to the motion time and is computed in the following way [START_REF] Consolini | An optimal complexity algorithm for minimum-time velocity planning[END_REF] The latter allows us to present the objective function to be minimized as follows

11 , , 1 1 time
( , ) 2

( ) ( )

i i i i k i k i i i k k s s v v      CC
1 1 , , 1 0 time( , ) min ii n k i k i i T       CC (25)
It is clear that the method of state-space discretization (21) applied above automatically takes into account the velocity constraint in [START_REF] Elbanhawi | Sampling-Based Robot Motion Planning: A Review[END_REF]

i i i i a v v t     (26) where 1 , , 1 time( ,
) ii i k i k i t  
 CC

. In addition, the non-skidding condition (9) should be also verified using the following inequality ()

i i i a v R g     ( 27
)
where

1 ii Rc 
is the radius of path curvature corresponding to the i th sampling point. Hence, the original trajectory generation problem is converted to a combinatorial one, which allows taking into account the wheel-ground adhesion limits and the constraints applied to the node subsequences that limit the robot acceleration.

The DP principle is adopted here too, breaking down the full-size problem into a set of sub-problems and aiming at finding all optimal sequences from the initial node 0 ,0 k

C to the current ones , { , } i k i i k  C
ensuring the minimum of the motion time [START_REF] Kim | Cornering Trajectory Planning Avoiding Slip for Differential-Wheeled Mobile Robots[END_REF] for which the accelerations computed via the subsequences [START_REF] Micheli | NMPC trajectory planner for urban autonomous driving[END_REF] in which the acceleration

p i k i p i p a g v R         CC

Algorithm: Time-optimal trajectory generation along the path

Input:

Matrix of states -C(k,i) of size m×n Array of radius -R(i) of size 1×n Output:

Minimum path length -Dmin Optimal path indicesk 0 (i), i = 1,2,…n Notations:

Distance matrix -D(k,i) of size m×n Pointer matrix -P(j,i-1) of size m×n Functions: Distance between nodesdist ( C(k1,i1), C(k2,i2) )

Adhesion test for a nodeadhs In more detail, an outline of the developed algorithm is presented below in the form of pseudo-code. The input includes the state matrix { ( , ) 0,1,... ; 0,1,... } C k i k m i n  containing information on the velocity and displacement, as well as the array of path curvatures

( C(k,i), R(i) ) Acceleration test for nodes -acc (C(k1,i1), C(k2,i2)) (1) Set D(k,1):=0; P(k,1):=null; ∀k=1,2,…m (2) For i = 2 to n do For k = 1 to m do For j = 1 to m do (a) If (acc (C(k,i), C(j,i-1)) = 0) & (adhs ( C(k,i), R(i) )=0 ) & (adhs ( C(j,i-1), R(i-1) )=0 ) r(j) := D(j,i-1)+dist ( C(k,i), C(j,i-1) ) else r (j)
{ ( ) 0,1,... } R i i n 
. The algorithm operates with two tables ( , ) D k i and ( , ) P k i that include the minimum distances for the sub-problem of lower size (for the path 0 ,0 , ... 28) is implemented. The computing starts from the second layer, and it tries all possible connections between the states in the current layer and the previous one while verifying the wheel-ground adhesion and acceleration limits in the sub-step (2a). The sub-step (2b) finds the optimal trajectories from all states { ( , ), } C k i k  belonging to the i th layer and records the references to { ( , 1), } C j i j  into the pointer matrix ( , ) P k i . In steps [START_REF] Bacha | Odin: Team victortango's entry in the darpa urban challenge[END_REF] and ( 4), the optimal solution is finally obtained and the corresponding path is extracted by means of backtracking.

Real-time Implementation of Developed Algorithms

For real-time implementation, a 'moving window' strategy was developed. This developed strategy breaks down the full-size problem into a set of lower dimension sub-problems corresponding to shorter displacements. As is shown in Fig. 8, a moving window is applied segment by segment. For each window, the developed DP-based algorithm from Subsection 3.2 is used to generate a trajectory segment, i.e. a local time-optimal motion from the current state to some intermediate (goal) states with maximum velocity and some ahead displacement. This procedure is periodically repeated while the current state is approaching the target one. It is implemented in real-time, generating new segment slightly in advance while the controller is carrying out the motion profile from the previous window. The latter allows us to execute the motion generation and implementation simultaneously.

Experimental Validation

The proposed methodology was validated via the experimental studies dealing with a mobile robot in an indoor environment, around 30 m 2 . The following subsections present some details concerning the robot technical parameters.

Figure 8 Multi-segment strategy of real-time optimal motion generation for the mobile robot

Robotic Platform and Experimental Setup

The mobile robot (see Fig. 3) is a car-like system with four-wheel driving and front steering, whose wheelbase is 0.450 m, and the length of the front/rear track is 0.482 m. The robot is localized by means of indoor GPS. Odometer, available in each in-wheel motors equipped with build-in hall-effect sensors, is used to measure and regulate the velocity of the wheels only. A LiDAR makes the environment scanning for local vision construction. More details concerning the sensors are given in Table . 1. Simulink. It should be mentioned that the sampling time used here is rather large to simplify the experiment monitoring but for future industrial applications, it will be essentially reduced.

The experimental environment is shown in Fig. 9. The robot's initial location and the target position were randomly selected by the experimenters. Within the test area, there is a no-go area (also treated as a static obstacle). A dynamic obstacle (box) was moved manually during the robot navigation. For the experiment, the maximum velocity was set to 0.50 m• s -1 , and the maximum acceleration was 0.30 m• s -2 . The robot initial location was placed at (1.886, 5.376, -39.586º ), and the target was defined by its position coordinates (0.645, 0.579). The no-go area was presented as a circle of radius 0.350 m centered at (1.770, 3.96). The detecting range of the LiDAR was set to 1.0 m for moving obstacles.

In the implemented motion planning algorithm, the robot workspace ( , )  was discretized with 10 n  samples for the polar radius  , and angle step 1     . It was converted into a directed graph composed of from 1800 to 2700 nodes after collision checking, which was used as an input to the developed DP-based algorithm producing a collision-free path. Then, the proposed procedure of the time-optimal trajectory generation was applied. Here, the robot state space ( , d d ) s s t was discretized with the number of the speed increments dd v s t  set to 30, and the number of the distance increments 10 n  was the same as for the  discretization in the path planning. Thus, the robot state space was converted into a directed graph composed of 300 nodes. Finally, a time sequence of the speeds () i vt for the robot gravity center was obtained and transformed into the angular speeds of the wheels.

An experimental result confirming the validity of the proposed approach is presented in Fig. 10, which shows the robot motion and its planned path evolution caused by changing of the environment. At the beginning of the experiment, the robot was placed at its initial location, and the planned path was obtained taking into account the static obstacle only (see case t = 0.0 sec, when the path was expected to be on the left side of the static obstacle).

Slightly later (see case t = 5.80 sec), when the robot posture changed, the route was modified and located on the other side of the obstacle to meet the shortest path objective. The main reason for this change is that the maximum steering constraint ( 2) is satisfied during the initial planning whereas it is actually violated in practice. That is caused by the inaccuracies coming from the approximate discrete presentation of constraint (2), as shown by equation [START_REF] Kim | Region with velocity constraints: map information and its usage for safe motion planning of a mobile robot in a public environment[END_REF]. Such violation may appear when the system status is close to the limit border. As shown in the figure, the initial path at the time t = 0.00 sec is quite curved, already at the limits of the maximum steering angle. It is also worth mentioning that the corresponding route segment at t To demonstrate the impact of the wheel-ground adhesion constraints on the robot motion, Fig. 12 presents the time profiles of the planned speeds and the corresponding path curvature to the total displacement. As can be seen from this figure, the path curvature is very low at the time instance t = 5.6 sec, 8.0 sec, and 12.0 sec, so the robot speed is planned to be maximum (or slightly lower because of the coarse discretization). In contrast, for other time segments, the path curvature cannot be ignored, and the robot speed is reduced in order to satisfy the wheel-ground adhesion constraint. It is worth mentioning that the proposed constraint can be applied either to four wheels or to the virtual wheel located at the robot's center of gravity for simplicity. Considering the computational capacity of the hardware, in the experimental study, the proposed constraint was applied to the center of gravity. As we assume the robot's center of gravity and geometric centroid are overlapped, so the velocities of two wheels on the same side should be the same. Such simplification is reasonable here because the robot wheelbase of 0.450 m is rather small and even shorter than its wheel track of 0.482 m. This simplification is also justified by Fig. 12 where the optimal time profiles for both the robot center and wheels are presented. To show the validity of the proposed adhesion constraint, Fig. 13 presents the time profiles of the planned longitudinal and lateral accelerations, as well as their resultant acceleration (see the left hand side of equation ( 9), the simplification of the resultant tire force). As can be seen from this figure, at the time instances t = 8.8 sec and 12.5 sec, the resultant acceleration reached its limit (or slightly lower because of the coarse discretization).

It is also shown in Fig. 12 that at those two time instances, the robot was trying to enter the high curvature path segment with relative high speed. This result confirms that by applying the proposed constraint, the resultant acceleration is always within the friction limit, and the adhesion condition is always satisfied. To demonstrate the effectiveness of the proposed technique, the obtained time-optimal trajectory has been also compared with the one from the conventional phase plane method which does not allow taking into account the non-linear adhesion constraint [START_REF] Mohammed | An efficient approach to path planning in 3D for Static and Dynamic Environments[END_REF] where the velocity and acceleration are coupled. As can be seen from Fig. 14, using the conventional phase plane method without wheel-ground adhesion constraint, the resultant acceleration of the robot will be out of the friction limits when the path curvature and the robot speed are both relative high, whereas the proposed solution ensures the wheel-ground adhesion for the entire robot motion.

Figure 14 Comparison of motions obtained from the proposed approach and conventional phase plane method

In our experiments, to implement the planned path and generated trajectory along this path, the PI speed controllers were employed for all wheels. Corresponding time profiles are presented in Fig. 15, which shows some small differences between the planned and real wheel speeds (as well as between the speeds of the front and rear wheels) that are not essential for the global motion and are compensated due to periodical path/trajectory re-planning in accordance with the current robot location and its environment. Therefore, the obtained experimental results confirm the validity and practical reasoning of the proposed robot motion planning technique.

Discussion

In spite of the obvious advantages of the proposed technique (practical benefits confirmed by the experimental study, the capability to take into account the velocity/acceleration limits, and the wheel-ground adhesion constraints), there are a number of limitations and open questions to be discussed. The first of them is related to the selection of the discretization step for the path planning and trajectory generation. It is clear that a smaller discretization step may provide faster and smoother motions but require extremely high computational efforts that may be beyond the system hardware capacity. On the other side, too excessive discretization does not yield significant improvement of our engineering objective that is focusing on generating in real-time the smooth, collision-free, and fast motions. In our experiments, the distance was discretized with the maximum step 0.6 m and the speed was sampled with the largest step 0.017 m• s -1 , which were sufficient to obtain in real-time fast and smooth robot motions within a workspace of 5×6 m 2 . Clearly, it is a trade-off between the robot motion quality and our hardware capacities. However, in future implementations, it is reasonable to adjust the discretization steps for both the path planning and trajectory generation in order to have a better adaptation to the robot working environment. It also should be mentioned that when the robot works in a wider workspaces like warehouses or factories, a hierarchical architecture will be applied to the path planner which includes a global one and a local one.

The global path planner is event-based, not updated at each time step, and the local one uses the same moving window as the trajectory generator, whose computing time remains the same. Therefore, even in big workspaces, the computational effort of the proposed technique will not increase significantly.

It is also worthy of mentioning that the main particularity of this work (dealing with the wheel-ground interaction modelling) is based on a number of assumptions simplifying the robot motion planning. For instance, the wheel-ground friction coefficient μ was assumed to be known and constant for all wheels while in the real working condition it can vary essentially. In practice, a simple way of selecting μ is to use the table including peak friction coefficient according to different road conditions, which is widely used in automotive field [START_REF] Jin | On the road friction recognition based on the driving wheels deceleration[END_REF]. For example, on the dry concrete road, maximum μ can be 0.9, whereas 0.1 on the ice road. They are experimentally based, and sufficient for common road surfaces. A more accurate way is to estimate such coefficient in real-time while using speed and force sensors installed on wheel. It is usually implemented on vehicle electronic systems as ABS or ESP. In addition, in the current work, the wheel slip angles and the robot chassis sideslip angles are always assumed to be ignorable. Hence, it is reasonable in the future to expand the proposed technique by including a more precise tire model.

Conclusion

This paper proposes a new method to generate the fastest collision-free motion for an autonomous mobile robot in real-time. In contrast to the existing motion planning algorithms, the proposed one takes into account not only constraints from the robot environment and its kinematics/dynamics, but also some limits caused by the wheel-ground interaction to avoid excessive skidding. An experimental study confirms the validity and practical reasoning of the proposed technique.

In the future, the developed approach could be enhanced by executing the perception and motion planning procedures with different time frequencies, which allows reducing the computational efforts and producing smoother trajectories. The circular expression of collision-free constraints for robots can be improved by using more accurate approximations, for example in [START_REF] Ito | Configuration-aware model predictive motion planning for Tractor-Trailer Mobile Robot[END_REF]. Also, more accurate approximations of the known/unknown obstacles would be necessary, in order to have a better adaptation to various obstacles, such as walls or tables, which are currently approximated by different small circles at each time step.

F

  online by the LiDAR with respect to the robot frame R , whose definition is shown in Fig. 4. The obstacles in this work are approximated by circles whose dimensions are slightly modified by adding the equivalent radius of the robot to their original values, as

Figure 3 {

 3 Figure 3 Estimation of robot position and orientation from the sensors (indoor GPS system and LiDAR)

Figure 4

 4 Figure 4 Kinematic structure of the mobile robot and its parameters

  the path curvature and collision constraints (2)-(4). In a formal way, this problem can be summarized as follows

.

  the longitudinal, lateral, and vertical forces respectively, μ is the friction coefficient that is assumed as a constant here. When the robot of mass m goes into a cornering manoeuvre with the velocity v (see Fig.5), the lateral force provides the is the curvature radius. However, if the tangent speed is too high, the robot is desired to decelerate slightly, which requires the longitudinal force In addition, the vertical force can be computed as non-skidding condition (8) can be expressed via the derivatives of the function () st as follows

  ( , ) , and discretize the allowable domain of the polar angle

.

  It is assumed that the pole of the polar system coincides with the origin of the robot frame R F and the polar axis is directed along the Cartesian X-axis of R F . In such settings, it is reasonable to limit the range of the polar coordinate  by the distance from the origin to the target point, in Fig.6. Further, in order to avoid excessive discretization and to reduce the real-time computational effort, the range of the polar angle   should be reduced in the following way: is the target location coordinates with respect to the robot frame. However, it is worth mentioning that the proposed technique allows doing a new search with the full range of the polar angle if there is no solution found in the reduced admissible area. This may happen when the numbers and dimensions of the obstacles are quite huge.

Figure 6

 6 Figure 6 Discretization of robot workspace for path planning: admissible and non-admissible nodes

S

  The developed DP-based algorithm breaks down the full-size problem presented above into a set of sub-problems, aiming at finding the shortest path from the initial node 0, 0 i th layer with the polar coordinate i i     . To present the basic idea of this algorithm, let us denote * , ki d as the length of the shortest path connecting the initial node 0, 0 S to the current node , i ki S . Then, taking into account the additivity of the objective (16), the shortest path for the nodes belongs to the next layer between the nodes belonging to the layers i and 1 i  . The latter yields the formula

  including all possible trajectories corresponding to the given path can be presented as a directed graph shown in Fig.7. In particular, it is clear that the robot velocity at the starting and end points is known, so only a single state 0 ,0 k C should be used to represent the starting point and a single state 0,n C for the target point, see the graph in Fig.7. Another particularity of this graph caused by the time-irreversibility is that the allowable connections between the nodes are limited to the subsequent configuration states edge weights correspond to the traveling time defined below.

Figure 7

 7 Figure 7 Graph-based presentation of the discretized state space for optimal motion generation along a given path

1 {

 1 corresponds to the node , pi C . This formula is applied sequentially starting from the second layer producing the matrix of the motion times , [] ki τ . When this matrix is created, the minimum value min n  is selected from the last column corresponding to the robot time-optimal motion from the initial state to the target point. Finally, by applying the backtracking, one can get the desired optimal connection in the state-space graph. It is described by relevant indices 1 2

4 )

 4 := Inf end (b) Set D(j,i) := min(r); P(j,i) := argmin(r); (3) Set Dmin := min( D(k,n) ); k 0 (n) := argmin(r) (For i = 2 to n do Set k 0 (i-1) := P(k 0 (i), i)



  respectively. The procedure is composed of four basic steps. The first step (1) initializes the distance and pointer matrices. In step (2), the recursive formula (

  is based on an Intel Atom processor N270 (1.6 GHz 1 GB DDR2 533 MHz RAM). The proposed algorithms were created on an external PC first and then downloaded to the robot CPU via Simulink Real Time 2017. This hardware configuration ran with the sampling time of 0.20 sec and utilizes the discrete solver from

Figure 9

 9 Figure 9 Experimental environment for indoor navigation

= 5 .

 5 Fig. 11.

Figure 10 Figure 12

 1012 Figure 10 Evolution of the robot location and its planned path in the dynamic environment

Figure 13

 13 Figure 13 Time profiles of planned longitudinal, lateral, and resultant accelerations

Figure 15

 15 Figure 15 Experimental time profiles of the wheel speeds (PI-controlled)

  .

	The current position coordinates and orientation angle of the robot	G	0 p		0 ( , G xy 0 , G G	0	)
	with respect to the global frame the desired target location by assumed to be not constrained in this work. Also, let us denote the center locations of the G F are measured in real-time (see Fig. 3b). Let us describe ( , , ) G G G G t t t t xy  p  is , where the target orientation G t known obstacles as ( ) ( ) ( ) { ( , ) 1, 2,... } G k G k G k obs obs obs x y k n   p

   of the nodes belonging to the final layer and applying the backtracking, one can get the desired optimal path in the considered graph. It is

									1, 2,... 1 in  . Finally, after the
	* ,1 ki d described by the recorded indices 1 2 3 selection of the minimum objective { , , , ... } n k k k k . It should be noted that the fusion of the
	optimal path 0, 0 [ SS ... 	, ki i	] opt	and a new segment	1 [] , , 1 ii k i k i    SS	is allowed if and only if
	all the above-mentioned constraints are satisfied, which are verified using the node
	subsequence	[	1	, 1	,			1	, 1	]

  , let us apply sampling to the allowable domain of the velocity

	where	max v v m 	and	max s s 	n	. Further, for each path point	i s it is possible to generate a
	number of the possible states ( , ) vs that differ in velocities, i.e. to produce a mapping from
						ki
	the geometric path to the robot state space ( , ) vs allowing to describe motion in time
						i	{	,	( , )	0,1,... }
								max vv  [0,	]	and
	displacement	max ss  [0,	]	with the steps	v  and	s  respectively
							; ; v k v k s i s      	k i	 	0,1,... 0,1,... n m	(21)
							i

  , but the relevant acceleration constraints

	i a		max ; a	i  	1, 2,... 1 n  must be examined for each candidate subsequence	, ii k i k  CC	1 	, 1 i 	,
	using an approximated expression
					1 ()

Table 1

 1 Robot sensors, their parameters, and locations

		Sensor parameters
		Accuracy ±2cm
	Indoor GPS
		Detection area ~30 m 2
	Marvel-Mind solution
		Frequency 4Hz (average value)
		Range 0.5-6 m
	LiDAR scanner
		Accuracy: 0.9º resolution (200 points for a 180º arc)
	RPLIDAR A2
		Frequency: 2000 Hz
	Odometer	14-pole motor
	The robot is actuated using four in-wheel motors (velocity control) and a steering
	motor (position control). The dynamic capacities and specifications of the motors are
	presented in Table. 2.
	Table 2 Motors parameters and control modes
		Basic parameters
	Servo motor	Maximum speed: 8.04 rad• s -1
	Savox SB-2290SG


Liagre Elleᵃ

Christopher J ᵃ Knüsel

Villotte Sébastien

Conclusions & Prospects Throwing Light on the Past A New Methodology for Analysing Activity-related Skeletal Changes in Humeri

The medial epicondyle of the humerus is a particularly relevant anatomical area to reveal physical activities performed during an individual's lifetime ¹⁻³ (e.g. throwing of objects, the use of axes and stone tool manufacture)⁴·⁵ and the sexual division of labour in past human groups. However, the qualitative recording of osseous alterations in this area remains particularly subjective and prevents large-scale comparisons.

• Complementary shape variables can be applied to quantify osseous alterations of the humeral medial epicondyle. • Through 3D-imaging, the method offers a non-invasive approach to more objective recording of these alterations. • Application to identified osteological collections will improve understanding of the analyses' results and their interpretation. The main aim of this research is to develop a novel method to record these alterations quantitatively through non-invasive 3D-imaging.

Quantitative Analysis

What? Topographical analyses • quantify the whole-surface morphology,

• through complementary variables such as curvature, relief and complexity

•

  The method can create new insights into physical activity and sexual division of labour in the past. ᵃ UMR5199 PACEA, University of Bordeaux, Bordeaux, France. | ᵇ UMR7206 Éco-Anthropologie, CNRS, MNHN, Université Paris Cité. Musée de l'Homme, Paris, France. | ᶜ Quaternary environments & Humans, OD Earth and History of life, Royal Belgian Institute of Natural Sciences, Brussels, Belgium. | ᵈ Unité de Recherches Art, Archéologie Patrimoine, Université de Liège, Liège, Belgium.

How?

  molaR package⁶ in an R environment Which analyses? • Dirichlet Normal Energy (DNE)(curvature measure that quantifies the degree of surface bending) • Relief Index (RFI) (relief measure calculated through the ratio of the 3D surface area to the 2D footprint) • Orientation Patch Count (OPC) (complexity measure that counts the number of similarly oriented regions) DNE RFI OPC


Guillaume Sarthou
email: guillaume.sarthou@laas.fr

Overworld: Assessing the geometry of the world for Human-Robot Interaction

Keywords: Multi-Modal Perception for HRI, Human-Robot Collaboration, Software Architecture for Robotic and Automation

For a robot to interact with humans in a given environment, a key need is to understand its environment in terms of the objects composing it, the other agents acting in it, and the relations between all of them. This capability is often called the geometrical situation assessment and is mainly related to spatial reasoning in time.

In this paper, we present Overworld, a novel lightweight and open-source framework, merging the key features of a decade of research in the domain. It permanently maintains a geometric state of the world from the point of view of the robot by aggregating perceptual information from several sources and reasoning on them to create a coherent world. Furthermore, Overworld implements perspective-taking by emulating the humans' ability to perceive to estimate the state of the world from their perspective. Finally, thanks to a strong link with an ontology framework, it ensures knowledge coherence in the whole robotic architecture. This work is part of a broader effort to develop a complete, stable, and shareable decisional robotic architecture for Human-Robot Interaction.

I. INTRODUCTION

F OR a robot to act on an environment, talk about it, or take decisions in relation to it, one key quality is the ability to reason about it. The robot is not omniscient, indeed, it is limited by the range of its sensors. However, the world is larger than that and can not be limited for example to a single image. This means that the robot has to reason in time, considering the evolution of the percepts. If the robot has no more data on a given object, it can be because it is out of its field of view or because it is now occluded by another one. Considering the presence of humans in the environment, a third explanation could be that the object has been moved by a human. At the difference of robots acting alone, the dynamic of the environment does not only take its origin from the robot's activity. The commonly used paradigm "the perception as an action" used for example in [START_REF] Beetz | Cram-a cognitive robot abstract machine for everyday manipulation in human environments[END_REF] does not hold. The robot has thus to permanently monitor its environment to allow a higher decisional level to react to uncontrollable events.

To facilitate the interaction with humans, making it smoother, more natural, and more efficient, a key capability of the robot at the situation assessment (SA) level is Visual Perspective Taking (VPT). It is defined in [START_REF] Michelon | Two kinds of visual perspective taking[END_REF] as "the ability to predict the visual experience of another agent". By doing so, we are able to estimate if another person can see an object or not. This ability is the first step to implement Theory of Mind (ToM) being "the ability to make inferences about what other people believe to be the case in a given situation" [START_REF] Baron-Cohen | Does the autistic child have a "theory of mind[END_REF]. As shown in [START_REF] Buisan | Efficient, situated and ontology based referring expression generation for human-robot collaboration[END_REF], [START_REF] Dogan | The impact of adding perspective-taking to spatial referencing during human-robot interaction[END_REF], or [START_REF] Trafton | Enabling effective human-robot interaction using perspective-taking in robots[END_REF], such an ability allows among other things the generation of efficient communication avoiding to consider facts or objects estimated as unknown by the agent the robot interacts with, or at least different from its perspective. See [START_REF] Gurney | Robots with theory of mind for humans: A survey[END_REF] for a survey on VPT use in robotics.

Another important challenge in the context of Human-Robot Interaction (HRI), is to make the robot's knowledge shareable with the humans partners. While the robot perceives the elements of the environment in terms of coordinates, humans are more likely to observe an environment in terms of symbolic relations (e.g. "the plate on the table" or "the knife in your hand"). The robot thus has to be able to extract such relations from its perception of the environment. More than allowing verbal interaction, this ability allows the execution and validation of symbolic task planning where for example the goal defined at the symbolic level could be to put the knife at the right of the plate rather than at a precise coordinate.

In this paper, we present Overworld, a novel lightweight, efficient, and open-source framework, merging the key features of a decade of research in the domain of geometric situation assessment for HRI. The main contributions of this work are an advanced geometrical reasoning process independent of the used perception modalities, a strong link with an ontology framework coming with meaning-full symbolic relations extraction, and parallel representation of worlds, all based on the VPT principle.

In §II we briefly discuss related work and how our contribution addresses multiple issues at a time. An overview of the software architecture is then provided in §III before a focus on the reasoning process in §IV and on the symbolic facts computation in §V. Finally, §VI presents results on two different robots performing a benchmark task for HRI and §VII concludes the paper.

II. RELATED WORK

In robotics, bridging the gap between sensing and deliberation (as language or task planning) is a common need. However, as explained in [START_REF] Heintz | Knowledge processing middleware[END_REF], it often consists of an adhoc integration of processing methods for application-specific or even scenario-specific approaches. In an architecture, the design of a hub for sensor fusion and geometric reasoning aims at proposing a more generic and reusable solution. Such a solution is known as symbol grounding. It establishes and maintains a link between what a system can sense and what it can reason about. Coradeschi et al. in [START_REF] Coradeschi | A short review of symbol grounding in robotic and intelligent systems[END_REF] present a review of such solutions. Among the reviewed solutions, we can cite the Grounded Situation Model (GSM) [START_REF] Mavridis | Grounded situation models for robots: Bridging language, perception, and action[END_REF] which proposes an amodal physical representation of the world used to maintain symbolic beliefs about the objects of the environment and the robot's own body.

To pass from a geometric representation to a symbolic one, spatial reasoning [START_REF] O'keefe | The spatial prepositions in english, vector grammar, and the cognitive map theory[END_REF] is used. Such abstraction is required for instance for grounded natural language processing [START_REF] Tellex | Natural language and spatial reasoning[END_REF] during Human-Robot Interaction. For example, [START_REF] Sisbot | Situation assessment for humanrobot interactive object manipulation[END_REF] presented a spatial reasoner to compute symbolic objects location (e.g Box isOn Table ) as well as relations between objects and agents (e.g. Box isVisibleBy Bob).

To systematize the bridging between sensing and deliberation, Heintz in [START_REF] Heintz | Dyknow: A stream-based knowledge processing middleware framework[END_REF] introduces the notion of knowledge processing middleware and draws some requirements. Among them, we can cite the need for a flexible configuration and reconfiguration or to permit the integration of information from distributed sources. In addition, he highlights that the knowledge process has to be decoupled and asynchronous to a certain degree. Indeed, the direct processing of the sensor data has to be done at a high frequency to allow among others filtering or tracking while the symbolic abstraction could be done at a lower frequency. The processing should thus not be seen as a continuous stream whose tempo is given by the sensors. This is particularly true when several sensors are used at the same time where a full process would be triggered at each new data of each sensor. DyKnow [START_REF]Bridging the sense-reasoning gap: Dyknow-stream-based middleware for knowledge processing[END_REF] is an implementation of such knowledge processing middleware which has been used in drone applications.

While previous contributions focus only on the robot representation of the world, the integration of such a process for Human-Robot Interaction brings new challenges to be tackled. Using the notion of VPT, Warnier et al. in [START_REF] Warnier | When the robot puts itself in your shoes. managing and exploiting human and robot beliefs[END_REF] estimate the beliefs of the humans collaborating with the robot to detect belief divergences and to create an estimate of the humans' knowledge bases. However, the proposed solution is simplistic with the use of a single world being the robot representation and with the use of the assumption that when the human is present in the scene he notices every action on all objects and thus knows their new positions even if he cannot see them. SPARK [START_REF] Milliez | A framework for endowing an interactive robot with reasoning capabilities about perspective-taking and belief management[END_REF] extends the previous work with more advanced reasoning on objects' positions trying to find hypotheses (i.e. occlusions) to explain why an object is not perceived anymore. However, even if maintaining separate belief bases, SPARK still works on a single world representation not allowing a genuinely independent representation of the human knowledge about the world. With Underworlds [START_REF] Lemaignan | Underworlds: Cascading situation assessment for robots[END_REF], Lemaignan et al. proposed a new paradigm to develop a geometrical SA for HRI. Through a principle of cascading, Underworlds allows the representation of multiple parallel representations of the world. Thanks to this, it allows to handle and to maintain truly independent models of the environment for each agent and thus allows the representation of proper false-belief situations. However, Underworlds comes more as a toolkit rather than a software even if example clients are available. As stated by the authors, "it does not provide any intrinsic high-level processing or reasoning capability". In addition, the cascading architecture has the side effect to create a continuous knowledge stream triggered by each new data from each sensor. Nevertheless, it allows fast prototyping as shown in [START_REF] Sallami | Simulation-based physics reasoning for consistent scene estimation in an hri context[END_REF] where it has been used to implement simulation-based physics reasoning but not in real-time and without VPT. It is thus mainly used to represent and share multiple parallel representations of the world, to be used by other components.

III. DESIGN AND ARCHITECTURE

Overworld aims at gathering the strengths of the previously presented contributions to propose an amodal and efficient solution in addition to provide meaningful geometrical and spatial reasoning capabilities. In this section, we first present used types and the knowledge stream with regard to the robotic architecture Overworld is integrated into. We then give key details on the specifics of its implementation.

A. Used types

Overworld considers two kinds of entities: the objects and the body parts.

Entity: An entity is defined by a unique identifier which can be a "true identifier" if it is a known identifier in the entire robotic architecture or a "hidden identifier" if it has been automatically created in Overworld. For example, when the robot grasps an object, we can perceive that an object exists in the robot's gripper but we can not identify it. In this latter case, we can use a "hidden identifier". An entity also owns a pose, a small history of its poses and a shape. The shape can be defined by basic geometric volume (cube, sphere, or cylinder) or a mesh (visual and collision mesh), in addition to a color, rendering texture and scale.

Object: An object is a specification of an entity. It can be defined as static and can have a mass. In addition, it can own what we call Points of Interest (PoI). Each PoI is a set of points relative to the object which have been used to perceive the object. Using tags to detect an object, the latter will have a PoI per tag and each PoI will be composed of the points of the four corners of the tag. These PoIs will be used to reason about the object's visibility.

Body part: A body part is a specification of an entity. It is defined by a type (e.g. hand, head, torso, or base), a frame name and the agent name it is related to.

Hand: A hand is a specification of a body part in the way that it can hold objects.

Agent: An agent is composed of body parts. Only a head is required to define it. In addition, it has an identifier, a type (robot or human), and a Field of View1 (FoV).

B. Knowledge stream

Overworld is part of the DACOBOT architecture [START_REF] Sarthou | The director task: a psychology-inspired task to assess cognitive and interactive robot architectures[END_REF]. It is strongly linked to the software Ontologenius [START_REF] Sarthou | Ontologenius: A long-term semantic memory for robotic agents[END_REF], a semantic knowledge base dedicated to HRI. Overworld both updates the knowledge bases and gets from it static knowledge about the entities. This second link allows a uniform representation of knowledge among the entire architecture and eases the configuration by avoiding hard-coded knowledge or the use of multiple configuration files with redundant knowledge.

The knowledge stream represented in Fig. 1 is based on the knowledge processing middleware proposed in [START_REF] Heintz | Dyknow: A stream-based knowledge processing middleware framework[END_REF]. Following the scheme from bottom to top, data coming from sensors or perception processes are sent to Overworld where percepts are first created independently for each sensor. Percepts are temporary and possibly incomplete representations of entities of the world. At this level, it can be enriched by static knowledge coming from the knowledge base. Considering a perception process based on tags, from the tags' ids Overworld can query the KB to fetch the internal unique identifiers (e.g. tag 24 corresponds to table_1) and the visual and collision 3D models to use, the texture or the color to apply, or the entity's mass if some of this information exists. The position of these percepts can be filtered either to smooth noise or to discard data when the sensor has moved for example.

From there, data have been processed in parallel and in an independent way for each input perception process. An assessment loop (detailed in Sec. IV) periodically pulls all the percepts of all perception processes and aggregates them to create entities in the complete representation of the world. For the entities with no data for a few loops, a geometric reasoning process tries to find explanations which can be that the entities are out of the FoV of the sensors or that visual occlusions exist. Once the world is stabilized, spatial reasoning abstracts the representation by generating symbolic facts which are sent to the KB.

In an asynchronous way, the KB runs semantic reasoning to deduce new facts from the coming ones (e.g. if <A, isOn, B> then <B, isUnder, A>) and store them all. Finally, the Supervision or other deliberative processes can either subscribe to patterns of facts (to avoid continuous pulling) or perform direct queries. As detailed in Sec. V, some facts are not required to be computed at each assessment loop and can be computed on demand.

With the described flow, Overworld allows flexible configuration and reconfiguration with the use of the ontology and permits the integration of information from distributed sources as required by [START_REF] Heintz | Dyknow: A stream-based knowledge processing middleware framework[END_REF]. In addition, knowledge processes are decoupled and asynchronous at some levels to avoid overprocessing while fitting the frequency requirement of each level of the architecture.

C. Implementation

Overworld is composed of an assessment process per agent it manages. The main one corresponds to the robot while the others correspond to the humans the robot interacts with. Each process is independent of the others and runs in a dedicated thread. Fig. 2 represents Overworld's architecture with two assessment processes.

A set of perception modules is used to fetch data from the agent's sensors. These modules are plugins of Overworld allowing easy addition of new perceptions capabilities and thus a gain in modularity. A module is dedicated either to perceive objects or body parts. They are responsible for percepts creation and filtering as presented in Sec. III-B. To facilitate its use, Overworld provides three module templates with protection mechanisms. These templates can respectively be used to subscribe to a ROS topic, synchronized ROS topics, and the use of standard C++ callbacks.

While the monolithic structure of SPARK [START_REF] Milliez | A framework for endowing an interactive robot with reasoning capabilities about perspective-taking and belief management[END_REF] provided good performance, the cascading structure of Underworlds [START_REF] Lemaignan | Underworlds: Cascading situation assessment for robots[END_REF] brought powerful modularity. With the plugin solution, Overworld aims at taking the best of both. To select the modules to be used for a given robot or experiment, Overworld considers a single configuration file defining the plugins to instantiate as well as their parameters if needed. Considering for example a plugin to perceive objects using the pressure sensors of a robot's gripper, we can describe in the configuration file that this plugin has to be loaded twice, that one is the left gripper with a given pressure threshold and the other for the right gripper with another pressure threshold.

As illustrated in Fig. 2, the perceptions modules are attached to perception managers. Overworld has an objects manager, a humans manager, and a robot manager. At each assessment loop (detailed in Sec. IV), the managers fetch the percepts of their modules and run an aggregation algorithm to create entities from the percepts and update them with the new data. This step is required as the same entity could have been perceived by several modules. Considering a robot's gripper perception module, it will provide a percept with an hidden id and a position. At the same time, this object is perceived by a tag module. As both percepts are at the same position with comparable volumes, the manager merges them to create a single entity. When the robot moves its arm and no more perceives the object by its tag, the manager still knows which real entity is manipulated and updates its position thanks to the gripper module.

The managers are also responsible for geometric reasoning. To do so, they rely on the Bullet2 [START_REF] Coumans | Pybullet, a python module for physics simulation for games, robotics and machine learning[END_REF] physics engine. Bullet works as a server and allows the creation of multiple parallel worlds. Each assessment process thus owns a world. The three managers of each process update this world with the newly created entities and with the updated data.

Thanks to this 3D representation of the world, the assessment process performs spatial reasoning to compute symbolic facts (detailed in Sec. V) and sends them to the ontology. In addition, for each human perceived by the robot, the robot's assessment process emulates the humans' ability to perceive. To do so, it uses the Bullet world to generate a segmentation image of the world from the point of view of each human. These images are then used to feed the perception modules of the humans' assessment processes. At the difference of [START_REF] Milliez | A framework for endowing an interactive robot with reasoning capabilities about perspective-taking and belief management[END_REF] this emulation and the independent process (and thus reasoning) allows a proper representation and detection of belief divergence between the robot and the humans but also between the humans.

IV. INTO THE ASSESSMENT LOOP

In this section, we focus on the reasoning process performed in the assessment loop. We first detail the geometric reasoning and then present the use of physics simulation.

A. Geometric reasoning

Once the percepts of the modules are fetched, the assessment loop is in charge of updating the agent's representation of the world. To perform this update, once the aggregation is done, we follow for each entity of the world the algorithm represented in Fig. 3.

If the entity has been perceived by one of the modules since the last loop, the entity is created if it was not already and its position is updated. If it has not been perceived, we check if the agent should have been able to perceive it, meaning if the entity is in the agent's FoV. If it is not, we can conclude that this absence of data is normal and we keep the entity's last position. This test is based on the entity's PoIs for those having some (i.e. is a PoI in the FoV?) and is based on the eight corners of its bounding box for those not having PoIs (i.e. is a corner in the FoV?).

In the case the entity is in the agent's FoV, the latter should have perceived it. We thus try to find an explanation for this lack of data. A first explanation could be that the entity is held by an agent. A second explanation could be that the entity is occluded by another one. This latter test differs if the entity has PoIs or not. If it has not, we generate a segmentation image of the scene from the agent's perspective. The entity is thus considered occluded if it does not appear on the image. For the entities having PoIs, we perform a batch of raycasts toward the points composing the PoIs in the FoV. If at least one point of each PoI hits another entity, we conclude that the entity is occluded. Fig. 4 illustrates such a situation with a table perceived with two tags which are occluded by a box. Each tag is a PoI of the table, each having five points. In the image we see two sets of five raycasts, all hitting the box.

When an explanation is found, the entity is kept in the world at its last position. Otherwise, we remove it from the world. The agent thus knows that the entity exists but does not know its current location.

B. Physics simulation

In a dynamic environment and especially with the presence of humans, an object can be moved from a visible position to an occluded one. In the previously studied example, the box had been moved on the tags but what happens if we put the box in front of the block aside and then slide the box backwards? The reasoning process would initially find an explanation but when we would slide the box, in the robot's world the block would "pass through" the box (as we don't update its pose). After that, no occlusion would be found and the block would be removed. The same would appear when we drop an object in a box. The last perceived would be above the box, and since the cube would have fallen, it would thus not be perceived anymore, and thus be removed from the scene as no occlusion would be detected between the camera and the last position of the cube.

Here the issue is that the algorithm does not rely on the physics of the objects. Physics simulation is commonly used in robotics to predict the effects of an action [START_REF] Beetz | Know rob 2.0-a 2nd generation knowledge processing framework for cognition-enabled robotic agents[END_REF] but not used in real-time to understand what is happening. Nevertheless, as previously explained, [START_REF] Sallami | Simulation-based physics reasoning for consistent scene estimation in an hri context[END_REF] has presented a proof of concept for HRI. In our implementation, rather than constantly simulating all the objects, we have chosen to only simulate those for which the search for an explanation is required. This means that we simulate the occluded objects and the ones that we would have been removed otherwise. In the first case, it allows them to react to the movement of other objects like in the example of the slipped box. For the others, it allows to test if their physics could lead them to a position where an occlusion would explain their absence of data. After a few simulation steps, if we still do not find any explanation, then we finally remove them. This addition to the previous reasoning process allows the understanding of more dynamic situations.

V. FROM GEOMETRIC TO SYMBOLIC

We consider two kinds of facts, those computed at each assessment loop and those computed on demand. For all the facts having inverses, Overworld only computes one of them and the other is deduced by the semantic reasoning of the ontology management system. Additional facts could be computed in the future.

A. Continuously computed facts

Overworld continuously computes six facts allowing to trigger higher decisional processes.

isOnTopOf(Object, Support): An object is on top of another if the second is defined as a support in the ontology, the lower z (z vertical) coordinate of its Axis Aligned Bounding Box (AABB) is approximately at the same level as the upper z coordinate of the support, and the projection of their AABB on the xy plane are overlapping. isInContainer(Object, Container): An object is in another if the second is defined as a container in the ontology, the lower z coordinate of its AABB is approximately contained in the z coordinates of the support, and that the projections of their AABB on the xy plane are overlapping.

isInHand(Pickable, Hand): An object is in a hand if it is defined as pickable in the ontology and if it is closer to the hand than a few centimeters. The object is then attached to the hand to follow its movement and is released once it is perceived at a different position from the hand.

isPerceiving(Agent, Agent): An agent is perceiving another agent if at least one of the second agent's body parts is in the FoV of the first agent.

isLookingAt(Agent, Object): An agent is looking at an object if the object is in the segmentation image generated from the agent's point of view.

hasHandMovingToward(Hand, Object): A hand is moving toward an object if the object is in a cone oriented by the mean vector resulting from the N last positions of the hand.

B. On demand facts

Facts with high dynamics and requiring instance computations can be computed on demand. For now, Overworld implements the computation of the facts isAtLeftOf and isInFrontOf. While [START_REF] Sisbot | Situation assessment for humanrobot interactive object manipulation[END_REF] implements a simplified version by only considering these relations between the agents and the objects, Overworld comes with a meaningful implementation of these facts, based on the theory proposed in [START_REF]Perspective taking and ellipsis in spatial descriptions[END_REF].

Following the principle of "thinking for speaking" [START_REF] Slobin | Thinking for speaking[END_REF], Levelt explains that such relations should be computed in a way that they will be understood. These relations can be computed with regard to three systems. The absolute uses the north-south dimension and is neither relative to the speaker's nor the object's coordinate system. The intrinsic perspective system uses the objects' intrinsic axis if some exist. For example, in most cultures, we assume a front and upward axis for a chair or a car. We can thus compute spatial relations using their coordinate system. Finally, the pragmatics perspective systems use the agents' orientation to compute relations. Following the VPT principle, these relations will thus differ depending on each agent. While the absolute system is rarely used, in most cultures, none of the others is dominant over the others.

Levelt also explains that while converseness (i.e. the inverse of a relation) and transitivity hold in the pragmatics and absolute system, they do not in the intrinsic one. From the experiments presented in [START_REF] Levelt | Some perceptual limitations on talking about space[END_REF], we can also notice the principle of canonical orientation explaining that if an object with an intrinsic axis is not in its canonical orientation, it cannot be used as a perspective system. In a simplified definition, an object is in a canonical orientation if its upward axis is aligned with the absolute system's upward axis. In other words, if a chair is placed on one of its sides, it can not be used to refer to another object using the intrinsic system. Finally, as already used in [START_REF] Sisbot | Situation assessment for humanrobot interactive object manipulation[END_REF] with the relation isNextTo, the size of the objects has an impact on the relation computation. Where two houses spaced of 5 meters are next to each other, two pens also spaced of 5 meters are not next to each other. Because the relations isAtLeftOf and isInFrontOf are only used for near objects, their sizes have to be considered. Overworld implements the computation of the relations isAtLeftOf and isInFrontOf in the intrinsic and pragmatic systems by following all the presented principles. While the intrinsic systems are used identically for all the agents' worlds, the pragmatic system is only related to the world owner. However, as all the worlds feed a dedicated ontology, the robot can still use PT by reasoning on the agents' estimated knowledge base.

To make these facts available to the rest of the architecture in a transparent way, Overworld takes advantage of Ontologenius (the semantic knowledge base) by providing a reasoner in the form of a plugin. The reasoner runs before each query to Ontologenius and if the query involves relations computable by Overworld, it computes them on demand before answering the query 3 .

VI. RESULTS

Overworld has been successfully integrated into the DA-COBOT robotic architecture and tested on two robots (Pr2 and Pepper) to pass the Director Task [START_REF] Sarthou | The director task: a psychology-inspired task to assess cognitive and interactive robot architectures[END_REF]. The implementation on Pr2 has been used with object detection based on tags and the robot grippers, and human detection based on external motion capture. The implementation on Pepper has been used only with tags for the objects and computer vision [START_REF] Khalidov | Real-time multiple head tracking using texture and colour cues[END_REF] to detect the humans.As illustrated in Fig. 5, the test of the Director Task is interesting to assess the usability of such a SA software as it requires a meaningful and stable computation of facts to generate and understand verbal instructions, as well as precise position management to allow the robot to effectively grasp objects. In addition, to generate efficient communication, this task requires ToM and thus VPT to estimate the others' knowledge about the world.

To go further, we proposed two new benchmark situations4 .

• The first consist of the robot picking a block and then dropping it in a box. The human partner then reverts the box to make the block fall on the table before sliding the box (and thus pushing the block) until this latter falls on the ground. This first situation evaluates the percepts' aggregation and the geometric reasoning with physics simulation. Indeed, initially, the robot perceives the block with a tag modality. Once picked, the tag becomes hidden and the robot has to use its gripper sensors to detect that the object is in the gripper. Finally, for the rest of the manipulation, the block is no more perceived and its position has to be estimated with geometric reasoning with physics simulation. • The second situation consists of two humans around the DT setup. One is on the robot's side while the other is in front of the robot. The second human then moves to the robot side to discover a previously hidden block. This second situation mainly evaluates the software scalability (with 13 objects and 2 humans) as well as the VPT. This means that three worlds have to be maintained simultaneously and that two perspectives have to be computed. Both Overworld and Underworlds have been tested against these benchmarks using a rosbag to provide them with identical inputs. As Underworlds is a toolkit not implementing high-level reasoning, we used the implementation developed to prototype Overworld. This implementation is based on an existing one [START_REF] Sallami | Development psychology inspired models for physical and social reasoning in human-robot joint action[END_REF] proposed by Underworlds's authors to ensure a correct software architecture. The tested Underworlds implementation integrates reasoning on objects without physics simulation. We have to note that Underworlds messages exchanged between worlds had to be modified to support PoI. The same symbolic facts were computed by both software. SPARK had not been tested as it is no longer available with features shown in its related works. As explained in [START_REF] Lemaignan | Artificial cognition for social human-robot interaction: An implementation[END_REF], it was able to run at 10Hz without physics simulation.

The first benchmark is passed by Overworld at 20Hz with a correct object position estimation and VPT. Underworlds runs it at 8Hz with several objects disappearances due to difficulties to synchronize data. Furthermore, once picked by the robot, the block position was no more estimated. The second benchmark is passed by Overworld and Underworlds respectively at 13Hz and 3.5Hz. Both positions' estimations were correct as the robot is only a spectator of a static situation and uses a single perception capability. We see that Overworld is at least twice faster as Underworlds and is less impacted by scaling up the complexity. These results are summarized in Tab. I.

Additional results are available in the attached video 5 .

VII. CONCLUSION

In this paper, we have presented Overworld, our geometrical SA system for HRI. We exposed how its architecture and its integration into a robotic architecture allow to build a coherent knowledge processing middleware with a strong and bidirectional link to an ontology easing the system configuration and adding semantic reasoning on top of the spatial reasoning. The configurable plugin system has been shown to facilitate the system extension with new perception capabilities and to allow its use on several robotic platforms. For the community to use it, some can either implement their own perception modules or take existing ones to match their robot perception capabilities and then take advantage of all Overworld reasoning mechanisms.

With the current contribution, we have also highlighted advanced reasoning capabilities both in terms of geometrical reasoning to create a coherent world and in terms of meaningful symbolic relations extractions. Finally, we have demonstrated that Overworld can maintain several parallel representations of the world to implement ToM through the use of VPT. In addition to the ability to emulate the human ability to perceive, we have shown that Overworld allows a proper false-belief representation.

Even if we have successfully gathered decades of research on SA for HRI, Overworld could be brought far ahead in future work. Where some reasoning processes focus on the agents' FoV underlying the use of a single visual sensor, we want to integrate a fine representation of the agents' sensors and integrating them into the reasoning process. Such addition will lead to a finer representation of the used modules to perceive a given entity allowing more precise reasoning.

For now, Overworld uses the convenient and efficient 4x4 transformation matrix to represent the entities' position in the world. However, we could gain with explicit management of uncertainties especially when the same entity is perceived by several perception modules at the time. We thus plan to integrate a position representation with a covariance matrix for the next developments.

In an effort to share this work with the robotics community, a website 6 and the code7 are available.

Fig. 1 .

 1 Fig. 1. A partial view of the knowledge stream among the robotic architecture using Overworld. Blocks with a dot of the top-left corner are asynchronous from the block below. The dotted arrow from the Knowledge Bases (KBs) to the Situation Assessment (SA) represents on-demand abstraction. The dashed arrow from the SA to the KBs represents queries about the entities to create.

Fig. 2 .

 2 Fig. 2. Schema of Overworld architecture considering one human. Blue boxes represent perception modules as plugins. They feed the grey boxes (being perception managers) with percepts which are then used to create a representation of the world. The human's perceptions modules are emulations of the human ability to perceive in the robot's world.

Fig. 3 .

 3 Fig. 3. Diagram of the geometric reasoning algorithm to manage the objects' positions. Explanations can be occlusion or grasp detection.

Fig. 4 .

 4 Fig. 4. Third-person view of the robot world in the Bullet GUI. Red lines are a debug visualization of raycasts performed to detect occlusions between the robot's sensor and the PoIs.

Fig. 5 .

 5 Fig. 5. A third-person view of the real setup from the robot side (left image) and the corresponding estimation of the human's representation of the situation (right image). The robot has estimated that the human cannot know that a block exists in the left-top box (red circle).

TABLE I SUMMARY

 I OF COMPARISON BETWEEN OVERWORLD AND UNDERWORLDS OVER TWO BENCHMARKS.

			Situation 1			Situation 2
		Frequency	Positions before grasping	Positions during grasping	Positions after dropping	Frequency	Positions	Perspective estimation
	Underworlds	8 Hz	Fluctuating	Correct	Missing	3.5 Hz	Fluctuating	Correct
	Overworld	20 Hz	Correct	Correct	Correct	13 Hz	Correct	Correct

For now the FoV only represents a camera placed on the agents' head. Further development should rather describe the agents' sensors and thus not requiring robot's head.

Overworld uses a custom C++ bullet API based on PyBullet.

For efficiency issues, Overworld is not requested to recompute the facts under a given duration corresponding to its assessment loop.

Available at https://gitlab.laas.fr/gsarthou/overworld benchmark

https://youtu.be/LUkjts8UacI

https://sarthou.github.io/overworld

https://github.com/sarthou/overworld

ACKNOWLEDGEMENT

The author wants to thank Guilhem Buisan for his work on the implementation of this software.

This paper was recommended for publication by Editor Gentiane Venture upon evaluation of the Associate Editor and Reviewers' comments. This work was supported by the Agence Nationale de la Recherche DISCUTER project ANR-21-ASIA-0005.