自平衡两轮车

自平衡两轮车

——21ic 程序匠人

引子——北京天安门长安街上的自平衡两轮车

认识平衡车

关键字：

Segway

同轴两轮

自平衡（姿态控制）

惯性导航(MEMS)：陀螺仪、加速度计

数据融合：卡尔曼滤波(Kalman Filter)

硬件框架

MCU

SH88F2051

电源

3V 、5V

加速度传感器

MMA7361L

陀螺仪

ENC-03M

电机驱动

L293D

电机测速

H42B6

直射式红外光电传感器

电机电压

JTAG

串口

XY 加速度

角速度

PWM 脉冲

平衡原理

加速度传感器信号

根据加速度计算角度

ADRx

ADRy

-225

-180-135-90-4504590135180225270315360405450495540180

135

90

45

-45

-90

-135

-180

真实角度双轴检测到的角度单轴检测到的角度ADRx ADRy

根据单轴信号求角度acc = -asin(Ax)*180/π根据双轴信号求角度acc = atan2(Ax,Ay)*180/π

单轴求角度范围：+90~-90，超出部分出错

陀螺仪与加速度传感器信号

加速度计

●测量线性运动

●输出加速度

●速度变化越快，输出量越大

●通过三角函数计算可得到加速度计当前与重

力方向的夹角。

陀螺仪

●测量旋转运动

●输出角速度

●旋转越快，输出量越大

●有了角速度数据后即可通过积分获得角度

数据。

快速转动时慢速转动时

陀螺仪

信号

加速度计

信号

两种传感器信号优缺点对比

加速度计

●优点：无累积误差，长时间

稳定。

●缺点：

●1、加速度计对震动非常敏

感。电机转动以及路面崎岖

等因素所产生的震动会有很

大的噪声。

●2、当加速度计运动时，其

输出量是运动加速度与重力

加速度的混合数据，这会严

重影响角度计算的准确性。

陀螺仪

●优点：数据噪声较小，短

时间内误差小。

●缺点：

●1、陀螺仪以及放大电路有

温漂。

●2、积分会产生累积误差，

这种误差会随着时间推移

而越来越严重，导致数据

失效。

数据融合（取长补短）

●加速度计长时间较准确，短时间误差大。

●陀螺仪短时间准确，长时间不准确。

●通过对两个传感器的有效融合和估计，可以获得合适的

角度信息。

●用加速度传感器长时间稳定的特性，

弥补陀螺仪的零点漂移及A/D采样值

单调性误差积累增长。

卡尔曼滤波：根据上一状态的估计值和当前状态的观测值推出当前状态的估计值。

《线性滤波与预测问题的新方法》

论文作者：kalman.1960《A New Approach to Linear Filtering and Prediction Problems 》

角速度

加速度角度卡尔曼

滤波器

真实角度

姿态控制（PID调节PWM占空）

duty = Kp_ang * angle ;// = 比例系数*角度

duty += Kd_ang * angle_dot ;// + 微分系数*角速度duty += Kp_spd * speed ;// + 比例系数*车速

duty += Ki_spd * position ;// + 积分系数*车位移

整定方法：

●1.先调角度K值——让小车站起来。

●2.调角速度K值——让小车站稳定。

●3.调水平速度K值和位移K值——让小车跑起来。

软件开发（KEIL ）

JTAG调试（S-Lab）

串口辅助调试

帧格式：帧长+ 命令+ 数据+ 校验和

对象

帧命令对象帧命令读软件版本0X05参数初始化0X0A 加速度计X 轴AD 结果0X50PWM 占空0X5A 加速度计Y 轴AD 结果0X51X 轴基准0X60陀螺仪AD 结果0X52X 轴比例0X61加速度传感器计算角度0X53Y 轴基准0X62陀螺仪计算角速度

0X54

Y 轴比例

0X63

滤波之后的角度0X55陀螺仪基准0X64滤波之后的角速度0X56调试电位器1AD 结果0X70滤波之后的角速度偏差0X57调试电位器2AD 结果0X71滤波之后的车速0X58调试电位器3AD 结果0X72滤波之后的车位移

0X59

调试电位器4AD 结果

0X73

通讯协议

PC 与MCU 之间以帧为通讯单位。

PC 发送命令帧查询。（PC －> MCU ） MCU 发送应答帧。（MCU －>PC ） 命令帧和应答帧格式相同，内容不同。

《串口猎人》

推荐理由：（需要理由吗？不需要吗？需要吗？……）

✓自动轮发（自动添加帧头、帧尾、帧长、校验和）

✓自动接收（灵活的帧格式）

✓提取数据（8个通道、多字节、BCD/HEX、有/无符号通吃）

✓图形显示（示波器、码表、柱状图）

✓数据保存（有利于后续分析）

✓免费使用（欢迎提出宝贵建议）

下载网址：/user1/349/archives/2011/84521.html