矽睿IMU QMI8610 应用指南之机器人
QMI8610机器人（Robots）应用指南
随着物联网、大数据和移动应用等信息技术的快速发展，机器人技术也得到飞速发展，其定义也不断充实和创新，传统的机械臂已经被可自主运动和定位导航机器人取代，其应用领域也越来越丰富，除了应用于居家扫地、家庭陪护、教育教学外，还应用于仓储及物流、外科手术及医疗康复、楼宇及室内配送、复杂环境与特殊对象的专业清洁、城市应急安防、能源和矿产采集等。导航机器人想要自由行走，实现自主定位导航是关键，自主定位导航包括定位、建图与路径规划。当前机器人自主导航普遍采用了多种技术组合的模式，即 “激光雷达+深度视觉+GPS+惯性导航等多传感器融合定位方式” ，以实现优势互补。
惯性导航，是指在机器人上安装陀螺仪和加速度传感器，在行驶区域的地面上安装定位块，通过对陀螺仪偏差信号（角速率）的计算及地面定位块信号的采集来确定自身的位置和航向，从而实现导引。通过惯性导航技术能够实现定位精准、地面处理工作量小、路径灵活性强。
矽睿科技推出的6轴MEMS传感器——QMI8610可以应用于机器人姿态平衡控制引擎、自主导航系统。QMI8610内置3轴陀螺仪和3轴加速度传感器，通过高精度的测量数据，确保机器人姿态、动作和导航控制的运动准确性和稳定性。QMI8610传感器内部集成了Digital Signal Processor（DSP）motion co-processor，易于集成到机器人中，运用基于扩展卡尔曼滤波理论的算法（XKF3），融合 3D 惯性传感器数据（定位和速度增量）和 3D 磁力计数据来定向角度，矽睿科技还提供广泛的传感器组合（3D 地磁传感器和气压传感器），非常适合满足将机器人应用提升到更高水平所需的规格。
下面简要介绍QMI8610主要寄存器参数设置、数据输出格式、机器人姿态解算基础知识，为客户设计提供应用指南。
1
导航中坐标系坐标转换
a.地理坐标系（
系）---
地理坐标系是指原点
位于地球表面任意一点，
轴指向水平东向，
轴指向水平北向，
轴沿大地垂线方向指向天，也被称为东北天坐标系。
b.导航坐标系（
系）---
导航坐标系是在导航时根据导航系统工作的需要而选取的作为导航基准的坐标系。一般情况下导航坐标系选为地理坐标系重合，也可选水平面
与地理坐标系重合，方向上有一个夹角。
c.载体坐标系（
系）---
载体坐标系原点在载体的质心，
轴和
轴在当地水平面内，
轴指向载体的右侧，
轴沿载体纵轴方向并指向前，
垂直于载体竖直向上。
坐标系构成右手直角坐标系，当载体没有俯仰、倾斜时，
即为水平面，
轴沿垂线指向天顶。载体坐标系相对于导航坐标系所确定的状态可以用姿态角来表示。
地球坐标系/地理坐坐标系/导航坐标系
2
主要寄存器参数设置和数据输出格式
QMI8610 IIC通信示意图
a. 初始化配置
1）读取Chip_ id：读取 0x00 寄存器，Chip_ id = 0xfc；
2）配置加速度计传感器寄存器0x03：设置加速度计的量程 range、输出速率odr 等；
3）配置陀螺仪寄存器0x04：设置陀螺仪的量程 range、输出速率odr 等；
4）配置陀螺仪和加速度计滤波寄存器0x06，设置陀螺仪、加速度计的低通滤波器、高通滤波器等；
5）配置陀螺仪使能寄存器0x08：使能或失能陀螺仪和加速度传感器工作。
b. 数据寄存器数据读取和处理
1）判断状态寄存器（0x16）是否 data ready：
bit0 为1 表示 Accelerometer new data available，
bit1 为 1 表示 Gyroscope new data available； 
FisImu_read_reg(FisRegister_Ax_L | 0x80, buf_reg, 12);
raw_acc_xyz[0] = (short) ((buf_reg[1] << 8) | ( buf_reg[0]));
raw_acc_xyz[1] = (short) ((buf_reg[3] << 8) | ( buf_reg[2]));
raw_acc_xyz[2] = (short) ((buf_reg[5] << 8) | ( buf_reg[4]));
raw_gyro_xyz[0] = (short) ((buf_reg[7] << 8) | ( buf_reg[6]));
raw_gyro_xyz[1] = (short) ((buf_reg[9] << 8) | ( buf_reg[8]));
raw_gyro_xyz[2] = (short) ((buf_reg[11] << 8) | ( buf_reg[10]));
// ONE_G = 9.807f //单位m/s2 //acc_lsb_div：加速度分辨率
acc_t[0] = (float) (raw_acc_xyz[0] * ONE_G) / acc_lsb_div;
acc_t[1] = (float) (raw_acc_xyz[1] * ONE_G) / acc_lsb_div;
acc_t[2] = (float) (raw_acc_xyz[2] * ONE_G) / acc_lsb_div;
// 0.01745f = pi(3.1415926) / 180 //单位rad/s // gyro_lsb_div：陀螺仪分辨率
gyro_t[0] = (float) (raw_gyro_xyz[0] * 0.01745f) / gyro_lsb_div;
gyro_t[1] = (float) (raw_gyro_xyz[1] * 0.01745f) / gyro_lsb_div;
gyro_t[2] = (float) (raw_gyro_xyz[2] * 0.01745f) / gyro_lsb_div;
3
四元数、旋转矩阵和欧拉角输出
a. 求姿态矩阵
1） 旋转Z轴
旋转前坐标：
，旋转后坐标：
即
2） 旋转Y轴
转前坐标：
，旋转后坐标：
即
3） 旋转X轴
旋转前坐标：
，旋转后坐标：
即
4） 载体坐标系和导航坐标系转换
设导航坐标系（
系）坐标为
，载体坐标系（
系）坐标为
，则：
即
即坐标从
系到
系的转换矩阵为
反过来，坐标从
系到
系的转换矩阵为
b. 四元数旋转矩阵
设旋转前坐标：
, 旋转后坐标：
四元数为：
c. 四元数转换欧拉角
与
以上矩阵一一对应。
可得：
可得欧拉角：
航向角：
俯仰角：
横滚角：
上海矽睿科技有限公司从事高端MEMS传感器业务。公司以消费电子、汽车、工业控制、通信与医疗等领域为主要目标市场，设计和生产优质传感器产品，并为客户提供相应的系统解决方案和服务。我们拥有自主知识产权的磁传感器、加速度传感器、压力传感器、光传感器以及世界领先的CMOS集成六轴组合传感器技术。矽睿科技以推动传感技术发展为使命，与您一起创造更智能化的感知世界。
上海总部
地址：上海市浦东新区海科路99号6号楼3层 邮编：201203
电话：+86 21 6951 7300
网址：www.qstcorp.com
商务洽谈：business@qstcorp.com
具体业务联系电话如下：
北京 梁先生：18910931745
上海 陈先生：18516102033
深圳 陈先生：18503055277