使用 Cwiid Python 的 Wiimote Motion Plus 的俯仰和偏航
Pitch and Yaw from a Wiimote Motion Plus using Cwiid Python
我正在尝试使用 python 中的 cwiid 提取 wiimote 的方向。我已经设法获得加速度计值，但似乎没有任何与纯陀螺仪数据相关的对象属性。
这个人在 python 中设法做到了，但据我所知，没有 python 在线示例代码。
https://www.youtube.com/watch?v=cUjh0xQO6eY
wiibrew 上有关于控制器数据的信息，但这似乎又被排除在任何 python 库之外。
有人有什么建议吗？ This link 有获取陀螺仪数据的示例，但使用的包似乎不可用。
几天前我真的在找这个，找到了这个post：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）用于偏航
所以这个问题有几个部分，首先是如何从运动加传感器中提取陀螺仪数据。为此，首先需要启用 motion plus。
陀螺仪提供 angular 旋转矢量，但由于积分误差引起的漂移，您不能简单地使用这两者的组合来获得欧拉角。问题的第二部分是如何使用这些数据来给出位置，这是通过使用卡尔曼滤波器（高度复杂的矩阵序列）或互补滤波器（不太复杂的数学运算）来完成的。这两个滤波器本质上是结合了陀螺仪和加速度计数据，因此如上面的评论中所述，导致更稳定的测量、更少的漂移，并且系统在摇动遥控器时不易损坏。
卡尔曼滤波器：
http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/
互补滤镜
https://www.instructables.com/Angle-measurement-using-gyro-accelerometer-and-Ar/
目前仍在开发核心，但希望在明天完成后post。
我用来测试测量的基础代码可以在这里找到：
http://andrew-j-norman.blogspot.com/2010/12/more-code.html。非常方便，因为它会在记录后自动绘制传感器读数。您可以通过这样做看到，即使在静止时，通过使用 angular 速度的简单积分进行的位置估计也会导致位置向量发生漂移。
编辑：
对此进行测试允许陀螺仪传感器准确计算随时间变化的角度，但是加速度仍然存在漂移 - 我认为这是不可避免的。
这是一张演示陀螺仪运动传感器的图片：
刚刚完成代码：
#!/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",
timevector, data[1], "g",
timevector, 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,0]
position = [0,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,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", timevector, a, 1, n_graphs)
if n_graphs == 5:
plotter("Angle Rate", timevector, ar, 4, n_graphs)
plotter("Angle", timevector, aa, 5, n_graphs)
show()
我正在尝试使用 python 中的 cwiid 提取 wiimote 的方向。我已经设法获得加速度计值，但似乎没有任何与纯陀螺仪数据相关的对象属性。
这个人在 python 中设法做到了，但据我所知，没有 python 在线示例代码。 https://www.youtube.com/watch?v=cUjh0xQO6eY wiibrew 上有关于控制器数据的信息，但这似乎又被排除在任何 python 库之外。
有人有什么建议吗？ This link 有获取陀螺仪数据的示例，但使用的包似乎不可用。
几天前我真的在找这个，找到了这个post：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）用于偏航
所以这个问题有几个部分，首先是如何从运动加传感器中提取陀螺仪数据。为此，首先需要启用 motion plus。
陀螺仪提供 angular 旋转矢量，但由于积分误差引起的漂移，您不能简单地使用这两者的组合来获得欧拉角。问题的第二部分是如何使用这些数据来给出位置，这是通过使用卡尔曼滤波器（高度复杂的矩阵序列）或互补滤波器（不太复杂的数学运算）来完成的。这两个滤波器本质上是结合了陀螺仪和加速度计数据，因此如上面的评论中所述，导致更稳定的测量、更少的漂移，并且系统在摇动遥控器时不易损坏。
卡尔曼滤波器： http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/
互补滤镜 https://www.instructables.com/Angle-measurement-using-gyro-accelerometer-and-Ar/
目前仍在开发核心，但希望在明天完成后post。 我用来测试测量的基础代码可以在这里找到： http://andrew-j-norman.blogspot.com/2010/12/more-code.html。非常方便，因为它会在记录后自动绘制传感器读数。您可以通过这样做看到，即使在静止时，通过使用 angular 速度的简单积分进行的位置估计也会导致位置向量发生漂移。
编辑： 对此进行测试允许陀螺仪传感器准确计算随时间变化的角度，但是加速度仍然存在漂移 - 我认为这是不可避免的。
这是一张演示陀螺仪运动传感器的图片：
#!/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",
timevector, data[1], "g",
timevector, 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,0]
position = [0,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,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", timevector, a, 1, n_graphs)
if n_graphs == 5:
plotter("Angle Rate", timevector, ar, 4, n_graphs)
plotter("Angle", timevector, aa, 5, n_graphs)
show()