姿态解算学习01_关于维特JY61P学习使用

---

前言

提示：记录自己学习使用维特JY61P姿态传感器：

---

一、姿态传感器的特点

陀螺仪存在零漂现象，误差随着时间增长越来越大

加速度计存在零偏现象，短时间内数据误差较大

二、数据读取

使用串口通信协议接受数据

根据JY61P说明书中数据包格式描述，数据是按照十六进制进行发送，以0x55为开头。一旦读取到0x55，表明能接收到JY61P发送的有效信息，接着根据第二位判断这串数据包含的信息，0x51表示这串数据记录三轴加速度，0x52记录的是三轴角速度，0x53记录的是欧拉角，因此编写数据转换函数，可以获得获取三轴加速度、三轴角速度、欧拉角信息。

以加速度为例：

每个数据分低字节和高字节依次传送，二者组合成一个有符号的short类型的数据。例如 X 轴加速度数据 Ax，其中 AxL 为低字节，AxH 为高字节。

2.数据处理

以x轴方向为物体前进方向。因此绕x轴旋转的角度为横滚角（RoLL），绕y轴旋转的角度为俯仰角（Pitch），绕z轴旋转的角度为偏航角（Yaw）。

原始数据存在较多的噪声，出现较多的尖峰，因此通过滑动窗口滤波的方式，对原始的加速度计数据、陀螺仪数据进行均值处理

通过加速度计计算横滚角和俯仰角，对陀螺仪角度进行积分计算偏航角

代码如下（示例）：

for (int j = 1; j < filter_num; ++j) {
                    //加速度滑动滤波,去掉低位，空出最后一位
                    acc_aver_filter[j - 1].a[0] = acc_aver_filter[j].a[0];
                    accx += (float)acc_aver_filter[j].a[0] / 32768 * 16;
                    acc_aver_filter[j - 1].a[1] = acc_aver_filter[j].a[1];
                    accy += (float)acc_aver_filter[j].a[1] / 32768 * 16;
                    acc_aver_filter[j - 1].a[2] = acc_aver_filter[j].a[2];
                    accz += (float)acc_aver_filter[j].a[2] / 32768 * 16;

                    //角速度滑动滤波，去掉低位，空出最后一位
                    gyro_aver_filter[j - 1].w[0] = gyro_aver_filter[j].w[0];
                    gyrox += (float)gyro_aver_filter[j].w[0] / 32768 * 2000;
                    gyro_aver_filter[j - 1].w[1] = gyro_aver_filter[j].w[1];
                    gyroy += (float)gyro_aver_filter[j].w[1] / 32768 * 2000;
                    gyro_aver_filter[j - 1].w[2] = gyro_aver_filter[j].w[2];
                    gyroz += (float)gyro_aver_filter[j].w[2] / 32768 * 2000;

                }
                acc_aver_filter[filter_num-1] = JY61.stcAcc;
                accx += (float)acc_aver_filter[filter_num - 1].a[0] / 32768 * 16;
                accy += (float)acc_aver_filter[filter_num - 1].a[1] / 32768 * 16;
                accz += (float)acc_aver_filter[filter_num - 1].a[2] / 32768 * 16;

                gyro_aver_filter[filter_num - 1] = JY61.stcGyro;
                gyrox += (float)gyro_aver_filter[filter_num - 1].w[0] / 32768 * 2000;
                gyroy += (float)gyro_aver_filter[filter_num - 1].w[1] / 32768 * 2000;
                gyroz += (float)gyro_aver_filter[filter_num - 1].w[2] / 32768 * 2000;

                Ax = accx / filter_num;
                Ay = accy / filter_num;
                Az = accz / filter_num;

                Wx = gyrox / filter_num;
                Wy = gyroy / filter_num;
                Wz = gyroz / filter_num;
				a_roll = atan(Ay / sqrt(Ax*Ax + Az * Az))*57.2974;
				a_pitch = atan(-Ax / sqrt(Az*Az + Ay * Ay))*57.2974;
				a_yaw += gyroz * 0.02;

				//a_roll = kalman_filter(a_roll, Wx);
				//a_pitch = kalman_filter(a_pitch, Wy);
				//a_yaw = kalman_filter(a_yaw, Wz);

---

总结

通过加速度计求解俯仰角、横滚角在静态时，精度能够满足使用要求
由于JY61P中没有磁力计，只用加速度计求解航向角误差很大，并且没有什么意义，因此计算航向角是通过陀螺仪的角速度进行积分计算