Antoine Rioux

Claudia Esteves

Jean-Bernard Hayet

Wael Suleiman

Cooperative SLAM-Based Object Transportation by Two Humanoid Robots in a Cluttered Environment

In this work, we tackle the problem of making two humanoid robots navigate in a cluttered environment while transporting a very large object that simply can not be moved by a single robot. We present a complete navigation scheme, from the incremental construction of a map of the environment and the computation of collision-free trajectories to the control to execute those trajectories. We present experiments conducted on real Nao robots, equipped with RGB-D sensors mounted on their heads, moving an object around obstacles. Our experiments show that a significantly large object can be transported without changing the robot's main hardware, and therefore enacting the capacity of humanoid robots in real-life situations.

I. INTRODUCTION

One of the advantages of having arms on a robot is that it can manipulate a load. This capacity can be useful for a wide range of actions, including transporting objects from one place to another. This can be particularly useful for automated construction and rescue missions situations. However, using one robot only, the maximum payload is generally low and the size of transported objects limited. One way to deal with this issue is to distribute the weight to multiple robots. In this work we deal with the problem of having two humanoid robots cooperating to handle a bulky object among obstacles.

This problem has been tackled in previous works using mainly two approaches: (1) a leader-follower control, or (2) a synchronized control. In the first approach [START_REF] Inoue | Cooperative transportation system for humanoid robots using simulation-based learning[END_REF] [2], one of the robots, the leader, based on its position and its surrounding, computes the plan for the system or is directly guided by a human operator. The second robot follows the leader. This technique is easy to implement, but does not allow closed-loop cooperation easily, because, as the follower only responds to the leader movement when it has already started, a significant time delay is introduced.

Most human-robot cooperative system use this approach [START_REF] Yokoyama | Cooperative works by a human and a humanoid robot[END_REF] [4] since these problems are alleviate by adding a human into the loop, which instinctively correct both micro and macro errors in the system.

In the second approach [START_REF] Stephen | Cooperative humanoid stretcher manipulation and locomotion[END_REF] [6], an external centralized controller computes the motion for all the robots simultaneously, based on the information of the environment provided by the robots. As a result, the synchronized motions can start and A. Rioux and W. Suleiman -Electrical and Computer Engineering Department, Faculty of Engineering, University of Sherbrooke, Canada; C. Esteves -Department of Mathematics. Universidad de Guanajuato, México; J-B. Hayet -Centro de Investigación en Matemáticas (CIMAT), Guanajuato, México. {antoine.rioux, wael.suleiman}@usherbrooke.ca, {cesteves, jbhayet}@cimat.mx Fig. 1. The Nao robots holding an object together stop together. However, both synchronization system rely on "step synchronization" instead of position synchronization. While easier to synchronized, this process slows down the system significantly and increased lateral instability as the entire system swing from side to side in harmony. Furthermore, the transported object is held rigidly and cannot be articulated to improve the footprint and motions of the system. In this work, which belongs to the latter approach, a framework for synchronized cooperative autonomous humanoid robots navigating in a cluttered environment while manipulating an object in closed-loop is presented.

Our main contributions are: (1) a low dimensional multirobot motion planning algorithm to find an obstacle-free trajectory using a map of the environment autonomously constructed by the robots, (2) a continuous and consistent odometry system integrating the robots visual data and actuators information, (3) a synchronization strategy that uses the projection of the robots, and (4) an efficient real-time wholebody control scheme that generates the motions of the closedloop robot-object-robot system. The remainder of the paper is organized as follows: Section II presents the proposed planning algorithm. In Section III, a brief description of how the map of the environment is constructed is given. Section IV details the proposed synchronization approach and Section V describes the control scheme to execute the planned trajectories. Finally, in Section VI, results of simulation and real world experiments are presented and discussed.

II. PLANNING A VALID PATH

To navigate through a cluttered environment, the computation of a collision-free path for both robots is essential. For this, we chose a lattice-based graph planning with an ARA* search [START_REF] Likhachev | ARA*: Anytime A* with provable bounds on sub-optimality[END_REF], motivated by the use of motion primitives ensuring feasible robot-object-robot configurations and transitions. The environment is modelled by a 2D grid cost-map that discriminates obstacles from free space at a fixed threshold and allows obstacles inflation to increase the security margin.

A. State representation

Each node of the search graph needs a representation of the complete robots-object state. To achieve this, it is possible to model the state in

R 2 × S 1 × R 2 × S 1 × R 2 × S 1 : s = (x r1 , y r1 , θ r1 , x ob , y ob , θ ob , x r2 , y r2 , θ r2 ), (1)
where x ri , y ri and θ ri (i = 1, 2) are the positions and orientation of the i-th robot, and x ob , y ob and θ ob are those of the object. As the working space of our robot's arms is too small to fully take advantage of both rotation and translation, the system is simplified by setting a pivot point at the middle of the pair of hands for both robots shown in Fig. 2. The closed-loop grasping of the robot on the table is shown in Fig. 1. The pivot points positions maximize the rotation range within the robot workspace, resulting in a smaller 5 dimensions state space R 2 × S 1 × S 1 × S 1 : Even though the above simplification removes the ability of the object to translate on the plane, the robot retains enough manipulability to minimize the robot-object-robot collision area around obstacles. The simplified state representation of equation ( 2) is shown in Fig. 3.

s = (x r1 , y r1 , θ r1 , θ ob , θ r2 ). (2)
In a lattice-based graph planner, transitions between nodes are triggered by actions chosen within a finite fixed-set of motion primitives. An important feature of the lattice representation is that all connections are feasible paths. Therefore, it is really suitable for highly constrained systems, such as a system of two robots transporting an object, in contrast to other commonly used forms of graph search, including Von Neumann or Moore neighborhood.

The set of motion primitives used for this problem is showed in Fig. 4. It includes (a) forward, backward, sideway motion and every diagonal motion, (b) rotations around each robot and the object center and special movements such as (c) C-turns and (d) S-turns. The last two are more complex and make use of the hands articulations to increase agility around obstacles. Executing complex motions specific to a system in a coherent and logical way is the main reason we use motion primitives over homogeneous sampling of the system DOFs. The joints of the system that allow these configurations are the aforementioned pivot points.

B. Path Cost Function

The cost of a transition from state s to s is based on the time to execute that transition and is computed as follows:

g(s, s ) =    √ (∆x r1 ) 2 +(∆y r1 ) 2 ṙ1 + × DF if ∆x r1 = 0 or ∆y r1 = 0 √ (∆x r2 ) 2 +(∆y r2 ) 2 ṙ2 + × DF otherwise ( 3
)
where ∆x ri , ∆y ri are the variations of the x and y coordinates of the i-th robot pelvis, between states s and s , DF is a difficulty factor associated with each primitive, ṙ+ is the robot maximal linear velocity. This ratio gives us the approximate time to execute the primitive. Since the state representation does not contain any information about the position of the second robot, we must determine ∆x r2 and ∆y r2 with the first robot position. To do so, we compute the projection of the object by rotating the first robot by θ r1 , given a hand rotation of θ ob . Then, we project the second robot around the object given a hand rotation or θ r2 .

The difficulty factor DF is a special value set by the system expert in order to prioritize or penalize certain motions. For example, turning in place then going forward takes longer to execute than moving in diagonal. For relatively long distance, however, moving in a straight line has a smaller trajectory footprint, is more natural looking, minimizes walking oscillation and causes less drift and slippage than moving in diagonal. For those reasons, all the diagonal primitives have higher DF than turning in place and moving forward. The difference in cost will still, however, favor a diagonal movement where turning in place is not worth it, i.e. for short diagonal movements.

C. Search Algorithm

A* is one of the most popular search algorithms. In addition to the use of a path cost function, a heuristic biases the search towards the most promising states. Even though A* is optimal when it finds a solution, that solution does not always exist or cannot be found within a reasonable time. The Anytime Repairing A* (ARA*) focuses on delivering a suboptimal solution as fast as possible; this solution is then optimized iteratively within a predefined limited time. Also, the states are expanded from goal to start, so that the heuristic costs remain valid after replanning and do not need to be recomputed. The cost function takes the form of:

f (s, s ) = g(s, s ) * max(Cost cells (s, s ))+ h, >= 1 (4)
where g(s, s ) is the path cost of equation ( 3), h is the heuristic that uses a 2D grid containing all the Dijkstra distance costs from the goal to the start states and

Cost cells (s, s ) =      1 free space 2 to 99 inflation ∞ obstacles (5)
includes the cost of cells between s and s . The search is biased towards states closer to goal and returns a solution that is, at worst, times the cost of the optimal solution. The inflation is a zone around obstacles where all the cells have a higher cost. It is used as a security margin to bias the search farther from obstacles and reduce danger of collisions. It can be set as a decreasing gradient from the obstacles or a fixed value in the range above.

III. SIMULTANEOUS LOCALIZATION AND MAPPING

To navigate in a cluttered environment, robust and precise sensing is primordial to determine the position of obstacles, detect collisions and to plan valid paths. Also, odometry drift must be constantly corrected by an accurate localization to ensure that planned paths are closely followed. However, the Nao robot has only two cameras in its head for sensing and using them for localization has proven to be a very difficult task [START_REF] Oßwald | Learning reliable and efficient navigation with a humanoid[END_REF], because of the robot sway motion and the low resolution pictures. Furthermore, in our case, the field of view of the Nao is greatly obstructed by the object being transported and by the other robot.

A. Real-Time Appearance-Based Mapping (RTAB-Map)

We chose to add a depth camera on the top of each Nao's head for mapping [START_REF] Maier | Real-time navigation in 3D environments based on depth camera data[END_REF], based on RTAB-Map [START_REF] Labbe | Online global loop closure detection for large-scale multi-session graph-based slam[END_REF], [START_REF] Labbe | Rtab-map project on ros[END_REF]. RTAB-Map provides a robust odometry system based on visual information. It can also create 3D maps of the environment as well as constructing a 2D occupancy grid map by projecting the obstacles on the ground plane.

B. Odometry fusion

A problem that occurs when using the visual odometry produced by RTAB-Map is that it may lose track of the position for multiple reasons, such as lack of detected features in the observed environment, rapid movements of the camera or intense oscillations. When this occurs, we could go back to where the tracking was lost, but this is not efficient and may even be impossible. For this reason, a fusion of the visual odometry, the robot's internal odometry and the error between those reference frames is used to improved the overall odometry.

Since the camera is rigidly linked to the robot by a transformation T v a , we can write

T o a = T v a * T o v
, where T o a , T o v are the homogeneous transformation matrix between the map frame and, respectively, the robot frame and the camera frame. The previous equation can be rewritten to include the encoders-based robot odometry T o r , that does not take into account slipping, drift and other real world errors,

T o a = T v a * T o v * T o r -1 * T o r = T v a * T r v * T o r , (6)
where T v r is the error between the encoders odometry and the visual odometry. Since this equation only holds while the visual odometry is valid, the last valid T r v at time t = t lost is used when a loss occurs at t = t lost ,

T o a (t) = T v a * T r v (t) * T o r (t) if T o v exists, T v a * T r v (t lost ) * T o r (t) otherwise. ( 7
)
This approach consistently provides smooth odometry. Even when the visual information is abruptly discontinued, it continues to generate sufficiently accurate localization data until an adequate image or a reset command is processed by RTAB-Map and the visual odometry is restored.

IV. SYNCHRONIZATION A. Object Stability and Hand Stabilization

At low speed, a humanoid robot CoM moves horizontally from one support foot to the other in order to stay in balance. This lateral motion causes the entire upper body to oscillate laterally at an amplitude proportional to the distance between the center of its feet, which, in our case, causes the transported object to move by the same amplitude.

Since the object to carry is fully controlled by the robots' hands, it is possible to reduce this effect so as to improve the closed-loop stability. On the one hand, if both robots swing at the same time, synchronization is maintained without effort, but the object and anything on it would swing dangerously. On the other hand, if the robots swing in any other way, the force generated by this movement will be transmitted to the other robot and cause instability or fall.

Our solution to compensate this instability without changing the walking gait is to use the robots' hands and keep them at a fixed position in space, relatively to the planned trajectory. This position is determined at the starting position of the robot and corresponds to the transformation between feet and hands. Those transformations will be the input of the whole-body control scheme described in Section V.

B. Synchronized trajectories

Even if both robots are independent and receive their own trajectory to follow, these trajectories are lined up in such a way that they are at a constant distance from each other. However, real robots being imperfect, do not necessarily move at the exact same speed given the same command. For this reason, the trajectory is segmented into waypoints, each waypoint being a state of the graph plan. To progress to the next waypoint, both robots have to agree that they are close enough to their current waypoint in term of position and orientation. If only one robot is near its waypoint, it will slow down while converging to it, until the other robot agrees that they can proceed to the next waypoint.

C. Synchronized projections

Now that the whole system is more stable with regards to the individual swing added by both robots, the position of each robot needs to be synchronized with the other one along the planned trajectory. To do so, the projection of each robot with respect to the other is computed by using their respective hands, world frames and transported object properties. First, the position of the center of the object with respect to a robot i can be found by:

T ri ob = midpoint(T ri hr * T hr ob , T ri h l * T h l ob ) (8)
where T ri ob , T ri h (r,l) , T h (r,l) ob are respectively the current transformations between the robot and the object frames, the robot and its hands and the object center. The function midpoint computes the midpoint of two points.

With these transformations, we find the projection synchronization position for each robot as follows:

T o pi = T o rj * T rj ob * T ri ob -1 for i, j ∈ {1, 2} : i = j, (9)
where T o pi is the projected robot i position in the world frame and T o r i c is the odometry data from the other robot i c . Other points different from the center could be used instead, such as the pivot points. However, a constant offset transformation would need to be taken into consideration in the previous equation. We can finally compare this projected position to the actual position of each robot in order to find the projection synchronization error e pi

e pi = T o pi * T o ri -1 . (10)

D. Synchronization as Kinematics tasks

Our objective is to express the synchronization between the two robots as kinematics tasks that can be solved using a whole-body control scheme:

• The hands synchronization can be expressed as a kinematics task on the hands positions of each robot. • The synchronized projection can be expressed as a kinematics task on the chest frame of each robot. More precisely, this kinematic task is on the horizontal positions (x and y) and the orientation around the vertical axis (z) of the chest frame.

V. CONTROL Once a collision-free trajectory is found by the ARA* algorithm, a set of footprints are defined along the trajectory as shown in Fig. 5. The second step is to define a Zero Moment Point (ZMP) trajectory. A trajectory of the Center of Mass (CoM) of the robot is then obtained using the preview control algorithm proposed in [START_REF] Kajita | Biped walking pattern generation by using preview control of zero-moment point[END_REF]. This algorithm, widely used in humanoid robotics, is simple to implement, yet efficient and yields a smooth CoM trajectory by minimizing the CoM jerk trajectory. The feet trajectories are obtained by spline interpolation between the footprints and the hands trajectories and orientations are defined in order to minimize the walking swing effect as well as to follow the object orientation. To obtain the joint trajectories for each humanoid robot, a whole-body control scheme with prioritized tasks is formulated as follows:

Initial position and orientation

min q qT Q q subject to First priority    J c q = ṙc J lf q = ṙlf J rf q = ṙrf Second priority    J lh q = ṙlh J rh q = ṙrh J ch q = ṙch
Joint velocity limits q-≤ q ≤ q+ [START_REF] Labbe | Rtab-map project on ros[END_REF] where q ∈ R n is the joint velocity vector, Q is a positive semi-definite matrix, J c ∈ R 3×n , J lf ∈ R 6×n , J rf ∈ R 6×n , J lh ∈ R 6×n , J rh ∈ R 6×n , J ch ∈ R 3×n are the Jacobian matrices of CoM, left foot, right foot, left hand, right hand and chest, respectively. ṙc , ṙlf , ṙrf , ṙlh , ṙrh , ṙch are the linear and angular velocity of CoM, left foot, right foot, left hand, right hand and chest, respectively. Since we are only interested in the horizontal velocity and angular velocity around the vertical axis for the chest frame, ṙch ∈ R 3 , and qand q+ are the joint velocity limits.

The optimization problem ( 11) can be transformed into a standard Quadratic Programming (QP) problem [START_REF] Rioux | Humanoid navigation and heavy load transportation in a cluttered environment[END_REF], which can be solved in real-time by using an appropriate QP solver.

A. Dynamic collision avoidance and Replanning

It is important to check frequently for collision in case an obstacle has moved from its original position, the robots drift away from their planned trajectory or the synchronization process of the robots requires a replanning of the whole trajectory. Hence, the future trajectory is always monitored for potential collisions with obstacles in the 2D occupancy grid. When a collision is foreseen, a replanning is necessary and the trajectory is deformed as shown, for the purpose of clarity for a single robot, in Fig. 6. Even though, at first glance, the support polygon seems increased by including the robot-object-robot closed loop, the robots' support polygons are still defined by the contact between the feet and the ground. This is because the robots' arms are not fully bended, therefore the robots could fall forwards or backwards.

The new collision-free trajectory is found by the ARA* algorithm from the goal to the point at which the collision has been predicted. If the potential collision is due to drift and the environment has not changed, the Dijkstra grid does not need to be recalculated, therefore greatly accelerating the replanning. As the humanoid robot walking pattern cannot be changed instantly, a time interval t c is required to change the planned footprints. In the implementation of ZMP preview control, a finite time horizon of 2 steps is used to compute the CoM trajectory. Therefore, if a collision is foreseen at instant t c , the new collision-free trajectory provided by the ARA* algorithm is deformed to keep the next two footprints unchanged as shown in Fig. 6. The robot will however stop if the deformed trajectory is in collision. Contrary to what Fig. 6 might suggest, 2 steps do not represent a significant distance. Since the robots are very small and the feet tend to slide, 2 steps are only a few centimetres or less.

VI. RESULTS

Experiments were conducted on Nao humanoid robots (Fig. 1), manufactured by Aldebaran Robotics [START_REF] Gouaillier | Mechatronic design of NAO humanoid[END_REF]. Their dimensions are 573mm of height, 311mm of width and 275mm of depth for a total weight of 5.2kg. The two arms as well as both legs have 5 DOF each, while the head has 2 DOF and the pelvis and hands have 1 DOF each. An IMU provides odometry data and 36 magnetic rotary encoders give joint angle information with a precision of 0.1 • . On top of their heads, we have added an Asus Xtion Pro Live consumer-level depth camera (see Fig. 1).

A. Articulating the arms

Without any correction, the average oscillation peak-topeak position movement of the hands is 47.6 mm. However, with the whole body control, the average hand distance from desired position has been reduced to 16.4 mm, reducing the hand error by 65.5%. As a result, significantly less oscillations are transmitted to the table, leading to a safer and enhanced carrying ability and load stability. However, the error cannot be completely cancelled, this is mainly because: I) the hands trajectories are second priority tasks, II) Nao has only 4 DOFs in each arm.

B. Navigating in a cluttered environment

To test the system as a whole and to validate the proposed algorithms, we conducted two series of 5 experiments. In each experiment, the robots starting and goal positions are chosen in such a way that the robots have to navigate among objects on the ground. Each of them serves as an obstacle to be avoided by the robots and the object. They are placed to form various feasible paths and force tight turns in order to take advantage of the additional degrees of freedom (the rotation of the object θ ob and θ r2 ). The two series include the same experiments, except for one series that has the active projection correction and the trajectory synchronization while the other has only the latter. Fig. 7 shows the start and end positions for each type of experiment. In this figure, the obstacles are in yellow, while the red areas around them are inflation zones where the cost is higher than in free space, to prevent the robots from passing too close to obstacles. These zones are used as a security buffer and the center of the robots and the object should avoid, if possible, planning to pass inside it. The cyan zone is a forbidden zone, because if the center of the object or the robots enters it, it means that an edge is in collision with an obstacle.

The ARA* planner parameter initial value = 3 means that the suboptimal solution cannot be worse than 3 times the optimal solution cost. A time limit of 5 seconds was chosen and within that time, was successfully decreased to 1 on every run, which corresponds to the optimal solution. For each generated path, we measured the total time to execute the trajectory, the trajectory length, the initial solution and optimal solution times. These results are summarized in Table I. We observe that the average speed is nearly the same, which is surprising because it was expected to be significantly slower with the projection synchronization. Indeed, for the robots to properly stay synchronized, they try to match their speed and relative position while following their respective trajectory. This means that the fastest robot slows down to accommodate the other while the slowest tries to speed up. Since the difference in average velocity is only about 1.8%, it could also be explained by other factors, such as different battery level and motors temperature.

The slowest robot was most likely already slowing down the fastest one by not agreeing to pass the waypoints, forcing a slow down. The projection synchronization forced the slow one to move through these waypoints faster, causing the slow down on the fastest to be mostly cancelled by these faster waypoint transitions.

Even though in our case the trajectories are quite small, the use of sub-optimal solutions has proven to be significantly faster. Indeed, for our experimental settings, it is about 12.5 times faster to compute the solution for = 3 as opposed to the optimal solution. This could be especially useful for real life large scale distances and experiments where quick reactions and planning are necessary.

In Table II The error is significantly reduced on every axes for both robots. More importantly, the errors in Y and Yaw converge to zero. However, we still have some error in the X axis, because it is the main direction of movement and the system wants to follow the planned trajectories as a main priority. The focus here is to reduce the errors, but not necessarily cancel them completely if it means that it will prevent normal motion or highly impair it. As shown previously, the average velocity barely changes, while the error reduction has been greatly improved. It is important to note that the ground in the testing room had sufficient texture and patterns to produce reliable data for the SLAM library. When tested on a plain ground, RTAB-Map could not, however, extract enough features for visual localization with the obstacles only. With the table linking the too robots together, significant drift cause by the visual odometry imperfection can make the Nao drift when navigating as shown in Fig. 8.

VII. CONCLUSION AND FUTURE WORK

In this paper, we have proposed a system capable of carrying a long table with two humanoid robots while navigating in a cluttered environment; we also gave practical insights into the implementation of the proposed approach on a real humanoid robot. When moving throughout the environment, a depth camera and a SLAM library map the obstacles in real-time and provide a visual odometry. This information is then fused with each robot odometry to provide a consistent, continuous and reliable odometry data. Moreover, by controlling the hands adequately by using a whole-body control scheme, we were able to articulate the object in tight turns and to significantly reduce the lateral swing from propagating to the object and between robots.

In future works, in order to make the system more reactive and human-like, the arms should be used to absorb a part of the error instead of having to quickly move in fear of being unbalanced. In addition. the cameras in front of the Nao could be used to look at the other robot's relative position, instead of relying on hand position. As it is now, each robot can never really verify its relative position with respect to the other robot, and, as a result, it cannot detect drift errors in the visual odometry.

Fig. 2 .

 2 Fig. 2. Top view: Pivots position

Fig. 3 .

 3 Fig. 3. Simplified state representation.

Fig. 4 .

 4 Fig. 4. Set of possible motion primitives.

Fig. 5 .

 5 Fig. 5. Overview of the motion planning procedure

Fig. 6 .

 6 Fig.6. Replanning in case of collision detection: tc is the instant at which a collision is foreseen, the new collision-free trajectory is in dashed-blue line, the deformed trajectory is in red line.

Fig. 7 .

 7 Fig. 7. Map of the starting and ending positions with obstacles, lethal and security inflation zones around them, the Nao and object footprints and goal position/orientation.

Fig. 8 .

 8 Fig. 8. Snapshots of the Naos navigating with an object

TABLE I

 I

	STATISTICS ABOUT THE EXPERIMENTS

  , the errors in X, Y and Yaw for both robots were measured at 10 Hz during each experiments.

			Robot 1			Robot 2
		X	Y	Yaw	X	Y	Yaw
	No proj (m)	0.032	0.024	-0.009	0.033	0.0175	0.009
	No proj Std (m)	0.0025	0.007	0.013	0.002	0.011	0.013
	Projection (m)	0.010	0.003	0.006	0.011	0.007	-0.006
	Projection Std (m)	0.001	0.004	0.0125	0.002	0.007	0.0125
	Error reduction (%)	68.75	87.50	33.33	66.67	60.00	33.33

TABLE II ERRORS STATISTICS

 IISTATISTICS

ACKNOWLEDGMENT

This research is supported by Natural Sciences and Engineering Research Council of Canada (NSERC), and partially by a collaborative research project funded by "XIVe GROUPE DE TRAVAIL QU ÉBEC-MEXIQUE" and a Mitacs Globalink research internship.


Antoine Rioux

Wael Suleiman

Humanoid Navigation and Heavy Load Transportation in a Cluttered Environment

Although in recent years several studies aimed at the navigation of robots in cluttered environments, just a few have addressed the problem of robots navigating while moving a large or heavy object. This is especially useful when transporting loads with variable weights and shapes without having to change the robot hardware. On one hand, a major advantage of using a humanoid robot to move an object is that it has arms to firmly grasp it and control it. On the other hand, humanoid robots tend to have higher drift than their wheeled counterparts as well as having significant lateral swing while walking, which propagates to anything they carry. In this work, we present algorithms for a humanoid robot navigating in a cluttered environment while pushing a cart-like object. In addition, the algorithms make use of the hands and arms to articulate the cart when executing tight turns using whole body control scheme to reduce the lateral swing effect on the load and ensure a safe transport. Experiments conducted on a real Nao robot assessed the proposed approach and algorithms, they show that the payload of a humanoid robot can be significantly increased without changing the humanoid robot's hardware, and therefore enact the capacity of humanoid robots in reallife situations.

I. INTRODUCTION

One of the advantages of having arms on a robot is that it can carry a load. This capacity can be useful for a wide range of actions, including transporting objects from one place to another. However, the maximum payload is generally pretty low and generates a lot of instability if it is held at the arm's length. While it is possible to increase the strength of the motors in the legs and arms, it is not the best solution since a motor's power is proportional to its size, weight and price. Instead of putting the entire load directly on the robot, a cart-like object can be used to help supporting the weight. The cart can then be pushed by the robot and moved around more easily without having to modify the robot's hardware to fit the load.

Many researches have been done on navigating robots in a cluttered environment, but adding a controllable object supporting a heavy load and navigating it safely has not been widely examined. Furthermore, using a humanoid robot, which has unstable balance and great motion swinging, increases the difficulty of the task in hand. The objective is to plan a stable and safe trajectory that uses a whole body control scheme.

To address the problem of navigating a cart-like object supporting a load with a humanoid robot in a cluttered environment, the following sub-problems should be solved: Antoine Rioux and Wael Sulieman are with Electrical and Computer Engineering Department, the Faculty of Engineering, University of Sherbrooke, Sherbrooke, Canada {antoine.rioux, wael.suleiman}@usherbrooke.ca (I) planning, (II) sensing and (III) controlling the cartlike object. To plan a safe trajectory between obstacles, an anytime search-based planner exploits a given set of motion primitives that consider both the robot and the cart footprint. The second step is to find the humanoid robot's footprints, the humanoid's feet and hands trajectories are then computed in order to minimize the swing effect and follow the cart trajectory using a task priority whole body control scheme.

The main contribution of the paper is providing a framework for humanoid navigation in a cluttered environment while manipulating a cart-like objet that carries a heavy load.

This paper is organized as follows. Section 2 presents an overview of related works. Section 3 describes the robotcart model and the implemented algorithms. In Section 4, simulation and real results are presented and analyzed.

II. RELATED WORK

Many studies have been done on robots moving objects to a specific goal. Most of them though are executed with multiple wheeled robots that position themselves around the object to push it in the desired direction. One of the first examples of this behavior for a movable target tried to reproduce how ants move a prey bigger than them by trial and error [START_REF] Kube | Cooperative transport by ants and robots[END_REF]. Another added the possibility of pulling the object by using a rudimentary one degree of freedom arm [START_REF] Wang | A multiple robot system for cooperative object transportation with various requirements on task performing[END_REF], while [START_REF] Yamashita | Motion planning of multiple mobile robots for cooperative manipulation and transportation[END_REF] have the additional benefit of being able to manipulate an object in 3D when necessary, instead of 2D. In these researches, the manipulator comes in contact with the object at only one point, which barely allows any control over the object while moving and manipulating it. Holonomic wheeled robots are indeed less complex to control than humanoids, they are mainly useful at sliding box-like object on the ground. Therefore, a robot with humanoid arms allow a better control of the structure of an object and is more suitable to control cart-like objects with loads.

Other works have explored the usage of humanoid robots that push objects while keeping a firm grip on the handles. In theory, two arms are enough to fully constrain and control all the degrees of freedom of a cart. It was demonstrated that Honda's ASIMO is capable of moving a large cart in rooms and hallways [START_REF] Shigemi | Development of new ASIMO[END_REF]. However, the cart is mostly controlled as a Dubins car instead of taking advantage of the full possibility of the humanoid robot's holonomic movement. Also, the arms serve only as a mean of attaching the cart to the robot and are not taken into consideration to control the cart further.

Another project used a biped robot to push a person on a wheelchair [START_REF] Nozawa | Wheelchair support by a humanoid through integrating environment recognition, whole-body control and human-interface behind the user[END_REF]. To keep balance while performing the desired task, this HRP-2 and the ASIMO both implement an approach with a Zero-Moment Point (ZMP) offset. Although the HRP-2 has a heavy load to move on the wheelchair, the possible movements are akin to those of the ASIMO robot as the cart is controlled like a Dubins car. Furthermore, none of these examples minimize the footprint of the robot-cart contraption by turning the object because they are considered as one static bloc. In a tight and cluttered environment, taking advantage of the arms to manipulate the cart-like object is beneficial and possibly necessary.

In [START_REF] Stilman | Navigation among movable obstacles: Real-time reasoning in complex environments[END_REF], a planning method for humanoids to navigate among movable obstacles has been proposed. The main purpose of that method is to find a path from a starting to a goal points in a complex environment where the robot can easily move objects to create a clear path, if it exists one. Our objective is however different, as we are interested in not only navigating in a cluttered environment, but also transporting a heavy load that is significantly bigger than the humanoid payload.

In the work of [START_REF] Scholz | Cart pushing with a mobile manipulation system: Towards navigation with moveable objects[END_REF], a PR2 robot possessing a wheeled holonomic base and two 7 DoF (Degree of Freedom) arms is used to push a small cart and control its orientation. However, despite having humanoid arms to orient the cart, since the PR2 has wheels instead of legs, no lateral swing is transmitted to the transported object causing oscillations and instability, which is a problem with humanoid robots. Furthermore, the cart is very small and light weight with respect to the PR2, in contrast to our proportionally big cart that is able to carry a load heavier than the robot itself.

III. ROBOT-CART MODEL AND ALGORITHMS A. Path finding

To be able to navigate trough a cluttered environment, a path provided by a motion planning algorithm is essential. Among the different possibilities, we chose a lattice-based graph planning with an ARA* search [START_REF] Likhachev | ARA*: Anytime A* with provable bounds on sub-optimality[END_REF]. This choice is mainly motivated by the use of motion primitives that assures feasible robot-cart configurations and transitions. The environment is modeled by a 2D grid costmap that discriminates obstacles from free space at a fixed threshold and allow obstacles inflations to increase the security margin.

Each node of the search graph needs a complete state representation of the robot-cart to properly operate. To achieve this, it is possible to model the state in

R 2 × S 1 × R 2 × S 1 : s = (x r , y r , θ r , x ca , y ca , θ ca ) (1)
Where x r , y r and θ r are the positions and orientation of the robot, x ca , y ca and θ ca are those of the cart. As the working space of our robot's arms is too small to fully take advantage of both rotation and translation, the problem can be simplified by setting a pivot point positioned in the middle of both hands to reduce the dimensionality of the search space, the chosen position of the pivot point also maximizes the rotation range within the robot's workspace (see pivot point 1 in Fig. 5), resulting in a 4 dimensions state space

R 2 × S 1 × S 1 : s = (x r , y r , θ r , θ ca ) (2)
Even-though the above simplification removes the ability of the cart to translate on the plane, the robot retains enough manipulability to minimize the cart footprint on tight turns. In a lattice-based graph planner, the transition between the nodes is a discrete action chosen within a fixed-set of possible actions called motion primitives. An important feature of the lattice representation is that each of those connections is a feasible path, in contrast to other forms of graph search, including "4-connected" or "8-connected" grid. This makes it really suitable for highly constrained systems, such as a robot moving a cart.

Because the cart can carry different loads, multiple sets of primitives are needed depending on the load's weight. Without load, the robot is holonomic and can move in any direction. A subset of movements composed of forward, backward, diagonal, rotate in place and turn while moving forward is used to reduce planning time while focusing on forward movements. With a heavy load though, moving sideways and rotating in place becomes really difficult because of the increased friction. For this reason, when the weight becomes too important, rotation occurs around a pivot point situated between the two table's legs touching the ground (see pivot point 2 in Fig. 5). Thus, changing the feasible primitives is necessary for the lattice representation to remain coherent. An example of right turn for the omnidirectional and the heavy load sets of primitives are presented in Fig. 2.

The cost function of a transition from state s to s is based on the time to execute that transition and is computed as follows:

Cost = √ (∆xr) 2 +(∆yr) 2 ṙ+ × DF if ∆xr = 0 or ∆yr = 0 ∆θr θ+ × DF otherwise ( 3
) where ∆x r , ∆y r and ∆θ r are the differences between the x r , y r and θ r , which are the coordinates of the robot's pelvis joint, between states s and s , DF is a difficulty factor associated with each primitives, ṙ+ is the maximal robot linear velocity and θ+ is the maximal angular velocity for turning in place. The Euclidean distance between both states is computed and then divided by the maximum velocity of the robot in the direction of the movement to give the approximate time to execute the primitive.

For both instances, the difficulty-factor associated with each primitive is then multiplied by the time cost. This DF is used to prioritize or penalize certain motions or directions, which result in a smoother and a more natural looking trajectory. For example, turning in place then moving forward takes a longer time than moving in diagonal. However, on a long distance, the former reduces the trajectory footprint and is therefore more natural looking while reducing the chances of drifts caused by the table movements. For those reasons, moving sideway has a higher DF than turning and moving forward.

A* is one of the most popular search method at finding a solution path using a cost function. In addition to the cost function, a heuristic bias the search towards the most promising states. In our case, our heuristic is a 2D grid containing all the Dijkstra distance costs from the start to the goal states. Even though A* is optimal when it finds a solution, that solution may not always exists or cannot be found within a certain time limit. The Anytime Repairing A* (ARA*) planner focuses on delivering a suboptimal solution as fast as possible, this solution is then optimized iteratively to obtain the optimal solution.

B. Humanoid footprints and whole body control scheme

Once a collision-free trajectory is found by the ARA* algorithm, a set of footprints are defined along the trajectory as it is shown in Fig. 3. Even-though, at first glance, the support polygon appears to be increased by adding the cart, the robot's support polygon is always defined by the contact between the feet and the ground. This is because the robot's arms are not fully bended, therefore the robot could fall forward or backward .

The second step is to define a Zero Moment Point (ZMP) trajectory. A trajectory of the Center of Mass (CoM) of the robot is then obtained using the preview control algorithm proposed in [START_REF] Kajita | Biped Walking Pattern Generation by using Preview Control of Zero-Moment Point[END_REF]. This algorithm has been widely used by researchers in humanoid robotics, it is simple to implement, yet efficient and yields a smooth CoM trajectory by minimizing the CoM jerk trajectory. The feet trajectories are obtained by spline interpolation between the footprints and the hands trajectories and orientations are defined in order to minimize the walking swing effect as well as follow the cart orientation. To obtain the humanoid robot's joint trajectories, a whole body control scheme with prioritized tasks is formulated as follows:

Initial position and orientation

min q qT Q q subject to First priority    J c q = ṙc J lf q = ṙlf J rf q = ṙrf Second priority J lh q = ṙlh J rh q = ṙrh Joint velocity limits q-≤ q ≤ q+ (4)
where q ∈ R n is the joint velocity vector, Q is a positive semi-definite matrix, J c ∈ R 3×n , J lf ∈ R 6×n , J rf ∈ R 6×n , J lh ∈ R 6×n , J rh ∈ R 6×n are the jacobian matrices of CoM, left foot, right foot, left hand and right hand respectively. ṙc , ṙlf , ṙrf , ṙlh , ṙrh are the linear and angular velocity of CoM, left foot, right foot, left hand and right hand respectively.

qand q+ are generalized joint velocity limits defined as follows:

q+ j = ζ (q + j -qj )-qs qi-qs if q + j -q j ≤ q i q+ j otherwise q- j = -ζ
(qj -q - j )-qs qi-qs if q j -q - j ≤ q i qj otherwise [START_REF] Nozawa | Wheelchair support by a humanoid through integrating environment recognition, whole-body control and human-interface behind the user[END_REF] where q * j is the j element of the vector q * , q j is the value of joint j, q + j and q - j are the upper and lower limits for the joint j, ζ, q i and q s are user-defined positive constants, q i is usually called the interference distance. It can be easily proven that the equalities constraints in [START_REF] Nozawa | Wheelchair support by a humanoid through integrating environment recognition, whole-body control and human-interface behind the user[END_REF], not only yield a motion within the humanoid's velocity limits, but also the joints limits are respected as well with a safety margin equals to q s : q - j + q s ≤ q j ≤ q + j -q s Eq. ( 5) provides a compact and efficient way for dealing with both of velocity and joint limits, it has been originally proposed in [START_REF] Kanehiro | A Local Collision Avoidance Method for Non-strictly Convex Objects[END_REF].

The optimization problem (4) can be efficiently approximated by the following standard Quadratic Programming (QP) problem:

min X X T HX subject to J 1 X = ṙ1 J 2 X = ṙ2 X -≤ X ≤ X + (6)
Where: 15 and δ 2 ∈ R 12 are slack variables.

• X =   q δ 1 δ 2   , δ 1 ∈ R
• H =   Q 0 0 0 Q 1 0 0 0 Q 2   , Q 1 ∈ R 15×15 and Q 2 ∈
R 12×12 are user-defined positive definite matrices. In order to respect the task priority, the following condition should be satisfied:

Q 1 Q 2 . • J 1 =   J c I 3×3 0 3×6 0 3×6 0 3×12 J lf 0 6×3 I 6×6 0 6×6 0 6×12 J rf 0 6×3 0 6×6 I 6×6 0 6×12   , I n×n is the identity matrix. • J 2 = J lh 0 6×15 I 6×6 0 6×6 J rh 0 6×15 0 6×6 I 6×6 • ṙ1 =   ṙc ṙlf ṙrf   and ṙ2 = ṙlh ṙrh • X + =   q+ δ + 1 δ + 2   and X -=   q- δ - 1 δ - 2  
The QP problem ( 6) can be solved in real-time using an appropriate QP solver such as uQuadProg solver [START_REF] Kanehiro | Integrating geometric constraints into reactive leg motion generation[END_REF] or qpOASES solver [START_REF] Ferreau | qpOASES: A parametric active-set algorithm for quadratic programming[END_REF].

C. Mapping, localization and replanning

To move in a cluttered environment, a robust and precise sensing input is primordial to determine the position of obstacles, detect collisions and to plan valid long term and short term paths. Also, odometry drift must be constantly verified and corrected by a localization mechanism to ensure a close monitoring of the planned path. The humansize humanoid robots, such as HRP-2 or the humanoids robots which participated in DARPA Challenge, are able to build a 3D map on the fly using their very sophisticated proprioceptive and exteroceptive sensors. However, the Nao robot has only two cameras in the head for sensing and localization. A first approach would be using those cameras, however this has proven to be a very difficult task [START_REF] Stasse | Real-time 3d slam for humanoid robot considering pattern generator information[END_REF] [START_REF] Oßwald | Learning reliable and efficient navigation with a humanoid[END_REF]. Indeed, as explained previously, a humanoid robot swings laterally while walking, which leads to pictures of poor quality. Furthermore, the field of view of the Nao is greatly obstructed by the large table and load. As a result, it is hard to precisely determine the position of the environment and obstacles with respect to the robot.

A second approach would be adding a Kinect camera on the top of Nao's head for mapping [START_REF] Maier | Real-time navigation in 3d environments based on depth camera data[END_REF], in our case a second Kinect camera placed on the front of the cart would probably improve the quality of the 3D mapping. This approach will be studied in a future work.

Our main purpose in this paper is to validate the motion planning approach and whole body control scheme, we therefore, as many related research [START_REF] Hak | Reverse control for humanoid robot task recognition[END_REF], [START_REF] Levihn | Autonomous environment manipulation to assist humanoid locomotion[END_REF], [START_REF] Kim | Catching objects in flight[END_REF], only to cite few, opted for a complete external sensing and localization. Our system is a Vicon motion capture system constituted of 8 MX20 and 4 T40 cameras. It runs at 100 Hz with a precision of 1 mm.

Markers are placed on the obstacles to construct the 2D costmap of the environment. A global costmap keep tracks of the initial obstacles positions for long term planning, while the local costmap is updated every time a marker moves to ensure safe short term planning and real-time obstacle tracking.

Four markers are placed on a trapezoid shape on the top of Nao's head, they are used to keep track of the robot position and orientation. Another four markers are placed on each corners of the cart and they are also used to track the cart position and orientation.

A collision might occur if an obstacle has been moved or a drift from the planned trajectory happened. When a collision is foreseen, a replanning is necessary as shown in Fig. 4. The new collision-free trajectory is found by the algorithm ARA* starting from the point at which the collision has been predicted. If the potential collision is due to drift, the Dijkstra grid does not need to be recalculated, accelerating therefore the replanning. As the walking pattern trajectory for a humanoid robot cannot be changed instantly, a time interval t c is required to change the planned footprints. In the implementation of ZMP preview control, a finite time horizon of 2 steps is used to compute the CoM trajectory. Therefore, if a collision is foreseen at instant t c , the new collision-free trajectory provided by the algorithm ARA* is deformed to keep the next two footprints unchanged as shown in Fig. 4, the robot will however stop if the deformed trajectory is in collision.

Initial position and orientation

IV. RESULTS

Experiments were conducted on a Nao humanoid robot (Fig. 1), manufactured by Aldebaran Robotics [START_REF] Gouaillier | Mechatronic design of NAO humanoid[END_REF]. On top of its head, we added 4 motion capture markers in order to track the robot position and orientation.

The cart-like object, shown in Fig. 1, is a mini table 600mm long by 300mm wide. On one side, the two legs are 300mm high and are set on omnidirectional wheels. On the other side, the two legs are half the length so that the Nao can fully support this side of the table. Furthermore, these legs are round and small to fit more tightly in Nao's hands.

The primary objective of our approach is to increase the maximum load weight carried by a Nao humanoid robot, without destabilizing it, while maintaining sufficient flexibility and agility. For that purpose, an experiment aimed at measuring the maximum carrying capacity of the Nao without any modifications has been carried out. Nao is placed in the same pose as in Fig. 1, then a small board (333g) is attached to both hands and is used to support various amount of calibration weights. Even with low weight, the robot is rapidly out of balance and has difficulty following planned trajectories, this is because stabilization algorithms are constantly prioritized to avoid falling. At 300g additional weight, however, Nao fell within the first steps nearly every run, which we determined to be its maximum carrying limit.

With the introduction of the cart, two sets of motion primitives are available, an omnidirectional and a heavy load sets. An example of a right turn for both set is shown in Fig. 2. The carry load at which the friction becomes too high to consider the heavy load set is 700g. In reality, the robot can push higher load, however the wood structure of the cart-like objet cannot support a load higher than 7,000g. To summarize, the maximum carrying capacity of the Nao robot alone is 633g and by using the cart, it is 7,000g, which is 11 times its normal capacity.

In the case of omnidirectional set, the hands and arms are strong enough to articulate the cart while turning to obtain smooth trajectories. The maximum angle at which our robot can turn the cart is 30 degrees, as illustrated in Fig. 5. Over that limit, one hand is colliding with the torso while the other lies outside of the robot workspace.

Pivot point 2

Pivot point 1 While walking in a straight line of 1 m, our Nao is affected by a drifting of an average of 10.5 cm. Without any corrections, this error would lead the robot to constantly diverge from the planned trajectories. The correct localization information provided by the external sensing allow us to modify the trajectory to cancel the drift .

While pushing heavy load, however, the robot cannot rotate in place or move laterally to cancel any drift errors, a quick replanning is therefore executed when the robot diverges too much from the planned trajectory.

As showed in Fig. 6, without any hand position correction, the lateral swing causes large oscillations that are transmitted The hands lateral movement while walking with and without corrections to the table and the load. The average peak-to-peak position movement is 56.25 mm. However, with the proposed corrections, the hand distance from desired position has been reduced to 28.20 mm, reducing the hand error by 49.87%. As a result, significantly less oscillations are transmitted to the table, leading to a safer and enhanced carrying ability and load stability. However, note that the error cannot be completely cancelled because: I) the hand trajectories are second priority task, that means the robot will respect those trajectories as far as the trajectories of first priority are fully followed, II) the Nao robot has only 5 degrees of freedom in each arm, III) the joint backlash of the Nao robot.

To test the system as a whole and to validate the proposed algorithms, we conducted three series of 5 experiments. The omnidirectionnal set of primitive has 12 different primitives with θ r sampling of 11.25 • while the heavy set have only possible primitives, but with a precision of 5.625 • . In each experiment, the robot starting and goal positions were chosen in a way that the robot had to navigate through a field of motion capture markers on the ground. Every marker serves as an obstacle that must be avoided by the robot and the cart. They were placed to form various feasible paths and force tight turns in order to take advantage of the additional degree of freedom (the rotation of the cart θ ca ). The three series were composed of the same experiments containing the same initial configuration of the obstacles in the environment and using the same initial and goal positions and orientations, but with different transported objects. Fig. 7 shows the start and end positions for each type of experiment. In this figure, the obstacles are in black, while the grey areas around them are an inflation zone where the cost is higher than free space to prevent the robot from passing too close to obstacles. The vertical and horizontal black lines are virtual walls to prevent the planner from detecting optimal trajectory passing around the experimental setup.

The first series consists of the Nao robot alone, without a cart or load. It uses the omnidirectional primitives set to navigate through the obstacles. The second one was Fig. 7. Map of the obstacles (black), the inflation zone around them (grey) and experiments with the Nao (red square), the cart (orange rectangle) and goal position/orientation (red arrow). Note that the initial posture collides with the inflation zone, but it is not in collision with the obstacles. conducted with the Nao holding the cart, which increased the navigation footprints significantly. These tests also used the omnidirectional set to construct the plan. For the third and final series, the robot is pushing the cart with an additional load of 2,300g, in this case the heavy load set of primitives have been used to allow the Nao to plan a trajectory in the cluttered environment.

The ARA* planner initial = 3 means that the suboptimal solution cannot be worse than 3 times the cost of the optimal solution. A time limit of 10 seconds was chosen and within that time successfully decreased to 1 on every run, which corresponds to the optimal solution. For each generated path, we measured the total time to execute the trajectory, the trajectory length, the initial solution time, the optimal solution time, the initial expanded nodes and the final expanded nodes. The PC that has been used to generate these results has: a i7-3770 Processor with 8 cores at 3.4GHz and 8GB of RAM. These results are summarized in Table II It can be observed that the average velocity is lower while using the cart. This is explained by the friction of the wheels of the cart causing slippage as the robot tries to move, slowing its movements down. Every step in a direction results in a slippage in the opposite direction, thus progressing less distance with each step. This leads to a reduced speed of 17.14% for when pushing the empty cart and 31.43% speed reduction with the additional 2.3 kg load.

Even-though the primitives with the robot alone and with the cart are the same, it is hard to find a path as optimal with it. Since the length is only about 25% longer though, moving with the table does not impair too much the robot ability to travel the cluttered environment swiftly.

The weight primitives trajectory length however is higher in comparison, about 33% higher than the Nao alone. This is primarily due to the change of primitives. The movement is not as smooth, in particular, moving sideways or in diagonal become impossible. Also, turning in place versus turning around a pivot placed r = 0.60m away increased the trajectory by at least L = θπr 180 every time the robot makes a turn.

The biggest problem we encountered is the very limited space where the Vicon is precise. Indeed, the precision is under 1 mm while correctly positioned, but during our experimentations, often when Nao got too close to the edges of the "sweet spot", it starts to lose the markers and fails to follow correctly and consistently the localization. As a result the working space was limited, however our algorithm is more generic and already ready for large scale autonomous navigation. We plan changing this system for a more autonomous one as it will be in Section V.

Also, the friction of the wheels to the ground is not high enough to completely prevent them from sliding and thus to act as a perfect pivot while using heavy load. This causes additional errors when turning and walking that can accumulate and force a replanning. For this reason, with the heavy load, 1.8 replanning were needed on average, while only 0.4 for the cart alone and 0 without the cart when no obstacles are moved.

When an obstacle is moved in the initial trajectory of the robot or cart, a replanning is essential to avoid collision. On all our experiments, there was no collision between neither the Nao or the cart with any obstacles or the virtual walls 1 .

V. CONCLUSION AND FUTURE WORK

In future work, our priority will be to remove any dependency on the external localization system, that is the Vicon system, and instead use other techniques to make the system completely autonomous. As suggested in Section III-B, adding Kinects to the Nao and the table will be explored. Also, since we are using multiple sets of primitives depending on the load weight, an automatic estimation of the load weight, to decide which set to be used, would improve the system and make it more general and autonomous.

Fig. 1 .

 1 Fig. 1. The Nao robot holding the cart-like object

  Fig. 2. A right turn executed by the robot (blue) pushing the cart (green) to the goal (red arrow) with both sets of primitives.

Fig. 3 .

 3 Fig. 3. Overview of the motion planning procedure

Fig. 4 .

 4 Fig.4. Replanning in case of collision detection: tc is the instant at which a collision is foreseen, the new collision-free trajectory is in dashed-blue line, the deformed trajectory is in red line.

Fig. 5 .

 5 Fig. 5. The arms posture while turning the cart at maximum angle (30 degrees)

  Fig.6.The hands lateral movement while walking with and without corrections

  .

		No Cart	Cart Omni	Cart Heavy Load
	Total time (s)	69.75	104.90	134.71
	Total time Std (s)	14.69	15.76	27.62
	Trajectory length (m)	2.41	3.03	3.20
	Trajectory length Std (m)	0.53	0.26	0.48
	Average velocity (m/s)	0.035	0.029	0.024
	Initial solution time ( = 3) (s)	0.020	0.026	0.012
	Initial solution time Std (s)	0.020	0.024	0.008
	Optimal solution time ( = 1) (s)	0.20	0.41	0.44
	Optimal solution time Std (s)	0.11	0.46	0.38
	Initial node expansions	541.6	769.8	892.4
	Initial node expansions Std	492.5	497.0	508.7
	Total node expansions	4775	10694	25165
	Total node expansions Std	2760	11436	24105

TABLE I EXPERIMENTAL STATISTICS

 ISTATISTICS

ACKNOWLEDGMENT

We thank the anonymous reviewers for their helpful comments. This research is supported by a discovery grant (Prof. 1 A video of the experiments is available at https://goo.gl/RYUAY7 Wael Suleiman) from the Natural Sciences and Engineering Research Council of Canada (NSERC).


Wei Wang

Dragan Nešić

Romain Postoyan
email: romain.postoyan@univ-lorraine.fr

Observer design for networked control systems with FlexRay ⋆

We design state observers for nonlinear networked control systems (NCS) implemented over FlexRay. FlexRay is a communication protocol used in the automotive industry, which has the feature to switch between two scheduling rules during its communication cycles. These switches induce technical difficulties when modeling, designing and analysing observers for such systems compared to standard NCS. We present a solution based on the emulation approach. Given an observer in the absence of communication constraints, we implement it over the network and we provide sufficient conditions on the latter, to preserve the stability property of the observer. In particular, we provide explicit bounds on the maximal allowable transmission intervals, which adapt to the lengths of the segment associated to each scheduling rule. We assume that the plant dynamics and measurements are affected by noise and we guarantee an input-to-state stability property for the corresponding estimation error system. The overall system is modeled as a hybrid system and the analysis relies on the use of a novel hybrid Lyapunov function.

Introduction

Networked control systems (NCS) attract great attention due to the features they offer in terms of ease of installation and flexibility. NCS are characterized by the use of a shared serial communication channel to connect spatially distributed sensors and actuators with the control unit. While the usage of a network offers great advantages over traditional point-to-point set-ups, it also introduces communication constraints in terms of limited sampling, scheduling, delay, packet loss, quantization, which may have a severe impact on the desired requirements. In this paper, we focus on FlexRay networks and we concentrate on the effect of sampling and scheduling. FlexRay is a protocol developed by BMW, Daimler-Chrysler, Philips and Freescale in 2000 to provide appropriate communications for implementing Xby-wire technology in automotive control [START_REF] Consortium | FlexRay Communications System -Protocol Specification V2[END_REF][START_REF] Schmidt | Message scheduling for the Flexray protocol: the dynamic segment[END_REF]. It works with communication cycles, that alternate between a static and a dynamic segment during which a specific scheduling rule is used [START_REF] Consortium | FlexRay Communications System -Protocol Specification V2[END_REF]; we thus have to deal with a switched protocol. Our objective is to design observers for uncertain nonlinear NCS with FlexRay. This study is motivated by the fact that this communication protocol is increasingly used in the automotive industry and estimation methods for such systems are currently missing in the literature; only stabilization results are available to the best of our knowledge, see e.g. [START_REF] Naghshtabrizi | Analysis of distributed control systems with shared communication and computation resources[END_REF], Wang et al. 2015).

We consider the scenario where the plant input and the measurements are sent to the observer via the network. Actuators and sensors are grouped into nodes, and only one of these transmit its data to the observer at each transmission instant. The latter therefore only has access to partial and sampled information of the plant input and output. The solution we propose is based on emulation. The idea is to first synthesize the observer while ignoring the communication constraints. At this stage, any of the continuous-time observer design techniques available in the literature can be applied. Then, the observer is implemented over the network and conditions on the latter are derived to preserve the desired error convergence properties. Similar results exist for NCS with nonswitched protocols see [START_REF] Postoyan | A framework for the observer design for networked control systems[END_REF][START_REF] Postoyan | Tracking control for nonlinear networked control systems[END_REF], however none of these apply to our problem because of the switches between the two scheduling rules exhibited by FlexRay. Indeed, these switches re-quire a new model, appropriate assumptions on the observer and the network, and a new stability analysis. In addition, we consider plants with input, perturbed dynamics and noisy measurements, which is more general and induces additional technical difficulties compared with [START_REF] Postoyan | A framework for the observer design for networked control systems[END_REF][START_REF] Postoyan | Tracking control for nonlinear networked control systems[END_REF].

We model the overall system as a hybrid system in the formalism of [START_REF] Rev | Hybrid dynamical systems: Modeling, Stability, and Robustness[END_REF], for which a jump either describes a segment switch or a transmission. We assume that the transmissions during the static and the dynamic segments are governed by possibly (distinct) input-tostate stable (ISS) protocols, which include round-robin (RR) and maximum-error-first try-once-discard (TOD) (Walsh & Ye 2001), as particular cases. The concept of ISS protocol was introduced in [START_REF] Tabbara | Input-output stability with input-to-state stable protocols for quantized and networked control systems[END_REF]) and appears to be very useful for NCS subject to measurement noise. We provide explicit bounds on the maximum allowable transmission intervals (MATI) for each segment and we guarantee an input-to-state stability property for the estimation error system. The analysis relies on a novel hybrid Lyapunov function.

Our contributions are threefold. First, we present an observer design strategy for NCS with FlexRay for the first time to the best of our knowledge. Second, the MATI bounds we propose are much simpler to compute than those derived in (Wang et al. 2015), where the corresponding stabilization problem is addressed. This is due to the novel hybrid Lyapunov function we construct. Third, our results extend the works in [START_REF] Postoyan | A framework for the observer design for networked control systems[END_REF][START_REF] Postoyan | Tracking control for nonlinear networked control systems[END_REF]) to perturbed systems with control inputs and noisy measurements in the particular case where there is a single segment.

Problem Statement

Consider the nonlinear plant

ẋp = f p (x p , u, w) y p = g(x p ) + v, (1)
where x p ∈ R nx is the state, u ∈ R nu is the control input, w ∈ R nw is the external disturbance, y p ∈ R ny is the plant output affected by the noise v ∈ R ny . The functions u(•) and v(•) are assumed to be Lebesgue measurable and differentiable. Moreover, these functions and their time-derivatives are assumed to have a finite L ∞ norm. We assume that we know an observer of the form

ẋo = f o (x o , u, y p -y o ) y o = g(x o ), (2)
where x o ∈ R nx is the estimate of the state x p and y o ∈ R ny is the output of the observer.

We consider the scenario where the plant and the observer communicate over a network and FlexRay is used to schedule transmissions, see Figure 1. In particular, the control signal u is directly available to the plant and transmitted to the estimation unit via the network to the observer as û. We assume that the signal u and the nodes, where a node corresponds to a group of co-located sensors or/and actuators, and the latter have access to the measurements of some components of (y p , u). Because of the network, the observer has no longer access to y p and u, but to their networked versions ŷp and û. At each transmission instant t i , i ∈ Z >0 , only one node is granted access to the channel by the scheduling policy to send its packet. FlexRay uses two different policies that operate in a cyclic fashion to schedule the transmissions, as we explain in Section 3.

Our objective is to provide conditions on the observer and the network, in particular on the scheduling rules and the MATIs associated to each segment, under which the state of observer (2) (approximately) converges to the state of plant [START_REF] Rev | Hybrid dynamical systems: Modeling, Stability, and Robustness[END_REF].

FlexRay

In this section, we briefly present FlexRay and the assumptions we make on the network; for more details, see (Wang et al. 2015). FlexRay works with pre-set communication cycles of length T > 0. Each cycle contains a static segment of length T 1 > 0, a dynamic segment of length T 2 > 0 and two protocol segments called symbol window and network idle time, see [START_REF] Consortium | FlexRay Communications System -Protocol Specification V2[END_REF].

The lengths of the protocol segments can be considered as negligible compared to T 1 and T 2 , and hence we ignore them in the network modeling, which allows us to write

T = T 1 + T 2 .
Distinct network access techniques are applied for the static and the dynamic segments. In particular, the time division multiple access (TDMA) approach is employed for the static segment and the nodes get access to the network in a prefixed manner. The static segment consists of time slots of equal length. The chosen node codes its information into packets and then sends them over the communication channel at the beginning of each time slot. The dynamic segment on the other hand uses the flexible time division multiple access (FTDMA) technique [START_REF] Cena | On the properties of the flexible time division multiple access technique[END_REF], and is composed of minislots, which are substantially shorter than static time slots. A dynamic protocol is used to assign priority among the nodes based on the online information.

The priority can be assigned in a centralized manner, the node with highest priority is assigned with the 'earliest' minislot and the minislots are idle when no nodes compete for access.

We further make the following assumptions on the static and the dynamic segments as in (Wang et al. 2015).

Assumption 1

(1) Data are transmitted instantaneously and a transmission occurs at the end of the two segments. (2) The static time slots are of length τ s MATI > 0, where

τ s MATI is such that N 1 τ s MATI = T 1 with N 1 ∈ Z >0 .
(3) The inter-transmission time interval in the dynamic segments, denoted by τ dy,i , satisfies

τ dy,i ∈ [τ min , τ d MATI ]
, where τ d MATI is the MATI associated with the dynamic segment and τ min ∈ (0, min{τ s MATI , τ d MATI }] refers to the length of a minislot which satisfies N 2 τ min = T 2 with N 2 ∈ Z >0 .

We next introduce two clock variables, which are useful to obtain a hybrid model of NCS with FlexRay in the formalism of [START_REF] Rev | Hybrid dynamical systems: Modeling, Stability, and Robustness[END_REF]. We introduce τ 1 ∈ R ≥0 to model the time elapsed since the last transmission, and τ 2 ∈ R ≥0 to describe the time elapsed since the last segment switch. We also introduce a state variable q ∈ {1, 2} which keeps track of whether we are in the static segment (q = 1) or in the dynamic segment (q = 2). Then, the transmission and segments switching mechanisms can be modeled as

τ1 = 1 τ2 = 1 q = 0    (τ 1 , τ 2 , q) ∈ C 1 ∪ C 2 (3a) τ + 1 = 0 τ + 2 = τ 2 q + = q      (τ 1 , τ 2 , q) ∈ D trans 1 ∪ D trans 2 (3b) τ + 1 = τ 1 τ + 2 = 0 q + = 3 -q      (τ 1 , τ 2 , q) ∈ D seg 1 ∪ D seg 2 , (3c)
where

C 1 := [0, τ s MATI ] × [0, T 1 ] × {1} C 2 := [0, τ d MATI ] × [0, T 2 ] × {2} D trans 1 := {τ s MATI } × [0, T 1 ] × {1} D trans 2 := τ min , τ d MATI × [0, T 2 ] × {2} D seg 1 := {0} × {T 1 } × {1} D seg 2 := {0} × {T 2 } × {2}.
We can see from (3b) that a transmission jump, corresponding to τ 1 being reset to 0, occurs when τ 1 reaches τ s MATI during the static segment (q = 1), and is allowed to occur whenever τ 1 ∈ [τ min , τ d MATI ] during the dynamic segment (q = 2). On the other hand, a segment jump, corresponding to τ 2 being reset to 0 and q being toggled to 3 -q, occurs when τ 2 reaches T 1 when q = 1 and T 2 when q = 2. In this case, τ 1 has to be equal to 0, which corresponds to a transmission at the end of each segment, which is in agreement with item (1) in Assumption 1. Otherwise, the solutions to (3) will stop to exist, which is not a problem in the subsequent analysis. Hence, only the (maximal) solutions, which satisfy the second part of item (1) of Assumption 1, are complete.

Observer Emulation

We emulate observer (2) as follows

ẋo = f o (x o , û, ŷp -ŷo ) . (4)
As already mentioned in Section 2, the emulated observer (4) no longer depends on (y p , u), but on (ŷ p , û) because of the network. These variables are generated by the observer based on the received data. Furthermore, observer (4) does not depend on its own output y o , as in ( 2), but on ŷo . The variable ŷo is an artificially introduced networked version of y o . The idea to use ŷo instead of y o was suggested in [START_REF] Postoyan | Tracking control for nonlinear networked control systems[END_REF]) and allows ensuring stronger stability properties for the estimation error system, see Section VIII in [START_REF] Postoyan | Tracking control for nonlinear networked control systems[END_REF]. We next present the dynamics of ŷp and û before proceeding with the construction of the dynamics of ŷo .

Let (y p , u) = (y p1 , • • • , y pn y , u 1 , • • • , u nu ) and (ŷ p , û) = (ŷ p1 , • • • , ŷpn y , û1 , • • • , ûnu )
, where n y , n u ∈ Z >0 . Suppose that node j ∈ {1, • • • , ℓ} is selected by the protocol at time t i , i ∈ Z >0 , and say that the components y pj y and u ju of y p and u, respectively, are associated to node j, with j y ∈ {1, • • • , n y } and

j u ∈ {1, • • • , n u }. Then ŷpj y (t + i ) = y pj y (t i ) ûju (t + i ) = u ju (t i ), (5)
while for all other components of ŷp and û,

ŷp ky (t + i ) = ŷp ky (t i ) ûku (t + i ) = ûku (t i ), (6)
with

k y ∈ {1, • • • , n y } and k u ∈ {1, • • • , n u } satisfying k y = j y and k u = j u .
We can see from ( 6) that the components of ŷ and û corresponding to the j-th node are updated and the other components are kept unchanged.

For simplicity, we use zero-order-hold devices to implement the observer which gives ẏp = 0 and u = 0 for t ∈ [t i , t i+1 ] and i ∈ Z >0 , however other choices are possible.

The variable ŷo is constructed to evolve along the same vector field as ŷp between two successive transmission instants, i.e., ẏo = 0 for t ∈ [t i , t i+1 ]. We now explain how ŷo jumps at each transmission instant. Let y o = (y o1 , • • • , y on y ) and ŷo = (ŷ o1 , • • • , ŷon y ). At each transmission of a component of ŷp , say ŷpj y with j y ∈ {1, . . . , n y }, the corresponding component of ŷo , that is ŷoj y , is reset to y oj y , which gives

ŷoj y (t + i ) = y oj y (t i ) if ŷpj y (t + i ) = y pj y (t i ) ŷoj y (t i ) otherwise. ( 7
)
In view of ( 7), the dynamics of ŷo , loosely speaking, mimics the dynamics of ŷp .

For the sake of convenience, we introduce the networkinduced error on the plant output e p := ŷp -y p ∈ R ne , n e := n y , and the network-induced error on the input e u := û -u ∈ R nu . We also define an artificially introduced network-induced error on the observer output e o := ŷo -y o ∈ R ne . Using the definitions of e p , e u and e o , we can rewrite (4) as

ẋo = f o (x o , u + e u , y p -y o + e), (8)
where e := e p -e o corresponds to the network-induced error on the observation error y p -y o .

Switched Protocols

In this section, we model the transmission mechanisms of FlexRay under the assumptions made in Section 3 and we present the class of scheduling rules we consider.

Model

The network-induced errors introduced in the previous section are useful to model the scheduling mechanism of FlexRay. Let e p = (e p1 , • • • , e pn e ), e o = (e o1 , • • • , e on e ) and e u = (e u1 , • • • , e un u ). If the node j associated to y pj y and u ju (see Section 4) has access to the network, it follows from (5), ( 6) and ( 7) that e pj y (t + i ) = ŷpj y (t + i ) -y pj y (t i ) = 0 with j y ∈ {1, • • • , n e } and e p ky (t + i ) = ŷp ky (t + i ) -y p ky (t i ) = e p ky (t i ) for k y ∈ {1, • • • , n e } satisfying k y = j y . Note that similar dynamics properties also hold for e o and e u by following the same lines. We then model the dynamics of e p , e o and e u at each transmission time using the general form e p (t + i ) = h p i, e p (t i ), e u (t i ), e o (t i ), q(t i ) , e o (t + i ) = h o i, e p (t i ), e u (t i ), e o (t i ), q(t i ) and e u (t + i ) = h u i, e p (t i ), e u (t i ), e o (t i ), q(t i ) . The functions h p , h o and h u are assumed to be continuous functions; an example is provided in Section 5.2. We then derive that e(t + i ) = h e i, e p (t i ), e(t i , ), e u (t i ), q(t i ) ,

where h e is obtained from h p and h o , noting that we make it depend on (i, e p , e, e u , q) and not (i, e p , e u , e o , q), which is equivalent in view of the definition of e. In view of (8), the dynamics of the observer is only affected by e and e u . We therefore introduce the variable e := (e, e u ).

In that way, we term the equation below,

e(t + i ) = h e (i, e p (t i ), e(t i ), q(t i )), (10)
which specifies how e at transmission time t i is mapped at t + i as the protocol equation, where h e is continuous since so are h p , h u and h o . Recall that q : R ≥0 → {1, 2} is a switching signal indicating which segment is active. The map h e has the form of h e (i, e p (t i ), e(t i ), q(t i )) := (2 -q(t i ))h e,1 (i, e p (t i ), e(t i )) + (q(t i ) -1)h e,2 (i, e p (t i ), e(t i )),

where h e,1 is the scheduling rule corresponding to the static segment and h e,2 corresponds to the dynamic segment.

Assumption

We assume that the scheduling rule (10), which governs the transmissions of the static and the dynamic segments, satisfies the properties listed below.

Assumption 2 For each m ∈ {1, 2}, there exist W m :

R ne+nu × Z ≥0 → R ≥0 , which is locally Lipschitz in its first argument, σ v m ∈ K ∞ , constants α Wm , α Wm > 0 and ρ m ∈ (0, 1) such that for any κ ∈ Z ≥0 , e p ∈ R ne , e ∈ R ne+nu and v ∈ R ne : (1) α Wm |e| ≤ W m (e, κ) ≤ α Wm |e|, (2) W m (h e,m (κ, e p , e), κ + 1) ≤ ρ m W m (e, κ) + σ v m (|v|).
Assumption 2 is a stability condition on the protocol equation ( 10). The underlying idea is to treat protocols as discrete-time dynamical systems as suggested in [START_REF] Nešić | Input-output stability properties of networked control systems[END_REF]), which appears to be extremely useful to study NCS stability. More precisely, Assumption 2 states that ( 10) is an (exponentially) input-to-state stable (ISS) protocol, as termed in [START_REF] Tabbara | Input-output stability with input-to-state stable protocols for quantized and networked control systems[END_REF]. We next provide an example of a switched protocol that satisfies these conditions.

Example 1 We consider a switched protocol which employs the RR scheduling rule during the static segment and a TOD-like protocol during the dynamic segment. RR grants access to the nodes in a periodic fashion and it follows from [START_REF] Nešić | Input-output stability properties of networked control systems[END_REF]) that h e,1 (i, e p , e) := (I -∆(i))e, where i ∈ Z >0 , By following similar lines as in Propositions 3-4 in [START_REF] Postoyan | Tracking control for nonlinear networked control systems[END_REF], we have that the switched RR-TOD-like protocol satisfies Assumption 2 with

∆(i) = diag{ δ1 (i)I n1 , • • • , δℓ (i)I n ℓ }, I
ρ 1 = ρ 2 = ℓ-1 ℓ , σ v 1 (s) = √ ℓ(1 + ρ 1 )s and σ v 2 (s) = (1 + ρ 2 )s for s ≥ 0, α W1 = 1, α W1 = √ ℓ, α W2 = α W2 = 1,
where W 1 (e, κ) := κ+l i=κ |φ(i, κ, e)| 2 , W 2 (e, κ) := |e|, φ(i, κ, e) is the solution to e(κ + 1) = h e,1 (κ, e p , e(κ)) at time i with initial condition e and κ.

NCS Model

We now write the overall system. We introduce for this purpose the variable κ ∈ Z ≥0 to count the number of transmissions, which is useful to model scheduling policies such as RR. We also introduce the estimation error ξ := x o -x p ∈ R n ξ with n ξ = n p , and d := (d u , d v ), where d u and d v respectively denote the timederivative of the input signal u and of the noise v. Let ψ := (ξ, x p , e, e p , κ, τ 1 , τ 2 , q) be the full state vector. In view of Sections 2 to 5, we model the overall system using the hybrid formalism of [START_REF] Cai | Characterizations of inputto-state stability for hybrid systems[END_REF][START_REF] Rev | Hybrid dynamical systems: Modeling, Stability, and Robustness[END_REF], which gives The mapping F in ( 12) is defined as

ψ = F (ψ, u, v, w, d) ψ ∈ C ψ + = G(ψ) ψ ∈ D, (12) where
(u, v, w, d) ∈ R 2nu+nw+2ne , C := R n ξ +np+2ne+nu × Z ≥0 ×(C 1 ∪C 2 ), D := R n ξ +np+2ne+nu ×Z ≥0 ×(D 1 ∪D 2 ), D 1 := D trans 1 ∪ D seg 1 , D 2 := D trans 2 ∪ D seg 2 ,
F (ψ, u, v,w, d) := (f ξ (ψ, u, v, w), f p (ψ, u, v, w), (13) g e (ψ, u, v, w, d), g p (ψ, u, v, w, d), 0, 1, 1, 0),
where f ξ , f p are obtained by direct calculations from ( 1) and (2), g e (ψ, u, v, w, d

) := (g e (ψ, u, v, w, d v ), g u (d u )), g e (ψ, u, v, w, d v ) := ∂g ∂xo (x p +ξ) f o x p +ξ, u+e u , g(x p )+ v-g(x p +ξ)+e -∂g ∂xp (x p )f p (x p , u+e u , w)-d v , g u (d u ) := -d u , g p (ψ, u, v, w, d) := -∂g ∂xp (x p )f p (x p , u + e u , w) -d v .
We note that the derivatives of the input signal and of the measurement noise do appear in the dynamics of the system.

The mapping G(ψ)

:= G trans (ψ) when (τ 1 , τ 2 , q) ∈ D trans 1 ∪ D trans 2 and G(ψ) := G seg (ψ) when (τ 1 , τ 2 , q) ∈ D seg 1 ∪ D seg 2
, where G trans (ψ) := (ξ, x p , h e (κ, e p , e, q), h p (κ, e p , e, q), κ + 1, 0, τ 2 , q) corresponds to a transmission jump, G seg (ψ) := (ξ, x p , e, e p , κ, τ 1 , 0, 3 -q) corresponds to a segment switching jump, h p and h e come from Section 5.1. We can see that the map G gen-erates two kinds of jumps whether it corresponds to a transmission or a segment switch.

Stability Analysis

We first assume an exponential growth condition on the e-subsystem during two consecutive transmissions, like in [START_REF] Postoyan | A framework for the observer design for networked control systems[END_REF][START_REF] Postoyan | Tracking control for nonlinear networked control systems[END_REF].

Assumption 3 For each m ∈ {1, 2}, there exist a continuous function We next assume that the observer (2) has been designed such that the following conditions hold.

H m : R n ξ → R ≥0 , σ m ∈ K ∞ and L m ≥ 0 such that for all ξ ∈ R n ξ , v ∈ R ne , w ∈ R nw , d ∈ R nu+ne , κ ∈ Z ≥0
Assumption 4 There exist a continuously differentiable function V : R n ξ → R ≥0 and α V , α V ∈ K ∞ such that for each m ∈ {1, 2} there exist µ m ∈ K ∞ and ε m , γ m > 0 such that for all ξ ∈ R n ξ , e ∈ R ne+nu , v ∈ R ne , w ∈ R nw and κ ∈ Z ≥0 , [START_REF] Rev | Hybrid dynamical systems: Modeling, Stability, and Robustness[END_REF]

α V (|ξ|) ≤ V (ξ) ≤ α V (|ξ|), (2) ∇V (ξ), f ξ (ψ, v, w) ≤ -ε m V (ξ) -ε m W 2 m (e, κ) - H 2 m (ξ) + γ 2 m W 2 m (e, κ) + µ m (|(u, v, w)|)
, where W m and H m come respectively from Assumptions 2-3.

Assumption 4 ensures that observer (2) is robust for plant [START_REF] Rev | Hybrid dynamical systems: Modeling, Stability, and Robustness[END_REF]. In particular, the ξ-system in ( 12) is L 2 stable from (W m , √ µ m ) to H m , and ISS with respect to (u, v, w).

We next present the bounds we impose on the MATI of each segment, that is τ s MATI and τ d MATI .

Assumption 5 Given T 1 , T 2 > 0, m ∈ {1, 2}, let The bounds in ( 14) depend on constants introduced in Assumptions 2-4 and on the arbitrarily selected segment lengths T 1 and T 2 . Hence, once Assumptions 2-4 have been verified, we derive the MATI bounds to be implemented using Assumption 5 for any given value of T 1 and T 2 . We note that the smaller T m , the smaller T m . On the other hand, when T 1 goes to infinity and only one segment is activated (noting that L1 (T 1 ) → L 1 here), τ MATI,1 corresponds to the MATI in [START_REF] Postoyan | Tracking control for nonlinear networked control systems[END_REF] where observers are designed for NCS with nonswitched protocols.

T m (14) =          1 Lm(Tm)rm arctan(θ(T m )) γm > Lm(Tm) 1 Lm(Tm)+ρmγmλm
Remark 1 Contrary to the MATI bounds in (Wang et al. 2015) where the corresponding stabilization problem was studied, the expression in Assumption 5 exclusively depends on parameters coming from Assumptions 2-4, which make the designs of τ s MATI and τ d MATI easier.

All the required assumptions have been stated, we can now present the stability property of the observer. The proof of the next theorem is provided in the appendix.

Theorem 1 Suppose Assumptions 2-5 hold for system (12). Then, there exist β ∈ KL and γ ∈ K ∞ such that for each solution pair1 ψ, u, v, w, d to system (12) and for all (t, j) ∈ dom ψ, u, v, w, d , |ψ(t, j)| A ≤ β |ψ(0, 0)| A , t + j + γ(||(u, v, w, d)|| (t,j) ), where A := {ψ : ξ = 0, e = 0}, γ(s) :=

1 1-exp(-̺τmin) α U • (∆ F (s) + ̺∆ G (s)) for some α U , ∆ F , ∆ G ∈ K ∞ and ̺ > 0.
We can see from Theorem 1 that the estimation error ξ and the network-induced error e converge to a ball centered at the origin and whose radius depends on the L ∞ norm of the input (u, v, w, d). We can also see from Theorem 1 that ξ and e do not a priori converge to the origin even when w = 0 and v = 0 since, in this case, ξ(t, j), e(t, j) ≤ β |(ξ(0, 0), e(0, 0))| , t + j + γ(||d u || (t,j) ). The nonlinear ISS gain γ in Theorem 1 depends on τ min , which is the minimum time between two tranmission jumps during the dynamic segment. We can also see that γ goes to infinity when τ min approaches zero. This is due to our stability analysis which requires the Lyapunov function to decrease for some time, at least τ min , during flows in order to compensate a possible increase at jump, see details in the proof of Theorem 1 given in the appendix.

Conclusions

We proposed an observer design approach for nonlinear NCS with FlexRay. FlexRay is composed of communications cycles and each cycle consists of a static and a dynamic segment for which different scheduling policies apply. For an observer designed in the absence of communication constraints, we investigated the conditions on the network to preserve its convergence property. In particular, we provided explicit segment-dependent maximal allowable transmission interval (MATI) bounds and showed that the estimation error verifies an input-tostate stable (ISS) property, provided that MATI bounds are satisfied and scheduling rules are ISS. The bounds T m on the MATIs in Assumption 5 actually correspond to the time it takes for φm to decrease from 1/ρ m to ρ m in Lemma 1.

Fig. 1 .

 1 Fig. 1. Block diagram of the setup noisy measurements y p are transmitted through ℓ ∈ Z >0nodes, where a node corresponds to a group of co-located sensors or/and actuators, and the latter have access to the measurements of some components of (y p , u). Because of the network, the observer has no longer access to y p and u, but to their networked versions ŷp and û. At each transmission instant t i , i ∈ Z >0 , only one node is granted access to the channel by the scheduling policy to send its packet. FlexRay uses two different policies that operate in a cyclic fashion to schedule the transmissions, as we explain in Section 3.

  ns is the identity matrix of dimension n s with ℓ s=1 n s = n e , δs satisfies δs (i) = 1 when i = s + kℓ for k ∈ Z ≥0 and δs (i) = 0 otherwise, for s ∈ {1, . . . , ℓ} and i ∈ Z ≥0 .On the other hand, TOD gives access to the node with the largest mismatch between the current signal value and the last transmitted one (Walsh & Ye 2001). We partition e as e = (e 1 , • • • , e ℓ ) and then a TOD-like protocol from[START_REF] Postoyan | Tracking control for nonlinear networked control systems[END_REF] grants access to the node where |e j | is the biggest for j ∈ {1, • • • , ℓ}. Then, h e,2 (i, e p , e) := (I -Ψ(e))e, where Ψ(e) := diag{ψ 1 (e)I n1 , ψ 2 (e)I n2 , • • • , ψ ℓ (e)I n ℓ }. The function ψ s satisfies ψ s (e) = 1 when s = min(arg max j∈{1,••• ,ℓ} |e j |) and ψ s (e) = 0 otherwise, for s ∈ {1, • • • , ℓ} and i ∈ Z ≥0 .

  and almost all e ∈ R ne+nu :∂Wm(e,κ) ∂e, g e(ψ, u, v, w, d) ≤ L m W m (e, κ) + H m (ξ) + σ m (|(u, v, w, d)|), where the function W m comes from Assumption 2.The above condition is satisfied when W m is globally Lipschitz in e with constants S m ≥ 0, which is the case for the protocol in Example 1, and when|g e (ψ, u, v, w, d)| ≤ S 3 |e| + S 4 |ξ| + S 5 |(u, v, w, d)| for all (ψ, u, v, w, d) ∈ R n ξ +np+2ne+nu ×Z ≥0 ×R 2 ≥0 ×{1, 2}×R2nu+2nv+nw , with S 3 , S 4 , S 5 > 0. In this case, Assumption 3 holds with L m := SmS3 α Wm , H m (ξ) := S m S 4 |ξ| and σ m (s) := S m S 5 s for all ξ ∈ R n ξ and s ≥ 0, where α Wm > 0 comes from Example 1.

- 1 .

 1 )rm arctanh(θ(T m )) γm < Lm(Tm) where Lm (T m ) := 1 2 2L m + 1 Tm ln(λ m ) and λ m :Wm with L m , γ m , α Wm , α Wm > 0 and ρ m ∈ (0, 1) coming from Assumptions 2-4, θ(T m ) The constants τ s MATI and τ d MATI are such that τ s MATI < T1 ⌈T1/T1⌉ and τ d MATI < T 2 .

Lemma 1

 1 Walsh, G. C. & Ye, H. (2001), 'Scheduling of networked control systems', IEEE Control Systems Magzine 21, 57-65. Wang, W., Nešić, D. & Postoyan, R. (2015), 'Emulationbased stabilization of networked control systems implemented on Flexray', Automatica 59, 73-83. A Proof of Theorem 1 We need the following two technical lemmas to show the main results. Lemma 1 follows from the same lines as the proof of Lemma 3 in (Wang et al. 2015) and Lemma 2 can be derived according to the second paragraph of the proof of Theorem 1 in (Postoyan et al. 2014), using Lemma 1. Hence, their proofs are omitted. The solution φm , m ∈ {1, 2}, of d φm ds= -2 Lm (T m ) φm -γ m λ m φ2 m -γm λm with φm (0) = 1 ρm satisfies φm (s) ∈ [ρ m , 1/ρ m ] for all s ∈ [0, T m ],where Lm (T m ), λ m and T m come from (14), γ m > 0 from Assumption 4 and T m > 0.

See(Cai & Teel

2009) for a definition of the notion of solution pair to system (12) as well as for a definition of the L∞ norm used in Theorem 1.

⋆ Supported by the Australian Research Council under the Discovery Project (DP170104099), and the ANR under the grant COMPACS (ANR-13-BS03-0004-02).

Let T 1 , T 2 > 0 be given. Let ρ * 1 , ρ * 2 , η * 1 , η * 2 > 0, functions φ 1 and φ 2 come from Lemma 2. We consider the Lyapunov function, for ψ ∈ C ∪ D U (ψ) := V (ξ) + γ 1 φ 1 (τ 1 )ϑ 1 (τ 2 )W 2 1 (e, κ)(2 -q) + γ 2 φ 2 (τ 1 )ϑ 2 (τ 2 )W 2 2 (e, κ)(q -1), (A.1) where W 1 and W 2 come from Assumption 2, V from Assumption 4, ϑ m (s) := exp( λm s) for s ≥ 0, m ∈ {1, 2}, and λm := 1/T m • ln(λ m ) > 0. The functions φ m and ϑ m in (A.1) are constructed differently compared to (Wang et al. 2015), which allows us to derive simpler expressions of the MATI in Assumption 5, as mentioned in Remark 1. We note that U is locally Lipschitz in ξ, x p , e, τ 1 and τ 2 in view of Assumptions 2-4 and the definitions of φ m in Lemma 2.

We next show that there exist α

From the definition of ϑ m , m ∈ {1, 2}, Lemma 2 and Assumptions 2-4, we have that α

The constructions of α U and α U use the same technique as the proof of Proposition 2 in (Wang et al. 2015).

We next consider (P2). We omit below the dependence of φ 1 on τ 1 and ϑ 1 on τ 2 for the sake of convenience. In view of page 99 in [START_REF] Teel | On assigning the derivative of a disturbance attenuation control lyapunov function[END_REF] and the definition of ϑ 1 , for all ψ ∈ C with q = 1 and any

We have 2γ

Hence, in view of (A.2), the facts that ϑ 1 (τ 2 ) ≤ λ 1 for all τ 2 ∈ [0, T 1 ], and 2γ

Then, in view of Lemma 2, the items subscripted by (a) in (A.3) equal to zero when ψ ∈ C. Then, from (A.1),

The same reasoning applies when q = 2.

2 For x, v ∈ R n and locally Lipschitz U : R n → R, let U • (x; v) be the Clarke derivative of the function U at x in the direction v, i.e. U • (x; v) := lim sup y→x,λ↓0 U (y+λv)-U (y) λ .

7

We next investigate (P3) and distinguish two cases: transmission jumps and segment switching jumps. We only consider the case when q = 1 as the same conclusion can be obtained for q = 2 by following the same lines.

Case I:Transmission jumps.

Case II: Segment switching jumps.

Let (ψ, u, v, w, d) be a solution (12). Note that U is locally Lipschitz in ξ, x p , e, τ 1 , τ 2 from item (P1). In view of page 99 in [START_REF] Teel | On assigning the derivative of a disturbance attenuation control lyapunov function[END_REF], for all i ∈ Z ≥0 and almost all s ∈ [t i , t i+1 ], d ds U (ψ(s, i)) ≤ U • (ψ(s, i); F (ψ(s, i), u(s, i), v(s, i), w(s, i), d(s, i))) ≤ -̺U (ψ(s, i)) + ∆ F (|(u(s, i), v(s, i), w(s, i), d(s, i))|). We then invoke the standard comparison principle to obtain U (ψ(t 1 , 0)) ≤ exp(-̺t 1 )U (ψ(0, 0)) + ̺ -1 ∆ F (||(u, v, w, d)|| (t1,0) ). Noting that u, v and w do not undergo jumps, we have that ||(u, v, w, d)|| (tj ,j+1) = ||(u, v, w, d)|| (tj ,j) for any (t j , j) and (t j , j + 1) ∈ dom ψ. According to item (P3), U

) ) when t 1 corresponds to a transmission jump instant, and U (ψ(t 1 , 1)) = U (ψ(t 1 , 0)) when t 1 corresponds to a segment switch.

The segment jumps generate no increase of U and that τ min > 0 is the minimum inter-transmission time for both static and dynamic segment, the following is obtained by induction. U (ψ(t, j)) ≤ exp(-̺t)U (ψ(0, 0)) + ∆ F (||(u, v, w, d)|| (t,j) ) j-1 k=0 exp(-̺τ min ) k + ̺∆ v G (||v|| t,j ) • j-1 k=0 exp(-̺τ min ) k for all (t, j) ∈ dom ψ. Consequently, using item (P1), we get that |ψ(t, j)| A ≤ β(|ψ(0, 0)| A , t + j) + γ(||(u, v, w, d)|| (t,j) ), for all (t, j) ∈ dom ψ, where β(s 1 , s 2 ) := α -1 U • (exp(-̺s 2 )α U (s 1 )), γ(s) :=