WO2019057173A1 - 传感器数据处理的方法及装置 - Google Patents

传感器数据处理的方法及装置 Download PDF

Info

Publication number
WO2019057173A1
WO2019057173A1 PCT/CN2018/107049 CN2018107049W WO2019057173A1 WO 2019057173 A1 WO2019057173 A1 WO 2019057173A1 CN 2018107049 W CN2018107049 W CN 2018107049W WO 2019057173 A1 WO2019057173 A1 WO 2019057173A1
Authority
WO
WIPO (PCT)
Prior art keywords
sensor
data
rotation
translation
sampling
Prior art date
Application number
PCT/CN2018/107049
Other languages
English (en)
French (fr)
Inventor
李晚龙
温丰
Original Assignee
华为技术有限公司
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 华为技术有限公司 filed Critical 华为技术有限公司
Priority to EP18859154.9A priority Critical patent/EP3680690B1/en
Publication of WO2019057173A1 publication Critical patent/WO2019057173A1/zh
Priority to US16/825,145 priority patent/US11379698B2/en

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/485—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an optical system or imaging system
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/14—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by recording the course traversed by the object
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/53—Determining attitude
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G06F18/251—Fusion techniques of input or preprocessed data

Definitions

  • the present application relates to the field of sensor applications, and in particular, to a method and device for processing sensor data.
  • Intelligent terminals such as self-driving cars and intelligent robots are new types of terminals that have received much attention in recent years.
  • the sensors are equivalent to their eyes.
  • autonomous vehicles can identify roads, vehicles on the road, pedestrians, obstacles, and/or basic transportation facilities. Realizing the positioning of intelligent terminals through sensors is one of the current technical issues of great concern.
  • the development trend of intelligent terminals shows that the use of only one sensor to realize the positioning of intelligent terminals will face more and more challenges. Therefore, how to realize the positioning of intelligent terminals through multiple types of sensors has become one of the urgent technical problems to be solved. .
  • the prior art adopts a method based on volume kalman filter (CKF) to realize data fusion of nonlinear asynchronous multi-sensor.
  • the data fusion method of the prior art can only perform linearization processing on the current time. Therefore, it is necessary to strictly follow the time sequence of the collected data of the sensor, and the data processing mode is fixed, and the applicable range is small.
  • the prior art data fusion strictly follows the chronological linearization process, which will bring about the gradual accumulation of errors, which makes the positioning accuracy of the smart terminal low and the applicability is poor.
  • the embodiment of the present invention provides a method and a device for processing sensor data, which can improve the operation flexibility of data fusion of multiple sensors, improve the data processing efficiency of the sensor, and further improve the positioning accuracy of the smart terminal, and have high applicability.
  • the first aspect provides a method of sensor data processing, the method comprising: determining a reference sensor from at least two sensors in the terminal, and determining respective sampling moments of the reference sensor.
  • the sensor type of the sensor K is determined.
  • the observation data of different types of sensors can be processed by different data processing methods to determine that the observation data of each sensor is aligned with the rotation data, the translation data and the covariance matrix at the sampling moment of the reference sensor, and the data processing The way is more flexible.
  • K is a first sensor is a first sensor type
  • the sampling timing from the reference sensor is determined, from the latest sampling time t 1 T 1 of the first sensor and the sampling interval t 1 is determined according to T 1
  • Interpolating coefficient ⁇ 1 at the time of interpolation and calculating first rotation data and first translation data of the first sensor at T 1 according to ⁇ 1 and observation data of the first sensor, and calculating a first rotation data and a first translation data corresponding to the first A covariance matrix.
  • the observation data of the sensor includes the posture of the terminal measured by the sensor at any time, and will not be described below.
  • the rotation matrix and translation vector of the terminal measured by a sensor at a sampling instant t i in a certain coordinate system can be used to acquire a rotation matrix and a translation vector of the terminal at any sampling time.
  • the first type of sensor herein is used to acquire (or the terminal equipped with the sensor) rotation data and translation data at any sampling instant.
  • the first sensor is only one example of a first type of sensor, and the first type of sensor includes, but is not limited to, a first sensor, which is not limited herein.
  • the two sampling times T 2 and T 3 closest to the distance t 1 are determined from the sampling moments of the reference sensor, and are determined according to the sampling interval of the second sensor and t 1 T interpolation coefficient interpolation and interpolation coefficients ⁇ 2 when T 3 interpolation on the 2 ⁇ 3, and in accordance with ⁇ 2, 3 [lambda] observed data and the second sensor a second sensor is calculated from T 2 to T 3 of the second Rotating the data and the second translation data to calculate a second covariance matrix corresponding to the second rotation data and the second translation data.
  • a second type of sensor is used to acquire rotational data (eg, a rotation matrix) and translation data (eg, a translation vector) of the relative motion of the sensor between any two adjacent sampling instants.
  • rotational data eg, a rotation matrix
  • translation data eg, a translation vector
  • a radar sensor or a visual sensor can be used to acquire a rotation matrix and a translation vector of the relative motion of the terminal between two adjacent sampling instants.
  • the second sensor is only one example of the second type of sensor, and the second type of sensor includes, but is not limited to, the second sensor, which is not limited herein.
  • the rotation data (such as a rotation matrix), the translation data (such as a translation vector), and the covariance matrix of each sensor at the sampling moment of the reference sensor are calculated in a corresponding manner.
  • the rotation data, the translation data, and the covariance matrix at each sampling instant of the reference sensor including T 1 , T 2 , and T 3 are fused to obtain a pose estimation value of the terminal at each sampling moment.
  • the fused rotation data includes at least first rotation data and/or second rotation data
  • the fused translation data includes at least first translation data and/or second translation data
  • the fused covariance matrix includes at least a first covariance matrix , and / or a second covariance matrix.
  • the implementation manner provided by the embodiment of the present application may be based on the motion state data of the rotation data and the translation data of each sensor in the European space and the group space based on the motion perception. Perform mathematical derivation and/or interpolation calculation to transform the observation data of each sensor reaching the data fusion center at different times to the same time, thereby realizing the data fusion of the observation data of each sensor at the same time.
  • the fusion of observation data supports the out-of-order input of observation data of multiple sensors, the data processing method is flexible, the map construction accuracy rate is higher, and the applicability is stronger.
  • the rotation data and the translation data of the position of the terminal at different times are collected by a plurality of sensors, and the process of estimating the pose of the terminal according to the rotation data and the translation data of the terminal at different positions can be understood as being based on motion perception. Terminal pose estimation.
  • the embodiment of the present application may determine the nearest neighbor sampling time t 2 before t 1 according to the sampling interval of the first sensor, and calculate the interpolation coefficient when interpolating on T 1 according to t 1 and t 2 ⁇ 1 ;
  • the observation data of the first sensor includes a rotation matrix and a translation vector of the first sensor; wherein the first rotation data of the first sensor at T 1 is calculated according to the observation data of ⁇ 1 and the first sensor
  • the first translation data includes: acquiring a rotation matrix of the first sensor on t 1 And translation vector G p 1 , and the rotation matrix of the first sensor on t 2 And translation vector G p 2 ; according to ⁇ 1 , G p 1 , And G p 2 calculate the first rotation matrix of the first sensor on T 1 And a first translation vector G p T1 ;
  • G p T1 meets:
  • G p T1 (1 - ⁇ 1 ) G p 2 + ⁇ 1 G p 1 .
  • the first rotation matrix For the first rotation data the first translation vector G p T1 is the first translation data. Indicates the pose of the first sensor on T 1 in the global coordinate system, and G represents the global coordinate system. Represents the pose of the first sensor on t 1 in the global coordinate system, Indicates the pose of the first sensor at t 2 in the global coordinate system.
  • the observation data of the input of the first type of sensor at a certain alignment time is subjected to interpolation calculation and/or mathematical derivation corresponding to the one-dimensional pose calculation method.
  • the mode is transformed into observation data at the timing of the alignment.
  • the observation data of each sampling moment of the reference sensor can be fused to realize the fusion of the observation data of the multi-sensor, and the asynchronous arrival of the observation data of the multi-sensor can be supported, the operation is simple, and the data processing efficiency is high. .
  • the embodiment of the present application may calculate a covariance matrix P t1 corresponding to the pose of the first sensor on t 1 , and calculate a covariance matrix corresponding to the pose of the first sensor on t 2 .
  • P t2 calculating a Jacobian matrix H u according to the first rotation matrix and the first translation vector, and calculating a covariance matrix P T1 corresponding to the first rotation matrix and the first translation vector according to P t1 and P t2 ; wherein, Hu satisfies :
  • the P T1 satisfies:
  • P 1,2 represents P t1 P t2
  • Representing the estimated value of the rotation matrix R, O 3*3 , O 6*6 represent the all-zero matrix of 3*3 and 6*6
  • I represents the identity matrix
  • i represents T 1
  • G represents the global coordinate system
  • ⁇ represents the interpolation
  • the coefficients ⁇ 1 , Jr are the right Jacobian matrix
  • Logv represents the logarithm of the matrix.
  • the error vectors for angle and displacement respectively.
  • the embodiment of the present application can calculate the covariance matrix corresponding to the pose of each sensor at the sampling moment of the reference sensor, thereby improving the accuracy of obtaining the pose estimation value of the terminal through data fusion of the multi-sensor, and the applicability is higher. .
  • the embodiment of the present application determines the interpolation coefficient ⁇ 2 when interpolating on T 2 according to the sampling interval of the second sensor and t 1 interpolation interpolation coefficients at ⁇ 3 T 3 may perform the following specific operations: determining a nearest neighbor sampling time t before 1 t 2, and t 1 and t 2 are determined based on the sampling interval T 2 according to the second sensor, and t when the interpolation coefficient interpolation and interpolation coefficients ⁇ 2 when interpolation of the 3 T ⁇ 3;
  • ⁇ 3 meets:
  • T 2 represents the sampling time closest to the distance t 1 in the sampling time of the reference sensor
  • T 3 represents the sampling time closest to the distance t 2 in the sampling time of the reference sensor.
  • the observation data of the second sensor provided by the embodiment of the present application may include a rotation matrix and a translation vector of the second sensor.
  • the embodiment of the present application may perform the following operations when calculating the second rotation data and the second translation data of the second sensor from T 2 to T 3 according to the observation data of ⁇ 2 , ⁇ 3 and the second sensor: acquiring the second sensor at t 1 Rotation matrix between and t 2 And a translation vector 2 p 1 of the second sensor between t 1 and t 2 ; according to ⁇ 2 , ⁇ 3 , And 2 p 1 to calculate a second rotation matrix of the relative motion of the second sensor between T 2 and T 3 And a second translation vector T3 p T2 ;
  • T3 p T2 meets:
  • the second translation vector T3 p T2 is the second translation data.
  • the embodiment of the present application may also calculate a rotation matrix and a covariance matrix P t12 corresponding to the translation vector of the second sensor between t 1 and t 2 ; according to the second rotation matrix and the second translation The vector calculates the Jacobian matrix H u and calculates a covariance matrix P T12 corresponding to the second rotation matrix and the second translation vector according to P t12 ;
  • O 3*3 represents an all-zero matrix of 3*3
  • b represents T 3
  • e represents T 2
  • ⁇ b and ⁇ e represent interpolation coefficients ⁇ 3 and ⁇ 2 , respectively
  • Jr is right Jacobian matrix
  • Logv represents the logarithm of the matrix
  • the observation data of the input of the second type of sensor adjacent to a certain alignment time is subjected to interpolation calculation and mathematical derivation corresponding to the binary pose calculation method. Transformed into observation data at the timing of the alignment. Then, in the data fusion phase, the observation data of each sampling moment of the reference sensor can be fused to realize the fusion of the observation data of the multi-sensor, and the asynchronous arrival of the observation data of the multi-sensor can be supported, the operation is more flexible, and the data processing efficiency is realized. higher.
  • the embodiment of the present application may further be based on a rotation matrix and a translation vector at each sampling moment of the reference sensor including the T 1 , T 2 , and/or T 3 Constructing a verification map in the global coordinate system by the pose estimation value of the terminal at each sampling moment;
  • the verification map can be used to provide reference data for determining the online pose estimation value of the terminal.
  • the embodiment of the present application can transform the rotation data, the translation data, and/or the covariance matrix input by each sensor to the corresponding alignment time according to the type of each sensor, and by rotating data, translation data, and / or covariance matrix fusion, optimize the verification map in the output global coordinate system.
  • the online pose estimation of the terminal can be completed without GPS sensor input.
  • the fusion of the observation data of the plurality of sensors supports the out-of-order input of the observation data of the plurality of sensors, the data processing mode is flexible, the map construction accuracy rate is higher, and the applicability is stronger.
  • the embodiment of the present application may also activate an online positioning function of the terminal, and obtain a verification map of the terminal; when any of the observation data of the sensor L is acquired at any time t 3 , the sensor L is determined. If the sensor L is the third sensor of the first type, the sampling time T 4 closest to the distance t 3 is determined from each sampling time of the reference sensor, and the fourth rotation of the third sensor on T 4 is calculated. Data and fourth translation data, and a fourth covariance matrix corresponding to the fourth rotation data and the fourth translation data; if the sensor L is the fourth sensor of the second type, performing the following steps a and b:
  • the fused rotation data includes at least a fourth rotation Data, fifth rotation data or sixth rotation data
  • the fused translation data includes at least a fourth translation data, a fifth translation data or a sixth translation data
  • the fused covariance matrix includes at least a fourth covariance matrix and a fifth covariance Matrix, or sixth covariance matrix.
  • the observation data of the sensor can be matched with the verification map, the matched observation data is integrated with the observation data collected by each sensor in real time, and the fusion result of the observation data of the plurality of sensors is increased.
  • the quantity is smoothed and iteratively optimized, and finally the online pose estimation value of the current terminal is estimated in real time, the operation is convenient, the online pose estimation accuracy is high, and the applicability is stronger.
  • the first type of sensor including the first sensor and/or the third sensor, is used to acquire rotation data and translation data of the first type of sensor at any of its sampling instants.
  • a global positioning system (GPS) sensor can be used to acquire the rotation matrix and translation vector of the terminal at any sampling time.
  • GPS global positioning system
  • a second type of sensor including a second sensor and/or a fourth sensor is used to acquire the relative motion of the second type of sensor between any two adjacent sampling instants thereof.
  • Rotate data and pan data For example, a radar sensor or a visual sensor can be used to acquire the rotation matrix and translation vector of the relative motion of the terminal between two adjacent sampling instants.
  • the pose provided by the embodiment of the present application includes a position and a posture.
  • the position refers to the translation of the three directions of x, y, and z in the coordinate system
  • the posture is the rotation of three directions of x, y, and z in the coordinate system.
  • the pose can be represented by a translation vector including three directions of x, y, and z in a specified coordinate system, and a rotation matrix in three directions of x, y, and z of the specified coordinate system.
  • the observation data of the sensor including the pose of the terminal measured by the sensor at any time.
  • the sensor measures the rotation matrix and translation vector of the terminal in a certain coordinate system at a sampling instant t i .
  • Data alignment of sensors refers to the process of transforming observation data of different sensors under different time bases into observation data of different sensors at the same time base.
  • the observations of different sensors at the same time belong to the observation under the same time base.
  • the observation data of different sensors belong to the observation data under the same time base.
  • Observations of different sensors at different times belong to observations under different time bases.
  • the observation data of different sensors belong to observation data under different time bases.
  • the same time reference can be understood as the same sampling frequency or the same sampling period, etc., and the sampling frequency or adoption period can be understood as the sampling frequency or sampling frequency of the reference sensor.
  • Observations of different sensors at the same time can be understood as observations of sensors with the same sampling frequency at the same sampling time.
  • Observations of different sensors at different times can be understood as observations of sensors with different sampling frequencies at different times.
  • the second aspect there is provided a method of processing sensor data which comprising: determining from the at least two terminal sensor in a reference sensor, and it is determined that the reference sensor at each sampling time; and when any one acquires arrival time t 1
  • the sensor type of the sensor K is determined; if the sensor K is the first sensor of the first type, the sampling time T 1 closest to the distance t 1 is determined from each sampling time of the reference sensor, and acquired t 2 the first sensor and the corresponding observations at sample time before t 1, t 1 and t 2 are determined in the interpolation on the interpolation coefficient T 1 ⁇ 1 according to a first sensor according to the [lambda] 1 and t 1 and t
  • the observation data of 2 calculates the first rotation data of the first sensor at T 1 and the first translation data, and calculates a first covariance matrix corresponding to the first rotation data and the first translation data; if the sensor K is the second type of the second type sensors, in each sampling timing from the reference sensor is determined
  • the embodiment of the present application may also activate an online positioning function of the terminal, and obtain a verification map of the terminal; when any of the observation data of the sensor L is acquired at any time t 3 , the sensor L is determined. the sensor type; if L is a third sensor of the first sensor type, at each sampling timing from the reference sensor is determined from the latest sampling time t 3 T 4, acquiring a third sensor at sampling time t 4 before 3 t and the corresponding observed data; t 3 and t 4 when the interpolation coefficients determined in the interpolation.
  • the third sensor is calculated in accordance with the T the fourth rotation data and the fourth data on the translation of 4, and the fourth rotation data and the fourth data corresponding to the fourth translational covariance matrix; L If the sensor is of the second type of a fourth sensor, the following steps are performed a step b:
  • the fourth sensor in the observation data matches with the parity map 3 t, determining a reference sensor on the fourth rotation data and reference pan T 3 data; determining the distance from the reference sensor at each sampling timing in t 3 latest sampling time T 4, the fifth and the fifth rotation data and translation data for the fourth sensor T 4 is calculated based on the reference data for the fourth rotation sensors on t 3 and the reference translation data, and rotation data of the fifth a fifth covariance matrix corresponding to the fifth translation data;
  • the fused rotation data includes at least the Four rotation data, fifth rotation data or sixth rotation data
  • the fused translation data includes at least a fourth translation data, a fifth translation data or a sixth translation data
  • the fused covariance matrix includes at least a fourth covariance matrix, and a fifth Covariance matrix, or sixth covariance matrix.
  • the data fusion center can match the observation data of the sensor with the verification map, fuse the observed observation data with the observation data collected by each sensor in real time, and fuse the observation data of multiple sensors.
  • the result is incremental iterative optimization, and finally estimates the online pose estimation of the current terminal in real time, which is convenient to operate, with high accuracy of online pose estimation and strong applicability.
  • a third aspect provides an apparatus for sensor data processing, the apparatus comprising means and/or modules for performing the sensor data processing method provided by any of the above-described first aspect and/or any one of the possible implementations of the first aspect Therefore, the beneficial effects (or advantages) of the sensor data processing method provided by the first aspect can also be achieved.
  • a fourth aspect provides a terminal, the terminal comprising a memory, a processor, a receiver, and a transmitter; wherein the processor is configured to call the memory stored sensor data processing program code to perform the first aspect and/or the first aspect described above
  • the processor is configured to call the memory stored sensor data processing program code to perform the first aspect and/or the first aspect described above
  • a sensor data processing method provided by a possible implementation.
  • the embodiment of the present application provides a computer storage medium for storing computer software instructions required for the sensor data processing method provided in the above first aspect, the instruction including the terminal required to perform the manner designed by the first aspect described above. program.
  • the embodiment of the present application further provides a chip, which is coupled to a receiver and/or a transmitter in the terminal, and is used to implement the technical solution provided by the first aspect of the embodiment of the present application.
  • “coupled” in the context of the present application means that the two components are combined directly or indirectly with each other. This combination may be fixed or movable, which may allow for the transfer of fluid, electrical, electrical or other types of signals between the two components.
  • the operational flexibility of data fusion of multiple sensors can be improved, the data processing efficiency of the sensor can be improved, and the positioning accuracy of the smart terminal can be improved, and the applicability is high.
  • FIG. 1 is a schematic diagram of multi-sensor observation of terminal pose estimation according to an embodiment of the present application.
  • FIG. 2 is a schematic diagram of a map construction manner based on multi-sensor data fusion provided by an embodiment of the present application
  • FIG. 3 is a schematic flowchart of a method for processing sensor data provided by an embodiment of the present application.
  • FIG. 4 is a schematic diagram of an interpolation method provided by an embodiment of the present application.
  • FIG. 5 is another schematic diagram of an interpolation manner provided by an embodiment of the present application.
  • FIG. 6 is a schematic diagram of an online positioning manner based on multi-sensor data fusion provided by an embodiment of the present application
  • FIG. 7 is another schematic flowchart of a method for processing sensor data provided by an embodiment of the present application.
  • FIG. 8 is a schematic structural diagram of an apparatus for processing sensor data according to an embodiment of the present application.
  • FIG. 9 is a schematic structural diagram of a communication device according to an embodiment of the present application.
  • Position including position and posture.
  • the position refers to the translation of the three directions of x, y, and z in the coordinate system
  • the posture is the rotation of three directions of x, y, and z in the coordinate system.
  • the pose can be represented by a translation vector including three directions of x, y, and z in a specified coordinate system, and a rotation matrix in three directions of x, y, and z of the specified coordinate system.
  • the pose is described by taking the representation of the rotation matrix and the translation vector as an example, and the rotation matrix and the translation vector default to the rotation matrix and the translation vector in the same coordinate system.
  • the observation data of the sensor including the pose of the terminal measured by the sensor at any time.
  • the sensor measures the rotation matrix and translation vector of the terminal in a certain coordinate system at a sampling instant t i .
  • Data alignment of sensors refers to the process of transforming observation data of different sensors under different time bases into observation data of different sensors at the same time base.
  • the observations of different sensors at the same time belong to the observation under the same time base.
  • the observation data of different sensors belong to the observation data under the same time base.
  • Observations of different sensors at different times belong to observations under different time bases.
  • the observation data of different sensors belong to observation data under different time bases.
  • the same time reference can be understood as the same sampling frequency or the same sampling period, etc., and the sampling frequency or adoption period can be understood as the sampling frequency or sampling frequency of the reference sensor.
  • Observations of different sensors at the same time can be understood as observations of sensors with the same sampling frequency at the same sampling time.
  • Observations of different sensors at different times can be understood as observations of sensors with different sampling frequencies at different times.
  • a graph refers to a structure composed of vertex and edge, such as a factor graph provided by an embodiment of the present application.
  • the vertex can represent a normal point, and the vertex can also represent the observation time node of the sensor, that is, the sampling time of the observation data of the sensor.
  • a unary side and/or a binary side where the unary side refers to the side connecting a vertex.
  • one edge is connected to one or several vertices, and the edge can be used to represent a relationship between a vertex and a vertex.
  • the edge connecting a vertex is called a unary side, and the side connecting two vertices is called a binary side.
  • the one-dimensional pose observation is analogous to the one-yuan side, and the one-dimensional pose observation refers to the pose observation of the terminal at any sampling time.
  • a unitary pose can be understood as a pose that can be determined by data acquired at a single sampling time, or can be understood as a pose that can be determined by one frame of data, including a rotation matrix and a translation vector.
  • a sensor corresponding to a one-dimensional pose observation may be referred to as a first type of sensor.
  • the first type of sensor is used to acquire the rotation matrix and translation vector of the sensor (or the terminal equipped with the sensor) at any sampling instant.
  • GPS global positioning system
  • Binary pose observation is analogous to the binary edge.
  • Binary pose observation refers to the pose observation of the terminal at any two adjacent sampling moments.
  • a binary pose can be understood as a pose that requires at least two sampling moments to be determined, or can be understood as a pose that requires at least two frames of data to determine, including a rotation matrix and a translation vector.
  • the sensor corresponding to the binary pose observation can be referred to as the second type of sensor.
  • a second type of sensor is used to acquire the rotation matrix and translation vector of the relative motion of the sensor between any two adjacent sampling instants.
  • a radar sensor or a visual sensor can be used to acquire a rotation matrix and a translation vector of the relative motion of the terminal between two adjacent sampling instants.
  • Lie algebra refers to an important class of unbound algebra.
  • Lie algebra is a mathematical concept introduced by the Norwegian mathematician Sophie Lee in the study of continuous transform groups in the late 19th century. It is closely related to Li Qun's research.
  • Lie algebra is not only a tool for linearization of group theory problems, but also a source of many important problems in finite group theory and linear algebra.
  • Li Qun refers to a group of differential manifolds that can solve the rotational motion of a rigid body.
  • the group space provided in the embodiment of the present application refers to a space corresponding to the group of Li.
  • European space the space corresponding to European geometry is called European space.
  • European geometry is the law of the connection between the angle established by the ancient Greek mathematician Euclidean and the distance between the spaces.
  • the corresponding space is called the European space.
  • the method for processing sensor data provided by the embodiment of the present application is applicable to map construction of a terminal and positioning of a terminal.
  • the terminal may include a self-driving car, an intelligent robot, a drone, a tablet computer, a personal digital assistant (PDA), a mobile internet device (MID), a wearable device, and an e-book reader. (e-book reader) and other devices.
  • the terminal may also be a portable, pocket-sized, hand-held, computer-integrated or in-vehicle mobile device, which is not limited herein. For convenience of description, in the following description, the above-mentioned device will be described by taking a terminal as an example.
  • the map construction and/or positioning of the terminal is based on the fusion of the multi-sensor observation data to realize the pose estimation of the terminal.
  • the pose estimation of the terminal may include an estimation of a rotation matrix and a translation vector of a relative motion of the terminal between two time nodes (eg, two adjacent sampling moments of a certain second type of sensor), and/ Or an estimate of the rotation matrix and translation vector of the terminal at the current time node (eg, a sampling instant of a first type of sensor).
  • the optimization method based on the smoothing method is one of the main research methods used in current map construction and/or positioning.
  • Optimization methods based on smoothing methods such as nonlinear least squares optimization methods including g2o and isam, can adopt global optimization or overall consideration processing to obtain better composition effects, thus also making optimization methods based on smoothing method. It is called the main research method of map construction and/or location in large-scale environment.
  • the terminal provided by the embodiment of the present application can estimate the pose of the terminal by using a plurality of sensors assembled by the terminal, and can integrate the observation data of the plurality of sensors and optimize the fusion result of the observation data of the plurality of sensors.
  • the positioning map of the output terminal or the online pose estimation value of the terminal is processed.
  • FIG. 1 is a schematic diagram of multi-sensor observation of terminal pose estimation according to an embodiment of the present application.
  • the data fusion of the multi-sensor provided by the embodiment of the present application can be first modeled by means of a factor graph, and the change of the motion state of the terminal is represented by a factor graph.
  • the circle indicates the different locations where the terminal is located at different times.
  • the rotation matrix and translation vector of the terminal at the position indicated by each circle are the poses of the terminal at various positions.
  • the factor graph shown in FIG. 1 can be used to represent the observations of different sensors under the same time reference, that is, the factor graph shown in FIG.
  • FIG. 1 can also be a schematic diagram of the data relationship after the observation data of each sensor is aligned on the time axis.
  • multiple sensors mounted on the terminal include GPS sensors, radar, and visual odometers.
  • X1, X2, X3, and X4 represent four consecutive sampling moments of the reference sensor
  • G, V, and L are factors corresponding to three sensors of the GPS sensor, the visual odometer, and the radar, respectively.
  • the factors corresponding to the GPS sensor are unary side factors (eg, G1, G2, G3, and G4), that is, the GPS sensor can be the first type of sensor.
  • the factors corresponding to the visual odometer and radar are binary edge factors, such as V12, V23, V34, and L12, L23, and L34, and the radar and visual odometer can be the second type of sensor.
  • the rotation matrix and the translation vector of the position of the terminal at different times are collected by multiple sensors, and the process of terminal pose estimation can be realized according to the rotation matrix and the translation vector of the terminal at different positions, which can be understood as the terminal position based on motion perception.
  • Estimated posture The embodiment of the present application can implement the functions of map construction and terminal positioning (also referred to as determining the terminal online pose estimation value) based on the motion perception-based terminal pose estimation.
  • Embodiment 1 is a diagrammatic representation of Embodiment 1:
  • the embodiment of the present application provides a possible implementation manner for implementing map construction by multi-sensor observation data fusion. It can be understood that different sensors have different sampling frequencies, and the time delays of the observation data transmitted by different sensors to the data fusion center are also different. Therefore, there are multiple sensing processes in the fusion process of the observation data of multiple sensors. Observing the problem of asynchronous arrival of data. Among them, the asynchronous arrival of the observed data refers to the observation data of different sensors reaching the data fusion center at different times.
  • the above-mentioned data fusion center may be a module for realizing functions such as integration of observation data of a plurality of sensors in the terminal, and is not limited herein, and will not be described again.
  • the implementation provided by the embodiment of the present application can perform mathematical derivation and/or interpolation on the motion state data such as the rotation matrix and the translation vector of each sensor in the European space and the group space based on the motion perception.
  • the calculation is to transform the observation data of each sensor reaching the data fusion center at different times to the same time, thereby realizing the data fusion of the observation data of each sensor at the same moment.
  • the embodiment of the present application can globally optimize the fusion result of the observation data of the plurality of sensors according to the fusion of the observation data of the respective sensors on the same time reference and output a global 3D map.
  • the fusion of the observation data of the plurality of sensors supports the out-of-order input of the observation data of the plurality of sensors, the data processing mode is flexible, the map construction accuracy rate is higher, and the applicability is stronger.
  • FIG. 2 is a schematic diagram of a map construction manner based on multi-sensor data fusion provided by an embodiment of the present application.
  • three data processing processes of S1, S2, and S3 are mainly included.
  • S1 represents the input of observation data of a plurality of sensors, including data input of the sensor 1, the sensor 2, ..., and the sensor n.
  • the observation data of different sensors can be input to the data fusion center at different time nodes (for example, different moments), that is, the data fusion center supports the asynchronous arrival of the observation data of the plurality of sensors.
  • S2 represents data alignment of a plurality of sensors, that is, transforms observation data of different sensors under different time references into observation data of different sensors at the same time reference.
  • the process of data alignment of multiple sensors can also be referred to as sensor alignment. It can be understood that the sampling frequency of different sensors is different, so the collection time of the observation data collected by different sensors for determining the posture of the terminal is also different. Therefore, the observation data of different sensors input to the data fusion center at different times belong to the observation data under different time bases.
  • the data fusion center can perform mathematical derivation and/or interpolation calculation on the rotation matrix and translation vector of each sensor in the European space and the group space, so as to transform the observation data of each sensor reaching the data fusion center at different times to the same moment.
  • the observed data for each sensor can represent a pose estimate for the terminal.
  • the observation data of the sensor 1, the sensor 2, the ..., the sensor n can respectively represent the n pose estimation values of the terminal on the same time reference, including the pose estimation value 1, the pose estimation value 2, ..., the posture posture. Estimated value n.
  • the global optimization can adopt a nonlinear least squares optimization method based on the smoothing method, such as g2o.
  • FIG. 3 is a schematic flowchart of a method for processing sensor data provided by an embodiment of the present application.
  • the implementation described in each step in the method provided by the embodiment of the present application may be performed by a data fusion center of the terminal, and the method includes the following steps:
  • the posture change of the terminal may be observed during the movement of the terminal.
  • the sensors installed on the terminal include, but are not limited to, a GPS sensor, a visual odometer, and a radar, which may be determined according to actual application scenarios, and are not limited herein.
  • the embodiment of the present application will be described with an example of a GPS sensor, a visual odometer, and a radar as a plurality of sensors mounted on the terminal.
  • the GPS sensor is the first type of sensor
  • the visual odometer and radar are the second type of sensor.
  • the observation data collected by each sensor when observing the pose change of the terminal may be input to the data fusion center of the terminal.
  • the data fusion center can process the observation data of multiple sensors to achieve map construction.
  • the data fusion center may determine a reference sensor from the plurality of sensors, and use the sampling frequency of the reference sensor as a reference to convert the observation data of the sensors of different sampling frequencies into an interpolation calculation and/or a mathematical derivation. Observation data presented at the same sampling frequency.
  • each sampling time of the reference sensor can be determined, and each sampling time of the reference sensor is used as an alignment time of the observation data of the multi-sensor.
  • the observation data of the input other than the reference sensor at an adjacent alignment time is transformed into the alignment time by interpolation calculation and/or mathematical derivation. Observation data.
  • the data fusion center only needs to fuse the observation data at each sampling time of the reference sensor to realize the fusion of the observation data of the multi-sensor, and the operation is simple and the data processing efficiency is high.
  • the data fusion center can monitor the input of the observation data of each sensor after each sensor confirms that the working state is normal, and then can perform data alignment on any observation data input by any sensor.
  • the observation data input by each sensor is converted into observation data at the corresponding alignment time.
  • Steps S14 and S15 are performed if the type of the sensor K is the first type, and steps S16 and S17 are performed if the type of the sensor K is the second type.
  • the data fusion center when the data fusion center receives the observation data input by any sensor (for example, the sensor K) at a certain time (for example, t 1 ), it first determines which type of the sensor K is determined.
  • the data processing method performs data alignment on the observed data.
  • the observation data input by the first type of sensor may adopt an implementation manner of a one-dimensional pose observation (for convenience of description, the following will be described by taking a one-dimensional pose calculation method as an example), and the observation data of the sensor is calculated to correspond to the reference.
  • the pose of the sensor at the sampling moment (including the rotation matrix and the translation vector).
  • the observation data input by the second type of sensor can adopt the implementation method of binary pose observation (for convenience of description, the following will be described by taking the binary pose calculation as an example), and the observation data of the sensor is calculated corresponding to the reference sensor.
  • the rotation matrix and translation vector at the sampling instant is the implementation manner of a one-dimensional pose observation.
  • the data fusion center may calculate the rotation matrix and the translation vector of the observation data of the first type of sensor corresponding to the sampling moment of the reference sensor by using a one-dimensional pose calculation method.
  • the calculation process may include the following steps:
  • the sampling time T 1 closest to the distance t 1 is determined from each sampling time of the reference sensor.
  • T 1 may also be referred to as the nearest neighbor timestamp of t 1 or the nearest neighbor sampling time.
  • the interpolation coefficient interpolation on a sampling interval T 1 ⁇ 1 and t 1 is determined in accordance with the first sensor.
  • the terminal-assembled sensor may include one or more first-type sensors, which are not limited herein.
  • the first type of sensor will be described by taking the first sensor as an example.
  • each acquisition data is collected at a sampling time, such as a GPS sensor, and the observation data collected at that time is given at each moment.
  • the data fusion center may acquire the observation data at the sampling moment after the first sensor acquires the observation data at any sampling time (for example, t 2 ).
  • the data fusion center can calculate the t 1 and t 2 based on the observation data input on t 1 and the observation data on t 2 stored in advance.
  • Observation data (including rotation matrix and translation vector) at the sampling instant (for example, T 1 ) between the reference sensors. That is, t 2 is the nearest neighbor sampling instant of t 1 .
  • FIG. 4 is a schematic diagram of an interpolation manner provided by an embodiment of the present application.
  • the data fusion center may first determine that each of the sampling moments of the first sensor is adjacent to t 1 and is at t 1 according to a sampling interval of the first sensor (eg,
  • t 1 and t 2 are two consecutive observation time nodes, T 1 is a certain time node between t 1 and t 2 , and the time node is a sampling moment of the reference sensor.
  • Data fusion center interpolation coefficient may be calculated at interpolation based on 1 T t 1 and t 2, T 1, for example ⁇ 1.
  • t 1 and t 2 are two consecutive unary pose observation time nodes of the first sensor, and T 1 is a target time node interpolated between t 1 and t 2 .
  • the first T 1 and the first rotation data translation data in the first sensor to the observed data is calculated according to [lambda] 1 and the first sensor.
  • the data of the observation data on t 1 and t 2 can be deduced in the European space and the Lie group space to obtain the first The rotation matrix and translation vector of the observation data of a sensor on T 1 .
  • data fusion center rotation matrix can be obtained on the t 1 a first sensor (e.g. G 1 R), and a translation vector (e.g., G p 1), a first sensor and a rotation matrix, for example, in the t 2 And translation vectors (such as G p 2 ).
  • the data fusion center can be based on ⁇ 1 and And G p 2 calculating the first rotation data of the first sensor on T 1 (eg, the first rotation matrix And the first translation data (eg, the first translation vector G p T1 ).
  • G p T1 meets:
  • G p T1 (1 - ⁇ 1) G p 2 + ⁇ 1 G p 1 ;
  • the data fusion center obtains the rotation matrix and the translation vector on the upper T 1 , and can also calculate the covariance matrix corresponding to the rotation matrix and the translation vector on T 1 (ie, the first covariance matrix).
  • the first covariance matrix corresponding to the rotation matrix and the translation vector on T 1 above can be understood as the reliability coefficient of the rotation matrix and the translation vector of the first sensor on T 1 in the data fusion process of the multi-sensor. , or the accuracy factor.
  • the credibility coefficient or the accuracy coefficient can also be understood as a weight.
  • the rotation matrix and the translation vector fused on T 1 include a rotation matrix and a translation vector of a plurality of sensors
  • the rotation matrix and the translation vector of the sensor may be determined according to the covariance matrix corresponding to the rotation matrix and the translation vector of each sensor.
  • the rotation matrix and the translation vector of each sensor at the time (T 1 ) and the reliability thereof the rotation matrix and the translation vector of the plurality of sensors are merged, so that the terminal can be more accurately determined at the time (T 1 ).
  • the rotation matrix and translation vector on the top
  • the data fusion center can calculate the pose of the first sensor on t 1 (for example Corresponding covariance matrix P t1 and calculate the pose of the first sensor on t2 (eg Corresponding covariance matrix P t2 .
  • the first rotation data and the first translation data for example Calculating a Jacobian matrix (denoted as H u 1), and calculating a covariance matrix P T1 corresponding to the first rotation matrix and the first translation vector according to P t1 and P t2 .
  • P 1,2 represents P t1 P t2
  • O 3*3 represents the all-zero matrix of 3*3 and 6*6
  • I represents the identity matrix
  • i represents T 1
  • G represents the global coordinate system
  • ⁇ represents the interpolation
  • the coefficients ⁇ 1 , Jr are the right Jacobian matrix
  • Logv represents the logarithm of the matrix.
  • the data fusion center may calculate the rotation matrix and the translation vector of the observation data of the second type of sensor corresponding to the sampling moment of the reference sensor by using a binary pose calculation method.
  • the calculation process may include the following steps:
  • two sampling times T 2 and T 3 closest to the distance t 1 are determined from the respective sampling times of the reference sensor.
  • T 2 and T 3 the two sampling instants closest to the distance t 1 can be denoted as T 2 and T 3 .
  • T 2 may represent t nearest neighbor sampling timing 1
  • T 3 may represent a t-th nearest neighbor sampling time 1, i.e., T 3 from t length of time t 1 after the time length of a distance T 2 distance, but Other sampling moments are the sampling moments of the next nearest neighbors before the length of time t 1 .
  • T 3 may be the sampling time closest to the sampling time t 2 before the distance t 1 in each sampling time of the reference sensor.
  • T 1 and T 2 described in the embodiments of the present application may be the same sampling moment, that is, the nearest neighbor sampling moments of t 1 .
  • the second sensor determines the interpolation coefficient of the interpolation T 2 ⁇ 2 when the interpolation and interpolation coefficients according to the 3 T ⁇ 3.
  • the terminal-assembled sensor may include one or more second-type sensors, which are not limited herein.
  • the second type of sensor will be described by taking the second sensor as an example.
  • observing the motion state of the terminal requires acquiring two sets of observation data of the terminal (for example, two sets of rotation matrix and translation vector) at two consecutive sampling moments, and obtaining the terminal in the two through the operation of two sets of observation data.
  • the rotation matrix and the translation vector of the terminal output by the second sensor are a rotation matrix and a translation vector of the relative motion of the terminal between two adjacent sampling moments.
  • FIG. 5 is another schematic diagram of an interpolation manner provided by an embodiment of the present application.
  • the data fusion center may acquire the observation data at the sampling time after acquiring the observation data of the second sensor at any sampling time (for example, t 2 ).
  • the data fusion center can calculate the rotation from t 1 and t 2 based on the observation data input on t 1 and the observation data on t 2 stored in advance.
  • Matrix and translation vector for example,
  • the data center may first fusion root sensor according to a second sampling interval (e.g.,
  • a second sampling interval e.g.,
  • ⁇ 3 meets:
  • t 1 and t 2 are two consecutive binary pose observation time nodes of the second sensor
  • T 2 is the sampling moment of the reference sensor closest to the distance t 1
  • T 3 is the sampling of the reference sensor closest to the distance t2 1 time.
  • the data fusion center available in t 1 and a second sensor rotation matrix (e.g., 2 1 R) between the 2 t, and a second sensor is a translation vector between t 1 and t 2 are (e.g. 2 p 1 ), and according to ⁇ 2 , ⁇ 3 , And 2 p 1 calculate second rotation data of the relative motion of the second sensor between T 2 and T 3 (eg second rotation matrix And second translation data (eg second translation vector T3 p T2 ).
  • T3 p T2 meets:
  • the data fusion center may calculate a rotation matrix corresponding to the second sensor between t 1 and t 2 and a covariance matrix P t12 corresponding to the translation vector, and calculate a Jacobian matrix according to the second rotation matrix and the second translation vector ( For example, Hu2), and the covariance matrix P T12 corresponding to the second rotation matrix and the second translation vector is calculated according to P t12 .
  • O 3*3 represents an all-zero matrix of 3*3
  • b represents T 3
  • e represents T 2
  • ⁇ b ⁇ e represents interpolation coefficients ⁇ 3 and ⁇ 2 , respectively
  • Jr is Right Jacques Ratio matrix
  • Logv represents the logarithm of the matrix
  • the data fusion center may fuse the rotation matrix and the translation vector of each sensor at each sampling moment of the reference sensor based on a smoothing manner to obtain a pose estimation value of the terminal at each sampling moment.
  • the fused rotation data includes at least first rotation data and/or second rotation data
  • the fused translation data includes at least first translation data and/or second translation data
  • the fused covariance matrix includes at least a first covariance matrix , and / or a second covariance matrix.
  • the data fusion center can globally optimize the pose estimation value of the terminal obtained by the fusion at each sampling moment of the reference sensor, and output the verification map constructed under the global coordinates.
  • the verification map is used to provide reference data for determining the online pose estimation value of the subsequent terminal.
  • the embodiment of the present application can transform the rotation data, the translation data, and/or the covariance matrix input by each sensor to the corresponding alignment time according to the type of each sensor, and by rotating data, translation data, and / or covariance matrix fusion, optimize the verification map in the output global coordinate system.
  • the online pose estimation of the terminal can be completed without GPS sensor input.
  • the fusion of the observation data of the plurality of sensors supports the out-of-order input of the observation data of the plurality of sensors, the data processing mode is flexible, the map construction accuracy rate is higher, and the applicability is stronger.
  • Embodiment 2 is a diagrammatic representation of Embodiment 1:
  • the embodiment of the present application provides a possible implementation manner for realizing online positioning of a terminal through multi-sensor observation data fusion.
  • Online positioning can also be referred to as online pose estimation.
  • the fusion of observation data of different positions of the terminal at different times does not depend on the observation data input of the first type of sensor (for example, a GPS sensor).
  • the observation data collected by the second type of sensor can be matched with the verification map to obtain an initial estimation value of the online pose estimation of the terminal, which can be used as reference data for the online pose estimation value.
  • the initial estimate of the online pose estimation of the terminal is a more accurate observation data.
  • the online positioning of the terminal has higher requirements for real-time performance.
  • the real-time input observation of each sensor can be further performed.
  • the data is optimized to output a more accurate online pose estimate.
  • the data fusion center can fuse the observation data obtained by matching the verification map with the observation data collected by each second type of sensor in real time, and perform incremental smooth iterative optimization on the fusion result of the observation data of multiple sensors.
  • the online 3D pose estimation value of the current terminal is estimated in real time, the operation is convenient, the online pose estimation accuracy is high, and the applicability is stronger.
  • FIG. 6 is a schematic diagram of an online positioning manner based on multi-sensor data fusion according to an embodiment of the present application.
  • the four data processing procedures of S4, S5, S6, and S7 may also be included in the map construction mode shown in FIG. 6.
  • S4 represents the input of the observation data of the plurality of sensors, including the data input of the sensor 1, the sensor 2, ..., the sensor n.
  • the observation data of different sensors can be input to the data fusion center at different time nodes (for example, different moments), that is, the data fusion center supports the asynchronous arrival of the observation data of the plurality of sensors.
  • the verification data input of the S5 verification map includes, but is not limited to, reference data obtained by matching the observation data of the sensor 1 with the verification map, reference data obtained by matching the observation data of the sensor 2 with the verification map, and the sensor n.
  • the S6 indicates the data alignment of the multiple sensors.
  • the observed data for each sensor can represent a pose estimate for the terminal.
  • the observation data of sensor 1, sensor 2, ..., sensor n can represent the n pose estimates of the terminal at the same time reference, including pose estimation value 1, pose estimation value 2, ..., pose estimation.
  • the value is n.
  • the incremental smoothing iteration can use the nonlinear least squares optimization method based on the smoothing method, such as iasm.
  • FIG. 7 is another schematic flowchart of a method for processing sensor data provided by an embodiment of the present application.
  • the method provided in the embodiment of the present application includes the following steps:
  • the terminal may generate a verification map by using the implementation manner described in the foregoing embodiment, and store the verification map in a specified storage space of the terminal.
  • the verification map can be obtained from the specified storage space, and the verification map is used as reference data for estimating the online pose of the terminal.
  • the implementation of the data fusion center to monitor the input of the observation data of each sensor can be referred to the implementation manner described in step S12 in the first embodiment, and details are not described herein again.
  • step S73 when any one time during either a sensor 3 acquires observation data L t, L sensor type determining sensor. If the type of the sensor L is the first type, step S74 is performed, and if the type of the sensor L is the second type, steps S75 and S76 are performed.
  • the data fusion center when the data fusion center receives the observation data input by any sensor (for example, the sensor L) at a certain time (for example, t 3 ), it first determines which type of the sensor L is used. The data processing method performs data alignment on the observed data.
  • the sensor K and the sensor L may also be the same sensor, which are not limited herein.
  • the observation data input by the first type of sensor may adopt an implementation manner of a one-dimensional pose observation (for convenience of description, the following will be described by taking a one-dimensional pose calculation method as an example), and the observation data of the sensor is calculated to correspond to the reference.
  • a one-dimensional pose observation for convenience of description, the following will be described by taking a one-dimensional pose calculation method as an example
  • the observation data of the sensor is calculated to correspond to the reference.
  • the pose of the sensor at the sampling moment including the rotation matrix and the translation vector
  • the data fusion center may also match the observation data of the sensor L with the verification map, and calculate and match the observed observation data according to the implementation method of the one-dimensional pose observation.
  • the data is aligned to the pose of the reference sensor at the sampling instant (including the rotation matrix and the translation vector).
  • the rotation matrix and the translation vector of the reference data at the sampling time of the reference sensor refer to the implementation described in step S14 in the first embodiment, and details are not described herein.
  • the data fusion center can adopt the implementation method of the binary pose observation (for convenience of description, the following will be described by taking the binary pose calculation as an example) to calculate the observation data of the sensor. Corresponds to the rotation matrix and translation vector at the sampling instant of the reference sensor.
  • the first type of sensor herein will be described by taking a third sensor as an example.
  • the first sensor and the third sensor may also be the same sensor, and are not limited herein.
  • the data fusion center may determine the sampling time closest to the distance t 3 (for example, T 4 ) from each sampling time of the reference sensor, and calculate the third sensor to be on the T 4
  • the rotation data eg, the fourth rotation data
  • the translation data e.g, the fourth translation data
  • the covariance e.g, the fourth covariance matrix
  • the implementation manner of calculating the fourth rotation data, the fourth translation data, and/or the fourth covariance matrix by the data fusion center may be referred to the implementation manner described in each step of steps S14 and S15 in the first embodiment. This will not be repeated here.
  • the rotation data may be a rotation matrix
  • the translation data may be a translation vector.
  • a fourth sensor will be described as an example of the second type of sensor.
  • the second sensor and the fourth sensor may also be the same sensor, and are not limited herein.
  • the data fusion terminal may match the observation data of the fourth sensor on t 3 with the verification map, and determine the reference rotation data and the reference translation data of the fourth sensor on t 3 .
  • reference a rotation matrix and/or a reference translation vector.
  • the data fusion center may match the currently acquired fourth sensor observation data with the verification map, and input the matching result into a processing process of the one-dimensional pose calculation method, for example,
  • the implementations described in the steps a1 to c1 in the first embodiment and the implementations described in the step S15 in the first embodiment are not described herein again.
  • the data fusion center may calculate rotation data (fifth rotation data, such as a fifth rotation matrix) and translation data of the fourth sensor on T 4 according to the reference rotation matrix and/or the reference translation vector of the fourth sensor on t 3 ( a fifth translation data, such as a fifth translation vector, and a covariance matrix corresponding to the fifth rotation data and the fifth translation data (eg, a fifth covariance matrix).
  • the data fusion center may determine two sampling moments closest to the distance t 3 from each sampling moment of the reference sensor, such as T 5 and T 6 , where T 5 represents the nearest neighbor of t 3 sampling time, T 6 represents the next nearest neighbor sampling time t 3, e.g. adjacent sampling instants t 3 and t 3 before (e.g. t. 4) nearest neighbor sampling time.
  • the fourth sensor sampling interval, and t 3 is determined t 4, and the interpolation coefficient (e.g., ⁇ 5) when interpolation in interpolation coefficients T 5 and 6 T at the interpolation (e.g., ⁇ 6), and in accordance with ⁇ 5
  • the observation data of ⁇ 6 and the fourth sensor on t 3 and t 4 calculate rotation data (sixth rotation data, such as sixth rotation matrix) and translation data (sixth translation) of the fourth sensor from T 5 to T 6 Data, such as a sixth translation vector, calculates a covariance matrix (eg, a sixth covariance) corresponding to the sixth rotation data and the sixth translation data.
  • the data fusion center calculates that the observation data of the second type of sensor corresponds to the rotation data and the translation data (for example, the rotation matrix and the translation vector) at the sampling moment of the reference sensor, and the implementation manner of the corresponding covariance matrix can be seen.
  • the implementations described in the foregoing steps S16 and 17 in the first embodiment are not described herein again.
  • fusion rotation data on each sampling time comprises 6 including T 4, T 5, and T is a reference sensor, and translating the data covariance matrix for the terminal to obtain online pose estimation value.
  • the data fusion center may fuse the rotation matrix, the translation vector, and/or the covariance matrix of each sensor at each sampling moment of the reference sensor based on the smoothing manner to obtain the terminal at each sampling moment.
  • Estimated pose The fused rotation data includes at least a fourth rotation data, a fifth rotation data or a sixth rotation data
  • the fused translation data includes at least a fourth translation data, a fifth translation data or a sixth translation data
  • the fused covariance matrix is at least A fourth covariance matrix, a fifth covariance matrix, or a sixth covariance matrix is included.
  • the data fusion center can match the observation data of the sensor with the verification map, fuse the observed observation data with the observation data collected by each sensor in real time, and fuse the observation data of multiple sensors.
  • the result is incremental iterative optimization, and finally estimates the online pose estimation of the current terminal in real time, which is convenient to operate, with high accuracy of online pose estimation and strong applicability.
  • FIG. 8 is a schematic structural diagram of an apparatus for processing sensor data according to an embodiment of the present application.
  • the device for processing sensor data provided by the embodiment of the present application may include:
  • the determining unit 81 is configured to determine one reference sensor from at least two sensors in the terminal, and determine each sampling moment of the reference sensor.
  • the obtaining unit 82 is configured to acquire observation data of any sensor.
  • Determining unit 81, t 1 for further acquisition unit 82 acquires the observation data to either a sensor K, and K is determined that the sensor type of sensor at any one time.
  • the determining unit 81 is further configured to: when the sensor K is the first sensor of the first type, determine the sampling time T 1 closest to the distance t 1 from each sampling moment of the reference sensor, according to the sampling interval of the first sensor and t 1 The interpolation coefficient ⁇ 1 when interpolating on T 1 is determined.
  • the calculating unit 83 is configured to determine the ⁇ 1 determined by the unit 81 and the observation data of the first sensor acquired by the obtaining unit 82, calculate the first rotation data and the first translation data of the first sensor at T 1 , calculate the first rotation data, and calculate A first covariance matrix corresponding to the translation data.
  • the determining unit 81 is further configured to: when the sensor K is the second sensor of the second type, determine two sampling times T 2 and T 3 closest to the distance t 1 from each sampling moment of the reference sensor, according to the second sensor sampling interval t 1, and determines the interpolation coefficient of the interpolation and T 2 ⁇ 2 when interpolating the interpolation coefficients a 3 T ⁇ 3.
  • the calculating unit 83 is further configured to calculate, according to the observation data of ⁇ 2 , ⁇ 3 and the second sensor determined by the determining unit 81, the second rotation data and the second translation data of the second sensor from T 2 to T 3 , and calculate the second Rotating the second covariance matrix corresponding to the second translation data.
  • the data fusion unit 84 is configured to fuse the rotation data, the translation data, and the covariance matrix at each sampling moment of the reference sensor including the T 1 , T 2 , and T 3 processed by the calculation unit 83 to obtain the terminal in each Estimated pose at the sampling moment;
  • the fused rotation data includes at least first rotation data and/or second rotation data
  • the fused translation data includes at least first translation data and/or second translation data
  • the fused covariance matrix includes at least a first covariance matrix , and / or a second covariance matrix.
  • the determining unit 81 is configured to:
  • the observation data of the first sensor includes a rotation matrix and a translation vector of the first sensor; and an acquisition unit 82 is configured to acquire a rotation matrix of the first sensor on t 1 And translation vector G p 1 , and the rotation matrix of the first sensor on t 2 And the translation vector G p 2 .
  • a calculating unit 83 configured to acquire ⁇ 1 according to the acquiring unit 81, G p 1 , And G p 2 calculate the first rotation matrix of the first sensor on T 1 And a first translation vector G p T1 ;
  • G p T1 meets:
  • G p T1 (1 - ⁇ 1 ) G p 2 + ⁇ 1 G p 1 ;
  • the first translation vector G p T1 is the first translation data.
  • the computing unit 83 is configured to:
  • P 1,2 represents P t1 P t2
  • O 3*3 represents the all-zero matrix of 3*3 and 6*6
  • I represents the identity matrix
  • i represents T 1
  • G represents the global coordinate system
  • ⁇ represents the interpolation
  • the coefficients ⁇ 1 , Jr are the right Jacobian matrix
  • Logv represents the logarithm of the matrix.
  • the determining unit 81 is configured to:
  • the second sensor sampling interval t 1 is determined prior to sampling time t 1 t 2, when the interpolation and interpolation coefficients in the T 2 ⁇ 2 interpolation and interpolation coefficient T3 on t 1 and t 2 are determined in accordance with ⁇ 3 according to Where ⁇ 2 satisfies:
  • ⁇ 3 meets:
  • T 2 represents the sampling time closest to the distance t 1 in the sampling time of the reference sensor
  • T 3 represents the sampling time closest to the distance t 2 in the sampling time of the reference sensor.
  • the observation data of the second sensor includes a rotation matrix and a translation vector of the second sensor; and an acquisition unit 82 is configured to acquire a rotation matrix of the second sensor between t 1 and t 2 And the translation vector 2 p 1 of the second sensor between t 1 and t 2 .
  • the calculating unit 83 is configured to acquire ⁇ 2 , ⁇ 3 according to the acquiring unit 82, And 2 p 1 to calculate a second rotation matrix of the relative motion of the second sensor between T 2 and T 3 And a second translation vector T3 p T2 ;
  • T3 p T2 meets:
  • the computing unit 83 is configured to:
  • O 3*3 represents an all-zero matrix of 3*3
  • b represents T 3
  • e represents T 2
  • ⁇ b and ⁇ e represent interpolation coefficients ⁇ 3 and ⁇ 2 , respectively
  • Jr is right Jacobian matrix
  • Logv represents the logarithm of the matrix
  • the foregoing apparatus further includes:
  • the map construction unit 85 is configured to: according to the rotation matrix and the translation vector at each sampling moment of the reference sensor including T 1 , T 2 and/or T 3 processed by the data fusion unit 84, and the terminal at each sampling moment
  • the pose estimate constructs a check map in the global coordinate system; wherein the check map is used to provide reference data for determining the online pose estimate of the terminal.
  • the foregoing apparatus further includes:
  • the startup unit 86 is configured to start an online positioning function of the terminal, and acquire a verification map of the terminal.
  • Determining unit 81 when the obtaining unit 82 is further configured to obtain either a 3 L sensor observations t, determining L at any one time sensor type sensor.
  • the determining unit 81 is further configured to determine the sampling time T 4 closest to the distance t 3 from each sampling time of the reference sensor when the sensor L is the third sensor of the first type.
  • the calculating unit 83 is further configured to calculate fourth rotation data and fourth translation data of the third sensor on T 4 , and a fourth covariance matrix corresponding to the fourth rotation data and the fourth translation data;
  • the determining unit 81 and the calculating unit 83 are also used to perform an operation:
  • the determining unit 81 is further configured to match the observation data of the fourth sensor on the t 3 with the verification map, determine the reference rotation data and the reference translation data of the fourth sensor on t 3 , from each sampling moment of the reference sensor Determining the sampling time T 4 closest to the distance t 3 ;
  • the determining unit 81 is further configured to determine two sampling times T 5 and T 6 closest to the distance t 3 from each sampling moment of the reference sensor, and determine the interpolation time on the T 5 according to the sampling interval of the fourth sensor and t 3 interpolation coefficient interpolation coefficients ⁇ 5 and 6 T at the time of interpolation on the ⁇ 6.
  • the calculating unit 83 is further configured to calculate sixth rotation data and sixth translation data of the fourth sensor from T 5 to T 6 according to the observation data of ⁇ 5 , ⁇ 6 and the fourth sensor, and calculate the sixth rotation data and the sixth a sixth covariance matrix corresponding to the translation data;
  • the data fusion unit 84 is further configured to fuse the rotation data and the translation data at each sampling moment of the reference sensor including T 4 , T 5 and T 6 processed by the calculation unit 83, and the covariance matrix to obtain the terminal online.
  • a pose estimation value wherein the fused rotation data includes at least a fourth rotation data, a fifth rotation data or a sixth rotation data, and the fused translation data includes at least a fourth translation data, a fifth translation data, or a sixth translation data, and the fusion
  • the covariance matrix includes at least the fourth covariance matrix, the fifth covariance matrix, or the sixth covariance matrix.
  • the first type of sensor including the first sensor and/or the third sensor, is used to acquire rotation data and translation data of the first type of sensor at any of its sampling instants.
  • a second type of sensor comprising a second sensor and/or a fourth sensor is used to acquire the relative motion of the second type of sensor between any two adjacent sampling instants thereof. Rotate data and pan data.
  • the apparatus for processing sensor data provided by the embodiment of the present application may also be the terminal provided by the embodiment of the present application.
  • the apparatus for processing sensor data provided by the embodiments of the present application may perform the implementations described in the foregoing implementations by using various units built therein, and details are not described herein again.
  • FIG. 9 is a schematic structural diagram of a communication device 40 according to an embodiment of the present application.
  • the communication device 40 provided by the embodiment of the present application includes a processor 401, a memory 402, a transceiver 403, and a bus system 404.
  • the processor 401, the memory 402 and the transceiver 403 are connected by a bus system 404.
  • the above memory 402 is used to store programs.
  • the program can include program code, the program code including computer operating instructions.
  • the memory 402 includes, but is not limited to, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read only memory (EPROM), or Portable disc read-only memory (CD-ROM). Only one memory is shown in Fig. 9, and of course, the memory can be set to a plurality as needed.
  • the memory 402 may also be a memory in the processor 401, which is not limited herein.
  • Memory 402 stores the following elements, executable modules or data structures, or subsets thereof, or their extended sets:
  • Operation instructions include various operation instructions for implementing various operations.
  • Operating system Includes a variety of system programs for implementing various basic services and handling hardware-based tasks.
  • the processor 401 controls the operation of the communication device 40.
  • the processor 401 may be one or more central processing units (CPUs).
  • CPUs central processing units
  • the CPU may be a single-core CPU. It can also be a multi-core CPU.
  • bus system 404 which may include, in addition to the data bus, a power bus, a control bus, a status signal bus, and the like.
  • bus system 404 may include, in addition to the data bus, a power bus, a control bus, a status signal bus, and the like.
  • bus system 404 may include, in addition to the data bus, a power bus, a control bus, a status signal bus, and the like.
  • various buses are labeled as bus system 404 in FIG. For ease of representation, only the schematic drawing is shown in FIG.
  • Processor 401 may be an integrated circuit chip with signal processing capabilities. In the implementation process, each step of the foregoing method may be completed by an integrated logic circuit of hardware in the processor 401 or an instruction in a form of software.
  • the processor 401 may be a general-purpose processor, a digital signal processing (DSP), an application specific integrated circuit (ASIC), a field-programmable gate array (FPGA), or Other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components.
  • DSP digital signal processing
  • ASIC application specific integrated circuit
  • FPGA field-programmable gate array
  • the general purpose processor may be a microprocessor or the processor or any conventional processor or the like.
  • the steps of the method disclosed in the embodiments of the present application may be directly implemented by the hardware decoding processor, or may be performed by a combination of hardware and software modules in the decoding processor.
  • the software module can be located in a conventional storage medium such as random access memory, flash memory, read only memory, programmable read only memory or electrically erasable programmable memory, registers, and the like.
  • the storage medium is located in the memory 402, and the processor 401 reads the information in the memory 402 in conjunction with its hardware to perform the steps of the method of sensor data processing described in the various embodiments above.
  • the program can be stored in a computer readable storage medium, when the program is executed
  • the flow of the method embodiments as described above may be included.
  • the foregoing storage medium includes various media that can store program codes, such as a ROM or a random access memory RAM, a magnetic disk, or an optical disk.

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Mathematical Physics (AREA)
  • Mathematical Analysis (AREA)
  • Computational Mathematics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • General Engineering & Computer Science (AREA)
  • Databases & Information Systems (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Software Systems (AREA)
  • Computing Systems (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Computation (AREA)
  • Algebra (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Evolutionary Biology (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Automation & Control Theory (AREA)
  • Mechanical Engineering (AREA)
  • Robotics (AREA)
  • Force Measurement Appropriate To Specific Purposes (AREA)
  • Health & Medical Sciences (AREA)
  • General Health & Medical Sciences (AREA)
  • Medical Informatics (AREA)
  • Multimedia (AREA)
  • Testing Or Calibration Of Command Recording Devices (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

本申请实施例提供一种传感器数据处理的方法及装置，该方法包括：从至少两个传感器中确定出基准传感器及其采样时刻；当时刻t 1获取到传感器K的观测数据时，确定传感器K的类型；若传感器K为第一类型的第一传感器，则确定出距离t 1最近的采样时刻T 1以及插值系数λ 1，并计算在T 1的第一旋转数据、第一平移数据以及第一协方差矩阵；若传感器K为第二类型的第二传感器，则确定出距离t 1最近的两个采样时刻T 2、T 3以及插值系数λ 2、λ 3，并计算第二传感器从T 2到T 3的第二旋转数据、第二平移数据以及第二协方差矩阵；融合各采样时刻上的旋转数据、平移数据和协方差矩阵。采用本申请实施例，可提高多传感器的数据融合的操作灵活性和数据处理效率。

Description

传感器数据处理的方法及装置 技术领域
本申请涉及传感器应用领域，尤其涉及一种传感器数据处理的方法及装置。
背景技术
自动驾驶汽车和智能机器人等智能终端是近年来备受广泛关注的新型终端。对于自动驾驶汽车等智能终端而言，传感器相当于它们的眼睛。例如，通过传感器，自动驾驶汽车能够识别道路、道路上的车辆、行人、障碍物和/或基础交通设施等。通过传感器实现智能终端的定位是当前备受关注的技术问题之一。此外，智能终端的发展趋势表明，仅使用一个传感器实现智能终端的定位将面临越来越多的挑战，因此，如何通过多种类型的传感器实现智能终端的定位成为当前亟待解决的技术问题之一。
然而，通过多个传感器实现智能终端的定位需要解决多个传感器的采集数据的融合，通过多个传感器的采集数据的融合实现智能终端的定位。现有技术采用基于容积卡尔曼滤波(cubature kalman filter，CKF)的方法实现非线性异步多传感器的数据融合。现有技术的数据融合方法只能对当前时刻做线性化处理，因此需要严格按照传感器的采集数据的时间顺序进行融合，数据处理方式固定，适用范围小。此外，现有技术的数据融合严格按照时间顺序的线性化处理还将带来误差的逐渐累积，使得智能终端的定位准确性低，适用性差。
发明内容
本申请实施例提供了一种传感器数据处理的方法及装置，可提高多个传感器的数据融合的操作灵活性，提高传感器的数据处理效率，进而可提高智能终端的定位准确性，适用性高。
第一方面提供了一种传感器数据处理的方法，该方法包括：从终端中的至少两个传感器中确定出一个基准传感器，并确定出该基准传感器的各采样时刻。当任一时刻t 1获取到任一传感器K的观测数据时，确定传感器K的传感器类型。这里，不同类型的传感器的观测数据可采用不同的数据处理方式进行处理，以确定各个传感器的观测数据对准到基准传感器的采样时刻上的旋转数据、平移数据和协方差矩阵等数据，数据处理方式更灵活。其中，若传感器K为第一类型的第一传感器，则从基准传感器的各采样时刻中确定出距离t 1最近的采样时刻T 1，根据第一传感器的采样间隔和t 1确定在T 1上插值时的插值系数λ 1，并根据λ 1和第一传感器的观测数据计算第一传感器在T 1的第一旋转数据和第一平移数据，计算第一旋转数据和第一平移数据对应的第一协方差矩阵。这里，传感器的观测数据包括传感器在任一时刻测量得到的终端的位姿，下面不再赘述。例如，传感器在某个采样时刻t i上测量得到的终端在某个坐标系下的旋转矩阵和平移向量，例如GPS传感器可用于采集终端在任一采样时刻的旋转矩阵和平移向量。可以理解，这里的第一类型的传感器用于采集(或者装配有该传感器的终端)在任一采样时刻上的旋转数据和平移数据。第一传感器仅 是第一类型的传感器的一个示例，第一类型的传感器包括但不限于第一传感器，在此不做限制。
若传感器K为第二类型的第二传感器，则从基准传感器的各采样时刻中确定出距离t 1最近的两个采样时刻T 2和T 3，根据第二传感器的采样间隔和t 1确定在T 2上插值时的插值系数λ 2和在T 3上插值时的插值系数λ 3，并根据λ 2、λ 3和第二传感器的观测数据计算第二传感器从T 2到T 3的第二旋转数据和第二平移数据，计算第二旋转数据和第二平移数据对应的第二协方差矩阵。这里，第二类型的传感器用于采集传感器在任意两个相邻的采样时刻之间的相对运动的旋转数据(例如旋转矩阵)和平移数据(例如平移向量)。例如雷达传感器或者视觉传感器，可用于采集终端在两个相邻的采样时刻之间的相对运动的旋转矩阵和平移向量。可以理解，第二传感器仅是第二类型的传感器的一个示例，第二类型的传感器包括但不限于第二传感器，在此不做限制。
在本申请实施例中，根据传感器的类型采用相应的方式计算得到各个传感器在基准传感器的采样时刻上的旋转数据(例如旋转矩阵)、平移数据(例如平移向量)和协方差矩阵之后，则可融合包括T 1、T 2和T 3在内的基准传感器的各采样时刻上的旋转数据、平移数据和协方差矩阵以得到终端在所述各采样时刻上的位姿估计值。其中，融合的旋转数据至少包括第一旋转数据和/或第二旋转数据，融合的平移数据至少包括第一平移数据和/或第二平移数据，融合的协方差矩阵至少包括第一协方差矩阵，和/或第二协方差矩阵。
在本申请实施例中，对于多传感器的观测数据异步到达等问题，本申请实施例提供的实现方式可基于运动感知在欧式空间和群空间上对各个传感器的旋转数据和平移数据等运动状态数据进行数学推导和/或插值计算，以将各个传感器在不同时刻到达数据融合中心的观测数据变换到同一个时刻上，进而可实现各个传感器的观测数据在该同一个时刻上的数据融合多个传感器的观测数据的融合支持多个传感器的观测数据的乱序输入，数据处理方式灵活，地图构建准确率更高，适用性更强。其中，通过多个传感器采集终端在不同时刻所处的位置的旋转数据和平移数据，并根据终端在不同位置上的旋转数据和平移数据实现终端位姿估计的过程即可理解为基于运动感知的终端位姿估计。
在一种可能的实现方式中，本申请实施例可根据第一传感器的采样间隔确定t 1之前的最近邻采样时刻t 2，并根据t 1和t 2计算在T 1上插值时的插值系数λ 1；
其中，λ 1满足：
Figure PCTCN2018107049-appb-000001
在一种可能的实现方式中，第一传感器的观测数据包括第一传感器的旋转矩阵和平移向量；其中根据λ 1和第一传感器的观测数据计算第一传感器在T 1的第一旋转数据和第一平移数据包括：获取第一传感器在t 1上的旋转矩阵
Figure PCTCN2018107049-appb-000002
和平移向量 Gp 1，以及第一传感器在t 2上的旋转矩阵
Figure PCTCN2018107049-appb-000003
和平移向量 Gp 2；根据λ 1、
Figure PCTCN2018107049-appb-000004
Gp 1、
Figure PCTCN2018107049-appb-000005
和 Gp 2计算第一传感器在T 1上的第一旋转矩阵
Figure PCTCN2018107049-appb-000006
和第一平移向量 Gp T1；
其中，
Figure PCTCN2018107049-appb-000007
满足：
Figure PCTCN2018107049-appb-000008
Gp T1满足：
Gp T1＝(1-λ 1) Gp 2+λ 1 Gp 1。
其中，第一旋转矩阵
Figure PCTCN2018107049-appb-000009
为第一旋转数据，第一平移向量 Gp T1为第一平移数据。
Figure PCTCN2018107049-appb-000010
表示在全局坐标系下第一传感器在T 1上的位姿，G表示全局坐标系，
Figure PCTCN2018107049-appb-000011
表示在全局坐标系下第一传感器在t 1上的位姿，
Figure PCTCN2018107049-appb-000012
表示在全局坐标系下第一传感器在t 2上的位姿。本申请实施例可在多传感器的观测数据的对准过程中，第一类型的传感器在邻近某个对准时刻的输入的观测数据通过一元位姿计算方式对应的插值计算和/或数学推导等方式变换为在该对准时刻上的观测数据。之后在数据融合阶段，只需将基准传感器的各个采样时刻上的观测数据进行融合即可实现多传感器的观测数据的融合，可支持多传感器的观测数据的异步到达，操作简单，数据处理效率高。
在一种可能的实现方式中，本申请实施例可计算第一传感器在t 1上的位姿对应的协方差矩阵P t1，并计算第一传感器在t 2上的位姿对应的协方差矩阵P t2；根据第一旋转矩阵和第一平移向量计算雅可比矩阵H u，并根据P t1和P t2计算第一旋转矩阵和第一平移向量对应的协方差矩阵P T1；其中，H u满足：
Figure PCTCN2018107049-appb-000013
所述P T1满足：
Figure PCTCN2018107049-appb-000014
其中，P 1,2表示P t1P t2，
Figure PCTCN2018107049-appb-000015
表示对旋转矩阵R的估计值，O 3*3、O 6*6表示3*3和6*6的全零矩阵，I表示单位矩阵，i代表T 1，G代表全局坐标系，λ表示插值系数λ 1，Jr为右雅克比矩阵，Logv代表矩阵的对数运算，
Figure PCTCN2018107049-appb-000016
分别为角度及位移的误差向量。本申请实施例可计算各个传感器在基准传感器的采样时刻上的位姿对应的协方差矩阵，进而可提高在通过多传感器的数据融合得到终端的位姿估计值时的准确率，适用性更高。
在一种可能的实现方式中，对应第二类型的传感器的观测数据的处理，本申请实施例在根据第二传感器的采样间隔和t 1确定在T 2上插值时的插值系数λ 2和在T 3上插值时的插值系数λ 3时具体可执行如下操作：根据第二传感器的采样间隔和t 1确定t 1之前的最近邻采样时刻t 2，并根据t 1和t 2确定在T 2上插值时的插值系数λ 2和在T 3上插值时的插值系数λ 3；
其中，λ2满足：
Figure PCTCN2018107049-appb-000017
λ3满足：
Figure PCTCN2018107049-appb-000018
其中，T 2表示基准传感器的采样时刻中距离t 1最近的采样时刻，T 3表示基准传感器的采样时刻中距离t 2最近的采样时刻。
在一种可能的实现方式中，本申请实施例提供的第二传感器的观测数据可包括第二传感器的旋转矩阵和平移向量。本申请实施例根据λ 2、λ 3和第二传感器的观测数据计算第二传感器从T 2到T 3的第二旋转数据和第二平移数据时可执行如下操作：获取第二传感器在t 1和t 2之间的旋转矩阵
Figure PCTCN2018107049-appb-000019
以及第二传感器在t 1和t 2之间的平移向量 2p 1；根据λ 2、λ 3、
Figure PCTCN2018107049-appb-000020
和 2p 1计算第二传感器在T 2和T 3之间的相对运动的第二旋转矩阵
Figure PCTCN2018107049-appb-000021
和第二平移向量 T3p T2；
其中，
Figure PCTCN2018107049-appb-000022
满足：
Figure PCTCN2018107049-appb-000023
T3p T2满足：
其中，第二旋转矩阵
Figure PCTCN2018107049-appb-000025
为第二旋转数据，第二平移向量 T3p T2为第二平移数据。
在一种可能的实现方式中，本申请实施例还可计算第二传感器在t 1和t 2之间的旋转矩阵和平移向量对应的协方差矩阵P t12；根据第二旋转矩阵和第二平移向量计算雅可比矩阵H u，并根据P t12计算第二旋转矩阵和第二平移向量对应的协方差矩阵P T12；
其中，H u满足：
Figure PCTCN2018107049-appb-000026
P T12满足：
Figure PCTCN2018107049-appb-000027
其中，
Figure PCTCN2018107049-appb-000028
表示对旋转矩阵R的估计值，O 3*3表示3*3的全零矩阵，b代表T 3,e代表T 2，λ b和λ e分别表示插值系数λ 3和λ 2，Jr为右雅克比矩阵，Logv代表矩阵的对数运算，
Figure PCTCN2018107049-appb-000029
分别为角度及位移的误差向量。
本申请实施例可在多传感器的观测数据的对准过程中，第二类型的传感器在邻近某个对准时刻的输入的观测数据通过二元位姿计算方式对应的插值计算和数学推导等方式变换为在该对准时刻上的观测数据。之后在数据融合阶段，只需将基准传感器的各个采样时刻上的观测数据进行融合即可实现多传感器的观测数据的融合，可支持多传感器的观测数据的异步到达，操作更灵活，数据处理效率更高。
在一种可能的实现方式中，本申请实施例还可根据包括所述T 1、T 2和/或T 3在内的所述基准传感器的各采样时刻上的旋转矩阵和平移向量，以及所述终端在所述各采样时刻上 的位姿估计值在全局坐标系下构建校验地图；
其中，校验地图可用于为终端的在线位姿估计值的确定提供参考数据。
本申请实施例可根据各个传感器的类型对各个传感器输入的旋转数据、平移数据和/或协方差矩阵变换至相应的对准时刻上，并通过对各个对准时刻上的旋转数据、平移数据和/或协方差矩阵进行融合，优化输出全局坐标系下的校验地图。通过校验地图可实现在无GPS传感器输入的情况下完成终端的在线位姿估计。在本申请实施例中，多个传感器的观测数据的融合支持多个传感器的观测数据的乱序输入，数据处理方式灵活，地图构建准确率更高，适用性更强。
在一种可能的实现方式中，本申请实施例还可启动终端的在线定位功能，并获取终端的校验地图；当任一时刻t 3获取到任一传感器L的观测数据时，确定传感器L的传感器类型；若传感器L为第一类型的第三传感器，则从基准传感器的各采样时刻中确定出距离t 3最近的采样时刻T 4，并计算第三传感器在T 4上的第四旋转数据和第四平移数据，以及第四旋转数据和第四平移数据对应的第四协方差矩阵；若传感器L为第二类型的第四传感器，则执行如下步骤a和步骤b：
a、将第四传感器在t 3上的观测数据与校验地图进行匹配，确定第四传感器在t 3上的参考旋转数据和参考平移数据；从基准传感器的各采样时刻中确定出距离t 3最近的采样时刻T 4并根据第四传感器在t 3上的参考旋转数据和参考平移数据计算第四传感器在T 4上的第五旋转数据和第五平移数据，以及第五旋转数据和第五平移数据对应的第五协方差矩阵；
b、从基准传感器的各采样时刻中确定出距离t 3最近的两个采样时刻T 5和T 6，根据第四传感器的采样间隔和t 3确定在T 5上插值时的插值系数λ 5和在T 6上插值时的插值系数λ 6，并根据λ 5、λ 6和第四传感器的观测数据计算第四传感器从T 5到T 6上的第六旋转数据和第六平移数据，计算第六旋转数据和第六平移数据对应的第六协方差矩阵；
融合包括T 4、T 5和T 6在内的基准传感器的各采样时刻上的旋转数据、平移数据和协方差以得到终端的在线位姿估计值；其中，融合的旋转数据至少包括第四旋转数据、第五旋转数据或第六旋转数据，融合的平移数据至少包括第四平移数据、第五平移数据或第六平移数据，融合的协方差矩阵至少包括第四协方差矩阵、第五协方差矩阵，或第六协方差矩阵。
在本申请实施例中，可将传感器的观测数据与校验地图进行匹配，将匹配得到的观测数据与各个传感器实时采集的观测数据进行融合，并对多个传感器的观测数据的融合结果进行增量平滑迭代优化，最终实时估计出当前终端的在线位姿估计值，操作方便，在线位姿估计精度高，适用性更强。
在一种可能的实现方式中，包括第一传感器和/或第三传感器在内的第一类型的传感器用于采集第一类型的传感器在其任一采样时刻上的旋转数据和平移数据。例如全球定位***(global positioning system，GPS)传感器可用于采集终端在任一采样时刻的旋转矩阵和平移向量。
在一种可能的实现方式中，包括第二传感器和/或第四传感器在内的第二类型的传感器用于采集第二类型的传感器在其任意两个相邻的采样时刻之间的相对运动的旋转数据和平移数据。例如雷达传感器或者视觉传感器，可用于采集终端在两个相邻的采样时刻之间 的相对运动的旋转矩阵和平移向量。
在一种可能的实现方式中，本申请实施例提供的位姿，包括位置和姿态。其中，位置指坐标系中x，y，z三个方向的平移，姿态为坐标系中x，y，z三个方向的旋转。位姿可通过包括在指定坐标系的x，y，z三个方向的平移向量，和在该指定坐标系的x，y，z三个方向的旋转矩阵表示。
传感器的观测数据，包括传感器在任一时刻测量得到的终端的位姿。例如，传感器在某个采样时刻t i上测量得到的终端在某个坐标系下的旋转矩阵和平移向量。
传感器的数据对准，指的是将不同传感器在不同时间基准下的观测数据变换为不同传感器在同一时间基准下的观测数据的过程。其中，同一时刻不同传感器的观测属于同一时间基准下的观测，此时，不同传感器的观测数据属于同一个时间基准下的观测数据。不同时刻不同传感器的观测属于不同时间基准下的观测，此时，不同传感器的观测数据属于不同时间基准下的观测数据。其中，同一时间基准可以理解为同一采样频率或者同一采样周期等，该采样频率或者采用周期可以理解为基准传感器的采样频率或者采样频率。同一时刻不同传感器的观测可以理解为采样频率相同的传感器在相同的采样时刻上的观测，不同时刻不同传感器的观测可以理解为采样频率不同的传感器在不同时刻上的观测。
第二方面提供了一种传感器数据处理的方法，该包括：从终端中的至少两个传感器中确定出一个基准传感器，并确定出基准传感器的各采样时刻；当任一时刻t 1获取到任一传感器K的观测数据时，确定传感器K的传感器类型；若传感器K为第一类型的第一传感器，则从基准传感器的各采样时刻中确定出距离t 1最近的采样时刻T 1，并获取第一传感器在t 1之前的采样时刻t 2及其对应的观测数据，根据t 1和t 2确定在T 1上插值时的插值系数λ 1，根据λ 1和第一传感器在t 1和t 2的观测数据计算第一传感器在T 1的第一旋转数据和第一平移数据，计算第一旋转数据和第一平移数据对应的第一协方差矩阵；若传感器K为第二类型的第二传感器，则从基准传感器的各采样时刻中确定出距离t 1最近的两个采样时刻T 2，获取第二传感器在t 1之前的采样时刻t 2及其对应的观测数据；从基准传感器的各采样时刻中确定出距离t 2最近的采样时刻T 3，根据t 2和t 1确定在T 2上插值时的插值系数λ 2和在T 3上插值时的插值系数λ 3，并根据λ 2、λ 3和第二传感器的观测数据计算第二传感器从T 2到T 3的第二旋转数据和第二平移数据，计算第二旋转数据和第二平移数据对应的第二协方差矩阵；融合包括T 1、T 2和T 3在内的基准传感器的各采样时刻上的旋转数据、平移数据和协方差矩阵以得到终端在各采样时刻上的位姿估计值；其中，融合的旋转数据至少包括第一旋转数据和/或第二旋转数据，融合的平移数据至少包括第一平移数据和/或第二平移数据，融合的协方差矩阵至少包括第一协方差矩阵，和/或第二协方差矩阵。
在一种可能的实现方式中，本申请实施例还可启动终端的在线定位功能，并获取终端的校验地图；当任一时刻t 3获取到任一传感器L的观测数据时，确定传感器L的传感器类型；若传感器L为第一类型的第三传感器，则从基准传感器的各采样时刻中确定出距离t 3最近的采样时刻T 4，获取第三传感器在t 3之前的采样时刻t 4及其对应的观测数据；根据t 3和t 4确定在T 4上插值时的插值系数，并根据插值系数和第三传感器在t 3和t 4上的观测数据，计算第三传感器在到T 4上的第四旋转数据和第四平移数据，以及第四旋转数据和第四平移数据对应的第四协方差矩阵；若传感器L为第二类型的第四传感器，则执行如下步骤a和步骤b：
a、将第四传感器在所述t 3上的观测数据与校验地图进行匹配，确定第四传感器在t 3上的参考旋转数据和参考平移数据；从基准传感器的各采样时刻中确定出距离t 3最近的采样时刻T 4，并根据第四传感器在t 3上的参考旋转数据和参考平移数据计算第四传感器在T 4上的第五旋转数据和第五平移数据，以及第五旋转数据和第五平移数据对应的第五协方差矩阵；
b、从基准传感器的各采样时刻中确定出距离t 3最近的两个采样时刻T 5，获取第四传感器在t 3之前的采样时刻t 4及其对应的观测数据；从基准传感器的各采样时刻中确定出距离t 4最近的采样时刻T 6，根据t 3和t 4确定在T 5上插值时的插值系数λ 5和在T 6上插值时的插值系数λ 6，并根据λ 5、λ 6和第四传感器在t 3和t 4上的观测数据计算第四传感器从T 5到T 6上的第六旋转数据和第六平移数据，计算第六旋转数据和第六平移数据对应的第六协方差矩阵；
融合包括T 4、T 5和T 6在内的基准传感器的各采样时刻上的旋转数据和平移数据，以及协方差矩阵以得到终端的在线位姿估计值；其中，融合的旋转数据至少包括第四旋转数据、第五旋转数据或第六旋转数据，融合的平移数据至少包括第四平移数据、第五平移数据或第六平移数据，融合的协方差矩阵至少包括第四协方差矩阵、第五协方差矩阵，或第六协方差矩阵。
在本申请实施例中，数据融合中心可将传感器的观测数据与校验地图进行匹配，将匹配得到的观测数据与各个传感器实时采集的观测数据进行融合，并对多个传感器的观测数据的融合结果进行增量平滑迭代优化，最终实时估计出当前终端的在线位姿估计值，操作方便，在线位姿估计精度高，适用性更强。
第三方面提供了一种传感器数据处理的装置，该装置包括用于执行上述第一方面和/或第一方面的任意一种可能的实现方式所提供的传感器数据处理方法的单元和/或模块，因此也能实现第一方面提供的传感器数据处理方法所具备的有益效果(或者优点)。
第四方面提供了一种终端，该终端包括存储器、处理器、接收器和发射器；其中，该处理器用于调用存储器存储的传感器数据处理程序代码执行上述第一方面和/或第一方面任意一种可能实现方式所提供的传感器数据处理方法。
本申请实施例提供了一种计算机存储介质，用于储存为上述第一方面提供的传感器数据处理方法所需的计算机软件指令，该指令其包含终端执行上述第一方面所设计的方式所需的程序。
本申请实施例还提供了一种芯片，该芯片与终端中的接收器和/或发射器耦合，用于执行本申请实施例第一方面提供的技术方案。应理解，在本申请实施例中“耦合”是指两个部件彼此直接或间接地结合。这种结合可以是固定的或可移动性的，这种结合可以允许流动液、电、电信号或其它类型信号在两个部件之间通信。
通过本申请实施例，可提高多个传感器的数据融合的操作灵活性，提高传感器的数据处理效率，进而可提高智能终端的定位准确性，适用性高。
附图说明
图1是本申请实施例提供的终端位姿估计的多传感器观测的示意图；
图2是本申请实施例提供的基于多传感器的数据融合的地图构建方式示意图；
图3是本申请实施例提供的传感器数据处理的方法的一流程示意图；
图4是本申请实施例提供的插值方式的一示意图；
图5是本申请实施例提供的插值方式的另一示意图；
图6是本申请实施例提供的基于多传感器的数据融合的在线定位方式示意图；
图7是本申请实施例提供的传感器数据处理的方法的另一流程示意图；
图8是本申请实施例提供的传感器数据处理的装置结构示意图；
图9是本申请实施例提供的一种通信设备的结构示意图。
具体实施方式
为方便理解本申请实施例提供的传感器数据处理的方法中各种可能的实现方式，下面将对本申请实施例提供的各个参数的含义进行说明。
位姿，包括位置和姿态。其中，位置指坐标系中x，y，z三个方向的平移，姿态为坐标系中x，y，z三个方向的旋转。位姿可通过包括在指定坐标系的x，y，z三个方向的平移向量，和在该指定坐标系的x，y，z三个方向的旋转矩阵表示。为方便描述，在后续描述中，位姿将旋转矩阵和平移向量的表示方式为例进行说明，并且旋转矩阵和平移向量默认为同一个坐标系下的旋转矩阵和平移向量。
传感器的观测数据，包括传感器在任一时刻测量得到的终端的位姿。例如，传感器在某个采样时刻t i上测量得到的终端在某个坐标系下的旋转矩阵和平移向量。
传感器的数据对准，指的是将不同传感器在不同时间基准下的观测数据变换为不同传感器在同一时间基准下的观测数据的过程。其中，同一时刻不同传感器的观测属于同一时间基准下的观测，此时，不同传感器的观测数据属于同一个时间基准下的观测数据。不同时刻不同传感器的观测属于不同时间基准下的观测，此时，不同传感器的观测数据属于不同时间基准下的观测数据。其中，同一时间基准可以理解为同一采样频率或者同一采样周期等，该采样频率或者采用周期可以理解为基准传感器的采样频率或者采样频率。同一时刻不同传感器的观测可以理解为采样频率相同的传感器在相同的采样时刻上的观测，不同时刻不同传感器的观测可以理解为采样频率不同的传感器在不同时刻上的观测。
图(graph)是指由顶点(vertex)和边(edge)组成的结构，例如本申请实施例提供的因子图。其中，顶点可以表示普通的点，顶点也可以表示传感器的观测时间节点，即传感器的观测数据的采样时刻。
一元边和/或二元边，其中，一元边指的是连接一个顶点的边。在图中，一条边连接着一个或者若干个顶点，边可用于表示顶点与顶点之间的一种关系。连接一个顶点的边叫一元边，连接两个顶点的边叫二元边。
一元位姿观测，类比于一元边，一元位姿观测指的是任一采样时刻上终端的位姿观测。一元位姿可以理解为通过单个采样时刻采集的数据即可确定的位姿，或者可以理解为通过一帧数据即可确定的位姿，包括旋转矩阵和平移向量。一元位姿观测对应的传感器可称为第一类型的传感器。第一类型的传感器用于采集传感器(或者装配有该传感器的终端)在任一采样时刻上的旋转矩阵和平移向量。例如全球定位***(global positioning system， GPS)传感器可用于采集终端在任一采样时刻的旋转矩阵和平移向量。
二元位姿观测，类比于二元边，二元位姿观测指的是任意两个相邻的采样时刻上终端的位姿观测。二元位姿可以理解为需要至少两个采样时刻采集的数据才能确定的位姿，或者可以理解为需要至少两帧数据才能确定的位姿，包括旋转矩阵和平移向量。二元位姿观测对应的传感器可称为第二类型的传感器。第二类型的传感器用于采集传感器在任意两个相邻的采样时刻之间的相对运动的旋转矩阵和平移向量。例如雷达传感器或者视觉传感器，可用于采集终端在两个相邻的采样时刻之间的相对运动的旋转矩阵和平移向量。
李代数，指的是一类重要的非结合代数。李代数是挪威数学家索菲斯·李在19世纪后期研究连续变换群时引进的一个数学概念，它与李群的研究密切相关。李代数不仅仅是群论问题线性化的工具，它还是有限群理论及线性代数中许多重要问题的来源。
李群，指的是微分流形的群，可以求解刚体的旋转运动。
群空间，本申请实施例中提供的群空间指李群对应的空间。
欧式空间，欧式几何对应的空间叫欧式空间。欧式几何由古希腊数学家欧几里得建立的角和空间中距离之间联系的法则，其对应的空间叫欧式空间。
本申请实施例提供的传感器数据处理的方法适用于终端的地图构建和终端的定位。其中，上述终端可包括自动驾驶汽车、智能机器人、无人机、平板电脑、个人数码助理(personal digital assistant，PDA)、移动互联网设备(mobile Internet device，MID)、可穿戴设备和电子书阅读器(e-book reader)等装置。上述终端也可以是便携式、袖珍式、手持式、计算机内置的或者车载的移动装置，在此不做限制。为方便描述，在后续描述中，上述提到的装置将以终端为例进行说明。
终端的地图构建和/或定位是基于多传感器的观测数据的融合实现终端的位姿估计。其中，上述终端的位姿估计可包括终端在两个时间节点(例如某个第二类型的传感器的两个相邻的采样时刻)之间的相对运动的旋转矩阵和平移向量的估计，和/或终端在当前时间节点(例如某个第一类型的传感器的某个采样时刻)的旋转矩阵和平移向量的估计。
基于smoothing方式的优化方法是当前地图构建和/或定位所采用的主要研究方法之一。基于smoothing方式的优化方法，例如包括g2o和isam在内的非线性最小二乘优化方法，可以采用全局优化或整体考虑的处理方式以获得更好的构图效果，因此也使得基于smoothing方式的优化方法称为大规模环境下地图构建和/或定位的主要研究方法。
本申请实施例提供的终端可通过其装配的多个传感器，基于smoothing方式对终端的位姿进行估计，可通过融合多个传感器的观测数据，并对多个传感器的观测数据的融合结果进行优化处理以输出终端的定位地图或者终端的在线位姿估计值等。下面将结合图1至图9，本申请实施例提供的传感器数据处理的方法及装置进行描述。
参见图1，是本申请实施例提供的终端位姿估计的多传感器观测的示意图。本申请实施例提供的多传感器的数据融合可首先采用因子图的方式进行建模，通过因子图表示终端的运动状态的变化。如图1所示，随着终端的运动状态的变化，圆圈表示不同时刻终端所在的不同位置。终端在各个圆圈所表示的位置上的旋转矩阵和平移向量则为终端在各个位置上的位姿。图1所示的因子图可用于表示不同传感器在同一时间基准下的观测，即图1所示的因子图也可为各个传感器的观测数据在时间轴上对准完成之后的数据关系示意图。例 如，假设终端上装配的多个传感器包括GPS传感器、雷达和视觉里程计等。图1中，X1、X2、X3和X4表示基准传感器连续的4个采样时刻，G、V、L分别为GPS传感器、视觉里程计、雷达三个传感器对应的因子。其中，GPS传感器对应的因子是一元边因子(例如G1、G2、G3和G4)，即，GPS传感器可为第一类型的传感器。视觉里程计和雷达对应的因子是二元边因子，例如，V12、V23、V34，以及L12、L23和L34，雷达和视觉里程计可为第二类型的传感器。
通过多个传感器采集终端在不同时刻所处的位置的旋转矩阵和平移向量，并根据终端在不同位置上的旋转矩阵和平移向量实现终端位姿估计的过程即可理解为基于运动感知的终端位姿估计。本申请实施例可通过基于运动感知的终端位姿估计实现地图构建和终端定位(也称终端在线位姿估计值的确定)的功能。
实施例一：
本申请实施例提供了通过多传感器的观测数据融合实现地图构建的一种可能的实现方式。可以理解，不同的传感器具有不同的采样频率，并且不同的传感器的观测数据传送到数据融合中心的时间延迟也不尽相同，因此，多个传感器的观测数据的融合过程中存在着多传感的观测数据异步到达的问题。其中，观测数据的异步到达指代不同传感器的观测数据在不同时刻到达数据融合中心。其中，上述数据融合中心可为终端中用于实现多个传感器的观测数据的融合等功能的模块，在此不做限制，后续不再赘述。
对于多传感器的观测数据异步到达等问题，本申请实施例提供的实现方式可基于运动感知在欧式空间和群空间上对各个传感器的旋转矩阵和平移向量等运动状态数据进行数学推导和/或插值计算，以将各个传感器在不同时刻到达数据融合中心的观测数据变换到同一个时刻上，进而可实现各个传感器的观测数据在该同一个时刻上的数据融合。
本申请实施例可根据各个传感器在同一时间基准上的观测数据的融合，并对多个传感器的观测数据的融合结果进行全局优化并输出全局的3D地图(map)。在本申请实施例中，多个传感器的观测数据的融合支持多个传感器的观测数据的乱序输入，数据处理方式灵活，地图构建准确率更高，适用性更强。
参见图2，图2是本申请实施例提供的基于多传感器的数据融合的地图构建方式示意图。在图2所示的地图构建方式中主要包括S1、S2和S3的3个数据处理过程。
S1表示多个传感器的观测数据的输入，包括传感器1、传感器2、…、传感器n的数据输入。其中，不同传感器的观测数据可在不同的时间节点(例如不同时刻)输入至数据融合中心，即数据融合中心支持多个传感器的观测数据的异步到达。
S2表示多个传感器的数据对准，即，将不同传感器在不同时间基准下的观测数据变换为不同传感器在同一时间基准下的观测数据。多个传感器的数据对准的过程也可称为传感器的对准。可以理解，不同传感器的采样频率不同，因此不同传感器采集用于确定终端位姿的观测数据的采集时刻也不尽相同。因此，不同时刻输入至数据融合中心的不同传感器的观测数据属于不同时间基准下的观测数据。数据融合中心可在欧式空间和群空间上对各个传感器的旋转矩阵和平移向量进行数学推导和/或插值计算，以将各个传感器在不同时刻到达数据融合中心的观测数据变换到同一个时刻上，进而可实现将不同传感器在不同时间基准下的观测数据变换为同一时间基准下的观测数据。之后在同一时间基准下，每个传感 器的观测数据可表示终端的一个位姿估计值。例如，传感器1、传感器2、…、传感器n的观测数据在同一个时间基准可分别表示终端的n个位姿估计值，包括，位姿估计值1、位姿估计值2、…、位姿估计值n。
S3表示完成传感器的数据对准后，将所有传感器的观测数据进行融合，将融合得到的数据进行全局优化，最终优化可输出全局的3D地图。其中，全局优化可采用基于smoothing方式的非线性最小二乘优化方法，例如g2o等。
下面将结合图3对上述图2所示的地图构建方式的一些可行的实现方式进行描述。参见图3，图3是本申请实施例提供的传感器数据处理的方法的一流程示意图。本申请实施例提供的方法中各个步骤所描述的实现方式可由终端的数据融合中心执行，该方法包括步骤：
S11，从终端中的至少两个传感器中确定出一个基准传感器，并确定出所述基准传感器的各采样时刻。
在一种可行的实施方式中，终端的***上电之后，终端上装配的多个传感器完成预热并确认工作状态正常之后，则可在终端的运动过程中，对终端的位姿变化进行观测。具体实现中，终端上装配的传感器包括但不限于GPS传感器、视觉里程计和雷达，具体可根据实际应用场景确定，在此不做限制。为方便描述，本申请实施例将以GPS传感器、视觉里程计和雷达作为终端上装配的多个传感器的示例进行说明。其中，GPS传感器为第一类型的传感器，视觉里程计和雷达为第二类型的传感器。
在一种可行的实施方式中，终端上装配的各个传感器确认工作状态正常之后，各个传感器对终端的位姿变化进行观测时采集得到的观测数据可输入至终端的数据融合中心。数据融合中心可对多个传感器的观测数据进行融合等处理以实现地图构建。具体实现中，数据融合中心可从多个传感器中确定出一个基准传感器，以基准传感器的采样频率为基准，以将不同采样频率的传感器的观测数据通过插值计算和/或数学推导等方式变换为以相同采样频率呈现的观测数据。
数据融合中心确定出基准传感器之后，则可确定出该基准传感器的各个采样时刻，将该基准传感器的各个采样时刻作为多传感器的观测数据的对准时刻。在多传感器的观测数据的对准过程中，除了基准传感器之外的其他传感器在邻近某个对准时刻的输入的观测数据通过插值计算和/或数学推导等方式变换为在该对准时刻上的观测数据。之后在数据融合阶段，数据融合中心只需将基准传感器的各个采样时刻上的观测数据进行融合即可实现多传感器的观测数据的融合，操作简单，数据处理效率高。
S12，监测各个传感器的观测数据的输入。
在一种可行的实施方式中，数据融合中心可在各传感器确认工作状态正常之后，对各个传感器的观测数据的输入进行监测，进而可对任一传感器输入的任一观测数据进行数据对准，将每个传感器输入的观测数据变换为相应的对准时刻上的观测数据。
S13，当任一时刻t 1获取到任一传感器K的观测数据时，确定传感器K的传感器类型。若传感器K的类型为第一类型则执行步骤S14和S15，若传感器K的类型为第二类型，则执行步骤S16和S17。
在一种可选的实施方式中，数据融合中心可在某一时刻(例如t 1)接收到任一传感器 (例如传感器K)输入的观测数据时，首先确定该传感器K的类型确定采用哪种数据处理方式对该观测数据进行数据对准。
可选的，第一类型的传感器输入的观测数据可采用一元位姿观测对应的实现方式(为方便描述，后续将以一元位姿计算方式为例进行说明)计算该传感器的观测数据对应到基准传感器的采样时刻上的位姿(包括旋转矩阵和平移向量)。第二类型的传感器输入的观测数据可采用二元位姿观测对应的实现方式(为方便描述，后续将以二元位姿计算方式为例进行说明)计算该传感器的观测数据对应到基准传感器的采样时刻上的旋转矩阵和平移向量。
S14，计算第一类型的传感器的观测数据对应到基准传感器的采样时刻上的位姿。
在一种可行的实现方式中，数据融合中心可采用一元位姿计算方式计算第一类型的传感器的观测数据对应到基准传感器的采样时刻上的旋转矩阵和平移向量。其中，计算过程可包括如下步骤：
a1，从基准传感器的各采样时刻中确定出距离t 1最近的采样时刻T 1。
可选的，T 1也可称为t 1的最近邻时间戳，或者最近邻采样时刻。
b1，根据第一传感器的采样间隔和t 1确定在T 1上插值时的插值系数λ 1。
可选的，终端装配的传感器可包括一个或者多个第一类型的传感器，在此不做限制。为方便描述，第一类型的传感器将以第一传感器为例进行说明。对于第一传感器，每次采集数据采集的是一个采样时刻上的观测数据，例如GPS传感器，每个时刻都给出了在该时刻采集到的观测数据。数据融合中心获取到第一传感器在任一采样时刻(例如t 2)上的观测数据之后可存储该采样时刻上的观测数据。在t 2的下一个采样时刻(例如t 1)上的观测数据输入进来时，数据融合中心可根据t 1上输入的观测数据和预先存储的t 2上的观测数据计算在t 1和t 2之间的基准传感器的采样时刻(例如T 1)上的观测数据(包括旋转矩阵和平移向量)。即，t 2为t 1的最近邻采样时刻。
参见图4，图4是本申请实施例提供的插值方式的一示意图。在图4所示的插值方式中，数据融合中心可首先根据第一传感器的采样间隔(例如|t 1-t 2|)确定第一传感器的各个采样时刻中与t 1相邻并且在t 1之前的采样时刻(例如t 2)。t 1和t 2是两个连续的观测时间节点，T 1为t 1和t 2之间的某一个时间节点，并且该时间节点是基准传感器的某个采样时刻。数据融合中心可根据t 1和t 2、T 1计算在T 1上插值时的插值系数，例如λ 1。
其中，λ 1满足：
Figure PCTCN2018107049-appb-000030
其中，上述t 1和t 2为第一传感器的两个连续一元位姿观测时间节点，T 1为在t 1和t 2之间插值的目标时间节点。
c1，根据所述λ 1和所述第一传感器的观测数据计算所述第一传感器在所述T 1的第一旋转数据和第一平移数据。
在一种可行的实施方式中，数据融合中心确定了T 1和λ 1之后，则可在欧式空间和李群空间上对t 1和t 2上的观测数据进行数据推导等处理，以得到第一传感器的观测数据在T 1上的旋转矩阵和平移向量。具体的，数据融合中心可获取第一传感器在t 1上的旋转矩阵(例 如 G 1R)和平移向量(例如 Gp 1)，以及第一传感器在t 2上的旋转矩阵例如
Figure PCTCN2018107049-appb-000031
和平移向量(例如 Gp 2)。其中，上述G全局坐标系。
Figure PCTCN2018107049-appb-000032
表示在全局坐标系下第一传感器在t 1上的位姿，
Figure PCTCN2018107049-appb-000033
表示在全局坐标系下第一传感器在t 2上的位姿。数据融合中心可根据λ 1和
Figure PCTCN2018107049-appb-000034
和 Gp 2计算第一传感器在T 1上的第一旋转数据(例如第一旋转矩阵
Figure PCTCN2018107049-appb-000035
)和第一平移数据(例如第一平移向量 Gp T1)。
其中，
Figure PCTCN2018107049-appb-000036
满足：
Figure PCTCN2018107049-appb-000037
Gp T1满足：
Gp T1＝(1-λ1) Gp 2+λ1 Gp 1；
其中，
Figure PCTCN2018107049-appb-000038
表示在全局坐标系下第一传感器在T 1上的位姿。
S15，计算所述第一旋转数据和所述第一平移数据对应的第一协方差矩阵。
在一种可行的实现方式中，数据融合中心获取得到上T 1上的旋转矩阵和平移向量，还可计算T 1上的旋转矩阵和平移向量对应的协方差矩阵(即第一协方差矩阵)。可以理解，上述T 1上的旋转矩阵和平移向量对应的第一协方差矩阵可以理解为在多传感器的数据融合过程中，T 1上的第一传感器的旋转矩阵和平移向量的可信度系数，或者准确系数。这里，可信度系数或者准确系数也可以理解为权重。若在T 1上融合的旋转矩阵和平移向量中包括多个传感器的旋转矩阵和平移向量，则可根据各个传感器的旋转矩阵和平移向量对应的协方差矩阵确定该传感器的旋转矩阵和平移向量在该时刻(T 1)上的可信度。根据各个传感器在该时刻(T 1)上的旋转矩阵和平移向量及其可信度，将多个传感器的旋转矩阵和平移向量进行融合，则可更加准确地确定终端在该时刻(T 1)上的旋转矩阵和平移向量。
具体实现中，数据融合中心可计算第一传感器在t 1上的位姿(例如
Figure PCTCN2018107049-appb-000039
)对应的协方差矩阵P t1，并计算第一传感器在t2上的位姿(例如
Figure PCTCN2018107049-appb-000040
)对应的协方差矩阵P t2。根据第一旋转数据和第一平移数据(例如
Figure PCTCN2018107049-appb-000041
)计算雅可比矩阵(记为H u1)，并根据P t1和P t2计算第一旋转矩阵和第一平移向量对应的协方差矩阵P T1。
其中，上述H u1满足：
Figure PCTCN2018107049-appb-000042
上述P T1满足：
Figure PCTCN2018107049-appb-000043
其中，P 1,2表示P t1P t2，
Figure PCTCN2018107049-appb-000044
表示对旋转矩阵R的估计值，O 3*3、O 6*6表示3*3和6*6的全零矩阵，I表示单位矩阵，i代表T 1，G代表全局坐标系，λ表示插值系数λ 1，Jr为右雅克比矩阵，Logv代表矩阵的对数运算，
Figure PCTCN2018107049-appb-000045
分别为角度及位移的误差向量。
S16，计算第二类型的传感器的观测数据对应到基准传感器的采样时刻上的位姿。
在一种可行的实现方式中，数据融合中心可采用二元位姿计算方式计算第二类型的传感器的观测数据对应到基准传感器的采样时刻上的旋转矩阵和平移向量。其中，计算过程可包括如下步骤：
a2，从基准传感器的各采样时刻中确定出距离t 1最近的两个采样时刻T 2和T 3。
为区别于上述第一类型的传感器的位姿计算过程所采用的参数，在第二类型的传感器的位姿计算过程中，距离t 1最近的两个采样时刻可以记为T 2和T 3。其中，T 2可表示t 1的最近邻的采样时刻，T 3可表示t 1的次近邻采样时刻，即T 3距离t 1的时间长度在T 2距离t 1的时间长度之后距离，但在其他采样时刻距离t 1的时间长度之前的次近邻的采样时刻。其中，T 3可以基准传感器的各采样时刻中为距离t 1之前的采样时刻t 2最近的采样时刻。具体实现中，本申请实施例中所描述的T 1和T 2可为同一个采样时刻，即均为t 1的最近邻采样时刻。
b2，根据第二传感器的采样间隔和t 1确定在T 2上插值时的插值系数λ 2和在T 3上插值时的插值系数λ 3。
可选的，终端装配的传感器可包括一个或者多个第二类型的传感器，在此不做限制。为方便描述，第二类型的传感器将以第二传感器为例进行说明。对于第二传感器，观测终端运动状态需要在两个连续的采样时刻获取得到终端的两组观测数据(例如两组旋转矩阵和平移向量)，通过两组观测数据的运算才能得到终端在这两个观测时间点之间的相对运动状态。第二传感器输出的终端的旋转矩阵和平移向量是终端在两个相邻的采样时刻之间的相对运动的旋转矩阵和平移向量。
参见图5，图5是本申请实施例提供的插值方式的另一示意图。在图5所示的插值方式中，数据融合中心获取到第二传感器在任一采样时刻(例如t 2)上的观测数据之后可存储该采样时刻上的观测数据。在下一个采样时刻(例如t 1)上的观测数据输入进来时，数据融合中心可根据t 1上输入的观测数据和预先存储的t 2上的观测数据计算从t 1和t 2之间的旋转矩阵和平移向量，例如，
Figure PCTCN2018107049-appb-000046
具体实现中，数据融合中心可首先根根据第二传感器的采样间隔(例如|t 1-t 2|)和t 1确定t 1之前的采样时刻t 2，并根据t 1和t 2确定在T 2上插值时的插值系数λ 2和在T 3上插值时的插值系数λ 3。
其中，λ 2满足：
Figure PCTCN2018107049-appb-000047
λ 3满足：
Figure PCTCN2018107049-appb-000048
其中，上述t 1和t 2为第二传感器的两个连续二元位姿观测时间节点，T 2为距离t 1最近的基准传感器的采样时刻，T 3为距离t2 1最近的基准传感器的采样时刻。
c2，根据λ 2、λ 3和第二传感器的观测数据计算第二传感器从T 2到T 3的第二旋转数据和第二平移数据。
具体实现中，数据融合中心可获取第二传感器在t 1和所述t 2之间的旋转矩阵(例如 2 1R)，以及第二传感器在t 1和t 2之间的平移向量(例如 2p 1)，并根据λ 2、λ 3、
Figure PCTCN2018107049-appb-000049
和 2p 1计算第二 传感器在T 2和T 3之间的相对运动的第二旋转数据(例如第二旋转矩阵
Figure PCTCN2018107049-appb-000050
)和第二平移数据(例如第二平移向量 T3p T2)。
其中，
Figure PCTCN2018107049-appb-000051
满足：
Figure PCTCN2018107049-appb-000052
T3p T2满足：
Figure PCTCN2018107049-appb-000053
S17，计算第二旋转数据和第二平移数据对应的第二协方差矩阵。
具体实现中，数据融合中心可计第二传感器在t 1和t 2之间的旋转矩阵和平移向量对应的协方差矩阵P t12，并根据第二旋转矩阵和第二平移向量计算雅可比矩阵(例如Hu2)，并根据P t12计算第二旋转矩阵和第二平移向量对应的协方差矩阵P T12。
其中，H u2满足：
Figure PCTCN2018107049-appb-000054
P T12满足：
Figure PCTCN2018107049-appb-000055
其中，
Figure PCTCN2018107049-appb-000056
表示对旋转矩阵R的估计值，O 3*3表示3*3的全零矩阵，b代表T 3,e代表T 2，λ bλ e分别表示插值系数λ 3和λ 2，Jr为右雅克比矩阵，Logv代表矩阵的对数运算，
Figure PCTCN2018107049-appb-000057
Figure PCTCN2018107049-appb-000058
分别为角度及位移的误差向量。
S18，融合包括T 1、T 2和T 3在内的基准传感器的各采样时刻上的旋转数据、平移数据和协方差矩阵。
在一种可行的实施方式中，数据融合中心可基于smoothing方式将各个传感器在基准传感器的各个采样时刻上的旋转矩阵和平移向量进行融合以得到终端在各采样时刻上的位姿估计值。其中，融合的旋转数据至少包括第一旋转数据和/或第二旋转数据，融合的平移数据至少包括第一平移数据和/或第二平移数据，融合的协方差矩阵至少包括第一协方差矩阵，和/或第二协方差矩阵。数据融合中心可将融合得到的终端在基准传感器的各个采样时刻上的位姿估计值进行全局优化，输出在全局坐标下构建的校验地图。其中，上述校验地图用于为后续终端的在线位姿估计值的确定提供参考数据。
本申请实施例可根据各个传感器的类型对各个传感器输入的旋转数据、平移数据和/或协方差矩阵变换至相应的对准时刻上，并通过对各个对准时刻上的旋转数据、平移数据和/或协方差矩阵进行融合，优化输出全局坐标系下的校验地图。通过校验地图可实现在无GPS传感器输入的情况下完成终端的在线位姿估计。在本申请实施例中，多个传感器的观测数 据的融合支持多个传感器的观测数据的乱序输入，数据处理方式灵活，地图构建准确率更高，适用性更强。
实施例二：
本申请实施例提供了通过多传感器的观测数据融合实现终端的在线定位的一种可能的实现方式。在线定位也可称为在线位姿估计。在线定位时，终端在不同时刻上的不同位置的观测数据的融合不依赖于第一类型的传感器(例如GPS传感器)的观测数据输入。有校验地图时，第二类型的传感器采集的观测数据可以跟校验地图进行匹配，获取终端的在线位姿估计的初始估计值，可作为在线位姿估计值的参考数据。终端的在线位姿估计的初始估计值是一个较准确的观测数据，然而，终端的在线定位对于实时性的要求更高，因此，获取初始估计值之后，也可进一步根据各个传感器实时输入的观测数据进行优化以输出精度更高的在线位姿估计值。具体实现中，数据融合中心可将与校验地图匹配得到的观测数据与各个第二类型的传感器实时采集的观测数据进行融合，并对多个传感器的观测数据的融合结果进行增量平滑迭代优化，最终实时估计出当前终端的在线3D位姿估计值，操作方便，在线位姿估计精度高，适用性更强。
参见图6，图6是本申请实施例提供的基于多传感器的数据融合的在线定位方式示意图。在图6所示的地图构建方式中也可包括S4、S5、S6和S7的4个数据处理过程。
S4表示多个传感器的观测数据的输入，包括传感器1、传感器2、…、传感器n的数据输入。其中，不同传感器的观测数据可在不同的时间节点(例如不同时刻)输入至数据融合中心，即数据融合中心支持多个传感器的观测数据的异步到达。
S5校验地图的校验数据输入，包括但不限于传感器1的观测数据与校验地图匹配得到的参考数据1、传感器2的观测数据与校验地图匹配得到的参考数据2、…、传感器n的观测数据与校验地图匹配得到的参考数据n的输入。
S6表示多个传感器的数据对准，具体可参见上述实施例一中S2所描述的实现方式，在此不再赘述。之后在同一时间基准下，每个传感器的观测数据可表示终端的一个位姿估计值。例如，传感器1、传感器2、…、传感器n的观测数据在同一个时间基准可分别表示终端的n个位姿估计值，包括位姿估计值1、位姿估计值2、…、位姿估计值n。
S7表示完成传感器的数据对准后，将所有传感器的观测数据和传感器与校验地图匹配得到的参考数据进行融合，将融合得到的数据进行增量平滑迭代优化，最终实时估计出当前终端的在线3D位姿估计值。其中，增量平滑迭代优可采用基于smoothing方式的非线性最小二乘优化方法，例如iasm等。
下面将结合图7对上述图6所示的地图构建方式中各个部分的具体实现方式进行描述。参见图7，图7是本申请实施例提供的传感器数据处理的方法的另一流程示意图。本申请实施例提供的方法包括步骤：
S71，启动终端的在线定位功能，并获取终端的校验地图。
在一种可行的实施方式中，终端可通过上述实施例所描述的实现方式生成校验地图，并存储至终端的指定存储空间中。当终端启动在线定位功能时，可从该指定存储空间中获取得到该校验地图，并将该校验地图作为后续估计终端的在线位姿的参考数据。
S72，监测各个传感器的观测数据的输入。
具体实现中，数据融合中心监测各个传感器的观测数据的输入的实现方式可参见上述实施例一中步骤S12所描述的实现方式，在此不再赘述。
S73，当任一时刻t 3获取到任一传感器L的观测数据时，确定传感器L的传感器类型。若传感器L的类型为第一类型则执行步骤S74，若传感器L的类型为第二类型，则执行步骤S75和S76。
在一种可选的实施方式中，数据融合中心可在某一时刻(例如t 3)接收到任一传感器(例如传感器L)输入的观测数据时，首先确定该传感器L的类型确定采用哪种数据处理方式对该观测数据进行数据对准。可选的，上述传感器K和传感器L也可为同一个传感器，在此不做限制。
可选的，第一类型的传感器输入的观测数据可采用一元位姿观测对应的实现方式(为方便描述，后续将以一元位姿计算方式为例进行说明)计算该传感器的观测数据对应到基准传感器的采样时刻上的位姿(包括旋转矩阵和平移向量)，具体可参见上述实施例一中步骤S14所描述的实现方式，在此不再赘述。
若传感器L为第二类型的传感器，数据融合中心还可将该传感器L的观测数据与校验地图进行匹配，并将匹配得到的观测数据按照一元位姿观测对应的实现方式计算匹配得到的观测数据对准到基准传感器的采样时刻上的位姿(包括旋转矩阵和平移向量)。其中，将匹配得到的观测数据对准到基准传感器的采样时刻上的旋转矩阵和平移向量的实现方式可参见上述实施例一中步骤S14所描述的实现方式，在此不再赘述。
若传感器L为第二类型的传感器，则数据融合中心可采用二元位姿观测对应的实现方式(为方便描述，后续将以二元位姿计算方式为例进行说明)计算该传感器的观测数据对应到基准传感器的采样时刻上的旋转矩阵和平移向量。
S74，计算第一类型的传感器的观测数据对应到基准传感器的采样时刻上的位姿。
具体实现中，为方便描述，并区别与实施例一中的第一传感器，这里的第一类型的传感器将以第三传感器为例进行说明。其中，第一传感器和第三传感器也可为同一个传感器，在此不做限制。
若传感器L为第一类型的第三传感器，数据融合中心则可从基准传感器的各采样时刻中确定出距离t 3最近的采样时刻(例如T 4)，并计算第三传感器在到T 4上的旋转数据(例如第四旋转数据)和平移数据(例如第四平移数据)，以及第四旋转数据和第四平移数据对应的协方差(例如第四协方差矩阵)。具体实现中，数据融合中心计算第四旋转数据、第四平移数据和/或第四协方差矩阵的实现方式可参见上述实施例一中的步骤S14和S15中各个步骤所描述的实现方式，在此不再赘述。这里，旋转数据可为旋转矩阵，平移数据可为平移向量，具体可参见上述实施例一中相应的实现方式，在此不再赘述。
S75，将传感器L的观测数据与校验地图进行匹配以得到传感器L在t 3上的参考旋转数据和参考平移数据。
为方便描述，并区别与实施例一中的第二类型的第二传感器，这里将以第四传感器作为第二类型的传感器为例进行说明。其中，第二传感器和第四传感其也可为同一个传感器，在此不做限制。
具体实现中，数据融合终端可将第四传感器在t 3上的观测数据与校验地图进行匹配，确定第四传感器在t 3上的参考旋转数据和参考平移数据。例如，参考旋转矩阵和/或参考平移向量。
可选的，若终端中存在校验地图，数据融合中心则可将当前获取的第四传感器的观测数据与校验地图进行匹配，并将匹配结果输入一元位姿计算方式的处理流程中，例如上述实施例一中步骤a1至c1所描述的实现方式，以及上述实施例一中步骤S15所描述的实现方式，在此不再赘述。数据融合中心可根据第四传感器在t 3上的参考旋转矩阵和/或参考平移向量计算第四传感器在到T 4上的旋转数据(第五旋转数据，例如第五旋转矩阵)和平移数据(第五平移数据，例如第五平移向量)，以及第五旋转数据和第五平移数据对应的协方差矩阵(例如第五协方差矩阵)。
S76，计算第二类型的传感器的观测数据对应到基准传感器的采样时刻上的旋转数据和平移数据及其对应的协方差。
在一种可行的实施方式中，数据融合中心可从基准传感器的各采样时刻中确定出距离t 3最近的两个采样时刻，例如T 5和T 6，其中，T 5表示t 3的最近邻采样时刻，T 6表示t 3的次近邻采样时刻，例如与t 3相邻并且在t 3之前的采样时刻(例如t 4)的最近邻采样时刻。根据第四传感器的采样间隔和t 3确定出t 4，以及在T 5上插值时的插值系数(例如λ 5)和在T 6上插值时的插值系数(例如λ 6)，并根据λ 5、λ 6和第四传感器在t 3和t 4上的观测数据计算第四传感器从T 5到T 6上的旋转数据(第六旋转数据，例如第六旋转矩阵)和平移数据(第六平移数据，例如第六平移向量)，计算第六旋转数据和第六平移数据对应的协方差矩阵(例如第六协方差)。具体实现中，数据融合中心计算第二类型的传感器的观测数据对应到基准传感器的采样时刻上的旋转数据和平移数据(例如旋转矩阵和平移向量)及其对应的协方差矩阵的实现方式可参见上述实施例一中步骤S16和17所描述的实现方式，在此不再赘述。
S77，融合包括T 4、T 5和T 6在内的基准传感器的各采样时刻上的旋转数据、平移数据和协方差矩阵以得到所述终端的在线位姿估计值。
在一种可行的实施方式中，数据融合中心可基于smoothing方式将各个传感器在基准传感器的各个采样时刻上的旋转矩阵、平移向量和/或协方差矩阵进行融合以得到终端在各采样时刻上的位姿估计值。其中，融合的旋转数据至少包括第四旋转数据、第五旋转数据或第六旋转数据，融合的平移数据至少包括第四平移数据、第五平移数据或第六平移数据，融合的协方差矩阵至少包括第四协方差矩阵、第五协方差矩阵，或第六协方差矩阵。
在本申请实施例中，数据融合中心可将传感器的观测数据与校验地图进行匹配，将匹配得到的观测数据与各个传感器实时采集的观测数据进行融合，并对多个传感器的观测数据的融合结果进行增量平滑迭代优化，最终实时估计出当前终端的在线位姿估计值，操作方便，在线位姿估计精度高，适用性更强。
参见图8，图8是本申请实施例提供的传感器数据处理的装置结构示意图。本申请实施例提供的传感器数据处理的装置可包括：
确定单元81，用于从终端中的至少两个传感器中确定出一个基准传感器，并确定出所述基准传感器的各采样时刻。
获取单元82，用于获取任一传感器的观测数据。
确定单元81，还用于在任一时刻t 1获取单元82获取到任一传感器K的观测数据时，确定传感器K的传感器类型。
确定单元81，还用于在传感器K为第一类型的第一传感器时，从基准传感器的各采样时刻中确定出距离t 1最近的采样时刻T 1，根据第一传感器的采样间隔和t 1确定在T 1上插值时的插值系数λ 1。
计算单元83，用于确定单元81确定的λ 1和获取单元82获取的第一传感器的观测数据计算第一传感器在T 1的第一旋转数据和第一平移数据，计算第一旋转数据和第一平移数据对应的第一协方差矩阵。
确定单元81，还用于在传感器K为第二类型的第二传感器时，从基准传感器的各采样时刻中确定出距离t 1最近的两个采样时刻T 2和T 3，根据第二传感器的采样间隔和t 1确定在T 2上插值时的插值系数λ 2和在T 3上插值时的插值系数λ 3。
计算单元83，还用于根据确定单元81确定的根据λ 2、λ 3和第二传感器的观测数据计算第二传感器从T 2到T 3的第二旋转数据和第二平移数据，计算第二旋转数据和第二平移数据对应的第二协方差矩阵。
数据融合单元84，用于融合计算单元83处理得到的包括T 1、T 2和T 3在内的基准传感器的各采样时刻上的旋转数据、平移数据和协方差矩阵以得到终端在所述各采样时刻上的位姿估计值；
其中，融合的旋转数据至少包括第一旋转数据和/或第二旋转数据，融合的平移数据至少包括第一平移数据和/或第二平移数据，融合的协方差矩阵至少包括第一协方差矩阵，和/或第二协方差矩阵。
在一种可行的实施方式中，确定单元81用于：
根据所述第一传感器的采样间隔确定所述t 1之前的最近邻采样时刻t 2，并根据所述t 1和t 2计算在所述T 1上插值时的插值系数λ 1；
其中，所述λ 1满足：
Figure PCTCN2018107049-appb-000059
在一种可行的实施方式中，第一传感器的观测数据包括第一传感器的旋转矩阵和平移向量；获取单元82，用于获取第一传感器在t 1上的旋转矩阵
Figure PCTCN2018107049-appb-000060
和平移向量 Gp 1，以及第一传感器在t 2上的旋转矩阵
Figure PCTCN2018107049-appb-000061
和平移向量 Gp 2。
计算单元83，用于根据获取单元81获取的λ 1、
Figure PCTCN2018107049-appb-000062
Gp 1、
Figure PCTCN2018107049-appb-000063
和 Gp 2计算第一传感器在T 1上的第一旋转矩阵
Figure PCTCN2018107049-appb-000064
和第一平移向量 Gp T1；
其中，
Figure PCTCN2018107049-appb-000065
满足：
Figure PCTCN2018107049-appb-000066
Gp T1满足：
Gp T1＝(1-λ 1) Gp 2+λ 1 Gp 1；
其中，所述第一旋转矩阵
Figure PCTCN2018107049-appb-000067
为第一旋转数据，所述第一平移向量 Gp T1为第一平移数据。
在一种可行的实施方式中，计算单元83用于：
计算第一传感器在所述t 1上的位姿对应的协方差矩阵P t1，并计算第一传感器在t 2上的 位姿对应的协方差矩阵P t2；根据第一旋转数据和第一平移数据计算雅可比矩阵H u，并根据P t1和P t2计算第一旋转数据和第一平移数据对应的协方差矩阵P T1；
其中，H u满足：
Figure PCTCN2018107049-appb-000068
P T1满足：
Figure PCTCN2018107049-appb-000069
其中，P 1,2表示P t1P t2，
Figure PCTCN2018107049-appb-000070
表示对旋转矩阵R的估计值，O 3*3、O 6*6表示3*3和6*6的全零矩阵，I表示单位矩阵，i代表T 1，G代表全局坐标系，λ表示插值系数λ 1，Jr为右雅克比矩阵，Logv代表矩阵的对数运算，
Figure PCTCN2018107049-appb-000071
分别为角度及位移的误差向量。
在一种可行的实施方式中，确定单元81用于：
根据第二传感器的采样间隔和t 1确定t 1之前的采样时刻t 2，并根据t 1和t 2确定在T 2上插值时的插值系数λ 2和在T3上插值时的插值系数λ 3；其中，λ 2满足：
Figure PCTCN2018107049-appb-000072
λ3满足：
Figure PCTCN2018107049-appb-000073
其中，T 2表示基准传感器的采样时刻中距离t 1最近的采样时刻，T 3表示基准传感器的采样时刻中距离t 2最近的采样时刻。
在一种可行的实施方式中，第二传感器的观测数据包括第二传感器的旋转矩阵和平移向量；获取单元82，用于获取第二传感器在t 1和t 2之间的旋转矩阵
Figure PCTCN2018107049-appb-000074
，以及第二传感器在t 1和t 2之间的平移向量 2p 1。
计算单元83，用于根据获取单元82获取的λ 2、λ 3、
Figure PCTCN2018107049-appb-000075
和 2p 1计算第二传感器在T 2和T 3之间的相对运动的第二旋转矩阵
Figure PCTCN2018107049-appb-000076
和第二平移向量 T3p T2；
其中，
Figure PCTCN2018107049-appb-000077
满足：
Figure PCTCN2018107049-appb-000078
T3p T2满足：
Figure PCTCN2018107049-appb-000079
在一种可行的实施方式中，计算单元83用于：
计算第二传感器在t 1和t 2之间的旋转矩阵和平移向量对应的协方差矩阵P t12；根据第二旋转数据和第二平移数据计算雅可比矩阵H u，并根据P t12计算第二旋转数据和第二平移数据对应的协方差矩阵P T12；
其中，H u满足：
Figure PCTCN2018107049-appb-000080
P T12满足：
Figure PCTCN2018107049-appb-000081
其中，
Figure PCTCN2018107049-appb-000082
表示对旋转矩阵R的估计值，O 3*3表示3*3的全零矩阵，b代表T 3,e代表T 2，λ b和λ e分别表示插值系数λ 3和λ 2，Jr为右雅克比矩阵，Logv代表矩阵的对数运算，
Figure PCTCN2018107049-appb-000083
分别为角度及位移的误差向量。
在一种可行的实施方式中，上述装置还包括：
地图构建单元85，用于根据数据融合单元84处理得到的包括T 1、T 2和/或T 3在内的基准传感器的各采样时刻上的旋转矩阵和平移向量，以及终端在各采样时刻上的位姿估计值在全局坐标系下构建校验地图；其中，该校验地图用于为终端的在线位姿估计值的确定提供参考数据。
在一种可行的实施方式中，上述装置还包括：
启动单元86，用于启动终端的在线定位功能，并获取所述终端的校验地图。
确定单元81，还用于当获取单元82在任一时刻t 3获取到任一传感器L的观测数据时，确定传感器L的传感器类型。
确定单元81，还用于在传感器L为第一类型的第三传感器时，从基准传感器的各采样时刻中确定出距离t 3最近的采样时刻T 4。
计算单元83，还用于计算第三传感器在T 4上的第四旋转数据和第四平移数据，以及第四旋转数据和第四平移数据对应的第四协方差矩阵；
若传感器L为第二类型的第四传感器，确定单元81和计算单元83还用于执行操作：
确定单元81，还用于将第四传感器在t 3上的观测数据与校验地图进行匹配，确定第四传感器在t 3上的参考旋转数据和参考平移数据，从基准传感器的各采样时刻中确定出距离t 3最近的采样时刻T 4；
计算单元，还用于根据第四传感器在t 3上的参考旋转数据和参考平移数据计算第四传感器在T 4上的第五旋转数据和第五平移数据，以及第五旋转数据和第五平移数据对应的第五协方差矩阵。
确定单元81，还用于从基准传感器的各采样时刻中确定出距离t 3最近的两个采样时刻T 5和T 6，根据第四传感器的采样间隔和t 3确定在T 5上插值时的插值系数λ 5和在T 6上插值时 的插值系数λ 6。
计算单元83，还用于根据λ 5、λ 6和第四传感器的观测数据计算第四传感器从T 5到T 6上的第六旋转数据和第六平移数据，计算第六旋转数据和第六平移数据对应的第六协方差矩阵；
数据融合单元84，还用于融合计算单元83处理得到的包括T 4、T 5和T 6在内的基准传感器的各采样时刻上的旋转数据和平移数据，以及协方差矩阵以得到终端的在线位姿估计值；其中，融合的旋转数据至少包括第四旋转数据、第五旋转数据或第六旋转数据，融合的平移数据至少包括第四平移数据、第五平移数据或第六平移数据，融合的协方差矩阵至少包括所述第四协方差矩阵、第五协方差矩阵，或第六协方差矩阵。
在一种可行的实施方式中，包括第一传感器和/或第三传感器在内的第一类型的传感器用于采集第一类型的传感器在其任一采样时刻上的旋转数据和平移数据。
在一种可行的实施方式中，包括第二传感器和/或第四传感器在内的第二类型的传感器用于采集第二类型的传感器在其任意两个相邻的采样时刻之间的相对运动的旋转数据和平移数据。
具体实现中，本申请实施例提供的传感器数据处理的装置也可为本申请实施例提供的终端。本申请实施例提供的传感器数据处理的装置可通过其内置的各个单元执行上述各个实现所描述的实现方式，在此不再赘述。
请参见图9，图9是本申请实施例提供的一种通信设备40的结构示意图。如图9所示，本申请实施例提供的通信设备40包括处理器401、存储器402、收发器403和总线***404。其中，上述处理器401、存储器402和收发器403通过总线***404连接。
上述存储器402用于存放程序。具体地，程序可以包括程序代码，程序代码包括计算机操作指令。存储器402包括但不限于是随机存储记忆体(random access memory，RAM)、只读存储器(read-only memory，ROM)、可擦除可编程只读存储器(erasable programmable read only memory，EPROM)、或便携式只读存储器(compact disc read-only memory，CD-ROM)。图9中仅示出了一个存储器，当然，存储器也可以根据需要，设置为多个。存储器402也可以是处理器401中的存储器，在此不做限制。
存储器402存储了如下的元素，可执行模块或者数据结构，或者它们的子集，或者它们的扩展集：
操作指令：包括各种操作指令，用于实现各种操作。
操作***：包括各种***程序，用于实现各种基础业务以及处理基于硬件的任务。
上述处理器401控制通信设备40的操作，处理器401可以是一个或多个中央处理器(central processing unit，CPU)，在处理器401是一个CPU的情况下，该CPU可以是单核CPU，也可以是多核CPU。
具体的应用中，通信设备40的各个组件通过总线***404耦合在一起，其中总线***404除包括数据总线之外，还可以包括电源总线、控制总线和状态信号总线等。但是为了清楚说明起见，在图9中将各种总线都标为总线***404。为便于表示，图9中仅是示意性画出。
本申请实施例提供的上述各个实施例揭示的传感器数据处理的方法可以应用于处理器401中，或者由处理器401实现。处理器401可能是一种集成电路芯片，具有信号的处理能力。 在实现过程中，上述方法的各步骤可以通过处理器401中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器401可以是通用处理器、数字信号处理器(digital signal processing，DSP)、专用集成电路(application specific integrated circuit，ASIC)、现场可编程门阵列(field－programmable gate array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本申请实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本申请实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器402，处理器401读取存储器402中的信息，结合其硬件执行上述各个实施例所描述传感器数据处理的方法发的步骤。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，该流程可以由计算机程序来指令相关的硬件完成，该程序可存储于计算机可读取存储介质中，该程序在执行时，可包括如上述各方法实施例的流程。而前述的存储介质包括：ROM或随机存储记忆体RAM、磁碟或者光盘等各种可存储程序代码的介质。

Claims (14)

  1. 一种传感器数据处理的方法，其特征在于，包括：
    从终端中的至少两个传感器中确定出一个基准传感器，并确定出所述基准传感器的各采样时刻；
    当任一时刻t 1获取到任一传感器K的观测数据时，确定所述传感器K的传感器类型；
    若所述传感器K为第一类型的第一传感器，则从所述基准传感器的各采样时刻中确定出距离所述t 1最近的采样时刻T 1，根据所述第一传感器的采样间隔和所述t 1确定在所述T 1上插值时的插值系数λ 1，并根据所述λ 1和所述第一传感器的观测数据计算所述第一传感器在所述T 1的第一旋转数据和第一平移数据，计算所述第一旋转数据和所述第一平移数据对应的第一协方差矩阵；
    若所述传感器K为第二类型的第二传感器，则从所述基准传感器的各采样时刻中确定出距离所述t 1最近的两个采样时刻T 2和T 3，根据所述第二传感器的采样间隔和所述t 1确定在所述T 2上插值时的插值系数λ 2和在所述T 3上插值时的插值系数λ 3，并根据所述λ 2、λ 3和所述第二传感器的观测数据计算所述第二传感器从T 2到T 3的第二旋转数据和第二平移数据，计算所述第二旋转数据和所述第二平移数据对应的第二协方差矩阵；
    融合包括所述T 1、T 2和T 3在内的所述基准传感器的各采样时刻上的旋转数据、平移数据和协方差矩阵以得到所述终端在所述各采样时刻上的位姿估计值；
    其中，融合的所述旋转数据至少包括所述第一旋转数据和/或所述第二旋转数据，融合的所述平移数据至少包括所述第一平移数据和/或所述第二平移数据，融合的所述协方差矩阵至少包括所述第一协方差矩阵，和/或所述第二协方差矩阵。
  2. 根据权利要求1所述的方法，其特征在于，所述根据所述第一传感器的采样间隔和所述t 1确定在所述T 1上插值时的插值系数λ 1包括：
    根据所述第一传感器的采样间隔确定所述t 1之前的最近邻采样时刻t 2，并根据所述t 1和t 2计算在所述T 1上插值时的插值系数λ 1；
    其中，所述λ 1满足：
    Figure PCTCN2018107049-appb-100001
  3. 根据权利要求2所述的方法，其特征在于，所述第一传感器的观测数据包括所述第一传感器的旋转矩阵和平移向量；
    所述根据所述λ 1和所述第一传感器的观测数据计算所述第一传感器在所述T 1的第一旋转数据和第一平移数据包括：
    获取所述第一传感器在所述t 1上的旋转矩阵
    Figure PCTCN2018107049-appb-100002
    和平移向量 Gp 1，以及所述第一传感器在所述t 2上的旋转矩阵
    Figure PCTCN2018107049-appb-100003
    和平移向量 Gp 2；
    根据所述λ 1、
    Figure PCTCN2018107049-appb-100004
    Gp 1、
    Figure PCTCN2018107049-appb-100005
    和 Gp 2计算所述第一传感器在所述T 1上的第一旋转矩阵
    Figure PCTCN2018107049-appb-100006
    和第一平移向量 Gp T1；
    其中，所述
    Figure PCTCN2018107049-appb-100007
    满足：
    Figure PCTCN2018107049-appb-100008
    所述 Gp T1满足：
    Gp T1＝(1-λ 1) Gp 2+λ 1 Gp 1；
    其中，所述第一旋转矩阵
    Figure PCTCN2018107049-appb-100009
    为第一旋转数据，所述第一平移向量 Gp T1为第一平移数据。
  4. 根据权利要求1所述的方法，其特征在于，所述根据所述第二传感器的采样间隔和所述t 1确定在所述T 2上插值时的插值系数λ 2和在所述T 3上插值时的插值系数λ 3包括：
    根据所述第二传感器的采样间隔和所述t 1确定所述t 1之前的最近邻采样时刻t 2，并根据所述t 1和所述t 2确定在所述T 2上插值时的插值系数λ 2和在所述T 3上插值时的插值系数λ 3；
    其中，所述λ 2满足：
    Figure PCTCN2018107049-appb-100010
    所述λ 3满足：
    Figure PCTCN2018107049-appb-100011
    其中，T 2表示所述基准传感器的采样时刻中距离t 1最近的采样时刻，T 3表示所述基准传感器的采样时刻中距离t 2最近的采样时刻。
  5. 根据权利要求4所述的方法，其特征在于，所述第二传感器的观测数据包括所述第二传感器的旋转矩阵和平移向量；
    所述根据所述λ 2、λ 3和所述第二传感器的观测数据计算所述第二传感器从T 2到T 3的第二旋转数据和第二平移数据包括：
    获取所述第二传感器在所述t 1和所述t 2之间的旋转矩阵
    Figure PCTCN2018107049-appb-100012
    以及所述第二传感器在所述t 1和所述t 2之间的平移向量 2p 1；
    根据所述λ 2、所述λ 3、所述
    Figure PCTCN2018107049-appb-100013
    和所述 2p 1计算所述第二传感器在所述T 2和T 3之间的相对运动的第二旋转矩阵
    Figure PCTCN2018107049-appb-100014
    和第二平移向量 T3p T2；
    其中，所述
    Figure PCTCN2018107049-appb-100015
    满足：
    Figure PCTCN2018107049-appb-100016
    所述 T3p T2满足：
    Figure PCTCN2018107049-appb-100017
    其中，所述第二旋转矩阵
    Figure PCTCN2018107049-appb-100018
    为第二旋转数据，所述第二平移向量 T3p T2为第二平移数据。
  6. 根据权利要求3或5所述的方法，其特征在于，所述方法还包括：
    根据包括所述T 1、T 2和T 3在内的所述基准传感器的各采样时刻上的旋转矩阵和平移向量，以及所述终端在所述各采样时刻上的位姿估计值在全局坐标系下构建校验地图；
    其中，所述校验地图用于为所述终端的在线位姿估计值的确定提供参考数据。
  7. 根据权利要求6所述的方法，其特征在于，所述方法还包括：
    启动终端的在线定位功能，并获取所述终端的校验地图；
    当任一时刻t 3获取到任一传感器L的观测数据时，确定所述传感器L的传感器类型；
    若所述传感器L为第一类型的第三传感器，则从所述基准传感器的各采样时刻中确定出距离所述t 3最近的采样时刻T 4，并计算所述第三传感器在所述T 4上的第四旋转数据和第四平移数据，以及所述第四旋转数据和所述第四平移数据对应的第四协方差矩阵；
    若所述传感器L为第二类型的第四传感器，则执行如下步骤a和步骤b：
    a、将所述第四传感器在所述t 3上的观测数据与所述校验地图进行匹配，确定所述第四传感器在所述t 3上的参考旋转数据和参考平移数据；从所述基准传感器的各采样时刻中确定出距离所述t 3最近的采样时刻T 4并根据所述第四传感器在t 3上的参考旋转数据和参考平移数据计算所述第四传感器在所述T 4上的第五旋转数据和第五平移数据，以及所述第五旋转数据和所述第五平移数据对应的第五协方差矩阵；
    b、从所述基准传感器的各采样时刻中确定出距离所述t 3最近的两个采样时刻T 5和T 6，根据所述第四传感器的采样间隔和所述t 3确定在所述T 5上插值时的插值系数λ 5和在所述T 6上插值时的插值系数λ 6，并根据所述λ 5、λ 6和所述第四传感器的观测数据计算所述第四传感器从T 5到T 6上的第六旋转数据和第六平移数据，计算所述第六旋转数据和所述第六平移数据对应的第六协方差矩阵；
    融合包括所述T 4、T 5和T 6在内的所述基准传感器的各采样时刻上的旋转数据和平移数据，以及协方差矩阵以得到所述终端的在线位姿估计值；
    其中，融合的所述旋转数据至少包括所述第四旋转数据、第五旋转数据或第六旋转数据，融合的所述平移数据至少包括所述第四平移数据、第五平移数据或第六平移数据，融合的所述协方差矩阵至少包括所述第四协方差矩阵、第五协方差矩阵，或第六协方差矩阵。
  8. 一种传感器数据处理的装置，其特征在于，包括：
    确定单元，用于从终端中的至少两个传感器中确定出一个基准传感器，并确定出所述基准传感器的各采样时刻；
    获取单元，用于获取任一传感器的观测数据；
    所述确定单元，还用于在任一时刻t 1所述获取单元获取到任一传感器K的观测数据时，确定所述传感器K的传感器类型；
    所述确定单元，还用于在所述传感器K为第一类型的第一传感器时，从所述基准传感器的各采样时刻中确定出距离所述t 1最近的采样时刻T 1，根据所述第一传感器的采样间隔和所述t 1确定在所述T 1上插值时的插值系数λ 1；
    计算单元，用于所述确定单元确定的所述λ 1和所述获取单元获取的所述第一传感器的观测数据计算所述第一传感器在所述T 1的第一旋转数据和第一平移数据，计算所述第一旋转数据和所述第一平移数据对应的第一协方差矩阵；
    所述确定单元，还用于在所述传感器K为第二类型的第二传感器时，从所述基准传感 器的各采样时刻中确定出距离所述t 1最近的两个采样时刻T 2和T 3，根据所述第二传感器的采样间隔和所述t 1确定在所述T 2上插值时的插值系数λ 2和在所述T 3上插值时的插值系数λ 3；
    所述计算单元，还用于根据所述确定单元确定的根据所述λ 2、λ 3和所述第二传感器的观测数据计算所述第二传感器从T 2到T 3的第二旋转数据和第二平移数据，计算所述第二旋转数据和所述第二平移数据对应的第二协方差矩阵；
    数据融合单元，用于融合所述计算单元处理得到的包括所述T 1、T 2和T 3在内的所述基准传感器的各采样时刻上的旋转数据、平移数据和协方差矩阵以得到所述终端在所述各采样时刻上的位姿估计值；
    其中，融合的所述旋转数据至少包括所述第一旋转数据和/或所述第二旋转数据，融合的所述平移数据至少包括所述第一平移数据和/或所述第二平移数据，融合的所述协方差矩阵至少包括所述第一协方差矩阵，和/或所述第二协方差矩阵。
  9. 根据权利要求8所述的装置，其特征在于，所述确定单元用于：
    根据所述第一传感器的采样间隔确定所述t 1之前的最近邻采样时刻t 2，并根据所述t 1和t 2计算在所述T 1上插值时的插值系数λ 1；
    其中，所述λ 1满足：
    Figure PCTCN2018107049-appb-100019
  10. 根据权利要求9所述的装置，其特征在于，所述第一传感器的观测数据包括所述第一传感器的旋转矩阵和平移向量；
    所述获取单元，用于获取所述第一传感器在所述t 1上的旋转矩阵
    Figure PCTCN2018107049-appb-100020
    和平移向量 Gp 1，以及所述第一传感器在所述t 2上的旋转矩阵
    Figure PCTCN2018107049-appb-100021
    和平移向量 Gp 2；
    所述计算单元，用于根据所述获取单元获取的所述λ 1、
    Figure PCTCN2018107049-appb-100022
    Gp 1、
    Figure PCTCN2018107049-appb-100023
    和 Gp 2计算所述第一传感器在所述T 1上的第一旋转矩阵
    Figure PCTCN2018107049-appb-100024
    和第一平移向量 Gp T1；
    其中，所述
    Figure PCTCN2018107049-appb-100025
    满足：
    Figure PCTCN2018107049-appb-100026
    所述 Gp T1满足：
    Gp T1＝(1-λ 1) Gp 2+λ 1 Gp 1；
    其中，所述第一旋转矩阵
    Figure PCTCN2018107049-appb-100027
    为第一旋转数据，所述第一平移向量 Gp T1为第一平移数据。
  11. 根据权利要求8所述的装置，其特征在于，所述确定单元用于：
    根据所述第二传感器的采样间隔和所述t 1确定所述t 1之前的采样时刻t 2，并根据所述t 1和所述t 2确定在所述T 2上插值时的插值系数λ 2和在所述T 3上插值时的插值系数λ 3；
    其中，所述λ 2满足：
    Figure PCTCN2018107049-appb-100028
    所述λ 3满足：
    Figure PCTCN2018107049-appb-100029
    其中，T 2表示所述基准传感器的采样时刻中距离t 1最近的采样时刻，T 3表示所述基准传感器的采样时刻中距离t 2最近的采样时刻。
  12. 根据权利要求11所述的装置，其特征在于，所述第二传感器的观测数据包括所述第二传感器的旋转矩阵和平移向量；
    所述获取单元，用于获取所述第二传感器在所述t 1和所述t 2之间的旋转矩阵
    Figure PCTCN2018107049-appb-100030
    以及所述第二传感器在所述t 1和所述t 2之间的平移向量 2p 1；
    所述计算单元，用于根据所述获取单元获取的所述λ 2、所述λ 3、所述
    Figure PCTCN2018107049-appb-100031
    和所述 2p 1计算所述第二传感器在所述T 2和T 3之间的相对运动的第二旋转矩阵
    Figure PCTCN2018107049-appb-100032
    和第二平移向量 T3p T2；
    其中，所述
    Figure PCTCN2018107049-appb-100033
    满足：
    Figure PCTCN2018107049-appb-100034
    所述 T3p T2满足：
    Figure PCTCN2018107049-appb-100035
    其中，所述第二旋转矩阵
    Figure PCTCN2018107049-appb-100036
    为第二旋转数据，所述第二平移向量 T3p T2为第二平移数据。
  13. 根据权利要求10或12所述的装置，其特征在于，所述装置还包括：
    地图构建单元，用于根据所述数据融合单元处理得到的包括所述T 1、T 2和T 3在内的所述基准传感器的各采样时刻上的旋转矩阵和平移向量，以及所述终端在所述各采样时刻上的位姿估计值在全局坐标系下构建校验地图；
    其中，所述校验地图用于为所述终端的在线位姿估计值的确定提供参考数据。
  14. 根据权利要求13所述的装置，其特征在于，所述装置还包括：
    启动单元，用于启动终端的在线定位功能，并获取所述终端的校验地图；
    所述确定单元，还用于当所述获取单元在任一时刻t 3获取到任一传感器L的观测数据时，确定所述传感器L的传感器类型；
    所述确定单元，还用于在所述传感器L为第一类型的第三传感器时，从所述基准传感器的各采样时刻中确定出距离所述t 3最近的采样时刻T 4；
    所述计算单元，还用于计算所述第三传感器在所述T 4上的第四旋转数据和第四平移数据，以及所述第四旋转数据和所述第四平移数据对应的第四协方差矩阵；
    若所述传感器L为第二类型的第四传感器，所述确定单元和所述计算单元还用于执行操作：
    所述确定单元，还用于将所述第四传感器在所述t 3上的观测数据与所述校验地图进行匹配，确定所述第四传感器在所述t 3上的参考旋转数据和参考平移数据，从所述基准传感 器的各采样时刻中确定出距离所述t 3最近的采样时刻T 4；
    所述计算单元，还用于根据所述第四传感器在t 3上的参考旋转数据和参考平移数据计算所述第四传感器在所述T 4上的第五旋转数据和第五平移数据，以及所述第五旋转数据和所述第五平移数据对应的第五协方差矩阵；
    所述确定单元，还用于从所述基准传感器的各采样时刻中确定出距离所述t 3最近的两个采样时刻T 5和T 6，根据所述第四传感器的采样间隔和所述t 3确定在所述T 5上插值时的插值系数λ 5和在所述T 6上插值时的插值系数λ 6；
    所述计算单元，还用于根据所述λ 5、λ 6和所述第四传感器的观测数据计算所述第四传感器从T 5到T 6上的第六旋转数据和第六平移数据，计算所述第六旋转数据和所述第六平移数据对应的第六协方差矩阵；
    所述数据融合单元，还用于融合所述计算单元处理得到的包括所述T 4、T 5和T 6在内的所述基准传感器的各采样时刻上的旋转数据和平移数据，以及协方差矩阵以得到所述终端的在线位姿估计值；
    其中，融合的所述旋转数据至少包括所述第四旋转数据、第五旋转数据或第六旋转数据，融合的所述平移数据至少包括所述第四平移数据、第五平移数据或第六平移数据，融合的所述协方差矩阵至少包括所述第四协方差矩阵、第五协方差矩阵，或第六协方差矩阵。
PCT/CN2018/107049 2017-09-22 2018-09-21 传感器数据处理的方法及装置 WO2019057173A1 (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
EP18859154.9A EP3680690B1 (en) 2017-09-22 2018-09-21 Sensor data processing method and apparatus
US16/825,145 US11379698B2 (en) 2017-09-22 2020-03-20 Sensor data processing method and apparatus

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN201710870160.5A CN109543703B (zh) 2017-09-22 2017-09-22 传感器数据处理的方法及装置
CN201710870160.5 2017-09-22

Related Child Applications (1)

Application Number Title Priority Date Filing Date
US16/825,145 Continuation US11379698B2 (en) 2017-09-22 2020-03-20 Sensor data processing method and apparatus

Publications (1)

Publication Number Publication Date
WO2019057173A1 true WO2019057173A1 (zh) 2019-03-28

Family

ID=65809545

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2018/107049 WO2019057173A1 (zh) 2017-09-22 2018-09-21 传感器数据处理的方法及装置

Country Status (4)

Country Link
US (1) US11379698B2 (zh)
EP (1) EP3680690B1 (zh)
CN (1) CN109543703B (zh)
WO (1) WO2019057173A1 (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112444246A (zh) * 2020-11-06 2021-03-05 北京易达恩能科技有限公司 高精度的数字孪生场景中的激光融合定位方法
CN113391266A (zh) * 2021-05-28 2021-09-14 南京航空航天大学 基于非圆多嵌套阵降维子空间数据融合的直接定位方法
CN117294832A (zh) * 2023-11-22 2023-12-26 湖北星纪魅族集团有限公司 数据处理方法、装置、电子设备和计算机可读存储介质

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10957066B2 (en) * 2019-03-19 2021-03-23 General Electric Company Systems and methods for locating humans using dynamic field robotic-sensor network of human robot team
US11495062B2 (en) * 2019-04-02 2022-11-08 The Boeing Company Seat component fault determination and prediction
CN110926465A (zh) * 2019-12-11 2020-03-27 哈尔滨工程大学 一种mems/gps松组合导航方法
WO2021141049A1 (ja) * 2020-01-10 2021-07-15 ファナック株式会社 制御システム
US11954179B2 (en) * 2020-09-11 2024-04-09 Inceptio Hongkong Limited Resolving out of sequence data in sensor tracking
CN114384923B (zh) * 2020-10-22 2023-05-23 福建江夏学院 一种x对称结构测量无人机姿态角的方法
CN114666784A (zh) * 2020-12-23 2022-06-24 维沃移动通信有限公司 上报终端传感器信息的方法、终端和可读存储介质
CN113408586B (zh) * 2021-05-24 2022-01-04 杭州电子科技大学 一种基于双向解相关的乱序数据融合方法
CN113327344B (zh) * 2021-05-27 2023-03-21 北京百度网讯科技有限公司 融合定位方法、装置、设备、存储介质及程序产品
CN113804194B (zh) * 2021-10-25 2024-03-15 中国第一汽车股份有限公司 一种行驶设备的定位方法、装置、设备及存储介质

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103105611A (zh) * 2013-01-16 2013-05-15 广东工业大学 一种分布式多传感器智能信息融合方法
US20150363361A1 (en) * 2014-06-16 2015-12-17 Mitsubishi Electric Research Laboratories, Inc. Method for Kernel Correlation-Based Spectral Data Processing
CN106597498A (zh) * 2017-01-18 2017-04-26 哈尔滨工业大学 多传感器融合***空时偏差联合校准方法

Family Cites Families (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3904215B2 (ja) * 2003-09-19 2007-04-11 三菱電機株式会社 センサ管理装置及びセンサ管理方法
WO2013124292A1 (en) * 2012-02-21 2013-08-29 Karlsruher Institut für Technologie Method and system for simultaneous receiver calibration and object localisation for multilateration
CN103455675B (zh) * 2013-09-04 2016-08-24 哈尔滨工程大学 一种基于ckf的非线性异步多传感器信息融合方法
CN103455642B (zh) 2013-10-10 2017-03-08 三星电子（中国）研发中心 一种多媒体文件检索的方法和装置
CN103714045A (zh) * 2014-01-09 2014-04-09 北京理工大学 面向异步多速率不均匀采样观测数据的信息融合估计方法
CN103942447B (zh) 2014-04-30 2015-03-04 中国人民解放军空军预警学院监控***工程研究所 一种多源异类传感器数据融合方法及装置
US10012504B2 (en) * 2014-06-19 2018-07-03 Regents Of The University Of Minnesota Efficient vision-aided inertial navigation using a rolling-shutter camera with inaccurate timestamps
WO2018086133A1 (en) * 2016-11-14 2018-05-17 SZ DJI Technology Co., Ltd. Methods and systems for selective sensor fusion
US10241520B2 (en) * 2016-12-22 2019-03-26 TCL Research America Inc. System and method for vision-based flight self-stabilization by deep gated recurrent Q-networks
US10267924B2 (en) * 2017-01-04 2019-04-23 Qualcomm Incorporated Systems and methods for using a sliding window of global positioning epochs in visual-inertial odometry

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103105611A (zh) * 2013-01-16 2013-05-15 广东工业大学 一种分布式多传感器智能信息融合方法
US20150363361A1 (en) * 2014-06-16 2015-12-17 Mitsubishi Electric Research Laboratories, Inc. Method for Kernel Correlation-Based Spectral Data Processing
CN106597498A (zh) * 2017-01-18 2017-04-26 哈尔滨工业大学 多传感器融合***空时偏差联合校准方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
See also references of EP3680690A4 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112444246A (zh) * 2020-11-06 2021-03-05 北京易达恩能科技有限公司 高精度的数字孪生场景中的激光融合定位方法
CN112444246B (zh) * 2020-11-06 2024-01-26 北京易达恩能科技有限公司 高精度的数字孪生场景中的激光融合定位方法
CN113391266A (zh) * 2021-05-28 2021-09-14 南京航空航天大学 基于非圆多嵌套阵降维子空间数据融合的直接定位方法
CN113391266B (zh) * 2021-05-28 2023-04-18 南京航空航天大学 基于非圆多嵌套阵降维子空间数据融合的直接定位方法
CN117294832A (zh) * 2023-11-22 2023-12-26 湖北星纪魅族集团有限公司 数据处理方法、装置、电子设备和计算机可读存储介质
CN117294832B (zh) * 2023-11-22 2024-03-26 湖北星纪魅族集团有限公司 数据处理方法、装置、电子设备和计算机可读存储介质

Also Published As

Publication number Publication date
CN109543703A (zh) 2019-03-29
US11379698B2 (en) 2022-07-05
CN109543703B (zh) 2023-06-23
EP3680690A1 (en) 2020-07-15
EP3680690A4 (en) 2020-12-02
EP3680690B1 (en) 2024-04-17
US20200226434A1 (en) 2020-07-16

Similar Documents

Publication Publication Date Title
WO2019057173A1 (zh) 传感器数据处理的方法及装置
CN107179086B (zh) 一种基于激光雷达的制图方法、装置及***
CN109521403B (zh) 多线激光雷达的参数标定方法及装置、设备及可读介质
Hartley et al. Rotation averaging
US10109104B2 (en) Generation of 3D models of an environment
CN107194962B (zh) 点云与平面图像融合方法及装置
KR100855657B1 (ko) 단안 줌 카메라를 이용한 이동로봇의 자기위치 추정 시스템및 방법
JP2022510418A (ja) 時間同期処理方法、電子機器及び記憶媒体
CN112781594B (zh) 基于imu耦合的激光雷达迭代最近点改进算法
CN112556685B (zh) 导航路线的显示方法、装置和存储介质及电子设备
Lagudi et al. An alignment method for the integration of underwater 3D data captured by a stereovision system and an acoustic camera
CN111811507B (zh) 移动设备姿态的确定方法、装置、存储介质及电子设备
Tong et al. Multi-UAV collaborative absolute vision positioning and navigation: a survey and discussion
CN112539746B (zh) 基于多频卡尔曼滤波的机器人视觉/ins组合定位方法及***
CN111260751A (zh) 基于多传感器移动机器人的建图方法
CN110967017A (zh) 一种用于双移动机器人刚体协作搬运的协同定位方法
CN102663666B (zh) 基于fpga的二维图像重采样算法加速器
Zhang et al. SDF-Loc: Signed distance field based 2D relocalization and map update in dynamic environments
CN113310505A (zh) 传感器***的外参标定方法、装置及电子设备
CN114882115B (zh) 车辆位姿的预测方法和装置、电子设备和存储介质
Cai et al. Bi-direction direct RGB-D visual odometry
WO2020019116A1 (zh) 多源数据建图方法、相关装置及计算机可读存储介质
CN114046800B (zh) 一种基于双层滤波框架的高精度里程估计方法
CN115760930A (zh) 基于多摄像头的水面目标跟踪方法、装置、计算机设备及存储介质
CN115435790A (zh) 一种视觉定位与视觉里程计位姿融合的方法及***

Legal Events

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

Ref document number: 18859154

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

ENP Entry into the national phase

Ref document number: 2018859154

Country of ref document: EP

Effective date: 20200408