本帖最后由 ed8841 于 2012-10-8 11:38 编辑
最近入手一个HT公司的陀螺仪，也尝搭建了几种Segway机器人。在制作过程中看了不少Segway的文章，其中HT公司的一片文章写的最为详细，而且还有范例程序可以下载，对照范例程序和这篇文章，我对Segway也算是略知一二了。
原文的地址是：http://www.hitechnic.com/blog/gyro-sensor/htway/
为了方便大家阅读，我把原理介绍的部分翻译成中文了，前言和项目背景什么的就懒得翻译了。本人英文和中文水平都有限的很，大家将就着看吧。
译文内容：
HTWay – A Segway type robot
工作原理
起先, 你并不清楚这个机器人是如何工作的。甚至不需要知道是如何计算平衡的，你只需把它搭建好并装载NXC或NXT-G的程序就可以运行了。NXC和NXT-G在编写程序的时候，都采用了控制模块代码和平衡模块代码分离的方式。你还可使用其他传感器，例如超声波、亮度传感器，但必须保证安装陀螺仪传感器，因为它是平衡控制的关键部件。你可以自己编写代码，更改程序中的2个全局变量，实现控制机器人的前后左右运动。在NXC程序中，这2个变量的名称是motorControlDrive 和motorControlSteer；在NXT-G程序中，这2个变量的名称是controlDrive 和 controlSteer。这些变量的单位都是“角度/秒”。转向控制是基于2个马达预期设定值的差异。
你可以下载NXC和NXT-G的程序代码，这两种程序代码的工作原理是相同的。以下是我列出NXC程序代码中的一个小片段。如果你是NXT-G的程序员，尝试按照这段代码用NXT-G重新编写，或者在NXT-G的程序中找出相同功能的代码。你会发现相同的计算公式，用NXT-G比用NXC要复杂的多，而且NXC的代码更易于理解。
为了保持平衡，机器人使用循环控制的方法。在循环中有4个参数需要计算，并最终计算出用多大的马达驱动功率使得机器人能够保持平衡。将程序简化一下，一下就使用NXC编写的平衡控制代码。
while(true) { GetGyroData(gyroSpeed, gyroAngle); GetMotorData(motorSpeed, motorPos);
power = KGYROSPEED * gyroSpeed + KGYROANGLE * gyroAngle + KPOS * motorPos + KSPEED * motorSpeed;
OnFwd(LEFT_MOTOR, power); OnFwd(RIGHT_MOTOR, power); Wait(WAIT_TIME); } 注：因为考虑到前进、转弯的控制和马达只能接受+/-100范围内的参数等因素，实际的NXC代码会比上面的范例更完善一些。 在上面这段代码中，你可以发现在每次循环的过程中都会获取到4个参数：gyroSpeed, gyroAngle,motorSpeed, 和 motorPos。这4个参数（或变量）描述了当前机器人的状态。 | gyroSpeed | 这个数据来源于陀螺仪，代表机器人的角速度.如果机器人在向前倾倒的过程中，那么将得到一个正数值。它的单位是“度/秒”。 | | gyroAngle | 这个参数表示机器人的角度。如果机器人向前倾斜，这个参数是正数值，向后倾斜为负数值。（实际情况并非完全如此，稍后会解释）。它的单位是“度”。 | | motorPos | 它代表马达转过的角度。在HTway项目中，它是两个马达转动角度的和。它的作用是使机器人保持在原地。 | | motorSpeed | 它代表马达的转速，单位是“度/秒”，它也是两个马达速度的和。它的作用是消除机器人的振荡，并对振荡产生阻尼。 |
这4个变量分别乘以各自的系数，并经过四元一次计算可以得到驱动马达的功率，以使得机器人保持平衡。使机器人能够正常工作的诀窍是找到合适的系数。 为了让大家能够理解平衡公式中每个参数的用途，我将分别逐个说明每个参数的作用。首先，我们假设机器人保持完美的平衡，并且在处目标区域的中心点，在那种情况下4个参数都会是0。换而言之，机器人完全垂直，所以gyroAngle是0,机器人既不前倾也不后仰，而且轮子没有任何转动，并处在在处目标区域的中心点。因为4个变量都是0，那么计算出的功率值同样也是0。 那么如果机器人向前倾倒gyroAngle将不再是0，比如gyroAngle为5度，接下来会发生什么情况？机器人向前倾斜，为了保持平衡就必须驱动机器人向前行驶。当常量系数KGYROANGLE乘以gyroAngle,将会得到使机器人恢复平衡的马达功率值。 又如，假设其他3个参数都是0，gyroSpeed 为正数，也许是10度/秒。虽然机器人仍然是直立而且没有移动，但是他已经有了向前倾倒的趋势。在某种程度上，你也可以认为这是一种这变得不稳定的先兆，虽然现在是直立的，但是即将向前倾倒。这个参数可以使机器人在倾倒前就得到纠正。在机器人修正已经发生倾斜时，这个参数也起到很重要的作用，它可以防止当机器人已经直立时，gyroAngle参数仍在响应。 那么两个马达的参数又有什么作用呢？假设机器人完全直立而且很稳定（原文：没有发生倾倒），但是距离目标原点，马达已经向前转动了100度，在这种情况下motorPos 参数为100。请注意，在HTWay项目中motorPos 表示的是2个马达转动角度的和。机器人与原点之间的实际距离还与轮子的大小有关。如果直接驱动机器人返回原点，那是不行的。如果你试图这样做，你会发现机器人将向前摔倒。正确的解决方法是先向前驱动机器人，因为想使机器人向后回到原点，必须先使其有一定的后倾角度。为了使机器人向后倾倒，就要让机器人向前移动！ motorSpeed 参数的作用与motorPos参数有些类似。如果机器人在向前行驶的过程中，首先你必须保证足够的功率以维持速度从而保持平衡，然后你甚至还需要再增加一点点功率值让机器人向后倾斜一点，这样可以促使机器人降低行驶速度。 在NXC程序的开头处就对这4个常量系数做出定义： #define KGYROANGLE 7.5 #define KGYROSPEED 1.15 #define KPOS 0.07 #define KSPEED 0.1
关于轮子尺寸的说明
在HTWay程序中，通过主机面板上的3个按钮你可以选择不同的轮子。这样就可以确定ratioWheel这个全局变量是0.8、1.0还是1.4，这3个数值分别对应NXT2.0轮子、NXT1.0轮子和RCX大轮子。这个参数是怎用使用的？真正的平衡公式与上面第一次给出的公式还是有一些小差别的。以下就是程序中使用的完整的平衡公式： power = (KGYROSPEED * gyroSpeed + KGYROANGLE * gyroAngle) /ratioWheel + KPOS * motorPos + KDRIVE * motorControlDrive + KSPEED * motorSpeed; 轮胎的尺寸仅仅对陀螺仪的两个参数修正起作用，而与马达的两个参数无关。原因是较大的轮子需要较少的功率来补偿平衡。既然在相同输入值下的大轮子比小移动的更远，那么大轮子可以降低输入值，以达到与小轮子相同的移动距离。那么为什么不需要用轮子尺寸来修正马达的两个参数呢？原因是轮子的尺寸有自举效果。例如机器人向前多移动了1寸，使用小轮子机器人的motorPos参数会比使用大轮子的机器人更大。显然，小轮子机器人会用更多的马达驱动来达到与大轮子机器人相同移动距离的目的。 在上面的代码中你还会发现motorControlDrive 参数出现在平衡公式中，但事实上它并没有直接驱动机器人。当这个参数变化时可以帮助机器人降低速度。当你开始向前驱动机器人时，这个参数将会促使机器人首先向后驱动一点点，当你想停止机器人时，他会给机器人一些额外的向前驱动的动力，使机器人向后倾斜，从而减速。事实上为了驱动机器人采用了以下的代码： motorPos -= motorControlDrive * tInterval; 每次循环，motorPos 变量都会被motorControlDrive这个全局变量按比例调整。motorControlDrive的单位是“度/秒”，所以用它乘以积分时间后的总和来调整motorPos的值，这样机器人在每个控制循环都会移动。这就使机器人向着目标位置移动。 积分
如果你是一个刚入门的机器人建造者，很有可能从没听说过积分。如果你很有经验，你很可能希望从没听到过它。事实证明积分对于本项目来说非常有效和必不可少的。而且他也不是很难懂。 问题时陀螺仪不会给你一个真正的角度值。你不能仅仅读取了陀螺仪的值，然后要求机器人向前或向后倾斜。你必须告诉它角速度，换句话说就是告诉机器人翻到的速度有多快。还有如果你知道了角速度，如何能够算出角度？那就是积分的作用了。 积分运算实际上很简单，就是把一串无穷的系列数据按时间累加。假设在某个时间点我们知道机器人的角度，我们把这个角度写入gyroAngle变量。每次循环我们从陀螺仪获取角速度，它的单位是“度/秒”。如果我们知道每次循环的间隔时间，那么每次角度变化的总和来更新gyroAngle变量。假设每秒循环100次，那么间隔时间就是0.01秒。为了更新gyroAngle变量，我们只要将gyroSpeed乘以积分时间然后和gyroAngle做加法就可以了。 下面的NXC代码说明了如何获取陀螺仪的数据： void GetGyroData(float &gyroSpeed, float &gyroAngle) { float gyroRaw;
gyroRaw = SensorHTGyro(GYRO); gOffset = EMAOFFSET * gyroRaw + (1-EMAOFFSET) * gOffset; gyroSpeed = gyroRaw - gOffset;
gAngleGlobal += gyroSpeed*tInterval; gyroAngle = gAngleGlobal; } 上面这段代码中还有一个重点确定，就是陀螺仪的偏移值。因为陀螺传感器技术上的原因，即使在传感器没有任何角度情况下，它的原始数据也不会是0。由于我们期望在陀螺仪静止的时候输出值为0，那么就需要维护好陀螺仪的偏移值，用过这个值来调整陀螺仪的输出值就可以算出准确的角速度。 为了维护好陀螺仪的偏移值，程序做了两件事情。当机器人开始运行平衡程序前先让机器人静止躺在地上，然后采集100个传感器的数据作为样本。但是这个值只是陀螺仪偏移值得初始值。当机器人开始运行时，我们还需要适当的调整偏移值来防止零点漂移（如果不修正，机器人会像一边漂移）。在HTWay项目中，用类似指数移动平均的长期移动平均值为维护陀螺仪的偏移值。假设机器人是平衡的，那么角速度的长期平均值应该为0，所以陀螺传感器值的长期平均可以用作陀螺仪的偏移量。 在上面的代码中，EMAOFFSET非常小（仅0.0005），所以即使机器人前后移动保持平衡，gOffset的值也不会立即产生大幅度的变化。只有在偏移值在很长的时间内都存在偏差，偏移值才会有显著的变化（此句为原文翻译，很难读懂。根据我对代码的解读和实测，我理解为：只有在陀螺仪的读数长期与偏移值存在偏差，偏移值才会有显著的变化）。这种指数式移动平均值的方法又可以成为低通滤波器。 gyroAngle and motorPos…没有真正的零点 在机器人开始平衡时,程序假设机器人是垂直的。但是当你放开手时，你不可能保证机器人还是垂直的，总会产生1度或2度的变化。另外，尽管程序在维护陀螺仪的偏移值和不断的用角速度积分计算出gyroAngle，但这样也不是十全十美的，gyroAngle还是会随着时间产生一点点的漂移。听起来像场灾难，不是吗？但它并不是个问题。事实上当你放不操控机器人时，机器人会轻轻的前后摆动（或振荡）维持平衡，gyroAngle和motorPos 会以0点为目标调整。原因是这两个参数会相互影响达到一个平衡点。例如，gyroAngle在1度附近时机器人平衡的很好，为了补偿平衡，motorPos可能是-107度。当着两个数值代入平衡公式中分别乘以他们的系数后，他们将会相互抵消。
(全文完) | 
IP卡
狗仔卡
发表于 2012-10-8 10:37:13
提升卡
置顶卡
沉默卡
变色卡
千斤顶
显身卡
发表于 2012-10-9 12:49:52
