控制算法设计

从数学角度来看，最优化方法是一种求极值的方法，即在一组约束为等式或不等式的条件下，使系统的目标函数达到极值，即最大值或最小值。从工程角度，最优控制是在可行条件下寻求最佳控制策略，以使控制系统能够最优地到达目标。

轮足机器人的建模

最优控制策略的构建离不开对机器人的建模。只有充分分析机器人的物理模型，才能更好地理解机器人系统的性能，以及如何优化机器人控制器的设计系统。

平衡与纵向运动控制

对于轮足机器人的平衡与纵向运动控制，主要关注机器人机体姿态、腿部姿态和驱动轮的运动。称机器人中机体与腿部机构转轴、腿部机构与驱动轮轴之间的连线为摆杆，得到如图1所示的轮足倒立摆模型，模型中变量与参数的定义如表1和表2所示。

图1. 轮足倒立摆模型
表1. 轮足倒立摆模型变量定义

符号	含义	正方向	单位
$\theta$	摆杆与竖直方向夹角	图示为正方向	rad
$\alpha$	摆杆与机体相对角度	同$\theta$	rad
$\phi$	机体与水平夹角	图示为正方向	rad
$x_c$	驱动轮位移	箭头所示	m
$x$	膝部结构转轴位移	同$x_c$	m
$T$	驱动轮输出力矩	同$\theta$	Nm
$T_p$	膝关节输出力矩	同$\alpha$	Nm
$N$	驱动轮对摆杆力的水平分量	箭头所示	N
$P$	驱动轮对摆杆力的竖直分量	箭头所示	N
$N_f$	地面对驱动轮的摩擦力	箭头所示	N
$N_M$	摆杆对机体水平方向分量	箭头所示	N
$P_M$	摆杆对机体竖直方向分量	箭头所示	N
$F$	摆杆推力	向外为正	N

表2. 轮足倒立摆模型参数定义

符号	单位	含义
$R$	m	驱动轮半径
$L$	m	摆杆重心到驱动轮轴距离
$L_M$	m	摆杆重心到机体转轴距离
$l$	m	机体重心到其转轴距离
$m_w$	kg	驱动轮转子质量
$m_p$	kg	摆杆质量
$M$	kg	机体质量
$I_w$	Kg·m²	驱动轮转子转动惯量
$I_p$	Kg·m²	摆杆质量转动惯量
$I_M$	Kg·m²	机体绕转轴转动惯量

对驱动轮分析，有：

$$\begin{equation} {m_w}\ddot x = {N_f} - N \end{equation}$$ $$\begin{equation} {I_w}\frac{{\ddot x}}{R} = T - {N_f}R \end{equation}$$

消去 $N_f$ 可以得到：

$$\begin{equation} \ddot x = \frac{{T - NR}}{{\frac{{{I_w}}}{R} + {m_w}R}} \end{equation}$$

针对摆杆进行分析，有：

$$\begin{equation} N - {N_M} = {m_p}\frac{{{\partial ^2}}}{{\partial {t^2}}}(x + L\sin \theta ) \end{equation}$$ $$\begin{equation} P - {P_M} - {m_p}g = {m_p}\frac{{{\partial ^2}}}{{\partial {t^2}}}(L\cos \theta ) \end{equation}$$ $$\begin{equation} {I_p}\ddot \theta = \left( {PL + {P_M}{L_M}} \right)\sin \theta - \left( {NL + {N_M}{L_M}} \right)\cos \theta - T + {T_p} \end{equation}$$

对机体进行分析，有：

$$\begin{equation} {N_M} = M\frac{{{\partial ^2}}}{{\partial {t^2}}}\left( {x + \left( {L + {L_M}} \right)\sin \theta - l\sin \phi } \right) \end{equation}$$ $$\begin{equation} {P_M} - Mg = M\frac{{{\partial ^2}}}{{\partial {t^2}}}\left( {\left( {L + {L_M}} \right)\cos \theta + l\cos \phi } \right) \end{equation}$$ $$\begin{equation} {I_M}\ddot \phi = {T_p} + {N_M}l\cos \phi + {P_M}l\sin \phi \end{equation}$$

定义状态空间模型中的状态向量 $\textbf{x}$ 为：

$$\textbf{x} = {\begin{bmatrix} \theta \\ {\dot \theta }\\ x\\ {\dot x}\\ \phi \\ {\dot \phi } \end{bmatrix}}$$

控制向量 $\textbf{u}$ 为：

$$\textbf{u} = {\begin{bmatrix} T\\ {{T_p}} \end{bmatrix}}$$

定义系统的非线性模型：

$$\dot{\textbf{x}} = f(\textbf{x},\textbf{u})$$

由式(3) ~ (9)，可以得到系统非线性模型的符号表达式。根据状态向量和控制向量，在非线性模型的平衡点处使用雅可比矩阵线性化，可以得到：

$$A = \frac{{\partial f}}{{\partial {\textbf{x}}}}(\bar {\textbf{x}},\bar {\textbf{u}}),B = \frac{{\partial f}}{{\partial {\textbf{u}}}}(\bar {\textbf{x}},\bar {\textbf{u}})$$

其中 $\bar {\textbf{x}},\bar {\textbf{u}}$ 为系统的平衡点，即方程 $f\left(\bar {\textbf{x}},\bar {\textbf{u}}\right)=0$ 的解：

$$\bar x = {\begin{bmatrix} 0\\ 0\\ x\\ 0\\ 0\\ 0 \end{bmatrix}},\bar u = {\begin{bmatrix} 0\\ 0 \end{bmatrix}}$$

有：

$$\dot {\textbf{x}} = {\begin{bmatrix} 0&1&0&0&0&0\\ {{A_1}}&0&0&0&{{A_2}}&0\\ 0&0&0&1&0&0\\ {{A_3}}&0&0&0&{{A_4}}&0\\ 0&0&0&0&0&1\\ {{A_5}}&0&0&0&{{A_6}}&0 \end{bmatrix}} \textbf{x} + {\begin{bmatrix} 0&0\\ {{B_1}}&{{B_2}}\\ 0&0\\ {{B_3}}&{{B_4}}\\ 0&0\\ {{B_5}}&{{B_6}} \end{bmatrix}}\textbf{u}$$

所有的状态变量均可以通过数据融合解算或测量得到结果，此时系统输出 $\textbf{y}$ 为：

$$\textbf{y} = {I_6}\textbf{x}$$

其中，${I_6}$ 为六维的单位矩阵。通过将参数带入模型中，确定状态空间模型状态矩阵 $A$ 和控制矩阵 $B$ ，不难看出矩阵满秩，即系统可控。

轮足机器人的控制器设计

LQR

根据上文建立的轮足机器人的系统模型，设计控制律为系统状态的线性组合，则：

$$\textbf{u} = - K\textbf{x} = - {\begin{bmatrix} {{K_{11}}}&{{K_{12}}}&{{K_{13}}}&{{K_{14}}}&{{K_{15}}}&{{K_{16}}}\\ {{K_{21}}}&{{K_{22}}}&{{K_{23}}}&{{K_{24}}}&{{K_{25}}}&{{K_{26}}} \end{bmatrix}} {\begin{bmatrix} {\dot \theta }\\ {\dot \theta }\\ x\\ {\dot x}\\ \phi \\ {\dot \phi } \end{bmatrix}}$$

使用线性二次型调节器（LQR）计算增益矩阵 $K$，定义代价函数 $J$ 为：

$$J = \int_0^\infty {\left( {{x^T}\textbf{Q}x + {u^T}\textbf{R}u} \right)} {\rm{ d}}t$$

为了使代价函数具有最小值，则输入 $\textbf{u}$ 应满足：

$$\textbf{u} = - \textbf{R}^{-1} \textbf{B}^T \textbf{Px}$$

即增益矩阵 $\textbf{K}$ 应满足：

$$\textbf{K} = \textbf{R}^{-1} \textbf{B}^T \textbf P$$

其中P满足代数黎卡提方程：

$$\textbf{A}^T \textbf P + \textbf P \textbf A - \textbf P \textbf B \textbf{R}^{ - 1} \textbf{B}^T \textbf P + \textbf Q = 0$$

通过上述方法可以使系统在线性化平衡点附近稳定，为了使机器人具备跟踪轨迹的功能，还需要在系统输入中加入参考输入，即：

$$\textbf u = \textbf {K} (\textbf{x}d - \textbf x)$$

其中 $\textbf{x}_d$ 由机器人的期望位移 $\tilde {\textbf{x}}$ 构成：

$$\textbf{x}_d = {\begin{bmatrix} 0\\ 0\\ \tilde {\textbf{x}}\\ 0\\ 0\\ 0 \end{bmatrix}}$$

在建立轮足机器人的物理模型时，没有考虑到腿长的变化对系统带来的影响。因此为了考虑在不同腿长下的工况，定义一定的腿长区间，在腿长区间内每隔10mm对系统模型进行一次线性化，并求解LQR的增益矩阵 $\textbf{K}$。对于增益矩阵中的每一个元素 $\textbf{K}_{ij}$ 随腿长 $L_0=L+L_M$ 的变化拟合多项式方程可以得到：

$${K_{ij}}\left( {{L_0}} \right) = {p_{0|ij}} + {p_{1|ij}}{L_0} + {p_{2|ij}}L_0^2 + {p_{3|ij}}L_0^3$$

综上所述，机器人的纵向运动控制律为：

$$\textbf u = \textbf K ({L_0})(\textbf{x}_d - \textbf x)$$

VMC

图2. 五连杆机构参数定义图

五连杆机构参数定义如图所示，其中 $A$，$E$ 两转动副由电机驱动，其角度 $\phi_1$,$\phi_4$ 可通过电机编码器测得。控制任务中主要关注五连杆机构末端 C点的位置，通常可用直角坐标 $(x,y)$ 或极坐标 $(L_0,\phi_0)$ 表示。结合图3-2，可以对末端C点列出如下等式：

$$\left\{ \begin{array}{l} {x_B} + {l_2}\cos {\phi _2} = {x_D} + {l_3}\cos {\phi _3}\\ {y_B} + {l_2}\sin {\phi _2} = {y_D} + {l_3}\sin {\phi _3} \end{array} \right.$$

求解方程组可以得到角度 $\phi_2$ ：

$${\phi _2} = 2\arctan \left( {\frac{{{B_0} + \sqrt[{}]{{A_0^2 + B_0^2 - C_0^2}}}}{{{A_0} + {C_0}}}} \right)$$

其中：

$$\begin{array}{l} {A_0} = 2{l_2}({x_D} - {x_B})\\ {B_0} = 2{l_2}({y_D} - {y_B})\\ {C_0} = l_2^2 + l_{BD}^2 - l_3^2\\ {l_{BD}} = \sqrt {{{({x_D} - {x_B})}^2} + {{({y_D} - {y_B})}^2}} \end{array}$$

通过 $\phi_2$ 可以计算出C点的直角坐标：

$$\left\{ \begin{array}{l} {x_C} = {l_1}\cos {\phi _1} + {l_2}\cos {\phi _2}\\ {y_C} = {l_1}\sin {\phi _1} + {l_2}\sin {\phi _2} \end{array} \right.$$

进而得到极坐标：

$$\left\{ \begin{array}{l} {L_0} = \sqrt {{{({x_C} - {l_5}/2)}^2} + y_C^2} \\ {\phi _0} = \arctan \frac{{{y_C}}}{{{x_C} - {l_5}/2}} \end{array} \right.$$

VMC（Virtual model control，虚拟模型控制）是一种描述交互行为的方法，利用虚拟的机械构件产生的虚拟力来获得真实的执行器的扭矩或力，这些关节扭矩产生的效果与虚拟力的效果相同，均驱使机器人按照期望控制量运动。VMC方法是直觉控制方法中的典型代表，其特点是不需要进行机器人运动学的逆解，极大地提高了计算效率和控制效率。

在闭链五连杆问题中，我们需要获得五连杆机构末端的推力 $F$ 和沿中心轴的力矩 $T_p$ 与 $A$，$E$ 两转动副的关节电机扭矩 $T_1$ 和 $T_2$ 的映射关系。定义 $\textbf{x}=\left[L_0\ \phi_0\right]^T$, $\textbf{q}=\left[\phi_1\ \phi_4\right]^T$ ，对正运动学模型 $\textbf{x}=f\left(\textbf{q}\right)$ 求全微分，有：

$$\begin{equation} \left\{\begin{matrix} {\delta {L_0} = \frac{{\partial {f_1}}}{{\partial {\phi _1}}}\delta {\phi _1} + \frac{{\partial {f_1}}}{{\partial {\phi _4}}}\delta {\phi _4}} \\ {\delta {\phi _0} = \frac{{\partial {f_2}}}{{\partial {\phi _1}}}\delta {\phi _1} + \frac{{\partial {f_2}}}{{\partial {\phi _4}}}\delta {\phi _4}} \end{matrix}\right. \end{equation}$$

即：

$$\begin{equation} \delta x = {\begin{bmatrix} {\frac{{\partial {f_1}}}{{\partial {\phi _1}}}}&{\frac{{\partial {f_1}}}{{\partial {\phi _4}}}}\\ {\frac{{\partial {f_2}}}{{\partial {\phi _1}}}}&{\frac{{\partial {f_2}}}{{\partial {\phi _4}}}} \end{bmatrix}} \delta q \end{equation}$$

定义雅可比矩阵 $\textbf{J}$ 为：

$${\bf{J}} = \ {\begin{bmatrix} {\frac{{\partial {f_1}}}{{\partial {\phi _1}}}}&{\frac{{\partial {f_1}}}{{\partial {\phi _4}}}}\\ {\frac{{\partial {f_2}}}{{\partial {\phi _1}}}}&{\frac{{\partial {f_2}}}{{\partial {\phi _4}}}} \end{bmatrix}}$$

有：

$$\begin{equation} \delta {\textbf{x}} = \textbf J \delta {\textbf q} \end{equation}$$

式(12)表示将关节速度 $\dot{\textbf{q}}$ 映射为五连杆姿态变化率 $\dot{\textbf{x}}$ 。根据虚功原理，有：

$$\begin{equation} \textbf{T}^T \delta \textbf{q} + {( - \textbf{F})^T}\delta {\textbf{x}} = 0 \end{equation}$$

其中 $\textbf{T}=\left[T_1\ T_2\right]^T$ , $\textbf{F}=\left[F\ T_p\right]^T$ 。将式(12)带入式(13)，有：

$$\begin{equation} \textbf T = \textbf{J}^T \textbf F \end{equation}$$

即可通过正运动学模型雅可比矩阵求解关节电机的输出力矩。

但通过此种方法推导出的正运动学模型中存在大量的平方和三角函数嵌套运算，直接求解雅可比矩阵 $\textbf{J}$ 会出现嵌入式平台难以运算的问题。因此，利用简单的理论力学静力学解算知识也可以得到与VMC相同的结果，且表达式较为直接，不需要大量的矩阵运算：

$$\begin{equation} F = \frac{{{T_2}\cos \left( {{\varphi _0} - {\varphi _3}} \right)}}{{{l_4}\sin \left( {{\varphi _3} - {\varphi _4}} \right)}} - \frac{{{T_1}\cos \left( {{\varphi _0} - {\varphi _2}} \right)}}{{{l_1}\sin \left( {{\varphi _1} - {\varphi _2}} \right)}} \end{equation}$$ $$\begin{equation} T = {L_0}\left( {\frac{{{T_1}\cos \left( {{\varphi _0} - {\varphi _2}} \right)}}{{{l_1}\sin \left( {{\varphi _1} - {\varphi _2}} \right)}} - \frac{{{T_2}\sin \left( {{\varphi _0} - {\varphi _3}} \right)}}{{{l_4}\sin \left( {{\varphi _3} - {\varphi _4}} \right)}}} \right) \end{equation}$$

整理可以得到最终的关节力矩与虚拟力的映射关系：

$$\begin{equation} {T_1} = - \frac{{{L_1}\sin \left( {{\varphi _1} - {\varphi _2}} \right)\left( {T\cos \left( {{\varphi _0} - {\varphi _3}} \right) + F{L_0}\sin \left( {{\varphi _0} - {\varphi _3}} \right)} \right.}}{{{L_0}\sin \left( {{\varphi _2} - {\varphi _3}} \right)}} \end{equation}$$ $$\begin{equation} {T_2} = - \frac{{{L_4}\sin \left( {{\varphi _3} - {\varphi _4}} \right)\left( {T\cos \left( {{\varphi _0} - {\varphi _2}} \right) + F{L_0}\sin \left( {{\varphi _0} - {\varphi _2}} \right)} \right.}}{{{L_0}\sin \left( {{\varphi _2} - {\varphi _3}} \right)}} \end{equation}$$

PID转向控制、腿长控制和横滚角补偿

轮足机器人的轮式模型可以认为是两轮差速驱动运动模型（图3），由两个独立驱动的轮子驱动，图3-3中 $\textbf{H}$ 为两个驱动轮的距离，$\textbf{R}$ 为机器人的旋转中心点（ICR）与两轮轴线中心点的距离。在两轮差速运动学模型中，机器人可能做直线运动或圆弧运动。当 ${v}_1=v_2$ 时，机器人做直线运动，此时的ICR在无穷远处，机器人的速度方向为ICR圆的切线方向，即有 $v=v_1=v_2$ 。当 $v_1\ \neq v_2$ 时，存在两种情况：

图3. 两轮差速驱动运动模型

(1) ${ v}_1$ 与 $v_2$ 同号，机器人绕ICR做圆周运动或圆弧运动，有：

$$\begin{equation} \omega = \frac{{{v_2}}}{{R - \frac{H}{2}}} = \frac{{{v_1}}}{{R + \frac{H}{2}}} = \frac{v}{R} \end{equation}$$

(2) ${v}_1$ 与 $v_2$ 异号或其中一个为0时，机器人做绕自身中心的旋转，机器人中处的线速度为0，其角速度为：

$$\begin{equation} \omega = \frac{{2({v_1} - {v_2})}}{H} \end{equation}$$

因此，两轮差速驱动模型的正向运动学为：

$$\begin{equation} \left\{ \begin{array}{l} \omega = \frac{{{v_1} - {v_2}}}{H}\\ v = \omega *R = \frac{{{v_1} + {v_2}}}{2}\\ R = \frac{{H({v_1} + {v_2})}}{{2({v_1} - {v_2})}} \end{array} \right. \end{equation}$$

可以使用PD控制进行简单的转向控制。根据期望航向角速与状态观测器得到的反馈航向角速的误差经过PD控制器得到驱动轮的力矩输出，将输出相反地叠加到纵向运动控制得到的驱动轮力矩控制量中，即可实现转向控制。

为了使腿的长度变化具有类似弹簧的阻尼特性，使用PD控制进行腿长控制。因为轮足机器人的腿部主要承受的是机器人的机体重量，因此可以在PD控制中加入前馈控制，补偿机体的重力。

轮足机器人的主要重量都在机器人的机体上，因此在转弯时，较高的重心会使机器人向外倾斜，表现为机体的横滚角会出现一定的偏移。单纯的腿长控制并不足以克服这种扰动。为此加入横滚角补偿，将横滚角误差作为PID控制器的输入，乘以一定的比例系数，将补偿输出以相反的符号叠加到腿长控制器的输出中，让机器人在转弯或复杂地形运动时保证机体的水平。

本章参考资料

[1] 陈阳, 王洪玺, 张兰勇. 轮腿式平衡机器人控制[J]. 信息与控制, 2023, 52(5): 648-659

[2] 于红英,唐德威,王建宇.平面五杆机构运动学和动力学特性分析[J].哈尔滨工业大学学报,2007(06):940-943.

[3] Jia Q, Cao X, Sun H, et al. A novel design of a two-wheeled robot[C]//2007 2nd IEEE Conference on Industrial Electronics and Applications. IEEE, 2007: 1226-1231.

Last update on: 2025/01/24