#关节动力学 #运动控制 #力控制
详细可参考书本:《现代机器人学：机构规划与控制》第11.5章，其中的动力学方程见第8章相关内容。
其他参考资料：
[【现代机器人学】学习笔记十：机器人控制_zkk9527的博客-CSDN博客]

力控制

关节空间中的动力学方程

机器人的动力学方程可以写为如下等效形式：
τ=M(θ)θ¨+h(θ,θ˙)=M(θ)θ¨+c(θ,θ˙)+g(θ)=M(θ)θ¨+θ˙TΓ(θ)θ˙+g(θ)=M(θ)θ¨+C(θ,θ˙)θ˙+g(θ)\begin{aligned} \tau &=M\left( \theta \right) \ddot{\theta}+h\left( \theta ,\dot{\theta} \right)\\ &=M\left( \theta \right) \ddot{\theta}+c\left( \theta ,\dot{\theta} \right) +g\left( \theta \right)\\ &=M\left( \theta \right) \ddot{\theta}+\dot{\theta}^T\Gamma \left( \theta \right) \dot{\theta}+g\left( \theta \right)\\ &=M\left( \theta \right) \ddot{\theta}+C\left( \theta ,\dot{\theta} \right) \dot{\theta}+g\left( \theta \right)\\ \end{aligned}τ​=M(θ)θ¨+h(θ,θ˙)=M(θ)θ¨+c(θ,θ˙)+g(θ)=M(θ)θ¨+θ˙TΓ(θ)θ˙+g(θ)=M(θ)θ¨+C(θ,θ˙)θ˙+g(θ)​
式中，M(θ)M\left( \theta \right)M(θ)是n×nn\times nn×n的对称正定质量矩阵，h(θ,θ˙)h\left( \theta ,\dot{\theta} \right)h(θ,θ˙)是由于重力和二阶速度项所引起的广义力之和，c(θ,θ˙)c\left( \theta ,\dot{\theta} \right)c(θ,θ˙)是二阶速度力，g(θ)g\left( \theta \right)g(θ)是重力，Γ(θ)\Gamma \left( \theta \right)Γ(θ)为对M(θ)M\left( \theta \right)M(θ)相对于θ\thetaθ求取偏导数而得到的第一类Christoffel符号的n×n×nn\times n\times nn×n×n矩阵，C(θ,θ˙)C\left( \theta ,\dot{\theta} \right)C(θ,θ˙)是n×nn\times nn×n科里奥利矩阵，其第(i,j)(i,j)(i,j)个元素为Cij(θ,θ˙)=∑k=1nΓijk(θ)θ˙kC_{ij}\left( \theta ,\dot{\theta} \right) =\sum_{k=1}^n{\Gamma _{ijk}\left( \theta \right) \dot{\theta}_k}Cij​(θ,θ˙)=∑k=1n​Γijk​(θ)θ˙k​.
如果机器人的末端执行器对环境施加一个力旋量Ftip\mathcal{F} _{tip}Ftip​，则应该在机器人动力学方程的等号右侧添加JT(θ)FtipJ^T(\theta)\mathcal{F} _{tip}JT(θ)Ftip​项。

力控制

令Ftip\mathcal{F} _{tip}Ftip​为机械手对环境所施加的力旋量，则[[#关节空间中的动力学方程]]为：
M(θ)θ¨+c(θ,θ˙)+g(θ)+b(θ˙)+JT(θ)Ftip=τ(11.48)M\left( \theta \right) \ddot{\theta}+c\left( \theta ,\dot{\theta} \right) +g\left( \theta \right) +b\left( \dot{\theta} \right) +J^T\left( \theta \right) \mathcal{F} _{tip}=\tau \tag{11.48}M(θ)θ¨+c(θ,θ˙)+g(θ)+b(θ˙)+JT(θ)Ftip​=τ(11.48)
考虑到机器人在力控制任务期间移动缓慢，可以忽略其加速度和速度项，上式简化为：
g(θ)+JT(θ)Ftip=τ(11.49)g\left( \theta \right) +J^T\left( \theta \right) \mathcal{F} _{tip}=\tau \tag{11.49} g(θ)+JT(θ)Ftip​=τ(11.49)
在没有对机器人末端执行器的力-力矩进行直接测量的情况下，得到单独的关节角度反馈可用于实施前馈控制（[[误差动力学与PID控制#前馈控制]]）：
τ=g~(θ)+JT(θ)Fd\tau =\tilde{g}\left( \theta \right) +J^T\left( \theta \right) \mathcal{F} _d τ=g~​(θ)+JT(θ)Fd​
可见，该控制律需要一个好的重力补偿模型g~(θ)\tilde{g}\left( \theta \right)g~​(θ)，并且需要能够精确控制机器人关节处所产生的力矩。
若进一步在机器人与末端执行器之间配备一个六轴力-力矩传感器，直接测量末端执行器的力旋量Ftip\mathcal{F} _{tip}Ftip​，则可以考虑具有前馈项和重力补偿项的PI力控制器（[[误差动力学与PID控制#前馈加反馈线性化]]）：
τ=g~(θ)+JT(θ)(Fd+KfpFe+Kfi∫Fe(t)dt)(11.51)\tau =\tilde{g}\left( \theta \right) +J^T\left( \theta \right) \left( \mathcal{F} _d+K_{fp}\mathcal{F} _e+K_{fi}\int{\mathcal{F} _e\left( t \right) \mathrm{d}t} \right) \tag{11.51} τ=g~​(θ)+JT(θ)(Fd​+Kfp​Fe​+Kfi​∫Fe​(t)dt)(11.51)
式中，Fe=Fd−Ftip\mathcal{F} _e=\mathcal{F} _d-\mathcal{F} _{tip}Fe​=Fd​−Ftip​，KfpK_{fp}Kfp​和KfiK_{fi}Kfi​分别是比例和积分环节的正定增益矩阵。在完美重力模型的情况下，将上式代入到式（11.49）得到误差动力学方程，即
(Kfp+1)Fe+Kfi∫Fe(t)dt=J(θ)(g(θ)−g~(θ))\left( K_{fp}+1 \right) \mathcal{F} _e+K_{fi}\int{\mathcal{F} _e\left( t \right) \mathrm{d}t}=J\left( \theta \right) \left( g\left( \theta \right) -\tilde{g}\left( \theta \right) \right) (Kfp​+1)Fe​+Kfi​∫Fe​(t)dt=J(θ)(g(θ)−g~​(θ))
若g~(θ)\tilde{g}\left( \theta \right)g~​(θ)的模型不准确，即上式右侧受到非零但恒定的力扰动，对上式求导可得
KfpF˙e+KfiFe=0⇒Fe(t)=e−Kfp/KfitFe(0)\begin{array}{c} K_{fp}\dot{\mathcal{F}}_e+K_{fi}\mathcal{F} _e=0\\ \Rightarrow \mathcal{F} _e\left( t \right) =e^{-{{K_{fp}}\Bigg/{K_{fi}}}\,\,t}\mathcal{F} _e\left( 0 \right)\\ \end{array}Kfp​F˙e​+Kfi​Fe​=0⇒Fe​(t)=e−Kfp​/Kfi​tFe​(0)​
这表明对于正定的KfpK_{fp}Kfp​和KfiK_{fi}Kfi​，Fe\mathcal{F} _eFe​将收敛至零（参见一阶误差动力学的解）。

运动-力混合控制

任务空间中的动力学方程

参见书本第8.6节
考虑一个6-dof开链机器人的[[#关节空间中的动力学方程]]，即
τ=M(θ)θ¨+h(θ,θ˙),θ∈R6,τ∈R6(8.82)\tau =M\left( \theta \right) \ddot{\theta}+h\left( \theta ,\dot{\theta} \right) , \theta \in \mathbb{R} ^6, \tau \in \mathbb{R} ^6 \tag{8.82} τ=M(θ)θ¨+h(θ,θ˙),θ∈R6,τ∈R6(8.82)
忽略末端执行器作用力Ftip\mathcal{F} _{tip}Ftip​。末端执行器的运动旋量V=(ω,v)\mathcal{V} =\left( \omega ,v \right)V=(ω,v)与关节速度θ˙\dot \thetaθ˙通过下式关联：
V=J(θ)θ˙⇒V˙=J˙(θ)θ˙+J(θ)θ¨\mathcal{V} =J\left( \theta \right) \dot{\theta}\Rightarrow \dot{\mathcal{V}}=\dot{J}\left( \theta \right) \dot{\theta}+J\left( \theta \right) \ddot{\theta}V=J(θ)θ˙⇒V˙=J˙(θ)θ˙+J(θ)θ¨
在J(θ)J(\theta)J(θ)可逆的位形θ\thetaθ处，可解得
{θ˙=J−1Vθ¨=J−1V˙−J−1J˙J−1V\begin{cases} \dot{\theta}=J^{-1}\mathcal{V}\\ \ddot{\theta}=J^{-1}\dot{\mathcal{V}}-J^{-1}\dot{J}J^{-1}\mathcal{V}\\ \end{cases}{θ˙=J−1Vθ¨=J−1V˙−J−1J˙J−1V​
将其带入式(8.82)中可得
τ=M(θ)(J−1V˙−J−1J˙J−1V)+h(θ,J−1V)\tau =M\left( \theta \right) \left( J^{-1}\dot{\mathcal{V}}-J^{-1}\dot{J}J^{-1}\mathcal{V} \right) +h\left( \theta ,J^{-1}\mathcal{V} \right) τ=M(θ)(J−1V˙−J−1J˙J−1V)+h(θ,J−1V)
将J−TJ^{-T}J−T表示为(J−1)T=(JT)−1\left( J^{-1} \right) ^T=\left( J^T \right) ^{-1}(J−1)T=(JT)−1，对上式两端同时左乘J−TJ^{-T}J−T，得到
J−Tτ=J−TM(θ)(J−1V˙−J−1J˙J−1V)+h(θ,J−1V)=J−TMJ−1V˙−J−TMJ−1J˙J−1V+J−Th(θ,J−1V)\begin{aligned} J^{-T}\tau &=J^{-T}M\left( \theta \right) \left( J^{-1}\dot{\mathcal{V}}-J^{-1}\dot{J}J^{-1}\mathcal{V} \right) +h\left( \theta ,J^{-1}\mathcal{V} \right)\\ &=J^{-T}MJ^{-1}\dot{\mathcal{V}}-J^{-T}MJ^{-1}\dot{J}J^{-1}\mathcal{V} +J^{-T}h\left( \theta ,J^{-1}\mathcal{V} \right)\\ \end{aligned}J−Tτ​=J−TM(θ)(J−1V˙−J−1J˙J−1V)+h(θ,J−1V)=J−TMJ−1V˙−J−TMJ−1J˙J−1V+J−Th(θ,J−1V)​
将J−TτJ^{-T}\tauJ−Tτ表示为力旋量F\mathcal{F}F，上式即为任务空间中的动力学方程
⇒F=Λ(θ)V˙+η(θ,V)(8.89)\Rightarrow \;\;\mathcal{F} =\Lambda \left( \theta \right) \dot{\mathcal{V}}+\eta \left( \theta ,\mathcal{V} \right) \tag{8.89}⇒F=Λ(θ)V˙+η(θ,V)(8.89)
其中
Λ(θ)=J−TMJ−1η(θ,V)=J−Th(θ,J−1V)−Λ(θ)J˙J−1V\begin{aligned} \Lambda \left( \theta \right) &=J^{-T}MJ^{-1}\\ \eta \left( \theta ,\mathcal{V} \right) &=J^{-T}h\left( \theta ,J^{-1}\mathcal{V} \right) -\Lambda \left( \theta \right) \dot{J}J^{-1}\mathcal{V}\\ \end{aligned}Λ(θ)η(θ,V)​=J−TMJ−1=J−Th(θ,J−1V)−Λ(θ)J˙J−1V​

受约束动力学

详细参见书本第8.7节
考虑nnn关节机器人受到一组kkk个完整或非完整Pfaffian速度约束，
A(θ)θ˙=0,A(θ)∈Rk×nA\left( \theta \right) \dot{\theta}=0,\;\;A\left( \theta \right) \in \mathbb{R} ^{k\times n} A(θ)θ˙=0,A(θ)∈Rk×n
假设约束对机器人没有作用，即与约束条件对应的广义力τcon\tau_{con}τcon​满足
τconTθ˙=0\tau _{con}^{T}\dot{\theta}=0 τconT​θ˙=0
这一假设意味着τcon\tau_{con}τcon​必须是矩阵AT(θ)A^T\left( \theta \right)AT(θ)列向量的线性组合，即τcon=AT(θ)λ\tau_{con}=A^T\left( \theta \right)\lambdaτcon​=AT(θ)λ，其中λ∈Rk\lambda\in \mathbb{R}^kλ∈Rk，因为这些是θ˙\dot{\theta}θ˙被施加在约束条件A(θ)θ˙=0A\left( \theta \right) \dot{\theta}=0A(θ)θ˙=0下不做功的广义力为
∀λ∈Rk,(AT(θ)λ)Tθ˙=λTA(θ)θ˙=0\forall \lambda \in \mathbb{R} ^k,\;\;\left( A^T\left( \theta \right) \lambda \right) ^T\dot{\theta}=\lambda ^TA\left( \theta \right) \dot{\theta}=0 ∀λ∈Rk,(AT(θ)λ)Tθ˙=λTA(θ)θ˙=0
将约束力AT(θ)λA^T\left( \theta \right)\lambdaAT(θ)λ添加到运动方程中，可以用n+kn+kn+k个未知量{θ¨,λ}\{\ddot{\theta},\lambda\}{θ¨,λ}（正向运动学）或n+kn+kn+k个未知量{τ,λ}\{\tau,\lambda\}{τ,λ}（逆向运动学）来写出n+kn+kn+k个约束运动方程，即受约束动力学方程可表示为
{τ=M(θ)θ¨+h(θ,θ˙)+AT(θ)λA(θ)θ˙=0(8.93)\begin{cases} \tau =M\left( \theta \right) \ddot{\theta}+h\left( \theta ,\dot{\theta} \right) +A^T\left( \theta \right) \lambda\\ A\left( \theta \right) \dot{\theta}=0 \tag{8.93}\\ \end{cases} {τ=M(θ)θ¨+h(θ,θ˙)+AT(θ)λA(θ)θ˙=0​(8.93)
其中，λ\lambdaλ是一组拉格朗日乘子，而AT(θ)λA^T\left( \theta \right) \lambdaAT(θ)λ是作用域约束的关节力和力矩。

运动-力混合控制

如果我们对位置闭环，那就无法控制力。
如果我们对力闭环，那就无法控制位置。

考虑任务空间中的kkk个自然约束表达为Pfaffian约束：
A(θ)V=0,V∈R6,A(θ)∈Rk×6(11.55)A\left( \theta \right) \mathcal{V}=0,\;\;\mathcal{V}\in \mathbb{R}^6,\;\;A\left( \theta \right) \in \mathbb{R} ^{k\times 6} \tag{11.55} A(θ)V=0,V∈R6,A(θ)∈Rk×6(11.55)
那么根据[[#任务空间中的动力学方程]]式(8.89)与[[#受约束动力学]]式(8.93)，任务空间中的约束动力学为
F=Λ(θ)V˙+η(θ,V)+AT(θ)λ⏟Ftip(11.56)\mathcal{F} =\Lambda \left( \theta \right) \dot{\mathcal{V}}+\eta \left( \theta ,\mathcal{V} \right) +\underset{\mathcal{F} _{tip}}{\underbrace{A^T\left( \theta \right) \lambda }} \tag{11.56} F=Λ(θ)V˙+η(θ,V)+Ftip​AT(θ)λ​​(11.56)
式中，λ∈Rk\lambda\in\mathbb{R}^kλ∈Rk是拉格朗日乘数，Ftip\mathcal{F} _{tip}Ftip​是机器人施加到约束上的力旋量。期望的力旋量Fd\mathcal{F}_dFd​必须位于AT(θ)A^T(\theta)AT(θ)的列向量空间中。
在任意时刻都需要满足式(11.55)，采用时间导数代替：
A(θ)V˙+A˙(θ)V=0(11.57)A\left( \theta \right) \dot{\mathcal{V}}+\dot{A}\left( \theta \right)\mathcal{V}=0 \tag{11.57} A(θ)V˙+A˙(θ)V=0(11.57)
为了确保系统状态在满足式（11.55）时能够同时满足上式，任何期望加速度V˙d\dot{\mathcal{V}}_dV˙d​都应该满足A(θ)V˙d=0A\left( \theta \right) \dot{\mathcal{V}}_d=0A(θ)V˙d​=0。
根据式(11.56)可得V˙=Λ−1(F−η−ATλ)\dot{\mathcal{V}}=\Lambda ^{-1}\left( \mathcal{F} -\eta -A^T\lambda \right)V˙=Λ−1(F−η−ATλ)，将其带入式(11.57)可得
AΛ−1(F−η−ATλ)+A˙V=0⇒AΛ−1(F−η)+A˙V=AΛ−1ATλ⇒λ=(AΛ−1AT)−1AΛ−1(F−η)+A˙V=(AΛ−1AT)−1AΛ−1−AV˙(11.58)\begin{array}{l} A\Lambda ^{-1}\left( \mathcal{F} -\eta -A^T\lambda \right) +\dot{A}\mathcal{V} =0\\ \Rightarrow A\Lambda ^{-1}\left( \mathcal{F} -\eta \right) +\dot{A}\mathcal{V} =A\Lambda ^{-1}A^T\lambda\\ \Rightarrow \lambda =\left( A\Lambda ^{-1}A^T \right) ^{-1}A\Lambda ^{-1}\left( \mathcal{F} -\eta \right) +\dot{A}\mathcal{V}\\ \,\,\;\;\;\;\;\; =\left( A\Lambda ^{-1}A^T \right) ^{-1}A\Lambda ^{-1}-A\dot{\mathcal{V}}\\ \tag{11.58} \end{array} AΛ−1(F−η−ATλ)+A˙V=0⇒AΛ−1(F−η)+A˙V=AΛ−1ATλ⇒λ=(AΛ−1AT)−1AΛ−1(F−η)+A˙V=(AΛ−1AT)−1AΛ−1−AV˙​(11.58)
进一步利用式(11.58)可以求得机器人对约束施加的力旋量Ftip=AT(θ)λ\mathcal{F_{tip}}=A^T(\theta)\lambdaFtip​=AT(θ)λ。
将式(11.58)带入到式(11.56)并进行操作，受约束动力学(11.56)中的nnn个方程可表示为n−kn-kn−k个独立的运动方程（可参考书本8.7节）：
P(θ)F=P(θ)(ΛV˙+η)P\left( \theta \right) \mathcal{F} =P\left( \theta \right) \left( \Lambda \dot{\mathcal{V}}+\eta \right) P(θ)F=P(θ)(ΛV˙+η)
式中，P(θ)=I−AT(AΛ−1AT)−1AΛ−1P\left( \theta \right) =I-A^T\left( A\Lambda ^{-1}A^T \right) ^{-1}A\Lambda ^{-1}P(θ)=I−AT(AΛ−1AT)−1AΛ−1.
n×nn\times nn×n矩阵P(θ)P(\theta)P(θ)的秩为n−kn-kn−k，它将任意的一个机械手力旋量F\mathcal{F}F投影到力旋量的子空间，该子空间可使末端执行器沿与约束相切的方向移动。秩为kkk的矩阵I−P(θ)I-P(\theta)I−P(θ)将任意的一个力旋量F\mathcal{F}F投影到与约束相对作用的力旋量子空间上。因此，P(θ)P(\theta)P(θ)将nnn维力空间划分为两部分：用于解决运动控制任务的力旋量；以及用于解决力控制任务的力旋量。即力旋量F\mathcal{F}F可以划分为：
F=P(θ)F+(I−P(θ))F\mathcal{F}=P(\theta)\mathcal{F}+(I-P(\theta))\mathcal{F} F=P(θ)F+(I−P(θ))F
因此，运动-力混合控制器通过P(θ)P(\theta)P(θ)和(I−P(θ))(I-P(\theta))(I−P(θ))的投影，可以看作是任务空间运动控制器（[[任务空间中的运动控制#任务空间中的运动控制]]）和任务空间力控制器（[[#力控制]]）之和，每个控制器都被投影到相应的子空间中以产生力。 假设在末端执行器坐标系{b}\{b\}{b}中表达力旋量和运动旋量，则运动-力混合控制器可表达为：
τ=JbT(θ)F=JbT(θ)[P(θ)F+(I−P(θ))F]=JbT(θ){P(θ)[Λ~(θ)(ddt([AdX−1Xd]Vd)+KpXe+Ki∫Xe(t)dt+KdVe)]+(I−P(θ))(Fd+KfpFe+Kfi∫Fe(t)dt)+η~(θ,Vb)}\begin{aligned} \tau &=J_{b}^{T}\left( \theta \right) \mathcal{F}\\ &=J_{b}^{T}\left( \theta \right) \left[ P\left( \theta \right) \mathcal{F} +\left( I-P\left( \theta \right) \right) \mathcal{F} \right]\\ &=J_{b}^{T}\left( \theta \right) \left\{ \begin{aligned} P\left( \theta \right) \left[ \widetilde{\Lambda }\left( \theta \right) \left( \frac{\mathrm{d}}{\mathrm{d}t}\left( \left[ Ad_{X^{-1}X_d} \right] \mathcal{V} _d \right) +K_pX_e+K_i\int{X_e\left( t \right) \mathrm{d}t+K_d}\mathcal{V} _e \right) \right]\\ +\left( I-P\left( \theta \right) \right) \left( \mathcal{F} _d+K_{fp}\mathcal{F} _e+K_{fi}\int{\mathcal{F} _e\left( t \right) \mathrm{d}t} \right)\\ +\tilde{\eta}\left( \theta ,\mathcal{V} _b \right)\\ \end{aligned} \right\}\\ \end{aligned} τ​=JbT​(θ)F=JbT​(θ)[P(θ)F+(I−P(θ))F]=JbT​(θ)⎩⎨⎧​P(θ)[Λ(θ)(dtd​([AdX−1Xd​​]Vd​)+Kp​Xe​+Ki​∫Xe​(t)dt+Kd​Ve​)]+(I−P(θ))(Fd​+Kfp​Fe​+Kfi​∫Fe​(t)dt)+η~​(θ,Vb​)​⎭⎬⎫​​
上式中，两个控制器的动力学通过PPP与I−PI-PI−P这两个正交投影而相互解耦，所以控制器继承了独立的力和运动控制器在各自子空间上的误差动态和稳定性分析。
然而，上述力-运动混合控制很难在刚性环境中实施，因为无法确定在任何时间都有效的A(θ)V=0A(\theta)\mathcal{V}=0A(θ)V=0，然而这对于指定期望运动和期望力、计算投影都是必要的，但关于环境的任何模型都会存在一些不确定性。解决该困难的一种方法是：

1. 基于力反馈，使用实时估计算法来识别约束方向；
2. 另一种方法是通过选择低反馈增益来牺牲一些性能，进而使运动控制器变“软”，同时力控制器更能容忍力的误差。