四足机器人传统控制方法学习笔记

运动学与动力学

单腿运动学

正运动学

目标：已知三个关节的角度 (𝜃1,𝜃2,𝜃3) ，需要求出足端位置 (𝑥,𝑦,𝑧)

𝑇01=[rot𝑥(𝜃1)03×103×31]=[10000cos⁡(𝜃1)−sin⁡(𝜃1)00sin⁡(𝜃1)cos⁡(𝜃1)00001]𝑇12=[rot𝑦(𝜃2)03×103×31]=[cos⁡(𝜃2)0sin⁡(𝜃2)00100−sin⁡(𝜃2)0cos⁡(𝜃2)00001]𝑇23=[rot𝑦(𝜃3)[0𝑙1𝑙2]03×31]=[cos⁡(𝜃3)0sin⁡(𝜃3)0010𝑙1−sin⁡(𝜃3)0cos⁡(𝜃3)𝑙20001]𝑙1={−𝑙𝑎𝑏𝑎𝑑,right𝑙𝑎𝑏𝑎𝑑,left𝑙2=−𝑙ℎ𝑖𝑝𝑝3=[00𝑙31]𝑙3=−𝑙𝑘𝑛𝑒𝑒

根据以上变换即可求出足端位置：

(1)[𝑥𝑝𝑦𝑝𝑧𝑝1]=[𝑙3sin⁡(𝜃1+𝜃2)+𝑙2sin⁡(𝜃2)−𝑙3sin⁡(𝜃1)cos⁡(𝜃2+𝜃3)+𝑙1cos⁡(𝜃1)−𝑙2cos⁡(𝜃2)sin⁡(𝜃1)𝑙3cos⁡(𝜃1)cos⁡(𝜃2+𝜃3)+𝑙1sin⁡(𝜃1)+𝑙2cos⁡(𝜃1)cos⁡(𝜃2)1]

逆运动学

目标：已知足端位置 (𝑥,𝑦,𝑧) ，需要求出三个关节的角度 (𝜃1,𝜃2,𝜃3)

求 𝜃1

𝑃0=𝑅01𝑃1[𝑦𝑝𝑧𝑝]=[cos⁡(𝜃1)−sin⁡(𝜃1)sin⁡(𝜃1)cos⁡(𝜃1)][𝐿−𝐿]
整理得
{𝑦𝑝=𝑙1cos⁡(𝜃1)+𝐿sin⁡(𝜃1)𝑧𝑝=𝑙1sin⁡(𝜃1)−𝐿cos⁡(𝜃1)⇒{𝑦𝑝cos⁡(𝜃1)=𝑙1+𝐿tan⁡(𝜃1)𝑧𝑝cos⁡(𝜃1)=𝑙1tan⁡(𝜃1)−𝐿
由于 𝑧𝑝≠0 ，可以推出 𝑦𝑝𝑧𝑝=𝑙1+𝐿tan⁡(𝜃1)𝑙1tan⁡(𝜃1)−𝐿 ，从而可以解出 tan⁡(𝜃1)=𝑧𝑝𝑙1+𝑦𝑝𝐿𝑦𝑝𝑙1−𝑧𝑝𝐿 ，即 𝜃1=arctan⁡(𝑧𝑝𝑙1+𝑦𝑝𝐿𝑦𝑝𝑙1−𝑧𝑝𝐿)
求 𝜃3 （余弦定理）

从图中可以得
其中∠𝐴𝑂3𝑃=arccos⁡(|𝑂3𝐴→|2+|𝑂3𝑃→|2−|𝐴𝑃→|22|𝑂3𝐴→||𝑂3𝑃→|)其中{|𝐴𝑃→|=𝑥𝑝2+𝑦𝑝2+𝑧𝑝2−𝑙12|𝑂3𝐴→|=𝑙2|𝑂3𝑃→|=𝑙3=arccos⁡((𝑙1+𝑙2+𝑙3)−(𝑥𝑝2+𝑦𝑝2+𝑧𝑝2)2𝑙2𝑙3)
则可求出 |𝜃3|=𝜋−∠𝐴𝑂3𝑃 ，选定逆时针为正，则 𝜃3=|𝜃3|
求 𝜃2

由前文的正运动学公式 (1) 可得：
{𝑥𝑝=(𝑙3cos⁡𝜃3+𝑙2)sin⁡𝜃2+𝑙3sin⁡𝜃3cos⁡𝜃2𝑦𝑝=−𝑙3sin⁡𝜃1cos⁡(𝜃2+𝜃3)+𝑙1cos⁡𝜃1−𝑙2cos⁡𝜃2sin⁡𝜃1𝑧𝑝=𝑙3cos⁡𝜃1cos⁡(𝜃2+𝜃3)+𝑙1sin⁡𝜃1+𝑙2cos⁡𝜃1cos⁡𝜃2
可以求出
(1)𝑦𝑝sin⁡𝜃1−𝑧𝑝cos⁡𝜃1=−𝑙3cos⁡(𝜃2+𝜃3)(sin2⁡𝜃1+cos2⁡𝜃1)−𝑙2cos⁡𝜃2(sin2⁡𝜃1+cos2⁡𝜃1)(2)=𝑙3sin⁡𝜃3sin⁡𝜃2−(𝑙3cos⁡𝜃3+𝑙2)cos⁡𝜃2
同除 𝑥𝑝
𝑦𝑝sin⁡𝜃1−𝑧𝑝cos⁡𝜃1𝑥𝑝=𝑙3sin⁡𝜃3sin⁡𝜃2−(𝑙3cos⁡𝜃3+𝑙2)cos⁡𝜃2(𝑙3cos⁡𝜃3+𝑙2)sin⁡𝜃2+𝑙3sin⁡𝜃3cos⁡𝜃2
令 {𝑎1=𝑦𝑝sin⁡𝜃1−𝑧𝑝cos⁡𝜃1𝑎2=𝑥𝑝𝑚1=𝑙3sin⁡𝜃3𝑚2=𝑙3cos⁡𝜃3+𝑙2 ，上式可以表示为：
𝑎1𝑎2=𝑚1sin⁡𝜃2−𝑚2cos⁡𝜃2𝑚1cos⁡𝜃2+𝑚2sin⁡𝜃2=𝑚1tan⁡𝜃2−𝑚2𝑚1+𝑚2tan⁡𝜃2
同除 cos⁡(𝜃2) 可以解得：
tan⁡(𝜃2)=𝑎1𝑚1+𝑎2𝑚2𝑎2𝑚1−𝑎1𝑚2
即可求出 𝜃2 ：
𝜃2=arctan⁡(𝑎1𝑚1+𝑎2𝑚2𝑎2𝑚1−𝑎1𝑚2)

至此完成逆运动学求解。

雅可比矩阵

对前文的正运动学公式 (1) 求导：

{𝑥˙𝑝=0⋅𝜃˙1+(𝑙3cos⁡(𝜃2+𝜃3)+𝑙2cos⁡𝜃2)⋅𝜃˙2+𝑙3cos⁡(𝜃2+𝜃3)⋅𝜃˙3𝑦˙𝑝=(−𝑙1sin⁡𝜃1−𝑙2cos⁡𝜃1cos⁡𝜃2−𝑙3cos⁡𝜃1cos⁡(𝜃2+𝜃3))⋅𝜃˙1+(𝑙2sin⁡𝜃1sin⁡𝜃2+𝑙3sin⁡𝜃1sin⁡(𝜃2+𝜃3))⋅𝜃˙2+𝑙3sin⁡𝜃1sin⁡(𝜃2+𝜃3)⋅𝜃˙3𝑧˙𝑝=(𝑙1cos⁡𝜃1−𝑙2sin⁡𝜃1cos⁡𝜃2−𝑙3sin⁡𝜃1cos⁡(𝜃2+𝜃3))⋅𝜃˙1+(−𝑙2cos⁡𝜃1sin⁡𝜃2−𝑙3cos⁡𝜃1sin⁡(𝜃2+𝜃3))⋅𝜃˙2+(−𝑙3cos⁡𝜃1sin⁡(𝜃2+𝜃3))⋅𝜃˙3

整理成矩阵形式：

(2)[𝑥˙𝑝𝑦˙𝑝𝑧˙𝑝]=[𝐽11𝐽12𝐽13𝐽21𝐽22𝐽23𝐽31𝐽32𝐽33]⋅[𝜃˙1𝜃˙2𝜃˙3]=𝐽⋅[𝜃¨1𝜃˙2𝜃˙3]

对其求逆即可得到逆雅可比

(3)[𝜃1˙𝜃2˙𝜃3˙]=𝐽−1[𝑥˙𝑝𝑦˙𝑝𝑧˙𝑝]

单腿静力学

当机器人腿部静止于地面，其关节力矩 𝜏=[𝜏1𝜏2𝜏3] ，足端对地面的作用力 𝐹=[𝐹𝑥𝐹𝑦𝐹𝑧]

可以得到其关系

𝜏1𝜃1˙+𝜏2𝜃2˙+𝜏3𝜃3˙=𝐹𝑥𝑥𝑝˙+𝐹𝑦𝑦𝑝˙+𝐹𝑧𝑧𝑝˙

整理成矩阵形式

[𝜏1𝜏2𝜏3][𝜃˙1𝜃˙2𝜃˙3]=[𝐹𝑥𝐹𝑦𝐹𝑧][𝑥˙𝑝𝑦˙𝑝𝑧˙𝑝]𝜏𝑇⋅𝜃˙=𝐹𝑇⋅𝐽⋅𝜃˙

则可以得到：

𝜏=𝐽𝑇⋅𝐹

同样也可以通过关节力矩得到足端对地面的力

𝐹=𝐽−𝑇⋅𝜏

地面对足端的力 𝐹′=−𝐹

四足运动学

改变姿态

目标：已知机身姿态，求关节角度

令足端静止在地面上，将右前腿 𝑃0 点设为世界坐标系 {𝑠} 的原点，则 𝑝𝑠𝑖 为常量

当 {𝑠} 和 {𝑏} 平行，此时 𝑅𝑠𝑏=𝐼 ，可以得到四条腿相对于世界坐标系的位置：

𝑝𝑠𝑖=𝑅𝑠𝑏⋅[𝑝𝑏𝑖(0)−𝑝𝑏0(0)]=𝐼⋅[𝑝𝑏𝑖(0)−𝑝𝑏0(0)]=𝑝𝑏𝑖(0)−𝑝𝑏0(0)

在初始状态下，世界坐标系到机身坐标系的齐次变换矩阵为

𝑇𝑠𝑏(0)=[𝑅𝑠𝑏𝑂𝑠01×31]=[𝐼3−𝑝𝑏0(0)01×30]

此时改变机身姿态，将其分解为平移与旋转平移旋转{平移𝑝𝑑旋转𝑟𝑝𝑦(𝛼𝛽𝛾)

𝑇𝑠𝑏=[𝑅𝑧(𝛼)𝑅𝑦(𝛽)𝑅𝑥(𝛾)−𝑝𝑏0(0)+𝑝𝑑01×31]

即可求出机身坐标系下的足端坐标

𝑝𝑏𝑖=𝑇𝑏𝑠⋅𝑝𝑠𝑖=𝑇𝑠𝑏−1⋅𝑝𝑠𝑖

经过平移可以得到机器人单腿坐标系下的足端坐标，通过运动学逆解可以得到关节角度。

足端速度

足端速度由两方面叠加产生

关节转动产生的足端速度，利用雅可比矩阵可以求得： 𝑣𝑏𝑗=𝐽𝜃˙
机身移动或旋转产生的足端速度： 𝑣𝑏𝑒 ，此项待求

首先考虑刚体上一点的速度与加速度。在刚体平移或转动时，对于刚体上位置为 𝑝 的质点𝑃 ，只考虑平移不考虑旋转，即只需要研究速度 𝑝˙ 与加速度 𝑝¨ 。

设刚体在 {𝑏} 中的平移速度为 𝑣𝑏 、旋转速度为 𝜔𝑏 ，即原点 𝑝𝑂 的平移速度为 𝑣𝑏 。刚体上位于 𝑝𝑃 的质点以 𝑣𝑏 平移，以 𝜔𝑏 绕 𝑝𝑂 旋转，则：

𝑝˙𝑃=𝑣𝑏+𝑤𝑏×(𝑝𝑃−𝑝𝑂)=𝑣𝑏+[𝑤𝑏]×(𝑝𝑃−𝑝𝑂)

对等式两边求导可得：

其中𝑝¨𝑃=𝑣˙𝑏+[𝜔˙𝑏]×(𝑝𝑃−𝑝𝑂)+[𝜔𝑏]×(𝑝˙𝑃−𝑝˙𝑂)其中 𝑝˙𝑂=𝑣𝑏=𝑣˙𝑏+[𝜔𝑏]×(𝑝𝑃−𝑝𝑂)+[𝜔𝑏]×[𝜔𝑏]×(𝑝𝑃−𝑝𝑂)

当前时刻原点与 {𝑏} 重合，𝑝𝑂=0 ，则:

(4)𝑝˙𝑃=𝑣𝑏+[𝜔𝑏]×𝑝𝑃𝑝¨𝑃=𝑣˙𝑏+[𝜔˙𝑏]×𝑝𝑃+[𝜔𝑏]×[𝜔𝑏]×𝑝𝑃

将机器人视为刚体，足端牵连速度为：（式中的bfB代表：{b}下feet2body）

𝑣𝑏𝑒=𝑣𝑏+[𝜔𝑏]×𝑝𝑏𝑓𝐵

结合开头的叠加， {𝑠} 下足端的速度可以表示为：

𝑣𝑠𝑓=𝑅𝑠𝑏(𝑣𝑏𝑒+𝑣𝑏𝑗)=𝑅𝑠𝑏𝑣𝑏+𝑅𝑠𝑏[𝜔𝑏]×𝑝𝑏𝑓𝐵+𝐽𝜃˙)=𝑣𝑠+𝑅𝑠𝑏([𝜔𝑏]×𝑝𝑏𝑓𝐵+𝐽𝜃˙)

但式中的 𝑣𝑠 无法测得。

{𝑠} 下足端相对于机身的速度可以表示为：

(5)𝑣𝑠𝑓𝐵=𝑣𝑠𝑓−𝑣𝑠=𝑅𝑠𝑏([𝜔𝑏]×𝑝𝑏𝑓𝐵+𝐽𝜃˙)

四足动力学

单刚体动力学

对于密度 𝜌(𝑥,𝑦,𝑧) ，质量 𝑚=∫𝑧∫𝑦∫𝑥𝜌(𝑥,𝑦,𝑧)d𝑥d𝑦d𝑧=∫𝐵𝜌d𝑉 的单刚体，将物体坐标系 {𝑏} 建在重心上，则 ∫𝐵𝜌𝑝𝑏d𝑉=03×1 ， ∫𝐵𝜌[𝑝𝑏]d𝑉=03×3 。

根据牛二定律 𝐹=𝑚𝑎 与式 (4) ，可以求出重心处所受的合外力 𝑓𝑏 ：

𝑓𝑏=∫𝐵𝜌𝑝¨𝑏d𝑉=∫𝐵𝜌(𝑣˙𝑏+[𝜔˙𝑏]×𝑝𝑏+[𝜔𝑏]×[𝜔𝑏]×𝑝𝑏)d𝑉

提取常量 𝑣𝑏 和 𝜔𝑏 ：

𝑓𝑏=𝑣˙𝑏∫𝐵𝜌d𝑉+[𝜔˙𝑏]×∫𝐵𝜌𝑝𝑏𝑑𝑉+[𝜔𝑏]×[𝜔𝑏]×∫𝐵𝜌𝑝𝑏d𝑉

第一项中的 ∫𝐵𝜌d𝑉=𝑚 ， ∫𝐵𝜌𝑝𝑏𝑑𝑉=0 ，则上式整理为：

(6)𝑓𝑏=𝑚𝑣˙𝑏

计算重心处的合外力矩（力矩的计算公式为 𝑀=𝑝×𝑓 ）：

①②③𝑀𝑏=∫𝐵[𝑝𝑏]×d𝑓=∫𝐵[𝑝𝑏]×𝜌𝑝¨𝑏d𝑉=∫𝐵𝜌[𝑝𝑏]×d𝑉⋅𝑣˙𝑏―①+∫𝐵𝜌[𝑝𝑏]×[𝜔˙𝑏]×𝑝𝑏d𝑉―②+∫𝐵𝜌[𝑝𝑏]×[𝜔𝑏]×[𝜔𝑏]×𝑝𝑏d𝑉―③

分开看这三项：

对于式①：
∫𝐵𝜌[𝑝𝑏]×d𝑉=0
对于式②：（ 𝑎×𝑏=−𝑏×𝑎 ）
∫𝐵𝜌[𝑝𝑏]×[𝜔˙𝑏]×𝑝𝑏d𝑉=−∫𝐵𝜌[𝑝𝑏]×[𝑝𝑏]×𝜔˙𝑏d𝑉=(−∫𝐵𝜌[𝑝𝑏]×[𝑝𝑏]×d𝑉)―⋅𝜔˙𝑏=𝐼𝑏―𝜔˙𝑏
定义刚体惯性张量 𝐼𝑏=−∫𝐵𝜌[𝑝𝑏]×[𝑝𝑏]×d𝑉
对于式③：（ [𝑎]×[𝑏]×𝑐=(𝑎𝑇𝑐)𝑏−(𝑎𝑇𝑏)𝑐 ， 𝑎×𝑎=[𝑎]×𝑎=0 ）
∫𝐵𝜌[𝑝𝑏]×[𝜔𝑏]×𝑝𝑏d𝑉=∫𝐵𝜌[𝑝𝑏]×[(𝜔𝑏𝑇𝑝𝑏)𝜔𝑏−(𝜔𝑏𝑇𝜔𝑏)𝑝𝑏]d𝑉=∫𝐵𝜌[(𝜔𝑏𝑇𝑝𝑏)[𝑝𝑏]×𝜔𝑏−(𝜔𝑏𝑇𝜔𝑏)[𝑝𝑏]×𝑝𝑏]d𝑉=∫𝐵𝜌[(𝜔𝑏𝑇𝑝𝑏)[𝑝𝑏]×𝜔𝑏−0]d𝑉=−∫𝐵𝜌[(𝜔𝑏𝑇𝑝𝑏)[𝜔𝑏]×𝑝𝑏−0]d𝑉=−∫𝐵𝜌[(𝜔𝑏+𝑝𝑏)[𝜔𝑏]×𝑝𝑏−(𝑝𝑏𝑇𝑝𝑏)[𝜔𝑏]×𝜔𝑏]d𝑉=−∫𝐵𝜌[𝜔𝑏]×[(𝜔𝑏𝑇𝑝𝑏)𝑝𝑏−(𝑝𝑏𝑇𝑝𝑏)𝜔𝑏]d𝑉=−∫𝐵𝜌[𝜔𝑏]×[𝑝𝑏]×[𝑝𝑏]×𝜔𝑏d𝑉=[𝜔𝑏]×(−∫𝐵𝜌[𝑝𝑏]×[𝑝𝑏]×d𝑉―)𝜔𝑏=[𝜔𝑏]×𝐼𝑏―𝜔𝑏
倒数第二项的下划线部分即为惯性张量。

综上可以得到旋转刚体的欧拉方程：

(7)𝑀𝑏=𝐼𝑏𝜔˙𝑏+[𝜔𝑏]×𝐼𝑏𝜔𝑏

上式中的惯性张量：

𝐼𝑏=−∫𝐵𝜌[𝑝𝑏]×[𝑝𝑏]×d𝑉=−∫𝐵𝜌[0−𝑝𝑧𝑝𝑦𝑝𝑧0−𝑝𝑥−𝑝𝑦𝑝𝑥0][0−𝑝𝑧𝑝𝑦𝑝𝑧0−𝑝𝑥−𝑝𝑦𝑝𝑥0]d𝑉=∫𝐵𝜌[𝑝𝑦2+𝑝𝑧2−𝑝𝑥𝑝𝑦−𝑝𝑥𝑝𝑧−𝑝𝑥𝑝𝑦𝑝𝑥2+𝑝𝑧2−𝑝𝑦𝑝𝑧−𝑝𝑥𝑝𝑧−𝑝𝑦𝑝𝑧𝑝𝑥2+𝑝𝑦2]d𝑉=[𝐼𝑥𝑥𝐼𝑥𝑦𝐼𝑥𝑧𝐼𝑥𝑦𝐼𝑦𝑦𝐼𝑦𝑧𝐼𝑥𝑧𝐼𝑦𝑧𝐼𝑧𝑧]

即可以得到下面的结论。可以计算形状不规则、密度不均匀的物体。

{𝐼𝑥𝑥=∫𝐵𝜌(𝑝𝑦2+𝑝𝑧2)d𝑉𝐼𝑦𝑦=∫𝐵𝜌(𝑝𝑥2+𝑝𝑧2)d𝑉𝐼𝑧𝑧=∫𝐵𝜌(𝑝𝑥2+𝑝𝑦2)d𝑉𝐼𝑥𝑦=−∫𝐵𝜌𝑝𝑥𝑝𝑦d𝑉𝐼𝑥𝑧=−∫𝐵𝜌𝑝𝑥𝑝𝑧d𝑉𝐼𝑦𝑧=−∫𝐵𝜌𝑝𝑦𝑝𝑧d𝑉

例如对于长方体：

𝐼𝑏=112[𝑚(𝜔2+ℎ2)000𝑚(𝑙2+ℎ2)000𝑚(𝑙2+𝜔2)]

在机身坐标系中， 𝐼𝑏 为常量（只与形状、密度分布有关），刚体旋转后世界坐标系下的 𝐼𝑠 会改变。

单刚体动能

由式 (4) ，可以得到刚体整体的动能：（ 𝑣𝑥2+𝑣𝑦2+𝑣𝑧2=𝑣𝑇𝑣 ）

与有关①②③与有关④𝐾=∫𝐵12𝜌𝑣𝑇𝑣d𝑉=12∫𝐵𝜌(𝑣𝑏+[𝜔𝑏]×𝑝𝑏)𝑇(𝑣𝑏+[𝜔𝑏]×𝑝𝑏)d𝑉=12∫𝐵𝜌(𝑣𝑏𝑇+𝑝𝑏𝑇[𝜔𝑏]×𝑇)(𝑣𝑏+[𝜔𝑏]×𝑝𝑏)d𝑉=12∫𝐵𝜌𝑣𝑏𝑇𝑣𝑏―与𝑣𝑏有关d𝑉―①+12𝑣𝑏𝑇[𝜔𝑏]×∫𝐵𝜌𝑝𝑏d𝑉―0―②+12∫𝐵𝜌𝑝𝑏𝑇d𝑉―0[𝜔𝑏]×𝑇𝑣𝑏―③+12∫𝐵𝜌𝑝𝑏𝑇[𝜔𝑏]×𝑇[𝜔𝑏]×―与𝜔𝑏有关𝑝𝑏d𝑉―④

分开看这四项：

式②、③为0
式①与 𝑣𝑏 有关，定义为移动动能 𝐾𝑚
𝐾𝑚=12∫𝐵𝜌𝑣𝑏T𝑣𝑏d𝑉=12∫𝐵𝜌d𝑉𝑣𝑏T𝑣𝑏=12𝑚𝑣𝑏T𝑣𝑏
式③与 𝜔𝑏 有关，定义为转动动能 𝐾𝑡
𝐾𝑡=12∫𝐵𝜌𝑝𝑏𝑇[𝜔𝑏]×𝑇[𝜔𝑏]×𝑝𝑏d𝑉=−12∫𝐵𝜌𝑝𝑏𝑇[𝜔𝑏]×[𝜔𝑏]×𝑝𝑏d𝑉=−12∫𝐵𝜌𝑝𝑏𝑇[(𝜔𝑏𝑇𝑝𝑏)𝜔𝑏−(𝜔𝑏𝑇𝜔𝑏)𝑝𝑏]𝑑𝑉=−12∫𝐵𝜌[(𝜔𝑏𝑇𝑝𝑏)(𝑝𝑏𝑇𝜔𝑏)−(𝜔𝑏𝑇𝜔𝑏)(𝑝𝑏𝑇𝑝𝑏)]𝑑𝑉=−12∫𝐵𝜌[(𝑝𝑏𝑇𝜔𝑏)(𝜔𝑏𝑇𝑝𝑏)−(𝑝𝑏𝑇𝑝𝑏)(𝜔𝑏𝑇𝜔𝑏)]𝑑𝑉=−12∫𝐵𝜌𝜔𝑏𝑇[(𝑝𝑏𝑇𝜔𝑏)𝑝𝑏−(𝑝𝑏𝑇𝑝𝑏)𝜔𝑏]𝑑𝑉=12𝜔𝑏𝑇∫𝐵−𝜌[𝑝𝑏]×[𝑝𝑏]×𝜔𝑏𝑑𝑉=12𝜔𝑏𝑇𝐼𝑏𝜔𝑏
在不同坐标系下，刚体的转动动能 𝐾𝑡 相等。又由 𝜔𝑏=𝑅𝑏𝑠𝜔𝑠 ，故：
𝐾𝑠𝑡=𝐾𝑏𝑡12𝜔𝑠T𝐼𝑠𝜔𝑠=12𝜔𝑏T𝐼𝑏𝜔𝑏𝜔𝑠T𝐼𝑠𝜔𝑠=(𝑅𝑏𝑠𝜔𝑠)T𝐼𝑏(𝑅𝑏𝑠𝜔𝑠)𝜔𝑠T𝐼𝑠𝜔𝑠=𝜔𝑠T(𝑅𝑠𝑏𝐼𝑏𝑅𝑠𝑏T)𝜔𝑠𝐼𝑠=𝑅𝑠𝑏𝐼𝑏𝑅𝑠𝑏T
由此可以得到世界坐标系下刚体的惯性张量，这将在下一节中被用到。

四足动力学

目标：找到足端力与机器人运动的联系。

由式 (6) 和 (7) 可以得到：

{𝑓𝑏=𝑚𝑣˙𝑏𝑀𝑏=𝐼𝑏𝜔˙𝑏+[𝜔𝑏]×𝐼𝑏𝜔𝑏―

由于四足机器人运动时一般不会快速旋转，上式中 𝜔𝑏 很小，故划线部分可以忽略。则上式变为：

{𝑓𝑏=𝑚𝑣˙𝑏𝑀𝑏=𝐼𝑏𝜔˙𝑏

在世界坐标系中同样满足上式，即为：

{𝑓𝑠=𝑚𝑣˙𝑠𝑀𝑠=𝐼𝑠𝜔˙𝑠=𝑅sb𝐼𝑏𝑅sbT𝜔˙𝑠

其中，合外力 𝑓𝑠 由两部分组成：

重力 𝑚𝑔
足端力 𝑓𝑖𝑠

合力矩 𝑀𝑠 由两部分组成：

重力穿过重心，力矩为 0
足端力对重心的力矩 𝑝𝑔𝑖×𝑓𝑖𝑠=[𝑝𝑔𝑖]𝑓𝑖𝑠

则：

(8){𝑚𝑔+∑𝑖=03𝑓𝑖𝑠=𝑚𝑣˙𝑠∑𝑖=03[𝑝𝑖𝑔]×𝑓𝑖𝑠=𝑅𝑠𝑏𝐼𝑏𝑅𝑠𝑏T𝜔˙𝑠

整理成矩阵形式

[𝐼𝐼𝐼𝐼[𝑝𝑔0]×[𝑝𝑔1]×[𝑝𝑔2]×[𝑝𝑔3]×][𝑓𝑠0𝑓𝑠1𝑓𝑠2𝑓𝑠3]=[𝑚(𝑣˙𝑠−𝑔)𝑅𝑠𝑏𝐼𝑏𝑅𝑠𝑏T𝜔˙𝑠]

此时完成了足端力与机器人运动的联系。但此时无法获得机器人姿态 𝑅𝑠𝑏 ，且需要知道机器人在世界坐标系下的 𝑣𝑠 、 𝜔𝑠 。获取这些信息需要用到下一章的状态估计器。

状态估计器

状态估计器是四足机器人系统中比较重要的一环节，通过状态估计器可以获得四足机器人质心位姿（位置和姿态角）及速度。姿态角可根据IMU通过坐标变换得到；位置和速度需要使用最小二乘法或卡尔曼滤波器求得，最终将质心位姿和速度作为机械狗的MPC+WBC控制器及单刚体动力学的反馈输入，进而形成了整个系统的闭环控制。本章节分别介绍最小二乘估计和离散卡尔曼滤波器这两个优化方法。

最小二乘与卡尔曼滤波

在讲状态估计器之前有必要介绍一下最小二乘和卡尔曼滤波的关系。线性回归是一类问题的概念，而最小二乘估计、最小均方误差估计是参数估计方法，最小二乘法、Kalman、梯度下降都是参数优化方法。最小二乘是优化方法中的一种特殊情况，卡尔曼滤波是最小二乘法的一种特殊情况。 古典最小二乘中，假设了每一次测量的权重相同，但事实上这样并不合理，后来演化为加权最小二乘法，至此最小二乘估计所做的都是批处理（Batch），这样比较占内存，不符合动态系统状态估计的需要，即每一次更新输入时，都要从新计算之前所有的记录值。而后，提出递推最小二乘法，模型就不用每次都重新计算了。与递归最小二乘相似，卡尔曼滤波加入了系统内部变化的考虑。即利用process model对系统在下一时刻的状态进行预测。当对于系统不够了解时，使用最小二乘法比较合适，而对于系统了解比较多时，可以采用Kalman滤波。改变量测噪声、系统噪声都会对Kalman滤波的效果产生影响，而不会对最小二乘滤波产生影响，而改变最小二乘的阶数会对其产生影响。

使用IMU获得姿态

使用IMU可以估计质心姿态。现有的IMU基本都可以返回四元数 𝑞=[𝑤𝑥𝑦𝑧] ，经过计算即可获得质心姿态角，其公式如下：

𝑅𝑠𝑏=[1−2𝑦2−2𝑧22𝑥𝑦−2𝑧𝑤2𝑥𝑧+2𝑦𝑤2𝑥𝑦+2𝑧𝑤1−2𝑥2−2𝑧22𝑦𝑧−2𝑥𝑤2𝑥𝑧−2𝑦𝑤2𝑦𝑧+2𝑥𝑤1−2𝑥2−2𝑦2]

当然，也可以通过旋转矩阵转成欧拉角（Roll、Pitch、Yaw）：

Roll=arctan⁡2(𝑅𝑠𝑏(3,2),𝑅𝑠𝑏(3,3))Pitch=arcsin⁡(−𝑅𝑠𝑏(3,1))Yaw=arctan⁡2(𝑅𝑠𝑏(2,1),𝑅𝑠𝑏(1,1))

在IMU的局部坐标系 {𝑏} 中：

𝑚𝑎=−𝑘𝑠+𝑚𝑅𝑏𝑠𝑔𝑠=𝑚𝑘(𝑅𝑏𝑠𝑔−𝑎)

上式中由于重力加速度 𝑔=[00\-9.8] 是世界坐标系下的，所以需要左乘旋转矩阵 𝑅𝑏𝑠 。则加速度计的读数为：

𝑎𝑏=𝑎𝑜𝑢𝑡=−𝑘𝑚𝑠=𝑎−𝑅𝑏𝑠𝑔=𝑎−𝑅𝑠𝑏T𝑔

上述公式建立了加速度计读数与质心姿态的关系，通过左乘旋转矩阵可以计算出IMU在 {𝑠} 坐标系下的加速度：

(9)𝑎𝑠=𝑅𝑠𝑏𝑎=𝑅𝑠𝑏𝑎𝑏+𝑅𝑠𝑏𝑅𝑠𝑏𝑇𝑔=𝑅𝑠𝑏𝑎𝑏+𝑔

状态空间模型

状态估计器需要一个描述四足机器人运动状态的状态空间模型。对于一个状态空间模型需要对其进行能控性和能观性的分析，来判断此学系统是否能够被输入所控制，但本文省略这一部分的内容。本章节先介绍连续状态空间模型，再推出离散状态空间模型。

连续状态空间模型

“连续”指的是时间连续、定常线性系统的状态空间模型：

(10){𝑥˙(𝑡)=𝐴𝑐𝑥(𝑡)+𝐵𝑐𝑢(𝑡)𝑦(𝑡)=𝐶𝑐𝑥(𝑡)

其中：

𝑥(𝑡) ：状态向量
𝑢(𝑡) ：控制向量
𝑦(𝑡) ：输出向量
𝐴𝑐 ：系统矩阵
𝐵𝑐 ：输入矩阵
𝐶𝑐 ：输出矩阵

则 𝑥 应为要估计的量， 𝑦 为能直接测量的量。

令机身加速度 𝑎𝑠 为系统输入量 𝑢 ，上式可变为标准形式：

(11)𝑥˙=[03×3𝐼303×1203×1803×18]𝑥+[03×3𝐼303×3][𝑅𝑠𝑏𝑎𝑏+𝑔]

对于可测量的输出变量 𝑦 ，前文通过IMU可测得 {𝑠} 坐标系下的机身姿态 𝑅𝑠𝑏 ； {𝑏} 下的加速度 𝑎𝑏 、角速度 𝜔𝑏 ；通过正向运动学，可以求出 {𝑏} 坐标系下足端相对于机身的位置 𝑝𝑏𝑓𝐵 ，转换到 {𝑠} 坐标系下，即 𝑝𝑠𝑓𝐵=𝑅𝑠𝑏𝑝𝑏𝑓𝐵 (式12)，即可写出输出变量：

将其写成式 (10) 的标准形式：(式13)

离散状态空间模型

由于控制算法运行于计算机，需要对连续的状态空间模型离散化，使状态空间变量均为时间的函数。设采样时间间隔为 d𝑡 ，则：

𝑥˙(𝑘−1)=𝑥(𝑘)−𝑥(𝑘−1)𝑑𝑡⇒𝑥(𝑘)=𝑥(𝑘−1)+d𝑡𝑥˙(𝑘−1)

将式 (10) 带入

𝑥(𝑘)=𝑥(𝑘−1)+d𝑡(𝐴𝑐𝑥(𝑘−1)+𝐵𝑐𝑢(𝑘−1))=(𝐼+d𝑡𝐴𝑐)―𝑥(𝑘−1)+d𝑡𝐵𝑐―𝑢(𝑘−1)

则离散状态空间模型可以表示为：

其中{𝑥(𝑘)=𝐴𝑥(𝑘−1)+𝐵𝑢(𝑘−1)𝑦(𝑘)=𝐶𝑥(𝑘)其中{𝐴=𝐼+d𝑡𝐴𝑐𝐵=d𝑡𝐵𝑐𝐶=𝐶𝑐

最小二乘估计

输出向量 𝑦 的测量值中含有噪声向量 𝑣 ，假设状态向量 𝑥𝑘 为定值 𝑥 ，离散状态空间中的 𝑦 变为 𝑦𝑘=𝐶𝑥+𝑣𝑘 。假设测量噪声 𝑣∼(0,𝑅) ，其中 𝑅 为协方差矩阵， 𝑅=diag(𝜎12,𝜎22,…,𝜎𝑛2) 。需要在有噪声的情况下获得状态向量的最优估计值 𝑥^ 。

加权最小二乘估计

定义测量残差 𝜖𝑦=𝑦−𝐶𝑥^ 。根据各个传感器的误差方差定义为权重，方差越小权重越大。定义代价函数：

𝐽=𝜖𝑦12𝜎12+𝜖𝑦22𝜎22+⋯+𝜖𝑦𝑛2𝜎𝑛2=𝜖𝑦𝑇𝑅−1𝜖𝑦=(𝑦−𝐶𝑥^)𝑇𝑅−1(𝑦−𝐶𝑥^)=𝑦𝑇𝑅−1𝑦−𝑦𝑇𝑅−1𝐶𝑥^−𝑥^𝑇𝐶𝑇𝑅−1𝑦+𝑥^𝑇𝐶𝑇𝑅−1𝐶𝑥^

求 𝐽 的偏导数 𝜕𝐽𝜕𝑥^=−2𝑦𝑇𝑅−1𝐶+2𝑥^𝑇𝐶𝑇𝑅−1𝐶 ，令其为 0 ，可以计算出 𝑥^=(𝐶𝑇𝑅−1𝐶)−1𝐶𝑇𝑅−1𝑦 。

但此方法需要使用一段时间内所有的测量值 𝑦 ，其计算量及内存占用太大，故改用递推公式。

递推最小二乘估计

定义 𝑥^ 的递推公式：（其中 𝐾𝑘 为最优修正系数矩阵）

(13){𝑦𝑘=𝐶𝑥+𝑣𝑘𝑥^𝑘=𝑥^𝑘−1+𝐾𝑘(𝑦𝑘−𝐶𝑥^𝑘−1)

定义 𝑘 时刻的状态估计误差：

(14)𝜔𝑘=𝑥^−𝑥^𝑘=𝑥^−𝑥^𝑘−1−𝐾𝑘(𝑦𝑘−𝐶𝑥^𝑘−1)=𝜔𝑘−1−𝐾𝑘𝐶(𝑥−𝑥^𝑘−1)−𝐾𝑘𝑣𝑘=(𝐼−𝐾𝑘𝐶)𝜔𝑘−1−𝐾𝑘𝑣𝑘

再次使用二次规划，令 𝑘 时刻各状态估计误差方差和最小，则 𝑘 时刻的代价函数 𝐽𝑘 为：

𝐽𝑘=𝐸(𝜔𝑘12+𝜔𝑘22+⋯+𝜔𝑘𝑛2)=𝜎𝑘12+𝜎𝑘22+⋯+𝜎𝑘𝑛2=Tr(𝑃𝑘)

估计误差协方差 𝑃𝑘=𝐸(𝜔𝑘𝜔𝑘𝑇) ，将式 (14) 带入可以得到：

(15)𝑃𝑘=𝐸(𝜔𝑘𝜔𝑘𝑇)=(𝐼−𝐾𝑘𝐶)𝑃𝑘−1(𝐼−𝐾𝑘𝐶)𝑇+𝐾𝑘𝑅𝐾𝑘𝑇

目的为使 𝐽 最小，对其求偏导：

𝜕𝐽𝑘𝜕𝐾𝑘=2(𝐼−𝐾𝑘𝐶)𝑃𝑘−1(−𝐶𝑇)+2𝐾𝑘𝑅=0

解得最优的 𝐾𝑘 为：

(16)𝐾𝑘=𝑃𝑘−1𝐶𝑇(𝑅+𝐶𝑃𝑘−1𝐶𝑇)−1

最小二乘估计流程总结

确定初始状态的最优估计值 𝑥^0 和估计值的协方差 𝑃0 。如果对系统状态 𝑥 完全不了解，则可以将 𝑥^0 设为任意值，同时令 𝑃0=∞𝐼 ，表示无穷大的协方差，即完全不信任 𝑥^0 。
估计器运行开始，在 𝑘=1 时刻，机器人从各个传感器得到输出向量 𝑦1 ，并根据式 (16) 计算出当前时刻的最优修正系数矩阵 𝐾1=𝑃0𝐶𝑇(𝑅+𝐶𝑃0𝐶𝑇)−1 。根据式 (13) 可以计算出当前时刻的最优估计值 𝑥^1=𝑥^0+𝐾1(𝑦1−𝐶𝑥^0) 。根据式 (15) 可以计算出估计误差协方差 𝑃1=(𝐼−𝐾1𝐶)𝑃0(𝐼−𝐾1𝐶)𝑇+𝐾1𝑅𝐾1𝑇 。
当 𝑘=2,3,⋯ ，估计器重复第二步，估计值 𝑥^ 不断逼近真实状态 𝑥 。

过程噪声

前文假设状态向量为定值。若状态向量是变化的，增加过程噪声 𝑤 ，且其期望为0。则离散状态空间模型变为：

𝑥𝑘=𝐴𝑥𝑘−1+𝐵𝑢𝑘−1+𝑤

状态期望为：

𝑥𝑘―=𝐸(𝑥𝑘)=𝐴𝐸(𝑥𝑘−1)+𝐵𝐸(𝑢𝑘−1)+𝐸(𝑤𝑘−1―0)=𝐴𝑥―𝑘−1+𝐵𝑢―𝑘−1

状态向量 𝑥𝑘 的协方差为：（ 𝑄=𝑤𝑤𝑇 为过程噪声协方差）

𝑃𝑘=𝐸[(𝑥𝑘−𝑥―𝑘)(𝑥𝑘−𝑥―𝑘)𝑇]=𝐴𝑃𝑘−1𝐴𝑇+𝑤𝑘−1𝑤𝑘−1𝑇=𝐴𝑃𝑘−1𝐴𝑇+𝑄―

扩展卡尔曼滤波器

最小二乘估计只能对固定状态 𝑥 进行估计，而扩展卡尔曼滤波器可以对变化的状态 𝑥𝑘 进行估计。

TODO

步态轨迹规划

落脚点的选取

平移

TODO 图

上图为四足机器人的一条腿从抬起到落下的过程，分区看图中的几个过程：

➂为中性落脚点
➀~➁阶段为摆动相，机器人速度为 𝑣𝑥 ，摆动时间为 𝑇𝑠𝑤𝑖𝑛𝑔 ，则 𝑑12=𝑣𝑥𝑇𝑠𝑤𝑖𝑛𝑔 。
➁~➃阶段为支撑相，机器人速度为 𝑣𝑥 ，触底时间为 𝑇𝑠𝑡𝑎𝑛𝑐𝑒 ，则 𝑑23=Δ𝑥=12𝑣𝑥𝑇𝑠𝑡𝑎𝑛𝑐𝑒 。

图中落脚点 𝑥𝑓=𝑥𝑏+𝑑12+𝑑23=𝑥𝑏+𝑣𝑥𝑇𝑠𝑤𝑖𝑛𝑔+12𝑣𝑥𝑇𝑠𝑡𝑎𝑛𝑐𝑒

设相位 𝑝𝑡=𝑡−𝑡0𝑡1−𝑡0 ，则落脚点变为 𝑥𝑓=𝑥𝑝(𝑝)+𝑣𝑥(1−𝑝)𝑇𝑠𝑤𝑖𝑛𝑔+12𝑣𝑥𝑇𝑠𝑡𝑎𝑛𝑐𝑒

若落脚点位于中性落脚点左侧，重力会对落脚点产生顺时针的力矩从而向右加速，所以若 𝑣𝑥<𝑣𝑥𝑑 ，则需左移落脚点。

{𝑥𝑓=𝑥𝑝(𝑝)+𝑣𝑥(1−𝑝)𝑇𝑠𝑤𝑖𝑛𝑔+12𝑣𝑥𝑇𝑠𝑡𝑎𝑛𝑐𝑒+𝑘𝑥(𝑣𝑥−𝑣𝑥𝑑)𝑦𝑓=𝑦𝑝(𝑝)+𝑣𝑦(1−𝑝)𝑇𝑠𝑤𝑖𝑛𝑔+12𝑣𝑦𝑇𝑠𝑡𝑎𝑛𝑐𝑒+𝑘𝑦(𝑣𝑦−𝑣𝑦𝑑)

旋转

𝜃𝑓=𝜃𝑝+𝜃0+𝜔(1−𝑝)𝑇𝑠𝑤𝑖𝑛𝑔+12𝜔𝑇𝑠𝑡𝑎𝑛𝑐𝑒+𝑘𝜔(𝜔−𝜔𝑑)

则世界坐标系下的落脚点为

{𝑥𝑓=𝑅cos⁡𝜃𝑓𝑦𝑓=𝑅sin⁡𝜃𝑓

结合平移与旋转，落脚点坐标为：

{𝑥𝑖=𝑥𝑏+𝑅cos⁡𝜃𝑓+𝑣𝑥(1−𝑝)𝑇𝑠𝑤𝑖𝑛𝑔+12𝑣𝑥𝑇𝑠𝑡𝑎𝑛𝑐𝑒+𝑘𝑥(𝑣𝑥−𝑣𝑥𝑑)𝑦𝑖=𝑦𝑏+𝑅sin⁡𝜃𝑓+𝑣𝑦(1−𝑝)𝑇𝑠𝑤𝑖𝑛𝑔+12𝑣𝑦𝑇𝑠𝑡𝑎𝑛𝑐𝑒+𝑘𝑦(𝑣𝑦−𝑣𝑦𝑑)

MPC控制器

目标

通过MPC可以得到：

质心位置、速度、加速度、旋转角度、角速度
足底力
足端位置、速度、加速度

让机器狗质心跟踪最新的参考轨迹，计算未来一段时间内的足底力

简化单刚体动力学

下面考虑单刚体动力学：

𝑝¨=∑𝑖=1𝑛𝑓𝑖𝑚−𝑔𝑑𝑑𝑡(𝐼𝜔)=∑𝑖=1𝑛𝑟𝑖×𝑓𝑖

对上式其进行简化

𝑑𝑑𝑡(𝐼𝜔)=𝐼𝜔+𝜔×(𝐼𝜔)≈𝐼𝜔˙ ，其中 𝐼=𝑅𝐼𝑏𝑅𝑇≈𝑅𝑧(𝜓)𝐼𝑏𝑅𝑧𝑇(𝜓)
简化pitch和row

IMU获得的角速度为身体坐标系：
𝜔𝑠=𝑅𝑧(𝜓)𝑅𝑦(𝜃)𝑅𝑥(𝜙)𝜔𝑏𝜔𝑏=[𝑅𝑧(𝜓)𝑅𝑦(𝜃)𝑅𝑥(𝜙)]−1𝜔𝑠[𝜙˙𝜃˙𝜓˙]=[cos⁡(𝜓)/cos⁡(𝜃)sin⁡(𝜓)/cos⁡(𝜃)0−sin⁡(𝜓)cos⁡(𝜓)0cos⁡(𝜓)tan⁡(𝜃)sin⁡(𝜓)tan⁡(𝜃)1]𝜔
在运动过程中机器人的pitch和row均接近于0，故可以忽略
[𝜙˙𝜃˙𝜓˙]=[cos⁡(𝜓)sin⁡(𝜓)0−sin⁡(𝜓)cos⁡(𝜓)0001]𝜔=𝑅𝑧𝑇(𝜓)𝜔

状态方程

下面写出状态方程：

状态变量

位置 𝑝=[𝑥𝑦𝑧] ，速度 𝑝˙=[𝑥˙𝑦˙𝑧˙] ，姿态角 𝛩=[𝜃𝜙𝜓] ，姿态角速度 𝜔=[𝜃˙𝜙˙𝜓˙] ，重力加速度 𝑔=[00−9.8] 。状态变量则为13维变量： 𝑥=[𝜃𝑝𝑤𝑝˙𝑔(3)] 。
控制变量

控制变量为12维变量： 𝑢=[𝑓1𝑓2𝑓3𝑓4] 。其中 𝑓𝑖 为足底力。
状态方程
{𝑥˙=𝐴𝑥+𝐵𝑢𝑦=𝐶𝑥+𝐷𝑢
将各个已知量带入：
𝑑𝑑𝑡[𝜃𝑝𝜔𝑝˙𝑔(3)]=[03×303×3𝑅𝑧𝑇(𝜓)03×303×103×303×303×3𝐼303×103×303×303×303×303×103×303×303×303×3[001]01×301×301×301×31][𝜃𝑝𝜔𝑝˙𝑔(3)]+[03×303×303×303×303×303×303×303×3𝐼−1[𝑟1]×𝐼−1[𝑟2]×𝐼−1[𝑟3]×𝐼−1[𝑟4]×𝐼3/𝑚𝐼3/𝑚𝐼3/𝑚𝐼3/𝑚01×301×301×301×3][𝑓1𝑓2𝑓3𝑓4]
要想写成代码，需要对其离散化。使用前向欧拉法离散化状态方程：
𝑥˙=𝑥𝑘+1−𝑥𝑘Δ𝑡=𝐴𝑥𝑘+𝐵𝑢𝑘𝑥𝑘+1=(𝐼+Δ𝑡𝐴)―+Δ𝑡𝐵―𝑢𝑘=𝐴―𝑥𝑘+𝐵―𝑢𝑘
其中的 𝐴― 和 𝐵― 表示为：
𝐴―=[𝐼303×3𝑅𝑧𝑇(𝑑𝜓𝑘)Δ𝑇03×303×103×3𝐼303×3𝐼3Δ𝑇03×103×303×3𝐼303×303×103×303×303×3𝐼3[001]Δ𝑇01×301×301×301×31]𝐵―=[03×303×303×303×303×303×303×303×3𝐼𝑘−1[𝑟1𝑘]×Δ𝑇𝐼𝑘−1[𝑟2𝑘]×Δ𝑇𝐼𝑘−1[𝑟3𝑘]×Δ𝑇𝐼𝑘−1[𝑟4𝑘]×Δ𝑇𝐼3Δ𝑇/𝑚𝐼3Δ𝑇/𝑚𝐼3Δ𝑇/𝑚𝐼3Δ𝑇/𝑚01×301×301×301×3]

约束

MPC最大的优势就是可以对输入量和状态变量进行约束，所以可以对输入进行约束。输入约束可以分为以下两个约束条件：

足底反力

摆动时足底力为0，state=0， 𝑓𝑖=03×1

触地时最大足底力为 𝑓𝑚𝑎𝑥 ，state=1， 𝑓𝑖𝑧≤𝑓𝑚𝑎𝑥
摩擦锥

为保证足底与地面不滑动，足底反力的水平分量不能大于竖直分量 ×𝜇 ，即
|𝑓𝑖𝑥|≤𝜇𝑓𝑖𝑧|𝑓𝑖𝑦|≤𝜇𝑓𝑖𝑧𝑓𝑖𝑧>0

结合以上两个约束，可以得到总的输入约束：

[00000]≤[−10𝜇0−1𝜇10𝜇01𝜇001][𝑓𝑖𝑥𝑓𝑖𝑦𝑓𝑖𝑧]≤[+∞+∞+∞+∞𝑓max]

优化问题

首先建立代价函数（式中 𝑄 、 𝑅 为对角正定/半正定矩阵，需要对其进行调参）

误差加权和输入加权和min𝑥,𝑢∑𝑖=0𝐾−1‖𝑥𝑖+1−𝑥𝑖+𝑖,𝑟𝑒𝑓‖𝑄𝑖―误差加权和+‖𝑢𝑖‖𝑅𝑖―输入加权和

使其满足：

支撑相摆动相{𝑥𝑖+1=𝐴𝑖𝑥𝑖+𝐵𝑖𝑢𝑖,𝑖=0⋯𝑘−1𝑐𝑖―≤𝐶𝑖𝑢𝑖≤𝑐𝑖―,𝑖=0⋯𝑘−1(支撑相)𝐷𝑖𝑢𝑖=0,𝑖=0⋯𝑘−1(摆动相)

对其进行二次规划，化为qpOASES标准形式：

min𝑥12𝑥𝑇𝐻𝑥+𝑥𝑇𝑔(𝑤0)𝑠.𝑡.𝑙𝑏𝐴(𝜔0)≤𝐴𝑥≤𝑢𝑏𝐴(𝜔0)𝑙𝑏(𝜔0)≤𝑥≤𝑢𝑏(𝜔0)

预测步长为 𝑛 ，状态量 𝑋𝑘=[𝑥𝑘𝑘+1𝑥𝑘𝑘+2⋮𝑥𝑘𝑘+𝑛] ，输入量 𝑈𝑘=[𝑢𝑘𝑘𝑢𝑘𝑘+1⋮𝑢𝑘𝑘+𝑛−1] ，则

𝑋𝑘=[𝐴―𝐴―2⋮𝐴―𝑁]𝑥𝑘+[𝐵―0⋯0𝐴―𝐵―𝐵―⋯⋮⋮⋮⋱⋮𝐴―𝑁−1𝐵―𝐴―𝑁−2𝐵―⋯𝐵―]𝑈𝑘

上式可以记为 𝑋=𝐴𝑞𝑝𝑥0+𝐵𝑞𝑝𝑈 。对于目标值 𝑋ref=[𝑥𝑘+1𝑥𝑘+2⋮𝑥𝑘+𝑛] ，优化问题可以转化为：

min𝑈𝐽(𝑈)=‖𝑋−𝑋ref‖𝐿+‖𝑈‖𝐾=(𝑋−𝑋𝑟𝑒𝑓)𝑇𝐿(𝑋−𝑋𝑟𝑒𝑓)+𝑈𝑇𝐾𝑈=(𝐴𝑞𝑝𝑥0+𝐵𝑞𝑝𝑈−𝑦)𝑇𝐿(𝐴𝑞𝑝𝑥0+𝐵𝑞𝑝𝑈−𝑦)+𝑈𝑇𝐾𝑈=(𝑥0𝑇𝐴𝑞𝑝𝑇+𝑈𝑇𝐵𝑞𝑝𝑇−𝑦𝑇)𝐿(𝐴𝑞𝑝𝑥0+𝐵𝑞𝑝𝑈−𝑦)+𝑈𝑇𝑘𝑈=𝑈𝑇(𝐵𝑞𝑝𝑇𝐿𝐵𝑞𝑝+𝐾)𝑈+𝑈𝑇𝐵𝑞𝑝𝑇𝐿(𝐴𝑞𝑝𝑥0−𝑦)+(𝐴𝑞𝑝𝑥0−𝑦)𝑇𝐿𝐵𝑞𝑝𝑈+(𝐴𝑞𝑝𝑥0−𝑦)𝑇𝐿(𝐴𝑞𝑝𝑥0−𝑦)=𝑈𝑇(𝐵𝑞𝑝𝑇𝐿𝐵𝑞𝑝+𝐾)𝑈+𝑈𝑇𝐵𝑞𝑝𝑇𝐿(𝐴𝑞𝑝𝑥0−𝑦)+[𝑈𝑇𝐵𝑞𝑝𝑇𝐿(𝐴𝑞𝑝𝑥0−𝑦)]𝑇+(𝐴𝑞𝑝𝑥0−𝑦)𝑇𝐿(𝐴𝑞𝑝𝑥0−𝑦)

观察上式，第二第三项L左右均为向量且L为对角阵，故两者相同；第四项中的 𝐴𝑞𝑝𝑥0−𝑦 无控制量的影响，仅为状态量的变化，为常数项，可以省略。故上式可以整理为：

min𝑈𝐽(𝑈)=𝑈𝑇(𝐵𝑞𝑝𝑇𝐿𝐵𝑞𝑝+𝐾)𝑈+2𝑈𝑇𝐵𝑞𝑝𝑇𝐿(𝐴𝑞𝑝𝑥0−𝑦)

即可以整理为：

min𝑈12𝑈𝑇𝐻𝑈+𝑈𝑇𝑔𝑠.𝑡.𝑐―≤𝐶𝑈≤𝑐―

其中：（维度中的 𝑛 为足数4， 𝑘 为单次MPC的迭代次数）

𝐻=2(𝐵𝑞𝑝𝑇𝐿𝐵𝑞𝑝+𝐾)∈𝑅3𝑛𝑘×3𝑛𝑘𝑔=2𝐵𝑞𝑝𝑇𝐿(𝐴𝑞𝑝𝑥0−𝑦)∈𝑅3𝑛𝑘×1

至此可得机器狗跟踪质心轨迹的一组最优足端支撑力， 𝑢0=[𝑓1𝑓2𝑓3𝑓4]

WBC控制器

计算关节位置、速度、加速度

零空间

通过MPC可得到最优足端力 𝑢0 ，可通过 𝜏=𝐽(𝜃)𝑇𝐹 计算关节力矩，但只适用于低速场景。在高速场景下，足端与地面的接触时间非常短，无法进行优化，此时动态性能较差，故需要使用WBC控制器，其输入参数为接触状态、支撑力、足端轨迹、质心轨迹。且其控制频率比MPC快。

当机器人高速运动时，四条腿触地时间很短，机器人大部分时间处于欠驱动状态，与无人机欠驱动系统不同，四足机器人与地面的频繁接触变成了干扰项。此时机器人相当于与一个虚拟浮动基座相连。此虚拟浮动基座有6个自由度（冗余），此时机器人的状态变得不确定。

将需要控制的变量进行优先级划分，使用零空间理论：

级别	内容	约束
0级	足端接触力	高级别
1级	机器人姿态	不能翻
2级	机器人位置	可适量浮动
3级	摆动退轨迹	低级别

冗余自由度机器人的优化

𝐴 的零空间是 𝐴𝑥=0 的所有解 𝑥 的集合，也就是 𝑥 固定，无论 𝐴 取何值，结果均为0。类比串联机械臂，末端固定而其他关节转动时，所有能转动的位形构成零空间。

前文推导了末端速度与关节角速度之间的关系：𝑥˙=𝐽(𝜃)𝜃˙ ，若 𝐽(𝜃) 可逆，可以得到 𝜃˙=𝐽−1(𝜃)𝑥˙ 。

若对于机械臂，遇到奇异位形，或关节数大于末端坐标数，此时 𝐽(𝜃) 不可逆。故引入伪逆， 𝐽(𝜃) 的伪逆表示为 𝐽†(𝜃) ，无论 𝐽(𝜃) 是否可逆，关节角速度均可表示为 𝜃˙=𝐽†(𝜃)𝑥˙ 。

伪逆的计算可以分两种情况

欠约束，关节数n＜末端坐标数m， 𝐽†𝐽=𝐼,𝐽†=(𝐽𝑇𝐽)−1 ，此时方程可能无解，用代价函数使结果逼近设定值。
冗余约束，关节数n＞末端坐标数m， 𝐽𝐽†=𝐼,𝐽†=𝐽𝑇(𝐽𝐽𝑇)−1 ，此时方程有无数解，设定关节约束得到最优结果。

对于冗余约束机械臂，若想在保持末端位姿不变的前提下控制其他关节的状态，则需利用零空间。

𝜃˙=𝐽†(𝜃)𝑥˙ 是一个特解，其通解为 𝜃˙=𝐽†(𝜃)𝑥˙+(𝐼−𝐽†(𝜃)𝐽(𝜃))𝑧 TODO原理

将 𝑥˙ 称为任务空间， 𝜃˙ 称为控制空间， 𝑧 称为零空间。 𝑥˙ 中为高优先级的任务， 𝑧 中为低优先级的任务。

两个任务

对于两个任务，高优先级任务为 𝑤˙1 ，低优先级任务为 𝑤˙2

𝑤˙1=𝐽1(𝜃)𝜃˙𝑤˙2=𝐽2(𝜃)𝜃˙

对于第一个高优先级任务的式子，其冗余控制方程为：

𝜃˙=𝐽1†(𝜃)𝑤˙1+(𝐼−𝐽1†(𝜃)𝐽1(𝜃))𝑧

将其带入第二个式子，得到：

𝑤˙2=𝐽2(𝜃)(𝐽1†(𝜃)𝑤˙1+(𝐼−𝐽1†(𝜃)𝐽1(𝜃))𝑧)

整理得：

𝐽2(𝜃)(𝐼−𝐽1†(𝜃)𝐽1(𝜃))𝑧=𝑤˙2−𝐽2(𝜃)𝐽1†(𝜃)𝑤˙1

此时结果可能不唯一，使用最小二乘法优化代价函数。同样需要分为以下两种情况：

欠约束， min|𝑧|2
冗余约束， min|𝐽2(𝜃)(𝐼−𝐽1†(𝜃)𝐽1(𝜃))𝑧−𝑤˙2+𝐽2(𝜃)𝐽1†(𝜃)𝑤˙1|2

多个任务

推广到一般形式，对于n个任务，符合任务优先级的关节速度为：

𝑞˙=∑𝑖=1𝑛𝑁𝑖𝑞˙𝑖𝑞˙𝑖=(𝐽𝑖𝑁𝑖―)†(𝑥˙𝑖∗−𝐽𝑖∑𝑛=1𝑛−1𝑁𝑘𝑞˙𝑘)

式中 𝑥˙𝑖∗ 为最优解， 𝑁𝑖― 为组合雅可比矩阵 𝐽𝑖― 的零空间投影矩阵， 𝐽𝑖―=[𝐽1𝐽2⋮𝐽𝑖−1] 为所有任务优先级高于 𝑖 的任务的雅可比矩阵的组合。

写成迭代形式：

𝑞˙𝑖=𝑞˙𝑖−1+(𝐽𝑖𝑁𝑖−1)†(𝑥˙𝑖∗−𝐽𝑖𝑞˙𝑖−1)

关节位置

根据上文的推导，可以得到关节位置的迭代形式：

Δ𝑞1=𝐽1†(𝑥1𝑑𝑒𝑠−𝑥1)Δ𝑞2=Δ𝑞1+𝐽2|𝑝𝑟𝑒†(𝑥2des−𝑥2−𝐽2Δ𝑞1)⋮Δ𝑞𝑖=Δ𝑞𝑖−1+𝐽𝑖|𝑝𝑟𝑒†(𝑥𝑖des−𝑥𝑖−𝐽𝑖Δ𝑞𝑖−1)

其中：

𝐽𝑖|𝑝𝑟𝑒=𝐽𝑖𝑁𝑖−1,𝑁𝑖−1=𝑁0𝑁1|0⋯𝑁𝑖−1|𝑖−2,𝑁0=𝐼−𝐽𝑐†𝐽𝑐,𝑁𝑖|𝑖−1=𝐼−𝐽𝑖|𝑖−1†𝐽𝑖|𝑖−1.

其中 𝐽𝑖 为第 𝑖 项任务雅可比矩阵； 𝑥𝑖des 为第 𝑖 项任务的期望位置； Δ𝑞𝑖 为根据第 𝑖 项任务迭代计算出的关节位置增量； 𝐽𝑖|𝑝𝑟𝑒 为第 𝑖 项任务到先前任务的零空间投影； 𝐽𝑐 为接触约束雅可比矩阵；

关节速度与加速度

同理可以得到关节速度与加速度（ dyn 代表动态一致伪逆： 𝐽dyn―=𝐴−1𝐽𝑇(𝐽𝐴−1𝐽𝑇)−1 ）：

𝑞˙𝑖cmd=𝑞˙𝑖−1cmd+𝐽𝑖|𝑝𝑟𝑒†(𝑥˙𝑖des−𝐽𝑖𝑞˙𝑖−1cmd),𝑞¨𝑖cmd=𝑞¨𝑖−1cmd+𝐽𝑖|𝑝𝑟𝑒dyn―(𝑥¨𝑖cmd−𝐽˙𝑖𝑞˙−𝐽𝑖𝑞¨𝑖−1cmd)

其中（ 𝐾𝑝 和 𝐾𝑑 分别是位置和速度反馈增益，cmd代表控制命令）：

𝑞𝑖cmd=𝑞𝑖+Δ𝑞𝑖𝑥¨𝑖cmd=𝑥¨des+𝐾𝑝(𝑥𝑖des−𝑥𝑖)+𝐾𝑑(𝑥˙des−𝑥˙)

计算出的 𝑞𝑖cmd 和 𝑞˙𝑖cmd 发送到关节PD控制器、 𝑞¨𝑖cmd 发送到QP规划器计算扭矩命令

计算关节力矩

浮动基动力学模型

对于固定机械臂这种基坐标系固定的机器人，只需知道每个关节的角度即可知道机器人的姿态。但对于四足机器人来说基座是不固定的，故需要在世界系与浮动基座机器人本体系之间建立一个没有实体的 6 自由度关节，其关节空间向量为 𝑞=[𝑞𝑓𝑞𝑗] ，其中 𝑞𝑓 代表身体浮动基的6个自由度， 𝑞𝑗 代表关节的12个自由度。浮动基动力学方程为：

𝐴𝑞¨+𝑏+𝑔=𝑆𝑗𝑇𝜏+𝐽𝑖𝑛𝑡𝑇𝑓𝑖𝑛𝑡−𝐽𝑐𝑇𝑓𝑖

上式中 𝐴 为质量（惯量）矩阵； 𝑏 为可科氏力与离心力项； 𝑔 为重力项； 𝑆𝑗 为驱动关节选择矩阵，将驱动关节的扭矩映射到整个配置空间； 𝑓𝑖𝑛𝑡 为内力； 𝑓𝑖 为足底力； 𝐽 为约束雅可比矩阵，其中 𝑖𝑛𝑡 代表内部约束； 𝑐 代表接触约束， 𝐽 满足下式：

0=𝐽𝑞˙0=𝐽𝑞¨+𝐽˙𝑞˙