Articulated robot control is a well-understood field, and industrial robots can attain impressive levels of precision even along complex trajectories. However, for optimal performance it is important for the controller designer and the trajectory designer (or trajectory planner) to cooperate or at least be cognizant about the limitations of each others' components.
As an example, consider an industrial robot performing a semi-repetitive task like welding, where improving productivity means reducing the duration of every cycle. The robot cannot instantaneously accelerate and decelerate due to inertia, so if the desired trajectory slows down faster than the robot's motors can brake, then the robot will overshoot the desired target. Also, if the robot is operating in tight quarters, the amount of clearance needed between the robot and obstacles is typically proportional to its speed. The applications engineer must either closely coordinate with the controls engineer to design the trajectory according to the controller's specifications --- or else face a tedious process of trial-and-error.
In many respects this chapter is quite vague on the details about how to actually implement a beautifully-performing articulated robot controller. This is by necessity; almost every robot that you purchase or build is going to have a different set of control capabilities and interfaces. An inexpensive hobby robot will not, by and large, have pinpoint accuracy and convenient trajectory design tools. A hydraulic industrial robot will not have a light touch. In most situations, as a robot engineer you often must (if I may twist a quote by Donald Rumsfeld) go to war with the robot you have, not the robot you wish you have. And one of the first battles you will face is to "wrap" the out-of-the-box control functionality of the robot into a useful, familiar interface that will help the application designer work with higher-level primitive operations.
Who controls what? The motor controller¶
Rarely would anyone find a robot whose computer applies voltages or currents directly to the motors. Rather, between the CPU and the motors sits a device called the motor controller (or more simply, the controller) that takes digital inputs from the CPU and outputs analog voltages or currents to the motors. It may also have the responsibility of converting readings from the motor encoders back into position readings for the computer.
The controller's electronics or microprocessor perform some simple transformations of the digital input to derive the voltage or current signal. In the simplest case, it may just act as a digital-to-analog converter, in which case it is called a motor driver. However, most controllers perform other operations as well, such as calibration, PID control, filtering, velocity limiting, temperature regulation, and/or joint trajectory interpolation. These are important functions to prevent damage to the motor, such as burnout, and can also be more convenient for the end user. The end user in this case would be an application designer or a control engineer, who is interested in higher-level concerns than how the motor is driven. However, this also adds to complexity, since the user must understand the controller's functions and limitations!
In an ideal world, the designers of motor controllers should also understand and implement the end user's desired functionality of the motor. Institutions with dedicated mechatronics labs might be able to do this, but more likely the mechatronic specialists are separate from the control engineers. As a result the burden likely falls on the user to understand (and compensate for!) the choices made by the motor controller designer. It can be frustrating to plan for, say, controlling a robot with velocity inputs... and then to learn that the controller only supports Go-To commands.
Therefore, the first step in deciding how to control a robot is to characterize the inputs and performance characteristics of the controller. Some of these characteristics include maximum executable velocity or acceleration, PID gains, frequency response, tracking error, nominal and peak load, etc. More sophisticated (and expensive) controllers may also provide fault detection and recovery, motion queues, and gravity compensation. Another degree of sophistication is whether the motor controller operates a single joint (requiring multiple controllers per robot) or multiple joints. The latter are known as multi-axis controllers, and can perform sophisticated operations including Cartesian control and synchronized Go-To movements onboard. This offloads work that would otherwise be performed on the CPU onto the controller.
As a general rule of thumb, cheap motor controllers require the application developer to put in more effort to provide high-level functionality, like trajectory execution or Cartesian movement. But more expensive controllers require the application developer to spend more time reverse-engineering the controller's behavior. For example, how long will it take to go from configuration A to configuration B? The rest of this chapter will describe how high-level functionality might be provided --- whether it resides on the CPU or the motor controller.
Trajectory following¶
Articulated robots are an example of a second order dynamic system, and here we consider only fully-actuated articulated robots in which each configuration variable has a corresponding control variable. In other words, $n = 2m$, since the state variable $x=(q,\dot{q})$ is the phase of the system, and the control $u$ is assumed to affect the rate of change of $\dot{q}$.
The basic idea of trajectory following is to define a desired joint space trajectory $q_D(t)$ and to have the robot's actual trajectory $q(t)$ follow it. Assuming the robot can precisely measure its configuration and velocity, the general method for doing so is simply to send the desired configuration $q_D(t)$ and velocity and $\dot{q}_D(t)$ to a lower level controller, such as a PID controller, while updating the setpoint at a sufficiently high rate. However, to obtain accurate tracking, the designer of the trajectory must be cognizant of the control performance of the robot as a second order system, since at the lowest level the motor controller must eventually convert setpoint commands into joint torques.
First, discontinuities in $q_D(t)$ should be forbidden since the robot will accelerate quickly, and possibly overshoot the target. Similarly, discontinuities in $\dot{q}_D(t)$ should be avoided if possible, because exact tracking would require instantaneous acceleration. Naively requesting sudden changes of velocity runs the risk of overheating motors and overshooting setpoints.
Secondly, joint stops and self-collision should be avoided with some margin of error, since the controller will not execute the trajectory exactly, especially at high speed. The magnitude of the margins should be chosen proportionally to the magnitude of control errors, which depend on the stiffness of the underlying controller.
There are several functions that a motor controller may provide to enforce trajectory smoothness and safety. Or, the end user might need to (or to prefer to) implement these his/herself. These functions will be discussed below.
Motion queues (motion generation)¶
To improve the convenience for a robot application designer, and to avoid some of the risks of allowing execution of non-smooth trajectories, most motor controllers provide a Go-To command, in which the desired endpoint is reached gradually over time. In essence, the motor controller is generating the robot's reference trajectory $q_D(t)$ as it goes.
This is a simple form of a motion queue mode of operation in which the application requests milestones or trajectory segments to be queued up on the controller (i.e., multiple Go-To commands, or a Move-Path command). Over time, the controller will execute the requested trajectory sequence without further input from the application (Fig. 2). The convenience of this mode of operation is quite apparent when programming long sequences of instructions with discrete event logic.
To implement such motion queues, the controller maintains a future trajectory in its memory. As in typical trajectory following, time is advanced on each control loop, and the desired setpoint is updated. Furthermore, the future trajectory can be modified at any time by an external request signal (e.g., a function call). For example, adding a Go-To request will append an interpolating curve from the end of the queued trajectory to the requested target position (a waypoint). If the current queue is empty, then the curve will originate from the current robot position. This type of asynchronous request is convenient because the application developer does not need to develop a fixed-rate control loop. Moreover, motion queues might be directed to cycle, making it convenient to define repetitive movements used in factory settings.
Motion queue methods differ greatly in their interpolating curves, speed and velocity control, their ability to handle joint-space vs. Cartesian space targets, and whether an existing queue may be interrupted. Some of these differences are discussed below.
Interpolation profiles¶
Given the robot at rest and a target configuration, an interpolation profile is a continuous trajectory that satisfies certain operational constraints on the robot's joint dynamics. A sequence of Go-To commands can be implemented by concatenating several interpolation profiles, in which the robot stops/starts at subsequent waypoint configurations.
Piecewise linear¶
The most basic interpolation profile is a piecewise linear curve. Additional milestones are added in a manner that limits the maximum absolute joint velocity (Fig. 3). However, the trajectory exhibits instantaneous changes of velocity. This type of profile is most often observed in inexpensive hobbyist controllers.
Trapezoidal velocity profile¶
A refinement of this approach is a bounded-acceleration, bounded-velocity curve. To arrive at a target configuration in a minimum-time fashion under these bounds, while also starting and stopping at 0 velocity, a trapezoidal velocity profile may be used (Fig. 4). Assume the waypoint joint value $\theta_{g}$ is greater than the start value $\theta_{s}$ (in the reverse case, the profile may simply be flipped). The velocity profile ramps up from 0 at maximum acceleration $a_{max}$ until the maximum velocity $v_{max}$ is reached, then proceeds at constant velocity for some time, and then decelerates to 0 velocity at minimum slope. If the target is close to the original configuration, the maximum velocity may not actually be reached, in which case the velocity profile takes on a triangular shape.
The duration $t_f$ of the 2 or 3 segments of the profile are determined analytically from $\theta_{g}-\theta_{s}$ and the acceleration / velocity bounds. Specifically, first consider the 2-segment case. The curve $\theta(t)$ begins with an upwards-facing parabola for duration $t_f/2$ and ends with a downwards facing parabola with an equal duration. Hence, the displacement at the midpoint $(\theta_{g}-\theta_{s})/2$ must be equal to $\frac{1}{2} a_{max} (t_f/2)^2$. In other words, $t_f = 2 \sqrt{(\theta_{g}-\theta_{s})/a_{max}}$. If the midpoint velocity $a_{max} t_f /2 > v_{max}$ , then the velocity bound is reached and we must use the 3 segment case. In this case, the first segment accelerates at acceleration $a_{max}$ for time $t_1 = v_{max}/a_{max}$, and the last segment decelerates for the same amount of time $t_3 = t_1$. In this time they each cover distance $\frac{1}{2} a_{max} (v_{max}/a_{max})^2 = v_{max}^2 /(2 a_{max})$, and hence the second segment must cover distance $\theta_g - \theta_s - v_{max}^2 /a_{max}$. It does so at speed $v_{max}$, so it takes time $t_2 = (\theta_g - \theta_s)/v_{max} - v_{max}/a_{max}$. Ultimately, the overall time is $t_f = 2t_1 + t_2 = (\theta_g - \theta_s)/v_{max} + v_{max}/a_{max}$.
Bounded-jerk interpolation¶
A second potential extension, which provides even smoother curves, is a bounded-jerk trajectory. Jerk is another name for the third time derivative of a function, and bounded-jerk paths will see the acceleration profile having a positive trapezoid and negative trapezoidal components. Determination of the number of segments, segment timing, and overall duration is similar to the above construction, although higher-order polynomials need to be solved.
General methods and cubic curves¶
Other potential interpolation profiles include cubic curves (Hermite interpolation) and sinusoidal curves (cosine smoothing). In each case, coefficients of the profile and the interpolation duration are determined by matching the boundary conditions and any operational constraints on the velocity or acceleration of the curve.
For example, a cubic curve $\theta(u)$ interpolating between $\theta_s$ and $\theta_g$ over the range $u\in [0,1]$ has the form $$\theta(u) = \theta_s + (\theta_g-\theta_s)(a + bu + cu^2 + du^3)$$ Using the boundary conditions $\theta(0) = \theta_s$, $\theta(1) = \theta_g$, $\theta'(0) = 0$, and $\theta'(1)=0$, we have four linear constraints that must be satisfied: $a=0$, $a+b+c+d=1$, $b = 0$, and $b + 2c + 3d = 0$. The solution is $a=0$, $b=0$, $c=3$, $d=-2$, and hence $$\theta(u) = \theta_s + (\theta_g-\theta_s)(3u^2 - 2u^3)$$ is a smooth interpolator known as the Hermite curve.
The remaining question is how to set the time duration of the curve $t_f$ to maintain velocity and acceleration limits. We define $u = t/t_f$, giving the expression for velocity $$\dot{\theta}(t) = \frac{d}{dt} \theta(u(t)) = \theta^\prime(u(t)) u^\prime(t) = (\theta_g-\theta_s)(6u - 6u^2) /t_f = 6(\theta_g-\theta_s)(t/t_f^2 - t^2/t_f^3)$$ This is maximized at $t=t_f/2$, which gives the maximum velocity along the curve as a function of $t_f$ $$v^\star(t_f) = \frac{3}{2}(\theta_g-\theta_s)/t_f.$$ The acceleration is $$\ddot{\theta}(t) = 6(\theta_g-\theta_s)(1/t_f^2 - 2t/t_f^3)$$ which has maximum absolute value at the endpoints. Hence, the maximum acceleration along the curve is $$a^\star(t_f) = 6(\theta_g-\theta_s)/t_f^2.$$ It is then a simple matter to determine whether the curve is acceleration limited or velocity limited, and then to set $t_f$ appropriately.
Speed and velocity control¶
Speed control is a fairly common addition for Go-To commands, and simply modulates the rate of progression of "time" along the trajectory. However, to avoid sudden changes in velocity it is important not to modify path speed instantaneously while the robot is moving. Alternatively, speed control may be provided by modulating the maximum velocity / acceleration used to derive the interpolation profile.
Velocity control is a desirable capability for maximizing robot speed because the target joint velocities can be chosen so that movement to the next milestone does not require stopping. Trapezoidal velocity profiles and cubic curves are the most convenient forms for allowing waypoint velocities. To do so, the specified velocities should be incorporated as boundary conditions when solving for the form of the interpolation profile.
Cartesian commands¶
More sophisticated multi-axis motor controllers, such as those found in industrial robots, may mix joint-space and Cartesian commands (Fig. 5).
Cartesian motion queue functionality is useful to guide tools along workspace paths, while joint-space waypoints are superior for moving quickly between configurations. A motion queue can construct Cartesian interpolation profiles, with two caveats.
First, IK solutions along the interpolation profiles should be verified to exist when given a Cartesian Go-To command. If no solution exists, the command should return an error code.
Secondly, during execution, an IK solver must be used to find the joint space configuration and velocity for the interpolated Cartesian target position. These values will be then used as setpoints to the low level controller. Alternatively, operational space control methods can be used for direct Cartesian control without the use of an IK solver using only Jacobian information. Operational space control will be briefly covered below. In either case, error codes may be invoked during execution when no IK solution exists, the robot hits joint limits, or passes near singularities.
Interruption¶
Interruption is a helpful feature for motion queues when the robot may need to react to unexpected conditions. As an example, a stop and reset may be triggered when the robot mistakenly drops an object during a transfer task.
To stop movement an unspecified time, it is a simple matter to clear the remainder of the queue once a robot reaches a stopped waypoint. But if the robot should be stopped during the middle of movement, some care must be taken. One option could be to execute a maximal braking trajectory, which slows down each joint as quickly as possible. The issue with this approach is that the robot will likely drift from the desired trajectory because each joint is slowed at a different rate, which may cause collisions. An alternative is just to slow the robot down along its current trajectory by reducing the path speed to zero.
Another mode of interruption is to instantaneously start moving to a new waypoint, clearing the remainder of the queue. This was illustrated for the simple piecewise linear interpolation profile in Fig. 3. However, to avoid sudden changes of velocity, the interpolation profile used in the queue should support nonzero initial velocities.
The most sophisticated mode of interruption would to allow unlimited editing of the motion queue at any point in time. Providing such functionality usually requires a fair amount of bookkeeping on the controller, and hence is rarely implemented for most industrial robots.
State machines¶
For many tasks, the robot controller should perform some notion of switching between discrete modes of operation. For example, to implement Cartesian commands and braking trajectories, the controller must activate different functions, accept different input, and report different status codes depending on which mode it is in. This type of functionality can be thought of as a state machine, and is usually implemented as one as well.
A state machine prescribes several possible discrete states $S_0,S_1,\ldots,S_N$ under which the controller can operate. (These are distinct from the robot's state in the context of dynamics, and order does not matter.) The controller can be in one state $S(t) \in \{ S_0,S_1,\ldots,S_N \}$ at any given time, starting with the initial state $S_0$. The controller still operates on a fixed control loop, and performs whatever processing is designated by state $S(t)$. This includes input-output processing, but also potentially reacting to signals which indicate that the state machine should change state. These signals $T_{ij}$ are known as transitions. If at time $t$, $S(t) = S_i$, and transition $T_{ij}$ becomes active, then we change the state $S(t+\Delta t)\gets S_j$ for the next time step. (If multiple transition signals are raised at the same time, the controller implements some tie-breaking policy.)
State machines are a very useful tool for visualizing the functionality of controllers from low-level motor controllers to high-level supervisors, and are typically drawn as directed graphs (Fig. 6). We shall revisit these diagrams later when we discuss system integration.
Feedforward torque control¶
Although PID control is sometimes adequate for strong motors or lightweight robots, it is not sufficient to achieve the level of trajectory tracking accuracy demanded by industrial applications. There are three reasons for this.
Gravity biases the torques needed to keep the robot upright. A PID controller compensates for gravity via the integral term, but the amount of compensation required is highly configuration dependent. In fact, if a joint axis flips orientation, the torque needed to resist a given load will need to entirely reverse sign.
The effective inertia of the subchain at each joint is highly dependent on its configuration. For example, if an arm is outstretched, the shoulder joint will need to apply stiffer torques to track a trajectory than if it is tucked.
At high speed, the Coriolis term becomes significant, causing biasing torques that must be compensated.
As a result, many industrial robots use model-based techniques to calculate feedforward torques that reflect some of the biases coming from the robot's dynamics. The simplest form of feedforward torque computation is gravity compensation. In this mode of operation, the feedforward torques only include the generalized gravity term of the robot's dynamics equation: $$\tau_{ff} = G(q)$$ where $q$ is the current sensed configuration. This torque is combined with the feedback torque $\tau_{fb}$ resulting from the PID controller to obtain the overall torque: $$\tau = \tau_{fb} + \tau_{ff}.$$ In this form of operation, when the robot is stationary, the force from gravity will be exactly compensated for by $\tau_{ff}$, and hence $\tau_{fb}=0$ since the PID controller will not observe any error. This does, however, assume that the masses, centers of masses, and link lengths of all robot links are measured precisely for the calculation of $G$. In practice, these estimates will have small errors and hence the feedback term is required for regulating the errors between the true $G(q)$ and the estimate $\tilde{G}(q)$. Moreover, once the robot begins moving, the PID controller will need to take on the responsibility of compensating for inertial and Coriolis effects. See Fig. 7 for an illustration.
A more sophisticated controller is computed torque control, which also accounts for inertial and Coriolis effects during movement. Supposing we have a desired acceleration $\ddot{q}_D$. If we use inverse dynamics to calculate the necessary torque needed to guide the robot upon this acceleration: $$\tau_{ID}(\ddot{q}) = B(q) \ddot{q} + C(q,\dot q) + G(q) \label{eq:InverseDynamics}$$ then feeding this torque to the robot will generate the desired acceleration, assuming no disturbances and modeling errors. There are generally two methods for compensating for error. The first method uses $\tau_{ID}$ as a feedforward control, combined with a PID feedback: $$\tau = \tau_{ID}(\ddot{q}_D) + \tau_{fb}.$$ The second method, computed torque control, instead computes a desired acceleration as the sum of $\ddot{q}_D$ with a feedback acceleration: $$\ddot{q}_{fb} = - K_P(q-q_D) - K_I I(t) - K_D(\dot{q}-\dot{q}_D)$$ The resulting controller uses the torques computed for the feedforward + feedback accelerations: $$\tau = \tau_{ID}(\ddot{q}_D + \ddot{q}_{fb}).$$ This approach is appealing because with acceleration being the control variable, the PID constants can be chosen easily so the joint space dynamics are convergent. The main drawback of computed torque control methods is the additional computational expense of computing inverse dynamics, which must be recomputed on a per-timestep basis. It also faces the general difficulty of calibrating accurate inertial parameters of a robot.
Operational space control¶
It is often desired to control the end effector of an articulated robot more accurately in Cartesian space than in joint space. Operational space control is a method for doing so without the use of inverse kinematics. Another application of operational space control is to apply or respond to forces in Cartesian space, which is a key step in force control and impedance control. Properly applying these concepts requires a deep understanding of how to transform forces and robot dynamics between joint space and the operational space.
Here, operational space is synonymous with the definition of task space that we have used throughout this course. Specifically, a task space $x = \phi(q)$ is defined with the Jacobian $J(q) = \frac{\partial \phi}{\partial q}(q)$. As usual, the task space is typically a combination of Cartesian position and/or orientation of an end effector. (Here we have chosen the notation $\phi$ rather than the more typical $x = f(q)$ so as to not conflict with the $f(x,u)$ notation used for a dynamic system.)
Mathematical derivation¶
To get a sense for this approach, let us suppose that the robot's dynamics are first-order and velocity controlled. In other words, we wish to determine a control $u$ such that $\dot{q} = u$. Let $x_D(t)$ be a task space trajectory that we wish to control, and let $x = \phi(q)$ be the current task space coordinate. We can define a commanded task velocity $\dot{x}_{cmd} = -K_P (x - x_D(t)) + \dot{x}_D(t)$ via a feedback position gain $K_P$ and the feedforward velocity $\dot{x}_D(t)$. To convert from the task velocity to the control, we can use the Jacobian relationship $\dot{x} = J(q)\dot{q}$. Using the pseudoinverse, we have $u = \dot{q} = J(q)^+ \dot{x}_{cmd}$. If $x_D$ is stationary, then given any starting configuration, this will drive the task coordinate $x$ to converge toward $x_D$.
Note that we could also use the Jacobian transpose rule $\dot{q} = J(q)^T \dot{x}_{cmd}$ and derive a stable controller in task space. To see this, observe that the dynamics of the task space error $e = x - x_D$ are given by $$\dot{e} = J \dot{q} = - J J^T K_P e.$$ Hypothesizing the Lyapunov function $V(e) = \frac{1}{2} e^T K_P e$, we can calculate $$\dot{V}(e) = \frac{\partial V}{\partial e}(e)^T \dot{e}.$$ Under the observation that $\frac{\partial V}{\partial e}(e) = K_P e$, observe that $\dot{V}(e) = -e^T K_P J J^T K_P e$. If $J$ and $K_P$ are nonsingular, then $K_P J J^T K_P$ is a symmetric positive definite matrix, and hence this is a negative definite function, i.e., negative for all $e\neq 0$. Hence, this system is Lyapunov stable away from singularities.
Typically, operational space control is applied with second-order dynamics and a torque-controlled robot. This makes it possible to treat the operational coordinate as a sort of virtual rigid body under spring-damper dynamics. Suppose we wish to control the dynamics of the task space coordinate according to the trajectory $x_D(t)$. Then the desired acceleration is $$\ddot{x}_{cmd} = -K_P(x-x_D(t)) - K_D(\dot{x} - \dot{x}_D) + \ddot{x}_D(t). \label{eq:SpringDamperAcceleration}$$ Computing the time derivative of the task space mapping twice, we obtain: $$\ddot{x} = \frac{d}{d t}(J(q)\dot{q}) = J(q)\ddot{q} + \dot{J}(q)\dot{q}$$ Let us examine how the task space acceleration is affected by task space forces, $f_{cmd}$, such that the resulting torque is $\tau = J^T f_{cmd}$. For convenience of the expression, we drop the dependence of several terms on $q$: $$\begin{split} \ddot{x} &= J(B^{-1}(\tau - C(q,\dot{q}) - G) + \dot{J}\dot{q} \\ &= JB^{-1} J^T f_{ext} - JB^{-1} C(q,\dot{q}) - JB^{-1} G + \dot{J}\dot{q} \end{split}$$ Recall from the discussion on effective inertia that the matrix $\tilde{B} = (JB^{-1}J^T)^{-1}$ is the effective inertia of the task space coordinate. Multiplying both sides by $\tilde{B}$ and rearranging, we get $$\tilde{B}\ddot{x} + \tilde{B}(JB^{-1} C(q,\dot{q}) - \dot{J}\dot{q}) + \tilde{B}JB^{-1}G = f_{ext}$$
If we let $\tilde{C}(q,\dot{q}) = \tilde{B}(JB^{-1} C(q,\dot{q}) - \dot{J}\dot{q})$ and $\tilde{G} = \tilde{B}JB^{-1}G$, we have the task space dynamics expressed in a familiar form: $$\tilde{B}\ddot{x} + \tilde{C}(q,\dot{q}) + \tilde{G} = f_{ext}.$$ Thus, with a little linear algebra, we can solve for $f_{ext}$ such that $\ddot{x} = \ddot{x}_{cmd}$. Applying $\tau = J^T f_{cmd}$ we can finally derive the output joint torques $\tau$.
The result of operational space control (with $K_P$, $K_D$ diagonal) is that the system response to step changes in a Cartesian task space is linear in Cartesian space. If same step change was achieved by a change in configuration setpoint to a PID controller, the system response would be nonlinear.
However, there are a number of difficulties in implementing operational space control. First, it is important not to let the Jacobian become singular, because the effective inertia will become infinite. Even if it is near singular, the inversion involved in computing the effective inertia matrix will cause unacceptably large torques to be computed. Second, if the configuration has met one or more joint limits, some directions in the task space will become uncontrollable. In light of these issues achieving stable operational control often requires a significant amount of tuning.
Force and impedance control¶
Using operational space control and a force sensor, it is possible to obtain a number of interesting behaviors, including Cartesian force control where the end effector can apply a specified force, or "zero gravity mode" where the end effector behaves like a free-floating rigid body. These two behaviors are known, respectively, as force control and impedance control.
In both cases, force sensing can be accomplished with a sensing element (a force/torque sensor) on the end effector, or indirectly via joint torque sensors. Joint torque sensing is generally less accurate at estimating the forces applied to the end effector due to loss of observability at singularities, but does not require external equipment.
It should be noted that force sensors are prone to noise, requires frequent recalibration, and runs a risk of irreparable damage to force sensors by overloading. Moreover, to obtain stable control loops a great deal of tuning, calibration, and optimization of sensor signal filters, the force sensor's inertial parameters, the robot's inertial parameters, and time delays. Hence, implementing force and impedance control successfully in practice requires substantial amounts of effort from even experienced control engineers.
Force control is the process of regulating a force applied to the environment in closed-loop fashion. To illustrate how this might works, consider a 1-D task space $x = \phi(q)$ denoting the end effector position along some axis. Assume that the robot is contacting the environment at the initial configuration $q_0$, and that we would like to apply a desired force $f_D$ at the end effector. The basic idea of force control is to sense the external force $f_{ext}$ and if it is too high, to reduce $x$, and if is too low, to increase $x$. Although this is conceptually simple, the implementation is not!
The main issues to address are that 1) the force sensor does not directly report $f_{ext}$ but rather a combination of external force, gravity, inertial forces due to end-effector acceleration, and noise, 2) it is not clear how much to increase or reduce $x$ to obtain a desired change in force, and 3) those changes in $x$ need to be converted into joint-level commands. Operational control handles issue 3, but not 1 or 2. To extract the external forces from the sensed forces, the operation of a force sensor should be modeled in detail. Accelerations will be observed as force spikes in the sensed force signal, and changes in the end effector orientation will be observed as a bias in the force signal: $$f_{sensed} = f_{ext} + f_{gravity} + f_{inertial} \label{eq:ForceSensor}$$ The best way to estimate the external force is to model the force of gravity and inertia on the sensor by estimating the sensor's orientation and acceleration and subtracting their sum from the sensed force. Orientation and acceleration must be estimated from the robot's encoders or an inertial measurement unit.
Next, we need some way to link the deviation of force $f_D - f_{ext}$ and the control signal given to the operational space controller (usually $\ddot{x}_{cmd}$ or if joint torque control is available, the commanded end effector force $f_{cmd}$). This can be done using a PID loop where $f_D - f_{ext}$ is the error signal and the operational space control signal is the output. In the case that $\ddot{x}_{cmd}$ is the output, the gain should be on the order of the inverse of the stiffness of the environment - end effector system, while if it is the end effector force, the gains should be on the order of 1.
Impedance control implements a behavior where the robot's end effector as though it were a mass-spring-damper (MSD) system subjected to an external force (usually without gravity). The equations of an MSD are as follows: $$\ddot{x} = M^{-1} (f_{ext} - K (x_D-x) - D \dot{x}_D) \label{eq:MassSpringDamper}$$ with $M$ the mass matrix, $K$ the stiffness matrix, and $D$ the damping matrix. For example, a zero-gravity mode sets $K$ and $D$ to 0 and the robot's end effector behaves like a free-floating mass in response to applied forces. To simulate MSD behavior on a robot's end effector, equation ($\ref{eq:MassSpringDamper}$) can be used in conjunction with ($\ref{eq:ForceSensor}$) to estimate $f_{ext}$, and the resulting $\ddot{x}$ should be fed into an operational space controller as $\ddot{x}_{cmd}$.
An implementer of such systems should be aware that it is difficult to simulate a low-mass MSD with a high-mass robot, since the accelerations encountered by the mass in response to an applied force may exceed the limits of the robot's torques. The range of accelerations that a robot can perform is also known as its control bandwidth. Lighter robots, with stronger motors, and with low mass near the end effector tend to have larger control bandwidth than heavy ones.
Summary¶
Key takeaways:
To maximize performance of an articulated robot, its trajectories should be designed with motor controller characteristics in mind (and vice versa).
A robot's commands from its CPU are processed through a motor controller board, which may implement wildly differing amounts of functionality for safety, accuracy, and convenience purposes.
Motion queues are a common function of motor controllers that create trajectory interpolation profiles from asynchronous commands.
Gravity compensation and computed torque control are two forms of feedforward torque control that improve the accuracy of trajectory tracking.
Operational space control redefines the PID control in some Cartesian task space rather than joint space. This allows relating torques to forces, as needed for applications like force and impedance control.
Exercises¶
Suppose that your robot only has an ability to perform velocity control, in which the CPU specifies the joints' desired velocities. Commands are read in at a fixed rate (say, 100Hz). Describe how you would implement a PID controller on the CPU that tracks a trajectory $q_D(t)$. Keep in mind that velocities are not executed perfectly, and that the robot should obtain 0 steady-state error if the trajectory is stationary.
Propose a data representation and algorithms for a motion queue controller such that updating the current setpoint takes $O(1)$ time, and such that an application on the robot's CPU can append to it and clear it in $O(1)$ time. Your implementation should discard portions of the queue that are in the past, either with a linked list or circular buffer. (For simplicity, use piecewise linear interpolation.)
Motor controllers have a limited amount of memory and must determine desired setpoints at a very fast rate. Why must the motion queue size, in practice, be limited? If interrupts are allowed, what else limits the motion queue size?
Empirically study the performance of high-gain PID control on a simulated 6DOF robot. First, tune the constants so that it is stable. How does it react to gravity when it is first turned on? How does it react to disturbances? How well does it track when commanded to a very different configuration?
Consider a straight line movement in Cartesian space. In the absence of disturbances, how would the output of an operational space controller compare to that of a Cartesian motion queue executed with a joint space PID controller? How would they differ in rejecting disturbances?
Implementation. Implement a Cartesian velocity motion queue controller for a planar 2R manipulator. As commands, the controller should take velocities $(v_x,v_y)$ and movement durations $t$, and simply use piecewise linear interpolation. Develop some sensible scheme to handle singularities and joint limits by stopping motion. Test its performance in simulation with a variety of command sequences.