三轴数字陀螺仪篇.docVIP

机器人衡态调整 三轴数字陀螺仪篇 北京森汉科技有限公司 目录 一．硬件介绍 1 1．陀螺仪芯片 1 2．陀螺仪的载板 2 3．SHR-8S机器人三轴分布 3 4．STC12C5A60S芯片接口 4 5．载板实物 4 二．陀螺仪操作 5 1．访问格式 5 2．软件接口 5 三．姿势状态分析 5 1．自纠正归位系统介绍 5 2．自纠正跳位系统介绍 6 四．姿态调整程序解析 8 1．行走姿态调整程序 8 2．静止站立姿态调整程序 19 3．左右平移姿态调整程序 25 一．硬件介绍 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 缓存 高抗撞击能力 扩展的工作温度范围 -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 载板背面图 3．SHR-8S机器人三轴分布 在此特别的说明一下，这里讲到的三轴与L3G4200D芯片载板的三轴完全重合。 根据三轴数字陀螺仪载板在机器人身长的插接方式，如下图1-4所示的坐标。 图1-4 载板在机器人上的坐标图 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 载板实物图 二．陀螺仪操作 本陀螺仪操作接口含有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倍处理。 三．姿势状态分