Safe Self-Collision Avoidance for Versatile Robots based on Bounded Potentials

We present a novel and intrinsically safe collision avoidance method for torque-or force-controlled robots. We propose to insert a dedicated module after the nominal controller into the existing feedback loop to blend the nominal con-trol signal with repulsive forces derived from an artiﬁcial potential. This blending is regulated by the system’s mechanical energy in a way that guarantees collision avoidance and at the same time allows navigating close to collisions. Although using well-known ingredients from previous reactive methods, our approach overcomes their limitations in respect of achieving reliability without sig-niﬁcantly restricting the set of reachable conﬁgurations. We demonstrate the ﬁtness of our approach by comparing it to a standard potential-based method in simulated experiments with a walking excavator.


Introduction
When deploying robots in the field instead of factories, new challenges arise in the design of safety systems that are able to cope with the varying modes of operation.This particularly applies to forestry, agriculture and construction, where research is focusing on (semi-)automation and teleoperation of the involved heavy machinery [5], [4], [13].These machines are especially prone to colliding with themselves due Fig. 1 Top row: The M545 (on the left), its CAD model with joint labels (in the middle) and the approximate geometric model we use for collision avoidance (on the right).Bottom: Schematic view of the control loop with our collision avoidance module inserted.It modifies the force/torque commands f o , which the nominal controller C generates from the reference r and the state feedback (q, q), to steer the mechanical system P with the resulting commands f. to their high versatility, which is essential for tasks in unstructured environments.A prime example are walking excavators such as the M545 developed by Menzi Muck AG 1 (depicted in Fig. 1), which motivated our development of a suitable collision avoidance method.
In order to be generally applicable, a method should be a) usable both for assisting human operators and for supervising autonomous maneuvers and b) easily integrable into an existing control system.Probably the most straightforward way to meet these requirements is to base collision avoidance on insertion of a dedicated module into the existing control loop.In this work, we focus on such a design where the respective module is inserted after the nominal controller (as shown in Fig. 1).We present a corresponding solution, which is applicable to any fully actuated robotic system with a controller producing force or torque commands utilizing full state feedback.
Our approach builds up on artificial potentials, which have been used by previous works to plan a path and at the same time obtain a corresponding feedback law [6], [9], [10].When using artificial potentials for robot control, one can elegantly guarantee collision avoidance via energy considerations.One possibility is to let the potential function approach infinity towards the boundary of the set of collisionfree configurations [6].However, this is impractical as it requires unbounded joint torques.Conversely, it has been suggested to use finite potential functions and to confine the robot to states whose (artificial) energy is lower than the value of the potential function for any collision configuration [9].Our method employs the same reasoning and similarly guarantees collision avoidance based on the principle of energy conservation.
On the other hand, our approach implements a form of reactive control, which denotes a generic class of methods that are designed to react spontaneously to events (such as the robot getting close to an obstacle) and typically integrate collision avoidance into a hierarchy or "stack" of tasks [2], [8].In contrast, other reactive collision avoidance approaches [1], [10] simply add repulsive forces to the nominal commands and therefore exhibit the modularity we desire.They also derive the repulsive actions from artificial potentials, as it is common for reactive methods.Consequently, they inherit the well-known properties of artificial potentials to prohibit navigation through narrow passages and to trap the robot in local minima [7].Our method overcomes these limitations to a large extent by applying the potential force only when it is found to be necessary to ensure collision avoidance (again employing the same energy consideration).
However, the most significant improvement is the guarantee for collision avoidance (provided that the required forces or torques are feasible), which additive methods [1], [10] cannot have by design.With them, reliability at best results from a proper tuning of the potential height and needs to be verified by extensive testing.Note that for reactive methods based on velocity commands [8], [11], [12] such a theoretical guarantee is not attainable either, as they do not take into account the robot's dynamics, which are formulated on the force level.To the best of our knowledge, our way to incorporate artificial potentials into reactive control is novel, especially as it achieves safe collision avoidance while largely preserving workspace accessibility.
The remainder of this article is organized as follows: Sec. 2 introduces the preliminary notations and results as well as an additive reactive collision avoidance method, which will serve as a baseline for comparison with our approach.Then, in Sec. 3 we describe our method and in Sec. 4 we experimentally compare it to the baseline method.Finally, Sec. 5 gives a conclusion.

Preliminaries
Notation: We denote column vectors by bold face lower-case letters.Then, for a scalar a and two vectors b, c, let ∂ a/∂ b and ∂ c/∂ b denote the row vector and the matrix having the entries ∂ a/∂ b j and ∂ c i /∂ b j , respectively.Further, we use dots to indicate differentiation with respect to time (e.g.ȧ ≡ d dt a).

Artificial Potentials and Energy Conservation
We consider mechanical systems subject to holonomic constraints whose equations of motion take the form d dt with the kinetic energy T , the gravitational potential V g , the generalized coordinates q ∈ R N and the generalized forces f ∈ R N .We let q represent the joint variables (angles) and f the joint forces or torques, which constitute the control vector, as we focus on fixed-base robotic systems 2 .With this particular choice of q, the kinetic energy is not explicitly dependent on time and takes the form T = 1 2 qT M(q) q, where M is the mass matrix.
We then consider a control vector of the form f = ∂ (V g − V A )/∂ q T + f, where V A (q) is an artificial potential field and f is a new (yet unspecified) control vector.Substituting this in (1) and rearranging terms yields Then, given the functional forms of T = T (q, q) and V A = V A (q), one can derive from (2) the rate of change of the (artificial) energy implying ĖA = 0 for f = 0, which is the principle of energy conservation [3].

Obstacles, Closest Points and Collisions
A scene consisting of a robotic system and its static environment can be seen as a collection of rigid bodies, and we define that a collision occurs if for any pair of bodies (i, j) ∈ P c the mutual distance d i j equals zero.Here, P c denotes a subset of all possible body pairs, and d i j is defined as the shortest distance two points can have when one is chosen on body i and the other on body j.We denote a pair of such points that have this distance and lie on body i, j, respectively, by (p i j , p ji ).We further define the corresponding unit vectors e i j and Jacobian matrices J i j as where p i j is regarded as a fixed body point during differentiation (according to the standard definition of the Jacobian).
Defining that only certain body pairs can produce a collision is useful in practice, since some bodies are normally in contact (e.g.adjacent links) and others need not to be taken into account because they can never collide.Observe that for static obstacles the d i j are continuous functions in q only.Further, we define the globally shortest distance d * as d * (q) := min and let denote the corresponding pairs of globally closest points, unit vectors and Jacobian matrices, respectively (as defined above).Then, let Q c := {q ∈ R N |d * (q) > 0} denote the set of collision-free configurations.

Classic Collision Avoidance with Artificial Potentials
As a baseline to compare our method to in Sec. 4, we briefly introduce a standard approach to collision avoidance presented in textbooks [10].It is a straightforward implementation of additive potential-based reactive control (see Sec. 1).
The control vector takes the form where we add to the nominal control f o the artificial potential force f A and some damping f D .Let f A := −∂V A /∂ q T , and we define the artificial potential V A using the terminology introduced in Sec.2.2 as with the unilateral spring potential U being defined as where d 0 > 0 is the range of repulsion and α > 0 is the spring stiffness.Then, f A takes the explicit form Further, we define f D such that it is equivalent to the forces of viscous dampers attached to the closest points.Let denote such a force with a damping factor increasing linearly up to κ > 0 with decreasing distance.Then, we define the corresponding generalized forces as 3 Method Before describing our method, we outline the underlying generic idea (Sec.3.1), which allows it to guarantee collision avoidance while preserving workspace accessibility.The subsequent sections then describe several extensions we make to derive our method from this idea.

Energy Bounding Control Strategy
We guarantee collision avoidance based on energy considerations [9].Namely, we employ an artificial potential V A , which reaches its supremum over Q c , denoted by everywhere on the boundary of Then, a way to ensure collision avoidance is to control the (artificial) energy E A to stay below Vc , since this implies V A (q(t)) < Vc (as T ≥ 0), which implies that q(t) cannot cross ∂ Q c .A suitable control law would be e.g.
where E A,max < Vc and the damping f D (q, q) fulfills f T D q < 0.Then, as soon as E A > E A,max , this control law causes ĖA < 0 (according to Sec. 2.1) and therefore E A can never reach Vc .

Artificial Potential Force
Using the terminology introduced in Sec.2.2, we define our particular choice of artificial potential as where d 0 > 0 is the range and α > 0 the strength of the potential force.The negative gradient of V A yields the generalized artificial potential force as

Dissipative Repulsion and Damping
The control law (13) disables any nominal control if we assume that the robot is in a state with E A > E A,max .However, this situation is only a temporary due to damping.Additionally, in order to assist the process of energy dissipation, we partially suppress the artificial potential force when its effect would be to increase the system's kinetic energy.To this end, we scale the contribution of f A to the control vector f by the factor ω A ∈ [0, 1] defined as where ε > 0 and ḋ * is the rate of change of the globally shortest distance, given as ḋ * = e * T 1 (J * 1 − J * 2 ) q. Observe that ω A vanishes as soon as the globally closest points are moving away from each other with a speed greater than 1/ε.Conversely, the absorption of energy due to f A is undiminished, as ω A = 1 for ḋ * < 0.
The particular damping term we use corresponds to viscous forces acting on the pair of globally closest points.With the damping constant κ > 0, we define f D as q. (17)

Transitional Switching
We define a transitional zone between some energy level E A,trans < E A,max and E A,max , where both nominal control and collision avoidance forces are applied to some extent.This replaces the switching behavior from (13), where always exclusively one of the two can be active.How this relates to our energy considerations (Sec.3.1) and the potential function (Sec.3.2) is illustrated in Fig. 2. Now, let f L o denote the limited nominal control, which is the contribution of f o to the actual control signal.If the robot is in a state with E A > E A,trans , we restrict this contribution by demanding that where || • || denotes the euclidean norm, f max > 0 and λ E is defined as How we actually compute f L o from f o such that it respects this limit is the subject of the next section.
Also, λ E regulates the extent to which damping and artificial potential forces are applied.Mainly for the sake of a cleaner notation, we define two corresponding scaling factors, which we derive from λ E , as where γ > 1 (we consistently use γ = 8).We then state the transitional control law where g := ∂V g /∂ q T denotes gravity compensation.
For E A > E A,max , we have again ĖA < 0 since then f L o = 0 (according to (18), ( 19)) while the damping force f D and the "dissipative potential force" ω A f A are fully active (according to (20), ( 21)).Therefore, (22) guarantees collision avoidance in the same way as (13).

Limiting of Nominal Control
Our method computes the limited nominal control f L o in two stages.Beforehand, we subtract the gravity compensation term g from f o as it is already present in (22).

Limiting the Collision Acceleration
The first limit only applies if d * is smaller than some threshold d start > 0.Then, this step computes a modification of its input fo := f o − g, whose contribution to the relative acceleration of the globally closest points towards each other does not exceed a certain bound.This bound is proportional to with d start and d end specifying where limiting starts and where it reaches its full effect, respectively.They are to be chosen such that 0 < d end < d start .
We first compute the joint accelerations that fo would contribute according to Then, we compute a new acceleration vector q′ o , which differs from qo only in its component along the steepest descent of shortest distance −∂ d * /∂ q T , according to with µ ≥ 0 being chosen as small as possible such that where a ′ c is the acceleration of the closest points towards each other due to q′ o and a max > 0 specifies the maximum value of the bound on a ′ c .The µ satisfying this is given by } .
With it, we compute q′ o from (25), and multiplication with the mass matrix then gives the corresponding generalized forces achieving this acceleration.The output of this step is therefore

Limiting the Nominal Control's Norm
Next, only if E A ≥ E A,trans , we enforce a limit on the norm of f ′ o , which is the output of the first limiting procedure described above.To be specific, we compute the limited nominal control f L o according to with ν ∈ [0, 1] in the first case being chosen as large as possible such that the limit (18) is respected.The value of ν then follows as ν = min{1, λ E f max /||f ′ o ||}.

Summary
The final transitional control law (22) adds several new features to the basic variant (13).First of all, it increases energy dissipation via the factor ω A , which partially hinders f A to feed back the energy stored in V A .Next, we introduced a blending zone, where both collision avoidance and nominal control are active, to prevent high-frequency oscillations due to switching ("jittering").The latter would occur in the control signal e.g. when E A reaches E A,max due to acceleration by the nominal control.The limiting procedure (Sec.3.5) implements a measure to prevent oscillat- ing motions when f o actively pushes the robot into an obstacle.Namely, it selectively and rigorously reduces the ability of f o to accelerate the robot towards an obstacle whenever it is near one (i.e.d * < d start , also marked in Fig. 2).
Regarding workspace accessibility, the transitional control law makes only a little restriction, as it only enforces slower motions near obstacles (as visible in Fig. 2, where the white space above V A (d * ) corresponds to the allowed kinetic energies).Particularly, the robot can take any path through the configuration space along which d * > d end and V A < E A,trans , provided that one can arbitrarily slow down the motion on this path in order to respect the kinetic energy and acceleration limits.Excavators and manipulators typically comply with this condition as they are strong enough to continuously compensate gravity.Thus, they do not need to exploit their dynamics as they can perform every motion in a quasi-static fashion.

Experiments
We compare our method to a standard approach based on artificial potentials (Sec.2.3) in experiments with a simulation of the walking excavator M545.In this section, we refer to the standard approach as AP and to our method as AP+.
When applying the geometric considerations described in Sec.2.2, we represent the excavator's links by simplistic triangular meshes (depicted in Fig. 1).Note that the shapes are not strictly convex [11] and consequently the closest points (p i j , p ji ) are not uniquely defined for certain parallel configurations.However, these are not important in practice since they only occur in infinitely short time intervals.

Experimental Design
We compare both methods in three experiments testing different requirements:  1. Driving with full force into a collision: In this safety check, the excavator's shovel is pushed down into one of its wheels (as shown in Fig. 3) to test the methods' ability to prevent a collision even if the operator deliberately tries to cause one.2. Steering through a narrow passage: Here, the excavator's shovel is steered into the space between its front legs (as illustrated by Fig. 4) in order to examine how workspace accessibility and versatility are affected.3. "Scratching" collision: In this experiment, the excavator turns while the boom is not sufficiently extended such that a small avoiding motion is needed to prevent the shovel from colliding with a wheel (as depicted in Fig. 5).Here, we test the usefulness in a practical situation, when slight mistakes need to be corrected.
Throughout the experiments, the methods' parameters remain the same (as given in Table 1), and this also holds for the nominal controller.It tracks a shovel position reference trajectory, and it composes f o from a PD-feedback term and a feedforward term encompassing inverse dynamics.

Results and Discussion
The results of the three experiments are presented in Fig. 3, 4, 5, respectively.Consistently, the top right plot shows for a selected joint the nominal and modified actu- ator forces/torques (which are related to the joint forces/torques by a configurationdependent scaling factor).See Fig. 1 for the names given to the excavator's joints.The upper plot in Fig. 3 shows that in the first experiment, AP+ starts pulling back the shovel earlier and thereby avoids the collision, whereas AP fails to do so, as shown by the shortest distance d * (t) reaching zero in the bottom plot.The collision for AP is also visible in the respective simulation snapshots, in contrast to the smooth deflection for AP+ (also shown in Fig. 3).
In Fig. 4 and 5, the bottom right plots show the shovel extension x S (t), which is the coordinate of the shovel's center point with respect to an axis that lies within the boom's plane of motion and points to where the excavator is facing (as illustrated in the left images).The plots also show the reference for x S that is tracked by the nominal controller and one clearly sees that it cannot be followed with AP in the second experiment, at least not beyond a certain proximity to the passage (also shown by simulation snapshots on the left of Fig. 4).With AP+ on the other hand, entering the narrow space is possible (as shown by the middle image).The top plot in Fig. 4 shows for AP+ oscillations of τ T , which occur within the narrow passage and are caused by the switching direction of repulsion as the shovel is closer to either one or the other leg.
In the third experiment, the avoiding motion is larger with AP than with AP+, as the higher peak in the bottom plot in Fig. 5 shows.On the other hand, it shows a "sticky" behavior of AP+ near to the obstacle, as towards the end the reference is not reached for awhile.This is explained by the limiting, which is applied to f o if E A is high to Sec. 3.5).
In summary, AP+ outperforms AP regarding both safety and versatility or workspace accessibility as the first and second experiment demonstrate.In the third experiment, both AP and AP+ perform well in the sense that they can successfully deflect the dangerous motion allowing to resume tracking of the original trajectory.However, we argue that from a safety point of view AP+ appears superior since a slower behavior close to collisions is preferable over large unexpected deflections.

Conclusion
We presented a novel collision avoidance method (Sec.3) and compared it to a standard approach based on artificial potentials (Sec.2.3) in simulated experiments with a walking excavator (Sec.4).The experiments demonstrate that our method is superior due to its intrinsic safety and the significantly smaller restriction it imposes on the robot's accessible workspace.Also, our method performs well in transforming slightly colliding ("scratching") motions into harmless ones, allowing to quickly resume the original task.
Primarily for conceptual simplicity, we used a potential that only depends on the shortest distance, which however may cause oscillations in enclosures.As the class of suitable potential functions is very broad, the best choice remains yet to be determined.
On the other hand, our method is very general as it is applicable to a very generic type of torque-or force-controlled robots.Further, due to its modular design, it is easy to integrate our method into existing control systems.In future work, we consider it worthwhile to explore e.g. the insertion of a second control module before the nominal controller.This would provide another means apart from the transitional blending we use to achieve a good interplay of collision avoidance with nominal control.
Finally, we have validated our approach as a way to overcome the limitations of standard reactive methods based on artificial potentials.Most importantly, provided that the required finite forces or torques are feasible, our method guarantees collision avoidance for any nominal control signal.

Fig. 2
Fig. 2 Zones in the d * -E Adiagram with different associated modes of blending nominal control and collision avoidance.Note that the blending is regulated exclusively by the globally shortest distance d * and the (artificial) energy E A that the robot exhibits.The zones' shapes depend on the artificial potential function V A (d * ) as well as the parameters E A,max , E A,trans , d 0 , d start and d end .

Fig. 3
Fig. 3 Experiment 1: The left and the middle image show simulation snapshots for AP (classic method) and AP+ (our method), respectively, where the curved arrows mark the path of the shovel joint.The plots show the nominal (--) and the actual (-) boom joint force F B (t) (top) and the globally shortest distance d * (t) (bottom) for both AP (black) and AP+ (red).

Fig. 4 Fig. Experiment 3 :
Fig.4Experiment 2: The left and the middle image show simulation snapshots for AP (classic method) and AP+ (our method), respectively, where the curved arrows mark the path of the shovel joint.The plots show the nominal (--) and the actual (-) turn joint torque τ T (t) (top) and the shovel extension x s (t) (bottom) for both AP (black) and AP+ (red).The bottom plot also shows the reference for the nominal controller (green).The left image also shows e x S as the direction of increasing x S .