陀螺仪测斜数据算法

1. 陀螺仪原理，怎么测角度

陀螺仪测角度的工作原理：

陀螺仪本身与引力有关，因为引力的影响，不均衡的陀螺仪，重的一端将向下运行，而轻的一端向上。在引力场中，重物下降的速度是需要时间的，物体坠落的速度远远慢于陀螺仪本身旋转的速度时，将导致陀螺仪偏重点，在旋转中不断的改变陀螺仪自身的平衡，并形成一个向上旋转的速度方向。

如果陀螺仪偏重点太大，陀螺仪自身的左右互作用力也会失效。而在旋转中，陀螺仪如果遇到外力导致，陀螺仪转轮某点受力。陀螺仪会立刻倾斜，而陀螺仪受力点的势能如果低于陀螺仪旋转时速，这时受力点，会因为陀螺仪倾斜，在旋转的推动下，陀螺仪受力点将从斜下角，滑向斜上角。

而在向斜上角运行时，陀螺仪受力点的势能还在向下运行。这就导致陀螺仪到达斜上角时，受力点的剩余势能将会将在位于斜上角时，势能向下推动。

而与受力点相反的直径另一端，同样具备了相应的势能，这个势能与受力点运动方向相反，受力点向下，而它向上，且管这个点叫“联动受力点”。当联动受力点旋转180度，从斜上角到达斜下角，这时联动受力点，将陀螺仪向上拉动。在受力点与联动受力互作用力下，陀螺仪回归平衡。

(1)陀螺仪测斜数据算法扩展阅读：

陀螺仪的应用：

1、隧道中心线测量：

在隧道等挖掘工程中，坑内的中心线测量一般采用难以保证精度的长距离导线。特别是进行盾构挖掘的情况，从立坑的短基准中心线出发必须有很高的测角精度和移站精度，测量中还要经常进行地面和地下的对应检查，以确保测量的精度。

特别是在密集的城市地区，不可能进行过多的检测作业而遇到困难。如果使用陀螺经纬仪可以得到绝对高精度的方位基准，而且可减少耗费很高的检测作业（检查点最少），是一种效率很高的中心线测量方法。

2、通视障碍时的方向角获取：

当有通视障碍，不能从已知点取得方向角时，可以采用天文测量或陀螺经纬仪测量的方法获取方向角（根据建设省测量规范）。与天文测量比较，陀螺经纬仪测量的方法有很多优越性：对天气的依赖少、云的多少无关、无须复杂的天文计算、在现场可以得到任意测线的方向角而容易计算闭合差。

3、日影计算所需的真北测定：

在城市或近郊地区对高层建筑有日照或日影条件的高度限制。在建筑申请时，要附加日影图。此日影图是指，在冬至的真太阳时的8点到16点为基准，进行为了计算、图面绘制所需要的高精度真北方向测定。使用陀螺经纬仪测量可以获得不受天气、时间影响的真北测量。

2. MPU6050 加速度计 陀螺仪 PC机上实现数据融合算法

我知道的也不多，说一下我的理解吧。拿正点原子的程序为例mou6050：原子的程序配合上位机能输出6个数据，加速度输出：ax，ay，az角速度输出：wx，wy，wz分别在上位机上显示，这个数据是原始数据，dmp结算后的四元数。而单片机TFT屏幕上显示的Pitch、Roll、Yaw角度是通过陀螺仪的四元数解算出来，这个数据有个问题即使陀螺仪放不平（有个倾斜角），mpu6050上电后是以此时的状态为0度角度，这样测出来的数据肯定是错误的。原因是：陀螺仪测量的是角速度变化率，它也不知道0度在哪，它是以刷新的那个时刻记为0度开始积分的。因此陀螺仪单独是没法用的需要校准，校准的传感器可以是地磁传感器或者加速度计。而陀螺仪地磁和加速度计又有自己的缺点，需要他们把彼此的数据做个融合。

3. 手机是怎么通过振动计算每天行走的步数的

现在智能手机内部都会配置陀螺仪、加速度传感器等一系列感应硬件，而步数正是依靠这些感应器对我们携带手机过程中的各类活动数据进行监测，然后由手机软件通过分析、计算得来，是不是挺神奇！

陀螺仪：可以测量出手机的角度，从而检测到人体重心的偏移。当人在行走的时候，手中或者口袋中的手机是会随着运动而出现角度偏移的，当陀螺仪检测到持续而且有规律的角度偏移时，手机就会开始判断用户正在走路。

陀螺仪又称“角速度传感器”，它测量的是手机偏转、倾斜的角度，通过角速度传感器可以知晓用户的实际动作，也就是走路时的摆动幅度。

加速度传感器又称“G-Sensor”，测量x、y、z三个轴的加速度，通过加速度传感器可以知晓用户在各个方向上的加速度，即走路的加速度。这里需要注意，因为人走路的加速度是在一个较小的范围值内的，一般不大于10m/s，如果大于这个值的加速度（骑自行车），是会被过滤掉的。

4. cruizcore R6093u输出角度转化公式

ENC03以前用过，我记得还是一个模拟的陀螺仪，受温度湿度影响超级大--特别是温度，除非自己写温度补偿。

简单点的陀螺仪算角度就是陀螺仪测得的原始数据，减去offset，除以比例系数--这个要看你ad的精度以及陀螺仪的量程，我看ENC03是+-90°/s的量程，如果选取的AD是16bit的，那么这个比例系数就是2^(16-1)/90，其他情况类推，最后乘上一个时间dt，把每次取得的值累加就好。

但是陀螺仪的温漂和零漂(零漂就是你那个offset，找的话很麻烦)，并且你的k60也无法保证定时器的精准，所以结果就是累计误差越来越大，使用的现象就是陀螺仪正转90度在反转回90度，发现值已经开始偏差--对了，转的过程如果超量程值直接就错了，比如你按200°/s的速度转，但是陀螺仪的测量范围只有90°，MCU无法检测到这个，他会认为你就是转了90°.

所以陀螺仪一般不是作为角度的直接输出的，ENC03还只是消费级的传感器，我试过用工业级的数字陀螺仪，结果比消费级的好很多，但是实际半小时恒定旋转再放回到原点，误差也在15°左右。

如果对地的夹角，例如对重力的，拿加速度传感器，三轴的，取每个轴向的值然后归一化，再对每个轴取arccos就可以得到了。

对东南西北的话就需要地磁传感器，首先校准--方法很多，基本的八字校准或者高级点的六点快速校准，接下来跟加速度的方法差不多，就是加速度的是相对于重力的，地磁传感器的是相对于地磁线的。

最后最后，说一下，最好的还是将加速度+地磁传感器的值先按轴向取好，再乘上一定的比例系数：比例系数和各个传感器的可信度以及系统MCU的频率有关，最后融合到陀螺仪里面，效果是最好的。

题主可以搜一下Madgwick算法，开源的，2010一个外国人的算法，比kalman滤波运算量要小，然后结果还很准确

5. 加速度计和陀螺仪融合的算法

给你arino的卡尔曼滤波融合算法，非原创，我只是封装了算法。

H文件：
/*
* KalmanFilter.h
* Non-original
* Author: x2d
* Copyright (c) 2012 China
*
*/

#ifndef KalmanFilter_h
#define KalmanFilter_h

#include <WProgram.h>

class KalmanFilter
{
public:
KalmanFilter();

/*
卡尔曼融合计算
angle_m: 加速度计测量并通过atan2(ax,ay)方法计算得到的角度（弧度值）
gyro_m:陀螺仪测量的角速度值（弧度值）
dt:采样时间(s)
outAngle:卡尔曼融合计算出的角度（弧度值）
outAngleDot:卡尔曼融合计算出的角速度（弧度值）
*/
void getValue(double angle_m, double gyro_m, double dt, double &outAngle, double &outAngleDot);

private:
double C_0, Q_angle, Q_gyro, R_angle;
double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
double angle, angle_dot;
double P[2][2];
double Pdot[4];
};

CPP文件：
/*
* KalmanFilter.cpp
* Non-original
* Author: x2d
* Copyright (c) 2012 China
*
*/

#include "KalmanFilter.h"

KalmanFilter::KalmanFilter()
{
C_0 = 1.0f;
Q_angle = 0.001f;
Q_gyro = 0.003f;
R_angle = 0.5f;
q_bias = angle_err = PCt_0 = PCt_1 = E = K_0 = K_1 = t_0 = t_1 = 0.0f;
angle = angle_dot = 0.0f;
P[0][0] = 1.0f;
P[0][1] = 0.0f;
P[1][0] = 0.0f;
P[1][1] = 1.0f;
Pdot[0] = 0.0f;
Pdot[1] = 0.0f;
Pdot[2] = 0.0f;
Pdot[3] = 0.0f;
}

void KalmanFilter::getValue(double angle_m, double gyro_m, double dt, double &outAngle, double &outAngleDot)
{
/*
Serial.print("angle_m = ");
Serial.print(angle_m);
Serial.print(";");
Serial.print("gyro_m = ");
Serial.print(gyro_m);
Serial.print(";");
*/

angle+=(gyro_m-q_bias) * dt;
angle_err = angle_m - angle;
Pdot[0] = Q_angle - P[0][1] - P[1][0];
Pdot[1] = -P[1][1];
Pdot[2] = -P[1][1];
Pdot[3] = Q_gyro;
P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * P[0][1];
P[0][0] -= K_0 * t_0;
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;
angle += K_0 * angle_err;
q_bias += K_1 * angle_err;
angle_dot = gyro_m-q_bias;

outAngle = angle;
outAngleDot = angle_dot;
/*
Serial.print("angle = ");
Serial.print(angle);
Serial.print(";");
Serial.print("angle_dot = ");
Serial.print(angle_dot);
Serial.print(";");
*/
}
#endif