【行空板 Python编程学习主控板】六轴加速度传感器可视化测试

qinyunti · 发表于2022-11-27 20:58

【行空板 Python编程学习主控板】六轴加速度传感器可视化测试 [复制链接]

3b7e2c21fb1213033440acc4f4615850

前言

开发板自带六轴加速度传感器可实时测量三轴加速度，三轴角速度。

通过这些数据可以进行姿态解算，知道开发板的姿态变化，进而可利用此进行交互。

参考

https://wiki.unihiker.com/pinpong_python_lib#target_9

传感器数据获取

# -*- coding: utf-8 -*-

import time

from pinpong.board import *

from pinpong.extension.unihiker import *

while True:

ax = accelerometer.get_x() #读取加速度X的值

ay = accelerometer.get_y() #读取加速度Y的值

az = accelerometer.get_z() #读取加速度Z的值

a = accelerometer.get_strength() #读取加速度强度（x、y、z方向的合力）

gx = gyroscope.get_x() #读取陀螺仪X的值

gy = gyroscope.get_y() #读取陀螺仪Y的值

gz= gyroscope.get_z() #读取陀螺仪Z的值

time.sleep(0.1)

姿态解算

import math

#IMU算法更新

Kp = 100 #比例增益控制加速度计/磁强计的收敛速度

Ki = 0.002 #积分增益控制陀螺偏差的收敛速度

halfT = 0.001 #采样周期的一半

#传感器框架相对于辅助框架的四元数(初始化四元数的值)

q0 = 1

q1 = 0

q2 = 0

q3 = 0

#由Ki缩放的积分误差项(初始化)

exInt = 0

eyInt = 0

ezInt = 0

def Update_IMU(ax,ay,az,gx,gy,gz):

global q0

global q1

global q2

global q3

global exInt

global eyInt

global ezInt

# print(q0)

#测量正常化

norm = math.sqrt(ax*ax+ay*ay+az*az)

#单元化

ax = ax/norm

ay = ay/norm

az = az/norm

#估计方向的重力

vx = 2*(q1*q3 - q0*q2)

vy = 2*(q0*q1 + q2*q3)

vz = q0*q0 - q1*q1 - q2*q2 + q3*q3

#错误的领域和方向传感器测量参考方向之间的交叉乘积的总和

ex = (ay*vz - az*vy)

ey = (az*vx - ax*vz)

ez = (ax*vy - ay*vx)

#积分误差比例积分增益

exInt += ex*Ki

eyInt += ey*Ki

ezInt += ez*Ki

#调整后的陀螺仪测量

gx += Kp*ex + exInt

gy += Kp*ey + eyInt

gz += Kp*ez + ezInt

#整合四元数

q0 += (-q1*gx - q2*gy - q3*gz)*halfT

q1 += (q0*gx + q2*gz - q3*gy)*halfT

q2 += (q0*gy - q1*gz + q3*gx)*halfT

q3 += (q0*gz + q1*gy - q2*gx)*halfT

#正常化四元数

norm = math.sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3)

q0 /= norm

q1 /= norm

q2 /= norm

q3 /= norm

#获取欧拉角 pitch、roll、yaw

pitch = math.asin(-2*q1*q3+2*q0*q2)*57.3

roll = math.atan2(2*q2*q3+2*q0*q1,-2*q1*q1-2*q2*q2+1)*57.3

yaw = math.atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3)*57.3

return pitch,roll,yaw

分别旋转三个轴，可以看到三个分量值变化。

打印欧拉角如下

可视化

将角度投影到平面，根据角度计算x，y坐标的分量，从原点画线，就可以形成一个圆形轨迹，角度变化就对应着在圆形轨迹移动

这里只体现了r，y两个轴的角度

def Draw_Pos(gui,p,r,y):

l = 100

LCDX=240

LCDY=320

px = l*math.sin(p)

py = l*math.cos(p)

gui.draw_line(x0=LCDX/2,y0=LCDY/2,x1=LCDX/2+px,y1=LCDY/2+py,width=4,color=(255,0,0),onclick=lambda:print("line clicked"))

测试

综合以上得到测试程序如下