基于卡尔曼滤波的自平衡电单车设计_刘展志.pdf

常熟理工学院学报（自然科学）Journal of Changshu Institute of Technology（Natural Sciences）第 37 卷第 2 期2023 年 3 月Vol.37 No.2Mar.,2023基于卡尔曼滤波的自平衡电单车设计刘展志，王彦栋，张 宁，李 鑫（常熟理工学院 电气与自动化工程学院，江苏 常熟 215500）摘要：针对自行车在运动过程中因自身不精确的系统模型和外界扰动等因素导致的车体不稳定甚至倾倒的问题，模拟自行车结构，设计了一种前后两轮的自平衡电单车控制系统 该系统采用卡尔曼滤波对 MPU6050 原始数据进行处理，系统的可行性得到了分析验证 在建立电单车物理模型后，选择位置式 PID 算法控制舵机，实现了电单车姿态的快速调整，最终使系统能够运行稳定关键词：卡尔曼滤波；姿态检测；舵机控制；PID 算法 中图分类号：TP242.6 文献标志码：A 文章编号：1008-2794（2023）02-0048-05收稿日期：2022-02-16通信作者：李鑫，高级实验师，研究方向：智能控制技术与现代检测技术，E-mail：lixin_自行车控制系统是无人驾驶技术的新兴问题无人自行车与常见的四轮车相比具有运动灵活、成本低、绿色环保 1 等优点，在日常出行解决交通拥堵等领域具有独特的应用前景 常规自行车由车把控制平衡，以后轮传动作为驱动，车身侧由骑行者调整自身重心及姿态以持续维持系统平衡，而在非连续控制的情况下系统极不稳定本文设计的自平衡电单车系统结构上与自行车类似且体型较小，采用低成本的检测和控制方法，能较好地模拟自行车自主行驶中的平衡和控制问题，可输出稳定的姿态角并使系统快速响应，以达到平衡和转向的目的1 系统总体设计姿态检测和舵机控制以 STC16F40 单片机作为主控制器，使用MPU6050 作为姿态传感器，并将数据经过卡尔曼滤波进行融合，最终得出系统的车身倾角 再将它与上位机发送的蓝牙信号叠加后可实时控制舵机改变目标角度2 系统硬件设计考虑到卡尔曼滤波在软件上的复杂性，本系统选用了一款 STC 公司新发布的性价比较高的 16 位单片机，速度比传统的 8051 约快 70 倍 系统使用单电机驱动后轮，驱动电路使用 DRV8701 芯片搭建 H 桥，舵机则图 1 系统设计框图使用了扭矩较大、响应速度较快的 S3010 电单车的运行方向通过蓝牙串口模块，由手机 APP 对其进行遥控对于姿态原始数据的读取则是采用软件模拟 I2C 协议，最终将数据发送到 OLED 屏幕上，便于观测和调试系统设计框图如图 1 所示DOI:10.16101/32-1749/z.2023.02.021第 2 期493 姿态检测算法设计3.1 姿态数据处理流程首先，通过单片机模拟 I2C 协议读取 MPU6050 的三轴加速度计和陀螺仪数据 1，配置相关寄存器得到用于数据处理的加速度值对三个轴与水平面之间的夹角用加速度反正切公式进行归一化设 accx、accy、accz 为归一化前 x、y、z 轴的加速度，按（1）式计算，（1）并将计算出的 x 轴方向和 y 轴方向的加速度与直接读出的角速度通过卡尔曼滤波进行数据融合处理，即可得出平稳的横滚角和俯仰角数据3.2 卡尔曼滤波自平衡电单车作为一种非线性的欠驱动系统极不稳定 2，因此要时刻保证车身的平衡即应引入性能较好的滤波和姿态融合算法，用以计算出电单车相对于原始数据更为精确的姿态在系统无法对外界干扰建模的情况下，需要先对系统进行先验估计，估计量 xk可由（2）式得到 （2）式中：A 为系统状态矩阵，xk-1为上一次状态变量的估计值，B 为控制矩阵，uk-1为修正量将相关概念映射到 MPU6050 中，以 x 轴方向为例，设加速度计测得的偏移角为 anglex，加速度计对其测量的误差为 Qbiasx，陀螺仪在 x 轴上的角速度为 Gyrox，dt 为控制器的采样周期，则有 （3）根据先验误差的协方差计算公式 （4）式中：Pk为第 k 次的先验估计协方差矩阵，Pk-1为第（k-1）次时误差的协方差矩阵，N 为过程噪声的协方差矩阵由于在 MPU6050 中，加速度噪声和角速度噪声独立，则可推导出在自平衡电单车中计算先验误差的协方差矩阵公式 （5）卡尔曼增益在算法中对滤波融合效果起着至关重要的作用，反映系统对误差的矫正深度，计算方法根据卡尔曼增益计算公式 （6）式中 R 为加速度计测量噪声的协方差，即真实测量数据误差的协方差，也就是带有高斯白噪声的测量值，记为 Zk设 R_angle 为 MPU6050 中的角度测量噪声，由此可以得出分别对应加速度计测量的误差量 Qbiasx、偏移角 anglex的卡尔曼增益 K0和 K1，计算公式为 （7）即 （8）将计算出的卡尔曼增益和加速度计的测量值 angle_new 代入后验估计方程，即可得到当前系统姿态的最优估计值刘展志，等：基于卡尔曼滤波的自平衡电单车设计xxxtttAAPk H TKk=HPk H T+R常熟理工学院学报（自然科学）2023 年50 （9）即 （10）此时所得的 anglex即为 x 轴方向的最优横滚角最后，根据更新误差协方差公式进行后验估计，可算出此时误差的协方差矩阵，方便下次解算 （11）其中 I 为单位矩阵，即 （12）同理，对于y轴方向的最优俯仰角的求解只需将偏移角angley及其测量的误差量 Qbiasy代入式（3），逐步推导即可求得俯仰角可用于高效检测行驶过程中的坡道路面，以便对电机转速和车体的平衡参数进行及时调节图 2 为卡尔曼滤波流程图3.3 滤波效果分析将卡尔曼滤波器的算法通过软件进行函数处理，将模拟噪声的协方差矩阵值从1100进行遍历，得出的卡尔曼滤波的后验估计值即为最优估计值当测量噪声干扰值较大时，系统的测量速图 2 卡尔曼滤波流程图图 3 速度比较折线图图 4 位置比较折线图度与实际速度的误差较大，但通过卡尔曼滤波的调节，仍可保证后验估计速度与实际速度折线相吻合同理，若增大过程噪声干扰值，系统则更依赖于测量速度，而先验估计的响应速度则会有所下降，但仍可满足自平衡系统对于数据实时更新的速度需求 3 速度比较折线如图 3 所示由于自平衡电单车更关注系统位置误差带来的影响，且对于位置误差，当调节噪声值时，发现实际位置、测量位置、先验估计位置和后验估计位置在数值上与速度曲线的变化趋势一致，但误差率相对于速度变化要小得多因此在相同的高斯白噪声干扰下，4 条折线都能做到基本重合，满足了系统对于姿态角度的测量精度需求 图4为位置比较折线图 通过调节卡尔曼滤波器中的高斯白噪声 Q 和 R 的参数对干扰信号进行预测，可以达到预测系统所处姿态角度的目的，且响应速度和位置估计的精确度完全能满足控制系统的需求4 平衡控制算法设计4.1 物理模型构建在完成姿态检测后，将程序最终输出的横滚角乘以一定的比例常数，串入舵机的目标转角中，即可实现舵机对电单车的平衡控制电单车的平衡控制原理可以从简单的舵机左右摆动出发，通过单环PID拟合物理速度/（ms-1）舵机角度/()HH第 2 期51理模型 最外面的 3 个圆分别为前轮转向轨迹、中心转向轨迹和后轮转向轨迹其中 b 为舵机转角，a 为车模重心在地面投影与后轮圆心在地面投影的夹角，R 为重心转向半径，R 为后轮转向半径，L 为车模前轮与地面接触点到车模后轮与地面接触点的距离，Lm为车模重心在地面的投影到后轮与地面接触点的距离 在离心力计算公式中 （13）其中 Ax为车模倾斜时重力在车模竖直方向的分量对舵机控制公式的推导过程为：最终可得车模的舵机转角 b 与车模倾斜时竖直方向的加速度 Ax的关系 通过实际车模的运行调试可知，使用此模型调节参数的效果较好，且能适应不同的场地.4.2 位置式 PID 及参数整定在建立物理模型后，选择采用位置式 PID 作为舵机控制算法设每次采样时间点为 kj（j=1,2,）用来模拟连续的时间，用矩形法数值积分替代积分，用一阶后向差分近似替代微分 5，公式为 （14）即得到位置式 PID 的离散表达形式 （15）式中：T 为采样周期，E(K-1)与 E(K)分别为第(k-1)和第 k 次由横滚角和蓝牙信号叠加后输入舵机的转角 在电单车行进过程中，要保证其按照蓝牙控制的方向行驶，需用上位机实时发送期望的舵机方向和打角量，与姿态解算得出的横滚角一并叠加，串入舵机的比例控制部分后，再使用位置式 PID 计算此时所需的加速度 6，并代入式（13）推导出此时舵机的转角，从而得到输出 PWM 的占空比，即可遥控电单车按任意方向行驶关键程序语句为：图 5 自平衡电单车物理模型模型的效果 但是算法对非线性系统调整效果较差，需要不断修改参数，因此调试难度较高对于电单车，可以先简化出物理模型，并将车模作为一个倒立摆，转向时的离心力充当控制倒立摆的加速度，且需要关联到舵机的转角大小经过一系列变换，并在物理模型中加入了速度影响因子后，对车模在不同平衡速度上的控制具有更好的适应性从车模方向看，电单车可近似为一阶倒立摆 4（左右两侧倾倒）倒立摆的控制原理为控制方向与倾倒方向一致，而对于电单车则需要进行一些转换物体在做圆周运动时会产生离心力，因此在电单车模型中就可以利用倾倒时倒立摆的重力在垂直车模即竖直方向的分量来抵消离心力 图5为自平衡电单车物刘展志，等：基于卡尔曼滤波的自平衡电单车设计EEETTT，Ax，LmAxLmAxLLmv2v2LmLmLm常熟理工学院学报（自然科学）2023 年52其中：Angle_Error_Count 为横滚角的积分值；Gyro 可以直接从陀螺仪中读出 y 轴方向的角速度值后，再进行简单的加权平均后求得，也可以用第 k 次横滚角减去第(k-1)次的横滚角数值求得Balance_P、Balance_I、Balance_d 是需要通过实验调整的参量；mulit 与 L、Lm 是通过建立的物理模型求出的常数；Bluetooth_value 为调试时发送的蓝牙信号值最终可以得到舵机的转角 Angle_out在调节 PID 参数的过程中，应先调试 P 和 D 参数P 参数能够较为直观地体现在直立和转向的响应效果上；D 参数则能够消除高频振荡，即防止舵机抖动 当PD两个参数调整到一个合适的范围内时，可以看出电单车沿既定方向行驶一段距离后开始转圈 此时加入I作为积分控制，可图 6 电单车外观使电单车能够保持更长时间且更稳定的直线行驶，此时电单车的抗逆性良好，即在一定程度的干扰下仍能保持稳定电单车外观如图 6 所示5 结语本设计利用卡尔曼滤波算法，精准得出了稳定的单车姿态倾角，再使用位置式PID算法控制舵机实现了车体的自平衡和转向，且在蓝牙信号的控制下可以长时间稳定运行 通过调试可使电单车在0.5 1.6 m/s之间的任一速度匀速行驶系统具有响应快、精度高且鲁棒性强等特点，较好地解决了自行车的相关平衡与控制问题参考文献 1 陈睿，李鑫基于 MEMS 的姿态检测仪的实现与设计 J 常熟理工学院学报，2020，34（8）：42-48 2 王涵，李艳基于切换 PID 的无人自行车的平衡控制 J 电子器件，2019，42（3）：774-780 3 孙蔚蓝，钱莉基于 MEMS 和卡尔曼滤波的姿态解算研究 J 自动化与仪器仪表，2018（1）：1-3 4 吕宽州，陈素霞，黄金振基于陀螺仪的无人自行车平衡控制方法 J 计算机测量与控制，2015，23（7）：2344-2346 5 杨龙基于位置式 PID 算法的压力控制设计及 MATLAB 仿真 J 电子技术与软件工程，2018，24：27 6 刘吉明，白小峰，何世安基于位置式 PID 温控系统设计 J 环境技术，2020，38（6）：124-127Design of Self Balancing Motorcycle Based on Kalman FilterLIU Zhanzhi,WANG Yandong,ZHANG Ning,LI Xin(School of Electrical and Autom