Design and Implementation of a Two-Wheeled Self-Balancing Car Using a Fuzzy Kalman Filter
使用模糊卡尔曼滤波器设计和实现两轮自平衡车

Abstract 摘要

To improve the upright balancing performance of the two-wheeled self-balancing car, this paper proposes an attitude estimation algorithm based on fuzzy Kalman filtering. Fuzzy logic is used to correct the inclination angle and angular velocity of the two-wheeled self-balancing car, thereby optimizing the state of the Kalman filter and ultimately improving the balancing performance of the car. This paper combines dual closed-loop PID control with the complementary filtering algorithm, Kalman filtering algorithm, and fuzzy Kalman filtering algorithm to conduct experiments on a physical two-wheeled self-balancing car. The experimental results validate the superiority of the fuzzy Kalman filtering algorithm proposed in this paper for improving the upright balancing performance of the two-wheeled self-balancing car.
为了提高两轮自平衡车的直立平衡性能，本文提出了一种基于模糊卡尔曼滤波的姿态估计算法。利用模糊逻辑修正两轮自平衡车的倾角和角速度，从而优化卡尔曼滤波器的状态，最终提高自平衡车的平衡性能。本文将双闭环 PID 控制与互补滤波算法、卡尔曼滤波算法和模糊卡尔曼滤波算法相结合，在实物两轮自平衡车上进行了实验。实验结果验证了本文提出的模糊卡尔曼滤波算法在改善两轮自平衡车直立平衡性能方面的优越性。

1. Introduction 1.导言

With the advancement of science and technology, the two-wheeled self-balancing car has garnered renewed research interest and widespread application development owing to its simple structure, agile maneuverability, capacity to navigate narrow spaces and execute specialized tasks in unique environments, and adaptability to diverse terrains. Presently, various types of robots resembling the principles of the self-balancing car are ubiquitous in major venues such as shopping malls, transportation hubs, airports, and dining establishments [1]. The two-wheeled self-balancing car system resembles an inverted pendulum system, characterized by multivariable dynamics, nonlinearity, and strong coupling, rendering its study inherently complex [2]. However, in certain intricate environments, diverse interferences may induce disparate speeds between the left and right motors of the two-wheeled self-balancing car, thereby impeding the system’s ability to follow its intended trajectory. Key to maintaining precision and stability in controlling the car system lies in the accuracy and real-time acquisition of the vehicle’s attitude information and velocity measurements.
随着科学技术的进步，两轮自平衡车因其结构简单、机动灵活、能在狭窄空间中穿行、能在特殊环境中执行专门任务、能适应各种地形等特点，重新引起了人们的研究兴趣，并得到了广泛的应用开发。目前，在购物中心、交通枢纽、机场和餐饮场所等主要场所，与自平衡车原理相似的各类机器人无处不在[1]。双轮自平衡车系统类似于倒立摆系统，具有多变量动力学、非线性和强耦合等特点，因此其研究本身就很复杂[2]。然而，在某些错综复杂的环境中，各种干扰可能会导致双轮自平衡车的左右电机速度不一致，从而阻碍系统按照预定轨迹运行。要保持汽车系统控制的精确性和稳定性，关键在于准确、实时地获取汽车的姿态信息和速度测量值。

Attitude information stands as a cornerstone for ensuring the successful operation of the entire system. However, the two-wheeled self-balancing car cannot consistently traverse smooth, disturbance-free environments. Achieving optimal attitude control solely through attitude sensors in complex environments poses a considerable challenge. The integration of filtering algorithms into the main controller ensures the integrity and accuracy of attitude information. A plethora of scholars have extensively researched this area, exploring methods such as feedback control [3], complementary filtering algorithms [4], and Kalman filtering algorithms [5]. Reference [4] presents a system design for a quadcopter based on an improved complementary filtering fusion approach, showcasing minimal static error, smooth dynamic curve transitions, and ideal real-time tracking performance. Similarly, reference [6] utilizes a complementary filtering algorithm to enhance the system’s dynamic characteristics with low computational overhead, facilitating real-time measurement of terrain undulations during the locomotion process of intelligent agents. The Kalman filtering algorithm, pioneered by American scientist Kalman in 1960, initially demonstrated significant effectiveness in the field of positioning and navigation. In contrast to complementary filtering algorithms, the Kalman filtering algorithm cleverly integrates observed and estimated data, managing errors in a closed-loop manner and confining them within a defined range, rendering it more straightforward to implement than alternative filters. Importantly, the Kalman filtering algorithm does not necessitate all past observations but estimates the current signal value based on the preceding estimate and the most recent observation. Implemented through state equations and recursion methods, the Kalman filter does not impose requirements on the stationarity or time invariance of signals [7]. Reference [8] applies the Kalman filtering algorithm to PID control of a micro-rotorcraft system, successfully conducting experimental flight control tests. This method achieves excellent filtering effects, fulfilling the objective of stable flight for the aircraft. Similarly, reference [9] realizes real-time tracking of mobile robots using the Kalman filtering algorithm. References [10,11] advocate for the use of Kalman filtering for attitude estimation in aircraft attitude control, leveraging quaternions as state variables and accelerometers as observation quantities for state updates. The Kalman filtering algorithm demonstrates commendable real-time performance and convergence. However, challenges persist due to the covariance matrix and measurement process noise inherent in the Kalman filtering model, making it difficult to accurately ascertain the statistical characteristics of the covariance matrix. While the Kalman filter’s theoretical foundation lies in linear Gaussian systems, its efficacy may diminish for nonlinear, non-Gaussian systems. In contrast, fuzzy control can address nonlinear and fuzzy problems, rendering it suitable for complex systems.
姿态信息是确保整个系统成功运行的基石。然而，双轮自平衡车无法始终在平滑、无干扰的环境中行驶。在复杂环境中，仅通过姿态传感器来实现最佳姿态控制是一个相当大的挑战。将滤波算法集成到主控制器中可确保姿态信息的完整性和准确性。大量学者对这一领域进行了广泛研究，探索了反馈控制[ 3]、互补滤波算法[ 4]和卡尔曼滤波算法[ 5]等方法。参考文献[ 4]介绍了一种基于改进的互补滤波融合方法的四旋翼飞行器系统设计，展示了最小的静态误差、平滑的动态曲线转换和理想的实时跟踪性能。同样，参考文献[6]利用互补滤波算法，以较低的计算开销增强了系统的动态特性，方便了智能代理在运动过程中对地形起伏的实时测量。卡尔曼滤波算法由美国科学家卡尔曼于 1960 年首创，最初在定位和导航领域显示出显著效果。与其他滤波算法相比，卡尔曼滤波算法巧妙地整合了观测数据和估计数据，以闭环方式管理误差，并将误差限制在确定的范围内，因此比其他滤波算法更易于实现。重要的是，卡尔曼滤波算法并不需要所有过去的观测数据，而是根据之前的估计值和最近的观测数据来估计当前的信号值。 卡尔曼滤波通过状态方程和递归方法实现，对信号的静止性和时间不变性没有要求[ 7]。参考文献[ 8] 将卡尔曼滤波算法应用于微型旋翼机系统的 PID 控制，成功地进行了飞行控制实验测试。该方法取得了很好的滤波效果，实现了飞行器稳定飞行的目标。同样，参考文献[ 9] 利用卡尔曼滤波算法实现了移动机器人的实时跟踪。参考文献[ 10, 11]主张在飞机姿态控制中使用卡尔曼滤波进行姿态估计，利用四元数作为状态变量，加速度计作为状态更新的观测量。卡尔曼滤波算法的实时性能和收敛性值得称赞。然而，由于卡尔曼滤波模型中固有的协方差矩阵和测量过程噪声，很难准确确定协方差矩阵的统计特性，因此挑战依然存在。虽然卡尔曼滤波的理论基础是线性高斯系统，但对于非线性、非高斯系统，其功效可能会减弱。相比之下，模糊控制可以解决非线性和模糊问题，因此适用于复杂系统。

Fuzzy control represents an intelligent control methodology grounded in fuzzy set theory, fuzzy linguistic variables, and fuzzy logic reasoning, which emulates human fuzzy reasoning and decision-making processes [12]. This approach encompasses three primary components: fuzzification, fuzzy control rule generation, and defuzzification. Fuzzification converts precise inputs into fuzzy inputs, while fuzzy control rule generation infers output fuzzy quantities from input fuzzy quantities. Finally, the defuzzification process transforms fuzzy outputs into precise outputs. In addressing the challenge of inaccurate attitude angle estimation during carrier motion, reference [13] proposes the adoption of a fuzzy Kalman filtering algorithm for attitude estimation. Leveraging accelerometer data as an input to the fuzzy controller, this approach notably enhances accuracy during motion and augments real-time performance under static, rotational, and motion conditions. Similarly, reference [14] introduces a parking kinematic model employing fuzzy Kalman filtering for vehicle speed estimation. This model effectively mitigates calculation errors in displacement and heading angle during parking maneuvers. Reference [15] combines fuzzy theory with a Kalman filter for processing unobservable spectral components of uncertain experimental data. In reference [16], a fuzzy PI model regulator and an adaptive Kalman filter are applied to accurately estimate the state of charge. Moreover, reference [17] constructs a mobile robot kinematic model within Simulink and designs a fuzzy controller based on real-time robot angles to facilitate precise obstacle avoidance.
模糊控制是一种基于模糊集理论、模糊语言变量和模糊逻辑推理的智能控制方法，它模拟人类的模糊推理和决策过程[12]。这种方法包括三个主要部分：模糊化、模糊控制规则生成和去模糊化。模糊化将精确输入转换为模糊输入，而模糊控制规则生成则从输入模糊量推导出输出模糊量。最后，去模糊化过程将模糊输出转化为精确输出。针对载体运动过程中姿态角估计不准确的难题，参考文献[ 13]提出采用模糊卡尔曼滤波算法进行姿态估计。这种方法利用加速度计数据作为模糊控制器的输入，显著提高了运动过程中的精度，并增强了静态、旋转和运动条件下的实时性能。同样，参考文献[14]介绍了一种采用模糊卡尔曼滤波的停车运动学模型，用于车辆速度估算。该模型有效地减少了停车过程中位移和航向角的计算误差。参考文献 [ 15] 将模糊理论与卡尔曼滤波器相结合，用于处理不确定实验数据中不可观测的频谱成分。在参考文献[ 16]中，应用模糊 PI 模型调节器和自适应卡尔曼滤波器来精确估计充电状态。此外，参考文献[ 17] 在 Simulink 中构建了移动机器人运动学模型，并设计了基于实时机器人角度的模糊控制器，以促进精确避障。

This study addresses the issue of low stability in sensor data acquisition for the two- wheeled self-balancing car by integrating a fuzzy controller into the Kalman filter, proposing the fuzzy Kalman filtering algorithm to enhance the speed and stability of the upright loop of the two-wheeled self-balancing car. Accelerometer data are utilized as an input for the fuzzy controller, and fuzzy logic is employed to adjust the parameters of the Kalman filtering algorithm. The Kalman filtering measurement noise variance is continuously adjusted to obtain the optimal estimate of the car’s balance posture. Additionally, a dual closed-loop PID control algorithm is formulated, incorporating attitude information and velocity as feedback control variables. This algorithm is combined with the use of the high-performance STM32F103C8T6 (STMicroelectronics corporation from Geneva, Switzerland) chip as the main controller to enhance the stability and anti-interference capability of the two-wheeled self-balancing car system. To compare the proposed fuzzy Kalman filtering algorithm with other methods, namely complementary filtering and Kalman filtering algorithms, in terms of speed and stability on the two-wheeled self-balancing car, this study conducted comparative verifications. Through simulation comparisons, the fuzzy Kalman filtering algorithm proposed in this paper demonstrates stronger system stability and anti-interference capability.
本研究针对两轮自平衡车传感器数据采集稳定性低的问题，将模糊控制器集成到卡尔曼滤波器中，提出了模糊卡尔曼滤波算法，以提高两轮自平衡车直立回路的速度和稳定性。利用加速度计数据作为模糊控制器的输入，并采用模糊逻辑调整卡尔曼滤波算法的参数。卡尔曼滤波的测量噪声方差会不断调整，以获得汽车平衡姿态的最佳估计值。此外，还制定了双闭环 PID 控制算法，将姿态信息和速度作为反馈控制变量。该算法结合使用高性能 STM32F103C8T6（瑞士日内瓦意法半导体公司）芯片作为主控制器，以增强两轮自平衡车系统的稳定性和抗干扰能力。为了比较所提出的模糊卡尔曼滤波算法与其他方法（即互补滤波算法和卡尔曼滤波算法）在两轮自平衡车上的速度和稳定性，本研究进行了对比验证。通过仿真比较，本文提出的模糊卡尔曼滤波算法具有更强的系统稳定性和抗干扰能力。

This paper is structured as follows: Section 2 describes the two-wheeled self-balancing car system and includes the dynamic model of the considered two-wheeled self-balancing car and the PD control algorithm of the upright loop of the two-wheeled self-balancing car. Section 3 proposes the fuzzy Kalman filtering algorithm to leverage information derived from observation data to continuously estimate and rectify the model and noise statistical characteristics in real time. To compare with other filtering algorithms, this section also briefly introduces the classical Kalman filtering theory without fuzzy theory and the widely used complementary filtering theory. Section 4 focuses on verifying the effectiveness of the mentioned methods under a laboratory test environment. Section 5 summarizes the findings and conclusions of the study.
本文的结构如下：第 2 节介绍了两轮自平衡车系统，包括所考虑的两轮自平衡车动态模型和两轮自平衡车直立环的 PD 控制算法。第 3 节提出了模糊卡尔曼滤波算法，利用从观测数据中获得的信息来实时连续地估计和修正模型和噪声的统计特性。为了与其他滤波算法进行比较，本节还简要介绍了不含模糊理论的经典卡尔曼滤波理论和广泛使用的互补滤波理论。第 4 节重点验证了上述方法在实验室测试环境下的有效性。第 5 节总结了研究结果和结论。

2. The Two-Wheeled Self-Balancing Car System
2.两轮自平衡车系统

Figure 1 depicts a physical representation of the two-wheeled self-balancing car under study in this paper. The primary components of the system include an angle acquisition module, MPU-6050 sensor (TDK corporation from Japan Tokyo), STM32 control module (STMicroelectronics corporation from Geneva, Switzerland), motor drive module, and signal acquisition module. The overall circuit design of the experimental car system is illustrated in Figure 2. Utilizing the STM32F103C8T6 (STMicroelectronics corporation from Geneva, Switzerland) as the main control chip, it facilitates high-speed processing of sensor data, execution of control algorithms, and communication with the upper computer. The vehicle is powered by a 3S battery, providing a stable 12 V output voltage. The motor drive circuit employs the TB6612FNG motor drive chip (Toshiba-Semiconductor from Bangkok, Thailand). The attitude sensor utilized is the MPU-6050 (TDK corporation from Tokyo, Japan), which integrates both an accelerometer and a gyroscope, capable of measuring acceleration and angular velocity along the vertical, coronal, and sagittal axes.
图 1 是本文研究的两轮自平衡车的实物图。系统的主要组件包括角度采集模块、MPU-6050 传感器（日本东京 TDK 公司）、STM32 控制模块（瑞士日内瓦意法半导体公司）、电机驱动模块和信号采集模块。实验车系统的整体电路设计如图 2 所示。采用 STM32F103C8T6（瑞士日内瓦意法半导体公司）作为主控芯片，可实现传感器数据的高速处理、控制算法的执行以及与上位机的通信。车辆由 3S 电池供电，提供稳定的 12 V 输出电压。电机驱动电路采用 TB6612FNG 电机驱动芯片（泰国曼谷东芝半导体公司）。使用的姿态传感器是 MPU-6050（日本东京 TDK 公司），它集成了加速度计和陀螺仪，能够测量沿垂直、冠状和矢状轴的加速度和角速度。

Figure 1. Photograph of the two-wheeled self-balancing car.
图 1.两轮自平衡车的照片。

Figure 2. Block diagram of the circuit system.
图 2.电路系统框图。

2.1. Dynamic Model of the Two-Wheeled Self-Balancing Car
2.1.两轮自平衡车的动态模型

The two-wheeled self-balancing car can be conceptualized as an inverted pendulum model [18], with its force analysis diagram illustrated in Figure 3.
双轮自平衡车可以被看作是一个倒立摆模型[ 18]，其受力分析图如图 3 所示。

Figure 3. Inverted pendulum system for the two-wheeled self-balancing car.
图 3.双轮自平衡车的倒立摆系统。

In Figure 3, the mass of the car body is denoted as 𝑚$m$, the inclination angle with respect to the vertical direction is 𝜃$\theta$, the angular velocity of the car body is represented by 𝜛$\varpi$, the acceleration of the car is denoted by 𝑎$a$, and the magnitude of the inertial force acting on the car, inertial 𝐹$F$, is proportional to the acceleration 𝑎$a$, with the proportionality coefficient being the mass 𝑚$m$, i.e., 𝐹=𝑚𝑎$F=ma$. When the inclination angle 𝜃$\theta$ is small, the acceleration 𝑎$a$ is directly proportional to the inclination angle 𝜃$\theta$, with the proportionality coefficient denoted by 𝑘1$k_1$, yielding the dynamic equation of the car:
在图 3 中，车体的质量表示为 𝑚$m$ ，相对于垂直方向的倾角为 𝜃$\theta$ ，车体的角速度表示为 𝜛$\varpi$ ，汽车的加速度表示为 𝑎$a$ ，作用在汽车上的惯性力大小（惯性力 𝐹$F$ ）与加速度 𝑎$a$ 成正比，比例系数为质量 𝑚$m$ ，即 𝐹=𝑚𝑎$F=ma$ 。当倾角 𝜃$\theta$ 较小时，加速度 𝑎$a$ 与倾角 𝜃$\theta$ 成正比，比例系数用 𝑘1$k_1$ 表示，从而得到汽车的动态方程：

𝐹=𝑚𝑔𝜃−𝑚𝑘1𝜃.$$F=mg\theta -m{k}_1\theta .$$

(1)

When 𝑘1>𝑔$k_1>g$, the direction of the restoring force 𝐹$F$ is opposite to the direction of inclination, thereby causing the car to return to its equilibrium position. If the car cannot accurately reach the neutral position and stabilize, it will inevitably oscillate around the neutral position, thus prolonging the time required for balance. In order to mitigate oscillations and achieve stability at the equilibrium position promptly, it is necessary to take into account additional damping force 𝑘2𝜃′$k_2{\theta }^{\prime }$. The magnitude of this added damping force is directly proportional to the angular velocity 𝜃′${\theta }^{\prime }$, yet acts in the opposite direction, with the proportionality coefficient denoted by 𝑘2$k_2$. Therefore, the dynamic equation of the car at this juncture can be succinctly expressed as follows:
当 𝑘1>𝑔$k_1>g$ 时，恢复力 𝐹$F$ 的方向与倾斜方向相反，从而使汽车回到平衡位置。如果小车不能准确到达中性位置并稳定下来，它将不可避免地围绕中性位置摆动，从而延长平衡所需的时间。为了减少振荡并迅速在平衡位置实现稳定，有必要考虑额外的阻尼力 𝑘2𝜃′$k_2{\theta }^{\prime }$ 。这种附加阻尼力的大小与角速度 𝜃′${\theta }^{\prime }$ 成正比，但作用方向相反，比例系数用 𝑘2$k_2$ 表示。因此，此时汽车的动态方程可以简洁地表示如下：

𝐹=𝑚𝑔𝜃−𝑚(𝑘1𝜃+𝑘2𝜃′).$$F=mg\theta -m(k_1\theta +k_2{\theta }^{\prime }).$$

(2)

The acceleration of the car can then be obtained as follows:
汽车的加速度可按下式求得：

𝑎=𝑘1𝜃+𝑘2𝜃′.$$a=k_1\theta +k_2{\theta }^{\prime }.$$

(3)

Managing both the direction and magnitude of acceleration facilitates the control of the car’s attitude. In this context, the adjustment of the pulse width modulation (PWM) magnitude mirrors the regulation of the car’s acceleration. This method of controlling acceleration also encompasses the manipulation of the motor PWM, effectively serving as the controller for the car’s upright loop. The incremental PD, PI, and PD algorithms are, respectively, employed in the upright, velocity, and steering loops to achieve functions such as balancing and turning of the two-wheeled self-balancing car.
管理加速度的方向和大小有助于控制汽车的姿态。在这种情况下，脉宽调制（PWM）幅度的调整反映了汽车加速度的调节。这种控制加速度的方法还包括对电机 PWM 的控制，从而有效地充当了汽车直立回路的控制器。增量式 PD、PI 和 PD 算法分别用于直立环、速度环和转向环，以实现两轮自平衡车的平衡和转弯等功能。

This note introduces a Fuzzy-Kalman-filtering-based attitude estimation algorithm to enhance the upright stability performance of the two-wheeled self-balancing car, with particular emphasis on filtering the measured angles and angular velocities of the car. The velocity and steering loops are utilized to control the car’s speed and turning.
本说明介绍了一种基于模糊-卡尔曼滤波的姿态估计算法，以提高双轮自平衡车的直立稳定性能，特别强调对汽车的测量角度和角速度进行滤波。速度和转向环路用于控制汽车的速度和转弯。

Considering the state-space model of the two-wheeled self-balancing car depicted in Figure 1, it is formulated as follows:
双轮自平衡车的状态空间模型如图 1 所示，其计算公式如下：

[𝜃(𝑘)𝜃′𝑏(𝑘)]=[10−𝑇1][𝜃(𝑘−1)𝜃′𝑏(𝑘−1)]+[𝑇0]𝜃′(𝑘−1)+[𝑞𝑞],$$\begin{array}{cc}\hfill \left[\begin{array}{c}\theta \left(k\right)\\ {\theta }_b^{\prime }\left(k\right)\end{array}\right]=& \left[\begin{array}{cc}1& -T\\ 0& 1\end{array}\right]\left[\begin{array}{c}\theta \left(k-1\right)\\ {\theta }_b^{\prime }\left(k-1\right)\end{array}\right]+\hfill \\ & \left[\begin{array}{c}T\\ 0\end{array}\right]{\theta }^{\prime }\left(k-1\right)+\left[\begin{array}{c}q\\ q\end{array}\right]\hfill \end{array},$$

(4)

𝜃𝑐(𝑘)=𝜃(𝑘)+𝑟,$${\theta }_c\left(k\right)=\theta \left(k\right)+r,$$

(5)

where 𝜃(𝑘)∈ℝ$\theta \left(k\right)\in \mathbb{R}$ and 𝜃′(𝑘)∈ℝ${\theta }^{\prime }\left(k\right)\in \mathbb{R}$, respectively, represent the actual angle and angular velocity measured by the gyroscope of the two-wheeled self-balancing car at time 𝑘$k$. 𝜃′𝑏(𝑘)∈ℝ${\theta }_b^{\prime }\left(k\right)\in \mathbb{R}$ represents the gyroscope’s static error, where static error refers to the angular velocity value output by the gyroscope when the two-wheeled self-balancing car is stationary. 𝜃𝑐(𝑘)∈ℝ${\theta }_c\left(k\right)\in \mathbb{R}$ denotes the angle calculated by the accelerometer, 𝑇∈ℝ$T\in \mathbb{R}$ represents the filtering period, 𝑞∈ℝ$q\in \mathbb{R}$ and 𝑟∈ℝ$r\in \mathbb{R}$, respectively, denote the covariance of process noise and measurement noise.
其中， 𝜃(𝑘)∈ℝ$\theta \left(k\right)\in \mathbb{R}$ 和 𝜃′(𝑘)∈ℝ${\theta }^{\prime }\left(k\right)\in \mathbb{R}$ 分别代表两轮自平衡车陀螺仪在 𝑘$k$ 时间测量到的实际角度和角速度。 𝜃′𝑏(𝑘)∈ℝ${\theta }_b^{\prime }\left(k\right)\in \mathbb{R}$ 表示陀螺仪的静态误差，静态误差是指两轮自平衡车静止时陀螺仪输出的角速度值。 𝜃𝑐(𝑘)∈ℝ${\theta }_c\left(k\right)\in \mathbb{R}$ 表示加速度计计算的角度， 𝑇∈ℝ$T\in \mathbb{R}$ 表示滤波周期， 𝑞∈ℝ$q\in \mathbb{R}$ 和 𝑟∈ℝ$r\in \mathbb{R}$ 分别表示过程噪声和测量噪声的协方差。

2.2. The PD Control for the Two-Wheeled Self-Balancing Car
2.2.两轮自平衡车的 PD 控制

Upon initialization of the self-balancing car, the first step involves state detection to determine whether the current orientation matches the target inclination angle. If not, the motor is adjusted to accelerate accordingly, aiming to bring the vehicle to an upright position through angle control. Concurrently, the velocity sensor provides feedback on motor status to fine-tune input values, thereby establishing a complete negative feedback system. Upon the acquisition of angle information by the sensors, the angular velocity undergoes PD control within the upright loop to regulate the vehicle’s posture. Increasing the proportional coefficient enhances the vehicle’s responsiveness, while the differential coefficient influences the stability of the self-balancing car when it is not inclined. Once the proportional and differential coefficients are established, adjustments to the motion posture of the two-wheeled self-balancing car can be made based on speed information provided by the encoder feedback. The expression for its PD control within the upright loop can be simplified as follows:
自平衡车初始化后，第一步涉及状态检测，以确定当前方位是否与目标倾斜角相匹配。如果不匹配，则相应地调整电机加速，旨在通过角度控制使车辆达到直立位置。与此同时，速度传感器提供有关电机状态的反馈，以微调输入值，从而建立一个完整的负反馈系统。在传感器获得角度信息后，角速度在直立环路内进行 PD 控制，以调节车辆的姿态。增加比例系数可提高车辆的响应速度，而微分系数则会影响自平衡车在非倾斜状态下的稳定性。比例系数和微分系数确定后，就可以根据编码器反馈的速度信息对两轮自平衡车的运动姿态进行调整。其直立环路内的 PD 控制表达式可简化如下：

𝑀(𝑘)=𝑘𝑝𝑒(𝑘)+𝑘𝑑(𝑒(𝑘)−𝑒(𝑘−1))=𝑘𝑝[𝜃̲(𝑘)−𝜃(𝑘)]+𝑘𝑑(0−𝜃′(𝑘)).$$\begin{array}{cc}\hfill M(k)& =k_{p}e(k)+k_d(e(k)-e(k-1))\hfill \\ & =k_p[\overline{\theta }\left(k\right)-\theta \left(k\right)]+k_d(0-\theta \prime \left(k\right))\hfill \end{array}.$$

(6)

where 𝜃̲(𝑘)$\overline{\theta }\left(k\right)$ denotes the desired equilibrium angle, typically set to zero, and 𝑀(𝑘)$M(k)$ signifies the PWM value. 𝑘𝑝$k_p$ and 𝑘𝑑$k_d$, respectively, correspond to the proportional and derivative coefficients of the PD control algorithm.
其中， 𝜃̲(𝑘)$\overline{\theta }\left(k\right)$ 表示所需的平衡角，通常设为零， 𝑀(𝑘)$M(k)$ 表示 PWM 值。 𝑘𝑝$k_p$ 和 𝑘𝑑$k_d$ 分别对应于 PD 控制算法的比例系数和导数系数。

The precision of angle data acquisition significantly impacts the efficacy of control algorithms. However, relying solely on accelerometers and gyroscopes for angle determination presents certain limitations. Accelerometers provide relatively accurate long-term readings but exhibit notable fluctuations, potentially due to high-frequency noise contamination. Conversely, gyroscopes offer short-term accuracy but are susceptible to integration errors, suggesting the presence of low-frequency noise. To mitigate the adverse effects of noise on data accuracy, angle measurements obtained from accelerometers and gyroscopes can undergo filtering and smoothing procedures, thereby enhancing measurement precision.
角度数据采集的精度对控制算法的效果有很大影响。然而，仅依靠加速度计和陀螺仪来确定角度存在一定的局限性。加速度计可提供相对精确的长期读数，但会出现明显的波动，这可能是高频噪声污染造成的。相反，陀螺仪提供短期精度，但容易出现积分误差，表明存在低频噪声。为了减轻噪声对数据精度的不利影响，加速度计和陀螺仪获得的角度测量值可以经过过滤和平滑处理，从而提高测量精度。

3. Fuzzy Kalman Filtering Algorithm Design
3.模糊卡尔曼滤波算法设计

In optimal conditions, when a dual-wheel self-balancing car operates under normal conditions, and with known system and measurement noise parameters, fixed complementary and Kalman filters effectively mitigate measurement errors. However, in scenarios where the vehicle’s measurement sensors deviate due to environmental disturbances or when external noise is uncertain, relying solely on a Kalman filter with predetermined noise parameters may fail to accurately ascertain the vehicle’s genuine angle and velocity information. This paper proposes a fusion of fuzzy control theory and the Kalman filtering method, leveraging information derived from observation data to continuously estimate and rectify the model and noise statistical characteristics in real time. Such an approach promises to enhance filtering performance and bolster the stability of the vehicle.
在最佳条件下，当双轮自平衡车在正常条件下运行，并具有已知的系统和测量噪声参数时，固定互补和卡尔曼滤波器可有效减少测量误差。然而，在车辆的测量传感器因环境干扰而出现偏差或外部噪声不确定的情况下，仅依靠具有预定噪声参数的卡尔曼滤波器可能无法准确确定车辆的真实角度和速度信息。本文提出了一种融合模糊控制理论和卡尔曼滤波法的方法，利用从观测数据中获得的信息来实时不断地估计和修正模型和噪声的统计特性。这种方法有望提高滤波性能，增强车辆的稳定性。

To compare with other filtering algorithms, this section first introduces the fuzzy Kalman filter algorithm proposed by the author, and then briefly introduces the classical Kalman filtering theory without fuzzy theory and the widely used complementary filtering theory.
为了与其他滤波算法进行比较，本节首先介绍了作者提出的模糊卡尔曼滤波算法，然后简要介绍了没有模糊理论的经典卡尔曼滤波理论和广泛使用的互补滤波理论。

3.1. Fuzzy Kalman Filtering Algorithm Design
3.1.模糊卡尔曼滤波算法设计

This section first introduces the Kalman filtering algorithm for filtering the angle and angular velocity of the self-balancing car, followed by the establishment of the fuzzification and defuzzification processes of process noise and measurement noise based on the fuzzy theory.
本节首先介绍了对自平衡车的角度和角速度进行滤波的卡尔曼滤波算法，然后基于模糊理论建立了过程噪声和测量噪声的模糊化和去模糊化过程。

The covariance of process noise and measurement noise experienced by the two-wheeled self-balancing car is:
两轮自平衡车所经历的过程噪声和测量噪声的协方差为

{𝑞(𝑘)=𝛼(𝑘)·𝑞𝑟(𝑘)=𝛽(𝑘)·𝑟$$\left\{\begin{array}{c}q\left(k\right)=\alpha \left(k\right)·q\\ r\left(k\right)=\beta \left(k\right)·r\end{array}\right.$$

(7)

where 𝑞(𝑘)∈ℝ$q\left(k\right)\in \mathbb{R}$ and 𝑟(𝑘)∈ℝ$r\left(k\right)\in \mathbb{R}$ respectively, represent the covariance of process noise and measurement noise at time instant 𝑘$k$, while 𝛼(𝑘)∈ℝ$\alpha \left(k\right)\in \mathbb{R}$ and 𝛽(𝑘)∈ℝ$\beta \left(k\right)\in \mathbb{R}$ are used to adjust the parameters of measurement noise and process noise covariance.
其中 𝑞(𝑘)∈ℝ$q\left(k\right)\in \mathbb{R}$ 和 𝑟(𝑘)∈ℝ$r\left(k\right)\in \mathbb{R}$ 分别表示时间瞬间 𝑘$k$ 的过程噪声协方差和测量噪声协方差，而 𝛼(𝑘)∈ℝ$\alpha \left(k\right)\in \mathbb{R}$ 和 𝛽(𝑘)∈ℝ$\beta \left(k\right)\in \mathbb{R}$ 则用于调整测量噪声和过程噪声协方差的参数。

The Kalman filter, as a linear optimal estimation algorithm, utilizes a “predict–measure–correct” pattern to eliminate random disturbances of the system by updating the measurement values, thus optimizing the restoration of the system’s state. The Kalman filter has two main functions, namely prediction and correction, as shown in Figure 4. Prediction involves the time update equation, while correction involves the measurement update equation. The time update equation estimates the a priori estimate and covariance of the state variables at the current time by evaluating the state value at the previous time, while the measurement equation calculates the posterior estimate by combining the a priori estimate with the current measurement value, achieving the purpose of correction.
卡尔曼滤波器作为一种线性最优估计算法，利用 "预测-测量-修正 "模式，通过更新测量值来消除系统的随机干扰，从而优化系统状态的恢复。卡尔曼滤波器有两个主要功能，即预测和修正，如图 4 所示。预测涉及时间更新方程，而修正涉及测量更新方程。时间更新方程通过评估前一时间的状态值来估计当前时间状态变量的先验估计值和协方差，而测量方程则通过将先验估计值与当前测量值相结合来计算后验估计值，从而达到修正的目的。

Figure 4. Kalman filter update schematic.
图 4 卡尔曼滤波器更新示意图卡尔曼滤波器更新示意图。

A prior estimation of the angle and angular velocity of the two-wheeled self-balancing car is designed as follows:
双轮自平衡车的角度和角速度的先验估计设计如下：

⎡⎣⎢𝜃̂(𝑘)𝜃̂𝑏(𝑘)⎤⎦⎥=[10−𝑇1]⎡⎣⎢𝜃̂(𝑘−1)𝜃̂𝑏(𝑘−1)⎤⎦⎥+[𝑇0]𝜃′̂(𝑘−1),$$\left[\begin{array}{c}\widehat{\theta }\left(k\right)\\ {\widehat{\theta }}_b\left(k\right)\end{array}\right]=\left[\begin{array}{cc}1& -T\\ 0& 1\end{array}\right]\left[\begin{array}{c}\widehat{\theta }\left(k-1\right)\\ {\widehat{\theta }}_b\left(k-1\right)\end{array}\right]+\left[\begin{array}{c}T\\ 0\end{array}\right]\widehat{{\theta }^{\prime }}\left(k-1\right),$$

(8)

𝜃̂𝑐(𝑘)=𝜃̂(𝑘)$${\widehat{\theta }}_c\left(k\right)=\widehat{\theta }\left(k\right)$$

(9)

where 𝜃̂(𝑘),𝜃′̂(𝑘),𝜃̂𝑐(𝑘)$\widehat{\theta }\left(k\right),\widehat{{\theta }^{\prime }}\left(k\right),{\widehat{\theta }}_c\left(k\right)$ and 𝜃̂𝑏(𝑘)${\widehat{\theta }}_b\left(k\right)$ are, respectively, the estimated values of the car’s angle, angular velocity, accelerometer angle, and static error.
其中 𝜃̂(𝑘),𝜃′̂(𝑘),𝜃̂𝑐(𝑘)$\widehat{\theta }\left(k\right),\widehat{{\theta }^{\prime }}\left(k\right),{\widehat{\theta }}_c\left(k\right)$ 和 𝜃̂𝑏(𝑘)${\widehat{\theta }}_b\left(k\right)$ 分别为汽车角度、角速度、加速度计角度和静态误差的估计值。

The accelerometer angle obtained from the prior estimate needs to be corrected through posterior estimation. The covariance matrix of the posterior estimate is defined as 𝑃(𝑘)$P\left(k\right)$ [19], yielded using the following equation:
从先验估计中获得的加速度计角度需要通过后验估计进行修正。后验估计的协方差矩阵定义为 𝑃(𝑘)$P\left(k\right)$ [ 19]，通过以下公式得出：

𝑃(𝑘)=(𝐼−𝐾(𝑘−1)𝐻)(𝐴𝑃(𝑘−1)𝐴𝑇+𝑄(𝑘−1)),𝐾(𝑘)=[𝐾1(𝑘)𝐾2(𝑘)]=⎡⎣⎢⎢⎢𝑝11(𝑘)𝑝11(𝑘)+𝑟(𝑘)𝑝21(𝑘)𝑝11(𝑘)+𝑟(𝑘)⎤⎦⎥⎥⎥,$$\begin{gathered} P\left(k\right)=\left(I-K\left(k-1\right)H\right)\left(AP\left(k-1\right)A^T+Q\left(k-1\right)\right), \\ K\left(k\right)=\left[\begin{array}{c}{K}_1\left(k\right)\\ K_2\left(k\right)\end{array}\right]=\left[\begin{array}{c}\frac{p_{11}\left(k\right)}{p_{11}\left(k\right)+r\left(k\right)}\\ \frac{p_{21}\left(k\right)}{p_{11}\left(k\right)+r\left(k\right)}\end{array}\right], \end{gathered}$$

𝜃𝑓c(𝑘)=𝜃̂𝑐(𝑘)+𝑃(𝑘)(𝜃𝑐(𝑘)−𝜃̂𝑐(𝑘)),$${\theta }_{\mathrm{c}}^f\left(k\right)={\widehat{\theta }}_c(k)+P(k)({\theta }_c(k)-{\widehat{\theta }}_c(k)),$$

(10)

where 𝐼$I$ is the unit matrix, 𝐾(𝑘)$K\left(k\right)$ is the Kalman gain vector, 𝐻=[10], 𝐴=[10−𝑇1],$H=\left[\begin{array}{cc}1& 0\end{array}\right],\hspace{0.33em}A=\left[\begin{array}{cc}1& -T\\ 0& 1\end{array}\right],$ 𝑄(𝑘−1)=[𝑞(𝑘−1)00𝑞(𝑘−1)],𝑃(𝑘)=[𝑝11(𝑘)𝑝21(𝑘)𝑝12(𝑘)𝑝22(𝑘)]$Q\left(k-1\right)=\left[\begin{array}{cc}q\left(k-1\right)& 0\\ 0& q\left(k-1\right)\end{array}\right],P\left(k\right)=\left[\begin{array}{cc}{p}_{11}\left(k\right)& p_{12}\left(k\right)\\ p_{21}\left(k\right)& p_{22}\left(k\right)\end{array}\right]$, and 𝜃𝑓c(𝑘)${\theta }_{\mathrm{c}}^f\left(k\right)$ represents the corrected accelerometer angle, which is also the final output of the filter.
其中， 𝐼$I$ 是单位矩阵， 𝐾(𝑘)$K\left(k\right)$ 是卡尔曼增益向量， 𝐻=[10], 𝐴=[10−𝑇1],$H=\left[\begin{array}{cc}1& 0\end{array}\right],\hspace{0.33em}A=\left[\begin{array}{cc}1& -T\\ 0& 1\end{array}\right],$ 𝑄(𝑘−1)=[𝑞(𝑘−1)00𝑞(𝑘−1)],𝑃(𝑘)=[𝑝11(𝑘)𝑝21(𝑘)𝑝12(𝑘)𝑝22(𝑘)]$Q\left(k-1\right)=\left[\begin{array}{cc}q\left(k-1\right)& 0\\ 0& q\left(k-1\right)\end{array}\right],P\left(k\right)=\left[\begin{array}{cc}{p}_{11}\left(k\right)& p_{12}\left(k\right)\\ p_{21}\left(k\right)& p_{22}\left(k\right)\end{array}\right]$ ， 𝜃𝑓c(𝑘)${\theta }_{\mathrm{c}}^f\left(k\right)$ 代表修正后的加速度计角度，也是滤波器的最终输出。

Two time-varying parameters, 𝛼(𝑘)$\alpha \left(k\right)$ and 𝛽(𝑘)$\beta \left(k\right)$ are selected as the outputs of the fuzzy control; the noise covariance matrix of the filter is modified by adjusting the values of 𝛼(𝑘)$\alpha \left(k\right)$ and 𝛽(𝑘)$\beta \left(k\right)$.
选择两个时变参数 𝛼(𝑘)$\alpha \left(k\right)$ 和 𝛽(𝑘)$\beta \left(k\right)$ 作为模糊控制的输出；通过调整 𝛼(𝑘)$\alpha \left(k\right)$ 和 𝛽(𝑘)$\beta \left(k\right)$ 的值来修改滤波器的噪声协方差矩阵。

Let 𝑒𝜃(𝑘)=𝜃𝑐(𝑘)−𝜃𝑓𝑐(𝑘)$e_{\theta }\left(k\right)={\theta }_c\left(k\right)-{\theta }_c^f\left(k\right)$ be defined as the residual, indicating the extent to which the self-balancing car system relies on measurement values. The variance and mean of the residual can be used to assess the performance of the filter, with the mean and variance of residual are computed as follows:
将 𝑒𝜃(𝑘)=𝜃𝑐(𝑘)−𝜃𝑓𝑐(𝑘)$e_{\theta }\left(k\right)={\theta }_c\left(k\right)-{\theta }_c^f\left(k\right)$ 定义为残差，表示自平衡车系统对测量值的依赖程度。残差的方差和均值可用来评估滤波器的性能，残差的均值和方差计算如下：

𝑒̲(𝑘)=1𝑛∑𝑘=𝑚−𝑛𝑚𝑒𝜃(𝑘),$$\overline{e}\left(k\right)=\frac{1}{n}{\displaystyle \sum _{k=m-n}^m{e}_{\theta }\left(k\right)},$$

(11)

𝑃̂𝑒(𝑘)=1𝑛∑𝑘=𝑚−𝑛+1𝑚𝑒𝜃(𝑘)2$${\widehat{P}}_e\left(k\right)=\frac{1}{n}{\displaystyle \sum _{k=m-n+1}^m{e}_{\theta }}{\left(k\right)}^2$$

(12)

where 𝑛∈ℝ$n\in \mathbb{R}$ represents the statistical time length, 𝑚∈ℤ+$m\in {\mathbb{Z}}^{+}$. The mean and variance of the residual are chosen as inputs for fuzzy control. Next, the inputs 𝛼(𝑘)$\alpha \left(k\right)$ and 𝛽(𝑘)$\beta \left(k\right)$ are fuzzified.
其中， 𝑛∈ℝ$n\in \mathbb{R}$ 表示统计时间长度， 𝑚∈ℤ+$m\in {\mathbb{Z}}^{+}$ 表示统计时间长度。残差的均值和方差被选为模糊控制的输入值。接下来，输入 𝛼(𝑘)$\alpha \left(k\right)$ 和 𝛽(𝑘)$\beta \left(k\right)$ 将被模糊化。

Drawing upon the fuzzy method outlined in reference [12], the mean residual 𝑒̲(𝑘)$\overline{e}\left(k\right)$ and residual variance 𝑃̂𝑒(𝑘)${\widehat{P}}_e\left(k\right)$ are subjected to fuzzification for the fuzzy rules. Triangular membership functions, comprising large (L), medium (M), and small (S) fuzzy sets, are selected. The waveform diagrams illustrating the membership functions for the mean and variance of the residual are depicted in Figure 5 and Figure 6, respectively.
借鉴参考文献[12]中概述的模糊方法，对平均残差 𝑒̲(𝑘)$\overline{e}\left(k\right)$ 和残差方差 𝑃̂𝑒(𝑘)${\widehat{P}}_e\left(k\right)$ 进行模糊化处理，形成模糊规则。选择了由大（L）、中（M）和小（S）模糊集组成的三角形成员函数。图 5 和图 6 分别显示了残差均值和方差的成员函数波形图。

Figure 5. Residual mean affiliation distribution.
图 5.残差平均隶属关系分布。

Figure 6. Residual variance affiliation distribution.
图 6.残差隶属关系分布。

The curves in the graph represent different scenarios based on the mean residual being less than or equal to 1 and the residual variance being less than or equal to 0.4, indicating that both fall within the small (S) fuzzy interval, signifying reliable measurement information for the two-wheeled self-balancing car. When the mean is between 1 and 3.5 and the variance is between 0.4 and 1.4, both are in the medium (M) fuzzy interval, indicating relatively reliable measurement information. Moreover, when the mean is between 3.5 and 5 and the variance is between 1.4 and 2, both are in the large (L) fuzzy interval, indicating unreliable measurement information. Based on the actual data and empirical evidence from the car’s operation, fuzzy rules are established as shown in Table 1.
图中的曲线代表了平均残差小于或等于 1 和残差方差小于或等于 0.4 的不同情况，表明两者都属于小（S）模糊区间，表示两轮自平衡车的测量信息是可靠的。当均值介于 1 和 3.5 之间，方差介于 0.4 和 1.4 之间时，二者均处于中（M）模糊区间，表明测量信息相对可靠。此外，当均值介于 3.5 和 5 之间，方差介于 1.4 和 2 之间时，两者都处于大（L）模糊区间，表明测量信息不可靠。根据汽车运行的实际数据和经验证据，建立了如表 1 所示的模糊规则。

Table 1. Fuzzy rule table.

𝐈𝐧𝐩𝐮𝐭 𝒆̲(𝒌)$\mathbf{Input} \overline{\mathit{e}}\left(\mathit{k}\right)$	𝐈𝐧𝐩𝐮𝐭 𝑷̂𝒆(𝒌)$\mathbf{Input} {\widehat{\mathit{P}}}_{\mathit{e}}\left(\mathit{k}\right)$	𝐎𝐮𝐭𝐩𝐮𝐭 𝜶(𝒌)$\mathbf{Output} \mathit{\alpha }\left(\mathit{k}\right)$, 𝜷(𝒌)$, \mathit{\beta }\left(\mathit{k}\right)$
	S	S
S	M	M
	L	S
	S	M
M	M	L
	L	L
	S	M
L	M	L
	L	L

Through fuzzy reasoning, the mean and variance of the residual are assessed, facilitating the real-time adjustment of covariance parameters for measurement noise and process noise in the Kalman filter. The centroid method [19] is employed as a defuzzification algorithm to derive specific numerical outputs.
通过模糊推理，对残差的均值和方差进行评估，便于实时调整卡尔曼滤波器中测量噪声和过程噪声的协方差参数。采用中心法[ 19] 作为去模糊化算法，得出具体的数值输出。

(1)

When the output falls within the S fuzzy interval:
当输出在 S 模糊区间内时：

𝛼(𝑘)=1𝛽(𝑘)=1$$\begin{array}{l}\alpha \left(k\right)=1\\ \beta \left(k\right)=1\end{array}$$

(13)

(2)

When the output falls within the M fuzzy interval:
当输出在 M 模糊区间内时：

𝛼(𝑘)=0.8−0.1𝑃̂𝑒(𝑘)𝛽(𝑘)=1.4+0.1𝑃̂𝑒(𝑘)$$\begin{array}{l}\alpha \left(k\right)=0.8-0.1{\widehat{P}}_e\left(k\right)\\ \beta \left(k\right)=1.4+0.1{\widehat{P}}_e\left(k\right)\end{array}$$

(14)

(3)

When the output falls within the L fuzzy interval:
当输出在 L 模糊区间内时：

𝛼(𝑘)=0.6−0.2𝑒̲(𝑘)𝛽(𝑘)=1.2+0.2𝑒̲(𝑘)$$\begin{array}{l}\alpha \left(k\right)=0.6-0.2\overline{e}\left(k\right)\\ \beta \left(k\right)=1.2+0.2\overline{e}\left(k\right)\end{array}$$

(15)

Remak 1. 

The calculation coefficients in defuzzification Equations (13)–(15) are primarily selected based on the scale values of the membership distribution chart. In this study, keeping the parameter 𝛼(𝑘)$\alpha \left(k\right)$ as a fixed value, the parameter 𝛽(𝑘)$\beta \left(k\right)$ is continuously adjusted to achieve the optimal filtering effect. Conversely, when the parameter 𝛽(𝑘)$\beta \left(k\right)$ is fixed, adjustments are made to the parameter 𝛼(𝑘)$\alpha \left(k\right)$. Eventually, based on approximately 50 experimental and trial datasets, the feasibility of this dataset is inferred and further utilized for subsequent experiments to compare and validate the effectiveness of the algorithm.

去模糊化公式（13）-（15）中的计算系数主要根据成员分布图的标度值进行选择。在本研究中，在保持参数 𝛼(𝑘)$\alpha \left(k\right)$ 为固定值的情况下，不断调整参数 𝛽(𝑘)$\beta \left(k\right)$ 以达到最佳过滤效果。反之，当参数 𝛽(𝑘)$\beta \left(k\right)$ 固定时，对参数 𝛼(𝑘)$\alpha \left(k\right)$ 进行调整。最终，基于约 50 个实验和试用数据集，推断出该数据集的可行性，并进一步用于后续实验，以比较和验证算法的有效性。

For the purpose of comparative experiments, in the case where the PD control parameters remain unchanged for the upright, velocity, and steering loops of the two-wheeled self-balancing car, the following two sections will introduce two commonly used filtering algorithms for the car, namely the complementary filtering algorithm and traditional Kalman filtering algorithm.
为了进行对比实验，在两轮自平衡车的直立环、速度环和转向环的 PD 控制参数保持不变的情况下，下面两节将介绍两种常用的汽车滤波算法，即互补滤波算法和传统卡尔曼滤波算法。

3.2. Kalman Filtering Algorithm
3.2.卡尔曼滤波算法

The prior estimation of the angle and angular velocity of the two-wheeled self-balancing car is designed in the same way as (8) and (9), while the covariance matrix of the posterior estimate is
两轮自平衡车角度和角速度的先验估计设计方法与 (8) 和 (9) 相同，而后验估计的协方差矩阵为

𝑃(𝑘)=(𝐼−𝐾(𝑘−1)𝐻)(𝐴𝑃(𝑘−1)𝐴𝑇+𝑄),𝐾(𝑘)=[𝐾1(𝑘)𝐾2(𝑘)]=⎡⎣⎢⎢⎢𝑝11(𝑘)𝑝11(𝑘)+𝑟𝑝21(𝑘)𝑝11(𝑘)+𝑟⎤⎦⎥⎥⎥,$$\begin{gathered} P\left(k\right)=\left(I-K\left(k-1\right)H\right)\left(AP\left(k-1\right)A^T+Q\right), \\ K\left(k\right)=\left[\begin{array}{c}{K}_1\left(k\right)\\ K_2\left(k\right)\end{array}\right]=\left[\begin{array}{c}\frac{p_{11}\left(k\right)}{p_{11}\left(k\right)+r}\\ \frac{p_{21}\left(k\right)}{p_{11}\left(k\right)+r}\end{array}\right], \end{gathered}$$

𝜃𝑓c(𝑘)=𝜃̂𝑐(𝑘)+𝑃(𝑘)(𝜃𝑐(𝑘)−𝜃̂𝑐(𝑘)),$${\theta }_{\mathrm{c}}^f\left(k\right)={\widehat{\theta }}_c(k)+P(k)({\theta }_c(k)-{\widehat{\theta }}_c(k)),$$

(16)

where the meanings of 𝐼,𝐻,𝐴,𝑃(𝑘)$I,H,A,P(k)$, and 𝜃𝑓c(𝑘)${\theta }_{\mathrm{c}}^f\left(k\right)$ are the same as in Equation (10), 𝑄=[𝑞00𝑞]$Q=\left[\begin{array}{cc}q& 0\\ 0& q\end{array}\right]$. 𝜃̂c(𝑘)${\widehat{\theta }}_{\mathrm{c}}\left(k\right)$ is computed using Equation (9).
其中 𝐼,𝐻,𝐴,𝑃(𝑘)$I,H,A,P(k)$ , 和 𝜃𝑓c(𝑘)${\theta }_{\mathrm{c}}^f\left(k\right)$ 的含义与公式 (10) 相同， 𝑄=[𝑞00𝑞]$Q=\left[\begin{array}{cc}q& 0\\ 0& q\end{array}\right]$ .3# 的计算使用公式 (9)。

3.3. Complementary Filtering Algorithm
3.3.互补滤波算法

The complementary filtering algorithm, with its characteristics of low computational complexity, high reliability, and low precision requirements for inertial devices, is widely used in the attitude applications of various robots. The complementary filtering algorithm combines a low-pass filter and a high-pass filter, which are then overlaid to enhance the accuracy of attitude angular velocity [20]. The principle of the complementary filter is illustrated in Figure 6.
互补滤波算法具有计算复杂度低、可靠性高、对惯性设备精度要求低等特点，被广泛应用于各种机器人的姿态应用中。互补滤波算法结合了低通滤波器和高通滤波器，然后将两者叠加以提高姿态角速度的精度[20]。互补滤波器的原理如图 6 所示。

From Figure 7, it can be observed that in the low-frequency range below the cutoff frequency of the low-pass filter, the accelerometer primarily influences, while in the high-frequency range above the cutoff frequency of the high-pass filter, the gyroscope dominates. By adjusting the cutoff frequency of the filter, the filtering effects on the gyroscope and accelerometer can be tuned. The time-domain expression of the output angular velocity after the complementary filter is as follows:
从图 7 中可以看出，在低于低通滤波器截止频率的低频范围内，主要受加速度计的影响，而在高于高通滤波器截止频率的高频范围内，主要受陀螺仪的影响。通过调节滤波器的截止频率，可以调整陀螺仪和加速度计的滤波效果。互补滤波器后输出角速度的时域表达式如下：

𝜃(𝑘)=𝜆𝜃𝑚(𝑘)−(1−𝜆)[𝜃(𝑘−1)+Δ𝑡𝜃′(𝑘)]$$\theta (k)=\lambda {\theta }_m(k)-(1-\lambda )[\theta (k-1)+\Delta t{\theta }^{\prime }\left(k\right)]$$

(17)

where the proportional coefficient 𝜆=Δ𝑡Δ𝑡+𝜏$\lambda =\frac{\Delta t}{\Delta t+\tau }$, which represents the degree of trust idifferent data, Δ𝑡$\Delta t$ is the sampling time, 𝜏$\tau$ is the time constant of the filter, the angle transformed from the accelerometer is denoted as 𝜃𝑚(𝑘)${\theta }_m(k)$, the gyroscope measures angular velocity as 𝜃′(𝑘)${\theta }^{\prime }\left(k\right)$, and 𝜃(𝑘)$\theta (k)$ represents the angle output after the filtering algorithm is processed.
其中，比例系数 𝜆=Δ𝑡Δ𝑡+𝜏$\lambda =\frac{\Delta t}{\Delta t+\tau }$ 表示对不同数据的信任程度， Δ𝑡$\Delta t$ 是采样时间， 𝜏$\tau$ 是滤波器的时间常数，加速度计转换的角度表示为 𝜃𝑚(𝑘)${\theta }_m(k)$ ，陀螺仪测量的角速度表示为 𝜃′(𝑘)${\theta }^{\prime }\left(k\right)$ ， 𝜃(𝑘)$\theta (k)$ 表示滤波算法处理后的角度输出。

Figure 7. The principle of complementary filter.
图 7.互补滤波器的原理。

4. Experimental Verification
4.实验验证

This experiment compares the time for a dual-wheel self-balancing car to return to balance from an unbalanced state and the angular fluctuations in the balanced state under the fuzzy Kalman filtering algorithm, Kalman filtering algorithm [21], and complementary filtering algorithm [20]. The self-balancing car is placed on the ground, and it is rotated around the motor axis to find the angle at which the car approaches balance, which is considered the mechanical center of the robot. In this experiment, the mechanical center of the robot is defined as 0°$0°$.
本实验比较了模糊卡尔曼滤波算法、卡尔曼滤波算法[ 21] 和互补滤波算法[ 20] 下双轮自平衡车从不平衡状态恢复平衡的时间以及平衡状态下的角度波动。将自平衡小车放在地面上，绕电机轴旋转，找到小车接近平衡的角度，该角度被认为是机器人的机械中心。在本实验中，机器人的机械中心定义为 0°$0°$ 。

The CH340G driver for the USB to TTL module was installed, MCUISP v0.995 software was used to flash the pre-written program into the STM32 microcontroller (STMicroelectronics corporation from Geneva, Switzerland), the robot was connected to the PC using a MicroUSB cable, the MiniBalance 3.5 software was opened, the appropriate port was selected, and the baud rate was set to 115,200. This configuration allows the operator to view the data waveform of the robot’s inclination angle through the software. To ensure the stable operation of the dual-wheel self-balancing car depicted in Figure 1, it is imperative to fine-tune the controller parameters governing the car’s upright, speed, and steering loops. The PD controller parameters for the upright loop are set as: 𝐾𝑝=225$K_p=225$ and 𝐾𝑑=1.08$K_d=1.08$; the PI controller parameters for the speed loop are set as: 𝐾𝑝=160$K_p=160$ and 𝐾𝑖=0.80$K_i=0.80$; the PD controller parameters for the steering loop are set as: 𝐾𝑝=42$K_p=42$ and 𝐾𝑑=0.40$K_d=0.40$. The covariance of process noise and measurement noise are set as 𝑞=0.25$q=0.25$ and 𝑟=0.16$r=0.16$; the filtering period is set as 𝑇=0.005$T=0.005$, i.e., filtering is performed every 5 milliseconds.
安装 USB 转 TTL 模块的 CH340G 驱动程序，使用 MCUISP v0.995 软件将预先编写的程序闪存到 STM32 微控制器（瑞士日内瓦意法半导体公司）中，使用 MicroUSB 电缆将机器人连接到 PC，打开 MiniBalance 3.5 软件，选择适当的端口，并将波特率设置为 115 200。通过这种配置，操作员可以通过软件查看机器人倾角的数据波形。为确保图 1 所示的双轮自平衡小车稳定运行，必须对控制小车直立、速度和转向环路的控制器参数进行微调。直立环路的 PD 控制器参数设置为0# 和 𝐾𝑑=1.08$K_d=1.08$ ；速度环路的 PI 控制器参数设置为： 𝐾𝑝=160$K_p=160$ 和 𝐾𝑖=0.80$K_i=0.80$ ；转向环路的 PI 控制器参数设置为： 𝐾𝑝=160$K_p=160$ 和 𝐾𝑖=0.80$K_i=0.80$ ：速度环的 PI 控制器参数设置为： 𝐾𝑝=160$K_p=160$ 和 𝐾𝑖=0.80$K_i=0.80$ ；转向环的 PD 控制器参数设置为： 𝐾𝑝=42$K_p=42$ 和 𝐾𝑑=0.40$K_d=0.40$ ：4# 和 𝐾𝑑=0.40$K_d=0.40$ 。过程噪声和测量噪声的协方差设为 𝑞=0.25$q=0.25$ 和 𝑟=0.16$r=0.16$ ；滤波周期设为 𝑇=0.005$T=0.005$ ，即每 5 毫秒滤波一次。

Measurements were conducted to observe the car’s behavior under three different filtering algorithms as it transitioned from tilting backward at angles of 10°, 20°, and 30° (denoted as −10°, −20°, and −30°), as well as tilting forward at angles of 10°, 20°, and 30° (denoted as 10°, 20°, and 30°), until it reached a balanced state. Subsequently, angle fluctuation graphs were generated, as depicted in Figure 8, Figure 9, Figure 10, Figure 11, Figure 12 and Figure 13. To assess the variations in the car’s stability across the three filtering algorithms, the integral absolute error (IAE) was employed as the error metric, where 𝐼𝐴𝐸=∑𝑘=200600|𝑒(𝑘)|$IAE={\displaystyle \sum _{k=200}^{600}\left|e\left(k\right)\right|}$.
在三种不同的滤波算法下，小车分别以 10°、20° 和 30°（分别表示为 -10°、-20° 和 -30°）的角度向后倾斜，以及以 10°、20° 和 30°（分别表示为 10°、20° 和 30°）的角度向前倾斜，直至达到平衡状态。随后，生成了角度波动图，如图 8、图 9、图 10、图 11、图 12 和图 13 所示。为了评估小车稳定性在三种过滤算法中的变化，采用了积分绝对误差（IAE）作为误差指标，其中 𝐼𝐴𝐸=∑𝑘=200600|𝑒(𝑘)|$IAE={\displaystyle \sum _{k=200}^{600}\left|e\left(k\right)\right|}$ .

Figure 8. Angular fluctuations of −10° back to equilibrium [20,21].
图 8.-10°角波动恢复平衡[ 20, 21]。

Figure 9. Angular fluctuations of −20° back to equilibrium [20,21].
图 9.-20°角波动恢复平衡[ 20, 21]。

Figure 10. Angular fluctuations of −30° back to equilibrium [20,21].
图 10.-30°角波动恢复平衡[ 20, 21]。

Figure 11. Angular fluctuations of 10° back to equilibrium [20,21].
图 11.10° 角波动恢复平衡[ 20, 21]。

Figure 12. Angular fluctuations of 20° back to equilibrium [20,21].
图 12.20° 角波动恢复平衡[ 20, 21]。

Figure 13. Angular fluctuations of 30° back to equilibrium [20,21].
图 13.30° 角波动恢复平衡[ 20, 21]。

Figure 8, Figure 9, Figure 10, Figure 11, Figure 12 and Figure 13 depict the effectiveness of the proposed fuzzy Kalman filtering algorithm, Kalman filtering algorithm [21], and complementary filtering algorithm [20] in restoring the system to a balanced state when the vehicle is tilted backward or forward by 10°, 20°, and 30°. Table 2 presents the time taken for the system to return to a balanced state under each algorithm, while Table 3 shows the IAE metric values for each algorithm at various tilt angles. The first 200 scales are the adjustment time for the two-wheeled self-balancing car at a certain starting angle, and the filtering algorithm is added at around the 200th scale. From the simulation results, it is evident that the jitter of the two-wheeled self-balancing car is minimized when employing the designed Kalman filtering algorithm proposed in this study compared to the other two algorithms. Additionally, the time required for the system to recover to a balanced state from the same angle is shorter, and the absolute error is smaller than with the other two algorithms. Note that there is a visible difference between positive and negative tilting, which may be caused by the measurement error in the experimental process.
图 8、图 9、图 10、图 11、图 12 和图 13 描述了当车辆向后或向前倾斜 10°、20° 和 30°时，所提出的模糊卡尔曼滤波算法、卡尔曼滤波算法 [ 21] 和互补滤波算法 [ 20] 恢复系统平衡状态的效果。表 2 列出了每种算法下系统恢复平衡状态所需的时间，表 3 则显示了每种算法在不同倾斜角度下的 IAE 指标值。前 200 个刻度是两轮自平衡车在某个起始角度下的调整时间，在第 200 个刻度左右加入了过滤算法。从仿真结果可以看出，与其他两种算法相比，采用本研究提出的卡尔曼滤波算法设计的两轮自平衡车的抖动最小。此外，与其他两种算法相比，系统从同一角度恢复到平衡状态所需的时间更短，绝对误差更小。需要注意的是，正倾斜和负倾斜之间存在明显差异，这可能是由实验过程中的测量误差造成的。

Table 2. Equilibrium time for different angles (unit: seconds).

		−10°	−20°	−30°	10°	20°	30°
Fuzzy Kalman filtering algorithm		0.75	1.05	1.60	0.83	1.01	1.63
Kalman filtering algorithm (Liu 2015) [21]		0.95	1.30	2.00	1.02	1.21	2.12
Complementary filtering algorithm (Liang 2015) [20]		1.45	1.80	2.45	1.43	1.85	2.39

Table 3. Tilt angle integral absolute error.

		−10°	−20°	−30°	10°	20°	30°
Fuzzy Kalman filtering algorithm		255.84	243.43	335.70	337.07	232.82	352.51
Kalman filtering algorithm (Liu 2015) [21]		302.27	298.31	398.30	356.58	271.24	473.89
Complementary filtering algorithm (Liang 2015) [21]		592.61	378.13	632.18	697.28	455.38	609.13

5. Conclusions 5.结论

In this paper, an attitude estimation algorithm based on a fuzzy Kalman filter is proposed, which uses fuzzy logic to correct the tilt angle of a two-wheeled self-balancing car, so as to optimize the state of the Kalman filter and improve the balance performance of the car. Under the same controller parameters, the effects of the fuzzy Kalman filtering algorithm, Kalman filter algorithm [21], and complementary filtering algorithm [20] are compared by experimenting with the two-wheeled self-balancing car, and the results show that the fuzzy Kalman filter algorithm designed in this note makes the two-wheeled self-balancing car more quickly and precisely return to the balance position. The fuzzy Kalman filter algorithm can effectively realize the balance control of two-wheeled cars, which has certain reference significance for the design and research of speed loops and steering rings of racing balance cars.
本文提出了一种基于模糊卡尔曼滤波器的姿态估计算法，利用模糊逻辑对两轮自平衡车的倾角进行修正，从而优化卡尔曼滤波器的状态，提高小车的平衡性能。在相同的控制器参数下，通过对两轮自平衡车的实验，比较了模糊卡尔曼滤波算法、卡尔曼滤波算法[ 21]和互补滤波算法[ 20]的效果，结果表明，本文设计的模糊卡尔曼滤波算法使两轮自平衡车更快速、精确地回到平衡位置。模糊卡尔曼滤波算法能有效实现两轮车的平衡控制，对赛车平衡车速度环和转向环的设计与研究具有一定的参考意义。