From 8531c4dc9ea4f0b278ccefce0d15506706253261 Mon Sep 17 00:00:00 2001 From: MobKBK <15059009+mobkbk@user.noreply.gitee.com> Date: Tue, 26 May 2026 03:28:37 +0800 Subject: [PATCH] first commit --- README_ODOMETRY.md | 148 +++++++++ __pycache__/imu_decode.cpython-310.pyc | Bin 0 -> 3409 bytes __pycache__/main_odometry.cpython-310.pyc | Bin 0 -> 7744 bytes __pycache__/test_3d_demo.cpython-310.pyc | Bin 0 -> 1591 bytes .../trajectory_tracker.cpython-310.pyc | Bin 0 -> 5600 bytes __pycache__/visualize_3d.cpython-310.pyc | Bin 0 -> 3713 bytes imu_decode.py | 137 ++++++++ main_odometry.py | 305 ++++++++++++++++++ test_3d_demo.py | 56 ++++ trajectory_tracker.py | 198 ++++++++++++ visualize_3d.py | 114 +++++++ 11 files changed, 958 insertions(+) create mode 100644 README_ODOMETRY.md create mode 100644 __pycache__/imu_decode.cpython-310.pyc create mode 100644 __pycache__/main_odometry.cpython-310.pyc create mode 100644 __pycache__/test_3d_demo.cpython-310.pyc create mode 100644 __pycache__/trajectory_tracker.cpython-310.pyc create mode 100644 __pycache__/visualize_3d.cpython-310.pyc create mode 100644 imu_decode.py create mode 100644 main_odometry.py create mode 100644 test_3d_demo.py create mode 100644 trajectory_tracker.py create mode 100644 visualize_3d.py diff --git a/README_ODOMETRY.md b/README_ODOMETRY.md new file mode 100644 index 0000000..887e01a --- /dev/null +++ b/README_ODOMETRY.md @@ -0,0 +1,148 @@ +# IMU 惯性三维里程计 + 3D 坐标显示 + +## 概述 + +基于 STM32 IMU 串口解码系统，在 PC 端实现 3D 轨迹重建和实时可视化。下位机 (STM32) 已完成 EKF 姿态估计，PC 端接收四元数姿态和滤波后加速度数据，进行偏置标定、重力补偿与双重积分，重建三维运动轨迹。 + +## 架构 + +``` +STM32 (200Hz) + └── 串口帧 (48 bytes): header + timestamp(uint32) + gyro[3] + accel[3] + quat[4] + CRC16 + │ +PC 端管线: + ├── imu_decode.py # 串口帧解析 (CRC 校验 + 解包 48 字节帧) + ├── trajectory_tracker.py # 核心算法 (四元数→旋转, 偏置标定, 重力补偿, 双重积分, ZUPT) + ├── visualize_3d.py # matplotlib 3D 动画窗口 + └── main_odometry.py # 主入口 (串联所有模块) +``` + +## 串口协议 (v2) + +| 偏移 | 大小 | 内容 | +|------|------|------| +| 0 | 2B | 帧头 0xAA 0x55 | +| 2 | 4B | 时间戳 uint32_t (ms, 自启动) | +| 6 | 12B | 滤波后 Gyro[3] (float32 × 3) | +| 18 | 12B | 滤波后 Accel[3] (float32 × 3) | +| 30 | 16B | 四元数 qw, qx, qy, qz (float32 × 4) | +| 46 | 2B | CRC16 (对前 46 字节) | + +**坐标系**: 右手系, X-前 Y-左 Z-上 (Z-up) + +## 算法管线 + +``` +串口帧 (已解析) + ├── timestamp ms ──→ dt = Δts / 1000 (精确时间步长) + ├── gyro[3] rad/s ──→ 减 gyro_bias → ZUPT 静止检测 + ├── accel[3] m/s^2 ──→ 减 accel_bias (EKF 四元数标定) + └── qw/qx/qy/qz ──→ 四元数 → 旋转矩阵 (scipy Rotation.from_quat) + │ + a_world = R @ (a_body - accel_bias) + a_linear = a_world - [0, 0, 9.81] + │ + 加速度死区 (|a_i| < 0.03 → 0) + ZUPT: ‖gyro‖ < 0.05 AND ‖a_linear‖ < 0.20 AND var(a_linear) < 0.005 + │ + 梯形积分 → 速度 → 位置 (使用时间戳真实 dt) + │ + matplotlib 3D 实时轨迹显示 +``` + +### 关键: 加速度计偏置标定 + +EKF 四元数与加速度计之间存在不一致时会导致积分漂移。启动时采集 200 帧静止数据，利用 EKF 四元数标定： + +``` +R = from_quat(q_mean) +gravity_body_expected = R^T @ [0, 0, 9.81] +accel_bias = mean(accel_measured) - gravity_body_expected +``` + +标定后 `R @ (accel - bias) ≈ [0, 0, 9.81]`，静止时 `a_linear ≈ 0`，ZUPT 正常触发。 + +## 依赖 + +```bash +pip install numpy matplotlib pyserial scipy +``` + +## 用法 + +### 实时模式 + +```bash +python main_odometry.py COM5 +python main_odometry.py COM5 921600 +python main_odometry.py COM5 --save traj.csv +``` + +### 回放模式 + +```bash +python main_odometry.py --replay traj.csv +``` + +回放时按 **空格键** 暂停/继续。 + +### 调试 + +```bash +python imu_decode.py COM5 # 仅解析帧并打印 +python test_3d_demo.py # 模拟数据 3D 演示(无需串口) +``` + +## 文件说明 + +### `imu_decode.py` +串口帧解析模块。48 字节帧 = 2B header + 44B payload + 2B CRC16。 + +- `parse_frame(payload)` → dict: timestamp_ms, gyro(3), accel(3), quat(4) +- `quat_to_euler(qw,qx,qy,qz)` → yaw, pitch, roll (deg) + +### `trajectory_tracker.py` +核心跟踪算法： + +- `quat_to_rotation(qw,qx,qy,qz)` — 四元数 → 旋转矩阵 +- `rotate_accel(accel_body, R)` — 机体→世界坐标 +- `gravity_compensate(a_world)` — 减重力 [0, 0, 9.81] +- `apply_deadzone(a)` — 加速度死区 +- `Tracker` — 位置/速度/姿态跟踪器 +- `Tracker.calibrate_from_samples()` — 利用四元数标定偏置 + +| ZUPT 参数 | 默认值 | +|-----------|--------| +| `zupt_threshold_accel` | 0.20 m/s^2 | +| `zupt_threshold_gyro` | 0.05 rad/s | +| `zupt_frames` | 15 帧 | +| `deadzone_threshold` | 0.03 m/s^2 | +| `var_window_size` | 30 帧 | +| `zupt_var_threshold` | 0.005 m^2/s^4 | + +### `visualize_3d.py` +matplotlib 3D 实时显示：蓝色轨迹线、红点当前位置、原点 RGB 坐标轴、自适应等比例坐标。 + +### `main_odometry.py` +运行入口：串口采集 + 3D 显示、CSV 保存、回放。dt 由时间戳差值计算。 + +## CSV 格式 + +```csv +timestamp_ms,gyro_x,gyro_y,gyro_z,accel_x,accel_y,accel_z,qw,qx,qy,qz,pos_x,pos_y,pos_z +12345,0.001,0.002,-0.001,0.75,-0.05,9.81,0.996,0.001,-0.002,-0.036,0.000,0.000,0.000 +... +``` + +## 验证结果 + +| 场景 | 结果 | +|------|------| +| 25s 静止 (偏置标定后) | 0.000m 漂移 | +| 2 m/s^2 × 0.5s 运动 | 0.250m 位移 (理论值) | + +## 调试 + +- 漂移：检查标定输出 `accel_bias` 是否合理 (通常 X/Y < 0.8, Z < 0.1) +- ZUPT 不触发：增大 `zupt_threshold_accel` 或减小 `zupt_var_threshold` +- 3D 卡顿：增大 `refresh_interval` diff --git a/__pycache__/imu_decode.cpython-310.pyc b/__pycache__/imu_decode.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..a634a87829b07e2dc65d5d0a22bf7eb6f578820a GIT binary patch literal 3409 zcmZuz>u(fC67Qbr-JN~;{RTn@lHi5K;T4iw;eZ3qfeuK*Lkfe@YP>x*i`TpBo*B%p zW~3wzNDv5+3yMw##ZHdnf)2Rcp%Wj_eYpSOzRbr(Sj) z)m>esWHLtZTj)Boqn(iNg24*H0Plfj^-)3u5wt+`Mo)Dbj1^s(B)V!cq0G^JwYQXE&H8X5RMu=d0h}>j`1=~$TXaw|%3`YmiA`^{>wM+KW=at5(r{iH}mo8I?a76sXdW`7`!_LDgZ@^*c| z>@@q#fjNbOH;x7^)w7H)JG@N5Ny{3*WdI~k7YNBAdiJRRLeTa?vo-<|)T1r|ueErJ zONFwV*uN8>3hOYv{r{muciIM+t+h}G?8aXp-8E4Hg z;Y?3gvbjoS%ra%(C}f41D+zNfKkYQ(P`ToVV2Z6fJHgz-pJj?h2PmU5fgAczY`de~K6Hley9H|$2B*W9QZ zbK?_mif=OTw(5KeQ!U;3x_0?I-?4+&?tQUz=_6h?Dut3EVlfWi5z}^L(aHn^WQcsu z=CgVGG@mjnR)(+q?KH>V=7ZZe+fKP)rqVq4-Ia_P$FTX}j)p>p&&;N0&ZTE6>6vOO zSfU!*bztk3i4>Equs5IKgnlTP#%E}pQv9%GODAXhVW(I&a#N{DWDZbzKD1# zG$x1_x<;$9C~=hq2p9w}?1rtunYAwD?0zDmQ_3|e2cJtLZba^KBY`Y7rOIK@1WTLQ zpN`toP-N+1860oZ|Ns+uv)xUuMI}6!KmHq@H%1C1|#NmdEH)**J~%- zF1Oq5aeL<&SoC@Q-hgQLR=M!PIaGe*Cf!gU5gn_Dw_0?%t3}s56WzB|&`5eA*nRs# z65ghs>(o_6?>xIsMIYd#=tu0XnkT>+0MsH@1=7`kTUX$=CSC*lcJab$RlZ0bgzxeyw;}5=n^tam8uX$DFdw4a<#|=mDp`js(nFTK&=GKy9J1H3d zaPMO6pXd33L&rI!g!+~HwMTdO;Aztkrp)k>5&qh1DKw4+m-*Q8U9ed`4#XJ%vUKlH zwdtu)8=8aCXCfIej6MbJ&PLGF_IB0NKlGQtlZ4ay0~We^lU z{_e<$gYS&&%N`s(>_=dd0H_7CC^0w57m(mi{qX+Lk$s~_f=8v9)6SIgMZf)bW@Wr& zNO7QOn^HPu8;X)b$ud)Gd{!<=+h=%Ud==LByWHR@)DAlk-UQB*B;VMC`cpO48#u$ zI}2~MuL%2>xt&cL`Qm<1vakl}p>6QAGKE2f#S{gSJEID1fxZoU6{QMID19`c#)4xT2rafUni#>^jG^GpXjy+ZW-7?ezGbumG z!?<}Zo5ek5vnfTs3JkdgNNSb*4Pp$T&+vXqeD?hiyoQsePxBIsMV~_Tnf1d)xoj3i z5M{X=_k@HnJixO+Y6|N2no1muAc1@QAT&g$mskbsfG=k*}QxoWO@F99pQ)19- ZDr*fQfmqSPT5Bw>4Qq5Y<>=`r{{wv~d?Ww> literal 0 HcmV?d00001 diff --git a/__pycache__/main_odometry.cpython-310.pyc b/__pycache__/main_odometry.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..4b90fb6b3cc991254839bc36ba1fe28a1a6ee968 GIT binary patch literal 7744 zcmai3YmgLGmd;mYRb@T;Nxx~*ch?BbOEi{MkpLXs!IlL_*Ak~J>LzV>*Q;-4(Nt~L zE(!=J2oZz$TC`dnK_ftSm=F}jjfs85{@j0?5&L6ysOs*BiI~_QyAewmy5G53-86pOB&RX!Um$kAsq%bV$c35 zsTC{i^d8G}czS#|WAC|%_=*+WDrVE=>#}ngwltCmc={t-Hg4Xs-HUA7`1IDtH*QXC z-7?|@H*Mb}m_UYSR^RJIifL|JsogxCvl6lwe1fMl6BY;C3U=JeI0asQa?F~tct)m{ zo4+7Z9nU5__In^<$(3rDx@oy8yRvOCb%*52OzV|c0Mol|;jQ8-e5tF5H~NIi2PZ{M zM|PBM3AI&6b=Cc{pP!KUO3{+aR7XCj^h#CD(P{w}bdhH&l}wAassTrLg09vr?U$>% ztGfZTy>(J@Law%7-XTrOiBM@D272}Kt5+}m8f!ND$>sX-Q&%tTt6w^^+?@UTWc}@T zX3xCec;`g@#H%xJ9WdEg#xWa*Pnjcyyk%e!OmlAEiTb(MXJ3A?{>kOWkt?WK|KK;~ zs=XUGf~;G&+-%gQ=RQ7aVwTb5nfL2Q51PevIa^4x-|u@-H1Wh7&Ep6sI`=V=pMzAC z`yWY!J&jvw#_1e!4a8HkR^HQXhnF&rr zn_^a`z^t9$-_4#uBlY)9FPgD$2T=lstl=M(4Y^fT=jgSdyTt8syWJkQ*X^6ucSwn7={8L8-03$P=iY174%N@UwceaNbZGAF zm&_3p`?~q&wND$z&SNW|pPo7Y`qeLMjRWu2-#lA?;rPr~=lvtGk&oJ>_WU1q&H8!X ztY7?fX{bx118sFB(@mld-a?U1Mo*T491y?ISunFx7-A|J~;;>37*x>&UG z%u{n?c~8j~(GYzBCq?Xa;Xse=;%SF8d0}B@6aAbTPevBacG9>$rC!XRmgYiuQ85kk zIb(8aeu+F|a_=I(yoj&N^T}|gkj=tiSd4ejpp?8$IfV!LN#rLh z$*?~Nd`>bX20)yS&i!<6Gc7mM3Z+7I`J{*X3NK8+I%Mq!LA0nQcgn2_DWH%Xf;tKz zC8ZRHa-zS{{702RIr;~qYd&wF9={Jw+@PEWQP74*C?^GK%aQg&JvlNcfJ_x226ji` z%N$rZrZAOh$5dD)wWhgrNMVy|fl*k)_heE7j?Q#f!hsJ8B^Ejzpd;@D$3rgJ%`h`i zJLrVn;C^|hQVl_Eg>%5R1aP#@^Og9DH~Y-_qn;q4KWFK=CZdgxS1I z+v%9-S!eB(vS`)7mqeUsEjle#qm}=y{F@W2#a(T@WxRD-J|Xi5oHiO26lRRcP^~Xb zmcVtuHph{63Ka?kT5UsaiOBg1G{(AwLS0hrs&+$FwbwdGow{wLK%uZG-|lv}og&}o zM$o#<{|QPr(j&3%eyQ5;_Os}OZ2uN%&xFSRk@Z&l^9oTnUX4IScanlzPeo9ItO84SK)X%@uc>S}++3EV1$Jd+R!&km? zu7Cbn7 z&9vN1D_+o77<=cL@;p=FYcU*t4-wJ>oJN-n6bp80ub-6tq*99Ejc=QL!(>)e^C7bm zHVK~#%}qL+Mn^*`l$=r@)ewfNe*S3V@QL-NKMb-d4-5%Ke@z{}XZJM)rhw2{r88mX zR@%Q@U)IUH1pP*^!t3n%wKn&|LN~4&W4$-_Z`{zT)nno=9`cH``;hi zwQU2ZfnSrK@zhoKvP$4t)3G0@XeM(Wn45(b!aCZ_3uA!OI?z9uJ1ov3B_ zLjZEEAN>2qtG{ZT{LK6xGB{-3P+B5A@2@6IJxpT#+#9opE|`g4VU{>qLQgBeZh0!) z7*C(#V~zzi@27<+ehW$HKGgK|9RhL+)$1A8XV(ia%%7*xa(HSn>v*xT<+3a<*2I5x;zAuxqpEsv*$it9Qi5eBAR@p~VU5cjm$?dZKT}X=yZvZu6b_Z{0G5#OgzW7f% zW#v0%UkE*h&>H-NZ+y;>c@e|A!RcIv0s=JHwvzoE93=P#vIF;#0XrY>t}2cUlua%u zT*%GRv%#w7$~D#1NJ&q&JOy7tlQe0+Dogp$YRSH33gz^F8jqvF#S8$2lTV@fY7~kx z;FmGIPuj0kVZcWvMIjI52A>4xxoW^@23?qO^t>GgL3W|sF#^4&k`qe41>9QV;8!#` z7@hrc(|-sy<&1^5kp&JdEy#tMaxp3s<|o`Ni7%8G%@UZArc9(+(mG!QwT`!q14ytzcw34roscW>%Ktf?wI$+B7XXaHBW{PksuL3B4nxZR z7pxaS7>&MMtkO-_0e;xL+)dXNUK*{qSS!JaH4>ay8^MV+5xy;8QMI?)2e6>+!KnC@ z(~a88XwCA7r3nC}ln8{Ka@*KE*YCCgqL=_k1Ar7A6L@d`65K5l8YEw1w^j!Ll0X4I z{5tQt{eWjJwVnkSM6hXKItVz>!MXrsI-0Xg0H__U-zEUiKfA4{(=y&m`wp|($8Hxp zaA~zY9}xLFM81Os0D?LM2>J%{{Z)9d>`t_O7xu}}!Z}M|ls^F+x*Mg--9fQSR)8M{ zPdjOzr(HsPZxasv_YXg@4Pr6ZCfAC`EGDcHb%phiN76;DDNxh9(>iedQ;3wGx6nVHMy z5{jozSY?3^0Kx*+9xLyBBqw!YvDqSvrWf)lNM5Dh32zv%XPvV4ZKaP)ogW+>zCT-l zgJ?fE>sF5mi%eGUZWglccjOzTSL>?jXD3QMa8TC=tM;rLhsAww@6NJDNU=)9h z3Py-LPGlRA%^r}5FOLFU)jv2ebK)hFZ^zrjAm2f4 zJOPq4HX{nT9kDhb6GDt)N;tt+ejA7v;OYFRg|HuDjE)bx?xdHu6Cv<(9e@Zt#*g(=&UcY8X;2!WDR6eMH9MW2rr@VlrgZI`4*aRyLC7(fwQ07X>6lgAbf zo$v~Op2#?n2@s5p?d5wZdmSFHrRo`}FjQ^bk*fpLAaTt=b&A&3S{ zV*r&zflvm%?F_W56c_+P0Wul_x}iUI8W4~vhrS(F7o#OzASfUipeB^#A5~g)9f1Zy zH&p)~xlg6ms0Bhz6glmpKm~9T^-HCO631@76*aEou)*H}HQh>MQucMKoeF;mL!~8H z_|ah`jYCJ?{s$088kZTY$1%A|hf{-^3K-eMBEQ`>M35y>C zS`soX$hXetYZ@#t;;-WYexIXLDy%b(HTWQZ7g%m4c11j(4ogQK!h}}6ZOLl zNIQhxkFw5Yx}=$Q&9|879ucSJNAF^wAtKDOGkmNlwk5+yd-4Y1!}#u9{rE-b$PZr> zmoSNh>IL#?#4Wubvv#LTa3-|ptt=D*Dvn9Nl_k+%ek@gvlQ=9G&Te;F))1*D?;_cosP@S|I|2#2xh6A6d6e)!Wxb^{tNh^wLW z9=IZ+a%lc4$FF$#)kVU-zVJP*gv@uNGZAJq;89<_^2++k;LWd{DtFxcz4>QrR^Pj7 zRb}{3h<)!~s&e;_%g=w!!o^e~=7lil;=aj?BWhgAL8ot{>nH9-(~Q9>^UL_((BvBa z%@i$D5{iT_xx`-Qw7SAK5X4mKjZEMuT_Z=aPE;PDfyX;kv zrUR$IU5Vn70#3^fRL6nT zDO}BzZC~Za@lvRCew-=?;HehNzM6iL3fgjMrPLv9d&cY%g2WXowU+sYfDDoFwf)H?J+km!d0Ypk zQi~={=u4q0L-*$jtdzAL#N_z~Uz0*%e`ZL1(fkZ3QP7ZT3>a%dPlR@cdZEz1`+s}0 BrrH1i literal 0 HcmV?d00001 diff --git a/__pycache__/test_3d_demo.cpython-310.pyc b/__pycache__/test_3d_demo.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..27054b1977ccb5fed50af0b8fb967f382e72dc64 GIT binary patch literal 1591 zcmZ8h-H#Jh6u);q+G#t}?UuS=e4xfeZPa316B1)w3>ZTMqJkpLCgW`H-8##BlsmVS zS!a?(J`xn8MvNv9Xu*fYh#H?(BksRo^zA;WY|A!2`D%=KX1d_UJGp0m=i}bF=bYai z-!SqBM(Ijr;dBO}TZyuHNTBS38O(tosDTJ3l7kyk`j#7V`c_=Ek->>icCxP4&{C{8 zIXB8^M0)Kn)A+Ri9hj_C3JaL(FO(r3_#R-~w%OpM|g6v`bktS#Wnu z0?5JK4KrB62=TN6@8cM8$d)L8Fi~TPWMY|QV}(mSsgGin%S1bcF^Id<*nkfHPrK{sqFUv^=qYFhbgr=2X>)U) zSuPDa+rhSX|7%sC&K~OeDP``FV9Xs@qH>!%wDor7s~3M;rCen&=dOo zZ~^ECJX`OdUt76Ruh*-?^XIC&IddM{T^*dfG`w+jW6@8mt?6|9GBZl9i;x>f-e5o# z)Y^XLk5^VMKeTHkgEmS{`*xmt{>?#eruFyrKY!jo_wr09q1SlBTTAcMwz4FaDIkQx zZI=qQ&1{bgx$SUa%-b!mTNpo$C_<|q@A*A{Zq2Wwz^EImnnp7{{maloA= zw8bb5MAiz-V;zeN)nd#FMGkb=;>NagGRM<2>_rNxrfj-*C=c$7X{Ht1BzB}ne%AE~u;sag Ws=*Wp1wk1g)$yar+7poUoqqutHsf*t literal 0 HcmV?d00001 diff --git a/__pycache__/trajectory_tracker.cpython-310.pyc b/__pycache__/trajectory_tracker.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..1484ab07a6ddcafe35c96eda1b538a4d6f7843c4 GIT binary patch literal 5600 zcmai2>vI#=72jR0o|a`{%p)Pd=F!$_3?!kQwr)~LG6~Ie!n6+QOg1FjMY}dKvLxS? zz;f3eCpIxjAmjldgaS5617wOxCof}%@rV2e{kSvzP-02e^h^7-)7S6ZE6KKSrq{kl z=ia^d?z!il^SkG4wzV}Pp+u%S`ro@llD?tF@}i=#8`aT4EJ>Fv*;3N7uHe^~_6?}I zDoeI9>F<*)^^!a#>jBGe1;!;kXa%hhenVE+ir_bF#jU8-Ixg#xJ(3l(+QucTZA{Ty zEae#~v8JHQ5aX$*pVi7^*UO`q7iM0q%zaj!I9|E(`r`DIc5C#MR-T+G&5u>4&y+ts zUA{6^I)5e-sZ38*X6L&jnpQe@u5@&)eEJjZ)+=voB>kt|DF+9O(zwp;5ojRRS$sBP2CWYSI>2U4aZMx|33 z+hiD4pRi4P{(&wH|3BIO;|E_L2~HlbPMoM*zFs=^iq=`abiH)*Qmu8rra$}3eOmSW z+48kZDNjz6rr&Rj zg*HkrovhrP?$Q?K&o5qoiOZeLGG-_9wq;S>U-gv(U&7t9G|o|A$y^^ z5)!@&^-*CykiM-)|Mshy3-|6$(wWx_@d4DwQJwW5d1+F1Wmj>1uGFVa$&<>Ig!7XN zXYg?45s)5n6hgbp&m#g|Y1*T#&!KY@W$;LsHaLV@L>=zdp4s>G&WDzCKu2h%O}4E! z#T*E&hqbP7jC}O zumf)93p4LlP9HCwe-lPh`E)L!cuFSc1x?3f%q%7pJyJ7coP{U%@Qn!)#)@8)i)Ra) z*@7M}VoYHqL=qm^D8!SPTtrfj3scxAglk8$G;y-@#vIJQ^xC=7n{QPoPQuuNdwL%I z^;7$vBev!>%@VZtG4pUL56xx=a(2cs^EU03%Mnu88t|2wE!;GB-KH}?o^pZi15?)+ zS>yU+n)SKc3o(?V+aRrTB# z+#?byYr$X^1@Y8m7RTx{4>-`VY*(OBZ1trZt?+*qxZ|kPF3> zc3G9#2Czxg6~%|vZdB*zAhx8-fLcJT?hE_E{&3J*YXL?9ovIbFS^%bgJHWwI8j}IE z98C3)O%S_gocbbGJ2uIP-Do>=ogQtHI9V{JF)c z;jt90y(8pwV1a^>s#8)QJ@Ese zBqIQjl8hh$l@W4P(6H+Vjkp2OmSwY}%lKA{X2tqoE=Jq3{;+F_~C zvcK>aU0njknqiH4hYLACLET)0uXT(2;MVB7rXemCu$r(%t#e?9^WsBv>nxR~Mf!>? z+pRSgq2}@hm}%|c*H+U4z04f29Zam3#rX@BxohIYN;8*ndkPs=yt{77OPin-8bBl* zM_gg6%6`~n#*tLU${sPC)UaK9efixRr7x!|AD=0m{GjyVWq7032@zJ=HqCG7k7f&F zPU`& z#0%5HJT~!K#n@!FkjdN3Yp(RIVfwHRR&BS%XFJ=L+(z3g7< zAxLH4A@W@!-viOxZx^c>{X&M3%B1p!@k2~-=t_x3qjF3MC;_?t7xhKufG;3N<&D1K zcnzYq*C!?k9O{M8q5%LGaGG#u6>c2pRk=Hon{qI~uP?9m`&}g;u;dfUh{9X4XaOFA z8>JD7TcrLFxKKV^UujGo3Fjkj$PJGvBawWI8yS;FTJll11!G&$*MgN|lWi_wXCzwV zTfn#Cd3+cMccgMBg{rvelOQC0$ZzQ(jxu4~*)_NLaDVBOd zncRSTE9OSgv+AJP%!034svrNn~F^gmA zidoH}=89l@wLp{PqhjUpt}|a$&wN(C_J``}d5leTX_Zrds&yXw6o7_c0uefrl*T&| z{rIhXF=ul$;^=c#_Ed8y%Vl$(518Sp1GbqF-p_G> z;um&55e^1-6StMfHX<64dq5KHY&(8*l@3z(^OIR^AD&|6^)?ZdHgXo$O3TD)eyDyQ z>H_0Pay>4tNJQ*?z?On#>ut3jNSrb=Nm~!69K)gXE~yK=HIw<&VOtNF9Q>MLDp1H- z2sNL>E1f4n?h4104mskB`69lM41fYe#g#Z9N(H4SAzArvNKyW+2I6u+i7FwVD#!2_ zP&UdDxm|7tzzxUhU?!GN61LV$8<+-^rP0Vs{eTKer~y|2-a>4{TS1CETmT1viZ~y* zFz1kR0i>wU53VJz5OBE>w*@d3b))?N!MFry3b?I+xtJSsgT0hs`wD-7tu4&GA@2I> z#6)@WOI-JGOJS}HUrmox-mX2M$EhQCG;Ny$avl`ErMdaVD^>hvFn|LD-FB4&>4l#_S#1iI}u0Em|#l z;@4$knwYnQGM%f0OM^#t3=ZuWEbbT_M*dfSU)RdaTtq%g0^gRAF3C*a5xTX`on48g zzzJHcDdbya7SqJM>e8E&mB zgOB<4z0*VvDM+HGWb8sNP?owlf2wru0y&G8DGcEI8Q`KaH;%6AkB-=7k;>3iugtU2-T;R;3V+wL`v8OUNJb Hw+H_R*+!{h literal 0 HcmV?d00001 diff --git a/__pycache__/visualize_3d.cpython-310.pyc b/__pycache__/visualize_3d.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..a4741a4118c955fd2532cc886c5d9fc7b8602a62 GIT binary patch literal 3713 zcmZ`+O>7&-6`t8YE`KD`vTVt=T5O8`h=ra0$Sn$^jgzKHniN$JP1|@OV6oz?s8yC+ zW_D?b1a(N|#CC1PE#kl0wM#ihZ3l?sBt{!sP<-gA=ic^Iv?3+miXH_^7>%B}T>_nUXFwJ!d-dG^Eg zn{PMITu7ygc3AfO&~qno(c1eLn`hrUV2pk9$2FsQ@-M9mcMlk?OP{UXcyKsn7zd5@ zcdxFW`Zzvo?ZHDa*IaqGd1`U(*Z0>RT!%KEd*_cpHn0A+)i|;C$#Uz=2{=M<);>7Z z`taQ9XQyN4`pUgj8^k2zIU4o7SSjV7E4d()>zDF4y}4{C)pb%4LyTs+G zsW6QtjuTU5I!i*Yu@p-~pI{l*2ffbv*$(KFEX#I6pJD@`sKF@vDz|5xeCYG)FI{KW z;g$XHi7i@-^$@G*^Bo$SPc(X-q0(Yk#uHz+YV!J=~iOuw9GE&AbkG5O}+{^8|Mf@P?3AfK#o$I%)JT5lX@xa1MSa3ze{8 z`0ZU(VZ~0i4>Y^Kasn<6?;e-ucp*R>^-Ac5o^!Z2?jR6Q)4qP`v6bAJ;!ZGaxuwwI z$87I#2cPYW_u7}w8KcD@cfe>bUs}I&!x$SY2F5>6ofJ6UI3*{wb&#f=fF?)H#FSeI z+?vz&VQflc4CYd0?jsHWeKl$FU;#K#{(Zzu^#GESqNL}RoLQHJ(~;`gla6Of+;3~J z)J!~I;oK>OunKI8zwU7S21jczjEVC1Bb>r0qP#Ql*CP>qEhn35-uHZN4%nRgvzF(S z3gNWH(G1sWna$hzX~&x4cG0nz9onWc>AB%}ZXn8TRa+#N zy)e???iKDtGW4dFw@b(Dz)TeFl3j4PnGKxKnsrMI#1Ic;rftTopfZWBB9iR6ND)*r z1hcVF8Bt$8zPDBOK`v8!qv9TeqtZTi3vS7>=bRu)TX1ltECyUc9q5)VtlcpcByvim z1dcZqsp7MI5H+gXb8dj@xJ|eL6Q3V?;qU~v^NtnR$D9dPDHf|%zFM6K9PZkl#hkp) zoQY#@P_aGGy*0+3E?41G03A29tX*(M@I`1etzXL00Xia&$cj87jnEOQ{a4reWrb!Q z)%NzDJoX7scTdRU`*jP_2rFUce*yvxp+Q;KoJm7sQpdDOLuT>@tuUoa&!|jg+9DMW zfHZ`e7kaxAX!e`NdT(4qCqNqDAXxIc_~x%jDXEY!Regnmfl_jml;~_MRhp&ni!2SM znW+qbu+5WKn>SBGB-rrAwd8BY=oI$>!ZrPo%H7{J1B!SN4v)T@a1CJbBbV@4toI%jGIb^}ezk~&=ge7{b($TRAYnK0=siOEyaUf^{`^s$kGBUzT@d1+prR{*oLAwfWtmn4CC z+K}sAYfzBG)V-uYA!v!dvr55Lof%;(4HeIToid<0&Uri#v32DhWG>&HMpNh^sb9{}eewVukZVKR02=SeBLuqz9oUvfZqPbKm#4y2j zh-|nswUPKg^ZNFArhv_;mG6PPt;T(@i1=04PrMIi0G87_e{aK+t)E$FUbzR)_V8-! z%$pl|XW{bivHa~wC%XUR2tNSFjpve)R4Pa5JY=3=S|zBFT!dIBm%}npZb^77{#97a zzlO~yG&wzXAm74?Z)1ag!%W$pXXSmLvmnwS4}chmN3cklw!Nt>kI26PTWqKS{1qTs za4&;&m<~(B5RjhaV0vKL7Kspn!csFpz>`r-(mIX?{eAn=a&zh0=1g`RyvAGHemDATCiB0D*{U9eGg1=3Ae%FMbf`)Z-8) zgd}R-^C6fI0so2eJOhj}R3T4Rs_oe(k2nBth9KvN>|7c%fD&d%R+O@=qR%QG(izKo z0}_P}Ct+F4&s!G%5^hBA&(Ux4L)hRmaa0CJ2{viTz@Z2ncxSwBfXdd1LQ|4%=!3eV z@6pp4g(E+gk#B8L)3K1r5H|9*VQ SXE5pnn1L!pPhlh*kpBn93@3j8 literal 0 HcmV?d00001 diff --git a/imu_decode.py b/imu_decode.py new file mode 100644 index 0000000..fc568d4 --- /dev/null +++ b/imu_decode.py @@ -0,0 +1,137 @@ +import serial +import struct +import time + +# ========== CRC16 预计算查找表 ========== +crc_tab = [] +for i in range(256): + crc = 0 + c = i + for _ in range(8): + if (crc ^ c) & 1: + crc = (crc >> 1) ^ 0xA001 + else: + crc >>= 1 + c >>= 1 + crc_tab.append(crc) + + +def crc16(data): + """CRC16-Modbus 校验 (查表法)""" + crc = 0xFFFF + for b in data: + crc = (crc >> 8) ^ crc_tab[(crc ^ b) & 0xFF] + return crc + + +# ========== 帧格式常量 (新协议 48 字节) ========== +# 0 2B 帧头 0xAA 0x55 +# 2 4B 时间戳 uint32_t (ms, 自启动) +# 6 12B 滤波后 Gyro[3] (float32 × 3) +# 18 12B 滤波后 Accel[3] (float32 × 3) +# 30 16B 四元数 qw, qx, qy, qz (float32 × 4) +# 46 2B CRC16 (对前 46 字节) +HEADER = b'\xAA\x55' +HEADER_LEN = 2 +PAYLOAD_LEN = 44 # uint32 + 3f + 3f + 4f = 4+12+12+16 +CRC_LEN = 2 +FRAME_LEN = HEADER_LEN + PAYLOAD_LEN + CRC_LEN # = 48 + +FIELD_NAMES = ['timestamp_ms', + 'gyro_x', 'gyro_y', 'gyro_z', + 'accel_x', 'accel_y', 'accel_z', + 'qw', 'qx', 'qy', 'qz'] + + +def parse_frame(payload_bytes): + """解包 44 字节 payload + + Returns: + dict with keys: timestamp_ms, gyro (3-tuple), accel (3-tuple), quat (4-tuple: qw,qx,qy,qz) + """ + ts, gx, gy, gz, ax, ay, az, qw, qx, qy, qz = struct.unpack(' 1 else 'COM3' + baud = int(sys.argv[2]) if len(sys.argv) > 2 else 115200 + main(port, baud) diff --git a/main_odometry.py b/main_odometry.py new file mode 100644 index 0000000..9a0069a --- /dev/null +++ b/main_odometry.py @@ -0,0 +1,305 @@ +""" +IMU 惯性三维里程计 — 主入口 + +用法: + python main_odometry.py COM5 [baud] + python main_odometry.py COM5 --save traj.csv + python main_odometry.py --replay traj.csv +""" + +import sys +import time +import struct +import csv +import argparse + +import numpy as np +import serial +import matplotlib.pyplot as plt + +from imu_decode import HEADER, PAYLOAD_LEN, CRC_LEN, crc16, parse_frame +from trajectory_tracker import Tracker +from visualize_3d import TrajectoryViewer + + +def read_frame(ser): + """从串口读取一帧, 返回解析后的 dict 或 None + + 逐字节寻找帧头 0xAA 0x55, 校验 CRC, 解包 payload。 + """ + while True: + b = ser.read(1) + if not b: + return None + if b[0] == 0xAA: + b2 = ser.read(1) + if not b2 or b2[0] != 0x55: + continue + + frame = ser.read(PAYLOAD_LEN + CRC_LEN) + if len(frame) < PAYLOAD_LEN + CRC_LEN: + return None + + payload_bytes = frame[:PAYLOAD_LEN] + crc_recv = struct.unpack(' 0.1: + dt = 0.005 # 回退到 200Hz 默认值 + else: + dt = 0.005 + last_ts = ts + + pos = tracker.update(gyro, accel, qw, qx, qy, qz, dt) + + # 保存 CSV + if csv_writer: + csv_writer.writerow([ts, gx, gy, gz, ax, ay, az, qw, qx, qy, qz, + pos[0], pos[1], pos[2]]) + + # 30Hz 刷新显示 + now = time.time() + if now - last_draw >= 0.033: + viewer.update(tracker.history_array) + plt.pause(0.001) + last_draw = now + + frame_count += 1 + if frame_count % 200 == 0: + print(f"[{frame_count:06d}] ts={ts} dt={dt*1000:.1f}ms " + f"pos=({pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f})") + + except KeyboardInterrupt: + print(f"\n停止。共接收 {frame_count} 帧。") + finally: + ser.close() + if csv_file: + csv_file.close() + print(f"轨迹已保存至 {save_csv}") + viewer.close() + + +def run_replay(csv_path): + """回放模式: 从 CSV 文件加载数据并显示 3D 轨迹""" + # 加载 CSV + rows = [] + with open(csv_path, 'r') as f: + reader = csv.DictReader(f) + for row in reader: + rows.append(row) + + print(f"加载 {len(rows)} 帧数据") + + # 从开头静止帧标定偏置 + calib_samples = min(200, len(rows) // 4) + accel_samples, gyro_samples = [], [] + qw_s, qx_s, qy_s, qz_s = [], [], [], [] + for i in range(calib_samples): + row = rows[i] + accel_samples.append([float(row['accel_x']), float(row['accel_y']), float(row['accel_z'])]) + gyro_samples.append([float(row['gyro_x']), float(row['gyro_y']), float(row['gyro_z'])]) + qw_s.append(float(row['qw'])) + qx_s.append(float(row['qx'])) + qy_s.append(float(row['qy'])) + qz_s.append(float(row['qz'])) + accel_bias, gyro_bias = Tracker.calibrate_from_samples( + np.array(accel_samples), np.array(gyro_samples), + np.array(qw_s), np.array(qx_s), np.array(qy_s), np.array(qz_s)) + print(f"标定 (前{calib_samples}帧): accel_bias=({accel_bias[0]:.4f},{accel_bias[1]:.4f},{accel_bias[2]:.4f})" + f" gyro_bias=({gyro_bias[0]:.4f},{gyro_bias[1]:.4f},{gyro_bias[2]:.4f})") + + tracker = Tracker() + tracker.accel_bias = accel_bias + tracker.gyro_bias = gyro_bias + viewer = TrajectoryViewer() + + print("开始回放 ...(空格键暂停)") + + last_ts = None + last_draw = time.time() + idx = 0 + paused = False + + def on_key(event): + nonlocal paused + if event.key == ' ': + paused = not paused + print("暂停" if paused else "继续") + + viewer.fig.canvas.mpl_connect('key_press_event', on_key) + + try: + while plt.fignum_exists(viewer.fig.number) and idx < len(rows): + if not paused: + row = rows[idx] + gyro = np.array([float(row['gyro_x']), float(row['gyro_y']), float(row['gyro_z'])]) + accel = np.array([float(row['accel_x']), float(row['accel_y']), float(row['accel_z'])]) + qw, qx, qy, qz = float(row['qw']), float(row['qx']), float(row['qy']), float(row['qz']) + + # 使用时间戳计算 dt + ts = int(row['timestamp_ms']) if 'timestamp_ms' in row else None + if ts is not None and last_ts is not None: + dt = (ts - last_ts) / 1000.0 + if dt <= 0 or dt > 0.1: + dt = 0.005 + else: + dt = 0.005 + if ts is not None: + last_ts = ts + + tracker.update(gyro, accel, qw, qx, qy, qz, dt) + idx += 1 + + now = time.time() + if now - last_draw >= 0.033: + viewer.update(tracker.history_array) + plt.pause(0.001) + last_draw = now + else: + plt.pause(0.05) + + if idx % 200 == 0: + pos = tracker.position + print(f"[{idx:06d}/{len(rows)}] dt={dt*1000:.1f}ms " + f"pos=({pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f})") + + except KeyboardInterrupt: + print("\n回放中断。") + finally: + viewer.close() + + print(f"回放完成，共处理 {idx} 帧。") + + +def main(): + parser = argparse.ArgumentParser(description='IMU 惯性三维里程计') + parser.add_argument('port', nargs='?', default=None, + help='串口号 (如 COM5)') + parser.add_argument('baud', nargs='?', type=int, default=115200, + help='波特率 (默认 115200)') + parser.add_argument('--save', metavar='FILE', + help='保存轨迹到 CSV 文件') + parser.add_argument('--replay', metavar='FILE', + help='从 CSV 文件回放轨迹') + args = parser.parse_args() + + if args.replay: + run_replay(args.replay) + elif args.port: + run_live(args.port, args.baud, save_csv=args.save) + else: + parser.print_help() + print("\n示例:") + print(" python main_odometry.py COM5") + print(" python main_odometry.py COM5 921600") + print(" python main_odometry.py COM5 --save traj.csv") + print(" python main_odometry.py --replay traj.csv") + + +if __name__ == '__main__': + main() diff --git a/test_3d_demo.py b/test_3d_demo.py new file mode 100644 index 0000000..684651a --- /dev/null +++ b/test_3d_demo.py @@ -0,0 +1,56 @@ +"""3D 显示验证 — 模拟圆周运动轨迹 (无需串口, 使用四元数)""" +import numpy as np +import time +import matplotlib.pyplot as plt +from scipy.spatial.transform import Rotation +from trajectory_tracker import Tracker +from visualize_3d import TrajectoryViewer + + +def main(): + tracker = Tracker(zupt_frames=50) + viewer = TrajectoryViewer(title="IMU 3D Demo — 四元数模拟") + + dt = 0.005 # 200Hz + t = 0.0 + omega = 0.5 # 角速度 (rad/s) + last_draw = time.time() + + print("3D 演示运行中... 按 Ctrl+C 停止") + + try: + while plt.fignum_exists(viewer.fig.number): + # X-Y 平面圆周运动 + Z 轴正弦起伏 + ax_linear = -0.25 * np.cos(omega * t) + ay_linear = -0.25 * np.sin(omega * t) + az_linear = 0.3 * np.sin(2 * t) + + # 模拟四元数 (绕 Z 轴旋转 omega*t) + yaw = omega * t + r = Rotation.from_euler('ZYX', [np.degrees(yaw), 0, 0]) + q = r.as_quat() # [x, y, z, w] + + # body 加速度 = R^T @ (a_linear + gravity) + a_world = np.array([ax_linear, ay_linear, az_linear + 9.81]) + accel_body = r.as_matrix().T @ a_world + + gyro = np.array([0.0, 0.0, omega]) + + pos = tracker.update(gyro, accel_body, q[3], q[0], q[1], q[2], dt) + + now = time.time() + if now - last_draw >= 0.033: + viewer.update(tracker.history_array) + plt.pause(0.001) + last_draw = now + + t += dt + + except KeyboardInterrupt: + print("停止。") + finally: + viewer.close() + + +if __name__ == '__main__': + main() diff --git a/trajectory_tracker.py b/trajectory_tracker.py new file mode 100644 index 0000000..f06e628 --- /dev/null +++ b/trajectory_tracker.py @@ -0,0 +1,198 @@ +""" +IMU 惯性三维里程计 — 核心算法模块 + +管线: + 四元数 → 旋转矩阵 (scipy Rotation.from_quat) + a_world = R @ (a_body - accel_bias) + a_linear = a_world - [0, 0, 9.81] + 双重积分 (梯形积分 + ZUPT 静止检测 + 加速度死区) + +使用 EKF 四元数标定加速度计偏置, 保证 R @ corrected_accel ≈ [0,0,g]。 +""" + +import numpy as np +from scipy.spatial.transform import Rotation + +GRAVITY = np.array([0.0, 0.0, 9.81]) # Z-up 右手系 + + +def quat_to_rotation(qw, qx, qy, qz): + """四元数 → body→world 旋转矩阵 + + Args: + qw, qx, qy, qz: STM32 四元数 (scalar-first) + Returns: + R: 3x3 旋转矩阵 + """ + return Rotation.from_quat([qx, qy, qz, qw]).as_matrix() + + +def rotate_accel(accel_body, R): + """机体加速度 → 世界坐标系""" + return R @ np.asarray(accel_body) + + +def gravity_compensate(a_world): + """减去重力向量""" + return a_world - GRAVITY + + +def apply_deadzone(a, threshold=0.03): + """幅值小于阈值的分量置零""" + a = np.asarray(a).copy() + a[np.abs(a) < threshold] = 0.0 + return a + + +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): + """ + Args: + zupt_threshold_accel: ZUPT ‖a_linear‖ 阈值 (m/s^2) + zupt_threshold_gyro: ZUPT ‖gyro‖ 阈值 (rad/s) + zupt_frames: 连续静止帧数阈值 + deadzone_threshold: 加速度分量死区 (m/s^2) + var_window_size: 方差窗口大小 (帧) + zupt_var_threshold: ZUPT 方差阈值 (m^2/s^4) + """ + self.position = np.zeros(3) + self.velocity = np.zeros(3) + + self.position_history = [self.position.copy()] + + self.zupt_threshold_accel = zupt_threshold_accel + self.zupt_threshold_gyro = zupt_threshold_gyro + self.zupt_frames = zupt_frames + self.deadzone_threshold = deadzone_threshold + self.var_window_size = var_window_size + self._zupt_var_threshold = zupt_var_threshold + + # 传感器偏置 + self.accel_bias = np.zeros(3) + self.gyro_bias = np.zeros(3) + + # ZUPT 状态 + self._zupt_counter = 0 + 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 + + def update(self, gyro, accel, qw, qx, qy, qz, dt): + """处理一帧 IMU 数据 + + Args: + gyro: 机体角速度 [gx, gy, gz] rad/s + accel: 机体加速度 [ax, ay, az] m/s^2 + qw, qx, qy, qz: EKF 四元数 (scalar-first) + dt: 时间步长 (s), 由时间戳差值计算 + """ + self.qw, self.qx, self.qy, self.qz = qw, qx, qy, qz + + accel = np.asarray(accel, dtype=float) + gyro = np.asarray(gyro, dtype=float) - self.gyro_bias + + # 0. 减去加速度计偏置 + accel_corrected = accel - self.accel_bias + + # 1. 四元数 → 旋转矩阵 + R = quat_to_rotation(qw, qx, qy, qz) + + # 2. 机体加速度 → 世界加速度 → 重力补偿 + a_world = rotate_accel(accel_corrected, R) + a_linear = gravity_compensate(a_world) + + # 3. 加速度死区 + a_linear = apply_deadzone(a_linear, self.deadzone_threshold) + + # 4. ZUPT 静止检测 + gyro_norm = np.linalg.norm(gyro) + linear_magnitude = np.linalg.norm(a_linear) + + self._linear_var_window.append(a_linear.copy()) + if len(self._linear_var_window) > self.var_window_size: + self._linear_var_window.pop(0) + + linear_variance = 0.0 + if len(self._linear_var_window) >= self.var_window_size: + linear_variance = np.var(self._linear_var_window, axis=0).mean() + + 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: + self._zupt_counter += 1 + else: + self._zupt_counter = 0 + + zupt_active = self._zupt_counter >= self.zupt_frames + + # 5. 梯形积分 (使用真实 dt) + if dt > 0: + if zupt_active: + self.velocity[:] = 0.0 + self._prev_accel_linear = np.zeros(3) + else: + a_prev = self._prev_accel_linear + self.velocity = self.velocity + (a_prev + a_linear) * dt / 2.0 + self._prev_accel_linear = a_linear.copy() + self.position = self.position + self.velocity * dt + + self.position_history.append(self.position.copy()) + return self.position + + @staticmethod + def calibrate_from_samples(accel_samples, gyro_samples, qw_samples, qx_samples, qy_samples, qz_samples): + """从静止采样数据计算传感器偏置 + + 利用 EKF 四元数计算理论 body 重力: R(q)^T @ [0, 0, 9.81] + 偏置 = 实测均值 - 理论均值 + + Args: + accel_samples: Nx3, 机体加速度 (m/s^2) + gyro_samples: Nx3, 角速度 (rad/s) + qw/qx/qy/qz_samples: N, EKF 四元数分量 + + Returns: + accel_bias: (3,) 加速度计偏置 (m/s^2) + gyro_bias: (3,) 陀螺仪偏置 (rad/s) + """ + accel_mean = np.mean(accel_samples, axis=0) + gyro_bias = np.mean(gyro_samples, axis=0) + + # 用平均四元数计算理论的 body 重力向量 + qw_m = np.mean(qw_samples) + qx_m = np.mean(qx_samples) + qy_m = np.mean(qy_samples) + qz_m = np.mean(qz_samples) + R_mean = quat_to_rotation(qw_m, qx_m, qy_m, qz_m) + gravity_body_expected = R_mean.T @ GRAVITY + + accel_bias = accel_mean - gravity_body_expected + + return accel_bias, gyro_bias + + def reset(self): + """重置轨迹, 保留偏置""" + self.position = np.zeros(3) + self.velocity = np.zeros(3) + self.position_history = [self.position.copy()] + self._zupt_counter = 0 + self._prev_accel_linear = np.zeros(3) + self._linear_var_window = [] + + @property + def history_array(self): + """返回 Nx3 numpy 数组""" + return np.array(self.position_history) diff --git a/visualize_3d.py b/visualize_3d.py new file mode 100644 index 0000000..be45e10 --- /dev/null +++ b/visualize_3d.py @@ -0,0 +1,114 @@ +""" +IMU 3D 轨迹实时可视化 + +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): + """ + Args: + title: 窗口标题 + refresh_interval: 刷新间隔 (ms), 默认 33ms ≈ 30Hz + """ + self.refresh_interval = refresh_interval + + self.fig = plt.figure(figsize=(8, 7)) + self.fig.canvas.manager.set_window_title(title) + self.ax = self.fig.add_subplot(111, projection='3d') + + # 轨迹线 (蓝色) + self.traj_line, = self.ax.plot([], [], [], 'b-', linewidth=1.0, label='Trajectory') + + # 当前点 (红色) + self.current_point, = self.ax.plot([], [], [], 'ro', markersize=6, label='Current') + + # 坐标系指示 (原点处) + 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._setup_axes() + + # 动画 + self.anim = FuncAnimation(self.fig, self._animate, interval=self.refresh_interval, + cache_frame_data=False, blit=False) + + 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]) + self.ax.set_zlim([-1, 1]) + + try: + self.ax.set_box_aspect([1, 1, 1]) + except NotImplementedError: + pass + + 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: + return + + 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) + + # 更新当前点 + self.current_point.set_data([x[-1]], [y[-1]]) + self.current_point.set_3d_properties([z[-1]]) + + # 自适应坐标轴范围 + 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 + half = (all_coords.max() - all_coords.min()) / 2 + margin + + self.ax.set_xlim([mid - half, mid + half]) + 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)