陀螺儀測斜數據演算法

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