3

我有一个 IMU，我正在尝试仅使用加速度计和磁力计来获取它的方向。我正在使用从加速度计返回的重力矢量和从磁力计返回的矢量，并找到它们的叉积来获得东矢量和北矢量。我有以下代码：

import time
import math
from pyquaternion import Quaternion
import numpy as np
import vectormath as vmath

# Read acceleration, magnetometer, and gyroscope
accel_x, accel_y, accel_z = sensor.acceleration
mag_x, mag_y, mag_z = sensor.magnetic
gyro_x, gyro_y, gyro_z = sensor.gyro

# Define vectors for each sensor reading
accelerometer = vmath.Vector3(accel_x, accel_y, accel_z)
gyroscope = vmath.Vector3(math.radians(gyro_x), math.radians(gyro_y), math.radians(gyro_z))
magnetometer = vmath.Vector3(mag_x, mag_y, mag_z)

# Normalize the magnetometer and accelerometer vectors
magnetometer = magnetometer.normalize()
accelerometer = accelerometer.normalize()

down = -accelerometer # Get down vector, which is opposite the accelerometer reading
east = down.cross(magnetometer) # Get vector pointing east (perpendicular to down vector and magnetometer vector)
east = east.normalize() # Normalize east vector
north = east.cross(down) # Get vector pointing north (perpendicular to east vector and down vector)
north = north.normalize() # Normalize north vector

# Define vectors for axes in world view
eastworld = vmath.Vector3(0, 1, 0)
northworld = vmath.Vector3(-1, 0, 0)
downworld = vmath.Vector3(0, 0, -1)

# Find the angle between each vector and its respective world axis vector
anglediffX = round(east.angle(eastworld, unit='deg'))
anglediffY = round(north.angle(northworld, unit='deg'))
anglediffZ = round(down.angle(downworld, unit='deg'))

print(anglediffX, anglediffY, anglediffZ)

当我运行脚本时，我似乎得到了准确的结果，但是根据我旋转 IMU 的方式，两个轴可能会锁定并且它们的旋转值相同。我错过了什么吗？

4

0 回答 0