【Follow me第二季第4期】任务二：IMU知识+IMU数据读取+IMU控制立方体

HonestQiao

【Follow me第二季第4期】任务二：IMU知识+IMU数据读取+IMU控制立方体 [复制链接]

 

在 Arduino® Nano RP2040 Connect 上，板载了一颗IMU传感器，集速度计、陀螺仪于一体，可以用于运动状态的获取和分析。

这篇分享，就来一起研究研究这个IMU传感器的使用。

 

 

 

一、认识板载IMU传感器

通过 Arduino® Nano RP2040 Connect 的官方资料，可以了解到板载的IMU传感器为LSM6DSOXTR：

 其具体位置，如下图所示：

  

 

LSM6DSOXTR 是ST 生产的，从ST官方资料可以了解其功能：

 

IMU传感器通过检测加速度和角速度，从而感知在三维空间中的运动：【参考：IMU是什么？ IMU（惯性测量单元）是一个…… |作者：Barak Or 博士 |走向数据科学】

 

二、从IMU传感器读取数据

在Arduino® Nano RP2040 Connect适配的micropython环境中，已经默认提供了该传感器的驱动程序，可以很方便的调用。

从Pinout可以了解，其使用IIC连接：

 

要从其读取数据，使用下面的代码即可：

import time
from machine import Pin, I2C
from lsm6dsox import LSM6DSOX

i2c = I2C(0, scl=Pin(13), sda=Pin(12))
lsm = LSM6DSOX(i2c)

while True:
    accx, accy, accz = lsm.accel()
    print(f"accel: x:{accx:.2f}m/s2, y:{accy:.2f}m/s2, z{accz:.2f}m/s2")
    gyrox, gyroy, gyroz = lsm.gyro()
    print(f"gyro: x:{gyrox:.2f}°/s, y:{gyroy:.2f}°/s, z{gyroz:.2f}°/s")
    print("")
    time.sleep(0.5)

上面的代码逻辑如下：

1. 引入需要的模块：LSM6DSOX
2. 实例化IIC接口
3. 实例化传感器对象LSM6DSOX
4. 在循环中读取加速度值和角速度值并输出

运行后，输出结果如下：

 

 

此时如果拿起来板子，就会看到上数值快速发生了变化。

可以尝试不同的运动方式，观察数值变化的规律。

参考下面的图，沿不同轴向运动，则能够观察出更好的规律：

 

三、使用IMU数据控制立方体

1. Arduino® Nano RP2040 Connect 运行程序：

光看从IMU读取输出的数据，有点乏味，可以通过电脑结合，读取IMU的数据，在程序中控制一个立方体的呈现。

为了让电脑程序更好的获取IMU数据，开发板运行的程序，稍微修改一下：

import time
from machine import Pin, I2C
from lsm6dsox import LSM6DSOX

i2c = I2C(0, scl=Pin(13), sda=Pin(12))
lsm = LSM6DSOX(i2c)

while True:
    accx, accy, accz = lsm.accel()
    print(f"accel: x:{accx:.2f}m/s2, y:{accy:.2f}m/s2, z{accz:.2f}m/s2", end="")
    gyrox, gyroy, gyroz = lsm.gyro()
    print(f"\tgyro: x:{gyrox:.2f}°/s, y:{gyroy:.2f}°/s, z{gyroz:.2f}°/s")
    #print("")
    time.sleep(0.1)

 

把数据在一行输出，方便电脑上面读取和解析，实际输出的数据如下：

accel: x:-0.16m/s2, y:-0.40m/s2, z0.91m/s2    gyro: x:0.06°/s, y:0.31°/s, z0.12°/s

 

电脑上要读取 Arduino® Nano RP2040 Connect 输出的数据，可以直接使用 USB 串口，这样就不能和micropython 编辑器同时使用。

把上面的程序，保存到 Arduino® Nano RP2040 Connect 上的main.py，然后断开IDE的设备串口连接，并重新插拔开发板，即可开机自动运行。

此时，用串口工具查看 Arduino® Nano RP2040 Connect 的输出：

 

这样开发板的工作，就准备好了。

确保正常输出后，要关闭串口工具，这样子后面的程序才能使用串口和读取数据，否则会冲突。

 

2. 电脑程序编写

在电脑上，使用Python，可以很快编写一个从串口读取数据，解析数据，然后控制立方体的程序。

 

在电脑上使用Python来呈现3D物体和控制，需要用到：matplotlib、mpl_toolkits、numpy库，从串口读取数据还需要pyserial，可以使用下面的命令安装：

pip install numpy
pip install matplotlib
pip install mpl_toolkits
pip install pyserial

然后编写对应的程序：

import serial
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import re
import time

# 串口配置
port = '/dev/cu.usbmodem14201'  # 根据实际情况修改串口号
baud_rate = 115200

# 初始化串口
ser = serial.Serial(port, baud_rate, timeout=1)

# 初始化3D图形
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')

# 固定位置
x, y, z = 0, 0, 0
roll, pitch, yaw = 0, 0, 0

# 更新3D物体的位置和姿态
def update_3d_object(ax, x, y, z, roll, pitch, yaw):
    ax.clear()
    ax.set_xlim([-10, 10])
    ax.set_ylim([-10, 10])
    ax.set_zlim([-10, 10])

    # 绘制3D物体（这里用一个更大的立方体表示）
    size = 2  # 立方体的边长
    cube = np.array([
        [x-size, y-size, z-size],
        [x+size, y-size, z-size],
        [x+size, y+size, z-size],
        [x-size, y+size, z-size],
        [x-size, y-size, z+size],
        [x+size, y-size, z+size],
        [x+size, y+size, z+size],
        [x-size, y+size, z+size]
    ])

    # 应用旋转矩阵
    R = rotation_matrix(roll, pitch, yaw)
    cube_rotated = np.dot(cube, R.T)

    # 绘制立方体
    colors = ['r', 'g', 'b', 'c', 'm', 'y']  # 不同的颜色
    for i in range(4):
        ax.plot([cube_rotated[i, 0], cube_rotated[(i+1) % 4, 0]],
                [cube_rotated[i, 1], cube_rotated[(i+1) % 4, 1]],
                [cube_rotated[i, 2], cube_rotated[(i+1) % 4, 2]], colors[i % 6])
        ax.plot([cube_rotated[i+4, 0], cube_rotated[(i+1) % 4 + 4, 0]],
                [cube_rotated[i+4, 1], cube_rotated[(i+1) % 4 + 4, 1]],
                [cube_rotated[i+4, 2], cube_rotated[(i+1) % 4 + 4, 2]], colors[i % 6])
        ax.plot([cube_rotated[i, 0], cube_rotated[i+4, 0]],
                [cube_rotated[i, 1], cube_rotated[i+4, 1]],
                [cube_rotated[i, 2], cube_rotated[i+4, 2]], colors[i % 6])

    plt.draw()
    plt.pause(0.001)

# 旋转矩阵
def rotation_matrix(roll, pitch, yaw):
    Rx = np.array([[1, 0, 0],
                   [0, np.cos(roll), -np.sin(roll)],
                   [0, np.sin(roll), np.cos(roll)]])
    Ry = np.array([[np.cos(pitch), 0, np.sin(pitch)],
                   [0, 1, 0],
                   [-np.sin(pitch), 0, np.cos(pitch)]])
    Rz = np.array([[np.cos(yaw), -np.sin(yaw), 0],
                   [np.sin(yaw), np.cos(yaw), 0],
                   [0, 0, 1]])
    return np.dot(Rz, np.dot(Ry, Rx))

# 读取并解析串口数据
def read_imu_data():
    global roll, pitch, yaw
    if ser.in_waiting > 0:
        data = ser.readline().decode('utf-8').strip()
        match = re.match(r'accel: x:(.*?)m/s2, y:(.*?)m/s2, z(.*?)m/s2\t?gyro: x:(.*?)°/s, y:(.*?)°/s, z(.*?)°/s', data)
        if match:
            accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z = map(float, match.groups())
            # 仅根据角速度更新姿态
            dt = 0.1  # 假设采样间隔为0.1秒
            roll += gyro_x * dt
            pitch += gyro_y * dt
            yaw += gyro_z * dt
            print(data)
            print("")

# 主循环
while True:
    read_imu_data()
    update_3d_object(ax, x, y, z, roll, pitch, yaw)
    time.sleep(0.01)

 

上面程序的具体逻辑，就不详细说明了，感兴趣的可以研究一下。

重点在最后的主循环中的处理，先读取IMU的数据，就是前面输出的一行数据，然后使用读取到的数据，更新3D立方体的显示，然后延时继续。

 

运行上面的程序，具体效果如下：

 

视频如下：

iShot_2024-11-22_17.19.00

参考前面的测试，沿着不同的轴向运动，看到的效果会更好。

 

四、小结

上面只是演示了在 Arduino® Nano RP2040 Connect 上，LSM6DSOXTR IMU传感器的简单使用。

更进一步的，还可以通过该IMU传感器，来进行振动的检测、手势的识别、计步、倾斜度检测、自由落体、唤醒、单击和双击等等，其用途是非常广泛的。

wangerxian

6啊，如果把立方体换成一个物体，这样更有感觉。

HonestQiao

wangerxian 发表于 2024-11-22 17:51 6啊，如果把立方体换成一个物体，这样更有感觉。

例如换成一架飞机，直接可以玩模拟飞行了

秦天qintian0303

再来一个就完成了，前三个传感器学习还是挺不错得 

HonestQiao

秦天qintian0303 发表于 2024-11-22 21:00 再来一个就完成了，前三个传感器学习还是挺不错得 

是的是的，这几个传感器，多个环境都能很方便的调用。

wangerxian

HonestQiao 发表于 2024-11-22 19:14 例如换成一架飞机，直接可以玩模拟飞行了

是的，那样就很炫酷。

hellokitty_bean

HonestQiao 发表于 2024-11-22 19:14 例如换成一架飞机，直接可以玩模拟飞行了

Microsoft貌似有一个模拟飞行的游戏，配合手柄玩。。。。。。。

hellokitty_bean

貌似马航的那个失事飞机飞行员，玩的就是那个软件。。。。。。。。是不是飞行员都在用那个模拟飞行软件就不得人知了。。。。

HonestQiao

hellokitty_bean 发表于 2024-11-25 19:38 貌似马航的那个失事飞机飞行员，玩的就是那个软件。。。。。。。。是不是飞行员都在用那个模拟飞行软件就不 ...

那个做的确实好玩

怀66

学习了！！！感谢大佬分享