CN114740448B - 用于车载雷达的目标状态估计方法、装置及存储介质 - Google Patents

用于车载雷达的目标状态估计方法、装置及存储介质 Download PDF

Info

Publication number
CN114740448B
CN114740448B CN202210649541.1A CN202210649541A CN114740448B CN 114740448 B CN114740448 B CN 114740448B CN 202210649541 A CN202210649541 A CN 202210649541A CN 114740448 B CN114740448 B CN 114740448B
Authority
CN
China
Prior art keywords
current frame
target
vector
state
state vector
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202210649541.1A
Other languages
English (en)
Other versions
CN114740448A (zh
Inventor
李�瑞
郭坤鹏
胡溢鑫
张燎
冯友怀
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing Hawkeye Electronic Technology Co Ltd
Original Assignee
Nanjing Hawkeye Electronic Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nanjing Hawkeye Electronic Technology Co Ltd filed Critical Nanjing Hawkeye Electronic Technology Co Ltd
Priority to CN202210649541.1A priority Critical patent/CN114740448B/zh
Publication of CN114740448A publication Critical patent/CN114740448A/zh
Application granted granted Critical
Publication of CN114740448B publication Critical patent/CN114740448B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/02—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00
    • G01S7/41—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
    • G01S7/415—Identification of targets based on measurements of movement associated with the target
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/66—Radar-tracking systems; Analogous systems
    • G01S13/72—Radar-tracking systems; Analogous systems for two-dimensional tracking, e.g. combination of angle and range tracking, track-while-scan radar
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
    • G01S13/931—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/02—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00
    • G01S7/40—Means for monitoring or calibrating
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • General Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

本发明提供了一种用于车载雷达的目标状态估计方法、装置及存储介质，以解决现在技术中的对目标跟踪容易出现目标跟踪中断、丢失等问题。其中，所述方法包括：获取携载所述雷达的车辆的当前状态信息以及所述雷达的探测目标的当前帧的实测状态向量以及前一帧的目标状态向量；基于所述当前状态信息对所述前一帧的目标状态向量进行补偿并根据补偿后的所述前一帧的目标状态向量估计当前帧的预测状态向量；根据预设的方式并基于所述当前帧的实测状态向量以及所述当前帧的预测状态向量确定当前帧的目标状态向量。采用本发明实施例的技术方案能够大幅度提升车载雷达在复杂运动场景中目标跟踪的稳定性和连续性。

Description

用于车载雷达的目标状态估计方法、装置及存储介质
技术领域
本发明涉及车载雷达技术领域，特别涉及一种用于车载雷达的目标状态估计方法、装置及存储介质。
背景技术
近年来，用于车载毫米波雷达的目标状态估计方法成为一个新的研究热点，车载毫米波雷达具有全天候工作的优势，通常被安装在汽车保险杠的正中间，面向汽车的前进方向，被用于汽车环境感知的一部分，车载毫米波雷达通过向环境发射微波，微波遇到目标发生反射，雷达接收器通过回波在短时间内就能探测出目标的速度、角度、距离等。车载毫米波雷达方向性好、探测距离远且受环境（雨雪、风沙等）干扰小，并且成本也低于激光雷达，目前已在辅助驾驶系统中得到广泛应用，能够实现毫米波防撞雷达、自适应巡航、盲区检测、辅助变道等功能。
但是，当车辆在行进过程中，由于车身姿态的不断变化，会导致在使用车载毫米波雷达探测目标的实测过程中，车载毫米波雷达检测点超出目标的关联门限，使得目标跟踪不上而出现中断、丢失等；并且，亦会因为相对速度的变化导致目标的动、静态属性判别出错，进而影响到辅助驾驶的功能决策。
因此，为了解决上述技术问题，亟需提供一种新的用于车载雷达的目标状态估计方法以实现稳定连续跟踪目标。
发明内容
为了克服现有技术的不足，本发明的目的在于提供一种用于车载雷达的目标状态估计方法、装置及存储介质，以解决现在技术中的对目标跟踪容易出现目标跟踪中断、丢失等的问题。
本发明的目的采用以下技术方案实现：
根据本发明的一方面，提供一种用于车载雷达的目标状态估计方法，其中，所述方法包括：获取携载所述雷达的车辆的当前状态信息以及所述雷达的探测目标的当前帧的实测状态向量以及前一帧的目标状态向量；基于所述当前状态信息对所述前一帧的目标状态向量进行补偿并根据补偿后的所述前一帧的目标状态向量估计当前帧的预测状态向量；根据预设的方式并基于所述当前帧的实测状态向量以及所述当前帧的预测状态向量确定当前帧的目标状态向量。
进一步地，所述当前状态信息至少包括当前偏航角速度，所述目标状态向量包括与所述目标相关联的位置信息以及速度信息。
可选地，所述根据预设的方式并基于所述当前帧的实测状态向量以及所述当前帧的预测状态向量确定当前帧的目标状态向量包括：根据扩展卡尔曼滤波方式确定所述当前帧的目标状态向量。
进一步地，根据所述当前偏航角速度计算在预设时间段内的偏航角度；
基于所述偏航角度构造目标状态补偿矩阵，并根据所述目标状态补偿矩阵对所述前一帧的目标状态向量进行补偿。
进一步地，所述目标状态补偿矩阵由下式表示：
Figure DEST_PATH_IMAGE001
；
其中，
Figure DEST_PATH_IMAGE002
表示所述目标状态补偿矩阵，
Figure DEST_PATH_IMAGE003
表示所述偏航角度。
进一步地，所述根据补偿后的所述前一帧的目标状态向量估计当前帧的预测状态向量包括：根据补偿后的所述前一帧的目标状态向量以及预设的状态转移矩阵计算所述当前帧的预测状态向量。
进一步地，所述根据扩展卡尔曼滤波方式确定所述当前帧的目标状态向量包括：根据前一帧的状态协方差、所述预设的状态转移矩阵以及预设的过程噪声协方差计算当前帧的预测状态协方差；根据所述当前帧的预测状态协方差、与所述预测状态向量相关联的雅克比矩阵以及预设的测量噪声矩阵计算卡尔曼增益系数；根据所述当前帧的预测状态向量构造当前帧的预测极坐标向量，并将极坐标形式的所述当前帧的实测状态向量与所述预测极坐标向量相减以得到第一差值向量；将所述第一差值向量乘以所述卡尔曼增益系数，再与所述当前帧的预测状态向量求和，以得到所述当前帧的目标状态向量。
进一步地，所述根据扩展卡尔曼滤波方式确定所述当前帧的目标状态向量还包括：用单位矩阵减去所述卡尔曼增益系数与所述雅克比矩阵的乘积以得到第二差值向量；将所述第二差值向量乘以所述当前帧的预测状态协方差，以得到当前帧的状态协方差。
进一步地，所述基于所述当前状态信息对所述前一帧的目标状态向量进行补偿并根据补偿后的所述前一帧的目标状态向量估计当前帧的预测状态向量，包括：按照下式估计所述当前帧的预测状态向量：
Figure DEST_PATH_IMAGE004
；
其中，
Figure DEST_PATH_IMAGE005
表示所述预设的状态转移矩阵，
Figure DEST_PATH_IMAGE006
表示所述前一帧的目标状态向量，
Figure DEST_PATH_IMAGE007
表示所述当前帧的预测状态向量，
Figure DEST_PATH_IMAGE008
表示所述目标状态补偿矩阵。
进一步地，所述根据前一帧的状态协方差、所述预设的状态转移矩阵以及预设的过程噪声协方差计算当前帧的预测状态协方差，包括：
按照下式计算所述当前帧的预测状态协方差：
Figure DEST_PATH_IMAGE009
；
其中，
Figure DEST_PATH_IMAGE010
表示所述前一帧的状态协方差，
Figure DEST_PATH_IMAGE011
表示所述当前帧的预测状态协方差，
Figure DEST_PATH_IMAGE012
表示所述过程噪声协方差，
Figure 522575DEST_PATH_IMAGE005
表示所述预设的状态转移矩阵，
Figure DEST_PATH_IMAGE013
表示所述预设的状态转移矩阵的转置矩阵。
进一步地，所述根据所述当前帧的预测状态协方差、与所述预测状态向量相关联的雅克比矩阵以及预设的测量噪声矩阵计算卡尔曼增益系数，包括：
按照下式计算所述卡尔曼增益系数：
Figure DEST_PATH_IMAGE014
；
其中，
Figure 296758DEST_PATH_IMAGE011
表示所述当前帧的预测状态协方差，
Figure DEST_PATH_IMAGE015
表示所述雅克比矩阵，
Figure DEST_PATH_IMAGE016
表示所述雅克比矩阵的转置矩阵，
Figure DEST_PATH_IMAGE017
表示所述测量噪声矩阵，
Figure DEST_PATH_IMAGE018
表示所述卡尔曼增益系数。
进一步地，所述根据所述当前帧的预测状态向量构造当前帧的预测极坐标向量，包括：
根据下式构造所述当前帧的预测极坐标向量：
Figure DEST_PATH_IMAGE019
；
其中，
Figure DEST_PATH_IMAGE020
表示所述当前帧的预测极坐标向量，
Figure DEST_PATH_IMAGE021
表示所述当前帧的预测状态向量中x方向的距离分量，
Figure DEST_PATH_IMAGE022
表示所述当前帧的预测状态向量中y方向的距离分量，
Figure DEST_PATH_IMAGE023
表示所述当前帧的预测状态向量中x方向的速度分量，
Figure DEST_PATH_IMAGE024
表示所述当前帧的预测状态向量中y方向的速度分量。
进一步地，根据所述当前帧的预测状态向量构造当前帧的预测极坐标向量，并将极坐标形式的所述当前帧的实测状态向量与所述预测极坐标向量相减以得到第一差值向量，以及将所述第一差值向量乘以所述卡尔曼增益系数，再与所述当前帧的预测状态向量求和，以得到所述当前帧的目标状态向量包括：
按照下式计算所述当前帧的目标状态向量：
Figure DEST_PATH_IMAGE025
；
其中，
Figure DEST_PATH_IMAGE026
表示所述当前帧的预测状态向量，
Figure 413356DEST_PATH_IMAGE018
表示所述卡尔曼增益系数，Z表示所述当前帧的实测状态向量，
Figure 887063DEST_PATH_IMAGE020
表示所述预测极坐标向量，
Figure DEST_PATH_IMAGE027
表示所述当前帧的目标状态向量。
进一步地，所述用单位矩阵减去所述卡尔曼增益系数与所述雅克比矩阵的乘积以得到第二差值向量，并将所述第二差值向量乘以所述当前帧的预测状态协方差，以得到当前帧的状态协方差，包括：
按照下式计算所述当前帧的状态协方差：
Figure DEST_PATH_IMAGE028
；
其中，
Figure DEST_PATH_IMAGE029
是单位矩阵，
Figure DEST_PATH_IMAGE030
表示所述当前帧的状态协方差，K表示所述卡尔曼增益系数，
Figure 856156DEST_PATH_IMAGE015
表示所述雅克比矩阵，
Figure 807932DEST_PATH_IMAGE011
表示所述当前帧的预测状态协方差。
进一步地，所述方法还包括：针对所述目标，根据所述当前帧的目标状态向量中的x方向的速度分量以及y方向的速度分量计算当前帧的该目标的绝对速度；基于所述当前帧的该目标的绝对速度、以及当前帧针对该目标的关联检测点的动静属性进行多帧连续判断以确定该目标的动静属性。
进一步地，所述当前状态信息还包括当前车速，并且所述基于所述当前帧的该目标的绝对速度、以及当前帧针对该目标的关联检测点的动静属性进行多帧连续判断以确定该目标的动静属性，包括：在所述当前帧针对该目标的关联检测点的动静属性为动态的情况下，判断所述当前车速与所述当前帧该目标的绝对速度的差值的绝对值是否大于第一预设阈值，若大于所述第一预设阈值，则对所维护的动态属性次数的值加一，并在所述动态属性次数的值大于第二预设阈值的情况下判定该目标当前帧的动静属性为动态，反之则在下一帧重新进行判断；
在所述当前帧针对该目标的关联检测点的动静属性为静态的情况下，判断所述当前车速与所述当前帧该目标的绝对速度的差值的绝对值是否小于第三预设阈值，若小于所述第三预设阈值，则对所维护的静态属性次数的值加一，并在所述静态属性次数的值大于第四预设阈值的情况下，判定该目标当前帧的动静属性为静态，反之则在下一帧重新进行判断。
根据本发明另一方面还提供一种用于车载雷达的目标状态估计装置，所述装置包括：信息获取单元，用于获取携载所述雷达的车辆的当前状态信息以及所述雷达的探测目标的当前帧的实测状态向量以及前一帧的目标状态向量；预测状态向量估计单元，用于基于所述当前状态信息对所述前一帧的目标状态向量进行补偿并根据补偿后的所述前一帧的目标状态向量估计当前帧的预测状态向量；目标状态向量确定单元，用于根据预设的方式并基于所述当前帧的实测状态向量以及所述当前帧的预测状态向量确定当前帧的目标状态向量。
根据本发明另一方面还提供一种计算机可读存储介质，包括上述用于车载雷达的目标状态估计方法。
相比现有技术，本发明实施例提供的用于车载雷达的目标状态估计方法、装置及存储介质，解决了现在技术中的对目标跟踪容易出现目标跟踪中断、丢失等问题，能够大幅度提升车载雷达在复杂运动场景中目标跟踪的稳定性和连续性，同时进一步提高目标动静态属性的准确度。
进一步地，对目标动静属性（类别）的判断则是基于本车车速与目标跟踪后的绝对速度进行判断，目标跟踪后的绝对速度相比于目标的径向速度更加稳定可靠，更加接近真实目标的运动情况，然后再基于所述当前帧的该目标的绝对速度、以及当前帧针对该目标的关联检测点的动静属性进行多帧连续判断以确定该目标的动静属性。从而能够避免由于目标频繁的动静态转变而影响ADAS（Advanced Driving Assistance System，高级驾驶辅助系统）功能决策。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的一种用于车载雷达的目标状态估计方法的流程示意图。
图2是毫米波雷达测量目标的极坐标示意图。
图3是本发明实施例提供的经补偿运动平台（携载雷达的车辆）的偏航角度后目标坐标信息的示意图。
图4是本发明实施例提供的针对目标的动静态属性判断的流程图。
图5是本发明实施例提供的一种用于车载雷达的目标状态估计装置的结构框图。
具体实施方式
上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其他目的、特征和优点能够更明显易懂，以下特举较佳实施例，并配合附图，详细说明如下。
在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接或可以相互通讯；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
图1是本发明实施例提供的一种用于车载雷达的目标状态估计方法的流程示意图。
参考图1所示，本发明实施例提供的一种用于车载雷达的目标状态估计方法包括以下步骤：
步骤S10，获取携载所述雷达的车辆的当前状态信息以及所述雷达的探测目标的当前帧的实测状态向量以及前一帧的目标状态向量；
步骤S20，基于所述当前状态信息对所述前一帧的目标状态向量进行补偿并根据补偿后的所述前一帧的目标状态向量估计当前帧的预测状态向量；
步骤S30，根据预设的方式并基于所述当前帧的实测状态向量以及所述当前帧的预测状态向量确定当前帧的目标状态向量。
以下将具体描述步骤S10至S30。
在步骤S10中，获取携载所述雷达的车辆的当前状态信息，所述当前状态信息至少包括当前偏航角速度和当前速度。
获取所述雷达的探测目标的当前帧的实测状态向量，例如，根据该雷达每帧返回的目标探测数据实时计算所述目标相对于所述雷达的距离以及所述目标相对于所述雷达的径向速度，以构造所述雷达的探测目标的当前帧的实测状态向量。示例性地，所述目标状态向量包括与所述目标相关联的位置信息以及速度信息。
此外，还获取所述雷达的探测目标的前一帧的目标状态向量，该前一帧的目标状态向量通常指的是前一帧的最优状态估计的目标向量。
在步骤S20中，基于所述当前状态信息对所述前一帧的目标状态向量进行补偿并根据补偿后的所述前一帧的目标状态向量估计当前帧的预测状态向量；具体地，获取运动平台（携载雷达的车辆）当前姿态改变量，包括当前运动平台（携载雷达的车辆）的三轴偏离原始三轴的角度增量，以及沿原始三轴的速度增量；根据运动平台（携载雷达的车辆）当前姿态改变量计算得到运动平台（携载雷达的车辆）的当前偏航角度，通过将运动平台（携载雷达的车辆）自身的姿态变化，补偿至前一帧的目标状态向量中，可以避免运动平台（携载雷达的车辆）在大角度转弯过程中导致目标跟踪的丢失。
在步骤S30中，根据预设的方式并基于所述当前帧的实测状态向量以及所述当前帧的预测状态向量确定当前帧的目标状态向量。
示例性地，本发明实施例中，对车辆周围的目标跟踪可根据扩展卡尔曼滤波方式确定所述当前帧的目标状态向量。具体过程是：先进行初始化，根据不同的运动模型，将各个变量进行初始化设置，以得到一个初始的目标状态向量；完成初始化后，根据前一帧的目标状态向量，基于不同运动模型下的预设的状态转移矩阵，预测当前帧的目标状态向量；将预测当前帧的目标状态向量与当前帧的实测状态向量通过加权计算以确定当前帧的目标状态向量；同样地，根据当前帧的目标状态向量做下一帧的预测，再与下一帧的实际观测结果进行加权，就可以得到下一帧的目标状态向量；通过再预测、再观测，如此往复即可实现对目标的稳定连续跟踪。
本发明实施例所提供的技术方案，将运动平台（携载雷达的车辆）自身的姿态变化，补偿至前一帧的目标状态向量中，可以避免运动平台（携载雷达的车辆）在大角度转弯过程中导致目标跟踪的丢失，并且可以大幅度提升车载雷达在复杂运动场景中目标跟踪的稳定性和连续性。
需要说明的是，毫米波雷达观察世界的方式与激光雷达有所不同。激光雷达测量的原理是光的直线传播，因此在测量时能直接获得目标（障碍物）在笛卡尔坐标系下x方向、y方向和z方向上的距离；而毫米波雷达的原理是多普勒效应，它所测量的数据都是在极坐标系下的。图2是毫米波雷达测量目标的极坐标示意图。如图2所示，毫米波雷达能够测量目标在极坐标下离雷达的距离
Figure DEST_PATH_IMAGE031
、方向角
Figure DEST_PATH_IMAGE032
以及距离变量率（径向速度）
Figure DEST_PATH_IMAGE033
。
具体地，携载所述雷达的车辆的航迹状态向量表示为：
Figure DEST_PATH_IMAGE034
所述基于所述当前状态信息对所述前一帧的目标状态向量进行补偿，包括：根据所述当前偏航角速度计算在预设时间段内的偏航角度，然后基于所述偏航角度构造目标状态补偿矩阵，并根据所述目标状态补偿矩阵对所述前一帧的目标状态向量进行补偿。例如，
Figure DEST_PATH_IMAGE035
，其中，
Figure DEST_PATH_IMAGE036
表示预设时间段内的偏航角度，
Figure DEST_PATH_IMAGE037
表示预设的时间段，
Figure DEST_PATH_IMAGE038
表示所述当前偏航角速度。
所述目标状态补偿矩阵由下式表示：
Figure DEST_PATH_IMAGE039
；
其中，
Figure 933364DEST_PATH_IMAGE008
表示所述目标状态补偿矩阵，
Figure 781234DEST_PATH_IMAGE036
表示所述偏航角度。
经补偿后的前一帧的目标状态向量，例如由下式表示：
Figure DEST_PATH_IMAGE040
；
其中，
Figure DEST_PATH_IMAGE041
表示前一帧的目标状态向量，
Figure DEST_PATH_IMAGE042
表示经补偿后的前一帧的目标状态向量，
Figure 565519DEST_PATH_IMAGE008
表示目标状态补偿矩阵。
示例性地，前一帧的目标状态向量为一个4*1的矩阵，可用下式表示：
Figure DEST_PATH_IMAGE043
；
其中，
Figure DEST_PATH_IMAGE044
表示前一帧的目标状态向量，
Figure DEST_PATH_IMAGE045
表示前一帧的目标状态向量中x方向的距离分量，
Figure DEST_PATH_IMAGE046
表示前一帧的目标状态向量中y方向的距离分量，
Figure DEST_PATH_IMAGE047
表示前一帧的目标状态向量中x方向的速度分量，
Figure DEST_PATH_IMAGE048
表示前一帧的目标状态向量中y方向的速度分量。
图3是本发明实施例提供的经补偿运动平台（携载雷达的车辆）的偏航角度后目标坐标信息的示意图。
具体地，如图3所示，细线轴坐标系为前一帧的目标距离雷达的原始坐标系，粗线轴坐标系为当前基于运动平台（携载雷达的车辆）姿态改变后的坐标系，经补偿运动平台（携载雷达的车辆）的偏航角度后，即可生成基于在粗线轴坐标系下的前一帧的目标状态向量。
经补偿后的前一帧的目标状态向量可用下式表示：
Figure DEST_PATH_IMAGE049
；
其中，各个变量表示如下：
Figure DEST_PATH_IMAGE050
，
Figure DEST_PATH_IMAGE051
，
Figure DEST_PATH_IMAGE052
，
Figure DEST_PATH_IMAGE053
；
其中，
Figure DEST_PATH_IMAGE054
表示经补偿后的目标状态向量，
Figure DEST_PATH_IMAGE055
表示经补偿后的前一帧的目标状态向量中x方向的距离分量，
Figure DEST_PATH_IMAGE056
表示经补偿后的前一帧的目标状态向量中y方向的距离分量，
Figure DEST_PATH_IMAGE057
表示经补偿后的前一帧的目标状态向量中x方向的速度分量，
Figure DEST_PATH_IMAGE058
表示经补偿后的前一帧的目标状态向量中y方向的速度分量。
所述根据补偿后的所述前一帧的目标状态向量估计当前帧的预测状态向量包括：根据补偿后的所述前一帧的目标状态向量以及预设的状态转移矩阵计算所述当前帧的预测状态向量。
具体地，所述根据扩展卡尔曼滤波方式确定所述当前帧的目标状态向量包括：根据前一帧的状态协方差、所述预设的状态转移矩阵以及预设的过程噪声协方差计算当前帧的预测状态协方差；根据所述当前帧的预测状态协方差、与所述预测状态向量相关联的雅克比矩阵以及预设的测量噪声矩阵计算卡尔曼增益系数；根据所述当前帧的预测状态向量构造当前帧的预测极坐标向量，并将极坐标形式的所述当前帧的实测状态向量与所述预测极坐标向量相减以得到第一差值向量；将所述第一差值向量乘以所述卡尔曼增益系数，再与所述当前帧的预测状态向量求和，以得到所述当前帧的目标状态向量。
进一步地，用单位矩阵减去所述卡尔曼增益系数与所述雅克比矩阵的乘积以得到第二差值向量；将所述第二差值向量乘以所述当前帧的预测状态协方差，以得到当前帧的状态协方差。
首先，根据扩展卡尔曼滤波方式的第一个公式计算当前帧的预测状态向量，按照下式估计所述当前帧的预测状态向量：
Figure 180040DEST_PATH_IMAGE004
；
其中，
Figure 897330DEST_PATH_IMAGE005
表示所述预设的状态转移矩阵，
Figure 916101DEST_PATH_IMAGE006
表示所述前一帧的目标状态向量，
Figure 859786DEST_PATH_IMAGE007
表示所述当前帧的预测状态向量，
Figure 294310DEST_PATH_IMAGE008
表示所述目标状态补偿矩阵。
也即，
Figure 426958DEST_PATH_IMAGE026
是根据经补偿后的前一帧的目标状态向量所预测出的当前帧的预测状态向量。
示例性地，以2维的匀速运动模型为例，前一帧的目标状态向量表示如下：
Figure 147789DEST_PATH_IMAGE043
；
其中，
Figure 782033DEST_PATH_IMAGE041
表示前一帧的目标状态向量，
Figure DEST_PATH_IMAGE059
表示前一帧的目标状态向量中x方向的距离分量，
Figure 941618DEST_PATH_IMAGE046
表示前一帧的目标状态向量中y方向的距离分量，
Figure 774445DEST_PATH_IMAGE047
表示前一帧的目标状态向量中x方向的速度分量，
Figure 541544DEST_PATH_IMAGE048
表示前一帧的目标状态向量中y方向的速度分量。
经补偿运动平台（携载雷达的车辆）的偏航角度后，得到补偿后的前一帧的目标状态向量表示如下：
Figure DEST_PATH_IMAGE060
；
其中，
Figure 413816DEST_PATH_IMAGE054
表示经补偿后的目标状态向量，
Figure 190142DEST_PATH_IMAGE055
表示经补偿后的前一帧的目标状态向量中x方向的距离分量，
Figure 611896DEST_PATH_IMAGE056
表示经补偿后的前一帧的目标状态向量中y方向的距离分量，
Figure 64743DEST_PATH_IMAGE057
表示经补偿后的前一帧的目标状态向量中x方向的速度分量，
Figure 204738DEST_PATH_IMAGE058
表示经补偿后的前一帧的目标状态向量中y方向的速度分量。
系统的预设的状态转移矩阵
Figure DEST_PATH_IMAGE061
表示为：
Figure DEST_PATH_IMAGE062
；
其中，T表示单位间隔时间。
根据匀速运动模型的公式：s1=s0+vt，经过时间△t后的预测状态向量应该是：
Figure DEST_PATH_IMAGE063
；
其中，
Figure 40599DEST_PATH_IMAGE055
表示经补偿后的前一帧的目标状态向量中x方向的距离分量，
Figure 582439DEST_PATH_IMAGE056
表示经补偿后的前一帧的目标状态向量中y方向的距离分量，
Figure 956919DEST_PATH_IMAGE057
表示经补偿后的前一帧的目标状态向量中x方向的速度分量，
Figure 849789DEST_PATH_IMAGE058
表示经补偿后的前一帧的目标状态向量中y方向的速度分量。
接下来，根据扩展卡尔曼滤波方式的第二个公式计算所述当前帧的预测状态协方差，按照下式计算所述当前帧的预测状态协方差：
Figure DEST_PATH_IMAGE064
；
其中，
Figure 482764DEST_PATH_IMAGE010
表示所述前一帧的状态协方差，
Figure 20056DEST_PATH_IMAGE011
表示所述当前帧的预测状态协方差，
Figure 690072DEST_PATH_IMAGE012
表示所述过程噪声协方差，
Figure 7921DEST_PATH_IMAGE005
表示所述预设的状态转移矩阵，
Figure 946052DEST_PATH_IMAGE013
表示所述预设的状态转移矩阵。
需要说明的是，上述公式中的
Figure 337850DEST_PATH_IMAGE010
表示所述前一帧的不确定的程度，这个不确定程度，在卡尔曼滤波器初始化时会很大，随着越来越多的数据注入滤波器中，不确定程度会变小，
Figure 178767DEST_PATH_IMAGE030
的专业术语叫状态协方差矩阵（state covariance matrix）；这里的
Figure DEST_PATH_IMAGE065
表示过程噪声协方差矩阵（process covariance matrix），即无法用
Figure DEST_PATH_IMAGE066
表示的噪声，例如，车辆运动时突然到了上坡，这个影响是无法用之前的状态转移方程来估计了。
示例性地，构成加速度过程噪声协方差矩阵
Figure 842967DEST_PATH_IMAGE065
可由下式表示：
Figure DEST_PATH_IMAGE067
；
其中，
Figure DEST_PATH_IMAGE068
表示沿x轴方向的加速度，
Figure DEST_PATH_IMAGE069
表示沿y轴方向的加速度，T表示单位间隔时间。
示例性地，状态协方差矩阵
Figure 785122DEST_PATH_IMAGE030
可由下式表示：
Figure DEST_PATH_IMAGE070
；
由于毫米波雷达测量目标（障碍物）在径向上的位置和速度相对准确，不确定度较低，因此可以对状态协方差矩阵
Figure 890481DEST_PATH_IMAGE030
进行如下初始化：
Figure DEST_PATH_IMAGE071
。
由于过程噪声矩阵
Figure 558092DEST_PATH_IMAGE065
对整个系统存在影响，但又不能太确定对系统的影响有多大。而且不同的车载雷达产品过程噪声矩阵
Figure 257057DEST_PATH_IMAGE065
也不尽相同。对于简单的模型来说，这里可以直接使用单位矩阵或空值进行计算，即：
Figure DEST_PATH_IMAGE072
。
由于得到所述当前帧的目标状态向量，需要先根据所述当前帧的预测状态向量构造当前帧的预测极坐标向量，然后将极坐标形式的所述当前帧的实测状态向量与所述预测极坐标向量相减以得到第一差值向量，以及将所述第一差值向量乘以所述卡尔曼增益系数，再与所述当前帧的预测状态向量求和。
所以，根据扩展卡尔曼滤波方式的第三个公式计算卡尔曼增益系数，按照下式计算所述卡尔曼增益系数：
Figure DEST_PATH_IMAGE073
；
其中，
Figure 599308DEST_PATH_IMAGE011
表示所述当前帧的预测状态协方差，
Figure 28015DEST_PATH_IMAGE015
表示所述雅克比矩阵，
Figure 86101DEST_PATH_IMAGE017
表示所述测量噪声矩阵，
Figure 662576DEST_PATH_IMAGE018
表示所述卡尔曼增益系数。
式中所述雅克比矩阵表示如下：
Figure DEST_PATH_IMAGE074
。
需要说明的是，所述卡尔曼增益系数
Figure 244736DEST_PATH_IMAGE018
就是为了求所述第一差值向量的权值，
Figure 465633DEST_PATH_IMAGE018
的取值范围为[0，1]；
Figure 288095DEST_PATH_IMAGE017
表示所述测量噪声矩阵（measurement covariance matrix），表示的是测量值与真值之间的差值。一般地，雷达传感器的厂家会提供，也可以通过实际测试和调试得到。
示例性地，测量噪声矩阵
Figure 86287DEST_PATH_IMAGE017
可由下式表示：
Figure DEST_PATH_IMAGE075
；
其中，
Figure DEST_PATH_IMAGE076
表示在距离维度的误差，
Figure DEST_PATH_IMAGE077
表示在角度维度的误差，
Figure DEST_PATH_IMAGE078
表示在径向速度的误差。
故所述根据所述当前帧的预测状态向量构造当前帧的预测极坐标向量，包括：根据下式构造所述当前帧的预测极坐标向量：
Figure DEST_PATH_IMAGE079
；
其中，
Figure 564148DEST_PATH_IMAGE020
表示所述当前帧的预测极坐标向量，
Figure 498606DEST_PATH_IMAGE021
表示所述当前帧的预测状态向量中x方向的距离分量，
Figure DEST_PATH_IMAGE080
表示所述当前帧的预测状态向量中y方向的距离分量，
Figure 960812DEST_PATH_IMAGE023
表示所述当前帧的预测状态向量中x方向的速度分量，
Figure 872398DEST_PATH_IMAGE024
表示所述当前帧的预测状态向量中y方向的速度分量。
接下来，根据扩展卡尔曼滤波方式的第四个公式计算所述当前帧的目标状态向量：
Figure DEST_PATH_IMAGE081
；
其中，
Figure 547093DEST_PATH_IMAGE026
表示所述当前帧的预测状态向量，
Figure 601637DEST_PATH_IMAGE018
表示所述卡尔曼增益系数，Z表示所述当前帧的实测状态向量，
Figure 765902DEST_PATH_IMAGE020
表示所述预测极坐标向量，
Figure 397740DEST_PATH_IMAGE027
表示所述当前帧的目标状态向量。同时，将得到的所述当前帧的目标状态向量
Figure 469602DEST_PATH_IMAGE027
，再去更新第一个公式中的前一帧的目标状态向量，以作为前一帧的最优的估计状态信息。
毫米波雷达观测当前帧的实测状态向量Z是包含位置、角度和径向速度的3*1的列向量，而目标状态向量
Figure 519597DEST_PATH_IMAGE027
是包含位置和速度信息的4*1的向量，根据上述公式可知，所述雅克比矩阵（测量矩阵）的维度是4*3的向量。
接下来，根据扩展卡尔曼滤波方式的第五个公式计算所述当前帧的状态协方差。具体地，用单位矩阵减去所述卡尔曼增益系数与所述雅克比矩阵的乘积以得到第二差值向量，并将所述第二差值向量乘以所述当前帧的预测状态协方差，以得到当前帧的状态协方差。
按照下式计算所述当前帧的状态协方差：
Figure DEST_PATH_IMAGE082
；
其中，
Figure DEST_PATH_IMAGE083
是单位矩阵，
Figure 71408DEST_PATH_IMAGE030
表示所述当前帧的状态协方差，K表示所述卡尔曼增益系数，
Figure 206854DEST_PATH_IMAGE015
表示所述雅克比矩阵，
Figure 816827DEST_PATH_IMAGE011
表示所述当前帧的预测状态协方差。同时，将得到的所述当前帧的状态协方差
Figure 236176DEST_PATH_IMAGE030
，再去更新第二个公式中的前一帧的状态协方差，以作为前一阵的最优的状态协方差。
通过再预测、再观测，如此往复即可实现对目标的稳定连续跟踪。
常用技术中，一般根据本车的运动速度结合目标关联检测点信息计算目标的对地径向速度的绝对值；根据动静态判断阈值大小对动静状态判断阈值进行区间划分，将目标的对比径向速度的绝对值与动静状态判断阈值进行比较，判断目标的对地径向速度的绝对值所处区间；根据目标的对地径向速度的绝对值所处区间结合本车状态和目标距离信息判断目标所处状态。这种单传依靠目标关联检测点的区分动目标静态的方法，会由于目标关联检测点存在虚影、杂波、假点等不利因素造成判别出错的概率大大增加，从而影响汽车驾驶的相关决策判断。
而且，常用技术中，采用的是将目标的径向速度（径向距离变化率）与本车车速在目标方位角度上的余弦乘积相比较的方法来对目标进行跟踪，这种单纯的依靠径向速度进行判断可能会因为径向速度的解错而导致动静属性（类别）判断出错，例如，当目标横穿于汽车的正前方时，目标的径向速度会从大变小，0，然后再从小变大，很容易造成动静类别判断出错。
而在本发明实施例中，对目标动静属性（类别）的判断则是基于本车车速与目标跟踪后的绝对速度进行判断，目标跟踪后的绝对速度相比于目标的径向速度更加稳定可靠，更加接近真实目标的运动情况，然后再基于所述当前帧的该目标的绝对速度、以及当前帧针对该目标的关联检测点的动静属性进行多帧连续判断以确定该目标的动静属性。从而能够避免由于目标频繁的动静态转变而影响ADAS（Advanced Driving Assistance System，高级驾驶辅助系统）功能决策。
具体地，本发明实施例用于车载雷达的目标状态估计方法还包括：针对所述目标，根据所述当前帧的目标状态向量中的x方向的速度分量以及y方向的速度分量计算当前帧的该目标的绝对速度，基于所述当前帧的该目标的绝对速度、以及当前帧针对该目标的关联检测点的动静属性进行多帧连续判断以确定该目标的动静属性。
图4是本发明实施例提供的针对目标的动静态属性判断的流程图。
本车的当前状态信息还包括当前车速，示例性地，如图4所示，所述基于所述当前帧的该目标的绝对速度、以及当前帧针对该目标的关联检测点的动静属性进行多帧连续判断以确定该目标的动静属性，包括：在所述当前帧针对该目标的关联检测点的动静属性为动态的情况下，判断所述当前车速与所述当前帧该目标的绝对速度的差值的绝对值是否大于第一预设阈值，若大于所述第一预设阈值，则对所维护的动态属性次数的值加一，并在所述动态属性次数的值大于第二预设阈值的情况下判定该目标当前帧的动静属性为动态，反之则在下一帧重新进行判断；在所述当前帧针对该目标的关联检测点的动静属性为静态的情况下，判断所述当前车速与所述当前帧该目标的绝对速度的差值的绝对值是否小于第三预设阈值，若小于所述第三预设阈值，则对所维护的静态属性次数的值加一，并在所述静态属性次数的值大于第四预设阈值的情况下，判定该目标当前帧的动静属性为静态，反之则在下一帧重新进行判断。
采用本发明的技术方案，可以大幅度提升车载雷达在复杂运动场景中目标跟踪的稳定性和连续性，同时进一步提高目标动静态属性的准确度。
图5是本发明实施例提供的一种用于车载雷达的目标状态估计装置的结构框图。
如图5所示，根据本发明的另一方面，本发明实施例还提供一种用于车载雷达的目标状态估计装置300，所述用于车载雷达的目标状态估计装置300包括：信息获取单元310，用于获取携载所述雷达的车辆的当前状态信息以及所述雷达的探测目标的当前帧的实测状态向量以及前一帧的目标状态向量；预测状态向量估计单元320，用于基于所述当前状态信息对所述前一帧的目标状态向量进行补偿并根据补偿后的所述前一帧的目标状态向量估计当前帧的预测状态向量；目标状态向量确定单元330，用于根据预设的方式并基于所述当前帧的实测状态向量以及所述当前帧的预测状态向量确定当前帧的目标状态向量。
所述当前状态信息至少包括当前偏航角速度，所述目标状态向量包括与所述目标相关联的位置信息以及速度信息。
所述根据预设的方式并基于所述当前帧的实测状态向量以及所述当前帧的预测状态向量确定当前帧的目标状态向量包括：根据扩展卡尔曼滤波方式确定所述当前帧的目标状态向量。
进一步地，根据所述当前偏航角速度计算在预设时间段内的偏航角度；基于所述偏航角度构造目标状态补偿矩阵，并根据所述目标状态补偿矩阵对所述前一帧的目标状态向量进行补偿。
所述目标状态补偿矩阵由下式表示：
Figure DEST_PATH_IMAGE084
；
其中，
Figure 211085DEST_PATH_IMAGE008
表示所述目标状态补偿矩阵，
Figure 161724DEST_PATH_IMAGE036
表示所述偏航角度。
所述根据补偿后的所述前一帧的目标状态向量估计当前帧的预测状态向量包括：根据补偿后的所述前一帧的目标状态向量以及预设的状态转移矩阵计算所述当前帧的预测状态向量。
所述根据扩展卡尔曼滤波方式确定所述当前帧的目标状态向量包括：根据前一帧的状态协方差、所述预设的状态转移矩阵以及预设的过程噪声协方差计算当前帧的预测状态协方差；根据所述当前帧的预测状态协方差、与所述预测状态向量相关联的雅克比矩阵以及预设的测量噪声矩阵计算卡尔曼增益系数；根据所述当前帧的预测状态向量构造当前帧的预测极坐标向量，并将极坐标形式的所述当前帧的实测状态向量与所述预测极坐标向量相减以得到第一差值向量；将所述第一差值向量乘以所述卡尔曼增益系数，再与所述当前帧的预测状态向量求和，以得到所述当前帧的目标状态向量。
进一步地，用单位矩阵减去所述卡尔曼增益系数与所述雅克比矩阵的乘积以得到第二差值向量；将所述第二差值向量乘以所述当前帧的预测状态协方差，以得到当前帧的状态协方差。
所述基于所述当前状态信息对所述前一帧的目标状态向量进行补偿并根据补偿后的所述前一帧的目标状态向量估计当前帧的预测状态向量，包括：按照下式估计所述当前帧的预测状态向量：
Figure DEST_PATH_IMAGE085
；
其中，
Figure 404748DEST_PATH_IMAGE005
表示所述预设的状态转移矩阵，
Figure 288391DEST_PATH_IMAGE006
表示所述前一帧的目标状态向量，
Figure 886731DEST_PATH_IMAGE007
表示所述当前帧的预测状态向量，
Figure 59087DEST_PATH_IMAGE008
表示所述目标状态补偿矩阵。
所述根据前一帧的状态协方差、所述预设的状态转移矩阵以及预设的过程噪声协方差计算当前帧的预测状态协方差，包括：按照下式计算所述当前帧的预测状态协方差：
Figure 10862DEST_PATH_IMAGE009
；
其中，
Figure 889956DEST_PATH_IMAGE010
表示所述前一帧的状态协方差，
Figure 268985DEST_PATH_IMAGE011
表示所述当前帧的预测状态协方差，
Figure 348543DEST_PATH_IMAGE012
表示所述过程噪声协方差，
Figure 369589DEST_PATH_IMAGE005
表示所述预设的状态转移矩阵。
所述根据所述当前帧的预测状态协方差、与所述预测状态向量相关联的雅克比矩阵以及预设的测量噪声矩阵计算卡尔曼增益系数，包括：
按照下式计算所述卡尔曼增益系数：
Figure DEST_PATH_IMAGE086
；
其中，
Figure 368769DEST_PATH_IMAGE011
表示所述当前帧的预测状态协方差，
Figure 574491DEST_PATH_IMAGE015
表示所述雅克比矩阵，
Figure 518176DEST_PATH_IMAGE017
表示所述测量噪声矩阵，
Figure 15017DEST_PATH_IMAGE018
表示所述卡尔曼增益系数。
所述根据所述当前帧的预测状态向量构造当前帧的预测极坐标向量，包括：
根据下式构造所述当前帧的预测极坐标向量：
Figure DEST_PATH_IMAGE087
；
其中，
Figure DEST_PATH_IMAGE088
表示所述当前帧的预测极坐标向量，
Figure 822698DEST_PATH_IMAGE021
表示所述当前帧的预测状态向量中x方向的距离分量，
Figure 481212DEST_PATH_IMAGE080
表示所述当前帧的预测状态向量中y方向的距离分量，
Figure 115456DEST_PATH_IMAGE023
表示所述当前帧的预测状态向量中x方向的速度分量，
Figure 337359DEST_PATH_IMAGE024
表示所述当前帧的预测状态向量中y方向的速度分量。
根据所述当前帧的预测状态向量构造当前帧的预测极坐标向量，并将极坐标形式的所述当前帧的实测状态向量与所述预测极坐标向量相减以得到第一差值向量，以及将所述第一差值向量乘以所述卡尔曼增益系数，再与所述当前帧的预测状态向量求和，以得到所述当前帧的目标状态向量包括：
按照下式计算所述当前帧的目标状态向量：
Figure DEST_PATH_IMAGE089
；
其中，
Figure 576710DEST_PATH_IMAGE026
表示所述当前帧的预测状态向量，
Figure 734022DEST_PATH_IMAGE018
表示所述卡尔曼增益系数，Z表示所述当前帧的实测状态向量，
Figure 9889DEST_PATH_IMAGE020
表示所述预测极坐标向量，
Figure 910849DEST_PATH_IMAGE027
表示所述当前帧的目标状态向量。
所述用单位矩阵减去所述卡尔曼增益系数与所述雅克比矩阵的乘积以得到第二差值向量，并将所述第二差值向量乘以所述当前帧的预测状态协方差，以得到当前帧的状态协方差，包括：
按照下式计算所述当前帧的状态协方差：
Figure 473549DEST_PATH_IMAGE028
；
其中，
Figure 801762DEST_PATH_IMAGE083
是单位矩阵，
Figure 66390DEST_PATH_IMAGE030
表示所述当前帧的状态协方差，K表示所述卡尔曼增益系数，
Figure 771041DEST_PATH_IMAGE015
表示所述雅克比矩阵，
Figure 516143DEST_PATH_IMAGE011
表示所述当前帧的预测状态协方差。
应当理解，用于车载雷达的目标状态估计装置中各个单元（模块）的执行原理、其他方面以及效果可参见前述实施例的内容，此处不再赘述。
本发明实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机程序，所述计算机程序被处理器执行时实现以上描述的任一实施例的用于车载雷达的目标状态估计方法。
对上述步骤的具体限定和实现方式可以参看用于车载雷达的目标状态估计方法的实施例的步骤以及方法，在此不再赘述。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。
以上对本发明实施例所提供的用于车载雷达的目标状态估计、装置及存储介质进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的技术方案及其核心思想；本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例的技术方案的范围。

Claims (15)

1.一种用于车载雷达的目标状态估计方法，其特征在于，所述方法包括：
获取携载所述雷达的车辆的当前状态信息以及所述雷达的探测目标的当前帧的实测状态向量以及前一帧的目标状态向量；
基于所述当前状态信息对所述前一帧的目标状态向量进行补偿并根据补偿后的所述前一帧的目标状态向量估计当前帧的预测状态向量，其中，所述根据补偿后的所述前一帧的目标状态向量估计当前帧的预测状态向量包括：根据补偿后的所述前一帧的目标状态向量以及预设的状态转移矩阵计算所述当前帧的预测状态向量；
根据扩展卡尔曼滤波方式并基于所述当前帧的实测状态向量以及所述当前帧的预测状态向量确定当前帧的目标状态向量；
其中，所述根据扩展卡尔曼滤波方式确定所述当前帧的目标状态向量包括：
根据前一帧的状态协方差、所述预设的状态转移矩阵以及预设的过程噪声协方差计算当前帧的预测状态协方差；
根据所述当前帧的预测状态协方差、与所述预测状态向量相关联的雅克比矩阵以及预设的测量噪声矩阵计算卡尔曼增益系数；
根据所述当前帧的预测状态向量构造当前帧的预测极坐标向量，并将极坐标形式的所述当前帧的实测状态向量与所述预测极坐标向量相减以得到第一差值向量；
将所述第一差值向量乘以所述卡尔曼增益系数，再与所述当前帧的预测状态向量求和，以得到所述当前帧的目标状态向量。
2.如权利要求1所述的用于车载雷达的目标状态估计方法，其特征在于，所述当前状态信息至少包括当前偏航角速度，所述目标状态向量包括与所述目标相关联的位置信息以及速度信息。
3.如权利要求2所述的用于车载雷达的目标状态估计方法，其特征在于，
根据所述当前偏航角速度计算在预设时间段内的偏航角度；
基于所述偏航角度构造目标状态补偿矩阵，并根据所述目标状态补偿矩阵对所述前一帧的目标状态向量进行补偿。
4.如权利要求3所述的用于车载雷达的目标状态估计方法，其特征在于，
所述目标状态补偿矩阵由下式表示：
Figure 5631DEST_PATH_IMAGE001
；
其中，
Figure 222986DEST_PATH_IMAGE002
表示所述目标状态补偿矩阵，
Figure 429976DEST_PATH_IMAGE003
表示所述偏航角度。
5.如权利要求1所述的用于车载雷达的目标状态估计方法，其特征在于，所述根据扩展卡尔曼滤波方式确定所述当前帧的目标状态向量还包括：
用单位矩阵减去所述卡尔曼增益系数与所述雅克比矩阵的乘积以得到第二差值向量；
将所述第二差值向量乘以所述当前帧的预测状态协方差，以得到当前帧的状态协方差。
6.如权利要求5所述的用于车载雷达的目标状态估计方法，其特征在于，所述基于所述当前状态信息对所述前一帧的目标状态向量进行补偿并根据补偿后的所述前一帧的目标状态向量估计当前帧的预测状态向量，包括：
按照下式估计所述当前帧的预测状态向量：
Figure 841628DEST_PATH_IMAGE004
；
其中，
Figure 501280DEST_PATH_IMAGE005
表示所述预设的状态转移矩阵，
Figure 522326DEST_PATH_IMAGE006
表示所述前一帧的目标状态向量，
Figure 318243DEST_PATH_IMAGE007
表示所述当前帧的预测状态向量，
Figure 399332DEST_PATH_IMAGE002
表示所述目标状态补偿矩阵。
7.如权利要求6所述的用于车载雷达的目标状态估计方法，其特征在于，所述根据前一帧的状态协方差、所述预设的状态转移矩阵以及预设的过程噪声协方差计算当前帧的预测状态协方差，包括：
按照下式计算所述当前帧的预测状态协方差：
Figure 546279DEST_PATH_IMAGE008
；
其中，
Figure 371016DEST_PATH_IMAGE009
表示所述前一帧的状态协方差，
Figure 287019DEST_PATH_IMAGE010
表示所述当前帧的预测状态协方差，
Figure 273430DEST_PATH_IMAGE011
表示所述过程噪声协方差，
Figure 907673DEST_PATH_IMAGE005
表示所述预设的状态转移矩阵，
Figure 270522DEST_PATH_IMAGE012
表示所述预设的状态转移矩阵的转置矩阵。
8.如权利要求7所述的用于车载雷达的目标状态估计方法，其特征在于，所述根据所述当前帧的预测状态协方差、与所述预测状态向量相关联的雅克比矩阵以及预设的测量噪声矩阵计算卡尔曼增益系数，包括：
按照下式计算所述卡尔曼增益系数：
Figure 306611DEST_PATH_IMAGE013
；
其中，
Figure 463923DEST_PATH_IMAGE010
表示所述当前帧的预测状态协方差，
Figure 585462DEST_PATH_IMAGE014
表示所述雅克比矩阵，
Figure 752002DEST_PATH_IMAGE015
表示所述雅克比矩阵的转置矩阵，
Figure 377018DEST_PATH_IMAGE016
表示所述测量噪声矩阵，
Figure 475205DEST_PATH_IMAGE017
表示所述卡尔曼增益系数。
9.如权利要求8所述的用于车载雷达的目标状态估计方法，其特征在于，所述根据所述当前帧的预测状态向量构造当前帧的预测极坐标向量，包括：
根据下式构造所述当前帧的预测极坐标向量：
Figure 818461DEST_PATH_IMAGE018
；
其中，
Figure 523112DEST_PATH_IMAGE019
表示所述当前帧的预测极坐标向量，
Figure 330531DEST_PATH_IMAGE020
表示所述当前帧的预测状态向量中x方向的距离分量，
Figure 829646DEST_PATH_IMAGE021
表示所述当前帧的预测状态向量中y方向的距离分量，
Figure 456936DEST_PATH_IMAGE022
表示所述当前帧的预测状态向量中x方向的速度分量，
Figure 902961DEST_PATH_IMAGE023
表示所述当前帧的预测状态向量中y方向的速度分量。
10.如权利要求9所述的用于车载雷达的目标状态估计方法，其特征在于，根据所述当前帧的预测状态向量构造当前帧的预测极坐标向量，并将极坐标形式的所述当前帧的实测状态向量与所述预测极坐标向量相减以得到第一差值向量，以及将所述第一差值向量乘以所述卡尔曼增益系数，再与所述当前帧的预测状态向量求和，以得到所述当前帧的目标状态向量包括：
按照下式计算所述当前帧的目标状态向量：
Figure 564887DEST_PATH_IMAGE024
；
其中，
Figure 438165DEST_PATH_IMAGE007
表示所述当前帧的预测状态向量，
Figure 818330DEST_PATH_IMAGE017
表示所述卡尔曼增益系数，Z表示所述当前帧的实测状态向量，
Figure 802467DEST_PATH_IMAGE019
表示所述预测极坐标向量，
Figure 584478DEST_PATH_IMAGE025
表示所述当前帧的目标状态向量。
11.如权利要求5所述的用于车载雷达的目标状态估计方法，其特征在于，所述用单位矩阵减去所述卡尔曼增益系数与所述雅克比矩阵的乘积以得到第二差值向量，并将所述第二差值向量乘以所述当前帧的预测状态协方差，以得到当前帧的状态协方差，包括：
按照下式计算所述当前帧的状态协方差：
Figure 628657DEST_PATH_IMAGE026
；
其中，
Figure 997584DEST_PATH_IMAGE027
是单位矩阵，
Figure 785412DEST_PATH_IMAGE028
表示所述当前帧的状态协方差，K表示所述卡尔曼增益系数，
Figure 421929DEST_PATH_IMAGE014
表示所述雅克比矩阵，
Figure 637010DEST_PATH_IMAGE010
表示所述当前帧的预测状态协方差。
12.如权利要求1所述的用于车载雷达的目标状态估计方法，其特征在于，所述方法还包括：
针对所述目标，根据所述当前帧的目标状态向量中的x方向的速度分量以及y方向的速度分量计算当前帧的该目标的绝对速度；
基于所述当前帧的该目标的绝对速度、以及当前帧针对该目标的关联检测点的动静属性进行多帧连续判断以确定该目标的动静属性。
13.如权利要求12所述的用于车载雷达的目标状态估计方法，其特征在于，所述当前状态信息还包括当前车速，并且所述基于所述当前帧的该目标的绝对速度、以及当前帧针对该目标的关联检测点的动静属性进行多帧连续判断以确定该目标的动静属性，包括：
在所述当前帧针对该目标的关联检测点的动静属性为动态的情况下，判断所述当前车速与所述当前帧该目标的绝对速度的差值的绝对值是否大于第一预设阈值，若大于所述第一预设阈值，则对所维护的动态属性次数的值加一，并在所述动态属性次数的值大于第二预设阈值的情况下判定该目标当前帧的动静属性为动态，反之则在下一帧重新进行判断；
在所述当前帧针对该目标的关联检测点的动静属性为静态的情况下，判断所述当前车速与所述当前帧该目标的绝对速度的差值的绝对值是否小于第三预设阈值，若小于所述第三预设阈值，则对所维护的静态属性次数的值加一，并在所述静态属性次数的值大于第四预设阈值的情况下，判定该目标当前帧的动静属性为静态，反之则在下一帧重新进行判断。
14.一种用于车载雷达的目标状态估计装置，其特征在于，所述装置包括：
信息获取单元，用于获取携载所述雷达的车辆的当前状态信息以及所述雷达的探测目标的当前帧的实测状态向量以及前一帧的目标状态向量；
预测状态向量估计单元，用于基于所述当前状态信息对所述前一帧的目标状态向量进行补偿并根据补偿后的所述前一帧的目标状态向量估计当前帧的预测状态向量，其中，所述根据补偿后的所述前一帧的目标状态向量估计当前帧的预测状态向量包括：根据补偿后的所述前一帧的目标状态向量以及预设的状态转移矩阵计算所述当前帧的预测状态向量；
目标状态向量确定单元，用于根据扩展卡尔曼滤波方式并基于所述当前帧的实测状态向量以及所述当前帧的预测状态向量确定当前帧的目标状态向量；
其中，所述根据扩展卡尔曼滤波方式确定所述当前帧的目标状态向量包括：
根据前一帧的状态协方差、所述预设的状态转移矩阵以及预设的过程噪声协方差计算当前帧的预测状态协方差；
根据所述当前帧的预测状态协方差、与所述预测状态向量相关联的雅克比矩阵以及预设的测量噪声矩阵计算卡尔曼增益系数；
根据所述当前帧的预测状态向量构造当前帧的预测极坐标向量，并将极坐标形式的所述当前帧的实测状态向量与所述预测极坐标向量相减以得到第一差值向量；
将所述第一差值向量乘以所述卡尔曼增益系数，再与所述当前帧的预测状态向量求和，以得到所述当前帧的目标状态向量。
15.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质中存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1至13中任一项所述的用于车载雷达的目标状态估计方法。
CN202210649541.1A 2022-06-10 2022-06-10 用于车载雷达的目标状态估计方法、装置及存储介质 Active CN114740448B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210649541.1A CN114740448B (zh) 2022-06-10 2022-06-10 用于车载雷达的目标状态估计方法、装置及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210649541.1A CN114740448B (zh) 2022-06-10 2022-06-10 用于车载雷达的目标状态估计方法、装置及存储介质

Publications (2)

Publication Number Publication Date
CN114740448A CN114740448A (zh) 2022-07-12
CN114740448B true CN114740448B (zh) 2022-09-02

Family

ID=82287161

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210649541.1A Active CN114740448B (zh) 2022-06-10 2022-06-10 用于车载雷达的目标状态估计方法、装置及存储介质

Country Status (1)

Country Link
CN (1) CN114740448B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115327501B (zh) * 2022-10-13 2022-12-23 南京隼眼电子科技有限公司 水平安装角自标定方法、装置、电子设备及存储介质

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102034112A (zh) * 2010-12-17 2011-04-27 浙江大学 一种相控阵三维声学摄像声纳动静目标识别方法
CN104502907A (zh) * 2014-12-15 2015-04-08 西安电子工程研究所 一种机载雷达地面动静目标稳定跟踪方法
EP3032273A1 (en) * 2014-12-12 2016-06-15 Panasonic Intellectual Property Management Co., Ltd. On-board radar apparatus and region detection method
CN106772351A (zh) * 2016-11-16 2017-05-31 西安电子科技大学 基于有限步记忆的卡尔曼滤波方法
CN109946694A (zh) * 2019-03-22 2019-06-28 哈尔滨工业大学 基于随机有限集的圆周sar多目标跟踪方法
CN111985379A (zh) * 2020-08-13 2020-11-24 中国第一汽车股份有限公司 基于车载雷达的目标追踪方法、装置、设备及车辆
WO2020262070A1 (ja) * 2019-06-25 2020-12-30 株式会社デンソー 追跡装置

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102034112A (zh) * 2010-12-17 2011-04-27 浙江大学 一种相控阵三维声学摄像声纳动静目标识别方法
EP3032273A1 (en) * 2014-12-12 2016-06-15 Panasonic Intellectual Property Management Co., Ltd. On-board radar apparatus and region detection method
CN104502907A (zh) * 2014-12-15 2015-04-08 西安电子工程研究所 一种机载雷达地面动静目标稳定跟踪方法
CN106772351A (zh) * 2016-11-16 2017-05-31 西安电子科技大学 基于有限步记忆的卡尔曼滤波方法
CN109946694A (zh) * 2019-03-22 2019-06-28 哈尔滨工业大学 基于随机有限集的圆周sar多目标跟踪方法
WO2020262070A1 (ja) * 2019-06-25 2020-12-30 株式会社デンソー 追跡装置
CN111985379A (zh) * 2020-08-13 2020-11-24 中国第一汽车股份有限公司 基于车载雷达的目标追踪方法、装置、设备及车辆

Also Published As

Publication number Publication date
CN114740448A (zh) 2022-07-12

Similar Documents

Publication Publication Date Title
CN111208492B (zh) 车载激光雷达外参标定方法及装置、计算机设备及存储介质
CN112198503A (zh) 一种目标航迹预测优化方法、装置及雷达系统
CN112083441B (zh) 激光雷达和毫米波雷达深度融合的障碍物检测方法及系统
CN110531357A (zh) 估计移动目标在水平面中速度大小的方法和雷达检测系统
US20080300787A1 (en) Method and apparatus for on-vehicle calibration and orientation of object-tracking systems
CN112526521B (zh) 一种汽车毫米波防撞雷达的多目标跟踪方法
CN112098960A (zh) 确定用于道路车辆雷达自动对准控制器的雷达传感器的对准角度的方法
CN112147651B (zh) 一种异步多车协同目标状态鲁棒估计方法
JP2024103654A (ja) 計測精度算出装置、自己位置推定装置、制御方法、プログラム及び記憶媒体
CN114740448B (zh) 用于车载雷达的目标状态估计方法、装置及存储介质
CN112710339A (zh) 用于校准车辆传感器的方法和设备
US20220365193A1 (en) Method for estimating correction angles in a radar sensor for motor vehicles
CN118050707A (zh) 激光雷达的标定方法及装置、存储介质、终端设备
CN110542902B (zh) 一种适用于车载毫米波雷达的简易测高方法
KR20220089392A (ko) 무인 수상선의 지구물리적 내비게이션 장치 및 방법
CN112731321A (zh) 基于mimo认知雷达的移动机器人避障及地图绘制方法
CN113625238B (zh) 车载毫米波雷达俯仰角误差校准方法、装置、存储介质及电子设备
CN116879863B (zh) 一种连续波4d毫米波雷达多目标测量方法及系统
CN112325770B (zh) 一种车端单目视觉测量相对精度置信度评估方法及系统
CN116679276A (zh) 雷达目标动静态判别方法、装置及存储介质
US20240272297A1 (en) Inverse radar sensor model and evidential grid mapping processors
CN116381632B (zh) 雷达横滚角的自标定方法、装置及存储介质
WO2024178603A1 (zh) 一种占据栅格地图生成方法及装置
CN113670296B (zh) 基于超声波的环境地图生成方法及装置
US20230358877A1 (en) Method, Computer Program, Storage Medium, Device for Tracking an Object

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant