Trajectory Basics VII: Adaptive Tracking III

Picking up from where we left off, we will talk about two remainders in this article: 1. the inverse dynamics formulation \(\mathcal{M}\) for path points within look-ahead distance, and 2. the open loop method to generate a velocity profile under system constraints.

Let’s first revisit the last diagram from article VI. In the close loop method, say the look-ahead distance is \(3\rho\). After passing point 0, the tracker will include point 1, 2, 3 for the next step planning. To compute the speed upper bound in task space at these points, generically speaking, we have to find the system control space states in order to utilize \(\mathcal{M}\) in order to compute the limits in task space, because generally inverse dynamics is dependent on the control space states. Given the tracker as an end effector of a fully-actuated articulated arm at position \(x\), consider the simple case first, where the joint velocity \(\dot{q}\) and acceleration \(\ddot{q}\) limits are given, as the so-called first-order constraints:

\[\begin{align}\dot{x} &= J(q)\dot{q} \\ \ddot{x}&=\dot{J}(q)\dot{q} + J(q)\ddot{q}\end{align}\]

In this case, we just need kinematics equations and conveniently solve for limits in task space. However, this is quite uncommon, as \(\ddot{q}\) drops lower when \(\dot{q}\) increases, given fixed torque; the constant speed and acceleration limits only occur when motors operate at low effort. Most limits are given at the torque level \(\tau\), and we need joint space dynamics:

\[\begin{equation}\tau = M(q)\ddot{q} + \dot{q}^TC(q)\dot{q} + g(q)\end{equation}\]

where \(M, C, g\) are the mass matrix, Coriolis/centrifugal force, and gravity on each joint, correspondingly, relying on the current \(q\) states of the system. To bridge this with end effector’s behavior, we also need task space dynamics:

\[\begin{equation}F = A(x)\ddot{x} + \mu(\dot{x}, x) + p(x)\end{equation}\]

where \(x\) is the end effector position, \(A\) is the tracker’s inertial matrix, \(\mu\) is the centrifugal/Coriolis force vector on the tracker, and \(p\) is the gravity force vector. Simplify the expressions and use wide hats for estimation, we rewrite the equation in the form of closed loop control:

\[\begin{align}F&=\widehat{A}F_{in} +\widehat{\mu}_x + \widehat{p}_x\\A^{-1}F &= A^{-1}\widehat{A}F_{in} + A^{-1}((\widehat{\mu}-\mu)+ (\widehat{p}-p))\end{align}\]

Assuming unit mass and perfect estimation, \(A^{-1}F = \mathbb{I}\cdot\ddot{x}\), we arrive at

\[\begin{equation}F_{in} = \ddot{x}_{goal} - k_v(\dot{x}-\dot{x}_{goal})- k_p(x-x_{goal})\end{equation}\]

We can transform the task space dynamics back into joint space by applying inverse dynamics \(J^{T}(q)\), where \(J\) is the Jacobian matrix at the end effector. This dynamics formulation for end effector position \(x\) can be expressed by the aligned joint states \(q\) as follows.

\[\begin{align}\tau &= J^{T}(q)F_x\\A(x)&=J^{-T}(q)M(q)J^{-1}(q)\\\mu(\dot{x},x)&=J^{-T}(q)C(q) - A_x(q)\dot{J}(q)\dot{q}\\p(x)&=J^{-T}(q)g(q)\end{align}\]

We can now plug in the error signals and solve for the limits. In the 2d-plane example above, we are solving for \(v\) in the equation

\[\mathbf{e} = (\frac{v-v\cos\theta}{t_\epsilon} - k_p(v-v\cos\theta)-k_v\rho\cos\theta, \frac{v\sin\theta}{t_\epsilon}-k_pv\sin\theta-k_v\rho\sin\theta)\]

under the dynamics constraint

\[\begin{equation}\mathbf{e}\leq J^{-T}(q)\tau_{\max}\end{equation}\]

However, all the equations above involve Jacobian matrix depending on \(q\). Unfortunately, solving for \(q\) at path points given future task position \(x\) is relatively cumbersome, even when searching from current joint state. It either involves inverse kinematics, or in our case, a simulation forward in operational space. Luckily, we can leverage the fact that the states of a system on a path without singularity are continuous, such that in close proximity, the values are very close. If there is no control singularity while traversing the trajectory, with \(\rho\) relatively small in the work space, we can safely assume similar values between current state and future points within look-ahead distance. Moreover, in practice, the major limiting factor is the constraint \(v\tau < d(1, P)\). The conversion from control space limit to task space is thus reduced to a trivial problem, where the only requirement left is to ensure a singularity-free trajectory. In the robot end effector example, a common practice when operating in task space, is to preemptively check the Jacobian matrix and adjust the target position. Usually a small adjustment within error tolerance is sufficient to avoid the robot going into singularity.

Closed on the close loop method, we can now talk about the open loop method. Recall the velocity profiling method in article IV, where we briefly talked about combining multiple pieces of trajectories together for a full velocity profile. It mentioned a catch to ensure the initial/final velocity and acceleration limits to align with neighboring intervals. The open loop method is to systematically address this alignment by formulating it as an optimization problem under constraint in time domain, which is known as Time-Optimal Path Parameterization (TOPP). The prerequisite is that the trajectory has to be at least \(C^2\) continuous, which is given by our spline-based trajectory generation.

There are multiple approaches to this problem. Given a discretization of the trajectory, there is numerical integration method that searches along the motion feasible cone (ring a bell?), and also optimization methods that solve control inputs under kinematic constraints at discretized points. We introduce a relatively more robust method based on reachability analysis. Let’s say we have a path \(\mathcal{P}\) that is \(C^2\) continuous. First, review the concept of time-parameterization: in our task space setting, with end effector position \(x\in\mathbb{R}^3\), the path is represented as \(\mathbf{x}(u), u\in[0, 1]\), and \(\mathbf{x}(u(t))\) is the time-parameterization of the trajectory. Thus we have \(u, \dot{u}, \ddot{u}\) encoding the dimensionless position, velocity, and acceleration of the tracker. Using the task space dynamics equation from above, the problem is formulated as

\[\begin{equation}A(\bf{x})\ddot{x} + \mu(\dot{x}, x) + p(x) \in\mathcal{R}(x)\end{equation}\]

The only difference from equation (4) is the RHS, where \(\mathcal{R}(\bf{x})\) is the force bound at given task position. It can be mapped back to joint space using Jacobian as shown above. To project this constraint onto the path, take the first and second order derivatives of \(\mathbf{x}(u)\) and substitute the terms. With re-grouping of coefficients we get

\[\begin{align}a(u)\ddot{u} &+ b(u)\dot{u}^2 + c(u) \in r(u)\\a(u) &= A(\mathbf{x}(u))\mathbf{x}'(u)\\b(u)&=A(\mathbf{x}(u))\mathbf{x}''(u)+\mu(\mathbf{x}(u), \mathbf{x}'(u))\\c(u)&=p(\mathbf{x}(u))\\r(u) &= r(\mathbf{x}(u))\end{align}\]

Now, discretize the path by \(N\) intervals and we have \(s_0=0, s_1, ..., s_i, ..., s_N=1\). The bigger \(N\) is, the better profile follows the constraint, since we assume constant acceleration within each interval, and more points would continuously restrain the tracker to abide the force limits. For convenience, assume unit mass, the acceleration \(\ddot{u}\) is directly the control input, and since we know \(v^2 - v_0^2 = 2at\) from classic kinematics, we denote the state of the system as \(\dot{u}_i^2\), so that we have a simple system model as \(\dot{u}_{i+1}^2 = \dot{u}_{i}^2 + 2\delta_i\ddot{u}\). Using Equation 14 from above, we can visualize the control \(\ddot{u}\) and the state \(\dot{u}_i^2\) by plot:

\(\Omega_i\) is the polygon that describes the possible control and state pairs under the dynamics constraint, and the dashed lines show given a state/control value, the possible control/state options are within the interval. To further simplify the problem, set \(\delta_i = 0.5\) so we have \(2\delta_i = 1\). Denote \(\mathbb{K}\) as the set of states, given \(\dot{u}^2_{\min}\) and \(\dot{u}^2_{\max}\in K_{i-1}\), with \(2\delta_i=1\) we can rotate the intervals clockwise by 90 degrees to find their corresponding max and min state boundaries at current step. This draws the boundary of \(\Omega_i(\mathbb{K}_{i-1})\) under dynamics constraint given previous state range.

With the updated boundaries, we can proceed to repeat this process at the constraint polygon at next discretized step \(\Omega_{i+1}\). Now that it forms a recursive process, given path \(\mathcal{P}\) of \(N\) points and the initial/final conditions, velocity \(\dot{u}_0\), \(\dot{u}_{N-1}\), starting from the end of the trajectory, we canfind the set of the states in the previous \(u_{N-2}\) such that at \(u_{N-1}\), enforced velocity \(\dot{u}_{N-1}\) is reachable, under the given acceleration constraint. Repeat until reaching the start; if initial condition \(\dot{u}_0\) is not in the initial set of states, the problem is unsolvable. Otherwise, perform a greedy forward pass to choose the maximum acceleration allowed. This profile we get is the so-called trajectory oracle, which shows the optimal parameterization under dynamics constraints; there is no faster feasible way of tracking the trajectory.

An intuitive way to understand this is to imagine some path points, each specified with a maximum velocity and acceleration/deceleration. One can solve it with convex optimization methods, just as in article II, but when velocity and acceleration constraints on the pre-generated path points that do not match with original spline-based profile, this reachability analysis based method performs better. This is the underlying idea from the paper Time-Optimal Path Parameterization based on Reachability Analysis (TOPPRA), curious readers can find a more detailed and strict formulation there. The example introduced here solves the simplest kinematics constraints, whereas the original method is capable of solving much more complicated dynamics, such as legged robots with conic constraints.

Equipped with methods from the series, we have quite a foundation sufficient to tackle some commonly seen tracking problems. Back to the original question, how can we traverse a path as fast and accurate as possible? Readers probably have a better idea than that at the beginning of this series. In the next and the last article of the series, we will see the real-world applications of these methods, by comparing trajectory tracking between an end-effector of a fully actuated 6-DoF robot arm and a wheeled mobile robot—an autonomous driving car.