gyro boy代码解释

topikachu · 发表于 2014-3-13 22:04:34

本帖最后由 topikachu 于 2014-3-14 14:52 编辑

ev3 教育版的gyro boy平衡算法还是典型的pid控制。解释一下

gyro boy有两个线程，其中一个是平衡控制与行进线程，另一个是行进的触发线程。
先解释平衡控制

系统启动后重置（RST）重置步进电机的角度传感器，陀螺仪，和CHK用计时器
计算零漂(gOS)，方法是间隔0.004取200次陀螺仪的角速度取平均。但是这200次中最大和最小值之差要小于2。如果不满足，重新取200次，存入gOS
喇叭响，屏幕显示Awake Up，st=1 （st 的用法下面解释）
进入平衡姿态控制的循环
计算每轮的所用平均时间（GT）：第一轮重置计时器，并且假设第一轮0.014s，cLo+1.以后每轮都用总时间/cLo,得出平均时间，存入tInt
平衡参数计算（GG）：先计算 gOS=0.0005*原始角速度+9.9995*gOS ，修正陀螺仪角速度误差（某种形式的滤波？），（原始角速度-gOS）*iInt就是这轮的角度，再和前一轮角度相加，得到总的角度
位移参数计算（GM）：因为移动量（水平位移）和电机转动成正比，所以移动量以电机转动的圈数为单位计算。首先计算出两个电机总的旋转度数，扣掉前一次的度数，算出这轮的位移。这次位移加上前一次的绝对位置（mPos）得出新值，更新mPos。然后以最近连续四次的位移的平均值除以/tInt算出平均位移速度。
PID计算（EQ) 如果期望机器人有水平方向的移动，那么一起算上（mPos=mPos-Cdrv*tInt）。然后先计算平衡用数据gPwr=0.8*gSpd+15*gAng。然后计算水平移动数据mPwr=0.02*mSpd+0.12*mPos 。最后合成的速度是gPwr+mPwr-0.01*Cdrv (Cdrv为带符号功率参数。如Cdrv为正，最终功率将往负值偏移。因是负反馈，所以机器人向前运动。反之也然。谢谢4楼ntwuhuu提示)。然后限制功率在±100之间
最终功率计算（cntrl）再计算一次mPos （mPos=mPos-Cdrv*tInt，为什么？不懂！）。然后根据机器人的旋转（CStr)，调整两轮的输出功率
输出到两个电机
检测机器人状态（CHK）：如果连续1s功率输出绝对值都是在100，说明机器人倒了
将这一轮补足到0.005秒
如果机器人倒了，退出平衡循环，等待重置

行进触发线程：

初始状态st是零，直到平衡线程完成初始化，Awake Up，st=1
st=1的情况下，机器人向前运动4秒钟，完成平衡，st=2
st=2后，根据颜色传感器，发出前后左右停的指令
当超声波发现前面有障碍后，会将Cdrv降到-10，随机选择一个方向让机器人后退

结论：
Gyro Boy使用了两组PID并联，同时控制平衡和移动。

KEVIN0602 · 发表于 2014-3-17 20:55:58

有没有现成的程序发一个给我呢？我的gyro boy一直找不到平衡，一下就倒了

topikachu · 发表于 2014-3-13 22:18:43

糖伯虎发表于 2014-3-13 22:13
两组PID?请指教！

看EQ模块0.8*gSpd+15*gAng 和 0.02*mSpd+0.12*mPos 一个算平衡，一个算水平位移。

糖伯虎 · 发表于 2014-3-13 22:13:57

两组PID?请指教！

ntwuhui · 发表于 2014-3-13 23:01:46

不太清楚为什么要再扣掉0.01*Cdrv，补偿什么?不懂！

检测颜色传感器识别颜色（1号端口）
如果是红色那么
播放频率2000音量100的音调0.1秒
Cdrv=Cstr=0 // PS: 红色停止
如果是绿色那么
播放频率2000音量100的音调0.1秒
Cdrv=150 Cstr=0 // PS: 绿色前进，Cdrv：前后运动功率
如果是蓝色那么
播放频率2000音量100的音调0.1秒
Cstr=70 // PS: 蓝色左拐，Cstr：左右拐弯功率
如果是黄色那么
播放频率2000音量100的音调0.1秒
Cstr=-70 // PS: 黄色右拐
如果是白色那么
播放频率2000音量100的音调0.1秒
Cdrv=-75 // PS: 白色后退

复制代码

这一段代码中如果前进 Cdrv=150，后退为-75，符号决定了修正方向， 0.01*Cdrv=1.5, 个人以为这里补偿的是机器人前进或后退时的需要额外的功率从数值上看，前进会比后退更明显。

topikachu · 发表于 2014-3-14 07:52:14

ntwuhui 发表于 2014-3-13 23:01
这一段代码中如果前进 Cdrv=150，后退为-75，符号决定了修正方向， 0.01*Cdrv=1.5, 个人以为这里补偿的是 ...

谢谢提示。已更新正文。

topikachu · 发表于 2014-3-18 09:06:45

链接: http://pan.baidu.com/s/1dDgk5DR 密码: l149

jay525053 · 发表于 2014-8-25 19:45:53

初学的慢慢理解下！

tkocmeng · 发表于 2014-8-26 14:14:40

KEVIN0602 发表于 2014-3-17 20:55
有没有现成的程序发一个给我呢？我的gyro boy一直找不到平衡，一下就倒了

你先检查一下接线，软件自带的gyro boy程序是没问题的，最有可能的是gyro和touch接反了

KEVIN0602 · 发表于 2014-11-2 17:59:03

tkocmeng 发表于 2014-8-26 14:14
你先检查一下接线，软件自带的gyro boy程序是没问题的，最有可能的是gyro和touch接反了

我找到问题原因了。是我没正确的放置它，所以他一下子就倒了。谢谢你。

yeafeel · 发表于 2014-11-7 20:27:03

收藏了

renyiwx · 发表于 2016-3-16 22:03:50

st=1的情况下，机器人向前运动4秒钟，完成平衡，st=2

我看下来，感觉st=1是刚开始机器人向后退的起身动作
还有就是，gOS初始值是-0.25度，大概是指机器人在支架上时的初始角度

流云之阑 · 发表于 2017-1-4 16:51:47

楼主，EV3的电机是步进电机吗？步进电机不是开环控制吗？

追星星的猩猩 · 发表于 2017-2-6 21:09:15

topikachu 发表于 2014-3-13 22:18
看EQ模块0.8*gSpd+15*gAng 和 0.02*mSpd+0.12*mPos 一个算平衡，一个算水平位移。

谁是P谁是I谁是D?!这个EQ看不懂

orange · 发表于 2018-3-4 10:44:16

KEVIN0602 发表于 2014-3-17 20:55
有没有现成的程序发一个给我呢？我的gyro boy一直找不到平衡，一下就倒了

教育版环境里有

账号		自动登录	找回密码
密码			马上注册