WO2022037370A1 - 一种运动估计方法及装置 - Google Patents

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

Info

Publication number
WO2022037370A1
WO2022037370A1 PCT/CN2021/108627 CN2021108627W WO2022037370A1 WO 2022037370 A1 WO2022037370 A1 WO 2022037370A1 CN 2021108627 W CN2021108627 W CN 2021108627W WO 2022037370 A1 WO2022037370 A1 WO 2022037370A1
Authority
WO
WIPO (PCT)
Prior art keywords
velocity vector
estimated value
sensor
carrier
translational velocity
Prior art date
Application number
PCT/CN2021/108627
Other languages
English (en)
French (fr)
Inventor
王建国
陈默
Original Assignee
华为技术有限公司
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 华为技术有限公司 filed Critical 华为技术有限公司
Publication of WO2022037370A1 publication Critical patent/WO2022037370A1/zh

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/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
    • 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
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/46—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being of a radio-wave signal type
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Definitions

  • the present application relates to the field of sensor technology, and in particular, to a motion estimation method and device.
  • Advanced driver assistant system or autonomous driving (ADS) system will be equipped with a variety of sensors, such as millimeter-wave radar, lidar, ultrasonic sensors such as sonar, visual sensors such as cameras or cameras and other sensors, It is used to perceive the surrounding environment information, and the surrounding environment information includes moving objects and stationary objects.
  • sensors such as millimeter-wave radar, lidar, ultrasonic sensors such as sonar, visual sensors such as cameras or cameras and other sensors, It is used to perceive the surrounding environment information, and the surrounding environment information includes moving objects and stationary objects.
  • different methods are usually used for analysis and processing, for example, classifying, identifying and tracking moving objects (such as vehicles and pedestrians), classifying and identify.
  • additional information can be provided for autonomous driving, such as avoiding obstacles, providing drivable areas, etc.
  • the sensor can usually be mounted on the carrier, and the sensor follows the movement of the carrier on which the sensor is located.
  • the motion of the carrier where the sensor is located makes it impossible to analyze the moving target and the stationary target independently. Therefore, it is necessary to estimate the motion of the carrier where the sensor is located, so as to realize the separation of the moving target and the stationary target.
  • the tracking of moving targets is usually based on motion models, such as constant velocity (CV)/constant acceleration (CA)/coordinated circular motion (CT) models, and the models usually assume relative ground or In the geodetic coordinate system, the motion of the carrier where the sensor is located will cause the above model to fail or the tracking performance to degrade. Therefore, it is necessary to compensate for the motion of the carrier where the sensor is located.
  • the present application provides a motion estimation method and device for accurately estimating the motion of a carrier where a sensor is located.
  • the present application provides a motion estimation method, the method comprising:
  • the estimated first translational velocity vector of the carrier is determined, wherein, The estimated value of the first rotation angular velocity vector is determined according to the estimated value of the rotation angular velocity vector of the M first sensors.
  • the first rotational angular velocity vector estimate of the carrier is determined. value, and then combined with the estimated value of the instantaneous velocity vector of the second sensor to determine the estimated value of the first translational velocity vector of the carrier, which helps to obtain more accurate motion of the carrier where the sensor is located.
  • the estimated value of the first translational velocity vector is determined based on the following relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • v 2,j is the estimated value of the instantaneous velocity vector of the j-th second sensor
  • r 2,j is the estimated value of the j-th second sensor
  • the above relational expression is obtained based on the relation between the translational velocity vector, the instantaneous velocity vector, the rotational angular velocity and the positional translation vector of the rigid body, and the above relational expression can have various deformations. According to the above relationship, the estimated value of the first translational velocity vector can be determined more accurately.
  • the estimated value of the first translational velocity vector satisfies the following relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • w 2,j is the weighting coefficient of the j-th second sensor
  • v 2,j is the instantaneous velocity vector of the j-th second sensor
  • the estimated value, r 2,j is the position translation vector of the coordinate system of the jth second sensor relative to the coordinate system of the carrier.
  • the estimated value of the first rotational angular velocity vector satisfies the following relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • w 1,i is the weighting coefficient of the ith first sensor
  • ⁇ 1,i is the estimated value of the rotational angular velocity vector of the ith first sensor.
  • it also includes:
  • a second estimated translational velocity vector of the carrier is determined according to the estimated first translational velocity vector and the normalized estimated translational velocity vector of the M' first sensors.
  • the estimated value of the normalized translational velocity vector obtained by the first sensor evaluating its own motion is more accurate, and the estimated value of the normalized translational velocity vector of the first sensor and the estimated value of the first translational velocity vector are more accurate.
  • the values are fused to obtain the second estimated value of the translational velocity vector of the carrier, which can further improve the accuracy of the estimated value of the translational velocity vector of the carrier.
  • the estimated value of the second translational velocity vector is determined based on the following relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1,i is the position translation vector of the coordinate system of the ith first sensor relative to the carrier coordinate system
  • s i is the ith first sensor
  • the normalization parameter or scaling factor of the translational velocity vector of a sensor, si is determined by the estimated value of the first translational velocity vector.
  • the above relational expression is obtained based on the relation between the translational velocity vector, the instantaneous velocity vector, the rotational angular velocity and the positional translation vector of the rigid body, and the above relational expression can have various deformations. According to the above relationship, the estimated value of the second translational velocity vector can be determined more accurately.
  • the estimated value of the second translational velocity vector satisfies the following relationship:
  • t K is the estimated value of the second translational velocity vector
  • t k is the estimated value of the translational velocity vector of the carrier in the k-th iteration
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1,i is the position translation vector of the coordinate system of the ith first sensor relative to the carrier coordinate system
  • s i,k is the kth
  • s i,k is the estimated value of the first translational velocity vector or the translational velocity of the carrier in the k-1th iteration
  • the vector estimate is determined.
  • the estimated value of the second translational velocity vector satisfies the following relationship:
  • t K is the estimated value of the second translational velocity vector
  • t k is the estimated value of the translational velocity vector of the carrier in the k-th iteration
  • w′ 1,i,k is the weighting coefficient of the i-th first sensor in the k-th iteration
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1,i is the position translation vector of the coordinate system of the ith first sensor relative to the carrier coordinate system
  • s i,k is the kth
  • s i,k is the estimated value of the first translational velocity vector or the translational velocity of the carrier in the k-1th iteration
  • the vector estimate is determined.
  • the normalization parameter or scale scaling factor of the translational velocity vector of the i-th first sensor in the k-th iteration satisfies the following relation:
  • t k-1 is the estimated value of the translational velocity vector of the carrier in the k-1th iteration
  • t0 is the estimated value of the first translational velocity vector
  • the estimated value of the translational velocity vector of the carrier is determined according to the estimated value of the normalized translational velocity vector of the plurality of first sensors. Using the parameters of the plurality of first sensors as the input of one iteration helps to improve the estimation accuracy of the second translational velocity vector.
  • the estimated value of the second translational velocity vector satisfies the following relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1 i is the position translation vector of the coordinate system of the ith first sensor relative to the carrier coordinate system
  • s i, l is the lth round
  • the normalization parameter or scaling factor of the translational velocity vector of the ith first sensor in the iteration, s i,l is estimated by the first translational velocity vector or or Sure.
  • the normalization parameter or scale scaling factor of the translational velocity vector of the i-th first sensor in the l-th round of iteration satisfies the following relationship:
  • determining the translational velocity vector estimated value of the carrier corresponding to each first sensor is equivalent to performing In one iteration, a parameter of a first sensor is used as the input of one iteration, and a relatively accurate second translational velocity vector estimation value can be quickly obtained.
  • it also includes:
  • the estimated value of the second rotational angular velocity vector of the carrier is determined based on the following relation:
  • ⁇ ' is the estimated value of the second rotational angular velocity vector
  • v 2,j is the estimated value of the instantaneous velocity vector of the jth second sensor
  • [r 2,j ] ⁇ is the antisymmetric matrix corresponding to r 2,j
  • r 2,j is the coordinate system of the jth second sensor relative to the carrier coordinate system position translation vector.
  • the more accurate second rotational angular velocity vector estimated value is further determined according to the more accurate second translational velocity vector estimated value.
  • the present application provides a motion estimation device, the device comprising:
  • the obtaining unit is used to obtain the estimated value of the rotational angular velocity vector of the M first sensors and the estimated value of the instantaneous velocity vector of the N second sensors; wherein, M ⁇ 1, N ⁇ 1;
  • the processing unit is configured to determine the first translational velocity of the carrier according to the estimated value of the instantaneous velocity vector of the N second sensors and the estimated value of the first rotational angular velocity vector of the carrier where the N second sensors are located Vector estimated value, wherein the first rotational angular velocity vector estimated value is determined according to the rotational angular velocity vector estimated values of the M first sensors.
  • the processing unit is specifically configured to determine the estimated value of the first translational velocity vector based on the following relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • v 2,j is the estimated value of the instantaneous velocity vector of the j-th second sensor
  • r 2,j is the estimated value of the j-th second sensor
  • the estimated value of the first translational velocity vector satisfies the following relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • w 2,j is the weighting coefficient of the j-th second sensor
  • v 2,j is the instantaneous velocity vector of the j-th second sensor
  • the estimated value, r 2,j is the position translation vector of the coordinate system of the jth second sensor relative to the coordinate system of the carrier.
  • the estimated value of the first rotational angular velocity vector satisfies the following relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • w 1,i is the weighting coefficient of the ith first sensor
  • ⁇ 1,i is the estimated value of the rotational angular velocity vector of the ith first sensor.
  • the acquiring unit is further configured to acquire normalized translational velocity vector estimation values of M' first sensors in the M first sensors, where 1 ⁇ M' ⁇ M; the processing unit is further configured to determine the second translational velocity vector of the carrier according to the first translational velocity vector estimated value and the normalized translational velocity vector estimated values of the M' first sensors Velocity vector estimate.
  • the processing unit is specifically configured to determine the estimated value of the second translational velocity vector based on the following relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1,i is the position translation vector of the coordinate system of the ith first sensor relative to the carrier coordinate system
  • s i is the ith first sensor
  • the normalization parameter or scaling factor of the translational velocity vector of a sensor, si is determined by the estimated value of the first translational velocity vector.
  • the estimated value of the second translational velocity vector satisfies the following relationship:
  • t K is the estimated value of the second translational velocity vector
  • t k is the estimated value of the translational velocity vector of the carrier in the k-th iteration
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1,i is the position translation vector of the coordinate system of the ith first sensor relative to the carrier coordinate system
  • s i,k is the kth
  • s i,k is the estimated value of the first translational velocity vector or the translational velocity of the carrier in the k-1th iteration
  • the vector estimate is determined.
  • the estimated value of the second translational velocity vector satisfies the following relationship:
  • t K is the estimated value of the second translational velocity vector
  • t k is the estimated value of the translational velocity vector of the carrier in the k-th iteration
  • w′ 1,i,k is the weighting coefficient of the i-th first sensor in the k-th iteration
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1,i is the position translation vector of the coordinate system of the ith first sensor relative to the carrier coordinate system
  • s i,k is the kth
  • s i,k is the estimated value of the first translational velocity vector or the translational velocity of the carrier in the k-1th iteration
  • the vector estimate is determined.
  • the normalization parameter or scale scaling factor of the translational velocity vector of the i-th first sensor in the k-th iteration satisfies the following relation:
  • t k-1 is the estimated value of the translational velocity vector of the carrier in the k-1th iteration
  • t0 is the estimated value of the first translational velocity vector
  • the estimated value of the second translational velocity vector satisfies the following relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1 i is the position translation vector of the coordinate system of the ith first sensor relative to the carrier coordinate system
  • s i,l is the lth round
  • the normalization parameter or scaling factor of the translational velocity vector of the ith first sensor in the iteration, s i,l is estimated by the first translational velocity vector or or Sure.
  • the normalization parameter or scale scaling factor of the translational velocity vector of the i-th first sensor in the l-th round of iteration satisfies the following relationship:
  • the processing unit is further configured to determine the estimated value of the second rotational angular velocity vector of the carrier based on the following relation:
  • ⁇ ' is the estimated value of the second rotational angular velocity vector
  • v 2,j is the estimated value of the instantaneous velocity vector of the jth second sensor
  • [r 2,j ] ⁇ is the antisymmetric matrix corresponding to r 2,j
  • r 2,j is the coordinate system of the jth second sensor relative to the carrier coordinate system position translation vector.
  • the present application provides a communication device, comprising at least one processor and a communication interface, the communication interface being configured to receive signals from other communication devices other than the communication device and transmit to the at least one processor Or send a signal from the at least one processor to other communication devices other than the communication device, and the at least one processor is used to implement the first aspect or any of the first aspects through logic circuits or executing code instructions methods in possible implementations.
  • the present application provides a computer-readable storage medium, where a computer program or instruction is stored, and when the computer program or instruction is executed by a communication device, the above-mentioned first aspect or the first aspect is realized.
  • the present application provides a computer program product, the computer program product includes a computer program or an instruction, when the computer program or instruction is executed by a communication device, the above-mentioned first aspect or any possible possibility of the first aspect is realized. method in the implementation.
  • the present application provides a chip, including at least one processor and an interface;
  • the interface for providing program instructions or data for the at least one processor
  • the at least one processor is configured to execute the program line instructions to implement the method in the first aspect or any possible implementation manner of the first aspect.
  • the present application provides a terminal, where the terminal includes any motion estimation apparatus provided in the second aspect, any communication apparatus provided in the third aspect, or any computer-readable storage medium provided in the fourth aspect.
  • the terminal may be a vehicle, an unmanned aerial vehicle, a robot, a smart home device, a satellite, or the like.
  • FIG. 1 is a schematic structural diagram of a self-motion estimation system provided by the application.
  • FIG. 2 is a schematic flowchart of a motion estimation method provided by the present application.
  • FIG. 3 is a schematic diagram of a multi-sensor configuration in a vehicle-mounted system provided by the present application.
  • FIG. 4 is a schematic diagram of a rotational angular velocity vector provided by the application.
  • FIG. 5 is a schematic diagram of a position translation vector provided by the application.
  • FIG. 6 is a schematic diagram of a transformation relationship between a carrier coordinate system and a sensor coordinate system provided by the application;
  • FIG. 7 is a schematic structural diagram of a motion estimation apparatus provided by the application.
  • FIG. 8 is a schematic structural diagram of a chip provided by the present application.
  • FIG. 1 is a schematic structural diagram of a self-motion estimation system provided by an embodiment of the present application.
  • the system includes a first sensor 1010 , a first motion sensing module 1011 , a second sensor 1020 , and a second motion sensing module 1021 and data processing module 1030.
  • the first sensor 1010 may be a visual sensor, such as a camera or camera, an infrared thermal imaging sensor, and the like.
  • the first sensor 1010 may provide visual measurement data, such as images or video.
  • the first motion sensing module 1011 is configured to determine motion measurement data according to the measurement data provided by the first sensor 1010, such as the rotational angular velocity vector of the sensor motion and/or the normalized or scaled translational velocity vector, etc. An estimate of the translational velocity vector with full scale information.
  • the second sensor 1020 may be a radar sensor, an ultrasonic sensor, an inertial measurement sensor or a positioning sensor, etc., for example, a millimeter-wave radar, sonar, lidar, inertial measurement unit (IMU) or global navigation satellite system (global navigation satellite system). system, GNSS), etc.
  • the second sensor 1020 is used to provide position measurement data and/or velocity measurement data, such as measurement data of position and/or radial velocity or velocity projection components.
  • the second motion sensing module 1021 is configured to determine motion measurement data, such as the instantaneous translational velocity vector of the sensor motion, according to the position measurement data and/or velocity measurement data provided by the second sensor 1020 .
  • the data processing module 1030 is configured to process the motion measurement data provided by the first motion sensing module 1011 and the second motion sensing module 1021 . In this application, the motion measurement data may also be referred to as motion sensing data.
  • the first sensor 1010 , the first motion sensing module 1011 , the second sensor 1020 , the second motion sensing module 1021 and the data processing module 1030 can be connected together by wired or wireless means; the first sensor 1010 and the second sensor 1020 can be distributed
  • the first motion sensing module 1011 and the second motion sensing module 1021 can be integrated with the first sensor 1010 and the second sensor 1020 respectively; they can also be integrated with the data processing module 1030; It can exist independently of other modules, which is not limited in this application.
  • the first sensor 1010, the first motion sensing module 1011, the second sensor 1020, the second motion sensing module 1021 and the data processing module 1030 are deployed on one processor system.
  • the first sensor 1010 and the second sensor 1020 are respectively deployed on one processor system; the first motion sensing module 1011, the second motion sensing module 1021 and the data processing module 1030 are deployed on one processor system .
  • the embodiments of the present application can be applied to multi-sensor systems in various carriers, wherein the carrier is vehicle-mounted (such as a car, motorcycle, or bicycle, etc.), airborne (such as drone, helicopter, jet plane, or balloon), ship-borne (such as ships, motorboats or ships, etc.), spaceborne (such as satellites), or intelligent bodies (such as robots, etc.).
  • vehicle-mounted such as a car, motorcycle, or bicycle, etc.
  • airborne such as drone, helicopter, jet plane, or balloon
  • ship-borne such as ships, motorboats or ships, etc.
  • spaceborne such as satellites
  • intelligent bodies such as robots, etc.
  • the carrier is a vehicle, which may carry at least one first sensor and at least one second sensor.
  • 1 first sensor and 1 second sensor are installed on the vehicle, or 1 first sensor and 5 second sensors are installed on the vehicle, or 6 first sensors and 5 second sensors are installed on the vehicle Wait.
  • the following three sensors can be used in vehicle motion estimation.
  • IMU is a device that measures the three-axis attitude angle (or angular velocity) and acceleration of an object.
  • an IMU will be equipped with a three-axis gyroscope and three-direction accelerometer to measure the angular velocity and acceleration of the object in three-dimensional space, and based on this, the movement speed and attitude of the object can be calculated.
  • Radar sensors that typically provide range, azimuth, and radial velocity measurements. Based on the measurement data of the azimuth angle and radial velocity components of the stationary target object, the instantaneous velocity of the sensor relative to the ground can be obtained by using the least squares method or other methods. In particular, the radar sensor can obtain a relatively accurate longitudinal velocity estimation value. . In addition, based on the above velocity estimation value, the yaw rate estimation value of the sensor can be further obtained.
  • Vision sensors can usually provide two or more consecutive frames of images. Based on the above two or more frames of images, using the optical flow method or the method corresponding to feature points or the method of directly optimizing the target object function of brightness (intensity), the translation velocity estimation value and rotational velocity estimation value of the sensor scale scaling can be obtained. .
  • the motion speed estimation of the IMU is based on the accumulation of the accelerometer, and the measurement error will accumulate over time, so there is a problem of error accumulation, which requires additional calibration with other sensors, and the accuracy of the IMU generally used in vehicles is too low , If you choose a high-precision IMU, the cost is high.
  • the vision sensor has the problem of scaling and scaling.
  • the depth information is coupled with the various components of the translational velocity. Usually, it is impossible to obtain an accurate depth estimation, and the estimated value of the translational velocity cannot be accurately obtained, or only one scale of the translational velocity can be obtained. Scaled estimates.
  • the embodiments of the present application provide a motion estimation method, which is used to more accurately determine the estimated value of translational velocity vector and estimated value of rotational angular velocity vector of the carrier, so as to realize accurate motion estimation of the carrier.
  • the estimated value of the translational velocity vector may also be the estimated value of the translational displacement vector, and the estimated value of the translational displacement vector may be the estimated value of the position offset vector between two frames, or it may be the estimated value of the two frames.
  • the product of the time difference between and the estimated value of the translation velocity vector may also be the estimated value of the translational displacement vector, and the estimated value of the translational displacement vector may be the estimated value of the position offset vector between two frames, or it may be the estimated value of the two frames.
  • FIG. 2 is a schematic flowchart of a motion estimation method provided by an embodiment of the present application.
  • the execution body of the method may be a sensor system or a fusion perception system or a planning/control system integrating the above systems, such as assisted driving or automatic driving. driving system, etc.
  • the execution body of the method may also be software or hardware (eg, a data processing device connected or integrated with the corresponding sensor by wireless or wire).
  • the following different execution steps may be implemented in a centralized manner, or, the following different execution steps may also be implemented in a distributed manner.
  • the method includes but is not limited to the following steps:
  • Step 201 Obtain the estimated values of the rotational angular velocity vectors of the M first sensors and the estimated instantaneous velocity vectors of the N second sensors.
  • M first sensors and N second sensors are carried on a carrier, the number of the first sensors is M ⁇ 1, and the number of the second sensors is N ⁇ 1.
  • the first sensor may be a visual sensor such as a camera, a camera, an infrared sensor or other imaging sensors, or an inertial measurement sensor such as an IMU
  • the second sensor may be a radar sensor such as a millimeter-wave radar, a lidar, etc., or an ultrasonic sensor such as Sonar etc.
  • the M (M>1) first sensors may be of the same type or different types.
  • the N (N>1) second sensors may be of the same type or different types.
  • the M first sensors and the N second sensors may be installed in the same position or different positions of the carrier. Take the car as an example to illustrate:
  • Example 1 a camera and a millimeter-wave radar are installed in the front end of the vehicle;
  • Example 2 1 camera and 1 millimeter-wave radar are installed at the front end of the vehicle, and 4 additional millimeter-wave radars are installed at the 4 corner positions of the vehicle;
  • Example 3 One millimeter-wave radar is installed at the front end of the vehicle, four additional millimeter-wave radars are installed at four corner positions, and six cameras are evenly installed on the vehicle.
  • IMU or GNSS can be further installed on the vehicle.
  • the multi-sensors of the in-vehicle system may include one camera, five millimeter-wave radars, and one IMU, where the installation position of the IMU may be close to the in-vehicle coordinate system (It can also be called the origin of the vehicle body coordinate system and the carrier coordinate system).
  • It can also be called the origin of the vehicle body coordinate system and the carrier coordinate system.
  • the origin of the vehicle coordinate system can be located at the center of the rear axle of the vehicle body.
  • the millimeter-wave radar can also be replaced by a laser radar or an ultrasonic sensor, etc.
  • the camera can also be replaced by a camera or an infrared sensor, etc., or at least one laser radar can be added on the original basis.
  • 2 of the 5 millimeter-wave radars can be replaced with lidars, or all 5 millimeter-wave radars can be replaced with lidars. Add 1 to 3 lidars on top of that.
  • the acquisition of the estimated values of the rotation angular velocity vectors of the M first sensors may be obtained directly from the sensors through a wired or wireless interface, wherein the estimated values of the rotation angular velocity vectors may be obtained through motion or estimation algorithms based on the measurement data of the sensors, or directly measured by the sensor;
  • the obtaining of the estimated values of the rotational angular velocity vectors of the M first sensors may be obtained by directly obtaining the measurement data of the sensors from the sensors through a wired or wireless interface, and the estimated rotational angular velocity vector values are obtained by motion or an estimation algorithm according to the measurement data of the sensors, or It is obtained directly from the sensor measurement data.
  • the estimated value of the rotational angular velocity vector can be obtained by estimation according to the measurement data of the first sensor.
  • the measurement data of the first sensor does not directly contain the measurement value of the motion measurement of the first sensor.
  • the first sensor is a visual sensor such as a camera
  • the raw measurement data of the camera is visual measurement data, such as an image or video, which can be based on feature points, lines, planes or regions in the image or video, based on optical characteristics of the data or
  • the geometry determines the camera's rotational angular velocity vector estimate.
  • the estimated value of the rotation angular velocity vector of the camera can be obtained based on the 8-point method, the 5-point method, the homography (Homography), or the optical flow method, or the like. It is the prior art to obtain the rotational angular velocity of the sensor based on the image or video, which will not be repeated here.
  • the estimated value of the rotational angular velocity vector can be directly obtained from the measurement data of the first sensor.
  • the first sensor can directly measure the vector containing the rotational angular velocity.
  • the first sensor is an inertial measurement sensor such as an IMU, and the IMU can directly measure the rotational angular velocity vector.
  • the M first sensors may include the same type or different types of first sensors.
  • the first sensor or the carrier moves in a plane, such as the ground or a plane track.
  • the above-mentioned estimated value of the rotation angular velocity vector obtained directly from the measurement data of the sensor or obtained through motion estimation based on the measurement data of the sensor can be obtained according to the transformation relationship between the sensor coordinate system and the carrier coordinates, and the rotation of the sensor in the carrier coordinate system can be obtained.
  • the obtaining the estimated value of the instantaneous velocity vector of the N second sensors may be obtained directly from the sensor through a wired or wireless interface, wherein the estimated value of the instantaneous velocity vector may be obtained through motion or an estimation algorithm based on the measurement data of the sensor;
  • the obtaining of the estimated instantaneous velocity vector values of the N second sensors may be directly obtained from the sensors through a wired or wireless interface, the measurement data of the sensors, and the estimated instantaneous velocity vector values are obtained according to the measurement data of the sensors through motion or an estimation algorithm.
  • the estimated value of the instantaneous velocity vector can be obtained by estimation according to the measurement data of the second sensor.
  • the measurement data of the second sensor does not directly contain the measurement value of the motion measurement of the second sensor.
  • the second sensor is a millimeter-wave radar or a lidar or an ultrasonic sensor such as a sonar
  • the measurement data of the second sensor may include position and radial velocity, or include angle and radial velocity.
  • the estimated value of the instantaneous velocity vector can be determined according to the measurement data of the stationary target, based on estimation methods such as the least squares method, the orthogonal distance regression method, or the minimum mean square error criterion.
  • the estimated value of the instantaneous velocity vector may also be determined based on the measurement data of the stationary target according to the multiple position measurement data of the second sensor.
  • the embodiments of the present application are not limited.
  • the number of estimated values of the rotational angular velocity vector obtained by the carrier from the first sensor may be less than or equal to the actual number of first sensors in the carrier, and the number of estimated values of the instantaneous velocity vector obtained by the carrier from the second sensor may be less than or equal to the actual number of The number of second sensors in the upload carrier.
  • the carrier carries 6 first sensors and 3 second sensors, and the carrier can obtain rotational angular velocity vector estimates of the 6 first sensors and instantaneous velocity vector estimates of the 3 second sensors.
  • the carrier carries 6 first sensors and 3 second sensors, and the carrier can obtain the estimated rotational angular velocity vector values of the 4 first sensors and the instantaneous velocity vector estimated values of the 2 second sensors.
  • Step 202 determine the first translational velocity of the carrier according to the estimated value of the instantaneous velocity vector of the N second sensors, the external parameters of the N second sensors and the estimated value of the first rotational angular velocity vector of the carrier where the N second sensors are located Vector estimates.
  • the external parameters of the N second sensors may include position translation vectors of the N second sensors relative to the carrier coordinate system, or include position translation vectors of the coordinate system origins of the N second sensors relative to the carrier coordinate system origin.
  • the position translation vector of the second sensor relative to the coordinate system of the carrier is used to translate the origin of the coordinate system of the second sensor to be consistent with the origin of the coordinate system of the carrier.
  • the five second sensors may include millimeter-wave radars, lidars or ultrasonic sensors.
  • the five second sensors are located at different positions on the vehicle.
  • the position translation vectors of the second sensors relative to the origin of the vehicle coordinate system are respectively r 21 , r 22 ,...,r 25
  • the estimated values of the instantaneous velocity vectors of the five second sensors are respectively v 21 , v 22 ,...,v 25 .
  • v 21 , v 22 , ..., v 25 are also different from each other.
  • the estimated value of the first translational velocity vector of the carrier is determined according to the estimated values of the instantaneous velocity vectors of the N second sensors, the external parameters of the N second sensors, and the estimated value of the first rotational angular velocity vector of the carrier, which may be,
  • the estimated value of the first translation velocity vector of the carrier is obtained based on the relationship between the translation velocity vector, the instantaneous velocity vector, the rotational angular velocity and the position translation vector of the rigid body, wherein the instantaneous velocity vector and the position translation vector are obtained from the instantaneous velocity vectors of the N second sensors
  • the estimated value and the external parameters of the N second sensors are determined, and the rotational angular velocity is determined from the estimated value of the first rotational angular velocity vector of the carrier.
  • determining the estimated value of the first translational velocity vector may be obtained based on the following relationship:
  • ⁇ is the estimated value of the first translational velocity vector of the carrier
  • ⁇ is the estimated value of the first rotational angular velocity vector of the carrier
  • v 2 is the estimated value of the instantaneous velocity vector of the second sensor
  • r 2 is the estimated value of the second sensor relative to the origin of the carrier coordinate system
  • Position translation vector ⁇ represents the cross product of the vector.
  • r 2 may be an external parameter of the second sensor, and may be a position translation vector of the origin of the coordinate system of the second sensor relative to the origin of the coordinate system of the carrier.
  • the estimated value of the first translational velocity vector may be determined based on the following relation:
  • t 2,j is the translation of the carrier determined according to the estimated value of the instantaneous velocity vector of the jth second sensor and its external parameters and the estimated value of the first rotational angular velocity vector of the carrier Velocity vector estimate.
  • the estimated value of the first translation velocity vector is determined based on a minimum mean square error (minimum mean square error, MMSE) or a least square (least square, LS) method, or the like.
  • MMSE minimum mean square error
  • LS least square
  • the first estimated value of the translational velocity vector is a weighted sum of N estimated values of translational velocity vectors of the carrier, wherein the estimated values of the N translational velocity vectors of the carrier are respectively based on the estimated values of the N second sensors.
  • the estimated value of the instantaneous velocity vector and its external parameters and the estimated value of the first rotational angular velocity vector of the carrier are determined.
  • the estimated value of the first translational velocity vector of the carrier satisfies the following relationship:
  • t 2,j is the translation of the carrier determined according to the estimated value of the instantaneous velocity vector of the jth second sensor and its external parameters and the estimated value of the first rotational angular velocity vector of the carrier Velocity vector estimated value
  • w 2,j is the weighting coefficient or weighting coefficient matrix corresponding to the jth second sensor.
  • the weighting coefficient w 2,j may be determined according to the probability density function or statistical characteristic or covariance matrix of the estimation error or measurement error of t 2,j .
  • w 2,j can be determined from the covariance matrix of the estimation error or measurement error of t 2,j , in, P 2,j is the covariance of the estimation error or measurement error of t 2,j .
  • the estimated value of the translational velocity vector of the carrier is determined according to the estimated value of the instantaneous velocity vector of the jth second sensor and its external parameters and the estimated value of the first rotational angular velocity vector of the carrier, which may be determined based on the following relation:
  • the estimated value of the first translational velocity vector can be determined according to the following relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector of the carrier
  • v 2,j is the estimated value of the instantaneous velocity vector of the jth second sensor
  • r 2,j is the jth
  • w 2,j is a weighting coefficient or a weighting coefficient matrix corresponding to the jth second sensor.
  • the weighting coefficient w 2,j may be determined according to the measurement error or estimation error of v 2,j and/or r 2,j .
  • w 2,j is determined according to the measurement error or estimation error of v 2,j and r 2,j , for example, w 2,j satisfies the following relation:
  • the estimated value of the first translation velocity vector is the mean value of the estimated values of N translation velocity vectors of the carrier, wherein the estimated values of the N translation velocity vectors of the carrier are respectively based on the instantaneous values of the N second sensors.
  • the estimated value of the velocity vector and its external parameters and the estimated value of the first rotational angular velocity vector of the carrier are determined.
  • the estimated value of the first translational velocity vector of the carrier satisfies the following relationship:
  • t 2,j is the translation of the carrier determined according to the estimated value of the instantaneous velocity vector of the jth second sensor and its external parameters and the estimated value of the first rotational angular velocity vector of the carrier Velocity vector estimate.
  • the estimated value of the first translational velocity vector satisfies the following relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector of the carrier
  • v 2,j is the estimated value of the instantaneous velocity vector of the jth second sensor
  • r 2,j is the jth The position translation vector of the coordinate system of the second sensor relative to the coordinate system of the carrier.
  • the estimated value of the first translational velocity vector is the weighted sum of the estimated values of N translational velocity vectors of the carrier
  • the estimated value of the first translational velocity vector is the weighted sum of N estimated translational velocity vector values of the carrier.
  • the estimated value of the velocity vector is the mean value of the estimated values of N translation velocity vectors of the carrier.
  • the second implementation is a special form of the first implementation. If each second sensor in the first implementation If the weighting coefficients are the same, the second implementation can be used. Further, this description is also applicable to the relationship between the weighted sum of the estimated values of the multiple sensors and the mean value of the estimated values of the multiple sensors in other implementations.
  • N is equal to 1. According to the estimated value of the first rotational angular velocity vector, the estimated value of the instantaneous velocity vector of the second sensor, and the position translation vector of the coordinate system of the second sensor relative to the coordinate system of the carrier, determine the first plane
  • the estimated value of the motion velocity vector can be obtained by referring to the above relational formula.
  • the distance regression (orthogonal distance regression, ODR) method is used to determine the estimated value t 2,j of the translational velocity vector of the carrier.
  • the estimated value of the first rotational angular velocity vector is determined according to the estimated value of the rotational angular velocity vector of M first sensors, where M ⁇ 1.
  • the estimated value of the first rotational angular velocity vector can be determined based on the following relational expression:
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • ⁇ 1,i is the estimated value of the rotational angular velocity vector of the i-th first sensor.
  • the estimated value of the first rotational angular velocity vector is determined based on the minimum mean square error or the least squares method.
  • the estimated value of the first rotational angular velocity vector is a weighted sum of the estimated values of M rotational angular velocity vectors of the carrier, wherein the estimated values of the M rotational angular velocity vectors of the carrier are respectively based on the rotational angular velocity vectors of the M first sensors Estimated value is determined.
  • the estimated value of the first rotational angular velocity vector of the carrier satisfies the following relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector of the carrier
  • ⁇ 1,i is the estimated value of the rotational angular velocity vector of the carrier obtained from the estimated value of the rotational angular velocity vector of the ith first sensor
  • w 1,i is the estimated value of the rotational angular velocity vector of the carrier with the ith Weighting coefficient or weighting coefficient matrix corresponding to the first sensor.
  • the weighting coefficient w 1,i may be determined according to the probability density function or statistical characteristic or covariance matrix of the estimation error or measurement error of ⁇ 1,i .
  • w 1,i can be determined according to the covariance matrix of the estimation error or measurement error of ⁇ 1,i , specifically as in, P 1,i is the covariance of the estimation error or measurement error of ⁇ 1,i .
  • the estimated value of the first rotational angular velocity vector is the average value of the estimated values of M rotational angular velocity vectors of the carrier, wherein the estimated values of the M rotational angular velocity vectors of the carrier are estimated respectively according to the rotational angular velocity vectors of the M first sensors value is determined.
  • the estimated value of the first rotational angular velocity vector of the carrier satisfies the following relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector of the carrier
  • ⁇ 1,i is the estimated value of the rotational angular velocity vector of the carrier obtained from the estimated value of the rotational angular velocity vector of the i-th first sensor.
  • M is equal to 1
  • the estimated value of the first rotational angular velocity vector is determined according to the estimated value of the rotational angular velocity vector of the first sensor, which can be obtained by referring to the above relational formula.
  • the estimated value of the rotational angular velocity vector of the first sensor, the estimated value of the instantaneous velocity vector of the second sensor, the estimated value of the first rotational angular velocity vector of the carrier, the estimated value of the first translational velocity vector of the carrier, etc. are all. Defined relative to the carrier coordinate system. In practical applications, the measurement data of the sensor is often defined relative to the sensor coordinate system. Therefore, the motion velocity vector obtained from the first sensor or the second sensor, including the rotational angular velocity vector and the translational velocity vector or the instantaneous velocity vector, is often relative to the sensor. It is easier to define. At this time, it is necessary to obtain the rotational angular velocity vector and translational velocity vector or the instantaneous velocity vector relative to the carrier coordinate system based on the external parameters of the sensor relative to the carrier coordinate system.
  • the transformation relationship between the carrier coordinate system and the sensor coordinate system can usually be determined by the external parameters of the sensor.
  • the external parameters can include rotation parameters and the position of the sensor coordinate system relative to the carrier coordinate system. Translation vector. Based on the rotation parameters, the orientation of the sensor coordinate system can be rotated to match the orientation of the carrier coordinate system. Based on the position translation vector, the origin of the sensor coordinate system can be translated to coincide with the origin of the carrier coordinate system.
  • the position translation vector of the sensor coordinate system relative to the carrier coordinate system may be the vector r as shown in FIG. 6 , or may be r 21 , r 22 , . . . , r 25 as shown in FIG. 5 ; 1,...,N,N ⁇ 1.
  • the rotation parameter is used to represent the rotation between the carrier coordinate system and the sensor coordinate system; specifically, it can be represented by a quaternion, a rotation matrix, an Euler angle, or the like. Among them, quaternions, rotation matrices, Euler angles, etc. can be transformed into each other. Exemplarily, the rotation matrix can be obtained according to quaternions, or the rotation matrix can be obtained according to Euler angles. Exemplarily, the directions of the sensor coordinate system and the carrier coordinate system may be consistent, and in this case, the rotation parameter is the unit matrix.
  • the motion velocity vector relative to the carrier coordinate system is obtained, wherein the motion velocity vector may include rotational angular velocity, translational velocity vector, instantaneous motion One or more of the velocity vectors.
  • the instantaneous velocity vector relative to the carrier coordinate system can be obtained based on the external parameters of the second sensor and the instantaneous velocity vector relative to the second sensor coordinate system, where the external parameters include rotation parameters.
  • the rotation parameter can be a rotation matrix
  • the instantaneous translational velocity vector relative to the carrier coordinate system can be determined by the following relation:
  • v 2,j is the estimated value of the instantaneous velocity vector of the jth second sensor relative to the carrier coordinate system
  • v′ 2,j is the estimated value of the instantaneous velocity vector of the jth second sensor relative to the sensor coordinate system
  • R 2 , j is the rotation parameter from the sensor coordinate system of the second sensor to the carrier coordinate system.
  • the rotation angular velocity vector relative to the carrier coordinate system can be obtained based on the external parameters of the first sensor and the rotation angular velocity vector relative to the first sensor coordinate system, wherein the external parameters include rotation parameters.
  • the rotation parameter can be a rotation matrix
  • the rotational angular velocity vector relative to the carrier coordinate system can be determined by the following relation:
  • ⁇ 1,i is the estimated value of the rotational angular velocity vector of the ith first sensor relative to the coordinate system of the carrier
  • ⁇ ′ 1,i is the rotational angular velocity vector of the ith first sensor relative to the coordinate system of the first sensor
  • R 1,i is the rotation matrix from the sensor coordinate system of the first sensor to the carrier coordinate system.
  • the rotation parameter can be a fixed value, or can be estimated according to an online algorithm in motion estimation
  • the position translation vector can be a fixed value, or can be estimated according to an online algorithm in motion estimation. This embodiment of the present application does not limit this.
  • the estimated value of the first rotational angular velocity vector of the carrier can be determined according to the external parameters of the sensors and the estimated rotational angular velocity vector values of the M first sensors.
  • the external parameters of the sensor may include rotational parameters of the sensor.
  • the estimated value of the first rotational angular velocity vector of the carrier is determined.
  • the estimated value of the first rotational angular velocity vector may be determined based on the following relational expression.
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • ⁇ ′ 1,i is the estimated value of the rotational angular velocity vector of the ith first sensor relative to the sensor coordinate system
  • R 1,i is the coordinate system of the ith first sensor Rotation parameter relative to the carrier coordinate system.
  • the estimated value of the first rotational angular velocity vector of the carrier is determined according to the estimated value of the rotational angular velocity vector of each of the M first sensors relative to the sensor coordinate system and the rotational parameters of each of the first sensors.
  • the first rotational angular velocity vector estimate is determined based on the relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector of the carrier
  • ⁇ ′ 1,i is the estimated value of the rotational angular velocity vector of the i-th first sensor relative to the sensor coordinate system
  • w 1,i is the same as the i-th first sensor.
  • the corresponding weighting coefficient or weighting coefficient matrix, R 1,i is the rotation parameter of the coordinate system of the ith first sensor relative to the coordinate system of the carrier.
  • the first rotational angular velocity vector estimate is determined based on the relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector of the carrier
  • ⁇ ' 1,i is the estimated value of the rotational angular velocity vector of the ith first sensor relative to the sensor coordinate system
  • R 1,i is the estimated value of the ith first sensor's rotational angular velocity vector The rotation parameter of the coordinate system relative to the carrier coordinate system.
  • the estimated value of the first rotational angular velocity vector of the carrier can be accurately determined.
  • the estimated value of the instantaneous velocity vector of the second sensor can be accurately obtained. Therefore, by using the method of the embodiment of the present application, the motion of the carrier can be compensated, which is helpful to realize the separation of the moving target and the stationary target, and at the same time helps to realize the positioning and tracking of the motion of the carrier.
  • the embodiment of the present application may further include the following steps 203 and 204 .
  • Step 203 Obtain the normalized translational velocity vector estimation values of M' first sensors among the M first sensors.
  • the M first sensors include M' first sensors, where M' ⁇ M, and the M' first sensors may be visual sensors such as cameras or cameras.
  • the M first sensors include M' visual sensors such as cameras or cameras and M-M' inertial measurement sensors such as IMUs.
  • the M' first sensors may be visual sensors such as cameras or cameras, and the normalized translational velocity vector estimates of the M' first sensors may be obtained, which may be images obtained from cameras or cameras or The video is determined based on the optical characteristics or geometric characteristics of the data according to the feature points, lines, or planes or regions in it, for example, it is obtained based on the 8-point method or the 5-point method, or the Homography or optical flow method.
  • the application examples are not limited.
  • the estimated value of the normalized translational velocity vector of the first sensor is understood as the estimated value of the translational velocity vector stretched according to the scale determined by the first sensor, that is, the estimated value of the normalized translational velocity vector of the first sensor The value is proportional to the translation velocity vector estimate of the first sensor. It should be understood that the estimated value of the translational velocity vector of the first sensor is the estimated value of the translational velocity vector of the actual motion of the first sensor.
  • the estimated value of the normalized translational velocity vector of the first sensor can be expressed as The estimated value of the translational velocity vector of the first sensor can be expressed as v 1 ′, and there is a relationship between the two:
  • v 1 ′ [v′ 1x , v′ 1y , v′ 1z ] T
  • s is a normalization parameter or a scaling factor of the estimated value of the translational velocity vector of the first sensor.
  • the normalization parameter or scaling factor may be the magnitude, norm, or modulus of the estimated translational velocity vector, or a certain component of the estimated translational velocity vector, such as a z-axis component.
  • the estimated value of the normalized translational velocity vector can be determined based on the following relation
  • u, v are the optical flow components on the image plane
  • s 1 [-f 0 x] T
  • s 2 [0 -f y] T
  • ⁇ represents the cross product of the vector
  • f is the focal length of the camera
  • x, y are the pixel positions of the image plane, x ⁇ [p x -w x ,p x +w x ],y ⁇ [p y -w y ,p y + w y ];
  • (p x , p y ) is the center position
  • w x and w y are non-negative integers
  • Z' is the relative depth of the target point corresponding to the pixel
  • ⁇ are the normalized translational velocity vectors and rotational angular velocity vectors relative to the sensor coordinate system.
  • t' is the absolute translation velocity vector relative to the sensor coordinate system
  • Z is the absolute depth of the target point corresponding to the pixel
  • t' z is the z-axis component of t'
  • obtaining the normalized translational velocity vector estimated values of the M' first sensors may further include: obtaining a normalized translational velocity vector relative to the carrier coordinate system according to external parameters of the first sensors An estimated value, wherein the external parameters of the first sensor include rotation parameters of the first sensor coordinate system relative to the carrier coordinate system.
  • a normalized translational velocity vector estimate relative to the carrier coordinate system for Wherein, R 1 is the rotation matrix of the coordinate system of the first sensor relative to the coordinate system of the carrier.
  • the above rotation transformation is an orthogonal transformation matrix, and the normalization parameter is not changed.
  • the normalization parameter may be used as an example for description.
  • Step 204 Determine the second estimated translational velocity vector of the carrier according to the estimated value of the first translational velocity vector and the normalized estimated translational velocity vector of the M' first sensors.
  • the estimated accuracy of the translational velocity vector of the carrier can be further improved based on the normalized estimated value of the translational velocity vector of the M' first sensors , and the estimated value of the translational velocity vector of the carrier obtained by further updating is referred to as the estimated value of the second translational velocity vector here.
  • the second translational velocity of the carrier can be determined according to the following normalized relationship between the translational velocity vector of the first sensor, the position translational vector of the first sensor, the translational velocity vector of the carrier, and the rotational angular velocity vector of the carrier Vector estimates:
  • t is the translational velocity vector of the carrier
  • ⁇ is the estimated value of the rotational angular velocity vector of the carrier
  • r 1 is the position translation vector of the first sensor relative to the carrier coordinate system
  • s is the normalized parameter of the translation velocity vector of the first sensor.
  • the first translational velocity vector of the carrier the first rotational angular velocity vector of the carrier, the normalized translational velocity vector of the first sensor, and the position translation vector of the first sensor relative to the carrier coordinate system may be used.
  • the relationship determines the vector estimate of the second translational velocity vector of the vector:
  • the estimated value of the second translational velocity vector of the carrier is determined according to the following relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector of the carrier
  • r 1,i is the position translation vector of the i-th first sensor relative to the carrier coordinate system
  • s i is the i-th first sensor’s position translation vector Normalization parameter for the translational velocity vector.
  • the estimated value of the second translational velocity vector of the carrier is determined according to the following relationship:
  • w′ 1, i is the weighting coefficient matrix corresponding to the i-th first sensor
  • ⁇ is the estimated value of the first rotational angular velocity vector of the carrier
  • r 1,i is the position translation vector of the i-th first sensor relative to the carrier coordinate system
  • s i is the i-th first sensor’s position translation vector Normalization parameter for the translational velocity vector.
  • the weighting coefficient matrix w′ 1,i can be based on The covariance matrix of the estimation error of is determined, which is similar to the previous method and will not be described in detail here.
  • the estimated value of the second translational velocity vector of the carrier is also determined according to the following relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector of the carrier, is the estimated value of the normalized translation velocity vector of the i-th first sensor
  • r 1,i is the position translation vector of the i-th first sensor relative to the carrier coordinate system
  • s i is the i-th first sensor’s position translation vector Normalization parameter for the translational velocity vector.
  • si can be specifically determined according to the following relational formula:
  • the translation velocity vector of the carrier As another implementation, it can be determined according to the relationship between the translation velocity vector of the carrier, the first rotational angular velocity vector of the carrier, the translation velocity vector of the first sensor, and the position translation vector of the first sensor relative to the coordinate system of the carrier.
  • the estimated value of the second translational velocity vector of the carrier which conforms to the following relation:
  • the estimated value of the second translational velocity vector of the carrier is given by the above Sure.
  • ⁇ is the estimated value of the first rotational angular velocity vector of the carrier, is the estimated value of the normalized translation velocity vector of the i-th first sensor, r 1,i is the position translation vector of the i-th first sensor relative to the carrier coordinate system, and s i is the i-th first sensor’s position translation vector
  • the normalization parameter of the translational velocity vector, si can be determined according to the following relation:
  • the estimated value of the second translational velocity vector may be determined in an iterative manner. Specifically, the estimated value of the translational velocity vector of the carrier can be obtained once in each iteration, and the estimated value of the translational velocity vector of the carrier obtained in the last iteration is used as the estimated value of the second translational velocity vector of the carrier.
  • the iterative implementation can further utilize the estimated value of the translational velocity vector of the sensor and the positional translational vector, thereby improving the estimation accuracy of the translational velocity vector of the carrier.
  • the estimated value of the second translational velocity vector of the carrier can be obtained according to the following relation:
  • t k is the estimated value of the translational velocity vector of the carrier obtained in the kth iteration. It can also be understood that the kth iteration (the last iteration) obtains t K as the estimated value of the second translational velocity vector of the carrier ⁇ is the estimated value of the first rotational angular velocity vector of the carrier, is the estimated value of the normalized translation velocity vector of the first sensor, r 1 is the position translation vector of the first sensor relative to the carrier coordinate system, sk is the normalized translation velocity vector of the first sensor in the k-th iteration Normalize parameters. Specifically, sk can be obtained according to the following relationship:
  • t k-1 is the estimated value of the translation velocity vector of the carrier obtained in the k-1th iteration
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1 is the position translation vector of the first sensor relative to the carrier coordinate system .
  • each iteration may be, according to the estimated value of the normalized translational velocity vectors of the M' first sensors, the translational velocity vectors of the M' first sensors in the k-1th iteration
  • the normalized parameters, the position translation vectors of the M' first sensors relative to the carrier coordinate system, and the estimated value of the first rotational angular velocity vector are used to determine the estimated value of the translation velocity vector of the carrier at the k-th iteration.
  • the normalization parameters of the translational velocity vectors of the M' first sensors in the kth iteration can be determined according to the estimated value of the translational velocity vectors of the carrier obtained in the k-1th iteration.
  • the second translational velocity vector of the carrier can be determined according to the translational velocity vector of the first sensor, the relationship between the positional translational vector of the first sensor and the translational velocity vector of the carrier and the rotational angular velocity vector of the carrier Estimated values, including:
  • the estimated value of the second translational velocity vector of the carrier is determined according to the following relation:
  • t k is the estimated value of the translational velocity vector of the carrier in the kth iteration
  • t k is the estimated value of the second translational velocity vector
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1,i is the position translation vector of the ith first sensor relative to the carrier coordinate system
  • s i,k is the kth iteration
  • the normalization parameter of the translational velocity vector of the ith first sensor, s i,k is determined by the estimated value of the first translational velocity vector or the estimated value of the translational velocity vector of the carrier in the k-1th iteration
  • w′ 1,i,k are the weighting coefficients of the i-th first sensor in the k-th iteration.
  • w′ 1, i, k can take a fixed value in each iteration or be determined according to a preset algorithm.
  • w′ 1,i,k can be determined according to the covariance matrix of the estimation error of s i ⁇ v 1,i - ⁇ r 1,i , which is similar to the previous method and will not be described in detail here.
  • t k is the estimated value of the translational velocity vector of the carrier in the kth iteration
  • t k is the estimated value of the second translational velocity vector
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1,i is the position translation vector of the ith first sensor relative to the carrier coordinate system
  • s i,k is the kth iteration
  • the normalization parameter of the translational velocity vector of the ith first sensor, s i,k is determined by the estimated value of the first translational velocity vector or the estimated value of the translational velocity vector of the carrier in the k-1 th iteration.
  • s i,k is determined according to the estimated value of the first translational velocity vector or the estimated value of the translational velocity vector of the carrier in the k-1th iteration, which can be in the following two cases:
  • s i,1 can be determined from the estimated value of the first translational velocity vector according to the following relation:
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1,i is the position translation vector of the coordinate system of the ith first sensor relative to the coordinate system of the carrier.
  • s i,k can be determined by the estimated value of the translational velocity vector of the carrier in the k-1th iteration according to the following relation:
  • t k-1 is the estimated value of the translational velocity vector of the carrier in the k-1 iteration
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1,i is the coordinate system of the ith first sensor relative to the carrier The position translation vector of the coordinate system.
  • M' is equal to 1, which can be determined according to the normalized translational velocity vector estimated value of the first sensor, the translational velocity vector of the first sensor in the k-1th iteration.
  • the normalized parameter, the position translation vector of the coordinate system of the first sensor relative to the coordinate system of the carrier, and the estimated value of the first rotational angular velocity vector determine the estimated value of the translational velocity vector of the carrier in the k-th iteration.
  • the normalization parameter of the translational velocity vector of the first sensor in the kth iteration is determined according to the estimated value of the first translational velocity vector or the estimated value of the translational velocity vector of the carrier in the k-1th iteration.
  • the iteration termination condition of the first iterative manner may be set such that the vector distance between the estimated value of the second translational velocity vector and the estimated value of the first translational velocity vector is not greater than the first preset threshold or threshold. It is equivalent to determining the vector distance between the estimated value of the translational velocity vector of the carrier obtained by the k-th iteration and the estimated value of the first translational velocity vector, and if the vector distance is greater than the first preset threshold or threshold, further execute In the k+1th iteration, if the vector distance is not greater than the first preset threshold or threshold, the iteration is determined to be terminated. At this time, the vector obtained by the kth iteration (that is, the Kth iteration, or the last iteration)
  • the estimated translational velocity vector of can be referred to as the second estimated translational velocity vector.
  • the estimated value of the second translational velocity vector and the estimated value of the first translational velocity vector satisfy the following relationship:
  • Threshold1 is a first preset threshold or threshold.
  • the iteration termination condition of the first iteration mode can also be set to reach the maximum number of iterations. It is equivalent to setting the maximum number of iterations to K, that is, K iterations are performed in total, and the estimated value of the translational velocity vector of the carrier obtained by the Kth iteration (ie, the last iteration) is called the second translational velocity vector estimated value.
  • the maximum number of iterations K may be set to be 20.
  • the estimated value of the translational velocity vector of the carrier is determined according to the estimated values of the normalized translational velocity vector of the plurality of first sensors. Using the parameters of the plurality of first sensors as the input of one iteration helps to improve the estimation accuracy of the second translational velocity vector.
  • the normalized parameter of the translational velocity vector of the ith first sensor is used to determine the estimated value of the translation velocity vector of the carrier corresponding to the ith first sensor.
  • the normalization parameter of the translational velocity vector of the i-th first sensor is determined according to the estimated value of the translational velocity vector of the carrier corresponding to the i-1-th first sensor.
  • the second iterative method is to determine the normalized parameter of the translational velocity vector of the next first sensor according to the estimated value of the translational velocity vector of the carrier corresponding to the previous first sensor, and then determine the next first sensor.
  • the estimated value of the translational velocity vector of the carrier corresponding to the sensor which is mainly applicable to the case where M' is greater than 1.
  • si can be obtained according to the following relational formula.
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1,i is the position translation vector of the coordinate system of the ith first sensor relative to the coordinate system of the carrier.
  • the embodiments of the present application may perform multiple rounds of iterations, and each round of iterations includes iterations between M' first sensors. It can be the estimated value of the translational velocity vector of the carrier obtained according to the following relation:
  • t i,L ′ is the second translational velocity vector estimate
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1 is the position translation vector of the coordinate system of the ith first sensor relative to the carrier coordinate system
  • s i,l is the lth round
  • the normalization parameter of the translational velocity vector of the ith first sensor in the iteration, s i,l is estimated by the first translational velocity vector or or Sure.
  • si, l can be obtained according to the following relational formula.
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1,i is the position translation vector of the coordinate system of the i-th first sensor relative to the coordinate system of the carrier.
  • the last iteration in the previous round of iterations is the previous iteration of the first iteration in the subsequent round of iterations, or in other words, the M'th iteration in the l-1 round of iterations is the first iteration in the lth round of iterations.
  • the previous iteration of 1 iteration may be the M'th iteration in the l-1th round of iterations, and the k+1th iteration may be the first iteration in the lth round of iterations.
  • the parameters of the next round of iterations can be determined by the parameters of the previous round of iterations.
  • the normalized parameters of the translational velocity vector of the first first sensor in the lth round of iterations are determined by the l-1th round of iterations.
  • the estimated value of the translational velocity vector of the carrier corresponding to the M'th first sensor is determined.
  • s i,l is the estimated value of the first translational velocity vector, or the estimated value of the translational velocity vector of the carrier corresponding to the i-1th first sensor in the l-th iteration, or the M-th in the l-1th iteration
  • the estimated value of the translation velocity vector of the carrier corresponding to the first sensor is determined, and there are three situations as follows:
  • i is equal to 1
  • l is equal to 1
  • s i, l are determined according to the estimated value of the first translational velocity vector, and can refer to the following relational expression.
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1,1 is the position translation vector of the coordinate system of the first first sensor relative to the coordinate system of the carrier.
  • s i, l is determined according to the estimated value of the translational velocity vector of the carrier corresponding to the i-1 th first sensor in the 1 th iteration, and the following relational expression can be referred to.
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1,i is the relative coordinate system of the i-th first sensor Position translation vector in the carrier coordinate system.
  • i is equal to 1
  • l is greater than 1
  • s 1 are determined according to the estimated value of the translational velocity vector of the carrier corresponding to the M'th first sensor in the l-1 round of iteration, and can refer to the following relational formula.
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1,i is the coordinate system of the ith first sensor Position translation vector relative to the carrier coordinate system.
  • the iterative termination condition of the second iterative manner may be set such that the vector distance between the estimated value of the second translational velocity vector and the estimated value of the first translational velocity vector is not greater than the second preset threshold or threshold.
  • the estimated value of the second translational velocity vector and the estimated value of the first translational velocity vector satisfy the following relationship:
  • Threshold2 is the second preset threshold or threshold.
  • the iteration termination condition of the second iteration mode can also be set to reach the maximum number of iteration rounds. It is equivalent to setting the maximum number of iteration rounds to L, that is, a total of L rounds of iterations are performed, and the estimated value of the translational velocity vector of the carrier corresponding to a certain first sensor in the Lth round of iteration (ie, the last round of iteration) is called is the estimated value of the second translational velocity vector.
  • the value of the maximum number of iteration rounds L is set to 5
  • the estimated value of the translational velocity vector of the carrier corresponding to the M'th first sensor in the fifth round is set to the second estimated value of the translational velocity vector.
  • determining the translational velocity vector estimated value of the carrier corresponding to each first sensor is equivalent to, for each A sensor performs one iteration, and a parameter of a first sensor is used as the input of one iteration, and a relatively accurate second translational velocity vector estimation value can be quickly obtained.
  • the estimated value of the normalized translational velocity vector obtained by the first sensor evaluating its own motion is more accurate, and the estimated value of the normalized translational velocity vector of the first sensor and the first estimated translational velocity vector are more accurate.
  • the estimated values are fused to obtain a second estimated value of the translational velocity vector of the carrier. Compared with the estimated value of the first translational velocity vector, the accuracy of the estimated value of the translational velocity vector of the carrier can be further improved.
  • the motion of the carrier is compensated by using the estimated value of the first rotational angular velocity vector of the carrier and the estimated value of the second translational velocity vector of the carrier, which is helpful to realize the separation of the moving target and the stationary target, and at the same time helps to realize the positioning of the motion of the carrier and tracking.
  • the first sensor in the M' first sensors can be understood as the first sensor that can obtain the normalized translational velocity vector estimation value
  • the M' first sensors can be visual sensors, such as cameras , cameras, infrared sensors, etc.
  • embodiments of the present application may further combine M-M' first sensors that can obtain estimated values of translational velocity vectors, wherein the estimated values of translational velocity vectors obtained by the MM' first sensors include scale information of the velocity vectors, For example, the complete information of each velocity component, rather than just the normalized value or direction information, improves the accuracy of the estimated value of the second translational velocity vector of the carrier.
  • the present application provides yet another method for determining the estimated value of the second translational velocity vector as follows.
  • step 203 the normalized translational velocity vector estimates of M' first sensors such as vision sensors and the translational velocity vector estimates of M-M' first sensors such as inertial sensors (eg, IMUs) may be obtained.
  • M' first sensors such as vision sensors
  • M-M' first sensors such as inertial sensors (eg, IMUs)
  • step 204 firstly determine the estimated translational velocity vector of the carrier corresponding to the M-M' first sensors, such as inertial sensors, according to the estimated translational velocity vector of the M-M' first sensors, such as inertial sensors.
  • Exemplary can be based on relational or It is determined that the specific description of the relational expression may refer to the foregoing implementation manner.
  • it can be based on the relational or Determine the translational velocity vector estimated value of the carrier corresponding to the vision sensor, and weight the translational velocity vector estimated value of the carrier corresponding to the visual sensor and the translational velocity vector estimated value of the carrier corresponding to the inertial sensor to obtain a second translational velocity vector estimated value.
  • the estimated value of the translational velocity vector of the carrier corresponding to the inertial sensor can be used as the estimated value of the translational velocity vector of the carrier in the first iteration, that is, k is equal to the corresponding value in 1 middle, Replaced with the translation velocity vector estimate of the carrier corresponding to the inertial sensor. Iteration is performed based on the iterative manner in step 204 until the iteration conditions are met, and the estimated value of the translational velocity vector of the carrier in the k-th iteration that meets the iteration condition is used as the estimated value of the second translational velocity vector.
  • the estimated value of the translational velocity vector of the carrier corresponding to the inertial sensor may be determined according to the estimated value of the translational velocity vector of the M inertial sensors, exemplarily, may be based on the relational expression or
  • the specific description of the relational expression can refer to the above implementation manner.
  • the estimated value of the translation velocity vector of the carrier corresponding to the inertial sensor and the estimated value of the first translation velocity vector are weighted to obtain the estimated value of the second translation velocity vector.
  • the estimated value of the rotational angular velocity vector of the carrier may be further updated according to the second estimated value of the translational velocity vector.
  • the updated rotational angular velocity vector estimated value is referred to herein as the second rotational angular velocity vector estimated value of the carrier.
  • step 205 the estimated value of the second rotational angular velocity vector of the carrier is determined according to the estimated value of the second translational velocity vector of the carrier.
  • the estimated value of the second rotational angular velocity vector can be determined based on the following relation:
  • ⁇ ' is the estimated value of the second rotational angular velocity vector
  • v 2 is the estimated value of the instantaneous velocity vector of the second sensor
  • r 2 is the position translation vector of the coordinate system of the second sensor relative to the coordinate system of the carrier
  • ⁇ represents the cross product of the vectors.
  • the estimated value of the second rotational angular velocity vector may be determined based on the following relationship:
  • ⁇ ' is the estimated value of the second rotational angular velocity vector
  • v 2,j is the estimated value of the instantaneous velocity vector of the jth second sensor
  • [r 2,j ] ⁇ is the antisymmetric matrix corresponding to r 2,j
  • r 2,j is the coordinate system of the jth second sensor relative to the carrier coordinate system position translation vector.
  • a second estimate of the rotational angular velocity vector is determined, where N ⁇ 1.
  • the second rotational angular velocity vector estimate is determined based on the relationship:
  • ⁇ ' is the estimated value of the second rotational angular velocity vector
  • ⁇ 2,j is the weighting coefficient of the j-th second sensor
  • v 2,j is the estimated value of the instantaneous velocity vector of the j-th second sensor
  • [r 2,j ] ⁇ is the antisymmetric matrix corresponding to r 2,j
  • r 2,j is the coordinate system of the jth second sensor relative to the carrier coordinate system position translation vector.
  • the first rotational angular velocity vector estimate is determined based on the relationship:
  • ⁇ ' is the estimated value of the second rotational angular velocity vector
  • v 2,j is the estimated value of the instantaneous velocity vector of the jth second sensor
  • [r 2,j ] ⁇ is the antisymmetric matrix corresponding to r 2,j
  • r 2,j is the coordinate system of the jth second sensor relative to the carrier coordinate system position translation vector.
  • N is equal to 1, according to the estimated value of the second rotational angular velocity vector, the estimated value of the instantaneous velocity vector of the second sensor, and the position translation vector of the coordinate system of the second sensor relative to the coordinate system of the carrier, determine the second The estimated value of the rotational angular velocity vector can be obtained by referring to the above relational formula.
  • the estimated value of the second translational velocity vector of the carrier and the estimated value of the second rotational angular velocity vector of the carrier may be used as the final estimated estimated value of the translational velocity vector of the carrier and the estimated value of the rotational angular velocity vector of the carrier, respectively.
  • the estimated first rotational angular velocity vector and the second estimated rotational angular velocity vector may be further fused, and the fused estimated rotational angular velocity vector may be used as the final estimated rotational angular velocity vector of the carrier.
  • the average value of the estimated value of the first rotational angular velocity vector of the carrier and the estimated value of the second rotational angular velocity vector of the carrier may be determined.
  • weighted combination is performed based on the minimum mean square error, and the estimated value of the first rotational angular velocity vector of the carrier and the estimated value of the second rotational angular velocity vector of the carrier are fused.
  • the estimated value of the second rotational angular velocity vector of the carrier is determined according to the estimated value of the second translational velocity vector of the carrier. Compared with the estimated value of the first rotational angular velocity vector, the estimated value of the rotational angular velocity vector of the carrier can be further improved. accuracy. Compensating the motion of the carrier by using the estimated value of the second rotational angular velocity vector of the carrier and the estimated value of the second translational velocity vector of the carrier is helpful to realize the separation of the moving target and the stationary target, and at the same time helps to realize the positioning of the motion of the carrier and tracking.
  • each functional module in each embodiment of the present application may be integrated into one processor, or may exist physically alone, or two or more modules may be integrated into one module.
  • the above-mentioned integrated modules can be implemented in the form of hardware, and can also be implemented in the form of software function modules.
  • an embodiment of the present application further provides a motion estimation apparatus for implementing the above method.
  • the motion estimation device may be a sensor system or a fusion perception system or a planning/control system integrating the above-mentioned systems, such as an assisted driving or automatic driving system.
  • the motion estimation device may also be software or hardware (eg, a data processing device connected or integrated with the corresponding sensor through wireless or wired).
  • the motion estimation device can be a vehicle with motion estimation function, or other components with motion estimation function.
  • the motion estimation device includes but is not limited to: on-board terminal, on-board controller, on-board module, on-board module, on-board components, on-board chip, on-board unit, on-board radar or on-board camera and other sensors.
  • device vehicle-mounted module, vehicle-mounted module, vehicle-mounted component, vehicle-mounted chip, vehicle-mounted unit, vehicle-mounted radar or camera, and implement the method provided in this application.
  • the motion estimation device may also be other smart terminals with motion estimation function other than the vehicle, or be provided in other smart terminals with motion estimation functions other than the vehicle, or be provided in components of the smart terminal.
  • the intelligent terminal may be other terminal equipment such as intelligent transportation equipment, smart home equipment, and robots.
  • the motion estimation device includes, but is not limited to, a smart terminal or a controller, a chip, a radar or a camera and other sensors in the smart terminal, and other components.
  • the motion estimation apparatus may be a general-purpose device or a dedicated device.
  • the motion estimation apparatus can also be a desktop computer, a portable computer, a network server, a personal digital assistant (PDA), a mobile phone, a tablet computer, a wireless terminal device, an embedded device, or other devices with processing functions. .
  • PDA personal digital assistant
  • This embodiment of the present application does not limit the type of the motion estimation apparatus.
  • the motion estimation apparatus may also be a chip or processor with processing functions, and the motion estimation apparatus may include multiple processors.
  • the processor can be a single-core (single-CPU) processor or a multi-core (multi-CPU) processor.
  • the chip or processor with processing function may be arranged in the sensor, or may not be arranged in the sensor, but arranged at the receiving end of the output signal of the sensor.
  • FIG. 7 exemplarily provides a motion estimation apparatus 700 for this application.
  • the motion estimation apparatus 700 may include: an acquisition unit 701 and a processing unit 702 . It should be understood that the description of the apparatus embodiment corresponds to the description of the method embodiment. Therefore, for the content not described in detail, reference may be made to the above method embodiment, which is not repeated here for brevity.
  • the obtaining unit 701 is configured to obtain the estimated value of the rotational angular velocity vector of the M first sensors and the estimated value of the instantaneous velocity vector of the N second sensors; wherein, M ⁇ 1, N ⁇ 1;
  • the processing unit 702 is configured to determine the first translation of the carrier according to the estimated value of the instantaneous velocity vector of the N second sensors and the estimated value of the first rotational angular velocity vector of the carrier where the N second sensors are located. an estimated value of the velocity vector, wherein the estimated value of the first rotational angular velocity vector is determined according to the estimated value of the rotational angular velocity vector of the M first sensors.
  • the processing unit 702 is specifically configured to determine the estimated value of the first translational velocity vector based on the following relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • v 2,j is the estimated value of the instantaneous velocity vector of the j-th second sensor
  • r 2,j is the estimated value of the j-th second sensor
  • the estimated value of the first translational velocity vector satisfies the following relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • w 2,j is the weighting coefficient of the j-th second sensor
  • v 2,j is the instantaneous velocity vector of the j-th second sensor
  • the estimated value, r 2,j is the position translation vector of the coordinate system of the jth second sensor relative to the coordinate system of the carrier.
  • the estimated value of the first rotational angular velocity vector satisfies the following relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • w 1,i is the weighting coefficient of the ith first sensor
  • ⁇ 1,i is the estimated value of the rotational angular velocity vector of the ith first sensor.
  • the obtaining unit 701 is further configured to obtain the normalized translational velocity vector estimation values of the M' first sensors in the M first sensors, where 1 ⁇ M' ⁇ M; the processing unit 702 is further configured to determine the second estimated value of the translational velocity vector of the carrier according to the estimated value of the first translational velocity vector and the estimated value of the normalized translational velocity vector of the M' first sensors Translation velocity vector estimate.
  • the processing unit 702 is specifically configured to determine the estimated value of the second translational velocity vector based on the following relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1,i is the position translation vector of the coordinate system of the ith first sensor relative to the carrier coordinate system
  • s i is the ith first sensor
  • the normalization parameter or scaling factor of the translational velocity vector of a sensor, si is determined by the estimated value of the first translational velocity vector.
  • the estimated value of the second translational velocity vector satisfies the following relationship:
  • t K is the estimated value of the second translational velocity vector
  • t k is the estimated value of the translational velocity vector of the carrier in the k-th iteration
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1,i is the position translation vector of the coordinate system of the ith first sensor relative to the carrier coordinate system
  • s i,k is the kth
  • s i,k is the estimated value of the first translational velocity vector or the translational velocity of the carrier in the k-1th iteration
  • the vector estimate is determined.
  • the estimated value of the second translational velocity vector satisfies the following relationship:
  • t K is the estimated value of the second translational velocity vector
  • t k is the estimated value of the translational velocity vector of the carrier in the k-th iteration
  • w′ 1,i,k is the weighting coefficient of the i-th first sensor in the k-th iteration
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1,i is the position translation vector of the coordinate system of the ith first sensor relative to the carrier coordinate system
  • s i,k is the kth
  • s i,k is the estimated value of the first translational velocity vector or the translational velocity of the carrier in the k-1th iteration
  • the vector estimate is determined.
  • the normalization parameter or scale scaling factor of the translational velocity vector of the i-th first sensor in the k-th iteration satisfies the following relation:
  • t k-1 is the estimated value of the translational velocity vector of the carrier in the k-1th iteration
  • t0 is the estimated value of the first translational velocity vector
  • the estimated value of the second translational velocity vector satisfies the following relationship:
  • ⁇ is the estimated value of the first rotational angular velocity vector
  • r 1 i is the position translation vector of the coordinate system of the ith first sensor relative to the carrier coordinate system
  • s i,l is the lth round
  • the normalization parameter or scaling factor of the translational velocity vector of the ith first sensor in the iteration, s i,l is estimated by the first translational velocity vector or or Sure.
  • the normalization parameter or scale scaling factor of the translational velocity vector of the i-th first sensor in the l-th round of iteration satisfies the following relationship:
  • the processing unit 702 is further configured to determine the estimated value of the second rotational angular velocity vector of the carrier based on the following relationship:
  • ⁇ ' is the estimated value of the second rotational angular velocity vector
  • v 2,j is the estimated value of the instantaneous velocity vector of the jth second sensor
  • [r 2,j ] ⁇ is the antisymmetric matrix corresponding to r 2,j
  • r 2,j is the coordinate system of the jth second sensor relative to the carrier coordinate system position translation vector.
  • FIG. 8 a schematic structural diagram of a chip provided by an embodiment of the present application is shown.
  • the chip 800 includes one or more processors 801 and an interface circuit 802 .
  • the chip 800 may further include a bus 803 .
  • the processor 801 may be an integrated circuit chip with signal processing capability. In the implementation process, each step of the above-mentioned method may be completed by an integrated logic circuit of hardware in the processor 801 or an instruction in the form of software.
  • the above-mentioned processor 801 may be a general purpose processor, a digital communicator (DSP), an application specific integrated circuit (ASIC), a field programmable gate array (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components .
  • DSP digital communicator
  • ASIC application specific integrated circuit
  • FPGA field programmable gate array
  • a general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.
  • the interface circuit 802 can be used to send or receive data, instructions or information.
  • the processor 801 can use the data, instructions or other information received by the interface circuit 802 to process, and can send the processing completion information through the interface circuit 802.
  • the chip further includes a memory, which may include a read-only memory and a random access memory, and provides operation instructions and data to the processor.
  • a portion of the memory may also include non-volatile random access memory (NVRAM).
  • NVRAM non-volatile random access memory
  • the memory stores executable software modules or data structures
  • the processor may execute corresponding operations by calling operation instructions stored in the memory (the operation instructions may be stored in the operating system).
  • the chip may be used in the communication apparatus (including the master node and the slave node) involved in the embodiments of the present application.
  • the interface circuit 802 may be used to output the execution result of the processor 801 .
  • processor 801 and the interface circuit 802 can be implemented by hardware design, software design, or a combination of software and hardware, which is not limited here.
  • the embodiments of the present application also provide a radar system for providing a motion estimation function for a vehicle. It includes at least one motion estimation device mentioned in the above-mentioned embodiments of the present application, and at least one motion estimation device in the system can be integrated into a whole machine or equipment, or at least one motion estimation device in the system can also be independently set as an element. or device.
  • Embodiments of the present application also provide a sensor system for providing a motion estimation function for a vehicle. It includes at least one motion estimation device mentioned in the above-mentioned embodiments of the present application, and at least one sensor such as a camera or a radar. At least one sensor device in the system can be integrated into a whole machine or equipment, or The at least one sensor device can also be provided independently as an element or device.
  • the embodiments of the present application further provide a system, which is applied to unmanned driving or intelligent driving, and includes at least one sensor such as the motion estimation device, camera, and radar mentioned in the above-mentioned embodiments of the present application.
  • At least one device can be integrated into a whole machine or device, or at least one device in the system can also be independently set as a component or device.
  • any of the above systems may interact with the vehicle's central controller to provide detection and/or fusion information for decision-making or control of the vehicle's driving.
  • An embodiment of the present application further provides a vehicle, where the vehicle includes at least one motion estimation device or any of the above-mentioned systems mentioned in the above-mentioned embodiments of the present application.
  • Embodiments of the present application further provide a communication device, including a processor and a communication interface, where the communication interface is configured to receive signals from other communication devices other than the communication device and transmit to the processor or transfer signals from the communication device to the processor.
  • the signal of the processor is sent to other communication devices other than the communication device, and the processor is used to implement the above method as shown in FIG. 2 through a logic circuit or executing code instructions.
  • Embodiments of the present application further provide a computer-readable storage medium, where computer programs or instructions are stored in the computer-readable storage medium.
  • the computer programs or instructions are executed by a communication device, the above method as shown in FIG. 2 is implemented.
  • Embodiments of the present application further provide a computer program product, where the computer program product includes a computer program or an instruction, and when the computer program or instruction is executed by a communication device, the above method as shown in FIG. 2 is implemented.
  • the embodiments of the present application may be provided as a method, a system, or a computer program product. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, optical storage, etc.) having computer-usable program code embodied therein.
  • These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory result in an article of manufacture comprising instruction means, the instructions
  • the apparatus implements the functions specified in the flow or flows of the flowcharts and/or the block or blocks of the block diagrams.

Abstract

一种运动估计方法及装置，属于传感器技术领域，可用于辅助驾驶和自动驾驶。其中方法包括：获取M个第一传感器的转动角速度矢量估计值和N个第二传感器的瞬时速度矢量估计值，M≥1，N≥1（201），根据N个第二传感器的瞬时速度矢量估计值和N个第二传感器所在的载体的第一转动角速度矢量估计值，确定载体的第一平动速度矢量估计值（202），其中，第一转动角速度矢量估计值是根据M个第一传感器的转动角速度矢量估计值确定的。方法可用于传感器感知的数据处理过程中，用于准确估计传感器所在载体的运动，可以应用于车联网，如车辆外联V2X、车间通信长期演进技术LTE-V、车辆-车辆V2V等。

Description

一种运动估计方法及装置
相关申请的交叉引用
本申请要求在2020年08月18日提交中国专利局、申请号为202010831971.6、申请名称为“一种运动估计方法及装置”的中国专利申请的优先权，其全部内容通过引用结合在本申请中。
技术领域
本申请涉及传感器技术领域，尤其涉及一种运动估计方法及装置。
背景技术
先进辅助驾驶系统(advanced driver assistant system，ADAS)或者自动驾驶(autonomous driving，AD)系统会配置多种传感器，例如毫米波雷达、激光雷达、超声波传感器如声呐、视觉传感器如相机或者摄像头等传感器，用于感知周边环境信息，周边环境信息包括运动目标和静止目标。对于运动目标和静止目标，通常采用不同的方法分析处理，示例性的，对运动目标(比如车辆、行人)进行分类、识别和跟踪，对静止目标(比如障碍物、护栏、路沿)分类和识别。通过上述方式，可以为自动驾驶提供额外信息，如规避障碍物、提供可行驶区域等。
传感器通常可以安装于载体，传感器跟随传感器所在载体运动。一方面，传感器所在载体的运动导致运动目标和静止目标无法独立分析，因此，需要估计传感器所在载体的运动，从而实现运动目标和静止目标的分离。另一方面，运动目标的跟踪通常基于运动模型，如常速度(constant velocity，CV)/常加速度(constant acceleration，CA)/匀速圆周运动(coordinated turn，CT)等模型，且模型通常假定相对地面或者大地坐标系，传感器所在载体的运动将导致上述模型失效或者跟踪性能下降，因此，需要对传感器所在载体的运动进行补偿。
综上，准确估计传感器所在载体的运动，是本领域人员正在解决的技术问题。
发明内容
本申请提供一种运动估计方法及装置，用于准确估计传感器所在载体的运动。
第一方面，本申请提供一种运动估计方法，该方法包括：
获取M个第一传感器的转动角速度矢量估计值和N个第二传感器的瞬时速度矢量估计值；其中，M≥1，N≥1；
根据所述N个第二传感器的瞬时速度矢量估计值和所述N个第二传感器所在的载体的第一转动角速度矢量估计值，确定所述载体的第一平动速度矢量估计值，其中，所述第一转动角速度矢量估计值是根据所述M个第一传感器的转动角速度矢量估计值确定的。
上述技术方案中，由于第一传感器的转动角速度矢量估计值和第二传感器的瞬时速度矢量估计值较为准确，根据至少一个第一传感器的转动角速度矢量估计值，确定载体的第一转动角速度矢量估计值，再结合第二传感器的瞬时速度矢量估计值，确定载体的第一平 动速度矢量估计值，有助于得到较为准确的传感器所在载体的运动。
在一种可能的实现方式中，所述第一平动速度矢量估计值基于如下关系式确定：
Figure PCTCN2021108627-appb-000001
其中，
Figure PCTCN2021108627-appb-000002
为第一平动速度矢量估计值，ω为第一转动角速度矢量估计值，v 2,j为第j个第二传感器的瞬时速度矢量估计值，r 2,j为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
上述关系式基于刚体的平动速度矢量、瞬时速度矢量、转动角速度以及位置平移矢量的关系得到，且上述关系式可以存在多种变形。根据上述关系式可以较准确确定出第一平动速度矢量估计值。
在一种可能的实现方式中，所述第一平动速度矢量估计值满足如下关系式：
Figure PCTCN2021108627-appb-000003
其中，
Figure PCTCN2021108627-appb-000004
为第一平动速度矢量估计值，ω为第一转动角速度矢量估计值，w 2,j为第j个第二传感器的加权系数，v 2,j为第j个第二传感器的瞬时速度矢量估计值，r 2,j为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
在一种可能的实现方式中，所述第一转动角速度矢量估计值满足如下关系式：
Figure PCTCN2021108627-appb-000005
其中，ω为第一转动角速度矢量估计值，w 1,i为第i个第一传感器的加权系数，ω 1,i为第i个第一传感器的转动角速度矢量估计值。
在一种可能的实现方式中，还包括：
获取所述M个第一传感器中M′个第一传感器的归一化的平动速度矢量估计值，其中，1≤M′≤M；
根据所述第一平动速度矢量估计值、所述M′个第一传感器的归一化的平动速度矢量估计值，确定所述载体的第二平动速度矢量估计值。
上述技术方案中，第一传感器评估自身运动得到的归一化的平动速度矢量估计值较准确，该将第一传感器的归一化的平动速度矢量估计值和第一平动速度矢量估计值进行融合，得到载体的第二平动速度矢量估计值，可进一步提高载体的平动速度矢量估计值的精确度。
在一种可能的实现方式中，所述第二平动速度矢量估计值基于如下关系式确定：
Figure PCTCN2021108627-appb-000006
其中，
Figure PCTCN2021108627-appb-000007
为第二平动速度矢量估计值，ω为第一转动角速度矢量估计值，
Figure PCTCN2021108627-appb-000008
为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s i为第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s i由第一平动速度矢量估计值确定。
上述关系式基于刚体的平动速度矢量、瞬时速度矢量、转动角速度以及位置平移矢量的关系得到，且上述关系式可以存在多种变形。根据上述关系式可以较准确确定出第二平 动速度矢量估计值。
在一种可能的实现方式中，所述第二平动速度矢量估计值满足如下关系式：
Figure PCTCN2021108627-appb-000009
其中，t K为第二平动速度矢量估计值
Figure PCTCN2021108627-appb-000010
t k为第k次迭代中载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，
Figure PCTCN2021108627-appb-000011
为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s i,k为第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s i,k由第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定。
在一种可能的实现方式中，所述第二平动速度矢量估计值满足如下关系式：
Figure PCTCN2021108627-appb-000012
其中，t K为第二平动速度矢量估计值
Figure PCTCN2021108627-appb-000013
t k为第k次迭代中载体的平动速度矢量估计值，w′ 1,i,k为第k次迭代中第i个第一传感器的加权系数，ω为第一转动角速度矢量估计值，
Figure PCTCN2021108627-appb-000014
为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s i,k为第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s i,k由第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定。
在一种可能的实现方式中，第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子满足如下关系式：
s i,k＝‖t k-1+ω×r 1,i‖
其中，t k-1为第k-1次迭代中载体的平动速度矢量估计值，t 0为第一平动速度矢量估计值。
上述技术方案中，每次迭代中，根据多个第一传感器的归一化的平动速度矢量估计值，确定载体的平动速度矢量估计值。将多个第一传感器的参数作为一次迭代的输入，有助于提高第二平动速度矢量的估计精度。
在一种可能的实现方式中，所述第二平动速度矢量估计值满足如下关系式：
Figure PCTCN2021108627-appb-000015
其中，
Figure PCTCN2021108627-appb-000016
为第二平动速度矢量估计值
Figure PCTCN2021108627-appb-000017
Figure PCTCN2021108627-appb-000018
为第l轮迭代中第i个第一传感器对应的载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，
Figure PCTCN2021108627-appb-000019
为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s i，l为第l轮迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s i,l由第一平动速度矢量估计值或
Figure PCTCN2021108627-appb-000020
或
Figure PCTCN2021108627-appb-000021
确定。
在一种可能的实现方式中，所述第l轮迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子满足如下关系式：
Figure PCTCN2021108627-appb-000022
其中，
Figure PCTCN2021108627-appb-000023
为第一平动速度矢量估计值；
Figure PCTCN2021108627-appb-000024
上述技术方案中，针对每个第一传感器的归一化的平动速度矢量估计值，确定每个第一传感器对应的载体的平动速度矢量估计值，相当于，针对每个第一传感器进行一次迭代，将一个第一传感器的参数作为一次迭代的输入，可以快速得到较为准确的第二平动速度矢量估计值。
在一种可能的实现方式中，还包括：
基于如下关系式确定所述载体的第二转动角速度矢量估计值：
Figure PCTCN2021108627-appb-000025
其中，ω′为第二转动角速度矢量估计值，v 2,j为第j个第二传感器的瞬时速度矢量估计值，
Figure PCTCN2021108627-appb-000026
为第二平动速度矢量估计值，
Figure PCTCN2021108627-appb-000027
为[r 2,j] ×的逆矩阵，[r 2,j] ×为与r 2,j对应的反对称矩阵，r 2,j为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
上述技术方案中，根据较准确的第二平动速度矢量估计值进一步确定较准确的第二转动角速度矢量估计值。
第二方面，本申请提供一种运动估计装置，该装置包括：
获取单元和处理单元；
所述获取单元用于获取M个第一传感器的转动角速度矢量估计值和N个第二传感器的瞬时速度矢量估计值；其中，M≥1，N≥1；
所述处理单元用于根据所述N个第二传感器的瞬时速度矢量估计值和所述N个第二传感器所在的载体的第一转动角速度矢量估计值，确定所述载体的第一平动速度矢量估计值，其中，所述第一转动角速度矢量估计值是根据所述M个第一传感器的转动角速度矢量估计值确定的。
在一种可能的实现方式中，所述处理单元具体用于基于如下关系式确定所述第一平动速度矢量估计值：
Figure PCTCN2021108627-appb-000028
其中，
Figure PCTCN2021108627-appb-000029
为第一平动速度矢量估计值，ω为第一转动角速度矢量估计值，v 2,j为第j个第二传感器的瞬时速度矢量估计值，r 2,j为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
在一种可能的实现方式中，所述第一平动速度矢量估计值满足如下关系式：
Figure PCTCN2021108627-appb-000030
其中，
Figure PCTCN2021108627-appb-000031
为第一平动速度矢量估计值，ω为第一转动角速度矢量估计值，w 2,j为第j个第二传感器的加权系数，v 2,j为第j个第二传感器的瞬时速度矢量估计值，r 2,j为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
在一种可能的实现方式中，所述第一转动角速度矢量估计值满足如下关系式：
Figure PCTCN2021108627-appb-000032
其中，ω为第一转动角速度矢量估计值，w 1,i为第i个第一传感器的加权系数，ω 1,i为第i个第一传感器的转动角速度矢量估计值。
在一种可能的实现方式中，所述获取单元还用于获取所述M个第一传感器中M′个第一传感器的归一化的平动速度矢量估计值，其中，1≤M′≤M；所述处理单元还用于根据所述第一平动速度矢量估计值、所述M′个第一传感器的归一化的平动速度矢量估计值，确定 所述载体的第二平动速度矢量估计值。
在一种可能的实现方式中，所述处理单元具体用于基于如下关系式确定所述第二平动速度矢量估计值：
Figure PCTCN2021108627-appb-000033
其中，
Figure PCTCN2021108627-appb-000034
为第二平动速度矢量估计值，ω为第一转动角速度矢量估计值，
Figure PCTCN2021108627-appb-000035
为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s i为第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s i由第一平动速度矢量估计值确定。
在一种可能的实现方式中，所述第二平动速度矢量估计值满足如下关系式：
Figure PCTCN2021108627-appb-000036
其中，t K为第二平动速度矢量估计值
Figure PCTCN2021108627-appb-000037
t k为第k次迭代中载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，
Figure PCTCN2021108627-appb-000038
为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s i,k为第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s i,k由第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定。
在一种可能的实现方式中，所述第二平动速度矢量估计值满足如下关系式：
Figure PCTCN2021108627-appb-000039
其中，t K为第二平动速度矢量估计值
Figure PCTCN2021108627-appb-000040
t k为第k次迭代中载体的平动速度矢量估计值，w′ 1,i,k为第k次迭代中第i个第一传感器的加权系数，ω为第一转动角速度矢量估计值，
Figure PCTCN2021108627-appb-000041
为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s i,k为第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s i,k由第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定。
在一种可能的实现方式中，第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子满足如下关系式：
s i,k＝‖t k-1+ω×r 1,i‖
其中，t k-1为第k-1次迭代中载体的平动速度矢量估计值，t 0为第一平动速度矢量估计值。
在一种可能的实现方式中，所述第二平动速度矢量估计值满足如下关系式：
Figure PCTCN2021108627-appb-000042
其中，
Figure PCTCN2021108627-appb-000043
为第二平动速度矢量估计值
Figure PCTCN2021108627-appb-000044
Figure PCTCN2021108627-appb-000045
为第l轮迭代中第i个第一传感器对应的载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，
Figure PCTCN2021108627-appb-000046
为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s i,l为第l轮迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s i,l由第一平动速度矢量估计值或
Figure PCTCN2021108627-appb-000047
或
Figure PCTCN2021108627-appb-000048
确定。
在一种可能的实现方式中，所述第l轮迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子满足如下关系式：
Figure PCTCN2021108627-appb-000049
其中，
Figure PCTCN2021108627-appb-000050
为第一平动速度矢量估计值；
Figure PCTCN2021108627-appb-000051
在一种可能的实现方式中，所述处理单元还用于基于如下关系式确定所述载体的第二转动角速度矢量估计值：
Figure PCTCN2021108627-appb-000052
其中，ω′为第二转动角速度矢量估计值，v 2,j为第j个第二传感器的瞬时速度矢量估计值，
Figure PCTCN2021108627-appb-000053
为第二平动速度矢量估计值，
Figure PCTCN2021108627-appb-000054
为[r 2,j] ×的逆矩阵，[r 2,j] ×为与r 2,j对应的反对称矩阵，r 2,j为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
第三方面，本申请提供一种通信装置，包括至少一个处理器和通信接口，所述通信接口用于接收来自所述通信装置之外的其它通信装置的信号并传输至所述至少一个处理器或将来自所述至少一个处理器的信号发送给所述通信装置之外的其它通信装置，所述至少一个处理器通过逻辑电路或执行代码指令用于实现上述第一方面或第一方面的任意可能的实现方式中的方法。
第四方面，本申请提供一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机程序或指令，当所述计算机程序或指令被通信装置执行时，实现上述第一方面或第一方面的任意可能的实现方式中的方法。
第五方面，本申请提供一种计算机程序产品，所述计算机程序产品包括计算机程序或指令，当所述计算机程序或指令被通信装置执行时，实现上述第一方面或第一方面的任意可能的实现方式中的方法。
第六方面，本申请提供一种芯片，包括至少一个处理器和接口；
所述接口，用于为所述至少一个处理器提供程序指令或者数据；
所述至少一个处理器用于执行所述程序行指令，以实现上述第一方面或第一方面的任意可能的实现方式中的方法。
第七方面，本申请提供一种终端，所述终端包含上述第二方面提供的任意运动估计装置、上述第三方面提供的任意通信装置、或者上述第四方面提供的任意计算机可读存储介质。进一步可选的，所述终端可以为车辆、无人机、机器人、智能家居设备或者卫星等。
上述第二方面至第七方面中任一方面可以达到的技术效果可以参照上述第一方面中有益效果的描述，此处不再重复赘述。
附图说明
图1为本申请提供的一种自运动估计系统的结构示意图；
图2为本申请提供的一种运动估计方法的流程示意图；
图3为本申请提供的一种车载系统中多传感器配置示意图；
图4为本申请提供的一种转动角速度矢量的示意图；
图5为本申请提供的一种位置平移矢量的示意图；
图6为本申请提供的一种载体坐标系与传感器坐标系之间的变换关系的示意图；
图7为本申请提供的一种运动估计装置的结构示意图；
图8为本申请提供的一种芯片的结构示意图。
具体实施方式
下面将结合附图，对本申请实施例进行详细描述。
请参见图1，图1是本申请实施例提供的一种自运动估计系统的结构示意图，该系统包括第一传感器1010、第一运动感知模块1011、第二传感器1020、第二运动感知模块1021和数据处理模块1030。
第一传感器1010可以为视觉传感器，例如，相机或者摄像头、红外热成像传感器等。第一传感器1010可以提供视觉测量数据，例如图像或者视频。第一运动感知模块1011用于根据第一传感器1010提供的测量数据确定运动测量数据，如传感器运动的转动角速度矢量和/或归一化或尺度伸缩(scaled)的平动速度矢量等或者带有完整尺度信息的平动速度矢量估计值。第二传感器1020可以为雷达传感器、超声波传感器、惯性测量传感器或者定位传感器等，例如，毫米波雷达、声呐、激光雷达、惯性测量单元(inertial measurement unit，IMU)或者全球导航卫星系统(global navigation satellite system，GNSS)等。第二传感器1020用于提供位置测量数据和/或速度测量数据，例如位置和/或径向速度或者速度投影分量的测量数据。第二运动感知模块1021用于根据第二传感器1020提供的位置测量数据和/或速度测量数据，确定运动测量数据，如传感器运动的瞬时平动速度矢量等。数据处理模块1030用于处理第一运动感知模块1011和第二运动感知模块1021提供的运动测量数据。本申请中，运动测量数据也可以称为是运动感知数据。
第一传感器1010、第一运动感知模块1011、第二传感器1020、第二运动感知模块1021和数据处理模块1030可以通过有线或者无线方式等连接在一起；第一传感器1010和第二传感器1020可以分布于载体的相同或者不同的位置；第一运动感知模块1011和第二运动感知模块1021可以分别与第一传感器1010和第二传感器1020集成在一起；也可以与数据处理模块1030集成在一起；也可以分别独立于其它模块存在，本申请不做限定。
在一种示例中，第一传感器1010、第一运动感知模块1011、第二传感器1020、第二运动感知模块1021和数据处理模块1030部署于一个处理器系统上。在又一种示例中，第一传感器1010、第二传感器1020分别部署于一个处理器系统上；第一运动感知模块1011、第二运动感知模块1021和数据处理模块1030部署于一个处理器系统上。
本申请实施例可适用各种载体中的多传感器系统，其中载体如车载(如汽车、摩托车或自行车等)、机载(如无人机、直升机或喷气式飞机或者气球)、船载(轮船、汽艇或者舰艇等)、星载(如卫星)或者智能体(如机器人等)等。
示例性的，载体为车载，车载上可以承载有至少一个第一传感器和至少一个第二传感器。比如，车载安装有1个第一传感器和1个第二传感器，或者，车载安装有1个第一传感器和5个第二传感器，或者，车载安装有6个第一传感器和5个第二传感器等。
目前，获得传感器运动速度或者载体运动速度估计的方法有多种。以车载平台为例，车载的运动估计中，可以是采用如下三种传感器。
一、IMU
IMU是测量物体三轴姿态角(或角速度)以及加速度的装置。一般地，一个IMU内会装有三轴的陀螺仪和三个方向的加速度计，来测量物体在三维空间中的角速度和加速度，并以此可以解算出物体运动速度和姿态。
二、雷达传感器
雷达传感器，通常可以提供距离、方位角和径向速度测量数据。基于静止目标物体的 方位角和径向速度分量测量数据，利用最小二乘方法或者其他的方式，可以获得传感器相对于大地的瞬时速度，特别的，雷达传感器可以获取到较为精确的纵向速度估计值。此外，基于上述速度估计值，可以进一步得到传感器的横摆角速度(yaw rate)估计值。
三、视觉传感器
视觉传感器通常可以提供连续的两帧或者多帧图像。基于上述两帧或者多帧图像，利用光流法或者特征点对应的方法或者直接优化光亮度(intensity)的目标物体函数的方法，可以获得传感器尺度伸缩的平动速度估计值和转动速度估计值。
但上述三种传感器均存在各自缺陷：
(1)IMU的运动速度估计是基于加速度计积累得到，测量误差会随时间累积，因而存在误差累积问题，需要利用其它传感器进行额外的校准，而且，一般用于车辆中的IMU的精度太低，若选用高精度的IMU，成本高昂。
(2)雷达传感器获取到的传感器的横向速度估计值的精确度较低，且不能获取传感器的俯仰角速度估计值(pitch rate)和滚转角速度(roll rate)估计值。
(3)视觉传感器存在尺度伸缩问题，深度信息与平动速度的各个分量耦合在一起，通常无法获得精确的深度估计，无法精确获得平动速度估计值，或者仅能获得平动速度的一个尺度伸缩的估计值。
为解决上述问题，本申请实施例提供一种运动估计的方法，用于较精确的确定载体的平动速度矢量估计值和转动角速度矢量估计值，从而对载体实现精确的运动估计。
需要指出的是，本申请中，平动速度矢量估计值也可以是平动位移矢量估计值，平动位移矢量估计值可以是两帧间的位置偏移矢量的估计值，也可以是两帧间的时间差和平动速度矢量估计值的乘积。
请参见图2，图2是本申请实施例提供的一种运动估计方法的流程示意图，该方法的执行主体可以是传感器系统或者融合感知系统或者集成上述系统的规划/控制系统如辅助驾驶或者自动驾驶系统等。或者，该方法的执行主体也可以是软件或者硬件(如与相应传感器通过无线或者有线连接或者集成在一起的数据处理装置)。
以下不同的执行步骤可以集中式实现，或者，以下不同的执行步骤也可以分布式实现。方法包括但不限于如下步骤：
步骤201，获取M个第一传感器的转动角速度矢量估计值和N个第二传感器的瞬时速度矢量估计值。
其中，M个第一传感器和N个第二传感器承载于一个载体上，第一传感器的数量M≥1，第二传感器的数量N≥1。
具体的，第一传感器可以为视觉传感器如相机、摄像头、红外传感器或其它成像传感器等，或惯性测量传感器如IMU，第二传感器可以为雷达传感器如毫米波雷达、激光雷达等，或超声波传感器如声呐等。
一种实现中，M(M＞1)个第一传感器可以是相同类型或不同类型，示例性的，M个第一传感器中包括M1个视觉传感器如相机或者摄像头和M2个惯性测量传感器如IMU，其中，M1+M2＝M，且M1≥0，M2≥0。和/或，N(N＞1)个第二传感器可以是相同类型或不同类型，示例性的，N个第二传感器中包括N1个雷达传感器如毫米波雷达、激光雷达和N2个超声波传感器如声呐，其中，N1+N2＝N，且N1≥0，N2≥0。
M个第一传感器和N个第二传感器可以安装于载体的相同位置或者不同位置。下面以 车载为例说明：
例子1，车载前端安装1个相机和1个毫米波雷达；
例子2，车载前端安装1个相机和1个毫米波雷达，车载4个角位置另外安装4个毫米波雷达；
例子3，车载前端安装1个毫米波雷达，4个角位置另外安装4个毫米波雷达，另外在车载上均匀安装6个相机。
此外，车载上还可以进一步安装IMU或者GNSS等。
示例性的，一种车载系统中多传感器配置示意图如图3所示，车载系统的多传感器可以包括1个相机、5个毫米波雷达和1个IMU，其中IMU的安装位置可以接近车载坐标系(也可以称为是车体坐标系、载体坐标系)的原点位置，车载坐标系的原点可以位于车体后轴中心。
上述例子1至例子3中，毫米波雷达也可以替换成激光雷达或超声波传感器等，相机也可以替换成摄像头或红外传感器等，也可以在原基础上增加至少一个激光雷达。
比如上述例子3中，可以将5个毫米波雷达的其中2个毫米波雷达替换为激光雷达，又或者将5个毫米波雷达全部替换为激光雷达，此外，还可以在原5个毫米波雷达的基础上增加1至3个激光雷达。
应理解，此处只是本申请示例性提供的车载上安装第一传感器和第二传感器的实现方式，其并不对本申请做任何限定。
具体的，所述获取M个第一传感器的转动角速度矢量估计值，可以是从传感器通过有线或者无线接口直接得到，其中转动角速度矢量估计值可以基于传感器的测量数据通过运动或者估计算法得到或者是由传感器直接测量得到；
或者，所述获取M个第一传感器的转动角速度矢量估计值，可以是从传感器通过有线或者无线接口直接得到传感器的测量数据，转动角速度矢量估计值根据传感器的测量数据通过运动或者估计算法得到或者是由传感器测量数据直接得到。
作为一种实现方式，转动角速度矢量估计值可以根据第一传感器的测量数据通过估计得到。此时，第一传感器的测量数据并不直接包含第一传感器的运动测量的测量值。
示例性的，第一传感器为视觉传感器如相机，相机的原始测量数据为视觉测量数据，比如图像或者视频，可以根据图像或者视频中的特征点、线、平面或者区域，基于数据的光学特性或者几何特性确定相机的转动角速度矢量估计值。例如，可以基于8点法或者5点法或者单应性(Homography)或者光流法等，得到相机的转动角速度矢量估计值。基于图像或者视频，得到传感器的转动角速度为现有技术，此处不赘述。
作为另一种实现方式，转动角速度矢量估计值可以从第一传感器的测量数据中直接得到。此时，第一传感器可以直接测量包含转动角速度矢量。
示例性的，第一传感器为惯性测量传感器如IMU，IMU可以直接测量转动角速度矢量。
需要指出的是，上述M个第一传感器可以包含相同类型或不同类型的第一传感器，示例性的，M个第一传感器可以包括M1个视觉传感器例如相机或者摄像头和M2个惯性测量传感器如IMU，其中，M1+M2＝M，且M1≥0，M2≥0，相应的，对M1个视觉传感器的测量数据处理得到M1个视觉传感器的转动角速度矢量估计值，以及从M2个惯性测量传感器的测量数据中直接得到读取M2个惯性测量传感器的转动角速度矢量估计值。
可选地，作为一种实现方式，转动角速度矢量可以为三维矢量，ω＝[ω x ω y ω z] T，如图4所示。
可选地，作为另一种实现方式，第一传感器或者载体在平面内运动，平面如地面或者平面轨道。转动角速度矢量可以表示为ω＝[0 0 ω z] T，此时，转动角速度矢量可以简化为用ω z表示。
需要指出的是，上述从传感器的测量数据直接得到或者基于传感器的测量数据通过运动估计得到转动角速度矢量估计值，可以根据传感器坐标系相对于载体坐标的变换关系，得到传感器在载体坐标系的转动角速度矢量估计值。
具体的，所述获取N个第二传感器的瞬时速度矢量估计值，可以是从传感器通过有线或者无线接口直接得到，其中瞬时速度矢量估计值可以基于传感器的测量数据通过运动或者估计算法得到；
或者，所述获取N个第二传感器的瞬时速度矢量估计值，可以是从传感器通过有线或者无线接口直接得到传感器的测量数据，瞬时速度矢量估计值根据传感器的测量数据通过运动或者估计算法得到。
作为一种实现方式，瞬时速度矢量估计值可以根据第二传感器的测量数据通过估计得到。此时，第二传感器的测量数据并不直接包含第二传感器的运动测量的测量值。
示例性的，第二传感器为毫米波雷达或者激光雷达或者超声波传感器如声呐，第二传感器的测量数据可以包括位置和径向速度，或者包括角度和径向速度。可以根据其中静止目标的测量数据，基于最小二乘法或者正交距离回归法或者最小均方误差准则等估计方法，确定瞬时速度矢量估计值。此外，也可以根据第二传感器的多次位置测量数据，根据其中静止目标的测量数据，确定瞬时速度矢量估计值。本申请实施例不做限定。
需要指出的是，载体获取第一传感器的转动角速度矢量估计值的数量可以小于或等于实际上载体中第一传感器的数量，载体获取第二传感器的瞬时速度矢量估计值的数量可以小于或等于实际上载体中第二传感器的数量。
第一个示例性中，载体上承载有6个第一传感器和3个第二传感器，载体可以获取6个第一传感器的转动角速度矢量估计值和3个第二传感器的瞬时速度矢量估计值。
第二个示例性中，载体上承载有6个第一传感器和3个第二传感器，载体可以获取4个第一传感器的转动角速度矢量估计值和2个第二传感器的瞬时速度矢量估计值。
步骤202，根据N个第二传感器的瞬时速度矢量估计值、N个第二传感器的外部参数和N个第二传感器所在的载体的第一转动角速度矢量估计值，确定载体的第一平动速度矢量估计值。
其中，N个第二传感器的外部参数可以包括N个第二传感器相对于载体坐标系的位置平移矢量，或者，包括N个第二传感器的坐标系原点相对于载体坐标系原点的位置平移矢量。示例性的，对于任一个第二传感器来说，第二传感器相对于载体坐标系的位置平移矢量用于将该第二传感器的坐标系原点平移至与载体坐标系原点一致。
以车载上安装5个第二传感器为例，该5个第二传感器可以包括毫米波雷达或者激光雷达或者超声波传感器，参照图5所示，5个第二传感器分别位于车载的不同位置，该5个第二传感器相对于车载坐标系原点的位置平移矢量分别为r 21,r 22,…,r 25，该5个第二传感器的瞬时速度矢量估计值分别为v 21,v 22,…,v 25。通常r 21，r 22，…，r 25互不相同时，v 21，v 22，…， v 25也互不相同。
具体的，根据N个第二传感器的瞬时速度矢量估计值和N个第二传感器的外部参数以及载体的第一转动角速度矢量估计值，确定载体的第一平动速度矢量估计值，可以是，载体的第一平动速度矢量估计值基于刚体的平动速度矢量、瞬时速度矢量、转动角速度以及位置平移矢量的关系得到，其中瞬时速度矢量以及位置平移矢量从N个第二传感器的瞬时速度矢量估计值和N个第二传感器的外部参数确定，转动角速度从载体的第一转动角速度矢量估计值确定。
具体的，刚体的平动速度矢量、瞬时速度矢量、转动角速度矢量以及位置平移矢量的关系式为t＝v-ω×r，或者t＝v+r×ω，其中，t为刚体的平动速度矢量，v为刚体的瞬时速度矢量，ω为刚体的转动角速度矢量，r为位置平移矢量，×表示矢量的叉积。
基于刚体的平动速度矢量、瞬时速度矢量、转动角速度矢量以及位置平移矢量的关系式，可以存在多种变形关系式，该多种变形关系式均可以得到载体的第一平动速度矢量估计值。
示例性的，确定第一平动速度矢量估计值可以基于如下关系式得到：
Figure PCTCN2021108627-appb-000055
或者
Figure PCTCN2021108627-appb-000056
其中，
Figure PCTCN2021108627-appb-000057
为载体的第一平动速度矢量估计值，ω为载体的第一转动角速度矢量估计值，v 2为第二传感器的瞬时速度矢量估计值，r 2为第二传感器相对于载体坐标系原点的位置平移矢量，×表示矢量的叉积。
具体的，上述关系式可以为
Figure PCTCN2021108627-appb-000058
其中，ω＝[ω x ω y ω z] T。
具体的，上述关系式也可以为
Figure PCTCN2021108627-appb-000059
其中，r 2＝[r x,2 r y,2 r z,2] T。
当然也可以根据其他的方式，本申请实施例不做限定。
具体的，r 2可以为第二传感器的外部参数，可以为第二传感器的坐标系原点相对于载体坐标系原点的位置平移矢量。
作为一种实现方式，第一平动速度矢量估计值可以是基于如下关系式确定：
Figure PCTCN2021108627-appb-000060
其中，
Figure PCTCN2021108627-appb-000061
为载体的第一平动速度矢量估计值，t 2,j为根据第j个第二传感器的瞬时速度矢量估计值及其外部参数和载体的第一转动角速度矢量估计值确定的载体的平动速度矢量估计值。
作为一种实现方式，基于最小均方误差(minimum mean sqaured error，MMSE)或者最小二乘(least square，LS)法等确定第一平动速度矢量估计值。
第一种实现方式中，第一平动速度矢量估计值是载体的N个平动速度矢量估计值的加权和，其中，载体的N个平动速度矢量估计值分别根据N个第二传感器的瞬时速度矢量估计值及其外部参数和载体的第一转动角速度矢量估计值确定。
一种具体实现中，载体的第一平动速度矢量估计值满足如下关系式：
Figure PCTCN2021108627-appb-000062
其中，
Figure PCTCN2021108627-appb-000063
为载体的第一平动速度矢量估计值，t 2,j为根据第j个第二传感器的瞬时速度矢量估计值及其外部参数和载体的第一转动角速度矢量估计值确定的载体的平动速度矢量估计值，w 2,j为与第j个第二传感器对应的加权系数或者加权系数矩阵。
具体的，加权系数w 2,j可以根据t 2,j的估计误差或测量误差的概率密度函数或统计特性或协方差矩阵确定。例如，w 2,j可以根据t 2,j的估计误差或测量误差的协方差矩阵确定，
Figure PCTCN2021108627-appb-000064
其中，
Figure PCTCN2021108627-appb-000065
P 2,j为t 2,j的估计误差或测量误差的协方差。
示例性的，根据第j个第二传感器的瞬时速度矢量估计值及其外部参数和载体的第一转动角速度矢量估计值确定载体的平动速度矢量估计值，可以是基于如下关系式确定：
t 2,j＝v 2,j-ω×r 2,j
相应的，作为第一种实现方式的一种具体示例，可以根据如下关系式确定第一平动速度矢量估计值：
Figure PCTCN2021108627-appb-000066
其中，
Figure PCTCN2021108627-appb-000067
为载体的第一平动速度矢量估计值，ω为载体的第一转动角速度矢量估计值，v 2,j为第j个第二传感器的瞬时速度矢量估计值，r 2,j为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量，w 2,j为与第j个第二传感器对应的加权系数或者加权系数矩阵。
具体的，加权系数w 2,j可以根据v 2,j和/或r 2,j测量误差或者估计误差确定。
例如，w 2,j根据v 2,j和r 2,j测量误差或者估计误差确定，例如，w 2,j满足如下关系式：
Figure PCTCN2021108627-appb-000068
其中，
Figure PCTCN2021108627-appb-000069
Figure PCTCN2021108627-appb-000070
其中，
Figure PCTCN2021108627-appb-000071
为v 2,j的测量误差或者估计误差的协方差矩阵，
Figure PCTCN2021108627-appb-000072
为r 2,j的测量误差或者估计误差的协方差矩阵。Ω根据载体的转动角速度矢量得到，具体为，
Figure PCTCN2021108627-appb-000073
其中，ω＝[ω x ω y ω z] T。
第二种实现方式中，第一平动速度矢量估计值是载体的N个平动速度矢量估计值的均值，其中，载体的N个平动速度矢量估计值分别根据N个第二传感器的瞬时速度矢量估计值及其外部参数和载体的第一转动角速度矢量估计值确定。
一种具体实现中，载体的第一平动速度矢量估计值满足如下关系式：
Figure PCTCN2021108627-appb-000074
其中，
Figure PCTCN2021108627-appb-000075
为载体的第一平动速度矢量估计值，t 2,j为根据第j个第二传感器的瞬时速度矢量估计值及其外部参数和载体的第一转动角速度矢量估计值确定的载体的平动速度矢量估计值。
示例性的，根据第j个第二传感器的瞬时速度矢量估计值及其外部参数和载体的第一转动角速度矢量估计值确定载体的平动速度矢量估计值，可以是基于t 2,j＝v 2,j-ω×r 2,j。 相应的，作为第二种实现方式的一种具体示例，第一平动速度矢量估计值满足如下关系式：
Figure PCTCN2021108627-appb-000076
其中，
Figure PCTCN2021108627-appb-000077
为载体的第一平动速度矢量估计值，ω为载体的第一转动角速度矢量估计值，v 2,j为第j个第二传感器的瞬时速度矢量估计值，r 2,j为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
此处，需要说明的是，第一种实现方式中，第一平动速度矢量估计值是载体的N个平动速度矢量估计值的加权和，而第二种实现方式中，第一平动速度矢量估计值是载体的N个平动速度矢量估计值的均值，也可以理解，第二种实现方式是第一种实现方式的一种特殊形式，若第一种实现方式中各第二传感器的加权系数相同，则可以为第二种实现方式。进一步的，该说明同样适用于其它实现方式中多个传感器估计值的加权和与多个传感器估计值的均值之间的关系。
作为另一种实现方式，N等于1，根据第一转动角速度矢量估计值、第二传感器的瞬时速度矢量估计值、第二传感器的坐标系相对于载体坐标系的位置平移矢量，确定第一平动速度矢量估计值，具体可参照上述关系式得到。
此外，还可以根据载体的第一转动角速度矢量估计值、第j个第二传感器的瞬时速度矢量估计值和第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量，通过正交距离回归(orthogonal distance regression，ODR)方法，确定载体的平动速度矢量估计值t 2,j。
本申请实施例中，第一转动角速度矢量估计值是根据M个第一传感器的转动角速度矢量估计值确定的，其中M≥1。
作为一种实现方式，第一转动角速度矢量估计值，可以基于如下关系式确定：
ω＝ω 1,i,i＝1,…,M,M≥1
其中，ω为第一转动角速度矢量估计值，ω 1,i为第i个第一传感器的转动角速度矢量估计值。
作为一种实现方式，基于最小均方误差或者最小二乘法等确定第一转动角速度矢量估计值。
第1种实现方式中，第一转动角速度矢量估计值是载体的M个转动角速度矢量估计值的加权和，其中，载体的M个转动角速度矢量估计值分别根据M个第一传感器的转动角速度矢量估计值确定。
一种具体实现中，载体的第一转动角速度矢量估计值满足如下关系式：
Figure PCTCN2021108627-appb-000078
其中，ω为载体的第一转动角速度矢量估计值，ω 1,i为根据第i个第一传感器的转动角速度矢量估计值得到载体的转动角速度矢量估计值，w 1,i为与第i个第一传感器对应的加权系数或者加权系数矩阵。
具体的，加权系数w 1,i可以根据ω 1,i的估计误差或测量误差的概率密度函数或统计特性或协方差矩阵确定。例如，w 1,i可以根据ω 1,i的估计误差或测量误差的协方差矩阵确定，具体为
Figure PCTCN2021108627-appb-000079
其中，
Figure PCTCN2021108627-appb-000080
P 1,i为ω 1,i的估计误差或测量误差的协方差。
第2种实现方式中，第一转动角速度矢量估计值是载体的M个转动角速度矢量估计值的均值，其中，载体的M个转动角速度矢量估计值分别根据M个第一传感器的转动角速度矢量估计值确定。
一种具体实现中，载体的第一转动角速度矢量估计值满足如下关系式：
Figure PCTCN2021108627-appb-000081
其中，ω为载体的第一转动角速度矢量估计值，ω 1,i为根据第i个第一传感器的转动角速度矢量估计值得到的载体的转动角速度矢量估计值。
作为另一种实现方式，M等于1，根据第一传感器的转动角速度矢量估计值，确定第一转动角速度矢量估计值，具体可参照上述关系式得到。
需要指出的是，以上，第一传感器的转动角速度矢量估计值、第二传感器的瞬时速度矢量估计值、载体的第一转动角速度矢量估计值、载体的第一平动速度矢量估计值等均为相对于载体坐标系定义的。而实际应用中，传感器的测量数据往往相对于传感器坐标系定义，因此，从第一传感器或者第二传感器得到运动速度矢量，包括转动角速度矢量和平动速度矢量或者瞬时速度矢量，往往相对于所在传感器定义比较方便。此时，需要基于传感器相对于载体坐标系的外部参数得到相对于载体坐标系的转动角速度矢量和平动速度矢量或者瞬时速度矢量。
不失一般性的，如图6所示，载体坐标系与传感器坐标系之间的变换关系通常可以用传感器的外部参数确定，外部参数可以包括旋转参数和传感器坐标系相对于载体坐标系的位置平移矢量。基于旋转参数，可将传感器坐标系的方向旋转至与载体坐标系的方向一致。基于位置平移矢量，可将传感器坐标系的原点平移至与载体坐标系的原点一致。
传感器坐标系相对于载体坐标系的位置平移矢量可以是如图6中矢量r，或者可以是如图5中的r 21,r 22,…,r 25；再如前述r 2,j,j＝1,…,N,N≥1。
旋转参数用于表示载体坐标系和传感器坐标系之间的旋转；具体的，可以是用四元数、旋转矩阵、欧拉角等表示。其中，四元数、旋转矩阵、欧拉角等可以互相转化。示例性的，旋转矩阵可以根据四元数得到，或旋转矩阵可以根据欧拉角得到。示例性的，传感器坐标系与载体坐标系的方向可以一致，此时，旋转参数为单位矩阵。
本申请实施例，进一步基于传感器的外部参数和相对于传感器坐标系的运动速度矢量，得到相对于载体坐标系的运动速度矢量，其中，运动速度矢量可以包括转动角速度、平动速度矢量、瞬时运动速度矢量中的一个或多个。
具体的，可以基于第二传感器的外部参数和相对于第二传感器坐标系的瞬时速度矢量，得到相对于载体坐标系的瞬时速度矢量，其中外部参数包括旋转参数。
例如，旋转参数可以是旋转矩阵，相对于载体坐标系的瞬时平动速度矢量可以由以下关系式确定：
v 2,j＝R 2,jv′ 2,j
其中，v 2,j为第j个第二传感器相对于载体坐标系的瞬时速度矢量估计值，v′ 2,j第j个第二传感器相对于传感器坐标系的瞬时速度矢量估计值，R 2,j为第二传感器的传感器坐标系到载体坐标系的旋转参数。
具体的，可以基于第一传感器的外部参数和相对于第一传感器坐标系的转动角速度矢 量，得到相对于载体坐标系的转动角速度矢量，其中外部参数包括旋转参数。
例如，旋转参数可以是旋转矩阵，相对于载体坐标系的转动角速度矢量可以由以下关系式确定：
ω 1,i＝R 1,iω′ 1,i
其中，ω 1,i为第i个第一传感器相对于载体坐标系的转动角速度矢量估计值，ω′ 1,i为第i个第一传感器相对于该第一传感器的坐标系的转动角速度矢量估计值，R 1,i为第一传感器的传感器坐标系到载体坐标系的旋转矩阵。
需要指出的是，旋转参数可以是固定值，也可以是运动估计中根据在线算法估计得到；位置平移矢量可以是固定值，也可以是运动估计中根据在线算法估计得到。本申请实施例对此不限定。
基于上述变换关系，可以根据传感器的外部参数、M个第一传感器的转动角速度矢量估计值，确定载体的第一转动角速度矢量估计值。其中传感器的外部参数可以包括传感器的旋转参数。
具体的，根据M个第一传感器的转动角速度矢量估计值，确定载体的第一转动角速度矢量估计值。
作为一种实现方式，可以基于如下关系式，确定第一转动角速度矢量估计值。
ω＝R 1,iω′ 1,i
其中，ω为第一转动角速度矢量估计值，ω′ 1,i为第i个第一传感器相对于传感器坐标系的转动角速度矢量估计值，R 1,i为第i个第一传感器的坐标系相对于载体坐标系的旋转参数。
作为一种实现方式，根据M个第一传感器中各第一传感器相对于传感器坐标系的转动角速度矢量估计值、各第一传感器的旋转参数，确定载体的第一转动角速度矢量估计值。
在一个示例中，基于如下关系式确定第一转动角速度矢量估计值：
Figure PCTCN2021108627-appb-000082
其中，ω为载体的第一转动角速度矢量估计值，ω′ 1,i为第i个第一传感器相对于传感器坐标系的转动角速度矢量估计值，w 1,i为与第i个第一传感器对应的加权系数或者加权系数矩阵，R 1,i为第i个第一传感器的坐标系相对于载体坐标系的旋转参数。
在又一个示例中，基于如下关系式确定第一转动角速度矢量估计值：
Figure PCTCN2021108627-appb-000083
其中，ω为载体的第一转动角速度矢量估计值，ω′ 1,i为第i个第一传感器相对于传感器坐标系的转动角速度矢量估计值，R 1,i为第i个第一传感器的坐标系相对于载体坐标系的旋转参数。
本申请实施例中，根据至少一个第一传感器的转动角速度矢量估计值，可以准确确定载体的第一转动角速度矢量估计值。结合第二传感器的瞬时速度矢量估计值，可以准确得到载体的第一平动速度矢量估计值。因此，采用本申请实施例的方法，可以实现载体的运动进行补偿，有助于实现运动目标和静止目标的分离，同时有助于实现对载体运动的定位和跟踪。
可选地，为了进一步提升载体的平动速度矢量估计值的准确性，本申请实施例可以进一步包括如下步骤203和步骤204。
步骤203，获取M个第一传感器中M′个第一传感器的归一化的平动速度矢量估计值。
具体的，M个第一传感器中包括M′个第一传感器，其中M′≤M，该M′个第一传感器可以是视觉传感器例如相机或者摄像头。
一个具体示例中，M个第一传感器包括M′个视觉传感器例如相机或者摄像头和M-M′个惯性测量传感器例如IMU。
作为一种实现方式，M′个第一传感器可以是视觉传感器例如相机或者摄像头，获取M′个第一传感器的归一化的平动速度矢量估计值，可以是根据相机或者摄像头得到的图像或者视频，根据其中的特征点、线、或者平面或者区域，基于数据的光学特性或者几何特性确定，例如基于8点法或者5点法或者单应性(Homography)或者光流法等方法得到，本申请实施例不做限定。
第一传感器的归一化的平动速度矢量估计值，理解为，根据第一传感器确定的尺度伸缩的平动速度矢量估计值，也即，第一传感器的归一化的平动速度矢量估计值与第一传感器的平动速度矢量估计值成比例。应理解，第一传感器的平动速度矢量估计值为第一传感器实际运动的平动速度矢量估计值。
示例性的，在第一传感器的坐标系中，第一传感器的归一化的平动速度矢量估计值可以表示为
Figure PCTCN2021108627-appb-000084
第一传感器的平动速度矢量估计值可以表示为v 1′，二者之间符合关系式：
Figure PCTCN2021108627-appb-000085
其中，v 1′＝[v′ 1x,v′ 1y,v′ 1z] T，
Figure PCTCN2021108627-appb-000086
s为第一传感器的平动速度矢量估计值的归一化参数或者尺度伸缩因子。
具体的，归一化参数或者尺度伸缩因子可以是平动速度矢量估计值的幅度或者范数或者模，或平动速度矢量估计值的某一分量，如z轴分量。
以光流法为例，可以基于以下关系式确定归一化的平动速度矢量估计值
Figure PCTCN2021108627-appb-000087
Figure PCTCN2021108627-appb-000088
Figure PCTCN2021108627-appb-000089
其中u,v为图像平面上的光流分量，s 1＝[-f 0 x] T，s 2＝[0 -f y] T，
Figure PCTCN2021108627-appb-000090
×表示矢量的叉积，f为相机焦距，x,y为图像平面的像素位置,x∈[p x-w x,p x+w x],y∈[p y-w y,p y+w y]；其中(p x,p y)为中心位置，w x和w y为非负整数，w x＝0,1,2,3,4…；w y＝0,1,2,3,4….。Z′为像素对应的目标点的相对深度，
Figure PCTCN2021108627-appb-000091
和ω为相对于传感器坐标系的归一化平动速度矢量和转动角速度矢量。
其中归一化的平动速度矢量估计值
Figure PCTCN2021108627-appb-000092
和相对深度Z′，满足如下关系
Figure PCTCN2021108627-appb-000093
或者，等价地
Figure PCTCN2021108627-appb-000094
Z＝sZ′
其中t′为相对于传感器坐标系的绝对平动速度矢量，Z为像素对应的目标点的绝对深度，t′ z为t′的z轴分量，s＝t′ z为尺度伸缩因子。需要指出的是，尺度伸缩因子s不限于t′ z，可以根据需要选择其它值，例如尺度伸缩因子s为t′的范数或者幅度等，如s＝‖t′‖。
需要指出的是，获取M′个第一传感器的归一化的平动速度矢量估计值，可以进一步包括，根据第一传感器的外部参数得到相对于载体坐标系的归一化的平动速度矢量估计值，其中第一传感器的外部参数包括第一传感器坐标系相对于载体坐标系的旋转参数。
例如，相对于载体坐标系的归一化的平动速度矢量估计值
Figure PCTCN2021108627-appb-000095
为
Figure PCTCN2021108627-appb-000096
其中，R 1为第一传感器的坐标系相对于载体坐标系的旋转矩阵。
需要指出的是，上述旋转变换为正交变换矩阵，并不改变归一化参数。下面，为方便描述，可以以归一化参数为例说明。
步骤204，根据第一平动速度矢量估计值、M′个第一传感器的归一化的平动速度矢量估计值，确定载体的第二平动速度矢量估计值。
本申请中，在确定出第一平动速度矢量估计值的基础上，可以基于M′个第一传感器的归一化的平动速度矢量估计值，进一步提高载体的平动速度矢量的估计精度，此处将进一步更新得到的载体的平动速度矢量估计值称为第二平动速度矢量估计值。
具体的，可以根据如下归一化的第一传感器的平动速度矢量、第一传感器的位置平移矢量、载体的平动速度矢量和载体的转动角速度矢量的关系，确定载体的第二平动速度矢量估计值：
Figure PCTCN2021108627-appb-000097
其中，t为载体的平动速度矢量，ω为载体的转动角速度矢量估计值，
Figure PCTCN2021108627-appb-000098
为第一传感器的归一化的平动速度矢量估计值，r 1为第一传感器相对于载体坐标系的位置平移矢量，s为第一传感器的平动速度矢量的归一化参数。
作为一种实现方式，可以根据载体的第一平动速度矢量、载体的第一转动角速度矢量、第一传感器的归一化的平动速度矢量以及第一传感器相对于载体坐标系的位置平移矢量的关系，确定载体的第二平动速度矢量估计值：
具体的，载体的第二平动速度矢量估计值根据以下关系式确定：
Figure PCTCN2021108627-appb-000099
其中，
Figure PCTCN2021108627-appb-000100
为载体的第二平动速度矢量估计值，ω为载体的第一转动角速度矢量估计值，
Figure PCTCN2021108627-appb-000101
为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i为第i个第一传感器相对于载体坐标系的位置平移矢量，s i为第i个第一传感器的平动速度矢量的归一化参数。
具体的，载体的第二平动速度矢量估计值根据以下关系式确定：
Figure PCTCN2021108627-appb-000102
其中，
Figure PCTCN2021108627-appb-000103
为载体的第二平动速度矢量估计值，w′ 1,i为与第i个第一传感器对应的加权系数矩阵，ω为载体的第一转动角速度矢量估计值，
Figure PCTCN2021108627-appb-000104
为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i为第i个第一传感器相对于载体坐标系的位置平移矢量，s i为第i个第一传感器的平动速度矢量的归一化参数。加权系数矩阵w′ 1,i可以根据
Figure PCTCN2021108627-appb-000105
的估计误差的协方差矩阵确定，与之前方法类似，此处不详述。
具体的，载体的第二平动速度矢量估计值也根据以下关系式确定：
Figure PCTCN2021108627-appb-000106
其中ω为载体的第一转动角速度矢量估计值，
Figure PCTCN2021108627-appb-000107
为第i个第一传感器的归一化的平动 速度矢量估计值，r 1,i为第i个第一传感器相对于载体坐标系的位置平移矢量，s i为第i个第一传感器的平动速度矢量的归一化参数。
可选的，s i具体可以根据下述关系式确定：
Figure PCTCN2021108627-appb-000108
其中
Figure PCTCN2021108627-appb-000109
为载体的第一平动速度矢量估计值。
作为另一种实现方式，可以根据上述载体的平动速度矢量、载体的第一转动角速度矢量和第一传感器的平动速度矢量以及第一传感器相对于载体坐标系的位置平移矢量的关系，确定载体的第二平动速度矢量估计值,符合以下关系式：
Figure PCTCN2021108627-appb-000110
其中，载体的第二平动速度矢量估计值由上述
Figure PCTCN2021108627-appb-000111
确定。ω为载体的第一转动角速度矢量估计值，
Figure PCTCN2021108627-appb-000112
为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i为第i个第一传感器相对于载体坐标系的位置平移矢量，s i为第i个第一传感器的平动速度矢量的归一化参数，s i具体可以根据下述关系式确定：
Figure PCTCN2021108627-appb-000113
其中
Figure PCTCN2021108627-appb-000114
Figure PCTCN2021108627-appb-000115
为载体的第一平动速度矢量估计值。
进一步的，基于上述两种实现方式中任一种实现方式，可以通过迭代方式确定第二平动速度矢量估计值。具体的，每次迭代可以得到一次载体的平动速度矢量估计值，最后一次迭代得到的载体的平动速度矢量估计值作为载体的第二平动速度矢量估计值。迭代实现方式可以进一步利用传感器的平动速度矢量估计值和位置平移矢量，从而提高载体的平动速度矢量的估计精度。
具体的，在第一种迭代方式中，可以是根据如下关系式得到载体的第二平动速度矢量估计值：
Figure PCTCN2021108627-appb-000116
其中，t k为第k次迭代中得到的载体的平动速度矢量估计值，也可以理解，第K次迭代(最后一次迭代)得到t K作为载体的第二平动速度矢量估计值
Figure PCTCN2021108627-appb-000117
ω为载体的第一转动角速度矢量估计值，
Figure PCTCN2021108627-appb-000118
为第一传感器的归一化的平动速度矢量估计值，r 1为第一传感器相对于载体坐标系的位置平移矢量，s k为第k次迭代中第一传感器的平动速度矢量的归一化参数。具体的，s k可以是根据如下关系式得到：
s k＝‖t k-1+ω×r 1‖
其中，t k-1为第k-1次迭代中得到的载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，r 1为第一传感器相对于载体坐标系的位置平移矢量。
作为一种具体实现，每次迭代可以是，根据M′个第一传感器的归一化的平动速度矢量估计值、第k-1次迭代中M′个第一传感器的平动速度矢量的归一化参数、M′个第一传感器相对于载体坐标系的位置平移矢量和第一转动角速度矢量估计值，确定第k次迭代载体的平动速度矢量估计值。其中，第k次迭代中M′个第一传感器的平动速度矢量的归一化参数可以根据第k-1次迭代中得到的载体的平动速度矢量估计值确定。通过迭代过程，将第K次迭代(最后一轮迭代)中得到符合预设条件的载体的平动速度矢量估计值作为载体的第二平动速度矢量估计值。
作为一种实现方式，可以根据上述第一传感器的平动速度矢量、第一传感器的位置平移矢量与载体的平动速度矢量和载体的转动角速度矢量的关系，确定载体的第二平动速度 矢量估计值，具体包括：
载体的第二平动速度矢量估计值根据以下关系式确定：
Figure PCTCN2021108627-appb-000119
其中，t k为第k次迭代中载体的平动速度矢量估计值，t K为第二平动速度矢量估计值
Figure PCTCN2021108627-appb-000120
ω为第一转动角速度矢量估计值，
Figure PCTCN2021108627-appb-000121
为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i为第i个第一传感器相对于载体坐标系的位置平移矢量，s i,k为第k次迭代中第i个第一传感器的平动速度矢量的归一化参数，s i,k由第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定，w′ 1,i,k为第k次迭代中第i个第一传感器的加权系数。
w′ 1,i,k在每次迭代中可以取固定值或者根据预设算法确定。示例性的，w′ 1,i,k可以根据s i·v 1,i-ω×r 1,i的估计误差的协方差矩阵确定，与之前方法类似，此处不详述。
或者，载体的第二平动速度矢量估计值
Figure PCTCN2021108627-appb-000122
根据以下关系式确定：
Figure PCTCN2021108627-appb-000123
其中，t k为第k次迭代中载体的平动速度矢量估计值，t K为第二平动速度矢量估计值
Figure PCTCN2021108627-appb-000124
ω为第一转动角速度矢量估计值，
Figure PCTCN2021108627-appb-000125
为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i为第i个第一传感器相对于载体坐标系的位置平移矢量，s i,k为第k次迭代中第i个第一传感器的平动速度矢量的归一化参数，s i,k由第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定。
s i,k是根据第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定，可以有如下两种情况：
k等于1，也即第一次迭代中，s i,1可以由第一平动速度矢量估计值根据如下关系式确定：
Figure PCTCN2021108627-appb-000126
其中，
Figure PCTCN2021108627-appb-000127
为第一平动速度矢量估计值，ω为第一转动角速度矢量估计值，r 1,i为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量。
k大于1，也即之后的几次迭代中，s i,k可以由第k-1次迭代中载体的平动速度矢量估计值根据如下关系式确定：
s i,k＝‖t k-1+ω×r 1,i‖
其中，t k-1为第k-1次迭代中载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，r 1,i为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量。
可选的，作为一种具体的实现方式，M′等于1，可以根据第一传感器的归一化的平动速度矢量估计值、第k-1次迭代中第一传感器的平动速度矢量的归一化参数、第一传感器的坐标系相对于载体坐标系的位置平移矢量和第一转动角速度矢量估计值，确定第k次迭代中载体的平动速度矢量估计值，具体可参照上述关系式得到。其中，第k次迭代中第一传感器的平动速度矢量的归一化参数是根据第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定。
需要说明的是，可以将第一种迭代方式的迭代终止条件设置为，第二平动速度矢量估计值和第一平动速度矢量估计值之间的矢量距离不大于第一预设的阈值或者门限。相当于， 确定第k次迭代得到的载体的平动速度矢量估计值与第一平动速度矢量估计值之间的矢量距离，若该矢量距离大于第一预设的阈值或者门限，则进一步执行第k+1次迭代，若该矢量距离不大于第一预设的阈值或者门限，则确定迭代终止，此时，第k次迭代(也即第K次迭代，或最后一次迭代)得到的载体的平动速度矢量估计值可以称为是第二平动速度矢量估计值。
示例性的，第二平动速度矢量估计值和第一平动速度矢量估计值满足如下关系式：
Figure PCTCN2021108627-appb-000128
其中，Threshold1为第一预设阈值或者门限。
另外，还可以将第一种迭代方式的迭代终止条件设置为，达到最大迭代次数。相当于，设置最大迭代次数为K，也即，共计执行K次迭代，将第K次迭代(也即最后一次迭代)得到的载体的平动速度矢量估计值称为是第二平动速度矢量估计值。示例性的，可以设置最大迭代次数K的取值为20。
上述第一种迭代方式中，每次迭代中，根据多个第一传感器的归一化的平动速度矢量估计值，确定载体的平动速度矢量估计值。将多个第一传感器的参数作为一次迭代的输入，有助于提高第二平动速度矢量的估计精度。
具体的，第二种迭代方式中，根据第i个第一传感器对应的载体的平动速度矢量估计值、第i个第一传感器的平动速度矢量的归一化参数、第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，确定第i个第一传感器对应的载体的平动速度矢量估计值。其中，第i个第一传感器的平动速度矢量的归一化参数是根据第i-1个第一传感器对应的载体的平动速度矢量估计值确定的。
等价地，第二种迭代方式为根据前一个第一传感器对应的载体的平动速度矢量估计值，确定后一个第一传感器的平动速度矢量的归一化参数，进而确定后一个第一传感器对应的载体的平动速度矢量估计值，其主要适用于M′大于1的情况。
可以是根据如下关系式得到的载体的平动速度矢量估计值：
Figure PCTCN2021108627-appb-000129
其中，
Figure PCTCN2021108627-appb-000130
为第i个第一传感器对应的载体的平动速度矢量估计值，
Figure PCTCN2021108627-appb-000131
为载体的第二平动速度矢量估计值
Figure PCTCN2021108627-appb-000132
ω为第一转动角速度矢量估计值，
Figure PCTCN2021108627-appb-000133
为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s i为第i个第一传感器的平动速度矢量的归一化参数，s i由第一平动速度矢量估计值或
Figure PCTCN2021108627-appb-000134
确定。
具体的，s i由可以是根据如下关系式得到。
Figure PCTCN2021108627-appb-000135
其中，
Figure PCTCN2021108627-appb-000136
为第i-1个第一传感器对应的载体的平动速度矢量估计值，
Figure PCTCN2021108627-appb-000137
为第一平动速度矢量估计值；ω为第一转动角速度矢量估计值，r 1,i为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量。
此外，本申请实施例可以执行多轮迭代，每轮迭代包括M′个第一传感器之间的迭代。可以是根据如下关系式得到的载体的平动速度矢量估计值：
Figure PCTCN2021108627-appb-000138
其中，
Figure PCTCN2021108627-appb-000139
为第l轮迭代中第i个第一传感器对应的载体的平动速度矢量估计值，也可 以理解，第L轮迭代(最后一轮迭代)中，t i,L′为第二平动速度矢量估计值
Figure PCTCN2021108627-appb-000140
ω为第一转动角速度矢量估计值，
Figure PCTCN2021108627-appb-000141
为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s i,l为第l轮迭代中第i个第一传感器的平动速度矢量的归一化参数，s i,l由第一平动速度矢量估计值或
Figure PCTCN2021108627-appb-000142
或
Figure PCTCN2021108627-appb-000143
确定。
具体的，s i，l由可以是根据如下关系式得到。
Figure PCTCN2021108627-appb-000144
其中，
Figure PCTCN2021108627-appb-000145
为第l轮迭代中第i-1个第一传感器对应的载体的平动速度矢量估计值，
Figure PCTCN2021108627-appb-000146
为第一平动速度矢量估计值；
Figure PCTCN2021108627-appb-000147
ω为第一转动角速度矢量估计值，r 1,i为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量。
需要说明的是，前一轮迭代中最后一次迭代是后一轮迭代中第一次迭代的前一次迭代，或者说，第l-1轮迭代中第M′次迭代是第l轮迭代中第1次迭代的前一次迭代。当然，也可以理解，第k次迭代可以是第l-1轮迭代中第M′次迭代，第k+1次迭代可以是第l轮迭代中第1次迭代。进一步的，后一轮迭代的参数可以由前一轮迭代的参数确定，具体的，第l轮迭代中第1个第一传感器的平动速度矢量的归一化参数由第l-1轮迭代中第M′个第一传感器对应的载体的平动速度矢量估计值确定。
s i,l是根据第一平动速度矢量估计值，或第l轮迭代中第i-1个第一传感器对应的载体的平动速度矢量估计值，或第l-1轮迭代中第M′个第一传感器对应的载体的平动速度矢量估计值确定，可以有如下三种情况：
i等于1，l等于1，s i,l是根据第一平动速度矢量估计值确定，可以参照如下关系式。
Figure PCTCN2021108627-appb-000148
其中，
Figure PCTCN2021108627-appb-000149
为第一平动速度矢量估计值，ω为第一转动角速度矢量估计值，r 1,1为第1个第一传感器的坐标系相对于载体坐标系的位置平移矢量。
i大于1，s i,l是根据第l轮迭代中第i-1个第一传感器对应的载体的平动速度矢量估计值确定，可以参照如下关系式。
Figure PCTCN2021108627-appb-000150
其中，
Figure PCTCN2021108627-appb-000151
为第l轮迭代中第i-1个第一传感器对应的载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，r 1,i为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量。
i等于1，l大于1，s 1,l是根据第l-1轮迭代中第M′个第一传感器对应的载体的平动速度矢量估计值确定，可以参照如下关系式。
Figure PCTCN2021108627-appb-000152
其中，
Figure PCTCN2021108627-appb-000153
为第l-1轮迭代中第M′个第一传感器对应的载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，r 1,i为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量。
需要说明的是，可以将第二种迭代方式的迭代终止条件设置为，第二平动速度矢量估计值和第一平动速度矢量估计值之间的矢量距离不大于第二预设阈值或门限。相当于，确定第l轮迭代中第i-1个第一传感器对应的载体的平动速度矢量估计值与第一平动速度矢量估计值之间的矢量距离，若该矢量距离大于第二预设阈值或门限，则进一步确定第l轮迭代中第i个第一传感器对应的载体的平动速度矢量估计值，若该矢量距离不大于第二预设 阈值或门限，则确定迭代终止，此时，可以将第l轮迭代中第i-1个第一传感器对应的载体的平动速度矢量估计值称为是第二平动速度矢量估计值。
示例性的，第二平动速度矢量估计值和第一平动速度矢量估计值满足如下关系式：
Figure PCTCN2021108627-appb-000154
其中，Threshold2为第二预设阈值或门限。
另外，还可以将第二种迭代方式的迭代终止条件设置为，达到最大迭代轮数。相当于，设置最大迭代轮数为L，也即，共计执行L轮迭代，将第L轮迭代(也即最后一轮迭代)中某个第一传感器对应的载体的平动速度矢量估计值称为是第二平动速度矢量估计值。示例性的，设置最大迭代轮数L的取值为5，设置第5轮中第M′个第一传感器对应的载体的平动速度矢量估计值为第二平动速度矢量估计值。
上述第二种迭代方式中，针对每个第一传感器的归一化的平动速度矢量估计值，确定每个第一传感器对应的载体的平动速度矢量估计值，相当于，针对每个第一传感器进行一次迭代，将一个第一传感器的参数作为一次迭代的输入，可以快速得到较为准确的第二平动速度矢量估计值。
本申请实施例中，第一传感器评估自身运动得到的归一化的平动速度矢量估计值较准确，该将第一传感器的归一化的平动速度矢量估计值和第一平动速度矢量估计值进行融合，得到载体的第二平动速度矢量估计值，相比于第一平动速度矢量估计值，可进一步提高载体的平动速度矢量估计值的精确度。采用载体的第一转动角速度矢量估计值和载体的第二平动速度矢量估计值对载体的运动进行补偿，有助于实现运动目标和静止目标的分离，同时有助于实现对载体运动的定位和跟踪。
本申请实施例中，M'个第一传感器中的第一传感器可以理解是可以获得归一化的平动速度矢量估计值的第一传感器，M'个第一传感器可以是视觉传感器，例如相机、摄像头、红外传感器等。一个示例中，M'个第一传感器可以是M1'个相机和M2'个红外传感器，其中，M1'+M2'＝M'，且M1'≥0，M2'≥0。
此外，本申请实施例还可以进一步结合M-M'个可以获取平动速度矢量估计值的第一传感器，其中M-M′个第一传感器获取的平动速度矢量估计值包含速度矢量的尺度信息，例如各个速度分量的完整信息，而不仅仅是归一化值或者方向信息，从而提高载体的第二平动速度矢量估计值的准确性。
也即，本申请提供再一种确定第二平动速度矢量估计值的方式如下。
在步骤203中可以是获取M′个第一传感器如视觉传感器的归一化的平动速度矢量估计值和M-M′个第一传感器如惯性传感器(如IMU)的平动速度矢量估计值。
在步骤204中，先根据M-M′个第一传感器如惯性传感器的平动速度矢量估计值，确定M-M′个第一传感器如惯性传感器对应的载体的平动速度矢量估计值。
示例性的，可基于关系式
Figure PCTCN2021108627-appb-000155
或
Figure PCTCN2021108627-appb-000156
确定，关系式的具体描述可参照上述实现方式。
作为一种直接实现方式，可以基于关系式
Figure PCTCN2021108627-appb-000157
或
Figure PCTCN2021108627-appb-000158
Figure PCTCN2021108627-appb-000159
确定视觉传感器对应的载体的平动速度矢量估计值，将视觉传感器对应的载体的平动速度矢量估计值、惯性传感器对应的载体的平动速度矢量估计值进行加权，得到第二平动速度矢量估计值。
作为一种迭代的实现方式，可以将惯性传感器对应的载体的平动速度矢量估计值作为 第一次迭代中的载体的平动速度矢量估计值，也即，k等于1中对应的
Figure PCTCN2021108627-appb-000160
中，
Figure PCTCN2021108627-appb-000161
替换为惯性传感器对应的载体的平动速度矢量估计值。基于步骤204中的迭代方式进行迭代，直至符合迭代条件，将符合迭代条件的第k次迭代中的载体的平动速度矢量估计值作为第二平动速度矢量估计值。
进一步的，在本申请中，还可能存在M′＝0，也即，在步骤203中获取M个惯性传感器的平动速度矢量估计值。
相应的，在步骤204中，可以根据M个惯性传感器的平动速度矢量估计值，确定惯性传感器对应的载体的平动速度矢量估计值，示例性的，可基于关系式
Figure PCTCN2021108627-appb-000162
Figure PCTCN2021108627-appb-000163
或
Figure PCTCN2021108627-appb-000164
确定惯性传感器对应的载体的平动速度矢量估计值，关系式的具体描述可参照上述实现方式。将惯性传感器对应的载体的平动速度矢量估计值和第一平动速度矢量估计值进行加权得到第二平动速度矢量估计值。
可选地，根据上述第二平动速度矢量估计值，可以进一步更新载体的转动角速度矢量估计值。此处将更新的该转动角速度矢量估计值称为是载体的第二转动角速度矢量估计值。
可选的，步骤205，根据载体的第二平动速度矢量估计值，确定载体的第二转动角速度矢量估计值。
可以基于如下关系式，确定第二转动角速度矢量估计值：
Figure PCTCN2021108627-appb-000165
或者，等价地
Figure PCTCN2021108627-appb-000166
或者，等价地
Figure PCTCN2021108627-appb-000167
其中，ω′为第二转动角速度矢量估计值，v 2为第二传感器的瞬时速度矢量估计值，
Figure PCTCN2021108627-appb-000168
为第二平动速度矢量估计值，r 2为第二传感器的坐标系相对于载体坐标系的位置平移矢量，×表示矢量的叉积。其中[r 2] ×为与r 2对应的反对称矩阵。
基于上述关系式，第二转动角速度矢量估计值的表达式可以为
Figure PCTCN2021108627-appb-000169
作为一种实现方式，可以基于如下关系式，确定第二转动角速度矢量估计值：
Figure PCTCN2021108627-appb-000170
其中，ω′为第二转动角速度矢量估计值，v 2,j为第j个第二传感器的瞬时速度矢量估计值，
Figure PCTCN2021108627-appb-000171
为第二平动速度矢量估计值，
Figure PCTCN2021108627-appb-000172
为[r 2,j] ×的逆矩阵，[r 2,j] ×为与r 2,j对应的反对称矩阵，r 2,j为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
其中，r 2,j＝[r x,2,j r y,2,j r z,2,j] T，
Figure PCTCN2021108627-appb-000173
作为一种实现方式，根据第二转动角速度矢量估计值、N个第二传感器的瞬时速度矢量估计值、N个第二传感器中各第二传感器的坐标系相对于载体坐标系的位置平移矢量，确定第二转动角速度矢量估计值，其中N≥1。
在一个示例中，基于如下关系式确定第二转动角速度矢量估计值：
Figure PCTCN2021108627-appb-000174
其中，ω′为第二转动角速度矢量估计值，ω 2,j为第j个第二传感器的加权系数，v 2,j为 第j个第二传感器的瞬时速度矢量估计值，
Figure PCTCN2021108627-appb-000175
为第二平动速度矢量估计值，
Figure PCTCN2021108627-appb-000176
为[r 2,j] ×的逆矩阵，[r 2,j] ×为与r 2,j对应的反对称矩阵，r 2,j为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
在又一个示例中，基于如下关系式确定第一转动角速度矢量估计值：
Figure PCTCN2021108627-appb-000177
其中，ω′为第二转动角速度矢量估计值，v 2,j为第j个第二传感器的瞬时速度矢量估计值，
Figure PCTCN2021108627-appb-000178
为第二平动速度矢量估计值，
Figure PCTCN2021108627-appb-000179
为[r 2,j] ×的逆矩阵，[r 2,j] ×为与r 2,j对应的反对称矩阵，r 2,j为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
作为另一种实现方式中，N等于1，根据第二转动角速度矢量估计值、第二传感器的瞬时速度矢量估计值、第二传感器的坐标系相对于载体坐标系的位置平移矢量，确定第二转动角速度矢量估计值，具体可参照上述关系式得到。
本申请中，可以将载体的第二平动速度矢量估计值和载体的第二转动角速度矢量估计值，分别作为最终确定出的载体的平动速度矢量估计值和载体的转动角速度矢量估计值。
此外，还可以将第一转动角速度矢量估计值和第二转动角速度矢量估计值进一步融合，将融合后的转动角速度矢量估计值作为最终确定的载体的转动角速度矢量估计值。具体融合中，可以是确定载体的第一转动角速度矢量估计值和载体的第二转动角速度矢量估计值的平均值。或者，基于最小均方误差进行加权组合，将载体的第一转动角速度矢量估计值和载体的第二转动角速度矢量估计值进行融合。
本申请实施例中，根据载体的第二平动速度矢量估计值，确定载体的第二转动角速度矢量估计值，相比于第一转动角速度矢量估计值，可进一步提高载体的转动角速度矢量估计值的精确度。采用载体的第二转动角速度矢量估计值和载体的第二平动速度矢量估计值对载体的运动进行补偿，有助于实现运动目标和静止目标的分离，同时有助于实现对载体运动的定位和跟踪。
本文中描述的各个实施例可以为独立的方案，也可以根据内在逻辑进行组合，这些方案都落入本申请的保护范围中。
本申请实施例中对模块的划分是示意性的，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式。另外，在本申请各个实施例中的各功能模块可以集成在一个处理器中，也可以是单独物理存在，也可以两个或两个以上模块集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。
与上述构思相同，本申请实施例还提供一种运动估计装置用于实现上述方法。
示例性的，该运动估计装置可以是传感器系统或者融合感知系统或者集成上述系统的规划/控制系统如辅助驾驶或者自动驾驶系统等。或者，该运动估计装置还可以是软件或者硬件(如与相应传感器通过无线或者有线连接或者集成在一起的数据处理装置)。
该运动估计装置可为具有运动估计功能的车辆，或者为具有运动估计功能的其他部件。该运动估计装置包括但不限于：车载终端、车载控制器、车载模块、车载模组、车载部件、车载芯片、车载单元、车载雷达或车载摄像头等其他传感器，车辆可通过该车载终端、车载控制器、车载模块、车载模组、车载部件、车载芯片、车载单元、车载雷达或摄像头， 实施本申请提供的方法。
该运动估计装置还可以为除了车辆之外的其他具有运动估计功能的智能终端，或设置在除了车辆之外的其他具有运动估计功能的智能终端中，或设置于该智能终端的部件中。该智能终端可以为智能运输设备、智能家居设备、机器人等其他终端设备。该运动估计装置包括但不限于智能终端或智能终端内的控制器、芯片、雷达或摄像头等其他传感器、以及其他部件等。
该运动估计装置可以是一个通用设备或者是一个专用设备。在具体实现中，该运动估计装置还可以台式机、便携式电脑、网络服务器、掌上电脑(personal digital assistant，PDA)、移动手机、平板电脑、无线终端设备、嵌入式设备或其他具有处理功能的设备。本申请实施例不限定该运动估计装置的类型。
该运动估计装置还可以是具有处理功能的芯片或处理器，该运动估计装置可以包括多个处理器。处理器可以是一个单核(single-CPU)处理器，也可以是一个多核(multi-CPU)处理器。该具有处理功能的芯片或处理器可以设置在传感器中，也可以不设置在传感器中，而设置在传感器输出信号的接收端。
如图7为本申请示例性提供的一种运动估计装置700。该运动估计装置700可以包括：获取单元701和处理单元702。应理解，装置实施例的描述与方法实施例的描述相互对应，因此，未详细描述的内容可以参见上文方法实施例，为了简洁，这里不再赘述。
示例性的，所述获取单元701用于获取M个第一传感器的转动角速度矢量估计值和N个第二传感器的瞬时速度矢量估计值；其中，M≥1，N≥1；
所述处理单元702用于根据所述N个第二传感器的瞬时速度矢量估计值和所述N个第二传感器所在的载体的第一转动角速度矢量估计值，确定所述载体的第一平动速度矢量估计值，其中，所述第一转动角速度矢量估计值是根据所述M个第一传感器的转动角速度矢量估计值确定的。
在一种可能的实现方式中，所述处理单元702具体用于基于如下关系式确定所述第一平动速度矢量估计值：
Figure PCTCN2021108627-appb-000180
其中，
Figure PCTCN2021108627-appb-000181
为第一平动速度矢量估计值，ω为第一转动角速度矢量估计值，v 2,j为第j个第二传感器的瞬时速度矢量估计值，r 2,j为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
在一种可能的实现方式中，所述第一平动速度矢量估计值满足如下关系式：
Figure PCTCN2021108627-appb-000182
其中，
Figure PCTCN2021108627-appb-000183
为第一平动速度矢量估计值，ω为第一转动角速度矢量估计值，w 2,j为第j个第二传感器的加权系数，v 2,j为第j个第二传感器的瞬时速度矢量估计值，r 2,j为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
在一种可能的实现方式中，所述第一转动角速度矢量估计值满足如下关系式：
Figure PCTCN2021108627-appb-000184
其中，ω为第一转动角速度矢量估计值，w 1,i为第i个第一传感器的加权系数，ω 1,i为第i个第一传感器的转动角速度矢量估计值。
在一种可能的实现方式中，所述获取单元701还用于获取所述M个第一传感器中M′个第一传感器的归一化的平动速度矢量估计值，其中，1≤M′≤M；所述处理单元702还用于根据所述第一平动速度矢量估计值、所述M′个第一传感器的归一化的平动速度矢量估计值，确定所述载体的第二平动速度矢量估计值。
在一种可能的实现方式中，所述处理单元702具体用于基于如下关系式确定所述第二平动速度矢量估计值：
Figure PCTCN2021108627-appb-000185
其中，
Figure PCTCN2021108627-appb-000186
为第二平动速度矢量估计值，ω为第一转动角速度矢量估计值，
Figure PCTCN2021108627-appb-000187
为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s i为第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s i由第一平动速度矢量估计值确定。
在一种可能的实现方式中，所述第二平动速度矢量估计值满足如下关系式：
Figure PCTCN2021108627-appb-000188
其中，t K为第二平动速度矢量估计值
Figure PCTCN2021108627-appb-000189
t k为第k次迭代中载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，
Figure PCTCN2021108627-appb-000190
为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s i,k为第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s i,k由第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定。
在一种可能的实现方式中，所述第二平动速度矢量估计值满足如下关系式：
Figure PCTCN2021108627-appb-000191
其中，t K为第二平动速度矢量估计值
Figure PCTCN2021108627-appb-000192
t k为第k次迭代中载体的平动速度矢量估计值，w′ 1,i,k为第k次迭代中第i个第一传感器的加权系数，ω为第一转动角速度矢量估计值，
Figure PCTCN2021108627-appb-000193
为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s i,k为第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s i,k由第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定。
在一种可能的实现方式中，第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子满足如下关系式：
s i,k＝‖t k-1+ω×r 1,i‖
其中，t k-1为第k-1次迭代中载体的平动速度矢量估计值，t 0为第一平动速度矢量估计值。
在一种可能的实现方式中，所述第二平动速度矢量估计值满足如下关系式：
Figure PCTCN2021108627-appb-000194
其中，
Figure PCTCN2021108627-appb-000195
为第二平动速度矢量估计值
Figure PCTCN2021108627-appb-000196
Figure PCTCN2021108627-appb-000197
为第l轮迭代中第i个第一传感器对应的 载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，
Figure PCTCN2021108627-appb-000198
为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s i,l为第l轮迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s i,l由第一平动速度矢量估计值或
Figure PCTCN2021108627-appb-000199
或
Figure PCTCN2021108627-appb-000200
确定。
在一种可能的实现方式中，所述第l轮迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子满足如下关系式：
Figure PCTCN2021108627-appb-000201
其中，
Figure PCTCN2021108627-appb-000202
为第一平动速度矢量估计值；
Figure PCTCN2021108627-appb-000203
在一种可能的实现方式中，所述处理单元702还用于基于如下关系式确定所述载体的第二转动角速度矢量估计值：
Figure PCTCN2021108627-appb-000204
其中，ω′为第二转动角速度矢量估计值，v 2,j为第j个第二传感器的瞬时速度矢量估计值，
Figure PCTCN2021108627-appb-000205
为第二平动速度矢量估计值，
Figure PCTCN2021108627-appb-000206
为[r 2,j] ×的逆矩阵，[r 2,j] ×为与r 2,j对应的反对称矩阵，r 2,j为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
与上述构思相同，如图8所示，为本申请实施例提供的一种芯片的结构示意图。
芯片800包括一个或多个处理器801以及接口电路802。可选的，所述芯片800还可以包含总线803。
其中，处理器801可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器801中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器801可以是通用处理器、数字通信器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)或者其它可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本申请实施例中的公开的各方法、步骤。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
接口电路802可以用于数据、指令或者信息的发送或者接收，处理器801可以利用接口电路802接收的数据、指令或者其它信息，进行加工，可以将加工完成信息通过接口电路802发送出去。
可选的，芯片还包括存储器，存储器可以包括只读存储器和随机存取存储器，并向处理器提供操作指令和数据。存储器的一部分还可以包括非易失性随机存取存储器(NVRAM)。
可选的，存储器存储了可执行软件模块或者数据结构，处理器可以通过调用存储器存储的操作指令(该操作指令可存储在操作系统中)，执行相应的操作。
可选的，芯片可以使用在本申请实施例涉及的通信装置(包括主节点和从节点)中。可选的，接口电路802可用于输出处理器801的执行结果。关于本申请的一个或多个实施例提供的数据传输方法可参考前述各个实施例，这里不再赘述。
需要说明的，处理器801、接口电路802各自对应的功能既可以通过硬件设计实现，也可以通过软件设计来实现，还可以通过软硬件结合的方式来实现，这里不作限制。
本申请实施例还提供一种雷达系统，用于为车辆提供运动估计功能。其包含至少一个本申请上述实施例提到的运动估计装置，该系统内的至少一个运动估计装置可以集成为一个整机或设备，或者该系统内的至少一个运动估计装置也可以独立设置为元件或装置。
本申请实施例还提供一种传感器系统，用于为车辆提供运动估计功能。其包含至少一 个本申请上述实施例提到的运动估计装置，以及，摄像头或雷达等传感器中的至少一个，该系统内的至少一个传感器装置可以集成为一个整机或设备，或者该系统内的至少一个传感器装置也可以独立设置为元件或装置。
本申请实施例还提供一种系统，应用于无人驾驶或智能驾驶中，其包含至少一个本申请上述实施例提到的运动估计装置、摄像头、雷达等传感器中的至少一个，该系统内的至少一个装置可以集成为一个整机或设备，或者该系统内的至少一个装置也可以独立设置为元件或装置。
进一步，上述任一系统可以与车辆的中央控制器进行交互，为所述车辆驾驶的决策或控制提供探测和/或融合信息。
本申请实施例还提供一种车辆，所述车辆包括至少一个本申请上述实施例提到的运动估计装置或上述任一系统。
本申请实施例还提供一种通信装置，包括处理器和通信接口，所述通信接口用于接收来自所述通信装置之外的其它通信装置的信号并传输至所述处理器或将来自所述处理器的信号发送给所述通信装置之外的其它通信装置，所述处理器通过逻辑电路或执行代码指令用于实现上述如图2中的方法。
本申请实施例还一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机程序或指令，当所述计算机程序或指令被通信装置执行时，实现上述如图2中的方法。
本申请实施例还提供一种计算机程序产品，所述计算机程序产品包括计算机程序或指令，当所述计算机程序或指令被通信装置执行时，实现上述如图2中的方法。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。

Claims (26)

  1. 一种运动估计的方法，其特征在于，包括：
    获取M个第一传感器的转动角速度矢量估计值和N个第二传感器的瞬时速度矢量估计值；其中，M≥1，N≥1；
    根据所述N个第二传感器的瞬时速度矢量估计值和所述N个第二传感器所在的载体的第一转动角速度矢量估计值，确定所述载体的第一平动速度矢量估计值，其中，所述第一转动角速度矢量估计值是根据所述M个第一传感器的转动角速度矢量估计值确定的。
  2. 如权利要求1所述的方法，其特征在于，所述第一平动速度矢量估计值基于如下关系式确定：
    Figure PCTCN2021108627-appb-100001
    其中，
    Figure PCTCN2021108627-appb-100002
    为第一平动速度矢量估计值，ω为第一转动角速度矢量估计值，v 2,j为第j个第二传感器的瞬时速度矢量估计值，r 2,j为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
  3. 如权利要求1所述的方法，其特征在于，所述第一平动速度矢量估计值满足如下关系式：
    Figure PCTCN2021108627-appb-100003
    其中，
    Figure PCTCN2021108627-appb-100004
    为第一平动速度矢量估计值，ω为第一转动角速度矢量估计值，w 2,j为第j个第二传感器的加权系数，v 2,j为第j个第二传感器的瞬时速度矢量估计值，r 2,j为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
  4. 如权利要求1至3任一项所述的方法，其特征在于，所述第一转动角速度矢量估计值满足如下关系式：
    Figure PCTCN2021108627-appb-100005
    其中，ω为第一转动角速度矢量估计值，w 1,i为第i个第一传感器的加权系数，ω 1,i为第i个第一传感器的转动角速度矢量估计值。
  5. 如权利要求1至4任一项所述的方法，其特征在于，还包括：
    获取所述M个第一传感器中M′个第一传感器的归一化的平动速度矢量估计值，其中，1≤M′≤M；
    根据所述第一平动速度矢量估计值、所述M′个第一传感器的归一化的平动速度矢量估计值，确定所述载体的第二平动速度矢量估计值。
  6. 如权利要求5所述的方法，其特征在于，所述第二平动速度矢量估计值基于如下关系式确定：
    Figure PCTCN2021108627-appb-100006
    其中，
    Figure PCTCN2021108627-appb-100007
    为第二平动速度矢量估计值，ω为第一转动角速度矢量估计值，
    Figure PCTCN2021108627-appb-100008
    为第i个第 一传感器的归一化的平动速度矢量估计值，r 1,i为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s i为第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s i由第一平动速度矢量估计值确定。
  7. 如权利要求5所述的方法，其特征在于，所述第二平动速度矢量估计值满足如下关系式：
    Figure PCTCN2021108627-appb-100009
    其中，t K为第二平动速度矢量估计值
    Figure PCTCN2021108627-appb-100010
    t k为第k次迭代中载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，
    Figure PCTCN2021108627-appb-100011
    为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s i,k为第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s i,k由第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定。
  8. 如权利要求5所述的方法，其特征在于，所述第二平动速度矢量估计值满足如下关系式：
    Figure PCTCN2021108627-appb-100012
    其中，t K为第二平动速度矢量估计值
    Figure PCTCN2021108627-appb-100013
    t k为第k次迭代中载体的平动速度矢量估计值，w′ 1,i,k为第k次迭代中第i个第一传感器的加权系数，ω为第一转动角速度矢量估计值，
    Figure PCTCN2021108627-appb-100014
    为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s i,k为第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s i,k由第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定。
  9. 如权利要求7或8所述的方法，其特征在于，第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子满足如下关系式：
    s i,k＝‖t k-1+ω×r 1,i‖
    其中，t k-1为第k-1次迭代中载体的平动速度矢量估计值，t 0为第一平动速度矢量估计值。
  10. 如权利要求5所述的方法，其特征在于，所述第二平动速度矢量估计值满足如下关系式：
    Figure PCTCN2021108627-appb-100015
    其中，
    Figure PCTCN2021108627-appb-100016
    为第二平动速度矢量估计值
    Figure PCTCN2021108627-appb-100017
    Figure PCTCN2021108627-appb-100018
    为第l轮迭代中第i个第一传感器对应的载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，
    Figure PCTCN2021108627-appb-100019
    为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s i,l为第l轮迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s i，l由第一平动速度矢量估计值或
    Figure PCTCN2021108627-appb-100020
    或
    Figure PCTCN2021108627-appb-100021
    确定。
  11. 如权利要求10所述的方法，其特征在于，所述第l轮迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子满足如下关系式：
    Figure PCTCN2021108627-appb-100022
    其中，
    Figure PCTCN2021108627-appb-100023
    为第一平动速度矢量估计值；
    Figure PCTCN2021108627-appb-100024
  12. 如权利要求5至11任一项所述的方法，其特征在于，还包括：
    基于如下关系式确定所述载体的第二转动角速度矢量估计值：
    Figure PCTCN2021108627-appb-100025
    其中，ω′为第二转动角速度矢量估计值，v 2,j为第j个第二传感器的瞬时速度矢量估计值，
    Figure PCTCN2021108627-appb-100026
    为第二平动速度矢量估计值，
    Figure PCTCN2021108627-appb-100027
    为[r 2,j] ×的逆矩阵，[r 2,j] ×为与r 2,j对应的反对称矩阵，r 2,j为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
  13. 一种运动估计的装置，其特征在于，包括：
    获取单元和处理单元；
    所述获取单元用于获取M个第一传感器的转动角速度矢量估计值和N个第二传感器的瞬时速度矢量估计值；其中，M≥1，N≥1；
    所述处理单元用于根据所述N个第二传感器的瞬时速度矢量估计值和所述N个第二传感器所在的载体的第一转动角速度矢量估计值，确定所述载体的第一平动速度矢量估计值，其中，所述第一转动角速度矢量估计值是根据所述M个第一传感器的转动角速度矢量估计值确定的。
  14. 如权利要求13所述的装置，其特征在于，所述处理单元具体用于基于如下关系式确定所述第一平动速度矢量估计值：
    Figure PCTCN2021108627-appb-100028
    其中，
    Figure PCTCN2021108627-appb-100029
    为第一平动速度矢量估计值，ω为第一转动角速度矢量估计值，v 2,j为第j个第二传感器的瞬时速度矢量估计值，r 2,j为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
  15. 如权利要求13所述的装置，其特征在于，所述第一平动速度矢量估计值满足如下关系式：
    Figure PCTCN2021108627-appb-100030
    其中，
    Figure PCTCN2021108627-appb-100031
    为第一平动速度矢量估计值，ω为第一转动角速度矢量估计值，w 2,j为第j个第二传感器的加权系数，v 2,j为第j个第二传感器的瞬时速度矢量估计值，r 2,j为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
  16. 如权利要求13至15任一项所述的装置，其特征在于，所述第一转动角速度矢量估计值满足如下关系式：
    Figure PCTCN2021108627-appb-100032
    其中，ω为第一转动角速度矢量估计值，w 1,i为第i个第一传感器的加权系数，ω 1,i为第i个第一传感器的转动角速度矢量估计值。
  17. 如权利要求13至16任一项所述的装置，其特征在于，所述获取单元还用于获取所述M个第一传感器中M′个第一传感器的归一化的平动速度矢量估计值，其中，1≤M′≤M；所述处理单元还用于根据所述第一平动速度矢量估计值、所述M′个第一传感器的归一化的平动速度矢量估计值，确定所述载体的第二平动速度矢量估计值。
  18. 如权利要求17所述的装置，其特征在于，所述处理单元具体用于基于如下关系式 确定所述第二平动速度矢量估计值：
    Figure PCTCN2021108627-appb-100033
    其中，
    Figure PCTCN2021108627-appb-100034
    为第二平动速度矢量估计值，ω为第一转动角速度矢量估计值，
    Figure PCTCN2021108627-appb-100035
    为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s i为第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s i由第一平动速度矢量估计值确定。
  19. 如权利要求17所述的装置，其特征在于，所述第二平动速度矢量估计值满足如下关系式：
    Figure PCTCN2021108627-appb-100036
    其中，t K为第二平动速度矢量估计值
    Figure PCTCN2021108627-appb-100037
    t k为第k次迭代中载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，
    Figure PCTCN2021108627-appb-100038
    为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s i,k为第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s i,k由第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定。
  20. 如权利要求17所述的装置，其特征在于，所述第二平动速度矢量估计值满足如下关系式：
    Figure PCTCN2021108627-appb-100039
    其中，t K为第二平动速度矢量估计值
    Figure PCTCN2021108627-appb-100040
    t k为第k次迭代中载体的平动速度矢量估计值，w′ 1,i,k为第k次迭代中第i个第一传感器的加权系数，ω为第一转动角速度矢量估计值，
    Figure PCTCN2021108627-appb-100041
    为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i为第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s i,k为第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s i,k由第一平动速度矢量估计值或第k-1次迭代中载体的平动速度矢量估计值确定。
  21. 如权利要求19或20所述的装置，其特征在于，第k次迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子满足如下关系式：
    s i,k＝‖t k-1+ω×r 1,i‖
    其中，t k-1为第k-1次迭代中载体的平动速度矢量估计值，t 0为第一平动速度矢量估计值。
  22. 如权利要求17所述的装置，其特征在于，所述第二平动速度矢量估计值满足如下关系式：
    Figure PCTCN2021108627-appb-100042
    其中，
    Figure PCTCN2021108627-appb-100043
    为第二平动速度矢量估计值
    Figure PCTCN2021108627-appb-100044
    Figure PCTCN2021108627-appb-100045
    为第l轮迭代中第i个第一传感器对应的载体的平动速度矢量估计值，ω为第一转动角速度矢量估计值，
    Figure PCTCN2021108627-appb-100046
    为第i个第一传感器的归一化的平动速度矢量估计值，r 1,i第i个第一传感器的坐标系相对于载体坐标系的位置平移矢量，s i,l为第l轮迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子，s i，l由第一平动速度矢量估计值或
    Figure PCTCN2021108627-appb-100047
    或
    Figure PCTCN2021108627-appb-100048
    确定。
  23. 如权利要求22所述的装置，其特征在于，所述第l轮迭代中第i个第一传感器的平动速度矢量的归一化参数或者尺度伸缩因子满足如下关系式：
    Figure PCTCN2021108627-appb-100049
    其中，
    Figure PCTCN2021108627-appb-100050
    为第一平动速度矢量估计值；
    Figure PCTCN2021108627-appb-100051
  24. 如权利要求17至23任一项所述的装置，其特征在于，所述处理单元还用于基于如下关系式确定所述载体的第二转动角速度矢量估计值：
    Figure PCTCN2021108627-appb-100052
    其中，ω′为第二转动角速度矢量估计值，v 2,j为第j个第二传感器的瞬时速度矢量估计值，
    Figure PCTCN2021108627-appb-100053
    为第二平动速度矢量估计值，
    Figure PCTCN2021108627-appb-100054
    为[r 2,j] ×的逆矩阵，[r 2,j] ×为与r 2,j对应的反对称矩阵，r 2,j为第j个第二传感器的坐标系相对于载体坐标系的位置平移矢量。
  25. 一种芯片，其特征在于，包括至少一个处理器和接口；
    所述接口，用于为所述至少一个处理器提供程序指令或者数据；
    所述至少一个处理器用于执行所述程序行指令，以实现如权利要求1至12中任一项所述的方法。
  26. 一种计算机可读存储介质，其特征在于，所述计算机可读存储介质中存储有计算机程序或指令，当所述计算机程序或指令被通信装置执行时，实现如权利要求1至12中任一项所述的方法。
PCT/CN2021/108627 2020-08-18 2021-07-27 一种运动估计方法及装置 WO2022037370A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202010831971.6A CN114076946A (zh) 2020-08-18 2020-08-18 一种运动估计方法及装置
CN202010831971.6 2020-08-18

Publications (1)

Publication Number Publication Date
WO2022037370A1 true WO2022037370A1 (zh) 2022-02-24

Family

ID=80281546

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2021/108627 WO2022037370A1 (zh) 2020-08-18 2021-07-27 一种运动估计方法及装置

Country Status (2)

Country Link
CN (1) CN114076946A (zh)
WO (1) WO2022037370A1 (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115575923A (zh) * 2022-12-08 2023-01-06 千巡科技(深圳)有限公司 一种地面机器人静止判断方法、系统、装置以及存储介质

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040080449A1 (en) * 2002-10-25 2004-04-29 Omron Corporation Radar device for a vehicle
CN101320089A (zh) * 2007-06-05 2008-12-10 通用汽车环球科技运作公司 用于车辆动力估计的雷达、激光雷达和摄像机增强的方法
JP2010085196A (ja) * 2008-09-30 2010-04-15 Toyota Motor Corp 物体挙動推定方法および物体挙動推定装置
CN106560728A (zh) * 2015-10-06 2017-04-12 通用汽车环球科技运作有限责任公司 用于目标速度估算的雷达视觉融合
CN109313258A (zh) * 2016-06-03 2019-02-05 奥托立夫开发公司 用于车辆环境检测系统的增强的物体检测和运动状态估计
US20190057263A1 (en) * 2017-08-21 2019-02-21 2236008 Ontario Inc. Automated driving system that merges heterogenous sensor data
CN110857983A (zh) * 2018-08-10 2020-03-03 通用汽车环球科技运作有限责任公司 使用具有不同观测角的多个雷达进行对象速度矢量估计

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040080449A1 (en) * 2002-10-25 2004-04-29 Omron Corporation Radar device for a vehicle
CN101320089A (zh) * 2007-06-05 2008-12-10 通用汽车环球科技运作公司 用于车辆动力估计的雷达、激光雷达和摄像机增强的方法
JP2010085196A (ja) * 2008-09-30 2010-04-15 Toyota Motor Corp 物体挙動推定方法および物体挙動推定装置
CN106560728A (zh) * 2015-10-06 2017-04-12 通用汽车环球科技运作有限责任公司 用于目标速度估算的雷达视觉融合
CN109313258A (zh) * 2016-06-03 2019-02-05 奥托立夫开发公司 用于车辆环境检测系统的增强的物体检测和运动状态估计
US20190057263A1 (en) * 2017-08-21 2019-02-21 2236008 Ontario Inc. Automated driving system that merges heterogenous sensor data
CN110857983A (zh) * 2018-08-10 2020-03-03 通用汽车环球科技运作有限责任公司 使用具有不同观测角的多个雷达进行对象速度矢量估计

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115575923A (zh) * 2022-12-08 2023-01-06 千巡科技(深圳)有限公司 一种地面机器人静止判断方法、系统、装置以及存储介质

Also Published As

Publication number Publication date
CN114076946A (zh) 2022-02-22

Similar Documents

Publication Publication Date Title
CN109887057B (zh) 生成高精度地图的方法和装置
CN110148185B (zh) 确定成像设备坐标系转换参数的方法、装置和电子设备
AU2018282302B2 (en) Integrated sensor calibration in natural scenes
WO2020093378A1 (en) Vehicle positioning system using lidar
WO2021143286A1 (zh) 车辆定位的方法、装置、控制器、智能车和系统
US10094919B2 (en) Radar-vision fusion for target velocity estimation
WO2019104571A1 (zh) 图像处理方法和设备
CN111308415B (zh) 一种基于时间延迟的在线估计位姿的方法和设备
CN112013877B (zh) 毫米波雷达与惯性测量单元的检测方法及相关装置
Eynard et al. Real time UAV altitude, attitude and motion estimation from hybrid stereovision
CN114730472A (zh) 车载相机的外部参数的标定方法及相关装置
WO2020024182A1 (zh) 一种参数处理方法、装置及摄像设备、飞行器
WO2022037370A1 (zh) 一种运动估计方法及装置
WO2021218346A1 (zh) 一种聚类的方法和装置
WO2020019175A1 (zh) 图像处理方法和设备、摄像装置以及无人机
US11866056B2 (en) Ballistic estimation of vehicle data
WO2021056283A1 (en) Systems and methods for adjusting a vehicle pose
CN116952229A (zh) 无人机定位方法、装置、系统和存储介质
WO2021056503A1 (zh) 可移动平台的定位方法、装置、可移动平台及存储介质
WO2020215296A1 (zh) 可移动平台的巡线控制方法、设备、可移动平台及系统
CN108416044B (zh) 场景缩略图的生成方法、装置、电子设备及存储介质
WO2021218347A1 (zh) 一种聚类的方法和装置
WO2021143664A1 (zh) 在载运工具中测量目标对象距离的方法、装置和载运工具
WO2022033139A1 (zh) 一种自运动估计方法及相关装置
CN112050829B (zh) 一种运动状态确定方法及装置

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 21857465

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 21857465

Country of ref document: EP

Kind code of ref document: A1