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

这篇具有很好参考价值的文章主要介绍了第十七届智能车竞赛 - 磁力计角度数据处理。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

磁力计角度数据处理


一、磁力计校正任务

  • 理论上,磁力计东向传感器北向传感器读取地磁场东向分量得到的数据应该是一样的
  • 表现的状态为车体旋转一周、动向传感器数据(作为X轴)和北向传感器数据(作为Y轴)作出的图像应该是一个圆
  • MATLAB 接收串口数据处理速度太慢,因此采用Python实时接收串口数据并画图

二、float类型数据的串口收发方式

2.1 float(double)类型数据存储方式复习

微机原理学过

  • 存储长度为32位(double 64位)

  • 符号位

    • 长度为1

  • 指数位

    • 长度为8(double 11位)
    • 故float范围为 − 2 128 ∼ + 2 128 -2^{128} \sim +2^{128} 2128+2128,即 − 3.40 × 1 0 + 38 ∼ + 3.40 × 1 0 + 38 -3.40 \times 10^{+38} \sim +3.40\times 10^{+38} 3.40×10+38+3.40×10+38
    • double范围为 − 2 1024 ∼ + 2 1024 -2^{1024} \sim +2^{1024} 21024+21024,即 − 1.79 × 1 0 + 308 ∼ + 1.79 × 1 0 + 308 -1.79\times 10^{+308} \sim +1.79\times 10^{+308} 1.79×10+308+1.79×10+308

  • 尾数位

    • 长度为23(double 52位)
    • 决定精度
    • float: 2 23 = 8388608 2^{23} = 8388608 223=8388608共7位,精度为6~7位有效数字
    • double: 2 52 = 4503599627370496 2^{52} = 4503599627370496 252=4503599627370496共16位,精度为15~16位有效数字

  • 例:将17.625换算成float型

    • 将17.625换算成二进制位:10001.101
    • 将10001.101向右移,直到小数点前只剩一位: 1.0001101 × 2 4 1.0001101\times2 ^4 1.0001101×24(右移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的高斯分布,符合预期效果文章来源地址https://www.toymoban.com/news/detail-416809.html

到了这里,关于第十七届智能车竞赛 - 磁力计角度数据处理的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 从零开始的第十七届智能车主板电源电路解读/设计1(基础四轮组别)

            作为一个参加过一年智能汽车的选手(下图是我第一次比赛的母板),当我再次重画电路的时候,仍然是对电路中的元器件作用不是很了解,于是决定开始去了解自己设计的电路而不是只是套用别人现有的设计。         所以作为我学习的记录也抱着分享经验

    2023年04月09日
    浏览(88)
  • 第十七届恩智浦杯室外ROS无人车竞速赛仿真

     学长说让以考促学,用做过的比赛来检验ROS的学习效果,看我们能不能灵活运用。(′д`)ゞ 目录 1.vscode准备工作 2.首先需要搭建gazebo仿真环境 3.launch文件打开gazebo仿真环境代码集成主要包括两大部分         才做了一个开头车还不会动,恩智浦杯的实物车出了点问题,

    2024年02月09日
    浏览(37)
  • 十七届智能车智能视觉组

    总结一下比赛过程,省二菜鸟,欢迎大佬指教 详情见十七届比赛细则第十七届智能车竞赛智能视觉组比赛细则_卓晴的博客-CSDN博客_智能视觉组,我这里简单介绍一下。 任务流程可以概括为:小车在起点出扫描一张A4纸(A4纸上有坐标点进而获得各个目标的位置)-扫描完后出

    2023年04月16日
    浏览(34)
  • 第十八届全国大学生智能汽车竞赛——摄像头算法(附带个人经验)

    参加了第十六,十七和第十八届全国大学生智能车竞赛,对摄像头的学习有部分心得,分享给大家,三届车赛,车赛生涯也算是到了尽头。打算从基础的算法开始,给各位一些个人看法,也是对车赛的一次总结。 闲话 :其实摄像头的算法有很多种,弄了两年摄像头,也只是

    2024年02月07日
    浏览(42)
  • 第十八届全国大学生智能汽车竞赛室外赛5g室外专项赛(湘大队)开源

            感谢十八届全国大学生智能汽车竞赛-室外赛的主办方和组委会让我们有了参加这次比赛的机会!当我们在参加5g室外专项赛时,我们发现很多的组又很多的同学都是第一次参加智能车竞赛对于智能车的了解还不是很深,对于代码如何编写需要哪些模块也不是很熟悉,

    2024年02月20日
    浏览(37)
  • 第七届福州大学信息安全竞赛——shellcode1 绕过strlen检查,绕过沙箱检查,执行orw shellcode拿到flag

    链接:https://pan.baidu.com/s/1HrMqh-lX-mkfueVeLzoEJg  提取码:oyel 这是一道非常让人蛋疼的题目,之前我只听说过沙箱,但是并没有自己实际接触过沙箱这个保护机制,大概作用就是开了沙箱之后,会禁用掉某些函数,一旦我们使用了这个函数,比如我们在栈溢出构造ROP,或者写入

    2024年02月04日
    浏览(45)
  • 第十七节——指令

    在Vue.js中,指令(Directives)是一种特殊的语法,用于为HTML元素添加特定的行为和功能。指令以v-作为前缀,通过在HTML标签中使用这些指令来操作DOM,修改元素的属性、样式或行为。 Vue.js提供了一组内置的指令,如v-model、v-bind、v-if、v-for等。此外,你也可以自定义指令来满足

    2024年02月06日
    浏览(34)
  • 【力扣刷题 | 第十七天】

    目录 前言: 55. 跳跃游戏 - 力扣(LeetCode) 45. 跳跃游戏 II - 力扣(LeetCode) 总结:         今天两道类型都是贪心算法,希望可以有所收获 给定一个非负整数数组  nums  ,你最初位于数组的  第一个下标  。 数组中的每个元素代表你在该位置可以跳跃的最大长度。 判断

    2024年02月15日
    浏览(42)
  • Scala第十七章节

    scala总目录 文档资料下载 章节目标 了解集合的相关概念 掌握Traversable集合的用法 掌握随机学生序列案例 1. 集合 1.1 概述 但凡了解过编程的人都知道 程序 = 算法 + 数据结构 这句话, 它是由著名的瑞士计算机科学家 尼古拉斯·沃斯 提出来的, 而他也是1984年图灵奖的获得者. 算

    2024年02月07日
    浏览(38)
  • NodeJs 第十七章 文件上传

    前端上传文件有两种方式 方式一:使用 FormData 发送 ajax 上传 方式二:使用 Form 表单上传(需要指定 enctype=\\\"multipart/form-data \\\") FormData 构造函数,用于创建 FormData 实例。 允许通过 append() 方法向 FormData 对象添加表单字段和文件数据。 自动将表单字段和文件数据封装成 multipart/f

    2024年01月19日
    浏览(43)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包