diff --git a/integration/acceleration-calculation.py b/integration/acceleration-calculation.py new file mode 100644 index 0000000000000000000000000000000000000000..5800d23f7d5b2316cb46d9bce5384db29204f6f8 --- /dev/null +++ b/integration/acceleration-calculation.py @@ -0,0 +1,69 @@ +import numpy as np +from scipy.spatial.transform import Rotation as R + + +class QuaternionMotion: + def __init__(self, quaternions, dt, position_vector): + self.quaternions = quaternions + self.dt = dt + self.position_vector = np.array(position_vector) + + def quaternion_derivative(self): + q_next = self.quaternions[1:] + q_prev = self.quaternions[:-1] + dq_dt = (q_next - q_prev) / self.dt + return dq_dt + + def angular_velocity(self): + dq_dt = self.quaternion_derivative() + angular_velocities = [] + for i in range(len(dq_dt)): + q = self.quaternions[i] + q_conj = np.array([q[0], -q[1], -q[2], -q[3]]) # 四元数的共轭 + + # 四元数微分公式来计算角速度 + omega_quat = 2 * self.quaternion_multiply(q_conj, dq_dt[i]) + omega = omega_quat[1:] # 提取角速度的向量部分 + angular_velocities.append(omega) + return np.array(angular_velocities) + + def quaternion_multiply(self, q1, q2): + w1, x1, y1, z1 = q1 + w2, x2, y2, z2 = q2 + return np.array([ + w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2, + w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2, + w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2, + w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2 + ]) + + def angular_acceleration(self): + omega = self.angular_velocity() + omega_next = omega[1:] + omega_prev = omega[:-1] + alpha = (omega_next - omega_prev) / self.dt + return alpha + + def linear_acceleration(self): + alpha = self.angular_acceleration() + linear_accelerations = np.cross(alpha, self.position_vector) + return linear_accelerations + + +def main(): + quaternions = np.array([ + [0.707, 0, 0.707, 0], + [0.707, 0, 0.707, 0.1], + [0.707, 0, 0.707, 0.2] + ]) + + dt = 0.1 # 时间间隔 + position_vector = [1, 0, 0] # 位置矢量 + + motion = QuaternionMotion(quaternions, dt, position_vector) + linear_acceleration = motion.linear_acceleration() + print("线性加速度: ", linear_acceleration) + + +if __name__ == "__main__": + main()