相对运动坐标系的跟踪处理

       通常做毫米波雷达跟踪时,并没有特别考虑坐标系的问题,实际上运动模型和算法都是假设坐标系静止的,而实际上毫米波是装在车上随车运动,既有平动,也有转动。如果仍用静止坐标系的假设,则会带来明显的误差。

        以下尝试说明这个问题。

        假定目标固定不动,毫米波雷达安装在车上随着自车运动,如下图所示。目标坐标一直为(10,10),自车t0时刻在(0,0),坐标系角度0°;t1时刻在(1,1),坐标系角度10°,t2时刻在(2,2),坐标系角度20°,且有一定的旋转。

        t0时刻下目标坐标为(10,10),

        t1时刻坐标系下目标坐标为(cx1,cy1)=(9,9)再旋转θ1=10°,则有

x1 = cx1*cosθ1 + cy1*sinθ1
y1 = -cx1*sinθ1 + cy1*cosθ1

        将cx1,cy1和cosθ1,sinθ1带入得x1=10.4256 ,y1=7.3008

        t2时刻坐标系系目标坐标为(cx2,cy2)=(8,8)再旋转θ2 = 20°,则有

x2 = cx2*cosθ2 + cy2*sinθ2
y2 = -cx2*sinθ2 + cy2*cosθ2

        将cx2,cy2和cosθ2,sinθ2带入得x2=10.2536 ,y2=4.7816

        因此从t0时刻到t1时刻,在自车坐标系下

Rvx1 = (x1 - x0)/Δt = 0.4256m/s
Rvy1 = (y1 - y0)/Δt = -2.6992m/s

        而世界坐标系下

vx1 = -1m/s
vy1 = -1m/s

        从t1时刻到t2时刻,在自车坐标系下

Rvx2 = (x2 - x1)/Δt = -0.272m/s
Rvy2 = (y2 - y2)/Δt = -2.5192m/s

        而世界坐标系下

vx2 = -1m/s
vy2 = -1m/s

        自车坐标系下看到的目标速度和世界坐标系下不同,主要是自车有旋转运动,不只是平移运动。如果自车只是平动,则计算的速度和世界坐标系下保持一致。

        自车旋转的情况下,可考虑将前后的坐标统一,再计算相对速度。

        t1时刻自车坐标系下

        x0 = 11.584,y0 = 8.112,则有

Mvx1 = - 1.1584m/s
Mvy1 = -0.8112m/s

        t2时刻自车坐标系下

        x1 = 11.5353, y1 = 5.3793, 则有

Mvx2 = -1.2817m/s
Mvy2 = -0.5977m/s

        可以看到补偿之后的速度误差更小,外推时估计更准。

        再举一个更直观的例子,自车绕着坐标原点做圆周运动,半径为10,每个周期50ms,周期之间转动1°(0.349rad)。

        如果简单的将两帧之间做差分,则得到目标的相对横纵向速度一直在变化,实际上应该是个恒定值。

         如果在坐标之间做坐标转换的补偿,则可以得到准确的相对速度,这也可以通过公式推导。

vx = -r*(yawrate*Δt)/Δt = -r*yawrate = -3.49m/s
vy = 0m/s

猜你喜欢

转载自blog.csdn.net/weixin_41691854/article/details/129838181