自己做的双轮自平衡小车

zlstone

自己做的自平衡小车，基本达到预期效果，图片（拍摄效果不佳）及文件如下，视频请见：
http://v.youku.com/v_show/id_XMTc3ODg0Mjky.html
http://v.youku.com/v_show/id_XMTgxNTAxOTA4.html
http://v.youku.com/v_show/id_XMTgxNTAzMTI4.html
资料供大家参考；
所有制作资料ourdev_562070.rar(文件大小:1.44M) (原文件名:ProjectRII.rar)

照片 (原文件名:RII_2.jpg)


照片 (原文件名:RII_1.jpg)


照片 (原文件名:RII_3.jpg)

06023116

MARK

songxiang

动作够快的，赞一个！

zbjzxc

记号~~

mankata

NB啊！

pinocchio

mark

Argee

N!做个大的载人阿,出套件，8-）

plc_avr

强贴，MARK！

guhenggao

mark

odqqdo

强，做个大的载人阿,出套件，8-）  赞同

ly520cs

mark

lrzxc

mark~~

lwqdyxgood

楼主很强，前两年我也做过，不过当时只有加速度传感器，没有倾角测量和陀螺仪。测出的加速度当然是重力和小车动作加速度的合成。这个问题一直没有解决，后来只能作罢，很遗憾。如今看见楼主大作，很是欢喜，方便的话能不能公布你的方案啊，让大家也学习学习。

zlstone

控制器：ATmega16；8MHz；
加速度传感器：MMA2260；陀螺仪：EWTS82；
传感器的融合：卡尔曼滤波；
马达：EN_2342CR（速比64）+双路12脉冲编码器+CD40106对信号整形；
驱动板芯片：CD4001+IR2111+IRF1404（驱动电流可以很大）；
制作资料在压缩包里面，供参考；

renxupeng

mark

iamhope

楼主好牛牛牛！

highnose

深夜，顶

zlstone

今天做了取消马达转速传感器的测试，利用PWM的值低通滤波后代替转速信号，也可以稳定控制，但在移动距离偏差较大时不能修正，小范围内和利用传感器闭环控制差不多；源文件如下；
点击此处下载 ourdev_562809PCVEZ9.txt(文件大小:11K) (原文件名:main_speed_sensor_less.c.txt)

gyl19881023

回复【楼主位】zlstone
-----------------------------------------------------------------------

牛人啊

hahahagg

环路控制怎么这么慢啊?
振荡很久才稳定...

fengyuzhails

楼主好牛啊！～最近一直在各大网站搜索关于两轮平衡小车的文章，然后去理解学习，不过国内这方面愿意公开的人少之又少，看得我那是一头雾水啊……包括国内一网站的开源“家酿双轮平衡车”也是不了了之，而且也没有对平衡小车的算法和工作原理进行介绍。到Google上搜了一些国外的，有几个开源项目，不过都是大型的平衡车，觉得做起来有点不靠谱，所以想做一属于自己的小车实在难上加难啊。
幸得楼主资料，观看视频后，叹之又叹啊！～做的效果实在太棒了！～而且性能也十分优越。可是楼主您虽然公布了全部的东东，但是您就像是给了肉体，却没有灵魂啊！～您能不能就您的作品经行一些详细的分析呢？或者写个自己的报告，让大家顶礼膜拜哈！～学习楼主的算法，了解楼主小车的工作原理，我想是每个来自阿莫论坛坛友真正想要的东西！～
悄悄地说一声：楼主您的电路咱打不开……下个软件说要3.8G，（我勒个擦……实在太大）
楼主如果把一堆资料扔在这，多半就沉默了，开自己的帖子，写自己的文章，这样好东西就不会埋没下去，让大家顶起来！～

a___yue

视频超酷！

format

敢问楼主的车轮和电机哪里买的？

价格？

我前几天有构思

比较简单，7260+增量式PID+pwm

程序写完，但是车轮和电机木有

电机是减速电机，轮子是卡带机的皮带轮

不理想~~~~~~~

zlstone

OK：其实控制很简单，利用定时器产生10ms中断，每次中断进行如下工作：
1：AD采样加速度传感器和陀螺仪，然后卡尔曼滤波得出角度与角速度；（滤波模块借鉴老外的）（互补滤波效果感觉不是很好）；
2：计算车轮的速度，积分得出位置；
3：利用PD算法得出PWM值=K1*angle + K2*angle_dot + K3*speed + K4 * position；
4：前进后退给定参考速度计算即可；
注：
1：增量式轮速传感器A相中断，读取B相电位判断前进还是后退，在10ms的时间内累加，计算车轮的速度；
2：由于10ms累加的轮速信号不多，直接计算车体会发抖，所以增加了低通滤波，解决问题；
3：我在外面增加4个电位器可以手动调节4个K值，方便调试；
以上；

zlstone

附件的文件夹里面有matlab建模的资料，可以求出所需的4个K的值，（参考NXT的），大家可自行调试K值看看对车辆的影响；

zlstone

回复【20楼】hahahagg
环路控制怎么这么慢啊?
振荡很久才稳定...
-----------------------------------------------------------------------

其实我也在思考，可能原因：
1：对速度做了滤波导致相位延迟；
2：倾斜角速度和车轮速度的K值不够大；
经实践：K值增大后，振荡减小；:)

zlstone

回复【23楼】format
敢问楼主的车轮和电机哪里买的？
价格？
-----------------------------------------------------------------------

电机在淘宝上买二手的，大几十元一个，因为考虑到减速器，而且带传感器；
其实后面实践得知可以不要位置传感器，这样选择电机的空间就很大了，买便宜的；
建议：
1：电机扭矩要大；说白了，功率要大；转动惯量大似乎的更稳定，所以不用空心杯的也许更好；
2：减速比可以大一点，用较大的轮子；
3：重心越靠下，抗扰性越强；但是可能会影响稳定性；

zlstone

回复【21楼】fengyuzhails
-----------------------------------------------------------------------

谢谢fengyuzhails 的热情，作为回报，我把电路图转为Protel99se的格式了；但请注意以下说明：
1：控制板和驱动板我做了改动，增加了RS232，但我没试验过，可能有bug；sorry，原版的因为实在有很多不好的地方，所以...
2：驱动板的IR2111如果可以用IR2184最好，不行的话加个光耦吧，不要学我的电路，加个上拉电阻去匹配电平；不得已...
所以原理图供参考，遥控部分就自己发挥吧，呵呵，我用的是普通的改的，不值得借鉴；

附件忘了挂了，补在33楼；

fengyuzhails

谢谢楼主的回答！～这个平衡小车希望暑假能够成行。这几天也在消化楼主的资料。您的电机我在淘宝上看到了，不知道是不是一样的，现在贴出来给您看看！～

淘宝网上搜到的，我想和楼主您的电机是一样的吧 (原文件名:电机.JPG)


另外以下是对这个电机的描述：

型    号：德国冯哈勃Faulhaber带编码器空心杯减速电机2342L012（12V时120转每分钟）

工作电压: 12V

空载转速：8100RPM（转每分钟）

减速后速：120RPM（转每分钟）

直    　径：30mm

电机长度：42mm

总共长度：85mm

出轴直径：6mm

出轴长度：35mm

输出功率：17W

扭　　矩：大扭矩( 1.72Nm)

空载电流：75mA

堵转电流：1400mA

减速箱型号：日本定制（全金属精密行星减速箱）

减速比：64：1

编码器：光电式

输出路数：ＡＢ双路输出

每圈脉冲：12CPR（脉冲每圈）［可以改装其他高分辨率的光电编码器］

fengyuzhails

让我们把这个帖子火起来吧！～
下面这个机器人是我从国外网上搜到的，作者全部开源，包括电路图，程序，以及一些硬件结构。并且我到网上查了一下，外国佬电路里面的所有元器件都可以在淘宝里面买到，所以要仿制的朋友有福了。不过可惜的是外国佬好像说自己做的东西有点问题，咱英语不好，至于什么问题，请大家到他网站上研究研究，呵呵！～
网址是：http://www.robotdungeon.com/RoboticsProjectBalancingExplorer.html

我贴几张图片吧：

(原文件名:BETitle.jpg)


(原文件名:BEChassis.jpg)


(原文件名:BEMotors.jpg)


(原文件名:BEPCBs.jpg)


(原文件名:BEBrain.jpg)


(原文件名:BEControlBoard.jpg)


(原文件名:BEBrain.jpg)


(原文件名:BEMotorDrivers.jpg)


(原文件名:BESensorBoard.jpg)


这个是配置表 (原文件名:未命名.JPG)


以下是电路及代码：
主控制电路ourdev_563383WN4D5S.pdf(文件大小:76K) (原文件名:BESchematic.pdf)
传感器电路ourdev_563384XCZ5TB.pdf(文件大小:57K) (原文件名:BESchematic2.pdf)
代码ourdev_563385ZMMFO6.rar(文件大小:4K) (原文件名:BEMain.rar)




有不对的地方请大家指正，我提供的只是外国佬的电路程序，目的是让大家结合楼主的作品来学习，楼主的作品让人瞠目结舌。希望阿莫论坛在“zlstone”的带领下，是双轮平衡小车发扬光大！～

zlstone

回复【29楼】fengyuzhails 刘峰
-----------------------------------------------------------------------

我用的就是这个电机，二手的哈，也是在这个地方买的，功率还可以，Max：17W；好处就是带测速的，很方便，不用自己改装；
不用传感器虽然也可以在平路运行，但爬坡就有问题了:-）

fengyuzhails

回复【31楼】zlstone
-----------------------------------------------------------------------

哈哈！～那得赶快购买几个做了，二手的，卖光了就没得买了，嘻嘻！～我现在还在忙期末考，等考完后，先设计一个两轮结构，再做电路。到时候上图！～对了，楼主您的Protel格式电路在哪啊？貌似没找到……

zlstone

汗！忘了加上附件了，protel99格式的电路图，补上：
点击此处下载 ourdev_563431QMA9X5.rar(文件大小:75K) (原文件名:PCB_Schematic.rar)

markloveyou

牛

fengyuzhails

回复【33楼】zlstone
-----------------------------------------------------------------------

谢啦！～楼主，呵呵！～

我再上几张从网上找的小车图片，以供大家模仿！～


这个是国内一家网站上的开源作品“家酿双轮平衡车” (原文件名:1_1240836199QvLc.jpg)


同上“家酿双轮平衡车” (原文件名:家酿双轮平衡机器人车体图纸.jpg)


大家对他再熟悉不过了吧，相当经典啊 (原文件名:01-00000.jpg)


(原文件名:02-00000.jpg)


(原文件名:balancing-robot-known-as-nBot.jpg)


(原文件名:FrontIso.jpg)


(原文件名:ourdev_513258.jpg)


这个是本论坛一网友的作品 (原文件名:ourdev_544052.jpg)


(原文件名:Two-Wheel%20Robot_2.jpg)

希望这些图片能给那些不擅长做机械的同学一点帮助！～

fengyuzhails

顶一下吧，好帖子不能沉下去啦……

放几篇自平衡机器人论文
自平衡两轮机器人的分层模糊控制ourdev_563489VUE32M.pdf(文件大小:520K) (原文件名:自平衡两轮机器人的分层模糊控制.pdf)
一种复杂环境下两轮自平衡机器人稳定控制研究ourdev_563490WJ92KN.pdf(文件大小:447K) (原文件名:一种复杂环境下两轮自平衡机器人稳定控制研究.pdf)
双轮移动机器人运动目标追踪与避障控制算法ourdev_563491YFX5S7.pdf(文件大小:386K) (原文件名:双轮移动机器人运动目标追踪与避障控制算法.pdf)
平衡传感器使用手册ourdev_563492KP2OIJ.pdf(文件大小:161K) (原文件名:平衡传感器使用手册.pdf)
两轮自平衡机器人动力学建模及其平衡控制ourdev_563493TTO6MF.pdf(文件大小:318K) (原文件名:两轮自平衡机器人动力学建模及其平衡控制.pdf)

aliveghost

mark

a___yue

我再来顶一把，确实了得！

zlstone

其实我做的并不是很好，国外用乐高NXT做的才好，我的建模也是借鉴他的，可惜youtube上不去，里面有他的视频；我将NXT的资料发上来，供大家参考：
点击此处下载 ourdev_563625DGJPW5.zip(文件大小:8.29M) (原文件名:nxtway_gs.zip)
也可以自己到matlab center下载：
http://www.mathworks.com/matlabcentral/fileexchange/19147-nxtway-gs-self-balancing-two-wheeled-robot-controller-design

fengyuzhails

OK，呵呵！～谢谢啊，对了zlstone，你的石英陀螺仪淘宝上没有哇，阿里巴巴搜到了，但不知道多少钱啊。

fsclub

楼主提到用了陀螺仪ewts82,我手里也有几片。但据我所知这芯片是角速度计,不是陀螺吧?

zhengrob

赞一个！楼主做得已经很不错了。问一下，遥控通过改变哪个量使小车前进和后退？

zlstone

我很早买的，应该不会很贵才是，你可以用其他的替代啊，ENC-03应该也可以，或者这个
http://item.taobao.com/item.htm?id=4754657136
现在确实不知道在哪买的了，加速度传感器也可以用其他的，我是贪图我用的这两个传感器都是5V供电，所以... :-)

zlstone

http://item.taobao.com/item.htm?id=2701813217
终于帮你找了一家，你自己看看吧:-)
这里你也可以联系；
http://www.ureika.net/newshow.asp?Cid=4&sid=99&PID=137
这个似乎也不错哦，数字接口的加速度记；
http://item.taobao.com/item.htm?id=3352638324

zlstone

回复【42楼】zhengrob
赞一个！楼主做得已经很不错了。问一下，遥控通过改变哪个量使小车前进和后退？
-----------------------------------------------------------------------

右遥控直接增加左边电机PWM，减少右边的PWM；向左反之；前进则是在速度误差那里加上一个固定的数值，后退则是减去这个数值；
因为我用的是普通玩具车遥控器，所以没有速度比例的调节；

psocfans

mark

fengyuzhails

回复【44楼】zlstone
-----------------------------------------------------------------------

谢谢哈！～我一直以为自己在淘宝江湖你混的不错，不过还是没你厉害啊，居然找到了。
呵呵，不知道这个传感器温漂如何，我想看看吧！～

shark313

之前有不少坛子里的兄弟买了不少ewts82的,其实可以跟他们商量一下,如果他们手上有暂时不用的,可以转让点出来

fsclub

我手里就有几个。
但这玩意不是陀螺仪，是角速度计，

110112110

mark

dogg

强帖待观赏

zl_diy

不错。好资料

wjd40

有个点子。。
楼主这个方案可以改进成 “本田U3-X 独轮车 ” 的功能吗?
两套分别控制x,y方向。。

  那个复杂的轮子可以用皮球代替，就像机械鼠标那个橡胶球。。

zlstone

呵呵，现在没那么多精力，不过应该没什么问题，做过这个后有许多经验了；

fengyuzhails

楼上说的对，想做个小的，然后再挑战大一点的，搞个载人的，可能得换性能更好的传感器或者驱动电路得加强，楼主有什么思路？

12fen

有意思，做个载人的吧，

zlstone

其实我的板子就是用来做载人的，电路的接口和驱动板都是按照载人的设计的，电机选的是尤奈特MY1020Z，24V的，现在变懒了不太想做了；

fengyuzhails

回复【59楼】zlstone
-----------------------------------------------------------------------

哇！～帅！～如果小的双轮我做出来的话，我会把它变成大的！～到时候请楼主吃饭哈！～呵呵！～不过做成载人的，前进倒退及转向怎么控制呢？遥控那一部分程序要不要去掉，还是，人站上去就OK了？然后通过身体倾斜啥的就可以转向？

zlstone

你要做载人的，参考这些网页吧；
http://tlb.org/scooter.html
http://www.circuitcellar.com/avr2006/winners/de/at3329.htm
http://www.zzaag.org/index_eng.html

fengyuzhails

回复【61楼】zlstone
-----------------------------------------------------------------------

不得不再佩服楼主的搜索能力啊，汗颜！~呵呵~~

楼主能自我介绍一下吗？我很感兴趣哈！~
我是南京一所大专院校的大二学生，马上大三了，大三就是最后一年。参加过全国大学生电子竞赛。
现在想把这个当做自己的毕业设计，由于水平有限，资料不足的情况下，想先试做您的小车，成功后，我再设计一款自己的小车！~请不要见笑。

楼主方便的请留一下您的联系方式给我，我的邮箱是：fengyuzhails@126.com

您开源出自己的设计，我十分感激，因此暑假想送寄送一个用步进电机做的小车平台给您，驱动板是我自己设计的。

十分想向您学习学习，请多多指教啊！~

zlstone

其实自己做这个很久了，所以找了很多资料；我毕业7年多了，AVR也是前两年自学的，根本不是什么高手，只是自己爱好而已；小车平台先谢过了，因为自己目前还用不到，另外你还是学生，不应该让你破费的，我的邮箱zlzlstone@163.com ，相互交流吧；
另外做电子DIY都不是太难，重要的是如何建模，解决控制问题，所以，希望你在学校里能学好高等数学和控制理论，这些才是基础，也决定了你将来能在这条道路上走多远，要把自己的爱好和将来的工作结合起来，社会需要像你们这样有志的学生，但一定要打牢基础，希望你继续坚持自己的理想并预祝你早日成功，加油吧！

STM32_Study

敬佩zlstone的开源精神，以上讨论让人赞叹。阿莫应该要给酷啊！！

请问zlstone

利用角速度信号和加速度信号，是不是就能够准确计算出当前小车的倾角？

经过滤波之后，计算出来的结果稳定度怎样？

dongwen543

强啊

zlstone

常用计算方法有两种：互补滤波和卡尔曼滤波；都可以输出校正后的角度与角速度；我采用的是卡尔曼滤波，考虑到单片机的运算能力，是经过精简的，当然这些都是老外做的，关于卡尔曼滤波，我找了很多资料，较好的如下：
点击此处下载 ourdev_565626J3TP8W.pdf(文件大小:76K) (原文件名:Kalman Filter Tutorial for Balancing Robot_________.pdf)
点击此处下载 ourdev_565627SCGDWJ.pdf(文件大小:540K) (原文件名:kalman_intro_chinese_________.pdf)
互补滤波如下：点击此处下载 ourdev_565628SM4NEV.rar(文件大小:1.22M) (原文件名:filter.rar)
我觉得卡尔曼的效果很好，但是初期有较大波动，要经过一段时间才能稳定；
http://www.docin.com/p-52169676.html
这个地方的可供参考；

lin1936

回复【楼主位】zlstone
-----------------------------------------------------------------------

请问哪里可以买到陀螺仪啊？会不会很贵呢

lin1936

回复【12楼】lwqdyxgood
-----------------------------------------------------------------------

我以前也想用加速度传感器做，现在发现没有用，又买了个角度传感器（陀螺仪）

wenshui78

楼主，小车的轮胎、固定转轴和轮胎的器件是从何处购得的，忘告知，谢谢。

yangyh75

我一年前的双轮车——http://v.youku.com/v_show/id_XOTQwNjg0MjQ=.html（http://player.youku.com/player.php/sid/XOTQwNjg0MjQ=/v.swf）——用的电机好楼主一样，arduino主控，mma7260+idg300。如果只是控制小车平衡站立的话，编码器可以不用。

zlstone

回复69楼：
轮胎是从旅行包拆下来改装的，自己加了个转接的东东；没有固定转轴，直接用电机转轴接轮胎；

lin1936

回复【70楼】yangyh75
-----------------------------------------------------------------------
都是强人，好厉害啊

haleycui

回复【楼主位】zlstone
-----------------------------------------------------------------------

您好，谢谢您开源的代码和pcb板图，我是一个刚刚高中毕业的电子爱好者，早就计划制作平衡车，但上手制作之后发现平衡车的控制原理没有自己想象的那么简单。本以为平衡车的源代码到处都是，可一翻搜索，只见到Geoffrey D Bennett先生提供的源码，又没有电路图，对于一个只学过51单片机的我来说看他的代码简直就是天书，后来打算自学avr gcc，但gcc方面的学习资料非常少。后来无意间发现了您的视频，而且您贡献了全部的资料，最重要的是还有PCB，而且代码是ICC的。这使我欣喜若狂。在研究了您的代码和PCB后我马上就印制的PCB，进行了组装，但目前发现了两个错误，一个是板图上没有接晶振（我知道AVR又内部RC振荡器，但只用内部8MHz RC振荡器不能满足串口波特率精确度，总发生乱码）第二就是MAX3232的信号接收端连接有问题，无法让单片机接收到数据，我将原本的串口3引脚连接的MAX3232的7引脚变为MAX3232的13引脚。

    如果这个平衡小车做成功，我还想做个载人的。但您说的“尤奈特”电机好像不带编码器，这个问题怎么解决？谢谢。

zlstone

呵呵，的确我偷懒没有用晶振，另外，因为我用的板子是没有串口的,我是引线出来的，新的PCB我没有试验过，所以可能有一些问题，Sorry；
关于电机编码器，我已经试验过了不用传感器也可以，PWM输出的值滤波后代替即可，可以参见本贴17楼和61楼；:-)
我的打算是在低速的时候取消位置闭环控制，在防止超速的时候再加入，但还没有试验过，应该没问题，也可以参考Mr.Tlb的；
http://tlb.org/scooter.html ；

ggg12345

你做的真是太棒了

ourdev2010

呵呵，的确我偷懒没有用晶振，另外，因为我用的板子是没有串口的,我是引线出来的，新的PCB我没有试验过，所以可能有一些问题，Sorry；
关于电机编码器，我已经试验过了不用传感器也可以，PWM输出的值滤波后代替即可，可以参见本贴17楼和61楼；:-)
我的打算是在低速的时候取消位置闭环控制，在防止超速的时候再加入，但还没有试验过，应该没问题，也可以参考Mr.Tlb的；
http://tlb.org/scooter.html ；

记号

wuxi_stl

mark

haleycui

mark

xdco

mark

haleycui

按照您的资料我终于在昨晚完成了平衡小车的制作。十分感谢!目前我只用了一个电机控制板，将两只电机并联起来用。能够初步达到平衡状态，但它的可靠性还不是很好，稍微一碰就倒，我用的是65mm的轮胎，是不是因为太小，速度不够呀？

(原文件名:IMAG0084.jpg)


(原文件名:IMAG0085.jpg)


(原文件名:IMAG0086.jpg)


(原文件名:IMAG0087.jpg)

haleycui

视频地址是http://v.youku.com/v_show/id_XMTg5ODExMjA0.html

haleycui

小车还有一个比较严重的问题，从昨晚开始，在实验过程中，已经烧坏两片Mega16了，这是怎么回事，在线等

zlstone

抱歉我的疏忽忘了说明，驱动板的R18是连接Battery+电源与VCC（+12V）的，如果一致可以将其短接，如果Battery+与VCC不一致则不要接R18；
看你的图片，是采用短接R18，即VCC与马达供电电压一致，应该不会出问题，因为我也是这样接的；你要不先检测一下Mega的供电是否正常，摸摸芯片是否发烫，再仔细检查一下？我用的控制板因为有许多不好的地方，我发出来的板子是做了改善的（增加串口，原件布局改善），我没有重新试做过，所以还需要你自己认真检查一下；
另外，上次我做的板子有一个地方是厂家做的短路了，铜箔连在一起了，你不妨也检查一下；
你动作够快，赞一个！

zlstone

你讲的控制不稳定跟轮子小有一定关系，因为电机的速比较大，所以大一点的轮子比较好，仿真结果也是如此；
另外你不妨适当增大速度和位置的K值，可能会有所改善，因为这样会减小速度的振荡，速度越大，电机的扭矩就会越小；
板子上的4个电位器就是方便调节4个K值设定的，我是通过仿真算出基础的K值，然后再调整电位器获得理想的状态的：-）

lin1936

天天都在研读楼主的程序，可还是无果！看来avr没有学校，支持楼主

lin1936

弱弱的问下大虾们，angle_dot是什么意思呢

ostrich12

楼主的ewts82 在震动环境里是否可以保持真确的输出，我现在有ewts84，想用于载人的小车上不知可否。



还有，阿莫，这样的好帖，还有楼主积极的回复，没有裤子好像说不过去吧。

zlstone

我的程序很简单啊，晚上再传一个精简的；
回复【86楼】 lin1936 ：angle_dot指的是角速度，即角度求导；
回复【87楼】 ostrich12 ：因为采用kalman滤波，所以在震动环境下运行也没问题；
倒是【82楼】 haleycui反馈的问题，大家要注意下，我后面不会再做新的板子，所以请大家自己检查一下板子再开工，或者等 haleycui 调查清楚再做，有劳他了；

esdart

收藏

haleycui

您好zlstone ，您介意给我一个您的e-mail么，我真怕以后遇到了问题联系不到您

zlstone

zlzlstone@163.com

lin1936

学校放假了，在火车上得坐两天，但是没有忘记看楼主无私贡献出的程序！天天看着程序，忽然发现自己能把它背下来了，可是小弟只自学了51的C设计，不能理解程序，期待楼主的精简版的。现在在火车上我是一边看楼主的程序，一边再学AVR.有句话说能勤能补拙，希望自己努力会有果！还有就是真心的感谢楼主每天那么多的辛苦的回复！

zlstone

无速度传感器的精简版程序:供大家参考；
点击此处下载 ourdev_568263US6NZ3.txt(文件大小:6K) (原文件名:main_speed_sensor_less.c.txt)

shark313

精简版运行效果跟原来的有什么区别，是比原来的更稳定了，还是不如原来的，编码器作用是不是很大？

zlstone

只是删除了RS232通讯和多余的注释（包括互补滤波），效果和原来的一样，这个是没有速度传感器的版本；片内8M振荡器；

ostrich12

关于卡尔曼滤波的解释。正在看，大家看看。

1．    什么是卡尔曼滤波器
（What is the Kalman Filter?）

在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！

卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。如果对这编论文有兴趣，可以到这里的地址下载： http://www.cs.unc.edu/~welch/media/pdf/Kalman1960.pdf。

简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导_弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。

2．卡尔曼滤波器的介绍
（Introduction to the Kalman Filter）

为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。

在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。

假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。

好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。

假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。

由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因为Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。

现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。

就是这样，卡尔曼滤波器就不断的把covariance递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的covariance。上面的Kg，就是卡尔曼增益（Kalman Gain）。他可以随不同的时刻而改变他自己的值，是不是很神奇！

下面就要言归正传，讨论真正工程系统上的卡尔曼。

3．    卡尔曼滤波器算法
（The Kalman Filter Algorithm）

在这一部分，我们就来描述源于Dr Kalman 的卡尔曼滤波器。下面的描述，会涉及一些基本的概念知识，包括概率（Probability），随即变量（Random Variable），高斯或正态分配（Gaussian Distribution）还有State-space Model等等。但对于卡尔曼滤波器的详细证明，这里不能一一描述。

首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程（Linear Stochastic Difference equation）来描述：
X(k)=A X(k-1)+B U(k)+W(k)
再加上系统的测量值：
Z(k)=H X(k)+V(k)
上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵。Z(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的covariance 分别是Q，R（这里我们假设他们不随系统状态变化而变化）。

对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的covariances 来估算系统的最优化输出（类似上一节那个温度的例子）。

首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：
X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)
式(1)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。

到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k-1)的covariance还没更新。我们用P表示covariance：
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
式(2)中，P(k|k-1)是X(k|k-1)对应的covariance，P(k-1|k-1)是X(k-1|k-1)对应的covariance，A’表示A的转置矩阵，Q是系统过程的covariance。式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。

现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态(k)的最优化估算值X(k|k)：
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
其中Kg为卡尔曼增益(Kalman Gain)：
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)

到现在为止，我们已经得到了k状态下最优的估算值X(k|k)。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的covariance：
P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)
其中I 为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。

卡尔曼滤波器的原理基本描述了，式子1，2，3，4和5就是他的5 个基本公式。根据这5个公式，可以很容易的实现计算机的程序。

下面，我会用程序举一个实际运行的例子。。。
4．    简单例子
（A Simple Example）

这里我们结合第二第三节，举一个非常简单的例子来说明卡尔曼滤波器的工作过程。所举的例子是进一步描述第二节的例子，而且还会配以程序模拟结果。

根据第二节的描述，把房间看成一个系统，然后对这个系统建模。当然，我们见的模型不需要非常地精确。我们所知道的这个房间的温度是跟前一时刻的温度相同的，所以A=1。没有控制量，所以U(k)=0。因此得出：
X(k|k-1)=X(k-1|k-1) ……….. (6)
式子（2）可以改成：
P(k|k-1)=P(k-1|k-1) +Q ……… (7)

因为测量的值是温度计的，跟温度直接对应，所以H=1。式子3，4，5可以改成以下：
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) ……… (8)
Kg(k)= P(k|k-1) / (P(k|k-1) + R) ……… (9)
P(k|k)=（1-Kg(k)）P(k|k-1) ……… (10)

现在我们模拟一组测量值作为输入。假设房间的真实温度为25度，我模拟了200个测量值，这些测量值的平均值为25度，但是加入了标准偏差为几度的高斯白噪声（在图中为蓝线）。

为了令卡尔曼滤波器开始工作，我们需要告诉卡尔曼两个零时刻的初始值，是X(0|0)和P(0|0)。他们的值不用太在意，随便给一个就可以了，因为随着卡尔曼的工作，X会逐渐的收敛。但是对于P，一般不要取0，因为这样可能会令卡尔曼完全相信你给定的X(0|0)是系统最优的，从而使算法不能收敛。我选了X(0|0)=1度，P(0|0)=10。

该系统的真实温度为25度，图中用黑线表示。图中红线是卡尔曼滤波器输出的最优化结果（该结果在算法中设置了Q=1e-6，R=1e-1）。

haleycui

感谢楼上的资料，受益菲浅

haleycui

今天我对平衡小车的架构进行了改进，增大了车轮的直径，经过一上午的摸索，终于把4个k值调节的比较合适。烧片的问题也解决了。初步测试了一下转向，还不错。视频已经发到了http://v.youku.com/v_show/id_XMTkxNDUxNzcy.html   想问一下zlstone，视频中小车的左右调整的幅度是不是有些大，怎样才能使它尽量稳定，谢谢。
   
(原文件名:IMAG0099.jpg)


(原文件名:IMAG0100.jpg)


(原文件名:IMAG0102.jpg)

liumaojun_cn

mark

zlstone

回复【98楼】haleycui
感觉是摆动比较大；敢问haleycui你的供电是什么？如果只是普通12V 1A的变压器电源效果不好，我后来采用了12A的开关电源，二者有些差异；你不妨换个电源或者增加对电源电压的采样，再作一个电压对PWM的校正（应该需要滤波处理），应该有所改善；因为这两个电机的直流阻抗比较小，耗电蛮大的；
另外请问，烧片的原因为何？串口的问题能否详述？Thanks！

haleycui

我用的是12v的镍氢电池组。烧片是由于我一开始将两个电机并联了起来（我也不知道是为什么，分开之后就好了）。
(原文件名:改后.PNG)

zlstone

收到，谢谢！你的手工比我好，漂亮很多，呵呵；对了，你的轮胎要不换个较硬的试试看，我的这个是硬质的，外面包了很薄的一圈橡胶（摩托车内胎）；或者再把位置的K值和速度的K值调大点；