两轮自平衡车摆机器人建模与控制方法解析【附仿真】

✨ 长期致力于自平衡车摆机器人、平衡控制方法、模糊自适应算法、滤波算法、机器人建模研究工作，擅长数据搜集与处理、建模仿真、程序编写、仿真设计。
✅ 专业定制毕设、代码
✅如需沟通交流，点击《获取方式》

（1）基于自适应扩展卡尔曼滤波的姿态估计方法：

针对两轮自平衡车摆机器人中陀螺仪零漂和加速度计振动噪声的问题，提出一种自适应扩展卡尔曼滤波AEKF。系统状态变量包括倾斜角、倾斜角速度和陀螺仪偏置，观测变量为加速度计计算的倾斜角和陀螺仪角速度。AEKF的核心是噪声协方差矩阵的自适应调节：基于残差序列的实时协方差估计，采用滑动窗口（窗口大小50）计算新息协方差的理论值与实际值的比值，通过模糊逻辑调整过程噪声Q和观测噪声R。模糊规则表：当比值大于1.2时增大Q以信任观测，小于0.8时减小Q以信任模型。在机器人以1.5m/s行驶并经过5度斜坡时，AEKF估计的倾斜角误差为±0.3度，而标准EKF误差达到±1.2度。实验中采用MPU6050传感器，滤波更新频率200Hz，AEKF的单次计算耗时0.12ms，满足实时要求。

（2）基于拉格朗日方程的非线性动力学建模与递归LQR平衡控制：

建立车摆机器人的四自由度动力学模型（车身倾斜、车摆摆动、左右轮转动），通过拉格朗日方程推导出带约束的非线性微分方程组。将模型在平衡点线性化，得到7阶状态空间方程。提出一种递归LQR控制策略：在每个控制周期，根据当前状态实时重新计算LQR增益矩阵，而不是使用固定增益。由于直接求解Riccati方程计算量过大，采用一种离线计算增益簇加在线插值的方法：在倾斜角[-15°,15°]和摆角[-20°,20°]范围内离散成50x50个网格点，离线计算每个网格点的最优LQR增益，在线时通过双线性插值快速得到当前增益。仿真表明，递归LQR相比固定增益LQR在抵抗冲击扰动（5N推力）时恢复时间从1.2秒缩短到0.6秒，最大倾斜角从8度降至4度。在实物机器人上，递归LQR实现了1.0m/s平稳行驶和原地转弯。

（3）自适应模糊控制算法与非线性模型直接控制：

针对线性化模型无法覆盖大角度范围的问题，设计了一种直接基于非线性模型的自适应模糊控制器。该控制器采用Takagi-Sugeno模糊系统，输入为倾斜角和摆角，输出为电机力矩。输入空间划分成7个模糊集合（负大、负中、负小、零、正小、正中、正大），隶属函数采用高斯型。模糊规则共49条，规则结论参数通过自适应律在线更新。自适应律基于李雅普诺夫稳定性理论设计，保证跟踪误差收敛。在仿真中，给定一个30度的初始倾斜角，自适应模糊控制器在1.5秒内将机器人拉回到平衡位置，而LQR需要2.8秒且出现5度超调。实物实验包括平衡控制、直行控制、转向控制和抗干扰实验：在机器人平台上施加侧向推力（模拟行人碰撞），自适应模糊控制的恢复时间为0.4秒，最大偏离距离0.15m，优于LQR的0.7秒和0.28m。该方法还被推广到载人模式，驾驶员体重从50kg到80kg变化时，控制参数无需手动调整，系统自动适应。

import numpy as np from scipy.linalg import solve_continuous_are import matplotlib.pyplot as plt class AEKF: def __init__(self, dim_x=3, dim_z=2): self.x = np.zeros(dim_x) self.P = np.eye(dim_x) self.Q = np.eye(dim_x)*0.01 self.R = np.eye(dim_z)*0.1 self.F = np.eye(dim_x) self.H = np.array([[1,0,0],[0,1,0]]) self.window_size = 50 self.innovation_buffer = [] def predict(self, dt): self.F[0,1] = dt self.x = self.F @ self.x self.P = self.F @ self.P @ self.F.T + self.Q def update(self, z): y = z - self.H @ self.x self.innovation_buffer.append(y) if len(self.innovation_buffer) > self.window_size: self.innovation_buffer.pop(0) S = self.H @ self.P @ self.H.T + self.R K = self.P @ self.H.T @ np.linalg.inv(S) self.x = self.x + K @ y self.P = (np.eye(3) - K @ self.H) @ self.P if len(self.innovation_buffer) > 10: S_emp = np.cov(np.array(self.innovation_buffer).T) ratio = np.trace(S_emp) / np.trace(S) if ratio > 1.2: self.Q *= 1.05 elif ratio < 0.8: self.Q *= 0.95 return self.x[0] class RecursiveLQR: def __init__(self, A_grid, B_grid, Q, R): self.A_grid = A_grid self.B_grid = B_grid self.Q = Q self.R = R self.K_grid = {} for idx, (A, B) in enumerate(zip(A_grid, B_grid)): P = solve_continuous_are(A, B, Q, R) K = np.linalg.inv(R) @ B.T @ P self.K_grid[idx] = K def get_gain(self, theta, phi, theta_range=(-15,15), phi_range=(-20,20)): i_theta = int((theta - theta_range[0])/(theta_range[1]-theta_range[0]) * 49) i_phi = int((phi - phi_range[0])/(phi_range[1]-phi_range[0]) * 49) i_theta = np.clip(i_theta, 0, 49) i_phi = np.clip(i_phi, 0, 49) return self.K_grid[i_theta*50 + i_phi] def fuzzy_balance_controller(theta, phi, rule_params): mu = np.zeros(7) centers = [-20, -13.3, -6.7, 0, 6.7, 13.3, 20] sigma = 5.0 for i, c in enumerate(centers): mu[i] = np.exp(-0.5 * ((theta - c)/sigma)**2) u = 0 for i in range(7): for j in range(7): rule_fire = mu[i] * mu[j] u += rule_fire * rule_params[i*7+j] return np.clip(u, -12, 12)