Jinna Qin Design
email: jinna.qin@ensam.eu

Francois Léonard
email: leonard@enim.fr

Gabriel Abba
email: gabriel.abba@ensam.eu

Experimental External Force Estimation Using a Non-Linear Observer for 6 axes Flexible-Joint Industrial Manipulators

This paper proposes a non-linear observer to estimate not only the state (position and velocity) of links but also the external forces exerted by the robot during Friction Stir Welding (FSW) processes. The difficulty of performing this process with a robot lies in its lack of rigidity. In order to ensure a better tracking performance, the data such as real positions, velocities of links and external forces are required. However, those variations are not always measured in most industrial robots. Therefore, in this study, an observer is proposed to reconstruct those necessary parameters by using only measurements of motor side. The proposed observer is carried out on a 6 DOF flexiblejoint industrial manipulator used in a FSW process.

I. INTRODUCTION

A. Robot and control

Current studies propose to replace some special machinetools by industrial robots because of their effective cost and larger workspace compared with usual machine-tools. The technical and economic performances of production can be greatly improved by using a manipulator or a robotic system as a tool holder. However, performance issue is one of the reasons that prevent robotization of industrial manufacturing. Moreover, in industrial production, it is very difficult to change the mechanical elements or the inner robot control system. This study concerns an industrial manipulator KUKA KR500 MT-2, which has a payload of 500 kg and can be used for a FSW process [START_REF] Zimmer | Experimental investigation of the influence of the fsw plunge processing parameters on the maximum generated force and torque[END_REF], [START_REF] Mishra | Friction stir welding and processing[END_REF] where a very significant interaction force is required. Up to now, this process is usually carried out by some specially developed machines, having strongly rigid axes but more expensive. The objective of this work is to realize the required process with a 6 DOF serial chain industrial manipulator.

The concerned robot only has motor side measurements: the angular positions θ and speeds θ as well as the currents of actuators. Due to flexibility of the transmission chain, the state of links cannot be deduced from the one of motor side. In order to facilitate the realization of these operations, the state of links (q, q) and the external torque applied on the tool is required to be known. Either the components of the torque are difficult to measure or the sensor required for high forces of FSW is expensive. One solution is to use an observer to estimate those essential parameters [START_REF] Besanc ¸on | Nonlinear Observers and Applications[END_REF].

An extended state observer (ESO) is proposed for the state feedback control of a flexible joint robotic system in [START_REF] Talole | Extended-state-observerbased control of flexible-joint system with experimental validation[END_REF]. This approach is used for state estimation, which is then validated with a Quanser's flexible-joint module. An acceleration-based observer is presented in [START_REF] De Luca | An acceleration-based state observer for robot manipulators with elastic joints[END_REF] for state observation with an experimental validation. However, this observer needs measurement of link accelerations, which are generally not measured by industrial robots. [START_REF] Henriksson | Experimental comparison of observers for tool position estimation of industrial robots[END_REF] investigates methods for tool position estimation of industrial robots, which also requires tool acceleration measurement. An application of disturbance observers to non-linear systems is given in [START_REF] Kravaris | Nonlinear observer design for state and disturbance estimation[END_REF] and an observer-based adaptive robust control (ARC) approach for FSW process is discussed in [START_REF] Davis | Observer-based adaptive robust control of friction stir welding axial force[END_REF] proving that axial force can also be estimated by an observer, however, these approaches do not consider flexibility of joints. Unique Lyapunov-based non-linear observer is difficult to apply systematically [START_REF] Yao | Observer-based adaptive robust control of a class of nonlinear systems with dynamic uncertainties[END_REF]. Disturbance observer technique is widely used in mechanical servo systems, and a disturbance dual observer based control algorithm is proposed for industrial robots having flexible joints in [START_REF] Park | Disturbance observer based robust control for industrial robots with flexible joints[END_REF], which can use state of motor to construct state of link as well as disturbances. Nevertheless, in this paper the observer is under the assumption that the disturbance dynamics are known and proportional to the error of estimation.

B. Contributions and outline of the paper

The contribution of this paper is mainly about the extension of the observer proposed by [START_REF] Jankovic | Observer based control for elastic joint robots[END_REF] to cover estimation of external forces applied by the robot by using only position, velocity and current measurements of motor axes. This paper describes an improved observer of our previous work in [START_REF] Qin | Non-linear observer-based control of flexible-joint manipulators used in machine processing[END_REF]. This observer is adapted for a 6 DOF flexible robot system, and modified to deal with variable external forces. Simulations and experiments show that the new proposed observer has much better performance than the one we proposed before. With the new observer, significant and variable external forces can be well estimated for industrial robots. Moreover, in this paper, a complete flexible-mode manipulator model of robot KUKA KR500 MT-2 is described.

The modeling of robot and process are presented in Section II. The design of our new observer is described in Section III. In Section IV results of experiments with robot KUKA KR500 I.

MDH PARAMETERS OF ROBOT KUKA KR500 MT-2.

j µj σj αj dj θj rj 1 1 0 π 0 q1 -L1z 2 1 0 π/2 L1x q2 0 3 1 0 0 L2 π/2 + q3 0 4 1 0 -π/2 D4 q4 -L34 5 1 0 π/2 0 q5 0 6 1 0 -π/2 0 -π/2 + q6 -L5 7 0 2 0 0 0 -Ltz
MT-2 are provided. These results showed good performances of the proposed observer. Finally, conclusions and directions for future work are discussed in Section V .

II. MODELING OF FLEXIBLE JOINT ROBOT AND FSW PROCESS

The robot concerned in this study is a serial chain manipulator (see Fig. 1), main numerical values used for the proposed observer are given in the Appendix. The control problem of this kind of manipulator for precise manufacturing is a challenging task: the manipulator is an elastic multibody, multivariable and strongly coupled system. Its highly nonlinear dynamics state change rapidly while manipulator moves in its workspace.

A. Modeling of a 6 DOF flexible manipulator

The Modified Denavit-Hartenberg (MDH) notation is frequently used to model robots including the robot concerned in this research work [START_REF] Khalil | Modeling, Identification and Control of Robots[END_REF]. The MDH parameters for this manipulator are listed in Table I and are used for the modeling of robot with the help of software SY M ORO + [14] [START_REF] Khalil | Symoro+: A system for the symbolic modelling of robots[END_REF]. All the joints of this manipulator are revolute joints, except the seventh axis that represents the spindle fixed on link 6. The flexible joint robot model suggested by Spong [START_REF] Spong | Modeling and control of elastic joint robots[END_REF] is used in this research work under the assumption that the robot has flexible joints and rigid links. This robot has a gravity compensation system with a gas accumulator on the second axis to compensate the effects due to gravity of the robot, the expression of this compensator is not presented here, but is used in simulation in section IV-B. According to [START_REF] Spong | Robot Modeling and Control[END_REF], a flexible joint robot can be modeled as follows:

D(q)q = Γ -H(q, q) -F robot ( q) -J T (q)F (1)

I a θ = Γ m -N T v Γ -F motor ( θ) (2)
with the expression of link friction:

F robot ( q) = B s q + F s (sign( q)) (3)
and the expression of the friction at motors axes:

F motor ( θ) = B m θ + F m (sign( θ)) (4)
where J T (q) is the Jacobian matrix of the tool frame, F is the vector of efforts applied by robot on external environment, Γ m is the vector of motor torques, Γ is the vector of gearbox torque, vectors q = [q 1 q 2 q 3 q 4 q 5 q 6 ] T , q and q represent angular positions, velocities and accelerations, respectively. Vectors θ = [θ 1 θ 2 θ 3 θ 4 θ 5 θ 6 ] T , θ and θ denote the vector of angular positions, velocities and accelerations of motor axes, respectively. Here N v = N -1 is the gear transmissions ratio matrix, and N is a matrix of dimension 6 × 6. For the robot KUKA KR500 MT-2, N is not a diagonal matrix, there is also a coupling among axes 4, 5 and 6. The non-null elements are presented in Appendix. D(q) is the symmetric, uniformly positive definite and bounded inertia matrix of the robot. I a is the inertia matrix of the motor. H(q, q) represents the contribution due to centrifugal, Coriolis and gravitational forces and the gas accumulator of axis two. B m and B s are the viscous friction coefficient matrices of the motor and joint axes respectively. F m and F s are the matrices of Coulomb friction of the motor and joint axes.

Robots have two main sources of flexibilities: the flexibilities of body/arms, and those located at joints (which include those of motors and transmissions). In this study, links of robot are considered as rigid, only the flexibilities localized at gearboxes are taken into account and represented by a rigidity matrix K:

Γ = K(N v θ -q) (5)
where θ l = N v θ is the vector of angular positions after gear reduction (see Fig. 2).

B. Modeling of FSW process

FSW is commonly known as a solid state welding process, which gives the possibility to weld almost all types of aluminum alloys, even for some alloys that classified as nonweldable materials by fusion welding [START_REF] Nandan | Recent advances in frictionstir welding -process, weldment structure and properties[END_REF]. The advantage of this process is to avoid those problems encountered in classical welding by melting the metals. It is for this reason that FSW process has been implemented, primarily in transportation industry where aluminum is used to reduce the weight of mechanical structures [START_REF] Mishra | Friction stir welding and processing[END_REF], [START_REF] Zhao | Empirical dynamic modeling of friction stir welding processes[END_REF]. Up to now, this process has been carried out by some specially developed machine-tools, which are expensive and have smaller workspace compared with robot manipulators.

III. OBSERVER DESIGN

FSW needs reliable force control and tracking performance under a significant axial force. This problem can be solved if the state of the links (q, q) and external forces are known. Unfortunately, only state and current of motor axes and axial force (F z ) are measured by our robot. Therefore, an observer is proposed in this section to solve this problem.

Disturbance observer is a widely used technique in control for improving the performance of disturbance rejection [START_REF] Park | Disturbance observer based robust control for industrial robots with flexible joints[END_REF]. The unknown input observer (UIO) method is one of the most well-known approach to estimate states [START_REF] Yue | Robust adaptive control of a three-axis motion simulator with state observers[END_REF]. The proposed observer is based on a non-linear observer introduced in [START_REF] Jankovic | Observer based control for elastic joint robots[END_REF], which reconstructs articular positions and velocities of links with the ones measured from motor axes. This observer is then modified in order to get also an estimation of efforts by adding more state variables function of forces. In this paper, the observer proposed in [START_REF] Qin | Non-linear observer-based control of flexible-joint manipulators used in machine processing[END_REF] has been improved to obtain a better performance in estimating external forces, the block diagram is presented Fig. 4, and it is then implemented into a KUKA robot for experimental validation.

If the state variables x i is defined as x 1 = q = [q 1 q 2 q 3 q 4 q 5 q 6 ] T , x 2 = q = [ q1 q2 q3 q4 q5 q6 ] T ,

x 3 = θ = [θ 1 θ 2 θ 3 θ 4 θ 5 θ 6 ] T , x 4 = θ = [ θ1 θ2 θ3 θ4 θ5 θ6 ] T ,
then following state space model can be obtained:

         ẋ1 = x 2 ẋ2 = q = D(q) -1 [Γ -H(q, q) -F robot ( q) -J T (q)F ] ẋ3 = x 4 ẋ4 = θ = I a -1 [Γ m -N -T Γ -F motor ( θ)] (6)
Then, measured variables are now denoted as y, the following change of coordinates is considered:

             z 1 = [I a -1 N -T K] -1 x 4 = K -1 N T I a x 4 z 2 = x 1 = q z 3 = x 2 = q z 4 = -D -1 (z 2 )J T (z 2 )F y 1 = x 3 = θ y 2 = x 4 = θ (7) Moreover, if u = Γ m then:      ż1 = z 2 + K -1 N T [u -F motor (y 2 )] -N v y 1 ż2 = z 3 ż3 = q = z 4 + ψ(z 2 , z 3 , y 1 ) ż4 = -d dt [D -1 (z 2 )J T (z 2 )F ] (8)
where ψ is equal to:

ψ = D(z 2 ) -1 [K(N v y 1 -z 2 ) -H(z 2 , z 3 ) -F robot (z 3 )] (9)
If A and C are defined as following (I is the 6 order identity matrix and 0 is the 6 × 6 zero matrix):

A =    0 I 0 0 0 0 I 0 0 0 0 I 0 0 0 0    and C = (I 0 0 0) (10)
then:

ż = Az + g(z, y, u) + d(z, F, Ḟ ) (11)
with

g =      K -1 N T [u -F motor (y 2 )] -N v y 1 0 ψ(z 2 , z 3 , y 1 ) 0 (12)
and d(z, F, Ḟ ) = [0 0 0 -d dt [D -1 (z 2 )J T (z 2 )F ]] T . This non-linear observer is an improved version of the one described in [START_REF] Qin | Non-linear observer-based control of flexible-joint manipulators used in machine processing[END_REF]. The measured input y 1 and y 2 are now θ and θ, z 4 are here represent an additional acceleration which enables the additional acceleration ψ not to be a function of unknown disturbance external forces. Define now the following high gains matrix, with G a constant ≥ 1:

Γ G =    GI 0 0 0 0 G 2 I 0 0 0 0 G 3 I 0 0 0 0 G 4 I    (13)
and matrix L such as (A -LC) has all its eigenvalues in the left half of the complex plane, then a new observer is proposed as follow:

ż = (A -Γ G LC)ẑ + g(z, y, u) + Γ G Ky 2 (14)
where K = LK -1 N T I a and:

     z2 = y 1 -y1-ẑ2 ∥y1-ẑ2∥ N s sat( ∥y1-ẑ2∥ Ns ) z3 = ẑ3 ∥ẑ3∥ M s sat( ∥ẑ3∥ Ms ) z4 = ẑ4 ∥ẑ4∥ A s sat( ∥ẑ4∥ As ) (15)
with the saturation function:

sat(x) = { x if | x | ≤ 1 1 if | x | > 1 (16)
                 ∥ x 2 ∥< M s ∥ x 1 -x 3 ∥< N s ∥ F ∥< F M ∥ Ḟ ∥< F d ∥ J T (z 2 ) ∥< J M ∥ D -1 (z 2 ) ∥< D M A s = D M J M F M (
det[λI -(A -LC)] = λ 4 + L 1 λ 3 + L 2 λ 2 + L 3 λ + L 4 ( 18
)
where λ is an eigenvalue of matrix A -LC. A possible choice for L is then: L 1 = 4a, L 2 = 6a 2 , L 3 = 4a 3 and L 4 = a 4 . In this case, matrix A -LC has four stable eigenvalues in λ = -a, where a is a positive real fixing the dynamics of the proposed observer.

The estimation error e = z -ẑ satisfies the following differential equation:

ė = A c e + g(z, y, u) -g(z, y, u) + d(z, F, Ḟ ) (19)
Because of the form of disturbance d(z, F, Ḟ ) the following stability theorem can be proven for the proposed observer:

Theorem 1: There exists a constant G 0 such that error e(t) is bounded for any gain G > G 0 . The proof is similar to one described in [START_REF] Qin | Non-linear observer-based control of flexible-joint manipulators used in machine processing[END_REF], as 17)) and ∂D -1 (z2) ∂z2 and ∂J T (z2) ∂z2 are also bounded as they are only functions of sines and cosines.

∥ d dt [D -1 (z 2 )J T (z 2 )F ]] T ∥ is bounded since ż2 , F , Ḟ are bounded (see equation (

IV. EXPERIMENTAL VALIDATION

A. Test 1: Validation of proposed observer during diving phase of FSW process

On the first step, the observer is validated only with measured data. FSW process can be divided into three phases: diving, welding and removing the tool. During experiment, the robot is programmed to press on a work-piece, then the tool will enter the material step by step with a force (F z ) equalling 1000 N, 2000 N and 3000 N, just like the diving phase of FSW process as shown in Fig. 1. Meanwhile, state of rotor (θ, θ), current of each motor (I) and the axial force F zmeasured are measured with the Scope and function monitor FTCtrlMonitor of the robot controller. After that, the state and current are used off line as input of the observer. The observer should reconstruct the state of link (q obs , qobs ) and the external forces (F xobs , F yobs , F zobs ), as shown in Fig. 4. In this test, we have chosen tuning parameters of the observer G = 1, a = 25 for the first three axes and a = 50 for the other axes. Moreover, the lateral forces in direction x and y are also estimated, which improves operation performance as it is these lateral forces that prevent the robot to weld a straight line during FSW process. Fig. 5 shows the error of reconstructed angular speeds ( qobs -θl ), as θl is angular velocities after reduction gearbox. As mentioned before, the tool is in contact with work-piece at the beginning. During this experiment, only the axes of motor can have a little movement while applying a force in z direction, links of robot should rarely move. Fig. 6 presents the estimated forces and the measured force F zmeasured on direction z of the tool frame, since the robot only has axial force sensor. The F zobs matches well with the F zmeasured , and the observer has also reconstructed other forces F xobs and F yobs . Here, F yobs might be caused by friction.

B. Test 2: Validation of observer in the case of a complete trajectory

On the second step, the robot is programmed to move along a large trajectory in its workspace.Fig. 8 and Fig. 9 present the measured angular displacements and currents on each motor axis. Then, the desired displacement, speed and acceleration measurements fed a simulation model of robot KUKA KR500 MT-2 disturbed by an external force, as shown in Fig. 7. From t = 4s, an external force of F z = 1000 N is added to the robot system model. The reconstructed state of links (q obs , qobs ) and external forces (F xobs , F yobs , F zobs ) are compared with the measured state of motor axes (θ l , θl ) and the added forces to verify and validate the proposed observer. The results show a good performance in angular position, velocity and external force estimations as shown in Fig. 11 with a precision of 10 -7 rad for positions and 10 -5 rad/s for velocities. Fig. 10 shows the torque due to the added external force on each axis, which can reach about 1400 Nm on axis 2. If we defined χ = J T F -(J T F ) obs , then the relative mean square error can be calculated by: RM SE = χ rms /(J T F ) rms [START_REF] Yue | Robust adaptive control of a three-axis motion simulator with state observers[END_REF] where χ rms and (J T F ) rms are the root mean square value of χ and J T F respectively. The RM SE of axes 2, 3 and 5 are equal to: RM SE 2 = 0.79%, RM SE 3 = 0.83%, RM SE 5 = 0.37%.

The identified inertia parameters have good precisions, but here, in order to analyse the sensitivity of the proposed observer, two simulations have been carried out with an additional error of ±10% of inertia parameters (see Fig. Fig. 11. Observer error of angular positions q -q, velocities qq. and torques J T F -(J T F ) obs and Fig. 13). The estimations appear to be stable and quite accurate in spite of the error of inertia.

V. CONCLUSIONS

In this paper, a non-linear disturbance observer is proposed for flexible-joint industrial robots and a stability theorem has been introduced to prove the boundedness of observer estimation errors. A flexible model of the concerned KUKA robot has been developed, with precise identification parameters. A series of experimental validation tests demonstrates that the proposed observer is fast enough to estimate the system state with required accuracy and sensitivity, and better performance than the one proposed in [START_REF] Qin | Non-linear observer-based control of flexible-joint manipulators used in machine processing[END_REF]. Even under noises of measurement, this observer still provides satisfactory results in both state and external force estimations.

More precisely, in FSW diving phase, the estimated axial force matches well with the measured force. This method can also be applied to other robots and processes. For instance, in our laboratory, numerous experiments have been implemented to identify all the necessary parameters of KUKA KR500 MT-2. With more precise robot identification, the performance of observer can also be improved. Using both state of the rotor axes and the estimated state of robot links in the control of flexible robots can lead to more stable and robust feedback control as shown in [START_REF] Qin | Non-linear observer-based control of flexible-joint manipulators used in machine processing[END_REF], [START_REF] Spong | Robot Modeling and Control[END_REF]. Moreover, the reconstructed external forces can reduce the error on the work-piece. Next step consists now to implement some experiments on-line with industrial robots, and use the estimated lateral force to suppress lateral deviation observed in FSW process. Fig. 12. Comparison of estimation error of q and q with ±10% error of Inertia parameters.

Fig. 1 .

 1 Fig. 1. Robot KUKA KR500 MT-2 (by courtesy of Institut de Soudure)TABLE I. MDH PARAMETERS OF ROBOT KUKA KR500 MT-2.

Fig. 2 .

 2 Fig. 2. Robot and flexible gearbox model in 2D.

Fig. 3 .Fig. 4 .

 34 Fig. 3. Friction Stir Welding Model.

6 Fig. 5 .

 65 Fig. 5. Estimate error of angular velocity.

17 )Fig. 4

 174 Fig. 4 shows the diagram of the proposed observer where A c = A -Γ G LC. For only one axis, L = [L 1 L 2 L 3 L 4 ] T and:

Fig. 6 .

 6 Fig. 6. Comparison between measured force and estimated force.

Fig. 7 .

 7 Fig. 7. Block diagram for validation of force building in the case of a complex trajectory of robot KUKA KR500 MT-2.

Fig. 8 .Fig. 9 .

 89 Fig. 8. Robot KUKA KR500 MT-2 measured reference angle displacements.

12

 12

Fig. 10 .

 10 Fig. 10. Estimated torques due to external force.

Fig. 13 .

 13 Fig.[START_REF] Khalil | Modeling, Identification and Control of Robots[END_REF]. Comparison of J T F estimation error with ±10% error of Inertia parameters.

TABLE II .

 II NUMERICAL VALUES OF ROBOT KUKA KR500 MT-2

	N (i, j)		Link length	(m)	Rigidity	(N m/rad)
	N11	469.375	L1x	1.045	K1	6.61 10 6
	N22	469.375	L1z	0.500	K2	7.16 10 6
	N33	-504.770	L2	1.300	K3	3.08 10 6
	N44	-260.619	L34	1.025	K4	0.56 10 6
	N55	-251.977	D4	-0.055	K5	0.66 10 6
	N66	164.570	L5	0.290	K6	0.47 10 6
	N54	-1.0964	Ltz	0.350
	N64	-1.5836	Lty	0
	N65	1.5311

APPENDIX ACKNOWLEDGMENT

This study is a part of the project ANR-2010-SEGI-003-01-COROUSSO that is sponsored by the French National Agency of Research as a part of the program ANR-ARPEGE.


Françoise Bahoken

Laurent Beauguitte
email: beauguittelaurent@hotmail.com

Serge Lhomme
email: serge.lhomme@eivp-paris.fr

Bahoken Françoise

Beauguitte Laurent

Visualizing world flows: a challenge

published or not. The documents may come

The case study we choose to work on is 2012 world flight patterns between airports (and cities) (http://openflights.org/data.html). This topic is both relevant for globalization process and is well-documented in scientific literature. Then, the pictures already produced can be compared to ours. Moreover, taking a well-known example presents a didactic interest. Our question here is not the data reliability, always questionable when dealing with world statistics, but the visualization challenge involved if we aim to keep a geographical constraint regarding flows' destinations and origins.

Several procedures are tested in order to highlight their advantages and drawbacks. From the classical model of node-link maps (Fig. 1), we tested the variations of the result in order to suppress or to reduce the "spaghetti effect": flows filtered or not (dominant, major…), aggregated or not (from node or links points of view). We also tested alternatives cartographic ways of representations such as cartogram [START_REF] Dorling | The Visualization of Spatial Social Structure[END_REF], merge and edge bundling graphical options (Holten, 2006 -Fig. 2). Another classical option regards the projections' choices (Mercator, Polar or 3D globe). Our aim is not to define the best solution nor to impose a normative or a "geographically correct" point of view but to propose a reflection about the efficacy, the accuracy and the aesthetic (Tufte, 2001) of the selected methods. If producing nice world pictures is easy today, the major issue remains to produce relevant scientific content.

KEYWORDS

World flows, map visualization, flight patterns.

Figure 1 :Figure 2 :

 12 Figure 1: Mercator projection, spaghetti effect and color gradient


Daniel Zelazo
email: dzelazo@technion.ac.il.

Antonio Franchi
email: antonio.franchi@laas.fr

Paolo Robuffo Giordano

Rigidity Theory in SE(2) for Unscaled Relative Position Estimation using only Bearing Measurements

This work considers the problem of estimating the unscaled relative positions of a multi-robot team in a common reference frame from bearing-only measurements. Each robot has access to a relative bearing measurement taken from the local body frame of the robot, and the robots have no knowledge of a common reference frame. An extension of rigidity theory is made for frameworks embedded in the special Euclidean group SE(2) = R 2 × S 1 . We introduce definitions describing rigidity for SE(2) frameworks and provide necessary and sufficient conditions for when such a framework is infinitesimally rigid in SE(2). We then introduce the directed bearing rigidity matrix and show that an SE(2) framework is infinitesimally rigid if and only if the rank of this matrix is equal to 2|V| -4, where |V| is the number of agents in the ensemble. The directed bearing rigidity matrix and its properties are then used in the implementation and convergence proof of a distributed estimator to determine the unscaled relative positions in a common frame. Simulation results are given to support the analysis.

I. INTRODUCTION

. Many of these applications involve operating a robot team in environments where access to certain measurements in a common reference frame (i.e., inertial position measurements from GPS) are not available. This motivates control and estimation strategies that rely on sensing and communication capabilities that do not depend on knowledge of a common reference frame.

.

In many real-world scenarios, the sensors used to obtain relative measurements are likely to be physically coupled

to the frame of the robot. Furthermore, the sensors might also introduce additional constraints such as field-of-view restrictions or line-of-sight requirements. In these scenarios, the attitude of each agent must be considered to define the sensing graph. In many distributed control strategies for multi-robot teams using relative sensing, an implicit requirement is the team have knowledge of a common reference frame to generate the correct velocity input vectors. This information is either known directly from special sensors or communication with agents endowed with this information, or it must be estimated by each agent. This problem was considered in [START_REF] Franchi | Decentralized control of parallel rigid formations with direction constraints and bearing measurements[END_REF] for special classes of graphs (and extended to generic graphs using communication) and in [START_REF] Zelazo | Decentralized Rigidity Maintenance Control with Range-only Measurements for Multi-Robot Systems[END_REF] when only distance measurements are available.

This paper considers the unscaled relative position (URP) estimation problem for a team of agents that have access to bearing measurements. The adjective 'unscaled' means that the positions of the agents are estimated up to a common scale factor. The bearing sensor is attached to the body frame of each agent, and consequently the attitude of each agent (as measured from a common inertial frame) will influence which agents can be sensed. In this direction, we consider each agent as a point in SE(2) 1 The bearing measurements available for each agent induces a directed sensing graph. A contribution of this work is to provide necessary and sufficient conditions on the underlying sensing graph and positions of each agent in SE(2) for solving the URP relative position estimation problem with only bearing measurements.

Estimation using only relative bearings as exteroceptive measurements has been considered also in [START_REF] Stegagno | Mutual localization using anonymous bearing-only measures[END_REF], [START_REF] Cognetti | 3-D mutual localization with anonymous bearing measurements[END_REF]. However, in those works the robots also had access to egomotion sensors in order to disambiguate the anonymity of the measurements, an assumption not required in this work. Another similar problem set-up was also considered in [START_REF] Eren | Sensor and Network Topologies of Formations with Direction, Bearing, and Angle Information between Angents[END_REF]- [START_REF] Bishop | Stabilization of rigid formations with direction-only constraints[END_REF]. The main distinction with this work is the insistence that the bearing measurements between agents are expressed in the local frame of the agent. This turns out to be an important assumption and requires a new extension to the theory of rigidity. These works are related to the notion of parallel rigidity which attempts to keep the bearing vector between neighboring agents constant (i.e., the formation shape). Other references utilising parallel rigidity include [START_REF] Franchi | Shared control: Balancing autonomy and human assistance with a group of quadrotor UAVs[END_REF], [START_REF] Franchi | Decentralized control of parallel rigid formations with direction constraints and bearing measurements[END_REF], [START_REF] Shames | Analysis of Noisy Bearing-Only Network Localization[END_REF], [START_REF] Servatius | Constraining plane configurations in cad: combinatorics of directions and lengths[END_REF].

This then motivates the study of rigidity for formations in SE [START_REF] Franchi | Shared control: Balancing autonomy and human assistance with a group of quadrotor UAVs[END_REF], which is the main contribution of this work. Similar to parallel rigidity, the objective for formations in SE [START_REF] Franchi | Shared control: Balancing autonomy and human assistance with a group of quadrotor UAVs[END_REF] is to define a formation shape while also maintaing the relative bearings between each agent. The main distinction is the bearing measurements are expressed in the local frame of each agent, and the corresponding statements on SE(2) rigidity explicitly handle this distinction. Our approach is to mirror the development of formation rigidity, such as can be found in [START_REF] Asimow | The Rigidity of Graphs, II[END_REF], but for frameworks where each node in the directed graph is mapped to a point in SE [START_REF] Franchi | Shared control: Balancing autonomy and human assistance with a group of quadrotor UAVs[END_REF]. We derive a matrix we term the directed bearing rigidity matrix and show that a formation is infinitesimally rigid in SE(2) if and only if the dimension of the kernel of this matrix is equal to four. Furthermore, we show the infinitesimal motions that span the kernel are the trivial motions of a formation in SE(2), namely the translations, dilations, and coordinated rotations of the formation. The directed bearing rigidity matrix appears in the relative position estimator and provides the essential ingredient for the convergence proof of the estimator.

The paper is organized as follows. A brief review of concepts from rigidity theory with an emphasis on parallel rigidity is provided in §II. The development of rigidity theory for SE(2) is given in §III. The relative position estimation problem is given in §IV, and some numerical simulation examples are given in §V. Finally, concluding remarks and future research directions are discussed in §VI.

Preliminaries and Notations: The set of real numbers will be denoted as R, the 1-dimensional manifold on the unit circle as S 1 , and SE(2) = R 2 × S 1 is the Special Euclidean Group 2. The standard Euclidean 2-norm for vectors is denoted . . Directed graphs and the matrices associated with them will be widely used in this work; see, e.g., [START_REF] Godsil | Algebraic Graph Theory[END_REF]. A directed graph G is specified by a vertex set V, an edge set E ⊆ V × VThe neighborhood of the vertex i is the set N i = {j ∈ V | (i, j) ∈ E}, and the out-degree of vertex i is d out (i) = |N i |. The incidence matrix E(G) ∈ R |V|×|E| is a {0, ±1}-matrix with rows and columns indexed by the vertices and edges of G [START_REF] Godsil | Algebraic Graph Theory[END_REF].The complete directed graph, denoted K |V| is a graph with all possible directed edges (i.e. |E| = |V| (|V| -1)). The graph Laplacian of the matrix G is defined as

L(G) = E(G)E(G) T .

II. PARALLEL RIGIDITY THEORY

In this section we briefly review some fundamental concepts of parallel rigidity [START_REF] Eren | Formation shape control based on bearing rigidity[END_REF], [START_REF] Eren | Sensor and network topologies of formations with direction, bearing, and angle information between agents[END_REF]. For an overview on distance rigidity theory, please see [START_REF] Asimow | The Rigidity of Graphs, II[END_REF], [START_REF] Jackson | Notes on the Rigidity of Graphs[END_REF]. Parallel rigidity is built upon the notion of a bar-and-joint framework, denoted (G, p), where G = (V, E) is a directed graph and p : V → R 2 is a function mapping each node of the graph to a point in Euclidean space. In the following we denote by p(V) = p(v 1 ) T • • • p(v |V| ) ∈ R 2|V| the stacked position vector for the framework.

Parallel rigidity is concerned with angles formed between pairs of points and the lines joining them (i.e. the edges in the graph). These angles are measured with respect to some common reference frame. Two frameworks are said to be equivalent if 2 The notation x ⊥ denotes a π 2 counterclockwise rotation of x.

((p 1 (v i ) -p 1 (v j )) ⊥ ) T (p 2 (v i ) -p 2 (v j )) = 0 for all {v i , v j } ∈ E, 2 and congruent if ((p 1 (v i ) - p 1 (v j )) ⊥ ) T (p 2 (v i ) -p 2 (v j )) = 0 for all pairs v i , v j ∈ V.
Observe that for two frameworks to be congruent requires that the line segment between any pair of nodes in one framework is parallel to the corresponding segment in the other framework. Thus, two parallel congruent frameworks are related by an appropriate sequence of rigid-body translations and dilations of the framework.

Definition II.1 (Global Rigidity). A framework (G, p) is parallel globally rigid if all parallel equivalent frameworks to (G, p) are also parallel congruent to (G, p).

Consider now a trajectory defined by the time-varying position vector q(t) ∈ R 2|V| . We consider trajectories that are equivalent to a given framework (G, p) for all time. This induces a set of linear constraints that can be expressed as

((p(v i ) -p(v j )) ⊥ ) T ( qi (t) -qj (t)) = 0 (1)
for all {v i , v j } ∈ E. Here we employed a short-hand notation q i (t) to denote the position of node v i in the time-varying framework (G, q(t)). The velocities q(t) that satisfy the above constraints are referred to as the infinitesimal motions of a framework. Frameworks with infinitesimal motions that satisfy (1) and result in only rigid-body translations and dilations are known as infinitesimally rigid.

The |E| linear constraints given in ( 1) can be equivalently written in matrix form as

R ,G (p(V)) q(t) = 0. (2)
The matrix R ,G (p(V)) ∈ R |E| × 2|V| is referred to as the parallel rigidity matrix. The null-space of these matrices thus describe the infinitesimal motions. The main result of this section is summarized below.

Theorem II.2. A framework (G, p) is parallel infinitesimally rigid if and only if rk[R ,G (p(V))] = 2|V| -3. Furthermore, the three dimensional null-space of the parallel rigidity matrix are correspond to rigid-body translations and dilations of the framework.

III. RIGIDITY IN SE(2)

The concepts of distance and parallel rigidity introduced in §II provides a framework for describing formation shapes in R 2 . In this section, we extend these notions of rigidity for frameworks that are embedded SE(2). Our discussion follows closely the presentation of rigidity given in [START_REF] Asimow | The Rigidity of Graphs, II[END_REF], [START_REF] Krick | Stabilisation of infinitesimally rigid formations of multi-robot networks[END_REF]. To begin, we first modify the traditional bar-and-joint framework to handle points in SE(2).

Definition III.1. An SE(2) framework is the triple (G, p, ψ), where G = (V, E) is a directed graph, p : V → R 2 and ψ : V → S 1 maps each vertex to a point in SE(2) = R 2 × S 1 .
We denote by χ(v) = (p(v), ψ(v)) ∈ SE(2) the position and attitude vector of node v ∈ V. For notational convenience, we will refer to the vectors χ p = p(V) ∈ R 2|V| and χ ψ = ψ(V) ∈ S 1 |V| as the position and attitude components of the complete framework configuration. The vector χ(V) ∈ SE(2) |V| is the stacked position and attitude vector for the complete framework. We also denote by χ x p ∈ R |V| (χ y p ) as the x-coordinate (y-coordinate) vector for the framework configuration.

The defining feature of rigidity in SE( 2) is the specification of formations that maintain the relative bearing angle between points in the framework with respect to the local frame of each point. This is motivated by scenarios where a robot in a multi-robot team is able to measure the relative bearing between itself and other robots. The explicit use of directed graphs in the definition of SE(2) frameworks reinforces this motivation when considering that relative bearing sensors are likely to be attached to the body frame of the robots, and will have certain constraints such as fieldof-view restrictions that may exclude certain measurements, and in particular, bidirectional or symmetric measurements.

In this venue, we assume that a point χ(v) ∈ SE(2) has a bearing measurement of the point χ(u) if and only if the directed edge (v, u) belongs to the graph G (i.e., (v, u) ∈ E); this measurement is denoted β vu ∈ S 1 . The relative bearing is measured from the body coordinate system of that point.

We define the directed bearing rigidity function associated with the

SE(2) framework, b G : SE(2) |V| → S 1 |E| , as b G (χ(V)) = β e1 • • • β e |E| T ; (3)
we use the notation e i ∈ E to represent a directed edge in the graph and assume a labeling of the edges in G.

The bearing measurement can be equivalently written as a unit vector pointing from the body coordinate of the point χ(v) to the point χ(u), i.e.,

r vu (p, ψ) = r x vu r y vu = cos(β vu ) sin(β vu ) , (4)
which also satisfies the relationship β vu = atan r y vu r x vu . Observe, therefore, that the bearing measurement can be expressed directly in terms of the relative positions and attitudes of the points expressed in the world frame,

r vu (p, ψ) = cos(ψ(v)) sin(ψ(v)) -sin(ψ(v)) cos(ψ(v)) (p(u) -p(v)) p(v) -p(u) = T (ψ(v)) T (p(u) -p(v)) p(v) -p(u) = T (ψ(v)) T p vu ,
where the matrix T (ψ(v)) is a rotation matrix from the world frame to the body frame of agent v, and p vu is a shorthand notation for describing the normalized relative position vector from v to u. We now introduce formal definitions for rigidity in SE(2), and for the notions of equivalent and congruent formations in SE(2) frameworks.

Definition III.2 (Rigidity in SE(2)). The SE(2) framework (G, p, ψ) is rigid in SE(2) if there exists a neighborhood S of χ(V) ∈ SE(2) |V| such that b -1 K |V| (b K |V| (χ(V))) ∩ S = b -1 G (b G (χ(V))) ∩ S, where b -1 K |V| (b K |V| (χ(V))) ⊂ SE(2) denotes the pre-image of the point b K |V| (χ(V)) under the directed bearing rigidity map. The SE(2) framework (G, p, ψ) is roto-flexible in SE(2) if there exists an analytic path η : [0, 1] → SE(2) |V| such that η(0) = χ(V) and η(t) ∈ b -1 G (b G (χ(V))) -b -1 K |V| (b K |V| (χ(V)))
for all t ∈ (0, 1].

This definition states that an SE(2) framework (G, p, ψ) is rigid if and only if for any point q ∈ SE(2) sufficiently close to χ(V) with b G (χ(V)) = b G (q), that there exists a local bearing preserving map of SE(2) taking χ(V) to q. The term roto-flexible is used to emphasize that an analytic path in SE(2) can consist of motions in the plane in addition to angular rotations about the body axis of each point. Frameworks (G, p, ψ) and (G, q, φ) are bearing equivalent if

T (ψ(u)) T p uv = T (φ(u)) T q uv , (5)
for all (u, v) ∈ E and are bearing congruent if

T (ψ(u)) T p uv = T (φ(u)) T q uv and T (ψ(v)) T p vu = T (φ(v)) T q vu ,
for all u, v ∈ V.

Definition III.4 (Global rigidity of SE(2) Frameworks). A framework (G, p, ψ) is globally rigid in SE(2) if every framework which is bearing equivalent to (G, p, ψ) is also bearing congruent to (G, p, ψ).
Parallel rigidity is built on frameworks where the underlying graph is undirected. On the other side rigidity in SE( 2) is explicitly defined for directed graphs. We now define a corresponding notion of infinitesimal rigidity for SE(2) frameworks. Using the language introduced in Definition III.2, we consider a smooth motion along the path η with η(0) = χ(V) such that the initial rate of change of the directed bearing rigidity function is zero. All such paths satisfying this property are the infinitesimal motions of the SE(2) framework, and are characterized by the null-space of the Jacobian of the directed bearing rigidity function, ∇ χ b G (χ(V)), as can be seen by examining the first-order Taylor expansion of the directed bearing rigidity function.

In this venue, we introduce the directed bearing rigidity matrix, B G (χ(V)) as the Jacobian of the directed bearing rigidity function,

B G (χ(V)) := ∇ χ b G (χ(V)) ∈ R |E|×3|V| . (6) If a path η is contained entirely in b -1 K |V| (b K |V| (χ(V))
) for all t ∈ [0, 1], then the infinitesimal motions are entirely described by the tangent space to b -1

K |V| (b K |V| (χ(V))
), that we denote by T p . Furthermore, the space T p must therefore be a subspace of the kernel of the directed bearing rigidity matrix for any other graph G, i.e. T p ⊆ N [B G (χ(V))]; this follows from the definition of roto-flexible frameworks given in Definition III.2. This leads us to a formal definition for infinitesimal rigidity of frameworks in SE(2).

Definition III.5 (Infinitesimal Rigidity in

SE(2)). An SE(2) framework (G, p, ψ) is infinitesimally rigid if N [B G (χ(V))] = N B K |V| (χ(V)) . Otherwise, it is in- finitesimally roto-flexible in SE(2).
Definition III.5 leads to the main result of this section which relates the infinitesimal rigidity of an SE(2) framework to the rank of the directed bearing rigidity matrix.

Theorem III.6. An SE(2) framework is infinitesimally rigid if and only if rk[B G (χ(V))] = 3|V| -4.

Before proceeding with the proof of Theorem III.6, we first examine certain structural properties of N [B G (χ(V))]. First, we observe that the infinitesimal motions of an SE(2) framework are composed of motions in R 2 with motions in S 1 for each point. For an infinitesimal motion δχ ∈ N [B G (χ(V))], let δχ p denote the velocity component of δχ in R 2|V| and δχ ψ be the angular velocity component in R |V| .

Proposition III.7.

Every infinitesimal motion δχ ∈ N [B G (χ(V))] satisfies R ,G (χ p )δχ p = -R ψ (χ p )δχ ψ , where R ,G (χ p ) is the parallel rigidity matrix 3 defined in (2) and R ψ (χ p ) = D G (χ p )E T (G) with D G (χ p ) = diag{ℓ 2 e1 , • • • , ℓ 2
e |E| } a diagonal matrix containing the distances squared between all pairs of nodes defined by the edge-set E, and the matrix E ∈ R |V|×|E| is defined as

[E(G)] ik = 1, if e k = (v i , v j ) ∈ E 0, o.w. .
Proof. The result in Proposition III.7 is obtained directly from the evaluation of the Jacobian of the directed bearing rigidity function.

The first observation from Proposition III.7 is the relationship between the infinitesimal motions of an SE(2) framework and those of a parallel rigid framework. Indeed, if all agents maintain their attitude, i.e. when δχ ψ = 0, then the constraint reduces to the constraints for parallel rigidity. The corresponding infinitesimal motions are then the translations and dilations of the framework.

If the angular velocities of the agents are non-zero, then the infinitesimal motions of the framework correspond to what we term the coordinated rotations of the framework. A coordinated rotation consists of an angular rotation of each agent about its own body axis with a rigid-body rotation of the framework in R 2 . The coordinated rotations that satisfy Proposition III.7 are thus related to the subspace

R (G) = IM R ,G (χ p ) ∩ IM {-R ψ (χ p )} ⊂ R |E| ,
that we term the coordinated rotation subspace. Formally, the coordinated rotations can be constructed as

δχ p ∈ R -1 ,G [R (G)],
and

δχ ψ = -R † ψ (χ p )R ,G (χ p ) δχ p , where by (A) -1 [W ]
we mean the pre-image of the set W under the mapping A, and M † is the left-generalized inverse of the matrix M .

Proposition III.8. The coordinated rotation subspace is nontrivial. Equivalently, dim R (G) ≥ 1.

Proof. We prove this by explicitly constructing a vector in the coordinated rotation subspace. Consider a rigid-body rotation of the framework in R 2 described by

z p = I |V| ⊗ 0 1 -1 0 χ p .
It is a straight-forward exercise to verify that R ,G (χ

p )z p = D G (χ p )1 |E| . Furthermore, from the construction of E it follows that E T 1 |V| = 1 |E(G)| and therefore R ,G (χ p )z p = D G (χ p )E T (G)1 |V| concluding the proof.
The proof of Proposition III.8 shows how a coordinated rotation can be constructed for any SE(2) framework. Each point in the framework should rotate about its own axis at the same rate as the rigid-body rotation of the formation. This can be considered the SE(2) extension of the infinitesimal motions associated with distance rigidity. Proposition III.8 can now be used to make a stronger statement about the coordinated rotation subspace for the complete graph.

Proposition III.9. For the complete directed graph

K |V| , dim R (K |V| ) = 1.
Proof. The proof of Proposition III.8 constructs one vector in the coordinated rotation subspace. Assume that dim R (K |V| ) > 1. Then there must exist at least one other coordinated rotation that is orthogonal to the one constructed in Proposition III.8 and contains a non-trivial angular rotation of points in the framework. Note that in Proposition III.8 each agent was assigned a unit angular velocity in the same (counter-clockwise) direction. Thus, any other choice for angular velocities must either be described by each point rotating in the same direction, but non-uniform velocities, or at least two points rotating in opposite directions.

Considering this observation, it is sufficient to see if such a motion can be constructed for the graph K 2 . In this situation, E(K 2 ) = I 2 and one can directly conclude from Proposition III.7 that there can be no additional coordinated rotation then the one described.

Corollary III.10. An SE(2) framework is infinitesimally rigid in SE(2) if and only if

1) rk[R ,G (χ p )] = 2|V| -3 and 2) dim{R (G)} = 1.
Proof. The statement follows directly from Definition III.5, Proposition III.7 and Proposition III.8.

We are now ready to prove Theorem III.6.

Proof of Theorem III.6. Assume that rk[B G (χ(V))] = 3|V| -4. From Propositions III.7 and III.9 we conclude that rk[B K |V| (χ(V))] = 3|V|-4. By definition III.5, we conclude that the SE(2) framework (G, p, ψ) is infinitesimally rigid. Assume now that the SE(2) framework is infinitesimally rigid. By corollary III.10, we conclude

rk[R ,G (p(V)] = 2|V| -3 and dim{R G } = 1. Therefore, rk[B G (χ(V))] = 3|V| -4.
While the general structure of the coordinated rotation subspace can be difficult to characterize for arbitrary graphs, it does lead to a necessary condition on the underlying graph of the framework for infinitesimal rigidity.

Proposition III.11. If an SE(2) framework is infinitesimally rigid, then d out (v) ≥ 1 for all v ∈ V.
Proof. Assume that there exists a node v ∈ V such that d out (v) = 0. Then a solution to the expression in Proposition III.7 is δχ p = 0 and [δχ ψ ] i = 1 if i corresponds to node v and 0 otherwise. This motion does not belong to the subspace T p and therefore rk[B G (χ(V))] > 3|V|-4 and the framework is not infinitesimally rigid.

IV. ESTIMATION OF RELATIVE POSITIONS

Achieving high-level objectives such as formations for multi-robot systems require that all robots have knowledge of a common reference frame. This is to ensure that their velocity inputs vectors are all consistent when maneuvering to achieve the common formation task. However, often the sensed data that is available, such as a relative bearing measurement, is measured from the local body frame of each agent. Furthermore, agents do not have access to a global coordinate system. A requirement for multi-robot systems, therefore, is the ability to estimate a common reference frame in order to express to relative position information. This section describes how the results from §III can be used to distributedley estimate a common reference frame from only the relative bearing measurements.

In this direction, we consider an infinitesimally rigid SE( 2) framework (G, p, ψ). We assume that there are two points in the framework whose Euclidean distance is unknown but positive and constant; these points are indexed as ι and κ (i.e., the position of agent ι is p(ι)). Denote with ξιi ∈ R 2 the estimate of the quantity

ξ ιi = T (ψ(ι)) T p(i) -p(ι) p(ι) -p(κ) (7)
i.e., the relative position (expressed in the body frame of agent ι) of a virtual point that is on the line connecting agent ι and a generic agent i and whose distance from ι is p(i)-p(ι) p(ι)-p(κ) . Denote then with θi ∈ S 1 the estimate of the angle ϑ(i) defined by

T (ϑ(i)) = T (ψ(i)) T T (ψ(ι)), (8)
whose role will be clear in the following. Define then the following quantities:

ξij = ξιj -ξιi , rij = T ( θi ) ξij ξij , βij = atan2(r y ij , rx ij ). (9)
Thus the quantity ξij is an estimate of the relative position vector from i to j, scaled by the quantity p(ι)p(κ) , and expressed in a common reference frame whose origin is p(ι) and orientation is ψ(ι). Notice that ξij represents an unscaled estimate (in the sense explained in the Introduction) of the actual relative position between the agents. Similarly, the estimate of the attitude of the point i can be obtained from [START_REF] Krick | Stabilisation of infinitesimally rigid formations of multi-robot networks[END_REF].

The important fact is that if θ(i) = ϑ(i) and ξιi is equal to [START_REF] Tay | Generating isostatic frameworks[END_REF] we obtain (using also [START_REF] Krick | Stabilisation of infinitesimally rigid formations of multi-robot networks[END_REF]) that

rij = T (ϑ(i))T (ψ(ι)) T p(i) -p(j) p(i) -p(j) = T (ψ(i)) T p(i) -p(j) p(i) -p(j) = r ij ,
which justifies the fact that rij and βij represent our estimates of r ij (p, ψ), and β ij , respectively, defined in (4).

Our goal can be then recast as the design of an estimator that is able to compute ξιi and θ(i) for all i = 1 . . . |V| using the bearing measurements that corresponds to each directed edge of E. In order to do so we consider the following estimation error:

e( ξ, θ, p, ψ) = b G (χ(V)) -bG ( ξ, θ) (10)
where bG ( ξ, θ) ∈ R |E| is the vector of estimated relative bearings obtained from [START_REF] Aspnes | A theory of network localization[END_REF]. The objective of the estimation algorithm can be then stated as the minimization of the following scalar function

J(e) = 1 2 ke e( ξ, θ, p, ψ) 2 + k1 ξιι 2 + k2( ξικ 2 -1) 2 + k3(1 -cos θ(ι)) , (11)
where the nonnegative terms k 1 ξιι 2 , k 2 ( ξικ 2 -1) 2 and k 3 (1cos ϑ(ι)) account for the fact that at steady state the estimator should let ξιι converge to 0, ξικ converge to 1, and θ(ι) converge to 0. The positive gains k e , k 1 , k 2 , and k 3 are introduced here to tune the priority of the single error components within the overall error.

Minimization of ( 11) can be achieved by following the antigradient of J(e), i.e., by choosing:

ξ θ = -k e (∇ ( ξ, θ) e) T e -g( ξ, θ) (12)
where

g( ξ, θ) = • • • k1 ξι • • • k2( ξT κ ξκ -1) ξκ • • • k3 sin θ(ι) • • • T ,
and the terms k 1 ξιι , k 2 ( ξT ικ ξικ -1) ξικ , and k 3 sin θ(ι) appear at the ι-th and κ-th entry pairs of ξ and ι-th entry of θ, respectively, and all the other terms are zero.

As a matter of fact, considering that b G (χ(V)) is constant, the Jacobian of e( ξ, θ, p, ψ) can be expressed in terms of the directed bearing rigidity matrix as

∇ ( ξ, θ) e = -D -1 G ( ξ)R ,G ( ξ) E(G) T . (13)
Note that the form above is consistent with Proposition III.7, which can be obtained from the directed bearing rigidity matrix using an appropriate permutation matrix.

Proposition IV.1. If the framework (G, p, ψ) is (infinitesimally) rigid in SE(2) then the vector of true values

T (ψ(ι)) p(1) -p(ι) p(ι) -p(κ) T • • • T (ψ(ι)) p(|V|) -p(ι) p(ι) -p(κ) T ϑ(1) . . . ϑ(|V|)
T is an isolated local minimizer of e. Therefore, there exists an ǫ > 0 such that, for all initial conditions ( ξT 0 , θ0 ) T whose distance from the true values is less than ǫ, the estimation ξ and θ converge to the true values.

Proof. If the framework is infinitesimally rigid in SE(2), then in any sufficiently small neighborhood of the true bearing values, the only configurations that result e( ξ, θ, p, ψ) 2 being zero in [START_REF] Zelazo | Rigidity maintenance control for multi-robot systems[END_REF] are the trivial motions of the true values (i.e. the rigid-body translations, dilations, and coordinated rotations). For the true values the remaining terms of [START_REF] Zelazo | Rigidity maintenance control for multi-robot systems[END_REF] are zero and therefore is J(e) = 0. If any non-zero trivial motion is applied to the true values then at least one of the remaining terms in J(e) becomes positive. This means that the true values is an isolated local minimizer of [START_REF] Zelazo | Rigidity maintenance control for multi-robot systems[END_REF] and that the J(e) is locally convex around the true values. Therefore gradient descent is enough to converge to the true values if the initial error is sufficiently small.

V. SIMULATION EXAMPLE In this section we report a simulation example to illustrate the relative position estimator of Sect. IV. The simulation involves |V| = 6 agents over an SE(2) infinitesimally rigid framework shown in Fig. 1(a). The following gains were employed: k e = 5, k 1 = k 2 = k 3 = 100. The initial conditions ξ(t 0 ) and θ(t 0 ) for the estimator [START_REF] Zelazo | Decentralized Rigidity Maintenance Control with Range-only Measurements for Multi-Robot Systems[END_REF] were taken as their real values plus a (small enough) random perturbation. Figures 1(b-c) report the results for the simulation. In Fig. 1(b) is shown the behavior of e(t), the error vector between the measured and estimated bearing angles as defined in [START_REF] Calafiore | A distributed gradient method for localization of formations using relative range measurements[END_REF]. We note that under the action of the estimator [START_REF] Zelazo | Decentralized Rigidity Maintenance Control with Range-only Measurements for Multi-Robot Systems[END_REF], all the |E| components of e(t) converge to zero as expected owing to the infinitesimal rigidity of the considered framework. Next, Fig. 1(c) shows the trajectories of ξιi (t) and ψi (t) on the plane (with ψi (t) obtained from [START_REF] Krick | Stabilisation of infinitesimally rigid formations of multi-robot networks[END_REF] when evaluated upon the estimated θi ): here, the real (and constant) poses (p, ψ) are indicated by square symbols and thick green arrows, while the initial ξιi (t 0 ) and ψi (t 0 ) are represented by small circles and dashed black arrows. We can thus note how the estimated position and orientation of every agent converges towards its real value.

VI. CONCLUSION

This work proposed a distributed estimator for estimating the unscaled relative positions of a team of agents in a common reference frame. The key feature of this work is the estimation only requires bearing measurements that are expressed in the local frame of each agent. The estimator builds on a corresponding extension of rigidity theory for frameworks in SE [START_REF] Franchi | Shared control: Balancing autonomy and human assistance with a group of quadrotor UAVs[END_REF]. The main contribution of this work, therefore, was the characterization of infinitesimal rigidity in SE [START_REF] Franchi | Shared control: Balancing autonomy and human assistance with a group of quadrotor UAVs[END_REF]. It was shown that infinitesimal rigidity of the framework is related to the rank of the directed bearing rigidity matrix. The null-space of that matrix describes the infinitesimal motions of an SE(2) framework, and include the rigid body translations and dilations, in addition to coordinated rotations.

Definition III. 3 (

 3 Equivalent and Congruent SE(2) Frameworks).

Fig. 1 .

 1 Fig. 1. Simulation with an SE(2) infinitesimally rigid framework.

It has a position coordinate in R

and an attitude on the 1-dimensional manifold on the unit circle, S 1 .

The parallel rigidity matrix as shown in Proposition III.7 is slightly different then what was presented in[START_REF] Franchi | Shared control: Balancing autonomy and human assistance with a group of quadrotor UAVs[END_REF]. Proposition III.7 explicitly considers directed graphs, and therefore, a bidirectional edge results in two identical rows in Proposition III.7, whereas in (2) it is treated as a single edge.