Collision Avoidance of SDRE Controller using Artificial Potential Field Method: Application to Aerial Robotics*

This work presents the problem of collision avoidance of the state-dependent Riccati equation (SDRE) controller using the artificial potential field (APF) method. Two themes were selected to illustrate the importance of the problem, collision avoidance between the end-effectors of serial links manipulators and unmanned aerial vehicles (UAVs), working in a shared workspace. The structure of the SDRE has a good potential to accommodate APF formulation in the weighting matrix of states. The distance between the end-effectors or the center-of-mass (CoM) of UAVs is penalized to autonomously guide the robots in a collision-free trajectory while they are working in a common environment. If the robots get close to each other, the weighting matrix of states increases, which actuates the systems to escape from a possible collision. Several simulation studies were done to investigate the proposed controller and the effect of collision avoidance function. It was found that the higher power of the collision avoidance function handles the threat of the impact better. The distance between robots was considered as an index to assess the performance of the controller which showed successful results in the simulations.


I. INTRODUCTION
Employing several robots to do a particular task has been highlighted in the literature and led to investigations in cooperative control and manipulations. Cooperation in robotics is getting more frequent in industrial applications, manufacturing production lines, and laboratories. The obvious result of several working drones or manipulators with complex structures and links is to face possible collisions between the systems during operations. To avoid this potential threat, a study of collision avoidance is essential.
The state-dependent Riccati equation is an optimal control policy, possessing nonlinear structure, that provides a good systematic and fruitful control law. The SDRE was introduced in the 1960s and became popular in the 1990s [1]. The application of the SDRE was investigated in robotics. Innocenti et al. presented the manipulator control via the statedependent Riccati equation [2]. The state-dependent coefficient (SDC) parameterization, and the experiment were provided to illustrate the performance of the SDRE approach. Terashima et al. studied the SDRE control in the servo design framework for a two-degree-of-freedom (DoF) planar manipulator [3]. No need for linearization was reported as the advantage of that method, resulting in the easier tuning of weighting matrices. Flexible-link robots were also controlled successfully by the SDRE, which presented very complicated dynamics including large and small deformations [4,5].
Collision avoidance has been an interesting topic for UAV control. Multiple cooperative UAVs were employed for *Research funded by the European Commission H2020 Programme under HYFLIER project contract 779411, AERIAL-CORE project contract number 871479, and the Spanish Project ARM-EXTEND DPI2017-89790-R detection and searching for a region while they were constrained to maintain in a close range and avoid possible collisions [6]. An optimal/suboptimal trajectory search was used to find the best path. A modified Grossberg neural network algorithm was proposed to overcome the collision avoidance problem for the same case [7]. A model predictive control was responsible for tracking the trajectory if that was a feasible solution. Defining a penalty for collisions in the fitness function for UAV trajectory optimization was proposed for free path planning [8]. Fathian et al. presented robust threedimensional distributed formation guidance with collision avoidance and application to multirotor aerial vehicles [9]. The presented research used a path generator for the first loop and a tracker for the second one. The feasibility of the generated trajectory was also an issue. This current work proposes a dynamic collision avoidance approach to provide the collisionfree trajectory in one control loop.
The subject of collision avoidance, in this work, employs the artificial potential field method which has a history in the SDRE control though limited to obstacle avoidance. Villagra et al. presented a flatness-based vehicle steering guidance method with SDRE feedback gains regulated by a sensitivity approach [10]. The obstacle avoidance terms were set in the weighting matrix of states. The same approach was used for manipulators [11], and a cable-driven robot satisfactorily [12]. Spacecraft proximity operation with obstacle constraint was investigated, considering the concept of finite-time stabilization [13]. Mohammadi and Khaloozadeh presented optimal motion planning of unmanned ground vehicles using the SDRE controller in the presence of obstacles [14]. Alirezaei et al. [15] presented a collision-avoidance design using a state-dependent Riccati equation control with experimental robustness assessment. The composite structure of the SDRE, compensative integral mode and artificial potential field method were used to formulate nonlinear control of spacecraft formation flying with an anti-disturbance system and collision avoidance [16]. Moving obstacle avoidance was also studied for wheeled mobile manipulators [17].
The application of multi-robot systems is recently being more visible in the control field. A multi-robot platform highlights the cooperation and the subsequent challenges in the dynamic structure of the plant. Korayem and Nekoo studied a nonlinear sub-optimal SDRE approach for cooperative manipulators to enhance dynamic load-carrying capacity (DLCC) [18]. Two manipulators carried an object in a predefined trajectory. Sharing the load between two arms was solved by an optimal load distribution method. Huang et al. formulated an attitude take over control for the post-capture of target spaceship for a cooperative system [19].
This work incorporates the artificial potential field method within the SDRE control formalism to present collision avoidance safety guard for multi-robot systems, working in the shared environment. Setting the distance between the robots as denominators of weighting matrix ( ) Q x is the consequence of APF application. The subject of collision avoidance was once reported for wheeled mobile robots in Ref. [17], though the concept neither simulated nor analyzed for multiple manipulators or UAVs. So, the main contribution of this current research is to apply the concept of collision avoidance for a system of multiple manipulators and UAVs in a shared workspace. The system of robots, as a team, must avoid any collision in cooperation while they are close to other agents.
II. THE SDRE AND ARTIFICIAL POTENTIAL FIELD METHOD Consider a nonlinear system  [20]: should present a completely controllable pair for all x and [0, ) t   . The SDC parameterization must be designed in a way to guarantee the controllability of the { ( ( )), ( ( ))} t t A x B x in linear sense or at the equilibrium point { ( ), ( )} A 0 B 0 , considering that any term will not violate the controllability condition in the rest of the solution [21].
The performance index of SDRE possesses a quadratic form that tires leveling error rate and energy consumption [1]: is symmetric positive definite.
In order to satisfy the observability condition, the pair of . To guarantee the observability of the nonlinear pair (3), this pair must be observable in a linear sense similar to the controllability condition. The standard form of SDRE control law is found by applying stationary condition on Hamiltonian function as [5]: , is a solution to the state-dependent Riccati equation [22]: The idea of obstacle avoidance is enlightened by an artificial potential field method [23]. To import the APF in the structure of the SDRE, ( ( )) t Q x in (5), the weighting matrix of states should be modified. It has the form of 11 22 ( ( )) 0 0 is i -th diagonal element of the matrix. If the value of any diagonal element in (6) increases, this results in a faster response of the relevant state. As a result, it is common to set the distance between the end-effector of a robot (or CoM of UAVs) and an obstacle as the denominator of ( ( )) ii Q t x to prevent any collision [11]. An SDRE regulator equipped with obstacle avoidance structure provides a controller with safe path planning capability. Collision avoidance benefits from the same point of view. If two or more states of two different systems should not collide with each other during a solution, the distance between them should be set as the denominator of diagonal elements of both or one of the systems , of states for the first system and 2, ( ( )) j h t x is j-th function for the second system; the value of 0 p  defines the intensity of the collision avoidance. Here the system responsible for collision avoidance is system one, consequently 1 ( ( )) t Q x .
The collision avoidance term could be set in two systems though in some cases, this fails the generation of a safe path. Specifically, in a symmetric situation, two similar systems with symmetric initial and final conditions. In an event of a possible collision, both systems try to enhance the relative state to skip the collision. Nevertheless, they fail. This problem will be studied in the simulation section. So, it is strongly recommended to define one of the systems responsible for collision avoidance or set the collision avoidance term without any symmetric structure or gains.
Small or large workspace: The end-effectors of manipulators are usually working in a limited workspace, with the maximum distance between the end-effectors less than 1 meter. In that case 1, ( ( )) in Eq. (7) increases as the collision distance decrease. Therefore, the responsible manipulator for collision avoidance tries to regulate faster towards the final condition. However, for a large workspace, control of UAVs in an open environment, the distance between the drones are usually more than 1 meter. In that case, 1, ( ( )) ii Q t x acts oppositely and weakens the responsible UAV for obstacle avoidance in a possible collision. So, the following condition completes the collision avoidance strategy for the large workspace: d is the distance function, representing the distance between the systems as in which m is 2 for planer collision avoidance of end-effectors and 3 for 3D collision avoidance of UAVs.

A. End-effector Collision Avoidance
The case study of this section is dedicated to collision avoidance between the end-effectors of two manipulators, working in a shared workspace, Fig. 1. The left arm is placed at ( , 0) d  and the right arm at ( , 0) d , with the assumption of symmetric structure and the same kinematics and dynamics parameters. The gravity, M q q C q q q G q q τ M q q C q q q G q q τ The parameters of the robot and the definition of them could be followed in Ref. [24].

 
The end-effector positions were set to define the distance between them as penalty functions: . Change in p increase or decrease the speed and performance of collision avoidance. More discussion will be done in simulations.

B. Hexarotors Collision-free Path Planning
In this case, the workspace is open and free and the systems are two identical hexacopters with fixed tilted rotors. This structure provides a fully actuated structure [25]. The state vector of the system is defined as The index i defines the number of the system. Considering the state vector (10) and assumption of hovering condition is a rotation matrix, m is the total mass of the system, diagonal matrix representing drag forces, 3 3 3 ,2 could be found in Ref. [27]. The SDC parameterization of the system (11) is uniformly defined since the system is fully actuated: 2  3 3   3 3  3 3  3 3  ,2   3 3  3 3  3 3  1  3 3  3 3  3 3 The control law also excludes the gravity from the SDC matrices The collision scalar function is the distance between the two CoM of the UAVs , The scalar function then is located in the relevant diagonal arrays of the responsible UAV for collision avoidance.

A. End-effector Collision Avoidance
The presented dynamics in Section III, a system of two manipulators are considered for simulation, with regards to the parameters in Table I. The time of simulation was set 1 second. The weighting matrices were selected as 2 2 0.01 11 ,22 ( ) ( ) 10 diag( ( ) 1, ( ) 1,1,1), x is based on (9) . The trajectories of the left and right arm are presented in Fig. 2. p was defined 1, and L was set 1 and 0 to turn on/off the collision avoidance guard, to show that without this structure, the manipulators collided with each other in this particular example. In this example, both arms tried to protect the regulation task although the role of the left arm is more highlighted than the right due to the geometry of the start and end-point design. The distance between the endeffectors is depicted in Fig. 3.  The next simulation is provided to discuss the weakness of the collision avoidance of the system and emphasize on the remedy, which is the consideration of the collision avoidance term for one of the robots. The simulation was performed in 5 seconds. The weighting matrices were selected as 2   . In this simulation, the collision avoidance role was assigned to the right arm. The motion of the arms and the parameters are symmetric. So, they must collide without any collision avoidance structure. The trajectories were generated successfully without any collision and the right arm regulated to end-point faster than the left one to skip the impact, Fig. 4.  The verification was successfully bolded the point on one arm protection rather than two. If the safety collision terms in the structure was considered for the two arms, they would definitely collide with each other. The reason is that they both try to speed up, but the symmetric design directs them to each other.

B. Hexarotors Collision-free Path Planning
A symmetric regulation for both UAVs was designed to make sure without a collision avoidance strategy, the systems would crash. The initial and final conditions of the systems are   The mass of the system is 1.
where Q is defined based on (12) with the power of 7 P  . The position variables of both multirotor systems are illustrated in Fig. 5. The orientation states are depicted in Fig.  6. It was possible to pose the UAVs in desired orientation due to fully actuate state of the titled systems, however, the angles must be small to satisfy the hovering condition. Configuration and trajectories of the systems are demonstrated in Fig. 7. The systems successfully regulated to the final condition without collision. The second UAV regulated faster and skipped the collision at t=0.24s when the first system was at the beginning of the motion. Distance between the systems during the control task is presented in Fig. 8 where the minimum one gained almost 0.95m, covering a safe distance for both drones. The obstacle avoidance term was presented in Fig. 9, showing that the function forced the second UAV to move faster at the beginning and the end of the regulation since the workspace of the operation was a large area. The minimum value was also 1, safe for the design.

V. CONCLUSIONS
This work presented the subject of collision avoidance between the end-effectors of robotic manipulators and UAVs, working in a shared environment. The state-dependent Riccati equation was used as the control unit for the regulation problem. This controller easily accepts the artificial potential field structure in the weighting matrix of states to avoid any collision with an obstacle or possible collision with another system. The subject of obstacle avoidance was studied in the SDRE field though collision avoidance was reported once for wheeled mobile platforms. So, the end-effector collision avoidance and UAV collision avoidance were highlighted in this research. The weakness of the SDRE collision avoidance was pointed out (failure in symmetric design) and a remedy was suggested to overcome this matter (assign one system responsible for collision avoidance task). The case study was a system of two manipulators working near each other and two UAVs. Three simulations were done to show the effectiveness of the proposed design. The first one expressed a general regulation task, completed and the second a verification example to emphasize that collision avoidance function should be set on one of the arms. The symmetric design was reported as a threat to the success of the operation. The last simulation also showed collision avoidance by two UAVs in a symmetric motion. Without the collision avoidance controller, the systems would crash.
Proposal for future study: Considering the links of the manipulators in the collision avoidance function, a protecting sphere for the multirotor UAV and defining the problem for UAVs with manipulators are proposed for future research. Aerial manipulation of a system of UAVs without collision is the objective of the control approach considering safe cooperation. To increase the safety of the path planning, velocity terms could be added to the collision avoidance function to provide more design flexibility.
The proposed method was implemented for the systems of two agents. In a case that there are more than two robots, the design of a switch is necessary to assign one of the robots responsible for collision avoidance when they are close to each other. The design of the switch for the case when three agents are close is an interesting topic for future studies.