Inertial Wheel Pendulum based on Linear ADRC

Self-balance Control of Bicycle with Inertial Wheel Pendulum based on Linear ADRC

https://ieeexplore.ieee.org/document/10055063

1. 论文细节

1.1 摘要

摘要：本文研究了带惯性轮摆（IWP）的无人自行车在外部干扰和模型不完整情况下的自平衡控制问题。通过拉格朗日方法将无人自行车系统简化为倒立摆模型进行建模。为实现无人自行车在垂直方向上的稳定性并完成自平衡控制，提出了一种基于线性自抗扰控制（LADRC）的方法，并与PID控制器进行对比。为验证所提方法的性能，在具备真实物理特性仿真能力的ROS-Gazebo平台上进行实验，同时与比例-积分-微分（PID）控制器开展对比仿真。结果表明：基于LADRC控制器的无人自行车系统具有更优的自平衡性能，且对外部干扰和建模误差具有强鲁棒性。

关键词：线性自抗扰控制；自平衡控制；无人自行车；惯性轮摆；ROS

1.2 数学建模

假设自行车的质量为 $m_1$ ，质心为 $o$ ，自行车旋转产生的转动惯量为 $I_1$ ，自行车与垂直方向的夹角为 $\theta$ ，坐标原点 $o_1$ 到质心 $o$ 的距离为 $L_1$ ；设飞轮的质量为 $m_2$ ，飞轮的质心为 $o_2$ ，绕其质心的转动惯量为 $I_2$ ，飞轮的旋转角度为 $\phi$ ，原点 $o_1$ 到飞轮质心 $o_2$ 的距离为 $L_2$ 。然后，利用以下方程推导出系统的拉格朗日动力学方程：

\frac{d}{d t}\left(\frac{\partial L}{\partial \dot{q}}\right)-\frac{\partial L}{\partial q}=Q \tag{1}

其中 $L$ 为拉格朗日算子， $q$ 为广义坐标， $Q$ 为对应于该广义坐标的广义力。

L(q, \dot{q})=T-V . \tag{2}

拉格朗日算子 $L$ 由系统的总动能减去系统的总势能得到。本文选取 $\theta$ 和 $\phi$ 作为广义坐标，其对应的广义力分别为 $0$ 和 $\tau$ 。

系统的总动能由自行车动能与飞轮动能之和构成：

T=T_1+T_2 \tag{3}

即

T=\frac{1}{2}\left(m_1 L_1^2+m_2 L_2^2+I_1+I_2\right) \dot{\theta}^2+I_2 \dot{\theta} \dot{\phi}+\frac{1}{2} I_2 \dot{\phi}^2 . \tag{4}

系统的总势能可表示为：

V=\left(m_1 L_1+m_2 L_2\right) g \cos \theta \tag{5}

那么拉格朗日算子为：

\begin{array}{r} L=\frac{1}{2}\left(m_1 L_1^2+m_2 L_2^2+I_1+I_2\right) \dot{\theta}^2+I_2 \dot{\theta} \dot{\phi}+\frac{1}{2} I_2 \dot{\phi}^2- \\ \left(m_1 L_1+m_2 L_2\right) g \cos \theta . \tag{6} \end{array}

代入式(1)中的拉格朗日方程，推导出以下两个方程：

\left\{\begin{array}{r} \left(m_1 L_1^2+m_2 L_2^2+I_1+I_2\right) \ddot{\theta}+I_2 \ddot{\phi}- \\ \left(m_1 L_1+m_2 L_2\right) g \sin \theta=0 \\ I_2(\ddot{\theta}+\ddot{\phi})=\tau \end{array}\right. \tag{7}

上式是IWP系统的非线性数学模型表达式。为简化分析和计算，考虑在 $\theta=0$ 附近对系统进行线性化，表示为：

\left\{\begin{array}{r} \left(m_1 L_1^2+m_2 L_2^2+I_1+I_2\right) \ddot{\theta}+I_2 \ddot{\phi}- \\ \left(m_1 L_1+m_2 L_2\right) g \theta=0 \\ I_2(\ddot{\theta}+\ddot{\phi})=\tau \end{array}\right. \tag{8}

对式(8)进行参数化处理，得到以下两个方程：

\ddot{\theta} = \frac{(m_1 L_1 + m_2 L_2) g}{m_1 L_1^2 + m_2 L_2^2 + I_1} \theta - \frac{1}{m_1 L_1^2 + m_2 L_2^2 + I_1} \tau \tag{9}

\ddot{\phi} = -\frac{(m_1 L_1 + m_2 L_2) g}{m_1 L_1^2 + m_2 L_2^2 + I_1} \theta + \frac{m_1 L_1^2 + m_2 L_2^2 + I_1 + I_2}{(m_1 L_1^2 + m_2 L_2^2 + I_1) I_2} \tau \tag{10}

选择 $x=\left[x_1, x_2, x_3, x_4\right]^{\top}=[\theta, \dot{\theta}, \phi, \dot{\phi}]^{\top}$ 作为系统的状态变量，系统的状态空间方程如下所示：

\begin{bmatrix} \dot{\theta} \\ \ddot{\theta} \\ \dot{\phi} \\ \ddot{\phi} \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ \frac{(m_1 L_1 + m_2 L_2) g}{m_1 L_1^2 + m_2 L_2^2 + I_1} & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ -\frac{(m_1 L_1 + m_2 L_2) g}{m_1 L_1^2 + m_2 L_2^2 + I_1} & 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} \theta \\ \dot{\theta} \\ \phi \\ \dot{\phi} \end{bmatrix} + \begin{bmatrix} 0 \\ -\frac{1}{m_1 L_1^2 + m_2 L_2^2 + I_1} \\ 0 \\ \frac{m_1 L_1^2 + m_2 L_2^2 + I_1 + I_2}{(m_1 L_1^2 + m_2 L_2^2 + I_1) I_2} \end{bmatrix} \tau \tag{11}

IWP系统的参数与ROS-Gazebo环境中自行车的参数一致。自行车整体包括前轮、后轮、车身和把手，具体参数如表I所示：

参数	数值
自行车重心垂直高度（ $L_1$ ）	0.7 m
质量飞轮重心垂直高度（ $L_2$ ）	0.9 m
自行车总质量（ $m_1$ ）	5.54 kg
飞轮质量（ $m_2$ ）	5.35 kg
自行车转动惯量（ $I_1$ ）	2.7146 kg·m²
质量飞轮转动惯量（ $I_2$ ）	0.03 kg·m²
重力加速度（ $g$ ）	9.81 m/s²

1.3 自平衡自行车系统的LADRC控制器设计

线性自抗扰控制（LADRC）基于自抗扰控制理论，包含PD控制和线性扩张状态观测器（LESO）。本文利用Python语言在ROS中实现LADRC控制算法，需对LESO进行离散化处理。

1.3.1 A. LESO的离散化

对于带质量飞轮的无人自行车系统，自平衡问题可简化为车身倾角控制问题，因此系统可表示为：
$\ddot{\theta}=f+b_0 u$ ，
其中 $\theta$ 为车身倾角， $f$ 为包含系统内外扰动的总扰动。选取状态变量：
$x_1=\theta$ ， $x_2=\dot{\theta}$ ， $x_3=f$ ，
则 $x=\left[\begin{array}{ccc}\theta & \dot{\theta} & f\end{array}\right]^{\top}$ 为带扰动的扩张状态，连续型扩张状态观测器可表示为：

\left\{\begin{array}{l} \dot{x}=A x+B u+E \dot{f} \\ y=C \mathrm{x} \end{array}\right. \tag{12}

其中，
$A=\left[\begin{array}{lll}0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0\end{array}\right]$ ，
$B=\left[\begin{array}{c}0 \\ b_0 \\ 0\end{array}\right]$ ，
$E=\left[\begin{array}{l}0 \\ 0 \\ 1\end{array}\right]$ ，
$C=\left[\begin{array}{lll}1 & 0 & 0\end{array}\right]$ 。

由于 $\dot{f}$ 未知但可估计，可将其忽略。对应的连续型LESO为：

\left\{\begin{array}{l} \dot{z}=A z+B u+R(y-\hat{y}) \\ \hat{y}=C z \end{array}\right. \tag{13}

其中， $z=\left[\begin{array}{lll}z_1 & z_2 & z_3\end{array}\right]^{\top}$ 为观测器的状态向量， $R$ 为观测器误差反馈增益矩阵。整理观测器方程可得：

\left\{\begin{array}{l} \dot{z}=[A-R C] z+B u+R y \\ y_c=\mathrm{z} \end{array}\right. \tag{14}

联立式(12)和式(14)，经参数化后可得到：

\dot{x}-\dot{z}=(A-R C)(x-z) \tag{15}

为使观测器能良好观测上述系统的状态，期望观测误差为0时，可将特征方程的极点配置在相同位置 $-\omega_o$ ， $\omega_o$ 为观测器带宽。因此取观测器增益矩阵 $R=\left[\begin{array}{lll}3 \omega_o & 3 \omega_o{ }^2 & \omega_o{ }^3\end{array}\right]^{\top}$ ，使得 $\lambda(s)=|s I-(A-R C)|=\left(s+\omega_o\right)^3$ 。此时观测器增益矩阵与观测器带宽唯一相关，使得连续型LESO的设计变得简便。

最终设计的系统LESO为：

\dot{z}=\left[\begin{array}{ccc} -3 \omega_o & 1 & 0 \\ -3 \omega_o{ }^2 & 0 & 1 \\ -\omega_o{ }^3 & 0 & 0 \end{array}\right] z+\left[\begin{array}{c} 0 \\ b_0 \\ 0 \end{array}\right] u+\left[\begin{array}{c} 3 \omega_o \\ 3 \omega_o{ }^2 \\ \omega_o{ }^3 \end{array}\right] y . \tag{16}

由式(16)可得：

\left\{\begin{array}{l} \dot{z_1}=-3 \omega_o z_1+z_2+3 \omega_o y \\ \dot{z_2}=-3 \omega_o^2 z_1+z_3+b_0 u+3 \omega_o^2 y \\ \dot{z_3}=-\omega_o^3 z_1+\omega_o^3 y \end{array}\right. \tag{17}

式中 $z_{1-3}$ 表示LESO的状态向量，分别对应输出量 $y$ 、输出量微分 $\dot{y}$ 及总扰动 $f$ 的估计值。

LESO采用前向欧拉法进行离散化，离散化后的LESO如下所示：

\left\{\begin{array}{l} z_1(k+1)=z_1(k)+h\left(z_2(k)+3 \omega_o\left(y(k)-z_1(k)\right)\right), \\ z_2(k+1)=z_2(k)+h\left(z_3(k)+b_0 u(k)+3 \omega_o^2\left(y(k)-z_1(k)\right)\right), \\ z_3(k+1)=z_3(k)+h\left(\omega_o{ }^3\left(y(k)-z_1(k)\right)\right) \end{array}\right. \tag{18}

其中， $h$ 为积分步长。

1.3.2 B. LADRC控制器设计

本文设计的无人自行车自平衡控制系统通过质量飞轮旋转产生的力矩克服自行车重力的倾倒效应。因此，可通过设计控制器使车体倾斜角始终保持在 $0^{\circ}$ 来实现自行车平衡。

无人自行车自平衡LADRC控制器原理如下图所示（图2）：

系统的输出 $y$ 为自行车倾斜角，参考输入 $r$ 为期望倾斜角，控制量 $u$ 作为被控对象输入用于驱动质量飞轮旋转。由于线性扩张状态观测器（LESO）能对外部干扰和内部扰动进行估计补偿，线性状态误差反馈控制律可进一步简化为PD组合形式，以保留PID控制算法的优势。PD控制组合可表示为：

u_0=k_p\left(r-z_1(k+1)\right)-k_d z_2(k+1) \tag{19}

式中： $r$ 为给定期望倾斜角值， $z_1(k+1)$ 和 $z_2(k+1)$ 为离散化LESO的观测器状态， $k_p$ 和 $k_d$ 分别为比例放大因子和微分放大因子。控制律表达式为：

u=\frac{u_0-z_3(k+1)}{b_0} \tag{20}

当LESO理想工作时，系统总扰动 $f$ 可被 $z_3(k+1)$ 完全估计，此时被控对象简化为二重积分型系统：

\ddot{\theta}=f-z_3(k+1)+u_0=u_0 \tag{21}

从而使闭环传递函数成为无零点的纯二阶系统：

G=\frac{k_p}{s^2+k_d s+k_p} \tag{22}

其中 $k_p$ 和 $k_d$ 为待设计的控制器增益，其参数化选取关系为：

k_p=\omega_c^2, \quad k_d=2 \omega_c \tag{23}

$\omega_c$ 为控制器带宽，这使得PD控制器参数与控制器带宽形成唯一关联，从而简化控制器设计过程。

1.4 仿真与结果分析

本文采用ROS-Gazebo虚拟仿真平台验证无人自行车动力学性能及所提出控制方法的有效性。在Gazebo中进行系统仿真的首要步骤是建立无人自行车虚拟模型，如图3所示。

图3. Gazebo环境中的无人自行车模型

该无人自行车虚拟模型通过统一机器人描述文件格式（URDF）进行构建。主体结构包含四个连杆部件：前轮（wheel2）、后轮（wheel1）、车体框架（frame）及转向把手（steering），并配置有质量飞轮（balancer）用于实现无人自行车的自平衡控制。在此虚拟模型中，质量飞轮与车体无需实际接触，但二者之间存在特定的连接关系。本文无人自行车模型的URDF结构如下：

各连杆通过关节相互连接。该模型整体结构包含四个关节，各关节信息如下：

frame_wheel_joint：父关节为车架（frame），子关节为后轮（wheel1），控制后轮旋转以实现无人自行车模型的前进运动。关节类型为连续型（continuous），具有绕单轴无限旋转的特性。

frame_steering_joint：父关节为车架（frame），子关节为转向把手（steering），控制把手转向。关节类型为回转型（revolute），其特点为绕单轴旋转且运动范围受限。由于把手转向存在特定角度限制，该关节元素还包含"limit"（限位）与"dynamics"（动力学参数）属性。

steering_wheel_joint：父关节为转向把手（steering），子关节为前轮（wheel2），控制前轮旋转。关节类型为连续型（continuous），无附加约束。在自行车实际运动中，前轮绕其轴心作为被动关节旋转，不施加主动扭矩。

frame_balancer_joint：父关节为车架（frame），子关节为平衡飞轮（balancer），关节类型为连续型（continuous），用于控制质量飞轮的旋转。质量飞轮的旋转不受约束，可绕单轴连续转动。因此，在控制自行车虚拟模型时，前轮作为被动关节，仅需控制上述三个关节（后轮、转向把手及平衡飞轮）。这些关节属性与元素的设置能使自行车虚拟模型更贴近真实自行车特性。

此外，为实现对各关节的控制，需为每个关节配置相应的控制器类型与硬件接口类型。针对上述模型结构中的各关节，所需控制器类型与硬件接口类型的设置如表II所示。基于此配置，通过在ROS-Gazebo仿真环境中，基于上述配置分别对以下两种自平衡工况进行实验：(1)当系统参考输入变化（即期望倾角改变）时，自行车系统能否跟踪给定倾角；(2)当车把转角受到扰动时，自行车系统能否保持平衡。实验将LADRC算法与PID算法进行对比，经过多次实验比对结果，最终选定LADRC控制器参数为： $\omega_o=2.85$ 、 $\omega_c=1.0$ 、 $b_0=-0.13$ ，PID控制器参数为 $k_p=5.9$ 、 $k_i=0$ 、 $k_d=26.0$ （取消积分项对结果具有更好影响）。

本仿真实验中，自行车转向角上下限设置为 $\pm 30^{\circ}$ ，车把受力为正时向左转向、受力为负时向右转向，同时车把左转时转角为负、右转时转角为正。针对第一种自平衡工况，自行车初始车身倾角为 $0^{\circ}$ ，按 $0^{\circ} \rightarrow-5.7^{\circ} \rightarrow-11.4^{\circ} \rightarrow 0^{\circ} \rightarrow 5.7^{\circ}$ 顺序改变期望车身倾角。实验表明系统能跟踪给定倾角，自行车在特定倾角下仍能保持稳定，且当期望车身倾角为 $0^{\circ}$ 时，系统将恢复直立状态。

两种控制器作用下的自行车车身倾角变化曲线如图5所示，其中图5(a)为PID控制器效果，图5(b)为LADRC控制器效果。

扩张状态观测器(ESO)的跟踪效果如图6所示，其中 $z_1$ 表示ESO输出， $y$ 为系统输出。结果表明ESO能够准确估计系统输出。

仿真结果表明，配备惯性飞轮(IWP)的自行车系统能够将车身姿态维持在特定角度，且当期望倾角为 $0^{\circ}$ 时系统能逐渐恢复直立状态。与PID控制器相比，LADRC控制器具有更小的超调量，调节过程更为平缓且无明显波动。

针对第二种工况，众所周知转动车把会导致自行车失稳。因此将转向角变化视为扰动，基于车身自平衡控制分别进行两组实验来验证其鲁棒性。由于车把转角采用位置控制器，两组实验施加相同扰动：在仿真时间15秒时使车把左转至 $-30^{\circ}$ ，30秒时右转至 $30^{\circ}$ ，40秒时恢复至初始 $0^{\circ}$ 状态。仿真结果如下所示。

图7和图8分别展示了在扰动作用下，采用PID控制器和LADRC控制器时自行车车身倾角与车把转角的变化曲线。

仿真结果表明：当对车把施加扰动时，自行车车身倾角确实会发生显著变化，从而影响自行车的稳定性，但自行车系统在扰动下仍能保持平衡。与PID控制器相比，LADRC控制器波动更小，在扰动作用下能更好地将车身倾角恢复到接近 $0^{\circ}$ ，具有更好的鲁棒性。

1.5 结论

本文基于惯性轮摆原理设计了一种无人自行车自平衡系统。通过建立惯性轮摆的拉格朗日动力学模型，并将其视为自行车系统的简化数学模型，在存在未知扰动和建模误差的情况下，采用LADRC控制方法和PID控制器对无人自行车系统进行控制。在ROS-Gazebo物理仿真环境中实现了带质量飞轮的无人自行车虚拟模型，并在该虚拟模型上进行了仿真实验。仿真结果表明，LADRC控制算法具有更好的控制效果，能够有效抑制扰动。

2. ADRC自抗扰控制器的理论推导与公式整理

这是这个网页https://zhuanlan.zhihu.com/p/671469224 绘制的图，论文里的图没有绘制TD，并且SEF其实就是PD计算：

ADRC (Active Disturbance Rejection Control，自抗扰控制) 是一种先进的控制方法，它由三个主要部分组成：TD (跟踪微分器)、ESO (扩张状态观测器) 和 SEF (状态误差反馈控制律)。下面我将详细整理每个部分的公式和原理。

2.1. ADRC的基本思想

ADRC的核心思想是将复杂系统简化为最基本的控制问题，通过估计并补偿系统中的"总扰动"，无需精确的数学模型即可实现高质量控制。

对于自行车平衡系统，简化后的动态模型为：


ddot(θ) = f + b₀·u

其中：

θ 是自行车的倾斜角（输出）
u 是控制输入（作用在飞轮上的力矩）
b₀ 是输入增益系数（论文中为-0.13）
f 代表系统中所有未知动态和干扰的总和

2.2. ADRC的三个核心组件

2.2.1 TD（跟踪微分器）

TD的作用是对给定的参考信号r进行平滑处理，产生过渡过程，并提供其微分信号。在线性ADRC中，典型的二阶TD公式为：


dot(v₁) = v₂
dot(v₂) = r₀²·(r - v₁) - 2·r₀·v₂

其中：

v₁ 是对参考信号r的跟踪
v₂ 是v₁的微分（即r的近似微分）
r₀ 是跟踪微分器的带宽参数

离散化后：


v₁(k+1) = v₁(k) + h·v₂(k)
v₂(k+1) = v₂(k) + h·(r₀²·(r(k) - v₁(k)) - 2·r₀·v₂(k))

注意: 在论文中的自行车系统实现中，并没有显式使用TD组件，这是因为对于固定的目标值（平衡状态θ=0°）或简单变化的参考轨迹，可以直接使用原始参考信号，而不需要TD的平滑处理。

2.2.2 ESO（扩张状态观测器）

ESO是ADRC的核心部分，用于实时估计系统状态和总扰动。对于二阶系统，线性ESO（LESO）的连续形式为：


dot(z₁) = z₂ + β₁·(y - z₁)
dot(z₂) = z₃ + b₀·u + β₂·(y - z₁)
dot(z₃) = β₃·(y - z₁)

其中：

z₁, z₂, z₃ 分别是对系统输出y、其导数、以及总扰动f的估计
β₁, β₂, β₃ 是观测器增益
y 是系统实际输出
u 是控制输入

极点配置方法是一种控制系统设计技术，它通过合理选择控制器参数，将系统的闭环极点（特征方程的根）放置在期望位置，以获得预期的系统动态性能。

在控制理论中，系统的极点决定了系统的动态响应特性：

极点的实部决定了系统响应的衰减速度
极点的虚部决定了系统响应的振荡频率

在极点配置方法下，观测器增益设置为：


β₁ = 3·ω₀
β₂ = 3·ω₀²
β₃ = ω₀³

其中ω₀是观测器带宽，直接决定了ESO的响应速度。

离散化后的LESO公式（使用前向欧拉法）：


z₁(k+1) = z₁(k) + h·(z₂(k) + 3·ω₀·(y(k) - z₁(k)))
z₂(k+1) = z₂(k) + h·(z₃(k) + b₀·u(k) + 3·ω₀²·(y(k) - z₁(k)))
z₃(k+1) = z₃(k) + h·(ω₀³·(y(k) - z₁(k)))

这里h是采样时间（积分步长）。这些方程直接对应于论文中的式(18)以及代码中的_update_leso方法。

2.2.3 SEF（状态误差反馈控制律）

SEF基于ESO估计的状态，形成对系统的控制。对于二阶系统，线性SEF控制律包括两部分：

误差反馈部分(u₀)：
```
u₀ = k_p·(r - z₁) - k_d·z₂
```
其中：
- r 是目标值（参考输入）
- z₁, z₂ 是ESO估计的系统状态
- k_p, k_d 是控制增益
扰动补偿部分：
```
u = (u₀ - z₃) / b₀
```
其中z₃是ESO估计的总扰动。

对于线性ADRC，控制增益通常设置为：


k_p = ω_c²
k_d = 2·ω_c

其中ω_c是控制器带宽，直接决定了控制器的响应速度。

这些公式对应于论文中的式(19)、(20)和(23)以及代码中的update方法。

2.3. ADRC的参数整定

LADRC的关键参数只有三个：

b₀：系统输入增益（通常基于系统物理特性估计）
ω₀：观测器带宽（决定ESO的响应速度）
ω_c：控制器带宽（决定控制器的响应速度）

一般原则是：

ω₀ > ω_c（观测器应比控制器更快）
增大ω₀可提高观测精度，但也会增加噪声敏感性
增大ω_c可加快系统响应，但也可能导致控制信号过大或不稳定

在论文中，这些参数设置为：

b₀ = -0.13
ω₀ = 2.85
ω_c = 1.0

2.4. ADRC的工作流程

ADRC的工作流程可以概括为：

TD部分：对参考信号进行跟踪和微分（如果需要）
ESO部分：实时估计系统状态和总扰动
SEF部分：
- 基于状态估计，计算误差反馈控制：u₀ = k_p·(r - z₁) - k_d·z₂
- 加入扰动补偿：u = (u₀ - z₃)/b₀
执行：将控制信号u应用到系统

3. 代码

https://github.com/xxddccaa/adrc-python-iwp

目录