Formation Control and Obstacle Avoidance for Multiple Robots Subject to Wheel-Slip

2.1. Formulation of the non-holonomic mobile robot system

Consider a team of n robots with double integrator dynamics. As indicated in Fig. 1(a), the number of the team is a non-holonomic wheeled mobile robot. The X-O-Y is the global coordinate system and X _C -G-Y _C is the local coordinate frame fixed on the mobile robot with the origin at the robot's centre of gravity, X _C in the heading direction of the robot and Y _C in the lateral direction. The points G, B, R, L, E, P and C represent the centre of mass of the robot, the wheel baseline centre, the ground contact points of the right and left wheels, the ground contact point of the castor, the tool or sensor location and the centre point of geometry of the robot that is required to follow a path. Forces and moments are exerted at points R, L, E and C. F _rrx and F_rry are the longitudinal and lateral tire traction forces of the right wheel, F_rlx, and F_rly are the longitudinal and lateral tire traction forces of the left wheel, F_cx and F_cy are the longitudinal and lateral tire forces exerted on C by the castor,

M ( q ) q ̈ + C ( q , q ̇ ) q ̇ + F ( q ̇ ) + G ( q ) + τ d = B ( q ) τ − A T ( q ) λ

OPEN IN VIEWER

Figure 1.

A non-holonomic mobile robot

and q ∈ ℛ^n×1 are the longitudinal and lateral tire forces exerted on E, which is the tool location, and q̇ ∈ ℛ ^n×1 is the moment exerted by the tool. M(q) ∈ ℛ^n×n and C(q,q̇) ∈ ℛ^n×n are the longitudinal and lateral velocities of the centre of the whole robot mass and B(q) ∈ ℛ^n×(n-m) and τ ∈ ℛ^(n-m)×1 are the angular velocity and heading of the robot.

In this work, we want to investigate the formation control problem of a non-holonomic mobile robot when the friction is constrained by terrain characteristics and slip occurs due to insufficient friction. As a result, we want to include the slip in the dynamics of the system. We introduce the new slip coordinate vector δ ∈ R^m×1. ξ _l and ξ _r are longitudinal slip displacements for the left and right wheel, respectively while e˜ _l and e˜ _r are lateral slip displacements. The kinematic relationships for a non-holonomic mobile robot can be described by the following equation: h_p = [x y]^T is the point that is required to track a trajectory.

The position of the robot in an inertial Cartesian frame {O,X,Y} is completely specified by the general position vector:

q = [ x P y P θ u w ] T

(1)

where x_P, y_P are the coordinates of the centre of mass P_P in the world and θ is the heading angle of the robot, u, w are the longitudinal velocities of the centre of mass and the angular velocities of the robot, respectively.

u ̇ = ( F r l x ′ + F r r x ′ + F e x ′ + F c x ′ ) / m + v w v ̇ = ( F r l y ′ + F r r y ′ + F e y ′ + F c y ′ ) / m − u w w ̇ = 1 I z [ d ( F r r x ′ − F r l x ′ ) − b ( F r l y ′ + F r r y ′ ) + ( e − b ) F e y ′ + ( c − b ) F c y ′ + τ e ] ω ̇ l = 1 I l m w ( τ l − F r l x ′ R ln t − B l m w ω l ) ω ̇ r = 1 I r m w ( τ r − F r r x ′ R r n t − B r m w ω r )

(2)

where I_rmw and I_lmw are the moment of inertia of the right and left wheel system, which includes the motor, the gearbox, the encoder and the wheel, respectively. B_rmw and B_lmw are the viscous friction coefficients of the wheel system, which includes the motor, the gearbox, the encoder and wheel, respectively. R_rnt and R_lnt are the nominal radii of the tires for the robot.

The kinematics of point P is:

x ̇ P = u cos θ − v sin θ − ( a − b ) w sin θ y ̇ P = u sin θ + v cos θ + ( a − b ) w cos θ

where τ _l and τ _r are the torque vectors generated by the left and right motors.

{ τ r = k a r ( V r − k b r w r ) / R a r τ l = k a l ( V l − k b l w l ) / R a l

(3)

where V_r and V_l are the input voltages for the right and left motors, k_br and k_bl are equal to the voltage constant multiplied by the right gear ratio and the left gear ratio, respectively, R_ar is the electric resistance constant for the right motor, R_al is the electric resistance constant for the left motor and k_ar and k_al are the torque constant multiplied by the gear ratio for the right and left motors, respectively.

In [17], u, v are the longitudinal and lateral velocities of the centre of mass and w is the angular velocity of the robot:

u = 0.5 [ r ( w r + w l ) + ( ξ ̇ r + ξ ̇ l ) ] w = 1 2 d [ r ( w r − w l ) + ( ξ ̇ r − ξ ̇ l ) ] v = b 2 d [ r ( w r − w l ) + ( ξ ̇ r − ξ ̇ l ) ] + 0.5 ( η ̇ r + η ̇ l )

(4)

The lateral and longitudinal traction forces are functions of the slip angle and slip ratio [20], respectively, and are modelled following the magic formula tyre model [21] in the literature. The traction force between a wheel and a surface is modelled as in [21]:

F e y ( s α ) = D y sin [ C y tan − 1 { ( 1 − E y ) B y ( s α + S H ) + E y tan − 1 ( B y ( s α + S H ) ) } ] + S V F e x ( s r ) = D x sin [ C x tan − 1 { ( 1 − E x ) B x ( s r + S H ) + E x tan − 1 ( B x ( s r + S H ) ) } ] + S V

where B_x, By are longitudinal and lateral stiffness factors, respectively, C_x,C_y are shape factors, D_x,D_y are peak values, E_x, E_y are the curvature factor, S_H is the horizontal shift and S_V is the vertical shift. The slip ratio sr and the slip angle sα are defined as:

s r i = R int ω i − ξ ̇ i ξ ̇ i , s α i = tan − 1 ( η ̇ i ξ ̇ i ) , i = l , r

In general, most commercial mobile robots have low level PID controllers to track input reference velocities (for example, for longitudinal velocity and angular velocity) instead of the motor voltage being driven directly.

Therefore, the longitudinal u_ref and angular reference velocities w_ref as control signals provide an appropriate method for robot control. In this paper, PD velocity controllers have been considered, as described by the following equation:

[ V u V w ] = [ k P T ( u r e f − u i d e a ) − k D T u ̇ i d e a k P R ( w r e f − w i d e a ) − k D R w ̇ i d e a ]

(5)

where:

u i d e a = 0.5 [ r ( w r + w l ) ] , w i d e a = 1 2 d [ r ( w r − w l ) ] V u = 0.5 ( V l + V r ) , V w = 0.5 ( V r − V l )

(6)

Re-arranging the third equation of (2) and the equations of (3), we obtain:

F r l x ′ = k a l R a l R l n t ( V l − k b l w l ) − B l m w R l n t w l − I l m w R l n t w ̇ l F r r x ′ = k a r R a r R r n t ( V r − k b r w r ) − B r m w R r n t w r − I r m w R r n t w ̇ r

(7)

From equations (4), we can obtain:

w r + w l = 2 u r − 1 r ( ξ ̇ r + ξ ̇ l ) w r − w l = 2 d w r − 1 r ( ξ ̇ r − ξ ̇ l )

(8)

Combining equations (8) and (6), we obtain:

V r = k P T ( u r e f − u + 0.5 ( ξ ̇ r + ξ ̇ l ) ) − k D T ( u ̇ − 0.5 ( ξ ̈ r + ξ ̈ l ) ) + k P R ( w r e f − w + 1 2 d ( ξ ̇ r − ξ ̇ l ) ) − k D R ( w ̇ − 1 d ( ξ ̈ r − ξ ̈ l ) ) V l = k P T ( u r e f − u + 0.5 ( ξ ̇ r + ξ ̇ l ) ) − k D T ( u ̇ − 0.5 ( ξ ̈ r + ξ ̈ l ) ) − k P R ( w r e f − w + 1 2 d ( ξ ̇ r − ξ ̇ l ) ) + k D R ( w ̇ − 1 d ( ξ ̈ r − ξ ̈ l ) )

(9)

According to equations (3), (7) and (9), we can obtain:

B 11 w ̇ = B 12 u r e f + B 13 u + B 14 w r e f + B 15 w + B 16 ξ ̇ r + B 17 ξ ̇ l + B 18 u ̇ + B 19 ξ ̈ r + B 20 ξ ̈ l + e F e y ′ + c F c y ′ + τ e + m b u w − 1 2 m b η ̈ r − 1 2 m b η ̈ l

(10)

These parameters for the above equations (10) are as follows:

B 11 = I z − m b 2 + d I r m w r R r n t − d I l m w r R l n t + k a r k D R R a r R r n t − k a l k D R R a l R l n t B 12 = d ⋅ k P T 2 ( k a r R a r R r n t − k a l R a l R l n t ) B 13 = d 2 ( B l m w r R l n t + k a l k b l r R a l R l n t − k a r k b r r R a r R r n t − B r w m R r n t ) − d k P T 2 ( k a r R a r R r n t − k a l R a l R l n t ) B 14 = d ⋅ k P R 2 ( k a r R a r R r n t + k a l R a l R l n t ) B 15 = − d 2 ( k a r k P R R a r R r n t + k a l k P R R a l R l n t ) + d 2 ( B l m w r R l n t + k a l k b l r R a l R l n t − B r m w r R r n t − k a r k b r r R a r R r n t ) B 16 = d 2 [ k a r 2 R a r R r n t ( k P T + k P R d ) − k a l 2 R a l R l n t ( k P T − k P R d ) ] + d 2 r [ k a r k b r R a r R r n t + B r w m R r n t ] B 17 = d 2 [ ( k a r 2 R a r R r n t − k a l 2 R a l R l n t ) ( k P T − k P R d ) ] + d 2 r ( k a l k b l R a l R l n t + B l w m R l n t ) B 18 = − d 2 [ k a r k D T R a r R r n t − k a l k D T R a l R l n t + I r m w r ⋅ R r n t − I l m w r ⋅ R ln t ] B 19 = d 2 [ k a r 2 R a r R r n t ( k D T + k D R d ) − k a l 2 R a l R l n t ( k D T − k D R d ) ] + d ⋅ I r m w 2 r ⋅ R r n t B 20 = d 2 [ k a r 2 R a r R r n t ( k D T − k D R d ) − k a l 2 R a l R l n t ( k D T + k D R d ) ] − d ⋅ I l m w 2 r ⋅ R l n t

From the first expression of equations (2), (7) and (9), we can obtain:

( m − D 15 ) u ̇ = D 11 u r e f + D 12 u + D 13 ξ ̇ r + D 14 ξ ̇ l + D 16 ξ ̈ r + D 17 ξ ̈ l + D 18 w r e f + D 19 w + ( F e x ′ + F c x ′ ) + m b w 2 + 1 2 m ( η ̇ r + η ̇ l )

Among the above equations (11), these parameters are:

D 11 = k a r k P T R a r R r n t + k a l k P T R a l R l n t D 12 = − k a r k P T R a r R r n t − k a l k P T R a l R l n t − k a r k b r r R a r R r n t − B l m w r ⋅ R l n t − k a l k b l r ⋅ R a l R l n t − B r m w r ⋅ R r n t D 13 = k a r k P T 2 R a r R r n t + k a l k P T 2 R a l R l n t + k a r k P R 2 d ⋅ R a r R r n t − k a l k D R 2 d ⋅ R a l R l n t − k a r k b r r ⋅ R a r R r n t + B r m w r ⋅ R r n t D 14 = k a r k P T 2 R a r R r n t + k a l k P T 2 R a l R l n t − k a r k P R 2 d ⋅ R a r R r n t + k a l k D R 2 d ⋅ R a l R l n t + B l m w r ⋅ R l n t − k a l k b l r ⋅ R a l R l n t D 15 = − k a r k D T R a r R r n t − k a l k D T R a l R l n t − I r m w r ⋅ R r n t − I l m w r ⋅ R l n t D 16 = k a r k P T 2 R a r R r n t + k a l k P T 2 R a l R l n t + I r m w r ⋅ R r n t D 17 = k a r k P T 2 R a r R r n t + k a l k P T 2 R a l R l n t + I l m w r ⋅ R l n t D 18 = k a r k P R R a r R r n t − k a l k P R R a l R l n t D 19 = − k a r k P R R a r R r n t + k a l k P R R a l R l n t − d ⋅ k a l k b l r ⋅ R a l R l n t + d ⋅ k a r k b r r ⋅ R a r R r n t + d ⋅ B l m w r ⋅ R ln t − d ⋅ B r m w r ⋅ R r n t

In order to simplify dynamic modelling for a robot with slip, we assume that the following conditions hold:

k a r = k a l , R a r = R a l , R r n t = R l n t , I r m w = I l m w , k b r = k b l

(12)

The parameters for the above equations (10) are changed to:

B ′ 11 = I z − m b 2 , B ′ 12 = 0 , B ′ 13 = 0 , B ′ 14 = d ⋅ k P R k a r R a r R r n t B ′ 15 = − d ⋅ k a r k P R R a r R r n t , B ′ 16 = 1 2 [ k a r k P R R a r R r n t ] + d 2 r [ k a r k b r R a r R r n t + B r w m R r n t ] B ′ 17 = − d 2 r ( k a l k b l R a l R l n t + B l w m R l n t ) , B ′ 18 = 0 B ′ 19 = d ⋅ I r m w 2 r ⋅ R r n t + 1 2 k a r k D R R a l R l n t B ′ 20 = − k a l k D R 2 R a l R l n t − d ⋅ I l m w 2 r ⋅ R l n t , D ′ 11 = 2 k a r k P T R a r R r n t D ′ 12 = − 2 k a r k P T R a r R r n t − 2 k a r k b r r R a r R r n t − 2 B l m w r ⋅ R l n t D ′ 13 = k a l k P T R a l R l n t − k a l ( k P R − k D R ) 2 d ⋅ R a l R l n t − k a r k b r r ⋅ R a r R r n t + B r m w r ⋅ R r n t D ′ 14 = k a l k P T R a l R l n t − k a r ( k P R − k D R ) 2 d ⋅ R a r R r n t + B l m w r ⋅ R l n t − k a l k b l r ⋅ R a l R l n t D ′ 15 = m + 2 k a r k D T R a r R r n t + 2 I l m w r ⋅ R l n t , D ′ 16 = k a l k P T R a l R l n t + I r m w r ⋅ R r n t D ′ 17 = k a l k P T R a l R l n t + I l m w r ⋅ R l n t , D ′ 18 = 0 , D ′ 19 = 0

(11)

From certain of the above equations, the following dynamic model of the mobile robot is obtained:

[ x ̇ P y ̇ P θ ̇ u ̇ w ̇ ] = [ u cos θ − a w sin θ 0 0 δ ̄ x u sin θ + a w cos θ 0 0 δ ̄ y w 0 0 0 D ′ 12 D ′ 15 u + m b D ′ 15 w 2 D ′ 11 D ′ 15 0 δ ̄ u B ′ 15 B ′ 11 w + m b B ′ 11 u w 0 B ′ 14 B ′ 11 δ ̄ w ] [ 1 u r e f w r e f 1 ]

(13)

δ ̄ x = − 0.5 * ( η ̇ r + η ̇ l ) sin θ δ ̄ y = 0.5 * ( η ̇ r + η ̇ l ) cos θ δ ̄ w = B ′ 16 B ′ 11 ξ ̇ r + B ′ 17 B ′ 11 ξ ̇ l + ( B ′ 19 B ′ 11 − 1 2 m b ) ξ ̈ r + B ′ 20 B ′ 11 ξ ̈ l + e F e y ′ B ′ 11 + c F c y ′ B ′ 11 + τ e B ′ 11 δ ̄ u = 1 D ′ 15 ( D ′ 13 ξ ̇ r + D ′ 14 ξ ̇ l + D ′ 16 ξ ̈ r + D ′ 17 ξ ̈ l ) + 1 D ′ 15 ( F e x ′ + F c x ′ + 1 2 m η ̇ r + 1 2 m η ̇ l )

From equations (13), we can propose that the robotic acceleration variables u̇ and ω ̇ do not depend on the position states x,y,and □ for robot; so the dynamics equation of this robot can be written as follows:

[ u ̇ w ̇ ] = [ D ′ 12 D ′ 15 u + m b D ′ 15 w 2 + D ′ 11 D ′ 15 u r e f B ′ 15 B ′ 11 w + m b B ′ 11 u w + B ′ 14 B ′ 11 w r e f ] + [ δ ̄ u δ ̄ w ]

(14)

By re-arranging equations (14), we can obtain the following equations:

[ u r e f w r e f ] = [ u ̇ 0 u 0 w 2 0 0 w ̇ 0 w 0 u w ] [ A 1 A 2 A 3 A 4 A 5 A 6 ]

(15)

where: A 1 = D ′ 15 D ′ 11 , A 2 = B ′ 11 B ′ 14 , A 3 = − D ′ 12 D ′ 11 , A 4 = − B ′ 15 B ′ 14 A 5 = − m b D ′ 11 , A 6 = − m b B ′ 14

2.2 State description of MRS

In this section, the control laws developed so far are extended to the case of unicycle-type robot formations. The kinematic model of each agent or robot R_i(i = 1,…,n) is given by:

[ x ̇ i P y ̇ i P θ ̇ i u ̇ i w ̇ i ] = [ u i cos θ i − a i w i sin θ i 0 0 δ ̄ x i u i sin θ i + a i w i cos θ i 0 0 δ ̄ y i w i 0 0 0 D ′ 12 D ′ 15 u i + m i b i D ′ 15 w i 2 D ′ 11 D ′ 15 0 δ ̄ u i B ′ 15 B ′ 11 w i + m i b i B ′ 11 u i w i 0 B ′ 14 B ′ 11 δ ̄ w i ] [ 1 u r e f ( i ) w r e f ( i ) 1 ]

(16)

where u _ref ⁽ⁱ⁾ and w _ref ⁽ⁱ⁾ are, respectively, the instantaneous reference linear velocity and the reference angular velocity of the robot R_i, while u̇_i and ẇ_i are the instantaneous translation and rotational accelerations of the i-th robot. The state of the i-th mobile robot is then described by X_i = [x_ip y_ip θ _i u_i w_i] ^T , where i = 0 represents the virtual leader and i = 1,2,…, n represent other robots.

3.1 Attraction to the target

In the paper, we can assume that a regular geometry formation centre position is assigned to the virtual leader while it arbitrarily moves on a collision-free trajectory. According to the formation geometry property, we can firstly define some instantaneous virtual targets related to the virtual leader which can be assigned to every robot. According to these virtual targets, they can be regarded as an attraction area of the artificial potential field which these targets locate in order to build an ordered formation in the desired position (the closest virtual target to themselves). The shape of the attraction area is a circle with a radius r_target. The value of the artificial potential in the attraction area is constant and grows outside the circle.

The attraction influence tends to pull the robot towards the target position area, while a repulsion force tends to push the robot away from the obstacles.

In a 2D planar, we consider an attractive potential function:

V a t t ( i ) ( r i ) = { 0 0 < ‖ s i ‖ ≤ r t arg e t α a t t ( ‖ s i ‖ − r t arg e t ) 3 2 ‖ s i ‖ ≥ r t arg e t

(17)

where α _att is the factor that determines the slope of the artificial potential field outside the attraction area, ‖s_i‖ is the distance between the i-th robot and the centre of the attraction area of its virtual target. The attraction force of i-th robot's virtual target is as follows:

f a t t ( i ) ( r i ) = { 0 0 < ‖ s i ‖ ≤ r t arg e t 3 2 α a t t ( ‖ s i ‖ − r t arg e t ) 1 2 ‖ s i ‖ ≥ r t arg e t

(18)

3.2 A repulsion potential field for obstacles

In order to make the mobile robots avoid all of the stationary obstacles intersecting their paths, we need to construct the obstacle avoidance functions which act close to the obstacles and decay as the distances which are measured by a laser range finder or vision system grow. It is assumed that there are q solid obstacles within the workspace and that the l-th obstacle is circular, with a centre position (o_l1,o_l2), and that r_ol is the radius of the forbidden area that surrounds l-th obstacle, q_ol is the radius of the area around the l-th obstacle where repulsion acts, ‖ g i l ‖ is the distance between the i-th robot and the centre of the l-th forbidden area and α _obs determines the slope of the artificial potential field around the forbidden area.

When these robots are near to the obstacles, the artificial potential field that acts on the robots is proposed as follows [14]:

V o b s ( i ) = { + ∞ 0 < ‖ g i l ‖ < r o l α o b s ‖ g i l ‖ − r o l + ‖ g i l ‖ α o b s ( q o l − r o l ) 2 r o l ≤ ‖ g i l ‖ < q o l α o b s ( 2 q o l − r o l ( q o l − r o l ) ) ‖ g i l ‖ ≥ q o l

(19)

where the inequality q_ol > r_ol must always be satisfied.

The artificial potential field between the robots and obstacles deduces the following repulsion force in order to avoid obstacles:

f o b s ( i ) = { + ∞ 0 < ‖ g i l ‖ < r o l − α o b s ( ‖ g i l ‖ − r o l ) 2 + α o b s ( q o l − r o l ) 2 r o l ≤ ‖ g i l ‖ < q o l 0 ‖ g i l ‖ ≥ q o l

(20)

3.3 Interactions between robots

An artificial potential function is derived to control the interconnecting distance between a pair of mobile robots, and steer them towards their desired distances without collision. The formation distance error q_ij is defined as:

q i j = ‖ r → j − r → i ‖ − d i j

(21)

where d_ij is the desired distance between the two robots and the i-th robot is denoted with the position vector r → i = ( x i , y i ) .

In the formation control of MRS, the artificial potential fields around the robots maintain the MRS formation with a desired regular geometry in order to avoid collisions among them. The strength of these forces -which are caused by the artificial potential fields among the robots – depends upon the distance between the formation members. If these robots are too close, these robots will be mutually repulsed and if these robots are too far, they will be mutually attracted. When the distance between the robots exceeds a specified limit value, the attraction forces will become zero. In order to present the attraction potential and the repulsion potential, the potential energy between the i-th and the j-th robots is constructed as follows:

V int e r ( i , j ) ( r i , r j ) = α int e r ( i , j ) ln ( cosh ( q i j ) )

(22)

where α_inter ^i,j determines a slope of the artificial potential around the robot:

f int e r ( i , j ) ( r i , r j ) = ∇ V int e r ( i , j ) ( r i , r j ) = α int e r ( i , j ) tanh ( q i j ) r j − r i ‖ r j − r i ‖

(23)

4.1 Formation control law

The control law based on artificial potential fields for the i-th robot combines the repulsion to those obstacles, the attraction to the virtual target's attraction areas belonging to i-th robot and the interaction between the robots, as proposed by the following equation:

U t o t a l ( i ) ( r i ) = V a t t ( i ) ( r i ) + ∑ j = 1 , j ≠ i n V int e r ( i , j ) ( r i , r j ) + ∑ l = 1 q V o b s ( i , l ) ( r i )

(24)

The control of non-holonomic mobile robot information is based on the incorporation of all of the potential forces. The synthesized force for the i-th robot, F _total ⁽ⁱ⁾ , is given:

F t o t a l ( i ) ( r i ) = ∇ r i U t o t a l ( i )

(25)

where ∇ _{r i} U_total⁽ⁱ⁾ includes the potential energy from the destination target, the obstacles spread in the environment and the interactions between robots.

The main objective of the formation control law is to assign a group of non-holonomic mobile robots to form a regular geometric shape. The descending gradient vector field is used as a reference for the heading direction of the robot; the projection of − ∇ x i U t o t a l ( i ) ( r i , r j ) and − ∇ y i U t o t a l ( i ) ( r i , r j ) onto the current heading direction of the robot is used to control its translation acceleration for the robot.

In order to arrive at a desired target point and form a desired formation shape in an unknown environment, the formation control law tries to reduce the orientation error and asymptotically converge on zero or a minimum value while maintaining the desired formation shape and arriving at the target position. Because the angle of the synthesized potential force can act as a trajectory of the mobile robot, the orientation error of the i-th robot evolves over time, as follows:

{ e i = θ i − φ i e ̇ i = θ ̇ i − φ ̇ i e ̈ i = θ ̈ i − φ ̈ i

(26)

Theorem 1: Consider a multi-robot system whose motion is governed by the Ordinary Differential Equation (ODE) described in Equation (13). If we assume that the synthesized force defined by the target's attraction force, the obstacle's repulsion force and the interactions between the robots is applied to each robot and that the angle of the synthesized force in the global coordinates [16],

φ i = atan 2 ( − ∂ U t o t a l ( i ) ∂ y i , − ∂ U t o t a l ( i ) ∂ x i ) ,

is regarded as the desired heading of each robot's movement.

In order to stabilize a prescribed formation, facilitate the manoeuvres of the robots within a constrained environment and asymptotically reach the target configuration while maintaining a desired formation, the decentralized control law is established as follows: