简单的mems陀螺仪姿态算法介绍
测试传感器：mpu9250，九轴传感器，其中有三个轴就是陀螺仪的三个方向角速度。
陀螺仪在每个采样点获得：采样时刻（单位微妙），xyz三个方向的角速度（单位弧度/秒），记为：wx， wy， wz。陀螺仪静止时，wx， wy， wz也是有读数的，这就是陀螺仪的零漂。
实验一：将陀螺仪绕x轴旋转时，只有wx有读数；将陀螺仪绕y轴旋转时，只有wy有读数；将陀螺仪绕z轴旋转时，只有wz有读数；
实验二：将陀螺仪绕xy面上的轴旋转，wz读数为零，即与旋转轴垂直的轴上的角速度为零。
因为陀螺仪采样率很高（1000hz），通过瞬时读数计算姿态，可以看做：先绕x轴旋转，再绕y轴旋转，再绕z轴旋转。
下面这段代码实现了一个简单的陀螺仪姿态算法，开机并静置几十秒后，拿着陀螺仪旋转，十几分钟内姿态是正确的，之后由于积分累积，误差就越来越大了。
// 参数说明：
// samplets : 采样时刻，单位：微秒
// wx, wy, wz ：陀螺仪采样，单位：弧度/秒
void gyroexperiment(uint64_t samplets, float wx, float wy, float wz)
{
// 传感器启动时刻
static uint64_t s_lasttime = 0;
static uint64_t s_lastlog = 0;
if(s_lasttime == 0){
s_lasttime = samplets;
s_lastlog = samplets;
return;
}
// 采用启动后5秒-35秒的采用平均值作为陀螺仪零漂
// 在此期间，应保持陀螺仪静止
static float s_wx = 0, s_wy = 0, s_wz = 0; // 陀螺仪零漂
static uint64_t s_elapsed = 0;
if(s_elapsed 5000000){
s_wx += (wx - s_wx) / (s_samplecnt + 1);
s_wy += (wy - s_wy) / (s_samplecnt + 1);
s_wz += (wz - s_wz) / (s_samplecnt + 1);
s_samplecnt++;
}
s_elapsed += (samplets - s_lasttime);
s_lasttime = samplets;
}
// 初始姿态，采用三个轴向量表示
static float xx=1,xy=0,xz=0; // x轴
static float yx=0,yy=1,yz=0; // y轴
static float zx=0,zy=0,zz=1; // z轴
// 根据陀螺仪读数计算三个轴的旋转量
float interval = (samplets - s_lasttime) / 1e6;
float rx = (wx - s_wx) * interval;
float ry = (wy - s_wy) * interval;
float rz = (wz - s_wz) * interval;
// 分别构造绕三个轴旋转的四元数
float cos,sin;
cos = cosf(rx/2); sin = sinf(rx/2); quaternion qx(cos, xx * sin, xy * sin, xz * sin);
cos = cosf(ry/2); sin = sinf(ry/2); quaternion qy(cos, yx * sin, yy * sin, yz * sin);
cos = cosf(rz/2); sin = sinf(rz/2); quaternion qz(cos, zx * sin, zy * sin, zz * sin);
// 依次旋转三个轴向量
quaternion q = qx*qz*qy; q.normalize(); quaternion qi = q.inverse();
quaternion qx(0, xx, xy, xz); qx = q*qx*qi; qx.normalize(); xx = qx.q2; xy = qx.q3; xz = qx.q4; // 旋转x轴;
quaternion qy(0, yx, yy, yz); qy = q*qy*qi; qy.normalize(); yx = qy.q2; yy = qy.q3; yz = qy.q4; // 旋转y轴;
quaternion qz(0, zx, zy, zz); qz = q*qz*qi; qz.normalize(); zx = qz.q2; zy = qz.q3; zz = qz.q4; // 旋转z轴;
// 每1秒输出一次姿态数据
s_lasttime = samplets;
if(samplets - s_lastlog > 1000000){
console->printf(attitude: [%f, %f, %f]; [%f, %f, %f]; [%f, %f, %f]\n, xx, xy, xz, yx, yy, yz, zx, zy, zz);
s_lastlog = samplets;
}
}
速度快能耗低 光导AI芯片企业获盖茨等近千万美元投资
什么是5G核心网的微服务
FPC的结构层次的不同性质介绍
10.5英寸iPad Pro怎么样？新iPad Pro评测：未来长时间内的主力生产工具
电子工程师结合创造性与习惯
简单的mems陀螺仪姿态算法介绍
软通动力天枢元宇宙研究院在南京江宁高新区揭牌成立
用于智能电网的基于红外的无线电力技术
智能家居的新入口，将全方位管理你和你的家
电动车电池的保养方法_电动车电池的容量怎么看
基于角色访问控制技术的数控机床信息集成系统设计
一文看懂康佳转型之路如何走
华为助力精诊科技打造多模态融合的手术计划工作站
word文档如何解密
雷达水位计介绍
保证SMT加工品质的三种方法!
NEO宣布启动 DevCon 2019
强力磁铁是什么？有哪些应用？
刺客信条再添佳作，什么样的显示器才能够驾驭
让双摄像头成为手机行业的主流配置，还有核心问题需解决
陀螺仪在每个采样点获得：采样时刻（单位微妙），xyz三个方向的角速度（单位弧度/秒），记为：wx， wy， wz。陀螺仪静止时，wx， wy， wz也是有读数的，这就是陀螺仪的零漂。
实验一：将陀螺仪绕x轴旋转时，只有wx有读数；将陀螺仪绕y轴旋转时，只有wy有读数；将陀螺仪绕z轴旋转时，只有wz有读数；
实验二：将陀螺仪绕xy面上的轴旋转，wz读数为零，即与旋转轴垂直的轴上的角速度为零。
因为陀螺仪采样率很高（1000hz），通过瞬时读数计算姿态，可以看做：先绕x轴旋转，再绕y轴旋转，再绕z轴旋转。
下面这段代码实现了一个简单的陀螺仪姿态算法，开机并静置几十秒后，拿着陀螺仪旋转，十几分钟内姿态是正确的，之后由于积分累积，误差就越来越大了。
// 参数说明：
// samplets : 采样时刻，单位：微秒
// wx, wy, wz ：陀螺仪采样，单位：弧度/秒
void gyroexperiment(uint64_t samplets, float wx, float wy, float wz)
{
// 传感器启动时刻
static uint64_t s_lasttime = 0;
static uint64_t s_lastlog = 0;
if(s_lasttime == 0){
s_lasttime = samplets;
s_lastlog = samplets;
return;
}
// 采用启动后5秒-35秒的采用平均值作为陀螺仪零漂
// 在此期间，应保持陀螺仪静止
static float s_wx = 0, s_wy = 0, s_wz = 0; // 陀螺仪零漂
static uint64_t s_elapsed = 0;
if(s_elapsed 5000000){
s_wx += (wx - s_wx) / (s_samplecnt + 1);
s_wy += (wy - s_wy) / (s_samplecnt + 1);
s_wz += (wz - s_wz) / (s_samplecnt + 1);
s_samplecnt++;
}
s_elapsed += (samplets - s_lasttime);
s_lasttime = samplets;
}
// 初始姿态，采用三个轴向量表示
static float xx=1,xy=0,xz=0; // x轴
static float yx=0,yy=1,yz=0; // y轴
static float zx=0,zy=0,zz=1; // z轴
// 根据陀螺仪读数计算三个轴的旋转量
float interval = (samplets - s_lasttime) / 1e6;
float rx = (wx - s_wx) * interval;
float ry = (wy - s_wy) * interval;
float rz = (wz - s_wz) * interval;
// 分别构造绕三个轴旋转的四元数
float cos,sin;
cos = cosf(rx/2); sin = sinf(rx/2); quaternion qx(cos, xx * sin, xy * sin, xz * sin);
cos = cosf(ry/2); sin = sinf(ry/2); quaternion qy(cos, yx * sin, yy * sin, yz * sin);
cos = cosf(rz/2); sin = sinf(rz/2); quaternion qz(cos, zx * sin, zy * sin, zz * sin);
// 依次旋转三个轴向量
quaternion q = qx*qz*qy; q.normalize(); quaternion qi = q.inverse();
quaternion qx(0, xx, xy, xz); qx = q*qx*qi; qx.normalize(); xx = qx.q2; xy = qx.q3; xz = qx.q4; // 旋转x轴;
quaternion qy(0, yx, yy, yz); qy = q*qy*qi; qy.normalize(); yx = qy.q2; yy = qy.q3; yz = qy.q4; // 旋转y轴;
quaternion qz(0, zx, zy, zz); qz = q*qz*qi; qz.normalize(); zx = qz.q2; zy = qz.q3; zz = qz.q4; // 旋转z轴;
// 每1秒输出一次姿态数据
s_lasttime = samplets;
if(samplets - s_lastlog > 1000000){
console->printf(attitude: [%f, %f, %f]; [%f, %f, %f]; [%f, %f, %f]\n, xx, xy, xz, yx, yy, yz, zx, zy, zz);
s_lastlog = samplets;
}
}
速度快能耗低 光导AI芯片企业获盖茨等近千万美元投资
什么是5G核心网的微服务
FPC的结构层次的不同性质介绍
10.5英寸iPad Pro怎么样？新iPad Pro评测：未来长时间内的主力生产工具
电子工程师结合创造性与习惯
简单的mems陀螺仪姿态算法介绍
软通动力天枢元宇宙研究院在南京江宁高新区揭牌成立
用于智能电网的基于红外的无线电力技术
智能家居的新入口，将全方位管理你和你的家
电动车电池的保养方法_电动车电池的容量怎么看
基于角色访问控制技术的数控机床信息集成系统设计
一文看懂康佳转型之路如何走
华为助力精诊科技打造多模态融合的手术计划工作站
word文档如何解密
雷达水位计介绍
保证SMT加工品质的三种方法!
NEO宣布启动 DevCon 2019
强力磁铁是什么？有哪些应用？
刺客信条再添佳作，什么样的显示器才能够驾驭
让双摄像头成为手机行业的主流配置，还有核心问题需解决