完善资料让更多小伙伴认识你，还能领取20积分哦， 立即完善>
3天内不再提示
电子发烧友论坛|
|
|
相关推荐
4个回答
|
|
|
一、加速度计
（1）计算公式 先介绍一下MPU6050芯片的座标系的定义： 令芯片表面朝向自己，将其表面文字转至正确角度，此时，以芯片内部中心为原点，水平向右的为X轴，竖直向上的为Y轴，指向自己的为Z轴。见下图： 然后我们知道三轴加速度计： ACCEL_XOUT 16位二进制补码值。存储最近的X轴加速计测量值。 ACCEL_YOUT 16位二进制补码值。存储最近的Y轴加速度计测量值。 ACCEL_ZOUT 16位二进制补码值。存储最近的Z轴加速计测量值。 三个加速度分量均以重力加速度 g 的倍数为单位，能够表示的加速度范围，即倍率可以统一设定，有4个可选倍率：±2g、±4g、±8g、±16g。上一篇分析完了，初始化MPU6050设置加速度计输出的满量程范围为± 2g，加速度计每个 LSB 的灵敏度应为 16384 LSB/g。 满量程范围± 2g和灵敏度16384 LSB/g有啥关系? 上面说了这三个加速度分量是16位的二进制补码值，且是有符号的。故而其输出范围 -32768~32767。((2^16)/2) 32767/2 = 16384 即加速度计的灵敏度。 那这个灵敏度又有啥用呢？我们拿串口调试工具，显示的一组数据来举个例子： A X: 03702 Y: 12456 Z: 06268 G X:-00023 Y:-00059 Z: 00005 加速度计 X 轴获取原始数据位 03702，那么它对应的加速度数据是：03702/16384 = 0.23g. g为加速度的单位，重力加速度定义为1g, 等于9.8米每平方秒。 具体的加速度公式：加速度数据 = 加速度轴原始数据/加速度灵敏度 （2）加速度计详解 我们可以把加速度计想象为一个正立方体盒子里放着一个球，如果我们把这个盒子放在没有引力场的地方，或者没有其他可能影响球的位置的场地，球就会漂浮在盒子的中间。 你可以想象这个盒子在远离任何宇宙体的外太空中，或者如果这样一个地方很难想象至少有一个宇宙飞船围绕着这个行星在一切都处于失重状态的轨道上运行。 从上面的图片可以看出，我们为每个轴分配了一对墙（我们移除了墙Y +，所以我们可以看到框内）。 想象一下，每面墙都是压力敏感的。 如果我们将盒子突然移动到左边（我们用加速度 1g = 9.8m/s^2 加速），那么球就会碰到墙壁 X-。 然后，我们测量球施加到墙上的压力，并在X轴上输出 -1g 的值。 请注意，加速度计实际上会检测到与加速度矢量方向相反的力。 这股力量通常被称为惯性力量或虚构力量。 有一点你应该从中学到，就是加速度计通过施加在其中一个墙上的力来间接测量加速度（根据我们的模型，这可能是一个弹簧或者是现实生活中的其他加速度计）。 这个力可能是由加速引起的，但是我们将在下一个例子中看到它并不总是由加速引起的。 如果我们将模型放在地球上，球就会落在Z墙上，并在底壁上施加1g的力，如下图所示： 在这种情况下，盒子不动，但我们仍然可以在Z轴上读取-1g。球施加在墙上的压力是由重力引起的。从理论上讲，它可能是一种不同类型的力量。例如，如果你想象我们的球是金属的，把一个磁铁放在盒子旁边就可以把球移动到另一面墙上。据说这仅仅是为了证明加速度计措施实质上并不加速。恰恰是加速度引起加速度计的力检测机制所捕获的惯性力。 虽然这个模型并不完全如何构建MEMS传感器，但它在解决与加速度计相关的问题时通常是有用的。实际上有类似的传感器，里面有金属球，他们叫做倾斜开关，但是它们比较原始，通常只能告诉设备是否在一定范围内倾斜，而不是倾斜的程度。 到目前为止，我们已经分析了单轴上的加速度计输出，这就是单轴加速度计所能得到的结果。三轴加速度计的实际价值来自于它们可以检测所有三个轴上的惯性力。让我们回到我们的盒子模型，让我们把框向右旋转45度。现在球会碰到2个墙：Z-和X-如下图所示： 0.71的值不是任意的，它们实际上是SQRT（1/2）的近似值。 随着我们介绍下一个加速计模型，这将变得更加清晰。 在以前的模型中，我们已经固定了引力并旋转了我们的想象框。 在最后的两个例子中，我们分析了两个不同盒子的输出，而力矢量保持不变。 虽然这对于理解加速度计如何与外力相互作用很有用，但是如果我们将坐标系固定到加速度计的轴上，并想象力矢量在我们周围旋转，那么执行计算就更为实用。 请看上面的模型，我保留了箭头的颜色，这样你就可以从以前的模型转换到新的模型。 试想一下，新模型中的每个轴都垂直于前一个模型中的各个面。 矢量R是加速度计测量的力矢量（它可以是来自上述示例的重力或惯性力，或者是两者的组合）。 Rx，Ry，Rz是R矢量在X，Y，Z轴上的投影。 请注意以下关系： R^2 = Rx^2 + Ry^2 + Rz^2 方程1 上面有提到，SQRT（1/2）〜0.71的值不是随机的。 如果用上面的公式来表示，在回忆我们的引力为1g后，我们可以证实： 1^2 = (-SQRT(1/2) )^2 + 0 ^2 + (-SQRT(1/2))^2 简单地通过在方程1中代入 R = 1，Rx = -SQRT（1/2），Ry = 0，Rz = -SQRT（1/2） 得出：SQRT（1/2） = √0.5 ≈ 0.71 注：SQRT为平方根计算，SQRT（1/2）的意思就是0.5的平方根。 经过很长的理论序言，我们正在接近现实生活加速度计。 值Rx，Ry，Rz实际上与您的真实加速度计将输出的值线性相关，您可以使用它来执行各种计算。 在我们讲那些之前，让我们谈一谈加速度计将把这些信息传递给我们的方式。大多数加速度计将分为两类：数字和模拟。数字加速度计将使用 I2C，SPI 或 USART 等串行协议为您提供信息，而模拟加速度计将输出预定义范围内的电压电平，您必须使用 ADC（模数转换器）模块将其转换为数字值。关于 ADC 的工作原理在这里不做详细讨论，一方面是因为它是如此广泛的话题，另一方面是因为它不同于一个平台。一些微控制器将有一个内置的ADC模块，其中一些将需要外部元件来执行 ADC 转换。无论使用什么类型的 ADC 模块，你都会得到一定的范围内的值。例如一个 10 位ADC 模块将输出一个范围在 0到1023的值，注意1023 = 2 ^ 10 -1。一个12位ADC模块将输出一个范围在0到4095的值，注意4095 = 2 ^ 12-1。 让我们继续考虑一个简单的例子，假设我们的10位ADC模块给了我们三个加速度计通道（轴）的以下值： AdcRx = 586 AdcRy = 630 AdcRz = 561 每个ADC模块都会有一个参考电压，我们假设在我们的例子中是3.3V。 要将10位的adc值转换为电压，我们使用下面的公式： VoltsRx = AdcRx * Vref / 1023 这里有一个简单的说明：对于8位ADC，最后的分频器是255 = 2 ^ 8 -1，对于12位ADC，最后的分频器是4095 = 2 ^ 12 -1。 将这个公式应用于所有3个通道，我们得到： VoltsRx = 586 * 3.3V / 1023 ≈ 1.89V (我们将所有结果舍入到2个小数点) VoltsRy = 630 * 3.3V / 1023 ≈ 2.03V VoltsRz = 561 * 3.3V / 1023 ≈ 1.81V 每个加速度计都有一个零g的电压水平，你可以在规格中找到它，这是对应于 0g 的电压。 为了得到有符号的电压值，我们需要计算从这个水平的转变。 假设我们的 0g 电压等级是VzeroG = 1.65V。 我们计算零g电压的电压偏移如下： DeltaVoltsRx = 1.89V – 1.65V = 0.24V DeltaVoltsRy = 2.03V – 1.65V = 0.38V DeltaVoltsRz = 1.81V – 1.65V = 0.16V 现在我们的加速度计读数以伏特为单位，它仍然不是 g（9.8 m / s ^ 2），做最后的转换我们应用加速度计的灵敏度，通常用mV/g表示。 可以说我们的灵敏度= 478.5mV/g = 0.4785V/g。 灵敏度值可以在加速度计规格中找到。 为了得到以g表示的最终力值，我们使用下面的公式： Rx = DeltaVoltsRx / Sensitivity Rx = 0.24V / 0.4785V/g ≈ 0.5g Ry = 0.38V / 0.4785V/g ≈ 0.79g Rz = 0.16V / 0.4785V/g ≈ 0.33g 我们当然可以将所有步骤结合在一个公式中，但是我经历了所有的步骤，清楚地说明了如何从ADC读数到用g表示的力矢量分量。 Rx = (AdcRx * Vref / 1023 – VzeroG) / Sensitivity 方程2 Ry = (AdcRy * Vref / 1023 – VzeroG) / Sensitivity Rz = (AdcRz * Vref / 1023 – VzeroG) / Sensitivity ====================================================== 说明： 上面讲了好多理论，该作者是以模拟加速度计来做的介绍，最后得出的公式。 而具体到 MPU6050 加速度计的公式，就是我开头讲的：加速度数据 = 加速度轴原始数据/加速度灵敏度 ====================================================== 我们现在有三个组成部分来定义我们的惯性力矢量，如果该装置不受除引力以外的其他力的影响，我们可以认为这是我们的引力矢量的方向。 如果要计算设备相对于地面的倾斜度，则可以计算此向量与Z轴之间的角度。 如果您还对每个轴的倾斜方向感兴趣，则可以将这个结果分成两个分量：X和Y轴上的倾角，可以计算为重力矢量与X / Y轴之间的角度。 现在我们已经计算了Rx，Ry和Rz的值，计算这些角度比您想象的更简单。 让我们回到我们最后的加速度计模型，并做一些附加的符号： 我们感兴趣的角度是X，Y，Z轴与力矢量R之间的角度。我们将这些角度定义为Axr，Ayr，Azr。 你可以从R和Rx形成的直角三角形中注意到： cos(Axr) = Rx / R , 同理 : cos(Ayr) = Ry / R cos(Azr) = Rz / R 我们可以从方程1中推导出 R = SQRT（Rx ^ 2 + Ry ^ 2 + Rz ^ 2）。 我们可以使用 arccos（）函数（逆cos（）函数） 来找到我们的角度： Axr = arccos(Rx/R) Ayr = arccos(Ry/R) Azr = arccos(Rz/R) （重要公式） ====================================================== 说明： 这里的Rx、Ry、Rz 就是用公式（加速度数据 = 加速度轴原始数据/加速度灵敏度）得出的各轴的加速度数据。 R 也可以通过方程1 导出，R = SQRT（Rx ^ 2 + Ry ^ 2 + Rz ^ 2） 最后就可以通过下面的公式： Axr = arccos(Rx/R) Ayr = arccos(Ry/R) Azr = arccos(Rz/R) 得到X，Y，Z轴与力矢量R之间的角度（Axr，Ayr，Azr） 举个栗子： 当水平放置 MPU6050，只有 Z 轴感受到重力向量，它将输出 -1g。此时 R 就是重力向量。 Rx=0. Ry=0. Rz = -1g 满足 R ^ 2 = RX ^ 2 + RY ^ 2 + RZ ^ 2 = 1 得到重力向量与各个轴的夹角 Axr = arccos(RX / R) = 90度 Ayr = arccos(RY / R) = 90度 Azr = arccos(RZ / R) = 0 度 其中 arccos (1) = 0，arccos (0) = 90 工具：arccos 计算器 计算arccos的时候 点击左上角那个上档功能 cos就变成 arccos ====================================================== 我们用了很长的篇幅来解释加速度计模型，只是想得出这些公式。 根据您的应用程序，您可能想要使用我们派生的任何中间公式。 我们也将很快介绍陀螺仪模型，我们将看到如何将加速度计和陀螺仪数据结合起来，以提供更准确的倾角估计。 但是在我们这样做之前，让我们做一些更有用的符号： cosX = cos(Axr) = Rx / R cosY = cos(Ayr) = Ry / R cosZ = cos(Azr) = Rz / R 这个三元组通常称为方向余弦，它基本上代表了与我们的R向量具有相同方向的单位矢量（长度为1的矢量）。 您可以轻松验证： SQRT(cosX^2 + cosY^2 + cosZ^2) = 1 这是一个很好的属性，因为它使我们无法监视R向量的模数（长度）。 通常情况下，如果我们只是对惯性向量的方向感兴趣，为了简化其他计算，将其模数标准化是有意义的。 二、陀螺仪 （1）计算公式 然后我们知道三轴陀螺仪： GYRO_XOUT 16位二进制补码值。存储最新的X轴陀螺仪测量。 GYRO_YOUT 16位二进制补码值。存储最新的Y轴陀螺仪测量结果。 GYRO_ZOUT 16位二进制补码值。存储最新的Z轴陀螺仪测量结果。 三个角速度分量均以“度/秒”为单位，能够表示的角速度范围，即倍率可统一设定，有4个可选倍率：±250°/s， ±500°/s， ±1000°/s， ±2000°/s。上一篇分析完了，初始化MPU6050设置陀螺仪输出满量程范围为 ± 2000 °/s，陀螺仪每个 LSB 的灵敏度为 16.4 LSB/°/s。 满量程范围± 2000 °/s和灵敏度16.4 LSB/°/s有啥关系? 上面说了这三个陀螺仪分量是16位的二进制补码值，且是有符号的。故而其输出范围 -32768~32767。((2^16)/2) 32767/2000 = 16.4 即陀螺仪的灵敏度。 那这个灵敏度又有啥用呢？我们拿串口调试工具，显示的一组数据来举个例子： A X: 03702 Y: 12456 Z: 06268 G X:-00023 Y:-00059 Z: 00005 陀螺仪 X 轴获取原始数据位 -00023，那么它对应的陀螺仪数据是：-00023/16.4 = -1.4°/s 具体的陀螺仪公式：陀螺仪数据 = 陀螺仪轴原始数据/陀螺仪灵敏度 （2）陀螺仪详解 我们不打算像加速度计那样为陀螺仪引入任何等效的箱体模型，而是直接跳到第二个加速度计模型，我们将根据这个模型显示陀螺仪测量的结果。 每个陀螺仪通道测量围绕其中一个轴的旋转。 例如一个双轴陀螺仪将测量X轴和Y轴的旋转（或者有些人会说“大约”）。 为了用数字来表示这个旋转，我们来做一些符号。 首先让我们定义： Rxz - 是惯性力矢量R在XZ平面上的投影 Ryz - 是惯性力矢量R在YZ平面上的投影 从Rxz和Rz形成的直角三角形，使用毕达哥拉斯定理我们得到： Rxz^2 = Rx^2 + Rz^2 , 同理： Ryz^2 = Ry^2 + Rz^2 还要注意的是： 可以从等式1和上面的等式得出，R^2 = Rxz^2 + Ry^2 或者它可以从由R和Ryz形成的直角三角形导出 R^2 = Ryz^2 + Rx^2 在本文中，我们不打算使用这些公式，但是注意模型中所有值之间的关系是有用的。 相反，我们要定义Z轴和Rxz之间的角度，Ryz矢量如下： Axz - 是Rxz（R在XZ平面上的投影）与Z轴之间的角度 Ayz - 是Ryz（R在YZ平面上的投影）与Z轴之间的角度 现在我们正在接近陀螺仪的测量。 陀螺仪测量上述角度的变化率。 换句话说，它将输出一个与这些角度的变化率线性相关的值。 为了解释这个假设，我们假设我们已经在时间 t0 测量了围绕轴线 Y 的旋转角度（这将是Axz角度），并且我们将其定义为Axz0，接下来我们在稍后的时间 t1 测量该角度并且是 Axz1。 变化率计算如下： RateAxz = (Axz1 – Axz0) / (t1 – t0). 如果我们用度和秒来表示Axz，那么这个值将以 deg/s 表示。 这是一个陀螺仪的测量。 在实践中，陀螺仪（除非它是一个特殊的数字陀螺仪）很少会给你一个以 deg/s 表示的值。 与加速度计一样，您将得到一个ADC值，您需要使用类似于方程2的公式转换为 deg/s。 我们已经定义了加速度计的了，下面来介绍一下ADC转换陀螺仪的转换公式（我们假设我们使用的是10位ADC模块，8位ADC用1023取代1023，12位ADC用4095取代1023）。 RateAxz = (AdcGyroXZ * Vref / 1023 – VzeroRate) / Sensitivity 方程3 RateAyz = (AdcGyroYZ * Vref / 1023 – VzeroRate) / Sensitivity AdcGyroXZ，AdcGyroYZ - 是从我们的adc模块中获得的，它们分别表示在YZ平面上测量XZ中R向量投影旋转的通道，这相当于分别在Y轴和X轴周围进行旋转。 Vref - 是ADC参考电压，我们将在下面的例子中使用3.3V VzeroRate - 是零速率电压，换句话说就是陀螺仪不受任何旋转时输出的电压，对于Acc_Gyro电路板，例如1.23V（您可以在规格中找到这个值， 不要相信规格大多数陀螺仪在焊接后会受到轻微的偏移，所以使用电压表测量每个轴输出的VzeroRate，通常这个值在陀螺仪焊接后不会随时间变化，如果变化的话 - 写一个校准程序， 设备启动时，必须指示用户在启动陀螺仪时将设备保持在静止位置）。 Sensitivity - 陀螺仪的灵敏度，以mV /（deg / s）表示，通常写为mV / deg / s，基本上告诉你陀螺仪输出增加多少mV，如果将转速提高1度/秒。 Acc_Gyro板的灵敏度为2mV / deg / s或0.002V / deg / s 举一个例子，假设我们的ADC模块返回以下值： AdcGyroXZ = 571 AdcGyroXZ = 323 使用上面的公式，并使用Acc_Gyro板的规格参数，我们会得到： RateAxz = (571 * 3.3V / 1023 – 1.23V) / ( 0.002V/deg/s) ≈ 306 deg/s RateAyz = (323 * 3.3V / 1023 – 1.23V) / ( 0.002V/deg/s) ≈ -94 deg/s 换句话说，设备以306度/秒的速度围绕X轴（或者我们可以说它在XZ平面内旋转）绕X轴旋转（或者我们可以说它在YZ平面内旋转），速度为 - 94度/秒。请注意，负号表示设备的旋转方向与传统的正方向相反。按照惯例，一个旋转方向是正的。一个好的陀螺仪规格表会告诉你哪个方向是正的，否则你将不得不通过对该器件的实验来找到它，并注意哪个旋转方向导致输出引脚上的电压增加。这最好用示波器来完成，因为一旦停止旋转，电压将回落到零速率水平。如果您使用的是万用表，您必须保持恒定的旋转速率至少几秒钟，并记下旋转过程中的电压，然后将其与零速率电压进行比较。如果它大于零速率电压，则意味着旋转方向为正。 ====================================================== 说明： 上面讲的又是该作者是以双轴模拟陀螺仪来做的介绍，最后得出的公式。 而具体到 MPU6050 它是三轴陀螺仪其公式就是我开头讲的：陀螺仪数据 = 陀螺仪轴原始数据/加速度灵敏度 ====================================================== |
|
|
|
|
|
一、加速度计
（1）计算公式 先介绍一下MPU6050芯片的座标系的定义： 令芯片表面朝向自己，将其表面文字转至正确角度，此时，以芯片内部中心为原点，水平向右的为X轴，竖直向上的为Y轴，指向自己的为Z轴。见下图： 然后我们知道三轴加速度计： ACCEL_XOUT 16位二进制补码值。存储最近的X轴加速计测量值。 ACCEL_YOUT 16位二进制补码值。存储最近的Y轴加速度计测量值。 ACCEL_ZOUT 16位二进制补码值。存储最近的Z轴加速计测量值。 三个加速度分量均以重力加速度 g 的倍数为单位，能够表示的加速度范围，即倍率可以统一设定，有4个可选倍率：±2g、±4g、±8g、±16g。上一篇分析完了，初始化MPU6050设置加速度计输出的满量程范围为± 2g，加速度计每个 LSB 的灵敏度应为 16384 LSB/g。 满量程范围± 2g和灵敏度16384 LSB/g有啥关系? 上面说了这三个加速度分量是16位的二进制补码值，且是有符号的。故而其输出范围 -32768~32767。((2^16)/2) 32767/2 = 16384 即加速度计的灵敏度。 那这个灵敏度又有啥用呢？我们拿串口调试工具，显示的一组数据来举个例子： A X: 03702 Y: 12456 Z: 06268 G X:-00023 Y:-00059 Z: 00005 加速度计 X 轴获取原始数据位 03702，那么它对应的加速度数据是：03702/16384 = 0.23g. g为加速度的单位，重力加速度定义为1g, 等于9.8米每平方秒。 具体的加速度公式：加速度数据 = 加速度轴原始数据/加速度灵敏度 （2）加速度计详解 我们可以把加速度计想象为一个正立方体盒子里放着一个球，如果我们把这个盒子放在没有引力场的地方，或者没有其他可能影响球的位置的场地，球就会漂浮在盒子的中间。 你可以想象这个盒子在远离任何宇宙体的外太空中，或者如果这样一个地方很难想象至少有一个宇宙飞船围绕着这个行星在一切都处于失重状态的轨道上运行。 从上面的图片可以看出，我们为每个轴分配了一对墙（我们移除了墙Y +，所以我们可以看到框内）。 想象一下，每面墙都是压力敏感的。 如果我们将盒子突然移动到左边（我们用加速度 1g = 9.8m/s^2 加速），那么球就会碰到墙壁 X-。 然后，我们测量球施加到墙上的压力，并在X轴上输出 -1g 的值。 请注意，加速度计实际上会检测到与加速度矢量方向相反的力。 这股力量通常被称为惯性力量或虚构力量。 有一点你应该从中学到，就是加速度计通过施加在其中一个墙上的力来间接测量加速度（根据我们的模型，这可能是一个弹簧或者是现实生活中的其他加速度计）。 这个力可能是由加速引起的，但是我们将在下一个例子中看到它并不总是由加速引起的。 如果我们将模型放在地球上，球就会落在Z墙上，并在底壁上施加1g的力，如下图所示： 在这种情况下，盒子不动，但我们仍然可以在Z轴上读取-1g。球施加在墙上的压力是由重力引起的。从理论上讲，它可能是一种不同类型的力量。例如，如果你想象我们的球是金属的，把一个磁铁放在盒子旁边就可以把球移动到另一面墙上。据说这仅仅是为了证明加速度计措施实质上并不加速。恰恰是加速度引起加速度计的力检测机制所捕获的惯性力。 虽然这个模型并不完全如何构建MEMS传感器，但它在解决与加速度计相关的问题时通常是有用的。实际上有类似的传感器，里面有金属球，他们叫做倾斜开关，但是它们比较原始，通常只能告诉设备是否在一定范围内倾斜，而不是倾斜的程度。 到目前为止，我们已经分析了单轴上的加速度计输出，这就是单轴加速度计所能得到的结果。三轴加速度计的实际价值来自于它们可以检测所有三个轴上的惯性力。让我们回到我们的盒子模型，让我们把框向右旋转45度。现在球会碰到2个墙：Z-和X-如下图所示： 0.71的值不是任意的，它们实际上是SQRT（1/2）的近似值。 随着我们介绍下一个加速计模型，这将变得更加清晰。 在以前的模型中，我们已经固定了引力并旋转了我们的想象框。 在最后的两个例子中，我们分析了两个不同盒子的输出，而力矢量保持不变。 虽然这对于理解加速度计如何与外力相互作用很有用，但是如果我们将坐标系固定到加速度计的轴上，并想象力矢量在我们周围旋转，那么执行计算就更为实用。 请看上面的模型，我保留了箭头的颜色，这样你就可以从以前的模型转换到新的模型。 试想一下，新模型中的每个轴都垂直于前一个模型中的各个面。 矢量R是加速度计测量的力矢量（它可以是来自上述示例的重力或惯性力，或者是两者的组合）。 Rx，Ry，Rz是R矢量在X，Y，Z轴上的投影。 请注意以下关系： R^2 = Rx^2 + Ry^2 + Rz^2 方程1 上面有提到，SQRT（1/2）〜0.71的值不是随机的。 如果用上面的公式来表示，在回忆我们的引力为1g后，我们可以证实： 1^2 = (-SQRT(1/2) )^2 + 0 ^2 + (-SQRT(1/2))^2 简单地通过在方程1中代入 R = 1，Rx = -SQRT（1/2），Ry = 0，Rz = -SQRT（1/2） 得出：SQRT（1/2） = √0.5 ≈ 0.71 注：SQRT为平方根计算，SQRT（1/2）的意思就是0.5的平方根。 经过很长的理论序言，我们正在接近现实生活加速度计。 值Rx，Ry，Rz实际上与您的真实加速度计将输出的值线性相关，您可以使用它来执行各种计算。 在我们讲那些之前，让我们谈一谈加速度计将把这些信息传递给我们的方式。大多数加速度计将分为两类：数字和模拟。数字加速度计将使用 I2C，SPI 或 USART 等串行协议为您提供信息，而模拟加速度计将输出预定义范围内的电压电平，您必须使用 ADC（模数转换器）模块将其转换为数字值。关于 ADC 的工作原理在这里不做详细讨论，一方面是因为它是如此广泛的话题，另一方面是因为它不同于一个平台。一些微控制器将有一个内置的ADC模块，其中一些将需要外部元件来执行 ADC 转换。无论使用什么类型的 ADC 模块，你都会得到一定的范围内的值。例如一个 10 位ADC 模块将输出一个范围在 0到1023的值，注意1023 = 2 ^ 10 -1。一个12位ADC模块将输出一个范围在0到4095的值，注意4095 = 2 ^ 12-1。 让我们继续考虑一个简单的例子，假设我们的10位ADC模块给了我们三个加速度计通道（轴）的以下值： AdcRx = 586 AdcRy = 630 AdcRz = 561 每个ADC模块都会有一个参考电压，我们假设在我们的例子中是3.3V。 要将10位的adc值转换为电压，我们使用下面的公式： VoltsRx = AdcRx * Vref / 1023 这里有一个简单的说明：对于8位ADC，最后的分频器是255 = 2 ^ 8 -1，对于12位ADC，最后的分频器是4095 = 2 ^ 12 -1。 将这个公式应用于所有3个通道，我们得到： VoltsRx = 586 * 3.3V / 1023 ≈ 1.89V (我们将所有结果舍入到2个小数点) VoltsRy = 630 * 3.3V / 1023 ≈ 2.03V VoltsRz = 561 * 3.3V / 1023 ≈ 1.81V 每个加速度计都有一个零g的电压水平，你可以在规格中找到它，这是对应于 0g 的电压。 为了得到有符号的电压值，我们需要计算从这个水平的转变。 假设我们的 0g 电压等级是VzeroG = 1.65V。 我们计算零g电压的电压偏移如下： DeltaVoltsRx = 1.89V – 1.65V = 0.24V DeltaVoltsRy = 2.03V – 1.65V = 0.38V DeltaVoltsRz = 1.81V – 1.65V = 0.16V 现在我们的加速度计读数以伏特为单位，它仍然不是 g（9.8 m / s ^ 2），做最后的转换我们应用加速度计的灵敏度，通常用mV/g表示。 可以说我们的灵敏度= 478.5mV/g = 0.4785V/g。 灵敏度值可以在加速度计规格中找到。 为了得到以g表示的最终力值，我们使用下面的公式： Rx = DeltaVoltsRx / Sensitivity Rx = 0.24V / 0.4785V/g ≈ 0.5g Ry = 0.38V / 0.4785V/g ≈ 0.79g Rz = 0.16V / 0.4785V/g ≈ 0.33g 我们当然可以将所有步骤结合在一个公式中，但是我经历了所有的步骤，清楚地说明了如何从ADC读数到用g表示的力矢量分量。 Rx = (AdcRx * Vref / 1023 – VzeroG) / Sensitivity 方程2 Ry = (AdcRy * Vref / 1023 – VzeroG) / Sensitivity Rz = (AdcRz * Vref / 1023 – VzeroG) / Sensitivity ====================================================== 说明： 上面讲了好多理论，该作者是以模拟加速度计来做的介绍，最后得出的公式。 而具体到 MPU6050 加速度计的公式，就是我开头讲的：加速度数据 = 加速度轴原始数据/加速度灵敏度 ====================================================== 我们现在有三个组成部分来定义我们的惯性力矢量，如果该装置不受除引力以外的其他力的影响，我们可以认为这是我们的引力矢量的方向。 如果要计算设备相对于地面的倾斜度，则可以计算此向量与Z轴之间的角度。 如果您还对每个轴的倾斜方向感兴趣，则可以将这个结果分成两个分量：X和Y轴上的倾角，可以计算为重力矢量与X / Y轴之间的角度。 现在我们已经计算了Rx，Ry和Rz的值，计算这些角度比您想象的更简单。 让我们回到我们最后的加速度计模型，并做一些附加的符号： 我们感兴趣的角度是X，Y，Z轴与力矢量R之间的角度。我们将这些角度定义为Axr，Ayr，Azr。 你可以从R和Rx形成的直角三角形中注意到： cos(Axr) = Rx / R , 同理 : cos(Ayr) = Ry / R cos(Azr) = Rz / R 我们可以从方程1中推导出 R = SQRT（Rx ^ 2 + Ry ^ 2 + Rz ^ 2）。 我们可以使用 arccos（）函数（逆cos（）函数） 来找到我们的角度： Axr = arccos(Rx/R) Ayr = arccos(Ry/R) Azr = arccos(Rz/R) （重要公式） ====================================================== 说明： 这里的Rx、Ry、Rz 就是用公式（加速度数据 = 加速度轴原始数据/加速度灵敏度）得出的各轴的加速度数据。 R 也可以通过方程1 导出，R = SQRT（Rx ^ 2 + Ry ^ 2 + Rz ^ 2） 最后就可以通过下面的公式： Axr = arccos(Rx/R) Ayr = arccos(Ry/R) Azr = arccos(Rz/R) 得到X，Y，Z轴与力矢量R之间的角度（Axr，Ayr，Azr） 举个栗子： 当水平放置 MPU6050，只有 Z 轴感受到重力向量，它将输出 -1g。此时 R 就是重力向量。 Rx=0. Ry=0. Rz = -1g 满足 R ^ 2 = RX ^ 2 + RY ^ 2 + RZ ^ 2 = 1 得到重力向量与各个轴的夹角 Axr = arccos(RX / R) = 90度 Ayr = arccos(RY / R) = 90度 Azr = arccos(RZ / R) = 0 度 其中 arccos (1) = 0，arccos (0) = 90 工具：arccos 计算器 计算arccos的时候 点击左上角那个上档功能 cos就变成 arccos ====================================================== 我们用了很长的篇幅来解释加速度计模型，只是想得出这些公式。 根据您的应用程序，您可能想要使用我们派生的任何中间公式。 我们也将很快介绍陀螺仪模型，我们将看到如何将加速度计和陀螺仪数据结合起来，以提供更准确的倾角估计。 但是在我们这样做之前，让我们做一些更有用的符号： cosX = cos(Axr) = Rx / R cosY = cos(Ayr) = Ry / R cosZ = cos(Azr) = Rz / R 这个三元组通常称为方向余弦，它基本上代表了与我们的R向量具有相同方向的单位矢量（长度为1的矢量）。 您可以轻松验证： SQRT(cosX^2 + cosY^2 + cosZ^2) = 1 这是一个很好的属性，因为它使我们无法监视R向量的模数（长度）。 通常情况下，如果我们只是对惯性向量的方向感兴趣，为了简化其他计算，将其模数标准化是有意义的。 二、陀螺仪 （1）计算公式 然后我们知道三轴陀螺仪： GYRO_XOUT 16位二进制补码值。存储最新的X轴陀螺仪测量。 GYRO_YOUT 16位二进制补码值。存储最新的Y轴陀螺仪测量结果。 GYRO_ZOUT 16位二进制补码值。存储最新的Z轴陀螺仪测量结果。 三个角速度分量均以“度/秒”为单位，能够表示的角速度范围，即倍率可统一设定，有4个可选倍率：±250°/s， ±500°/s， ±1000°/s， ±2000°/s。上一篇分析完了，初始化MPU6050设置陀螺仪输出满量程范围为 ± 2000 °/s，陀螺仪每个 LSB 的灵敏度为 16.4 LSB/°/s。 满量程范围± 2000 °/s和灵敏度16.4 LSB/°/s有啥关系? 上面说了这三个陀螺仪分量是16位的二进制补码值，且是有符号的。故而其输出范围 -32768~32767。((2^16)/2) 32767/2000 = 16.4 即陀螺仪的灵敏度。 那这个灵敏度又有啥用呢？我们拿串口调试工具，显示的一组数据来举个例子： A X: 03702 Y: 12456 Z: 06268 G X:-00023 Y:-00059 Z: 00005 陀螺仪 X 轴获取原始数据位 -00023，那么它对应的陀螺仪数据是：-00023/16.4 = -1.4°/s 具体的陀螺仪公式：陀螺仪数据 = 陀螺仪轴原始数据/陀螺仪灵敏度 （2）陀螺仪详解 我们不打算像加速度计那样为陀螺仪引入任何等效的箱体模型，而是直接跳到第二个加速度计模型，我们将根据这个模型显示陀螺仪测量的结果。 每个陀螺仪通道测量围绕其中一个轴的旋转。 例如一个双轴陀螺仪将测量X轴和Y轴的旋转（或者有些人会说“大约”）。 为了用数字来表示这个旋转，我们来做一些符号。 首先让我们定义： Rxz - 是惯性力矢量R在XZ平面上的投影 Ryz - 是惯性力矢量R在YZ平面上的投影 从Rxz和Rz形成的直角三角形，使用毕达哥拉斯定理我们得到： Rxz^2 = Rx^2 + Rz^2 , 同理： Ryz^2 = Ry^2 + Rz^2 还要注意的是： 可以从等式1和上面的等式得出，R^2 = Rxz^2 + Ry^2 或者它可以从由R和Ryz形成的直角三角形导出 R^2 = Ryz^2 + Rx^2 在本文中，我们不打算使用这些公式，但是注意模型中所有值之间的关系是有用的。 相反，我们要定义Z轴和Rxz之间的角度，Ryz矢量如下： Axz - 是Rxz（R在XZ平面上的投影）与Z轴之间的角度 Ayz - 是Ryz（R在YZ平面上的投影）与Z轴之间的角度 现在我们正在接近陀螺仪的测量。 陀螺仪测量上述角度的变化率。 换句话说，它将输出一个与这些角度的变化率线性相关的值。 为了解释这个假设，我们假设我们已经在时间 t0 测量了围绕轴线 Y 的旋转角度（这将是Axz角度），并且我们将其定义为Axz0，接下来我们在稍后的时间 t1 测量该角度并且是 Axz1。 变化率计算如下： RateAxz = (Axz1 – Axz0) / (t1 – t0). 如果我们用度和秒来表示Axz，那么这个值将以 deg/s 表示。 这是一个陀螺仪的测量。 在实践中，陀螺仪（除非它是一个特殊的数字陀螺仪）很少会给你一个以 deg/s 表示的值。 与加速度计一样，您将得到一个ADC值，您需要使用类似于方程2的公式转换为 deg/s。 我们已经定义了加速度计的了，下面来介绍一下ADC转换陀螺仪的转换公式（我们假设我们使用的是10位ADC模块，8位ADC用1023取代1023，12位ADC用4095取代1023）。 RateAxz = (AdcGyroXZ * Vref / 1023 – VzeroRate) / Sensitivity 方程3 RateAyz = (AdcGyroYZ * Vref / 1023 – VzeroRate) / Sensitivity AdcGyroXZ，AdcGyroYZ - 是从我们的adc模块中获得的，它们分别表示在YZ平面上测量XZ中R向量投影旋转的通道，这相当于分别在Y轴和X轴周围进行旋转。 Vref - 是ADC参考电压，我们将在下面的例子中使用3.3V VzeroRate - 是零速率电压，换句话说就是陀螺仪不受任何旋转时输出的电压，对于Acc_Gyro电路板，例如1.23V（您可以在规格中找到这个值， 不要相信规格大多数陀螺仪在焊接后会受到轻微的偏移，所以使用电压表测量每个轴输出的VzeroRate，通常这个值在陀螺仪焊接后不会随时间变化，如果变化的话 - 写一个校准程序， 设备启动时，必须指示用户在启动陀螺仪时将设备保持在静止位置）。 Sensitivity - 陀螺仪的灵敏度，以mV /（deg / s）表示，通常写为mV / deg / s，基本上告诉你陀螺仪输出增加多少mV，如果将转速提高1度/秒。 Acc_Gyro板的灵敏度为2mV / deg / s或0.002V / deg / s 举一个例子，假设我们的ADC模块返回以下值： AdcGyroXZ = 571 AdcGyroXZ = 323 使用上面的公式，并使用Acc_Gyro板的规格参数，我们会得到： RateAxz = (571 * 3.3V / 1023 – 1.23V) / ( 0.002V/deg/s) ≈ 306 deg/s RateAyz = (323 * 3.3V / 1023 – 1.23V) / ( 0.002V/deg/s) ≈ -94 deg/s 换句话说，设备以306度/秒的速度围绕X轴（或者我们可以说它在XZ平面内旋转）绕X轴旋转（或者我们可以说它在YZ平面内旋转），速度为 - 94度/秒。请注意，负号表示设备的旋转方向与传统的正方向相反。按照惯例，一个旋转方向是正的。一个好的陀螺仪规格表会告诉你哪个方向是正的，否则你将不得不通过对该器件的实验来找到它，并注意哪个旋转方向导致输出引脚上的电压增加。这最好用示波器来完成，因为一旦停止旋转，电压将回落到零速率水平。如果您使用的是万用表，您必须保持恒定的旋转速率至少几秒钟，并记下旋转过程中的电压，然后将其与零速率电压进行比较。如果它大于零速率电压，则意味着旋转方向为正。 ====================================================== 说明： 上面讲的又是该作者是以双轴模拟陀螺仪来做的介绍，最后得出的公式。 而具体到 MPU6050 它是三轴陀螺仪其公式就是我开头讲的：陀螺仪数据 = 陀螺仪轴原始数据/加速度灵敏度 ====================================================== |
|
|
|
|
|
三、结合加速度计和陀螺仪数据
使用组合了加速度计和陀螺仪的组合IMU设备的第一步是对齐它们的坐标系。 最简单的方法是选择加速度计坐标系作为参考坐标系。 大多数加速计数据表将显示相对于物理芯片或设备图像的X，Y，Z轴的方向。 例如，这里是Acc_Gyro板的规格中所示的X，Y，Z轴的方向 接下来的步骤是： - 识别与上面讨论的 RateAxz，RateAyz 值相对应的陀螺仪输出。 - 由于陀螺仪相对于加速度计的物理位置，确定这些输出是否需要被反转 不要假设陀螺仪的输出标记为X或Y，它将对应于加速度计坐标系中的任何轴，即使该输出是IMU单元的一部分。 最好的方法是测试它。 下面是一个示例序列，用于确定陀螺仪的哪个输出对应上面讨论的RateAxz值。 - 从将设备放置在水平位置开始。加速度计的X和Y输出都会输出 零g 电压（例如Acc_Gyro电路板，这是1.65V） - 下一步开始绕 Y 轴旋转设备，另一种方式是在 XZ 平面上旋转设备，使X和Z加速度计输出变化，Y输出保持不变。 - 以恒定的速度旋转设备，注意陀螺仪输出的变化，其他陀螺仪的输出应该保持不变 - 在绕Y轴旋转（在XZ平面内旋转）期间改变的陀螺仪输出将为 AdcGyroXZ 提供输入值，从中计算出 RateAxz - 最后一步是确保旋转方向对应于我们的模型，在某些情况下，由于陀螺仪相对于加速度计的物理位置，您可能需要反转 RateAxz 值 - 再次执行上述测试，将设备围绕Y轴旋转，这次监控加速度计的X输出（在我们的模型中为AdcRx）。如果 AdcRx 增长（从水平位置开始的第一个90度旋转），则AdcGyroXZ应该减小。这是由于我们正在监测引力向量，当设备沿着一个方向旋转时，向量将沿相反方向旋转（相对于我们正在使用的设备坐标系统）。所以，否则你需要倒置RateAxz，你可以通过在公式3中引入一个符号因子来达到这个目的，如下所示： RateAxz = InvertAxz *（AdcGyroXZ * Vref / 1023 - VzeroRate）/Sensitivity，其中InvertAxz是1或-1 对于RateAyz，可以通过围绕X轴旋转设备来完成相同的测试，并且可以确定哪个陀螺仪输出对应于RateAyz，以及是否需要反转。 一旦你有了InvertAyz的值，你应该使用下面的公式来计算RateAyz： RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 – VzeroRate) / Sensitivity 如果你想在Acc_Gyro板上做这些测试，你会得到如下结果： - RateAxz的输出引脚为GX4，InvertAxz = 1 - RateAyz的输出引脚是GY4，InvertAyz = 1 从这一点开始，我们将考虑您已经设置了IMU，以便您可以计算Axr，Ayr，Azr（如定义的第1部分。加速度计）和RateAxz，RateAyz（如第2部分中定义的）的正确值。陀螺仪）。接下来，我们将分析这些值之间的关系，这对于获得设备相对于地平面的倾角的更精确的估计是有用的。 你可能会问自己，如果加速度计模型已经给了我们Axr，Ayr，Azr的倾斜角度，为什么我们要获取陀螺仪的数据呢？答案很简单：加速度计数据不能总是被信任100％。有几个原因，请记住，加速度计测量惯性力，这种力可能是由引力（理想情况下只有引力）引起的，但也可能是由设备的加速度（运动）引起的。因此，即使加速度计处于相对稳定的状态，对振动和机械噪声仍然非常敏感。这是大多数IMU系统使用陀螺仪消除任何加速度计误差的主要原因。但是，这是如何完成的？陀螺仪是否没有噪音？ 陀螺仪不是没有噪音的，但是因为它测量旋转，所以对线性机械运动（加速度计所受到的噪音类型）不那么敏感，但是陀螺仪还有其他类型的问题，例如漂移（不回到零速率值当旋转停止时）。尽管如此，通过对来自加速度计和陀螺仪的数据进行平均，我们可以获得比通过单独使用加速度计数据获得的当前设备倾角更好的估计。 在接下来的步骤中，我将介绍一种受卡尔曼滤波器中的一些想法启发的算法，然而它在嵌入式设备上实现起来要简单得多。在此之前，让我们先看看我们想要算法的计算。那么，引力矢量的方向是R = [Rx，Ry，Rz]，我们可以从中得出其他值，如Axr，Ayr，Azr或cosX，cosy，cosZ，它们会给我们一个关于我们设备的倾向的想法相对于地平面，我们在第一部分讨论了这些值之间的关系。有人可能会说 - 我们不是已经有了第1部分公式2中的Rx，Ry，Rz值吗？是的，但请记住，这些值仅来自加速度计数据，所以如果你直接在你的应用程序中使用它们，你可能会得到比应用程序能够容忍的更多的噪声。为了避免进一步的混淆，我们重新定义加速度计测量如下： Racc - 是由加速度计测量的惯性力矢量，由以下部件（X，Y，Z轴上的投影）组成： RxAcc = (AdcRx * Vref / 1023 – VzeroG) / Sensitivity RyAcc = (AdcRy * Vref / 1023 – VzeroG) / Sensitivity RzAcc = (AdcRz * Vref / 1023 – VzeroG) / Sensitivity 到目前为止，我们有一组测量值，我们可以从加速度计ADC值纯粹获得。 我们将这组数据称为“向量”，我们将使用下面的符号。 Racc = [RxAcc,RyAcc,RzAcc] 因为Racc的这些组件可以从加速度计数据中获得，所以我们可以把它看作是我们算法的一个输入。 请注意，因为Racc测量引力，如果你认为这个矢量的长度定义如下，那么你将是正确的。 |Racc| = SQRT(RxAcc^2 +RyAcc^2 + RzAcc^2) 不过要确定的是更新这个向量是有意义的，如下所示： Racc(标准化) = [RxAcc/|Racc| , RyAcc/|Racc| , RzAcc/|Racc|]. 这将确保您的归一化Racc向量的长度始终为1。 接下来，我们将介绍一个新的矢量，我们将会调用它 Rest = [RxEst,RyEst,RzEst] 这将是我们的算法的输出，这些是基于陀螺仪数据和基于过去估计的数据的校正值。 以下是我们的算法将要做的事情： - 加速计告诉我们：“你现在在位置Racc” - 我们说“谢谢，但让我检查”， 然后用陀螺仪数据以及过去的剩余数据校正这些信息，并输出新的估计向量Rest。 - 我们认为Rest是我们设备当前位置的“最佳选择”。 让我们看看我们如何才能使其工作。 我们将通过相信我们的加速度计并分配： Rest(0) = Racc(0) 顺便提一下，Rest和Racc是向量，所以上面的方程只是写出方程3的简单方法，并且避免了重复： RxEst(0) = RxAcc(0) RyEst(0) = RyAcc(0) RzEst(0) = RzAcc(0) 接下来，我们将在T秒的相同时间间隔内进行常规测量，我们将获得新的测量结果，我们将定义为Racc（1），Racc（2），Racc（3）等等。 我们还会在每个时间间隔Rest（1），Rest（2），Rest（3）等等上发布新的估计值。 假设我们在步骤n。 我们有两套我们想要使用的已知值： Rest（n-1） - 我们以前的估计，Rest（0）= Racc（0） Racc（n） - 我们目前的加速度计测量 在计算Rest（n）之前，我们介绍一个新的测量值，我们可以从陀螺仪中获得一个以前的估计值。 我们将它称为Rgyro，它也是一个由3个部分组成的向量： Rgyro = [RxGyro,RyGyro,RzGyro] 我们将一次计算这个向量一个组件。 我们将从RxGyro开始。 首先观察我们的陀螺仪模型中的以下关系，从由Rz和Rxz组成的直角三角形，我们可以推导出： tan(Axz) = Rx/Rz => Axz = atan2(Rx,Rz) atan2可能是你以前从未使用过的函数，它和atan类似，不同的是它返回（-PI，PI）范围内的值，而不是由atan返回的（-PI / 2，PI / 2） 2个参数，而不是一个。 它允许我们将Rx，Rz的两个值转换为360度全方位角度（-PI到PI）。 你可以在这里阅读关于atan2的更多信息。 所以知道RxEst（n-1）和RzEst（n-1）我们可以找到： Axz(n-1) = atan2( RxEst(n-1) , RzEst(n-1) ). 请记住，陀螺仪测量Axz角度的变化率。 所以我们可以估计新的角度Axz（n）如下： Axz(n) = Axz(n-1) + RateAxz(n) * T 请记住，RateAxz可以从我们的陀螺仪ADC读数中获得。 更精确的公式可以使用如下计算的平均旋转速率： RateAxzAvg = ( RateAxz(n) + RateAxz(n-1) ) / 2 Axz(n) = Axz(n-1) + RateAxzAvg * T 我们可以找到相同的方式： Ayz(n) = Ayz(n-1) + RateAyz(n) * T 好的，现在我们有Axz（n）和Ayz（n）。 我们从哪里去扣除RxGyro / RyGyro？ 从方程 1，我们可以写出矢量Rgyro的长度如下： |Rgyro| = SQRT(RxGyro^2 + RyGyro^2 + RzGyro^2) 另外，因为我们对Racc向量进行了归一化处理，所以我们可以假设它的长度是1，并且在旋转之后它没有改变，所以写出来是相对安全的： |Rgyro| = 1 让我们在下面的计算中采用一个临时的简短表示法： x =RxGyro , y=RyGyro, z=RzGyro 使用上面的关系，我们可以写： x = x / 1 = x / SQRT(x^2+y^2+z^2) 用SQRT（x^2+z^2）来划分分数的分子和分母， x = ( x / SQRT(x^2 + z^2) ) / SQRT( (x^2 + y^2 + z^2) / (x^2 + z^2) ) 注意 x / SQRT(x^2 + z^2) = sin(Axz)，所以： x = sin(Axz) / SQRT (1 + y^2 / (x^2 + z^2) ) 现在将SQRT内部的分子和分母相乘z^2 x = sin(Axz) / SQRT (1 + y^2 * z ^2 / (z^2 * (x^2 + z^2)) ) 注意 z / SQRT(x^2 + z^2) = cos(Axz) and y / z = tan(Ayz)，所以最后： x = sin(Axz) / SQRT (1 + cos(Axz)^2 * tan(Ayz)^2 ) 回到我们的符号，我们得到： RxGyro = sin(Axz(n)) / SQRT (1 + cos(Axz(n))^2 * tan(Ayz(n))^2 ) 同样的方式，我们发现 RyGyro = sin(Ayz(n)) / SQRT (1 + cos(Ayz(n))^2 * tan(Axz(n))^2 ) 注意：可以进一步简化这个公式。 通过将sin（Axz（n））的两部分除以得到： RxGyro = 1 / SQRT (1/ sin(Axz(n))^2 + cos(Axz(n))^2 / sin(Axz(n))^2 * tan(Ayz(n))^2 ) RxGyro = 1 / SQRT (1/ sin(Axz(n))^2 + cot(Axz(n))^2 * sin(Ayz(n))^2 / cos(Ayz(n))^2 ) 现在加减 cos(Axz(n))^2/sin(Axz(n))^2 = cot(Axz(n))^2 RxGyro = 1 / SQRT (1/ sin(Axz(n))^2 – cos(Axz(n))^2/sin(Axz(n))^2 + cot(Axz(n))^2 * sin(Ayz(n))^2 / cos(Ayz(n))^2 + cot(Axz(n))^2 ) 并通过分组条款1和2，然后3和4我们得到： RxGyro = 1 / SQRT (1 + cot(Axz(n))^2 * sec(Ayz(n))^2 ), where cot(x) = 1 / tan(x) and sec(x) = 1 / cos(x) 该公式仅使用2个三角函数，并且可以在计算上更简单。 如果你有Mathematica程序，你可以验证它 通过评估 FullSimplify [Sin [A] ^ 2 /（1 + Cos [A] ^ 2 * Tan [B] ^ 2）] 现在，我们终于可以找到： RzGyro = Sign(RzGyro)*SQRT(1 – RxGyro^2 – RyGyro^2). 当 RzGyro> = 0 时符号（RzGyro）= 1，当 RzGyro <0 时符号（RzGyro）= -1。 估计这个简单的方法是采取： Sign(RzGyro) = Sign(RzEst(n-1)) 实际上，当RzEst（n-1）接近于0时要小心。在这种情况下，您可以完全跳过陀螺仪相位，并指定：Rgyro = Rest（n-1）。 Rz用作计算Axz和Ayz角度的参考，当它接近0时，值可能溢出并触发不良结果。 你会在大浮点数字的地方tan（）/ atan（）函数实现可能缺乏精度。 所以让我们回顾一下我们到目前为止，我们在我们算法的第n步，我们已经计算出以下值： Racc - 来自我们的加速度计的当前读数 Rgyro - 从Rest（n-1）获得并获得当前的陀螺仪读数 我们使用哪个值来计算更新的估计值Rest（n）？ 你可能猜到我们会同时使用这两个。 我们将使用加权平均值，以便： Rest(n) = (Racc * w1 + Rgyro * w2 ) / (w1 + w2) 我们可以通过将分数的分子和分母除以w1来简化这个公式。 Rest(n) = (Racc * w1/w1 + Rgyro * w2/w1 ) / (w1/w1 + w2/w1) 在代入w2 / w1 = wGro后，我们得到： Rest(n) = (Racc + Rgyro * wGyro ) / (1 + wGyro) 在上面的公式中，wGyro告诉我们相比于我们的加速度计，我们相信我们的陀螺仪有多少。 这个值可以通过实验选择，通常在5到20之间的值将会产生好的结果。 该算法与卡尔曼滤波器的主要区别在于该权重相对固定，而在卡尔曼滤波器中，根据加速度计读数的测量噪声永久更新权重。 卡尔曼滤波器专注于为您提供“最好的”理论结果，而这种算法可以为您的实际应用提供“足够好”的结果。 您可以实现一个算法，根据您测量的噪音因素来调整wGyro，但是对于大多数应用程序，固定值将会很好地工作。 我们离得到最新的估计值只有一步之遥： RxEst(n) = (RxAcc + RxGyro * wGyro ) / (1 + wGyro) RyEst(n) = (RyAcc + RyGyro * wGyro ) / (1 + wGyro) RzEst(n) = (RzAcc + RzGyro * wGyro ) / (1 + wGyro) 现在让我们再次归一化这个向量： R = SQRT(RxEst(n) ^2 + RyEst(n)^2 + RzEst(n)^2 ) RxEst(n) = RxEst(n)/R RyEst(n) = RyEst(n)/R RzEst(n) = RzEst(n)/R 我们准备好再次重复我们的循环。 注意：要实际执行和测试这个算法，请阅读本文： Arduino code for IMU Guide algorithm. Using a 5DOF IMU (accelerometer and gyroscope combo) ====================================================== 说明： 这里一大堆的公式，看的我头大。不过加速度计和陀螺仪为什么结合这个问题倒是搞清楚了。 但是还是有问题啊，这个卡尔曼滤波程序怎么写？？ ====================================================== |
|
|
|
|
|
四、姿态融合
参看：MPU6050姿态融合 （下面内容是以STM32开发板来讲的，而我现在使用的C52单片机，有不符合我开发的地方，可做了解） 首先要明确，MPU6050 是一款姿态传感器，使用它就是为了得到待测物体（如四轴、平衡小车） x、y、z 轴的倾角（俯仰角 Pitch、滚转角 Roll、偏航角 Yaw） 。我们通过 I2C 读取到 MPU6050 的六个数据（三轴加速度 AD 值、三轴角速度 AD 值）经过姿态融合后就可以得到 Pitch、Roll、Yaw 角。 本帖主要介绍三种姿态融合算法：四元数法 、一阶互补算法和卡尔曼滤波算法。 （1）四元数法 可通过下面的算法，可以把六个数据转化成四元数（q0、q1、q2、q3），然后四元数转化成欧拉角（P、R、Y 角）。 #include #include "stm32f10x.h" //--------------------------------------------------------------------------------------------------- // 变量定义 #define Kp 100.0f // 比例增益支配率收敛到加速度计/磁强计 #define Ki 0.002f // 积分增益支配率的陀螺仪偏见的衔接 #define halfT 0.001f // 采样周期的一半 float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素，代表估计方向 float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差 float Yaw,Pitch,Roll; //偏航角，俯仰角，翻滚角 void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) { float norm; float vx, vy, vz; float ex, ey, ez; // 测量正常化 norm = sqrt(ax*ax + ay*ay + az*az); ax = ax / norm; //单位化 ay = ay / norm; az = az / norm; // 估计方向的重力 vx = 2*(q1*q3 - q0*q2); vy = 2*(q0*q1 + q2*q3); vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; // 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和 ex = (ay*vz - az*vy); ey = (az*vx - ax*vz); ez = (ax*vy - ay*vx); // 积分误差比例积分增益 exInt = exInt + ex*Ki; eyInt = eyInt + ey*Ki; ezInt = ezInt + ez*Ki; // 调整后的陀螺仪测量 gx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt; gz = gz + Kp*ez + ezInt; // 整合四元数率和正常化 q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT; q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT; q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT; q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT; // 正常化四元 norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); q0 = q0 / norm; q1 = q1 / norm; q2 = q2 / norm; q3 = q3 / norm; Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch ,转换为度数 Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // rollv //Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //此处没有价值，注掉 } 说明： 这里 IMUupdate 函数的形参就是我们获取的加速度计和陀螺仪的原始数据，将其带入不就行了吗？说明这个四元数法在C52单片机上也能写一写的。 不过要注意的是，四元数算法输出的是三个量 Pitch、Roll 和 Yaw，运算量很大。而像平衡小车这样的例子只需要一个角（Pitch 或 Roll ）就可以满足工作要求，个人觉得做平衡小车最好不用四元数法。 （2）一阶互补算法 MPU6050 可以输出三轴的加速度和角速度。通过加速度和角速度都可以得到 Pitch 和 Roll 角（加速度不能得到 Yaw 角），就是说有两组 Pitch、Roll 角，到底应该选哪组呢？别急，先分析一下。MPU6050 的加速度计和陀螺仪各有优缺点，三轴的加速度值没有累积误差，且通过算 tan() 可以得到倾角，但是它包含的噪声太多（因为待测物运动时会产生加速度，电机运行时振动会产生加速度等），不能直接使用；陀螺仪对外界振动影响小，精度高，通过对角速度积分可以得到倾角，但是会产生累积误差。所以，不能单独使用 MPU6050 的加速度计或陀螺仪来得到倾角，需要互补。一阶互补算法的思想就是给加速度和陀螺仪不同的权值，把它们结合到一起，进行修正。得到 Pitch 角的程序如下： //一阶互补滤波 float K1 =0.1; // 对加速度计取值的权重 float dt=0.001;//注意：dt的取值为滤波器采样时间 float angle; angle_ax=atan(ax/az)*57.3; //加速度得到的角度 gy=(float)gyo[1]/7510.0; //陀螺仪得到的角速度 Pitch = yijiehubu(angle_ax,gy); float yijiehubu(float angle_m, float gyro_m)//采集后计算的角度和角加速度 { angle = K1 * angle_m + (1-K1) * (angle + gyro_m * dt); return angle; } 互补算法只能得到一个倾角，这在平衡车项目中够用了，而在四轴飞行器设计中还需要 Roll 和 Yaw，就需要两个 互补算法，我是这样写的，注意变量不要搞混： //一阶互补滤波 float K1 =0.1; // 对加速度计取值的权重 float dt=0.001;//注意：dt的取值为滤波器采样时间 float angle_P,angle_R; float yijiehubu_P(float angle_m, float gyro_m)//采集后计算的角度和角加速度 { angle_P = K1 * angle_m + (1-K1) * (angle_P + gyro_m * dt); return angle_P; } float yijiehubu_R(float angle_m, float gyro_m)//采集后计算的角度和角加速度 { angle_R = K1 * angle_m + (1-K1) * (angle_R + gyro_m * dt); return angle_R; } 单靠 MPU6050 无法准确得到 Yaw 角，需要和地磁传感器结合使用。 （3）卡尔曼滤波 其实卡尔曼滤波和一阶互补有些相似，输入也是一样的。在此给出具体程序，和一阶互补算法一样，每次卡尔曼滤波只能得到一个方向的角度。 #include #include "stm32f10x.h" #include "Kalman_Filter.h" //卡尔曼滤波参数与函数 float dt=0.001;//注意：dt的取值为kalman滤波器采样时间 float angle, angle_dot;//角度和角速度 float P[2][2] = {{ 1, 0 }, { 0, 1 }}; float Pdot[4] ={ 0,0,0,0}; float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度 float R_angle=0.5 ,C_0 = 1; float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1; //卡尔曼滤波 float Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy { 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;//最优角速度 return angle； } 总结： 三种融合算法都能够输出姿态角（Pitch 和 Roll ），一次四元数法可以输出 P、R、Y 三个倾角，计算量比较大。一阶互补和卡尔曼滤波每次只能输出一个轴的姿态角。 再有这个一阶互补和卡尔曼滤波里的采集后计算的角度和角加速度，上面我们已经讲了，然后再带入到 Kalman_Filter 函数里，那么在C52单片机上实现也不成问题的。 （4）三种滤波比较源代码 参看：MPU6050数据采集及其意义和滤波（一阶互补滤波、二阶互补滤波、卡尔曼滤波） #include “Wire.h” #include “I2Cdev.h” #include “MPU6050.h” #include “Timer.h”//时间操作系统头文件 本程序用作timeChange时间采集并处理一次数据Timer t;//时间类 float timeChange=20;//滤波法采样时间间隔毫秒 float dt=timeChange*0.001;//注意：dt的取值为滤波器采样时间 // 陀螺仪 float angleAx,gyroGy;//计算后的角度（与x轴夹角）和角速度 MPU6050 accelgyro;//陀螺仪类 int16_t ax, ay, az, gx, gy, gz;//陀螺仪原始数据 3个加速度+3个角速度 //一阶滤波 float K1 =0.05; // 对加速度计取值的权重 //float dt=200.001;//注意：dt的取值为滤波器采样时间 float angle1;//一阶滤波角度输出 //二阶滤波 float K2 =0.2; // 对加速度计取值的权重 float x1,x2,y1;//运算中间变量 //float dt=200.001;//注意：dt的取值为滤波器采样时间 float angle2;//er阶滤波角度输出 //卡尔曼滤波参数与函数 float angle, angle_dot;//角度和角速度 float angle_0, angle_dot_0;//采集来的角度和角速度 //float dt=20*0.001;//注意：dt的取值为kalman滤波器采样时间 //一下为运算中间变量 float P[2][2] = {{ 1, 0 }, { 0, 1 }}; float Pdot[4] ={ 0,0,0,0}; float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度 float R_angle=0.5 ,C_0 = 1; float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1; void setup() { Wire.begin();//初始化 Serial.begin(9600);//初始化 accelgyro.initialize();//初始化 int tickEvent1=t.every(timeChange, getangle);//本语句执行以后timeChange毫秒执行回调函数getangleint tickEvent2=t.every(50, printout) ;//本语句执行以后50毫秒执行回调函数printout，串口输出 } void loop() { t.update();//时间操作系统运行 } void printout() { Serial.print(angleAx);Serial.print(’,’); Serial.print(angle1);Serial.print(’,’); Serial.print(angle2);Serial.print(’,’); // Serial.print(gx/131.00);Serial.print(’,’); Serial.println(angle);//Serial.print(’,’); // Serial.println(Output); } void getangle() { accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取原始6个数据 angleAx=atan2(ax,az)*180/PI;//计算与x轴夹角 gyroGy=-gy/131.00;//计算角速度 Yijielvbo(angleAx,gyroGy);//一阶滤波 Erjielvbo(angleAx,gyroGy);//二阶滤波 Kalman_Filter(angleAx,gyroGy); //卡尔曼滤波 } void Yijielvbo(float angle_m, float gyro_m) { angle1 = K1 * angle_m+ (1-K1) * (angle1 + gyro_m * dt); } void Erjielvbo(float angle_m,float gyro_m) { x1=(angle_m-angle2)(1-K2)(1-K2); y1=y1+x1dt; x2=y1+2(1-K2)(angle_m-angle2)+gyro_m; angle2=angle2+ x2dt; } void Kalman_Filter(double angle_m,double gyro_m) { 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;//最优角速度 } 五、欧拉角 参看：姿态角 – 维基百科 参看：姿态角（Euler角）：yaw pitch roll 上面提到的俯仰角 Pitch、滚转角 Roll、偏航角 Yaw、欧拉角等等都是什么呢？ 欧拉角又称姿态角（Euler角）并不是指哪个角度，而是是俯仰角、滚转角、偏航角这三个角度的统称。 你可以想象是飞机围绕XYZ三个轴分别转动形成的夹角。 （1）俯仰角θ（pitch） 机体坐标系X轴与水平面的夹角。当X轴的正半轴位于过坐标原点的水平面之上（抬头）时，俯仰角为正，否则为负。 （2）偏航角ψ（yaw）： 机体坐标系xb轴在水平面上投影与地面坐标系xg轴（在水平面上，指向目标为正）之间的夹角，由xg轴逆时针转至机体xb的投影线时，偏航角为正，即机头右偏航为正，反之为负。 （3）滚转角Φ（roll） 机体坐标系zb轴与通过机体xb轴的铅垂面间的夹角，机体向右滚为正，反之为负。 |
|
|
|
|
只有小组成员才能发言，加入小组>>
3309 浏览 9 评论
2988 浏览 16 评论
3490 浏览 1 评论
9050 浏览 16 评论
4084 浏览 18 评论
1168浏览 3评论
601浏览 2评论
const uint16_t Tab[10]={0}; const uint16_t *p; p = Tab;//报错是怎么回事？
592浏览 2评论
用NUC131单片机UART3作为打印口，但printf没有输出东西是什么原因？
2329浏览 2评论
NUC980DK61YC启动随机性出现Err-DDR是为什么？
1892浏览 2评论
/6 
关注我们的微信
下载发烧友APP
电子发烧友观察
版权所有 © 湖南华秋数字科技有限公司
电子发烧友 （电路图） 湘公网安备 43011202000918 号 电信与信息服务业务经营许可证：合字B2-20210191
工商网监
湘ICP备2023018690号
1930
评论
淘帖
1814
