CN114077719A - 一种自运动估计方法及相关装置 - Google Patents

一种自运动估计方法及相关装置 Download PDF

Info

Publication number
CN114077719A
CN114077719A CN202010822885.9A CN202010822885A CN114077719A CN 114077719 A CN114077719 A CN 114077719A CN 202010822885 A CN202010822885 A CN 202010822885A CN 114077719 A CN114077719 A CN 114077719A
Authority
CN
China
Prior art keywords
vector
sensor
velocity vector
carrier
coordinate system
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.)
Pending
Application number
CN202010822885.9A
Other languages
English (en)
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 CN202010822885.9A priority Critical patent/CN114077719A/zh
Priority to PCT/CN2021/098209 priority patent/WO2022033139A1/zh
Publication of CN114077719A publication Critical patent/CN114077719A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Data Mining & Analysis (AREA)
  • Mathematical Physics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • General Engineering & Computer Science (AREA)
  • Databases & Information Systems (AREA)
  • Algebra (AREA)
  • Software Systems (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Operations Research (AREA)
  • Evolutionary Biology (AREA)
  • Probability & Statistics with Applications (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Automation & Control Theory (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Computing Systems (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本申请实施例提供一种自运动估计方法及相关装置，该方法包括：获取传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量
Figure DDA0002631962570000011
根据第一转动速度矢量ωs、第一平动速度矢量
Figure DDA0002631962570000012
以及传感器的外部参数确定载体的第二转动速度矢量ωe和第二平动速度矢量te，其中，载体为传感器所在的载体，传感器的外部参数包括传感器坐标系与载体坐标系的转换关系，采用本申请实施例，能够确定传感器所在的载体或者传感器的运动速度，提高传感器所在的载体或者传感器自运动估计的精度，从而提高了辅助驾驶、自动驾驶或者无人驾驶的安全性。

Description

一种自运动估计方法及相关装置
技术领域
本申请涉及传感器技术领域，尤其涉及一种自运动估计方法及相关装置。
背景技术
先进辅助驾驶系统(advanced driver-assistance system，ADAS)或者自动驾驶系统(autonomous driving，AD)通常会配置多种传感器，例如相机或者摄像头等传感器，用于感知周边环境信息，包括运动目标和静止目标，例如，运动目标有车辆、行人等；静止目标有障碍物、护栏、灯杆和建筑物等等。对于运动目标和静止目标，通常采用不同的方法分析处理，如对运动目标进行分类、识别以及跟踪处理；对静止目标进行分类和识别处理，从而为无人驾驶提供额外的信息，如规避障碍物、提供可行驶区域等等。传感器通常可以安装于不同的载体平台，例如车辆、舰船、卫星、无人机以及机器人等，传感器将跟随传感器所在的载体平台运动。一方面，传感器的运动导致运动目标和静止目标无法独立分析，因此，需要估计传感器的运动实现运动目标和静止目标的分离；另一方面，运动目标的跟踪通常是基于模型的，且模型通常假定相对地面或者大地坐标系，但是传感器的运动将导致上述模型失效或者跟踪性能下降；此时，需要对传感器或者传感器载体的运动进行补偿。此外，对实际的不同场景，通过传感器运动估计实现对传感器运动平台的定位和跟踪也至关重要。
因此如何确定传感器以及传感器所在的载体的运动，是本领域人员正在解决的技术问题。
发明内容
本申请实施例公开了一种自运动估计方法及相关装置，能够确定传感器和传感器所在的载体的速度，提高传感器或者传感器所在的载体自运动估计的精度。
本申请实施例第一方面公开了一种自运动估计方法，包括：
获取传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000011
根据所述第一转动速度矢量ωs、所述第一平动速度矢量
Figure BDA0002631962550000012
以及所述传感器的外部参数确定载体的第二转动速度矢量ωe和第二平动速度矢量te，所述载体为所述传感器所在的载体，所述传感器的外部参数包括传感器坐标系与载体坐标系的转换关系。
在上述方法中，根据传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000013
以及传感器坐标系和载体坐标系的转换关系确定载体的第二转动速度矢量ωe和第二平动速度矢量te，其中，第二平动速度矢量te为完整的平动速度矢量，而现有技术当中，通常获得传感器自运动的归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000014
通常缺少一个自由度的信息，而本申请实施例能够精确的获得载体的第二转动速度矢量ωe和完整的第二平动速度矢量te，即通过外部参数恢复缺失的信息，从而更加准确的估计传感器所在的载体和传感器的运动速度，提高了传感器或者传感器所在的载体自运动估计的精度，从而提高了辅助驾驶、自动驾驶或者无人驾驶的安全性。
可选地，在一种实现方式中，所述转换关系包括：所述传感器坐标系相对于所述载体坐标系的旋转矩阵R和平移矢量r。
可选地，在又一种实现方式中，所述根据所述第一转动速度矢量ωs、所述第一平动速度矢量
Figure BDA0002631962550000015
以及传感器的外部参数确定所述载体的第二转动速度矢量ωe和第二平动速度矢量te，包括：根据所述旋转矩阵R和所述第一转动速度矢量ωs确定所述载体的第二转动速度矢量ωe。
在上述方法中，根据传感器坐标系相对于载体坐标器的外部参数，即旋转矩阵R或者四元数或者欧拉角和第一转动速度矢量ωs，确定载体的第二转动速度矢量ωe，可以有效利用传感器相对于载体的外部参数，获取传感器所在的载体的转动角速度，有效的提高传感器所在的载体的转动速度估计的精度。
可选地，在又一种实现方式中，所述根据所述第一转动速度矢量ωs、所述第一平动速度矢量
Figure BDA0002631962550000021
以及所述传感器的外部参数确定所述载体的第二转动速度矢量ωe和第二平动速度矢量te，包括：根据所述第二转动速度矢量ωe和所述平移矢量r确定所述载体转动引起的瞬时速度成分；根据所述旋转矩阵R和所述第一平动速度矢量
Figure BDA0002631962550000022
确定所述传感器相对于所述载体坐标系的归一化瞬时速度矢量；根据所述瞬时速度成分和所述归一化瞬时速度矢量确定所述载体的第二平动速度矢量te。
可选地，在又一种实现方式中，所述根据所述瞬时速度成分和所述归一化瞬时速度矢量确定所述载体的第二平动速度矢量te，包括：根据所述瞬间速度成分和所述归一化瞬时速度矢量确定所述传感器坐标系的平动速度矢量的幅度；根据所述传感器坐标系的平动速度矢量的幅度、所述归一化瞬时速度矢量以及所述瞬时速度成分确定所述载体的第二平动速度矢量te。
可选地，在又一种实现方式中，所述载体的第二平动速度矢量te，
Figure BDA0002631962550000023
所述s为所述传感器坐标系的平动速度矢量的幅度，所述
Figure BDA0002631962550000024
为所述归一化瞬时速度矢量，所述tR为所述瞬时速度成分。
可选地，在又一种实现方式中，所述方法还包括：根据所述传感器坐标系的平动速度矢量的幅度s和所述第一平动速度矢量
Figure BDA0002631962550000025
确定所述传感器自运动的第三平动速度矢量ts。
在上述方法中，通过传感器坐标系的平动速度矢量的幅度s和第一平动速度矢量
Figure BDA0002631962550000026
确定传感器自运动的第三平动速度矢量ts，其中，第一平动速度矢量
Figure BDA0002631962550000027
为归一化或者尺度伸缩的，且第三平动速度矢量ts为完整的平动速度矢量。而现有技术中，只能够获得归一化或者尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000028
与现有技术相比，本申请实施例在缺乏尺度信息的情况下，提供了一种确定传感器自运动的完整的第三平动速度矢量ts的方法。
可选地，在又一种实现方式中，所述传感器为视觉传感器，所述方法还包括：获取静止障碍物的流矢量，所述流矢量包括所述静止障碍物在所述视觉传感器的图像平面上的运动矢量；根据所述静止障碍物的流矢量、所述第三平动速度矢量ts和所述第一转动速度矢量ωs确定所述静止障碍物的深度z。
在上述方法中，通过静止障碍物的流矢量、第三平动速度矢量ts和第一转动速度矢量ωs确定静止障碍物的深度z，而现有技术中，存在尺度问题，深度信息与平动速度的各个分量耦合在一起，通常无法获得精确地深度估计，与现有技术相比，本申请实施例能够通过利用流矢量，获得更加精确的深度z。
本申请实施例第二方面公开了一种自运动估计装置，包括：
第一获取单元，用于获取传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000029
第一确定单元，用于根据所述第一转动速度矢量ωs、所述第一平动速度矢量
Figure BDA00026319625500000210
以及所述传感器的外部参数确定载体的第二转动速度矢量ωe和第二平动速度矢量te，所述载体为所述传感器所在的载体，所述传感器的外部参数包括传感器坐标系与载体坐标系的转换关系。
在上述装置中，根据传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000031
以及传感器坐标系和载体坐标系的转换关系确定载体的第二转动速度矢量ωe和第二平动速度矢量te，其中，第二平动速度矢量te为完整的平动速度矢量，而现有技术当中，通常获得传感器自运动的归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000032
通常缺少一个自由度的信息，因此，本申请实施例能够精确的获得载体的第二转动速度矢量ωe和完整的第二平动速度矢量te，即通过传感器的外部参数恢复缺失的信息，从而更加准确的估计传感器所在的载体和传感器的运动速度，提高了传感器或者传感器所在的载体自运动估计的精度，从而提高了辅助驾驶、自动驾驶或者无人驾驶的安全性。
可选的，在一种实现方式中，所述转换关系包括：所述传感器坐标系相对于所述载体坐标系的旋转矩阵R和平移矢量r。
可选的，在又一种实现方式中，所述第一确定单元，用于根据所述旋转矩阵R和所述第一转动速度矢量ωs确定所述载体的第二转动速度矢量ωe。
在上述装置中，根据传感器坐标系相对于载体坐标系的外部参数，即旋转矩阵R或者四元数或者欧拉角和第一转动速度矢量ωs，确定载体的第二转动速度矢量ωe，可以有效利用传感器相对于载体的外部参数，获取传感器所在的载体的转动角速度，有效的提高传感器所在的载体的转动速度估计的精度。
可选的，在又一种实现方式中，所述第一确定单元，用于根据所述第二转动速度矢量ωe和所述平移矢量r确定所述载体转动引起的瞬时速度成分；根据所述旋转矩阵R和所述第一平动速度矢量
Figure BDA0002631962550000033
确定所述传感器相对于所述载体坐标系的归一化瞬时速度矢量；根据所述瞬时速度成分和所述归一化瞬时速度矢量确定所述载体的第二平动速度矢量te。
可选的，在又一种实现方式中，所述第一确定单元，用于根据所述瞬间速度成分和所述归一化瞬时速度矢量确定所述传感器坐标系的平动速度矢量的幅度；根据所述传感器坐标系的平动速度矢量的幅度、所述归一化瞬时速度矢量以及所述瞬时速度成分确定所述载体的第二平动速度矢量te。
可选的，在又一种实现方式中，所述载体的第二平动速度矢量te，
Figure BDA0002631962550000034
所述s为所述传感器坐标系的平动速度矢量的幅度，所述
Figure BDA0002631962550000039
为所述归一化瞬时速度矢量，所述tR为所述瞬时速度成分。
可选的，在又一种实现方式中，所述装置还包括：第二确定单元，用于根据所述传感器坐标系的平动速度矢量的幅度s和所述第一平动速度矢量
Figure BDA0002631962550000035
确定所述传感器自运动的第三平动速度矢量ts。
在上述装置中，通过传感器坐标系的平动速度矢量的幅度s和第一平动速度矢量
Figure BDA0002631962550000036
确定传感器自运动的第三平动速度矢量ts，其中，第一平动速度矢量
Figure BDA0002631962550000037
为归一化或者尺度伸缩的，且第三平动速度矢量ts为完整的平动速度矢量。而现有技术中，只能够获得归一化或者尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000038
与现有技术相比，本申请实施例在缺乏尺度信息的情况下，提供了一种确定传感器自运动的完整的第三平动速度矢量ts的方法。
可选的，在又一种实现方式中，所述传感器为视觉传感器，所述装置还包括：第二获取单元，用于获取静止障碍物的流矢量，所述流矢量包括所述静止障碍物在所述视觉传感器的图像平面上的运动矢量；第三确定单元，用于根据所述静止障碍物的流矢量、所述第三平动速度矢量ts和所述第一转动速度矢量ωs确定所述静止障碍物的深度z。
在上述装置中，通过静止障碍物的流矢量、第三平动速度矢量ts和第一转动速度矢量ωs确定静止障碍物的深度z，而现有技术中，存在尺度问题，深度信息与平动速度的各个分量耦合在一起，通常无法获得精确地深度估计，与现有技术相比，本申请实施例能够通过利用光流矢量，获得更加精确的深度z。
本申请实施例第三方面公开了一种自运动估计装置，所述装置包括至少一个处理器和通信接口，可选的，还包括存储器。所述存储器用于存储计算机程序，所述至少一个处理器调用所述计算机程序，用于执行以下操作：
获取传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000041
根据所述第一转动速度矢量ωs、所述第一平动速度矢量
Figure BDA0002631962550000042
以及传感器的外部参数确定所述载体的第二转动速度矢量ωe和第二平动速度矢量te，所述载体为所述传感器所在的载体，所述传感器的外部参数包括传感器坐标系与载体坐标系的转换关系。
在上述装置中，根据传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000043
以及传感器坐标系和载体坐标系的转换关系确定载体的第二转动速度矢量ωe和第二平动速度矢量te，其中，第二平动速度矢量te为完整的平动速度矢量，而现有技术当中，通常获得传感器自运动的归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000044
通常缺少一个自由度的信息，而本申请实施例能够精确的获得载体的第二转动速度矢量ωe和完整的第二平动速度矢量te，即通过传感器的外部参数恢复缺失信息，从而更加准确的估计传感器所在的载体和传感器的运动速度，提高了传感器或者传感器所在的载体自运动估计的精度，从而提高了自动驾驶的安全性。
可选地，在一种实现方式中，所述转换关系包括：所述传感器坐标系相对于所述载体坐标系的旋转矩阵R和平移矢量r。
可选地，在又一种实现方式中，所述至少一个处理器，用于根据所述旋转矩阵R和所述第一转动速度矢量ωs确定所述载体的第二转动速度矢量ωe。
在上述装置中，根据传感器坐标系相对于载体坐标系的外部参数，即旋转矩阵R或者四元数或者欧拉角和第一转动速度矢量ωs，确定载体的第二转动速度矢量ωe，可以有效利用传感器相对于载体的外部参数，获取传感器所在的载体的转动角速度，有效的提高传感器所在的载体的转动速度估计的精度。
可选地，在又一种实现方式中，所述至少一个处理器，用于根据所述第二转动速度矢量ωe和所述平移矢量r确定所述载体转动引起的瞬时速度成分；根据所述旋转矩阵R和所述第一平动速度矢量
Figure BDA0002631962550000049
确定所述传感器相对于所述载体坐标系的归一化瞬时速度矢量；根据所述瞬时速度成分和所述归一化瞬时速度矢量确定所述载体的第二平动速度矢量te。
可选地，在又一种实现方式中，所述至少一个处理器，用于根据所述瞬间速度成分和所述归一化瞬时速度矢量确定所述传感器坐标系的平动速度矢量的幅度；根据所述传感器坐标系的平动速度矢量的幅度、所述归一化瞬时速度矢量以及所述瞬时速度成分确定所述载体的第二平动速度矢量te。
可选地，在又一种实现方式中，所述载体的第二平动速度矢量te，
Figure BDA0002631962550000045
所述s为所述传感器坐标系的平动速度矢量的幅度，所述
Figure BDA00026319625500000410
为所述归一化瞬时速度矢量，所述tR为所述瞬时速度成分。
可选地，在又一种实现方式中，所述至少一个处理器，还用于根据所述传感器坐标系的平动速度矢量的幅度s和所述第一平动速度矢量
Figure BDA0002631962550000046
确定所述传感器自运动的第三平动速度矢量ts。
可选地，在上述装置中，通过传感器坐标系的平动速度矢量的幅度s和第一平动速度矢量
Figure BDA0002631962550000047
确定传感器自运动的第三平动速度矢量ts，其中，第一平动速度矢量
Figure BDA0002631962550000048
为归一化或者尺度伸缩的，且第三平动速度矢量ts为完整的平动速度矢量。而现有技术中，只能够获得归一化或者尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000051
与现有技术相比，本申请实施例在缺乏尺度信息的情况下，提供了一种确定传感器自运动的完整的第三平动速度矢量ts的方法。
可选地，在又一种实现方式中，所述传感器为视觉传感器，所述至少一个处理器，还用于获取静止障碍物的流矢量，所述流矢量包括所述静止障碍物在所述视觉传感器的图像平面上的运动矢量；根据所述静止障碍物的流矢量、所述第三平动速度矢量ts和所述第一转动速度矢量ωs确定所述静止障碍物的深度z。
在上述装置中，通过静止障碍物的流矢量、第三平动速度矢量ts和第一转动速度矢量ωs确定静止障碍物的深度z，而现有技术中，存在尺度问题，深度信息与平动速度的各个分量耦合在一起，通常无法获得精确地深度估计，与现有技术相比，本申请实施例能够通过利用光流矢量，获得更加精确的深度z。
本申请实施例第四方面公开了一种计算机产品，当所述计算机程序产品在处理器上运行时，实现任意一方面或者任意一方面的可选的方案所描述的方法。
本申请实施例第五方面公开了一种芯片系统，所述芯片系统包括至少一个处理器和通信接口，可选的，所述芯片系统还包括存储器，所述至少一个处理器用于调用至少一个存储器中存储的计算机程序，以使得所述芯片系统所在的装置实现任意一方面或者任意一方面的可选的方案所描述的方法。
本申请实施例第六方面公开了一种计算机可读存储介质，所述计算机存储介质存储有计算机程序，所述计算机程序当被处理器执行时实现任意一方面或者任意一方面的可选的方案所描述的方法。
本申请实施例第七方面公开了一种终端，所述终端包含至少一个处理器和通信接口，可选的，还包括存储器，所述至少一个处理器用于调用至少一个存储器中存储的计算机程序，以使得所述终端实现任意一方面或者任意一方面的可选的方案所描述的方法。
本申请实施例第八方面公开了一种传感器，所述传感器包括至少一个处理器和通信接口，可选的，所述传感器还包括存储器，所述至少一个处理器用于调用至少一个存储器中存储的计算机程序，以使得所述传感器所在的装置实现任意一方面或者任意一方面的可选的方案所描述的方法。
附图说明
以下对本申请实施例用到的附图进行介绍。
图1是本申请实施例提供的一种自运动估计系统的结构示意图；
图2是本申请实施例提供的一种惯性测量单元的示意图；
图3是本申请实施例提供的一种自运动估计方法的流程图；
图4是本申请实施例提供的一种传感器坐标系的表示形式的示意图；
图5是本申请实施例提供的一种载体坐标系的表示形式的示意图；
图6是本申请实施例提供的一种自运动估计装置的结构示意图；
图7是本申请实施例提供的又一种自运动估计装置的结构示意图。
具体实施方式
下面结合本申请实施例中的附图对本申请实施例进行描述。
请参见图1，图1是本申请实施例提供的一种自运动估计系统的结构示意图，该系统包括传感器1001、运动测量模块1002和数据处理模块1003，其中，传感器1001可以为视觉传感器，例如，红外热成像传感器、相机或者摄像头等等。传感器1001用于提供视觉测量数据，例如图像或者视频；运动测量模块1002用于根据所述传感器数据获取运动测量数据，如传感器自运动的第一转动速度矢量ωs和归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000061
该数据处理模块1003用于处理传感器1002提供的测量数据，在一般情况下，运动测量模块1002和数据处理模块1003可以在同一个处理器中。
在一种示例中，传感器1001、运动测量模块1002和数据处理模块1003可以通过有线或者无线方式等完全或者不同集成在一起，例如传感器1001、运动测量模块1002和数据处理模块1003部署于一个处理器系统上；此时该自运动估计系统可以在同一个车载、同一个机载或者同一个星载或者智能体上。在又一种示例中，传感器1001在车载、机载或者星载或者智能体上，而运动测量模块1002和/或数据处理模块1003在云端，相应的，传感器1001将提供的视频数据或者参数化数据、或者运动测量模块1002提供的数据发送给在云端的数据处理模块1003，数据处理模块1003将处理后的结果发送给车载、机载或者星载或者智能体。其中，车载可以为如汽车、摩托车或者自行车等，机载可以为无人机、直升机或者喷气式飞机等等，星载可以为卫星等，智能体如机器人。
目前，获得运动速度的方法有多种，在一种方法中，车辆中通常安装有惯性测量单元(inertial measurement unit，IMU)，如图2所示，图2表示一种惯性测量单元，IMU是测量物体三轴姿态角(或角速度)以及加速度的装置。一般地，一个IMU内会装有三个单轴的陀螺仪和三个单轴的加速度计，陀螺仪用于检测载体相对于导航坐标系的角速度信号，而加速度计检测物体在载体坐标系统独立三轴的加速度信号，测量物体在三维空间中的角速度和加速度，并以此可以解算出物体运动速度和姿态。然而，在上面的方法中，物体的运动速度通常基于加速度积累得到的，但是通过加速度计测量的加速度的测量误差会随着时间累积，所以存在误差累积问题，需要利用其它传感器进行额外的校准，而且，一般用于车辆中的IMU的精度太低，若选用高精度的IMU，成本高昂。
在又一种方法中，车辆中通常安装有毫米波雷达传感器，雷达传感器通常能提供距离、方位角和径向速度等测量数据。基于静止目标的方位角和径向速度分量，可以根据最小二乘法或者其他的方式获得传感器相对于大地的瞬时速度。然而，在上述方法中，通常只能获得相对雷达传感器坐标系的两个速度分量，即横向速度和径向速度，但是无法获取第三个速度分量，此外，虽然径向速度误差比较低，但是横向速度误差比较高，难以满足系统的性能需求，而且，该方法只能获得一个转动角速度分量，即横摆角速度(yaw rate)，而无法获得纵向角速度(pitch rate)和滚动角速度(roll rate)。
在又一种方法中，车辆中安装相机或者摄像头，用于提供连续的图像。基于上述两帧或者多帧图像，利用光流法、特征点法或者直接优化光亮度的目标函数的方法，从而可以获得传感器相对于大地的(归一化或者尺度伸缩的)平移运动速度矢量和转动速度矢量。但是该方法存在尺度问题，且深度和平动速度的各个分量耦合在一起，通常无法获得更加准确的深度，从而导致无法获得精确的平移运动速度，或者只能获得归一化或者尺度伸缩的平移运动速度。
请参见图3，图3是本申请实施例提供的一种自运动估计方法，该方法的执行主体可以是传感器系统或者融合感知系统或者集成上述系统的规划/控制系统如辅助驾驶或者自动驾驶系统等。或者，该方法的执行主体也可以是软件或者硬件(如与相应传感器通过无线或者有线连接或者集成在一起的数据处理装置)。以下不同的执行步骤可以集中式实现，或者，以下不同的执行步骤也可以分布式实现。所述方法包括但不限于如下步骤：
步骤S301：获取来自传感器的传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000071
具体地，第一平动速度矢量
Figure BDA0002631962550000072
也可以称为第一平移运动速度矢量
Figure BDA0002631962550000073
传感器可以为视觉传感器如相机、摄像头、红外传感器或者其它成像传感器等等，本申请实施例不做限定。
具体地，获取传感器自运动的第一转动速度矢量ωs和第一平动速度矢量
Figure BDA0002631962550000074
的方式可以是通过有线或者无线方式获取，例如在同一处理器模块中可以直接读取；同一硬件系统中可以通过总线如外设部件互连标准(peripheral component interconnect，PCI)总线获取，或者通过网络如车内的各种类型控制器局域网络(controller area network，CAN)获取；或者通过无线方式如通过云端通信方式获取，当然也可以是其他的形式获取的，本申请实施例不做限定。
具体地，传感器自运动的第一转动速度矢量ωs和第一平动速度矢量
Figure BDA0002631962550000075
可以根据传感器获取的数据中的特征点、线或者、平面或者区域，基于数据的光学特性或者几何特性确定，例如基于8点法或者5点法或者单应性(Homography)或者光流法等方法得到，本申请实施例不做限定。
例如，第一转动速度矢量ωs可以表示为ωs＝[ωs，xωs，yωs，z]T，其中ωs，x，ωs，y和ωs，z为传感器自运动的第一转动速度在三个坐标轴的分量；归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000076
可以表示为
Figure BDA0002631962550000077
具体的，
Figure BDA0002631962550000078
可以是传感器自运动的第一平动速度矢量t′的归一化矢量值或者尺度伸缩的矢量值；例如归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000079
为传感器自运动的第一平动速度矢量t′的尺度伸缩或者归一化，
Figure BDA00026319625500000710
其中t′为传感器自运动的第一平动速度矢量，可以表示为：
t′＝[t′x t′y t′z]T，
其中归一化值s′或者尺度伸缩值s′，可以是矢量t′的幅度或者范数值，即s′＝||t′||。
或者，
Figure BDA00026319625500000711
其中归一化值s′或者尺度伸缩值s′，也可以是矢量t′某一分量，例如可以是矢量t′的第三分量t′z，即：
s′＝t′z，
其中归一化值s′或者尺度伸缩值s′也可以是其它未知的标量，例如深度，此处不做限定。
可选地，作为一种实现方式，传感器或者传感器所在的载体在平面内运动，所述平面如地面或者平面轨道。第一转动速度矢量ωs可以表示为ωs＝[0 0ωs，z]T，其中传感器自运动的第一转动速度在X轴方向的分量ωs，x＝0，传感器自运动的第一转动速度在Y轴方向的分量ωs，y＝0；归一化或尺度伸缩的第一平动速度矢量
Figure BDA00026319625500000712
可以表示为
Figure BDA00026319625500000713
此时，第一转动速度矢量ωs可以简化为用ωs，z表示，
Figure BDA00026319625500000714
可以简化为用
Figure BDA00026319625500000715
表示。
步骤S302：根据第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量
Figure BDA00026319625500000716
以及传感器的外部参数确定载体的第二转动速度矢量ωe和/或第二平动速度矢量te。
具体地，载体的第二平动速度矢量te为完整的平动速度矢量，该传感器的外部参数包括传感器坐标系与载体坐标系的转换关系，该外部参数可以包括传感器坐标系相对于载体坐标系平移和/或传感器坐标系相对于载体坐标系的旋转参数；或者，包括载体坐标系相对于传感器坐标系的平移和/或载体坐标系相对于传感器坐标系的旋转参数。
具体地，传感器坐标系相对于载体坐标系平移可以是传感器坐标系原点相对于载体坐标系原点的平移矢量；或者，传感器坐标系原点在载体坐标系原点中的位置矢量，此处不做限定。传感器坐标系相对于载体坐标系的旋转可以用传感器坐标系相对于载体坐标系旋转的旋转矩阵或者四元数或者欧拉角表示，此处不做限定。
具体地，载体坐标系相对于传感器坐标系平移可以是载体坐标系原点相对于传感器坐标系原点的平移矢量；或者，载体坐标系原点在传感器坐标系原点中的位置矢量，此处不做限定。载体坐标系相对于传感器坐标系的旋转可以用载体坐标系相对于传感器坐标系旋转的旋转矩阵或者四元数或者欧拉角表示，此处不做限定。
具体地，传感器坐标系相对于载体坐标系的外部参数与传感器在载体上的安装位置相关，如当传感器安装在车体(载体)的保险杠的中心位置时，则传感器坐标系如图4所示，通常X轴方向向前，采用右手坐标系。图4为一种传感器坐标系的表示形式，如传感器坐标系用O1XYZ表示。载体坐标系与载体固连，载体坐标系的原点为载体中心，如车载坐标系的原点位于车后轴中心位置，如图5所示，通常X轴方向向前，采用右手坐标系。图5为一种载体坐标系的表示形式，如载体坐标系用O2XbYbZb表示，其中O2Xb沿载体横轴向右，O2Yb沿载体纵轴向前，O2Zb沿载体立轴向上。
可选地，作为一种实现方式，转换关系可以包括传感器坐标系相对于载体坐标系的旋转矩阵R和平移矢量r。传感器测量的目标位置矢量为pb，在载体坐标系下的位置矢量为pe，可以表示为：
pe＝Rpb+r。
可选地，作为一种实现方式，传感器或者传感器所在的载体在平面内运动，上述坐标旋转可以用围绕z轴方向的旋转表示。
在一种实现方式中，根据旋转矩阵R和第一转动速度矢量ωs确定载体的第二转动速度矢量ωe。
具体地，所述载体的第二转动速度矢量ωe为旋转矩阵R和所述第一转动速度矢量ωs的乘积。例如，旋转矩阵为R，第一转动速度矢量ωs，确定载体的第二转动速度矢量为：
ωe＝Rωs。
由于传感器坐标系相对于载体坐标系的旋转可以用传感器坐标系相对于载体坐标系旋转的旋转矩阵或者四元数或者欧拉角表示，那么此时，也可以根据四元数或者欧拉角确定载体的第二转动速度矢量，此处不进一步赘述。
在一种示例中，传感器坐标系跟载体坐标系各个对应的轴方向相同，此时所述第二转动速度矢量与所述第一转动速度矢量相等，ωe＝ωs，此时R为单位矩阵I。
在一种实现方式中，根据第二转动速度矢量ωe和归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000081
以及传感器的外部参数，确定载体的第二平动速度矢量te。
具体地，传感器的外部参数可以包括传感器坐标系相对于载体坐标系的平移矢量r和/或旋转参数，例如旋转参数可以是旋转矩阵、四元数或者欧拉角等。
传感器的外部参数包括平移矢量r，在一种实现方式中，根据第二转动速度矢量ωe和平移矢量r确定载体转动引起的瞬时速度成分；根据旋转矩阵R和归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000091
确定传感器相对于载体坐标系的归一化瞬时速度矢量；根据瞬时速度成分和归一化瞬时速度矢量确定载体的第二平动速度矢量te。
具体地，根据第二转动速度矢量ωe和平移矢量r确定载体转动引起的瞬时速度成分tR可以是指载体转动引起的瞬时速度成分tR为第二转动速度矢量ωe和平移矢量r的叉积；tR＝ωe×r。
其中×表示矢量的叉积。具体地，可以为：
Figure BDA0002631962550000092
其中，ωe＝[ωe，x ωe，y ωe，z]T。
或者载体转动引起的瞬时速度成分tR为平移矢量r和第二转动速度矢量ωe的叉积的负向量，
tR＝-r×ωe，
其中×表示矢量的叉积。具体地，可以为
Figure BDA0002631962550000093
其中，r＝[rx ry rz]T。当然也可以根据其他的方式，本申请实施例不做限定。
其中，上述载体转动引起的瞬时速度成分tR可以是三维矢量，如tR＝[tR，x tR，ytR，z]T或者tR也可以是二维矢量。
可选地，作为一种实现方式，传感器或者传感器所在的载体在平面内运动，所述平面如地面或者平面轨道。此时，第一转动速度矢量ωs可以简化为用ωs，z表示，
Figure BDA00026319625500000915
可以简化为用
Figure BDA00026319625500000916
表示，平移矢量r＝[rx ry rz]T可以简化为用rx，ry表示，rz＝0。瞬时速度成分tR可以简化为：
tR，x＝-ωe，z·ry，
tR，y＝ωe，z·rx，
tR，z＝0。
tR可以简化为二维矢量，包括tR，x，tR，y两个分量。
在一种示例中，可以根据旋转参数和归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000094
确定传感器相对于载体坐标系的归一化瞬时速度矢量。
具体地，传感器相对于载体的旋转参数，例如旋转参数可以是旋转矩阵、四元数或者欧拉角等。
以旋转矩阵R为例，根据旋转矩阵R和归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000095
确定传感器相对于载体坐标系的归一化瞬时速度矢量可以是指传感器相对于载体坐标系的归一化瞬时速度矢量
Figure BDA0002631962550000096
为所述旋转矩阵R和归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000097
的乘积，即
Figure BDA0002631962550000098
在一种实现方式中，R为单位矩阵，传感器相对于载体坐标系的归一化瞬时速度矢量与归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000099
相等，
Figure BDA00026319625500000910
Figure BDA00026319625500000911
可以是三维矢量，
Figure BDA00026319625500000912
或者，
Figure BDA00026319625500000913
也可以是二维矢量，包括
Figure BDA00026319625500000914
两个分量。
在一种示例中，根据瞬时速度成分和归一化瞬时速度矢量确定载体的第二平动速度矢量te可以是指载体的第二平动速度矢量te为瞬时速度矢量与瞬时速度成分的差，所述瞬时速度矢量根据所述归一化瞬时速度矢量与瞬时速度成分确定。
具体地，所述载体的第二平动速度矢量te为：
Figure BDA0002631962550000101
其中，tR，y为tR的第二分量，即沿y轴方向的分量，
Figure BDA0002631962550000102
为
Figure BDA0002631962550000103
的第二分量，即沿y轴方向的分量。
具体地，所述瞬时速度矢量根据所述归一化瞬时速度矢量与瞬时速度成分确定，包括
所述瞬时速度矢量根据所述归一化瞬时速度矢量及传感器坐标系的平动速度矢量的幅度确定，其中传感器坐标系的平动速度矢量的幅度根据所述瞬时速度成分tR的第二分量以及所述归一化瞬时速度矢量的第二分量确定，所述传感器坐标系的平动速度矢量的幅度可以为：
Figure BDA0002631962550000104
在又一种示例中，根据瞬时速度成分和归一化瞬时速度矢量确定载体的第二平动速度矢量te包括：根据瞬间速度成分和归一化瞬时速度矢量确定传感器坐标系的平动速度矢量的幅度；根据传感器坐标系的平动速度矢量的幅度、归一化瞬时速度矢量以及瞬时速度成分确定载体的第二平动速度矢量te。
具体地，根据瞬间速度成分和归一化瞬时速度矢量确定传感器坐标系的平动速度矢量的幅度，包括：
Figure BDA0002631962550000105
其中，s表示传感器坐标系的平动速度矢量的幅度。
具体地，根据传感器坐标系的平动速度矢量的幅度、归一化瞬时速度矢量以及瞬时速度成分确定载体的第二平动速度矢量te，包括：
Figure BDA0002631962550000106
其中，s为传感器坐标系的平动速度矢量的幅度，
Figure BDA0002631962550000107
为归一化瞬时速度矢量，tR为瞬时速度成分。其中，te，y为0，由于车辆在被设计的时候，车辆如果转动，那么车后轴的位置的中心，即在载体坐标系的y轴方向的速度需要为0，即te，y为0，若te，y不为0，车辆会打滑，因此，te，y为0。
te可以是三维矢量，te＝[te，x te，y te，z]T或者te也可以是二维矢量，包括te，x，te，y两个速度分量。
可选的，本申请实施例可以进一步包括步骤S303：
步骤S303：根据传感器坐标系的平动速度矢量的幅度s和归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000108
确定传感器自运动的第三平动速度矢量ts。
具体地，传感器坐标系的平动速度矢量的幅度s的计算方式如步骤S302所述，本步骤不再赘述。传感器自运动的第三平动速度矢量ts为完整的平动速度矢量。根据传感器坐标系的平动速度矢量的幅度s和归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000109
确定传感器自运动的第三平动速度矢量ts，包括：
Figure BDA00026319625500001010
需要指出的是，传感器自运动的第三平动速度矢量为传感器瞬时速度矢量；或者传感器自运动的第三平动速度矢量为传感器坐标系原点的瞬时速度矢量。
进一步地，传感器自运动的第三平动速度矢量可以转换到传感器坐标系表示为t′s，如，
t′s＝R-1ts，
可选的，本申请实施例可以进一步包括步骤S304：
步骤S304：确定静止障碍物的深度z。
具体地，在步骤S304中，传感器为视觉传感器。
其中，步骤S304包括步骤S3041和步骤S3042。具体如下：
步骤S3041：获取静止障碍物的流矢量。
具体地，流矢量包括静止障碍物在视觉传感器的图像平面上的投影的运动矢量。获取静止障碍物的流矢量的方法有多种，流矢量可以通过光流获取，例如可以通过Lukas-Kanade光流算法或者Horn-Schunck光流算法等算法确定。本申请实施例不做限定。
为了理解方便，本申请实施例提供了一种利用Lukas-Kanade光流法获取静止障碍物的流矢量的方式，如下：
静止障碍物在图像上的位置(px，py)对应的流矢量可以用位置(px，py)对应的光流(u，v)计算，具体地，可以通过以下关系式获取，
Figure BDA0002631962550000111
其中G为梯度矩阵，b为失配矢量，具体地，
Figure BDA0002631962550000112
Figure BDA0002631962550000113
其中，Ix(x，y)和Iy(x，y)为图像点(x，y)的亮度在该点(x，y)处的x和y方向的梯度；It为图像亮度对时间的变化量，[px-wx，px+wx]和[py-wy，py+wy]为点(px，py)对应的窗口，其大小为(2wx+1)(2wy+1)，wx和wy为预先设定的参数，例如wx＝wy＝1，2，3......。
需要指出的是，即使对于Lukas-Kanade光流算法也有多种实现形式，例如基于金字塔结构分层估计，此处不赘述。
步骤S3042：根据所述静止障碍物的流矢量、所述第三平动速度矢量ts和所述第一转动速度矢量ωs确定所述静止障碍物的深度z。
具体地，传感器自运动的第三平动速度矢量ts可以转换到传感器坐标系表示为t′s，ωs为传感器自运动的第一转动速度矢量。静止障碍物的流矢量可以表示为(u，v)。
根据所述静止障碍物的流矢量、所述第三平动速度矢量t′s和所述第一转动速度矢量ωs确定所述静止障碍物的深度z，可以是基于以下关系式确定，
Figure BDA0002631962550000114
Figure BDA0002631962550000115
其中，s1＝[-f 0 x]T，s2＝[0 -f y]T，
Figure BDA0002631962550000116
×表示矢量的叉积，f为相机焦距，x，y为图像平面的像素位置，x∈[px-wx，px+wx]，y∈[py-wy，py+wy]；其中wx和wy为非负整数，wx＝0，1，2，3，4...；wy＝0，1，2，3，4...。
具体地，静止障碍物的深度z，可以根据以下关系式得到：
Figure BDA0002631962550000117
其中w1(x，y)和w2(x，y)为加权系数，可以根据上述参数u，v和t′s和ωs的估计精度或者误差确定。
具体地，作为一种实现，
Figure BDA0002631962550000121
Figure BDA0002631962550000122
或者
Figure BDA0002631962550000123
或者
Figure BDA0002631962550000124
或者
Figure BDA0002631962550000125
可选的，本申请实施例可以进一步包括步骤S305。
步骤S305：根据载体的第二转动速度矢量ωe和第二平动速度矢量te，以及通过其他传感器获得的载体的转动速度矢量ωe2和平动速度矢量te2确定载体的转动速度矢量
Figure BDA0002631962550000126
和平动速度矢量
Figure BDA0002631962550000127
具体地，载体的第二转动速度矢量ωe和第二平动速度矢量te可以是根据视觉传感器提供的数据获得的，载体的转动速度矢量ωe2和平动速度矢量te2可以是根据雷达传感器等其他传感器提供的数据获得的，其中，通过其他传感器可以获得一组或者多组载体的转动速度矢量和平动速度矢量。
例如，为了理解方便，本申请实施例中，除了通过视觉传感器获得载体的第二转动速度矢量ωe和第二平动速度矢量te之外，还通过雷达传感器获得载体的转动速度矢量ωe2和平动速度矢量te2。那么，载体的加权转动速度矢量
Figure BDA0002631962550000128
可以根据以下公式得到：
Figure BDA0002631962550000129
其中加权系数w1，w2可以为
Figure BDA00026319625500001210
或者根据ωe和ωe2的误差协方差或者标准差确定，例如正比于协方差或者标准差的逆矩阵或者倒数确定。
加权平动速度矢量
Figure BDA00026319625500001211
可以根据以下公式得到：
Figure BDA00026319625500001212
其中加权系数w1，w2可以为
Figure BDA00026319625500001213
或者根据ωe和ωe2的误差协方差或者标准差确定，例如正比于协方差或者标准差的逆矩阵或者倒数确定。
除了上述加权形式，还包括其他的加权形式，本申请实施例不做限定。
在图3所描述的方法中，根据传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量
Figure BDA00026319625500001214
以及传感器坐标系和载体坐标系的转换关系确定载体的第二转动速度矢量ωe和第二平动速度矢量te，其中，第二平动速度矢量te为完整的平动速度矢量，而现有技术当中，通常获得传感器自运动的归一化或尺度伸缩的第一平动速度矢量
Figure BDA00026319625500001215
通常缺少一个自由度的信息，而本申请实施例能够精确的获得载体的第二转动速度矢量ωe和完整的第二平动速度矢量te，即通过传感器的外部参数恢复缺失的信息，从而更加准确的估计传感器所在的载体和传感器的运动速度，提高了传感器或者传感器所在的载体自运动估计的精度，从而提高了辅助驾驶、自动驾驶或者无人驾驶的安全性。
上述详细阐述了本申请实施例的方法，下面提供了本申请实施例的装置。
请参见图6，图6是本申请实施例提供的一种自运动估计装置的结构示意图，该自运动估计装置可以包括第一获取单元601和第一确定单元602，其中，各个单元的详细描述如下。
第一获取单元601，用于获取传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000131
第一确定单元602，用于根据所述第一转动速度矢量ωs、所述第一平动速度矢量
Figure BDA0002631962550000132
以及所述传感器的外部参数确定载体的第二转动速度矢量ωe和第二平动速度矢量te，所述载体为所述传感器所在的载体，所述传感器的外部参数包括传感器坐标系与载体坐标系的转换关系。
在一种实现方式中，所述转换关系包括：所述传感器坐标系相对于所述载体坐标系的旋转矩阵R和平移矢量r。
在又一种实现方式中，所述第一确定单元602，用于根据所述旋转矩阵R和所述第一转动速度矢量ωs确定所述载体的第二转动速度矢量ωe。
在又一种实现方式中，所述第一确定单元602，用于根据所述第二转动速度矢量ωe和所述平移矢量r确定所述载体转动引起的瞬时速度成分；根据所述旋转矩阵R和所述第一平动速度矢量
Figure BDA0002631962550000133
确定所述传感器相对于所述载体坐标系的归一化瞬时速度矢量；根据所述瞬时速度成分和所述归一化瞬时速度矢量确定所述载体的第二平动速度矢量te。
可选地，在又一种实现方式中，所述第一确定单元602，用于根据所述瞬间速度成分和所述归一化瞬时速度矢量确定所述传感器坐标系的平动速度矢量的幅度；根据所述传感器坐标系的平动速度矢量的幅度、所述归一化瞬时速度矢量以及所述瞬时速度成分确定所述载体的第二平动速度矢量te。
可选地，在又一种实现方式中，所述载体的第二平动速度矢量te，
Figure BDA0002631962550000134
所述s为所述传感器坐标系的平动速度矢量的幅度，所述
Figure BDA0002631962550000135
为所述归一化瞬时速度矢量，所述tR为所述瞬时速度成分。
可选地，在又一种实现方式中，所述装置还包括：第二确定单元，用于根据所述传感器坐标系的平动速度矢量的幅度s和所述第一平动速度矢量
Figure BDA0002631962550000136
确定所述传感器自运动的第三平动速度矢量ts。
可选地，在又一种实现方式中，所述传感器为视觉传感器，所述装置还包括：第二获取单元，用于获取静止障碍物的流矢量，所述流矢量包括所述静止障碍物在所述视觉传感器的图像平面上的运动矢量；第三确定单元，用于根据所述静止障碍物的流矢量、所述第三平动速度矢量ts和所述第一转动速度矢量ωs确定所述静止障碍物的深度z。
需要说明的是，各个单元的实现及有益效果还可以对应参照图3所示的方法实施例的相应描述。
请参见图7，图7是本申请实施例提供的一种自运动估计装置700，该装置700包括至少一个处理器701和通信接口702，可选的，还包括存储器703。所述存储器703用于存储计算机程序，所述至少一个处理器701、存储器703和通信接口702通过总线704相互连接。
存储器703包括但不限于是随机存储记忆体(random access memory，RAM)、只读存储器(read-on1y memory，ROM)、可擦除可编程只读存储器(erasable programmableread only memory，EPROM)、或便携式只读存储器(compact disc read-only memory，CD-ROM)，该存储器703用于相关计算机程序及数据。通信接口702用于接收和发送数据。
处理器701可以是一个或多个中央处理器(central processing unit，CPU)，在处理器701是一个CPU的情况下，该CPU可以是单核CPU，也可以是多核CPU。
该装置700中的处理器701读取所述存储器703中存储的计算机程序，用于执行以下操作：
获取传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量
Figure BDA0002631962550000141
根据所述第一转动速度矢量ωs、所述第一平动速度矢量
Figure BDA0002631962550000142
以及所述传感器的外部参数确定载体的第二转动速度矢量ωe和第二平动速度矢量te，所述载体为所述传感器所在的载体，所述传感器的外部参数包括传感器坐标系与载体坐标系的转换关系。
可选地，在一种实现方式中，所述转换关系包括：所述传感器坐标系相对于所述载体坐标系的旋转矩阵R和平移矢量r。
可选地，在又一种实现方式中，所述处理器，用于根据所述旋转矩阵R和所述第一转动速度矢量ωs确定所述载体的第二转动速度矢量ωe。
可选地，在又一种实现方式中，所述处理器，用于根据所述第二转动速度矢量ωe和所述平移矢量r确定所述载体转动引起的瞬时速度成分；根据所述旋转矩阵R和所述第一平动速度矢量
Figure BDA0002631962550000143
确定所述传感器相对于所述载体坐标系的归一化瞬时速度矢量；根据所述瞬时速度成分和所述归一化瞬时速度矢量确定所述载体的第二平动速度矢量te。
可选地，在又一种实现方式中，所述处理器，用于根据所述瞬间速度成分和所述归一化瞬时速度矢量确定所述传感器坐标系的平动速度矢量的幅度；根据所述传感器坐标系的平动速度矢量的幅度、所述归一化瞬时速度矢量以及所述瞬时速度成分确定所述载体的第二平动速度矢量te。
可选地，在又一种实现方式中，所述载体的第二平动速度矢量te，
Figure BDA0002631962550000144
所述s为所述传感器坐标系的平动速度矢量的幅度，所述
Figure BDA0002631962550000145
为所述归一化瞬时速度矢量，所述tR为所述瞬时速度成分。
在又一种实现方式中，所述处理器，还用于根据所述传感器坐标系的平动速度矢量的幅度s和所述第一平动速度矢量
Figure BDA0002631962550000146
确定所述传感器自运动的第三平动速度矢量ts。
在又一种实现方式中，所述传感器为视觉传感器，所述处理器，还用于获取静止障碍物的流矢量，所述流失量包括所述静止障碍物在所述视觉传感器的图像平面上的运动矢量；根据所述静止障碍物的流矢量、所述第三平动速度矢量ts和所述第一转动速度矢量ωs确定所述静止障碍物的深度z。
需要说明的是，各个操作的实现及有益效果还可以对应参照图3所示的方法实施例的相应描述。
本申请实施例还提供一种芯片系统，所述芯片系统包括至少一个处理器，至少一个处理器和通信接口，可选的，所述芯片系统还包括存储器，所述至少一个处理器用于调用至少一个存储器中存储的计算机程序，以使得图3所示的方法流程得以实现。
本申请实施例还提供一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机程序，当其运行时，图3所示的方法流程得以实现。
本申请实施例还提供一种计算机程序产品，当所述计算机程序产品运行时，图3所示的方法流程得以实现。
本申请实施例还提供了一种终端，所述终端包含至少一个处理器和通信接口，可选的，还包括存储器，所述至少一个处理器用于调用至少一个存储器中存储的计算机程序，以使得图3所示的方法流程得以实现。可选的，所述终端可以为车辆、舰船、卫星、无人机、机器人、智能家居设备或者智能制造设备等。
本申请实施例还提供了一种传感器，所述传感器包括至少一个处理器和通信接口，可选的，所述传感器还包括存储器，所述至少一个处理器用于调用至少一个存储器中存储的计算机程序，以使得图3所示的方法流程得以实现。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，该流程可以由计算机程序来计算机程序相关的硬件完成，该计算机程序可存储于计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法实施例的流程。而前述的存储介质包括：ROM或随机存储记忆体RAM、磁碟或者光盘等各种可存储计算机程序代码的介质。

Claims (18)

1.一种自运动估计方法，其特征在于，包括：
获取传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量
Figure FDA0002631962540000013
根据所述第一转动速度矢量ωs、所述第一平动速度矢量
Figure FDA0002631962540000012
以及所述传感器的外部参数确定载体的第二转动速度矢量ωe和第二平动速度矢量te，所述载体为所述传感器所在的载体，所述传感器的外部参数包括传感器坐标系与载体坐标系的转换关系。
2.根据权利要求1所述的方法，其特征在于，所述转换关系包括：所述传感器坐标系相对于所述载体坐标系的旋转矩阵R和平移矢量r。
3.根据权利要求2所述的方法，其特征在于，所述根据所述第一转动速度矢量ωs、所述第一平动速度矢量
Figure FDA0002631962540000014
以及所述传感器的外部参数确定所述载体的第二转动速度矢量ωe和第二平动速度矢量te，包括：
根据所述旋转矩阵R和所述第一转动速度矢量ωs确定所述载体的第二转动速度矢量ωe。
4.根据权利要求1-3任一项所述的方法，其特征在于，所述根据所述第一转动速度矢量ωs、所述第一平动速度矢量
Figure FDA0002631962540000015
以及所述传感器的外部参数确定所述载体的第二转动速度矢量ωe和第二平动速度矢量te，包括：
根据所述第二转动速度矢量ωe和所述平移矢量r确定所述载体转动引起的瞬时速度成分；
根据所述旋转矩阵R和所述第一平动速度矢量
Figure FDA0002631962540000016
确定所述传感器相对于所述载体坐标系的归一化瞬时速度矢量；
根据所述瞬时速度成分和所述归一化瞬时速度矢量确定所述载体的第二平动速度矢量te。
5.根据权利要求4所述的方法，其特征在于，所述根据所述瞬时速度成分和所述归一化瞬时速度矢量确定所述载体的第二平动速度矢量te，包括：
根据所述瞬间速度成分和所述归一化瞬时速度矢量确定所述传感器坐标系的平动速度矢量的幅度；
根据所述传感器坐标系的平动速度矢量的幅度、所述归一化瞬时速度矢量以及所述瞬时速度成分确定所述载体的第二平动速度矢量te。
6.根据权利要求5所述的方法，其特征在于，所述载体的第二平动速度矢量te，
Figure FDA0002631962540000011
所述s为所述传感器坐标系的平动速度矢量的幅度，所述
Figure FDA0002631962540000018
为所述归一化瞬时速度矢量，所述tR为所述瞬时速度成分。
7.根据权利要求1-6任一项所述的方法，其特征在于，所述方法还包括：
根据所述传感器坐标系的平动速度矢量的幅度s和所述第一平动速度矢量
Figure FDA0002631962540000017
确定所述传感器自运动的第三平动速度矢量ts。
8.根据权利要求1-7任一项所述的方法，其特征在于，所述传感器为视觉传感器，所述方法还包括：
获取静止障碍物的流矢量，所述流矢量包括所述静止障碍物在所述视觉传感器的图像平面上的运动矢量；
根据所述静止障碍物的流矢量、所述第三平动速度矢量ts和所述第一转动速度矢量ωs确定所述静止障碍物的深度z。
9.一种自运动估计装置，其特征在于，包括：
第一获取单元，用于获取传感器自运动的第一转动速度矢量ωs、归一化或尺度伸缩的第一平动速度矢量
Figure FDA0002631962540000022
第一确定单元，用于根据所述第一转动速度矢量ωs、所述第一平动速度矢量
Figure FDA0002631962540000023
以及所述传感器的外部参数确定载体的第二转动速度矢量ωe和第二平动速度矢量te，所述载体为所述传感器所在的载体，所述传感器的外部参数包括传感器坐标系与载体坐标系的转换关系。
10.根据权利要求9所述的装置，其特征在于，所述转换关系包括：所述传感器坐标系相对于所述载体坐标系的旋转矩阵R和平移矢量r。
11.根据权利要求10所述的装置，其特征在于，
所述第一确定单元，用于根据所述旋转矩阵R和所述第一转动速度矢量ωs确定所述载体的第二转动速度矢量ωe。
12.根据权利要求9-11任一项所述的装置，其特征在于，
所述第一确定单元，用于根据所述第二转动速度矢量ωe和所述平移矢量r确定所述载体转动引起的瞬时速度成分；根据所述旋转矩阵R和所述第一平动速度矢量
Figure FDA0002631962540000024
确定所述传感器相对于所述载体坐标系的归一化瞬时速度矢量；根据所述瞬时速度成分和所述归一化瞬时速度矢量确定所述载体的第二平动速度矢量te。
13.根据权利要求12所述的装置，其特征在于，
所述第一确定单元，用于根据所述瞬间速度成分和所述归一化瞬时速度矢量确定所述传感器坐标系的平动速度矢量的幅度；根据所述传感器坐标系的平动速度矢量的幅度、所述归一化瞬时速度矢量以及所述瞬时速度成分确定所述载体的第二平动速度矢量te。
14.根据权利要求13所述的装置，其特征在于，所述载体的第二平动速度矢量te，
Figure FDA0002631962540000021
所述s为所述传感器坐标系的平动速度矢量的幅度，所述
Figure FDA0002631962540000026
为所述归一化瞬时速度矢量，所述tR为所述瞬时速度成分。
15.根据权利要求9-14任一项所述的装置，其特征在于，所述装置还包括：
第二确定单元，用于根据所述传感器坐标系的平动速度矢量的幅度s和所述第一平动速度矢量
Figure FDA0002631962540000025
确定所述传感器自运动的第三平动速度矢量ts。
16.根据权利要求9-15任一项所述的装置，其特征在于，所述传感器为视觉传感器，所述装置还包括：
第二获取单元，用于获取静止障碍物的流矢量，所述流矢量包括所述静止障碍物在所述视觉传感器的图像平面上的运动矢量；
第三确定单元，用于根据所述静止障碍物的流矢量、所述第三平动速度矢量ts和所述第一转动速度矢量ωs确定所述静止障碍物的深度z。
17.一种芯片系统，其特征在于，所述芯片系统包括至少一个处理器和通信接口，所述至少一个处理器用于调用至少一个存储器中存储的计算机程序，以使得所述芯片系统所在装置实现如权利要求1-8中任一项所述的方法。
18.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质中存储有计算机程序，当所述计算机程序在一个或多个处理器上运行时，执行如权利要求1-8中任一项所述的方法。
CN202010822885.9A 2020-08-13 2020-08-13 一种自运动估计方法及相关装置 Pending CN114077719A (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202010822885.9A CN114077719A (zh) 2020-08-13 2020-08-13 一种自运动估计方法及相关装置
PCT/CN2021/098209 WO2022033139A1 (zh) 2020-08-13 2021-06-03 一种自运动估计方法及相关装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010822885.9A CN114077719A (zh) 2020-08-13 2020-08-13 一种自运动估计方法及相关装置

Publications (1)

Publication Number Publication Date
CN114077719A true CN114077719A (zh) 2022-02-22

Family

ID=80246935

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010822885.9A Pending CN114077719A (zh) 2020-08-13 2020-08-13 一种自运动估计方法及相关装置

Country Status (2)

Country Link
CN (1) CN114077719A (zh)
WO (1) WO2022033139A1 (zh)

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102435188B (zh) * 2011-09-15 2013-10-02 南京航空航天大学 一种用于室内环境的单目视觉/惯性全自主导航方法
CN105606095B (zh) * 2016-03-17 2018-11-13 广州展讯信息科技有限公司 一种简化惯性导航设备安装要求的方法与装置
CN108318027B (zh) * 2017-01-18 2020-09-01 腾讯科技（深圳）有限公司 载体的姿态数据的确定方法和装置
US20180364048A1 (en) * 2017-06-20 2018-12-20 Idhl Holdings, Inc. Methods, architectures, apparatuses, systems directed to device position tracking
CN108549399B (zh) * 2018-05-23 2020-08-21 深圳市道通智能航空技术有限公司 飞行器偏航角修正方法、装置及飞行器

Also Published As

Publication number Publication date
WO2022033139A1 (zh) 2022-02-17

Similar Documents

Publication Publication Date Title
CN109887057B (zh) 生成高精度地图的方法和装置
US11650334B2 (en) Acceleration of real time computer vision processing on UAVs through GPS attitude estimation
JP7073315B2 (ja) 乗物、乗物測位システム、及び乗物測位方法
US10788830B2 (en) Systems and methods for determining a vehicle position
Panahandeh et al. Vision-aided inertial navigation based on ground plane feature detection
JP5991952B2 (ja) 周囲状況に対するカメラの姿勢を求めるための方法
CN112740274B (zh) 在机器人设备上使用光流传感器进行vslam比例估计的系统和方法
US11747144B2 (en) Real time robust localization via visual inertial odometry
CN109991636A (zh) 基于gps、imu以及双目视觉的地图构建方法及系统
CN113223161B (zh) 一种基于imu和轮速计紧耦合的鲁棒全景slam系统和方法
CN109300143B (zh) 运动向量场的确定方法、装置、设备、存储介质和车辆
CN112789655A (zh) 用于标定惯性测试单元和相机的系统和方法
CN111380514A (zh) 机器人位姿估计方法、装置、终端及计算机存储介质
CN110411457B (zh) 基于行程感知与视觉融合的定位方法、系统、终端和存储介质
CN111426320B (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
JP2019528501A (ja) マルチカメラシステムにおけるカメラ位置合わせ
KR102006291B1 (ko) 전자 장치의 이동체 포즈 추정 방법
KR102117313B1 (ko) 그래디언트 추정 장치, 그래디언트 추정 방법, 컴퓨터 프로그램 및 제어 시스템
CN110458885B (zh) 基于行程感知与视觉融合的定位系统和移动终端
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach
CN108322698B (zh) 基于多摄像机和惯性测量单元融合的系统和方法
CN116721166B (zh) 双目相机和imu旋转外参在线标定方法、装置及存储介质
CN116952229A (zh) 无人机定位方法、装置、系统和存储介质
US20200150280A1 (en) Vehicle localization
CN114077719A (zh) 一种自运动估计方法及相关装置

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