Improving Safety in Human-Robot Collaboration via Dynamic Active Constraints Enforcement*

—The problem of motion planning in obstacle clut-tered environments is an important task in robotics. In the literature several methodologies exist to address the problem. In this work we consider using the feedback-based approach, where the solution comes from designing a controller capable of guaranteeing trajectory tracking with obstacle avoidance. Commonly, all respective studies consider simpliﬁed robot dynamics, which is usually insufﬁcient in practical applications. In this work we focus on the collision avoidance problem with respect to a moving spherical object. We assume knowledge of a nominal controller that achieves tracking of a desired trajectory in the absence of obstacles, and we design an auxiliary control scheme to guarantee that the robot’s end-effector will always operate in a safe distance from the moving obstacle’s surface. The controller we develop does not take into account the actual robot dynamics, thus constituting a truly model-free approach. Experimental studies conducted on a KUKA LWR4+ robotic manipulator clarify and verify the proposed control scheme.


I. INTRODUCTION
Motion planning, and specifically robotic navigation in obstacle-cluttered environments, has become an increasingly important problem in the field of robotics. Owing to uncertainty of industrial environments where a manipulator typically operates, as well as the need for human-robot collaboration, the importance of safe robotic navigation arises naturally.
Several techniques have been employed to tackle motion planning problems. These include discretization of the continuous space and employment of discrete algorithms (e.g. Dijkstra, A*) [1], [2], [3], probabilistic roadmaps [4] and sampling based motion planning [5]. Feedback based motion planning [6] is also a commonly applied technique with certain advantages. Firstly, analytic closed-form solutions are offered, thus reducing the complexity of workspace discretization. Secondly, the verification of the validity of the solution is performed directly in terms of the closedloop system that arises, thus providing a solution to the control aspect of the motion planning problem. The main technique utilized for feedback-based motion planning is the construction of the so-called navigation functions introduced in [7], [8], then extended to more general workspaces in [9] and multi-robot systems in [10], [11]. An interesting approach, which guarantees safe navigation in predefined time is considered in [12], [13]. * The aforementioned works however, consider simplified robot dynamics, i.e., single integrators/unicycle kinematics, without taking into account robot dynamic parameters. As such, when applied to a practical scenario, the actual robot trajectory might deviate from the expected behavior and possibly jeopardize safety. The latter is especially true when the robot has to operate in close proximity to the obstacle. In [14], an obstacle-cluttered environment with an agent of 2ndorder uncertain dynamics is considered and safe navigation is guaranteed. However, even in the aforementioned work, the complete Euler-Lagrange class is not addressed.
In this work, we emphasize on the problem of collision avoidance in an environment with a possibly non-stationary spherical obstacle. Specifically, we consider a robotic manipulator with 3 revolute joints operated by a nominal controller and we propose an auxiliary control signal to ensure colission avoidance of the robot's end-effector. When the end-effector operates sufficiently far from the obstacle's surface, only the nominal controller is active. The auxiliary signal utilizes an artificial potential field and subsequently, a control torque is applied to guarantee certain bounds on the error between the robot's actual velocity and the artificial potential field. The controller we develop does not take into account the actual robot dynamics, thus constituting a truly model-free approach.
The paper is structured as follows. In section II, the considered problem is formulated. Section III presents the proposed scheme, while section IV provides a proof of the main result. Experimental validation of the proposed control scheme, conducted on a KUKA LWR4+ manipulator, is provided in section V. Finally, we conclude in section VI.

II. PROBLEM DESCRIPTION
Consider a robotic manipulator having 3 revolute joints, with q,q,q ∈ R 3 denoting the joint position, velocity and acceleration respectively. Let {t} be a frame attached at a kinematically known position p t ∈ R 3 on the end effector, referring to the robot inertia frame {O}. The dynamic model of the manipulator is described as where H(q) ∈ R 3×3 denoting the robot inertia matrix, C(q,q)q ∈ R 3 the centripetal and Coriolis force vector, G(q) ∈ R 3 the gravity vector, B ∈ R 3 is a constant damping matrix, while u is the applied joint torque. Let the manipulator be operated by a locally-lipschitz, time varying, state-feedback controller u n : R 3 ×R 3 ×R ≥0 → R 3 , with the objective of performing a given task, satisfying the following assumption. Assumption 1: System (1) with u = u n (q,q, t) is forward complete for any initial state, i.e., no trajectory of the closedloop system can escape to infinity in finite time. Problem statement: Consider a robotic manipulator (1), operated by a nominal controller u n satisfying Assumption 1. Further, consider a spherical moving obstacle of radius R and whose center lies on the (at least C 1 ) curve c : R ≥0 → R 3 in the manipulator's task space. Let the distance from the end-effector to the obstacle be given by the usual formula, . For any initial configuration, q(0) ∈ R 3 satisfying d(0, p t (q(0))), design an auxiliary control signal, u a , without incorporating any information regarding the robot dynamic model (1) such that (a) when the robot's end effector is positioned sufficiently far from the sphere's surface, i.e., d(t, p t ) ≥d > 0 whered is a specified constant, the closed-loop reduces to (1) with u = u n ; (b) the robot's end effector trajectory evolves away from the sphere's surface, i.e., d(t, p t ) > 0 for all t ∈ R ≥0 ; (c) all signals in the closed-loop remain bounded.
Remark 2: The construction of the hybrid system (9) -(12) and its subsequent analysis in Theorem 1, provide a solution to the problem formulated in section II. Indeed, item (a) of the problem statement is satisfied owing to the hybrid dynamics of (9) -(12), while items (b) and (c) are direct consequences of Theorem 1. The additional conclusion of non zeno hybrid flows is required for practical implementations, guaranteeing finite switchings in any bouded time interval.

IV. PROOF OF THEOREM 1
The solutions of the closed-loop system are non zeno. Indeed, the nominal controller is locally lipschitz and as such the closed-loop system is Lipschitz on the compact set q ∈ R 3 |d l ≤ d(t, p t (q)) ≤d . Therefore, the time between a switching from ν = 1 to ν = 2 must be bounded away from 0. Owing to Assumption 1, trajectories of the nominal system, i.e. ν = 1, are forward complete. Hence, to prove completeness for the stated initial configurations, it suffices to prove completeness of the initial value problem (14) q 1 = h(t, q 1 , ξ s ), (q 1 (0), ξ s (0)) = (q 10 , ξ s 0 ) ξ s = h s (t, q 1 , ξ s ), where, Let and note that Ω is an open set. Further, the nonlinearities in (14) are smooth, the control law (3) -(8), (13) is well defined and smooth in Ω and (q 0 1 , ξ s 0 ) ∈ Ω, owing to the admitted initial configurations and the update law of ρ 0 . Hence, the existence of a maximal solution to (14) is established [15]. Let [0, τ ) be the corresponding maximal interval. Our aim is to show that the solution is forward complete. Seeking for a contradiction, suppose τ is finite. Then, (q 1 , ξ s ) escapes any compact subset of Ω. Consider the Lyapunov function candidate, Differentiating with respect to time and employing (3) -(8), it is obtained, Differentiating V 1 with respect to time, employing (16) and via straightforward algebraic manipulation, it is obtained, Note that, Js is a bounded term since (q 1 , ξ s ) ∈ Ω∀t ∈ [0, τ ). Moreover, c is assumed C 1 , hence ∂d ∂cċ is also bounded. Therefore, (17) becomes, for some positive constantā ≥ 0. Thus, V 1 becomes negative whenever 1 1+ξ ≥ ρā k and it is concluded that ξ remains within a compact subset of Ω 1 . Following the argumentation [16] Claim 1, it is concluded that bothq r ,q r are bounded.
To proceed, consider the Lyapunov function candidate Differentiating (19) with respect to time and employing (1), (13) as well as the definition of s , it is obtaineḋ (20) In view of (20) and following the argumentation of [16] Claim 2, we conclude the existence of positive constants P,Q,R such thaṫ The latter implies that s remains bounded and a fortiori that ξ s remains within a compact subset of (−1, 1) 3 ; thus contradicting the assumption of τ being finite. Hence, any solution to (14) is forward complete. Finally, since ξ remains within a compact subset of (−1, ∞), there exists a positive constant d > 0 such that d(t, p t (q)) > d, thus concluding the proof of Theorem 1.

V. EXPERIMENTAL VALIDATION
To clarify and verify our approach, experiments were conducted on a KUKA LWR4+ robotic manipulator. The manipulator is initially at rest with the tip positioned at p t (0) =, with respect to the robot inertia frame. A PD controller has been utilized to force the manipulator to track the desired trajectory p d : R ≥0 → R 3 , given by p d (t) = (−0.2 + 0.1 cos(t), 0.55862 + 0.1 sin(t), 0.22907), i.e., a circular path on the plane z = 0.22907, as indicated in figure 2. Further, we consider a spherical obstacle of radius 0.02m, whose center c = (c 1 , c 2 , c 3 ) jumps periodically between points. Specifically, we consider t ≡ t(modT ) and allow the obstacle to jump (discontinuously) to a new position as follows: Notice that although the center's trajectory is only piecewise differentiable, the approach, which was presented for the C 1 case still holds; as long as at the time instant t * of a jump, the constraint d(t * , p t (t * )) > 0 is not violated. It was deemed appropriate to consider such a scenario, since vision systems that provide measurements of an obstacle's position display significant delays owing to processing. As such, the obstacle might appear to the controller as inanimate during a sampling interval and subsequently as jumping to a new position. Figure 1, depicts the evolution of the distance between the robot tip and the obstacle along with the corresponding mode of operation, indicated by the evolution of the discrete variable ν. Notice that the distance evolves away from zero for all t. Figure 2 depicts the robot's tip trajectory within the task space. Notice that deviation from the desired circular path arises (owing to the auxiliary controller) when the obstacle is within d l = 0.06m away from the robot's tip, while it operates with the designed PD controller when d >d and tracks the desired circular path. The rest of the controller parameters are chosen as follows, k = 2.5, b = 1, δ s = 0.2, ρ = 0.5.

VI. CONCLUSIONS
A hybrid control scheme for a robotic manipulator is proposed in this work, which ensures colission avoidance with respect to a moving spherical obstacle inhabiting the task space. The scheme operates with a nominal controller when the end-effector is sufficiently far from the obstacle's surface, while an auxiliary control signal is applied when the end-effectr is in close proximity to the obstacle's surface.
An important deficit of the propsed controller is the inability to guarantee that the scheme will eventually flow solely under the action of the nominal controller. Although this has not been a significant restriction as far as the implementation is concerned, it is in our immediate research interests to propose a formulation for the motion-planning problem in which we allow a globally asymptotically stabilizing nominal controller and with the additional requirement that the scheme finally converges to an equilibrium is imposed.