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

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

<div class='showpostmsg'><h1>b18472ede3497b6294d0457d83abc245 
 </h1>

<h1>前言</h1>

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

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

参考

<a href="https://wiki.unihiker.com/pinpong_python_lib#target_9">https://wiki.unihiker.com/pinpong_python_lib#target_9</a>

 

<h1 >传感器数据获取</h1>

<div class="parsedown-markdown">
# -*- 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)
</div>

 

 

<h1 >姿态解算</h1>

<div class="parsedown-markdown">
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
</div>

 

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

打印欧拉角如下

  

<h1 >可视化</h1>

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

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

<div class="parsedown-markdown">
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"))
</div>

 

<h1 >测试</h1>

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

<div class="parsedown-markdown">
# -*- coding: utf-8 -*-

import time

from pinpong.board import *

from pinpong.extension.unihiker import *

from unihiker import GUI #导入包

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

 

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"))

 

Board().begin()  #初始化

gui=GUI()#实例化GUI类

 

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的值

  p,r,y = Update_IMU(ax,ay,az,gx,gy,gz)

  print(p,y,y)

  Draw_Pos(gui,p,r,y)

  time.sleep(0.1)
</div>

 

 

<h1 >总结</h1>

以上测试了六轴传感器的基本操作与姿态解算，基于完善的官方python库，能很快就完成基本的Demo。

基于此可以完成更多的有意义的项目，比如水平仪，角度仪，手势控制等。
</div><script> var loginstr = '<div class="locked">查看本帖全部内容，请<a href="javascript:;" style="color:#e60000" class="loginf">登录</a>或者<a href="https://bbs.eeworld.com.cn/member.php?mod=register_eeworld.php&action=wechat" style="color:#e60000" target="_blank">注册</a></div>';

if(parseInt(discuz_uid)==0){
(function($){
var postHeight = getTextHeight(400);
$(".showpostmsg").html($(".showpostmsg").html());
$(".showpostmsg").after(loginstr);
$(".showpostmsg").css({height:postHeight,overflow:"hidden"});
})(jQuery);
} </script><script type="text/javascript">(function(d,c){var a=d.createElement("script"),m=d.getElementsByTagName("script"),eewurl="//counter.eeworld.com.cn/pv/count/";a.src=eewurl+c;m.parentNode.insertBefore(a,m)})(document,523)</script>

freebsder 发表于 2022-11-28 16:28

注释的不错，看得很清晰

rainbowczj 发表于 2022-12-3 10:02

学习了，非常感谢。

页: [1]

电子工程世界-论坛's Archiver

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