From e75155a3c864bf6cef6bddf8cde5f0348cf32f7f Mon Sep 17 00:00:00 2001 From: MobKBK <15059009+mobkbk@user.noreply.gitee.com> Date: Tue, 26 May 2026 04:19:57 +0800 Subject: [PATCH] update imu --- __pycache__/main_odometry.cpython-310.pyc | Bin 7772 -> 7788 bytes .../trajectory_tracker.cpython-310.pyc | Bin 5600 -> 5829 bytes __pycache__/visualize_3d.cpython-310.pyc | Bin 3508 -> 4046 bytes main_odometry.py | 4 +- trajectory_tracker.py | 22 ++++- visualize_3d.py | 88 ++++++++++-------- 6 files changed, 70 insertions(+), 44 deletions(-) diff --git a/__pycache__/main_odometry.cpython-310.pyc b/__pycache__/main_odometry.cpython-310.pyc index c3bd3ff9548cc9147dec9bf61c94298f396da87c..771f18b981e11afdb388b1bc56dd880194dc50ed 100644 GIT binary patch delta 590 zcmYk2&ubGw6vyXnvf5_5$u`-f`y-nowwQw8MWjOF$-h8_`lD$~*;scYNt|7zTUIE< zmR_XMckrZc5L6Ha*+0OW=t;c>!Fv%;o)oJy3&jq+5AV(U&YSPd*V^}*+>_-DVe8oo z)qVL^-k$&krzZCbe>T4Oa+D3zZq8LkFa&x&p3J16$hM+lm~u-jW<~-KCezT8W}%LC zsUhk^2$t}vbS!6#2#`=@M?0Yy8Td&mn#rCG7fEJePefz=e1( z>i~0fNm&FJ?<*GA@hhb&oKQA?`zrIHxYKTl*%PQ77JM;;0dxqjcE}LCkMXbkJk&pg zN~DE3S7%`)met6#=SINnS$FeYk&QC#`J^t`Vu{#=j=*g1GkqB^s}1w;aPW>Vk#06k zd}}BKW&Et_QiVUYE3$hk@lW-E0JFF~^SDH>v7a4J^n0G;JH0MlW+iRlY0aK%@pP5L z=4dmFQ;xk(DKB*n1E1tH;|5RHI2?|f94g0c{GmN!f3vNhgv~hC^8hRO-dKbt{xue+ z2TU_g`$4}=?{WDSo;7Q*g?G$Ta2=nU-xk)nBGYeCuib3XHSg*X+V0Olh@1rhYPn};fR7f;?qnZ6gj`r!TF_r3RfAOG+BR{vaAx{8t!*t+ms z_a44fj>jR3*@>^(&z++{8RTf%%X{h&A|Q+LL?#7=EKLuJH04bV0R)DWa8|CsIXo{n zq;dq|5myDqR0+rY|mr8>q3&xx(6`u%)VxRqFRgzl=E(`R;-#Fp#{GX4F?1Bg; zF6Lgu-*a2QtgfiFip%V`fH(1PRoJPronqVHh+nA-;>q03k3eU}Rj} zOK*UTZ}p-y9f_fdpL9dE`7AmTt7*r-^#c)R@St>97>)P3z8kpR4q0JF$tqqfJ98U6 zY;tLFahQx!u5*iA=c&QP#HYoQMcO>R%4LhoCKsJc50A@_*=!yer(idJXG{V#@S|CS zHOyPp!d=FU(m~j3lRn4y@RC)BJ-la~hIM>xeW~8$M5fmyetW%1HvQ!>((YYrhGduX k4K94+|yAJp9hJ6K=@yI?SCyxockLt|oA1&;l!2kdN diff --git a/__pycache__/trajectory_tracker.cpython-310.pyc b/__pycache__/trajectory_tracker.cpython-310.pyc index db840aa394b2638d29852356143dcc94c7120d5c..9f5abedaddae10dc4d28edf3500490e91467d690 100644 GIT binary patch delta 2026 zcmZuyOK%%h6u$S)!{eFp_z~xw)J@WsnzV%~rECyXLgle&g-Qj&fJTF7n%J>D=FTLo zqj4<}0V|NuY`bs}n|6UfVbK*k5M3g6vp`5J5E6d?sV$sy$7$WbSoiZc=X}18^SJ(P z^@r1zpUde4o}I7D%?k_PS`R4wBR1JzG>QLa6>7w@()%Omy*lI)pGZAtv zk^r-j45&v6pb@EnW=MwArF*E%jd9CGeSVz)7RKyFSAGxYD8ZP3QHC)I{;AO%)2{L+ zsjBMM7nG3LH|OI!nf!j3X;Gy5C- zr_f(GaM&A-AoO`JG3~Zw-9XEOKlz}KPUWO4c7Js9^HIOmDdOsT)krrilSziy$ zNZ+M>Bg{ocM4A@Zj8JaohlL2z)Hg@m0G=Ch4Y&o`;;C0`7<`0*(3dYWP)XOllY%TIbd^w<$=v_l71m7 zn8d&J$a5^Mnz5u_L90x(E~EII^kq zOHsyUu;1UK68{m4uvu|6SNoW~Ktg$V)Xj=gUFx8IsShRCtV9!$F-@XOnYj8q0Y>af z8V6uYpjZ|v@CzTltw5x}K3p3Q)`|$$F(SlArY;1cq)v>$W+dkZb>Q>ivLhoHzKBxgKWGz;v zx>S~QNu$tu(0nE>umYeA2tHY(>>rJ>2dXw=4=ssR?rWI>G_J`sNv0NjR5n9(TB0TK zv;9l?PuSo*o>ocoa{;(WM?$ai5<2p=TzaO!X?I&*P%|+g_Q9L};q@_oV~qF4_|9WK z3J_)o3!o;)S4!tki82`bTL4coRd-sA4>&}^zQ+Cb!4$XS?@HgljTOg5@l~_w)*fTZQW0f&pE!n z_nfbtpU(cWY6ZERLE!lQt5WOU%y-r!N*jsEextSIPgbFNG9$f1v&kK4C#ypm7ci2) zq8Hu>#p)oYF>%zG z0W+}#)M6Q^#|qGhRbVzEL+a6e%rwXBoQHQ=4Fb%M`E5_Sg>y{6D8MMfD0%W*q^fLv zLJ2VzA0Myq#=&~#Lz?`dElHEyOv-x2D#7woI}Ezbs2OyU8~V=!1$U$(G6)S&&7^YI zZ{Ox8(Q^uM8u1k344_un*$N|P+vQHQ&i(Fs(Dp_Z9F-ALQq6Iioo3`XFTz!K0brUY zwV29OdU%?WsZ&)_sY$1$vb179|hTh=fiLk;`pV>=6W4&wH3BF+vA`(&R|5<#t6_8wHaRoamGEfJse7{)xQ zZykv%JcPNXAZK8E%#%N2!xQ_YZ^b6KrZ-5i8{-3Jgk^y>yFvQ-IL|+biA-8$Pq{!C zampP@G_njCIOaB#z8z;g1`Bu)+x%+}WYL=UR8M`MT_lk_T<|hN43*A2L@ND>YnTwj zK2DH0QzD*LCSVa~ezY^NDv+@#vLG&Ec(nw32mj&j(FT?12M^72bnptZ=zu=yyCW zBsh@Ten&_tmBJuQC6H1oZ~AU$BsU?vDo*zdLa1*UtwqE+#1di!P%ZFh;U|?FJszZr zTd(`=<2I5qFRJB5*s<#Y=YBo%J#OJV6UpWLYH~L}H^>TQyY;BK?blS-3Aw-hq_owo z&~qdICfwP54KSx$tV9jTk_<_sAZp2^HnTz1G7uvgW&dc5J(5+Ms?21XBvT7cl}%HF z7H9!piXH1ZcMo2+2W;>>Oj=b&mA{DV!$PO@I=3;9PjjPPbluIc?RQfXckUcs8Sma1 zv+s`Cy)k>|(5@My$wcw7wKX}pP<+W24IckA_KRiuS#qy<|7}za7Xqs4qeU*FM|4DV z341JhGismc7mDB;Dq8hqJr($b_|cEax24Vts5WRK=WcH_!|OQL*g7ztTW@xwfcG4c z_aIrDJT*Iud1b^LqKKGBEF>RKo*z7i<|ROCb|W`x);IlVJ@EJy^uK|AE#yJy^QgzK zqVK;)3Jd>C_zVEG)B;9)XZ{85%zDcJ diff --git a/__pycache__/visualize_3d.cpython-310.pyc b/__pycache__/visualize_3d.cpython-310.pyc index f34ddc235e19c20ddc3ef990204771cbfe67ffd9..ea345d42badf5739f1661e14bd027993c0871128 100644 GIT binary patch literal 4046 zcmai1U2Ggz6~6b*@2-DhJ5J&xE({`2FL9zst|$rx7t&JtfRREe&B$mp-aB58GrP07 zGaGwXYk^3b5H)VnHq8$tbX|}jP#PKvO;WjeKte)-C*I}>m8{n*@qolzL4lxtII!Iy?eX<_K#LB{kDGY#_CT`)z4l` zr}K7L^!(6sr*Kl^=a=f|emrW79r$X+sDJ;xm5aX`HC8Tv(D=!ngK5LqXRN+IkFpiyQU#ZZ@x+Z@zvv6ECeVzg9o9 z2$LFj-UMpwHCC=(tDk>mI3%QGuCF5LUzrG&Y<{BF7l&n6RDq#Mh+9G`+whnN&(2{JS#-WSV` znle|odXkt5@8>#CounYiH1C2Djd$}NsCC}Uw?LiZ8c28`>Vx;rI@!<{<>y^z&Jm^E zu=yr&Cjv}l`_Yz`KEBTVyc3FYmTkBrUJ=1?2)^J6N=Qg%DXMvqR*A)`WR}fJvvN(T z(j`hs+*k4PTv-IJrNM|ctLF&*xXLwr#-cijt5Y33cosgX#Y=ba;E5oT{uZyRgE!E@ z>+ayG9lRdCbpdaz&Gzy^{8d?8Z{yqfjzvAb{W`D39eOw~=I(6sYFm~FB``{?K9Ep9 z-^F(?f;>V}8{oqz8BRbwk4$7(QwLEtH6R&kYSV5ma4SyJhrY?iIE!bc6dexbsq z_T_*BpN3^T!;1fUbz!t)v zvplDe3ui20!;K@Fi|Eu>;1$Z0=Vy=ResBfL?eWakNGf`vsltOx!iglPO*Ly5j@yB$ z;lnz^w`q}C&9%yR$w1@CV45JFI(Aic`|T>YkL-VvOaevkGnz1 z_CO2P7~fwkN4jOX1vj*;0oWrjfb`NnIzWe|_(zASEDZx4raDt;H|wR9oo)3emc4&% zk1XnFtza899b4>WXaM3#4Q#e$pQOe(TW?ETT5ro-UT-VtcyjG|>ZZQd?pM%ONeyhA zYj4J{OL5zm>1mTDR! zrDCQs)N)MicN-(-KHxO(LuukeGm`iiRBiHHrb|G`B4!$)G@lqo+%9iuSCV!L=frA_ z^kll5Pg-SftyO`R*^AsMf8Mf#B3Nd$b;1vy%@;i<4_4;zr@;<{Dd&XCuTwFE7eZ(x z1l_OL4rD)k0giXmxNsX2R*acP?EZ1-qKPsW}kF=;_wP!?3sCatlmTve7>>=S8C zs;V(EC|7op1@!4M%+R>BljJDW6zkS(8D?u-jz=&e!1*OAp05&!5n7eutiq$PgIZ%4 z6I!d(ngnf}5B|A>qix{m9UQ&Qk;!a|)5i%*qDqRZ+adip%ESdYH;r$B7f66RkV%}1 zXr{fpGE+iC5G)zPAPRf@=F2LlXU1r<>1F8iM&n_Q?@rn5{XoLK6b4rcKf>t)?kmv(`3 z)TLgEy z6bvICrIZ}o#uZ2k)Y3J$OajFu)x>;qb0!MfZ=PPPzkjEG z6wR)zU&w;N7T}AdJfufbF)TvMEyPh!d>rNq1IZ_FRbr}QFAj_%*$2c-+n#4-eP8gv zR3Tr?xdjup;HErdd()e>l7S^9_<~*_eRL3hLv)A@L8|hw@YQw{4`eAA&En{V0XxKd;_#~1|!S!(%`wn~oI=?lh?zEskbc+|OE;7{-Md#05t27?}bEA@J-C_la*7M|`{s z-Z;R==|rY!rVpq4^-t;j-D!c`j3z!0RiqS3`C?g&LEG$a|NDh35mQ}LP3l+_@w$f{ X7wJzVPxvBcMFFB*CVIM?4oLq2-I!jL delta 1893 zcmZWqU5pb|6ux)!oO|Zn zd%iRC!}2?WkwQm@f?&88d$JReW06k%u3aOjcXVqUzb@HM5?e|L0Ac9l{8`x39x*KQjxZ?-63Y1 zUNnYSawO6=hDo%{`sO1<88Cg8rM#G zM=#c^GtCR9{^PncxjSLeWG0s=7BYFu99I|St}e{qZ9F?W->Q85)V;=~StxD&iAx_!qJ6T;|sgq(}Yn(}as@TmA;*EgJ8r z!eA$r!QI7a7jJd(b{95n5V{|o)Vkc*!wI^f#8fGlOQZ^=yya{brZ(!*o2ATK2Gv6a znn`E!iR5m>1UJ*LN=0AM1@n!|Ya)p#rlj7o}7N$MU*V2=+F6E50ktfb?((AoT zA=D^LGY%xog)w^~PCD?!bc%ze2ZMbeM3{cZ{Rx7FZ2T@Rb2jw#4n!5gZSuP?i+2cA zW&N1N1E|acVtK-Y&e!;|U}Ib8M`bb3*|;p)BJ?SD5PEfr>qmu!3o*a0;RX%0_vaKsh_yn?uj!8TFnBTQLo%k z*F5y?)SN%$waX@VkE;37&f*?5NojJ=I`#fsr7?HIJMx})^pjSE2%rb1iSlsVU2b&Lnx6fS%rj|O6CkraYmG$K^Nx% z;g(T!1DQO@qzqFJn8}^RoI#u|%Bq#m!#;B*5LuG3fE8ZB5v*W=dy0zy6l8t?cR5#; z70!?0&z}?EYp9IfA8=j1k-0uv+>rj6ClA+Oa;wqL>jNG0qu|ygG>?2d)%`1@GYP z=E>`=kJ+TgQ`Qo$n93DQgLZ>VS1>ZLNLb#K0iD7$8L}bde55Zrsd1G3S}6OcXhFw| zjK)|~&=E#7Mn;|C=+J7GhK%5rIC)6!mWRS2%ED*?ItEM^{mRnS&aUWkiDg3}<>*r9 Hc69J>&Z`Sp diff --git a/main_odometry.py b/main_odometry.py index 1347a4f..86bbcb7 100644 --- a/main_odometry.py +++ b/main_odometry.py @@ -166,7 +166,7 @@ def run_live(port, baud, save_csv=None): # 30Hz 刷新显示 now = time.time() if now - last_draw >= 0.033: - viewer.update(tracker.history_array) + viewer.update(tracker.history_array, tracker.R) plt.pause(0.001) last_draw = now @@ -258,7 +258,7 @@ def run_replay(csv_path): now = time.time() if now - last_draw >= 0.033: - viewer.update(tracker.history_array) + viewer.update(tracker.history_array, tracker.R) plt.pause(0.001) last_draw = now else: diff --git a/trajectory_tracker.py b/trajectory_tracker.py index 8869255..5905429 100644 --- a/trajectory_tracker.py +++ b/trajectory_tracker.py @@ -66,6 +66,7 @@ class Tracker: self.velocity = np.zeros(3) self.position_history = [self.position.copy()] + self._max_history = 800 self.zupt_threshold_accel = zupt_threshold_accel self.zupt_threshold_gyro = zupt_threshold_gyro @@ -83,8 +84,9 @@ class Tracker: self._linear_var_window = [] self._prev_accel_linear = np.zeros(3) - # 当前四元数 (用于外部查询) + # 当前四元数和旋转矩阵 (用于外部查询) self.qw, self.qx, self.qy, self.qz = 1.0, 0.0, 0.0, 0.0 + self.R = np.eye(3) def update(self, gyro, accel, qw, qx, qy, qz, dt): """处理一帧 IMU 数据 @@ -104,7 +106,7 @@ class Tracker: accel_corrected = accel - self.accel_bias # 1. 四元数 → 旋转矩阵 - R = quat_to_rotation(qw, qx, qy, qz) + self.R = R = quat_to_rotation(qw, qx, qy, qz) # 2. 机体加速度 → 世界加速度 → 重力补偿 a_world = rotate_accel(accel_corrected, R) @@ -113,9 +115,10 @@ class Tracker: # 3. 加速度死区 a_linear = apply_deadzone(a_linear, self.deadzone_threshold) - # 4. ZUPT 静止检测 + # 4. ZUPT 静止/纯旋转检测 gyro_norm = np.linalg.norm(gyro) linear_magnitude = np.linalg.norm(a_linear) + accel_mag = np.linalg.norm(accel_corrected) self._linear_var_window.append(a_linear.copy()) if len(self._linear_var_window) > self.var_window_size: @@ -125,13 +128,22 @@ class Tracker: if len(self._linear_var_window) >= self.var_window_size: linear_variance = np.var(self._linear_var_window, axis=0).mean() + # 静止: gyro 低 + a_linear 小 + 方差低 is_static = ( gyro_norm < self.zupt_threshold_gyro and linear_magnitude < self.zupt_threshold_accel and linear_variance < self._zupt_var_threshold ) - if is_static: + # 纯旋转: a_linear 小 + 方差低 + body accel 幅值 ≈ 纯重力 (无平移) + is_rotation_only = ( + not is_static + and linear_magnitude < self.zupt_threshold_accel + and linear_variance < self._zupt_var_threshold + and abs(accel_mag - 9.81) < 0.3 + ) + + if is_static or is_rotation_only: self._zupt_counter += 1 else: self._zupt_counter = 0 @@ -150,6 +162,8 @@ class Tracker: self.position = self.position + self.velocity * dt self.position_history.append(self.position.copy()) + if len(self.position_history) > self._max_history: + self.position_history = self.position_history[-self._max_history:] return self.position @staticmethod diff --git a/visualize_3d.py b/visualize_3d.py index 868072e..4258c0d 100644 --- a/visualize_3d.py +++ b/visualize_3d.py @@ -2,9 +2,9 @@ IMU 3D 轨迹实时可视化 matplotlib 3D 窗口, 30Hz 刷新, 显示: - - 蓝色轨迹线 (自动降采样, 最多 800 点) - - 当前点红点 - - 原点坐标系指示 + - 蓝色轨迹线 (最近 800 点, Tracker 端已限长) + - 当前点红点 + 朝向指示线 (RGB = body XYZ) + - 原点世界坐标系指示 - 等比例坐标轴 """ @@ -15,15 +15,7 @@ import matplotlib.pyplot as plt class TrajectoryViewer: """3D 轨迹实时显示窗口""" - def __init__(self, title="IMU 3D Odometry", max_trail_points=800): - """ - Args: - title: 窗口标题 - max_trail_points: 轨迹线最多显示点数 (降采样, 避免渲染卡顿) - """ - self.max_trail_points = max_trail_points - self._full_count = 0 - + def __init__(self, title="IMU 3D Odometry"): self.fig = plt.figure(figsize=(8, 7)) self.fig.canvas.manager.set_window_title(title) self.ax = self.fig.add_subplot(111, projection='3d') @@ -34,27 +26,28 @@ class TrajectoryViewer: # 当前点 (红色) self.current_point, = self.ax.plot([], [], [], 'ro', markersize=6, label='Current') - # 坐标系指示 (原点处) + # 朝向指示线 (body XYZ, 用 plot 可原地更新) + self.body_x_line, = self.ax.plot([], [], [], 'r-', linewidth=2.0, alpha=0.9) + self.body_y_line, = self.ax.plot([], [], [], 'g-', linewidth=2.0, alpha=0.9) + self.body_z_line, = self.ax.plot([], [], [], 'b-', linewidth=2.0, alpha=0.9) + + # 世界坐标系指示 (固定在原点) axis_len = 0.3 - self.origin_axes = [ - self.ax.quiver(0, 0, 0, axis_len, 0, 0, color='r', arrow_length_ratio=0.15, label='X'), - self.ax.quiver(0, 0, 0, 0, axis_len, 0, color='g', arrow_length_ratio=0.15, label='Y'), - self.ax.quiver(0, 0, 0, 0, 0, axis_len, color='b', arrow_length_ratio=0.15, label='Z'), - ] + self.ax.quiver(0, 0, 0, axis_len, 0, 0, color='r', arrow_length_ratio=0.15) + self.ax.quiver(0, 0, 0, 0, axis_len, 0, color='g', arrow_length_ratio=0.15) + self.ax.quiver(0, 0, 0, 0, 0, axis_len, color='b', arrow_length_ratio=0.15) self._setup_axes() + self._frame = 0 - # 显示非阻塞窗口 plt.show(block=False) plt.pause(0.1) def _setup_axes(self): - """初始化坐标轴""" self.ax.set_xlabel('X (front)') self.ax.set_ylabel('Y (left)') self.ax.set_zlabel('Z (up)') self.ax.set_title("IMU 3D Trajectory (Z-up)") - self.ax.legend(loc='upper left') self.ax.set_xlim([-1, 1]) self.ax.set_ylim([-1, 1]) @@ -67,25 +60,18 @@ class TrajectoryViewer: self.ax.grid(True) - def update(self, history_array): - """更新显示的轨迹数据 (自动降采样) + def update(self, history_array, rotation_matrix=None): + """更新显示 Args: - history_array: Nx3 numpy array, 位置历史 + history_array: Nx3 位置历史 (已由 Tracker 限长为 800) + rotation_matrix: 3x3 body→world 旋转矩阵 (None 则不显示朝向) """ n = len(history_array) if n < 1: return - # 降采样: 超过 max_trail_points 时取等间隔子集 - if n > self.max_trail_points: - step = n // self.max_trail_points - indices = np.arange(0, n, step) - sampled = history_array[indices] - else: - sampled = history_array - - x, y, z = sampled[:, 0], sampled[:, 1], sampled[:, 2] + x, y, z = history_array[:, 0], history_array[:, 1], history_array[:, 2] self.traj_line.set_data(x, y) self.traj_line.set_3d_properties(z) @@ -95,13 +81,40 @@ class TrajectoryViewer: self.current_point.set_data([last[0]], [last[1]]) self.current_point.set_3d_properties([last[2]]) - # 自适应坐标轴 (每 20 次完整更新做一次, 减少开销) - self._full_count += 1 - if self._full_count % 5 == 0: + # 朝向 (原地更新, 不重建对象) + if rotation_matrix is not None: + self._update_orientation(last, rotation_matrix) + else: + self.body_x_line.set_data([], []) + self.body_x_line.set_3d_properties([]) + self.body_y_line.set_data([], []) + self.body_y_line.set_3d_properties([]) + self.body_z_line.set_data([], []) + self.body_z_line.set_3d_properties([]) + + # 坐标轴范围 (每 5 帧更新一次) + self._frame += 1 + if self._frame % 8 == 0: self._auto_scale(x, y, z) + def _update_orientation(self, position, R): + """更新 body 系朝向指示线 (不重建对象) + + Args: + position: (3,) 当前位置 + R: 3x3 body→world 旋转矩阵 + """ + px, py, pz = position + length = 0.15 + + for line, col in [(self.body_x_line, 0), + (self.body_y_line, 1), + (self.body_z_line, 2)]: + d = R[:, col] * length + line.set_data([px, px + d[0]], [py, py + d[1]]) + line.set_3d_properties([pz, pz + d[2]]) + def _auto_scale(self, x, y, z): - """自适应等比例坐标轴""" all_coords = np.concatenate([x, y, z]) margin = max(np.ptp(all_coords) * 0.2, 0.5) mid = (all_coords.min() + all_coords.max()) / 2 @@ -112,5 +125,4 @@ class TrajectoryViewer: self.ax.set_zlim([mid - half, mid + half]) def close(self): - """关闭窗口""" plt.close(self.fig)