三轴数字陀螺仪篇 (2).doc

机器人衡态调整三轴数字陀螺仪篇北京森汉科技有限公司ZXY三轴数字陀螺仪手册--目录第 0 页目录一．硬件介绍 ..................................................................11．陀螺仪芯片 .............................................................12．陀螺仪的载板 ...........................................................23．SHR-8S 机器人三轴分布 ..................................................34．STC12C5A60S 芯片接口 ...................................................45．载板实物 ...............................................................4二．陀螺仪操作 ................................................................51．访问格式 ...............................................................52．软件接口 ...............................................................5三．姿势状态分析 ..............................................................51．自纠正归位系统介绍 .....................................................52．自纠正跳位系统介绍 .....................................................6四．姿态调整程序解析 ..........................................................81．行走姿态调整程序 .......................................................82．静止站立姿态调整程序 ..................................................193．左右平移姿态调整程序 ..................................................25三轴数字陀螺仪手册第 0 页一．硬件介绍1．陀螺仪芯片如下图 1-1 所示，三轴数字陀螺仪 L3G4200D 芯片 X、Y、Z 三轴互相正交分布。

图 1-1 坐标方向及背面图图 1-1 中明确标出了 L3G4200D 芯片 X、Y、Z 轴的坐标方向及背面图本陀螺仪芯片可供用户选择其尺度量程，量程范围从 ±250 dps ～ ±2000 dps，低量程数值用于高精度慢速运动测量，而高量程则用于测量超快速的手势和运动这款器件提供一个 16 位数据输出，以及可配置的低通和高通滤波器等嵌入式数字功能就算时间推移或温度变化，这款器件仍然保持连续稳定的输出L3G4200D 特性（ST）： 三种可选全尺度(±250/500/2000 存保计划） I2C/SPI 数字输出接口 16 比特率值的数据输出 8 比特温度数据输出 两个数字输出线（中断和 data ready） 集成低和高通滤波器的用户可选带宽 在时间和温度上平稳 嵌入式 self-test 宽电源电压，2.4 V 到 3.6 V 低电压兼容的 IOS，1.8 V 嵌入式 power-down 和睡眠模式 嵌入式温度传感器 嵌入式 FIFO 缓存三轴数字陀螺仪手册第 1 页 高抗撞击能力 扩展的工作温度范围(-40°C 到+85°C) ECOPACK® RoHS 和 “Green”认证2．陀螺仪的载板（1）本次采用陀螺仪的 IIC 接口方式，访问陀螺仪内部的三轴数字值。

其每一轴的数字值均为有符号的 16 进制数，以补码的形式存放采用单片机两个 I/O 口（P4.1 与P3.3）模拟 IIC 与陀螺仪 IIC 接口（SCL、SDA）相接，使用 100kHz 采样速率对陀螺仪内部寄存器配置与三轴数字值的读取其中三轴 X、Y、Z 数字值的存放寄存器分别为 OUT_X_L与 OUT_X_H、OUT_Y_L 与 OUT_Y_H、OUT_Z_L 与 OUT_Z_H，只要对它们进行读取即可其应用载板正面及接线提示图如下图 1-2 所示图 1-2 载板正面图（2）陀螺仪芯片的载板背面图如图 1-3 所示图 1-3 载板背面图YZXGNDSCL+5VSDA三轴数字陀螺仪手册第 2 页3． SHR-8S 机器人三轴分布在此特别的说明一下，这里讲到的三轴与 L3G4200D 芯片载板的三轴完全重合根据三轴数字陀螺仪载板在机器人身长的插接方式，如下图 1-4 所示的坐标图 1-4 载板在机器人上的坐标图X、Y 平面Y、Z 平面Y、Z 平面XY⊕ZYZXYXZ⊙⊕ZX Y三轴数字陀螺仪手册第 3 页4． STC12C5A60S 芯片接口STC12C5A60S 芯片是 SHR-8S 人形机器人的主控板上面的 MCU，它负责机器人行走步伐函数的发生，另外还负责红外线接收与识别，与上位机三维图形化软件进行通讯的功能。

本次再给它加一个任务就是“三轴数字陀螺仪的数据采样和分析” 所以使用该芯片的 IIC采样端口，对 L3G4200D 进行三轴数字值采样具体的端口连接参考下面图 1-5图 1-5 STC12C5A60S 芯片接口图根据上图 1-5，使用如下的端口协议：STC12C5A60AD 芯片：P4.1 ---- 载板 SCL；P3.3 ---- 载板 SDA；5．载板实物载板的实物图，如下图 1-6 所示实物暂时未出图 1-6 载板实物图三轴数字陀螺仪手册第 4 页二．陀螺仪操作本陀螺仪操作接口含有 IIC 与 SPI 两种，由于考虑到 SPI 接口多占用 I/O 口的问题，所以此次采用 IIC 接口方式对陀螺仪芯片操作，即一根时钟线 SCL 和一根数据线 SDA 连接陀螺仪，实现数据传输1．访问格式基于标准 IIC 通信格式，对陀螺仪内部寄存器进行访问采样数率使用 100kHz，分别依次从六个寄存器（每一轴的数字值由低 8 位与高 8 位组合）读取出三轴 X、Y、Z 的数字值，其中六个寄存器分别为 OUT_X_L 与 OUT_X_H、OUT_Y_L 与 OUT_Y_H、OUT_Z_L 与OUT_Z_H。

2．软件接口基于标准 IIC 的通信协议，编写访问陀螺仪内部数据的接口子程序（void Read_IIC_XYZAxis(void) ） ；其依次读取出每一轴的高低 8 位，再分别组合存放于有符号16 位的整型变量当中，共 3 个变量（L3G4200D_dis_data[0]、L3G4200D_dis_data[1]、L3G4200D_dis_data[2]） ，分别对应于 X、Y、Z 三轴的角速度值由于用于控制机器人相对较快行走，所以降低陀螺仪返回的角速度值，以缩小 100 倍处理三．姿势状态分析机器人在行走运动时欲维持其各种平衡状态，首先要研究立正姿势下的平衡方程由于立正姿势本身就是一种静止状态，而且它相对于所有的稳定状态都可以直接过渡，所以要把立正姿势下的各种平衡关系分类研究透彻首先要探讨一下平衡的概念，所谓的平衡，也就是说系统在一定范围内保持着规律性的运动比如说钟摆本身就是一种特殊的平衡，还有像弹簧的往复震动，小孩子玩的陀螺不停的旋转等等都是一种稳定状态也就是一种平衡在人体的研究范畴里面，平衡状态的种类是最多的，它的种类要比天上的战斗机还多上百倍经常看到的杂技演员，在高空做的各种惊险动作，其实都是在从一个稳定态变化到另外的一个稳定态的过程。

在这个过程中，我们发现过渡过程都是运动的，而稳定态都是静止的不过，我们把各种状态归类总结成一下 2 种1．自纠正归位系统介绍在日常生活中经常会碰到类似的情况，正常行走的行人一不留神，脚踩到一个西瓜皮后，这个人挣扎了几下又直起身来，差一点就摔到了这种现象归类为自动纠正归位系统三轴数字陀螺仪手册第 5 页现象，如下面的流程图 3-1 所示图 3-1 自纠正归位系统图当稳定的系统受到外界干扰的时候，系统就面临着失稳的风险如果系统的自纠正能力很强，那么他就会很快地进行自我纠正并回到初始的稳定态上面的例子里，正常行走的行人就是一个稳定态，它本身就是动态平衡的，行走本身就是一种周期性的运动突然踩到一个西瓜皮代表一个很强的外界干扰因素随机性地进入系统，行人马上会手舞足蹈地翻腾起来，因为他不想摔倒这个翻腾的过程就是过度态，它相对来说会比较短暂随后这个人依靠躯干和四肢的运动，保持住了自己的重心，没有摔倒最后，他又继续向前行走我们把踩到西瓜皮的瞬间归为跳出段，把四肢翻腾的过程归为返回段2．自纠正跳位系统介绍还是刚才的那个行人，可是这回的情况不同了，这次他最后摔倒了这个行人，他起初是在正常行走，一不留神，脚踩到一个西瓜皮，这个人挣扎了几下却无济于事，结果就摔到了。

这种现象归类为自动跳位系统现象，如下面的流程图 3-2 所示图 3-2 自纠正跳位系统图稳定态（A）过渡态跳 段出返回段稳定态可以是静态也可以是动态 过渡态都是动态跳出段”是由外界干扰系统而产生返回段”是由反馈量进入系统而产生稳定态（A）过渡态跳 段出跳位段稳定态（A）可以是静态也可以是动态跳位段”也是由反馈量进入系统而产生稳定态（B）稳定态（B）可以是静态也可以是动态三轴数字陀螺仪手册第 6 页当稳定的系统受到外界干扰的时候，系统就面临着失稳的风险如果系统的自纠正能力在某一方面不是很强，那么它就会被另外的稳定态所代替在这个例子里，正常行走的行人就是稳定态（A） 突然踩到一个西瓜皮代表一个很强的外界干扰因素随机性地进入系统，行人马上会四肢翻腾，这个翻腾的过程就是过度态行人在拼命保持自己的重心，但是最后他还是摔倒了摔倒后，坐在了地上就是稳定态（B） 我们把踩到西瓜皮的瞬间归类为跳出段，把四肢翻腾但是仍然倒下的过程归类为跳位段参考以上的两个自纠正系统的特点，下面进行研究“人体立正姿势、双腿劈叉姿势、单腿站立姿势、原地踏步姿势、双足行走姿势等”各种姿势的稳定性和过渡性的特点本次使用陀螺仪，就是用于控制机器人处于这两种自纠正系统下维持它的稳态平衡。

也就是，在每一种稳态情况下，在一定较小时间内启动陀螺仪维持该稳态平衡，使其当前稳态的姿势状况不变比如机器人在行走一步时，走完前半步处于稳态（A） ，再走完下半步处于稳态（B） 在前半步的稳态（A）时，启动陀螺仪及时修正该稳态的偏移，以维持该状态不变下半步依然如此如果机器人处于站立等静止状态时，就等同处于一种稳态的情况，依然启动陀螺仪及时修正偏移，以维持稳态的姿势欲使机器人所有运动姿势正常化，首先调节好机器人所有运动姿势中每一种稳态正常化如下面流程图 3-3 所示图 3-3 陀螺仪应用系统图稳定态（A）可以是静态也可以是动态稳定态（B）可以是静态也可以是动态稳定态（ B ）“跳位段”也是由反馈量进入系统而产生稳定态（A）过渡态维持A 稳态维持B 稳态出位出跳 段段跳段跳三轴数字陀螺仪手册第 7 页四．姿态调整程序解析1．行走姿态调整程序在机器人行走过程中，当处在双腿着地某一姿态时，循环启动陀螺仪周期性地采集三轴数字陀螺仪的三轴数字。