Motion estimation method and device

Abstract

The present application discloses a motion estimation method and device, which belongs to the field of sensor technology and can be used for assisted driving and autonomous driving. The method includes: obtaining the rotational angular velocity vector estimation values ​​of M first sensors and the instantaneous velocity vector estimation values ​​of N second sensors, M≥1, N≥1, and determining the first translational velocity vector estimation value of the carrier based on the instantaneous velocity vector estimation values ​​of the N second sensors and the first rotational angular velocity vector estimation values ​​of the carrier where the N second sensors are located, wherein the first rotational angular velocity vector estimation value is determined based on the rotational angular velocity vector estimation values ​​of the M first sensors. The method can be used in the data processing process of sensor perception, and the method is used to accurately estimate the motion of the carrier where the sensor is located, and can be applied to vehicle networks, such as vehicle-to-vehicle (V2X), long-term evolution technology (LTE‑V) for inter-vehicle communication, and vehicle-to-vehicle (V2V).

Description

Technical Field

[0001] The present application relates to the field of sensor technology, and in particular to a motion estimation method and device. Background Art

[0002] Advanced driver assistant systems (ADAS) or autonomous driving (AD) systems are equipped with a variety of sensors, such as millimeter-wave radar, lidar, ultrasonic sensors such as sonar, and visual sensors such as cameras or cameras, to perceive surrounding environmental information, including moving and stationary targets. Different methods are usually used to analyze and process moving and stationary targets. For example, moving targets (such as vehicles and pedestrians) are classified, identified, and tracked, while stationary targets (such as obstacles, guardrails, and curbs) are classified and identified. In this way, additional information can be provided for autonomous driving, such as avoiding obstacles and providing drivable areas.

[0003] The sensor can usually be installed on a carrier, and the sensor moves with the carrier on which the sensor is located. On the one hand, the movement of the carrier on which the sensor is located makes it impossible to analyze moving targets and stationary targets independently. Therefore, it is necessary to estimate the movement of the carrier on which the sensor is located to separate moving targets from stationary targets. On the other hand, the tracking of moving targets is usually based on motion models, such as constant velocity (CV) / constant acceleration (CA) / uniform circular motion (CT) models, and the models usually assume relative ground or geodetic coordinate systems. The movement of the carrier on which the sensor is located will cause the above models to fail or the tracking performance to degrade. Therefore, it is necessary to compensate for the movement of the carrier on which the sensor is located.

[0004] In summary, accurately estimating the motion of the carrier on which the sensor is located is a technical problem that people in this field are trying to solve. Summary of the Invention

[0005] The present application provides a motion estimation method and apparatus for accurately estimating the motion of a carrier on which a sensor is located.

[0006] In a first aspect, the present application provides a motion estimation method, the method comprising:

[0007] Obtaining rotational angular velocity vector estimation values ​​of M first sensors and instantaneous velocity vector estimation values ​​of N second sensors; wherein M≥1, N≥1;

[0008] The first translational velocity vector estimate of the carrier is determined based on the instantaneous velocity vector estimates of the N second sensors and the first rotational angular velocity vector estimate of the carrier where the N second sensors are located, wherein the first rotational angular velocity vector estimate is determined based on the rotational angular velocity vector estimates of the M first sensors.

[0009] In the above technical solution, since the rotational angular velocity vector estimation value of the first sensor and the instantaneous velocity vector estimation value of the second sensor are relatively accurate, the first rotational angular velocity vector estimation value of the carrier is determined based on the rotational angular velocity vector estimation value of at least one first sensor, and then the first translational velocity vector estimation value of the carrier is determined in combination with the instantaneous velocity vector estimation value of the second sensor, which helps to obtain a more accurate movement of the carrier where the sensor is located.

[0010] In a possible implementation, the first translational velocity vector estimate is determined based on the following relationship:

[0011]

[0012] in, is the estimated value of the first translational velocity vector, ω is the estimated value of the first rotational angular velocity vector, v 2,j is the instantaneous velocity vector estimate of the jth second sensor, r 2,j is the position translation vector of the j-th second sensor's coordinate system relative to the carrier's coordinate system.

[0013] The above relationship is obtained based on the relationship between the rigid body's translational velocity vector, instantaneous velocity vector, rotational angular velocity, and position translation vector, and the above relationship can have multiple variations. According to the above relationship, the first translational velocity vector estimate can be determined more accurately.

[0014] In a possible implementation, the first translational velocity vector estimate satisfies the following relationship:

[0015]

[0016] in, is the estimated value of the first translational velocity vector, ω is the estimated value of the first rotational angular velocity vector, w 2,j is the weighting coefficient of the jth second sensor, v 2,j is the instantaneous velocity vector estimate of the jth second sensor, r 2,j is the position translation vector of the j-th second sensor's coordinate system relative to the carrier's coordinate system.

[0017] In a possible implementation, the first rotation angular velocity vector estimate satisfies the following relationship:

[0018]

[0019] Where ω is the estimated value of the first rotation angular velocity vector, w 1,i is the weighting coefficient of the i-th first sensor, ω 1,i is the estimated value of the rotational angular velocity vector of the i-th first sensor.

[0020] In a possible implementation, the method further includes:

[0021] Obtain normalized translational velocity vector estimates of M′ first sensors among the M first sensors, where 1≤M′≤M;

[0022] A second translational velocity vector estimate value of the carrier is determined according to the first translational velocity vector estimate value and the normalized translational velocity vector estimates of the M′ first sensors.

[0023] In the above technical solution, the normalized translational velocity vector estimation value obtained by the first sensor evaluating its own motion is more accurate. The normalized translational velocity vector estimation value of the first sensor and the first translational velocity vector estimation value are fused to obtain the second translational velocity vector estimation value of the carrier, which can further improve the accuracy of the translational velocity vector estimation value of the carrier.

[0024] In a possible implementation, the second translational velocity vector estimate is determined based on the following relationship:

[0025]

[0026] in, is the estimated value of the second translational velocity vector, ω is the estimated value of the first rotational angular velocity vector, is the normalized translational velocity vector estimate of the i-th first sensor, r 1,i is the position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system, s i is the normalized parameter or scale factor of the translational velocity vector of the i-th first sensor, s i Determined by the first translational velocity vector estimate.

[0027] The above relationship is obtained based on the relationship between the rigid body's translational velocity vector, instantaneous velocity vector, rotational angular velocity, and position translation vector, and the above relationship can have multiple variations. According to the above relationship, the second translational velocity vector estimate can be determined more accurately.

[0028] In a possible implementation, the second translational velocity vector estimate satisfies the following relationship:

[0029]

[0030] Among them, 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 kth iteration, ω is the estimated value of the first rotational angular velocity vector, is the normalized translational velocity vector estimate of the i-th first sensor, r 1,i is the position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system, s i,k is the normalization parameter or scale factor of the translational velocity vector of the i-th first sensor in the k-th iteration, s i,k Determined by the first translational velocity vector estimate or the translational velocity vector estimate of the carrier in the k-1th iteration.

[0031] In a possible implementation, the second translational velocity vector estimate satisfies the following relationship:

[0032]

[0033] Among them, 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 kth 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 rotation angular velocity vector, is the normalized translational velocity vector estimate of the i-th first sensor, r 1,i is the position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system, s i,k is the normalization parameter or scale factor of the translational velocity vector of the i-th first sensor in the k-th iteration, s i,k Determined by the first translational velocity vector estimate or the translational velocity vector estimate of the carrier in the k-1th iteration.

[0034] In one possible implementation, the normalized parameter or scale factor of the translational velocity vector of the i-th first sensor in the k-th iteration satisfies the following relationship:

[0035] s i,k =‖t k-1 +ω×r 1,i ‖

[0036] Among them, t k-1 is the estimated value of the translational velocity vector of the carrier in the k-1th iteration, and t0 is the estimated value of the first translational velocity vector.

[0037] In the above technical solution, in each iteration, the estimated translational velocity vector of the carrier is determined based on the normalized translational velocity vector estimates of the multiple first sensors. Using the parameters of the multiple first sensors as input for one iteration helps improve the estimation accuracy of the second translational velocity vector.

[0038] In a possible implementation, the second translational velocity vector estimate satisfies the following relationship:

[0039]

[0040] in, is the estimated value of the second translational velocity vector is the estimated value of the translational velocity vector of the carrier corresponding to the i-th first sensor in the l-th iteration, ω is the estimated value of the first rotational angular velocity vector, is the normalized translational velocity vector estimate of the i-th first sensor, r 1,i The position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system, s i，l is the normalized parameter or scale factor of the translational velocity vector of the i-th first sensor in the l-th iteration, s i,l Estimated by the first translational velocity vector or or Sure.

[0041] In a possible implementation, the normalized parameter or scale scaling factor of the translational velocity vector of the i-th first sensor in the l-th iteration satisfies the following relationship:

[0042]

[0043] in, is the estimated value of the first translational velocity vector;

[0044] In the above technical solution, the normalized translational velocity vector estimation value of each first sensor is used to determine the translational velocity vector estimation value of the carrier corresponding to each first sensor. This is equivalent to performing one iteration for each first sensor and using the parameters of a first sensor as the input of one iteration, so that a relatively accurate second translational velocity vector estimation value can be quickly obtained.

[0045] In a possible implementation, the method further includes:

[0046] The second rotational angular velocity vector estimation value of the carrier is determined based on the following relationship:

[0047]

[0048] Where ω′ is the estimated value of the second rotation angular velocity vector, v 2,j is the instantaneous velocity vector estimate of the j-th second sensor, is the estimated value of the second translational velocity vector, For [r 2,j ] × The inverse matrix, [r 2,j ] × For r 2,j The corresponding antisymmetric matrix, r 2,j is the position translation vector of the j-th second sensor's coordinate system relative to the carrier's coordinate system.

[0049] In the above technical solution, a more accurate second rotational angular velocity vector estimate is further determined based on the more accurate second translational velocity vector estimate.

[0050] In a second aspect, the present application provides a motion estimation device, the device comprising:

[0051] Acquisition unit and processing unit;

[0052] The acquisition unit is used to acquire the rotational angular velocity vector estimation values ​​of M first sensors and the instantaneous velocity vector estimation values ​​of N second sensors; wherein M≥1, N≥1;

[0053] The processing unit is used to determine the first translational velocity vector estimate value of the carrier based on the instantaneous velocity vector estimate values ​​of the N second sensors and the first rotational angular velocity vector estimate value of the carrier where the N second sensors are located, wherein the first rotational angular velocity vector estimate value is determined based on the rotational angular velocity vector estimate values ​​of the M first sensors.

[0054] In a possible implementation manner, the processing unit is specifically configured to determine the first translational velocity vector estimate based on the following relationship:

[0055]

[0056] in, is the estimated value of the first translational velocity vector, ω is the estimated value of the first rotational angular velocity vector, v 2,j is the instantaneous velocity vector estimate of the jth second sensor, r 2,j is the position translation vector of the j-th second sensor's coordinate system relative to the carrier's coordinate system.

[0057] In a possible implementation, the first translational velocity vector estimate satisfies the following relationship:

[0058]

[0059] in, is the estimated value of the first translational velocity vector, ω is the estimated value of the first rotational angular velocity vector, w 2,j is the weighting coefficient of the jth second sensor, v 2,j is the instantaneous velocity vector estimate of the jth second sensor, r 2,j is the position translation vector of the j-th second sensor's coordinate system relative to the carrier's coordinate system.

[0060] In a possible implementation, the first rotation angular velocity vector estimate satisfies the following relationship:

[0061]

[0062] Where ω is the estimated value of the first rotation angular velocity vector, w 1,i is the weighting coefficient of the i-th first sensor, ω 1,i is the estimated value of the rotational angular velocity vector of the i-th first sensor.

[0063] In one possible implementation, the acquisition unit is further used to obtain normalized translational velocity vector estimates of M′ first sensors among the M first sensors, where 1≤M′≤M; the processing unit is further used to determine the second translational velocity vector estimate of the carrier based on the first translational velocity vector estimate and the normalized translational velocity vector estimate of the M′ first sensors.

[0064] In a possible implementation manner, the processing unit is specifically configured to determine the second translational velocity vector estimate based on the following relationship:

[0065]

[0066] in, is the estimated value of the second translational velocity vector, ω is the estimated value of the first rotational angular velocity vector, is the normalized translational velocity vector estimate of the i-th first sensor, r 1,i is the position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system, s i is the normalized parameter or scale factor of the translational velocity vector of the i-th first sensor, s i Determined by the first translational velocity vector estimate.

[0067] In a possible implementation, the second translational velocity vector estimate satisfies the following relationship:

[0068]

[0069] Among them, 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 kth iteration, ω is the estimated value of the first rotational angular velocity vector, is the normalized translational velocity vector estimate of the i-th first sensor, r 1,i is the position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system, s i,k is the normalization parameter or scale factor of the translational velocity vector of the i-th first sensor in the k-th iteration, s i,k Determined by the first translational velocity vector estimate or the translational velocity vector estimate of the carrier in the k-1th iteration.

[0070] In a possible implementation, the second translational velocity vector estimate satisfies the following relationship:

[0071]

[0072] Among them, 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 kth 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 rotation angular velocity vector, is the normalized translational velocity vector estimate of the i-th first sensor, r 1,i is the position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system, s i,k is the normalization parameter or scale factor of the translational velocity vector of the i-th first sensor in the k-th iteration, s i,k Determined by the first translational velocity vector estimate or the translational velocity vector estimate of the carrier in the k-1th iteration.

[0073] In one possible implementation, the normalized parameter or scale factor of the translational velocity vector of the i-th first sensor in the k-th iteration satisfies the following relationship:

[0074] s i,k =‖t k-1 +ω×r 1,i ‖

[0075] Among them, t k-1 is the estimated value of the translational velocity vector of the carrier in the k-1th iteration, and t0 is the estimated value of the first translational velocity vector.

[0076] In a possible implementation, the second translational velocity vector estimate satisfies the following relationship:

[0077]

[0078] in, is the estimated value of the second translational velocity vector is the estimated value of the translational velocity vector of the carrier corresponding to the i-th first sensor in the l-th iteration, ω is the estimated value of the first rotational angular velocity vector, is the normalized translational velocity vector estimate of the i-th first sensor, r 1,i The position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system, s i，l is the normalized parameter or scale factor of the translational velocity vector of the i-th first sensor in the l-th iteration, s i,l Estimated by the first translational velocity vector or or Sure.

[0079] In a possible implementation, the normalized parameter or scale scaling factor of the translational velocity vector of the i-th first sensor in the l-th iteration satisfies the following relationship:

[0080]

[0081] in, is the estimated value of the first translational velocity vector;

[0082] In a possible implementation, the processing unit is further configured to determine an estimated value of a second rotational angular velocity vector of the carrier based on the following relationship:

[0083]

[0084] Where ω′ is the estimated value of the second rotation angular velocity vector, v 2,j is the instantaneous velocity vector estimate of the j-th second sensor, is the estimated value of the second translational velocity vector, For [r 2,j ] × The inverse matrix, [r 2,j ] × For r 2,j The corresponding antisymmetric matrix, r 2,j is the position translation vector of the j-th second sensor's coordinate system relative to the carrier's coordinate system.

[0085] In a third aspect, the present application provides a communication device comprising at least one processor and a communication interface, wherein the communication interface is used to receive signals from other communication devices outside the communication device and transmit them to the at least one processor or send signals from the at least one processor to other communication devices outside the communication device, and the at least one processor is used to implement the method in the above-mentioned first aspect or any possible implementation of the first aspect through logic circuits or execution code instructions.

[0086] In a fourth aspect, the present application provides a computer-readable storage medium, which stores a computer program or instructions. When the computer program or instructions are executed by a communication device, the method of the above-mentioned first aspect or any possible implementation of the first aspect is implemented.

[0087] In a fifth aspect, the present application provides a computer program product, which includes a computer program or instructions. When the computer program or instructions are executed by a communication device, it implements the method in the above-mentioned first aspect or any possible implementation of the first aspect.

[0088] In a sixth aspect, the present application provides a chip comprising at least one processor and an interface;

[0089] The interface is configured to provide program instructions or data to the at least one processor;

[0090] The at least one processor is configured to execute the program line instructions to implement the method in the above-mentioned first aspect or any possible implementation manner of the first aspect.

[0091] In a seventh aspect, the present application provides a terminal, comprising any of the motion estimation devices provided in the second aspect, any of the communication devices provided in the third aspect, or any of the computer-readable storage media provided in the fourth aspect. Furthermore, the terminal may optionally be a vehicle, a drone, a robot, a smart home device, or a satellite.

[0092] The technical effects that can be achieved in any of the second to seventh aspects can refer to the description of the beneficial effects in the first aspect, and will not be repeated here. BRIEF DESCRIPTION OF THE DRAWINGS

[0093] Figure 1 A schematic diagram of the structure of an ego-motion estimation system provided in this application;

[0094] Figure 2 A schematic diagram of a flow chart of a motion estimation method provided in this application;

[0095] Figure 3A schematic diagram of a multi-sensor configuration in a vehicle-mounted system provided by this application;

[0096] Figure 4 A schematic diagram of a rotational angular velocity vector provided in this application;

[0097] Figure 5 A schematic diagram of a position translation vector provided in this application;

[0098] Figure 6 A schematic diagram of the transformation relationship between a carrier coordinate system and a sensor coordinate system provided in this application;

[0099] Figure 7 A schematic structural diagram of a motion estimation device provided in this application;

[0100] Figure 8 A schematic diagram of the structure of a chip provided in this application. DETAILED DESCRIPTION

[0101] The embodiments of the present application will be described in detail below with reference to the accompanying drawings.

[0102] See Figure 1 , Figure 1 10 is a structural diagram of a self-motion estimation system provided in an embodiment of the present application, which includes a first sensor 1010, a first motion sensing module 1011, a second sensor 1020, a second motion sensing module 1021 and a data processing module 1030.

[0103] The first sensor 1010 may be a visual sensor, such as a camera or a camera head, an infrared thermal imaging sensor, etc. The first sensor 1010 may provide visual measurement data, such as an image or video. The first motion sensing module 1011 is configured to determine motion measurement data, such as the rotational angular velocity vector of the sensor's motion and / or a normalized or scaled translational velocity vector, or an estimated translational velocity vector with complete scale information, based on the measurement data provided by the first sensor 1010. The second sensor 1020 may be a radar sensor, an ultrasonic sensor, an inertial measurement sensor, or a positioning sensor, such as a millimeter-wave radar, sonar, a lidar, an inertial measurement unit (IMU), or a global navigation satellite system (GNSS). The second sensor 1020 is configured to provide position measurement data and / or velocity measurement data, such as measurement data of position and / or radial velocity or a velocity projection component. The second motion sensing module 1021 is configured to determine motion measurement data, such as the instantaneous translational velocity vector of the sensor's motion, based on the position measurement data and / or velocity measurement data provided by the second sensor 1020. The data processing module 1030 is used 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.

[0104] 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 at the same or different positions of the carrier; 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; they can also exist independently of other modules, which is not limited in this application.

[0105] In one example, 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 a single processor system. In another example, the first sensor 1010 and the second sensor 1020 are each deployed on a single processor system; and the first motion sensing module 1011, the second motion sensing module 1021, and the data processing module 1030 are deployed on a single processor system.

[0106] The embodiments of the present application can be applied to multi-sensor systems in various carriers, including vehicle-mounted (such as cars, motorcycles or bicycles, etc.), airborne (such as drones, helicopters or jets or balloons), ship-mounted (ships, motorboats or ships, etc.), satellite-mounted (such as satellites) or intelligent entities (such as robots, etc.), etc.

[0107] Exemplarily, the carrier is a vehicle, which may carry at least one first sensor and at least one second sensor. For example, the vehicle may be equipped with one first sensor and one second sensor, or one first sensor and five second sensors, or six first sensors and five second sensors, etc.

[0108] Currently, there are many methods for estimating the motion speed of a sensor or a carrier. Taking a vehicle-mounted platform as an example, the following three sensors can be used for vehicle-mounted motion estimation.

[0109] 1. IMU

[0110] An IMU is a device that measures an object's three-axis attitude angle (or angular velocity) and acceleration. Typically, an IMU is equipped with a three-axis gyroscope and three-directional accelerometers to measure the object's angular velocity and acceleration in three-dimensional space, from which the object's velocity and attitude can be calculated.

[0111] 2. Radar Sensor

[0112] Radar sensors typically provide range, azimuth, and radial velocity measurements. Based on the azimuth and radial velocity measurements of a stationary target object, the sensor's instantaneous velocity relative to the ground can be determined using least squares methods or other methods. In particular, radar sensors can obtain a relatively accurate estimate of longitudinal velocity. Furthermore, based on these velocity estimates, the sensor's yaw rate can be estimated.

[0113] 3. Visual Sensor

[0114] Vision sensors typically provide two or more consecutive frames of images. Based on these two or more frames, estimates of translational and rotational velocities can be obtained based on sensor scale using optical flow, feature point mapping, or direct optimization of a target object function related to light intensity.

[0115] However, the above three sensors all have their own defects:

[0116] (1) The IMU’s motion velocity estimation is based on the accumulation of accelerometers. The measurement error will accumulate over time, resulting in an error accumulation problem. Additional calibration with other sensors is required. Moreover, the accuracy of the IMUs generally used in vehicles is too low. If a high-precision IMU is selected, the cost will be high.

[0117] (2) The accuracy of the sensor's lateral velocity estimate obtained by the radar sensor is low, and the sensor's pitch rate and roll rate estimates cannot be obtained.

[0118] (3) Visual sensors have a scaling problem. The depth information is coupled with the various components of the translational velocity. Usually, it is impossible to obtain an accurate depth estimate or an accurate translational velocity estimate, or only a scaled estimate of the translational velocity can be obtained.

[0119] To solve the above problems, an embodiment of the present application provides a motion estimation method for more accurately determining the estimated value of the translational velocity vector and the estimated value of the rotational angular velocity vector of the carrier, thereby achieving accurate motion estimation of the carrier.

[0120] It should be pointed out that in this application, the estimated value of the translational velocity vector can also be the estimated value of the translational displacement vector. The estimated value of the translational displacement vector can be the estimated value of the position offset vector between two frames, or it can be the product of the time difference between the two frames and the estimated value of the translational velocity vector.

[0121] See Figure 2 , Figure 2 This is a flow chart of a motion estimation method provided in an embodiment of the present application. The method may be executed by a sensor system, a fused perception system, or a planning / control system that integrates these systems, such as an assisted driving or autonomous driving system. Alternatively, the method may be executed by software or hardware (such as a data processing device that is wirelessly or wiredly connected to or integrated with the corresponding sensor).

[0122] The following different execution steps can be implemented in a centralized manner, or the following different execution steps can also be implemented in a distributed manner. The method includes but is not limited to the following steps:

[0123] Step 201: Obtain rotational angular velocity vector estimation values ​​of M first sensors and instantaneous velocity vector estimation values ​​of N second sensors.

[0124] Wherein, M first sensors and N second sensors are carried on one carrier, the number of the first sensors M is ≥1, and the number of the second sensors N is ≥1.

[0125] Specifically, the first sensor can be a visual sensor such as a camera, a camera, an infrared sensor or other imaging sensor, or an inertial measurement sensor such as an IMU, and the second sensor can be a radar sensor such as a millimeter wave radar, a lidar, or an ultrasonic sensor such as a sonar.

[0126] In one implementation, the M (M>1) first sensors may be of the same or different types. For example, the M first sensors include M1 visual sensors, such as cameras, and M2 inertial measurement sensors, such as IMUs, where M1+M2=M, M1≥0, and M2≥0. And / or, the N (N>1) second sensors may be of the same or different types. For example, the N second sensors include N1 radar sensors, such as millimeter-wave radars or lidars, and N2 ultrasonic sensors, such as sonars, where N1+N2=N, N1≥0, and N2≥0.

[0127] The M first sensors and N second sensors can be installed at the same position or different positions on the carrier. The following takes the vehicle as an example:

[0128] Example 1: A camera and a millimeter-wave radar are installed on the front end of the vehicle;

[0129] Example 2: One camera and one millimeter-wave radar are installed at the front end of the vehicle, and four millimeter-wave radars are installed at the four corners of the vehicle.

[0130] Example 3: One millimeter-wave radar is installed at the front end of the vehicle, four millimeter-wave radars are installed at the four corners, and six cameras are evenly installed on the vehicle.

[0131] In addition, IMU or GNSS can be further installed on the vehicle.

[0132] For example, a schematic diagram of a multi-sensor configuration in a vehicle-mounted system is shown as follows: Figure 3 As shown, the multi-sensor of the vehicle-mounted system may include 1 camera, 5 millimeter-wave radars and 1 IMU, wherein the installation position of the IMU may be close to the origin of the vehicle-mounted coordinate system (also known as the vehicle body coordinate system, carrier coordinate system), and the origin of the vehicle-mounted coordinate system may be located at the center of the rear axle of the vehicle body.

[0133] In Examples 1 to 3 above, the millimeter-wave radar can be replaced by a laser radar or ultrasonic sensor, the camera can be replaced by a camera or infrared sensor, or at least one laser radar can be added to the original one.

[0134] For example, in Example 3 above, two of the five millimeter-wave radars can be replaced with lidars, or all five millimeter-wave radars can be replaced with lidars. In addition, one to three lidars can be added to the original five millimeter-wave radars.

[0135] It should be understood that this is only an exemplary implementation of the first sensor and the second sensor installed on the vehicle provided in the present application, and it does not limit the present application in any way.

[0136] Specifically, obtaining the rotational angular velocity vector estimation values ​​of the M first sensors may be directly obtained from the sensors through a wired or wireless interface, wherein the rotational angular velocity vector estimation value may be obtained based on measurement data of the sensors through a motion or estimation algorithm or directly measured by the sensors;

[0137] Alternatively, the acquisition of the rotational angular velocity vector estimation values ​​of the M first sensors can be by directly obtaining the sensor measurement data from the sensor through a wired or wireless interface, and the rotational angular velocity vector estimation value is obtained according to the sensor measurement data through a motion or estimation algorithm or is directly obtained from the sensor measurement data.

[0138] As an implementation manner, the estimated value of the rotation angular velocity vector can be obtained by estimation based on the measurement data of the first sensor. In this case, the measurement data of the first sensor does not directly include the measurement value of the motion measurement of the first sensor.

[0139] Exemplarily, the first sensor is a visual sensor such as a camera, and the camera's raw measurement data is visual measurement data, such as an image or video. The camera's rotational angular velocity vector estimate can be determined based on the optical or geometric properties of the feature points, lines, planes, or regions in the image or video. For example, the camera's rotational angular velocity vector estimate can be obtained based on an 8-point method, a 5-point method, a homography, or an optical flow method. Obtaining the sensor's rotational angular velocity based on an image or video is conventional technology and will not be described in detail here.

[0140] As another implementation, the estimated value of the rotational angular velocity vector may be directly obtained from measurement data of the first sensor. In this case, the first sensor may directly measure data including the rotational angular velocity vector.

[0141] Exemplarily, the first sensor is an inertial measurement sensor such as an IMU, which can directly measure the rotational angular velocity vector.

[0142] It should be pointed out that the above-mentioned M first sensors may include first sensors of the same type or different types. Exemplarily, the M first sensors may include M1 visual sensors such as cameras or cameras and M2 inertial measurement sensors such as IMUs, wherein M1+M2=M, and M1≥0, M2≥0. Accordingly, the measurement data of the M1 visual sensors are processed to obtain the estimated values ​​of the rotational angular velocity vectors of the M1 visual sensors, and the estimated values ​​of the rotational angular velocity vectors of the M2 inertial measurement sensors are directly obtained and read from the measurement data of the M2 inertial measurement sensors.

[0143] Optionally, as an implementation method, the rotation angular velocity vector may be a three-dimensional vector, ω=[ω x ω y ω z ] T ,like Figure 4 shown.

[0144] Alternatively, as another implementation, the first sensor or the carrier moves in a plane, such as the ground or a plane track. The rotational angular velocity vector can be expressed as ω = [0 0 ω z ] T , at this time, the rotation angular velocity vector can be simplified to z express.

[0145] It should be pointed out that the above-mentioned rotational angular velocity vector estimation value is obtained directly from the measurement data of the sensor or obtained through motion estimation based on the measurement data of the sensor. The rotational angular velocity vector estimation value of the sensor in the carrier coordinate system can be obtained according to the transformation relationship between the sensor coordinate system and the carrier coordinate system.

[0146] Specifically, the instantaneous velocity vector estimation values ​​of the N second sensors may be obtained directly from the sensors through a wired or wireless interface, wherein the instantaneous velocity vector estimation values ​​may be obtained through a motion or estimation algorithm based on measurement data of the sensors;

[0147] Alternatively, the instantaneous velocity vector estimation values ​​of the N second sensors may be obtained by directly obtaining sensor measurement data from the sensors through a wired or wireless interface, and the instantaneous velocity vector estimation value is obtained through a motion or estimation algorithm based on the sensor measurement data.

[0148] As an implementation manner, the instantaneous velocity vector estimation value can be obtained by estimation based on the measurement data of the second sensor. In this case, the measurement data of the second sensor does not directly include the measurement value of the motion measurement of the second sensor.

[0149] Exemplarily, the second sensor is a millimeter-wave radar, a lidar, or an ultrasonic sensor such as a sonar. The measurement data of the second sensor may include position and radial velocity, or angle and radial velocity. The instantaneous velocity vector estimate can be determined based on the measurement data of the stationary target using an estimation method such as the least squares method, the orthogonal distance regression method, or the minimum mean square error criterion. Furthermore, the instantaneous velocity vector estimate can also be determined based on multiple position measurement data of the second sensor and the measurement data of the stationary target. This is not limited in the present embodiment.

[0150] It should be noted that the number of rotational angular velocity vector estimation values ​​obtained by the carrier from the first sensor can be less than or equal to the number of first sensors actually in the carrier, and the number of instantaneous velocity vector estimation values ​​obtained by the carrier from the second sensor can be less than or equal to the number of second sensors actually in the carrier.

[0151] In the first exemplary embodiment, six first sensors and three second sensors are carried on the carrier, and the carrier can obtain the rotational angular velocity vector estimation values ​​of the six first sensors and the instantaneous velocity vector estimation values ​​of the three second sensors.

[0152] In the second exemplary embodiment, six first sensors and three second sensors are carried on the carrier, and the carrier can obtain the rotational angular velocity vector estimation values ​​of the four first sensors and the instantaneous velocity vector estimation values ​​of the two second sensors.

[0153] Step 202 : Determine a first translational velocity vector estimate of the carrier based on the instantaneous velocity vector estimates of the N second sensors, external parameters of the N second sensors, and a first rotational angular velocity vector estimate of the carrier where the N second sensors are located.

[0154] 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 position translation vectors of the coordinate system origins of the N second sensors relative to the carrier coordinate system origin. Exemplarily, for any second sensor, the position translation vector of the second sensor relative to the carrier coordinate system is used to translate the coordinate system origin of the second sensor to be consistent with the carrier coordinate system origin.

[0155] For example, five second sensors may be installed on a vehicle. The five second sensors may include millimeter wave radars, laser radars, or ultrasonic sensors. Figure 5 As shown, the five second sensors are located at different positions on the vehicle, and the position translation vectors of the five second sensors relative to the origin of the vehicle coordinate system are r 21 ,r 22 ,…,r 25 The estimated values ​​of the instantaneous velocity vectors of the five second sensors are v21 ,v 22 ,…,v 25 . Usually r 21 , r 22 ,…,r 25 When they are different, v 21 , v 22 ,…,v 25 Also different from each other.

[0156] Specifically, the first translational velocity vector estimate of the carrier is determined based on the instantaneous velocity vector estimates of the N second sensors, the external parameters of the N second sensors, and the first rotational angular velocity vector estimate of the carrier. The first translational velocity vector estimate of the carrier may be obtained based on the relationship between the translational 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 determined from the instantaneous velocity vector estimates of the N second sensors and the external parameters of the N second sensors, and the rotational angular velocity is determined from the first rotational angular velocity vector estimate of the carrier.

[0157] Specifically, the relationship between the rigid body's translational velocity vector, instantaneous velocity vector, rotational angular velocity vector and position translation vector is t = v - ω × r, or t = v + r × ω, where t is the rigid body's translational velocity vector, v is the rigid body's instantaneous velocity vector, ω is the rigid body's rotational angular velocity vector, r is the position translation vector, and × represents the cross product of the vectors.

[0158] Based on the relationship between the rigid body's translational velocity vector, instantaneous velocity vector, rotational angular velocity vector, and position translation vector, there may be multiple deformation relationship formulas, and the multiple deformation relationship formulas can all obtain the estimated value of the first translational velocity vector of the carrier.

[0159] Exemplarily, the first translational velocity vector estimate may be determined based on the following relationship:

[0160]

[0161] or

[0162]

[0163] in, 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, v2 is the estimated value of the instantaneous velocity vector of the second sensor, r2 is the position translation vector of the second sensor relative to the origin of the carrier coordinate system, and × represents the cross product of the vectors.

[0164] Specifically, the above relationship can be

[0165] Where ω=[ωx ω y ω z ] T .

[0166] Specifically, the above relationship can also be

[0167] Where r2=[r x,2 r y,2 r z,2 ] T .

[0168] Of course, other methods are also possible and are not limited in the embodiments of the present application.

[0169] Specifically, r2 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.

[0170] As an implementation manner, the first translational velocity vector estimate may be determined based on the following relationship:

[0171]

[0172] in, is the estimated value of the first translational velocity vector of the carrier, t 2,j is an estimated value of the translational velocity vector of the carrier determined according to the estimated value of the instantaneous velocity vector of the j-th second sensor and its external parameters and the estimated value of the first rotational angular velocity vector of the carrier.

[0173] As an implementation manner, the first translational velocity vector estimation value is determined based on a minimum mean square error (MMSE) or a least square (LS) method.

[0174] In a first implementation, the first translational velocity vector estimate is a weighted sum of N translational velocity vector estimates of the carrier, wherein the N translational velocity vector estimates of the carrier are respectively determined based on the instantaneous velocity vector estimates of the N second sensors and their external parameters and the first rotational angular velocity vector estimate of the carrier.

[0175] In a specific implementation, the estimated value of the first translational velocity vector of the carrier satisfies the following relationship:

[0176]

[0177] in, is the estimated value of the first translational velocity vector of the carrier, t 2,jis the estimated value of the translational velocity vector of the carrier determined based on the estimated value of the instantaneous velocity vector of the j-th second sensor and its external parameters and the estimated value of the first rotational angular velocity vector of the carrier, w 2,j is the weighting coefficient or weighting coefficient matrix corresponding to the j-th second sensor.

[0178] Specifically, the weighting coefficient w 2,j According to t 2,j The probability density function or statistical characteristics or covariance matrix of the estimation error or measurement error is determined. For example, w 2,j According to t 2,j The covariance matrix of the estimation error or measurement error is determined, in, P 2,j t 2,j The covariance of the estimation error or measurement error.

[0179] Exemplarily, the estimated value of the translational velocity vector of the carrier is determined based on the estimated value of the instantaneous velocity vector of the j-th second sensor and its external parameters and the estimated value of the first rotational angular velocity vector of the carrier, and can be determined based on the following relationship:

[0180] t 2,j =v 2,j -ω×r 2,j

[0181] Accordingly, as a specific example of the first implementation manner, the first translational velocity vector estimation value may be determined according to the following relationship:

[0182]

[0183] in, 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,j is the instantaneous velocity vector estimate of the jth second sensor, r 2,j is the position translation vector of the jth second sensor’s coordinate system relative to the carrier’s coordinate system, w 2,j is the weighting coefficient or weighting coefficient matrix corresponding to the j-th second sensor.

[0184] Specifically, the weighting coefficient w 2,j According to v 2,j and / or r 2,j Measurement error or estimation error determination.

[0185] For example, w 2,j According to v 2,j and r 2,j The measurement error or estimation error determines, for example, w 2,jSatisfies the following relationship:

[0186] in,

[0187] in, v 2,j The covariance matrix of the measurement error or estimation error, For r 2,j The covariance matrix of the measurement error or estimation error. Ω is obtained based on the rotational angular velocity vector of the carrier, specifically, Where ω=[ω x ω y ω z ] T .

[0188] In the second implementation method, the first translational velocity vector estimate is the average of N translational velocity vector estimates of the carrier, wherein the N translational velocity vector estimates of the carrier are respectively determined based on the instantaneous velocity vector estimates of the N second sensors and their external parameters and the first rotational angular velocity vector estimate of the carrier.

[0189] In a specific implementation, the estimated value of the first translational velocity vector of the carrier satisfies the following relationship:

[0190]

[0191] in, is the estimated value of the first translational velocity vector of the carrier, t 2,j is an estimated value of the translational velocity vector of the carrier determined according to the estimated value of the instantaneous velocity vector of the j-th second sensor and its external parameters and the estimated value of the first rotational angular velocity vector of the carrier.

[0192] Exemplarily, the estimated value of the translational velocity vector of the carrier is determined based on the estimated value of the instantaneous velocity vector of the j-th second sensor and its external parameters and the estimated value of the first rotational angular velocity vector of the carrier, which can be based on t 2,j =v 2,j -ω×r 2,j Accordingly, as a specific example of the second implementation, the estimated value of the first translational velocity vector satisfies the following relationship:

[0193]

[0194] in, 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,j is the instantaneous velocity vector estimate of the jth second sensor, r 2,jis the position translation vector of the j-th second sensor's coordinate system relative to the carrier's coordinate system.

[0195] It should be noted that in the first implementation, the first translational velocity vector estimate is the weighted sum of the N translational velocity vector estimates of the carrier, while in the second implementation, the first translational velocity vector estimate is the average of the N translational velocity vector estimates of the carrier. It can also be understood that the second implementation is a special form of the first implementation. If the weighting coefficients of the second sensors in the first implementation are the same, the second implementation can be used. Furthermore, this description also applies to the relationship between the weighted sum of multiple sensor estimates and the average of the multiple sensor estimates in other implementations.

[0196] As another implementation method, N is equal to 1, and the first translational velocity vector estimate is determined based on the first rotational angular velocity vector estimate, the instantaneous velocity vector estimate of the second sensor, and the position translation vector of the coordinate system of the second sensor relative to the carrier coordinate system. The specific value can be obtained by referring to the above relationship.

[0197] In addition, the estimated value of the carrier's translational velocity vector t2 can be determined by the orthogonal distance regression (ODR) method based on the estimated value of the first rotational angular velocity vector of the carrier, the estimated value of the instantaneous velocity vector of the j-th second sensor, and the position translation vector of the coordinate system of the j-th second sensor relative to the coordinate system of the carrier. ,j .

[0198] In the embodiment of the present application, the first rotational angular velocity vector estimation value is determined based on the rotational angular velocity vector estimation values ​​of M first sensors, where M≥1.

[0199] As an implementation manner, the first rotation angular velocity vector estimation value may be determined based on the following relationship:

[0200] ω=ω 1,i ,i=1,…,M,M≥1

[0201] Where ω is the estimated value of the first rotation angular velocity vector, ω 1,i is the estimated value of the rotational angular velocity vector of the i-th first sensor.

[0202] As an implementation manner, the first rotation angular velocity vector estimation value is determined based on minimum mean square error or least square method.

[0203] In a first implementation, the first rotational angular velocity vector estimation value is a weighted sum of M rotational angular velocity vector estimation values ​​of the carrier, wherein the M rotational angular velocity vector estimation values ​​of the carrier are respectively determined according to the rotational angular velocity vector estimation values ​​of the M first sensors.

[0204] In a specific implementation, the estimated value of the first rotational angular velocity vector of the carrier satisfies the following relationship:

[0205]

[0206] Where ω is the estimated value of the first rotational angular velocity vector of the carrier, ω 1,i To obtain the estimated value of the rotational angular velocity vector of the carrier according to the estimated value of the rotational angular velocity vector of the i-th first sensor, w 1,i is the weighting coefficient or weighting coefficient matrix corresponding to the i-th first sensor.

[0207] Specifically, the weighting coefficient w 1,i According to ω 1,i The probability density function or statistical characteristics or covariance matrix of the estimation error or measurement error is determined. For example, w 1,i According to ω 1,i The covariance matrix of the estimation error or measurement error is determined as follows: in, P 1,i ω 1,i The covariance of the estimation error or measurement error.

[0208] In a second implementation, the first rotational angular velocity vector estimation value is an average of M rotational angular velocity vector estimation values ​​of the carrier, wherein the M rotational angular velocity vector estimation values ​​of the carrier are respectively determined according to the rotational angular velocity vector estimation values ​​of the M first sensors.

[0209] In a specific implementation, the estimated value of the first rotational angular velocity vector of the carrier satisfies the following relationship:

[0210]

[0211] Where ω 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 according to the estimated value of the rotational angular velocity vector of the i-th first sensor.

[0212] As another implementation, M is equal to 1, and the first rotation angular velocity vector estimation value is determined according to the rotation angular velocity vector estimation value of the first sensor, which can be specifically obtained by referring to the above relationship.

[0213] It should be noted that the above-mentioned estimated angular velocity vector of the first sensor, the estimated instantaneous velocity vector of the second sensor, the estimated first angular velocity vector of the carrier, and the estimated first translational velocity vector of the carrier are all defined relative to the carrier coordinate system. However, in actual applications, sensor measurement data is often defined relative to the sensor coordinate system. Therefore, it is often more convenient to define the motion velocity vector obtained from the first or second sensor, including the angular velocity vector and translational velocity vector or instantaneous velocity vector, relative to the sensor. In this case, it is necessary to derive the angular velocity vector and translational velocity vector or instantaneous velocity vector relative to the carrier coordinate system based on the sensor's external parameters relative to the carrier coordinate system.

[0214] Without loss of generality, Figure 6 As shown, the transformation relationship between the carrier coordinate system and the sensor coordinate system can generally be determined using the sensor's extrinsic parameters. These extrinsic parameters can include a rotation parameter and a position translation vector of the sensor coordinate system relative to the carrier coordinate system. Based on the rotation parameter, the orientation of the sensor coordinate system can be rotated to align with that of the carrier coordinate system. Based on the position translation vector, the origin of the sensor coordinate system can be translated to align with the origin of the carrier coordinate system.

[0215] The position translation vector of the sensor coordinate system relative to the carrier coordinate system can be as follows Figure 6 The vector r can also be Figure 5 r in 21 ,r 22 ,…,r 25 ; As mentioned above r 2,j ,j=1,…,N,N≥1.

[0216] Rotation parameters are used to represent the rotation between the carrier coordinate system and the sensor coordinate system; specifically, they can be represented by quaternions, rotation matrices, Euler angles, etc. Quaternions, rotation matrices, Euler angles, etc. can be converted into each other. For example, a rotation matrix can be derived from quaternions, or from Euler angles. For example, the sensor coordinate system and the carrier coordinate system can be aligned in orientation, in which case the rotation parameter is the identity matrix.

[0217] In an embodiment of the present application, a motion velocity vector relative to the carrier coordinate system is further obtained based on the external parameters of the sensor and the motion velocity vector relative to the sensor coordinate system, wherein the motion velocity vector may include one or more of the rotational angular velocity, the translational velocity vector, and the instantaneous motion velocity vector.

[0218] Specifically, the instantaneous velocity vector relative to the carrier coordinate system may be obtained based on external parameters of the second sensor and the instantaneous velocity vector relative to the second sensor coordinate system, wherein the external parameters include rotation parameters.

[0219] For example, the rotation parameter can be a rotation matrix, and the instantaneous translational velocity vector relative to the carrier coordinate system can be determined by the following relationship:

[0220] v 2,j =R 2,j v′ 2,j

[0221] Among them, 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 The estimated instantaneous velocity vector of the j-th 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.

[0222] Specifically, the rotational angular velocity vector relative to the carrier coordinate system may be obtained based on external parameters of the first sensor and the rotational angular velocity vector relative to the first sensor coordinate system, wherein the external parameters include rotation parameters.

[0223] For example, the rotation parameter may be a rotation matrix, and the rotational angular velocity vector relative to the carrier coordinate system may be determined by the following relationship:

[0224] ω 1,i =R 1,i ω′ 1,i

[0225] Among them, ω 1,i is the estimated value of the angular velocity vector of the i-th first sensor relative to the carrier coordinate system, ω′ 1,i is the estimated value of the rotational angular velocity vector of the i-th 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.

[0226] It should be noted that the rotation parameter can be a fixed value or can be estimated using an online algorithm during motion estimation; the position translation vector can be a fixed value or can be estimated using an online algorithm during motion estimation. This embodiment of the present application does not limit this.

[0227] Based on the above transformation relationship, the first rotational angular velocity vector estimation value of the carrier can be determined according to the external parameters of the sensor and the rotational angular velocity vector estimation values ​​of the M first sensors. The external parameters of the sensor may include the rotation parameters of the sensor.

[0228] Specifically, the first rotational angular velocity vector estimation value of the carrier is determined according to the rotational angular velocity vector estimation values ​​of the M first sensors.

[0229] As an implementation manner, the first rotation angular velocity vector estimation value may be determined based on the following relationship.

[0230] ω=R 1,i ω′ 1,i

[0231] Where ω is the estimated value of the first rotation angular velocity vector, ω′ 1,i is the estimated value of the angular velocity vector of the i-th first sensor relative to the sensor coordinate system, R 1,i is the rotation parameter of the coordinate system of the i-th first sensor relative to the carrier coordinate system.

[0232] As an implementation manner, the first rotational angular velocity vector estimate value of the carrier is determined based on the rotational angular velocity vector estimate value of each first sensor of the M first sensors relative to the sensor coordinate system and the rotation parameters of each first sensor.

[0233] In one example, the first rotational angular velocity vector estimate is determined based on the following relationship:

[0234]

[0235] Where ω is the estimated value of the first rotational angular velocity vector of the carrier, ω′ 1,i is the estimated value of the angular velocity vector of the i-th first sensor relative to the sensor coordinate system, w 1,i is the weighting coefficient or weighting coefficient matrix corresponding to the i-th first sensor, R 1,i is the rotation parameter of the coordinate system of the i-th first sensor relative to the carrier coordinate system.

[0236] In yet another example, the first rotational angular velocity vector estimate is determined based on the following relationship:

[0237]

[0238] Where ω is the estimated value of the first rotational angular velocity vector of the carrier, ω′ 1,i is the estimated value of the angular velocity vector of the i-th first sensor relative to the sensor coordinate system, R 1,i is the rotation parameter of the coordinate system of the i-th first sensor relative to the carrier coordinate system.

[0239] In the embodiments of the present application, the estimated value of the first rotational angular velocity vector of the carrier can be accurately determined based on the estimated value of the rotational angular velocity vector of at least one first sensor. Combined with the estimated value of the instantaneous velocity vector of the second sensor, the estimated value of the first translational velocity vector of the carrier can be accurately obtained. Therefore, the method of the embodiments of the present application can compensate for the motion of the carrier, facilitate separation of moving and stationary targets, and facilitate positioning and tracking of the carrier's motion.

[0240] Optionally, in order to further improve the accuracy of the estimated value of the translational velocity vector of the carrier, the embodiment of the present application may further include the following steps 203 and 204.

[0241] Step 203: Obtain normalized translational velocity vector estimates of M′ first sensors among the M first sensors.

[0242] Specifically, the M first sensors include M′ first sensors, where M′≤M. The M′ first sensors may be visual sensors such as cameras or video cameras.

[0243] In a specific example, the M first sensors include M′ visual sensors such as cameras or video cameras and MM′ inertial measurement sensors such as IMUs.

[0244] As an implementation method, the M′ first sensors can be visual sensors such as cameras or cameras. The normalized translational velocity vector estimates of the M′ first sensors can be obtained based on the image or video obtained by the camera or camera, and can be determined based on the optical or geometric properties of the data according to the feature points, lines, planes or areas therein, for example, based on the 8-point method or the 5-point method or homography or optical flow method, etc., which is not limited in the embodiments of the present application.

[0245] The normalized translational velocity vector estimate of the first sensor is understood to be the translational velocity vector estimate scaled according to the scale determined by the first sensor. That is, the normalized translational velocity vector estimate of the first sensor is proportional to the translational velocity vector estimate of the first sensor. It should be understood that the translational velocity vector estimate of the first sensor is the translational velocity vector estimate of the actual motion of the first sensor.

[0246] For example, in the coordinate system of the first sensor, the normalized translational velocity vector estimate of the first sensor can be expressed as The estimated value of the translational velocity vector of the first sensor can be expressed as v1′, and the two conform to the relationship:

[0247]

[0248] Where v1′=[v′ 1x ,v′ 1y ,v′ 1z ] T , s is the normalization parameter or scale factor of the translational velocity vector estimate of the first sensor.

[0249] Specifically, the normalization parameter or scale expansion factor may be the amplitude, norm, or modulus of the translational velocity vector estimate, or a component of the translational velocity vector estimate, such as the z-axis component.

[0250] Taking the optical flow method as an example, the normalized translational velocity vector estimate can be determined based on the following relationship:

[0251]

[0252]

[0253] Where u, v are the optical flow components on the image plane, s1 = [-f 0 x] T , s2=[0 -fy] T , × represents the cross product of the vector, f is the focal length of the camera, x, y are the pixel positions on the image plane, x∈[p x -w x ,p x +w x ],y∈[p y -w y ,p y +w y ]; where (p x ,p y ) is the center position, w x and w y is a non-negative integer, w x =0,1,2,3,4…;w y =0,1,2,3,4…. Z′ is the relative depth of the target point corresponding to the pixel, and ω are the normalized translational velocity vector and rotational angular velocity vector relative to the sensor coordinate system.

[0254] The normalized translational velocity vector estimate is and relative depth Z′, satisfying the following relationship

[0255]

[0256] Or, equivalently

[0257] Z=sZ′

[0258] Where 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, and t′ z is the z-axis component of t′, s = t′ z is the scale expansion factor. It should be noted that the scale expansion factor s is not limited to t′ z , other values ​​can be selected as needed, for example, the scale expansion factor s is the norm or amplitude of t′, such as s = ‖t′‖.

[0259] It should be noted that obtaining the normalized translational velocity vector estimation values ​​of M′ first sensors may further include obtaining the normalized translational velocity vector estimation value relative to the carrier coordinate system based on the external parameters of the first sensor, wherein the external parameters of the first sensor include the rotation parameters of the first sensor coordinate system relative to the carrier coordinate system.

[0260] For example, the normalized translational velocity vector estimate relative to the vehicle coordinate system is for Wherein, R1 is the rotation matrix of the coordinate system of the first sensor relative to the coordinate system of the carrier.

[0261] It should be noted that the above rotation transformation is an orthogonal transformation matrix and does not change the normalization parameter. For the convenience of description, the normalization parameter can be used as an example.

[0262] Step 204 : Determine a second estimated translational velocity vector of the carrier based on the first estimated translational velocity vector and the normalized estimated translational velocity vectors of the M′ first sensors.

[0263] In the present application, based on the determination of the first translational velocity vector estimate value, the estimation accuracy of the translational velocity vector of the carrier can be further improved based on the normalized translational velocity vector estimate values ​​of M′ first sensors. The further updated translational velocity vector estimate value of the carrier is referred to as the second translational velocity vector estimate value.

[0264] Specifically, the estimated value of the second translational velocity vector of the carrier may be determined according to the following normalized relationship among the translational velocity vector of the first sensor, the position translation vector of the first sensor, the translational velocity vector of the carrier, and the rotational angular velocity vector of the carrier:

[0265]

[0266] Where t is the translational velocity vector of the carrier, ω is the estimated value of the rotational angular velocity vector of the carrier, is the normalized translational velocity vector estimate of the first sensor, r1 is the position translation vector of the first sensor relative to the carrier coordinate system, and s is the normalized parameter of the translational velocity vector of the first sensor.

[0267] As an implementation manner, the estimated value of the second translational velocity vector of the carrier can be determined based on the relationship between 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:

[0268] Specifically, the estimated value of the second translational velocity vector of the carrier is determined according to the following relationship:

[0269]

[0270] in, is 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 normalized translational velocity vector estimate 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 normalized parameter of the translational velocity vector of the i-th first sensor.

[0271] Specifically, the estimated value of the second translational velocity vector of the carrier is determined according to the following relationship:

[0272]

[0273] in, is the estimated value of the second translational velocity vector of the carrier, 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, is the normalized translational velocity vector estimate 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 normalized parameter of the translational velocity vector of the i-th first sensor. Weighting coefficient matrix w′ 1，i Can be based on The covariance matrix of the estimated error is determined similarly to the previous method and will not be described in detail here.

[0274] Specifically, the estimated value of the second translational velocity vector of the carrier is also determined according to the following relationship:

[0275]

[0276] Where ω is the estimated value of the first rotational angular velocity vector of the carrier, is the normalized translational velocity vector estimate 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 normalized parameter of the translational velocity vector of the i-th first sensor.

[0277] Optional, s i It can be determined based on the following relationship:

[0278]

[0279] in is the estimated value of the first translational velocity vector of the carrier.

[0280] As another implementation, the estimated value of the second translational velocity vector of the carrier can be determined based on the relationship between the translational velocity vector of the carrier, the first rotational angular velocity vector of the carrier, the translational velocity vector of the first sensor, and the position translation vector of the first sensor relative to the carrier coordinate system, which conforms to the following relationship:

[0281]

[0282] Among them, the second translational velocity vector estimate of the carrier is given by the above Determine. ω is the estimated value of the first rotational angular velocity vector of the carrier, is the normalized translational velocity vector estimate 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 normalized parameter of the translational velocity vector of the i-th first sensor, s i It can be determined based on the following relationship:

[0283]

[0284] in is the estimated value of the first translational velocity vector of the carrier.

[0285] Furthermore, based on either of the two aforementioned implementations, the second estimated translational velocity vector can be determined iteratively. Specifically, each iteration can yield an estimated translational velocity vector of the carrier, with the estimated translational velocity vector of the carrier obtained from the last iteration serving as the second estimated translational velocity vector of the carrier. The iterative implementation can further utilize the estimated translational velocity vector of the sensor and the position translation vector, thereby improving the accuracy of the estimation of the translational velocity vector of the carrier.

[0286] Specifically, in the first iterative method, the estimated value of the second translational velocity vector of the carrier may be obtained according to the following relationship:

[0287]

[0288] Among them, 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 as the second estimated value of the translational velocity vector of the carrier. ω is the estimated value of the first rotational angular velocity vector of the carrier, is the normalized translational velocity vector estimate of the first sensor, r1 is the position translation vector of the first sensor relative to the carrier coordinate system, s k is the normalized parameter of the translational velocity vector of the first sensor in the kth iteration. Specifically, s k It can be obtained according to the following relationship:

[0289] s k =‖t k-1 +ω×r1‖

[0290] Among them, t k-1 is the estimated value of the translational velocity vector of the carrier obtained in the k-1th iteration, ω is the estimated value of the first rotational angular velocity vector, and r1 is the position translation vector of the first sensor relative to the carrier coordinate system.

[0291] As a specific implementation, each iteration may be to determine the estimated translational velocity vector of the carrier at the kth iteration based on the normalized translational velocity vector estimates of the M′ first sensors, the normalized parameters of the translational velocity vectors of the M′ first sensors at the k-1th iteration, the position translation vectors of the M′ first sensors relative to the carrier coordinate system, and the estimated first rotational angular velocity vector. The normalized parameters of the translational velocity vectors of the M′ first sensors at the kth iteration may be determined based on the estimated translational velocity vector of the carrier obtained at the k-1th iteration. Through the iterative process, the estimated translational velocity vector of the carrier that meets the preset conditions obtained at the Kth iteration (the final iteration) is used as the second estimated translational velocity vector of the carrier.

[0292] As an implementation method, the second translational velocity vector estimate of the carrier may be determined based on the relationship between the translational velocity vector of the first sensor, the position translation vector of the first sensor, the translational velocity vector of the carrier, and the rotational angular velocity vector of the carrier, specifically including:

[0293] The estimated value of the second translational velocity vector of the carrier is determined according to the following relationship:

[0294]

[0295] Among them, 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 rotation angular velocity vector, is the normalized translational velocity vector estimate 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,k is the normalized parameter of the translational velocity vector of the i-th first sensor in the k-th iteration, s i,k Determined by the first translational velocity vector estimate or the translational velocity vector estimate of the carrier in the k-1th iteration, w′ 1,i,k is the weighting coefficient of the i-th first sensor in the k-th iteration.

[0296] w′ 1,i,k In each iteration, it can take a fixed value or be determined according to a preset algorithm. 1,i,k According to s i ·v 1，i -ω×r 1，i The covariance matrix of the estimated error is determined similarly to the previous method and will not be described in detail here.

[0297] Alternatively, the second translational velocity vector estimate of the carrier is Determined according to the following relationship:

[0298]

[0299] Among them, 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 rotation angular velocity vector, is the normalized translational velocity vector estimate 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,k is the normalized parameter of the translational velocity vector of the i-th first sensor in the k-th iteration, s i,k Determined by the first translational velocity vector estimate or the translational velocity vector estimate of the carrier in the k-1th iteration.

[0300] s i,k It is determined based on the first translational velocity vector estimate or the translational velocity vector estimate of the carrier in the k-1th iteration. There are two possible cases:

[0301] k is equal to 1, that is, in the first iteration, si,1 It can be determined by the first translational velocity vector estimate according to the following relationship:

[0302]

[0303] in, is the estimated value of the first translational velocity vector, ω 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 carrier coordinate system.

[0304] k is greater than 1, that is, in the next few iterations, s i,k It can be determined by the estimated value of the translational velocity vector of the carrier in the k-1th iteration according to the following relationship:

[0305] s i,k =‖t k-1 +ω×r 1,i ‖

[0306] Among them, t k-1 is the estimated value of the translational velocity vector of the carrier in the k-1th 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 i-th first sensor relative to the carrier coordinate system.

[0307] Optionally, as a specific implementation, M′ is equal to 1, and the estimated value of the carrier's translational velocity vector at the k-th iteration can be determined based on the normalized estimated value of the translational velocity vector of the first sensor, the normalized parameter of the translational velocity vector of the first sensor at the k-1-th iteration, the position translation vector of the first sensor's coordinate system relative to the carrier's coordinate system, and the estimated value of the first rotational angular velocity vector. Specifically, this can be determined with reference to the above relationship. The normalized parameter of the translational velocity vector of the first sensor at the k-th iteration is determined based on the estimated value of the first translational velocity vector or the estimated value of the carrier's translational velocity vector at the k-1-th iteration.

[0308] It should be noted that the iteration termination condition of the first iteration method can be set to that the vector distance between the second translational velocity vector estimate and the first translational velocity vector estimate is no greater than a first preset threshold or limit. This is equivalent to determining the vector distance between the translational velocity vector estimate of the carrier obtained at the kth iteration and the first translational velocity vector estimate. If the vector distance is greater than the first preset threshold or limit, then further performing the k+1th iteration. If the vector distance is not greater than the first preset threshold or limit, then determining that the iteration is terminated. At this point, the translational velocity vector estimate of the carrier obtained at the kth iteration (i.e., the Kth iteration, or the last iteration) can be referred to as the second translational velocity vector estimate.

[0309] Exemplarily, the second translational velocity vector estimate and the first translational velocity vector estimate satisfy the following relationship:

[0310]

[0311] Wherein, Threshold1 is a first preset threshold or limit.

[0312] In addition, the termination condition for the first iteration method can be set to reaching the maximum number of iterations. This is equivalent to setting the maximum number of iterations to K. That is, a total of K iterations are performed, and the estimated translational velocity vector of the carrier obtained at the Kth iteration (i.e., the last iteration) is referred to as the second estimated translational velocity vector. For example, the maximum number of iterations K can be set to 20.

[0313] In the first iterative method, in each iteration, the estimated translational velocity vector of the carrier is determined based on the normalized translational velocity vector estimates from the multiple first sensors. Using the parameters of the multiple first sensors as input for a single iteration helps improve the accuracy of the estimation of the second translational velocity vector.

[0314] Specifically, in the second iterative method, the estimated value of the carrier's translational velocity vector corresponding to the i-th first sensor is determined based on the estimated value of the carrier's translational velocity vector corresponding to the i-th first sensor, the normalized parameter of the translational velocity vector of the i-th first sensor, and the position translation vector of the i-th first sensor's coordinate system relative to the carrier's coordinate system. The normalized parameter of the i-th first sensor's translational velocity vector is determined based on the estimated value of the carrier's translational velocity vector corresponding to the (i-1)-th first sensor.

[0315] Equivalently, the second iterative method is to determine the normalized parameter of the translational velocity vector of the subsequent first sensor based on the estimated value of the translational velocity vector of the carrier corresponding to the previous first sensor, and then determine the estimated value of the translational velocity vector of the carrier corresponding to the subsequent first sensor. It is mainly applicable to the case where M′ is greater than 1.

[0316] It can be the estimated value of the translational velocity vector of the carrier obtained according to the following relationship:

[0317]

[0318] in, is the estimated value of the translational velocity vector of the carrier corresponding to the i-th first sensor, is the estimated value of the second translational velocity vector of the carrier ω is the estimated value of the first rotation angular velocity vector, is the normalized translational velocity vector estimate of the i-th first sensor, r 1,iThe position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system, s i is the normalized parameter of the translational velocity vector of the i-th first sensor, s i Estimated by the first translational velocity vector or Sure.

[0319] Specifically, i It can be obtained according to the following relationship.

[0320]

[0321] in, is the estimated value of the translational velocity vector of the carrier corresponding to the i-1th first sensor, is the estimated value of the first translational velocity vector; ω 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 carrier coordinate system.

[0322] In addition, the embodiment of the present application may perform multiple iterations, each iteration including iterations between M′ first sensors. The estimated value of the translational velocity vector of the carrier may be obtained according to the following relationship:

[0323]

[0324] in, is the estimated value of the translational velocity vector of the carrier corresponding to the i-th first sensor in the l-th iteration. It can also be understood that in the L-th iteration (the last iteration), t i,L ′ is the estimated value of the second translational velocity vector ω is the estimated value of the first rotation angular velocity vector, is the normalized translational velocity vector estimate of the i-th first sensor, r 1,i The position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system, s i,l is the normalized parameter of the translational velocity vector of the first sensor i in the lth iteration, s i，l Estimated by the first translational velocity vector or or Sure.

[0325] Specifically, i,l It can be obtained according to the following relationship.

[0326]

[0327] in, is the estimated value of the translational velocity vector of the carrier corresponding to the i-1th first sensor in the lth iteration, is the estimated value of the first translational velocity vector; ω is the estimated value of the first rotation angular velocity vector, r 1,i is the position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system.

[0328] It should be noted that the last iteration in the previous round of iterations is the previous iteration of the first iteration in the next round of iterations, or in other words, the M′th iteration in the l-1th round of iterations is the previous iteration of the first iteration in the l-1th round of iterations. Of course, it is also understandable that the kth iteration can be the M′th iteration in the l-1th round of iterations, and the k+1th iteration can be the first iteration in the lth round of iterations. Furthermore, the parameters of the next round of iterations can be determined by the parameters of the previous round of iterations. Specifically, the normalized parameters of the translational velocity vector of the first first sensor in the lth round of iterations are determined by the estimated value of the translational velocity vector of the carrier corresponding to the M′th first sensor in the l-1th round of iterations.

[0329] s i,l It is determined based on the first translational velocity vector estimate, or the translational velocity vector estimate of the carrier corresponding to the i-1th first sensor in the l-th iteration, or the translational velocity vector estimate of the carrier corresponding to the M′th first sensor in the l-1th iteration. There are three possible cases:

[0330] i is equal to 1, l is equal to 1, s i,l It is determined based on the estimated value of the first translational velocity vector and can be referred to the following relationship.

[0331]

[0332] in, is the estimated value of the first translational velocity vector, ω 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 sensor relative to the carrier coordinate system.

[0333] i is greater than 1, s i,l It is determined based on the estimated value of the translational velocity vector of the carrier corresponding to the (i-1)th first sensor in the lth iteration, and can be referred to the following relationship.

[0334]

[0335] in, is the estimated value of the translational velocity vector of the carrier corresponding to the i-1th first sensor in the lth 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 i-th first sensor relative to the carrier coordinate system.

[0336] i is equal to 1, l is greater than 1, s1,l It is determined based on the estimated value of the translational velocity vector of the carrier corresponding to the M′th first sensor in the l-1th iteration, and can be referred to the following relationship.

[0337]

[0338] in, is the estimated value of the translational velocity vector of the carrier corresponding to the M′th first sensor in the l-1th 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 i-th first sensor relative to the carrier coordinate system.

[0339] It should be noted that the iteration termination condition for the second iteration method can be set as that the vector distance between the second translational velocity vector estimate and the first translational velocity vector estimate is not greater than a second preset threshold or threshold. This is equivalent to determining the vector distance between the translational velocity vector estimate of the carrier corresponding to the (i-1)th first sensor in the l-th iteration and the first translational velocity vector estimate. If this vector distance is greater than the second preset threshold or threshold, then further determining the translational velocity vector estimate of the carrier corresponding to the (i-1)th first sensor in the l-th iteration. If this vector distance is not greater than the second preset threshold or threshold, the iteration is terminated. In this case, the translational velocity vector estimate of the carrier corresponding to the (i-1)th first sensor in the l-th iteration can be referred to as the second translational velocity vector estimate.

[0340] Exemplarily, the second translational velocity vector estimate and the first translational velocity vector estimate satisfy the following relationship:

[0341]

[0342] Wherein, Threshold2 is a second preset threshold or limit.

[0343] In addition, the iteration termination condition of the second iteration method can also be set to reaching the maximum number of iterations. This is equivalent to setting the maximum number of iterations to L, that is, performing a total of L iterations, and the estimated translational velocity vector of the carrier corresponding to a first sensor in the Lth iteration (i.e., the last iteration) is referred to as the second estimated translational velocity vector. Exemplarily, the maximum number of iterations L is set to 5, and the estimated translational velocity vector of the carrier corresponding to the M′th first sensor in the fifth iteration is set to the second estimated translational velocity vector.

[0344] In the above-mentioned second iterative method, the normalized translational velocity vector estimate of each first sensor is used to determine the translational velocity vector estimate of the carrier corresponding to each first sensor. This is equivalent to performing one iteration for each first sensor and using the parameters of a first sensor as the input of one iteration, so that a relatively accurate second translational velocity vector estimate can be quickly obtained.

[0345] In the embodiment of the present application, the normalized translational velocity vector estimate obtained by the first sensor by evaluating its own motion is relatively accurate. The normalized translational velocity vector estimate of the first sensor is fused with the first translational velocity vector estimate to obtain a second translational velocity vector estimate of the carrier. This further improves the accuracy of the carrier's translational velocity vector estimate compared to the first translational velocity vector estimate. Using the carrier's first rotational angular velocity vector estimate and the carrier's second translational velocity vector estimate to compensate for the carrier's motion helps separate moving and stationary targets, and also facilitates positioning and tracking of the carrier's motion.

[0346] In the embodiment of the present application, the first sensor among the M' first sensors can be understood as a first sensor that can obtain a normalized translational velocity vector estimate. The M' first sensors can be visual sensors, such as cameras, video cameras, infrared sensors, etc. In one example, the M' first sensors can be M1' cameras and M2' infrared sensors, where M1'+M2'=M', and M1'≥0 and M2'≥0.

[0347] In addition, the embodiments of the present application can be further combined with M-M' first sensors that can obtain translational velocity vector estimates, wherein the translational velocity vector estimates obtained by the MM' first sensors contain scale information of the velocity vector, such as complete information of each velocity component, rather than just normalized values ​​or direction information, thereby improving the accuracy of the second translational velocity vector estimate of the carrier.

[0348] That is, the present application provides another method for determining the estimated value of the second translational velocity vector as follows.

[0349] In step 203 , normalized translational velocity vector estimation values ​​of M′ first sensors such as visual sensors and translational velocity vector estimation values ​​of MM′ first sensors such as inertial sensors (eg, IMU) may be obtained.

[0350] In step 204, first, based on the estimated translational velocity vectors of the MM′ first sensors, such as inertial sensors, the estimated translational velocity vectors of the carrier corresponding to the MM′ first sensors, such as inertial sensors, are determined.

[0351] For example, based on the relationship or For a detailed description of the relationship, please refer to the above implementation method.

[0352] As a direct implementation method, it can be based on the relation or Determine an estimated translational velocity vector of the carrier corresponding to the visual sensor, and weight the estimated translational velocity vector of the carrier corresponding to the visual sensor and the estimated translational velocity vector of the carrier corresponding to the inertial sensor to obtain a second estimated translational velocity vector.

[0353] As an iterative implementation method, 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 1 corresponding to middle, The estimated translational velocity vector of the carrier corresponding to the inertial sensor is replaced. Iteration is performed based on the iterative method in step 204 until an iteration condition is met, and the estimated translational velocity vector of the carrier in the kth iteration that meets the iteration condition is used as the second estimated translational velocity vector.

[0354] Furthermore, in the present application, M′=0 may exist, that is, in step 203 , the estimated values ​​of the translational velocity vectors of M inertial sensors are obtained.

[0355] Accordingly, in step 204, the estimated translational velocity vector of the carrier corresponding to the inertial sensor can be determined based on the estimated translational velocity vectors of the M inertial sensors. For example, the estimated translational velocity vector of the carrier corresponding to the inertial sensor can be determined based on the relationship: or Determine the estimated translational velocity vector of the carrier corresponding to the inertial sensor. For a detailed description of the relationship, refer to the above implementation. Weight the estimated translational velocity vector of the carrier corresponding to the inertial sensor and the first estimated translational velocity vector to obtain a second estimated translational velocity vector.

[0356] Optionally, the estimated value of the rotational angular velocity vector of the carrier may be further updated based on the second estimated value of the translational velocity vector. The updated estimated value of the rotational angular velocity vector is referred to herein as the second estimated value of the rotational angular velocity vector of the carrier.

[0357] Optionally, in step 205 , a second estimated value of a rotational angular velocity vector of the carrier is determined based on the estimated value of the second translational velocity vector of the carrier.

[0358] The second rotational angular velocity vector estimate can be determined based on the following relationship:

[0359]

[0360] Or, equivalently

[0361]

[0362] Or, equivalently

[0363]

[0364] Wherein, ω′ is the estimated value of the second rotation angular velocity vector, v2 is the estimated value of the instantaneous velocity vector of the second sensor, is the estimated value of the second translation velocity vector, r2 is the position translation vector of the second sensor's coordinate system relative to the carrier's coordinate system, and × represents the cross product of the vectors. × is the antisymmetric matrix corresponding to r2.

[0365] Based on the above relationship, the expression of the second rotation angular velocity vector estimation value can be

[0366] As an implementation manner, the second rotation angular velocity vector estimation value may be determined based on the following relationship:

[0367]

[0368] Where ω′ is the estimated value of the second rotation angular velocity vector, v 2,j is the instantaneous velocity vector estimate of the j-th second sensor, is the estimated value of the second translational velocity vector, For [r 2,j ] × The inverse matrix, [r 2,j ] × For r 2,j The corresponding antisymmetric matrix, r 2，j is the position translation vector of the j-th second sensor's coordinate system relative to the carrier's coordinate system.

[0369] Among them, r 2,j =[r x,2,j r y,2,j r z,2,j ] T ,

[0370] As an implementation method, the second rotational angular velocity vector estimate is determined based on the second rotational angular velocity vector estimate, the instantaneous velocity vector estimate of N second sensors, and the position translation vector of the coordinate system of each of the N second sensors relative to the carrier coordinate system, where N≥1.

[0371] In one example, the second rotational angular velocity vector estimate is determined based on the following relationship:

[0372]

[0373] Where ω′ is the estimated value of the second rotation angular velocity vector, w 2,j is the weighting coefficient of the jth second sensor, v 2,j is the instantaneous velocity vector estimate of the j-th second sensor, is the estimated value of the second translational velocity vector, For [r 2,j ] × The inverse matrix, [r 2,j ] × For r 2,j The corresponding antisymmetric matrix, r 2,j is the position translation vector of the j-th second sensor's coordinate system relative to the carrier's coordinate system.

[0374] In yet another example, the first rotational angular velocity vector estimate is determined based on the following relationship:

[0375]

[0376] Where ω′ is the estimated value of the second rotation angular velocity vector, v 2,j is the instantaneous velocity vector estimate of the j-th second sensor, is the estimated value of the second translational velocity vector, For [r 2,j ] × The inverse matrix, [r 2,j ] × For r 2,j The corresponding antisymmetric matrix, r 2,j is the position translation vector of the j-th second sensor's coordinate system relative to the carrier's coordinate system.

[0377] In another implementation, N is equal to 1, and the second rotational angular velocity vector estimate is determined based on the second rotational angular velocity vector estimate, the instantaneous velocity vector estimate of the second sensor, and the position translation vector of the coordinate system of the second sensor relative to the carrier coordinate system. The specific value can be obtained by referring to the above relationship.

[0378] In the present application, the second translational velocity vector estimation value of the carrier and the second rotational angular velocity vector estimation value of the carrier may be used as the finally determined translational velocity vector estimation value of the carrier and the rotational angular velocity vector estimation value of the carrier, respectively.

[0379] In addition, the first rotational angular velocity vector estimate and the second rotational angular velocity vector estimate may be further fused, and the fused rotational angular velocity vector estimate may be used as the final determined rotational angular velocity vector estimate of the carrier. Specifically, the fusion may be performed by determining an average of the first rotational angular velocity vector estimate of the carrier and the second rotational angular velocity vector estimate of the carrier. Alternatively, the first rotational angular velocity vector estimate of the carrier and the second rotational angular velocity vector estimate of the carrier are fused by performing a weighted combination based on a minimum mean square error.

[0380] In the embodiments of the present application, a second estimated rotational angular velocity vector of the carrier is determined based on the second estimated translational velocity vector of the carrier. This further improves the accuracy of the estimated rotational angular velocity vector of the carrier compared to the first estimated rotational angular velocity vector. Using the second estimated rotational angular velocity vector of the carrier and the second estimated translational velocity vector of the carrier to compensate for the motion of the carrier facilitates separation of moving and stationary targets, and also facilitates positioning and tracking of the carrier's motion.

[0381] The various embodiments described herein may be independent solutions or may be combined according to internal logic, and all of these solutions fall within the scope of protection of this application.

[0382] The division of modules in the embodiments of the present application is illustrative and is merely a logical functional division. In actual implementation, other division methods may be used. Furthermore, the functional modules in the various embodiments of the present application may be integrated into a single processor, or may exist physically separately, or two or more modules may be integrated into a single module. The aforementioned integrated modules may be implemented in the form of hardware or software functional modules.

[0383] Similar to the above concept, an embodiment of the present application further provides a motion estimation device for implementing the above method.

[0384] Exemplarily, the motion estimation device may be a sensor system or a fused perception system or a planning / control system integrating the above systems, such as an assisted driving or autonomous driving system. Alternatively, the motion estimation device may be software or hardware (e.g., a data processing device connected to or integrated with the corresponding sensor via wireless or wired communication).

[0385] The motion estimation device may be a vehicle with motion estimation functionality, or other components with motion estimation functionality. The motion estimation device includes, but is not limited to, a vehicle-mounted terminal, a vehicle-mounted controller, a vehicle-mounted module, a vehicle-mounted component, a vehicle-mounted chip, a vehicle-mounted unit, a vehicle-mounted radar, or a vehicle-mounted camera, or other sensors. The vehicle may implement the method provided in this application through the vehicle-mounted terminal, vehicle-mounted controller, vehicle-mounted module, vehicle-mounted component, vehicle-mounted chip, vehicle-mounted unit, vehicle-mounted radar, or camera.

[0386] The motion estimation device may also be a smart terminal other than a vehicle with motion estimation capabilities, or may be installed in a smart terminal other than a vehicle with motion estimation capabilities, or may be installed in a component of such a smart terminal. The smart terminal may be other terminal devices such as smart transportation equipment, smart home appliances, robots, etc. The motion estimation device includes but is not limited to the smart terminal or its controller, chip, other sensors such as radar or cameras, and other components.

[0387] The motion estimation device can be a general-purpose device or a dedicated device. In a specific implementation, the motion estimation device 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 device with processing capabilities. The embodiments of the present application are not limited to the type of the motion estimation device.

[0388] The motion estimation device may also be a chip or processor with processing capabilities, and the motion estimation device may include multiple processors. The processor may be a single-core (single-CPU) processor or a multi-core (multi-CPU) processor. The chip or processor with processing capabilities may be disposed in the sensor, or may not be disposed in the sensor but be disposed at the receiving end of the sensor output signal.

[0389] like Figure 7 The present application provides an exemplary motion estimation device 700. The motion estimation device 700 may include an acquisition unit 701 and a processing unit 702. It should be understood that the description of the device embodiment corresponds to the description of the method embodiment. Therefore, for matters not described in detail, reference can be made to the method embodiment above and will not be repeated here for the sake of brevity.

[0390] Exemplarily, the acquisition unit 701 is configured to acquire the rotational angular velocity vector estimation values ​​of M first sensors and the instantaneous velocity vector estimation values ​​of N second sensors; wherein M≥1, N≥1;

[0391] The processing unit 702 is used to determine the first translational velocity vector estimate of the carrier based on the instantaneous velocity vector estimate values ​​of the N second sensors and the first rotational angular velocity vector estimate value of the carrier where the N second sensors are located, wherein the first rotational angular velocity vector estimate value is determined based on the rotational angular velocity vector estimate values ​​of the M first sensors.

[0392] In a possible implementation, the processing unit 702 is specifically configured to determine the first translational velocity vector estimate based on the following relationship:

[0393]

[0394] in, is the estimated value of the first translational velocity vector, ω is the estimated value of the first rotational angular velocity vector, v 2,j is the instantaneous velocity vector estimate of the jth second sensor, r 2,j is the position translation vector of the j-th second sensor's coordinate system relative to the carrier's coordinate system.

[0395] In a possible implementation, the first translational velocity vector estimate satisfies the following relationship:

[0396]

[0397] in, is the estimated value of the first translational velocity vector, ω is the estimated value of the first rotational angular velocity vector, w 2,j is the weighting coefficient of the jth second sensor, w 2,j is the instantaneous velocity vector estimate of the jth second sensor, r 2,j is the position translation vector of the j-th second sensor's coordinate system relative to the carrier's coordinate system.

[0398] In a possible implementation, the first rotation angular velocity vector estimate satisfies the following relationship:

[0399]

[0400] Where ω is the estimated value of the first rotation angular velocity vector, w 1,i is the weighting coefficient of the i-th first sensor, ω 1,i is the estimated value of the rotational angular velocity vector of the i-th first sensor.

[0401] In one possible implementation, the acquisition unit 701 is further used to obtain normalized translational velocity vector estimates of M′ first sensors among the M first sensors, where 1≤M′≤M; the processing unit 702 is further used to determine a second translational velocity vector estimate of the carrier based on the first translational velocity vector estimate and the normalized translational velocity vector estimate of the M′ first sensors.

[0402] In a possible implementation, the processing unit 702 is specifically configured to determine the second translational velocity vector estimate based on the following relationship:

[0403]

[0404] in, is the estimated value of the second translational velocity vector, ω is the estimated value of the first rotational angular velocity vector, is the normalized translational velocity vector estimate of the i-th first sensor, r 1,i is the position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system, s i is the normalized parameter or scale factor of the translational velocity vector of the i-th first sensor, s i Determined by the first translational velocity vector estimate.

[0405] In a possible implementation, the second translational velocity vector estimate satisfies the following relationship:

[0406]

[0407] Among them, 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 kth iteration, ω is the estimated value of the first rotational angular velocity vector, is the normalized translational velocity vector estimate of the i-th first sensor, r 1,i is the position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system, s i,k is the normalization parameter or scale factor of the translational velocity vector of the i-th first sensor in the k-th iteration, s i,k Determined by the first translational velocity vector estimate or the translational velocity vector estimate of the carrier in the k-1th iteration.

[0408] In a possible implementation, the second translational velocity vector estimate satisfies the following relationship:

[0409]

[0410] Among them, 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 kth 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 rotation angular velocity vector, is the normalized translational velocity vector estimate of the i-th first sensor, r 1,i is the position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system, s i,k is the normalization parameter or scale factor of the translational velocity vector of the i-th first sensor in the k-th iteration, s i,k Determined by the first translational velocity vector estimate or the translational velocity vector estimate of the carrier in the k-1th iteration.

[0411] In one possible implementation, the normalized parameter or scale factor of the translational velocity vector of the i-th first sensor in the k-th iteration satisfies the following relationship:

[0412] s i,k =‖t k-1 +ω×r 1,i ‖

[0413] Among them, t k-1 is the estimated value of the translational velocity vector of the carrier in the k-1th iteration, and t0 is the estimated value of the first translational velocity vector.

[0414] In a possible implementation, the second translational velocity vector estimate satisfies the following relationship:

[0415]

[0416] in, is the estimated value of the second translational velocity vector is the estimated value of the translational velocity vector of the carrier corresponding to the i-th first sensor in the l-th iteration, ω is the estimated value of the first rotational angular velocity vector, is the normalized translational velocity vector estimate of the i-th first sensor, r 1,i The position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system, s i,l is the normalized parameter or scale factor of the translational velocity vector of the i-th first sensor in the l-th iteration, s i,l Estimated by the first translational velocity vector or or Sure.

[0417] In a possible implementation, the normalized parameter or scale scaling factor of the translational velocity vector of the i-th first sensor in the l-th iteration satisfies the following relationship:

[0418]

[0419] in, is the estimated value of the first translational velocity vector;

[0420] In a possible implementation, the processing unit 702 is further configured to determine an estimated value of a second rotational angular velocity vector of the carrier based on the following relationship:

[0421]

[0422] Where ω′ is the estimated value of the second rotation angular velocity vector, v 2,j is the instantaneous velocity vector estimate of the j-th second sensor, is the estimated value of the second translational velocity vector, For [r 2,j ] × The inverse matrix, [r 2,j ] × For r 2,j The corresponding antisymmetric matrix, r 2,j is the position translation vector of the j-th second sensor's coordinate system relative to the carrier's coordinate system.

[0423] Same as above idea, Figure 8 , which is a schematic structural diagram of a chip provided in an embodiment of the present application.

[0424] The chip 800 includes one or more processors 801 and an interface circuit 802. Optionally, the chip 800 may further include a bus 803.

[0425] Among them, the processor 801 may be an integrated circuit chip with signal processing capabilities. In the implementation process, each step of the above method can be completed by an integrated logic circuit of the hardware in the processor 801 or an instruction in the form of software. The above-mentioned processor 801 can 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 device, a discrete gate or transistor logic device, a discrete hardware component. The various methods and steps disclosed in the embodiments of the present application can be implemented or executed. The general-purpose processor can be a microprocessor or the processor can also be any conventional processor, etc.

[0426] 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 it, and can send the processing completion information through the interface circuit 802.

[0427] Optionally, the chip further includes a memory, which may include a read-only memory and a random access memory, and provides operating instructions and data to the processor. A portion of the memory may also include a non-volatile random access memory (NVRAM).

[0428] Optionally, the memory stores an executable software module or a data structure, and the processor can perform corresponding operations by calling an operation instruction stored in the memory (the operation instruction may be stored in an operating system).

[0429] Optionally, the chip can be used in the communication device (including the master node and the slave node) involved in the embodiments of the present application. Optionally, the interface circuit 802 can be used to output the execution result of the processor 801. Regarding the data transmission method provided in one or more embodiments of the present application, reference can be made to the aforementioned embodiments and will not be repeated here.

[0430] It should be noted that the corresponding functions of the processor 801 and the interface circuit 802 can be implemented through hardware design, software design, or a combination of hardware and software, which is not limited here.

[0431] The present application also provides a radar system for providing motion estimation functionality for a vehicle. The radar system includes at least one motion estimation device as described in the above embodiments of the present application. The at least one motion estimation device within the system can be integrated into a complete device or apparatus, or can be independently configured as a component or device.

[0432] The present application also provides a sensor system for providing motion estimation functionality for a vehicle. The sensor system includes at least one motion estimation device as described in the above embodiments of the present application, and at least one sensor such as a camera or radar. The at least one sensor device within the system can be integrated into a complete device or apparatus, or can be independently configured as a component or apparatus.

[0433] An embodiment of the present application also provides a system for use in unmanned driving or intelligent driving, which includes at least one of the motion estimation devices, cameras, radars and other sensors mentioned in the above embodiments of the present application. At least one device in the system can be integrated into a complete machine or equipment, or at least one device in the system can also be independently set as a component or device.

[0434] Furthermore, any of the above systems may interact with a central controller of a vehicle to provide detection and / or fusion information for decision-making or control of the vehicle's driving.

[0435] An embodiment of the present application also provides a vehicle, which includes at least one motion estimation device or any of the above systems mentioned in the above embodiments of the present application.

[0436] The embodiment of the present application further provides a communication device, comprising a processor and a communication interface, wherein the communication interface is used to receive signals from other communication devices outside the communication device and transmit them to the processor or send signals from the processor to other communication devices outside the communication device, and the processor is used to implement the above-mentioned Figure 2 The method in .

[0437] The embodiment of the present application also provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program or instruction. When the computer program or instruction is executed by a communication device, the above-mentioned Figure 2 The method in .

[0438] The embodiment of the present application further provides a computer program product, wherein the computer program product includes a computer program or instructions. When the computer program or instructions are executed by a communication device, the above-mentioned Figure 2 The method in .

[0439] Those skilled in the art will appreciate that the embodiments of the present application can be provided as methods, systems, or computer program products. Therefore, the present application can adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application can adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to magnetic disk storage, optical storage, etc.) that contain computer-usable program code.

[0440] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the present application. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0441] These computer program instructions may also be stored in a computer readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.

[0442] Obviously, those skilled in the art may make various changes and modifications to the present application without departing from the scope of the present application. Thus, if these modifications and variations of the present application fall within the scope of the claims of the present application and their equivalents, the present application is intended to include these modifications and variations.

Claims

1. A method for motion estimation, characterized in that: include: Obtaining rotational angular velocity vector estimation values ​​of M first sensors and instantaneous velocity vector estimation values ​​of N second sensors; wherein M≥1, N≥1; determining a first translational velocity vector estimate of the carrier based on the instantaneous velocity vector estimates of the N second sensors and a first rotational angular velocity vector estimate of the carrier where the N second sensors are located, wherein the first rotational angular velocity vector estimate is determined based on the rotational angular velocity vector estimates of the M first sensors; The first translational velocity vector estimate is determined based on the following relationship: in, is the estimated value of the first translational velocity vector, ω is the estimated value of the first rotational angular velocity vector, v 2,j is the instantaneous velocity vector estimate of the jth second sensor, r 2,j is the position translation vector of the j-th second sensor's coordinate system relative to the carrier's coordinate system.

2. The method according to claim 1, wherein The first translational velocity vector estimate satisfies the following relationship: in, is the estimated value of the first translational velocity vector, ω is the estimated value of the first rotational angular velocity vector, w 2,j is the weighting coefficient of the jth second sensor, v 2,j is the instantaneous velocity vector estimate of the jth second sensor, r 2,j is the position translation vector of the j-th second sensor's coordinate system relative to the carrier's coordinate system.

3. The method according to claim 1 or 2, wherein: The first rotation angular velocity vector estimated value satisfies the following relationship: Where ω is the estimated value of the first rotation angular velocity vector, w 1,i is the weighting coefficient of the i-th first sensor, ω 1,i is the estimated value of the rotational angular velocity vector of the i-th first sensor.

4. The method according to any one of claims 1 to 3, wherein Also includes: Obtain normalized translational velocity vector estimates of M′ first sensors among the M first sensors, where 1≤M′≤M; A second translational velocity vector estimate value of the carrier is determined according to the first translational velocity vector estimate value and the normalized translational velocity vector estimates of the M′ first sensors.

5. The method according to claim 4, wherein The second translational velocity vector estimate is determined based on the following relationship: in, is the estimated value of the second translational velocity vector, ω is the estimated value of the first rotational angular velocity vector, is the normalized translational velocity vector estimate of the i-th first sensor, r 1,i is the position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system, s i is the normalized parameter or scale factor of the translational velocity vector of the i-th first sensor, s i Determined by the first translational velocity vector estimate.

6. The method according to claim 4, wherein The second translational velocity vector estimate satisfies the following relationship: Among them, 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 kth iteration, ω is the estimated value of the first rotational angular velocity vector, is the normalized translational velocity vector estimate of the i-th first sensor, r 1,i is the position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system, s i,k is the normalization parameter or scale factor of the translational velocity vector of the i-th first sensor in the k-th iteration, s i,k Determined by the first translational velocity vector estimate or the translational velocity vector estimate of the carrier in the k-1th iteration.

7. The method according to claim 4, wherein The second translational velocity vector estimate satisfies the following relationship: Among them, 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 kth 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 rotation angular velocity vector, is the normalized translational velocity vector estimate of the i-th first sensor, r 1,i is the position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system, s i,k is the normalization parameter or scale factor of the translational velocity vector of the i-th first sensor in the k-th iteration, s i,k Determined by the first translational velocity vector estimate or the translational velocity vector estimate of the carrier in the k-1th iteration.

8. The method according to claim 6 or 7, wherein: The normalized parameter or scale factor of the translational velocity vector of the i-th first sensor in the k-th iteration satisfies the following relationship: s i,k =‖t k-1 +ω×r 1,i ‖ Among them, t k-1 is the estimated value of the translational velocity vector of the carrier in the k-1th iteration, and t0 is the estimated value of the first translational velocity vector.

9. The method according to claim 4, wherein The second translational velocity vector estimate satisfies the following relationship: in, is the estimated value of the second translational velocity vector is the estimated value of the translational velocity vector of the carrier corresponding to the i-th first sensor in the l-th iteration, ω is the estimated value of the first rotational angular velocity vector, is the normalized translational velocity vector estimate of the i-th first sensor, r 1,i The position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system, s i,l is the normalized parameter or scale factor of the translational velocity vector of the i-th first sensor in the l-th iteration, s i,l Estimated by the first translational velocity vector or or Sure.

10. The method according to claim 9, wherein The normalized parameter or scale factor of the translational velocity vector of the i-th first sensor in the l-th iteration satisfies the following relationship: in, is the estimated value of the first translational velocity vector; 11. The method according to any one of claims 4 to 10, characterized in that Also includes: The second rotational angular velocity vector estimation value of the carrier is determined based on the following relationship: Where ω′ is the estimated value of the second rotation angular velocity vector, v 2,j is the instantaneous velocity vector estimate of the j-th second sensor, is the estimated value of the second translational velocity vector, For [r 2,j ] × The inverse matrix, [r 2,j ] × For r 2,j The corresponding antisymmetric matrix, r 2,j is the position translation vector of the j-th second sensor's coordinate system relative to the carrier's coordinate system.

12. A motion estimation device, characterized in that: include: Acquisition unit and processing unit; The acquisition unit is used to acquire the rotational angular velocity vector estimation values ​​of M first sensors and the instantaneous velocity vector estimation values ​​of N second sensors; wherein M≥1, N≥1; the processing unit being configured to determine a first translational velocity vector estimate value of the carrier based on the instantaneous velocity vector estimates of the N second sensors and a first rotational angular velocity vector estimate value of the carrier where the N second sensors are located, wherein the first rotational angular velocity vector estimate value is determined based on the rotational angular velocity vector estimates of the M first sensors; The processing unit is specifically configured to determine the first translational velocity vector estimate based on the following relationship: in, is the estimated value of the first translational velocity vector, ω is the estimated value of the first rotational angular velocity vector, v 2,j is the instantaneous velocity vector estimate of the jth second sensor, r 2,j is the position translation vector of the j-th second sensor's coordinate system relative to the carrier's coordinate system.

13. The device according to claim 12, wherein The first translational velocity vector estimate satisfies the following relationship: in, is the estimated value of the first translational velocity vector, ω is the estimated value of the first rotational angular velocity vector, w 2,j is the weighting coefficient of the jth second sensor, v 2,j is the instantaneous velocity vector estimate of the jth second sensor, r 2,j is the position translation vector of the j-th second sensor's coordinate system relative to the carrier's coordinate system.

14. The device according to claim 12 or 13, characterized in that The first rotation angular velocity vector estimated value satisfies the following relationship: Where ω is the estimated value of the first rotation angular velocity vector, w 1,i is the weighting coefficient of the i-th first sensor, ω 1,i is the estimated value of the rotational angular velocity vector of the i-th first sensor.

15. The device according to any one of claims 12 or 14, characterized in that The acquisition unit is further used to obtain normalized translational velocity vector estimates of M′ first sensors among the M first sensors, where 1≤M′≤M; the processing unit is further used to determine a second translational velocity vector estimate of the carrier based on the first translational velocity vector estimate and the normalized translational velocity vector estimate of the M′ first sensors.

16. The device according to claim 15, characterized in that The processing unit is specifically configured to determine the second translational velocity vector estimation value based on the following relationship: in, is the estimated value of the second translational velocity vector, ω is the estimated value of the first rotational angular velocity vector, is the normalized translational velocity vector estimate of the i-th first sensor, r 1,i is the position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system, s i is the normalized parameter or scale factor of the translational velocity vector of the i-th first sensor, s i Determined by the first translational velocity vector estimate.

17. The device according to claim 15, wherein The second translational velocity vector estimate satisfies the following relationship: Among them, 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 kth iteration, ω is the estimated value of the first rotational angular velocity vector, is the normalized translational velocity vector estimate of the i-th first sensor, r 1,i is the position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system, s i,k is the normalization parameter or scale factor of the translational velocity vector of the i-th first sensor in the k-th iteration, s i,k Determined by the first translational velocity vector estimate or the translational velocity vector estimate of the carrier in the k-1th iteration.

18. The device according to claim 15, wherein The second translational velocity vector estimate satisfies the following relationship: Among them, 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 kth 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 rotation angular velocity vector, is the normalized translational velocity vector estimate of the i-th first sensor, r 1,i is the position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system, s i,k is the normalization parameter or scale factor of the translational velocity vector of the i-th first sensor in the k-th iteration, s i,k Determined by the first translational velocity vector estimate or the translational velocity vector estimate of the carrier in the k-1th iteration.

19. The device according to claim 17 or 18, characterized in that The normalized parameter or scale factor of the translational velocity vector of the i-th first sensor in the k-th iteration satisfies the following relationship: s i,k =‖t k-1 +ω×r 1,i ‖ Among them, t k-1 is the estimated value of the translational velocity vector of the carrier in the k-1th iteration, and t0 is the estimated value of the first translational velocity vector.

20. The device according to claim 15, wherein The second translational velocity vector estimate satisfies the following relationship: in, is the estimated value of the second translational velocity vector is the estimated value of the translational velocity vector of the carrier corresponding to the i-th first sensor in the l-th iteration, ω is the estimated value of the first rotational angular velocity vector, is the normalized translational velocity vector estimate of the i-th first sensor, r 1,i The position translation vector of the coordinate system of the i-th first sensor relative to the carrier coordinate system, s i,l is the normalized parameter or scale factor of the translational velocity vector of the i-th first sensor in the l-th iteration, s i,l Estimated by the first translational velocity vector or or Sure.

21. The device according to claim 20, characterized in that The normalized parameter or scale factor of the translational velocity vector of the i-th first sensor in the l-th iteration satisfies the following relationship: in, is the estimated value of the first translational velocity vector; 22. The device according to any one of claims 15 to 21, characterized in that The processing unit is further configured to determine an estimated value of a second rotational angular velocity vector of the carrier based on the following relationship: Where ω′ is the estimated value of the second rotation angular velocity vector, v 2,j is the instantaneous velocity vector estimate of the j-th second sensor, is the estimated value of the second translational velocity vector, For [r 2,j ] × The inverse matrix, [r 2,j ] × For r 2,j The corresponding antisymmetric matrix, r 2,j is the position translation vector of the j-th second sensor's coordinate system relative to the carrier's coordinate system.

23. A chip, characterized in that: comprising at least one processor and an interface; The interface is configured to provide program instructions or data to the at least one processor; The at least one processor is configured to execute the program instructions to implement the method according to any one of claims 1 to 11.

24. A computer-readable storage medium, characterized in that The computer-readable storage medium stores a computer program or instructions, and when the computer program or instructions are executed by the communication device, the method according to any one of claims 1 to 11 is implemented.

Images