Kévin Dufour

Wael Suleiman

On Integrating Manipulability Index into Inverse Kinematics Solver

On Integrating Manipulability Index into Inverse Kinematics Solver

Kévin Dufour and Wael Suleiman

Abstract-This paper presents a method to maximize the manipulability index of a redundant industrial manipulator while solving the Inverse Kinematics (IK) problem as an optimization problem. Even though the IK problem is a widely studied topic, the integration of the manipulability index into IK has rarely been taken into account. As the relation between this index and the joint variables of the robot is not straightforward, we have tested different formulations using approximated derivatives. Obstacle avoidance has also been considered and the effect of modifying the Cartesian trajectory during the execution of a task has been thoroughly analyzed.

Different scenarios have been conducted in simulation and have proven that our modified inverse kinematics solver is efficient to maximize the manipulability index, even with the additional constraint of obstacle avoidance. Moreover, the relaxation of the trajectory constraints leads to a greater manipulability while ensuring the motion smoothness and satisfying the robot physical limitations.

I. INTRODUCTION

The field of collaborative robots is widely expanding as the manufacturers realize the opportunity of flexibility that this kind of robot offers [START_REF] Krüger | Cooperation of human and machines in assembly lines[END_REF]. A major concern, however, is the safety of the human operators of those robots, as they are designed to operate without a security fence or even in physical interaction with humans. There are different ways of improving the safety: I)-modifying the physical properties of the robot such as the material, the weight [START_REF] Albu-Schäffer | The DLR lightweight robot: design and control concepts for robots in human environments[END_REF] or the type of joint actuator, II)-adding a monitoring system [START_REF] Kulić | Pre-collision safety strategies for human-robot interaction[END_REF] in order to make it more human-friendly, III)-integrating some safety constraints into the planning and execution of the task.

Regarding the motion planning phase of a motion, methods such as RRT [START_REF] Lavalle | Randomized kinodynamic planning[END_REF] or PRM [START_REF] Kavraki | Probabilistic roadmaps for path planning in high-dimensional configuration spaces[END_REF] are among the very popular ones. These methods try to find the optimal path while considering different criteria. Their main drawback is the prohibitive time of computation that restrains their main use to offline planning, however a recent research [START_REF] Murray | The microarchitecture of a real-time robot motion planning accelerator[END_REF] on the design of specialized processors to reduce the computation time has shown promising results.

However, when trying to deal with a dynamic environment, it is interesting to generate a preliminary trajectory by motion planning algorithms to benefit from their global planning properties, and then modify it in real-time, by inverse kinematics methods for instance, during the execution. The robot in this case could react to the presence of moving obstacles, such as a human or another robot, while satisfying some constraints, for example the joint limits or a constraint on the velocity or acceleration of the end-effector. A well-known approach to solve IK problems is the reformulation as an optimization problem [START_REF] Escande | Hierarchical quadratic programming: Fast online humanoid-robot motion generation[END_REF], [START_REF] Suleiman | Infeasibility-free inverse kinematics method[END_REF]. An objective function depending on the optimization variable, often the joint velocity, is minimized or maximized subject to different linear equality and inequality constraints. Because of its general formulation, it is easy to add different constraints to meet the safety requirements, as long as it is possible to solve the optimization problem in real-time.

Since the robot is executing a task in a dynamic environment, an important feature is the ability of reacting correctly to unforeseen events. This ability is reflected by the manipulability index, related to the singularities of the Jacobian matrix [START_REF] Yoshikawa | Manipulability of Robotic Mechanisms[END_REF]. When this index becomes small and tends to zero, it means that the robot is close to a singularity and its ability to move away from that pose will be reduced. It is necessary, therefore, to maximize the manipulability index, which can be done by integrating it into the objective function of the IK optimization problem.

The main contribution of this paper is proposing a method to solve the IK problem while integrating the manipulability index, and mainly giving insights into the practical implementation of the new solver as well as the possible strategies to maximize the manipulability index during the task execution.

This paper is organized as follows: Section II presents the formulation of inverse kinematics as an optimization problem with respect to joint limits, obstacle avoidance and trajectory relaxation. In Section III, a new objective function to maximize the manipulability index is proposed. The method is validated through simulation experiments in Section IV.

II. INVERSE KINEMATICS PROBLEM FORMULATION A. General Definition

The inverse kinematics problem is generally written in the following form, called quadratic programming (QP) problem:

min q 1 2 qT Q q (1) subject to J q = ṙ (2) q-≤ q ≤ q+ (3) b -≤ A q ≤ b + (4)
where q ∈ R n is the joint velocity, Q a diagonal and positive semi-definite weighting matrix, J the Jacobian matrix, A, b + , b -the matrix and vectors defining the linear inequality constraints. ṙ is derived from the Cartesian trajectory r generated by an offline planner.

Equation [START_REF] Kulić | Pre-collision safety strategies for human-robot interaction[END_REF] expresses in a single form the joint range and velocity limits using the generalized boundaries q-, q+ defined by the velocity damper formula [START_REF] Suleiman | On inverse kinematics with inequality constraints: new insights into minimum jerk trajectory generation[END_REF], that formula combines the joint position and velocity so that the velocity smoothly decreases when approaching the joint boundaries and does not violate the limits.

The inequality constraint (4) can be used for collision avoidance by using the velocity damper definition, introduced initially in [START_REF] Faverjon | A local based approach for path planning of manipulators with a high number of degrees of freedom[END_REF]: when the distance d between two convex objects, one being a part of the robot (O 1 ) and the other an obstacle (O 2 ), becomes smaller than a constant d i , the following formula is applied:

-n T J(q, p 1 ) q ≤ ξ d -d s d i -d s if d ≤ d i ( 5
)
where n is the unit vector between the closest points p 1 and p 2 on the two objects, as described in Fig. 1, ξ is a positive coefficient and d s is a security distance that cannot be crossed. Let k be the number of distances between the robot and a set of obstacles satisfying d ≤ d i , the constraint (4) can then be rewritten as:

A q ≤ b +
where A ∈ R k×n and b + ∈ R k , such as their j th line is defined by:

A j = -n T j J j (q, p 1 ) b + j = ξ d j -d s d i -d s (6) O 1 O 2 p 1 p 2 d d s d i n Fig. 1

. Parameters for collision avoidance

It should be pointed out that this problem can be adapted to consider the acceleration or even jerk limits of the joints by replacing the optimization variable to the derivative of q and rewriting the constraints consequently [START_REF] Suleiman | On inverse kinematics with inequality constraints: new insights into minimum jerk trajectory generation[END_REF].

B. Relaxed Problem Formulation

The particularity of a redundant robot is that the solution of the IK problem is not unique, the best solution among those admissible is chosen by the optimization algorithm. However, in case of collision avoidance, there might be no solution if the obstacle is in collision with every configuration corresponding to a given pose in the trajectory. It is then interesting to allow the robot to deviate from the pre-defined Cartesian trajectory, if the path taken between the initial and the final poses is not important. A task like "pick-and-place" could be an example.

This can be achieved by adding a slack variable vector δ to the optimization variable, which was previously q and then becomes Z = q δ T . Indeed, if we write the Cartesian path constraint (2) with the new variable: J q = ṙδ, it appears that the trajectory is relaxed and the new one is, in velocity terms, ṙδ. The IK problem formulation thus becomes [START_REF] Kanehiro | Feasible pattern generation method for humanoid robots[END_REF], [START_REF] Suleiman | On inverse kinematics with inequality constraints: new insights into minimum jerk trajectory generation[END_REF]:

min Z 1 2 Z T Q Z Z subject to J Z = ṙ AZ ≤ b + Z -≤ Z ≤ Z + with Z = q δ , Q Z = Q 0 0 Q δ , J = J I A = A 0 , Z + = q+ δ + , Z -= q- δ -
The amplitude of the deviation is controlled by the bounds δ + and δ -of the slack variable and its associated weighting matrix Q δ , which is defined as follows:

Q δ (t) = f e (t)I n
where

f e (t) =      f 1 (t) if 0 ≤ t ≤ T ρ Q low if T ρ ≤ t ≤ T f -T ρ f 2 (t) otherwise
with I n ∈ R n×n the identity matrix, f 1 and f 2 polynomial functions of degree 5 defined to obtain a profile similar to Fig. 2, allowing a smooth, continuous behavior for δ, and as a result for q, by setting their first and second derivatives to zero at their limits. Their value, Q high , is very high at the beginning and at the end of the task so δ is close to 0. Refer to [START_REF] Kanehiro | Feasible pattern generation method for humanoid robots[END_REF] for a detailed description and explanation.

Q low Q high 0 Tρ T f -Tρ T f Stiffness of constraint Time

III. MANIPULABILITY INDEX A. Theoretical Definition

In order to have a motion that has enough admissible movements to escape unforeseen events, it is important to stay away as far as possible from the robot joint singularities. This ability, named manipulability, is defined in [START_REF] Yoshikawa | Manipulability of Robotic Mechanisms[END_REF] as:

m(q) = det(JJ T ) = σ 1 σ 2 • • • σ n (7)
with J the Jacobian matrix of the robot's end-effector for the configuration q and (σ i ) 1≤i≤n the singular values of J. This index is particularly useful because when it increases, the manipulability of the robot does as well, and when it gets close to 0, the robot is close to a singularity. Other definitions, however, do not have this property: usually they only tell if the robot is close to a singularity but they do not give any information about the relative distance to the singularity and then cannot be used to get away from it.

B. Practical Implementation

By integrating the manipulability index into the IK formulation, following optimization problem is obtained:

min q,qt 1 2 qT Q q -α m(q t ) (8)
subject to

J q = ṙ q-≤ q ≤ q+ b -≤ A q ≤ b +
where α is a positive coefficient, preceded by a negative sign in order to maximize m(q t ).

As m(q t ) is a nonlinear function in q, Eq (8) becomes a nonlinear optimization problem which is hard to solve within a fixed time and therefore the hard real-time constraints might not be satisfied.

In order to transform Eq (8) into a QP problem, the manipulability index is linearized as follows:

m(q t ) = m(q t-1 ) + ∇m T ∆q + ε 1 2 ∆q T H m ∆q = m(q t-1 ) + T ∇m T q + ε 1 2 T 2 qT H m q (9)
where H m is the Hessian matrix of m, ∇m is its gradient, T is the control loop sampling time of the robot and ε is a constant equal to 0 (first-order approximation) or 1 (secondorder approximation). By replacing (9) into (8), the following equivalent QP problem is obtained:

min q 1 2 qT Q q -α T ∇m T q + ε 1 2 T 2 qT H m q (10) subject to J q = ṙ q-≤ q ≤ q+ b -≤ A q ≤ b +
Because of the definition of m (7), it is not evident to find a direct analytical relationship between m and q and by consequence a proper definition for ∇m and H m . Thus, the gradient of m is approximated numerically by:

(∇m) i = ∂m ∂q i = m(q + δq i E i ) -m(q -δq i E i ) 2δq i
where (∇m) i is the i th element of vector ∇m, and

E i ∈ R n , E i = [0 . . . 0 1 ↑ i 0 . . . 0]
T and the Hessian matrix by:

(H m ) i,j = ∂ 2 m ∂q i ∂q j = (∇m) j (q + δq i E i ) -(∇m) j (q -δq i E i ) 2δq i
where:

(∇m) j (q + δq i E i ) = m(q+δqiEi+δqj Ej )-m(q+δqiEi-δqj Ej ) 2δqj (∇m) j (q -δq i E i ) = m(q-δqiEi+δqj Ej )-m(q-δqiEi-δqj Ej ) 2δqj
Using these formulations, it is then easy to compute the objective function since only the manipulability index of different configurations is computed, which indirectly implies, according to [START_REF] Escande | Hierarchical quadratic programming: Fast online humanoid-robot motion generation[END_REF], Jacobian matrix calculations.

IV. SIMULATION RESULTS

The optimization problem can be written in different ways depending on the desired constraints. In order to evaluate the performance of each definition, several scenarios have been carried out, showing the efficiency of the different approaches. In the first scenario, the manipulability index is already high, and we aim at proving that we can improve it nonetheless while precisely tracking the desired Cartesian trajectory all along and even avoiding an obstacle. The second scenario focus on the case of a trajectory getting close to a singularity and the effect of our algorithm to get the robot away from that singularity thanks to the robot redundancy. Finally, we will allow the relaxation of the desired trajectory in both scenarios and present its benefits.

The different approaches have been tested in simulation using a Baxter research robot from Rethink Robotics. A preliminary test has been conducted offline to measure the evolution of the manipulability index of the robot depending on the joint positions and to find an approximation of its maximal value. Thus, all the manipulability index shown in this section will be expressed relatively to that maximal value. The matrix Q in [START_REF] Suleiman | On inverse kinematics with inequality constraints: new insights into minimum jerk trajectory generation[END_REF] is equal to the identity matrix, and T = 0.01s. The optimization problem is solved by qpOASES [START_REF] Ferreau | qpOASES: A parametric active-set algorithm for quadratic programming[END_REF], which has been proven to be efficient and real-time compatible.

A. First Scenario

In this first scenario, the robot executes a simple cubic trajectory in the Cartesian space along the vertical axis. In this case, the slack variable δ (the relaxation variable) is not considered and the robot should strictly follow the trajectory.

First, the order of approximation of m will be studied by comparing the first-order (ε = 0), Fig. 3, and the secondorder (ε = 1), Fig. 4, for different values of the coefficient α, α = 0 meaning that the manipulability index is not maximized.

Fig. 3 shows the impact of α and that there is an optimal value for this coefficient: a maximum normalized value of m, 0.91, is reached for α = 5000. However increasing α beyond 5000 makes the manipulability profile decrease, as, for instance, the resulting profile for α = 25000 shows, this behavior can help find the optimal coefficient for a given trajectory. Moreover when α becomes too high, the QP problem (10) becomes infeasible.

Fig. 4 compares the results obtained when considering the second-order approximation to the optimal result found with the first-order approximation, it shows that considering the second-order approximation does not improve the optimal result obtained by only considering the first-order approximation. Moreover, the calculation of the Hessian implies the evaluation of 4 × n × n Jacobian matrices which seriously increases the total calculation time. For that reason, the manipulability index will thereafter be approximated by only its first-order formulation (ε = 0). The case of collision avoidance is then addressed, as shown in Fig. 7 a spherical obstacle that is located close to the elbow during the movement has been added.

The minimal distance between the robot and the obstacle is presented in Fig. 5 for different cases considering alternatively the obstacle constraint and/or the manipulability optimization. It shows that the robot successfully avoids the obstacle and does not cross the security distance d s while still be able to reach the goal. However, Fig. 6 also points that the maximization of the manipulability index and avoiding the collision with the obstacle are two opposite constraints,

α = 0 ε 1, α = 1000 ε = 1, α = 5000 ε = 0, α = 5000
Fig. 4. Normalized manipulability evolution during task depending on coefficient in scenario 1 using second-order approximation of m it shows that when the arm is far enough from the obstacle, the manipulability increases but when the robot becomes too close to the obstacle, the distance constraint has the priority and the manipulability index has the same value as for the non-optimized case. Fig. 7 illustrates the effect of the collision avoidance constraint on the configuration of the robot for a given time during the motion: the elbow, which becomes too close to the obstacle represented by a green sphere, is pushed away to satisfy the security distance. It should be noticed that the end-effector has the same pose in both configurations as the Cartesian trajectory constraint is always satisfied.

B. Second Scenario

In this scenario, a trajectory is generated offline to represent a critical situation as the manipulability index decreases during the motion and comes close to a singularity. Fig. 8 shows the evolution of the manipulability without optimization and the effect of our algorithm depending on the coefficient α. It shows that our method is generally able to increase the normalized manipulability but has a very slight impact near the singularity. In this case, the improvement of manipulability index is limited, this is mainly because only the redundancy is exploited and thus restricted by the robot physical limits. However, this limitation can be overcome by using the trajectory relaxation as shown in the following subsection.

C. Study of the Trajectory Relaxation

Both previous scenarios have been tested using the trajectory relaxation. For the first scenario, Fig. 9 shows that the relaxation method allows an increase of about 0.05 of the normalized manipulability index maximum to a value of 0.96. Moreover, the manipulability increases much faster since it reaches 0.80 in 0.87s instead of 1.72s without the slack variable. Fig. 10 shows the reference Cartesian trajectory of the end-effector and the modified one, pointing out the small deviations and showing that the robot resumes the task at the end to successfully reach the goal pose for the end-effector. For the second scenario, Fig. 11 shows a very important improvement of the manipulability index thanks to the relaxation. Thus, near the singularity, the manipulability index becomes close to 0.93 for α = 90000 and to 0.82 for α = 15000, which is a significant improvement compared to the value obtained in Section IV-B, proving that the robot succeeds to completely escape from the singular configuration. Fig. 12 presents the influence of α on the relaxation of the trajectory, and as it can be seen increasing α increases the deviations. However, even limited deviations as with α = 15000 leads to a high profile of manipulability.

Fig. 13 illustrates the difference between the final configurations in scenario 2 in two cases: I)-without relaxation nor manipulability optimization, II)-with relaxation and manipulability optimization, showing that the configuration of the robot has been changed to a more convenient one regarding the manipulability, while having the same endeffector pose.

V. CONCLUSION AND FUTURE WORK

In this paper, we presented a new method to integrate the manipulability index maximization into an optimizationbased IK solver. When considering the first-order approximation of the manipulability index, our algorithm does not require heavy calculations and is real-time it can be combined with other constraints without leading to infeasibility. Moreover, an implementation of our method while considering the trajectory relaxation has proven to give excellent results in maximizing the manipulability index with a reasonable deviation in the desired trajectory of the endeffector.

However, it should be noted that with this approach the optimal value of coefficient α, which allows the maximization of manipulability index, varies depending on the trajectory. For repetitive tasks the optimal value of α can be determined offline, however our experiments with the Baxter robot pointed out that the manipulability index has been improved whatever the value of α in the interval ]0, 250 T ]. Future work will focus on integrating other securityrelated constraints such as the avoidance of moving obstacles, taking into account human awareness and dealing with the presence of a human beside the robot. Moreover, we are

Fig. 2 .

 2 Fig. 2. Profile of constraint stiffness

Fig. 3 .

 3 Fig.3. Normalized manipulability index evolution while executing the task in scenario 1, in this case only the first-order approximation of m is used

Fig. 5 .

 5 Fig. 5. Minimal distance between the robot and the obstacle with different constraints

Fig. 6 .Fig. 7 .

 67 Fig. 6. Normalized manipulability evolution in obstacle avoidance scenario

Fig. 8 .

 8 Fig. 8. Normalized manipulability evolution while executing the task in scenario 2

Fig. 9 .-Fig. 10 .

 910 Fig. 9. Normalized manipulability evolution in scenario 1 while relaxing the constraints

  Fig. 11. Normalized manipulability evolution in scenario 2 while relaxing the constraints

Fig. 12 .

 12 Fig. 12. Reference and computed trajectories in scenario 2 while relaxing the constraints

  (a) Without relaxation (b) With relaxation nor manipulability optimization and manipulability optimization

Fig. 13 .

 13 Fig. 13. Final Pose of the Second Trajectory

ACKNOWLEDGMENT

This research is supported by a discovery grant (Prof. Wael Suleiman) from the Natural Sciences and Engineering Research Council of Canada (NSERC).


Julien Dupeyroux

Julien Diperi

Marc Boyron

Stéphane Viollet

Julien Serres

A bio-inspired celestial compass applied to an ant-inspired robot for autonomous navigation*

Common compass sensors used in outdoor environments are highly disturbed by unpredictable magnetic fields. This paper proposes to get inspiration from the insect navigational strategies to design a celestial compass based on the linear polarization of ultraviolet (UV) skylight. This bioinspired compass uses only two pixels to determine the solar meridian direction angle. It consists of two UV-light photosensors topped with linear polarizers arranged orthogonally to each other as it was observed in insects' Dorsal Rim Area. The compass is embedded on our ant-inspired hexapod walking robot called Hexabot. The performances of the celestial compass under various weather and UV conditions have been investigated. Once embedded onto the robot, the sensor was first used to compensate for yaw random disturbances. We then used the compass to maintain Hexabot's heading direction constant in a straight-forward walking task over a flat terrain while being perturbated in yaw by its walking behaviour. Experiments under various meteorological conditions provided steady state heading direction errors from 0.3 • (clear sky) to 1.9 • (overcast sky). These results suggest interesting precision and reliability to make this new optical compass suitable for autonomous field robotics navigation tasks.

I. INTRODUCTION

Autonomous navigation systems mostly make use of Global Positioning System (GPS) and Inertial Measurement Units (IMU). Unfortunately, the precision of GPS methods is in the range of one meter, which makes them not suitable for all robotic applications. Besides, local magnetic fields produced by ferrous materials in urban infrastructures can cause incorrect magnetometer measurements. These magnetic disturbances are classically compensated applying Kalman filtering and require data fusion from gyroscopes and accelerometers [START_REF] Bergamini | Estimating orientation using magnetic and inertial sensors and different sensor fusion approaches: accuracy assessment in manual and locomotion tasks[END_REF]. The solutions currently proposed remain sensitive to multiple sources of disturbances since gyroscopes and accelerometers are also prone to drifts. Biomimetic approach has led to interesting models for optimization, control and automation in robotics. In complex outdoor environments where magnetic interferences are increasingly present and often unpredictable, bio-inspired solutions would be interesting to abstract from magnetic pollution and GPS precision.

We propose here to get inspiration from the insects' skylight polarization compass [START_REF] Wehner | Visual navigation in insects: coupling of egocentric and geocentric information[END_REF] to provide a reliable measurement of the heading direction for outdoor robotic tasks. The scattering of sunlight within the Earth's atmosphere *This work was supported by the French Direction Générale de l'Armement (DGA), CNRS, Aix-Marseille Université, the Provence-Alpes-Côte d'Azur region and the French National Research Agency for Research (ANR) with the Equipex/Robotex project. 1 Aix-Marseille University, CNRS, ISM, Inst Movement Sci, Marseille, France. julien.serres@univ-amu.fr produces a polarization pattern across the sky. Solar radiations remain unpolarized until their entry into the atmosphere where scattering interactions with atmospheric constituents induce the partially linear polarization of the skylight [START_REF] Coulson | Polarization and Intensity of Light in the Atmosphere[END_REF].

The direction of the linear polarization of skylight at the zenith point is always perpendicular to the solar meridian 1 .

The angle direction of the solar meridian is not constant and highly depends on the position of the Earth about the sun and the location of the observer. Biological studies showed the existence of ommatidia sensitive to the polarization of skylight in the Dorsal Rim Area (DRA) of the insect's compound eye ( [START_REF] Labhart | Detectors for polarized skylight in insects: a survey of ommatidial specializations in the dorsal rim area of the compound eye[END_REF], [START_REF] Labhart | Polarization-opponent interneurons in the insect visual system[END_REF]). Each ommatidium is sensitive to a unique direction of polarization and its orthogonal direction, and the spectral sensitivity is generally in the ultraviolet (UV) range, except for a few species whose maximal sensitivity is in the blue range or in the green range. Many hypotheses have been proposed to explain why ommatidia are sensitive to UV light instead of other spectral ranges, but the most reasonable assumption is that the skylight polarization remains strong in the UV under canopies and clouds ( [START_REF] Brines | Skylight polarization patterns and animal orientation[END_REF], [START_REF] Barta | Why is it advantageous for animals to detect celestial polarization in the ultraviolet? Skylight polarization under clouds and canopies is strongest in the UV[END_REF]). In the locust brain, the information perceived through the DRA is first integrated by polarization neurons, called POL-neurons, in the optic lobe, which shows a high synaptic activity for three distinct orientations (10 • , 60 • and 130 • ), while in the central complex, POL-neurons show a rather uniform synaptic activity for all polarization angles [START_REF] Heinze | Linking the input to the output: new sets of neurons complement the polarization vision network in the locust central complex[END_REF]. DRA-based neural models mostly provide an estimated heading direction by computing the logarithmic difference between the response of the ommatidium to a single polarization orientation, and the response of the same ommatidium to the corresponding orthogonal polarization orientation [START_REF] Labhart | Polarization-opponent interneurons in the insect visual system[END_REF]. The study of the DRA in desert ants and honeybees showed that insects refer to a unique global polarization angle [START_REF] Wehner | Desert ant navigation: how miniature brains solve complex tasks[END_REF] to get their bearings. For instance, during a foraging trip in an unknown place, desert ants Cataglyphis integrate their heading direction through their celestial compass. Although their foraging trip consists of a slightly random exploration trajectory, their homing trajectory tends to be direct and straight to the nest [START_REF] Wehner | Desert ant navigation: how miniature brains solve complex tasks[END_REF].

The first autonomous wheeled robot to use a celestial compass, Sahabot 1, was created by Lambrinos et al. in the late 1990s [START_REF] Lambrinos | An autonomous agent navigating with a polarized light compass[END_REF]. Getting inspiration from the DRA of the cricket, the spectral sensitivity ranged from 400nm to 520nm. The project sought to test three models of heading direction measurement: (a) the scanning model uses only one polarization sensor, consisting of a pair of photodiodes mounted below orthogonally set linear polarizer, and makes the robot rotate to find angle that provides the highest sensor response. However, the rotating phase often induces 2D displacements and therefore increase the final position error in a navigational task; (b) the extended scanning model uses the same procedure as the scanning model but with three polarization sensors set at different orientations (0 • , 60 • and 120 • ) as in the optic lobe of insects. The solar meridian direction is then computed simply by subtracting the sensor signals. This method provides more reliable results since peaks detected at the corresponding linear polarization angle are sharper than in the scanning model, but 2D drift remains an issue; (c) the simultaneous model uses three polarization sensors without rotating the robot. Logarithmic differences are computed between each sub-unit of POL-sensors so that the heading direction can be correctly estimated. Tests were performed in the early morning and the average angular error was of 0.66 • using the simultaneous model, and 1.73 • using the simple scanning model. The simultaneous model was then applied to Sahabot 2 in order to implement antinspired path integration models [START_REF] Lambrinos | A mobile robot employing insect strategies for navigation[END_REF]. It is still unclear how insects distinguish solar and anti-solar angles, but some suggest that insects use a circadian clock to both dispel the heading direction ambiguity and compensate the sun path [START_REF] Wehner | Insect navigation: use of maps or Ariadne's thread?[END_REF]. Sahabot simply integrated the position of the sun to avoid any ambiguity ( [START_REF] Lambrinos | An autonomous agent navigating with a polarized light compass[END_REF], [START_REF] Lambrinos | A mobile robot employing insect strategies for navigation[END_REF]).

Chu et al. developed a celestial compass based on the one integrated in the Sahabot projects, using the simultaneous model ( [START_REF] Chu | Construction and performance test of a novel polarization sensor for navigation[END_REF], [START_REF] Chu | Application of a novel polarization sensor to mobile robot navigation[END_REF]). The optical compass was embedded onto a wheeled robot using a fuzzy logic controller to follow a preprogrammed trajectory. Tests were performed at the end of the day to prevent any sensors saturation. A miniaturized version of the celestial compass has also been proposed [START_REF] Chu | Integrated polarization dependent photodetector and its application for polarization navigation[END_REF] but no implementation onto a mobile robot has been recorded yet. Another implementation of the celestial compass has been embedded on a small Unmanned Aerial Vehicle (UAV) [START_REF] Chahl | Biomimetic attitude and orientation sensors[END_REF]. Three polarization sensors, including effective directions and their corresponding orthogonal, were integrated in an ocelli based autopilot designed to control the UAV roll and pitch over ten seconds of flight [START_REF] Chahl | Biomimetic attitude and orientation sensors[END_REF].

In this paper, we propose to merge both scanning and simultaneous models proposed by Lambrinos et al. into a UV-polarized light scanning model providing highly accurate measurement of the heading direction of our walking robot under various meteorological conditions and a low UVindex 2 . Section II presents the UV-polarized light compass and analyzes its performance under highly different weather and UV conditions. Section III describes the hexapod walking robot. Section IV examines two practical experiments of heading direction recovery using our celestial compass in real outdoor conditions.

II. THE UV-POLARIZED LIGHT COMPASS

A. The 3D-printed UV-polarized light sensor

The celestial compass uses two UV-light sensors mounted below rotating UV linear sheet polarizers held by two 70teeth gears (see figure 1.B) which are driven by a third one composed of 10 teeth and actuated by a stepper motor (AM0820-A-0,225-7, Faulhaber). Due to its symmetric properties, the two UV sheet polarizers holder gears turn in the same direction. The entire prototype was printed using PLA filament (polyactic acid). The angular resolution of the compass can be modified by changing the microstep settings of the stepper motor. The UV-light sensor is SG01D-18 (SgLux) which active area is equal to 0.5mm 2 and spectral sensitivity is between 200nm and 375nm with a maximum spectral response at 280nm. Each POL-unit has an angular field of view of ±50 • , and a refresh rate of 33.3Hz. The UV linear sheet polarizer has a local maximum single (resp. parallel) UV-light transmission of 52% (resp. 27%) for wavelengths between 270nm and 400nm. The peak transmission is located near 330nm.

We call POL-sensor any sub-unit of the compass composed of a UV-light photo-receptor topped with a UV sheet polarizer. The left (resp. right) POL-sensor is called UV 0 (resp. UV 1 ). Let x be the rotation angle of the UV sheet polarizer holder gears, and ψ the solar meridian direction angle. Therefore, the response of each POL-sensor unit is:

UV 0 (x) = A 0 + B 0 • cos(2(x + ψ)) UV 1 (x) = A 1 + B 1 • cos(2(x + ψ + π 2 )) (1)
where x ∈ [0; 2π] is the angle of rotation of the polarizers and ψ ∈ [0; π] is the solar meridian direction angle, A 0 and A 1 are offsets determined by the average UV-light radiance and inner bias of each photo-sensor, B 0 and B 1 are constants determined by the degree of polarization and inner gain of each photo-sensor, and UV 0 and UV 1 are π-periodic sinusoidal functions. POL-sensors measurements are normalized so that the minimum value is set at 0 and the maximum value is set at 1. In case of bad weather conditions, B 0 and B 1 values are significantly reduced due to the weakening of the degree of polarization, implying heavy noise disturbances in POLsensors measurements. To prevent incorrect computation of ψ, we propose to restrict the signal to its first harmonic. Let UV nc 0 (resp. UV nc 1 ) be the normalized and corrected UV 0 (resp. UV 1 ) POL-sensor measurement:

Algorithm 1 First harmonic restriction 1: for i∈ [0 : 1] do 2: UV i = fft(UV i ) 3: UV i [2 : length( UV i ) -3] = 0 4:
UV n i = abs(ifft( UV i ))

5:

UV nc i = UV n imin(UV n i ) + ε 6:

UV nc i = UV nc i /max(UV nc i )
where fft and ifft are respectively the direct and reverse fast Fourier transforms, and ε = 0.0001 is set to prevent from logarithm calculation failure. The POL-unit response is defined as follows :

p(x) = log 10 UV nc 1 (x) UV nc 0 (x) . ( 2
)
We then compute the solar meridian direction ψ by locating the two local minimum values of the p function, the first one being in [0; π] and the second one in [π; 2π]:

ψ = 1 2 arg min x∈[0;π] p(x) + arg min x∈[π;2π] p(x) -π . (3)
due to the symmetry of the polarization pattern around the zenith point, ψ is only known within [0; π]. Classical methods to eliminate the ambiguity between ψ Solar and ψ Anti-Solar use the ambient radiance distribution. As none of the tasks asked from the robot imply a turn back movement, there was no reason for ψ to change for ψ + π. Therefore, we assume that ψ ∈ [0; π]. Using the average value of the two minima of function p provides more accuracy in determining the solar meridian direction angle ψ relative to the robot.

B. Performances of the celestial compass under various weather conditions

The celestial compass was tested under various weather conditions in order to quantify its reliability and performances in determining the solar meridian angular direction. Four sets of data obtained in February and April 2017 under both clear and covered sky conditions are shown in figure 2. The UV-index was equal to 1 in February, and 7 in April according to the French meteorological services.

The table I provides the average peak-to-peak magnitudes UV 0,P-P (resp. UV 1,P-P ) of raw signals UV 0,k (resp. UV 1,k ), and the corresponding coefficient of variation Cv p , where k ∈ 1..n stands for the k-th test, n is the number of tests conducted, and p ∈ {0, 1} corresponds to the left and right POL-units. Letters standing for conditions are used with respect to the identifiers of graphs in figure 2. The estimated cosine waves UV nc 0 and UV nc 1 were compared to the normalized raw signals by applying the Mean Squared Error (MSE) method. Therefore, the error ε p,k is defined as

ε p,k = MSE(UV nc p,k ) = 1 N N ∑ i=1 UV nc p,k (i) -UV n p,k (i) 2 ( 4
)
where N is the total length of the signal. Due to some improvements made in the stepper-motor control, N got risen from 280 in February to 373 in April. Finally, in table II we compute the average error ε p as the mean value of all ε p,k for k ∈ 1..n . The coefficients of variation Cv[ε p ] were also calculated.

The overall results show that it is clearly impossible to distinguish whether the estimation of the angular direction ψ was done in winter or not, and under clear sky or not. We notice that both average errors and coefficients of variation are highly similar between February and April at a given weather condition. Besides, though the variability of signals is heavily increased under covered sky, the celestial compass remains fully able to accurately estimate ψ. The restriction of the signal to its first harmonic is consequently an interesting approach to provide a good estimate of the solar meridian angle direction ψ without introducing delay.

III. HEXABOT : THE ANT-INSPIRED ROBOT

A. The robot platform

A fully open source, 3D-printed, six-legged walking robot called Hexabot3 (figure 3) has been developped to mimic the desert ant in several navigational tasks such as homing in unknown environments. The overall weight of Hexabot, including batteries, is 925g, and the maximum length is 360mm with a maximum height of the center of mass is 145mm. Its battery endurance, depending on the capacity, ranges from half an hour to one hour. The robot has three DYNAMIXEL XL-320 actuators per leg which allows to reach high walking speed (approximately 35cm/s in optimal conditions) and execute complex motion when crossing over an uneven terrain. Besides, six-legged robots show more stable walking motion than the four-legged ones since they ARM Cortex-A7) which achieves sensor data acquisition and processing, and send high level orders the robot controller. The celestial compass is embedded on the dorsal part of the robot (see figure 1.A). Communication with the Raspberry Pi board was implemented by means of I2C protocol (figure 4).

IV. EXPERIMENTS

Hexabot was set to tripod gait for all navigational tasks as it provides an optimal compromise between high walking speed and moderate attitude disturbances. However, interactions between legs and the ground tend to cause a large drift of the trajectory. We propose here to use our UVpolarized light compass to contain the drift occurring after each stride. All experiments were done between 02/02/2017 and 02/20/2017 in outdoor conditions, at any time of the day, and were located in an open-air car park in the Luminy campus (43 • 14 01.6 N ; 5 • 26 39.2 E) of Aix-Marseille Université, Marseille, France. The angular precision of the UVlight polarized compass was arbitrarily set at 1.29 • for all experiments. The acquisition time was consequently of 42s.

Drift measurements were made for five outdoor straightforward walking tasks on a flat but rough terrain. Tests were conducted over six seconds at maximum walking speed. Results show an average heading direction disturbance of 28 • (magnetometer measurements). In those conditions, Hexabot drifts from initial walking axis by an average length of one meter.

A. Recovery of orientation under various weather conditions

The ability to reorientate Hexabot after a random yaw disturbance was first tested. The reorientation tasks consists of : (1) the robot acquires its initial heading direction with the celestial compass; (2) the robot turns by a random angle and then acquires its new heading direction; (3) the robot computes the difference between the initial and the new heading direction and uses it as an order to turn back to its initial orientation. Finally, we compare the ground truth measurements (magnetometer4 ) before the disturbance and after the angular correction. To prevent any yaw ambiguity, disturbances were set between -70 • and +70 • . Such a restriction makes sense as angular drifts over a straightforward walking task are systematically small (less than 30 • over a 3-meter walk) but randomly oriented in the both directions due to interactions with the ground.

Figure 5 shows heading errors under three different weather conditions. The variability of performances in orientation recovery can be explained by the non-regularity of Hexabot's stride length while turning due to interactions with the ground. In view of this, the presented results exhibit great performance, especially under clear sky conditions. The decrease noticed under cloudy sky and overcast sky conditions stems from the low degree of polarization of the skylight [START_REF] Brines | Skylight polarization patterns and animal orientation[END_REF] which implies the overpowering of the Rayleigh scattering, thus disturbing the polarization pattern of the skylight. Fig. 5. Heading direction angle errors in degrees in function of weather conditions. From left to right, the median heading direction angle error measured is equal to 0.4 • , -2.9 • , and -1.9 • . UV-index from 1 to 2 (source: French Meteorological services).

Considering the results obtained with Sahabot robot in [START_REF] Lambrinos | An autonomous agent navigating with a polarized light compass[END_REF], our UV-polarized light compass provides similar and even slightly better results under clear sky, and promising results under bad meteorological conditions such as clouds in the sky and a much lower level of UV radiance due to the period and the location of experiments.

B. Heading-lock over a straight-forward walking task

As mentioned above, Hexabot exhibits important drifts in yaw orientation and the celestial compass successfully corrected yaw disturbances at fix point. We now test this ability to maintain the robot within a straight-forward trajectory applying yaw correction after each walking step. First, the initial angle is acquired. Then Hexabot executes a series of strides during two seconds and measures its new yaw orientation, the value of which is compared to the initial one to compute the yaw angle correction to be applied. Finally, Hexabot executes the corresponding turning Fig. 6. Evolution of the measured heading direction angle ψ before (in red) and after (in blue) angular correction using only the UV-polarized light compass during a straight-forward walking task. Walking steps measurements from 1 to 6 were acquired on 02/18/2017 while the next six measurements were acquired on 02/20/2017. movement before moving to the next series of strides. Due to power supply limits and to avoid the impact of polarization shifts induced by the sun movements, data were acquired over two distinct days (02/18/2017 and 02/20/2017), but experiments were all performed at the same time (2:00 pm) under perfectly clear sky conditions with a UV index of 2.

Results for all experiments are shown in figure 6. The average heading angle error measured is of -0.3 • which is consistent with performances exhibited previously under clear sky conditions. The peak error measured is of 7.7 • , occurring during the ninth walking step. Since there were no clouds in the sky, the polarization pattern remained rather constant all over the experiments. As a consequence, the heading direction error is mainly caused by interactions between legs and the ground.

V. CONCLUSION

In this paper, a novel insect-inspired celestial compass was introduced. Performances analysis during winter and spring showed this compass can be used in all weather conditions, including high and low UV-index, clear and covered sky. The sensor was then embedded onto an ant-inspired walking robot to maintain the robot's heading direction constant while walking.

The heading recovery experiments performed both at fix point and during a walking task showed highly precise and reliable results under clear-sky conditions, with an average steady state error as small as 0.3 • . Results under cloudy-sky conditions also exhibits good performances, from 0.8 • under variable weather, to 1.9 • under overcast sky, but slightly less reliable due to the high variability of meteorological conditions.

Future work will focus on the impact of the turning uncertainty of the robot on the heading direction. Residual heading-lock errors can be reduced by changing to a closedloop system showing the suitability of this new optical compass sensor for autonomous robotic tasks such as homing.

Fig. 1 .

 1 Fig. 1. A. Top view of the Hexabot robot equipped with the UV-polarized light compass. (a) Celestial compass; (b) MinImu-9 v.3 gyro, accelerometer & compass (Pololu) used for outdoor ground truth measurement; (c) Raspberry Pi 2B board. B. An exploded view of the compass. (a) 3D-printed fixation (PLA, polyactic acid) for the UV sheet polarizer; (b) UV linear sheet polarizer (HNP'B replacement, UV grade 275 -750nm); (c) 3D-printed gears (PLA); (d) stepper motor AM0820-A-0,225-7 (Faulhaber); (e) ball bearing; (f) 3D-printed support (PLA); (g) UV-light sensor SG01D-18 (SgLux); (h) 3D-printed support (PLA) for UV-light sensor.

Fig. 2 .

 2 Fig. 2. Examples of signals obtained during several acquisitions from the UV-polarized light compass. Each pairs of graphs (a,b), (c,d), (e,f) and (g,h) show normalized raw and filtered outputs for UV 0 and UV 1 photo-sensors (left), and the POL-unit response for both raw and filtered data(right). Pair (a,b) was obtained in February 2017 under clear sky conditions with a UV index equal to 1, pair (c,d) at the same period but under covered sky. Pairs (e,f) and (g,h) were obtained in April 2017 with a UV index of 7 -the first one under clear sky, the second one under covered sky.

Fig. 3 .Fig. 4 .

 34 Fig. 3. Hexabot robot equiped with a pair of UV-polarized light sensors forming a celestial compass.

TABLE II STEADY

 II STATE ERROR BETWEEN NORMALIZED AND FILTERED DATA

	Conditions	ε 0	Cv[ε 0 ]	ε 1	Cv[ε 1 ]	n
	(a,b)	4.28e-03	6%	4.83e-03	4%	21
	(c,d)	9.02e-03	36%	7.31e-03	32%	15
	(e,f)	3.99e-03	10%	4.14e-03	5%	36
	(g,h)	6.14e-03	27%	8.36e-03	19%	36

The solar meridian is the circle that crosses the zenith and the sun relative to an observer on the Earth.

Experiments conducted with Sahabot were done in desert conditions whith high UV range (UV-index of 11 in Maharès, Tunisia, in August 1996). Most of the time, the sky was clear. Source: http://www.temis.nl/uvradiation/UVindex.html

Based on Metabot, a quadruped walking robot. See http://metabot.cc/ can operate static gait (i.e. three to five legs remain on the ground at any time).

Experiments were conducted with a calibrated magnetometer and away from any magnetic fields interference.

ACKNOWLEDGMENT

The authors would like to thank Grégoire Passault and Olivier Ly for their investment to the maintenance of the hexapod robot.