Research on Collision Avoidance Control of Multi-Arm Medical Robot Based on C-Space

A multi-arm robot for mandible reconstruction assisted surgery is developed. It has three manipulators and eighteen degrees of freedom. In order to prevent the collision of the manipulator caused by the overlap of the motion space of the manipulator during the space positioning of the whole mandible reconstruction operation, a collision avoidance path planning method for manipulator based on C-space is proposed. In order to obtain the space non-interference posture of one manipulator, and the interference posture of the other manipulators is taken as the obstacle mapping in C-space to obtain the free space after removing obstacles. At the same time, combining with the space position of the system manipulator, the C-space mapping mathematical model of feature points and line segments is studied, and the mapping mathematical model of any point and line segment in the plane is obtained. By introducing admissibility proof lemma to weigh the evaluation function, the improved heuristic A* search algorithm is used to search the best path for collision avoidance. The collision avoidance experiment is carried out through simulation and robot platform, and finally the animal experiment is carried out. These experimental results verify the feasibility and validity of the collision avoidance path planning method for the space manipulator.


I. INTRODUCTION
Mandibular defect is a common disease in surgery. Its symptoms directly affect the patients' appearance, phonation, swallowing, chewing and other physiological functions [1]. In serious cases, patients can't even have a normal life. Traditional mandible reconstruction surgery is performed by doctors bare-handed. Although better space positioning of bone can be achieved under the guidance of computer image, it is difficult to determine the space posture of bone. As a result, the accuracy of traditional operation is low and the treatment effect is not ideal [2]. In this paper, a set of mandible reconstruction robot system is developed to assist doctors to reconstruct patients' mandibles [3]. It can replace the doctor to achieve precise and stable control of bone, and make up for The associate editor coordinating the review of this manuscript and approving it for publication was Nikhil Padhi . the shortcomings of the traditional surgical treatment mode of mandible reconstruction, which has realistic significance for the development of medical services.
Mandible reconstruction surgery requires robots with high accuracy and flexibility. Further, the robot needs more workspace to satisfy the demands of a large number of flexible adjustments for different space positions and postures. In order to meet the above requirements, a surgical robot designed in this paper has three mechanical arms, and each arm has six degrees of freedom. The combination of active and passive methods is used to design the manipulator. The passive degree of freedom is redundant, and it can improve the adaptability of terminal posture. Three manipulators cooperate to complete the whole operation process. Firstly, the left and right arms clamp and fix the free branch bone of the skull. The middle arm holds the mandible reconstruct bone to move and locate it in the surgical space. But when free VOLUME 8, 2020 This branch bone is fixed with the left and right arms, the space between the two arms is narrow. At this time, the motion space of the manipulator overlaps and the left and right arms are easy to collide with the moving middle arm. This situation makes it impossible for the robot to complete the implantation of reconstructed bone. Therefore, the research of collision avoidance path planning of the manipulator is of great significance to the mandible reconstruction surgery robot.
There are many kinds of path planning spaces for robots. Two of the most typical methods are Euclid space and C-space. Euclid space needs coordinate transformation, which requires a large amount of calculation [4]. Lazona-Perz of MIT (Massachusetts Institute of Technology) first proposed a free space method based on C-space [5]. This method makes full use of the boundary information of obstacles and simplifies obstacles to feature points and lines, then obstacle boundaries are determined by solving critical collision angles. C-space obstacle mapping method has more advantages in computational efficiency, which can avoid a lot of ineffective and tedious calculations and is suitable for environment modeling in overall situation path planning. In this paper, the C-space is established by using the joint axis of the manipulator as the coordinate system. The interference gesture of a moving rigid body in three-dimensional space is transformed into a specific point and line in C-space. Using the C-space mapping method of obstacles to get obstacle space, and the free space is the complementary set of obstacle space. This paper chooses the improved heuristic A * search algorithm for collision avoidance path planning, and the robot platform and animal experiment were successfully passed. Experiments verify the feasibility of the collision avoidance path planning method and the robot platform for mandible reconstruction surgery.

II. MANIPULATOR CONFIGURATION
The design of surgical robots needs to meet the principles of high safety, large working space, and strong capability of obstacle avoidance [6]. At the same time, according to the main functions of the robot in the mandible reconstruction surgery [7], [8], a robot with three arms was designed [9]. As shown in Fig. 1, each manipulator has six active degrees of freedom and one passive degree of freedom. The SCARA type of robot configuration is selected to ensure that the manipulator meets the requirements of compact structure, flexible movement and high positioning accuracy. As shown in Fig. 2, each arm consists of four parts: the upper arm, the forearm, the wrist and the end. The active joints of the three arms have the same structure, and the left and right arms are exactly the same. The passive joint is used between the wrist structure and the execution end. Among them, the middle arm is a passive moving joint, and the left and right arms are passive spherical hinged joints. The execution end can be dragged directly by the doctor to transport and locate the bone. The wrist structure is designed by the combination of active and passive patterns to ensure that the robot has a large workspace and can achieve accurate gesture adjustment in a small range.

III. C-SPACE MAPPING OF OBSTACLES A. THE DESCRIPTION OF C-SPACE MAPPING
According to the configuration and working scene of the surgical robot for mandible reconstruction, the left arm and the right arm of the robot are not easy to collide, and the middle arm may collide with other mechanical arms. Therefore, the left arm and the right arm are regarded as obstacles, and the collision avoidance of the middle arm is studied. The 3-DOF wrist structure of the middle arm is perpendicular to the plane where the upper arm and the forearm are located, so the middle arm is simplified to a typical two-link structure for analysis. The C-space coordinate system of the middle arm is established by taking the position of second joint axis as the origin. The direction of the robot faces is the X-axis. The C-space obstacle mapping of the manipulator is decomposed into one or several line segments, and simplified into points when the Z-direction distance is large. The simplified model of the middle arm is shown in Fig. 3. The upper arm consists of a rectangle with length l 1 and width w 1 and a semicircle at both ends. The forearm consists of a rectangle with length l 2 and width w 2 and a semicircle at one end. The maximum distances of upper arm L 1 and forearm L 2 rotating around their own axes are l 1 + w 1 2 and l 2 2 + w 2 2 4, respectively. For solving the C-space obstacle mapping, only the boundary points of the collision area need to be calculated, that is, finding the critical collision angle. To this end, this paper defines it as follows: Assume that the range of motion of joint J k is [−π, π]. As shown in Fig. 4, the upper critical impact joint angle θ uc is formed when the connecting rod L rotates clockwise in contact with point P. The lower critical impact joint angle θ lc is formed when the connecting rod L rotates counter clockwise in contact with point P. The whole C-space obstacle boundary of the point can be described by the critical collision joint angle. If the line segment S collides with the connecting rod, and two endpoints of line segment S are used as feature points P 1 and P 2 . According to the mapping description of points, it is known that there are four critical collision angles of line segments. But there are two effective critical collision angles θ 1uc and θ 2lc . θ 1uc is the upper critical collision angle of P 1 . θ 2lc is the lower critical collision angle of P 2 .

B. C-SPACE MAPPING OF POINTS
As shown in Fig. 5, the obstacle ''point'' is positioned in the positive direction of the X-axis and is expressed as P = (x, 0). Therefore, when x ≥ l 1 + l 2 2 + w 2 2 4, point P does not constitute a motor barrier. For other values of X , P will become an obstacle to the manipulator. There are two cases to discuss.
In the First Case: When l 1 +w 1 2 ≤ x ≤ l 1 + l 2 2 + w 2 2 4, P only becomes a C-space obstacle to the forearm.
When d ∈ l 2 2 + w 2 2 4, x + l 1 , P will not constitute a C-space obstacle to the forearm. The d is the distance from the rotation center ro 1 of the forearm to point P.
The critical collision angle formula can be derived from the geometric relationship in Fig. 5. The formulas are as: In the Second Case: When x ≤ l 1 + w 1 2, P becomes a C-space obstacle to the upper arm and the forearm. In the interval θ 1 ∈ [θ 1lc , θ 1uc ], when θ 2 takes any value, the manipulator will collide with point P. It can be concluded that the critical impact joint angles of the forearm are −π and π. In the interval θ 1 / ∈ [θ 1lc , θ 1uc ], the critical collision joint angle of θ 2 is similar to that of the first case, but d ∈ [l 1 − x, l 2 2 + w 2 2 4]. Because two-bar linkage mechanism has symmetry in C-space, when P is in the negative direction of X-axis, only the sign of critical collision angle needs to be changed.
C-space mapping algorithm can be extended to any point in the plane. Assuming that P = (x, y) is a plane general point, its polar coordinate representation is P = (θ p , r p ). Rotating the angle of θ P through coordinate transformation, we can get the C-space mapping of P = (θ p , r p ). The formulas are as: There is a line segment P 1 P 2 that is perpendicular to the X-axis and located in the first quadrant, also, P 1 = (x, y 1 ) and P 2 = (x, y 2 ). Draw a circle O max . When the manipulator rotates around its upper arm's axis, the maximum distance of rotation l max is the radius, assuming that O is the center of the circle. According to the position relationship between O max and P 1 P 2 , it can be divided into four cases, as shown in Fig. 6.
In the First Case: P 1 P 2 and O max don't have intersection point, and P 1 and P 2 are outside the circle. In this case, the C-space of line segment P 1 P 2 is mapped to an empty set.
In the Second Case: P 1 P 2 and O max have an intersection point. This moment, the C-space mapping of line segment P 1 P 2 is the same as that of point P 1 .
In the Third Case: P 1 P 2 and O max have an intersection point, and P 1 is located in circle O max . The center of rotation of the forearm is expressed as ro 1 . The distance between ro 1 and the straight line where the P 1 P 2 is located is expressed as d rol . The distance between ro 1 and P 1 is expressed as d rop1 . The distance between origin and P 1 is expressed as d op1 .
(Line segments do not invade the inner circle) (1) When d op 1 ≥ l 1 + w 1 2 , P 1 will only become the C-space obstacle of the forearm.
When d rol ≤ l 2 2 + w 2 2 4 and d rop 1 ≥ l 2 2 + w 2 2 4 , as shown in Fig. 7(a), the forearm will not touch P 1 . The points where the forearm touches P 1 P 2 are A 1 and A 2 . The corresponding collision angles are θ 2uc and θ 2lc .
According to the geometric relationship in Fig. 7(a), for any θ 1 ∈ [θ 1 min , θ 1 max ], the critical collision joint angle of the forearm can be obtained as: 93222 VOLUME 8, 2020  θ 1min and θ 1max can be obtained as: d rol can be expressed as a function of θ 1 and x.
According to the geometric relationship in Fig. 7(b), for any θ 1 ∈ [θ 1 min , θ 1 max ], the critical collision joint angle of the forearm can be obtained as: θ 1 min and θ 1 max can be obtained as: The polar coordinate form of P 1 is P 1 = (r p1 , θ p1 ), then it is known that: (2) When d op 1 < l 1 + w 1 2 , P 1 can become the C-space obstacle of both arms. As shown in Fig. 8.
According to the geometric relationship in Fig. 8, the upper critical collision joint angle of the upper arm is: The lower critical collision angle θ 1lc is related to the value of d op1 . When d op 1 ∈ l 1 , l 1 + w 1 2 , as shown in Box I in Fig. 8, the line segment intersects with the upper arm at point B 2 . B 2 is on the circular arc. This moment, we can know:  When d op 1 ∈ (0, l 1 ), as shown in Box II in Fig. 8, the line segment intersects with the upper arm at point B 3 . B 3 on the upper edge of the arm. This moment, we can know: When θ 1 ∈ [θ 1lc , θ 1uc ], the collision between the upper arm and the line segment P 1 P 2 occurs, regardless of the angle of the forearm, the robot will collide. Thus, the line segment mapping to C-space of forearm is θ 2 ∈ [−π, π].
When θ 1 / ∈ [θ 1lc , θ 1uc ], P 1 P 2 only become the C-space obstacle of the forearm, the conclusion is the same as in the d op 1 ≥ l 1 + w 1 2 . In the Fourth Case: P 1 P 2 and O max do not intersect. Both P 1 and P 2 are located in the circle. Next, we will only discuss the critical collision angle when the line segment P 1 P 2 causes motion disturbance to the forearm.
When d rol ≥ l 2 2 + w 2 2 4, the arm doesn't collide with the line segment P 1 P 2 , and the C-space obstacle is mapped to an empty set.
When d rol < l 2 2 + w 2 2 4 and d rop 2 ≥ l 2 2 + w 2 2 4, as shown in Fig. 9(a), the point P 2 don't collide with the forearm boundary. The upper critical collision joint angle is caused by the contact between the end point A 1 and the line segment.
When d rol < l 2 2 + w 2 2 4 and d rop 2 < l 2 2 + w 2 2 4, as shown in Fig. 9(b), the point P 2 collides with the forearm boundary. The upper critical collision joint angle is caused by the contact between the point A 3 and the line segment.
The form of P 2 polar coordinates is P 2 = (r p2 , θ p2 ). It can be found that: If d rop 1 ≥ l 2 2 + w 2 2 4, then the lower critical collision angle is θ 2lc = f lc ol (θ 1 , x). If d rop 1 < l 2 2 + w 2 2 4, then the lower critical collision angle is θ 2lc = f lc ol (θ 1 , x). In summary, all C-space mapping descriptions of line segments perpendicular to the X-axis are obtained.
In order not to lose generality, the arbitrary line segments in the plane are further discussed. As shown in Fig. 10, it is assumed that the line segment P 1 P 2 is at any position in the plane. Firstly, the vertical line OH of the line segment P 1 P 2 is drawn from the origin O. The polar coordinate form of the H is H = (r OH , θ OH ). Then, coordinate transformation is carried out. Rotate the original coordinate OXY to get the new coordinate O X Y , and the rotation angle is θ OH . After the above operations, we can find that the line segment P 1 P 2 is perpendicular to the X -axis. In the new coordinate system,  the critical collision joint angle can be obtained and then converted back to the original coordinate (add angle θ OH ).

IV. PATH PLANNING ALGORITHM SELECTION
Common intelligent algorithms include A * algorithm, genetic algorithm, simulated annealing algorithm and ant colony algorithm. The convergence speed of genetic algorithm is so slow and easy to fall into local optimum [10]. Although it can be used in combination with simulated annealing algorithm, it cannot really solve the above shortcoming [11], [12]. The convergence speed of traditional ant colony algorithm is not ideal, and it cannot be well applied to reality robot path search planning [13], [14], [16]. Compared with the above algorithms, A * search algorithm has higher search efficiency [15]. It can evaluate the spatial location of each search to eliminate a large number of meaningless search paths, and ensure that the path searched in collision avoidance path planning is the optimal path, that is, the shortest path required for collision avoidance.
But the traditional A * algorithm still has the problems of long operation time and slow search speed [16], [17]. So an improved A * algorithm is used to solve this problem. The method is to weigh the evaluation function of A * algorithm by introducing admissibility proof lemma.
The evaluation function of A * algorithm can be expressed as: f (n) is valuation function; g(n) is the shortest path value from start to end; h(n) is the heuristic value of the shortest path from node n to the target point.
In this paper, the Euclidean distance d from the current position (θ 1c , θ 2c ) to the target position θ 1g , θ 2g is used as the estimation value h(n).  The actual situation is that the Euclidean distance between two nodes as the estimation value is often lower than the actual shortest distance. Although it guarantees the acceptability of the algorithm, it affects the search efficiency. Therefore, we introduce the admissibility proof lemma to weight the evaluation function.
Lemma: At each step before the termination of the A * algorithm, there is always a node n * , which has the following characteristics: (1) Node n * is on the best path to reach the target node; (2) A * algorithm has found an optimal path to node n * ; (3) f (n * ) f (n 0 ). VOLUME 8, 2020 According to the above lemma, this paper deals with the weighting of the evaluation function.
It is assumed that ω ∈ (0.5, 0.9), the weighted evaluation function will not affect the algorithm's acceptability, so that it can find a solution when there is a feasible solution, so this improvement can be accepted by the algorithm, and makes the search efficiency improved.

V. EXPERIMENTAL VERIFICATION A. COLLISION AVOIDANCE EXPERIMENT
During the actual operation of the robot system, the left and right arms are easy to become obstacles to the motion of the middle arm. In this paper, the collision avoidance control experiment of the middle arm is introduced as an example. The experimental process is shown in Fig. 11. The input information is the obstacle environment (mainly the manipulator itself or simple geometry) in the workspace and the initial state information of the manipulator. The output information is the angle value of the joint motion when the manipulator moves from the initial position to the target position, and the obstacles will not be encountered in the process.
Firstly, according to forward kinematics and inverse kinematics, the workspace information of the robot should be converted into joint space information, and the obstacle mapping should be completed in the C-space. In order to facilitate the implementation of search algorithm, the grid method is used to describe the C-space obstacle information, as shown in Fig. 12. Then, an improved A * search algorithm is used to obtain a collision avoidance motion path connecting the initial position and the target position. The motion angles of the second and third joints are obtained by trajectory planning. Finally, the joint controller drives the motor, and the motor drives the joint movement, so that the manipulator can avoid obstacles and reach the target position smoothly.
As shown in Fig. 13(a), the target point at the end of the middle arm of the robot is set as P point. By solving the inverse kinematics of the robot, the second and third joint angles of the middle arm are −32.13 • , 130.18 • , respectively. The target posture of the middle arm is shown in Fig. 13(b). If there is no obstacle, the middle arm starts to move from the initial position, and its joint trajectory curve is shown in the arrow curve in Fig 13(c). To verify the effectiveness of collision avoidance, the left arm is set as an obstacle in the middle arm trajectory, as shown in Fig. 13(c). Motion interference occurs between the left arm and the middle arm of the robot. After the collision avoidance path planning method to robot is used, the actual joint trajectory curve is shown in Fig. 13(d) and (e), which can effectively avoid obstacles.
The collision avoidance trajectory curve is shown in Fig. 14. The dotted line is the obstacle-free movement path, and the solid line is the collision avoidance path searched by the algorithm when the left arm obstructs the movement. By comparing the two motion paths, we can find that: in the initial stage of the actual operation of the manipulator, the upper arm speed is higher than the obstacle-free planning speed, and the increase of joint rotation angle is also higher; however, the forearm speed is lower than the obstacle-free planning speed, and the increase of joint rotation angle is also less than the corresponding planning angle. The turning point starting at in about 21 seconds, the robot arm avoids obstacles, and the joint of the upper arm rotates backward, and the forearm speed increases, and finally the robot arm reaches the target point.
The Fig. 15 shows the collision avoidance verification process of the robot. The results show that: the middle arm of the robot smoothly holds the bone model to avoid the left arm and to reach the target point, which verifies the correctness of the method.

B. ANIMAL EXPERIMENT
Finally, the collision avoidance between the middle arm and the other two arms was verified in the animal experiment. At the same time, it verifies whether the designed robot and obstacle avoidance method can be applied in clinical practice.
As shown in Fig. 16, the left and right arms of the fixed posture are mapped to C-space as obstacles. In C-space, the VOLUME 8, 2020  obstacle manipulator is regarded as a combination of line segments. Using the method described above, the collision avoidance path of the middle arm can be obtained.
Because the mandible of the goat is obvious, the goat with unilateral mandibular defects were selected for reconstruction surgery. When the goat mandible is missing, the connection between the left and right bones and the mandible will be free. The posture of goat's left and right free bone segments in image space is transmitted to robot space, as the target posture of left and right arms. The posture of the reconstructed implant is transmitted to the robot space, as the target posture of the middle arm. In order to avoid collision of the manipulator, a separation-time motion collision avoidance method is adopted to control the three manipulators to work together. The steps are as follows: 1) Firstly, the position and posture of the manipulator are initialized to ensure that all the manipulators are in the non-interference position. 2) The left and right arms are controlled to reach the planned position in turn. Next, the doctor fixed the arm end actuator to the free bone segment, as shown in Fig. 17(a). Free bone segment is controlled to move to preoperative planning position. 3) Loosen the left arm and move it to a proper distance.
The right arm maintains the preoperative designed position, and controls the movement of the middle arm to the target position and posture, as shown in Fig. 17(b). 4) Relative position of the middle arm and the right arm is maintained, and the implant bone segment and free bone were fixed with titanium nails, as shown in Fig. 17(c). Then adjust the passive joint, loosen the middle arm, and control the middle arm to a collision-free position, as shown in Fig. 17(d). 5) Reconnect the left arm to the free bone segment on the corresponding side. Through spatial registration, the free bone segment is driven by the manipulator to reach the target position. It is also connected with the implant bone block by the titanium nail, as shown in Fig. 17(e). The experiment shows that the manipulator can achieve collision avoidance movement and meet the requirements of operation. The correctness and feasibility of the path planning method proposed in this paper are further verified.

VI. CONCLUSION
In this paper, the C-space model of robot and the C-space mapping of obstacles are established. The C-space obstacle of the manipulator is represented as a series of point and line segment. Furthermore, the C-space is discretized by the grid method, and the geometric description of the obstacle information is completed. The improved heuristic A * search algorithm is used to realize collision avoidance path planning and passed the experimental verification. Although the method proposed in this paper has better met the requirements of animal experiments, there are still more theoretical problems to be solved in the clinical application of medical robots. In the follow-up work, in order to further improve the operation efficiency, it is necessary to carry out collision avoidance path planning and intelligent control of manipulator when the three arms move in real time.