From 62cae27c54ff6530d8f63d65429a513d69a79e37 Mon Sep 17 00:00:00 2001 From: MobKBK <15059009+mobkbk@user.noreply.gitee.com> Date: Tue, 26 May 2026 03:37:33 +0800 Subject: [PATCH] fast imu --- __pycache__/main_odometry.cpython-310.pyc | Bin 7744 -> 7772 bytes .../trajectory_tracker.cpython-310.pyc | Bin 5600 -> 5600 bytes __pycache__/visualize_3d.cpython-310.pyc | Bin 3713 -> 3508 bytes main_odometry.py | 3 +- trajectory_tracker.py | 6 +- visualize_3d.py | 58 +++++++++--------- 6 files changed, 35 insertions(+), 32 deletions(-) diff --git a/__pycache__/main_odometry.cpython-310.pyc b/__pycache__/main_odometry.cpython-310.pyc index 4b90fb6b3cc991254839bc36ba1fe28a1a6ee968..c3bd3ff9548cc9147dec9bf61c94298f396da87c 100644 GIT binary patch delta 1966 zcmaKsZ){Ul6u{qoeeK%ry|(N6zv~!muyrt)LEOZ_5HKVnf*>Fjcs}}`rCZl__q}a& z-e*yWCTa|FO^D*m8vQaqXv`90q96QbG$tl|GA|~C5QBU%(fCP8@SGOs#KbQ9y>rez z_ndR@x%YlG_T!jRQ53%bzdavDCN6)fyzlDIDOIgjs%W};re+yt&YP!sl^XfkmXy~@ z`6Yoy_%)YBV$1ua2V5k^Yi%>@bN=Z$)zqe{C79)(KDSD)t;oF7TWgO%LfS->U-2Fz z!~7R-hR9s@UEUK+3c7Mt(gVz^hwjN8%%{sMcAx%_k8i~%yr5hlM6f)1C%@r~*8iOU z-cl9=OqvKzgc{@$p}VaP)^>r|3R844Bhc3^c`>X<8Zc{m25ix!URgPlAPcdMf~!I7WK$2bP8KP+4uK)X+9464-?9K} zPY8N4E!b(6*5yfJehuH$q(r~b({{QnB3Zz0XRgI4OR^M;vKWiA1WPyC>@Mb^zw
(bP>%t`$0$4UIKyCvlnI94iEV!^&CxkMY6OIe4Z86z^afam=nY*Qdcb17)g#{6P zVNuD(2sr8F$iFAN^gk28e@@5_@=p0uXcHc~A;f0H7QQTR_Z41^~K zJ{aMH%Kp@?J4?S_J8u} za$MDl>A1?3xz>GZT`F*MymY3lmWs1R0W|YHfJgKYNtB300>lshF;9r_+rd8K=W=N8 z+8?CzfsGL&-WAV#T^`~E51DxHv?M zBRbqW1|xrvP0f9#J3*NbWP5rFHCj>66g6|l0DYWakG?&KTbd<~+n6=V))L8is@jZc z==a_X7<4?9vN~zhe&P$Uony@&?E*7>6tNqz2QdyPy!#IRmd5E`G(CpM0OsANdUR_C zzaEQ{9egN0GPED8*>3s-dK>`c{CkUe>#(8e2BmAzigi&1(Zk=0$NPFwY&908R$o!_ zA`T;t^6%owtw&IN8gUHZxQbP=WKR>5$^8w9lcthR7py0OZoo(P>myPR@UtgQEuGdBj-+9vhl&o~J5`QGnye zLq&~hNvnOyZzVn<_2nzcFd?V-N2yVAn%_)iGcTi~WYww$y?_%h0_HtKL#8%s<9SK9+e-orBtyoSFxCO%yM253363<;(MDm}zdJn#dvAwE_eyW;<+l?6$fGl}3 z+vT*Ws$R*fs^cOb8x-*t-x3k+j z(=FTFu_QziCB}wpG(t*R6Nx;iF?}+|XpH!vFNQZK8e=puCI;e*4{Czv%t}E_oK1fB zf6hJk-ZSUi*>87#x>GL8vLwLs-KE4t#|QEyf3GW7v|6dG>FQX`HY_((paqp0g=$?D zv{K=UKox$?ZBWC`Wv}F?@a`oDp>SqNqn%NRR!a<1DNdpLzaB}|l zxi4*bDayo&*hGApED$Jk`bIJjERhJ-0#GkxYGny(zBYK z7AFB#3M|n{Iw?nS(suJw3yZNhYZ~`26DQ-@&8&qbJiC=eV0SV=ud*nMqy=5c3Qm?~ z^~OnJeFkHCQly{j87FJ{kSyv%n13n3-~QP4Z#Q#JAwRxfih&v5kM+cz=L~ zL7$hm^C5YE6WxImZHOGAoxdgbMt=V5%5T>Wj2+xXTlrTqh5I@j?Mrn*fE%<7TGUEz z@US=1LC&HNMKZXy2eFoa9X%a<8s#y5G`8_%Kgx}WO^5-+X2d;+yE8=JYBsJR$nbxgk1Zs-_dj88W%nM zk8rkxm_n2hCc*6qaLH(T?$ zmvD(_*D3~m86-Uim<#szTUym{`JU_mImfHnP2_ogIs1M71&o#|8np~nqvO_SuznCd zi)(d61!3^_+EVMWBYB@2^x~;2;)=U(wm78~O@&`?yH9x?!rhRnqDNKpK8lx_X8CYV z>GIkYMH`z>U&YXXIWtwM)mwa&S92W!?3&9jK*CS?=x?2-x@v=Iv&bMM7;SoMt;1s diff --git a/__pycache__/trajectory_tracker.cpython-310.pyc b/__pycache__/trajectory_tracker.cpython-310.pyc index 1484ab07a6ddcafe35c96eda1b538a4d6f7843c4..db840aa394b2638d29852356143dcc94c7120d5c 100644 GIT binary patch delta 1017 zcmZva%TE(g6vmx)rnIFkSV}@cUM(O4z9Nay!~|A82_b}q5=}E{hf*`0JDeH7)&%|o z7v7Dy)D0`BrF=3$dJF7H?$T%bcw90Tk)-?=h(i;V6%U3>GiFMejd@Sh;IMYE9tP9@{4Y+=zJ-Eqg(Y?LVdl;?$T7U v4Y49G_l^$~r8G6QgPEa4tTo(?rq`kjulAN^9wHS}IPDEVn~d}n4%`KQDO`WooMP<2B?|N9nK73YXbkk zh1UgJ-QmK-u+fb_!i9@jn7T4-OkA+*!u#GaQUa6o>6tU+Bsj|)GWZ_5XHytViKe7pbr+5Vf@dSn8P09y+TD~CV?qn8aOXtM*L)1(`Bk*K(Gr4=+*Ma>Oxf2wmm|QE|>dty`uOYLTjD(5?hX zX%mRXfQ!I5P!P~_bV>Xf>S}|Sp_$dHWuFkjS#~Iv!IB>SI{Tz^Z2O1VACa9a=%j^W zZ=ep^0%eK1@uAzu$i5ZNth=`58EI_x|1G(`)e*n!h#MVo^C%9RNxg8>Q2E2$)gI}o z98WkSzMBi~VML?|Eb58yc+!J$q@^V=R2mU_CC!23dy4d>bLjkQU*q4MGl|SbxNIH` zI=6k4zcxICBTZKlyxL!wyNyyz;neGf)hC+s&;2uh0l7@v Az5oCK diff --git a/__pycache__/visualize_3d.cpython-310.pyc b/__pycache__/visualize_3d.cpython-310.pyc index a4741a4118c955fd2532cc886c5d9fc7b8602a62..f34ddc235e19c20ddc3ef990204771cbfe67ffd9 100644 GIT binary patch delta 1770 zcmZWpO>7%Q6rP{`^Tu`(Cruj11&5!uTj-C}LTUL4s8EDJkq}&1Dw(Zk5S%^H}l@S znR)ZQIlb-Lc%qU>=m<)9dAKyWnAnDQEj~0p@;qXcNpB-2oh7Ei$ScT@YdpLZf#^@hD^e+ zNm+mJGTtSDuYuqL{A?pj5z0f`C=y5sXh)#Ae*yT!M>)hWAofU!GQwz6j3ghGq>@}x zTB?uR7^7&cF^S1cX^K@bn2DA20uq&}Ok**oHzbzh`w{HN?=ah71ob!D9O*J)&s zU|3Y2M*RRAWGOb(TywmxTh|VQZp+=eNIL?$QK2h<*Id#^t^PK`$RnB30NcvOns9oL zv<6vP+=Goncrvw1!5?IAFh2_Z|R6QE-wB;1tf#43%hxWN-#6|7c2_Ch@nIbjlo98{fug>%Wvfu6H(*>yqd{98#Uxl#A8&{pk)cJ@{2K%h`S>iZ z6CbEOi?}XwM9{T_XCkOZF9CZId7f+SoG`GDpcSd1klh!3J)5+Z%E2jQ?a;JF4%la(2GqpMuhPo08q!O zd6k@-w;bEhxp*KmF;jHC3ZKteoLloDE(2#gyBey+GArh7*Hm3=uIkt<#3w=w1}9C) zv0QJ{%Z5s=$}G=53O?Nj05nCz64t3cj+0o&5;;JUB#t$j27V=hPbnzdR35$Wj+iB{9XZB0TTd*7Tufg7882_4)MK!rfx+^RpHDv70BE|v2215 zW=b=bGrj4y+<+y8U^xC<(1{<|B{C?K^G?OJ`4GtbegT`PzhBG+M-ton#Q)62$85v` zFR`bQ5#vz-`veHzJhHy+A(EmD5>MX`%}oMm2trJ>32jK5i1%@!GgN*MI#Vv!=Bo2+ zAsU>sysA_2oZ|64)p;SA%39Ci=#QXi;T`AQ*e%qan5(dwV?QO5-4%b9gfu;lQ}kbi CxyXe8 delta 1938 zcmZuyZEO@p7@pbLz3$!Z_2ak#ZLhcx`BxeAFuB0{VYX~4hgFQbV+_J=X1ckTW4zv4UF1EDaRdFP$?oq6V+ z_hWXl<4|WP7YYRswBT4It&D{_@rJQyI+jsPiRsHU#2PT#iKs}W!-z^FLQJN@dr0#Y zE{*NkJNC!hVGX;X9ffS7C!5OXb}E-mjJ33d-vmGd#V|!Nf#MjU3!L3}b%Zt@MlnBa zrY$fAXc&+p&SCtjuoO8zW`rPij- z&R)N)c6Vni^{-<`)$X;g4+hkFbB>TVux7U8#!Z6I7 zCh^&KxY)%>7k}X5eJ+me(1@(91VS+cN+@gvw({|nnenO0 ztuHI*FV5Z=QCCghDpd}ho1U7QzH@5&_VF&Ydg?~CbbV(0X65qf+Q=2PkO!X`YVGPx z*sJw=2WtZ3M!1sa)OofHvrf?Mgl=(5L}9RP7t_;5!p^aQ-6`XM!PeaOXM(S|-y~po z_w4>07#kJw2&Q;HVqWbQp{l6xkw8hG&<48t3_mgx(|k&ZK!$`NG9*IQ;<5mxNhSoZ z{BW5R#d;}O+-SGHw|W57;=QT-t?Px_ZQYyIzb@=*19`B>u@ zVow9nd=OG2%i5i9{Y&?DfW!^P^@5#?TM6AXG-=L-q5hP`9TV3X(+6C9(8WLp=lRt2 z&4uADfU%l^C_WLd7aECz+esLQiIWffKD-eW)$;cczPTXIj$VV9L71zjCMy>{t(He; z?@Z2~7^$3}>{6$HxKKTDXy&V9)vw2=?;cN{`;51rwj==r+Miok7Yx0c-xacXH<*Cu z5%M4l-6a|QAXJK-2QHOmi(wsGfzbPit>o+~Ae!u5k3G)?>p1BK5)0_28BgSLj9RV_ z3Okv~at!K<{kqvV@6zl!*rGfC24C}1+Di)sL^dj8{xa3mIt-%P*{RCJ#p>C+Gna4A zTsu?wXe(%-Qu}iU*lJ+s!*2qa3EfLmMHp;-EKT{=1eB7qLFrlTp-BldXBlh} zhkFv{W>Dmv31vl)7ars3tOcE4l{Lf5_&mx9FAuh!6MjL)&0u`rL-whB>J>HtjtAW0 z*OZZn1A?r|5m}TM%0WeBT(9}rHW*wfTgc=G*bAI*fnQqQ%-LpY-#~uN(XUz30xue3CJ){7NQA3#PG|KQ9)4c10uy6s$l7{s%5w0?hyb diff --git a/main_odometry.py b/main_odometry.py index 9a0069a..1347a4f 100644 --- a/main_odometry.py +++ b/main_odometry.py @@ -117,7 +117,7 @@ def run_live(port, baud, save_csv=None): 'pos_x', 'pos_y', 'pos_z']) print(f"打开串口 {port} @ {baud} baud ...") - ser = serial.Serial(port, baud, timeout=1) + ser = serial.Serial(port, baud, timeout=0.01) # 短超时, 避免 read(1) 阻塞 # 静止标定 accel_bias, gyro_bias = calibrate(ser) @@ -135,6 +135,7 @@ def run_live(port, baud, save_csv=None): while plt.fignum_exists(viewer.fig.number): frame = read_frame(ser) if frame is None: + time.sleep(0.001) # 无数据时短暂休眠, 避免忙等 continue ts = frame['timestamp_ms'] diff --git a/trajectory_tracker.py b/trajectory_tracker.py index f06e628..8869255 100644 --- a/trajectory_tracker.py +++ b/trajectory_tracker.py @@ -50,9 +50,9 @@ class Tracker: 使用 EKF 四元数进行姿态旋转, 标定加速度计偏置, ZUPT 抑制静止漂移。 """ - def __init__(self, zupt_threshold_accel=0.10, zupt_threshold_gyro=0.03, - zupt_frames=8, deadzone_threshold=0.02, - var_window_size=20, zupt_var_threshold=0.002): + def __init__(self, zupt_threshold_accel=0.20, zupt_threshold_gyro=0.05, + zupt_frames=15, deadzone_threshold=0.03, + var_window_size=30, zupt_var_threshold=0.005): """ Args: zupt_threshold_accel: ZUPT ‖a_linear‖ 阈值 (m/s^2) diff --git a/visualize_3d.py b/visualize_3d.py index be45e10..868072e 100644 --- a/visualize_3d.py +++ b/visualize_3d.py @@ -2,7 +2,7 @@ IMU 3D 轨迹实时可视化 matplotlib 3D 窗口, 30Hz 刷新, 显示: - - 蓝色轨迹线 + - 蓝色轨迹线 (自动降采样, 最多 800 点) - 当前点红点 - 原点坐标系指示 - 等比例坐标轴 @@ -10,19 +10,19 @@ matplotlib 3D 窗口, 30Hz 刷新, 显示: import numpy as np import matplotlib.pyplot as plt -from matplotlib.animation import FuncAnimation class TrajectoryViewer: """3D 轨迹实时显示窗口""" - def __init__(self, title="IMU 3D Odometry", refresh_interval=33): + def __init__(self, title="IMU 3D Odometry", max_trail_points=800): """ Args: - title: 窗口标题 - refresh_interval: 刷新间隔 (ms), 默认 33ms ≈ 30Hz + title: 窗口标题 + max_trail_points: 轨迹线最多显示点数 (降采样, 避免渲染卡顿) """ - self.refresh_interval = refresh_interval + self.max_trail_points = max_trail_points + self._full_count = 0 self.fig = plt.figure(figsize=(8, 7)) self.fig.canvas.manager.set_window_title(title) @@ -44,9 +44,9 @@ class TrajectoryViewer: self._setup_axes() - # 动画 - self.anim = FuncAnimation(self.fig, self._animate, interval=self.refresh_interval, - cache_frame_data=False, blit=False) + # 显示非阻塞窗口 + plt.show(block=False) + plt.pause(0.1) def _setup_axes(self): """初始化坐标轴""" @@ -56,7 +56,6 @@ class TrajectoryViewer: 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]) self.ax.set_zlim([-1, 1]) @@ -68,34 +67,41 @@ class TrajectoryViewer: self.ax.grid(True) - def _animate(self, frame): - """动画帧回调 (不做任何事, 数据由外部 update 驱动)""" - pass # 通过 plt.pause 驱动, FuncAnimation 仅用于保持窗口响应 - def update(self, history_array): - """更新显示的轨迹数据 + """更新显示的轨迹数据 (自动降采样) Args: history_array: Nx3 numpy array, 位置历史 """ - if len(history_array) < 1: + n = len(history_array) + if n < 1: return - x, y, z = history_array[:, 0], history_array[:, 1], history_array[:, 2] + # 降采样: 超过 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] - # 更新轨迹线 self.traj_line.set_data(x, y) self.traj_line.set_3d_properties(z) - # 更新当前点 - self.current_point.set_data([x[-1]], [y[-1]]) - self.current_point.set_3d_properties([z[-1]]) + # 当前点 + last = history_array[-1] + self.current_point.set_data([last[0]], [last[1]]) + self.current_point.set_3d_properties([last[2]]) - # 自适应坐标轴范围 - self._auto_scale(x, y, z) + # 自适应坐标轴 (每 20 次完整更新做一次, 减少开销) + self._full_count += 1 + if self._full_count % 5 == 0: + self._auto_scale(x, y, z) 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 @@ -105,10 +111,6 @@ class TrajectoryViewer: self.ax.set_ylim([mid - half, mid + half]) self.ax.set_zlim([mid - half, mid + half]) - def show(self): - """阻塞显示窗口""" - plt.show() - def close(self): """关闭窗口""" plt.close(self.fig)