CN113734198B - 一种目标相对航向获取方法及设备 - Google Patents

Abstract

本发明的目的是提供一种目标相对航向获取方法及设备，本发明利用车辆纵向速度、车辆横向速度、目标相对纵向速度、目标相对横向速度四个参数来计算目标相对航向，本发明既考虑到自车行驶动态对目标航向估算的影响，又兼顾了实现的简洁性，便于在嵌入式环境下布置实施，可以精确估算得到目标相对航向。得到精确的目标相对航向之后，后续车辆（本车）例如可以基于得到的目标相对航向，对目标采取可靠的避让措施，如控制车辆加速、减速、转向或变道等可靠操作。

Description

一种目标相对航向获取方法及设备

技术领域

本发明涉及一种目标相对航向获取方法及设备。

背景技术

航向问题的研究对象通常是移动载体本身，如飞机、轮船、汽车等，且参考方向为地理北极方向，一般使用惯导、全球定位系统等设备获取。但在自动驾驶领域，为了更加准确的评估或预测环境目标的将来可能轨迹或行为，往往需要该目标的航向或者相对航向信息，所谓的相对航向是指目标运动方向与自车运动方向的夹角γ。

在自动驾驶感知环节，获取目标航向的方法通常有两种：

(1)利用摄像头或激光雷达获得目标外形的像素坐标或点云信息，通过对像素或点云的处理获取目标朝向信息，这种方法的缺点是如果目标形状信息不完整或检测误差较大，估算的朝向结果可能非常差；

(2)利用目标在车辆坐标系下跟踪后的相对速度估算出目标相对航向，这种方法会在自车转弯或变道时带来较大的误差。

发明内容

针对现有技术的上述问题，本发明的一个目的是提供一种目标相对航向获取方法及设备。

根据本发明的一个方面，提供了一种目标相对航向获取方法，该方法包括：

获取时刻t_k下车辆在世界坐标系下的纵向速度v_ex和获取时刻t_k下车辆的在世界坐标系下的横向速度v_ey，获取时刻t_k下所述车辆的周围的环境中的目标在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry；

基于所述纵向速度v_ex、横向速度v_ey、相对纵向速度v_rx和相对横向速度v_ry，得到在时刻t_k下目标运动方向与自车运动方向的夹角γ。

进一步的，上述方法中，获取时刻t_k下车辆在世界坐标系下的纵向速度v_ex和获取时刻tk下车辆的在世界坐标系下的横向速度v_ey，获取时刻t_k下所述车辆的周围的环境中的目标在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry，包括：

构建世界坐标系，其中，设t_k时刻下世界坐标系的原点O为地球某点的位置，世界坐标系的横轴方向Y与该t_k时刻的车辆的车体系的横轴方向Ye平行，世界坐标系的纵轴方向X与该t_k时刻的车辆的车体系的纵轴方向Xe平行；

获取t_k时刻下所述车辆在所述世界坐标系下的纵向速度v_ex和获取时刻t_k下所述车辆的在所述世界坐标系下的横向速度v_ey；

获取时刻t_k下所述车辆的周围的环境中的目标在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry。

进一步的，上述方法中，基于所述纵向速度v_ex、横向速度v_ey、相对纵向速度v_rx和相对横向速度v_ry，得到时刻t_k下目标运动方向与自车运动方向的夹角γ，包括：

基于P和Q，得到时刻t_k下目标运动方向与自车运动方向的夹角γ，其中，

P＝v_exv_ry-v_eyv_rx；

Q＝v_exv_rx+v_eyv_ry+v_ex ²+v_ey ²。

进一步的，上述方法中，基于P和Q，得到时刻t_k下目标运动方向与自车运动方向的夹角γ，包括：

基于如下公式，得到时刻t_k下目标运动方向与自车运动方向的夹角γ：

进一步的，上述方法中，基于如下公式，得到时刻t_k下目标运动方向与自车运动方向的夹角γ之前，还包括：

构建t_k时刻所述车辆在世界坐标系下的航向角正切函数表达式

其中，θ₁为t_k时刻下所述车辆在世界坐标系下的航向即自车运动方向v_ego与世界坐标系纵轴的夹角；

构建t_k时刻目标在世界坐标系下航向角正切函数表达式：

其中，t_k时刻下目标在世界坐标系下的航向为目标运动方向v_object与世界坐标系纵轴的夹角记为θ₂；

利用和差公式构建目标相对航向角正切函数表达式，并将公式(1)和(2)代入化简得到：

得到目标运动方向与自车运动方向的夹角γ的公式：

γ＝atan2(P，Q)    (4)；

其中，

根据本发明的另一方面，还提供一种目标相对航向获取设备，其中，该设备包括：

获取装置，用于获取时刻t_k下车辆在世界坐标系下的纵向速度v_ex和获取时刻t_k下车辆的在世界坐标系下的横向速度v_ey，获取时刻t_k下所述车辆的周围的环境中的目标在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry；

计算装置，用于基于所述纵向速度v_ex、横向速度v_ey、相对纵向速度v_rx和相对横向速度v_ry，得到在时刻t_k下目标运动方向与自车运动方向的夹角γ。

进一步的，上述设备中，所述获取装置，用于构建世界坐标系，其中，设t_k时刻下世界坐标系的原点O为地球某点的位置，世界坐标系的横轴方向Y与该t_k时刻的车辆的车体系的横轴方向Ye平行，世界坐标系的纵轴方向X与该t_k时刻的车辆的车体系的纵轴方向Xe平行；获取t_k时刻下所述车辆在所述世界坐标系下的纵向速度v_ex和获取时刻t_k下所述车辆的在所述世界坐标系下的横向速度v_ey；获取时刻t_k下所述车辆的周围的环境中的目标在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry。

进一步的，上述设备中，所述计算装置，用于基于P和Q，得到时刻t_k下目标运动方向与自车运动方向的夹角γ，其中，

P＝v_exv_ry-v_eyv_rx；

Q＝v_exv_rx+v_eyv_ry+v_ex ²+v_ey ²。

进一步的，上述设备中，所述计算装置，用于基于如下公式，得到时刻tk下目标运动方向与自车运动方向的夹角γ：

进一步的，上述设备中，所述计算装置，还用于：

构建t_k时刻所述车辆在世界坐标系下的航向角正切函数表达式

其中，θ₁为t_k时刻下所述车辆在世界坐标系下的航向即自车运动方向v_ego与世界坐标系纵轴的夹角；

构建t_k时刻目标在世界坐标系下航向角正切函数表达式：

其中，t_k时刻下目标在世界坐标系下的航向为目标运动方向v_object与世界坐标系纵轴的夹角记为θ₂；

利用和差公式构建目标相对航向角正切函数表达式，并将公式(1)和(2)代入化简得到：

得到目标运动方向与自车运动方向的夹角γ的公式：

γ＝atan2(P，Q)    (4)；

其中，

根据本发明的另一方面，还提供一种基于计算的设备，其中，包括：

处理器；以及

被安排成存储计算机可执行指令的存储器，所述可执行指令在被执行时使所述处理器：

获取时刻t_k下车辆在世界坐标系下的纵向速度v_ex和获取时刻t_k下车辆的在世界坐标系下的横向速度v_ey，获取时刻t_k下所述车辆的周围的环境中的目标在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry；

基于所述纵向速度v_ex、横向速度v_ey、相对纵向速度v_rx和相对横向速度v_ry，得到在时刻t_k下目标运动方向与自车运动方向的夹角γ。

根据本发明的另一方面，还提供一种计算机可读存储介质，其上存储有计算机可执行指令，其中，该计算机可执行指令被处理器执行时使得该处理器：

获取时刻t_k下车辆在世界坐标系下的纵向速度v_ex和获取时刻t_k下车辆的在世界坐标系下的横向速度v_ey，获取时刻t_k下所述车辆的周围的环境中的目标在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry；

基于所述纵向速度v_ex、横向速度v_ey、相对纵向速度v_rx和相对横向速度v_ry，得到在时刻t_k下目标运动方向与自车运动方向的夹角γ。

与现有技术相比，考虑到现有技术中，由于仅使用目标的相对纵向速度和相对横向速度直接计算目标相对航向，而未考虑自车即车辆的运动状态，所以计算出的目标相对航向会有不准确，导致在自车基于计算出的目标相对航向进行转弯或变道操作时会带来较大的误差的问题，本发明提出一种利用车辆纵向速度、车辆横向速度、目标相对纵向速度、目标相对横向速度四个参数来计算目标相对航向的方法，本发明既考虑到自车行驶动态对目标航向估算的影响，又兼顾了实现的简洁性，便于在嵌入式环境下布置实施，可以精确估算得到目标相对航向。得到精确的目标相对航向之后，后续车辆(本车)例如可以基于得到的目标相对航向，对目标采取可靠的避让措施，如控制车辆加速、减速、转向或变道等可靠操作。

附图说明

通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1示出本发明一实施例的目标相对航向获取方法的流程图；

图2示出本发明一实施例的目标相对航向获取方法的原理图。

附图中相同或相似的附图标记代表相同或相似的部件。

具体实施方式

下面结合附图对本发明作进一步详细描述。

在本申请一个典型的配置中，终端、服务网络的设备和可信方均包括一个或多个处理器(CPU)、输入/输出接口、网络接口和内存。

内存可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)。内存是计算机可读介质的示例。

计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括非暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。

本发明提供一种目标相对航向获取方法，所述方法包括：

步骤S1，获取时刻t_k下车辆在世界坐标系下的纵向速度v_ex和获取时刻t_k下车辆的在世界坐标系下的横向速度v_ey，获取时刻t_k下所述车辆的周围的环境中的目标在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry；

具体的，所述目标可以是所述车辆的周围的环境中的人、其他车辆或物体；

所述目标相对航向为目标运动方向与自车运动方向的夹角γ；

时刻t_k下车辆在世界坐标系下的纵向速度v_ex和获取时刻t_k下车辆的在世界坐标系下的横向速度v_ey可以通过车速传感器获取；

刻t_k下所述车辆的周围的环境中的目标在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry，可以根据目标相对车辆(自车)的相对位置和角度，并利用卡尔曼滤波算法得到；

步骤S2，基于所述纵向速度v_ex、横向速度v_ey、相对纵向速度v_rx和相对横向速度v_ry，得到在时刻t_k下目标运动方向与自车运动方向的夹角γ。

在此，考虑到现有技术中，由于仅使用目标的相对纵向速度和相对横向速度直接计算目标相对航向，而未考虑自车即车辆的运动状态，所以计算出的目标相对航向会有不准确，导致在自车基于计算出的目标相对航向进行转弯或变道操作时会带来较大的误差的问题，本发明提出一种利用车辆纵向速度、车辆横向速度、目标相对纵向速度、目标相对横向速度四个参数来计算目标相对航向的方法，本发明既考虑到自车行驶动态对目标航向估算的影响，又兼顾了实现的简洁性，便于在嵌入式环境下布置实施，可以精确估算得到目标相对航向。得到精确的目标相对航向之后，后续车辆(本车)例如可以基于得到的目标相对航向，对目标采取可靠的避让措施，如控制车辆加速、减速、转向或变道等可靠操作。

本发明的目标相对航向获取方法一实施例中，步骤S1，获取时刻t_k下车辆在世界坐标系下的纵向速度v_ex和获取时刻t_k下车辆的在世界坐标系下的横向速度v_ey，获取时刻t_k下所述车辆的周围的环境中的目标在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry，包括：

如图1和2所示，步骤S11，构建世界坐标系，其中，设t_k时刻下世界坐标系的原点O为地球某点的位置，世界坐标系的横轴方向Y与该t_k时刻的车辆的车体系的横轴方向Ye平行，世界坐标系的纵轴方向X与该t_k时刻的车辆的车体系的纵轴方向Xe平行；

步骤S12，获取t_k时刻下所述车辆(自车)在所述世界坐标系下的纵向速度v_ex和获取时刻t_k下所述车辆的在所述世界坐标系下的横向速度v_ey；

步骤S13，获取时刻t_k下所述车辆的周围的环境中的目标(object)在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry。

在此，本实施例中，通过构建世界坐标系和车辆的车体系，可以基于世界坐标系和车辆的车体系，可靠获取到基于所述纵向速度v_ex、横向速度v_ey、相对纵向速度v_rx和相对横向速度v_ry，便于后续可靠估算得到在时刻t_k的目标运动方向与自车运动方向的夹角γ。

本发明的目标相对航向获取方法一实施例中，步骤S2，基于所述纵向速度v_ex、横向速度v_ey、相对纵向速度v_rx和相对横向速度v_ry，得到时刻t_k下目标运动方向与自车运动方向的夹角γ，包括：

基于P和Q，得到时刻t_k下目标运动方向与自车运动方向的夹角γ，其中，

P＝v_exv_ry-v_eyv_rx；

Q＝v_exv_rx+v_eyv_ry+v_ex ²+v_ey ²。

在此，通过P和Q，可以可靠得到时刻t_k下目标运动方向与自车运动方向的夹角γ。

本发明的目标相对航向获取方法一实施例中，基于P和Q，得到时刻t_k下目标运动方向与自车运动方向的夹角γ，包括：

基于如下公式，得到时刻t_k下目标运动方向与自车运动方向的夹角γ：

本发明的目标相对航向获取方法一实施例中，基于如下公式，得到时刻t_k下目标运动方向与自车运动方向的夹角γ之前，还包括：

Step2：构建t_k时刻所述车辆在世界坐标系下的航向角正切函数表达式

其中，θ₁为t_k时刻下所述车辆在世界坐标系下的航向即自车运动方向v_ego与世界坐标系的纵轴X的夹角；

Step3：构建t_k时刻目标在世界坐标系下航向角正切函数表达式：

其中，θ₂为t_k时刻下目标在世界坐标系下的航向即目标运动方向v_object与世界坐标系的纵轴X的夹角；

Step4：利用和差公式构建目标相对航向角正切函数表达式，并将公式(1)和(2)代入化简得到：

Step7：得到目标运动方向与自车运动方向的夹角γ的公式：

γ＝atan2(P，Q)    (4)；

其中，atan2(P，Q)为反正切函数，其取值范围与坐标点所处的象限有关，定义如下：

具体的，公式(5)具体可通过如下步骤得到：

Step1：构建世界坐标系，设t_k时刻世界坐标系原点为地球某点位置，世界坐标系的横纵轴方向与该时刻自车车体系方向相同。设t_k时刻，自车在世界坐标系下纵向速度为v_ex、自车横向速度为v_ey，目标在所述车辆的车体系下的相对纵向速度v_rx、相对横向速度v_ry；则，此t_k时刻下自车在世界坐标系下的航向为自车运动方向v_ego与世界坐标系纵轴的夹角记为θ₁，此t_k时刻下目标在世界坐标系下的航向为目标运动方向v_object与世界坐标系纵轴的夹角记为θ₂；目标在该t_k时刻的相对航向为目标运动方向v_object与自车运动方向v_ego的夹角，记为γ。坐标系及角度示意见图2，其中，v_ego代表车辆(自车)在世界坐标系下的运动方向、v_object代表目标在世界坐标系下的运动方向。

Step2：构建t_k时刻自车在世界坐标系下的航向角正切函数表达式：

Step3：构建t_k时刻目标在世界坐标系下航向角正切函数表达式：

Step4：利用和差公式构建目标相对航向角正切函数表达式，并将Step2、Step3中公式代入化简得到：

Step5：计算目标相对航向角正切函数表达式中的分子

P＝v_exv_ry-v_eyv_rx；

Step6：计算目标相对航向角正切函数表达式中的分母

Q＝v_exv_rx+v_eyv_ry+v_ex ²+v_ey ²；

Step7：计算目标相对航向角值

γ＝atan2(P，Q)；

其中，atan2(P，Q)为反正切函数，其取值范围与坐标点所处的象限有关，定义如下：

在此，所述在利用目标相对横向速度、目标相对纵向速度、自车横向速度、自车纵向速度，通过三角函数和差公式，只需经历1次反正切函数求解，即可获得目标相对航向，作为算法创新方法，这不是本领域工程师通常都能想到的方法。

在某些嵌入式运行环境中，反三角函数的求解所耗计算时间要远大于常规的加减乘除运算。本实施例带来的收益之一就是嵌入式运行环境下在保证计算精度的同时降低了算法计算时间。

此外，当所述相对航向角值|γ|≤0.1920rad(约11°)，也即本发明所述

在较小的精度损失同时，经历0次反正切函数求解即可获取相对航向角，即

这会进一步降低算法的计算时间。

根据本发明的另一方面，还提供一种目标相对航向获取设备，其中，该设备包括：

获取装置，用于获取时刻t_k下车辆在世界坐标系下的纵向速度v_ex和获取时刻t_k下车辆的在世界坐标系下的横向速度v_ey，获取时刻t_k下所述车辆的周围的环境中的目标在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry；

具体的，所述目标可以是所述车辆的周围的环境中的人、其他车辆或物体；

所述目标相对航向为目标运动方向与自车运动方向的夹角γ；

时刻t_k下车辆在世界坐标系下的纵向速度v_ex和获取时刻t_k下车辆的在世界坐标系下的横向速度v_ey可以通过车速传感器获取；

刻t_k下所述车辆的周围的环境中的目标在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry，可以根据目标相对车辆(自车)的相对位置和角度，并利用卡尔曼滤波算法得到；

计算装置，用于基于所述纵向速度v_ex、横向速度v_ey、相对纵向速度v_rx和相对横向速度v_ry，得到在时刻t_k下目标运动方向与自车运动方向的夹角γ。

在此，考虑到现有技术中，由于仅使用目标的相对纵向速度和相对横向速度直接计算目标相对航向，而未考虑自车即车辆的运动状态，所以计算出的目标相对航向会有不准确，导致在自车基于计算出的目标相对航向进行转弯或变道操作时会带来较大的误差的问题，本发明提出一种利用车辆纵向速度、车辆横向速度、目标相对纵向速度、目标相对横向速度四个参数来计算目标相对航向的方法，本发明既考虑到自车行驶动态对目标航向估算的影响，又兼顾了实现的简洁性，便于在嵌入式环境下布置实施，可以精确估算得到目标相对航向。得到精确的目标相对航向之后，后续车辆(本车)例如可以基于得到的目标相对航向，对目标采取可靠的避让措施，如控制车辆加速、减速、转向或变道等可靠操作。

如图1和2所示，本发明的目标相对航向获取设备一实施例中，所述获取装置，用于构建世界坐标系，其中，设t_k时刻下世界坐标系的原点O为地球某点的位置，世界坐标系的横轴方向Y与该t_k时刻的车辆的车体系的横轴方向Ye平行，世界坐标系的纵轴方向X与该t_k时刻的车辆的车体系的纵轴方向Xe平行；获取t_k时刻下所述车辆在所述世界坐标系下的纵向速度v_ex和获取时刻t_k下所述车辆的在所述世界坐标系下的横向速度v_ey；获取时刻t_k下所述车辆的周围的环境中的目标在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry。

在此，本实施例中，通过构建世界坐标系和车辆的车体系，可以基于世界坐标系和车辆的车体系，可靠获取到基于所述纵向速度v_ex、横向速度v_ey、相对纵向速度v_rx和相对横向速度v_ry，便于后续可靠估算得到在时刻t_k的目标运动方向与自车运动方向的夹角γ。

本发明的目标相对航向获取设备一实施例中，所述计算装置，用于基于P和Q，得到时刻t_k下目标运动方向与自车运动方向的夹角γ，其中，

P＝v_exv_ry-v_eyv_rx；

Q＝v_exv_rx+v_eyv_ry+v_tx ²+v_ey ²。

在此，通过P和Q，可以可靠得到时刻t_k下目标运动方向与自车运动方向的夹角γ。

本发明的目标相对航向获取设备一实施例中，所述计算装置，用于基于如下公式，得到时刻t_k下目标运动方向与自车运动方向的夹角γ：

本发明的目标相对航向获取设备一实施例中，所述计算装置，还用于：

构建t_k时刻所述车辆在世界坐标系下的航向角正切函数表达式

其中，θ₁为t_k时刻下所述车辆在世界坐标系下的航向即自车运动方向v_ego与世界坐标系纵轴的夹角；

构建t_k时刻目标在世界坐标系下航向角正切函数表达式：

其中，t_k时刻下目标在世界坐标系下的航向为目标运动方向v_object与世界坐标系纵轴的夹角记为θ₂；

利用和差公式构建目标相对航向角正切函数表达式，并将公式(1)和(2)代入化简得到：

得到目标运动方向与自车运动方向的夹角γ的公式：

γ＝atarn2(P，Q)    (4)；

其中，

根据本发明的另一方面，还提供一种基于计算的设备，其中，包括：

处理器；以及

被安排成存储计算机可执行指令的存储器，所述可执行指令在被执行时使所述处理器：

获取时刻t_k下车辆在世界坐标系下的纵向速度v_ex和获取时刻t_k下车辆的在世界坐标系下的横向速度v_ey，获取时刻t_k下所述车辆的周围的环境中的目标在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry；

基于所述纵向速度v_ex、横向速度v_ey、相对纵向速度v_rx和相对横向速度v_ry，得到在时刻t_k下目标运动方向与自车运动方向的夹角γ。

根据本发明的另一方面，还提供一种计算机可读存储介质，其上存储有计算机可执行指令，其中，该计算机可执行指令被处理器执行时使得该处理器：

获取时刻t_k下车辆在世界坐标系下的纵向速度v_ex和获取时刻t_k下车辆的在世界坐标系下的横向速度v_ey，获取时刻t_k下所述车辆的周围的环境中的目标在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry；

基于所述纵向速度v_ex、横向速度v_ey、相对纵向速度v_rx和相对横向速度v_ry，得到在时刻t_k下目标运动方向与自车运动方向的夹角γ。

本发明各设备实施例的详细内容具体可参见各方法实施例的对应部分，在此，不再赘述。

显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。

本发明仅利用车辆纵向速度、车辆横向速度、目标相对纵向速度、目标相对横向速度四个参数简便计算出目标相对航向角，计算时间及存储资源小，便于在嵌入式环境下布置实施。

需要注意的是，本发明可在软件和/或软件与硬件的组合体中被实施，例如，可采用专用集成电路(ASIC)、通用目的计算机或任何其他类似硬件设备来实现。在一个实施例中，本发明的软件程序可以通过处理器执行以实现上文所述步骤或功能。同样地，本发明的软件程序(包括相关的数据结构)可以被存储到计算机可读记录介质中，例如，RAM存储器，磁或光驱动器或软磁盘及类似设备。另外，本发明的一些步骤或功能可采用硬件来实现，例如，作为与处理器配合从而执行各个步骤或功能的电路。

另外，本发明的一部分可被应用为计算机程序产品，例如计算机程序指令，当其被计算机执行时，通过该计算机的操作，可以调用或提供根据本发明的方法和/或技术方案。而调用本发明的方法的程序指令，可能被存储在固定的或可移动的记录介质中，和/或通过广播或其他信号承载媒体中的数据流而被传输，和/或被存储在根据所述程序指令运行的计算机设备的工作存储器中。在此，根据本发明的一个实施例包括一个装置，该装置包括用于存储计算机程序指令的存储器和用于执行程序指令的处理器，其中，当该计算机程序指令被该处理器执行时，触发该装置运行基于前述根据本发明的多个实施例的方法和/或技术方案。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化涵括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。此外，显然“包括”一词不排除其他单元或步骤，单数不排除复数。装置权利要求中陈述的多个单元或装置也可以由一个单元或装置通过软件或者硬件来实现。第一，第二等词语用来表示名称，而并不表示任何特定的顺序。

Claims (10)

1.一种目标相对航向获取方法，其中，该方法包括：

获取时刻t_k下车辆在世界坐标系下的纵向速度v_ex和获取时刻t_k下车辆的在世界坐标系下的横向速度v_ey，获取时刻t_k下所述车辆的周围的环境中的目标在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry；

基于所述纵向速度v_ex、横向速度v_ey、相对纵向速度v_rx和相对横向速度v_ry，得到在时刻t_k下目标运动方向与自车运动方向的夹角γ，其中，基于所述纵向速度v_ex、横向速度v_ey、相对纵向速度v_rx和相对横向速度v_ry，得到时刻t_k下目标运动方向与自车运动方向的夹角γ，包括：

基于P和Q，得到时刻t_k下目标运动方向与自车运动方向的夹角γ，其中，

P＝v_exv_ry-v_eyv_rx；

Q＝v_exv_rx+v_eyv_ry+v_ex ²+v_ey ²。

2.根据权利要求1所述的方法，其中，获取时刻t_k下车辆在世界坐标系下的纵向速度v_ex和获取时刻t_k下车辆的在世界坐标系下的横向速度v_ey，获取时刻t_k下所述车辆的周围的环境中的目标在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry，包括：

构建世界坐标系，其中，设t_k时刻下世界坐标系的原点O为地球某点的位置，世界坐标系的横轴方向Y与该t_k时刻的车辆的车体系的横轴方向Ye平行，世界坐标系的纵轴方向X与该t_k时刻的车辆的车体系的纵轴方向Xe平行；

获取t_k时刻下所述车辆在所述世界坐标系下的纵向速度v_ex和获取时刻t_k下所述车辆的在所述世界坐标系下的横向速度v_ey；

获取时刻t_k下所述车辆的周围的环境中的目标在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry。

3.根据权利要求1所述的方法，其中，基于P和Q，得到时刻t_k下目标运动方向与自车运动方向的夹角γ，包括：

基于如下公式，得到时刻t_k下目标运动方向与自车运动方向的夹角γ：

4.根据权利要求3所述的方法，其中，基于如下公式，得到时刻t_k下目标运动方向与自车运动方向的夹角γ之前，还包括：

构建t_k时刻所述车辆在世界坐标系下的航向角正切函数表达式

其中，θ₁为t_k时刻下所述车辆在世界坐标系下的航向即自车运动方向v_object与世界坐标系的纵轴X的夹角；

构建t_k时刻目标在世界坐标系下航向角正切函数表达式：

其中，θ₂为t_k时刻下目标在世界坐标系下的航向即目标运动方向v_object与世界坐标系的纵轴X的夹角；

利用和差公式构建目标相对航向角正切函数表达式，并将公式(1)和(2)代入化简得到：

得到目标运动方向与自车运动方向的夹角γ的公式：

γ＝atan2(P，Q)       (4)；

其中，

5.一种目标相对航向获取设备，其中，该设备包括：

获取装置，用于获取时刻t_k下车辆在世界坐标系下的纵向速度v_ex和获取时刻t_k下车辆的在世界坐标系下的横向速度v_ey，获取时刻t_k下所述车辆的周围的环境中的目标在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry；

计算装置，用于基于所述纵向速度v_ex、横向速度v_ey、相对纵向速度v_rx和相对横向速度v_ry，得到在时刻t_k下目标运动方向与自车运动方向的夹角γ，其中，基于所述纵向速度v_ex、横向速度v_ey、相对纵向速度v_rx和相对横向速度v_ry，得到时刻t_k下目标运动方向与自车运动方向的夹角γ，包括：

基于P和Q，得到时刻t_k下目标运动方向与自车运动方向的夹角γ，其中，

P＝v_exv_ry-v_eyv_rx；

Q＝v_exv_rx+v_eyv_ry+v_ex ²+v_ey ²。

6.根据权利要求5所述的设备，其中，所述获取装置，用于构建世界坐标系，其中，设t_k时刻下世界坐标系的原点O为地球某点的位置，世界坐标系的横轴方向Y与该t_k时刻的车辆的车体系的横轴方向Ye平行，世界坐标系的纵轴方向X与该t_k时刻的车辆的车体系的纵轴方向Xe平行；获取t_k时刻下所述车辆在所述世界坐标系下的纵向速度v_ex和获取时刻t_k下所述车辆的在所述世界坐标系下的横向速度v_ey；获取时刻t_k下所述车辆的周围的环境中的目标在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry。

7.根据权利要求5所述的设备，其中，所述计算装置，用于基于如下公式，得到时刻t_k下目标运动方向与自车运动方向的夹角γ：

8.根据权利要求7所述的设备，其中，所述计算装置，还用于：

构建t_k时刻所述车辆在世界坐标系下的航向角正切函数表达式

其中，θ₁为t_k时刻下所述车辆在世界坐标系下的航向即自车运动方向v_object与世界坐标系的纵轴X的夹角；

构建t_k时刻目标在世界坐标系下航向角正切函数表达式：

其中，θ₂为t_k时刻下目标在世界坐标系下的航向即目标运动方向v_object与世界坐标系的纵轴X的夹角；

利用和差公式构建目标相对航向角正切函数表达式，并将公式(1)和(2)代入化简得到：

得到目标运动方向与自车运动方向的夹角γ的公式：

γ＝atan2(P，Q)        (4)；

其中，

9.一种基于计算的设备，其中，包括：

处理器；以及

被安排成存储计算机可执行指令的存储器，所述可执行指令在被执行时使所述处理器：

获取时刻t_k下车辆在世界坐标系下的纵向速度v_ex和获取时刻t_k下车辆的在世界坐标系下的横向速度v_ey，获取时刻t_k下所述车辆的周围的环境中的目标在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry；

基于所述纵向速度v_ex、横向速度v_ey、相对纵向速度v_rx和相对横向速度v_ry，得到在时刻t_k下目标运动方向与自车运动方向的夹角γ，其中，基于所述纵向速度v_ex、横向速度v_ey、相对纵向速度v_rx和相对横向速度v_ry，得到时刻t_k下目标运动方向与自车运动方向的夹角γ，包括：

基于P和Q，得到时刻t_k下目标运动方向与自车运动方向的夹角γ，其中，

P＝v_exv_ry-v_eyv_rx；

Q＝v_exv_rx+v_eyv_ry+v_ex ²+v_ey ²。

10.一种计算机可读存储介质，其上存储有计算机可执行指令，其中，该计算机可执行指令被处理器执行时使得该处理器：

获取时刻t_k下车辆在世界坐标系下的纵向速度v_ex和获取时刻t_k下车辆的在世界坐标系下的横向速度v_ey，获取时刻t_k下所述车辆的周围的环境中的目标在所述车辆的车体系下的相对纵向速度v_rx和获取时刻t_k下所述目标在车辆的车体系下的相对横向速度v_ry；

基于所述纵向速度v_ex、横向速度v_ey、相对纵向速度v_rx和相对横向速度v_ry，得到在时刻t_k下目标运动方向与自车运动方向的夹角γ，其中，基于所述纵向速度v_ex、横向速度v_ey、相对纵向速度v_rx和相对横向速度v_ry，得到时刻t_k下目标运动方向与自车运动方向的夹角γ，包括：

基于P和Q，得到时刻t_k下目标运动方向与自车运动方向的夹角γ，其中，

P＝v_exv_ry-v_eyv_rx；

Q＝v_exv_rx+v_etv_ry+v_ex ²+v_ey ²。

Images