第十七届智能车竞赛 - 磁力计角度数据处理

时间：2022-09-30 03:00:00 传感器ad值成了1

磁计角度数据处理

磁计角度数据处理
- 一、磁力计校正任务
- 二、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} \sim 2^{128}$ ，即 $?3.40 \times 10^{ 38} \sim 3.40\times 10^{ 38}$
- double范围为 $-2^{1024} \sim +2^{1024}$ ，即 $-1.79\times 10^{+308} \sim +1.79\times 10^{+308}$
尾数位
- 长度为23（double 52位）
- 决定精度
- float： $2^{23} = 8388608$ 共7位，精度为6~7位有效数字
- double： $2^{52} = 4503599627370496$ 共16位，精度为15~16位有效数字
例：将17.625换算成float型
- 将17.625换算成二进制位：10001.101
- 将10001.101向右移，直到小数点前只剩一位： $1.0001101\times2 ^4$ （右移4位）
- 符号部分，正数为0
- 指数部分，实际为4，但须加上127，为131，即二进制数10000011
- 尾数部分，0001101，其余补0
- 综上所述，17.625 的float 存储格式为
- 0 10000011 00011010000000000000000

2.2 下位机处理

float类型包含4个字节
串口一次只能发送1个字节
因此需要拆分成4个字节按顺序发送
接收和发送，高位在前还是低位在前，要相互匹配

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 其他初始化

# 图表初始化
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的高斯分布，符合预期效果

锐单商城拥有海量元器件数据手册、IC替代型号，打造电子元器件IC百科大全！

第十七届智能车竞赛 - 磁力计角度数据处理

磁计角度数据处理

相关文章