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