CN112050830B - 一种运动状态估计方法及装置 - Google Patents

一种运动状态估计方法及装置 Download PDF

Info

Publication number
CN112050830B
CN112050830B CN201910503710.9A CN201910503710A CN112050830B CN 112050830 B CN112050830 B CN 112050830B CN 201910503710 A CN201910503710 A CN 201910503710A CN 112050830 B CN112050830 B CN 112050830B
Authority
CN
China
Prior art keywords
measurement data
sensor
target reference
reference object
measurement
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
CN201910503710.9A
Other languages
English (en)
Other versions
CN112050830A (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.)
Huawei Technologies Co Ltd
Original Assignee
Huawei Technologies 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 Huawei Technologies Co Ltd filed Critical Huawei Technologies Co Ltd
Priority to CN201910503710.9A priority Critical patent/CN112050830B/zh
Priority to PCT/CN2020/093486 priority patent/WO2020244467A1/zh
Priority to EP20818619.7A priority patent/EP3964863A4/en
Publication of CN112050830A publication Critical patent/CN112050830A/zh
Priority to US17/542,699 priority patent/US20220089166A1/en
Application granted granted Critical
Publication of CN112050830B publication Critical patent/CN112050830B/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
    • 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/02—Systems using reflection of radio waves, e.g. primary radar systems; Analogous systems
    • G01S13/50—Systems of measurement based on relative movement of target
    • G01S13/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • G01S13/589—Velocity or trajectory determination systems; Sense-of-movement determination systems measuring the velocity vector
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • B60W40/10—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to vehicle motion
    • B60W40/105—Speed
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • B60W40/10—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to vehicle motion
    • B60W40/107—Longitudinal acceleration
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • 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/02—Systems using reflection of radio waves, e.g. primary radar systems; Analogous systems
    • G01S13/50—Systems of measurement based on relative movement of target
    • G01S13/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • G01S13/60—Velocity or trajectory determination systems; Sense-of-movement determination systems wherein the transmitter and receiver are mounted on the moving object, e.g. for determining ground speed, drift angle, ground track
    • 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/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • 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/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • G01S13/867—Combination of radar systems with cameras
    • 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
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
    • G01S15/50—Systems of measurement, based on relative movement of the target
    • G01S15/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • G01S15/588—Velocity or trajectory determination systems; Sense-of-movement determination systems measuring the velocity vector
    • 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
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
    • G01S15/50—Systems of measurement, based on relative movement of the target
    • G01S15/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • G01S15/60—Velocity or trajectory determination systems; Sense-of-movement determination systems wherein the transmitter and receiver are mounted on the moving object, e.g. for determining ground speed, drift angle, ground track
    • 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
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/88—Sonar systems specially adapted for specific applications
    • G01S15/93—Sonar systems specially adapted for specific applications for anti-collision purposes
    • G01S15/931—Sonar 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/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
    • 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/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • G01S13/865—Combination of radar systems with lidar systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Acoustics & Sound (AREA)
  • Automation & Control Theory (AREA)
  • Mathematical Physics (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Manufacturing & Machinery (AREA)
  • Electromagnetism (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

本申请实施例提供一种运动状态估计方法及装置，该方法包括：通过第一传感器获得多个测量数据，其中每个所述测量数据至少包括速度测量信息；根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态，所述运动状态至少包括第一传感器的速度矢量。采用本发明实施例，能够获得第一传感器更准确的运动状态。

Description

一种运动状态估计方法及装置
技术领域
本发明涉及车联网技术领域，尤其涉及一种传感器运动状态的估计方法及装置。
背景技术
在先进辅助驾驶***(Advanced Driving Assistant System，ADAS)或者自动驾驶(Autonomous driving，AD)***中通常配置多种传感器，例如雷达(Radar)传感器、超声波传感器、视觉传感器等，用于感知周边环境及目标信息。利用上述传感器获取的信息，可以实现对周边环境及物体的分类、识别以及跟踪等功能，进一步地，利用上述信息可以实现周边环境态势评估以及规划控制等，例如跟踪目标的航迹信息可以作为车辆规划控制的输入，提高车辆规划控制的效率和安全性。上述传感器的平台可以是车载、舰载、机载或者星载***等，传感器平台的运动对上述分类、识别以及跟踪等功能的实现产生影响。具体地，以车载***应用为例，传感器会随着所在车辆运动而运动，传感器视场中的目标(如目标车辆)也会进行运动，那么两者运动状态互相叠加之后，从传感器观测到的目标所做的运动就是不规则的运动。以雷达或者声纳或者超声波传感器为例，如图1所示场景，车辆a配置有雷达或者声纳或者超声波传感器，用于测量目标车辆的位置和速度信息；车辆b为目标车辆，车辆a正常直行，车辆b右拐弯；从图2可以看出，车辆a上的传感器观测到的车辆b运行航迹为不规则航迹。因此，估计传感器的运动状态，并补偿传感器运动的影响，能够有效的提高目标航迹的精度。
获取传感器的运动状态的方式包括：(1)利用全球卫星导航***(GlobalNavigation Satellite System,GNSS)例如全球定位***(Global Positioning System，GPS)卫星进行定位，通过测量多个卫星到本车辆接收机之间的距离，可以计算得到本车辆的具***置，根据连续多个时刻的具***置即可得到本车辆的运动状态。但是民用的GNSS的精度较低，一般在米量级，因此通常存在较大误差。(2)惯性测量单元(Inertialmeasurement unit，IMU)能够测量本车辆的三轴姿态角以及加速度，IMU通过测量得到的本车辆的加速度和姿态角来估计得到本车辆的运动状态。但是IMU具有误差累计的缺点，易受电磁干扰的影响。可以看出，现有技术测量的本车辆的运动状态的误差较大，如何获得更准确的传感器运动状态是本领域的技术人员正在研究的技术问题。
发明内容
本发明实施例公开了一种运动状态估计的方法及装置，能够获得第一传感器更准确的运动状态。
第一方面，本申请实施例提供一种运动状态估计方法，该方法包括：
通过第一传感器获得多个测量数据，其中每个所述测量数据至少包括速度测量信息；
根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态，所述运动状态至少包括第一传感器的速度矢量。
在上述方法中，通过第一传感器获得多个测量数据，根据其中与目标参照物对应的测量数据，所述测量数据至少包含速度测量信息。由于第一传感器与该目标参照物之间存在相对运动，第一传感器的测量数据可以包含对所述相对运动的速度的测量信息，因此，可以基于与目标参照物对应的测量数据，得到第一传感器的运动状态。此外，通常所述目标参照物空间相对传感器的分布多样，特别是与所述第一传感器有不同的几何关系，从而使得所述速度测量数据与所述第一传感器有不同的测量方程，特别是降低测量方程中的测量矩阵的条件数；而且，通过所述与目标参照物对应的测量数据众多，从而有效降低噪声或者干扰对运动状态估计的的影响。因此，利用本发明所述的方法，可以有效利用与目标参照物对应的测量数据，特别是目标参照物相对于传感器的几何关系以及数量，有效降低测量误差或者干扰的影响，从而使得这种确定运动状态的方式获得更高的精度；此外，所述方法利用单帧数据即可得到传感器的运动估计，从而可以获得很好的实时性。
结合第一方面，在第一方面的第一种可能的实现方式中，所述目标参照物为相对于参考系静止的物体。
结合第一方面，或者第一方面的上述任一种可能的实现方式，在第一方面的第二种可能的实现方式中，所述通过第一传感器获得多个测量数据，其中每个所述测量数据至少包括速度测量信息之后，所述根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态之前，还包括：
根据所述目标参照物的特征，从所述多个测量数据中确定与所述目标参照物对应的测量数据。
结合第一方面，或者第一方面的上述任一种可能的实现方式，在第一方面的第三种可能的实现方式中，所述目标参照物的特征包括所述目标参照物的几何特征和/或反射特征。
结合第一方面，或者第一方面的上述任一种可能的实现方式，在第一方面的第四种可能的实现方式中，所述通过第一传感器获得多个测量数据，其中每个所述测量数据至少包括速度测量信息之后，所述根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态之前，还包括：
根据第二传感器的数据，从所述第一传感器的多个测量数据中确定与所述目标参照物对应的测量数据。
结合第一方面，或者第一方面的上述任一种可能的实现方式，在第一方面的第五种可能的实现方式中，所述根据第二传感器的数据，从所述第一传感器的多个测量数据中确定与所述目标参照物对应的测量数据，包括：
将所述第一传感器的测量数据映射到第二传感器的测量数据的空间，或者，
将所述第二传感器的测量数据映射到所述第一传感器的测量数据的空间，或者，
将所述第一传感器的测量数据和第二传感器的测量数据映射到一个公共的空间，
根据由所述第二传感器测量数据确定的目标参照物，通过所述空间，确定所述第一传感器中目标参照物对应的测量数据。
结合第一方面，或者第一方面的上述任一种可能的实现方式，在第一方面的第六种可能的实现方式中，所述根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态，包括：
根据所述多个测量数据中与目标参照物对应的测量数据采用最小二乘LS估计和/或序贯分块滤波的方式，得到所述第一传感器的运动状态。可以理解，采用LS估计和/或序贯滤波估计的方式能够更有效地提高第一传感器运动状态(如速度)的估计精度。
结合第一方面，或者第一方面的上述任一种可能的实现方式，在第一方面的第七种可能的实现方式中，所述根据所述多个测量数据中与目标参照物对应的测量数据采用最小二乘LS估计和/或序贯分块滤波的方式，得到所述第一传感器的运动状态，包括：
根据对所述目标参照物对应的M个径向速度矢量及其对应的测量矩阵，进行序贯滤波，得到所述第一传感器的运动估计值，其中M≥2；所述径向速度矢量由所述多个测量数据中与目标参照物对应的测量数据中的K个径向速度测量值组成，所述对应的测量矩阵由K个方向余矢量组成，其中K≥1。
结合第一方面，或者第一方面的上述任一种可能的实现方式，在第一方面的第八种可能的实现方式中：
所述第一传感器的运动速度矢量为二维矢量，K＝2，其测量矩阵为：
Figure BDA0002087677110000031
其中，θm，i分别为所述目标参照物的第m组测量数据中第i个方位角测量数据，i＝1，2；
或者，所述第一传感器的运动速度矢量为三维矢量，K＝3，其测量矩阵为：
Figure BDA0002087677110000032
其中，θm，i为所述目标参照物的第m组测量数据中第i个方位角测量数据，φm，i为所述目标参照物的第m组测量数据中第i个俯仰角测量数据，i＝1，2，3。
结合第一方面，或者第一方面的上述任一种可能的实现方式，在第一方面的第九种可能的实现方式中，所述序贯滤波的公式如下：
Figure BDA0002087677110000033
Figure BDA0002087677110000034
Pm，1|0＝Pm-1，1|1
Pm，1|1＝(I-Gm-1Hm-1，K)Pm，1|0
其中，
Figure BDA0002087677110000035
为第m次滤波的速度矢量估计值，Gm为增益矩阵，/>
Figure BDA0002087677110000036
为第m个径向速度矢量测量值，Rm，K为第m个径向速度矢量测量误差协方差矩阵，m＝1，2，...，M。
第二方面，本申请实施例提供一种运动状态估计装置，该装置包括处理器、存储器和第一传感器，其中，所述存储器用于存储程序指令，所述处理器用于调用所述程序指令来执行如下操作：
通过所述第一传感器获得多个测量数据，其中每个所述测量数据至少包括速度测量信息；
根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态，所述运动状态至少包括第一传感器的速度矢量。
在上述装置中，通过第一传感器获得多个测量数据，根据其中与目标参照物对应的测量数据，所述测量数据至少包含速度测量信息。由于第一传感器与该目标参照物之间存在相对运动，第一传感器的测量数据可以包含对所述相对运动的速度的测量信息，因此，可以基于与目标参照物对应的测量数据，得到第一传感器的运动状态。此外，通常所述目标参照物空间相对传感器的分布多样，特别是与所述第一传感器有不同的几何关系，从而使得所述速度测量数据与所述第一传感器有不同的测量方程，特别是降低测量方程中的测量矩阵的条件数；而且，通过所述与目标参照物对应的测量数据众多，从而有效降低噪声或者干扰对运动状态估计的的影响。因此，利用本发明所述的方法，可以有效利用与目标参照物对应的测量数据，特别是目标参照物相对于传感器的几何关系以及数量，有效降低测量误差或者干扰的影响，从而使得这种确定运动状态的方式获得更高的精度；此外，所述方法利用单帧数据即可得到传感器的运动估计，从而可以获得很好的实时性。
结合第二方面，在第二方面的第一种可能的实现方式中，所述目标参照物为相对于参考系静止的物体。
结合第二方面，或者第二方面的上述任一种可能的实现方式，在第二方面的第二种可能的实现方式中，所述通过所述第一传感器获得多个测量数据，其中每个所述测量数据至少包括速度测量信息之后，所述根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态之前，所述处理器还用于：
根据所述目标参照物的特征，从所述多个测量数据中确定与所述目标参照物对应的测量数据。
结合第二方面，或者第二方面的上述任一种可能的实现方式，在第二方面的第三种可能的实现方式中，所述目标参照物的特征包括所述目标参照物的几何特征和/或反射特征。
结合第二方面，或者第二方面的上述任一种可能的实现方式，在第二方面的第四种可能的实现方式中，所述通过所述第一传感器获得多个测量数据，其中每个所述测量数据至少包括速度测量信息之后，所述根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态之前，所述处理器还用于：
根据第二传感器的数据，从所述第一传感器的多个测量数据中确定与所述目标参照物对应的测量数据。
结合第二方面，或者第二方面的上述任一种可能的实现方式，在第二方面的第五种可能的实现方式中，所述根据第二传感器的数据，从所述第一传感器的多个测量数据中确定与所述目标参照物对应的测量数据，具体为：
将所述第一传感器的测量数据映射到第二传感器的测量数据的空间，或者，
将所述第二传感器的测量数据映射到所述第一传感器的测量数据的空间，或者，
将所述第一传感器的测量数据和第二传感器的测量数据映射到一个公共的空间，
根据由所述第二传感器测量数据确定的目标参照物，通过所述空间，确定所述第一传感器中目标参照物对应的测量数据。
结合第二方面，或者第二方面的上述任一种可能的实现方式，在第二方面的第六种可能的实现方式中，所述根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态，具体为：
根据所述多个测量数据中与目标参照物对应的测量数据采用最小二乘LS估计和/或序贯分块滤波的方式，得到所述第一传感器的运动状态。可以理解，采用LS估计和/或序贯滤波估计的方式能够更有效地提高第一传感器运动状态(如速度)的估计精度。
结合第二方面，或者第二方面的上述任一种可能的实现方式，在第二方面的第七种可能的实现方式中，所述根据所述多个测量数据中与目标参照物对应的测量数据采用最小二乘LS估计和/或序贯分块滤波的方式，得到所述第一传感器的运动状态，具体为：
根据对所述目标参照物对应的M个径向速度矢量及其对应的测量矩阵，进行序贯滤波，得到所述第一传感器的运动估计值，其中M≥2；所述径向速度矢量由所述多个测量数据中与目标参照物对应的测量数据中的K个径向速度测量值组成，所述对应的测量矩阵由K个方向余矢量组成，其中K≥1。
结合第二方面，或者第二方面的上述任一种可能的实现方式，在第二方面的第八种可能的实现方式中：
所述第一传感器的运动速度矢量为二维矢量，K＝2，其测量矩阵为：
Figure BDA0002087677110000041
其中，θm，i分别为所述目标参照物的第m组测量数据中第i个方位角测量数据，i＝1，2；
或者，所述第一传感器的运动速度矢量为三维矢量，K＝3，其测量矩阵为：
Figure BDA0002087677110000051
其中，θm，i为所述目标参照物的第m组测量数据中第i个方位角测量数据，φmi为所述目标参照物的第m组测量数据中第i个俯仰角测量数据，i＝1，2，3。
结合第二方面，或者第二方面的上述任一种可能的实现方式，在第二方面的第九种可能的实现方式中，所述序贯滤波的公式如下：
Figure BDA0002087677110000052
Figure BDA0002087677110000053
Pm，1|0＝Pm-1，1|1
Pm，1|1＝(I-Gm-1Hm-1，K)Pm，1|0
其中，
Figure BDA0002087677110000054
为第m次滤波的速度矢量估计值，Gm为增益矩阵，/>
Figure BDA0002087677110000055
为第m个径向速度矢量测量值，Rm，K为第m个径向速度矢量测量误差协方差矩阵，m＝1，2，...，M。
第三方面，本申请实施例提供一种运动状态估计装置，该装置包括用于执行第一方面或者第一方面的任一可能的实现方式所描述的方法的部分或全部单元。
通过实施本发明实施例，通过第一传感器获得多个测量数据，根据其中与目标参照物对应的测量数据，所述测量数据至少包含速度测量信息。由于第一传感器与该目标参照物之间存在相对运动，第一传感器的测量数据可以包含对所述相对运动的速度的测量信息，因此，可以基于与目标参照物对应的测量数据，得到第一传感器的运动状态。此外，通常所述目标参照物空间相对传感器的分布多样，特别是与所述第一传感器有不同的几何关系，从而使得所述速度测量数据与所述第一传感器有不同的测量方程，特别是降低测量方程中的测量矩阵的条件数；而且，通过所述与目标参照物对应的测量数据众多，从而有效降低噪声或者干扰对运动状态估计的的影响。因此，利用本发明所述的方法，可以有效利用与目标参照物对应的测量数据，特别是目标参照物相对于传感器的几何关系以及数量，有效降低测量误差或者干扰的影响，从而使得这种确定运动状态的方式获得更高的精度；此外，所述方法利用单帧数据即可得到传感器的运动估计，从而可以获得很好的实时性。进一步地，可以理解，采用LS估计和/或序贯滤波估计的方式能够更有效地提高第一传感器运动状态(如速度)的估计精度。
附图说明
以下对本发明实施例用到的附图进行介绍。
图1是现有技术中的一种车辆运动场景示意图；
图2是现有技术中一种雷达探测的目标对象的运动状态示意图；
图3是本发明实施例提供的一种运动状态估计方法的流程示意图；
图4是本发明实施例提供的一种雷达探测的测量数据的分布示意图；
图5是本发明实施例提供的一种摄像头拍摄画面的示意图；
图6是本发明实施例提供的一种目标参照物从像素坐标向雷达坐标映射的场景示意图；
图7是本发明实施例提供的一种基于雷的运动状态对探测的探测目标的运动状态进行补偿的场景示意图；
图8是本申请实施例提供的一种运动状态估计装置的结构示意图；
图9是本申请实施例提供的又一种运动状态估计装置的结构示意图。
具体实施方式
下面结合本发明实施例中的附图对本发明实施例进行描述。
请参见图3，是本发明实施例提供的一种运动状态估计方法，该方法的执行主体可以是传感器***或者融合感知***或者集成上述***的规划/控制***如辅助驾驶或者自动驾驶***等，可以是软件或者硬件(如与相应传感器通过无线或者有线连接或者集成在一起的运动状态估计装置)形式，以下不同的执行步骤可以集中式也可以分布式实现。
该方法包括但不限于如下步骤：
步骤S301：通过第一传感器获得多个测量数据，其中每个所述测量数据至少包括速度测量信息。
具体地，所述第一传感器可以是雷达、声纳或者超声波传感器，或者具有测量频移能力的测向传感器器，所述测向传感器器通过测量接收信号相对已知频率的频移得到径向速度信息。所述第一传感器可以是车载、舰载、机载或者星载等传感器。例如该传感器可以为车辆、轮船、飞机或者无人机等***上的用于感知环境或者目标的传感器。例如，在辅助驾驶或者无人驾驶场景中，车辆要进行安全可靠的驾驶通常安装上述一种或者多种传感器，对周围环境或者物体的状态(包括运动状态)进行测量，并将测量数据的处理结果作为规划和控制的参考依据。
需要进一步指出的是，此处第一传感器的物理构成可以是一个或者多个物理传感器，例如其中各个物理传感器可以分别测量方位角、俯仰角以及径向速度，也可以是从多个物理传感器的测量数据导出所述方位角、俯仰角以及径向速度,此处不做限定。
所述测量数据至少包括速度测量信息，所述速度测量信息可以是径向速度测量信息，例如周围环境物体或者目标相对于传感器的径向速度，所述测量数据还可以包括角度测量信息，例如目标相对于传感器的方位角和/或俯仰角测量信息；还可以包括目标相对于传感器的距离测量信息。此外，所述测量数据还可以包括周围环境物体或者目标相对于传感器的方向余弦信息；上述测量数据信息还可以是传感器原始测量数据变换之后得到的信息，例如方向余弦信息可以从目标相对于传感器的方位角和/或俯仰角信息得到，或者从目标的直角坐标位置和距离测量得到。
本申请实施例中，以雷达或者声纳传感器为例，该传感器可以周期性或者非周期性地发射信号并从接收到的回波信号中得到测量数据，例如所述发射信号可以是线性调频信号，通过回波信号的时延可以得到目标的距离信息，通过多个回波信号之间的相位差可以得到该目标与传感器之间的径向速度信息，通过传感器的多个发射和/或接收天线阵列几何，可以得到目标相对于传感器的角度例如方位角和/或俯仰角信息。可以理解的是，由于周边环境物体或者目标的多样性，该传感器可以获得多个测量数据,供后续使用。图4示意了雷达传感器在一帧中获得的多个测量数据在空间的位置分布，每个测量数据所在的位置即为该测量数据点包含的位置信息(距离和方位角)所对应的位置。
步骤S302：根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态，所述运动状态至少包括第一传感器的速度矢量。
所述目标参照物可以是相对参考系静止的物体或者目标；例如以车载或者无人机机载传感器为例，所述参考系可以是大地坐标系，或者相对于大地匀速运动的惯性坐标系，所述目标参照物可以是周边环境中的物体，例如护栏、道路边沿、灯杆、建筑物等。以舰载传感器为例，所述目标参照物可以是水面浮标、灯塔、岸边或者岛屿建筑物等。以星载传感器为例，所述目标参照物可以是相对于恒星或者卫星静止或者匀速运动的参照物如飞船等。
在第一种可选的方案中，可以根据目标参照物的特征，从所述多个测量数据中得到所述目标参照物对应的测量数据。
所述目标参照物的特征可以是目标参照物的几何特征如曲线特征如直线、圆弧或者回旋螺线等，或者反射特征如雷达散射截面积(Radar Cross-Section，RCS)等。
以图4雷达测量数据为例，其中雷达测量包括距离、方位和径向速度测量信息。其中目标参照物为如图5所述的护栏或者路沿，所述目标参照物具有明显的几何特征，即其数据为直线或者回旋螺线。利用霍夫变换(Hough Transform)等特征识别技术可以从上述多个测量数据中把上述目标参照物的数据分离出来。
以霍夫变换识别直线几何特征的目标参照物为例，其获取路沿/护栏的过程如下
根据多个雷达距离和方位测量数据变换到霍夫变换空间，例如基于以下公式
Figure BDA0002087677110000071
其中rk和θk为雷达测量的第k个距离和方位角。
Figure BDA0002087677110000072
和ρi为霍夫变换空间参数，对于不同的/>
Figure BDA0002087677110000073
的取值，可以得到不同的ρi取值，典型的/>
Figure BDA0002087677110000074
为0和π之间的离散值。此外，需要指出的是，此处ρi通常对/>
Figure BDA0002087677110000075
经过量化得到。
对多个不同的雷达测量数据rk和θk，可以累积对应的不同参数
Figure BDA0002087677110000076
和ρi的计数或者权重。
在霍夫变换空间中得到一个或者多个峰值对应的参数，例如
利用上述霍夫变换空间中不同参数
Figure BDA0002087677110000077
和ρi的计数或者权重，可以得到一个或者多个计数或者权重峰值对应的参数/>
Figure BDA0002087677110000078
和/>
Figure BDA0002087677110000079
J为整数。
根据一个或者多个峰值对应的参数得到目标参照物对应的测量数据，例如对满足或者近似满足以下公式
Figure BDA00020876771100000710
或者不等式
Figure BDA00020876771100000711
其中Tρ为一个门限值，可以根据距离或者方位角或者上述参数
Figure BDA00020876771100000712
和pi的量化间隔或者分辨率得到。
霍夫变换还可以识别其他几何特征的目标参照物，如圆弧线、回旋螺线等，此处不一一列举。
在第二种可选的方案中，还可以根据第二传感器的数据，从所述第一传感器的多个测量数据中得到所述目标参照物对应的测量数据。
具体地，所述第二传感器可以是视觉传感器如摄像头或者相机传感器，或者成像传感器如红外传感器或者激光雷达传感器等。
所述第二传感器可以测量所述第一传感器探测范围内的目标参照物，包括周边环境、物体或者目标等。
具体地，所述第二传感器可以与第一传感器安装在同一平台上，其数据可以在同一平台上传输；也可以安装在不同的平台上，通过通信管道交换测量数据，例如安装在路边或者其它车载或者机载***通过云端发送或者接收测量数据或者其它辅助信息，如变换参数信息。以第二传感器为摄像头或者摄像头模组为例，该摄像头或者摄像头模组可以用于拍摄雷达或者声纳或者超声波传感器探测范围内的图像或者视频，可以是第一传感器探测范围内的局部或者全部的图像或者视频。其中该图像可以是一帧也可以是多帧。图5是本申请实施例雷达传感器探测范围内摄像头拍摄的视频图像中显示的画面。
根据所述第二传感器的测量数据，可以确定所述目标参照物，例如，该目标参照物可以为相对于参考系静止的物体。
可选的，所述参考系可以是大地等，如前述；
可选的，可以基于传统的分类或者识别方法或者机器学习方法识别所述目标参照物，如通过参数回归、支持向量机、图像分割等方法；也可以基于人工智能(ArtificialIntelligence，AI)如深度学习如深度神经网络等技术手段识别出第二传感器测量数据如视频或者图像中的目标参照物。
可选的，可以根据传感器的应用场景指定一个或者多个景物为目标参照物，例如，指定路边沿、路边标志牌、树木和建筑物中一项或者多项为目标参照物，预先存储该目标参照物的像素特征，从上述第二传感器的测量数据如图像或者视频中搜索与存储的像素特征相同，或者相似的像素特征，如果搜索到则认为所述图像或者视频中存在目标参照物，进而确定目标参照物在所述图像或者视频中的位置。总而言之，可以通过存储目标参照物的特征(包括但不限于像素特征)，然后采用特征对比的方式搜索到上述图像中的目标参照物。
其中，所述根据第二传感器的测量数据，从所述第一传感器的多个测量数据中得到所述目标参照物对应的测量数据，可以包括：
将第一传感器的测量数据映射到第二传感器的测量数据的空间，或者
将第二传感器的测量数据映射到第一传感器的测量数据的空间，或者
将第一传感器的测量数据映射和第二传感器的测量数据映射到一个公共的空间；
根据由第二传感器的测量数据中确定的目标参照物，通过所述空间，确定所述第一传感器中目标参照物对应的测量数据。
可选的，所述第一传感器的测量数据的空间可以是以所述第一传感器的坐标系为参考的空间；所述第二传感器的测量数据的空间可以是以所述第二传感器的坐标系为参考的空间；
所述公共的空间可以是所述两个传感器所在的传感器平台所在的坐标系为参考的空间，例如，可以是车辆坐标系，舰船坐标系或者飞机坐标系等；也可以是大地坐标系或者以某一恒星或者行星或者卫星为参考的坐标系等。可选的，所述第一传感器的测量数据映射和第二传感器的测量数据映射到一个公共的空间，以车辆坐标系为例，可以事先测定第一传感器如雷达在车辆坐标系的安装位置和第二传感器如摄像头的安装位置，将第一传感器的测量数据和第二传感器的测量数据映射到车辆坐标系。
根据所述第一传感器的多个测量数据与目标参照物对应的测量数据可以确定所述传感器的运动状态。
需要说明的是，目标参照物如果为相对于大地坐标系静止的物体，那么由于传感器平台在运动，因此传感器测得的目标参照物相对于传感器平台或者传感器是运动的而非静止的。可以理解的是，在分离得到目标参照物的测量数据之后，可以基于目标参照物的测量数据得到目标参照物的运动状态，或者等价地得到传感器的运动状态，其实现过程如下描述。
以下以第一传感器为雷达、第二传感器为摄像头为例，说明实现过程，此处并不限定具体传感器。
具体来说，可以先将所述雷达得到的多个测量数据和所述摄像头得到目标参照物的数据映射到同一坐标空间；该同一坐标空间可以为二维或者多维的坐标空间，可选的，可以将所述雷达得到的多个测量数据映射到所述摄像头得到的目标参照物所在的图像坐标系中，或者将所述摄像头得到的目标参照物映射到所述雷达得到的多个测量数据所在的雷达坐标系中，或者将二者映射到另外的一个公共坐标空间中。如图6所示，该目标参照物可以为路边沿601，602，603；图6示意了将所述雷达得到的多个测量数据从所在的雷达坐标系映射到目标参照物(通过粗黑线表示)所在的图像坐标系中的场景。
可选的，雷达得到的测量数据从所在的雷达坐标系映射到图像坐标系的投影映射关系式如公式1-1所示。
Figure BDA0002087677110000091
在公式1-1中，A为摄像头(或摄像头模组)的内参矩阵，由摄像头本身决定，决定从像素坐标系到相平面坐标系的映射关系。B为外参矩阵，由摄像头和雷达的相对位置关系决定，决定从相平面坐标系到雷达平面坐标系的映射关系，z1为景深信息。(x，y，z)为在雷达坐标系中的坐标(如果忽略垂直维度信息则z＝0)，(u，v)为目标参照物在像素坐标系中的坐标。
例如，对于无畸变的场景，其内参矩阵和外参矩阵可以分别为：
Figure BDA0002087677110000092
其中f为焦距，R和T表示雷达坐标系和图像坐标系的相对旋转和相对偏移。对于无畸变的场景可以进一步修正，为现有技术，此处不进一步赘述。
雷达测量的位置数据通常为极坐标或者球坐标形式，可以先转换为直角坐标，然后利用上述公式1-1映射到图像平面坐标系。例如上述雷达数据中的距离和方位角可以转换为直角坐标x和y；上述雷达测量数据中的距离、方位角和俯仰角可以转换为直角坐标x、y和z。
可以理解的是，还可以存在其他映射规则，其他映射规则此处不一一举例。
利用上述投影变换1-1，上述雷达测量数据中的位置测量数据变换到图像坐标系，得到对应的像素位置(u，v)。利用该像素位置可以确定对应的雷达数据是否为目标参照物的雷达测量数据。
具体地，利用深度学习或者图像或者视频可以通过目标检测、图像分割或或者语义分割或者实例分割，并对目标参照物建立起其数学表示，例如用边框(Bounding Box)表示。可以确定上述雷达测量数据对应的像素是否落在上述目标参照物的像素点范围内，从而确定对应的雷达测量数据是否对应目标参照物。
作为一种实现，其中一个目标参照物的边框可以用以下F1个不等式描述的区间表示：
aiu+biv≤ci，i＝1，2，...，F1； 1-3
典型情况下F1＝4.如果雷达测量数据对应的像素(u，v)满足上述不等式，则它属于目标参照物对应的数据，否则不是目标参照物对应的数据。
作为另一种实现，其中一个目标参照物的边框可以用F，个不等式描述的区间表示：
ci≤aiu+biv≤di，i＝1，2，...，F2； 1-4
典型情况下F2＝2.如果雷达测量数据对应的像素(u，v)满足上述不等式，则它属于目标参照物对应的数据，否则不是目标参照物对应的数据。
对于如何目标检测、图像分割或者语义分割或者实例分割，获取目标参照物的数学表示，以及判断雷达测量数据属于上述目标参照物的数据的具体实现方式，此处不加限定。
通过以上投影映射，上述雷达测量的多个测量数据和摄像头感知的目标参照物就处于同一个坐标空间，因此可以基于图像或视频检测、识别或者分割所述目标参照物，从而有效确认与目标参照物对应的雷达测量数据。
根据所述目标参照物对应的测量数据，可以确定所述第一传感器的运动状态，其中运动状态至少包括速度矢量；
所述第一传感器的测量数据至少包括速度信息，例如所述速度信息为径向速度信息。进一步地，所述测量数据还可以包括方位角和/或俯仰角信息或者方向余弦信息。
具体地，第一传感器的速度矢量可以根据以下方程确定传感器运动速度的估计：
Figure BDA0002087677110000101
或者等价地，
Figure BDA0002087677110000102
其中vs为第一传感器的速度矢量，vT为目标参照物的速度矢量，对于上述目标参照物而言，vs＝-vT。
因此，可以直接根据公式1-5得到第一传感器的速度矢量vs，或者等价地，根据公式1-6得到目标参照物的速度矢量vT，利用vs＝-vT得到第一传感器的速度矢量vs。以下以1-5为例说明，基于1-6可以等价对应得到，本文不进一步赘述。
上述
Figure BDA0002087677110000107
为所述第k个径向速度测量数据，/>
Figure BDA0002087677110000108
为对应的测量误差，其均值为0，方差为
Figure BDA0002087677110000109
其取值依赖于第一传感器的性能。
以2维速度矢量为例，vs和hk可以分别为
vs＝[vs，x vs，y]T 1-7
hk＝[Λx vy] 1-8
其中vs，x和vs，y为第一传感器速度矢量的两个分量，[ ]T表示矩阵或者矢量的转置；Λx和Λy为方向余弦，可以直接通过第一传感器测量得到，也可以通过以下公式计算得到
Λx＝cosθk，Λy＝sinθk 1-9
其中θk为方位角；
或者
Figure BDA0002087677110000103
其中rk通过距离测量得到，或者通过以下公式计算
Figure BDA0002087677110000104
以3维速度矢量为例，vs和hk可以分别为
vs＝[vs，x vs，y vs，z]T 1-12
hk＝[Λx Λy Λz] 1-13
其中vs，x，vs，y和vs，z为第一传感器速度矢量的三个分量，[ ]T表示矩阵或者矢量的转置；Λx，Λy和Λz为方向余弦，可以直接通过第一传感器测量得到，也可以通过以下公式计算得到
Λx＝cosφkcosθk，Λy＝coSφksinθk，Λz＝sinφk， 1-14
其中θk为方位角，φk为俯仰角；
或者
Figure BDA0002087677110000105
其中rk通过距离测量得到，或者通过以下公式计算
Figure BDA0002087677110000106
利用上述测量方程，根据所述目标参照物对应的测量数据，可以确定所述第一传感器的运动状态，下面例举几种可选的实现方案以便理解。
具体的，可以基于最小二乘(Least Squared，LS)估计和/或序贯分块滤波得到所述第一传感器的运动状态。
方案一：基于最小二乘(Least Squared，LS)估计得到所述第一传感器的运动状态。
具体的，可以基于第一径向速度矢量和及其对应的测量矩阵得到所述第一传感器的速度矢量的最小二乘估计值。可选的，所述速度矢量的最小二乘估计值为：
Figure BDA0002087677110000111
或者
Figure BDA0002087677110000112
其中
Figure BDA0002087677110000113
为传感器最小二乘估计值；
或者
Figure BDA0002087677110000114
其中
Figure BDA0002087677110000115
为传感器正则化最小二乘估计值；R为半正定矩阵或者正定矩阵，用于正则化，例如
R＝α·I 1-20
其中I为N1阶的单位阵，α为非负或者正常数，例如
Figure BDA0002087677110000116
其中，第一径向速度矢量
Figure BDA0002087677110000117
为由所述目标参照物对应的N1个测量数据中的N1个径向速度测量值组成的矢量；矩阵/>
Figure BDA0002087677110000118
为与第一径向速度矢量/>
Figure BDA0002087677110000119
对应的测量矩阵，N1为大于1的正整数。
第一径向速度矢量
Figure BDA00020876771100001110
和对应的测量矩阵/>
Figure BDA00020876771100001111
满足以下测量方程，
Figure BDA00020876771100001112
具体的，所述第一径向速度矢量
Figure BDA00020876771100001113
可以表示为/>
Figure BDA00020876771100001114
其中，/>
Figure BDA00020876771100001115
表示所述目标参照物对应的第i1个径向速度测量值，/>
Figure BDA00020876771100001116
为与之对应的测量误差矢量，由对应的径向速度测量误差组成，如前述；与之相对应，测量矩阵/>
Figure BDA00020876771100001117
可以表示为
Figure BDA00020876771100001118
/>
可选的，以第一传感器获得方位角和径向速度测量数据为例，所述径向速度测量矩阵
Figure BDA00020876771100001119
由/>
Figure BDA00020876771100001120
组成，其中/>
Figure BDA00020876771100001121
为方位角测量值，其中N1≥2。
可选的，以第一传感器获得方位角、俯仰角和径向速度测量数据为例，上述径向速度测量矩阵
Figure BDA00020876771100001122
由/>
Figure BDA00020876771100001123
Figure BDA00020876771100001124
Figure BDA00020876771100001125
组成，其中/>
Figure BDA00020876771100001126
为方位角测量值，/>
Figure BDA00020876771100001127
为俯仰角测量值，其中N1≥3。
类似的，上述测量方程中的径向速度测量矩阵
Figure BDA00020876771100001128
也可以由方向余弦得到，上述径向速度测量矩阵/>
Figure BDA00020876771100001129
由/>
Figure BDA00020876771100001130
组成，其中对于2维速度矢量，N1≥2；对于3维速度矢量，N1≥3。每个方向余弦矢量的各个分量如上所述，此处不进一步赘述。
作为一种实现，上述
Figure BDA00020876771100001131
或者/>
Figure BDA00020876771100001132
的选择，应该使得彼此间隔尽量大，可以得到更为精确的最小二乘估计。彼此间隔尽量大的角度选择可以使得上述测量矩阵的条件数尽可能小。
可选的，所述径向速度矢量中的各个径向速度分量的选择使得对应的测量矩阵的各个列矢量尽量彼此正交；
方案二：基于序贯分块滤波得到所述第一传感器的运动状态
具体的，可以根据M个径向速度矢量及其对应的测量矩阵，基于序贯分块滤波得到所述第一传感器的运动状态，其中序贯滤波每次使用所述目标参照物对应的径向速度矢量由K个径向速度测量数据组成。
可选的，序贯滤波的第m次估计公式如下：
Figure BDA0002087677110000121
其中Gm为增益矩阵；
Figure BDA00020876771100001216
由K个径向速度测量值组成，Hm，K由K个径向速度测量矩阵组成，如前述。对于二维速度矢量估计K≥2；对于三维速度矢量估计K≥3；
可选的，所述增益矩阵可以为
Figure BDA0002087677110000122
其中Rm，K为径向速度矢量测量误差协方差矩阵，例如，可以为
Figure BDA0002087677110000123
Pm，1|1＝(I-Gm-1Hm-1，K)Pm，1|0 1-26
Pm，1|0＝Pm-1，1|1 1-27
可选的，作为一种实现，可以根据先验信息得到初始估计及其协方差P0，1|1＝P0：
P0＝Q 1-28
Figure BDA0002087677110000124
其中，Q为预先设定的速度估计协方差矩阵；
方案三：基于最小二乘和序贯分块滤波得到所述第一传感器的运动状态；
具体的，可以将所述目标参照物对应第一传感器的测量数据分为两部分，其中第一部分数据用于得到所述第一传感器的速度矢量的最小二乘估计值；第二部分数据用于得到所述第一传感器的速度矢量的序贯分块滤波估计值；所述第一传感器的速度矢量的最小二乘估计值作为序贯分块滤波的初始值。
可选的，作为一种实现，可以根据最小二乘估计得到初始估计及其协方差P0，1|1＝P0：
P0＝PLS 1-30
Figure BDA0002087677110000125
其中，
Figure BDA0002087677110000126
或者/>
Figure BDA0002087677110000127
或者，根据正则化最小二乘估计得到初始估计及其协方差P0，1|1＝P0：
P0＝pRLS 1-32
Figure BDA0002087677110000128
其中，
Figure BDA0002087677110000129
其中，
Figure BDA00020876771100001210
为传感器速度的第m次序贯分块滤波值，IK为KxK单位矩阵。
可选的，上述
Figure BDA00020876771100001211
和Hm，K对于不同的m可以互不相同；K的取值对于不同的m可以相同，可以不同，可以视情况不同选择；上述序贯滤波估计可以有效减少测量噪声的影响，从而提高传感器估计的精度。
需要说明的是，可以首先得到目标参照物的运动速度，根据以下关系得到传感器的运动速度
Figure BDA00020876771100001212
Figure BDA00020876771100001213
或者
Figure BDA00020876771100001214
其中
Figure BDA00020876771100001215
为目标参照物速度的最小二乘估计值；
或者
Figure BDA0002087677110000131
Figure BDA0002087677110000132
其中
Figure BDA0002087677110000133
为目标参照物速度的正则化最小二乘估计值；
或者
Figure BDA0002087677110000134
Figure BDA0002087677110000135
其中Gm，
Figure BDA0002087677110000136
Hm，K，Pm-1如前述。
以传感器的方位角和径向速度测量和K＝2为例，第m个第二径向速度矢量表示为
Figure BDA0002087677110000137
其中，/>
Figure BDA0002087677110000138
分别为所述目标参照物的第m组测量数据中的第1和第2个径向速度测量数据；与之对应的测量矩阵为/>
Figure BDA0002087677110000139
其中，θm，i，i＝1，2为所述目标参照物的第m组测量数据中的第i个方位角测量数据。
类似地，以传感器的方位角、俯仰角和径向速度测量和K＝3为例，第m个第二径向速度矢量表示为
Figure BDA00020876771100001310
其中，/>
Figure BDA00020876771100001311
分别为所述目标参照物的第m组测量数据中第i个径向速度测量数据；与之对应的测量矩阵为
Figure BDA00020876771100001312
其中，θm，i，i＝1，2，3分别为所述目标参照物的第m组测量数据中第i个方位角测量数据，φm，i，i＝1，2，3分别为所述目标参照物的第m组测量数据中第i个俯仰角测量数据。
作为一种实现，M组测量数据的选择，应当使得各组测量数据对应的测量矩阵的条件数尽可能小；
作为一种实现，上述θm，i，i＝1，2或者θm，i，φm，i，i＝1，2，3选择，应该使得对应的测量矩阵的各个列矢量尽量彼此正交；
可选的，第一传感器的运动状态除了包括第一传感器的速度矢量，还可以进一步包括第一传感器的位置。例如，可以以指定的时间起点作为参考，根据运动速度和时间间隔得到第一传感器的位置。
在获得第一传感器的运动状态之后，可以基于该运动状态执行各种控制，具体执行什么控制此处不做限定。
可选的，上述第一传感器的运动速度估计可以提供作为其它传感器的运动速度估计。所述其它传感器为与第一传感器位于相同平台的传感器，例如与雷达/声纳/超声波传感器安装于相同车的摄像头或者视觉传感器或者成像传感器等。从而为其它传感器提供有效的速度估计；
可选的，可以根据所述第一传感器的运动状态，对目标对象的运动状态进行补偿，得到所述目标对象相对于大地坐标系的运动状态。本申请实施例中，该目标对象可以为探测车辆、障碍物、人、动物、或者其他物体。
如图7所示，其中左下图为以上得到的第一传感器的运动状态(如位置)，右边图为探测装置探测的目标对象的运动状态(如位置)，左上图为根据第一传感器的运动状态，对探测装置探测的目标对象的运动状态进行补偿，得到目标对象相对于大地坐标系的运动状态(位置)。
以上描述的是测量第一传感器的运动状态，实际上，该第一传感器所处的平台上可能还存在其他器件(如其他传感器)，因此该平台其他器件的运动状态与该第一传感器的运动状态相同或者相近，因此以上估计第一传感器的运动状态也可以等同于估计其他器件的运动状态。因此若出现采用上述原理估计该其他器件的运动状态的方案，同样落入本发明实施例的保护范围。
在图3所描述的方法中，通过第一传感器获得多个测量数据，根据其中与目标参照物对应的测量数据，所述测量数据至少包含速度测量信息。由于第一传感器与该目标参照物之间存在相对运动，第一传感器的测量数据可以包含对所述相对运动的速度的测量信息，因此，可以基于与目标参照物对应的测量数据，得到第一传感器的运动状态。此外，通常所述目标参照物空间相对传感器的分布多样，特别是与所述第一传感器有不同的几何关系，从而使得所述速度测量数据与所述第一传感器有不同的测量方程，特别是降低测量方程中的测量矩阵的条件数；而且，通过所述与目标参照物对应的测量数据众多，从而有效降低噪声或者干扰对运动状态估计的的影响。因此，利用本发明所述的方法，可以有效利用与目标参照物对应的测量数据，特别是目标参照物相对于传感器的几何关系以及数量，有效降低测量误差或者干扰的影响，从而使得这种确定运动状态的方式获得更高的精度；此外，所述方法利用单帧数据即可得到传感器的运动估计，从而可以获得很好的实时性。进一步地，可以理解，采用LS估计和/或序贯滤波估计的方式能够更有效地提高第一传感器运动状态(如速度)的估计精度。
上述详细阐述了本发明实施例的方法，下面提供了本发明实施例的装置。
请参见图8，图8是本发明实施例提供的一种运动状态估计装置80的结构示意图，可选的，该装置80可以是传感器***、融合感知***或者集成上述***的规划/控制***如辅助驾驶或者自动驾驶***等，可以是软件或者硬件。可选的，所述装置可以安装或者集成在车辆、轮船、飞机或者无人机等设备上，也可以安装或者连接于云端。该装置可以包括获取单元801和估计单元802，其中：
获取单元801用于通过第一传感器获得多个测量数据，其中每个所述测量数据至少包括速度测量信息；
估计单元802用于根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态，所述运动状态至少包括第一传感器的速度矢量。
在上述装置中，通过第一传感器获得多个测量数据，根据其中与目标参照物对应的测量数据，所述测量数据至少包含速度测量信息。由于第一传感器与该目标参照物之间存在相对运动，第一传感器的测量数据可以包含对所述相对运动的速度的测量信息，因此，可以基于与目标参照物对应的测量数据，得到第一传感器的运动状态。此外，通常所述目标参照物空间相对传感器的分布多样，特别是与所述第一传感器有不同的几何关系，从而使得所述速度测量数据与所述第一传感器有不同的测量方程，特别是降低测量方程中的测量矩阵的条件数；而且，通过所述与目标参照物对应的测量数据众多，从而有效降低噪声或者干扰对运动状态估计的的影响。因此，利用本发明所述的方法，可以有效利用与目标参照物对应的测量数据，特别是目标参照物相对于传感器的几何关系以及数量，有效降低测量误差或者干扰的影响，从而使得这种确定运动状态的方式获得更高的精度；此外，所述方法利用单帧数据即可得到传感器的运动估计，从而可以获得很好的实时性。
在一种可能的实现方式中，所述目标参照物为相对于参考系静止的物体。所述参考系可以是大地或者大地坐标系或者相对于大地的惯性坐标系。
在又一种可能的实现方式中，所述通过第一传感器获得多个测量数据，其中每个所述测量数据至少包括速度测量信息之后，所述根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态之前，还包括：
根据所述目标参照物的特征，从所述多个测量数据中确定与所述目标参照物对应的测量数据。
在又一种可能的实现方式中，所述目标参照物的特征包括所述目标参照物的几何特征和/或反射特征。
在又一种可能的实现方式中，所述通过第一传感器获得多个测量数据，其中每个所述测量数据至少包括速度测量信息之后，所述根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态之前，还包括：
根据第二传感器的数据，从所述第一传感器的多个测量数据中确定与所述目标参照物对应的测量数据。
在又一种可能的实现方式中，所述根据第二传感器的数据，从所述第一传感器的多个测量数据中确定与所述目标参照物对应的测量数据，包括：
将所述第一传感器的测量数据映射到第二传感器的测量数据的空间，或者，
将所述第二传感器的测量数据映射到所述第一传感器的测量数据的空间，或者，
将所述第一传感器的测量数据和第二传感器的测量数据映射到一个公共的空间，
根据由所述第二传感器测量数据确定的目标参照物，通过所述空间，确定所述第一传感器中目标参照物对应的测量数据。
在又一种可能的实现方式中，所述根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态，包括：
根据所述多个测量数据中与目标参照物对应的测量数据采用最小二乘LS估计和/或序贯分块滤波的方式，得到所述第一传感器的运动状态。可以理解，采用LS估计和/或序贯滤波估计的方式能够更有效地提高第一传感器运动状态(如速度)的估计精度。
在又一种可能的实现方式中，所述根据所述多个测量数据中与目标参照物对应的测量数据采用最小二乘LS估计和/或序贯分块滤波的方式，得到所述第一传感器的运动状态，包括：
根据对所述目标参照物对应的M个径向速度矢量及其对应的测量矩阵，进行序贯滤波，得到所述第一传感器的运动估计值，其中M≥2；所述径向速度矢量由所述多个测量数据中与目标参照物对应的测量数据中的K个径向速度测量值组成，所述对应的测量矩阵由K个方向余矢量组成，其中K≥1。
在又一种可能的实现方式中：
所述第一传感器的运动速度矢量为二维矢量，K＝2，所述径向速度矢量的测量矩阵为：
Figure BDA0002087677110000151
其中，θm，i分别为所述目标参照物的第m组测量数据中第i个方位角测量数据，i＝1，2；
或者，所述第一传感器的运动速度矢量为三维矢量，K＝3，所述径向速度矢量的测量矩阵为：
Figure BDA0002087677110000152
其中，θm，i为所述目标参照物的第m组测量数据中第i个方位角测量数据，φmi为所述目标参照物的第m组测量数据中第i个俯仰角测量数据，i＝1，2，3。
在又一种可能的实现方式中，所述序贯滤波的公式如下：
Figure BDA0002087677110000153
Figure BDA0002087677110000161
Pm，1|0＝Pm-1，1|1
Pm，1|1＝(I-Gm-1Hm-1，K)Pm，1|0
其中，
Figure BDA0002087677110000162
为第m次滤波的速度矢量估计值，Gm为增益矩阵，/>
Figure BDA0002087677110000163
为第m个径向速度矢量测量值，Rm，K为第m个径向速度矢量测量误差协方差矩阵，m＝1，2，...，M。
需要说明的是，各个单元的实现还可以对应参照图3所示的方法实施例的相应描述。
在图8所描述的装置80中，通过第一传感器获得多个测量数据，根据其中与目标参照物对应的测量数据，所述测量数据至少包含速度测量信息。由于第一传感器与该目标参照物之间存在相对运动，第一传感器的测量数据可以包含对所述相对运动的速度的测量信息，因此，可以基于与目标参照物对应的测量数据，得到第一传感器的运动状态。此外，通常所述目标参照物空间相对传感器的分布多样，特别是与所述第一传感器有不同的几何关系，从而使得所述速度测量数据与所述第一传感器有不同的测量方程，特别是降低测量方程中的测量矩阵的条件数；而且，通过所述与目标参照物对应的测量数据众多，从而有效降低噪声或者干扰对运动状态估计的的影响。因此，利用本发明所述的方法，可以有效利用与目标参照物对应的测量数据，特别是目标参照物相对于传感器的几何关系以及数量，有效降低测量误差或者干扰的影响，从而使得这种确定运动状态的方式获得更高的精度；此外，所述方法利用单帧数据即可得到传感器的运动估计，从而可以获得很好的实时性。进一步地，可以理解，采用LS估计和/或序贯滤波估计的方式能够更有效地提高第一传感器运动状态(如速度)的估计精度。
请参见图9，图9是本发明实施例提供的一种运动状态估计90，该装置90包括处理器901、存储器902和第一传感器903，所述处理器901、存储器902和第一传感器903通过总线904相互连接。
存储器902包括但不限于是随机存储记忆体(random access memory，RAM)、只读存储器(read-only memory，ROM)、可擦除可编程只读存储器(erasable programmableread only memory，EPROM)、或便携式只读存储器(compact disc read-only memory，CD-ROM)，该存储器902用于相关程序指令及数据。第一传感器903用于采集测量数据。
处理器901可以是一个或多个中央处理器(central processing unit，CPU)，在处理器901是一个CPU的情况下，该CPU可以是单核CPU，也可以是多核CPU。
装置90中的处理器901用于读取所述存储器902中存储的程序指令，执行以下操作：
通过所述第一传感器903获得多个测量数据，其中每个所述测量数据至少包括速度测量信息；
根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态，所述运动状态至少包括第一传感器的速度矢量。
在上述装置中，通过第一传感器获得多个测量数据，根据其中与目标参照物对应的测量数据，所述测量数据至少包含速度测量信息。由于第一传感器与该目标参照物之间存在相对运动，第一传感器的测量数据可以包含对所述相对运动的速度的测量信息，因此，可以基于与目标参照物对应的测量数据，得到第一传感器的运动状态。此外，通常所述目标参照物空间相对传感器的分布多样，特别是与所述第一传感器有不同的几何关系，从而使得所述速度测量数据与所述第一传感器有不同的测量方程，特别是降低测量方程中的测量矩阵的条件数；而且，通过所述与目标参照物对应的测量数据众多，从而有效降低噪声或者干扰对运动状态估计的的影响。因此，利用本发明所述的方法，可以有效利用与目标参照物对应的测量数据，特别是目标参照物相对于传感器的几何关系以及数量，有效降低测量误差或者干扰的影响，从而使得这种确定运动状态的方式获得更高的精度；此外，所述方法利用单帧数据即可得到传感器的运动估计，从而可以获得很好的实时性。
在一种可能的实现方式中，所述目标参照物为相对于参考系静止的物体。
可选的，所述参考系可以是大地或者大地坐标系或者相对于大地的惯性坐标系等。
在又一种可能的实现方式中，所述通过所述第一传感器获得多个测量数据，其中每个所述测量数据至少包括速度测量信息之后，所述根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态之前，所述处理器901还用于：
根据所述目标参照物的特征，从所述多个测量数据中确定与所述目标参照物对应的测量数据。
在又一种可能的实现方式中，所述目标参照物的特征包括所述目标参照物的几何特征和/或反射特征。
在又一种可能的实现方式中，所述通过所述第一传感器获得多个测量数据，其中每个所述测量数据至少包括速度测量信息之后，所述根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态之前，所述处理器901还用于：
根据第二传感器的数据，从所述第一传感器的多个测量数据中确定与所述目标参照物对应的测量数据。
在又一种可能的实现方式中，所述根据第二传感器的数据，从所述第一传感器的多个测量数据中确定与所述目标参照物对应的测量数据，具体为：
将所述第一传感器的测量数据映射到第二传感器的测量数据的空间，或者，
将所述第二传感器的测量数据映射到所述第一传感器的测量数据的空间，或者，
将所述第一传感器的测量数据和第二传感器的测量数据映射到一个公共的空间，
根据由所述第二传感器的测量数据确定的目标参照物，通过所述空间，确定所述第一传感器中目标参照物对应的测量数据。
在又一种可能的实现方式中，所述根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态，具体为：
根据所述多个测量数据中与目标参照物对应的测量数据采用最小二乘LS估计和/或序贯分块滤波的方式，得到所述第一传感器的运动状态。可以理解，采用LS估计和/或序贯滤波估计的方式能够更有效地提高第一传感器运动状态(如速度)的估计精度。
在又一种可能的实现方式中，所述根据所述多个测量数据中与目标参照物对应的测量数据采用最小二乘LS估计和/或序贯分块滤波的方式，得到所述第一传感器的运动状态，具体为：
根据对所述目标参照物对应的M个径向速度矢量及其对应的测量矩阵，进行序贯滤波，得到所述第一传感器的运动估计值，其中M≥2；所述径向速度矢量由所述多个测量数据中与目标参照物对应的测量数据中的K个径向速度测量值组成，所述对应的测量矩阵由K个方向余矢量组成，其中K≥1。
在又一种可能的实现方式中：
所述第一传感器的运动速度矢量为二维矢量，K＝2，其测量矩阵为：
Figure BDA0002087677110000171
其中，θm，i分别为所述目标参照物的第m组测量数据中第i个方位角测量数据，i＝1，2；
或者，所述第一传感器的运动速度矢量为三维矢量，K＝3，其测量矩阵为：
Figure BDA0002087677110000181
其中，θm，i为所述目标参照物的第m组测量数据中第i个方位角测量数据，φm，i为所述目标参照物的第m组测量数据中第i个俯仰角测量数据，i＝1，2，3。
在又一种可能的实现方式中，所述序贯滤波的公式如下：
Figure BDA0002087677110000182
Figure BDA0002087677110000185
Pm，1|0＝Pm-1，1|1
Pm，1|1＝(I-Gm-1Hm-1，K)Pm，1|0
其中，
Figure BDA0002087677110000183
为第m次滤波的速度矢量估计值，Gm为增益矩阵，/>
Figure BDA0002087677110000184
为第m个径向速度矢量测量值，Rm，K为第m个径向速度矢量测量误差协方差矩阵，m＝1，2，...，M。
需要说明的是，各个操作的实现还可以对应参照图3所示的方法实施例的相应描述。
在图9所描述的装置90中，通过第一传感器获得多个测量数据，根据其中与目标参照物对应的测量数据，所述测量数据至少包含速度测量信息。由于第一传感器与该目标参照物之间存在相对运动，第一传感器的测量数据可以包含对所述相对运动的速度的测量信息，因此，可以基于与目标参照物对应的测量数据，得到第一传感器的运动状态。此外，通常所述目标参照物空间相对传感器的分布多样，特别是与所述第一传感器有不同的几何关系，从而使得所述速度测量数据与所述第一传感器有不同的测量方程，特别是降低测量方程中的测量矩阵的条件数；而且，通过所述与目标参照物对应的测量数据众多，从而有效降低噪声或者干扰对运动状态估计的的影响。因此，利用本发明所述的方法，可以有效利用与目标参照物对应的测量数据，特别是目标参照物相对于传感器的几何关系以及数量，有效降低测量误差或者干扰的影响，从而使得这种确定运动状态的方式获得更高的精度；此外，所述方法利用单帧数据即可得到传感器的运动估计，从而可以获得很好的实时性。进一步地，可以理解，采用LS估计和/或序贯滤波估计的方式能够更有效地提高第一传感器运动状态(如速度)的估计精度。
本发明实施例还提供一种芯片***，所述芯片***包括至少一个处理器，存储器和接口电路，所述存储器、所述接口电路和所述至少一个处理器通过线路互联，所述至少一个存储器中存储有程序指令；所述程序指令被所述处理器执行时，实现图3所示的方法流程。
本发明实施例还提供一种计算机可读存储介质，所述计算机可读存储介质中存储有指令，当其在处理器上运行时，实现图3所示的方法流程。
本发明实施例还提供一种计算机程序产品，当所述计算机程序产品在处理器上运行时，实现图3所示的方法流程。
综上所述，通过实施本发明实施例，通过第一传感器获得多个测量数据，根据其中与目标参照物对应的测量数据，所述测量数据至少包含速度测量信息。由于第一传感器与该目标参照物之间存在相对运动，第一传感器的测量数据可以包含对所述相对运动的速度的测量信息，因此，可以基于与目标参照物对应的测量数据，得到第一传感器的运动状态。此外，通常所述目标参照物空间相对传感器的分布多样，特别是与所述第一传感器有不同的几何关系，从而使得所述速度测量数据与所述第一传感器有不同的测量方程，特别是降低测量方程中的测量矩阵的条件数；而且，通过所述与目标参照物对应的测量数据众多，从而有效降低噪声或者干扰对运动状态估计的的影响。因此，利用本发明所述的方法，可以有效利用与目标参照物对应的测量数据，特别是目标参照物相对于传感器的几何关系以及数量，有效降低测量误差或者干扰的影响，从而使得这种确定运动状态的方式获得更高的精度；此外，所述方法利用单帧数据即可得到传感器的运动估计，从而可以获得很好的实时性。进一步地，可以理解，采用LS估计和/或序贯滤波估计的方式能够更有效地提高第一传感器运动状态(如速度)的估计精度。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，该流程可以由计算机程序来指令相关的硬件完成，该程序可存储于计算机可读取存储介质中，该程序在执行时，可包括如上述各方法实施例的流程。而前述的存储介质包括：ROM或随机存储记忆体RAM、磁碟或者光盘等各种可存储程序代码的介质。

Claims (16)

1.一种运动状态估计方法，其特征在于，包括：
通过第一传感器获得多个测量数据，其中每个所述测量数据至少包括速度测量信息；
根据第二传感器的测量数据确定目标参照物，从所述第一传感器的多个测量数据中确定与所述目标参照物对应的测量数据；
根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态，所述运动状态至少包括第一传感器的速度矢量。
2.根据权利要求1所述的方法，其特征在于，所述通过第一传感器获得多个测量数据，其中每个所述测量数据至少包括速度测量信息之后，所述根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态之前，还包括：
根据所述目标参照物的特征，从所述多个测量数据中确定与所述目标参照物对应的测量数据。
3.根据权利要求2所述的方法，其特征在于，所述目标参照物的特征包括所述目标参照物的几何特征和/或反射特征。
4.根据权利要求1所述的方法，其特征在于，所述根据第二传感器的数据，从所述第一传感器的多个测量数据中确定与所述目标参照物对应的测量数据，包括：
将所述第一传感器的测量数据映射到第二传感器的测量数据的空间，或者，
将所述第二传感器的测量数据映射到所述第一传感器的测量数据的空间，或者，
将所述第一传感器的测量数据和第二传感器的测量数据映射到一个公共的空间，
根据由所述第二传感器的测量数据确定的目标参照物，通过所述空间，确定所述第一传感器中目标参照物对应的测量数据。
5.根据权利要求1-4任一项所述的方法，其特征在于，所述根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态，包括：
根据所述多个测量数据中与目标参照物对应的测量数据采用最小二乘LS估计和/或序贯分块滤波的方式，得到所述第一传感器的运动状态。
6.根据权利要求5所述的方法，其特征在于，所述根据所述多个测量数据中与目标参照物对应的测量数据采用最小二乘LS估计和/或序贯分块滤波的方式，得到所述第一传感器的运动状态，包括：
根据对所述目标参照物对应的M个径向速度矢量及其对应的测量矩阵，进行序贯滤波，得到所述第一传感器的运动估计值，其中M≥2；所述径向速度矢量由所述多个测量数据中与目标参照物对应的测量数据中的K个径向速度测量值组成，所述对应的测量矩阵由K个方向余矢量组成，其中K≥1。
7.根据权利要求6所述的方法，其特征在于：
所述第一传感器的运动速度矢量为二维矢量，K＝2，所述径向速度矢量对应的测量矩阵为：
Figure FDA0004116880850000021
其中，θm,i分别为所述目标参照物的第m组测量数据中第i个方位角测量数据，i＝1,2；
或者，所述第一传感器的运动速度矢量为三维矢量，K＝3，所述径向速度矢量对应的测量矩阵为：
Figure FDA0004116880850000022
其中，θm,i为所述目标参照物的第m组测量数据中第i个方位角测量数据,φm,i为所述目标参照物的第m组测量数据中第i个俯仰角测量数据，i＝1,2,3；m＝1,2,..,M。
8.根据权利要求7所述的方法，其特征在于，所述序贯滤波的公式如下：
Figure FDA0004116880850000023
Figure FDA0004116880850000024
Pm,1|0＝Pm-1,1|1
Pm,1|1＝(I-Gm-1Hm-1,K)Pm,1|0
其中，
Figure FDA0004116880850000025
为第m次滤波的速度矢量估计值，Gm为增益矩阵，/>
Figure FDA0004116880850000026
为第m个径向速度矢量测量值,Rm,K为第m个径向速度矢量测量误差协方差矩阵，m＝1,2,…,M。
9.一种运动状态估计装置，其特征在于，包括处理器、存储器和第一传感器，其中，所述存储器用于存储程序指令，所述处理器用于调用所述程序指令来执行如下操作：
通过所述第一传感器获得多个测量数据，其中每个所述测量数据至少包括速度测量信息；
根据第二传感器的测量数据确定目标参照物，从所述第一传感器的多个测量数据中确定与所述目标参照物对应的测量数据；
根据所述多个测量数据中与目标参照物对应的测量数据，所述运动状态至少包括第一传感器的速度矢量。
10.根据权利要求9所述的装置，其特征在于，所述通过所述第一传感器获得多个测量数据，其中每个所述测量数据至少包括速度测量信息之后，所述根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态之前，所述处理器还用于：
根据所述目标参照物的特征，从所述多个测量数据中确定与所述目标参照物对应的测量数据。
11.根据权利要求10所述的装置，其特征在于，所述目标参照物的特征包括所述目标参照物的几何特征和/或反射特征。
12.根据权利要求9所述的装置，其特征在于，所述根据第二传感器的数据，从所述第一传感器的多个测量数据中确定与所述目标参照物对应的测量数据，具体为：
将所述第一传感器的测量数据映射到第二传感器的测量数据的空间，或者，
将所述第二传感器的测量数据映射到所述第一传感器的测量数据的空间，或者，
将所述第一传感器的测量数据和第二传感器的测量数据映射到一个公共的空间，
根据由所述第二传感器的测量数据确定的目标参照物，通过所述空间，确定所述第一传感器中目标参照物对应的测量数据。
13.根据权利要求9-12任一项所述的装置，其特征在于，所述根据所述多个测量数据中与目标参照物对应的测量数据，得到第一传感器的运动状态，具体为：
根据所述多个测量数据中与目标参照物对应的测量数据采用最小二乘LS估计和/或序贯分块滤波的方式，得到所述第一传感器的运动状态。
14.根据权利要求13所述的装置，其特征在于，所述根据所述多个测量数据中与目标参照物对应的测量数据采用最小二乘LS估计和/或序贯分块滤波的方式，得到所述第一传感器的运动状态，具体为：
根据对所述目标参照物对应的M个径向速度矢量及其对应的测量矩阵，进行序贯滤波，得到所述第一传感器的运动估计值，其中M≥2；所述径向速度矢量由所述多个测量数据中与目标参照物对应的测量数据中的K个径向速度测量值组成，所述对应的测量矩阵由K个方向余矢量组成，其中K≥1。
15.根据权利要求14所述的装置，其特征在于：
所述第一传感器的运动速度矢量为二维矢量，K＝2，所述径向速度矢量对应的测量矩阵为：
Figure FDA0004116880850000031
其中，θm,i分别为所述目标参照物的第m组测量数据中第i个方位角测量数据，i＝1,2；
或者，所述第一传感器的运动速度矢量为三维矢量，K＝3，所述径向速度矢量对应的测量矩阵为：
Figure FDA0004116880850000032
其中，θm,i为所述目标参照物的第m组测量数据中第i个方位角测量数据,φm,i为所述目标参照物的第m组测量数据中第i个俯仰角测量数据，i＝1,2,3；m＝1,2,..,M。
16.根据权利要求15所述的装置，其特征在于，所述序贯滤波的公式如下：
Figure FDA0004116880850000033
Figure FDA0004116880850000034
Pm,1|0＝Pm-1,1|1
Pm,1|1＝(I-Gm-1Hm-1,K)Pm,1|0
其中，
Figure FDA0004116880850000035
为第m次滤波的速度矢量估计值，Gm为增益矩阵，/>
Figure FDA0004116880850000036
为第m个径向速度矢量测量值,Rm,K为第m个径向速度矢量测量误差协方差矩阵，m＝1,2,…,M。/>
CN201910503710.9A 2019-06-06 2019-06-06 一种运动状态估计方法及装置 Active CN112050830B (zh)

Priority Applications (4)

Application Number Priority Date Filing Date Title
CN201910503710.9A CN112050830B (zh) 2019-06-06 2019-06-06 一种运动状态估计方法及装置
PCT/CN2020/093486 WO2020244467A1 (zh) 2019-06-06 2020-05-29 一种运动状态估计方法及装置
EP20818619.7A EP3964863A4 (en) 2019-06-06 2020-05-29 METHOD AND DEVICE FOR MOTION STATE ESTIMATION
US17/542,699 US20220089166A1 (en) 2019-06-06 2021-12-06 Motion state estimation method and apparatus

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910503710.9A CN112050830B (zh) 2019-06-06 2019-06-06 一种运动状态估计方法及装置

Publications (2)

Publication Number Publication Date
CN112050830A CN112050830A (zh) 2020-12-08
CN112050830B true CN112050830B (zh) 2023-06-02

Family

ID=73608994

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910503710.9A Active CN112050830B (zh) 2019-06-06 2019-06-06 一种运动状态估计方法及装置

Country Status (4)

Country Link
US (1) US20220089166A1 (zh)
EP (1) EP3964863A4 (zh)
CN (1) CN112050830B (zh)
WO (1) WO2020244467A1 (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11872994B2 (en) * 2021-10-30 2024-01-16 Zoox, Inc. Estimating vehicle velocity

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104040369A (zh) * 2012-01-05 2014-09-10 罗伯特·博世有限公司 用于在车辆中不依赖于车轮的速度测量的方法和设备

Family Cites Families (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
FR2764075B1 (fr) * 1997-05-30 1999-08-27 Thomson Csf Procede de recalage de navigation d'un mobile au moyen d'une cartographie radar de zones de terrain a relief accentue
DE19858298C2 (de) * 1998-12-17 2001-05-31 Daimler Chrysler Ag Verwendung einer Einrichtung in einem Fahrzeug, mit der die Umgebung des Fahrzeuges mittels Radarstrahlen erkannt werden kann
US8855848B2 (en) * 2007-06-05 2014-10-07 GM Global Technology Operations LLC Radar, lidar and camera enhanced methods for vehicle dynamics estimation
ITRM20070399A1 (it) * 2007-07-19 2009-01-20 Consiglio Nazionale Ricerche Metodo di elaborazione di dati rilevati mediante radar ad apertura sintetica (synthetic aperture radar - sar) e relativo sistema di telerilevamento.
DE102009030076A1 (de) * 2009-06-23 2010-12-30 Symeo Gmbh Abbildungsverfahren mittels synthetischer Apertur, Verfahren zur Bestimmung einer Relativgeschwindigkeit zwischen einem wellenbasierten Sensor und einem Objekt bzw. Vorrichtung zur Durchführung der Verfahren
JP6629242B2 (ja) * 2014-05-28 2020-01-15 コーニンクレッカ フィリップス エヌ ヴェＫｏｎｉｎｋｌｉｊｋｅ Ｐｈｉｌｉｐｓ Ｎ．Ｖ． マルチチャネルｐｐｇ信号を使用するモーションアーチファクト低減
JP6425130B2 (ja) * 2014-12-18 2018-11-21 パナソニックＩｐマネジメント株式会社 レーダ装置及びレーダ状態推定方法
US10829122B2 (en) * 2016-06-17 2020-11-10 Robert Bosch Gmbh Overtake acceleration aid for adaptive cruise control in vehicles
EP3349033A1 (en) * 2017-01-13 2018-07-18 Autoliv Development AB Enhanced object detection and motion estimation for a vehicle environment detection system
CN107132542B (zh) * 2017-05-02 2019-10-15 北京理工大学 一种基于光学和多普勒雷达的小天体软着陆自主导航方法
CN108872975B (zh) * 2017-05-15 2022-08-16 蔚来(安徽)控股有限公司 用于目标跟踪的车载毫米波雷达滤波估计方法、装置及存储介质
US10634777B2 (en) * 2018-05-30 2020-04-28 Ford Global Technologies, Llc Radar odometry for vehicle
CN108663676A (zh) * 2018-07-25 2018-10-16 中联天通科技（北京）有限公司 一种新型组合导航中毫米波测速雷达***

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104040369A (zh) * 2012-01-05 2014-09-10 罗伯特·博世有限公司 用于在车辆中不依赖于车轮的速度测量的方法和设备

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
Kellner D.Verfahren zur Bestimmung von Objekt-und Eigenbewegung auf Basis der Dopplerinformation hochauflösender Radarsensoren.《Universität Ulm》.2016,第1-204页. *

Also Published As

Publication number Publication date
EP3964863A1 (en) 2022-03-09
EP3964863A4 (en) 2022-06-15
US20220089166A1 (en) 2022-03-24
WO2020244467A1 (zh) 2020-12-10
CN112050830A (zh) 2020-12-08

Similar Documents

Publication Publication Date Title
JP7398506B2 (ja) ローカライゼーション基準データを生成及び使用する方法及びシステム
Aqel et al. Review of visual odometry: types, approaches, challenges, and applications
CN111326023B (zh) 一种无人机航线预警方法、装置、设备及存储介质
AU2018282302B2 (en) Integrated sensor calibration in natural scenes
US10909395B2 (en) Object detection apparatus
CN111391823A (zh) 一种用于自动泊车场景的多层地图制作方法
US20220024549A1 (en) System and method for measuring the distance to an object in water
EP3137850A1 (en) Method and system for determining a position relative to a digital map
EP3447729B1 (en) 2d vehicle localizing using geoarcs
US7840075B2 (en) Marine radar system with three-dimensional memory
WO2008009966A2 (en) Determining the location of a vehicle on a map
US10109074B2 (en) Method and system for inertial measurement having image processing unit for determining at least one parameter associated with at least one feature in consecutive images
CN111175788A (zh) 自动驾驶车辆横向定位方法及其定位***
JP2023525927A (ja) 車両位置推定システム及び方法
EP1584896A1 (en) Passive measurement of terrain parameters
US20240152159A1 (en) Devices, systems and methods for navigating a mobile platform
CN112050830B (zh) 一种运动状态估计方法及装置
Aggarwal GPS-based localization of autonomous vehicles
Gu et al. SLAM with 3dimensional-GNSS
CN115965847A (zh) 交叉视角下多模态特征融合的三维目标检测方法和***
Iannucci et al. Cross-Modal Localization: Using automotive radar for absolute geolocation within a map produced with visible-light imagery
CN113867389A (zh) 一种无人机拍摄控制方法、装置、设备及存储介质
CN112050829B (zh) 一种运动状态确定方法及装置
Gakne Improving the accuracy of GNSS receivers in urban canyons using an upward-facing camera
Nomura et al. A Proposal of Automatic Camera Placement Method for Wide Sea Area Monitoring

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