串联式轮足机器人控制

LQR 控制器设计

连杆质心拟合

由于在分析和设计控制器的过程中，机器人的腿部连杆需要简化为一个直杆，所以就需要将原来的腿部连杆的转动惯量和质心拟合到虚拟的杆上，如图所示

这里使用机体坐标系，计算可以得到每个杆的坐标位置

Z_{l_1}=\begin{bmatrix}l_{1b}\cos\theta_1\\l_{1b}\sin\theta_1\end{bmatrix}\\Z_{l_2}=\begin{bmatrix}l_1\cos\theta_1+l_{2b}\cos(\theta_2+\theta_1-180\degree)\\l_1\sin\theta_1+l_{2b}\sin(\theta_2+\theta_1-180\degree)\end{bmatrix}

计算其组合体的质心位置

m_1Z_{l_1}+m_2Z_{l_2}=(m_1+m_2)Z_1

之后将组合体的质心映射到虚拟杆上，这里直接使用垂足进行映射，是由于点到直线，垂线最短，从而质心映射拟合时产生的误差会最小，联立如下方程求解

(\frac{Z_{1y}-Z_{ly}}{Z_{1x}-Z_{lx}})(\frac{C_y-A_y}{C_x-A_x})=-1\\\frac{C_y-Z_{ly}}{C_x-Z_{lx}}=\frac{C_y-A_y}{C_x-A_x}

求解即可得到 $Z_l$ 坐标，根据平行轴定理可以得到变换之后的质心处的转动惯量

I_l=I_1+m_1(Z_l-Z_{l_1})^T(Z_l-Z_{l_1})+I_2+m_2(Z_l-Z_{l_2})^T(Z_l-Z_{l_2})

同时也可以计算出所需要的 $L_w$ 和 $L_b$

L_w=\sqrt{(Z_l-C)^T(Z_l-C)}\\L_b=\sqrt{(Z_l-A)^T(Z_l-A)}

腿部连杆运动学正解

如图所示，首先基于机体坐标系 $x_b-y_b$ 来计算各点的坐标，腿长 $L$ 和腿部摆角角度 $\theta$

A_b=\begin{bmatrix}0\\0\end{bmatrix}\\B_b=\begin{bmatrix}l_1\cos\theta_1\\l_1\sin\theta_1\end{bmatrix}\\C_b=\begin{bmatrix}l_1\cos\theta_1+l_2\cos(\theta_2+\theta_1-180\degree)\\l_1\sin\theta_1+l_2\sin(\theta_2+\theta_1-180\degree)\end{bmatrix}

利用旋转矩阵将坐标点 $C_b$ 旋转到坐标系 $x-y$ 上，最终计算出来的 $C$ 如下

C=\begin{bmatrix}\cos\varphi&\sin\varphi\\-\sin\varphi&\cos\varphi\end{bmatrix}C_b

计算腿长 $L$ 和腿部摆动角度 $\theta$ 如下

L=\sqrt{C_x^2+C_y^2}\\\theta=\arctan(-\frac{C_x}{C_y})

解算得到

L=\sqrt{l_1^2-2\cos(\theta_2)l_1l_2+l_2^2}\\\theta=\arctan\Big(\frac{l_2\cos(\theta_1-\varphi+\theta_2)-l_1\cos(\varphi-\theta_1)}{-l_2\sin(\theta_1-\varphi+\theta_2)-l_1\sin(\varphi-\theta_1)}\Big)

在具体使用中还需要得到它们的导数，化简之后得到如下的公式

\dot{L}=\frac{l_1l_2\sin(\theta_2)\dot{\theta}_2}{L}\\\dot{\theta}=\frac{(l_2^2-l_1l_2\cos(\theta_2))\dot{\theta}_2}{L^2}-\dot{\varphi}+\dot{\theta}_1

VMC

此处采用机体坐标系即 $x_b-y_b$ ，根据上述所计算的 $C_b$ 点计算腿长参数和腿部摆动角 $\theta_b$ ，如下

L_b=\sqrt{C_{bx}^2+C_{by}^2}\\\theta_b=\arctan\frac{C_{by}}{C_{bx}}

计算其对于 $\theta_1$ 和 $\theta_2$ 的雅各比矩阵

\begin{bmatrix}L_b\\\theta_b\end{bmatrix}=J\begin{bmatrix}\theta_1\\\theta_2\end{bmatrix}

但是此处求解得到的雅各比矩阵 $J$ 十分复杂，所以将极坐标转换为平行坐标来计算

\begin{bmatrix}F_x\\F_y\end{bmatrix}=\begin{bmatrix}\cos\theta_b&-\frac{\sin\theta_b}{L_b}\\\sin\theta_b&\frac{\cos\theta_b}{L_b}\end{bmatrix}\begin{bmatrix}F\\T_b\end{bmatrix}

可以将 $C$ 点坐标进行求解雅各比矩阵，得到

\begin{bmatrix}\dot{C}_{bx}\\\dot{C}_{by}\end{bmatrix}=\begin{bmatrix}l_2\sin(\theta_1+\theta_2)-l_1\sin\theta_1&l_2\sin(\theta_1+\theta_2)\\l_1\cos\theta_1-l_2\cos(\theta_1+\theta_2)&-l_2\cos(\theta_1+\theta_2)\end{bmatrix}\begin{bmatrix}\dot{\theta}_{1}\\\dot{\theta}_{2}\end{bmatrix}=J\begin{bmatrix}\dot{\theta}_{1}\\\dot{\theta}_{2}\end{bmatrix}

由于速度雅可比矩阵是力矩雅各比矩阵的转置，则有

\begin{bmatrix}T_1\\T_2\end{bmatrix}=J^T\begin{bmatrix}F_x\\F_y\end{bmatrix}=J^T\begin{bmatrix}\cos\theta_b&-\frac{\sin\theta_b}{L_b}\\\sin\theta_b&\frac{\cos\theta_b}{L_b}\end{bmatrix}\begin{bmatrix}F\\T_b\end{bmatrix}

最终化简之后得到

\begin{bmatrix}T_1\\T_2\end{bmatrix}=\begin{bmatrix}l_2\sin(\theta_1+\theta_2-\theta_b)-l_1\sin(\theta_1-\theta_b)&\frac{l_1\cos(\theta_1-\theta_b)-l_2\cos(\theta_1+\theta_2-\theta_b)}{L_b}\\l_2\sin(\theta_1+\theta_2-\theta_b)&-\frac{l_2\cos(\theta_1+\theta_2-\theta_b)}{L_b}\end{bmatrix}\begin{bmatrix}F\\T_b\end{bmatrix}

反向 VMC 如下

\begin{bmatrix}F\\T_b\end{bmatrix}=\begin{bmatrix}\frac{\cos(\theta_1+\theta_2-\theta_b)}{l_1\sin(\theta_2)}&\frac{l_1\cos(\theta_1-\theta_b)-l_2\cos(\theta_1+\theta_2-\theta_b)}{l_1l_2\sin(\theta_2)}\\\frac{L_b\sin(\theta_1+\theta_2-\theta_b)}{l_1\sin(\theta_2)}&\frac{L_bl_1\cos(\theta_1-\theta_b)-L_bl_2\sin(\theta_1+\theta_2-\theta_b)}{l_1l_2\sin(\theta_2)}\end{bmatrix}\begin{bmatrix}T_1\\T_2\end{bmatrix}

腿部摆角补偿

由于串联轮腿的腿部质量并不是前后大致均匀分布的，所以需要加一个补偿，可以是 $\theta$ 的补偿，也可以是机体俯仰角 $pitch$ 的补偿，但是如果机体质心位置位于转动关节的上的话，就需要使用 $\theta$ 补偿，另外 $pitch$ 角的补偿导致上层平台并不能保证水平，所以这里只使用 $\theta$ 补偿