From 3b5ee152034fb4d9d968a6302ae4ad9a21843d0b Mon Sep 17 00:00:00 2001
From: ym13n22 <ym13n22@soton.ac.uk>
Date: Tue, 27 Aug 2024 16:04:07 +0100
Subject: [PATCH] window1 add acceleration
---
integration/Window.py | 89 ++++++++++++++++++++++++++++++++++++++++++-
1 file changed, 87 insertions(+), 2 deletions(-)
diff --git a/integration/Window.py b/integration/Window.py
index d6c1bc6..131033d 100644
--- a/integration/Window.py
+++ b/integration/Window.py
@@ -120,6 +120,9 @@ class Window:
sleep(1)
# Conversions
+
+ self.quaternionsList=[]
+ self.accelerations=[]
self.transmitting = False
self.toRad = 2 * np.pi / 360
self.toDeg = 1 / self.toRad
@@ -361,6 +364,31 @@ class Window:
q1 = float(splitPacket[2]) # qx
q2 = float(splitPacket[3]) # qy
q3 = float(splitPacket[4]) # qz
+ qs = [q0, q1, q2, q3]
+ #print(f"qs :{qs}")
+ self.quaternionsList.append(qs)
+ if(len(self.quaternionsList)>3):
+ self.quaternionsList.pop(0)
+
+
+ quaternionsArray = np.array(self.quaternionsList)
+ dt = 0.1 # 时间间隔
+ position_vector = [1, 0, 0] # 位置矢量
+
+ motion = QuaternionMotion(quaternionsArray, dt, position_vector)
+ linear_acceleration = motion.linear_acceleration()
+ print("线性加速度: ", linear_acceleration)
+ self.accelerations.append(linear_acceleration)
+ if(len(self.accelerations)>3):
+ self.accelerations.pop(0)
+ accelerationsArray = np.array(self.accelerations)
+ displacement=self.calculate_displacement(accelerationsArray,dt)
+
+ displacement_array = np.array(displacement)
+ combined_displacement = np.sum(displacement_array, axis=0)
+ print(f"combined_displacement :{combined_displacement}")
+
+
# Calculate Angles
roll = math.atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2))
@@ -380,8 +408,8 @@ class Window:
left = -yaw * 90 / 2
- self.left_label.config(text="left : "+str(left))
- self.up_label.config(text="up is : "+str(up))
+ self.left_label.config(text="left : "+str(combined_displacement))
+ #self.up_label.config(text="up is : "+str(displacement))
@@ -581,6 +609,63 @@ class Window:
else:
return 0 # 点在垂直线上（可选）
+ def calculate_displacement(self,acceleration, dt):
+ velocity = np.zeros_like(acceleration)
+ velocity[1:] = np.cumsum(acceleration[:-1] * dt, axis=0)
+
+ # 计算位移（通过对速度进行积分）
+ displacement = np.zeros_like(velocity)
+ displacement[1:] = np.cumsum(velocity[:-1] * dt, axis=0)
+
+ return displacement[1:] # 返回位移结果，去掉初始化的零位移
+
+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
+
class WelcomeWindow:
def __init__(self, root):
self.root = root
--
GitLab