Vrije Universiteit Brussel
Real-Time Control of Robot Arms for Safe Human-Robot Coexistence Merckaert, Kelly; Convens, Bryan; Nicotra, Marco M.; Vanderborght, Bram
Published in:
ICRA 2021 Workshop: Unlocking the potential of human-robot collaboration for industrial applications
Publication date:
2021
Document Version:
Accepted author manuscript Link to publication
Citation for published version (APA):
Merckaert, K., Convens, B., Nicotra, M. M., & Vanderborght, B. (2021). Real-Time Control of Robot Arms for Safe Human-Robot Coexistence. In ICRA 2021 Workshop: Unlocking the potential of human-robot collaboration for industrial applications IEEE.
General rights
Copyright and moral rights for the publications made accessible in the public portal are retained by the authors and/or other copyright owners and it is a condition of accessing publications that users recognise and abide by the legal requirements associated with these rights.
• Users may download and print one copy of any publication from the public portal for the purpose of private study or research.
• You may not further distribute the material or use it for any profit-making activity or commercial gain • You may freely distribute the URL identifying the publication in the public portal
Take down policy
If you believe that this document breaches copyright please contact us providing details, and we will remove access to the work immediately and investigate your claim.
Download date: 09. May. 2022
Real-Time Control of Robot Arms for Safe Human-Robot Coexistence*
Kelly Merckaert1,2, Bryan Convens1,3, Marco M. Nicotra4, and Bram Vanderborght1,3
Abstract— In the past years, manufacturers have been mov- ing from automated mass production to automated mass customization, where robots share a workspace with humans in constantly changing scenarios. To avoid collisions with humans and the environment while ensuring efficient task realization, we introduce a computationally efficient control scheme that relies on the Explicit Reference Governor (ERG) formalism to enforce input and state constraints in real- time. The resulting constrained control method can steer the robot arm to the desired end-effector pose (or a steady-state admissible approximation thereof) in the presence of actuator saturation, limited joint ranges, speed limits, static obstacles, and humans. The effectiveness of the proposed solution is shown with experimental validations on the Franka Emika Panda robot arm.
I. INTRODUCTION
In recent years, manufacturing companies are adopting mass customization strategies. The required increased flexi- bility in the production environment can be obtained by com- bining the complementary qualities of humans and robots [1], [2]. Although Human-Robot Collaboration (HRC) can bring the production line to a new level of flexibility and efficiency, the safety issue is of major importance [3].
To obtain safe motions of robot arms with kinematics constraints, sampling-based motion planning algorithms, as Rapidly exploring Random Trees (RRT), are used, e.g. [4].
However, these type of path planning algorithms do not take into account robot dynamics. Path planning approaches that consider both kinematic and dynamic constraints, i.e.
kinodynamic path planners, are computationally demanding and therefore hard to implement in real-time, e.g. [5].
In contrast to RRT motion planners, kinetostatic safety fields are reactive and computationally efficient, i.e. [6]. They are based on the cumulative danger field and on the repulsive potential field approach, that are applied in [7] and [8], respectively. Yet, these approaches do not take into account actuator input constraints.
The Saturation in the Null Space algorithm generates velocity commands that allows real-time control of robots for a large number of hard limits [9]. Although it guarantees an
*This work was supported by Fonds Wetenschappelijk Onderzoek (FWO) [grant numbers 37472, 60523, and 62062], the EU H2020 project SOPHIA [grant number 871237], and the Flemish Government under the program Onderzoeksprogramma Artifici¨ele Intelligentie (AI) Vlaanderen.
1 Authors are with the Mechanical Engineering department, Vrije Uni- versiteit Brussel, 1050 Brussels, Belgium.https://www.brubotics.
eu/research/renmm[email protected]
2Kelly Merckaert is affiliated to Flanders Make, Belgium
3Bryan Convens and Bram Vanderborght are afilliated to imec, Belgium
4 Marco M. Nicotra is with the Electrical, Computer, and Energy Engineering department, University of Colorado Boulder, Boulder, CO 80309, USA.https://www.colorado.edu/faculty/nicotra/
optimal solution, it also does not take into account actuator input constraints.
Due to recent advances in computational performances, Model Predictive Control (MPC) can be implemented on robot arms to handle both state and input constraints in real- time, e.g. [10], [11]. However, the application possibilities are limited due to the typically non-negligible computational cost.
For robot control in dynamic environments, strategies based on the Speed and Separation Monitoring (SSM) and the Power and Force Limiting (PFL) criterion modulate the velocity of the robot based on the distance between the human operator and the robot, i.e. [12], [13]. However, the strategies have usually no rigorous proof of convergence in finite time and often slow down the robot excessively.
Another way to tackle the HRC safety problem is by gen- erating reachable occupancies which account for all human movement, i.e. [14]. Although safe, this method results in a more conservative robot behavior.
We base ourselves on the Explicit Reference Governor (ERG), a closed-form feedback control scheme that can enforce both state and input constraints of nonlinear systems without having to solve an online optimization problem [15].
In [16] the idea was explored on a 2DOF robotic manipulator, here we analyze the methodology on the Franka Emika Panda robot arm.
II. TRAJECTORY-BASED EXPLICIT REFERENCE GOVERNOR
Consider the joint-space dynamic model of a robotic manipulator withnjoints,
M(q)¨q+C(q,q)˙ q˙+g(q) =τ, (1) whereq ∈ Rn is the vector of joint variables, M(q) > 0 is the mass matrix,C(q,q)˙ q˙ accounts for the Coriolis and centrifugal forces,g(q)is the influence of the gravity on the manipulator, andτ ∈Rn is the torque control input vector.
The system is subject to a variety of input and state constraints. Specifically, we consider the following classes of constraints:
• Actuator Saturation:the torque applied to the joints is limited,τmin,i≤τi≤τmax,i,∀i={1, . . . , n}.
• Operating Region:the robot arm has a limited operating range,qmin,i≤qi≤qmax,i,∀i={1, . . . , n}.
• Speed Limitation: to allow human-robot collaboration, we have to take into account the cobot’s inherent joint velocity limits, q˙min,i ≤ q˙i ≤q˙max,i, ∀i = {1, . . . , n}, and its Cartesian end-effector velocity limits,x˙min,e ≤
˙
xe≤x˙max,e.
Pre-Stabilized System Trajectory-Based
Explicit Reference Governor Navigation
Field Inverse
Kinematics
Dynamic Safety Margin
×
Z PD + g
Control
˙
qv qv
ρ
∆
xe,r qr
τ q,q˙
Fig. 1. Constrained Control Architecture−The desired end-effector posexe,r is transformed by means of akinematic inversioninto a desired joint referenceqr. This desired joint referenceqr is dynamically filtered by theTrajectory-Based ERGto the auxiliary joint referenceqvsuch that the target configuration will be reached while satisfying the system constraints. The trajectory-based ERG consists of aNavigation Field, which determines the direction ofq˙v, and aDynamic Safety Margin, which regulates the modulus ofq˙v. ThePD+g control unitpre-stabilizes the robot dynamics to the applied referenceqvand sends the computed torquesτ to the robot.
• Obstacles:the robot should be able to avoid a collection of possibly moving obstacles. We considerNwwalls,Ns
spherical obstacles, andNccylindrical obstacles.
Planar walls can be avoided by enforcingcwj ·pi ≤dwj,
∀i ∈ {1, ..., n+ 1},∀j ∈ {1, ..., Nw}, where cwj ∈R3 is the unit vector normal to the j-th wall (pointing in the inadmissible direction) and dwj ∈R describing the distance between the plane and the robot base.
Spherical obstacles can be avoided by enforcingkpsij− csjk ≥ rjs,∀i ∈ {1, ..., n}, ∀j ∈ {1, ..., Ns}, where csj and rsj are the center and the radius of sphere j, and psij is the point on linkithat is closest to spherej.
Cylindrical obstacles can be avoided by enforcing kpcij −tcijk ≥ rcj, ∀i ∈ {1, ..., n}, ∀j ∈ {1, ..., Nc}, wherercj is the radius of cylinderj,pcij is the point on link i that is closest to cylinder j and tcij is the point on cylinderj that is closest to link i.
Given the joint-space dynamic model (1) and the proposed constraints, the control objective is to design a computationally efficient control scheme that can run in real-time without relying on offline pre-generated trajectories that let a robot arm reach the end-effector reference pose xe,r, or a steady-state admissible approximation, while satisfying the input and state constraints.
Note that the satisfaction of the obstacle constraints can be guaranteed in the case of static obstacles. For the case of dynamic obstacles, the control law avoids collision if possible and halts the robot otherwise.
Based on the problem statement, we propose a multi- layer control architecture to decouple the control problem into sub-tasks, as depicted in Fig. 1. The control layer pre-stabilizes the dynamics of the robot arm to the applied reference qv, which is done by a classic PD control law with gravity compensation that does not account for any system constraints. TheTrajectory-Based Explicit Reference Governor (ERG) dynamically filters the reference qr so that all constraints are satisfied and is also responsible for reaching the target configurationqr.
In case a Cartesian end-effector pose referencexe,r is given to the robot arm, it first needs to be transformed into a joint
referenceqr by means of a kinematic inversion algorithm.
Given an auxiliary reference qv, the robot arm is pre- stabilized by a classic PD control law with gravity com- pensation,
τ =KP(qv−q)−KDq˙+g(q), (2) which leads to the following closed-loop system dynamics , M(q)q¨+C(q,q)˙ q˙=KP(qv−q)−KDq˙. (3) If the applied reference qv remains constant in (3), the closed-loop equilibrium configuration q¯v is asymptotically stable. The idea behind the ERG is to generate a reference qv(t)such that the transient dynamics of the closed-loop sys- tem cannot cause a constraint violation andlimt→∞qv(t) = qr. Rather than pre-computing suitable trajectoryqv(t), the ERG achieves these objectives by manipulating the derivative of the applied reference,
˙
qv =ρ(qv,qr) ∆(q,q,˙ qv), (4) whereρ(qv,qr)is the Navigation Field (NF), i.e. a vector field that generates the desired steady-state admissible path, and∆(q,q,˙ qv)is theDynamic Safety Margin(DSM), i.e. a scalar that quantifies the distance between the predicted tran- sient dynamics of the pre-stabilized system and the constraint boundaries if the currentqv were to remain constant. In the DSM, the forward system dynamics are simulated by the use of the Simplectic Euler for a finite time horizont0∈[t, t+T] with the predicted state initializations qb˙t0=t = q(t)˙ and qbt0=t=q(t).
III. RESULTS
We experimentally validated this methodology on the Franka Emika Panda robot arm.
For the experiments, the pre-stabilizing control runs at 1 kHzand the ERG runs in parallel at 100 Hz. The Orocos Kinematics and Dynamics Library [17] is used for kinematic inversion for experiments where an end-effector reference pose is given to the robot. To make the influence of the robot dynamics more significant, we replaced the standard Panda
Fig. 2. Trajectory-Based ERG Wall DSM − An orange steady-state inadmissible end-effector reference is given to the robot and is approximated by a green reference in front of the wall. Without the wall DSM, the robot does not slow down enough when closer to the wall, whereby the robot touches the wall because of the transient dynamics, as depicted by the red dashed line. With the wall DSM, the robot slows down earlier, resulting in smaller transient dynamics and no constraint violations, as depicted by the blue dashed line.
end-effector of 0.7 kg with a mass of 2.5 kg and attached it with tie wraps to the flange.
To show the importance of the DSM, an end-effector reference pose that is behind the wall is given to the robot.
The navigation field converts this reference to a steady-state admissible approximation in front of the wall, as visualized in Fig. 2. When the trajectory-based ERG is implemented without including the DSM of the wall constraint, the robot touches the wall at a certain moment. Although the repulsion field of the wall can repel the robot from the wall, it can only guarantee safety when the robot moves slowly. In case the robot moves fast, it has to be able to quickly accelerate and decelerate, such that it can stop immediately. When the DSM of the wall constraint is included, the robot will slow down earlier, resulting in a smaller overshoot and as such no constraint violations.
IV. CONCLUSION
The proposed real-time control scheme is able to steer the robot arm to a desired end-effector pose, or an admissible approximation, while avoiding actuator saturations, limited joint ranges, velocity constraints, and obstacles. For a com- plete discussion about the methodology and results in human- robot coexistence scenarios, e.g. the virtual wall experiment in Fig. 3, the authors refer to [18].
REFERENCES
[1] F. Ferraguti, A. Pertosa, C. Secchi, C. Fantuzzi, and M. Bonf`e,
“A Methodology for Comparative Analysis of Collaborative Robots for Industry 4.0,” in 2019 Design, Automation & Test in Europe Conference & Exhibition (DATE), (Florence, Italy), pp. 1070-1075, IEEE, 2019.
[2] B. Vanderborght, “Unlocking the potential of industrial human-robot collaboration for economy and society,” European Commission, 11 2019.
[3] A. M. Zanchettin, P. Rocco, S. Chiappa, and R. Rossi, “Towards and optimal avoidance strategy for collaborative robot,”Robotics and Computer-Integrated Manufacturing, vol. 59, pp. 47-55, 2019.
[4] L. Jaillet and J. M. Porta, “Path Planning Under Kinematic Constraints by Rapidly Exploring Manifolds,”IEEE Transactions on Robotics, vol.
29, no. 1, pp. 105-117, 2013.
Fig. 3. A human-robot coexistence scenario where the robot follows a wooden reference plate and a virtual wall separates the human’s and robot’s workspace.
[5] M. Kazemi, K. K. Gupta, and M. Mehrandezh, “Randomized kino- dynamic planning for robust visual servoing,”IEEE Transactions on Robotics, vol. 29, no. 5, pp. 1197-1211, 2013.
[6] M. Parigi Polverini, A. M. Zanchettin, and P. Rocco, “A computation- ally efficient safety assessment for collaborative robotics applications,”
Robotics and Computer-Integrated Manufacturing, vol. 46, pp. 25-37, 2017.
[7] B. Lacevic, P. Rocco, and A. M. Zanchettin, “Safety assessment and control of robotic manipulators using danger field,”IEEE Transactions on Robotics, vol. 29, no. 5, pp. 1257-1270, 2013.
[8] D. H. P. Nguyen, M. Hoffmann, A. Roncone, U. Pattacini, and G.
Metta, “Compact real-time avoidance on a humanoid robot for human- robot interaction,” in HRI ’18: Proceedings of the 2018 ACM/IEEE International Conference on Human-Robot Interaction, (Chicago, IL, USA), pp. 416-424, ACM, New York, 2018.
[9] F. Flacco, A. De Luca, and O. Khatib, “Control of Redundant Robots under Hard Joint Constraints: Saturation in the Null Space,” IEEE Transactions on Robotics, vol. 31, no. 3, pp. 637-654, 2015.
[10] G. Buizza Avanzini, A. M. Zanchettin, and P. Rocco, “Constrained Model Predictive Control for Mobile Robotic Manipulators,”Robotica, vol. 36, no. 1, pp. 19-38, 2018.
[11] A. S. Sathya, J. Gillis, G. Pipeleers, and J. Swevers, “Real-time Robot Arm Motion Planning and Control with Nonlinear Model Predictive Control using Augmented Lagrangian on a First-Order Solver,” in2020 European Control Conference (ECC), (Saint Petersburg, Russia), pp.
507-512, IEEE, 2020.
[12] N. Lucci, B. Lacevic, A. M. Zanchettin, and P. Rocco, “Combining Speed and Separation Monitoring with Power and Force Limiting for Safe Collaborative Robotics Applications,” IEEE Robotics and Automation Letters, vol. 5, no. 4, pp. 6121-6128, 2020.
[13] L. Joseph, J. Pickard, V. Padois, and D. Daney, “Online velocity constraint adaptation for safe and efficient human-robot workspace sharing,” in 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), (Las Vegas, NV, USA), pp.11045-11051, IEEE, 2020.
[14] A. Pereira and M. Althoff, “Calculating human reachable occupancy for guaranteed collision-free planning,” in 2017 IEEE/RSJ Interna- tional Conference on Intelligent Robots and Systems (IROS), (Van- couver, BC, Canada), pp. 4473-4480, IEEE, 2017.
[15] M. M. Nicotra and E. Garone, “The Explicit Reference Governor:
A General Framework for the Closed-Form Control of Constrained Nonlinear Systems,”IEEE Control Systems Magazine, vol. 38, no. 4, pp. 89-107, 2018.
[16] K. Merckaert, M. M. Nicotra, B. Vanderborght, and E. Garone,
“Constrained Control of Robotic Manipulators using the Explicit Reference Governor,” in2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), (Madrid, Spain), pp. 5155- 5162, IEEE, 2018.
[17] R. Smits, “KDL: Kinematics and Dynamics Library,”http://www.
orocos.org/kdl.
[18] K. Merckaert, B. Convens, C. Wu, A. Roncone, M. M. Nicotra, and B. Vanderborght, “Real-Time Motion Control of Robotic Manipula- tors for Safe Human-Robot Coexistence,” Robotics and Computer- Integrated Manufacturing, vol. 73, 2022.
