WO2019057146A1 - 一种处理数据的方法和装置 - Google Patents

一种处理数据的方法和装置 Download PDF

Info

Publication number
WO2019057146A1
WO2019057146A1 PCT/CN2018/106910 CN2018106910W WO2019057146A1 WO 2019057146 A1 WO2019057146 A1 WO 2019057146A1 CN 2018106910 W CN2018106910 W CN 2018106910W WO 2019057146 A1 WO2019057146 A1 WO 2019057146A1
Authority
WO
WIPO (PCT)
Prior art keywords
vector
state vector
constraint equation
state
linearization
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Ceased
Application number
PCT/CN2018/106910
Other languages
English (en)
French (fr)
Inventor
温丰
薛常亮
付宇新
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Huawei Technologies Co Ltd
Original Assignee
Huawei Technologies Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Huawei Technologies Co Ltd filed Critical Huawei Technologies Co Ltd
Priority to EP18857990.8A priority Critical patent/EP3677978B1/en
Publication of WO2019057146A1 publication Critical patent/WO2019057146A1/zh
Priority to US16/825,694 priority patent/US20200219000A1/en
Anticipated expiration legal-status Critical
Ceased legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N7/00—Computing arrangements based on specific mathematical models
    • G06N7/01—Probabilistic graphical models, e.g. probabilistic networks
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G05D1/0285—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using signals transmitted via a public communication network, e.g. GSM network
    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N20/00—Machine learning
    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments

Definitions

  • the present application relates to the field of artificial intelligence and, more particularly, to a method and apparatus for processing data.
  • Simultaneous Localization And Mapping is a key capability that robots can rely on to explore unknown environments.
  • the robot uses its own sensors (camera, laser, etc.) to observe the features in the environment, continuously modeling the environment during the movement, and then the system state vector - environmental characteristics Position and robot pose are estimated, and map construction and self-positioning are completed at the same time.
  • the state vector after the back-end optimization is continuously marginalized by Shure complement.
  • the new objective function will be constructed by the marginalized state vector as the input of the next optimization, but after the back-end optimization based on the Shure-compensated edged method, the back-end needs to optimize all the state vectors, and When the state vector is edged, the information matrix needs to be inverted, and the overhead of computing resources and computing time is relatively high.
  • the present application provides a method for processing data, and marginalizing the state vector before optimization, which helps to reduce computational resources and computation time overhead in the SLAM state vector estimation process, thereby improving the operational efficiency of the entire SLAM process and avoiding A system crash caused by a continuous increase in the state vector.
  • a method for processing data comprising: acquiring a state vector set of a smart device, the state vector set including at least one pose vector of the smart device and/or an environment in which the smart device is located At least one landmark feature vector, the at least one pose vector and/or the at least one landmark feature vector for mapping of the smart device and map construction of the environment in which the smart device is located; determining a first linearization constraint equation The first linearization constraint equation represents a linearization constraint relationship of the at least one pose vector and/or the at least one landmark feature vector; determining a first state vector, wherein the first state vector is in the state vector set a marginalized state vector; marginalizing the first state vector in the first linearization constraint equation to obtain a second linearization constraint equation, the second linearization constraint equation representing linearization of the second state vector a constraint relationship, the second state vector being a state vector other than the first state vector in the set of state vectors; Determining, by the second linearization constraint equation, an objective function
  • the method for processing data in the embodiment of the present application first performs edge processing on the state vector before optimization, which helps to reduce computational resources and computation time overhead in the SLAM state vector estimation process, thereby improving the operational efficiency of the entire SLAM process. To avoid system crashes caused by the continuous increase of the state vector.
  • At least one of the at least one pose vector may be selected as the state vector to be edged, or at least part of the at least one landmark feature vector may be selected as the state vector to be edged, or at least one At least a portion of the pose vector and the at least one landmark feature vector is used as a state vector to be edged.
  • the first state vector in the first linearization constraint equation is edged to obtain a second linearization constraint equation, including: And a null space of the Jacobian matrix of the first state vector, and the first state vector in the first linearization constraint equation is edged to obtain the second linearization constraint equation.
  • the state vector is edge-processed by zero space, and the non-critical state vector in the state vector is removed without losing information, thereby avoiding all state vectors being optimized.
  • the operation efficiency of the entire SLAM process is improved, and the system collapse problem caused by the continuous increase of the state vector is avoided.
  • the first state vector in the first linearization constraint equation is based on a null space of a Jacobian matrix of the first state vector
  • Performing edgeization to obtain the second linearization constraint equation includes: determining a Jacobian matrix of the first state vector; determining a Jacobian of the first state vector according to a Jacobian matrix of the first state vector a left zero space of the matrix; multiplying a left zero space of the Jacobian matrix of the first shape vector by the first linearization constraint equation to obtain the second linearization constraint equation.
  • the first state vector is determined Jacques than the left null space of matrix, where, H m is the vector of the first state Jacobian matrix, U m is the first state Jacques left null space vector than the matrix.
  • the left zero space of the Jacobian matrix of the first state vector is:
  • I is the unit matrix.
  • the second linearization constraint equation is:
  • H u is a Jacobian matrix of the second state vector
  • U m is a zero space of a Jacobian matrix of the first state vector
  • n is a Gaussian white noise vector.
  • the first state vector in the first linearization constraint equation is edged to obtain a second linearization constraint equation
  • the method further includes: rearranging the first linearization constraint equation according to the first state vector, and rearranging the first state vector and the second in the first linearization constraint equation after rearrangement The state vector is separated.
  • the first linearized constraint equation after the rearrangement is:
  • the method further comprises determining an information matrix based on the objective function, the information matrix being a constraint for processing the data next time.
  • the objective function is:
  • H' u U m ⁇ H u
  • R is the covariance of U m ⁇ n .
  • the method before the determining the first linearization constraint equation of the at least one pose vector and/or the at least one landmark feature vector, the method further comprises: determining a nonlinearization constraint equation of the at least one pose vector and/or at least one landmark feature vector; the nonlinearization constraint according to the at least one pose vector and/or the Jacobian matrix of the at least one landmark feature vector The equation is linearized to obtain the first linearization constraint equation.
  • the nonlinearization constraint equation is linearly optimized by a Gauss-Newton method to obtain a first linearization constraint equation.
  • the method further includes: determining, according to the objective function, an information matrix, the information matrix is used to optimize and update the second state vector, and constructing a new one based on the optimized updated second state vector The constraint term used to constrain the linearization constraint equation established when the data is processed next time.
  • the information matrix is:
  • ⁇ is the information matrix
  • the second state vector is optimized for the graph, and the second state vector is updated. And construct the a priori constraint of the next graph optimization according to the information matrix:
  • This a priori constraint is added to the entire pose as the new constraint for the next data processing, thus improving the overall graph optimization.
  • the error vector of the state vector to be preserved For the last estimate of the state vector to be retained, The estimated value for the state vector to be retained.
  • the method for processing data in the embodiment of the present application first performs edge processing on the state vector through zero space before optimizing the target function, and removes the non-critical state vector in the state vector without losing information, thereby avoiding all states. All the vectors are optimized to help reduce the computational resources and computation time overhead in the SLAM state vector estimation process, thereby improving the operational efficiency of the entire SLAM process and avoiding system crashes caused by the continuous increase of state vectors.
  • an apparatus for processing data for performing the first aspect or any of the possible implementations of the first aspect is provided.
  • the means for processing data may comprise means for performing the method of the first aspect or any of the possible implementations of the first aspect.
  • a processing data apparatus comprising: a memory, a processor, and a communication interface, the memory being for storing a computer program, the processor for calling and running the computer program from the memory such that The apparatus for processing data performs the method of any of the first aspect or the first aspect of the first aspect.
  • the processor is configured to:
  • Obtaining a state vector set of the smart device including at least one pose vector of the smart device and/or at least one landmark feature vector of the environment in which the smart device is located, the at least one pose vector and/or Or the at least one landmark feature vector is used for positioning of the smart device and map construction of an environment in which the smart device is located;
  • the first linearization constraint equation representing a linearization constraint relationship of the at least one pose vector and/or the at least one landmark feature vector
  • Determining a first state vector wherein the first state vector is a state vector to be edged in the state vector set; and edge-coding the first state vector in the first linearization constraint equation to obtain a second a linearization constraint equation, the second linearization constraint equation representing a linearization constraint relationship of the second state vector, the second state vector being a state vector of the state vector set other than the first state vector;
  • a computer program product comprising: a computer program (also referred to as a code, or an instruction) that, when executed on a computer, causes the computer to perform the above The method of any of the possible implementations of the aspect or the first aspect.
  • a computer readable storage medium for storing a computer program, the computer program comprising instructions for performing the method of the first aspect or any of the possible implementations of the first aspect.
  • the state vector is edge-processed by zero space before the objective function is optimized, and the non-critical state vector in the state vector is removed without losing information, so that all state vectors are all optimized. It helps to reduce the computational resources and computation time overhead in the SLAM state vector estimation process, thereby improving the operational efficiency of the entire SLAM process and avoiding the system crash caused by the continuous increase of the state vector.
  • FIG. 1 is a schematic diagram of an application scenario of a technical solution in the embodiment of the present application.
  • FIG. 2 is a schematic flowchart of a method for processing data according to an embodiment of the present application.
  • FIG. 3 is a schematic flowchart of a zero space edge method according to an embodiment of the present application.
  • FIG. 4 is another schematic flowchart of a method for processing data according to an embodiment of the present application.
  • FIG. 5 is another schematic flowchart of a zero space margining method according to an embodiment of the present application.
  • FIG. 6 is still another schematic flowchart of a method for processing data according to an embodiment of the present application.
  • FIG. 7 is a comparison of computational processes of a zero space based edgeization method and a Shure complement based edge method according to an embodiment of the present application.
  • FIG. 8 is a schematic block diagram of an apparatus for processing data according to an embodiment of the present application.
  • FIG. 9 is another schematic block diagram of an apparatus for processing data according to an embodiment of the present application.
  • A is known to be an m ⁇ n matrix.
  • a nullspace also known as kernel, is a set of n-dimensional vectors defined by the following formula:
  • the embodiments of the present application are applicable to the field of Simultaneous Localization And Mapping (SLAM) and any intelligent equipment used for state edge processing in nonlinear estimation problems.
  • SLAM Simultaneous Localization And Mapping
  • the smart device refers to any device, device or machine with computing processing capability.
  • the smart device in the embodiment of the present application may be a robot, an autonomous vehicle, or an unmanned aerial. Vehicles, Smart Homes, and mobile terminals, etc., do not limit the smart device.
  • FIG. 1 is a schematic diagram of an application scenario of the technical solution of the embodiment of the present application.
  • the technical solution of the embodiment of the present application is applicable to a SLAM system 100 of a robot, and the SLAM system includes a front end (Front- End) 110 and back-end 120, wherein the front end 110 mainly performs data acquisition and data association of the sensor, and the main function of the back end 120 is to perform parameter estimation and optimization on the model generated by the front end.
  • the technical solution of the embodiment of the present application is described below by taking a robot as an example.
  • FIG. 2 shows a schematic flow diagram of a method 200 of processing data in accordance with an embodiment of the present application.
  • the method 200 can be performed by the SLAM system 100 of FIG. 1, the method 200 comprising:
  • S210 Acquire a state vector set of the smart device, where the state vector set includes at least one pose vector of the smart device and/or at least one landmark feature vector of the environment in which the smart device is located, the at least one pose vector and/or the At least one landmark feature vector is used for the location of the smart device and the map construction of the environment in which the smart device is located.
  • the front end 110 of the SLAM system 100 of the robot acquires data of the visual sensor and the inertial sensor IMU, extracts at least one pose vector of the robot, and performs time synchronization with the acquired image on the acquired image to extract visual image information. At least one landmark feature vector of the environment in which the robot is located, and feature matching.
  • the feature matching is to determine which of the two acquired image data corresponds to the same landmark feature.
  • the robot generates a trajectory (x 1 , x 2 ,..., x k ) during the movement. Since the movement of the robot conforms to a certain running model law, the movement of the robot is established based on the IMU data and the motion model. equation:
  • u k represents an observation from a motion sensor such as an odometer or an Inertial Measurement Unit (IMU)
  • w k is a Gaussian white noise with a covariance Q k .
  • the visual sensor extracts the landmark feature vector, but the visual sensor can estimate the pose vector of the robot by using the data of the upper and lower frames, and the inertial sensor extracts the pose vector of the robot.
  • the robot has many road signs in the motion scene, and the robot measures a certain landmark point through the sensor at each position of the motion process to obtain corresponding observation values. Furthermore, the robot relies on the observation model to detect and track the features. Based on the extracted landmark feature vector, the observation model and the pose after the previous frame optimization, the robot's observation equation is established:
  • Z kj represents the observed value of the feature x fj when the robot is in the pose x k
  • v kj is the Gaussian white noise with the covariance R k .
  • the S210 acquisition state vector set may be performed by the front end 110 of the SLAM system 100 of FIG.
  • the noise is a Gaussian distribution satisfying N(0, ⁇ 2 ), where 0 represents the mean and ⁇ 2 represents the variance, and the variance of the noise corresponding to the specific equation of motion is Q k , which corresponds to the specific observation equation.
  • the variance of the noise is R k .
  • At least one pose vector of the robot and/or at least one landmark feature vector of the environment in which the robot is located is acquired at S210, and the established motion equation (1) and the observation equation (2) are the at least one pose
  • the vector and/or the nonlinearization equation of the at least one landmark feature vector linearizes the nonlinear equation by the at least one pose vector and/or the Jacobian matrix of the at least one landmark feature vector.
  • constructing a linear constraint equation is mainly to convert the nonlinearized constraint representation of the state vector into a linearized mathematical expression to simplify the solution process.
  • S220 determines that the at least one pose vector and/or the first linearization constraint equation of the at least one landmark feature vector may be performed by the back end 120 of the SLAM system 100.
  • F k is the Jacobian matrix of the state k of the robot pose in the motion model
  • H kj and H fj are the Jacobian matrices of the state pose vector of the robot pose vector and the landmark feature vector in the observation equation, respectively.
  • the first linearized constraint equation is obtained by linearizing the constraint equation with the Jacobian matrix:
  • one of the pose vector and the landmark feature vector of the robot may be selected as a specific state vector to be edged, or both may be simultaneously used as state vectors to be edged. .
  • linearized constraint equations are rearranged using the determined state vector to be optimized to prepare for subsequent edged operations.
  • S230 determines that the first state vector can be performed by the back end 120 of the SLAM system 100.
  • the point in the window that is longer than the current state point of the robot is taken as the point to be actually marginalized.
  • the size of the sliding window is set to D (executed only during system initialization), and the current The pose vector of the (kD) moment observed in the window and all the landmark feature vectors are used as state vectors to be marginalized.
  • a point at which the parallax is observed from the current state point of the robot can be regarded as an actual point to be marginalized by the parallax method.
  • S220 and S230 have no actual sequence, and the first linearization constraint equation may be determined first, then the state vector to be edged is determined, or the state vector to be edged may be determined first, and then the first linearity is determined.
  • the constraint equation is not limited to this application.
  • the method further includes:
  • the first linearization constraint equation is rearranged according to the first state vector, and the first state vector in the rearranged first linearization constraint equation is separated from the second state vector.
  • step S230 it is determined that the pose vector of the (kD) time observed in the current window and all the landmark feature vectors are used as the state vector to be edged, and the linearization constraint equation established in step S220 is rearranged, and is to be edged.
  • the state vectors are separated and the first linearized constraint equation (6) is obtained:
  • H u is the vector to be retained state Jacobian matrix
  • H m is the Jacobian matrix of the state vector to be edged
  • n is a Gaussian white noise vector.
  • S240 determines that the second linearization constraint equation can be performed by the back end 120 of the SLAM system 100.
  • the state vector in order to keep the SLAM state vector bounded, it is necessary to limit the data of the state vector to reduce the calculation overhead and the calculation time, so the state vector is edge-processed before the optimization, and the non-critical state in the state vector set is determined.
  • Vector avoiding all state vectors to be optimized, and by marginalizing the state vector to be edged in the first linearization constraint equation, obtaining a second linearization constraint equation of the second state vector, so that the second linearization constraint Only the required state vectors are retained in the equation.
  • the first state vector in the first linearization constraint is edged to obtain a second linearization constraint equation of the second state vector, including:
  • first state vector in the first linearization constraint equation is edged according to the zero space of the Jacobian matrix of the first state vector, to obtain the second linearization constraint equation.
  • FIG. 3 shows a schematic flow diagram of a zero space margining method 300, as shown in FIG. 3, which may be performed by the back end 120 of the SLAM system of FIG. 1, which is performed as shown in FIG. Method 300 includes:
  • the H m in the formula (7) is the Jacobian matrix of the state vector to be edged.
  • the left zero space of the Jacobian matrix of the edged state vector is calculated, and according to the constraint equation (7) after the linearization, the left zero space U m corresponding to H m is solved, and the equation is:
  • the calculated left linear space U m is left-multiplied by the first linearized constraint equation, and the equation is as follows:
  • the zero space edge method of the embodiment of the present application helps to reduce the calculation time by removing the non-critical state vector in the state vector by zero space of the state vector to be edged.
  • ML Maximum Likelihood
  • maximum probability estimation is a theoretical point estimation method. The basic idea of this method is: when n sets of sample observations are randomly selected from the model population. The most reasonable parameter estimator should be such that the probability of extracting the n sets of sample observations from the model is the greatest.
  • a new optimization objective function can be constructed for the back end 120 of the SLAM system 100 for optimization, specifically, the second linearization constraint according to the newly constructed reduction. Equation (11), construct an optimization objective function, and get:
  • H' u U m ⁇ H u
  • R is the covariance of U m ⁇ n .
  • the method 100 for processing data further includes:
  • the method further includes:
  • the information matrix after the edgeization is calculated according to the formula (12) as follows:
  • the optimal value of the second state vector is estimated by the graph optimization method, and the information matrix is determined, which is jointly used as a constraint condition for the next data processing.
  • the landmark feature vector is optimally estimated and optimized to obtain a trajectory of the robot and a map of the environment in which the robot is located.
  • Performing a map optimization on the second state vector and updating the second state vector Construct a priori constraint for the next graph optimization based on the obtained information matrix:
  • the error vector of the state vector to be preserved For the last estimate of the state vector to be retained, The estimated value for the state vector to be retained.
  • the method for processing data in the embodiment of the present application first performs edge processing on the state vector through zero space before optimizing the target function, and removes the non-critical state vector in the state vector without losing information, thereby avoiding all states. All the vectors are optimized to help reduce the computational resources and computation time overhead in the SLAM state vector estimation process, thereby improving the operational efficiency of the entire SLAM process and avoiding system crashes caused by the continuous increase of state vectors.
  • FIG. 4 shows a schematic flowchart of a method 400 for processing data according to an embodiment of the present application. As shown in FIG. 4, the method 400 includes:
  • Collect sensor information such as camera and IMU predict the image information of the camera, synchronize sensor data such as camera and IMU, and obtain the observed state quantity of the current pose of the smart device.
  • the nonlinearization constraint relationship between all state vectors established by S420 is linearized, the left zero space is used to remove the non-critical state vector in the state vector, and the optimization objective function is constructed according to the constraint of the state vector to be retained.
  • the EKF method or the nonlinear optimization method is used to optimally estimate and optimize all state vectors to obtain globally consistent trajectories and maps.
  • S410 and S420 may be processed by the front end 110 of the SLAM system 100, and S430 and S440 may be processed by the back end 120 of the SLAM system 100.
  • FIG. 5 shows a schematic flowchart of a zero space edged processing method 500 according to an embodiment of the present application. As shown in FIG. 5, the method 500 includes:
  • linear constraint equations are mainly to transform the nonlinearized constrained representation of the state vector into a linearized mathematical expression to simplify the solution process.
  • the linearized constraint equation is rearranged by using the determined state vector to be optimized to prepare for the subsequent edged operation. It mainly involves the following two steps:
  • the pose vector of the smart device or the landmark feature vector of the environment in which the smart device is located is edged, and at the same time, which pose vectors or landmark feature vectors are edged.
  • the Jacobian matrix of the state vector is calculated, and the linearized constraint equation of the state vector is constructed by using the Jacobian matrix, and the linearization equation is rearranged according to the determined state vector to be edged.
  • Calculating the left zero space is mainly to complete the calculation operation of the left zero space of the Jacobian matrix of the edged state vector.
  • the reduction objective function is mainly to perform the left multiplication space operation on the linearization equation, remove the non-critical state vector in the linearization constraint equation, obtain the linearization constraint equation after the marginalization, and construct a new linear equation based on the marginalization. Optimization of the objective function. It mainly involves the following two steps:
  • the null space of the Jacobian matrix of the edge vector to be edged is multiplied by the linearization constraint equation to remove the non-critical state vector in the linearization equation.
  • FIG. 6 shows a schematic flowchart of a method 600 for processing data according to an embodiment of the present application. As shown in FIG. 6, the method 600 includes:
  • S601 acquiring data of the visual sensor and the inertial sensor IMU, and performing time synchronization on the acquired image and the IMU data;
  • S604. Determine a state vector to be edged, and determine a state vector to be edged in the pose vector and the landmark feature vector by sliding the window method or the parallax method;
  • S607 Calculate a left zero space of a Jacobian matrix of the edged vector, and solve a left zero space of the state vector to be edged according to the first linearized constraint equation rearranged in S606;
  • the edged state vector is determined, and the reduced linearization constraint equation is determined.
  • the left zero space of the state vector to be edged calculated by S607 is left-multiplied by the first linearized constraint equation rearranged in the S606. Obtaining a reduced second linearization constraint equation;
  • Table 1 shows a comparison of computational time based on the Shure complement marginalization method and the zero-space marginalization method according to an embodiment of the present application. As can be seen from Table 1, compared to the Shure complementation marginalization method, based on zero space The marginalization method is 4.6 times more efficient.
  • FIG. 7 shows a comparison of calculation steps of the zero space method and the Shure complement method according to an embodiment of the present application.
  • the computational complexity of the two are compared, and it is assumed that m robot poses track n landmark features.
  • the black box indicates the most time-consuming step in the two marginal methods.
  • n is much larger than m. It can be seen that the computational complexity of the zero-space method is smaller than the Shure complement method.
  • FIG. 8 shows a schematic block diagram of an apparatus 700 for processing data according to an embodiment of the present application. As shown in FIG. 8, the apparatus 700 includes:
  • the obtaining unit 710 is configured to acquire a state vector set of the smart device, where the state vector set includes at least one pose vector of the smart device and/or at least one landmark feature vector of the environment in which the smart device is located, the at least one pose vector sum / or the at least one landmark feature vector is used for positioning of the smart device and map construction of the environment in which the smart device is located;
  • a determining unit 720 configured to determine a first linearization constraint equation, where the first linearization constraint equation represents a linearization constraint relationship of the at least one pose vector and/or the at least one landmark feature vector;
  • the determining unit 720 is further configured to determine a first state vector, where the first state vector is a state vector to be edged in the state vector set;
  • the edged unit 730 is configured to perform edgeization on the first state vector in the first linearization constraint equation to obtain a second linearization constraint equation, where the second linearization constraint equation represents a linearization constraint of the second state vector a relationship, the second state vector being a state vector of the state vector set other than the first state vector;
  • the determining unit 720 is further configured to determine an objective function of a maximum likelihood estimation of the state vector according to the second linearization constraint equation
  • the optimizing unit 740 is configured to optimize the at least one pose vector and/or the at least one landmark feature vector according to the objective function, and output a trajectory of the smart device and a map of an environment in which the smart device is located.
  • the edged unit 730 is specifically configured to:
  • first state vector in the first linearization constraint equation is edged according to the zero space of the Jacobian matrix of the first state vector, to obtain the second linearization constraint equation.
  • the edged unit is specifically configured to:
  • the second linearization constraint equation is obtained by multiplying the left zero space of the Jacobian matrix of the first shape vector by the first linearization constraint equation.
  • the second linearization constraint equation is:
  • n is a Gaussian white noise vector.
  • the apparatus 700 further includes:
  • a rearrangement unit configured to rearrange the first linearization constraint equation according to the first state vector, and separate the first state vector and the second state vector in the rearranged first linearization constraint equation .
  • the first linearization constraint equation after the rearrangement is:
  • H m is the Jacobian matrix of the first state vector
  • H u is the Jacobian matrix of the second state vector
  • z ⁇ is the residual vector of the observed vector
  • n is Gaussian White noise vector.
  • the determining unit 720 is further configured to:
  • an information matrix is determined, which is a constraint for processing the data next time.
  • the determining unit is further configured to:
  • the determining unit 720 is further configured to:
  • apparatus 700 for processing data may correspond to a SLAM system in method 200 of processing data in accordance with embodiments of the present application, which may include SLAM system execution for performing method 200 of processing data in FIG.
  • the module of the method are respectively used to implement the corresponding process of the method 200 for processing data in FIG. 2, and specifically, the obtaining unit 710 is configured to perform step 210 in the method 200.
  • the determining unit 720 is configured to perform step 220, step 230, and step 250 in the method 200.
  • the edge unit 730 is configured to perform step 240 in the method 200, and the optimizing unit 740 is configured to perform step 260 in the method 200.
  • the specific process of the above-mentioned corresponding steps has been described in detail in the method 200. For brevity, no further details are provided herein.
  • FIG. 9 is a schematic structural diagram of an apparatus 800 for processing data according to an embodiment of the present application.
  • the apparatus 800 for processing data includes a processor 810, a memory 820, and a communication interface 830.
  • the processor 810 is configured to execute an instruction in the memory 820.
  • the processor 510 is configured to execute the method provided by the foregoing method embodiment, and the processor 810 is further configured to control
  • the communication interface 830 communicates with the outside world.
  • apparatus 800 for processing data may correspond to SLAM system 100 in accordance with an embodiment of the present application, which may include means for performing the method performed by SLAM system 100 of method 200 of processing data in FIG.
  • each module in the apparatus 800 for processing data and the other operations and/or functions described above are respectively configured to implement the corresponding flow of the method 200 of processing data in FIG.
  • the specific process in which each module performs the above-mentioned corresponding steps has been described in detail in the method 200. For brevity, no further details are provided herein.
  • the embodiment of the present application further provides a computer readable storage medium, comprising a computer program, when executed on a computer, causing the computer to execute the method provided by the foregoing method embodiment.
  • the embodiment of the present application further provides a computer program product comprising instructions, when the computer program product is run on a computer, causing the computer to execute the method provided by the foregoing method embodiment.
  • the processor may be a central processing unit (CPU), and the processor may also be other general-purpose processors, digital signal processors (DSPs), and dedicated integration.
  • DSPs digital signal processors
  • 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 memory in the embodiments of the present application may be a volatile memory or a non-volatile memory, or may include both volatile and non-volatile memory.
  • the non-volatile memory may be a read-only memory (ROM), a programmable read only memory (ROMM), an erasable programmable read only memory (erasable PROM, EPROM), or an electrical Erase programmable EPROM (EEPROM) or flash memory.
  • the volatile memory can be a random access memory (RAM) that acts as an external cache.
  • RAM random access memory
  • RAM random access memory
  • SRAM static random access memory
  • DRAM dynamic random access memory
  • synchronous dynamic randomness synchronous dynamic randomness.
  • Synchronous DRAM SDRAM
  • DDR SDRAM double data rate synchronous DRAM
  • ESDRAM enhanced synchronous dynamic random access memory
  • SLDRAM synchronous connection dynamic random access memory Take memory
  • DR RAM direct memory bus random access memory
  • the disclosed systems, devices, and methods may be implemented in other manners.
  • the device embodiments described above are merely illustrative.
  • the division of the unit is only a logical function division.
  • there may be another division manner for example, multiple units or components may be combined or Can be integrated into another system, or some features can be ignored or not executed.
  • the mutual coupling or direct coupling or communication connection shown or discussed may be an indirect coupling or communication connection through some interface, device or unit, and may be in an electrical, mechanical or other form.
  • the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, may be located in one place, or may be distributed to multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of the embodiment.
  • each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit.
  • the functions may be stored in a computer readable storage medium if implemented in the form of a software functional unit and sold or used as a standalone product.
  • the technical solution of the present application which is essential or contributes to the prior art, or a part of the technical solution, may be embodied in the form of a software product, which is stored in a storage medium, including
  • the instructions are used to cause a computer device (which may be a personal computer, server, or network device, etc.) to perform all or part of the steps of the methods described in various embodiments of the present application.
  • the foregoing storage medium includes: a U disk, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk, and the like, which can store program codes. .

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Software Systems (AREA)
  • Multimedia (AREA)
  • Evolutionary Computation (AREA)
  • Artificial Intelligence (AREA)
  • General Engineering & Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Computing Systems (AREA)
  • Data Mining & Analysis (AREA)
  • Electromagnetism (AREA)
  • Medical Informatics (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Algebra (AREA)
  • Pure & Applied Mathematics (AREA)
  • Probability & Statistics with Applications (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Business, Economics & Management (AREA)
  • Health & Medical Sciences (AREA)
  • Game Theory and Decision Science (AREA)

Abstract

一种处理数据的方法和装置，该方法包括：获取状态向量集合（210），该状态向量集合包括智能设备的位姿向量和/或该智能设备所处环境的路标特征向量；确定第一线性化约束方程（220）；确定第一状态向量，该第一状态向量为该状态向量集合中待边缘化的状态向量（230）；对该第一线性化约束方程中的该第一状态向量进行边缘化，得到第二线性化约束方程（240）；根据该第二线性化约束方程，确定目标函数（250）；根据该目标函数，对该至少一个位姿向量和/或该至少一个路标特征向量进行优化，输出该智能设备的轨迹和该智能设备所处环境的地图（260）。该处理数据的方法，有助于降低计算资源和计算时间开销，提升整个SLAM过程的运行效率。

Description

一种处理数据的方法和装置
本申请要求于2017年9月22日提交中国专利局、申请号为201710868757.6、申请名称为“一种处理数据的方法和装置”的中国专利申请的优先权，其全部内容通过引用结合在本申请中。
技术领域
本申请涉及人工智能领域，并且更具体地，涉及一种处理数据的方法和装置。
背景技术
同时定位与地图构建(Simultaneous Localization And Mapping，SLAM)技术是机器人能够探索未知环境所依赖的关键能力。机器人在没有环境先验信息的情况下，利用自身搭载的传感器(摄像头、激光等)对环境中的特征进行观测，在移动过程中对环境进行持续建模，进而对系统状态向量——环境特征位置与机器人位姿进行估计，同时完成地图构建与自定位。
随着机器人在场景中不断移动，以及运动场景的不断扩大，环境特征位置和机器人位姿的数量都会持续增加，导致在通过滤波器或者非线性方法求解时的计算量以及内存开销持续增大，大大降低SLAM过程的实时性，甚至最终造成系统不可正常运行。
目前比较常用的是一种基于舒尔补的边缘化方法，在经典SLAM框架中，将后端优化之后的状态向量—机器人位姿和路标特征点，通过舒尔补持续的进行边缘化处理，并将通过边缘化后的状态向量构建新的目标函数作为下一次优化的输入，但是基于舒尔补的边缘化方法在后端优化之后，后端需要对所有的状态向量全部进行优化，并且在边缘化状态向量时，需要对信息矩阵求逆，计算资源和计算时间的开销相对较高。
因此，如何降低SLAM状态向量估计过程中的计算资源和计算时间开销，提升整个SLAM过程的运行效率成为了一个亟待解决的问题。
发明内容
本申请提供一种处理数据的方法，在优化之前先对状态向量进行边缘化处理，有助于降低SLAM状态向量估计过程中的计算资源和计算时间开销，从而提升整个SLAM过程的运行效率，避免状态向量持续增大造成的系统崩溃问题。
第一方面，提供了一种处理数据的方法，包括：获取智能设备的状态向量集合，所述状态向量集合包括所述智能设备的至少一个位姿向量和/或所述智能设备所处环境的至少一个路标特征向量，所述至少一个位姿向量和/或所述至少一个路标特征向量用于所述智能设备的定位和所述智能设备所处环境的地图构建；确定第一线性化约束方程，该第一线性化约束方程表示该至少一个位姿向量和/或该至少一个路标特征向量的线性化约束关系；确定第一状态向量，所述第一状态向量为所述状态向量集合中待边缘化的状态向量； 对所述第一线性化约束方程中的所述第一状态向量进行边缘化，得到第二线性化约束方程，该第二线性化约束方程表示第二状态向量的线性化约束关系，所述第二状态向量为所述状态向量集合中除所述第一状态向量以外的状态向量；根据所述第二线性化约束方程，确定该状态向量的最大似然估计的目标函数；根据所述目标函数，对所述至少一个位姿向量和/或所述至少一个路标特征向量进行优化，输出所述智能设备的轨迹和所述智能设备所处环境的地图。
因此，本申请实施例的处理数据的方法，在优化之前先对状态向量进行边缘化处理，有助于降低SLAM状态向量估计过程中的计算资源和计算时间开销，从而提升整个SLAM过程的运行效率，避免状态向量持续增大造成的系统崩溃问题。
在本申请实施例中，可以选择至少一个位姿向量中的至少部分作为待边缘化的状态向量，也可以选择至少一个路标特征向量中的至少部分作为待边缘化的状态向量，还可以至少一个位姿向量和至少一个路标特征向量中的至少部分作为待边缘化的状态向量。
结合第一方面，在第一方面的某些实现方式中，所述对所述第一线性化约束方程中的所述第一状态向量进行边缘化，得到第二线性化约束方程，包括：根据所述第一状态向量的雅克比矩阵的零空间，对所述第一线性化约束方程中的所述第一状态向量进行边缘化，得到所述第二线性化约束方程。
本申请实施例的处理数据的方法，通过零空间对状态向量进行边缘化处理，在不丢失信息的前提下，去掉状态向量中的非关键状态向量，避免所有的状态向量全部进行优化，有助于降低SLAM状态向量估计过程中的计算资源和计算时间开销，从而提升整个SLAM过程的运行效率，避免状态向量持续增大造成的系统崩溃问题。
结合第一方面，在第一方面的某些实现方式中，所述根据所述第一状态向量的雅克比矩阵的零空间，对所述第一线性化约束方程中的所述第一状态向量进行边缘化，得到所述第二线性化约束方程，包括：确定所述第一状态向量的雅克比矩阵；根据所述第一状态向量的雅克比矩阵，确定所述第一状态向量的雅克比矩阵的左零空间；将所述第一状体向量的雅克比矩阵的左零空间左乘所述第一线性化约束方程，得到所述第二线性化约束方程。
在一些可能的实现方式中，通过公式
H m TU m＝0
确定所述第一状态向量的雅克比矩阵的左零空间，其中，H m为所述第一状态向量的雅克比矩阵，U m为所述第一状态向量的雅克比矩阵的左零空间。
在一些可能的实现方式中，所述第一状态向量的雅克比矩阵的左零空间为：
Figure PCTCN2018106910-appb-000001
其中，I为单位矩阵。
结合第一方面，在第一方面的某些实现方式中，所述第二线性化约束方程为：
Figure PCTCN2018106910-appb-000002
其中，
Figure PCTCN2018106910-appb-000003
为所述第二状态向量的误差向量，H u为所述第二状态向量的雅克比矩阵，U m为所述第一状态向量的雅克比矩阵的零空间，
Figure PCTCN2018106910-appb-000004
为被观测向量的残差向量，n为高斯白噪声向量。
结合第一方面，在第一方面的某些实现方式中，所述对所述第一线性化约束方程中的所述第一状态向量进行边缘化，得到第二线性化约束方程之前，所述方法还包括：根据所 述第一状态向量，对所述第一线性化约束方程进行重排列，重排列后的所述第一线性化约束方程中的所述第一状态向量和所述第二状态向量分离。
结合第一方面，在第一方面的某些实现方式中，所述重排列后的所述第一线性化约束方程为：
Figure PCTCN2018106910-appb-000005
其中，
Figure PCTCN2018106910-appb-000006
为第二状态向量的误差向量，
Figure PCTCN2018106910-appb-000007
为所述第一状态向量的误差向量，H m为所述第一状态向量的雅克比矩阵，H u为所述第二状态向量的雅克比矩阵，
Figure PCTCN2018106910-appb-000008
为被观测向量的残差向量，n为高斯白噪声向量。
结合第一方面，在第一方面的某些实现方式中，所述方法还包括：根据所述目标函数，确定信息矩阵，所述信息矩阵为下一次处理数据的约束条件。
在一些可能的实现方式中，所述目标函数为：
Figure PCTCN2018106910-appb-000009
其中，
Figure PCTCN2018106910-appb-000010
H' u＝U m ΤH u，R为U m Τn的协方差。
结合第一方面，在第一方面的某些实现方式中，所述确定所述至少一个位姿向量和/或至少一个路标特征向量的第一线性化约束方程之前，所述方法还包括：确定所述至少一个位姿向量和/或至少一个路标特征向量的非线性化约束方程；根据所述至少一个位姿向量和/或至少一个路标特征向量的雅克比矩阵，对所述非线性化约束方程进行线性化，得到所述第一线性化约束方程。
在一些可能的实现方式中，通过高斯-牛顿法对所述非线性化约束方程进行线性优化，得到第一线性化约束方程。
在一些可能的实现方式中，所述方法还包括：根据该目标函数，确定信息矩阵，该信息矩阵用于优化并更新该第二状态向量，并基于优化更新后的第二状态向量，构建新的约束项，该约束项用于对下一次处理数据时建立的线性化约束方程进行约束。
在一些可能的实现方式中，所述信息矩阵为：
Figure PCTCN2018106910-appb-000011
其中，∑为所述信息矩阵。
在一些可能的实现方式中，对第二状态向量进行图优化，对第二状态向量进行更新
Figure PCTCN2018106910-appb-000012
并根据得到信息矩阵构建下一次图优化的先验约束：
Figure PCTCN2018106910-appb-000013
该先验约束作为下一次数据处理时新的约束加入整个位姿图中，从而完善整个图优化。
其中，
Figure PCTCN2018106910-appb-000014
为要保留的状态向量的误差向量，
Figure PCTCN2018106910-appb-000015
为要保留的状态向量上一次的估计值，
Figure PCTCN2018106910-appb-000016
为要保留的状态向量更新后的估计值。本申请实施例的处理数据的方法，在对目标函数优化之前先通过零空间对状态向量进行边缘化处理，在不丢失信息的前提下，去掉状态向量中的非关键状态向量，避免所有的状态向量全部进行优化，有助于降低SLAM状态向量估计过程中的计算资源和计算时间开销，从而提升整个SLAM过程的运行效率，避 免状态向量持续增大造成的系统崩溃问题。
第二方面，提供了一种处理数据的装置，用于执行第一方面或第一方面任一种可能的实现方式。具体地，该处理数据的装置可以包括用于执行第一方面或第一方面任一种可能的实现方式中的方法的单元。
第三方面，提供了一种处理数据装置，包括：存储器、处理器和通信接口，所述存储器用于存储计算机程序，所述处理器用于从所述存储器中调用并运行所述计算机程序，使得所述处理数据的装置执行上述第一方面或第一方面任一种可能实现方式中的方法。
具体地，所述处理器用于：
获取智能设备的状态向量集合，所述状态向量集合包括所述智能设备的至少一个位姿向量和/或所述智能设备所处环境的至少一个路标特征向量，所述至少一个位姿向量和/或所述至少一个路标特征向量用于所述智能设备的定位和所述智能设备所处环境的地图构建；
确定第一线性化约束方程，该第一线性化约束方程表示该至少一个位姿向量和/或该至少一个路标特征向量的线性化约束关系；
确定第一状态向量，所述第一状态向量为所述状态向量集合中待边缘化的状态向量；对所述第一线性化约束方程中的所述第一状态向量进行边缘化，得到第二线性化约束方程，该第二线性化约束方程表示第二状态向量的线性化约束关系，所述第二状态向量为所述状态向量集合中除所述第一状态向量以外的状态向量；
根据所述第二线性化约束方程，确定该状态向量的最大似然估计的目标函数；
根据所述目标函数，对所述至少一个位姿向量和/或所述至少一个路标特征向量进行优化，输出所述智能设备的轨迹和所述智能设备所处环境的地图。
第四方面，提供了一种计算机程序产品，所述计算机程序产品包括：计算机程序(也可以称为代码，或指令)，当所述计算机程序在计算机上运行时，使得所述计算机执行上述第一方面或第一方面中任一种可能实现方式中的方法。
第五方面，提供了一种计算机可读存储介质，用于存储计算机程序，该计算机程序包括用于执行第一方面或第一方面任一种可能实现方式的方法的指令。
本申请实施例在对目标函数优化之前先通过零空间对状态向量进行边缘化处理，在不丢失信息的前提下，去掉状态向量中的非关键状态向量，避免所有的状态向量全部进行优化，有助于降低SLAM状态向量估计过程中的计算资源和计算时间开销，从而提升整个SLAM过程的运行效率，避免状态向量持续增大造成的系统崩溃问题。
附图说明
图1是本申请实施例的技术方案的一种应用场景的示意图。
图2是本申请实施例的处理数据的方法的示意性流程图。
图3是本申请实施例的零空间边缘化方法的示意性流程图。
图4是本申请实施例的处理数据的方法的另一示意性流程图。
图5是本申请实施例的零空间边缘化方法的另一示意性流程图。
图6是本申请实施例的处理数据的方法的再一示意性流程图。
图7是本申请实施例的基于零空间的边缘化方法和基于舒尔补的边缘化方法的计算 过程比较。
图8是本申请实施例的处理数据的装置的示意性框图。
图9是本申请实施例的处理数据的装置的另一示意性框图。
具体实施方式
下面将结合附图，对本申请中的技术方案进行描述。
在对本申请实施例的技术方案介绍之前，首先介绍几个关键术语的定义。
零空间：已知A为一个m×n的矩阵。A的零空间(nullspace)，又称核(kernel)，是一组由下列公式定义的n维向量：
Figure PCTCN2018106910-appb-000017
即线性方程组Ax＝0的所有解x的集合。
雅克比矩阵：雅克比矩阵是一阶偏导数以一定的方式排列成的矩阵，设有m个n元函数组成的函数组：y i(x 1,x 2,...x n)(i＝1，2，…m)，称之为函数组。对这个函数组取一阶导数，获得下面的雅克比矩阵：
Figure PCTCN2018106910-appb-000018
本申请实施例适用于同时定位与地图构建(Simultaneous Localization And Mapping，SLAM)领域以及任何用于非线性估计问题中的状态边缘化处理的智能设备(Intelligent Equipment)中。
应理解，智能设备是指任何一种具有计算处理能力的设备、器械或者机器，本申请实施例中的智能设备可以是机器人(Robot)、自动驾驶汽车(Autonomous Vehicles)、无人机(Unmanned Aerial Vehicle)、智能家居(Smart Home)和手机终端等，本申请对该智能设备并不做任何限定。
图1示出了本申请实施例的技术方案的一种应用场景的示意图，如图1所示，本申请实施例的技术方案适用于机器人的SLAM系统100中，该SLAM系统包括前端(Front-end)110和后端(Back-end)120，其中，前端110主要完成传感器的数据采集和数据关联，而后端120主要功能是对前端产生的模型进行参数估计以及优化。下面以机器人为例对本申请实施例的技术方案进行描述。
图2示出了根据本申请实施例的处理数据的方法200的示意性流程图，如图2所示，该方法200可以由图1中的SLAM系统100执行，该方法200包括：
S210，获取智能设备的状态向量集合，该状态向量集合包括该智能设备的至少一个位姿向量和/或该智能设备所处环境的至少一个路标特征向量，该至少一个位姿向量和/或该至少一个路标特征向量用于该智能设备的定位和该智能设备所处环境的地图构建。
具体而言，该机器人的SLAM系统100中前端110获取视觉传感器、惯性传感器IMU 的数据，提取机器人的至少一个位姿向量，并对获取的图像进行和IMU数据进行时间同步，提取视觉图像信息中的该机器人所处环境的至少一个路标特征向量，并进行特征匹配。
应理解，特征匹配就是确定两次获取的图像数据哪些对应的是同一个路标特征。
例如，机器人在运动过程中会产生一条轨迹(x 1,x 2,...,x k)，由于机器人的运动符合一定的运行学模型规律，基于该IMU数据和运动模型，建立机器人的运动方程：
x k＝f k(x k-1,u k)+w k          (1)
其中，u k表示来自于里程计、惯性测量单元(Inertial Measurement Unit，IMU)等运动传感器的观测值，w k是协方差Q k为的高斯白噪声。
应理解，视觉传感器提取的是路标特征向量，但是视觉传感器可以通过上下两帧的数据推算出机器人的位姿向量，惯性传感器提取的是该机器人的位姿向量。
又例如，机器人在运动场景中存在许多路标，而机器人在运动过程的每个位置，都会通过传感器测量到一定的路标点，得到相应的观测值。进而机器人依靠观测模型来检测和跟踪特征，基于上述提取的该路标特征向量、观测模型以及上一帧优化之后的位姿作为先验信息，建立机器人的观测方程：
z kj＝h kj(x k,x fj)+v kj         (2)
其中，Z kj表示机器人在位姿x k时对于特征x fj的观测值，v kj是协方差R k为的高斯白噪声。
应理解，S210获取状态向量集合可以由图1中SLAM系统100的前端110执行。
还应理解，噪声是满足N(0，σ 2)的高斯分布，这里的0表示均值，σ 2表示方差，对应到具体的运动方程中噪声的方差就是Q k，对应到具体的观测方程中噪声的方差就是R k。
S220，确定第一线性化约束方程，该第一线性化约束方程表示该至少一个位姿向量和/或该至少一个路标特征向量的线性化约束关系。
具体而言，在S210获取该机器人的至少一个位姿向量和/或该机器人所处环境的至少一个路标特征向量，所建立的运动方程(1)和观测方程(2)为该至少一个位姿向量和/或该至少一个路标特征向量的非线性化方程，通过该至少一个位姿向量和/或该至少一个路标特征向量的雅克比矩阵，将该非线性方程线性化。
应理解，构建线性约束方程主要是将状态向量的非线性化的约束表示转换成线性化的数学表达以简化求解过程。
还应理解，S220确定该至少一个位姿向量和/或该至少一个路标特征向量的第一线性化约束方程可以由SLAM系统100的后端120执行。
分别计算约束方程中运动方程(1)和观测方程(2)的雅克比矩阵F k和H kj、H fj：
Figure PCTCN2018106910-appb-000019
这里F k为机器人位姿在运动模型中状态k的雅克比矩阵，H kj和H fj分别为机器人位姿向量和路标特征向量在观测方程中状态k的雅克比矩阵。
利用雅克比矩阵线性化约束方程后得到第一线性化约束方程：
线性化的运动方程：
Figure PCTCN2018106910-appb-000020
线性化的观测方程：
Figure PCTCN2018106910-appb-000021
S230，确定第一状态向量，该第一状态向量为该状态向量集合中待边缘化的状态向量。
具体而言，根据当前SLAM应用场景，可以从机器人的位姿向量和路标特征向量两者中选择一个作为具体的要进行边缘化处理状态向量，也可以将两者同时作为待边缘化的状态向量。
应理解，利用确定的待优化的状态向量对线性化的约束方程进行重排，为后续的边缘化操作做好准备。
还应理解，S230确定第一状态向量可以由SLAM系统100的后端120执行。
例如，可以通过滑动窗口的方法，将窗口中距离机器人当前状态点时间比较久的点作为实际要边缘化掉的点，首先设置滑动窗口的大小为D(仅在系统初始化时执行)，将当前窗口内观测到的(k-D)时刻的位姿向量和所有的路标特征向量作为要边缘化的状态向量。
又例如，可以通过视差法，将与机器人当前状态点观测视差较小的点作为实际的要边缘化掉的点。
应理解，S220和S230并没有实际的先后顺序，可以先确定该第一线性化约束方程，后确定待边缘化的状态向量，也可以先确定待边缘化的状态向量，后确定该第一线性化约束方程，本申请并不限于此。
S240，对该第一线性化约束方程中的该第一状态向量进行边缘化，得到第二线性化约束方程，该第二线性化约束方程表示第二状态向量的线性化约束关系，该第二状态向量为该状态向量集合中除该第一状态向量以外的状态向量。
可选地，在对该第一线性化约束方程中的该第一状态向量进行边缘化，得到第二状态向量的第二线性化约束方程之前，该方法还包括：
根据该第一状态向量，对该第一线性化约束方程进行重排列，重排列后的该第一线性化约束方程中的该第一状态向量和该第二状态向量分离。
例如，在S230中确定将当前窗口内观测到的(k-D)时刻的位姿向量和所有的路标特征向量作为待边缘化的状态向量，重排列步骤S220建立的线性化约束方程，将待边缘化的状态向量分离出来，得到排列后的第一线性化约束方程(6)：
Figure PCTCN2018106910-appb-000022
将重排列后的第一线性化约束方程记为：
Figure PCTCN2018106910-appb-000023
其中，
Figure PCTCN2018106910-appb-000024
表示要保留的状态向量的误差向量，H u为要保留的状态向量的雅克比矩阵，
Figure PCTCN2018106910-appb-000025
为待边缘化的状态向量的误差向量，H m为待边缘化的状态向量的雅克比矩阵，
Figure PCTCN2018106910-appb-000026
为被观测向量的残差向量，n为高斯白噪声向量。
应理解，该第一状态向量和该第一状态向量的误差向量存在一个差值，该第二状体向量和该第二状态向量的误差向量也存在一个差值，上述重排列后的第一线性化约束方程中
Figure PCTCN2018106910-appb-000027
和
Figure PCTCN2018106910-appb-000028
分离也可以理解为该第一状态向量和该第二状态向量分离。
还应理解，S240确定该第二线性化约束方程可以由SLAM系统100的后端120执行。
本申请实施例中，为了保持SLAM状态向量有界，需要限制状态向量的数据，以降低计算开销和计算时间，所以在优化之前对状态向量进行边缘化处理，确定状态向量集合中非关键的状态向量，避免所有的状态向量全部进行优化，通过对第一线性化约束方程中待边缘化的状态向量进行边缘化，得到该第二状态向量的第二线性化约束方程，使得第二线性化约束方程中只保留需要的状态向量。
可选地，对该第一线性化约束方程中的该第一状态向量进行边缘化，得到第二状态向量的第二线性化约束方程，包括：
根据该第一状态向量的雅克比矩阵的零空间，对该第一线性化约束方程中的该第一状态向量进行边缘化，得到该第二线性化约束方程。
图3示出了根据本申请实施例的零空间边缘化方法300的示意性流程图，如图3所示，该零空间边缘化方法300可以由图1中SLAM系统的后端120执行，该方法300包括：
S310，确定该第一状态向量的雅克比矩阵。
具体而言，公式(7)中的H m即为待边缘化的状态向量的雅克比矩阵。
S320，根据该第一状态向量的雅克比矩阵，确定该第一状态向量的雅克比矩阵的左零空间。
具体而言，计算待边缘化状态向量的雅克比矩阵的左零空间，根据线性化之后的约束方程(7)，求解H m对应的左零空间U m，求解方程为：
H m TU m＝0           (8)
给出一种解析解的方式对左零空间进行求解，U m的解析解表示为：
Figure PCTCN2018106910-appb-000029
S330，将该第一状体向量的雅克比矩阵的左零空间左乘该第一线性化约束方程，得到该第二线性化约束方程。
具体而言，将计算得到的左零空间U m左乘重排列后的第一线性化约束方程，方程如下所示：
Figure PCTCN2018106910-appb-000030
由于H m TU m＝0，所以最终得到的第二线性化约束方程为：
Figure PCTCN2018106910-appb-000031
本申请实施例的零空间边缘化方法，通过待边缘化状态向量的零空间，去掉状态向量中非关键的状态向量，有助于降低计算时间。
S250，根据该第二线性化约束方程，确定该状态向量的最大似然估计的目标函数。
应理解，最大似然估计(Maximum Likelihood，ML)也称为最大概率估计，是一种具有理论性的点估计法，该方法的基本思想是：当从模型总体随机抽取n组样本观测值后，最合理的参数估计量应该使得从模型中抽取该n组样本观测值的概率最大。
具体而言，根据新构建的第二线性化约束方程，可以构建新的优化目标函数用于SLAM系统100的后端120进行优化，具体的，根据新构建的约简后的第二线性化约束方程(11)，构建优化目标函数，得到：
Figure PCTCN2018106910-appb-000032
其中，
Figure PCTCN2018106910-appb-000033
H' u＝U m ΤH u，R为U m Τn的协方差。
可选地，该处理数据的方法100还包括：
可选地，所述方法还包括：
根据该目标函数，确定信息矩阵，该信息矩阵用于优化并更新该第二状态向量，并基于优化更新后的第二状态向量，构建新的约束项，该约束项用于对下一次处理数据时建立的线性化约束方程进行约束。
具体而言，在得到式(12)后，根据式(12)计算边缘化后的信息矩阵如下：
Figure PCTCN2018106910-appb-000034
具体而言，根据该目标函数，通过图优化方法估计该第二状态向量的最优值，并确定信息矩阵，共同作为下一次数据处理的约束条件。
S260，根据该目标函数，对该至少一个位姿向量和/或该至少一个路标特征向量进行优化，输出该智能设备的轨迹和该智能设备所处环境的地图。
具体而言，基于约简后的目标函数，利用扩展卡尔曼滤波器(Extended Kalman Filter，EKF)方法或者非线性优化方法，对所有的状态向量(机器人的位姿向量和当前机器人所处环境的路标特征向量)进行最优估计和优化，得到该机器人的轨迹和该机器人所处环境的地图。对第二状态向量进行图优化，并对第二状态向量进行更新
Figure PCTCN2018106910-appb-000035
根据得到的信息矩阵构建下一次图优化的先验约束：
Figure PCTCN2018106910-appb-000036
作为下次优化时新的约束加入整个位姿图中，完善整个图优化。
其中，
Figure PCTCN2018106910-appb-000037
为要保留的状态向量的误差向量，
Figure PCTCN2018106910-appb-000038
为要保留的状态向量上一次的估计值，
Figure PCTCN2018106910-appb-000039
为要保留的状态向量更新后的估计值。本申请实施例的处理数据的方法，在对目标函数优化之前先通过零空间对状态向量进行边缘化处理，在不丢失信息的前提下，去掉状态向量中的非关键状态向量，避免所有的状态向量全部进行优化，有助于降低SLAM状态向量估计过程中的计算资源和计算时间开销，从而提升整个SLAM过程的运行效率，避免状态向量持续增大造成的系统崩溃问题。
作为一个具体的实施例，图4示出了根据本申请实施例的处理数据的方法400的示意性流程图，如图4所示，该方法400包括：
S410，获取传感器的测量值。
采集相机、IMU等传感器信息，对相机的图像信息进行预测，对相机、IMU等传感器数据进行同步，获取智能设备当前位姿的观测状态量。
S420，建立状态向量的非线性化约束关系。
通过运动方程(1)和观测方程(2)，建立相邻帧之间传感器数据的约束关系，进而通过约束关系的传导建立所有状态向量之间的非线性化约束关系。
S430，基于零空间的边缘化处理。
对S420建立的所有状态向量之间的非线性化约束关系进行线性化处理，利用左零空间去除状态向量中的非关键的状态向量，并根据要保留的状态向量的约束构建优化目标函 数。
S440，对S430处理得到的目标函数进行优化。
基于约简后的目标函数，利用EKF方法或者非线性优化方法，对所有的状态向量进行最优估计和优化，得到全局一致的轨迹和地图。
应理解，S410和S420可以由SLAM系统100的前端110处理，S430和S440可以由SLAM系统100的后端120处理。
作为另一个具体的实施例，图5示出了根据本申请实施例的基于零空间边缘化处理方法500的示意性流程图，如图5所示，该方法500包括：
S510，构建线性化约束方程。
构建线性约束方程主要是将状态向量的非线性化的约束表示转换成线性化的数学表达以简化求解过程。同时，利用确定的待优化的状态向量对线性化的约束方程进行重排，为后续的边缘化操作做好准备。其主要涉及以下2个步骤：
S511，确定待边缘化的状态向量。
根据当前SLAM过程的应用场景，确定是对智能设备的位姿向量或者该智能设备所处环境的路标特征向量进行边缘化，同时确定对哪些位姿向量或者路标特征向量进行边缘化处理。
S512，线性化约束方程。
计算状态向量的雅克比矩阵，并利用雅克比矩阵构建状态向量的线性化约束方程，根据确定的待边缘化的状态向量，对线性化方程进行重排。
S520，计算左零空间。
计算左零空间主要是完成对待边缘化状态向量的雅克比矩阵的左零空间的计算操作。
S530，约简目标函数。
约简目标函数主要是对线性化方程进行左乘零空间的运算，去除线性化约束方程中非关键的状态向量，得到边缘化之后的线性化约束方程，并根据边缘化之后的线性方程构建新的优化目标函数。其主要涉及以下2个步骤：
S531，零空间左乘线性化约束方程。
将待边缘化状态向量的雅克比矩阵的零空间左乘线性化约束方程，去除线性化方程中非关键的状态向量。
S532，构建约简后的目标函数。
在去除非关键状态向量后，得到新的状态向量的约束表示，依据新的约束表示构建新的优化目标函数。
作为一个具体的实施例，图6示出了根据本申请实施例的处理数据方法600的示意性流程图，如图6所示，该方法600包括：
S601，获取视觉传感器、惯性传感器IMU的数据，并对获取的图像和IMU数据进行时间同步；
S602，提取视觉图像信息中的环境路标特征向量，并进行特征匹配；
S603，建立该至少一个位姿向量和/或至少一个路标特征向量的非线性化约束方程，包括运动方程和观测方程，基于S601的IMU数据和运动模型，建立智能设备的运动方程；基于S602提取的路标特征向量、观测模型和上一帧优化之后的位姿作为先验信息，建立 观测观测方程；
S604，确定待边缘化的状态向量，通过滑动窗口的方法或者视差法，确定位姿向量和路标特征向量中待边缘化的状态向量；
S605，线性化S603建立的非线性化约束方程，分别计算S603中运动方程和观测方程的雅克比矩阵，根据该雅克比矩阵，线性化S603中的非线性化约束方程，得到第一线性化约束方程；
S606，重排列S605建立的线性化约束方程，将待边缘化的状态向量分离出来，根据S604中确定的待边缘化的状态向量，将S605建立的第一线性化约束方程重排列，将该第一线性化约束方程中要保留的状态向量和待边缘化的状态向量分离；
S607，计算待边缘化向量的雅克比矩阵的左零空间，根据S606中重排列后的第一线性化约束方程，求解该待边缘化的状态向量的左零空间；
S608，边缘化状态向量，确定约简后的线性化约束方程，利用S607计算得到的该待边缘化的状态向量的左零空间左乘该S606中重排列后的该第一线性化约束方程，得到约简后的第二线性化约束方程；
S609，基于S608中确定的约简后的第二线性化约束方程，构建目标函数，计算边缘化后的信息矩阵；
S610，对目标函数进行图优化，并根据S609得到的信息矩阵构建下一次优化的先验约束；
S611，如果SLAM过程结束，则输出该智能设备的全部位姿和该智能设备所处环境的地图；否则，循环执行步骤S601到S610。
表1示出了根据本申请实施例的基于舒尔补边缘化方法与基于零空间边缘化方法的计算时间比较，由表1可以看出，相比于舒尔补边缘化方法，基于零空间边缘化方法的运行效率是其4.6倍。
表1
边缘化算法 平均运行时间(s)
基于舒尔补的边缘化算法 0.0440574
基于零空间的边缘化算法 0.00953969
图7示出了根据本申请实施例的零空间方法和舒尔补方法的计算步骤比较，如图7所示，对比分析两者的计算复杂度，假设m个机器人位姿跟踪n个路标特征，黑框表示两种边缘化方法中最耗时的步骤，通常情况下n是远大于m的，由此可见，零空间方法计算复杂度要小于舒尔补方法。
以上结合如2至图7，对本申请实施的数据处理的方法进行了详细描述，下面结合图8至图9，对本申请实施例的处理数据的装置进行详细描述。
图8示出了根据本申请实施例的处理数据的装置700的示意性框图，如图8所示，该装置700包括：
获取单元710，用于获取智能设备的状态向量集合，该状态向量集合包括智能设备的至少一个位姿向量和/或该智能设备所处环境的至少一个路标特征向量，该至少一个位姿向量和/或该至少一个路标特征向量用于该智能设备的定位和该智能设备所处环境的地图构建；
确定单元720，用于确定第一线性化约束方程，该第一线性化约束方程表示该至少一个位姿向量和/或该至少一个路标特征向量的线性化约束关系；
该确定单元720还用于确定第一状态向量，该第一状态向量为该状态向量集合中待边缘化的状态向量；
边缘化单元730，用于对该第一线性化约束方程中的该第一状态向量进行边缘化，得到第二线性化约束方程，该第二线性化约束方程表示第二状态向量的线性化约束关系，该第二状态向量为该状态向量集合中除该第一状态向量以外的状态向量；
该确定单元720还用于根据该第二线性化约束方程，确定该状态向量的最大似然估计的目标函数；
优化单元740，用于根据该目标函数，对该至少一个位姿向量和/或该至少一个路标特征向量进行优化，输出该智能设备的轨迹和该智能设备所处环境的地图。
可选地，该边缘化单元730具体用于：
根据该第一状态向量的雅克比矩阵的零空间，对该第一线性化约束方程中的该第一状态向量进行边缘化，得到该第二线性化约束方程。
可选地，该边缘化单元具体用于：
确定该第一状态向量的雅克比矩阵；
根据该第一状态向量的雅克比矩阵，确定该第一状态向量的雅克比矩阵的左零空间；
将该第一状体向量的雅克比矩阵的左零空间左乘该第一线性化约束方程，得到该第二线性化约束方程。
可选地，该第二线性化约束方程为：
Figure PCTCN2018106910-appb-000040
其中，
Figure PCTCN2018106910-appb-000041
为该第二状态向量的误差向量，H u为该第二状态向量的雅克比矩阵，U m为该第一状态向量的雅克比矩阵的零空间，
Figure PCTCN2018106910-appb-000042
为被观测向量的残差向量，n为高斯白噪声向量。
可选地，该装置700还包括：
重排列单元，用于根据该第一状态向量，对该第一线性化约束方程进行重排列，重排列后的该第一线性化约束方程中的该第一状态向量和该第二状态向量分离。
可选地，该重排列后的该第一线性化约束方程为：
Figure PCTCN2018106910-appb-000043
其中，
Figure PCTCN2018106910-appb-000044
为第二状态向量的误差向量，
Figure PCTCN2018106910-appb-000045
为该第一状态向量的误差向量，H m为该第一状态向量的雅克比矩阵，H u为该第二状态向量的雅克比矩阵，z～为被观测向量的残差向量，n为高斯白噪声向量。
可选地，该确定单元720还用于：
根据该目标函数，确定信息矩阵，该信息矩阵为下一次处理数据的约束条件。
可选地，该确定单元还用于：
确定该至少一个位姿向量和/或至少一个路标特征向量的非线性化约束方程；
根据该至少一个位姿向量和/或至少一个路标特征向量的雅克比矩阵，对该非线性化约束方程进行线性化，得到该第一线性化约束方程。
可选地，该确定单元720还用于：
根据该目标函数，确定信息矩阵，该信息矩阵用于优化并更新该第二状态向量，并基于优化更新后的第二状态向量，构建新的约束项，该约束项用于对下一次处理数据时建立的线性化约束方程进行约束。
应理解，处理数据的装置700可以对应于根据本申请实施例的处理数据的方法200中的SLAM系统，该处理数据的装置700可以包括用于执行图2中处理数据的方法200的SLAM系统执行的方法的模块。并且，该处理数据的装置700中的各模块和上述其他操作和/或功能分别为了实现图2中处理数据的方法200的相应流程，具体地，获取单元710用于执行方法200中的步骤210，确定单元720用于执行方法200中的步骤220、步骤230和步骤250，边缘化单元730用于执行方法200中的步骤240，优化单元740用于执行方法200中的步骤260，各单元执行上述相应步骤的具体过程在方法200中已经详细说明，为了简洁，在此不再赘述。
图9是本申请实施例提供的处理数据的装置800的结构性示意图。如图9所示，该处理数据的装置800包括：处理器810、存储器820和通信接口830。其中，存储器820中存储有指令，处理器810用于执行存储器820中的指令，当该指令被执行时，该处理器510用于执行上述方法实施例提供的方法，处理器810还用于控制通信接口830与外界进行通信。
具体地，处理数据的装置800可以对应于根据本申请实施例的SLAM系统100，该处理数据的装置800可以包括用于执行图2中处理数据的方法200的SLAM系统100执行的方法的模块。并且，该处理数据的装置800中的各模块和上述其他操作和/或功能分别为了实现图2中处理数据的方法200的相应流程。各模块执行上述相应步骤的具体过程在方法200中已经详细说明，为了简洁，在此不再赘述。
本申请实施例还提供一种计算机可读存储介质，该计算机可读存储介质包括计算机程序，当其在计算机上运行时，使得该计算机执行上述方法实施例提供的方法。
本申请实施例还提供一种包含指令的计算机程序产品，当该计算机程序产品在计算机上运行时，使得该计算机执行上述方法实施例提供的方法。
应理解，本申请实施例中，该处理器可以为中央处理单元(central processing unit，CPU)，该处理器还可以是其他通用处理器、数字信号处理器(digital signal processor，DSP)、专用集成电路(application specific integrated circuit，ASIC)、现成可编程门阵列(field programmable gate array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
还应理解，本申请实施例中的存储器可以是易失性存储器或非易失性存储器，或可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是只读存储器(read-only memory，ROM)、可编程只读存储器(programmable ROM，PROM)、可擦除可编程只读存储器(erasable PROM，EPROM)、电可擦除可编程只读存储器(electrically EPROM，EEPROM)或闪存。易失性存储器可以是随机存取存储器(random access memory，RAM)，其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的随机存取存储器(random access memory，RAM)可用，例如静态随机存取存储器(static RAM，SRAM)、动态随机存取存储器(DRAM)、同步动态随机存取存储器(synchronous DRAM，SDRAM)、 双倍数据速率同步动态随机存取存储器(double data rate SDRAM，DDR SDRAM)、增强型同步动态随机存取存储器(enhanced SDRAM，ESDRAM)、同步连接动态随机存取存储器(synchlink DRAM，SLDRAM)和直接内存总线随机存取存储器(direct rambus RAM，DR RAM)。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在本申请所提供的几个实施例中，应该理解到，所揭露的系统、装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。
所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read-Only Memory，ROM)、随机存取存储器(Random Access Memory，RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应以所述权利要求的保护范围为准。

Claims (15)

  1. 一种处理数据的方法，其特征在于，包括：
    获取智能设备的状态向量集合，所述状态向量集合包括所述智能设备的至少一个位姿向量和/或所述智能设备所处环境的至少一个路标特征向量，所述至少一个位姿向量和/或所述至少一个路标特征向量用于所述智能设备的定位和所述智能设备所处环境的地图构建；
    确定第一线性化约束方程，所述第一线性化约束方程表示所述至少一个位姿向量和/或所述至少一个路标特征向量的线性化约束关系；
    确定第一状态向量，所述第一状态向量为所述状态向量集合中待边缘化的状态向量；
    对所述第一线性化约束方程中的所述第一状态向量进行边缘化，得到第二线性化约束方程，所述第二线性化约束方程表示第二状态向量的线性化约束关系，所述第二状态向量为所述状态向量集合中除所述第一状态向量以外的状态向量；
    根据所述第二线性化约束方程，确定所述状态向量的最大似然估计的目标函数；
    根据所述目标函数，对所述至少一个位姿向量和/或所述至少一个路标特征向量进行优化，输出所述智能设备的轨迹和所述智能设备所处环境的地图。
  2. 根据权利要求1所述的方法，其特征在于，所述对所述第一线性化约束方程中的所述第一状态向量进行边缘化，得到第二线性化约束方程，包括：
    根据所述第一状态向量的雅克比矩阵的零空间，对所述第一线性化约束方程中的所述第一状态向量进行边缘化，得到所述第二线性化约束方程。
  3. 根据权利要求2所述的方法，其特征在于，所述根据所述第一状态向量的雅克比矩阵的零空间，对所述第一线性化约束方程中的所述第一状态向量进行边缘化，得到所述第二线性化约束方程，包括：
    确定所述第一状态向量的雅克比矩阵；
    根据所述第一状态向量的雅克比矩阵，确定所述第一状态向量的雅克比矩阵的左零空间；
    将所述第一状态向量的雅克比矩阵的左零空间左乘所述第一线性化约束方程，得到所述第二线性化约束方程。
  4. 根据权利要求2或3所述的方法，其特征在于，所述第二线性化约束方程为：
    Figure PCTCN2018106910-appb-100001
    其中，
    Figure PCTCN2018106910-appb-100002
    为所述第二状态向量的误差向量，H u为所述第二状态向量的雅克比矩阵，U m为所述第一状态向量的雅克比矩阵的零空间，
    Figure PCTCN2018106910-appb-100003
    为被观测向量的残差向量，n为高斯白噪声向量。
  5. 根据权利要求1至4中任一项所述的方法，其特征在于，所述对所述第一线性化约束方程中的所述第一状态向量进行边缘化，得到第二线性化约束方程之前，所述方法还包括：
    根据所述第一状态向量，对所述第一线性化约束方程进行重排列，重排列后的所述第一线性化约束方程中的所述第一状态向量和所述第二状态向量分离。
  6. 根据权利要求5所述的方法，其特征在于，所述重排列后的所述第一线性化约束方程为：
    Figure PCTCN2018106910-appb-100004
    其中，
    Figure PCTCN2018106910-appb-100005
    为第二状态向量的误差向量，
    Figure PCTCN2018106910-appb-100006
    为所述第一状态向量的误差向量，H m为所述第一状态向量的雅克比矩阵，H u为所述第二状态向量的雅克比矩阵，
    Figure PCTCN2018106910-appb-100007
    为被观测向量的残差向量，n为高斯白噪声向量。
  7. 根据权利要求1至6中任一项所述的方法，其特征在于，所述确定所述至少一个位姿向量和/或至少一个路标特征向量的第一线性化约束方程之前，所述方法还包括：
    确定所述至少一个位姿向量和/或至少一个路标特征向量的非线性化约束方程；
    根据所述至少一个位姿向量和/或至少一个路标特征向量的雅克比矩阵，对所述非线性化约束方程进行线性化，得到所述第一线性化约束方程。
  8. 一种处理数据的装置，其特征在于，包括：
    获取单元，用于获取智能设备的状态向量集合，所述状态向量集合包括所述智能设备的至少一个位姿向量和/或所述智能设备所处环境的至少一个路标特征向量，所述至少一个位姿向量和/或所述至少一个路标特征向量用于所述智能设备的定位和所述智能设备所处环境的地图构建；
    确定单元，用于确定第一线性化约束方程，所述第一线性化约束方程表示所述至少一个位姿向量和/或所述至少一个路标特征向量的线性化约束关系；
    所述确定单元还用于确定第一状态向量，所述第一状态向量为所述状态向量集合中待边缘化的状态向量；
    边缘化单元，用于对所述第一线性化约束方程中的所述第一状态向量进行边缘化，得到第二线性化约束方程，所述第二线性化约束方程表示第二状态向量的线性化约束关系，所述第二状态向量为所述状态向量集合中除所述第一状态向量以外的状态向量；
    所述确定单元还用于根据所述第二线性化约束方程，确定所述状态向量的最大似然估计的目标函数；
    优化单元，用于根据所述目标函数，对所述至少一个位姿向量和/或所述至少一个路标特征向量进行优化，输出所述智能设备的轨迹和所述智能设备所处环境的地图。
  9. 根据权利要求8所述的装置，其特征在于，所述边缘化单元具体用于：根据所述第一状态向量的雅克比矩阵的零空间，对所述第一线性化约束方程中的所述第一状态向量进行边缘化，得到所述第二线性化约束方程。
  10. 根据权利要求9所述的装置，其特征在于，所述边缘化单元具体用于：
    确定所述第一状态向量的雅克比矩阵；
    根据所述第一状态向量的雅克比矩阵，确定所述第一状态向量的雅克比矩阵的左零空间；
    将所述第一状体向量的雅克比矩阵的左零空间左乘所述第一线性化约束方程，得到所述第二线性化约束方程。
  11. 根据权利要求9或10所述的装置，其特征在于，所述第二线性化约束方程为：
    Figure PCTCN2018106910-appb-100008
    其中，
    Figure PCTCN2018106910-appb-100009
    为所述第二状态向量的误差向量，H u为所述第二状态向量的雅克比矩阵，U m 为所述第一状态向量的雅克比矩阵的零空间，
    Figure PCTCN2018106910-appb-100010
    为被观测向量的残差向量，n为高斯白噪声向量。
  12. 根据权利要求8至11中任一项所述的装置，其特征在于，所述装置还包括：
    重排列单元，用于根据所述第一状态向量，对所述第一线性化约束方程进行重排列，重排列后的所述第一线性化约束方程中的所述第一状态向量和所述第二状态向量分离。
  13. 根据权利要求12所述的装置，其特征在于，所述重排列后的所述第一线性化约束方程为：
    Figure PCTCN2018106910-appb-100011
    其中，
    Figure PCTCN2018106910-appb-100012
    为第二状态向量的误差向量，
    Figure PCTCN2018106910-appb-100013
    为所述第一状态向量的误差向量，H m为所述第一状态向量的雅克比矩阵，H u为所述第二状态向量的雅克比矩阵，
    Figure PCTCN2018106910-appb-100014
    为被观测向量的残差向量，n为高斯白噪声向量。
  14. 根据权利要求8至13中任一项所述的装置，其特征在于，所述确定单元还用于：
    确定所述至少一个位姿向量和/或至少一个路标特征向量的非线性化约束方程；
    根据所述至少一个位姿向量和/或至少一个路标特征向量的雅克比矩阵，对所述非线性化约束方程进行线性化，得到所述第一线性化约束方程。
  15. 一种计算机可读存储介质，其特征在于，包括计算机程序，当所述计算机程序在计算机上运行时，使得所述计算机执行权利要求1至7中任一项所述的处理数据方法。
PCT/CN2018/106910 2017-09-22 2018-09-21 一种处理数据的方法和装置 Ceased WO2019057146A1 (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
EP18857990.8A EP3677978B1 (en) 2017-09-22 2018-09-21 Data processing method and device
US16/825,694 US20200219000A1 (en) 2017-09-22 2020-03-20 Data Processing Method And Apparatus

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN201710868757.6 2017-09-22
CN201710868757.6A CN109542093B (zh) 2017-09-22 2017-09-22 一种处理数据的方法和装置

Related Child Applications (1)

Application Number Title Priority Date Filing Date
US16/825,694 Continuation US20200219000A1 (en) 2017-09-22 2020-03-20 Data Processing Method And Apparatus

Publications (1)

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

Family

ID=65811099

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2018/106910 Ceased WO2019057146A1 (zh) 2017-09-22 2018-09-21 一种处理数据的方法和装置

Country Status (4)

Country Link
US (1) US20200219000A1 (zh)
EP (1) EP3677978B1 (zh)
CN (1) CN109542093B (zh)
WO (1) WO2019057146A1 (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108876854B (zh) * 2018-04-27 2022-03-08 腾讯科技（深圳）有限公司 相机姿态追踪过程的重定位方法、装置、设备及存储介质
CN111238497B (zh) 2018-11-29 2022-05-06 华为技术有限公司 一种高精度地图的构建方法及装置
CN111256689B (zh) * 2020-01-15 2022-01-21 北京智华机器人科技有限公司 一种机器人定位方法、机器人和存储介质
US20250027773A1 (en) * 2023-07-18 2025-01-23 Qualcomm Incorporated Inertial navigation aided with multi-interval pose measurements

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20060041331A1 (en) * 2004-08-05 2006-02-23 Samsung Electronics Co., Ltd. Method used by robot for simultaneous localization and map-building
CN102402225A (zh) * 2011-11-23 2012-04-04 中国科学院自动化研究所 一种实现移动机器人同时定位与地图构建的方法
CN102706342A (zh) * 2012-05-31 2012-10-03 重庆邮电大学 一种智能移动机器人的定位与环境建模方法
CN103631264A (zh) * 2013-12-04 2014-03-12 苏州大学张家港工业技术研究院 一种同时定位与地图创建方法及装置
CN103644903A (zh) * 2013-09-17 2014-03-19 北京工业大学 基于分布式边缘无味粒子滤波的同步定位与地图构建方法
CN106197428A (zh) * 2016-07-10 2016-12-07 北京工业大学 一种利用测量信息优化分布式ekf估计过程的slam方法

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9243916B2 (en) * 2013-02-21 2016-01-26 Regents Of The University Of Minnesota Observability-constrained vision-aided inertial navigation
US9607401B2 (en) * 2013-05-08 2017-03-28 Regents Of The University Of Minnesota Constrained key frame localization and mapping for vision-aided inertial navigation
US20140341465A1 (en) * 2013-05-16 2014-11-20 The Regents Of The University Of California Real-time pose estimation system using inertial and feature measurements
US10306206B2 (en) * 2013-07-23 2019-05-28 The Regents Of The University Of California 3-D motion estimation and online temporal calibration for camera-IMU systems
US9658070B2 (en) * 2014-07-11 2017-05-23 Regents Of The University Of Minnesota Inverse sliding-window filters for vision-aided inertial navigation systems
CN104574387B (zh) * 2014-12-29 2017-11-21 张家港江苏科技大学产业技术研究院 水下视觉slam系统中的图像处理方法
CN106289181B (zh) * 2015-05-22 2018-12-18 北京雷动云合智能技术有限公司 一种基于视觉测量的实时slam方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20060041331A1 (en) * 2004-08-05 2006-02-23 Samsung Electronics Co., Ltd. Method used by robot for simultaneous localization and map-building
CN102402225A (zh) * 2011-11-23 2012-04-04 中国科学院自动化研究所 一种实现移动机器人同时定位与地图构建的方法
CN102706342A (zh) * 2012-05-31 2012-10-03 重庆邮电大学 一种智能移动机器人的定位与环境建模方法
CN103644903A (zh) * 2013-09-17 2014-03-19 北京工业大学 基于分布式边缘无味粒子滤波的同步定位与地图构建方法
CN103631264A (zh) * 2013-12-04 2014-03-12 苏州大学张家港工业技术研究院 一种同时定位与地图创建方法及装置
CN106197428A (zh) * 2016-07-10 2016-12-07 北京工业大学 一种利用测量信息优化分布式ekf估计过程的slam方法

Non-Patent Citations (1)

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

Also Published As

Publication number Publication date
CN109542093A (zh) 2019-03-29
US20200219000A1 (en) 2020-07-09
EP3677978B1 (en) 2022-04-06
EP3677978A1 (en) 2020-07-08
CN109542093B (zh) 2022-06-07
EP3677978A4 (en) 2020-08-05

Similar Documents

Publication Publication Date Title
US9709404B2 (en) Iterative Kalman Smoother for robust 3D localization for vision-aided inertial navigation
CN108537837B (zh) 一种深度信息确定的方法及相关装置
Wu et al. A Square Root Inverse Filter for Efficient Vision-aided Inertial Navigation on Mobile Devices.
CN110118556A (zh) 一种基于协方差交叉融合slam的机器人定位方法及装置
Carlone et al. Eliminating conditionally independent sets in factor graphs: A unifying perspective based on smart factors
WO2019057146A1 (zh) 一种处理数据的方法和装置
CN109903330B (zh) 一种处理数据的方法和装置
CN105981041A (zh) 使用粗到细级联神经网络的面部关键点定位
CN112528925B (zh) 行人跟踪、图像匹配方法及相关设备
CN111983636A (zh) 位姿融合方法、系统、终端、介质以及移动机器人
WO2018214086A1 (zh) 场景的三维重建方法、装置及终端设备
CN116665276B (zh) 一种基于局部-全局融合注意力的人脸对齐方法及系统
CN109685830B (zh) 目标跟踪方法、装置和设备及计算机存储介质
CN110751722B (zh) 同时定位建图方法及装置
CN114998630A (zh) 一种从粗到精的地对空图像配准方法
CN115700507B (zh) 地图更新方法及其装置
CN117237457B (zh) 一种面向非重叠多目视觉里程计的鲁棒初始化方法及系统
CN117451033B (zh) 一种同步定位与地图构建方法、装置、终端及介质
US10115208B2 (en) Image characteristic estimation method and device
US11361548B2 (en) Method and system for multi instance visual tracking based on observer motion modelling
Cheng et al. Robust linear pose graph-based SLAM
CN115908481A (zh) 目标跟踪方法、装置、电子设备和计算机可读存储介质
CN115047486A (zh) 基于最大似然估计平滑的激光里程计方法、系统、装置
CN114913201A (zh) 多目标跟踪方法、装置、电子设备、存储介质和产品
CN114415698A (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: 18857990

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: 2018857990

Country of ref document: EP

Effective date: 20200330