本文仅供学习使用
本文参考：
B站：CLEAR_LAB
笔者带更新-运动学
课程主讲教师：
Prof. Wei Zhang
课程链接 ：
https://www.wzhanglab.site/teaching/mee-5114-advanced-control-for-robotics/

---

机器人——运动能力、计算能力、感知决策能力 的机电系统

1. Basic Linear Control Design

1.1 Error Response

Steady-state error : $e_{\mathrm{ss}}=\underset{t\to \infty }{\lim }{\theta }_{\mathrm{e}}\left(t\right)$

Precent overshoot : P.O.

Rise time / Peak time :

Settling time : $T_{\mathrm{s}}$

1.2 Standard Second-Order Systems

详细推导见 ： （待补充）

1.3 Second-Order Response Characteristics

详细推导见 ： （待补充）

1.4 State-Space Controller Design

• Eigenvalue assignment : Find control gain $K$ such that $eig\left(A-BK\right)=ei{g}_{\mathrm{desired}}$
• Solvability : We can always find such $K$ if $\left(A,B\right)$ is controllable ($rank\left(m_{\mathrm{c}}\right)=n$)
• How to choose desired eigs? —— refer to 2nd-order system
  specification (P.O. $T_{\mathrm{s}}$ $T_{\mathrm{p}}$) $\stackrel{art}{\Rightarrow }$ dominant poles + other poles $\Rightarrow$ $ei{g}_{\mathrm{desired}}$$\stackrel{science}{\Rightarrow }$ $K$

2. Motion Control Problems

2.1 Robotic Motion Control Problem

Dynamic equation of fully-acuated robot (with external force) : $\{\begin{array}{l}\tau =M\left(q\right)\ddot{q}+c\left(q,\dot{q}\right)\dot{q}+g\left(q\right)+J^{\mathrm{T}}\left(q\right){\mathcal{F}}_{\mathrm{ext}}\\ y=h\left(q\right)\end{array}$
$q\in {\mathbb{R}}^n$ : joint positions (generalized coordinate)
$\tau \in {\mathbb{R}}^n$ : joint torque (generalized input)
$y$ : output (variable to be controlled) —— can be any func of $q$ , e.g. $y=q,y=\left[T\left(q\right)\right]\in SE\left(3\right)$

• Motion Control Problems : Let $y$ track given reference $y_{\mathrm{d}}$
  
  often times $q_{\mathrm{d}}$ is given by planner represented by polynomials , so that ${\dot{q}}_{\mathrm{d}},{\ddot{q}}_{\mathrm{d}}$ can be easily obtained

2.2 Variations in Robot Motion Control

• Joint-space vs. Task-space control
  Joint-space : $y\left(t\right)=q\left(t\right)$ , i.e. , want $q\left(t\right)$ to track a given $q_{\mathrm{d}}\left(t\right)$ joint reference
  Task-space : $y\left(t\right)=\left[T\left(q\left(t\right)\right)\right]\in SE\left(3\right)$ denotes end-effector pose/configuration, we want $y\left(t\right)$ to track $y_{\mathrm{d}}\left(t\right)$

• Actuation models:
  Velocity source : $u=\dot{q}$ —— directly control velocity
  Acceleration sources : $u=\ddot{q}$ —— directly control acceleration
  Torque sources : $u=\tau$ —— directly control torque
  
  Acutation model make sense if for ant given $u$ , the joint velocity $\dot{q}$ can immediatly reach $u$

Motion Control Problem
Design $u$ to set $y$ track desired reference $y_{\mathrm{d}}$

• Depending on our assumption on $u/y$
  output $y$ —— 6大基本问题
  $y\leftrightarrow q\in {\mathbb{R}}^n$ - joint variable : Joint space motion control (Velocity-resolved Joint-space control ; Acceleration-resolved Joint-space control ; Torque-resolved Joint-space control ; )
  $y\leftrightarrow \left[T\left(q\right)\right]\in SE\left(3\right)$ or $y=f\left(q\right)$ - task space variable - e.g. origin of end-effector frame : Task space motion control (Velocity-resolved Task-space ; Acceleration-resolved Task-space ; Torque-resolved Task-space ; )

Linear control / feedback lineariazation

3. Motion Control with Velocity/Acceleration as Input

3.1 Velocity-Resolved Control

Each joints’ velocity ${\dot{q}}_{\mathrm{i}}$ can be directly controlled

Good approximation for hydraulic actuators

Common approxiamtion of the outer-loop control for the Inner / outer loop control setup

3.2.1 Velocity-Resolved Joint Space Control

Joint-space ‘dynamics’ : single integrator $\dot{q}=u$

Joint-space tracking becomes standard linear tracking control problem : $u={\dot{q}}_{\mathrm{d}}+K_0\ddot{q}\Rightarrow \dot{\tilde{q}}+K_0\ddot{q}=0$ , where $\tilde{q}=q_{\mathrm{d}}-q$ is the joint position error. —— stable if $eig\left(-K_0\right)\in OLHP$

The error dynamic is stable if $-K_0$ is Hurwitz

3.2.2 Velocity-Resolved Task Space Control

For task space control , $y=\left[T\left(q\right)\right]$ needs to track $y_{\mathrm{d}}$ , $y$ can be ant function of $q$, in particular , it can represents position and/or the end-effector frame

Taking derivatives of $y$ , and letting $u=\dot{q}$ , we have : $\dot{y}=J_{\mathrm{a}}\left(q\right)u$
Note that $q$ is function of $y$ through inverse kinematics ($q=IK\left(y\right)$)
So the above dynamics can be written in terms of $y$ and $u$ only. The detailed form can be quite complex in general $\dot{y}=J_{\mathrm{a}}\left(IK\left(y\right)\right)u$

1. Let $v_{\mathrm{y}}$ be virtual control $\dot{y}=v_{\mathrm{y}}$ design $v_{\mathrm{y}}$ to track $y_{\mathrm{d}}$ (same as above)
2. Find actual control $u$ such that $J_{\mathrm{a}}\left(IK\left(y\right)\right)u\approx v_{\mathrm{y}}$

We can design outer-loop controller as if we can directly control $\dot{y}$
$$\dot{y}=v_{\mathrm{y}}={\dot{y}}_{\mathrm{d}}+K\left(y_{\mathrm{d}}-y\right)\stackrel{plugin\dot{y}=v_{\mathrm{y}}}{⟹}\dot{\tilde{y}}=-K\tilde{y}$$
We can select $K$ such that $-K$ is Hurtwiz , object of inner loop : determine $u=\dot{q}$ such that $\dot{y}\approx v_{\mathrm{y}}$

System(2) is nonlinear system , a commeon way is to break it into inner-outer loop , where the outer loop directly control velocity of $y$, and the inner loop tries to find $u$ to generate desired task space velocity

Outer loop : $\dot{y}=v_{\mathrm{y}}$ , where control $v_{\mathrm{y}}={\dot{y}}_{\mathrm{d}}+K_0\tilde{y}$ , resulting in task-space closed-loop error dynamics: $\dot{\tilde{y}}+K_0\tilde{y}=0$

Above task space tracking relies on a fictitious control $v_{\mathrm{y}}$ , i.e. , it assumes $\dot{y}$ can be arbitrarily controlled by selecting appropriate $u=\dot{q}$ , which is true if $J_{\mathrm{a}}$ is full-row rank

Inner loop : Given $v_{\mathrm{y}}$ from the outer loop, find the joint velocity control by solving
$$\{\begin{array}{l}{\min }_{\mathrm{u}}{\Vert v_{\mathrm{y}}-J_{\mathrm{a}}\left(q\right)u\Vert }^2+regularizationterm\\ subj.to:Constraintsonu,e.g.\{\begin{array}{l}{\dot{q}}_{\min }⩽u⩽{\dot{q}}_{\max }\\ q_{\min }⩽q+u\Delta t⩽q_{\max }\end{array}\end{array}$$
Inner-loop is essentially a differential IK controller
One can also use the pseudo-inverse control $u={J_{\mathrm{a}}}^{†}{v}_{\mathrm{y}}$

3.2 Acceleration-Resolved Control

3.2.1 Acceleration-Resolved Control in Joint Space

Joint acceleration cna be directly controlled , resulting in double-integrator dynamics $\ddot{q}=u$ . Given $q_{\mathrm{d}}$ reference , we want $q\to q_{\mathrm{d}}$ (double integartor)

Joint-space tracking becomes standard linear tracking control problem for double-integrator system:
$$u={\ddot{q}}_{\mathrm{d}}+K_1\dot{\tilde{q}}+K_0\tilde{q}=0,\tilde{q}\in {\mathbb{R}}^n$$
—— PD control , closed-loop system , where $\tilde{q}=q_{\mathrm{d}}-q$ is the joint position error.

Stablility condition : Let $x=\left[\begin{array}{c}\tilde{q}\\ \dot{\tilde{q}}\end{array}\right]\in {\mathbb{R}}^{2n}$ , $\left[\begin{array}{cc}0& E\\ -K_0& -K_1\end{array}\right]\left[\begin{array}{c}\tilde{q}\\ \dot{\tilde{q}}\end{array}\right],\dot{x}=Ax$
closed-loop system is stable . if $eig\left(A\right)\in OLHP$ or $A$ is Hurwitz

3.2.2 Acceleration-Resolved Control in Task Space

For task space control , $y=\left[T\left(q\right)\right]\in SE\left(3\right)$ needs to track $y_{\mathrm{d}}$

Note : For $y=f\left(q\right)$ $\dot{y}=J_{\mathrm{a}}\left(q\right)\dot{q}$ and $\ddot{y}={\dot{J}}_{\mathrm{a}}\left(q\right)\dot{q}+J_{\mathrm{a}}\left(q\right)\ddot{q}\Rightarrow \ddot{y}={\dot{J}}_{\mathrm{a}}\left(q\right)\dot{q}+J_{\mathrm{a}}\left(q\right)u\Leftarrow$ nonlinear dynamics

Following the same inner-outer loop strategy deiscussed before . Introduce virtual control , $a_{\mathrm{y}}$ such that $\ddot{y}=a_{\mathrm{y}}$ , we can design controller for $a_{\mathrm{y}}$ to let $y\to y_{\mathrm{d}}$

Outer-loop dynamics : $\ddot{y}=a_{\mathrm{y}}$ , with $a_{\mathrm{y}}$ being the outer-loop control input $a_{\mathrm{y}}={\ddot{y}}_{\mathrm{d}}+K_1\dot{\tilde{y}}+K_0\tilde{y}\Rightarrow \ddot{\tilde{y}}+K_1\dot{\tilde{y}}+K_0\tilde{y}=0$

—— PD control , stable if $\left[\begin{array}{cc}0& E\\ -K_0& -K_1\end{array}\right]$ Hurwitz

Inner-loop : given $a_{\mathrm{y}}$ from outer loop , find the “best” joint acceleration:
$\{\begin{array}{l}{\min }_{\mathrm{u}}{\Vert a_{\mathrm{y}}-{\dot{J}}_{\mathrm{a}}\left(q\right)\dot{q}-J_{\mathrm{a}}\left(q\right)u\Vert }^2+regularizationterm\\ subj.to:Constraintsonu\end{array}$
—— $u$ : optimization variable , ${\dot{J}}_{\mathrm{a}}\left(q\right),\dot{q},q$ - known
$\{\begin{array}{l}Acc:{\ddot{q}}_{\min }⩽u⩽{\ddot{q}}_{\max }\\ Vel:{\dot{q}}_{\min }⩽q+u\Delta t⩽{\dot{q}}_{\max }\end{array}$

Mathematically , the above problem is the same as the Differential IK problem

At any given time , $\dot{q},q$ can be measured , and then $y,\dot{y}$ can be computed, which allows us to compute outer loop control $a_{\mathrm{y}}$ and inner loop control $u$

4. Motion Control with Torque as Input and Task Space Inverse Dynamics

4.1 Recall Properties of Robot Dynamics

For fully actuated robot :
$$\tau =M\left(q\right)\ddot{q}+C\left(q,\dot{q}\right)\dot{q}+g\left(q\right)$$
$M\left(q\right)=\sum {J_{\mathrm{i}}}^{\mathrm{T}}{\left[{\mathcal{I}}_{\mathrm{i}}\right]}_{6\times 6}{J}_{\mathrm{i}}\in {\mathbb{R}}^{n\times n}$

There are many valid difinitions of $C\left(q,\dot{q}\right)$ , typical choice for $C$ include:
$C_{\mathrm{ij}}={\sum }_k^{}\frac{1}{2}\left(\frac{\partial M_{\mathrm{ij}}}{\partial q_{\mathrm{k}}}+\frac{\partial M_{\mathrm{ik}}}{\partial q_{\mathrm{j}}}-\frac{\partial M_{\mathrm{jk}}}{\partial q_{\mathrm{i}}}\right)$
For the above defined $C$ , we have $\dot{M}-2C$ is skew symmetric
For all valid $C$, we have ${\dot{q}}^{\mathrm{T}}\left[\dot{M}-2C\right]\dot{q}=0$
These properties play improtant role in designing motion controller

4.2 Computed Torque Control

For fully-actuated robot, we have $M\left(q\right)\succ 0$ and $\ddot{q}$ can be arbitrarily specified through torque control $u=\tau$
$$\ddot{q}=M^{-1}\left(q\right)\left[u-C\left(q,\dot{q}\right)\dot{q}-g\left(q\right)\right]$$

we know how to design controller if $u=\ddot{q}$

Thus , for fully-acuated robot, torque controlled case can be reduced to the acceleration-resolved case

Outer loop: $\ddot{q}=a_{\mathrm{q}}$ with joint acceleration as control input
$$a_{\mathrm{q}}=\ddot{q}+K_1\dot{\tilde{y}}+K_0\tilde{y}\Rightarrow \ddot{\tilde{q}}+K_1\dot{\tilde{q}}+K_0\tilde{q}=0$$

Inner loop : since $M\left(q\right)$ is square and nonsingular , inner loop control $u$ can be found analytically:
$$u=M\left(q\right)\left({\ddot{q}}_{\mathrm{d}}+K_1\dot{\tilde{q}}+K_0\tilde{q}\right)+C\left(q,\dot{q}\right)\dot{q}+g\left(q\right)$$

The control law is a function of $q,\dot{q}$ and the reference $q_{\mathrm{d}}$. It is called computed-torque control.

The control law also relies on system model $M,C,g$ if these model information are not accurate, the control will not perform well.
$y=f\left(q\right),\ddot{y}={\dot{J}}_{\mathrm{a}}\left(q\right)\dot{q}+J_{\mathrm{a}}\left(q\right)M^{-1}\left(u-C-g\right)$
Idea easily extends to task space : $\dot{y}=J_{\mathrm{a}}\left(q\right)\dot{q}$ and $\ddot{y}={\dot{J}}_{\mathrm{a}}\left(q\right)\dot{q}+J_{\mathrm{a}}\left(q\right)\ddot{q}$ —— $\tau =u=\tau ,\ddot{q}=M^{-1}\left[u-C-g\right]$

Outer loop : $\ddot{y}=a_{\mathrm{y}}$ and $a_{\mathrm{y}}={\ddot{y}}_{\mathrm{d}}+K_1\dot{\tilde{y}}+K_0\tilde{y}$

Inner loop : sekect torque control $u=\tau$ by
$\{\begin{array}{l}{\min }_{\mathrm{u}}{\Vert a_{\mathrm{y}}-{\dot{J}}_{\mathrm{a}}\left(q\right)\dot{q}-J_{\mathrm{a}}\left(q\right)M^{-1}\left(u-C\dot{q}-g\right)\Vert }^2\\ subj.to:Constraints\end{array}$
If $J_{\mathrm{a}}$is invertible and we don’t impose additional torque constraints, analytical control law can be easily obtained —— $u={\left(J_{\mathrm{a}}\left(q\right)M^{-1}\right)}^{-1}\left(a_{\mathrm{y}}-{\dot{J}}_{\mathrm{a}}\left(q\right)\dot{q}...\right)$

4.3 Inverse Dynamics Control

The computed-torque controller above is also canned inverse dynamics control

Forward dynamics : given $\tau$ to compute $\ddot{q}$ —— from torque to motion

Inverse dynamics : given desired acceleration $a_{\mathrm{q}}$, we inverted it to find the required control by $u=M{a}_{\mathrm{q}}+C\dot{q}+g$

Task space case can be viewed as inverting the task space dynamics —— Given $a_{\mathrm{y}}$ ($y$ task space) , find $\tau$ such that $\ddot{y}=a_{\mathrm{y}}$

With recent advances in optimization , it is often preferred to do ID with quedratic program

For example, above equation can be viewed as task-space ID. We can incorporate torque contraints explicitly as follows:
$\{\begin{array}{l}{\min }_{\mathrm{u}}{\Vert a_{\mathrm{y}}-{\dot{J}}_{\mathrm{a}}\left(q\right)\dot{q}-J_{\mathrm{a}}{M}^{-1}\left(u-C\dot{q}-g\right)\Vert }^2\\ subj.to:u_{-}⩽u⩽u_{+}\end{array}$
optimization variable $u\in {\mathbb{R}}^n$

This is equivalent to the following more popular form:
$\{\begin{array}{l}\underset{u,\ddot{q}}{\min }{\Vert a_{\mathrm{y}}-{\dot{J}}_{\mathrm{a}}\dot{q}-J_{\mathrm{a}}\ddot{q}\Vert }^2\\ subj.to:\begin{array}{c}M\ddot{q}+C\dot{q}+g=u\\ u_{-}⩽u\in {\mathbb{R}}^n⩽u_{+}\end{array}\end{array}$
optimization variable $u,\ddot{q}\in {\mathbb{R}}^n$