感知融合（六）：运动补偿算法计算目标绝对速度

前言

在自动驾驶场景中，常常需要计算目标的绝对速度以区分静止目标和运动目标，从而进行决策规划。我们都知道，目标的相对速度可以用卡尔曼滤波计算得到，那么目标的绝对速度呢？很多小伙伴可能说会，加上本车的车速就行了，那当我们位于弯道场景的时候呢？有经验的小伙伴可能会说利用yawrate就可以计算得到本车的横向速度。

没错，我们可以利用底盘输出给我们的车速和yawrate计算得到本车的横、纵向速度，从而得到目标的绝对速度，但这种方法精度足够高吗？我们可以利用高精度定位设备作为真值来进行对比，结论是能直观看出计算出的目标绝对速度与真实值存在不小的误差。

（假装有图片）

1. 运动补偿

那么怎么才能得到目标准确的绝对速度呢？我们的运动补偿算法登场了，所谓运动补偿，就是将 t-1 时刻到 t 时刻，本车的位姿变化给补偿掉，相当于本车静止不动，自然能得到目标准确的绝对速度了。我们既可以把 t-1 时刻目标车位置补偿到 t 时刻本车坐标系下，也可以把 t 时刻检测到的目标车位置补偿回 t-1 时刻的本车坐标系下，在这里就用前者来讲述补偿过程。

那么如何补偿本车的位姿变化呢？所谓位姿，就是位置和姿态，本车从 t-1 时刻运动到 t 时刻，产生改变其实也就这两点，t 时刻位置相较于 t-1 时刻产生了位移、t 时刻航向角相较于 t-1 时刻产生了旋转，两者相互独立，所以只需依次补偿这两点就完成了本车的运动补偿。

1) 位移补偿

首先我们来补偿位移，上面讲了位移是 t 时刻相较于 t-1 时刻产生了位置变化，对于自动驾驶系统来说，这里有两种方式获得本车的位移，第一种是用底盘输出的车速和yawrate进行计算；第二种是从高精度定位设备中获取，为了方便，这里就以定位数据为例。

从定位数据中能得到 t-1 时刻与 t 时刻本车的位置，注意定位设备给的位置是东北天/北东地坐标系下的，直接相减后的位移差值自然也是东北天/北东地坐标系下的，我们需要将差值转换到 t-1 时刻的本车坐标系下，因此还需要结合定位给出的本车航向角yaw信息进行坐标转换。

转换后我们就得到了 t 时刻相较于 t-1 时刻，在 t-1 时刻本车坐标系下的位移变化。将 t-1 时刻本车坐标系下目标障碍物相对位置减去此位移变化，得到的就是 t 时刻补偿后的目标障碍物相对位置了。

如果觉得很绕，可以举个例子：比如 1 辆目标车 t-1 时刻在我们正前方 10 米处，本车在 t 时刻相较于 t -1 时刻向前运动了 1 米，那么 t 时刻补偿后的目标车相对位置就是 10 - 1 = 9 米了，这样补偿掉本车位移才能更准确的计算目标车的绝对速度。

// 两帧间车辆位移、姿态差值
double dif_loc_x = current_host_info.px - last_host_info.px;
double dif_loc_y = current_host_info.py - last_host_info.py;
double dif_loc_yaw = current_host_info.yaw - last_host_info.yaw;

// 坐标旋转到上一帧航向角
double trans_x = dif_loc_x * cos(last_host_info.yaw) + dif_loc_y * sin(last_host_info.yaw);
double trans_y = -dif_loc_x * sin(last_host_info.yaw) + dif_loc_y * cos(last_host_info.yaw);

// 转换到上一帧坐标系，以东北天坐标系(东x，北y)为例
host_diff.diff_x= trans_y;
host_diff.diff_y = -trans_x;
host_diff.diff_yaw = dif_loc_yaw;

2) 角度补偿

位置补偿后，接下来就是角度补偿了，角度补偿很简单，就是由航向角变化导致的本车坐标系旋转，直接套用坐标系旋转公式即可，别忘了速度也要旋转哟。

坐标系旋转公式：
x’ = cos(θ) * x + sin(θ) * y
y’ = -sin(θ) * x + cos(θ) * y

// 略，直接套用坐标系旋转公式即可

这样本车的运动补偿就已经完成了， t-1 时刻的目标车位置被我们补偿到了 t 时刻本车坐标系下，再与 t 时刻检测到的目标车位置一起送进卡尔曼滤波器，便能得到一个十分准确的目标车绝对速度，是不是很简单呢？

2. 附录

定位（东北天）坐标系

坐标：东为X，北为Y
角度：东为0，北（逆时针）为正

atan2

atan2是一个函数，C 语言中atan2的函数原型为 double atan2(double y, double x)
返回值的单位为弧度，取值范围为(-π，+π]

atan2函数返回的是原点至点(x,y)的方位角，即与 x 轴的夹角（数学坐标系东X北Y）
结果为正表示从 X 轴逆时针旋转的角度，结果为负表示从 X 轴顺时针旋转的角度。

坐标旋转

x’ = cos(θ) * x + sin(θ) * y
y’ = -sin(θ) * x + cos(θ) * y