一种航向角确定方法、电子设备和存储介质

技术领域

本申请涉及自动驾驶技术领域，特别是涉及一种航向角确定方法、电子设备和存储介质。

背景技术

航向角对于估计目标的运动状态，预测目标的运行轨迹以及计算碰撞风险与碰撞点都有非常重要的作用。目前，摄像头与激光雷达都可以通过神经网络回归的方式估计目标的航向角，并通过恒定转(弯)率和速度模型(Constant Turn Rate and Velocity，CTRV)或恒定转(弯)率和加速度模型(Constant Turn Rate and Acceleration，CTRA)等动态模型使用卡尔曼滤波对目标姿态(包括航向角)进行估计，以降低误差。

以CTRV为例，将传感器实时测量的检测结果作为量测输入，滤波的预测则依赖于前面估计得到的横向角速度，最终通过调整过程噪声与量测噪声来计算航向角的权重，使航向角的计算更加平滑稳定。然而，该方式中，传感器的检测结果往往存在一定的误差，甚至会出现180°的模糊，使确定的航向角准确率低。

发明内容

本申请主要解决的技术问题是提供一种航向角确定方法、电子设备和存储介质，能够确定准确率较高的航向角。

为解决上述技术问题，本申请第一方面提供了一种航向角确定方法，该方法包括：基于至少一个传感器的检测结果，确定目标的初始航向角，并基于目标的速度信息和轨迹信息，确定目标的第一航向角；利用第一航向角对初始航向角进行修正，得到第二航向角；基于第二航向角和目标在上一时刻的目标航向角，确定目标在当前时刻的目标航向角。

其中，利用第一航向角对初始航向角进行修正，得到第二航向角，包括：判断第一航向角和初始航向角是否满足方向相反条件；响应于第一航向角和初始航向角满足方向相反条件，将第一航向角的反向向量的角度作为第二航向角；响应于第一航向角和初始航向角不满足方向相反条件，将初始航向角作为第二航向角。

其中，基于第二航向角和目标在上一时刻的目标航向角，确定目标在当前时刻的目标航向角，包括：判断第二航向角与预测航向角的差值是否大于第三预设角度，其中，预测航向角是利用目标在上一时刻的目标航向角进行卡尔曼滤波预测得到的；响应于第二航向角与预测航向角的差值大于第三预设角度，将第二航向角作为新的预测航向角，并利用第二航向角和新的预测航向角进行卡尔曼滤波更新，得到目标航向角；响应于第二航向角与预测航向角的差值小于或等于第三预设角度，利用第二航向角和预测航向角进行卡尔曼滤波更新，得到目标航向角。

其中，基于目标的速度信息和轨迹信息，确定目标的第一航向角，包括：基于速度信息中的速度比值，确定目标的第三航向角；其中，速度比值为目标的纵向速度与横向速度之间的比值；基于轨迹信息中的轨迹方程，确定目标的第四航向角；基于第一权重、第二权重分别对第三航向角、第四航向角进行加权，得到第一航向角；其中，第一权重和第二权重均与目标的速度相关，且第一权重和第二权重负相关。

其中，基于速度信息中的速度比值，确定目标的第三航向角，包括：将速度比值作为反正切函数的输入值，得到反正切函数的输出值；将反正切函数的输出值，作为第三航向角。

其中，基于轨迹信息中的轨迹方程，确定目标的第四航向角，包括：对轨迹方程求导，得到求导后的方程；利用目标在当前时刻的位置信息和求导后的方程，得到第四航向角。

其中，第二权重基于目标的速度和权重函数得到，第一权重为第一预设数值与第二权重之差，权重函数的获取步骤包括：获取预定义的目标的若干速度值和若干速度值分别对应的权重值；利用若干速度值和若干速度值分别对应的权重值进行样条函数拟合，得到权重函数。

其中，传感器的数量为两个；基于至少一个传感器的检测结果，确定目标的初始航向角，包括：基于第一传感器的数据，得到第五航向角，以及基于第二传感器的数据，得到第六航向角；基于第三权重、第四权重分别对第五航向角、第六航向角进行加权，得到初始航向角；其中，第三权重与目标距离成正比，目标距离为目标和第一传感器或第二传感器之间的距离；第四权重为第二预设数值与第三权重之差。

为解决上述技术问题，本申请第二方面提供了一种电子设备，该设备包括相互耦接的存储器和处理器，存储器存储有程序指令；处理器用于执行存储器中存储的程序指令，以实现上述第一方面提供的方法。

为解决上述技术问题，本申请第三方面提供了一种计算机可读存储介质，该计算机可读存储介质用于存储程序指令，该程序指令能够被执行以实现上述第一方面提供的方法。

本申请的有益效果是：区别于现有技术的情况，本申请基于至少一个传感器的检测结果，确定目标的初始航向角，并基于目标的速度信息和轨迹信息，确定目标的第一航向角；利用第一航向角对初始航向角进行修正，得到第二航向角；基于第二航向角和目标在上一时刻的目标航向角，确定目标在当前时刻的目标航向角。通过利用第一航向角对初始航向角进行修正，即可避免初始航向角出现180°的模糊，进而提高目标航向角的准确度。

附图说明

图1是本申请提供的航向角确定方法第一实施方式的流程示意图；

图2是本申请提供的航向角确定方法第一实施方式的步骤S11中确定第一航向角一实施方式的流程示意图；

图3是本申请提供的权重函数一实施方式的示意图；

图4是本申请提供的电子设备一实施方式的框架结构示意图；

图5是本申请提供的计算机可读存储介质一实施方式的框架示意图。

具体实施方式

下面结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性的劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

需要说明的是，本申请实施例中有涉及“第一”、“第二”等的描述，该“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。

在本文中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本发明的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其它实施例相结合。

请参阅图1，图1是本申请提供的航向角确定方法第一实施方式的流程示意图，该方法包括：

S11：基于至少一个传感器的检测结果，确定目标的初始航向角，并基于目标的速度信息和轨迹信息，确定目标的第一航向角。

在一实施方式中，传感器的数量为两个，则可以基于第一传感器的数据，得到第五航向角，基于第二传感器的数据，得到第六航向角；基于第三权重、第四权重分别对第五航向角、第六航向角进行加权，得到初始航向角；其中，第三权重与目标距离成正比，目标距离为目标和第一传感器或第二传感器之间的距离；第四权重为第二预设数值与第三权重之差。

具体地，第一传感器可以为雷达，如激光雷达，通过雷达扫描到的点云数据可以获得第五航向角；第二传感器可以为相机，通过相机检测算法可以得到第六航向角。第三权重可以根据目标和第一传感器或第二传感器之间的距离确定，当目标和第一传感器或第二传感器之间的距离越近，则第三权重越大；第三权重还可以根据雷达扫描到的点云数据中的点云数量确定，点云数量越多时，第三权重越大，反之，第三权重越小。第四权重为第二预设数值与第三权重之差，在一具体实施方式中，第二预设数值为1。初始航向角的计算公式如下所示。

θ

其中，θ

在其他实施方式中，传感器的数量也可以为一个，如仅使用雷达或者相机。

在一实施方式中，可以根据目标运动状态估计的结果，确定第一航向角，目标的运动状态估计可以使用常加速模型确定。具体地，可以利用目标的速度信息，确定目标的第三航向角，利用目标的轨迹信息确定第四航向角，对第三航向角和第四航向角进行加权处理，得到第一航向角。第一航向角的具体确定方式请参考下文，此处暂不做详述。

S12：利用第一航向角对初始航向角进行修正，得到第二航向角。

上述得到的初始航向角的精度较高，但方向较差；而第一航向角方向准确，但精度较差，因此，可以利用第一航向角对初始航向角进行修正。

在一实施方式中，利用第一航向角对初始航向角进行修正，得到第二航向角可以包括：判断第一航向角和初始航向角是否满足方向相反条件；响应于第一航向角和初始航向角满足方向相反条件，将第一航向角的反向向量的角度作为第二航向角；响应于第一航向角和初始航向角不满足方向相反条件，将初始航向角作为第二航向角。在一具体实施方式中，方向相反条件可以是第一航向角和初始航向角是否为钝角，若第一航向角和初始航向角为钝角，则认为第一航向角和初始航向角满足方向相反条件，否则，认为第一航向角和初始航向角不满足方向相反条件。

具体可利用下述公式确定第二航向角。

其中，θ

S13：基于第二航向角和目标在上一时刻的目标航向角，确定目标在当前时刻的目标航向角。

在一实施方式中，在确定目标在上一时刻的目标航向角后，可利用目标在上一时刻的目标航向角进行卡尔曼滤波预测，得到目标在当前时刻的预测航线角，再基于当前时刻的量测(即第二航向角)进行卡尔曼滤波更新，即可得到目标在当前时刻的目标航向角。其中，卡尔曼滤波预测和卡尔曼滤波更新的过程和现有技术相同，

卡尔曼滤波预测过程：x

P

卡尔曼滤波更新过程包括：K

xk＝xk+K

P

其中，x

在另一实施方式中，确定第二航向角后，可以先判断第二航向角和预测航向角的偏差。具体地，判断第二航向角与预测航向角的差值是否大于第三预设角度，其中，预测航向角是利用目标在上一时刻的目标航向角进行卡尔曼滤波预测得到的；响应于第二航向角与预测航向角的差值大于第三预设角度，将第二航向角作为新的预测航向角，并利用第二航向角和新的预测航向角进行卡尔曼滤波更新，得到目标航向角；响应于第二航向角与预测航向角的差值小于或等于第三预设角度，利用第二航向角和预测航向角进行卡尔曼滤波更新，得到目标航向角。其中，第二预设角度可以由用户设置，例如是5°、10°等，此处不做具体限定。

上述方式，基于至少一个传感器的检测结果，确定目标的初始航向角，并基于目标的速度信息和轨迹信息，确定目标的第一航向角；利用第一航向角对初始航向角进行修正，得到第二航向角；基于第二航向角和目标在上一时刻的目标航向角，确定目标在当前时刻的目标航向角。通过利用第一航向角对初始航向角进行修正，即可避免初始航向角出现180°的模糊，进而提高目标航向角的准确度。

请参阅图2，图2是本申请提供的航向角确定方法第一实施方式的步骤S11中确定第一航向角一实施方式的流程示意图，图3是本申请提供的权重函数一实施方式的示意图；基于目标的速度信息和轨迹信息，确定目标的第一航向角，包括：

S111：基于速度信息中的速度比值，确定目标的第三航向角。

在一实施方式中，根据目标运动状态估计的结果，获取目标的速度信息，目标的速度信息可以包含速度比值，速度比值为目标的纵向速度与横向速度之间的比值。在一具体实施方式中，以目标为原点，目标的前进方向为x轴建立坐标系，则目标在x方向的速度即为目标的纵向速度，目标在y方向的速度为目标的横向速度。具体的，可以如下述公式，将速度比值作为反正切函数的输入值，得到反正切函数的输出值，将反正切函数的输出值，作为第三航向角。

θ

其中，θ

可以理解地，其他实施方式中，也可以采用其他方式计算第三航向角，此处不做具体限定。

S112：基于轨迹信息中的轨迹方程，确定目标的第四航向角。

在一实施方式中，可以先根据目标在多帧中的位置，确定目标的轨迹方程，再对轨迹方程进行求导，得到求导后的方程；利用目标在当前时刻的位置信息和求导后的方程，得到第四航向角。

目标的轨迹方程可以采用加权递推最小二乘公式进行确定，具体地，假设目标在较短时间内是进行二次曲线的运动，其轨迹可以表示为y＝c

其中，

通过加权递推最小二乘公式，即可确定轨迹方程，通过对轨迹方程求导，得到求导后的方程y＝c

S113：基于第一权重、第二权重分别对第三航向角、第四航向角进行加权，得到第一航向角。

在一实施方式中，第一权重和第二权重均与目标的速度相关，且第一权重和第二权重负相关。具体地，第二权重可以基于目标的速度和权重函数得到，权重函数为与目标的速度有关的函数。具体地，可以根据实际情况预定义若干个目标的速度值和每个速度值对应的权重值，利用若干速度值和若干速度值分别对应的权重值进行样条函数拟合，得到权重函数。如图3所示，预先设定好速度在1.0，2.0，4.0，6.0，8.0m/s时的权重值，然后对数据进行样条拟合，得到的样条函数即为权重函数。

获取权重函数后，将目标当前的速度值代入权重函数，即可得到第二权重，第一预设数值与第二权重之差，即为第一权重，在一具体实施方式中，第一预设数值可以为1。第一航向角的计算过程可以参考下述公式。

θ

其中，θ

本申请将感知目标的运动估计与初始航向角估计进行解耦，单独计算初始航向角和第一航向角，再利用第一航向角对初始航向角进行修正，以得到准确度较高的目标航向角。具体地，本申请结合多传感器的原始量测信息与运动状态的滤波结果(即目标的速度信息和轨迹信息)对量测信息进行重新计算，得到第二航向角；再判断第二航向角与预测想想角是否一致，如果不一致，需要将预测航向角进行重置；最后再执行卡尔曼滤波的更新过程，得到目标航向角。

请参阅图4，图4是本申请提供的电子设备一实施方式的框架结构示意图。

电子设备40包括相互耦接的存储器41和处理器42，存储器41存储有程序指令，处理器42用于执行存储器41中存储的程序指令，以实现上述任一方法实施方式的步骤。在一个具体的实施场景中，电子设备40可以包括但不限于：微型计算机、服务器，此外，电子设备40还可以包括笔记本电脑、平板电脑等移动设备，在此不做限定。

具体而言，处理器42用于控制其自身以及存储器41以实现上述任一方法实施方式的步骤。处理器42还可以称为CPU(Central Processing Unit，中央处理单元)。处理器42可能是一种集成电路芯片，具有信号的处理能力。处理器42还可以是通用处理器、数字信号处理器(Digital Signal Processor,DSP)、专用集成电路(Application SpecificIntegrated Circuit,ASIC)、现场可编程门阵列(Field-Programmable Gate Array,FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。另外，处理器42可以由集成电路芯片共同实现。

请参阅图5，图5是本申请提供的计算机可读存储介质一实施方式的框架示意图。

计算机可读存储介质50存储有程序指令51，程序指令51被处理器执行时，用以实现上述任一方法实施方式的步骤。

计算机可读存储介质50具体可以为U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等可以存储计算机程序的介质，或者也可以为存储有该计算机程序的服务器，该服务器可将存储的计算机程序发送给其他设备运行，或者也可以自运行该存储的计算机程序。

若本申请技术方案涉及个人信息，应用本申请技术方案的产品在处理个人信息前，已明确告知个人信息处理规则，并取得个人自主同意。若本申请技术方案涉及敏感个人信息，应用本申请技术方案的产品在处理敏感个人信息前，已取得个人单独同意，并且同时满足“明示同意”的要求。例如，在摄像头等个人信息采集装置处，设置明确显著的标识告知已进入个人信息采集范围，将会对个人信息进行采集，若个人自愿进入采集范围即视为同意对其个人信息进行采集；或者在个人信息处理的装置上，利用明显的标识/信息告知个人信息处理规则的情况下，通过弹窗信息或请个人自行上传其个人信息等方式获得个人授权；其中，个人信息处理规则可包括个人信息处理者、个人信息处理目的、处理方式以及处理的个人信息种类等信息。

以上所述仅为本申请的实施方式，并非因此限制本申请的专利范围，凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。