立创梁山派开发板-21年电赛F题-送药小车-国产IMU移植及姿...

LCKFB · 发表于 2023-7-26 10:33:22

本帖最后由 LCKFB 于 2023-7-26 10:36 编辑

送药小车代码仓库：https://gitee.com/lcsc/medical_car更好的观看体验请去：https://dri8c0qdfb.feishu.cn/wiki/UjwwwO0KZii5bykPcE4cJZafnAg
送药小车立创开源平台资料：https://oshwhub.com/li-chuang-kai-fa-ban/21-dian-sai-f-ti-zhi-neng-song-yao-xiao-che

国产IMU移植及姿态解算
什么是IMU？
IMU（惯性测量单元，Inertial Measurement Unit） 是一种测量设备，用于测量物体的加速度、角速度和磁场等数据。IMU通常包括加速度计、陀螺仪和磁力计(九轴IMU才有)。

加速度计：用来测量物体在各个方向上的加速度。加速度计可以检测到重力加速度，因此也可以用来判断物体的姿态（比如我们手机通过知道它是竖直还是水平放置来切换屏幕的显示方向）。加速度计本质上来说测量的是力而不是加速度。只不过是因为加速度会恰好产生惯性力，这个惯性力被IMU里面的MEMS结构测量。
陀螺仪：用来测量物体围绕各个轴旋转的速度。陀螺仪能告诉我们物体的旋转情况，例如我们转动手机时，陀螺仪可以检测到这个动作。
磁力计（可选）：用来测量地球磁场的强度和方向。磁力计可以帮助我们判断物体相对于地球的方向，类似于指南针。当然，在地球南北极等地方，由于磁力线与地球重力方向相重合，他也就没用了。

推荐学习链接：imu_guide
IMU能做什么典型的应用场景有以下这些：

航空航天：在飞行器、卫星和火箭等领域，IMU用于测量和控制飞行器的姿态，以确保正确的导航和稳定性。
无人机：无人机依赖IMU来测量其姿态，以实现稳定的飞行控制。IMU还可以与GPS等其他传感器结合，提供更准确的导航和定位信息。
机器人：移动机器人、自动导航车辆和机械臂等机器人需要知道他自己在空间中的姿态和位置，IMU可以提供这些信息来实现精确的运动控制和导航。
智能手机和平板电脑：IMU用于检测设备的移动和旋转，从而实现屏幕旋转、游戏控制、运动识别等功能。
虚拟现实（VR）和增强现实（AR）：在VR和AR系统中，IMU用于追踪用户的头部和手部动作，实现身临其境的沉浸式体验，目前这种是低成本的解决方案，高大上一点的都用空间追踪或者视觉识别。
运动分析和健康监测：IMU可以安装在运动员身上，用于测量运动过程中的姿态、速度和加速度等参数，从而分析运动员的动作和技能。同时，IMU也可以用于健康监测，如步行计数、跑步速度等。最常见的的就是各种手环手表了，目前有一些运动鞋在鞋垫下面也安装了这种检测运动状态的部件，可以测量步频，步幅等运动状态信息。
导航系统：IMU与GPS等其他导航传感器结合，可以实现更高精度的定位和导航，这在室内导航、隧道和城市高楼间等GPS信号遮挡的环境中尤为重要。比如小车在过隧道时GPS信号就没有了，这时候通过IMU的计算来实现短时间内的位置更新。
汽车安全：IMU在汽车安全系统中的应用包括电子稳定控制系统（ESC）和防抱死刹车系统（ABS），通过监测车辆姿态的变化，可以实时调整刹车和动力，提高行车安全性。

IMU的性能和价格差距可以非常大，主要取决于应用场景的要求和预算。主要是下面这几个关键指标：

精度：不同应用场景对IMU的精度要求不同。例如，航空航天、高精度导航和高级机器人领域需要极高的精度，而智能手机和消费级VR/AR设备则可以接受较低的精度。高精度的IMU通常价格较高。
稳定性：稳定性指的是IMU在一段时间内，输出数据的准确性和一致性。在某些关键应用中，如航空航天和无人机，稳定性至关重要。高稳定性的IMU具有更高的价格。
更新率：不同应用场景对IMU数据更新率的要求不同。例如，高速运动控制和VR/AR应用可能需要更高的更新率，以实现更流畅的动作跟踪。高更新率的IMU价格更高。
功耗：对于便携式设备和电池供电系统，功耗是一个重要的考虑因素。低功耗的IMU可以延长设备的使用时间，但会牺牲一定的性能。低功耗且高性能的IMU通常价格较高。
尺寸：根据应用场景的空间限制，可能需要不同尺寸的IMU。在某些紧凑型设备中，需要更小的IMU，当然在其他应用中，尺寸可能不是关键因素。微型化的高性能IMU价格一般会更高。

根据上面这些因素，IMU的价格可以从几美元（用于消费级设备，如智能手机和游戏控制器）到数千甚至数万美元（用于航空航天和高精度导航等关键应用）不等。

类别	成本	稳定性	性能参数	应用领域
消费级	低（10美元以下）	较低	精度：较低更新率：较低至中等功耗：较低尺寸：较小	智能手机游戏控制器消费级VR/AR设备
工业级	中等（几十至数百美元）	中等	精度：中等更新率：中等至高功耗：中等尺寸：中等	机器人无人机汽车安全系统室内导航
航空级	高（数千至数万美元）	高	精度：高更新率：高功耗：中等至高尺寸：中等至大	航空航天高精度导航精密测量和监测系统

国产IMU介绍
为了配合梁山派的全国产化，这里打开立创商城进入姿态传感器/陀螺仪目录，第一栏的品牌/产地里面可以看到一个 QST（上海矽睿）这个是国产的，他家目前最新的IMU是QMI8658A，就选这个了。

QMI8658是一款低噪声、高带宽的六轴惯性测量单元(IMU），包含一个三轴陀螺仪和一个三轴加速计，采用2.5 x 3.0 x 0.86 mm 14-pin LGA 封装。支持多种通讯接口：I3C、I2C和SPI。可满足惯性导航高精度低功耗的要求，即使在低速率情况下的数据输出也可保持高精度。

打开他的数据手册，打开第二页，可以了解到他的关键参数有以下这些：
QMI8658A是一款6D MEMS惯性测量单元（IMU），具有低噪声、宽带宽和内置运动协处理器（没找到实际代码，不清楚是不是确实支持）。它适用于消费级和工业级应用。
关键参数：

陀螺仪噪声：低至13 mdps/√Hz（陀螺仪噪声是指测量角速度时产生的随机误差，这个数值表示在每平方根赫兹的频带宽度内，陀螺仪输出的角速度噪声的均方根（RMS）值是13 mdps。陀螺仪噪声越低，得到的角速度测量结果就越准确，从而提高姿态估计的准确性。）
加速度计噪声：低至150µg/√Hz（加速度计噪声是指测量线性加速度时产生的随机误差，这个数值表示在每平方根赫兹的频带宽度内，加速度计输出的线性加速度噪声的均方根（RMS）值是150µg。加速度计噪声越低，得到的线性加速度测量结果就越准确，从而提高运动跟踪的准确性。）
通信接口：支持MIPI™ I3C, I2C, 以及3线或4线SPI
陀螺仪和加速度计具有可编程数据率和滤波器的数字信号处理
尺寸：2.5 x 3.0 x 0.86 mm，14-pin LGA封装
1536字节的FIFO缓冲区，可降低系统功耗
集成计步器、敲击、任意运动、无运动、显著运动检测功能
陀螺仪测量范围：±16°/s至±2048°/s；加速度计测量范围：±2 g至±16 g
低功耗模式，用于有效的电源管理
可编程采样率和滤波器
集成温度传感器
广泛的工作温度范围：-40°C至85°C

应用领域：

智能手机
游戏控制器、遥控器和指向设备
机器人吸尘器
电动自行车和滑板车
蓝牙耳机
汽车安全系统
玩具

作为对比，世界上第一款集成了6轴IMU-MPU6050的陀螺仪噪声是5 mdps/√Hz，加速度计噪声是400μg/√Hz。在手册里，可以看到最初版本是2010年11月发布的。而国产IMU-QMI8658A是2022年1月发布的。
最出名的IMU就是mpu6050了，他是InvenSense 公司推出的全球首款整合 6 轴运动处理的组件，应美盛公司已经被日本的TDK公司买了，现在出的最新的IMU都是ICM开头的，比如ICM-42688，可以看一下这个网页的介绍，mpu开头的型号和icm-2x开头的型号已经不被推荐了（有些都已经停产了）。科学技术发展很快，这些传感器的制造工艺和精度也在飞速提升。所以大家学习或者做产品的话最好都是用新产品，电子产品都是买新不买旧嘛，更新换代太快了。传感器不好用是会难为死软件算法的。
我们可以看一下TDK官方网站的选型表，最新的ICM-42688-P的陀螺仪噪声2.8mdps/√Hz，加速度计噪声是（XY: 65, Z: 70）/√Hz。国产的QMI8658和国外差距确实还不小，我们选型的时候主要还是看使用场景和预算，选国产的好处就是价格便宜而且几乎没有断供风险嘛。不满足使用要求的话使用国外的IMU也无可厚非。
国产IMU移植到立创梁山派上
这些传感器厂家都会自己做一个驱动的，我们可以从厂家的驱动基础上改。但是我没有在他们的官网找到QMI8658A驱动，是在淘宝卖评估板的下面找到了驱动代码。具体代码在2_Code->applications->sensor->qmi8658里面。
这个IMU是支持IIC和SPI的，为了能获得更多的数据量，我在画电路板的时候就选择了SPI通讯。具体可以去立创开源平台查看。
这里我把接口文件单独成了一个文件qmi8658x_port.c,这里实现的函数是库文件qmi8658.c需要用到的通讯接口，具体代码如下：

#include <stdio.h>
#include <rtthread.h>
#include <board.h>
#include <gd32f4xx_libopt.h>
#include <qmi8658x_port.h>
void spi_gpio_config(void)
{
// SPI CS gpio
rcu_periph_clock_enable(SPI_CS_GPIO_CLK);
gpio_mode_set(SPI_CS_GPIO_PORT, GPIO_MODE_OUTPUT, SPI_CS_GPIO_PUPD,
SPI_CS_GPIO_PIN);
gpio_output_options_set(SPI_CS_GPIO_PORT, GPIO_OTYPE_PP, GPIO_OSPEED_25MHZ,
SPI_CS_GPIO_PIN);
// SPI SCK gpio init
rcu_periph_clock_enable(SPI_SCK_GPIO_CLK);
gpio_mode_set(SPI_SCK_GPIO_PORT, GPIO_MODE_AF, SPI_SCK_GPIO_PUPD,
SPI_SCK_GPIO_PIN);
gpio_af_set(SPI_SCK_GPIO_PORT, SPI_SCK_GPIO_ALT_FUNC, SPI_SCK_GPIO_PIN);
// SPI MISO gpio init
rcu_periph_clock_enable(SPI_MISO_GPIO_CLK);
gpio_mode_set(SPI_MISO_GPIO_PORT, GPIO_MODE_AF, SPI_MISO_GPIO_PUPD,
SPI_MISO_GPIO_PIN);
gpio_af_set(SPI_MISO_GPIO_PORT, SPI_MISO_GPIO_ALT_FUNC, SPI_MISO_GPIO_PIN);
// SPI MOSI gpio init
rcu_periph_clock_enable(SPI_MOSI_GPIO_CLK);
gpio_mode_set(SPI_MOSI_GPIO_PORT, GPIO_MODE_AF, SPI_MOSI_GPIO_PUPD,
SPI_MOSI_GPIO_PIN);
gpio_af_set(SPI_MOSI_GPIO_PORT, SPI_MOSI_GPIO_ALT_FUNC, SPI_MOSI_GPIO_PIN);
SET_SPI_NSS_HIGH();
}
void spi_config(void)
{
spi_parameter_struct spi_init_struct;
rcu_periph_clock_enable(RCU_SPI3);
/* configure SPI3 parameter */
spi_init_struct.trans_mode = SPI_TRANSMODE_FULLDUPLEX;
spi_init_struct.device_mode = SPI_MASTER;
spi_init_struct.frame_size = SPI_FRAMESIZE_8BIT;
spi_init_struct.clock_polarity_phase = SPI_CK_PL_LOW_PH_1EDGE;
spi_init_struct.nss = SPI_NSS_SOFT;
spi_init_struct.prescale = SPI_PSC_16;
spi_init_struct.endian = SPI_ENDIAN_MSB;
spi_init(SPI3, &spi_init_struct);
}
uint8_t spi_send_byte(uint8_t byte)
{
/* loop while data register in not emplty */
while (RESET == spi_i2s_flag_get(SPI3, SPI_FLAG_TBE))
;
/* send byte through the SPI3 peripheral */
spi_i2s_data_transmit(SPI3, byte);
/* wait to receive a byte */
while (RESET == spi_i2s_flag_get(SPI3, SPI_FLAG_RBNE))
;
/* return the byte read from the SPI bus */
return (spi_i2s_data_receive(SPI3));
}
uint8_t spi_flash_read_byte(void) { return (spi_send_byte(0xff)); }
uint8_t qst_imu_spi_write(uint8_t reg, uint8_t value)
{
spi_send_byte(reg & 0x7f);
spi_send_byte(value);
return 0;
}
uint8_t qst_imu_spi_write_bytes(uint8_t reg, uint8_t *value, uint8_t len)
{
uint8_t *p = value;
static uint32_t i = 0;
for (i = 0; i < len; i++)
{
qst_imu_spi_write(reg, *p++);
}
return 0;
}
uint8_t qst_imu_spi_read(uint32_t reg, uint8_t value)
{
spi_send_byte(reg | 0x80);
return spi_flash_read_byte();
}
uint8_t qst_imu_spi_read_bytes(uint8_t reg, void *buffer, uint8_t len)
{
static uint32_t i = 0;
uint8_t *p = buffer;
spi_send_byte(reg | 0x80);
for (i = 0; i < len; i++)
{
*p++ = spi_flash_read_byte();
}
return 0;
}
int sensor_spi_init(void)
{
spi_gpio_config();
spi_config();
spi_enable(SPI3);
return RT_EOK;
}
INIT_BOARD_EXPORT(sensor_spi_init);

复制代码

想要读写国产IMU-QMI8658A，实现上面的这些接口就可以了，下面分段解释下上面的代码：

首先，包含了一些必要的头文件，例如stdio.h，rtthread.h（实时操作系统RT-Thread）以及IMU传感器的接口头文件qmi8658x_port.h等。
spi_gpio_config()用于配置SPI接口所需的GPIO引脚。这包括CS（片选），SCK（时钟），MISO（主设备输入，从设备输出）和MOSI（主设备输出，从设备输入）引脚。然后，还设置了GPIO速度和复用功能。
spi_config()用于配置SPI接口的参数。这里使用的是SPI3接口，配置为全双工的主模式，数据帧大小为8位，查询QMI8658A的数据手册，翻到对SPI通讯介绍（Figure 18. SPI 4-Wire Single Byte Read and Write (Mode 0 and Mode 3)），可以了解到，他只支持SPI的Mode0和Mode3。所以这里选择的时钟极性和相位为CPOL=0和CPHA=0，也就是SPI的Mode0模式，软件NSS模式，预分频系数为16，以及大端模式。
spi_send_byte()用于通过SPI接口发送一个字节，并接收并返回从SPI总线读取的一个字节。它等待传输缓冲区为空，发送一个字节，然后等待接收缓冲区非空并读取一个字节。
spi_flash_read_byte()是一个简单的包装函数，用于读取一个字节，实际上调用spi_send_byte(0xff)。发送的这个0xff主要用来产生SPI时钟，这样QMI8658返回来的数据才有意义。
qst_imu_spi_write()用于向指定的寄存器地址写入一个字节。它首先发送寄存器地址（最高位为0，表示写操作），然后发送要写入的值。这里0x7f的二进制是01111111,任何一个数和他相与最高位都会变成0。
qst_imu_spi_write_bytes()用于向指定的寄存器地址连续写入多个字节。它循环调用qst_imu_spi_write()函数以完成这个任务。
qst_imu_spi_read()用于从指定的寄存器地址读取一个字节。它首先发送寄存器地址（最高位为1，表示读操作），然后调用spi_flash_read_byte()函数以获取返回的值。这里0x80的二进制是10000000,任何一个数和他相或最高位都会变成1。
qst_imu_spi_read_bytes()用于从指定的寄存器地址连续读取多个字节。它首先发送寄存器地址（最高位为1，表示读操作），然后循环调用spi_flash_read_byte()将读取的字节保存到缓冲区。
sensor_spi_init()用于初始化SPI接口。它首先调用spi_gpio_config()函数配置GPIO引脚，然后调用spi_config()函数配置SPI参数，最后使能SPI3。这个函数直接返回RT_EOK，表示初始化成功。
最后，INIT_BOARD_EXPORT(sensor_spi_init)宏用于将sensor_spi_init()函数导出，使其在系统启动时自动执行，完成对QMI8658A的SPI接口初始化。

什么是姿态解算?
姿态解算是一种计算物体在三维空间中的方向和角度的过程。通俗地说，姿态解算就是通过一些传感器（如陀螺仪、加速度计和磁力计）获取的数据，计算物体在空间中的朝向（即其姿态）。
我们生活中的产品比如无人机就用到了姿态解算。最常用的姿态解算算法是基于四元数和滤波算法的组合。四元数在表示旋转方面具有很好的数值稳定性和避免万向节锁的优点，这对于无人机的连续、平滑旋转至关重要。同时，由于无人机的动态特性和环境噪声，需要使用滤波算法来整合多种传感器数据（如陀螺仪、加速度计和磁力计），以提高姿态估计的准确性和鲁棒性。
扩展卡尔曼滤波（EKF）和互补滤波（CF）是在无人机控制中常用的滤波算法。EKF能够处理非线性系统，适用于结合加速度计、陀螺仪和磁力计数据，以获得更准确的姿态估计。然而，EKF的计算复杂度较高，可能不适合资源受限的嵌入式系统，立创梁山派的主控芯片GD32F470计算EKF是没问题的。相比之下，互补滤波是一种简单易实现的方法，通过加权平均加速度计和陀螺仪数据来估计物体的姿态。尽管互补滤波在某些情况下可能不如EKF准确，但其优点是计算简单，实时性好，适合无人机控制。还有一种滤波算法是卡尔曼滤波，它是一种线性滤波算法，用于估计系统的状态。在姿态解算中，卡尔曼滤波可以结合加速度计和陀螺仪数据，减少误差并提高姿态估计的准确性。
选择适用的姿态解算算法需要根据无人机的具体应用、硬件条件和性能要求来权衡。在实际使用中，还需要注意传感器的误差和噪声，通过校准和滤波等方法来提高姿态估计的准确性。
移植开源的Fusion实现姿态解算
克隆Fusion仓库，查看他的Examples里面的简单示例，可以知道，如果我们只有一个六轴IMU,那么把下面代码中gyroscope和accelerometer替换成IMU实际采集到的数据，宏定义SAMPLE_PERIOD改为实际的运行间隔就可以了。

#include "../../Fusion/Fusion.h"
#include <stdbool.h>
#include <stdio.h>
#define SAMPLE_PERIOD (0.01f) // replace this with actual sample period
int main() {
FusionAhrs ahrs;
FusionAhrsInitialise(&ahrs);
while (true) { // this loop should repeat each time new gyroscope data is available
const FusionVector gyroscope = {0.0f, 0.0f, 0.0f}; // replace this with actual gyroscope data in degrees/s
const FusionVector accelerometer = {0.0f, 0.0f, 1.0f}; // replace this with actual accelerometer data in g
FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, SAMPLE_PERIOD);
const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
printf("Roll %0.1f, Pitch %0.1f, Yaw %0.1f\n", euler.angle.roll, euler.angle.pitch, euler.angle.yaw);
}
}

复制代码

在实际的21年电赛送药小车工程中，具体姿态解算在att_thread.c文件中。

/**
- [url=home.php?mod=space&uid=41770]@brief[/url] 姿态解算软件定时器回调函数
- [url=home.php?mod=space&uid=87825]@note[/url] None
- @param parameter
- @retval None
*/
static void att_thread_timer_callback(void *parameter)
{
qmi8658_read_xyz(acc, gyro);
accelerometer.array[0] = (float)((acc[0]) / 9.807f);
accelerometer.array[1] = (float)((acc[1]) / 9.807f);
accelerometer.array[2] = (float)((acc[2]) / 9.807f);
gyroscope.array[0] =
(float)((gyro[0]) * (180.0f / 3.14159265358979323846f));
gyroscope.array[1] =
(float)((gyro[1]) * (180.0f / 3.14159265358979323846f));
gyroscope.array[2] =
(float)((gyro[2]) * (180.0f / 3.14159265358979323846f));
FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer,
SAMPLE_PERIOD);
const FusionEuler euler =
FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
fusion_data.euler.euler.pitch = euler.angle.pitch;
fusion_data.euler.euler.roll = euler.angle.roll;
fusion_data.euler.euler.yaw = euler.angle.yaw;
fusion_data.quaternion.quaternion.q0 = ahrs.quaternion.element.w;
fusion_data.quaternion.quaternion.q1 = ahrs.quaternion.element.x;
fusion_data.quaternion.quaternion.q2 = ahrs.quaternion.element.y;
fusion_data.quaternion.quaternion.q3 = ahrs.quaternion.element.z;
mcn_publish(MCN_HUB(fusion_topic), &fusion_data);
}

复制代码

可以看到，IMU的数据在传给fusion做解算前做了单位换算，这是因为Fusion要的陀螺仪数据单位是degrees/s，加速度计数据单位是g。
后面的欧拉角**Pitch（俯仰角）、Roll（横滚角）和Yaw（偏航角）**则表示了物体的姿态信息，以无人机作为举例：

Pitch（俯仰角）：表示物体绕x轴的旋转角度。当物体的前端向上抬起时，俯仰角为正；当物体的前端向下抬起时，俯仰角为负。例如，在无人机中，改变俯仰角可以使无人机上升或下降。
Roll（横滚角）：表示物体绕y轴的旋转角度。当物体的右侧向上抬起时，横滚角为正；当物体的左侧向上抬起时，横滚角为负。例如，在无人机中，改变横滚角可以使无人机向左或向右侧倾斜。
Yaw（偏航角）：表示物体绕z轴的旋转角度。当物体的前端向左旋转时，偏航角为正；当物体的前端向右旋转时，偏航角为负。例如，在无人机中，改变偏航角可以使无人机在水平面内绕自身中心旋转。

再后面的四元数q0, q1, q2, q3 是表示物体旋转的四元数。四元数是一种扩展的复数，用于描述和计算三维空间中的旋转。四元数具有四个分量（q0, q1, q2, q3），其中q0是标量部分，q1、q2和q3是向量部分。四元数相对于欧拉角具有以下优点：

避免万向节锁：欧拉角在某些特定姿态下会出现万向节锁现象，导致某个旋转自由度丢失（在某些特定姿态下，两个旋转轴可能会重合，导致一个旋转自由度丢失）。四元数则没有这个问题，让得旋转表示更加稳定和连续。
更高的数值稳定性：四元数在计算旋转时具有更好的数值稳定性，避免了欧拉角在接近临界值时的数值不稳定现象。
简化旋转计算：四元数可以通过简单的数学运算实现旋转的组合、插值和求逆等操作，计算效率高。

账号		自动登录	找回密码
密码			注册

立创梁山派开发板-21年电赛F题-送药小车-国产IMU移植及姿...

主题推荐