Force-Motion Control For A Six Degree-Of-Freedom Robotic Manipulator

TL;DR

The work tackles simultaneous motion and contact force control for a $6$-DOF spatial manipulator. It builds a unified controller by stacking a force channel on a modified motion controller, underpinned by a Lagrangian dynamic model with mass matrix $A$, gravity $g(\theta)$, Coriolis-like terms $B(\theta,\dot{\theta})$, and external load $\tau^{ext}$, and a spring-damper obstacle contact model with $K^{o}$ and $D^{o}$. A Jacobian-based mapping from task-space accelerations to joint accelerations, $\ddot{\theta}^{des} = J_{7}^{-1}(\ddot{\mathscr{X}}^{des} - \dot{J}_{7}\dot{\theta})$, enables pose-tracking in free space, while obstacle interaction is governed by $\bar{e}^{b} + \rho f = 0$ and $\dot{e}^{sys} + K e^{sys} = 0$ to bound the end-effector force by $f^{ref}$. Simulation with a digital twin demonstrates that the end-effector tracks the reference trajectory in free space and respects the force bound during contact, validating the approach and illustrating tunable convergence via gains $C$ and $K$.

Abstract

This paper presents a unified algorithm for motion and force control for a six degree-of-freedom spatial manipulator. The motion-force controller performs trajectory tracking, maneuvering the manipulator's end-effector through desired position, orientations and rates. When contacting an obstacle or target object, the force module of the controller restricts the manipulator movements with a novel force exertion method, which prevents damage to the manipulator, the end-effector, and the objects during the contact or collision. The core strategy presented in this paper is to design the linear acceleration for the end-effector which ensures both trajectory tracking and restriction of any contact force at the end-effector. The design of the controller is validated through numerical simulations and digital twin validation.

Figures (9)

• Figure 1: Coordinate frame assignment for CR3 showing x and z axes of all the coordinate frames that are assigned to the robot. The coordinate frames are assigned at the center of mass of each link. Note that the bottom piece is fixed to the ground; hence, it does not influence the dynamics of the manipulator. Also, frame 7 has been assigned at the end-effector location. (Original image credit: Dobot CR3 User Guide)
• Figure 2: Spring-damper force model. The original boundary of the obstacle (solid gray) is distorted (dashed gray) when the end-effector tip (red dot) hits the obstacle. $x$ is the end-effector position and $x^{o}$ is the obstacle position before the elastic deformation.
• Figure 3: Solid and dotted black lines represent the reference path in the free and obstacle spaces respectively. The red colored line represents the end-effector of the manipulator and the red dot represents the tip of the end-effector. The green dotted line is the projection of the reference path onto the top surface of the cube. At a particular time, the reference trajectory $x^{ref}$ is inside the obstacle. In this case, the end-effector (red dot) could acheive trajectory tracking along $x$ and $y$ axes whereas it cannot track the trajectory along the $z$ axis. As a result, the magnitude of the $z$-component of $e$ increases.
• Figure 4: The simplified architecture of the combined controller showcasing the integration of all the key components along with the dynamics of the 6 DOF manipulator. The blue shaded dotted blocks represent functions that compute the outputs specified by the corresponding label. The input to the manipulator is the joint torque $\tau$ and the output of the manipulator are the force exerted onto the obstacle $f$ and the position of the end-effector $x$.
• Figure 5: From the left to the right are the snapshots of the interface of VxSIM™ showing the obstacle detection feature while the end-effector moves from the free space to the obstacle space. The colored dots represent the locations where the rays emitted by the obstacle detector intersect the obstacle. Red and blue dots represent the closest and the farthest intersection points, respectively. Other intersection points are color mapped accordingly.