“原始日志 → ego pose → future waypoint label” 的工程流程图。可以。下面我把它改成一套从原始日志一步一步推到 future waypoint label的数学推导，只用：IMU轮速 / 轮脉冲 / 里程计方向盘转角 / 前轮转角CAN 里的车速、横摆角速度、加速度等不使用 camera、点云、GPS/GNSS。公开资料里，nuScenes 的 CAN bus 示例明确包含了position、velocity、acceleration、steering等低层车端信号；Waymo 的 E2E 数据则把轨迹标签定义在vehicle coordinates下；而 dead reckoning 相关工作说明，IMU 是自包含的，但会漂，需要轮速/里程计之类的信息来抑制漂移。(NuScenes)0. 先定一个“高中生能算”的版本我们先只看平面运动，也就是车在地上跑，不考虑上下起伏。定义在第k kk个时刻，车的状态是：状态 k = ( x k , y k , ψ k , v k ) \text{状态}_k = (x_k,\ y_k,\ \psi_k,\ v_k)状态k=(xk,yk,ψk,vk)其中：x k , y k x_k, y_kxk,yk：车在“世界坐标系”里的位置ψ k \psi_kψk：车头朝向，叫航向角v k v_kvk：车速你可以把它想成：我在哪、我朝哪、我多快。采样周期设成：Δ t = t k − t k − 1 \Delta t = t_k - t_{k-1}Δt=tk−tk−1比如 CAN 和 IMU 每0.01 0.010.01秒来一帧，那就是Δ t = 0.01 s \Delta t = 0.01\text{ s}Δt=0.01s。1. 原始日志长什么样在第k kk个时刻，我们从原始日志里拿到这些量：1.1 IMU 原始量角速度（尤其是 z 轴）：ω z , k i m u \omega_{z,k}^{imu}ωz,kimu纵向加速度：a x , k i m u a_{x,k}^{imu}ax,kimu1.2 轮速原始量假设四个轮子的角速度分别是：ω f l , k , ω f r , k , ω r l , k , ω r r , k \omega_{fl,k},\ \omega_{fr,k},\ \omega_{rl,k},\ \omega_{rr,k}ωfl,k,ωfr,k,ωrl,k,ωrr,k其中fl/fr/rl/rr分别表示前左、前右、后左、后右。1.3 转向原始量如果记录的是方向盘转角：θ s w , k \theta_{sw,k}θsw,k还需要一个转向传动比：i s t e e r i_{steer}isteer这样前轮转角大约是：δ k = θ s w , k i s t e e r \delta_k = \frac{\theta_{sw,k}}{i_{steer}}δk=isteerθsw,k这里δ k \delta_kδk才是车辆运动学真正用的“前轮转角”。2. 先把“轮速”变成“车速”如果轮胎半径是：r w r_wrw而轮子的角速度单位是弧度每秒，那么该轮对应的线速度就是：v w h e e l = r w ⋅ ω w h e e l v_{wheel} = r_w \cdot \omega_{wheel}vwheel=rw⋅ωwheel所以四个轮子的线速度分别是：v f l , k = r w ω f l , k v_{fl,k} = r_w \omega_{fl,k}vfl,k=rwωfl,kv f r , k = r w ω f r , k v_{fr,k} = r_w \omega_{fr,k}vfr,k=rwωfr,kv r l , k = r w ω r l , k v_{rl,k} = r_w \omega_{rl,k}vrl,k=rwωrl,kv r r , k = r w ω r r , k v_{rr,k} = r_w \omega_{rr,k}vrr,k=rwωrr,k如果想要一个简单的整车速度估计，可以先求平均：v k w h e e l = v f l , k + v f r , k + v r l , k + v r r , k 4 v_k^{wheel} = \frac{v_{fl,k}+v_{fr,k}+v_{rl,k}+v_{rr,k}}{4}vkwheel=4vfl,k+vfr,k+vrl,k+vrr,k如果你只信任后轮，也可以只用后轮平均：v k w h e e l = v r l , k + v r r , k 2 v_k^{wheel} = \frac{v_{rl,k}+v_{rr,k}}{2}vkwheel=2vrl,k+vrr,k为什么这一步合理？因为轮速/里程计本来就是地面车辆最常用的短距离速度或距离约束，公开 dead reckoning 文献也明确说它能帮助抑制纯 IMU 的漂移。3. 再用 IMU 和转角估计“转弯速度”车转弯最关键的是横摆角速度，也就是：r k = ψ ˙ k r_k = \dot\psi_krk=ψ˙k它表示“车头每秒转多少弧度”。3.1 直接用 IMU 的陀螺仪最直接的办法是：r k i m u = ω z , k i m u − b g r_k^{imu} = \omega_{z,k}^{imu} - b_grkimu=ωz,kimu−bg其中b g b_gbg是陀螺仪零偏。高中生可以先把它理解成：传感器总有点偏，所以要减掉一个固定误差。3.2 也可以由车速和转角算一个“理论横摆角速度”对于简单自行车模型：r k m o d e l = v k L tan δ k r_k^{model} = \frac{v_k}{L}\tan\delta_krkmodel=Lvk 自动驾驶：原始日志 → ego pose → future waypoint label
“原始日志 → ego pose → future waypoint label” 的工程流程图。可以。下面我把它改成一套从原始日志一步一步推到 future waypoint label的数学推导，只用：IMU轮速 / 轮脉冲 / 里程计方向盘转角 / 前轮转角CAN 里的车速、横摆角速度、加速度等不使用 camera、点云、GPS/GNSS。公开资料里，nuScenes 的 CAN bus 示例明确包含了position、velocity、acceleration、steering等低层车端信号；Waymo 的 E2E 数据则把轨迹标签定义在vehicle coordinates下；而 dead reckoning 相关工作说明，IMU 是自包含的，但会漂，需要轮速/里程计之类的信息来抑制漂移。(NuScenes)0. 先定一个“高中生能算”的版本我们先只看平面运动，也就是车在地上跑，不考虑上下起伏。定义在第k kk个时刻，车的状态是：状态 k = ( x k , y k , ψ k , v k ) \text{状态}_k = (x_k,\ y_k,\ \psi_k,\ v_k)状态k=(xk,yk,ψk,vk)其中：x k , y k x_k, y_kxk,yk：车在“世界坐标系”里的位置ψ k \psi_kψk：车头朝向，叫航向角v k v_kvk：车速你可以把它想成：我在哪、我朝哪、我多快。采样周期设成：Δ t = t k − t k − 1 \Delta t = t_k - t_{k-1}Δt=tk−tk−1比如 CAN 和 IMU 每0.01 0.010.01秒来一帧，那就是Δ t = 0.01 s \Delta t = 0.01\text{ s}Δt=0.01s。1. 原始日志长什么样在第k kk个时刻，我们从原始日志里拿到这些量：1.1 IMU 原始量角速度（尤其是 z 轴）：ω z , k i m u \omega_{z,k}^{imu}ωz,kimu纵向加速度：a x , k i m u a_{x,k}^{imu}ax,kimu1.2 轮速原始量假设四个轮子的角速度分别是：ω f l , k , ω f r , k , ω r l , k , ω r r , k \omega_{fl,k},\ \omega_{fr,k},\ \omega_{rl,k},\ \omega_{rr,k}ωfl,k,ωfr,k,ωrl,k,ωrr,k其中fl/fr/rl/rr分别表示前左、前右、后左、后右。1.3 转向原始量如果记录的是方向盘转角：θ s w , k \theta_{sw,k}θsw,k还需要一个转向传动比：i s t e e r i_{steer}isteer这样前轮转角大约是：δ k = θ s w , k i s t e e r \delta_k = \frac{\theta_{sw,k}}{i_{steer}}δk=isteerθsw,k这里δ k \delta_kδk才是车辆运动学真正用的“前轮转角”。2. 先把“轮速”变成“车速”如果轮胎半径是：r w r_wrw而轮子的角速度单位是弧度每秒，那么该轮对应的线速度就是：v w h e e l = r w ⋅ ω w h e e l v_{wheel} = r_w \cdot \omega_{wheel}vwheel=rw⋅ωwheel所以四个轮子的线速度分别是：v f l , k = r w ω f l , k v_{fl,k} = r_w \omega_{fl,k}vfl,k=rwωfl,kv f r , k = r w ω f r , k v_{fr,k} = r_w \omega_{fr,k}vfr,k=rwωfr,kv r l , k = r w ω r l , k v_{rl,k} = r_w \omega_{rl,k}vrl,k=rwωrl,kv r r , k = r w ω r r , k v_{rr,k} = r_w \omega_{rr,k}vrr,k=rwωrr,k如果想要一个简单的整车速度估计，可以先求平均：v k w h e e l = v f l , k + v f r , k + v r l , k + v r r , k 4 v_k^{wheel} = \frac{v_{fl,k}+v_{fr,k}+v_{rl,k}+v_{rr,k}}{4}vkwheel=4vfl,k+vfr,k+vrl,k+vrr,k如果你只信任后轮，也可以只用后轮平均：v k w h e e l = v r l , k + v r r , k 2 v_k^{wheel} = \frac{v_{rl,k}+v_{rr,k}}{2}vkwheel=2vrl,k+vrr,k为什么这一步合理？因为轮速/里程计本来就是地面车辆最常用的短距离速度或距离约束，公开 dead reckoning 文献也明确说它能帮助抑制纯 IMU 的漂移。3. 再用 IMU 和转角估计“转弯速度”车转弯最关键的是横摆角速度，也就是：r k = ψ ˙ k r_k = \dot\psi_krk=ψ˙k它表示“车头每秒转多少弧度”。3.1 直接用 IMU 的陀螺仪最直接的办法是：r k i m u = ω z , k i m u − b g r_k^{imu} = \omega_{z,k}^{imu} - b_grkimu=ωz,kimu−bg其中b g b_gbg是陀螺仪零偏。高中生可以先把它理解成：传感器总有点偏，所以要减掉一个固定误差。3.2 也可以由车速和转角算一个“理论横摆角速度”对于简单自行车模型：r k m o d e l = v k L tan δ k r_k^{model} = \frac{v_k}{L}\tan\delta_krkmodel=Lvk