一、机构自由度DoF

自由度M根据Grubler公式：

$M=6(n-g-1)+\sum_{i=1}^{n}f_i$

对于该机构：

总构件数 $n=6$ （虎克铰作为一个高副看待，不计算中心十字）
运动副（高副）数 $g=7$ （虎克铰作为一个高副看待）
总自由度（低副）数 $\sum_{i=1}^{n}f_i=16-2$ （一个连杆两端的两个球铰有一个局部自由度，看作一个球铰和一个虎克铰）

因此：

$M=6\times(6-7-1)+14=2$

该并联机构自由度数为2，2电机输入在A1,A2，末端构件脚部有两个旋转自由度，为全驱动机构。

Image Token: BVukbpAjIozvl9xTPRicPLhxnBc

二、逆向运动学

1. 建立坐标系

为了计算方便，把世界坐标系建在虎克铰中心，以初始姿态时的虎克铰两个副的方向作为X轴，Y轴（如上图）。

则脚部构件的旋转矩阵R为YX欧拉角（注意先后顺序，如果虎克铰两轴机械设计相反，则顺序相反）。

记为 $\begin{bmatrix} \theta_r\\ \theta_p \end{bmatrix}$ ，则旋转矩阵R为：

$R=\text{RotY}(\theta_p)\text{RotX}(\theta_r)$

众擎代码为：

2. 计算BA_bar旋转角度θ

注意到B点既在BA_bar，又在CB_rod上，因此B点在以C为圆心，rod长为半径的球面上，同时又在A为圆心，bar长为半径的空间圆上。

首先计算BA_bar初始向 $\mathbf{AB_0}$ ，圆心与球心向量 $\mathbf{CA}$

将B点带入球面约束方程：

${\left \| \mathbf{CA}+\mathbf{AB} \right \|}^2 ={L_{rod}}^2$ ${\left \| \mathbf{CA} \right \|}^2 + 2\mathbf{CA}\cdot\mathbf{AB} +{L_{bar}}^2 ={L_{rod}}^2$ $\mathbf{CA}\cdot\mathbf{AB} =\frac{ {L_{rod}}^2 -{L_{bar}}^2 - {\left \| \mathbf{CA} \right \|}^2}{2}$

代码中：

即：

$\mathbf{CA}\cdot\mathbf{AB} = \frac{ M}{2}$

由于电机轴线沿Y方向，因此可表示为：

$\mathbf{AB}=RotY(\theta) \mathbf{AB_0}=\begin{bmatrix} \mathbf{AB_0}_x\cos{\theta}+\mathbf{AB_0}_z\sin{\theta}\\ \mathbf{AB_0}_y(0)\\ -\mathbf{AB_0}_x\sin{\theta}+\mathbf{AB_0}_z\cos{\theta} \end{bmatrix}$

因此：

$\mathbf{CA}\cdot\mathbf{BA} = \mathbf{CA}_x(\mathbf{AB_0}_x\cos{\theta}+\mathbf{AB_0}_z)+\mathbf{CA}_z(-\mathbf{AB_0}_x\sin{\theta}+\mathbf{AB_0}_z\cos{\theta})$ $\mathbf{CA}\cdot\mathbf{BA} = (\mathbf{CA}_x\mathbf{AB_0}_z-\mathbf{CA}_z\mathbf{AB_0}_x)sin{\theta}+(\mathbf{CA}_x\mathbf{AB_0}_x+\mathbf{CA}_z\mathbf{AB_0}_z)\cos{\theta}$

代码：

即：

$\mathbf{CA}\cdot\mathbf{BA} = Ksin{\theta}+N\cos{\theta}$

因此：

$Ksin{\theta}+N\cos{\theta} = \frac{ M}{2}$

用三角等式消去其中余弦项，得到：

$4(N^2+K^2)\sin^2\theta-4MK\sin\theta+(M^2-4N^2)=0$

求解一元二次方程：

该方程有两个根，对应的是两种不同的构型，构型由初始构件安装条件确定，在运动过程中构型不会变化（除非在奇异点处有可能发生构型变化），本构型求根时取+号。

之后，可以根据求解角度计算出机构任何一点的空间位置，得到逆向运动学。

三、雅克比矩阵

雅克比矩阵的目的是推导末端欧拉角速度 $\begin{bmatrix} \dot{\theta_r}\\ \dot{\theta_p} \end{bmatrix}$ 与电机输入速度 $\begin{bmatrix} \dot{\theta_1}\\ \dot{\theta_2} \end{bmatrix}$ 的关系。

$\begin{bmatrix} \dot{\theta_r}\\ \dot{\theta_p} \end{bmatrix}=\mathbf{J}\begin{bmatrix} \dot{\theta_1}\\ \dot{\theta_2} \end{bmatrix}$

需要通过速度之间的关系推导 $\mathbf{J}$ 。

1. 欧拉角速度与角速度转换

如果脚部输入速度是 $\begin{bmatrix} \dot{\theta_r}\\ \dot{\theta_p} \end{bmatrix}$ ，求脚部的 $\begin{bmatrix} \omega_x\\ \omega_y\\ \omega_z \end{bmatrix}$ 。

$\begin{bmatrix} \omega_x\\ \omega_y\\ \omega_z \end{bmatrix}=\begin{bmatrix} 0\\ 1\\ 0 \end{bmatrix}\dot{\theta_p}+ RotY(\theta_p) \begin{bmatrix} 1\\ 0\\ 0 \end{bmatrix}\dot{\theta_r}=\begin{bmatrix} \cos\theta_p&0\\ 0&1\\ -\sin\theta_p&0 \end{bmatrix}\begin{bmatrix} \dot{\theta_r}\\ \dot{\theta_p} \end{bmatrix}$

加上线速度：

$\begin{bmatrix} \mathbf{v}\\ \mathbf{\omega} \end{bmatrix}=\begin{bmatrix} v_x\\ v_y\\ v_z\\ \omega_x\\ \omega_y\\ \omega_z \end{bmatrix}=\begin{bmatrix} 0&0\\ 0&0\\ 0&0\\ \cos\theta_p&0\\ 0&1\\ -\sin\theta_p&0 \end{bmatrix}\begin{bmatrix} \dot{\theta_r}\\ \dot{\theta_p} \end{bmatrix}= \mathbf{J_q} \begin{bmatrix} \dot{\theta_r}\\ \dot{\theta_p} \end{bmatrix}$

代码中 $\mathbf{J_q}$ 的计算：

2. 以沿杆速度建立等式

对于一个刚体杆，各点沿杆方向的线速度均相等，以此建立等式。

2.1 从电机端计算沿杆速度

设电机速度为 $\dot{\theta}$ ，转轴均沿Y轴正方向 $s$ ，则B点沿BC杆速度可表示为：

$\mathbf{BC}\cdot(\dot{\theta}\mathbf{s}\times\mathbf{AB})=\mathbf{s}\cdot(\mathbf{AB}\times\mathbf{BC})\dot{\theta}=J_{\theta}\dot{\theta}$

对于两个支链：

$\begin{bmatrix} {J_\theta}_1 &0\\ 0&{J_\theta}_2 \end{bmatrix}\begin{bmatrix} \dot{\theta_1}\\ \dot{\theta_2} \end{bmatrix}=\mathbf{J_\theta}\begin{bmatrix} \dot{\theta_1}\\ \dot{\theta_2} \end{bmatrix}$

代码中 $\mathbf{J_\theta}$ 的计算：

2.2 从脚部计算沿杆速度

设脚部构件原点的速度为 $\begin{bmatrix} \mathbf{v} \\ \mathbf{\omega} \end{bmatrix}$ ，则C点沿BC杆速度可表示为：

$\mathbf{BC}\cdot(\mathbf{v} +\mathbf{\omega}\times\mathbf{C})=\mathbf{BC}\cdot\mathbf{v}+(\mathbf{C}\times\mathbf{BC})\mathbf\cdot{\omega}=\begin{bmatrix} \mathbf{BC}^T&(\mathbf{C}\times\mathbf{BC})^T \end{bmatrix}\begin{bmatrix} \mathbf{v}\\ \mathbf{\omega} \end{bmatrix}=J_x\begin{bmatrix} \mathbf{v}\\ \mathbf{\omega} \end{bmatrix}$

对于两个支链：

$\begin{bmatrix} {J_x}_1 \\ {J_x}_2 \end{bmatrix}\begin{bmatrix} \mathbf{v}\\ \mathbf{\omega} \end{bmatrix}=\mathbf{J_x}\begin{bmatrix} \mathbf{v}\\ \mathbf{\omega} \end{bmatrix}$

代码中 $\mathbf{J_x}$ 的计算

3. 计算雅克比矩阵

由沿杆速度相等，以及欧拉角速度和角速度关系：

$\mathbf{J_\theta}\begin{bmatrix} \dot{\theta_1}\\ \dot{\theta_2} \end{bmatrix}=\mathbf{J_x}\begin{bmatrix} \mathbf{v}\\ \mathbf{\omega} \end{bmatrix}=\mathbf{J_x}\mathbf{J_q}\begin{bmatrix} \dot{\theta_r}\\ \dot{\theta_p} \end{bmatrix}$

因为：

$\begin{bmatrix} \dot{\theta_r}\\ \dot{\theta_p} \end{bmatrix}=\mathbf{J}\begin{bmatrix} \dot{\theta_1}\\ \dot{\theta_2} \end{bmatrix}$

所以：

$\mathbf{J}=(\mathbf{J_x}\mathbf{J_q})^{-1}\mathbf{J_\theta}$

代码中计算了 $\mathbf{J}$ 和 $\mathbf{J}^{-1}$ ：

4. 力雅克比矩阵

上面探讨的是速度之间的关系，下面讨论电机输入力 $\mathbf{F}$ 与脚部力 $\mathbf{\Tau}$ 的关系。

由虚功原理可得：

$\mathbf{F}^T\begin{bmatrix} \dot{\theta_1}\\ \dot{\theta_2} \end{bmatrix}=\mathbf{\Tau}^T\begin{bmatrix} \dot{\theta_r}\\ \dot{\theta_p} \end{bmatrix}$ $\mathbf{F}^T\begin{bmatrix} \dot{\theta_1}\\ \dot{\theta_2} \end{bmatrix}=\mathbf{\Tau}^T\mathbf{J}\begin{bmatrix} \dot{\theta_1}\\ \dot{\theta_2} \end{bmatrix}$ $\mathbf{F}^T =\mathbf{\Tau}^T\mathbf{J}$

两边转置：

$\mathbf{F}=\mathbf{J}^T\mathbf{\Tau}$ $\mathbf{\Tau}=(\mathbf{J}^{-1})^T\mathbf{F}$

因此：

$\mathbf{J_\tau}=(\mathbf{J}^{-1})^T$

四、正向运动学

大部分并联机构的正向运动学没有解析解，一般采用数值迭代的方式进行计算。最常见的迭代算法是Newton-Raphson算法（一种梯度下降法），众擎采用的就是这个方法。

1. Newton-Raphson迭代法

Newton-Raphson 迭代法具体迭代过程如下所述：

输入电机角度 $\mathbf{\Theta}=\begin{bmatrix} \theta_1\\ \theta_2\end{bmatrix}$ ，初始脚部欧拉角 $\mathbf{\Omega}_0=\begin{bmatrix} {\theta_r}_0\\ {\theta_p}_0\end{bmatrix}$ ，误差极限ER，最大迭代次数m
计算 $\mathbf{\Omega}_{k}$ 下的逆解 $\mathbf{\Theta_k}$ 和雅克比矩阵 $\mathbf{J}$

计算 $\Delta\mathbf{\Theta} = \mathbf{\Theta_k}-\mathbf{\Theta}$ ， $\mathbf{\Omega}_{k+1}=\mathbf{\Omega}_{k}+\mathbf{J}\Delta\mathbf{\Theta}$
若 $\Delta\mathbf{\Theta}$ 的范数小于ER，则迭代结束，正解为 $\mathbf{\Omega}_k$ ；否则，跳至第二步；
检查迭代次数，若其超过m ，则输出迭代过程不收敛

2. 代码

这个方法的收敛性有待验证，但是好在实际应用过程中基本上用不到运动学正解。

五、实际部署

由于URDF无法表示并联机构，因此一般直接将关节放在脚部进行简化，即训练时使用 $\begin{bmatrix} \theta_r\\ \theta_p \end{bmatrix}$ 代替 $\begin{bmatrix} \theta_1\\ \theta_2 \end{bmatrix}$ 。但是在实际部署时，实际控制的是电机 $\begin{bmatrix} \theta_1\\ \theta_2 \end{bmatrix}$ 。

Image Token: R8NObZyyYoR8qzxHw9lcEnyan2c