We often separate the identification procedure into two parts, were we - first estimate the state $\hat{\bx}_n$ given the input-output data $\bu_n, - \by_n$, and then focus on estimating the state-evolution dynamics in a - second step. The dynamics estimation algorithms fall into two main +
We often separate the identification procedure into two parts, in which + we first estimate the state $\hat{\bx}_n$ given the input-output data + $\bu_n, \by_n$, and then focus on estimating the state-evolution dynamics + in a second step. The dynamics estimation algorithms fall into two main categories:
Now let's thinking about estimating the dynamic parameters of +
Now let's thinking about estimating the inertial parameters of multibody system. We've been writing the manipulation equations in the form: \begin{equation}\bM({\bq})\ddot{\bq} + \bC(\bq,\dot{\bq})\dot\bq = \btau_g(\bq) + \bB\bu + \text{friction, etc.}\end{equation} Each of the terms in this equation can depend on the parameters $\balpha$ that we're trying to estimate. But the parameters enter the multibody equations in - a particular structured way: the equations are - affine in the lumped parameters. More precisely, the - manipulator equations above can be factored into the form $${\bf - W}(\bq,\dot{\bq}, \ddot{\bq}, \bu) \balpha_l(\balpha) + - \bw_0(\bq,\dot{\bq}, \ddot{\bq}, \bu) = 0,$$ where $\balpha_l$ are the - "lumped parameters". We sometimes refer to ${\bf W}$ as the "data matrix".
+ a particular structured way: the inverse dynamics equations are + affine in the inertial parametersTo proceed with the inverse dynamics formulation, let us first choose
+ a parameterization for the inertias.
Remarkably, the inverse dynamics are affine in these parameters. This
+ means that the manipulator equations above can be refactored into the
+ form \begin{equation}{\bf W}(\bq_n,\dot{\bq}_n, \ddot{\bq}_n, \bu_n)
+ \balpha_B + \bw_0(\bq_n,\dot{\bq}_n, \ddot{\bq}_n, \bu_n) = 0.
+ \label{eq:least_squares}\end{equation} We sometimes refer to the ${\bf
+ W}$ which is assembled by vertically stacking these equations evaluated
+ at all time samples, $n$, as the "data matrix". Given data trajectories
+ of positions, $\bq_n,$ and torque measurements, $\bu_n,$ we can estimate
+ $\dot{\bq}_n$ and $\ddot{\bq}_n$ with (acausal) filtering. Then we can
+ estimate the parameters using least-squares estimation. As such, we can
+ leverage all of the strong results and variants from linear estimation.
+ For instance, we can add terms to regularize the estimate (e.g. to stay
+ close to an initial guess), and we can write efficient recursive
+ estimators for optimal online estimation of the parameters using
+ recursive least-squares. My favorite recursive least-squares algorithm
+ uses incremental QR factorization
Not all values of $\alpha_B$ describe physically-valid inertias. For
+ instance, we know that $m_B$ has to be positive. There are constraints on
+ the inertia matrix itself.
Do these additional constraints matter? In the limit of infinite + data, then one might hope that the least-squares estimator would obtain + the correct values without the constraints. But their are two foils to + this ideal. First, we often wish to operate in a relatively low-data + regime. Second, it is important to understand that not all of the + inertial parameters will be identifiable. For instance, if you + imagine a robotic arm bolted to a table where the first link is a + rotation about the $z$ axis. Then no amount of data obtained from this + robot will given any information about the moments of inertia of the + first link about the $x$ and $y$ axes. Naively including these parameters + in the estimation problem could not only lead to poor estimates of the + unidentifiable parameters, but can even render the physical-validity + constraints on the inertial parameters vacuous.
+ +There are two general approaches to dealing with the unidentifiable
+ parameters. First, we can exclude them from the optimization. There are a
+ number of algorithms for symbolically extracting the identifiable
+ parameters, but it is more common (and likely just as effective) to
+ simply evaluate the null-space of a sufficiently rich data matrix, $W$,
+ to find the unidentifiable parameters by using, e.g., $\bR_1\alpha_l$
+ from the QR factorization: $${\bf W} = \begin{bmatrix} \bQ_1 & \bQ_2
+ \end{bmatrix} \begin{bmatrix} \bR_1 \\ 0 \end{bmatrix}, \quad \Rightarrow
+ \quad {\bf W}\balpha_B = \bQ_1 (\bR_1 \balpha_B) = \bQ_1
+ \bar{\balpha}_{B},$$ where $\bar{\balpha}_B$ are the identifiable
+ parameters. To implement the physical-validity constraints, we would need
+ to use estimates of the unidentifiable parameters. Alternatively, one
+ can formulate the optimization directly on the full parameters and add a
+ regularization term which (ideally) operates in the nullspace of the
+ primary objective to drive the unidentifiable parameters towards a prior
+ estimate.
The least-squares inverse-dynamics formulation also allows one to + simultaneously fit a number of other relevant dynamic parameters which + enter linearly into the equations. These include reflected inertial (as a + diagonal matrix times $\ddot{\bq}$), joint friction (as a constant torque + multiplied by the sign of $\dot{\bq}$), and damping (as a linear multiple + of $\dot{\bq}$).
+ +Allow me to mention one subtle point about numerical scaling. All of
+ the equations in the least-squares inverse-dynamics formulation
+ (\ref{eq:least_squares}) have units of torque. But the torques used on
+ the base link of a robotic arm might be numerically much larger than the
+ torques corresponding to the more distal joints.
Although it is highly recommended to separate kinematic an inertial + estimation, some amount of joint estimation is possible when needed. We + know that the inverse dynamics are affine in the inertial parameters. But + it turns out the can also be written as affine in the "lumped + parameters": nonlinear combinations of inertial and kinematic which do + not depend on the data. I feel this is best explained with a few simple + examples.
This is super interesting! It is tempting to thing about parameter - estimation for general dynamical systems in our standard state-space - form: $\bx[n+1] = f_\balpha(\bx[n], \bu[n]).$ But for multibody systems, - it seems that this would be the wrong thing to do, as it destroys this - beautiful affine structure.
-Very few robotics simulators have any way for you to access the @@ -383,29 +516,12 @@
Importantly, because we have reduced this to a least-squares problem, - we can also understand when it will not work. In particular, it - is quite possible that some parameters cannot be estimated from any - amount of joint data taken on the robot. As a simple example, consider a - robotic arm bolted to a table; the inertial parameters of the first link - of the robot will not be identifiable from any amount of joint data. Even - on the second link, only the inertia relative to the first joint axis - will be identifiable; the inertial parameters corresponding to the other - dimensions will not. In our least-squares formulation, this is quite - easy to understand: we simply check the (column) rank of the data matrix, - ${\bf W}$. In particular, we can extract the identifiable lumped - parameters by using, e.g., $\bR_1\alpha_l$ from the QR factorization: - $${\bf W} = \begin{bmatrix} \bQ_1 & \bQ_2 \end{bmatrix} \begin{bmatrix} - \bR_1 \\ 0 \end{bmatrix}, \quad \Rightarrow \quad {\bf W}\balpha_l = - \bQ_1 (\bR_1 \alpha_l).$$
+ taken in the torque coordinates, can again be solved using least squares. + Importantly, because we have reduced this to a least-squares problem, we + can also understand when it will not work. Once again, not all of + the lumped-parameters are identifiable, so we will typically need a + second step to operate on the identifiable lumped parameters or to add a + regularization term.For practical reasons, it might be convenient to take your estimates - of the lumped parameters, and try to back out the original parameters - (for instance, if you want to write them back out into a URDF file). For - this, I would recommend a final post-processing step that e.g. finds the - parameters $\hat{\balpha}$ that are as close as possible (e.g. in the - least-squares sense) to your original guess for the parameters, subject - to the nonlinear constraint that $\bR_1 \balpha_l(\hat{\balpha})$ matches - the estimated identifiable lumped parameters. -
- -There are still a few subtleties worth considering, such as how we
- parameterize the inertial matrices. Direct estimation of the naive
- parameterization, the six entries of a symmetric 3x3 matrix, can lead to
- non-physical inertial matrices.