使用Cwiid Python的Wiimote Motion Plus的俯仰和偏航

问题描述

我正在尝试在python中使用cwiid提取Wiimote的方向。我已经设法获得了加速度计的值，但似乎没有与纯陀螺仪数据相关的任何对象属性。

这个人设法用python做到了，但是据我所知，网上没有python示例代码。 https://www.youtube.com/watch?v=cUjh0xQO6eY wiibrew上有关于控制器数据的信息，但是同样，这似乎已从任何python库中排除。

有人有任何建议吗？ This链接有一个获取陀螺仪数据的示例，但所使用的软件包似乎不可用。

解决方法

几天前我实际上在寻找这个，发现这篇文章：https://ofalcao.pt/blog/2014/controlling-the-sbrick-with-a-wiimote。更具体地说，我认为您正在寻找的代码是：

# roll = accelerometer[0],standby ~125
# pitch = accelerometer[1],standby ~125

...

    roll=(wm.state['acc'][0]-125)
    pitch=(wm.state['acc'][1]-125)

我假设您可以将z轴（索引2）用于偏航

,

所以这个问题有几个部分，首先是如何从运动加传感器中提取陀螺仪数据。为此，首先需要启用运动加号。

陀螺仪提供角旋转矢量，但是由于积分误差引起的漂移，您不能简单地将这两种方法结合使用以获得欧拉角。问题的第二部分是如何使用此数据来给出位置，这是通过使用卡尔曼滤波器，高度复杂的矩阵序列或互补滤波器，不太复杂的数学运算来完成的。这两个滤波器本质上都是将陀螺仪和加速度计的数据组合在一起，因此，如上面的评论所述，可以实现更稳定的测量，更少的漂移以及在摇动遥控器时不易损坏的系统。

卡尔曼滤波器： http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/

Using PyKalman on Raw Acceleration Data to Calculate Position

补充过滤器 https://www.instructables.com/Angle-measurement-using-gyro-accelerometer-and-Ar/

目前仍在开发核心，但会在我完成后发布，希望明天发布。 我用于测试测量的基础代码位于： http://andrew-j-norman.blogspot.com/2010/12/more-code.html。非常方便，因为它会在记录后自动绘制传感器读数。通过这样做，您可以看到，即使在静止时，使用角速度的简单积分进行位置估计也会导致位置矢量发生漂移。

编辑： 通过测试，陀螺仪传感器可以准确地计算出随时间变化的角度，但是加速度仍然存在漂移-我认为这是不可避免的。

以下是演示陀螺仪运动传感器的图像：

刚完成代码：

#!/usr/bin/python

import cwiid
from time import time,asctime,sleep,perf_counter
from numpy import *
from pylab import *
import math
import numpy as np
from operator import add
HPF = 0.98
LPF = 0.02

def calibrate(wiimote):
    print("Keep the remote still")
    sleep(3)
    print("Calibrating")
    
    messages = wiimote.get_mesg()
    i=0
    accel_init = []
    angle_init = []
    while (i<1000):
        sleep(0.01)
        messages = wiimote.get_mesg()
        for mesg in messages:
            # Motion plus:
            if mesg[0] == cwiid.MESG_MOTIONPLUS:
                if record:
                    angle_init.append(mesg[1]['angle_rate'])
            # Accelerometer:
            elif mesg[0] == cwiid.MESG_ACC:
                if record:
                    accel_init.append(list(mesg[1]))
        i+=1

    accel_init_avg = list(np.mean(np.array(accel_init),axis=0))
    print(accel_init_avg)
    angle_init_avg = sum(angle_init)/len(angle_init)
    print("Finished Calibrating")
    return (accel_init_avg,angle_init_avg)
    
def plotter(plot_title,timevector,data,position,n_graphs):
   subplot(n_graphs,1,position)
   plot(timevector,data[0],"r",data[1],"g",data[2],"b")
   xlabel("time (s)")
   ylabel(plot_title)

print("Press 1+2 on the Wiimote now")
wiimote = cwiid.Wiimote()

# Rumble to indicate a connection
wiimote.rumble = 1
print("Connection established - release buttons")
sleep(0.2)
wiimote.rumble = 0
sleep(1.0)

wiimote.enable(cwiid.FLAG_MESG_IFC | cwiid.FLAG_MOTIONPLUS)
wiimote.rpt_mode = cwiid.RPT_BTN | cwiid.RPT_ACC | cwiid.RPT_MOTIONPLUS

accel_init,angle_init = calibrate(wiimote)
str = ""
print("Press plus to start recording,minus to end recording")
loop = True
record = False
accel_data = []
angle_data = []
messages = wiimote.get_mesg()
while (loop):
   sleep(0.01)
   messages = wiimote.get_mesg()
   for mesg in messages:
       # Motion plus:
       if mesg[0] == cwiid.MESG_MOTIONPLUS:
           if record:
               angle_data.append({"Time" : perf_counter(),\
                   "Rate" : mesg[1]['angle_rate']})
       # Accelerometer:
       elif mesg[0] == cwiid.MESG_ACC:
           if record:               
               accel_data.append({"Time" : perf_counter(),"Acc" : [mesg[1][i] - accel_init[i] for i in range(len(accel_init))]})
       # Button:
       elif mesg[0] == cwiid.MESG_BTN:
           if mesg[1] & cwiid.BTN_PLUS and not record:
               print("Recording - press minus button to stop")
               record = True
               start_time = perf_counter()
           if mesg[1] & cwiid.BTN_MINUS and record:
               if len(accel_data) == 0:
                   print("No data recorded")
               else:
                   print("End recording")
                   print("{0} data points in {1} seconds".format(
                       len(accel_data),perf_counter() - accel_data[0]["Time"]))
               record = False
               loop = False
       else:
           pass

wiimote.disable(cwiid.FLAG_MESG_IFC | cwiid.FLAG_MOTIONPLUS)
if len(accel_data) == 0:
   sys.exit()


timevector = []
a = [[],[],[]]
v = [[],[]]
p = [[],[]]
last_time = 0
velocity = [0,0]
position = [0,0]

for n,x in enumerate(accel_data):
   if (n == 0):
       origin = x
   else:
       elapsed = x["Time"] - origin["Time"]
       delta_t = x["Time"] - last_time
       timevector.append(elapsed)
       for i in range(3):
           acceleration = x["Acc"][i] - origin["Acc"][i]
           velocity[i] = velocity[i] + delta_t * acceleration
           position[i] = position[i] + delta_t * velocity[i]
           a[i].append(acceleration)
           v[i].append(velocity[i])
           p[i].append(position[i])
   last_time = x["Time"]

n_graphs = 3
if len(angle_data) == len(accel_data):
   n_graphs = 5
   angle_accel = [(math.pi)/2 if (j**2 + k**2)==0 else math.atan(i/math.sqrt(j**2 + k**2)) for i,j,k in zip(a[0],a[1],a[2])]
   ar = [[],[]] # Angle rates
   aa = [[],[]] # Angles
   angle = [0,0]
   for n,x in enumerate(angle_data):
       if (n == 0):
           origin = x
       else:
           delta_t = x["Time"] - last_time
           for i in range(3):
               rate = x["Rate"][i] - origin["Rate"][i]
               angle[i] = HPF*(np.array(angle[i]) + delta_t * rate) + LPF*np.array(angle_accel)
               ar[i].append(rate)
               aa[i].append(angle[i])
       last_time = x["Time"]


plotter("Acceleration",a,n_graphs)
if n_graphs == 5:
   plotter("Angle Rate",ar,4,n_graphs)
   plotter("Angle",aa,5,n_graphs)

show()