Abstract
As robots tend to work cooperatively with humans in shared workplaces, safety as regards robot-human interactions has caused a great deal of concern in the robot community, and control strategies have become a hot topic in robotics research. In order to guarantee the robot's safety and continuous motions, this paper proposes a novel safety-control strategy, which is strictly conservative and which consists of a pre-contact and post-contact safety strategy. We adopt an optimal motion trajectory-planning method, by use of which the jerk, acceleration and velocity of the robot's motion can be limited and a time-optimal motion can be obtained as a post-contact safety strategy for a position-controlled manipulator. The optimal motion trajectory planning not only reduces the impact forces during the collision period, but also maintains the efficiency of the manipulator and preserves continuous motions. Next, we describe a novel collision detection method as a pre-contact safety strategy to avoid collisions. The method proposed here can compute security warning region to handle the effect of robot motion on collision detection and detect collisions between non-convex polygon soups. Finally, the control strategy is implemented for a 7-DOF humanoid manipulator and the experimental results demonstrate the validity of this novel hybrid safety-control strategy.
Introduction
Increasingly, robots need to work around humans. They must be able to operate safely and reliably, otherwise they will cause human injury or damage to their surrounding objects. So, conventional safety strategies for industrial robots cannot satisfy the developmental demand of robots. Safety-control strategies for robots coexisting with humans or delicate equipment have garnered increasing attention in both industrial and research communities.
In this study, a safety strategy to prevent mechanical injury is discussed. In general, the safety-control strategies of robots can be classified in terms of pre-contact safety strategies and post-contact safety strategies [1, 2]. Pre-contact safety strategies consider the avoidance of collision while post-contact safety strategies aim to reduce potential injuries after collision. A survey of safety-control strategies is provided by Pervez [3]. Unintended collision between a robot and a human is a major source of injury. A large number of pre-contact safety strategies consisting of safety-planning and collision detection are proposed [4, 5]. Safety-planning and collision detection are available for the avoidance of obstacles or to stop the robot if avoidance is not possible. However, the existing pre-contact safety strategies have not focused on reducing the impact force. Post-contact safety strategies to reduce the impact force are needed to guarantee safety. The use of protective covering is one possibility - the method reduces the impact forces significantly [6] - but using it alone to absorb the impact force may not be effective. A large number of post-contact safety strategies reduce the impact force during collision by controlling the stiffness/compliance of the robot. In the existing work, some compliance controls in Cartesian space and compliance controls in joint space have been investigated [7, 8]. Formica [9] has proposed the use of measured torque to vary the compliance for motor therapy exercises, thus achieving safe interaction control. Vick [10] developed an algorithm based on a simplified sensor-less estimation of external forces and the saturation of joint control torques to keep the effective external forces within safety limits.
Most of the previous studies on manipulator safety-control strategies have been limited to a single safety strategy. In order to guarantee safety, many types of safety-control strategies are indispensable. At the same time, the stiffness/compliance of a robot cannot be controlled for conventional position-controlled manipulators. However, little few works have introduced safety-planning in restricting the potential impact forces as a post-contact safety strategy for conventional position-controlled manipulators. The aim of this paper is to propose a novel hybrid safety-control strategy consisting of a pre-contact safety strategy and a post-contact safety strategy. The former strategy corresponds to avoiding collisions by means of a collision detection system, while the latter strategy is proposed to minimize impact force by means of restricting the velocity and acceleration of the robot's motion. The novel safety-control strategy will be implemented on a 7-DOF humanoid manipulator which is covered by soft plastic (cf. Figure 1).

7-DOF humanoid manipulator
To achieve smooth motion and to minimize impact forces, optimal real-time motion trajectory-planning limiting the jerk, acceleration and velocity of the robot's motion is introduced as the post-contact safety strategy. Research on real-time trajectory planning has been published [11–17]. Biagiotti and Melchiorri [11] give an overview of trajectory planning. Liu [12] uses seven cubic polynomials for online control representing a smooth mono-dimensional motion. Limiting jerk in robot trajectories contributes to the extended life of the manipulator and more precise tracking accuracy. A review of motion-planning methods that focuses on jerk bounding can be found in [13]. In [13], Macfarlane and Croft present a jerk-bounded, near-time-optimal trajectory planner that uses quantic splines, but only for 1-DOF systems. Amirabdollahian et al. [14] use a seventh-order polynomial for the entire trajectory with a minimum jerk model, while Seki and Tadakuma [15] propose the use of a fifth-order polynomial for the entire trajectory. Kröger [16] introduces a new method for the motion trajectory-generation of robots with multiple degrees of freedom. The method enables robots to react to unforeseen and unpredictable events instantaneously and in any state of motion. Herrera and Sidobre [17] propose limiting maximum motion conditions and a time-optimal trajectory-planner based on seven cubic equations, but the approach is not generally applicable and is only functional in some special cases. Therefore, optimal trajectory planning will be analysed and supplemented in this paper.
A great deal of work on collision detection can be found in different contexts [18–24]. The common methods often employ a hierarchy of geometrical primitives to represent or bound objects. Examples include axis-aligned or oriented bounding boxes (AABB/OBB), spheres, sphere-swept line segments (capsules) and sphere-swept rectangles. Okada et al. [19] have developed a precise collision detection system for humanoid robots using an AABB-based collision detection technique and have demonstrated that an AABB-based collision detection method is much faster than other conventional bounding volume-representation-based methods. In [20], Okada develops a hybrid approach that uses both table-based collision checking for compound joints and online geometrical model checking with a simplified link shape for other joints. Santis [21] proposes a skeleton algorithm to detect collisions and avoid the real-time collisions of a humanoid manipulator interacting with humans. In [22], the authors make use of cylinders and spheres to cover a redundant manipulator, and a compact method for detecting collisions between two cylinders is introduced. The performance of the proposed scheme is demonstrated for a 7-DOF redundant manipulator via simulations and experiments. In [23], sphere-based geometric models are used for the human and the robot due to the efficiency of the collision detection. Steinbach [24] develops an algorithm for computing compact sphere tree hierarchies over general 3D polygonal meshes with a view to efficient collision detection and minimum-distance computation. In [25], the authors propose a geometric representation based on a swept-sphere line for safe human-robot interaction. Täubig et al. [26] present a real-time self-collision detection algorithm based on computing the swept volumes of all bodies, and uses the GJK-algorithm as a collision detection block to compute the distance between two objects. However, the method only detects collisions between convex hulls. In this paper, we will develop a novel collision detection method that can handle the influence of robot motion and detect collisions between non-convex polygon soups.
This paper is organized as follows. Section 2 gives the framework of the novel hybrid safety-control strategy. A post-contact safety-control strategy of the hybrid safety-control strategy is analysed in Section 3. Section 4 describes the novel collision detection as the pre-contact safety-control strategy. Section 5 presents the simulation and experimental results to demonstrate the validity of the novel hybrid safety-control strategy.
In this section, we propose a framework of the novel hybrid safety-control strategy. The framework is shown in Figure 2. The safety of the manipulator is directly linked with the velocity and acceleration of the manipulator's motion. Therefore, the first attempt is to limit the impact force of the manipulator by restricting the velocity and acceleration of its motion. However, excessive velocity restriction will significantly limit the performance of the manipulator. The manipulator's motion must perform under limited maximum motion conditions (Jmax, Amax and Vmax) in order to obtain the minimum execution time for improving the efficiency of the manipulator. Therefore, the hybrid safety-control strategy of the manipulator first introduces an optimal motion trajectory planning that limits the jerk, acceleration and velocity of robot motion and which can obtain optimal motion. The optimal motion trajectory-planning can reduce potential injury with the occurrence of collisions and maintain the high efficiency of the manipulator.

A framework of a novel hybrid safety-control strategy
Next, the second attempt looks to use the real-time collision detection of the high-fidelity virtual model of the manipulator to avoid collisions. Real-time collision detection is a fundamental and essential function for the development of safety-control strategy. Considering the effect of manipulator speed and acceleration on braking distances, a novel security warning region for collision detection will be introduced in this paper. Therefore, the contribution of this paper is a conservative and precise hybrid safety-control strategy. At the same time, a soft material cover will be considered in this research.
The motion trajectory planning limits jerk, acceleration and velocity in a Cartesian or joint space. As manipulator motion under maximum motion conditions (Jmax, Amax, Vmax), we obtain the minimum execution time for the improvement of the efficiency of the manipulator. Considering the canonical case of Figure 3 without loss of generality, the optimal motion can be separated into three different types categorized sectioned by integration:

Jerk, acceleration, speed and position curves
Case 1: the motion with a saturated jerk ± Jmax (AB, CD, EF, GH segments):
Case 2: the motion with a saturated acceleration ± Amax (BC, FG segments):
Case 3: the motion with a saturated velocity ± Vmax (DE segment):
Where J(t), A(t), V(t) and X(t) represent jerk, acceleration, velocity and position functions, respectively. Ai, Vi and Xi are the initial conditions.
In this case, the initial and final kinematic conditions are null. According to the symmetry effect of acceleration and the anti-symmetry effect of jerk, in Figure 3, we have:
where Tj is the constant jerk time. During Tj, the acceleration increases or decreases linearly according to the jerk. Ta is the constant acceleration time, and the velocity increases or decreases linearly according to the acceleration. Tv is the constant velocity time.
The motion trajectory can be separated into four different particular cases, according to whether or not maximum acceleration or maximum speed is reached (cf. Figure 4).
Case 1: Vmax is reached and Amax is reached, Tv ≥ 0 and Ta ≥ 0. Case 2: Vmax is unreached and Amax is reached, Tv = 0, Ta ≥ 0. Case 3: Amax is unreached and Vmax is unreached, Ta = Tv = 0, Tj ≥ 0 Case 4: Vmax is reached and Amax is unreached, Tv ≥ 0 and Ta = 0.

Three boundary conditions
Next, three boundary conditions are obtained, thus:
If
Condition 1: Vmax is reached and Tv = 0, Amax is reached:
Then, we have to find the traversed distance (Dthr1):
Condition 2: Amax is reached and Ta = Tv = 0:
We can find a distance (Dthr2):
Condition 3: Vmax is reached and Tv = 0, and Amax is unreached:
We can find a distance (Dthr3):
Then, we define the distance (D) as the difference between the origin (P0) and destination (Pf) positions, where:
The minimum execution time is given by:
Considering the boundary conditions, the algorithm of the motion trajectory-planning is shown as algorithm 1.

Algorithm of optimal motion trajectory-planning
Different trajectory cases depend on the distance between the initial position and final position and the motion constraints. When optimal motion trajectory-planning is used in the Cartesian space of the manipulator, the relationship between the manipulator joint velocities and the Cartesian velocities is given by:
where V and Ω represent the linear and angular velocities of the manipulator's end-effector and Jac is the Jacobian matrix. The linear velocities V and Ω can be directly obtained by the optimal motion trajectory planner.
Collision detection
Research into collision detection algorithms has a lengthy and extensive history in the computer geometry literature [18–24]. A challenge for real-time collision detection is computational cost. In order to reduce the computational cost of collision detection, the robot's collision model has two layers - one is a coarse model and the other is a fine model. Many collision detection algorithms first compute the bounding box hierarchies of the original polygon model as coarse models, and then detect the intersections between bounding boxes. If an intersection exists between two bounding boxes, it will calculate whether the original objects are overlapping. The hierarchy bounding box implementations reduce the memory requirements and computational costs.
Many different representations of bounding boxes have been proposed across the literature, such as axis-aligned bounding boxes (AABB), oriented bounding boxes (OBB), rectangle swept-spheres (RSS) and discrete orientation polytopes (k-DOP). An AABB is a rectangular bounding box at an axis-aligned orientation. Figure 5 shows the bounding box hierarchies of the original polygon model of the humanoid manipulator. It simplifies the computation process of collision detection. The AABB-based collision detection method generally yields faster collision queries than other bounding volume representation-based collision detection methods. Its precise collision detection reduces dangerous collisions of robots, prevents the reduction of the range of robot movement, and can detect collisions between non-convex polygonal objects.

Original polygon model and bounding box hierarchies of a humanoid manipulator
Open Inventor supports 3D predictive display and collision detection. A DualSceneCollider class based on an AABB collision detection algorithm is provided to support collision detection in Open Inventor. It performs very fast collision detection in the case of two scene graphs to prevent objects from passing through one another, and to avoid having two of them occupy the same space at the same time. This class can provide information about colliding triangles, the coordinates of common points, and so forth. A flowchart of the collision detection is shown in Figure 6. Each time the joint angles change, an intersection test is done between the two scenes by an automatically calling function. The function searches for pairs of intersecting triangles in an optimal manner. For each pair of intersecting triangles found, a function is called to provide different collision responses. For instance:
Highlight the intersecting shape. Obtain the coordinates of each pair of common points. Obtain the number of pairs of intersection triangles.

A flowchart of collision detection
In section 4.1, the precise collision detection-based AABB does not consider the manipulator's movement state (A,V), as it checks for collision by discrete approaches at some intermediate configurations from the motion trajectory planner. The robot's braking is triggered when collisions are detected. Due to ignoring the effect of the manipulator's movement state on braking distances, it is difficult to reduce dangerous collisions of the manipulator at high speeds, even if they are detected precisely.
The existing research has usually added safety margins around each link to cope with errors in both the modelling and influence of the movement state [16–18]. However, such margins significantly decrease the range of movement of the manipulator, and they are not an exact representation of the braking distance. Therefore, we have developed a new collision detection method which can handle the influence of high braking distances by simulating the process of braking. We first compute the security warning region of the manipulator joint Q = [qi0, qi1] according to (10), such that when the robot starts braking at the next cycle, it will stop within this security warning region. Where qi0 is the joint angle of the i-th revolute joint when the manipulator starts braking, qi1 is the joint angle of the i-th revolute joint when the manipulator robot stops braking. The security warning region is based on the joint angle velocity, joint angle acceleration, latency and worst case deceleration, as well as joint angle uncertainties. Next, the novel collision detection checks for collisions by AABB-based collision detection at the security warning region q1 i joint configuration (cf. Figure 7).

Sketch of the novel collision detection
In this paper, the joint braking distances are modelled by the security warning regions. In order to determine the security warning regions, we introduce a braking model of single revolute joints by assuming that:
The robot stays at the current movement state The process of joint braking is a uniformly decelerated motion, and the braking of the joint thereafter is at worst a deceleration of aB = 1200°/s2.
Experimental Platform
The hybrid safety-control strategy was implemented on a teleoperation system of a humanoid manipulator based on virtual reality technology. Figure 8 shows the structure of the teleoperation system. It consists of two sides - an operator and a remote robot - which are connected through a local area network (LAN). The communication cycle is fixed at 20 ms. At the operator side, a high-fidelity virtual model of the remote robot and the environment is created for predictive display and collision detection by Open Inventor. An optimal motion trajectory planner, a security warning region and a kinematics calculation are carried out on the operator side.

Structure of the humanoid manipulator teleoperation system
The remote robot side consists of a 7-DOF humanoid manipulator [27]. The humanoid manipulator is constructed by modular joints, has a human-like workspace and is able to handle a 10 Kg payload with full arm extension at 1 gravity conditions and has a total mass of less than 18.5 Kg. At the same time, the humanoid manipulator is equipped with various sensors that provide joint positions and force information for teleoperation. A manipulator controller is used to control the real manipulator robot system and communicate with the operator side.
Optimal motion trajectory planning was used in the joint space of the manipulator in the experiments. Figure 9 and Figure 10 denote the profile of the jerk, acceleration and velocity of the second and fourth joint motions when there is no collision, respectively. The motion trajectory of the second joint belongs to case four, while the fourth joint belongs to case one.

Jerk, acceleration, velocity profile of the second joint (Jmax = 2° / s3, Amax = 5° / s2, Vmax = 10° / s)

Jerk, acceleration, velocity profile of the fourth joint (Jmax = 5° / s3, Amax = 4° / s2, Vmax = 8° / s)
Figure 11 shows the predictive braking distances for different movement states of the second and fourth joints, and shows the predictive braking distances increasing or decreasing according to the acceleration and velocity of those joints.

Security warning region of the second joint and the fourth joint
The six braking manoeuvres that occurred are listed in Table 1. It shows the predictive braking distance and actual braking distance for different motion conditions of the joint, and demonstrates that the approach is conservative and precise.
Predictive and actual braking distance of different motion conditions
Figure 12 shows the security warning region and the desired and actual positions of the second joint without collision, while Figure 13 shows the security warning region and the desired and actual positions when objects collide, demonstrating that the security warning region prediction incurs overestimations. Overestimation is not only caused by the latency tL but also by the tracking accuracy of the manipulator.

Security warning region and the desired and actual position profiles without collision

Security warning region and the desired and actual position curves when objects collide

Simulated process of the novel safety-control strategy experiment
Figure 14 illustrates the simulation process of the experiment. The solid model describes the current movement state of the humanoid manipulator. The wireframe model describes the predictive braking state of the humanoid manipulator. The predictive braking distances are enlarged in order to simplify presentation for readers. The braking is triggered when the wireframe model of the manipulator and the base structure occurs collision in the virtual environment, such that the robot ceases motion.
The computational time for collision detection is reported in Figure 15. The computational time for detecting collisions is constant (0.07 ms) when there is no collision. However, when collisions occur, it requires more computational time and becomes 1.4 ms in the worst case, which is much smaller than the teleoperation period (20 ms), thus, real-time performance is obtained. The experimental results demonstrate the safety and efficiency of the novel hybrid safety-control strategy.

Computational time of collision detection
The previous optimal motion trajectory-planning algorithm was incomplete. Our first contribution is that case four was discussed and the optimal motion trajectory planner was supplemented. The new optimal motion trajectory planner can perform well in generating a global optimal trajectory in environments without collisions and can limit the maximum motion conditions of the robot.
Our second contribution is that a collision detection method which combines a high precision AABB-based collision detection library and a security warning region has been developed. The advantages of this method are that it can detect collisions between non-convex polygon soups and can handle the effect of robot motion during collision detection.
Finally, we have presented a novel hybrid safety-control strategy which comprises optimal motion trajectory-planning and security warning region collision detection. According to the results of our experiments, the novel hybrid safety-control strategy is able to ensure the smooth collision-free movement of the manipulator in real-time interactions, even with high speed motion.
Footnotes
7.
This work was supported by the National Natural Science Foundation of China (Grant No. 51305097), Research Fund for the Doctoral Program of Higher Education of China (20122302120046), China Postdoctoral Science Foundation Funded Project (2013M531021).
