Moving Horizon Planning for Human-Robot Interaction

The collaboration and interaction between humans and robots intensify with ongoing research and industry needs. Robots require a motion planner that contributes to a safe environment for humans. This paper provides the online trajectory planner Moving Horizon Planning for Human-Robot Interaction (MHP4HRI), customizable for various robots, considering obstacles and humans in their environment. The planner generates motion commands in a moving horizon manner, similar to Model Predictive Control. This enables robots to react to dynamic changes in the environment in real-time. Descriptions of the planner and the underlying algorithms are given, as well as details about the provided framework regarding the benefits and usage for the community. Furthermore, we aim to provide a growing framework with new features in the future regarding the optimization and interaction with the environment, especially humans. The code, implemented mainly in C++ for the Robot Operating System (ROS), is available at GitHub: https://github.com/rst-tu-dortmund/mhp4hri.

probabilistic optimization framework that samples noisy trajectories and optimizes them, CHOMP is a gradient-based optimization method that optimizes an initial trajectory by gradient descent.Both approaches generate a global trajectory for the complete planning horizon until reaching the goal.Therefore, the required computation time is high, and a reaction to dynamic changes in the environment is not possible.On the other hand, local approaches optimize a robot trajectory for a shorter planning horizon and repeat this optimization frequently [29,31].It is required to react to changes from obstacles and humans moving in the robot's workspace in time.Therefore, the local optimization must be executed in realtime in these environments.We defne 10 Hz to be real-time for this application.A drawback of local planners is that they may converge to local extremes that do not lead to the goal and can lead to a standstill in front of an obstacle if no other solution is found.A combination of local and global approaches overcomes this drawback and leverages the benefts of global and local optimizers [12].
The work at hand focuses on a local optimization method that is executed in real-time.We contribute a framework for Moving Horizon Planning (MHP) in dynamic environments with an extended consideration of humans and human motions in the robot environment.Figure 1 shows a visualization of the MHP4HRI and a real-life application with a human interfering with the robot's motion.To allow a wide usage of MHP4HRI, the structure is modular and customizable for various robots and environments, allowing the Human-Robot Interaction (HRI) community a tailored application for their needs.MHP4HRI ofers advantages compared to other open-source frameworks for solving optimization problems like Huge Quadratic Programming [7], planners of the Open Motion Planning Library [26], Trajectory Optimization [25], Augmented Lagrangian Trajectory Optimization [10], Horizon [20] or Fast constrained optimal control problem solver for robot Trajectory Optimization and control [28].Compared to these approaches, our contribution is an extensive consideration of the environment, while most other approaches focus on the robot's motion or the optimization problem.Other approaches that explicitly consider humans in the environment but do not publish their code use trajectory optimization with repulsive forces for collision avoidance [14], human motion predictions and an optimization-based safe interval path planning [27] or intention-aware motion predictions for an optimization-based planner [17].
MHP4HRI provides an open-source planner, environment, and code structure that enables all HRI community members to use it or adapt it to their needs by publishing the complete modular C++ code for ROS with detailed documentation.

PURPOSE
MHP4HRI delivers a local planner utilizing a MHP approach for robot manipulators.The framework includes a separate module for considering dynamic obstacles and humans, including forecasting their motions.Overall, it is possible to use our planner in dynamic environments to plan and execute robot trajectories optimized for various objectives and avoid collisions with obstacles.
The following paragraphs describe key aspects of the framework that help accomplish various problems, and Figure 2 shows a simplifed overview of the framework structure for a human-robot scenario.
Optimization Problem.The frst key aspect of the framework is formulating the optimization problem regarding the robot trajectory planning task.Therefore, the framework formulates the planning problem in a dynamic environment as a Nonlinear Programming (NLP) problem.A solution for a NLP problem optimizes a cost function subject to diferent constraints.The cost function considers various objectives, like the proximity of the robot to the goal state and minimal commanded joint velocities.Furthermore, adaptions of the cost function accomplish a proactive collision avoidance of obstacles inside the workspace.A NLP problem also considers constraints, like joint angle, velocity, and acceleration limits of the robot.In order to reduce the occurrence of collisions, the framework extends the constraints by collision constraints that prohibit robot trajectories from being too close to environmental obstacles.The NLP problem is solved by the Interior Point Optimizer (IPOPT) [30].Furthermore, MHP4HRI includes a hypergraph formulation of the NLP problem to reduce the computation time of the optimization [21,22].
A description, including mathematical formulations of the optimization problem, is given in Section 3.
Human Motion Forecasts and Uncertainties.The second key aspect treats the consideration of humans in the robot's environment.One part of our framework is a motion forecast module providing future poses of the human in the robot workspace.These future poses are directly considered during MHP, so the planner generates a robot trajectory that proactively avoids collisions with humans.Based on current and previous human pose data, a state estimation of a customized human skeleton and a subsequent extrapolation of estimated skeleton joint angles allow a simple but efcient forecast [18].Furthermore, an online uncertainty estimation method determines the current extrapolation uncertainty and allows its consideration in the planning process [19].A machine learning prediction approach is included in the framework as a further forecast option [1].Further details of the methods and integration into the framework are given in Section 3.

CHARACTERISTICS
This section frst describes the mathematical formulations of key aspects of the framework and second introduces the framework's installation and dependencies.

Mathematical formulation
The implementation of the framework utilizes various algorithms and methods introduced in diferent research papers and combines them in one framework to allow the community access to these methods.Therefore, this subsection briefy summarizes essential formulations and references related papers for further details.
In general, MHP4HRI ofers a planning algorithm for robot manipulators in dynamic environments that can be connected to various robots.The main diference to Model Predictive Control (MPC) approaches is that MHP4HRI does not control the robot directly but sends commands for the velocity controllers of the robot joints.
Similar to MPC approaches, MHP4HRI also relies on optimization methods.The discrete optimal control problem is formulated as a NLP problem with a cost function and inequality constraints h: u,x subject to The optimization variables are the control inputs, corresponding to the robot joint velocities, u ∈ R , and the states, corresponding to the robot joint angles, x ∈ R for a robot with degrees of freedom.The time step Δ = +1 − defnes the length between time steps of the discretization grid with 0 and horizon length .The discrete states x with = 0, 1, . . ., and controls u with = 0, 1, . . ., − 1 are the result of a full discretization transcription method [22].
The cost function = G (x) + C (u) + O (x) consists of three parts concerning the quadratic costs G and C for the distance to the goal state and the controls, as well as the weighted costs O for high proximity to collisions with obstacles or self collisions.The proximity costs are defned as: with the robot collision objects R ⊂ ℜ, the obstacle collision objects O ⊂ , the distance function , and an adaptation of the potential function () from Mohri et al. [16].The adaption decouples the dependency of the potential function at = 0 from the threshold value 0 and allows a separate scaling for each obstacle [13].
The inequality constraints h consider the joint limits of the robot, the joint velocity limits, and the acceleration limits.Furthermore, it is possible to activate collision constraints that prohibit the robot from getting too close to obstacles or itself.Note that more constraints increase the computational burden for solving the problem.For further information about the obstacle representations and variations, please refer to [13].
If forecasts of human motions are available, it is possible to consider them in Equation ( 2) or in collision constraints in h.Otherwise, MHP4HRI applies a snapshot method and assumes that every obstacle is static during each planning cycle.To forecast human motions, MHP4HRI ofers diferent opportunities.Based on a neural network approach, it is possible to use a trained network to predict human motions [1].Alternatively, a reduced human skeleton model allows the extrapolation of joint angle motions.An inverse kinematic determines the current joint angles of the skeleton model based on the current human body part poses that can be measured, for example, with a motion capture system.Diferent state estimations for the joint velocity and acceleration fll the state vector for the extrapolation of each skeleton joint [18].Since the extrapolation of human motions exhibits an error compared to the ground truth, an extension of the extrapolation approaches is included.This extension estimates the uncertainty of the extrapolation online with Gaussian Mixture Models [4] and applies this to the representation of the human in the workspace [19].

Installation and Dependencies
In order to use MHP4HRI, several installation options are available, and every user can customize the installation.We provide two main options for installing MHP4HRI, which consists of multiple ROS packages.The frst one is a Docker installation that provides the basic functionality of MHP4HRI and the possibility for customized tests and experiments.The Docker installation is independent of the operating system.
The second option is the manual installation of MHP4HRI.This option is recommended to optimize the performance, customize multiple aspects, or apply it to a real robot.For more details about the installation, please refer to the wiki of the MHP4HRI repository.
Multiple dependencies are required during the installation, which are explained in detail in the wiki.The frst dependency is related to solving the NLP problem.MHP4HRI uses IPOPT as a solver for the NLP problem [30].IPOPT relies on diferent linear solvers like the Harwell Subroutine Library (e.g., Multifrontal Approach 27 [6] or 57 [5]) or MUMPS [2].Other dependencies include external libraries for linear algebra, like Eigen [9] or Armadillo [23] or the prediction approach for human motions [1].
Requirements for the hardware are not specifed in detail since MHP4HRI runs on various operating systems and hardware.Nevertheless, the performance of the optimization solution is highly dependent on the hardware and varies.The wiki provides information about the hardware used during the development and constitutes hardware combinations solving the problem in time.Depending on the number of obstacles and background processes, our test systems also temporarily exceed the solving time for real-time capability.

CODE
This section provides an overview of the code structure and maintenance plan.Please note that MHP4HRI is licensed under the GNU General Public License v3.0 and is free for everyone [8].If you use MHP4HRI in your work, please cite the paper at hand.

Code structure
The GitHub repository of MHP4HRI (https://github.com/rsttu-dortmund/mhp4hri) is extensive and consists of multiple packages with various purposes and a wiki, recommended for every user to check for more details.
Signifcant parts of the code are written in C++ and integrated into ROS to provide easy usage for diferent robots and simulations.Small parts, primarily related to the machine learning part of human motion prediction [1], are written in Python but still integrated into ROS.MHP4HRI mainly comprises three ROS packages, each required for diferent parts of the solution: mhp_planner.This package provides functions related to the optimization problem and its solution strategy.Knowledge about the robot, or in general, the plant, the task, and the environment is required to defne and solve the optimization problem.Therefore, this package defnes interface classes for these elements that allow for implementing inheritance classes for problem variations.The interface classes thereby defne required functions that are equal for all derived classes or declare virtual functions that have to be implemented by derived classes.Furthermore, mhp_planner includes the hypergraph strategy and the integration of IPOPT as a solver for the NLP problem.An adaptable planning frequency of 10 Hz is applied to the local planner to reach real-time capability.mhp_robot.This package provides functions related to the robot and the environment.Like the mhp_planner package, interface classes determine functionalities for derived classes regarding robot kinematics, control, dynamics, and collision objects in the robot workspace.Further elements in this class treat the environment and adaptions like human motion forecasting and uncertainty estimation of extrapolation errors.A workspace-monitor updates the environment defnition of the robot iteratively.Therefore, the workspace-monitor executes various processes related to the environment in a fxed sequence.For example, the processes implement functions like updating obstacles and human positions based on measurements or estimating and extrapolating human motions.To ensure an actual environment in each planning iteration of the local planner, the workspace-monitor updates with a higher frequency of 25 Hz.mhp_robot_ur10_example.This package provides an example application of the mhp_robot and mhp_planner packages for the Universal Robots UR10 (UR10).mhp_robot_ur10_example is a ROS metapackage and groups diferent packages related to the UR10 and the planning problem.In particular, packages related to the gazebo simulation, description of the UR10, and drivers for the real robots are integrated into this metapackage [3,15].To utilize the functions of the planner and the environment packages, derived classes for the interface classes of the mhp_planner and mhp_robot packages are implemented.

Maintenance
The maintenance plan for MHP4HRI intends to provide a stable trajectory planner that increases functionality and efciency over time.Therefore, the code is hosted and maintained on the GitHub account of our institute to guarantee accessibility in the future, independent of single researchers.Due to a running research project, the code is continuously extended and improved and will also take issues and feature recommendations of the community into account.

USAGE
This section provides information about the usage and possible adaptions of MHP4HRI.Furthermore, exemplary experiments and results are presented to provide an insight into the capabilities.

Simulation and Reality
The simulation is ready to use out-of-the-box for the UR10 simulation and allows frst insights and tests with the planner and its possibilities.As usual for ROS, the user starts a launch fle that starts the simulation and the planner.The simulation allows different modes to test diferent scenarios and environments without risks of damaging the robot and the environment or harming humans.Therefore, two demonstrations extend the repository to show the planner's capabilities and environment module.The demonstrations allow testing the planner functions with a human inside the workspace and with two diferent workspace modes.One option is the simulation of human motions by defning the start and end states of the skeleton angles.Nevertheless, simulated human motions are unrealistic and do not consider errors due to measurements or sudden motion changes.Therefore, a second option replays a recorded human motion from a motion capture system.An Optitrack motion capture system records a demonstrative motion that is transformed into pose messages in the ROS network.The human poses are fnally recorded and can be replayed in the simulation.For more details of the demonstrations and required commands, please refer to the usage page of the GitHub wiki.
MHP4HRI also provides a launch fle to start the planner with a real UR10 robot and control it.The option to frst check the planner with a simulation and then apply it to a robot allows everyone a safe and easy start and brings HRI into the real world.A live demonstration of the planner with a UR10 is available on the wiki.

Customize MHP4HRI
A modular structure allows users to extract only parts of the framework they require for their work.This structure allows for adapting the framework to other robots, use-cases, and adding diferent obstacles and humans.The mhp_robot_ur10_example package represents an example of the implementation for a specifc robot.These adaptions are also possible for other robots, and the user can customize MHP4HRI to their needs.Furthermore, changing parts of MHP4HRI to adapt it to varying problems is possible.For example, the cost functions or constraints can be changed to consider further aspects like energy consumption or environmental impacts.

Experiments and Results
This section presents experimental results to show the capabilities of MHP4HRI concerning uncertainties of human motions.Therefore, we apply MHP4HRI in the UR10 simulation with a recorded human motion and compare the results with uncertainty consideration against the Safe Human-Robot Coexistence and Collaboration through Reachability Analysis (SaRA) approach [24] that stops the robot by intersections with human occupancy regions.Due to the potential functions and the uncertainty representation, MHP4HRI plans a trajectory around the motion of the human and does not stop the robot.
MHP4HRI leads to a faster motion of the robot and reduces the time to complete the robot task from 11.7 s for the SaRA approach by nearly 25 % to 8.8 s with MHP4HRI.A further criterion, especially in HRI, is safety.The minimum distances between the robot and human body parts (see Fig. 1a) conclude the experiments during the motion.Note that this is not an extensive safety analysis and only provides a brief insight into the performance of MHP4HRI in dynamic environments.MHP4HRI increases the averaged minimum distances between the robot and the human rounded with factor 3. A reason for the high diference besides the proactive collision avoidance is that we use the recorded motion of a human.Even if the robot is close to the human and SaRA already stopped the robot, the human moves further and gets closer.Nevertheless, this brief comparison shows possible use-cases and capabilities of the planner and its environment module for HRI.For videos and details, please refer to the wiki's experiments section of the usage page.

FUTURE RELEASES
We hope the community will contribute and add recommendations for features and issues for code improvement to MHP4HRI.
On the technical side, we aim to improve the performance of the planner and upgrade the code to ROS2.On the theoretical side, we aim to add various features and extensions.These features include further collocation methods, potential functions, and a task space extension.Furthermore, an extension concerning suboptimal solutions of local planners by diferent initializations and adaptive intermediate goals is planned to increase performance in workspaces with humans [12].Other features include new functions concerning the robot environment, like modeling environments and human motion tracking for data gathering.
(a) Visualization of the MHP4HRI (b) Real situation with a human in the workspace

Figure 1 :
Figure 1: Overview of the MHP4HRI in a real-life application.For full videos, please refer to the wiki at GitHub.

Figure 2 :
Figure 2: Overview of the MHP4HRI structure for a humanrobot scenario.