第十七届智能车竞赛 - 磁力计角度数据处理
磁力计角度数据处理
一、磁力计校正任务
二、float类型数据的串口收发方式
2.1 float（double）类型数据存储方式复习
2.2 下位机处理
2.3 Python接收串口数据、转换为float并绘制曲线
三、 校正处理
3.1 下位机读取和校正程序
3.2 上位机图像绘制
四、滤波处理
4.1 原始数据噪声含量观察和分析
4.2 消抖滤波、滑窗滤波、低通滤波算法实现
4.3 初始角度和实时角度都加【消抖+滑窗+低通】滤波计算后结果
磁力计角度数据处理
一、磁力计校正任务
- 理论上，磁力计东向传感器和北向传感器读取地磁场东向分量得到的数据应该是一样的
- 表现的状态为车体旋转一周、动向传感器数据（作为X轴）和北向传感器数据（作为Y轴）作出的图像应该是一个圆
-
MATLAB 接收串口数据处理速度太慢，因此采用Python实时接收串口数据并画图
二、float类型数据的串口收发方式
2.1 float（double）类型数据存储方式复习
微机原理学过
-
存储长度为32位（double 64位）
-
符号位
- 长度为1
-
指数位
- 长度为8（double 11位）
- 故float范围为 − 2^128 ∼ + 2^128 ,即-3.40×10^{+38} ~ +3.40× 10^{+38}
- double范围为 − 2^1024 ∼ + 2^1024,即-1.79×10^{+308} ~ +1.79× 10^{+308}
-
尾数位
- 长度为23（double 52位）
- 决定精度
- float：2 23 = 8388608 2^{23} = 8388608223精度为6~7位有效数字
- double：2 52 = 4503599627370496 2^{52} = 4503599627370496252共16位，精度为15~16位有效数字
-
例：将17.625换算成float型
void WireLess_Send_Mag(void){
// 先发X方向数据
hardware.wireless.Ptr_To_Tx = (uint8 *)&hardware.imu.m.x;
WireLess_Send(hardware.wireless.Ptr_To_Tx+3, 1);
WireLess_Send(hardware.wireless.Ptr_To_Tx+2, 1);
WireLess_Send(hardware.wireless.Ptr_To_Tx+1, 1);
WireLess_Send(hardware.wireless.Ptr_To_Tx+0, 1);
// 再发Y方向数据
hardware.wireless.Ptr_To_Tx = (uint8 *)&hardware.imu.m.y;
WireLess_Send(hardware.wireless.Ptr_To_Tx+3, 1);
WireLess_Send(hardware.wireless.Ptr_To_Tx+2, 1);
WireLess_Send(hardware.wireless.Ptr_To_Tx+1, 1);
WireLess_Send(hardware.wireless.Ptr_To_Tx+0, 1);
}
其中 WireLess_Send函数每次发送完后会清空TX_Buff
void WireLess_Send(uint8 *ptr_to_tx, uint8 bytes){
// 数据帧
if(bytes > BUFF_LENGTH){
bytes = BUFF_LENGTH; // 超出长度处理
}
// 发送
seekfree_wireless_send_buff(ptr_to_tx, bytes);
// clear txbuff
for(int i = 0; i < bytes; i++){
*(ptr_to_tx + i) = 0;
}
}
2.3 Python接收串口数据、转换为float并绘制曲线
STEP 1 串口初始化
# 串口测试程序
import serial
import matplotlib.pyplot as plt
import numpy as np
import struct
# 串口信息
comport = 'COM12'
baudrate = 115200
bytes = 1
print('You selected %s, baudrate %d, %d byte.' % (comport, int(baudrate), bytes))
# 初始化
serialport = serial.Serial(comport, int(baudrate), timeout=1, parity=serial.PARITY_NONE, rtscts=1)
if serialport.isOpen():
print("open success")
else:
print("open failed")
STEP 2 其他初始化
```python
# 图表初始化
plt.grid(True) # 添加网格
plt.ion() # interactive mode
plt.figure(1)
plt.xlabel('mx')
plt.ylabel('mz')
plt.title('Diagram of UART data by Python')
# 一些变量初始化
X = []
Y = []
i = 0
intdata = 0
data = ''
count = 0
x_max = 0
x_min = 0
y_max = 0
y_min = 0
STEP 3 串口只能发送8位 因此下位机需要将float拆分成4个字节依次发送 相应的python上位机需要合并4个字节
# 将接收到的4字节拼接为float类型
# 要注意是 高位在前 还是 低位在前
# 最好的办法就是试错 只有两种情况 看看转义出来的数据是否正常
def bytesToFloat(h1, h2, h3, h4):
ba = bytearray()
ba.append(h1)
ba.append(h2)
ba.append(h3)
ba.append(h4)
return struct.unpack("!f", ba)[0]
STEP 4 接收数据 拼接为float 画图显示 数据保存
while i < 1200: # 最多接收1200个数据
# 30000次数据后清除画布 重新绘制 避免数据量过大导致卡顿
# if i > 30000:
# X = []
# Y = []
# i = 0
# plt.cla()
count = serialport.inWaiting()
if count > 0: # 接收缓存区有数据
# Y方向数据
hex_4_1 = []
# 4个字节一组
for j in range(4):
# 接收到字节类型数据
data = serialport.read(1)
# 字节转int 否则字节型数据不能append
intdata = int.from_bytes(data, byteorder='big', signed=False)
# 将4字节拼接在一起
hex_4_1.append(intdata)
# 接收字节数+1
i = i + 1
# int转float
float_data_1 = bytesToFloat(hex_4_1[0], hex_4_1[1], hex_4_1[2], hex_4_1[3])
# 找 最大值 和 最小值
# 便于估算短轴长度
if float_data_1 > 0:
if float_data_1 > y_max:
y_max = float_data_1
if float_data_1 < 0:
if float_data_1 < y_min:
y_min = float_data_1
# X方向数据
hex_4_2 = []
# 4个字节一组
for h in range(4):
# 接收到字节类型数据
data = serialport.read(1)
# 字节转int 否则字节型数据不能append
intdata = int.from_bytes(data, byteorder='big', signed=False)
# 将4字节拼接在一起
hex_4_2.append(intdata)
# 接收字节数+1
i = i + 1
# int转float
float_data_2 = bytesToFloat(hex_4_2[0], hex_4_2[1], hex_4_2[2], hex_4_2[3])
# 找 最大值 和 最小值
# 便于估算长轴长度
if float_data_2 > 0:
if float_data_2 > x_max:
x_max = float_data_2
if float_data_2 < 0:
if float_data_2 < x_min:
x_min = float_data_2
# 显示转换后的浮点数据
print(i, float_data_1, float_data_2)
# 显示最大值最小值
print("(%.2f, %.2f), (%.2f, %.2f)" % (x_max, y_max, x_min, y_min))
# 将数据拼接成数组 便于画图
Y.append(float_data_1)
X.append(float_data_2)
# 画图
plt.plot(X, Y, '-r')
plt.scatter(float_data_2, float_data_1)
plt.draw()
plt.pause(0.002)
# 将数据保存为txt文件
# 留给后期进一步拟合椭圆方程
# 精确查看校正精确度
np.savetxt("data_1.txt", X)
np.savetxt("data_2.txt", Y)
三、 校正处理
每次直接在单片机上修改校正方式
然后将校正后数据发回上位机画图
查看绘制出来的曲线中心点是否接近原点、长短轴是否相近
3.1 下位机读取和校正程序
void IMU_Read_Mag(void){
// 读取数据保存到 imu963ra_mag_x imu963ra_mag_y imu963ra_mag_z
imu963ra_get_mag();
static int16 raw_mx = 0;
static int16 raw_my = 0;
static int16 raw_mz = 0;
// 原数据
raw_mx = imu963ra_mag_x;
raw_my = imu963ra_mag_y;
raw_mz = imu963ra_mag_z;
// 偏移
raw_mx = raw_mx + 312;
raw_my = raw_my + 160;
raw_mz = raw_mz;
// 旋转
// correct_x = correct_x * cos - correct_y * sin;
// correct_y = correct_x * sin + correct_y * cos;
// 长轴拉伸
raw_mx = raw_mx * 1.031;
// 最终数据
hardware.imu.m.x = raw_mx;
hardware.imu.m.y = raw_my;
hardware.imu.m.z = raw_mz;
}3.2 上位机图像绘制
最终目标是实现 1. 中心在原点 2. 长短轴相近
这样东向传感器和北向传感器读到的磁场数据才能相近，解算角度数据才可靠
四、滤波处理
矫正结束后，开始进行空间角度计算
由于噪声原因，测量的数据存在动态误差
因此需要观测数据分布情况，并进行滤波处理
4.1 原始数据噪声含量观察和分析
直接读取磁力计数据，首先读取刚上电时初始角度作为基准值，将此后每个时刻读到的值与基准值做差，得到相对角度，便于观察
时域图像
频率域图像
根据频率域曲线发现，磁力计数据更偏向于【0，0.5】
这也可能是由于初始值计算不准确造成的，因此需要在计算初始角度时，进行一些滤波处理
4.2 消抖滤波、滑窗滤波、低通滤波算法实现
消抖滤波，详见【消抖滤波】
// **************************************
// 消抖滤波
// **************************************
#define N 12
float Dithering_Filter(int data_channel, float data_stream){
static float effective_value[Channel_Number] = {0};
static int count[Channel_Number] = {0};
data_channel = data_channel -1;
while(effective_value[data_channel] != data_stream){
count[data_channel]++;
if(count[data_channel] >= N){
effective_value[data_channel] = data_stream;
return data_stream;
}
hardware.imu.Read_Mag();
data_stream = hardware.imu.m.x;
}
return effective_value[data_channel];
}
滑窗滤波，详见【滑窗滤波】
// **************************************
// 滑窗滤波
// **************************************
#define Window_Size 20
float Sliding_Window_Filtering(int data_channel, float data_stream){
data_channel = data_channel -1;
/* 滤波滑动窗口数组 */
static float Filter_Window[Channel_Number][Window_Size] = {0};
/* 更新滤波滑动窗口数组 */
for(int i = 0; i < Window_Size-1; i++){
Filter_Window[data_channel][i] = Filter_Window[data_channel][i+1]; // 旧数据前移
}
Filter_Window[data_channel][Window_Size-1] = data_stream; // 新数据填补
/* 计算窗口总值 */
float Sum = 0;
for(int i = 0; i < Window_Size; i++){
Sum += Filter_Window[data_channel][i];
}
/* 返回滑动平均值 */
return Sum / Window_Size;
}
低通滤波，详见【低通滤波】
// **************************************
// 低通滤波
// **************************************
const float fc = 1.0f; //截止频率
const float Ts = 0.001f; //采样周期
const float pi = 3.14159f; //π
float alpha =(2.0 * pi * fc * Ts) / (2.0 * pi * fc * Ts + 1); //滤波系数
float Low_Pass_Filter(int data_channel, float data_stream)
{
static float out_last[Channel_Number] = {0}; //上一次滤波值
float out;
data_channel = data_channel -1;
/***************** 如果第一次进入，则给 out_last 赋值 ******************/
static char fisrt_flag = 1;
if (fisrt_flag == 1)
{
fisrt_flag = 0;
out_last[data_channel] = data_stream;
}
/*************************** 一阶滤波 *********************************/
out = out_last[data_channel] + alpha * (data_stream - out_last[data_channel]);
out_last[data_channel] = out;
return out;
}
4.3 初始角度和实时角度都加【消抖+滑窗+低通】滤波计算后结果
时域信号
噪声限制在±0.2度以内
噪声频率呈现期望为0的高斯分布，符合预期效果
评论（0）
您还未登录，请登录后发表或查看评论