During my PhD, a control solution featuring an energetic constraint was developed to improve the safety of a robotic manipulator sharing its workspace with humans. This general control structure, exploits a generic safe controller that ensures the respect of multiple constraints thanks to a Linear Quadratic Problem formulation. With a unified energetic formulation, the controller allows to explicitly limit both the kinetic energy when moving and the wrench applied to the environment in case of contact with and unexpected obstacle. It is experimented on a redundant Kuka LWR4+ robot which end-effector shall precisely point toward a given location while following a trajectory.

Safety should be taken as a constraint in the control problem such that

$$ \begin{matrix} \boldsymbol{\tau}^{opt} = & \displaystyle \arg\min_{\boldsymbol{\tau}} & \boldsymbol{T}_{pos}(\boldsymbol{\tau}) + \boldsymbol{T}_{point}(\boldsymbol{\tau}) + \boldsymbol{R}(\boldsymbol{\tau}) \\\ & \textrm{s.t.} & A \boldsymbol{\tau} \le \boldsymbol{b} \\\ & & e_{c,k+1}(\boldsymbol{\tau})\leq e_c^{lim}& \end{matrix} $$

$$ \Delta e_c = e_{c,k+1} - e_{c,k} \approx \underbrace{\left(\boldsymbol{v}_k \Delta t + \frac{1}{2} \sum\limits_{i=k}^{k+n-1} \boldsymbol{\dot{v}}_i^c(\boldsymbol{\tau}) \Delta t^2 \right)^T}_\text{\normalsize Provisional displacement} \underbrace{\Lambda_k \sum\limits_{i=k}^{k+n-1} \boldsymbol{\dot{v}}_i^c(\boldsymbol{\tau})}_\text{\normalsize Pushing force}. $$

Transient contact

In this second experiment, the robot performs a back and forth motion while pointing towards a fix target. The regularisation task is set to minimize the gravity induced torques. At some point, a human operator is preventing the robot from moving for a brief period. The contact is then released.

It can be observed that when the robot is disturbed and almost stopped, the current kinetic energy becomes null while the provisional kinetic energy quickly reaches the defined limit, $e_c^{lim}$. The formulation of this constraint inside the QP ensures that the kinetic energy limit is never crossed. It has been checked during the experiment that the kinetic energy constraint reaches its limit before saturation of the PID controller. As a matter of fact, it can be observed that the provisional kinetic energy stays at the limit even after contact breaks. This allows a safer interaction. The system then reduces its error until accomplishment of the trajectory tracking task with a safe amount of energy.

Quasi-static contact

When the robot hits a fixed obstacle, the applied contact wrench must not exceed a limit specified by the ISO TS 15066. To assess that the kinetic energy constraint can also be used to indirectly limit the contact wrench, the ATI sensor recording forces and torques, is mounted on a girder that is placed on the robot trajectory. In this experiment, the robot is required to follow a straight line with a goal position beyond the ATI sensor.

Once the controller energy reaches its limit and $\boldsymbol{\dot{v}}^*_k$ reaches its saturation, there is a stabilization of the applied efforts to a value slightly above the limit (72 N).