小车姿态获取

小车姿态获取

1. 陀螺仪（Gyroscope）&加速计（Accelerometer）

三轴陀螺仪：也叫地感器，传统结构是内部有个陀螺，如下图所示（三轴陀螺），三轴陀螺仪的工作原理是通过测量三维坐标系内陀螺转子的垂直轴与设备之间的夹角，并计算角速度，通过夹角和角速度来判别物体在三维空间的运动状态。三轴陀螺仪可以同时测定上、下、左、右、前、后等6个方向（合成方向同样可分解为三轴坐标），最终可判断出设备的移动轨迹和加速度。

加速计：也叫重力感应器，实际上是可以感知任意方向上的加速度（重力加速度则只是地表垂直方向加速度），加速计通过测量组件在某个轴向的受力情况来得到结果，表现形式为轴向的加速度大小和方向（XYZ），这一点又有点类似于陀螺仪，但陀螺仪的更多关注自身旋转情况（原位运动），加速计则主要是测量设备的受力情况，也就是三轴运动情况，尽管加速计也可能在某个小范围换算出角速度的可能，但设计原理决定似乎更适合于空间运动判断。

工作原理：

Accelerometer & Gyro Tutorial

2. 加速度计和陀螺仪互补滤波

从mpu6050中通过ADC（analog-to-digital）采样读出三轴陀螺仪和三轴加速度计的初始值，需将其通过以下步骤转化为可用单位：

1. 校正偏移（offset）：让mpu6050模块静止，陀螺仪和加速度计保持水平且静止，此时读到的电压值就是便宜量，如果读数不稳定取平均值作为偏移量即可。
2. 转化系数（scale）：传感器输出电压值乘以系数，得到我们想要的单位，这个系数可以在传感器的数据手册（datasheet）中找到

由于加速度计长期内稳定，短期内振荡剧烈（而且在小车运动时，加速度计除了有重力加速度，还有小车运动加速度），而陀螺仪短期内可靠，长时间振荡剧烈，因此在实际应用中，我们采取融合它们各自的优势，利用加速度计和陀螺仪一起计算小车的转角。

互补滤波：

陀螺仪与加速度计的互补：

angle = (0.98) * (angle + gyro*dt) + (0.02) * (x_acc);

如果这里滤波器的循环频率时每秒100次(亦即循环周期 = 0.01s)，那么上述公式的时间常数就是：

$tau = (a * dt)/(1-a) = ( 0.98*0.01s )/0.02s = 0.49s$

这个时间常数确定了陀螺仪和加速度计的信任边界。信号时间周期低于半秒时，陀螺仪占主导地位，加速度计的噪声除掉；信号时间周期大于半秒时，加速度计的角度平均值就占据主导地位，有温漂的陀螺仪可以站一边去。

实验，是检验时间常数的标准之一。首先你要挑一个时间常数，然后算出滤波器的系数a，弄到代码中去。检查角度拟合曲线，如果拟合角度与实际角度总相差那么2°，就说明陀螺仪的温漂影响较大，你可能需要把时间常数设置到1s以下，来保证加速度计能尽快校正陀螺仪的温漂值。不过，时间常数越短，就意味着加速度计的噪声更容易通过。

Gyroscopes and Accelerometers on a Chip

加速度计和陀螺仪融合得到精确角度

3. 三轴磁力计

磁力计（Magnetic、M-Sensor）也叫地磁、磁感器，可用于测试磁场强度和方向，定位设备的方位，磁力计的原理跟指南针原理类似，可以测量出当前设备与东南西北四个方向上的夹角。所以，陀螺仪知道“我们转了个身”，加速计知道“我们又向前走了几米”，而磁力计则知道“我们是向西方向”的。

hmc5883l罗盘校正步骤

同时使用磁力计和加速计来运算出Orientation（方位计），运算出的方位信息需要同时结合磁场方向和方向运动情况才能得到，orientation涉及到了三个概念：

• pitch()：俯仰，将物体绕Y轴旋转
• yaw()：航向，将物体绕Z轴旋转
• roll()：横滚，将物体绕X轴旋转

地球磁场的水平分量用于计算磁航向，而俯仰角和滚转角分别是沿着水平X轴和垂直Y轴的倾斜角。这些倾斜角影响XY轴方向的磁场。当设备不处于水平位置时，即倾斜角不是零时，航向计算将不正确。因此可以通过利用加速计，通过旋转XY平面对方位进行计算之前，确保这些倾斜角得到补偿很重要。

def read_compensated_bearing(self, pitch, roll):
        '''
        Calculate a bearing taking in to account the current pitch and roll of the device as supplied as parameters
        '''
        self.read_raw_data()
        cos_pitch = (math.cos(pitch))
        sin_pitch = (math.sin(pitch))

        cos_roll = (math.cos(roll))
        sin_roll = (math.sin(roll))

        Xh = (self.scaled_x * cos_roll) + (self.scaled_z * sin_roll)
        Yh = (self.scaled_x * sin_pitch * sin_roll) + (self.scaled_y * cos_pitch) - (self.scaled_z * sin_pitch * cos_roll)

        bearing = math.atan2(Yh, Xh)
        if bearing < 0:
            return bearing + (HMC5883L.TWO_PI)
        else:
return bearing

scaled_x, scaled_y, scaled_z分别为三轴磁力计分量, bearing是通过磁力计算通过加速计和陀螺仪补偿计算所得的方位角

4. 参考

• 筆記, MPU-6050, google 搜尋與實驗心得 (MPU-6050, Arduino)
• imu demo