前言

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

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

通俗理解卡尔曼滤波（无人驾驶感知融合的经典算法）
前言我个人有近10年AI教育经验了,中间获得过一些名号,比如北理工校外导师,微软MVP兼CSDN技术专家,本博客也有1700多万PV了,在AI圈内有极高知名度.后2015年和团队一块创业创办AI职教 ...
激光感知（八）：计算目标最小凸多边形及航向角
目录前言一.点云凸包格雷厄姆扫描算法流程代码二.车辆航向角 1. PCA算法 2. 凸多边形最小外接矩形算法 3. L-shape拟合算法前言自动驾驶激光感知模块,常常需要求得目标物体的 ...
基于 V2I/V2N 的感知融合系统技术及应用研究
来源:智能汽车开发者平台摘要随着智能驾驶技术的研究与发展,单车智能感知的局限性逐渐显现出来,基于 V2X 的感知技术解决了单车感知在遮挡等场景下的局限性.介绍一种基于 V2I/V2N 的感知融合 ...
MDC功能软件-感知融合算法介绍
一.实车传感器布局华为网课中根据实际需要,布置了实车的传感器,包括0-8号共计九个camera.6个Rader和2个GPS传感器,图示如下: 这里的传感器布局与后面的ARXML文件配置是一一对应的, ...
多传感器融合感知技术的前沿算法与应用，包括Pointpillars、spconv+、BEVDet、LSS等算法
多传感器融合感知技术的前沿算法与应用,包括Pointpillars.spconv+.BEVDet.LSS等 2022年4月27日晚,由觉非科技算法总监郝爽带来的自动驾驶领域多传感器融合感知的技术算法解 ...
自动驾驶 Apollo 源码分析系列，感知篇(八)：感知融合代码的基本流程
说起自动驾驶感知系统,大家都会谈论到感知融合,这涉及到不同传感器数据在时间.空间的对齐和融合,最终的结果将提升自动驾驶系统的感知能力,因为我们都知道单一的传感器都是有缺陷的.本篇文章梳理 Apollo ...
自动驾驶 Apollo 源码分析系列，感知篇(九)：感知融合中的数据关联细节
前一篇文章讲了,Apollo 6.0 中融合的代码逻辑流程,但那是基于软件的角度进行梳理和分析的,这一篇文章基于上篇的成果进一步对算法进行更详细的分析,因为代码量奇大,所以本文重点讨论数据关联的一些细 ...
解读基于多传感器融合的卡尔曼滤波算法
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达本文转自|3D视觉工坊卡尔曼滤波器是传感器融合工程师用于自动驾驶 ...
“忽悠”智能机器人，竟然改改物品纹理就成功了！北航新研究：时空融合对抗攻击算法...
作者:刘艾杉编辑:鱼羊量子位报道 | 公众号 QbitAI 简单修改环境物体的纹理颜色,就能让机器人执行攻击者设计的错误行为! 来自北航.悉尼大学.伯克利和伦敦大学的一项最新研究成果显示: 通过 ...

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

目录

前言