传感器融合

阳光穿透心脏的1/2处 2022-06-08 12:28 227阅读 0赞

目的：**我们需要得到机器人运动的姿态信息，三个轴的角度以及角速度。**

本文大纲：

1、传感器相关模型

2、坐标变换

3、经验型卡尔曼数据融合

4、姿态解算流程

5、DMP

6、数据融合效果

本文出处：http://blog.csdn.net/haishaoli/article/details/52965457

本文内容：

姿态解算总框图（欧拉角方式）：

![20161030121533252][]

**一、数据模型、 噪声模型及曲线**

1、 欧拉角定义：

![Image 1][]

![20161030121604158][]

![Image 1][]2、 数据模型：

![Image 1][]

![20161030121630446][]

其中,φ表示的是测量的角度，b表示的是陀螺仪的飘移误差(为常值偏差)，ωgyro表示的是陀螺仪的角速度输出值。ωg为陀螺仪测量噪声，φacce为加速度计经过处理后得到的角度值，ωa为加速度计测量噪声。

噪声模型近似为高斯分布，如下图：

![Image 1][]

![20161030121707096][]

A、加速度计噪声图

![20161030121731150][]

B、陀螺仪噪声图

![Image 1][]

![20161030121746878][]

**二、 坐标变换（欧拉角方式）**：把陀螺仪角速度变换到加速度计的地理坐标系上

![20161030122100988][]

1、陀螺仪数据的使用：

A的逆：其中A矩阵为姿态矩阵。

下面内容来自《惯性导航》：主要是把三轴角速度从机体坐标系换算到地理坐标系上。

![Image 1][]

![20161030122211531][]

**使用上述公式，在只使用陀螺仪的条件下，可以求得地理坐标系下的三轴角速度信息。**

**角速度进行积分，就能得到相应的角度信息。**

2、加速度计数据的使用：

![20161030122940024][]

![20161030122958259][]

![Image 1][]

**使用上述公式，在只使用加速度计的条件下，可以求得俯仰角和横滚角信息。**

**三、 卡尔曼数据融合**

**A、 融合效果**：本文最后附图。

**B、一行代码**：

a = (1 - d%) \* b + d% \* c;

a 是 融合输出 ，b 是预测量，c 是观察量

这是关键的融合代码。

**C、提问**：

上述代码的含义是什么？

加权平均？互补滤波？低通高通滤波器？

同样的式子，不同的使用过程，有不同的意义。

如果系数是一个固定的值，则有可能是加权平均或者高通低通滤波器。

**我讲的是动态的系数。获取数据在变，方差（或协方差）在变，系数就在变。**

**D、理解：**

1、什么情况使用 ？ 高斯分布。

单模、零均和、高斯噪声 （高斯白噪声）。之前说明了传感器的噪声属于白噪声。

![20161030123135541][]

![Image 1][]

2、卡尔曼为什么是最佳估计 ？下面使用最小二乘来理解。

推导方式一：

q1 是预测量，q2是观察量，q是估计值，w是权值。下文来自《自主移动机器人导论》。

![Image 1][]

![20161030123210088][]

**其中q1和q2的系数之和为1。**

利用这种推导方式可说明卡尔曼是最佳估计，但是并不能推出方差的表达式，并且，权值W i还是我们自己取的。

**推导方式二：**

**运用贝叶斯定理推导**：推出融合表达式和方差表达式

![20161030123752629][]

![20161030123816051][]

3、卡尔曼的核心。 方差，或者说权重。其实就是卡尔曼增益。

![Image 1][]

![20161030124100467][]

**方差公式，跟电阻类似，结果比原来两者都小** **。**因此就算是预测量的累积误差较大，也能提高系统的精度，误差通过迭代而减少。这说明了该方法的有效性。

4、 获得卡尔曼增益的方式对比：

自动通过统计和计算数据方差 VS 人为经验估计方差 的对比 ：

1、自动需要高频数据，人为经验不用。

2、自动具有通用性，人为特定性。

3、自动计算量大，人为的计算量较少。

从5.35可推出：q = (d%) \* q1 + (1-d%) \* q2;

![20161030124149420][]

5、应用：我们的姿态融合，定位融合都是人为给出卡尔曼增益系数的，**叫做** **“经验型卡尔曼滤波”。**

经验之谈：

A、参数一般是给的很小，原因跟传感器（数据）的误差模型相关；

参数小的原因：分析卡尔曼增益的式子，预测量的方差较小，观测量的方差较大，不是同一个数量级，因此增益系数一般给的很小；见定位融合图，可看出方差情况。

B、 **“经验型卡尔曼滤波”的核心思想是变化卡尔曼增益系数；根据经验我们能知道哪些条件下传感器数据的方差大，哪些条件下方差小，从而改变增益系数。**

**C、融合线程的频率越高越好。频率高，一方面收敛快，另外一方面获得的预测量误差小。**

四、 姿态解算流程

以下是以数据流的方式去理解。  
1、传感器原始数据输入。  
2、传感器标定，方法很多，比如六面法。把标定后的数据换算成标准单位。  
3、确定坐标轴和数据符号，符号用右手定则。

4、 欧拉角变换或者四元数变换，变换是为了使陀螺仪角速度的坐标系与加速度计的坐标重合。

5、数据融合，减小方差，降低噪声。  
6、输出融合后的姿态信息。

五、 DMP

这是MPU6X00芯片的自带功能。不开源。

性能还不错，甚至更好。计算有一部分是利用IMU自身的资源，不过依赖芯片产商。

六、数据融合实验效果：

A、 姿态融合效果：蓝色的线是融合输出。平滑并且响应快。

![20161030124719828][]

![20161030124806596][]

![Image 1][] ![20161030124846284][]

![Image 1][]

![20161030124900814][]

![Image 1][]

B、 定位融合效果：**绿色的线是融合输出，蓝色的线是观测量，红色是预测量。**

![20161030124931970][]

[20161030121533252]: /images/20220608/be36d32dc51342ab8a435fe2837a66de.png
[Image 1]: 
[20161030121604158]: /images/20220608/22dd90f7a4384b31b44837e5c0da7743.png
[20161030121630446]: /images/20220608/caf8afc085ec4a3e98bdab93a9a504ac.png
[20161030121707096]: /images/20220608/2bc83f7d950e4a5fa282254cc75d3f7b.png
[20161030121731150]: /images/20220608/8c0451a4d76e4212ae2b059d894935b1.png
[20161030121746878]: /images/20220608/f5b6155855674541a59299228507e2f3.png
[20161030122100988]: /images/20220608/f97def34bb1948faa437b7838beb2505.png
[20161030122211531]: /images/20220608/28c23768a1f64ddcbe70235b0bcae345.png
[20161030122940024]: /images/20220608/16332bd3dc334a77a4073a208f5ec378.png
[20161030122958259]: /images/20220608/5d0c206c67f445b9a909f2be9ef0f016.png
[20161030123135541]: /images/20220608/37252b95bcb94c739591556154487cee.png
[20161030123210088]: /images/20220608/750241edfad04f9eb72abaf4ae01717b.png
[20161030123752629]: /images/20220608/03d8babbc5ab49cbb19c7faf5e3ddc7b.png
[20161030123816051]: /images/20220608/9a0413dbdfed4afe81fc9051eb04e49f.png
[20161030124100467]: /images/20220608/b2443c7e53784fb780f8805788eba5fa.png
[20161030124149420]: /images/20220608/26377dd0c90d49f791bd2f50fe9cfc0b.png
[20161030124719828]: /images/20220608/5b0e1befc00b403b99ddebd129f1a268.png
[20161030124806596]: /images/20220608/b27e3e203ff64fff9a1abb4c533ca4b4.png
[20161030124846284]: /images/20220608/4cb54b516afd465891b7e33db79f3bc0.png
[20161030124900814]: /images/20220608/aee56fbbce2b43039b104db980063787.png
[20161030124931970]: /images/20220608/84767af8a07c4ef4bd86593b52288954.png