Positioning method and related apparatus

Abstract

A positioning method and related device are applied to the field of automatic driving. The positioning data of an IMU is taken as a basis, and the positioning data of a sensor is combined for fusion positioning. The positioning data of the first sensor can optimize the positioning data of the IMU, and the optimization is performed in a time window as an optimization unit, the current positioning data and the historical positioning data of the IMU in the time window are optimized, in other words, the state quantity in the time window is optimized as a whole, a closed loop is formed, the accuracy of the current positioning data can be improved, the accuracy of the historical positioning data in the time window can also be improved, and the change of the state quantity in time is smoother, which is beneficial to terminal driving. Further, the time window can be pushed forward (i.e. sliding window) as time changes. Therefore, the correlation of the state quantity optimization can be improved when the multi-sensor data fusion is performed, and the real-time performance of the pose optimization is ensured.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of autonomous driving, and in particular, to a positioning method and related apparatus. BACKGROUND

[0002] In recent years, assisted driving technology is developing faster and faster, and more and more vehicles are equipped with autonomous driving systems and assisted driving systems. The assisted driving technology includes environment perception, positioning and navigation, path planning and decision control, among which, the positioning technology is a key link to realize the autonomous driving path planning and decision technology.

[0003] Firstly, the global navigation satellite system (GNSS) can be used for vehicle position positioning. However, the GNSS positioning is easy to fail in multi-layer roads or signal shielding scenes (such as high-rise buildings, tunnels, viaducts and underground garages), and may also produce drift, and the output positioning result has deviation.

[0004] The positioning data currently used by vehicle positioning usually comes from multiple sensors, that is, the vehicle positioning is multi-sensor fusion positioning. The multi-sensor fusion positioning mainly uses the data collected by inertial measurement units, radar-based detection devices (radar, lidar, etc.), global navigation satellite systems (global navigation satellite system, GNSS) and other sensors for positioning, and its advantage lies in strong anti-interference ability and can provide high-precision vehicle positioning information in a short time.

[0005] However, each sensor has its own output rhythm when outputting positioning data, and the time stamps of the obtained data are usually different, in other words, the timing of the multi-sensor output positioning data cannot be completely aligned, which makes it difficult to fuse the data of multiple sensors. How to fuse the positioning data of different sensors is a hot issue being studied by those skilled in the art. SUMMARY

[0006] The embodiments of the present application provide a positioning method and related apparatus, which can realize fusion of multi-sensor data and improve positioning accuracy.

[0007] In a first aspect, the embodiments of the present application provide a vehicle positioning method, comprising:

[0008] obtaining a first state quantity of a terminal and a historical state quantity before the first state quantity in time according to first measurement data from an inertial measurement unit (IMU);

[0009] obtaining a second state quantity of the terminal according to second measurement data from a first sensor;

[0010] According to the second state quantity, the first state quantity and the historical state quantity are optimized to obtain an optimized first state quantity and an optimized state quantity, and the optimized first state quantity and the optimized state quantity are used to determine the position of the terminal.

[0011] The timestamp of the first state quantity, the timestamp of the second state quantity and the timestamp of the historical state quantity are located in the same time window.

[0012] Optionally, the method can be applied to an electronic device, and the following will be described taking the electronic device as an example.

[0013] In the embodiments of the present application, the electronic device can fuse the positioning data of the IMU (such as the first state quantity and the historical state quantity) with the positioning data of the sensor (such as the first sensor) to perform positioning. The positioning data of the first sensor can optimize the positioning data of the IMU, and the optimization is performed in a time window, that is, the current positioning data and the historical positioning data of the IMU in the time window are optimized, in other words, the state quantities in the time window are optimized as a whole to form a closed loop. Therefore, by implementing the present solution, not only the accuracy of the current positioning data can be improved, but also the accuracy of the historical positioning data in the time window can be improved, and the change of the state quantity over time is more smooth, which is beneficial to the terminal driving.

[0014] Further, the time window can be pushed forward (i.e. sliding window) as time changes. Therefore, the present solution can improve the relevance of state quantity optimization when fusing multi-sensor data, and ensure the real-time performance of pose optimization.

[0015] The state quantity is a physical quantity describing the state of the terminal. The state of the terminal changes over time, and the state quantity of the terminal also changes over time. The terminal here can include intelligent terminals or mobile transportation tools such as vehicles, logistics robots, and drones. For example, the terminal can be a vehicle equipped with an intelligent driving system (such as a self-driving system or an assisted driving system).

[0016] In a possible implementation of the first aspect, the state quantity, such as the first state quantity, the second state quantity, the historical state quantity, etc., can include displacement. Further, the state quantity can also include one or more of angle, speed, acceleration, angular velocity, angular velocity deviation, speed deviation, acceleration, momentum, kinetic energy, angular momentum, etc.

[0017] For example, the first state quantity can be in the form of wherein represents the first state quantity, i indicates the number (such as the timestamp of the key frame), P represents displacement, V represents speed, R represents angle, is the deviation of angular velocity, is a bias of acceleration.

[0018] Optionally, T is used to indicate time, for example, a time stamp. Or T is used to indicate a map, for example, a grid map. Or, T is used to indicate a path.

[0019] In a possible implementation of the first aspect, the state quantity has a corresponding time stamp, for example, the first state quantity can be the displacement of the terminal at the first time, and the corresponding time stamp indicates the first time. Optionally, the time stamp corresponding to the state quantity can be stored in the key frame, for example, the positioning data of the IMU can contain multiple key frames, I1, I2, I3, and each key frame has a state quantity and a time stamp.

[0020] As a possible example, the time corresponding to the first state quantity can be the time when the measurement data of the state quantity is generated. Wherein, the generation time can be stored or transmitted in the form of a time stamp, and in the specific implementation process, the meaning represented by the time stamp can not necessarily be the generation time, for example, the generation time can also be replaced by the time of collecting data, or the time of outputting data, and the specific implementation is for reference.

[0021] Optionally, the first state quantity contains a first bias interpolation of displacement, the historical state quantity contains a second bias interpolation of displacement, and the second state quantity contains a third bias interpolation of displacement.

[0022] In a possible implementation of the first aspect, the historical state quantity contains a third state quantity, the third state quantity is a state quantity adjacent to the first state quantity in time, and the time stamp of the second state quantity is between the time stamp of the third state quantity and the time stamp of the first state quantity.

[0023] The optimization of the first state quantity and the historical state quantity according to the second state quantity comprises:

[0024] According to the time stamp of the second state quantity, the third state quantity and the first state quantity are interpolated to obtain a first intermediate state quantity, and the time stamp of the first intermediate state quantity is the same as the time stamp of the second state quantity.

[0025] According to the first intermediate state quantity and the second state quantity, the first state quantity and the historical state quantity are optimized.

[0026] In the above embodiments, the electronic device determines the timestamp (e.g., the first timestamp) of the positioning data (the second state quantity) of the first sensor. The positioning data of the IMU near the first timestamp is interpolated using the positioning data of the first sensor, to obtain the positioning data of the IMU at the first timestamp. The electronic device optimizes the positioning data of the IMU at the first timestamp with the positioning data of the first sensor, to obtain the error of the positioning data of the IMU at the first timestamp. The error is solved, so as to uniformly optimize the positioning data of the IMU in the time window, form a closed loop, and make the pose curve more continuous and smooth.

[0027] For example, the positioning data of the IMU can include , , , , , , , and . Among them, and obtain the positioning data from the wheel speed meter, for example, represented as . Among them, The timestamp is located between and , for example, represented as t e6 . The electronic device interpolates the positioning data of the IMU to determine the positioning data of the IMU at , for example, represented as , in other words, the state quantity and have the same timestamp. The electronic device obtains the error between the positioning data of the IMU and the positioning data of the wheel speed meter at according to and , and optimizes the positioning data of the IMU at according to the error .

[0028] Further, by solving the error of other state quantities of the IMU in the time window where , the other state quantities are optimized. , , , , , , and belong to the same time window, and the above state quantities are optimized according to the error , of course, Also located in the time window. Exemplarily, the optimized state quantity is denoted as , , , , , , and . Of course, the values of the above-mentioned multiple state quantities can be different.

[0029] Optionally, the width of the time window can be predefined, preconfigured or determined by calculation. For example, the width of the time window can be 10 seconds (s), or the width of the time window can be 100 key frames.

[0030] In a possible implementation of the first aspect, the optimization of the first state quantity and the historical state quantity according to the first intermediate state quantity and the second state quantity comprises:

[0031] inputting the first intermediate state quantity and the second state quantity into a target function;

[0032] solving the target function to obtain an error of the first state quantity;

[0033] obtaining the optimized first state quantity according to the first state quantity and the error of the first state quantity;

[0034] solving the target function to obtain an error of the historical state quantity;

[0035] obtaining the optimized historical state quantity according to the historical state quantity and the error of the historical state quantity.

[0036] Exemplarily, a target function is as follows:

[0037]

[0038] wherein, is a state quantity calculated according to the measurement data of the IMU, or the positioning data of the IMU. is a state quantity calculated according to the measurement data of the wheel speed meter, or the positioning data of the wheel speed meter. is a state quantity calculated according to the measurement data of the GNSS, or the positioning data of the GNSS. is a state quantity calculated according to the measurement data of the LiDAR, or the positioning data of the LiDAR.

[0039] If the first sensor is the wheel speed meter, the first term is calculated when the positioning data of the wheel speed meter is input, i.e. ​Similarly, if the first sensor is GNSS, the intermediate term is calculated when the positioning data of GNSS is input, i.e. Similarly, if the first sensor is LiDAR, the intermediate term is calculated when the positioning data of LiDAR is input, i.e. In some scenarios, the positioning data of the wheel speed meter, GNSS, and LiDAR may not be input at the same time. If the input time of the positioning data of the three is different, the positioning data is fused with the IMU positioning data according to the order of input.

[0040] Further, the electronic device solves the target function so that the value of the target function is minimized. In the solving process, the IMU state quantity is updated, so as to optimize the IMU positioning data. The solving method can include Newton, Gauss-Newton, or LM method.

[0041] For example, the first state quantity is in the form of The optimization process includes the following:

[0042]

[0043] wherein Exp(δφ) is the error of the angle, is the error of the displacement, is the error of the velocity, is the error of the angular velocity bias, is the error of the acceleration bias.

[0044] In a possible implementation of the first aspect, the first sensor includes one or more of a detection device, a wheel speed meter, a navigation positioning system, or the like. The detection device refers to a device with ranging and / or image detection capability, such as a vision-based sensor, a radar-based sensor, or the like. By fusing the positioning data of multiple sensors, the positioning accuracy can be improved. The navigation positioning system is, for example, GNSS.

[0045] For example, the detection device can be a laser radar, such as a mechanical laser radar or a solid-state laser radar. For another example, the detection device is a radar, which can be a millimeter wave radar or a centimeter wave radar. For another example, the detection device can be a camera, such as a depth camera.

[0046] In a possible implementation of the first aspect, the method further includes:

[0047] According to the third measurement data from the IMU, a fourth state quantity is obtained, and the timestamp of the fourth state quantity is after the timestamp of the first state quantity.

[0048] According to fourth measurement data from the second sensor, a fifth state quantity of the terminal is obtained, a timestamp of the fifth state quantity is located in a same time window as a timestamp of the fourth state quantity and a timestamp of the first state quantity;

[0049] According to the fifth state quantity, state quantities in a time window in which the fifth state quantity is located are optimized.

[0050] Further, according to the fifth state quantity, state quantities in a time window in which the fifth state quantity is located are optimized, including:

[0051] According to the fifth state quantity, the first state quantity after optimization is optimized to obtain a second-optimized first state quantity.

[0052] According to the fifth state quantity, the fourth state quantity is optimized to obtain the fourth state quantity after optimization.

[0053] In the above embodiment, the second sensor is a sensor other than the first sensor. In other words, the positioning data of the IMU can be fused with multiple sensors to improve positioning accuracy.

[0054] In a possible implementation of the first aspect, the first sensor is a detection device, and the second state quantity of the terminal is obtained according to second measurement data from the first sensor, including:

[0055] The second measurement data from the detection device is obtained, the second measurement data includes a plurality of point cloud frames, the plurality of point cloud frames include a first point cloud frame and a historical point cloud frame before the first point cloud frame, each point cloud frame in the plurality of point cloud frames includes a plurality of sampling points, each sampling point in the plurality of sampling points corresponds to a coordinate position and index information, and the index information includes height and / or intensity.

[0056] The first point cloud frame is projected onto a first plane to obtain first image data, the first image data indicating a distribution of index information of the sampling points in the first point cloud frame;

[0057] The historical point cloud frame is projected onto the first plane to obtain second image data, the second image data indicating a distribution of index information of the sampling points in the historical point cloud frame; and the first image data and the second image data are matched to obtain the second state quantity.

[0058] In the above embodiments, the positioning device projects the point cloud frame from the detection device in two dimensions to form a distribution image for certain index information, and performs image matching on the distribution image of the current point cloud frame and the distribution image of the historical point cloud frame, so as to obtain the state quantity of the terminal. Compared with the method of obtaining positioning data by using the point cloud frame without two-dimensionalization, the embodiments of the present application can significantly reduce the calculation amount and improve the positioning efficiency. Moreover, the matching is performed by using the two-dimensional image, so that the time for obtaining the state quantity can be shortened, so that the first state quantity can be fused with other positioning data as soon as possible, and the positioning accuracy is improved.

[0059] The index information can include height, and the first image data and the second image data can be height maps. Similarly, when the index information includes intensity, the first image data and the second image data can be intensity maps.

[0060] In a possible implementation of the first aspect, the coordinate position of the sampling point includes a horizontal coordinate value and a vertical coordinate value, and the first plane is a horizontal-vertical plane that is rasterized.

[0061] In a possible implementation of the first aspect, the first plane includes a plurality of grids, and the projecting the first point cloud frame onto the first plane to obtain the first image data includes:

[0062] determining, according to the coordinate position of the sampling point in the first point cloud frame, a grid position of the sampling point in the first point cloud frame in the plurality of grids;

[0063] determining, according to the index information of the sampling point in each grid in the plurality of grids, an index value of each grid in the plurality of grids;

[0064] determining the first image data, the first image data including the index values corresponding to the plurality of grids respectively.

[0065] In the above embodiments, when projected onto the plane, the projection can be performed in the form of grids, so that the index information of the sampling points in one grid is taken as a group, and the calculation amount during image matching is further reduced, and the positioning efficiency and accuracy are improved.

[0066] In a possible implementation of the first aspect, the first plane includes a plurality of grids, and the projecting the historical point cloud frame onto the first plane to obtain the second image data includes:

[0067] determining, according to the coordinate position of the sampling point in the historical point cloud frame, a grid position of the sampling point in the historical point cloud frame in the plurality of grids;

[0068] determining, according to the index information of the sampling point in each grid in the plurality of grids, an index value of each grid in the plurality of grids;

[0069] determining the second image data, the second image data comprising a plurality of grids respectively corresponding to an index value.

[0070] In a possible implementation of the first aspect, the method further comprises:

[0071] determining a grid size in the first plane according to one or more of a computing capability of the computing device, a size of a map in which the terminal is located, a number of point clouds of the first point cloud frame, a number of point clouds of the historical point cloud frame, a point cloud density of the first point cloud frame, and a point cloud density of the historical point cloud frame, the grid size comprising a length and / or a width of each grid in the plurality of grids.

[0072] In the above implementation, the size of the grid can be determined according to one or more of the computing capability, the size of the map, the number of point clouds, or the point cloud density. Thus, the size of the grid can be flexibly determined to balance the computing capability and the accuracy.

[0073] In the second aspect, an embodiment of the present application provides a positioning device, the positioning device comprising a pose determination module and a pose optimization module, and the positioning device is configured to implement the method of any one of the first aspect.

[0074] In a possible implementation of the second aspect, the pose determination module is configured to:

[0075] obtain a first state quantity of the terminal and a historical state quantity before a timestamp of the first state quantity according to first measurement data from an inertial measurement unit (IMU);

[0076] obtain a second state quantity of the terminal according to second measurement data from a first sensor;

[0077] the pose optimization module is configured to optimize the first state quantity and the historical state quantity according to the second state quantity, to obtain an optimized first state quantity and an optimized historical state quantity, and the optimized first state quantity and the optimized historical state quantity are used to determine the position of the terminal.

[0078] the timestamp of the first state quantity, the timestamp of the second state quantity, and the timestamp of the historical state quantity are located in a same time window.

[0079] In a possible implementation of the second aspect, the state quantity, for example, the first state quantity, the second state quantity, the historical state quantity, etc., can comprise a displacement. Further, the state quantity can further comprise one or more of an angle, a velocity, an acceleration, an angular velocity, an angular velocity deviation, a velocity deviation, an acceleration, a momentum, a kinetic energy, an angular momentum, etc.

[0080] For example, the first state quantity can be in the form of wherein represents the first state quantity, i indicates a number (e.g. a timestamp of a key frame), P represents a displacement, V represents a velocity, R represents an angle, is a bias of an angular velocity, is a bias of an acceleration.

[0081] Optionally, T is used to indicate time, e.g. a timestamp. Or T is used to indicate a map, e.g. a grid map. Or, T is used to indicate a path.

[0082] In yet another possible implementation of the second aspect, the state quantity has a corresponding timestamp, e.g. the first state quantity can be a displacement of the terminal at a first time instant.

[0083] In yet another possible implementation of the second aspect, the historical state quantities include a third state quantity, the third state quantity being a state quantity adjacent to the first state quantity in time, and a timestamp of the second state quantity being between a timestamp of the third state quantity and a timestamp of the first state quantity;

[0084] The pose optimization module is further configured to:

[0085] perform interpolation calculation on the third state quantity and the first state quantity according to the timestamp of the second state quantity to obtain a first intermediate state quantity, the first intermediate state quantity having the same timestamp as the second state quantity;

[0086] optimize the first state quantity and the historical state quantities according to the first intermediate state quantity and the second state quantity.

[0087] In yet another possible implementation of the second aspect, the pose optimization module is further configured to:

[0088] input the first intermediate state quantity and the second state quantity into an objective function;

[0089] solve the objective function to obtain an error of the first state quantity;

[0090] obtain an optimized first state quantity according to the first state quantity and the error of the first state quantity;

[0091] solve the objective function to obtain an error of the historical state quantity;

[0092] obtain an optimized historical state quantity according to the historical state quantity and the error of the historical state quantity.

[0093] In a further possible implementation form of the second aspect, the first sensor comprises one or more of a probing device, a wheel speed sensor, a navigation positioning system, etc.

[0094] By way of example, the probing device can be a laser radar, such as a mechanical laser radar, or a solid-state laser radar, etc.

[0095] In a further possible implementation form of the second aspect, the pose determination module is further configured to obtain, according to third measurement data from the IMU, a fourth state quantity, a timestamp of the fourth state quantity being after a timestamp of the first state quantity;

[0096] obtain, according to fourth measurement data from a second sensor, a fifth state quantity of the terminal, a timestamp of the fifth state quantity being at a same time window as the timestamp of the fourth state quantity and the timestamp of the first state quantity.

[0097] The pose optimization module is further configured to optimize, according to the fifth state quantity, state quantities within a time window in which the fifth state quantity is located.

[0098] In a further possible implementation form of the second aspect, the pose optimization module is further configured to optimize, according to the fifth state quantity, the first state quantity after optimization, to obtain a second-optimized first state quantity.

[0099] optimize, according to the fifth state quantity, the fourth state quantity, to obtain the fourth state quantity after optimization.

[0100] In a further possible implementation form of the second aspect, the first sensor is a probing device, and the pose determination module is further configured to:

[0101] obtain the second measurement data from the probing device, the second measurement data comprising a plurality of point cloud frames, the plurality of point cloud frames comprising a first point cloud frame and a historical point cloud frame before the first point cloud frame, each point cloud frame in the plurality of point cloud frames comprising a plurality of sampling points, each sampling point in the plurality of sampling points corresponding to a coordinate position and an index information, the index information comprising a height and / or an intensity;

[0102] project the first point cloud frame onto a first plane to obtain first image data, the first image data indicating a distribution of the index information of the sampling points in the first point cloud frame;

[0103] project the historical point cloud frame onto the first plane to obtain second image data, the second image data indicating a distribution of the index information of the sampling points in the historical point cloud frame; and match the first image data and the second image data to obtain the second state quantity.

[0104] In a further possible implementation form of the second aspect, the coordinate positions of the sampling points comprise horizontal coordinate values and vertical coordinate values, and the first plane is a rasterized horizontal-vertical plane.

[0105] In a further possible implementation form of the second aspect, the first plane comprises a plurality of grids, and the pose determination module is further configured to:

[0106] determine, according to the coordinate positions of the sampling points in the first point cloud frame, grid positions of the sampling points in the first point cloud frame in the plurality of grids;

[0107] determine, according to the index information of the sampling points in each of the plurality of grids, an index value of each of the plurality of grids;

[0108] determine the first image data, the first image data comprising the index values corresponding to the plurality of grids respectively.

[0109] In a further possible implementation form of the second aspect, the first plane comprises a plurality of grids, and the pose determination module is further configured to:

[0110] determine, according to the coordinate positions of the sampling points in the historical point cloud frame, grid positions of the sampling points in the historical point cloud frame in the plurality of grids;

[0111] determine, according to the index information of the sampling points in each of the plurality of grids, an index value of each of the plurality of grids;

[0112] determine the second image data, the second image data comprising the index values corresponding to the plurality of grids respectively.

[0113] In a further possible implementation form of the second aspect, the pose determination module is further configured to:

[0114] determine, according to one or more of a computing capability of the computing device, a size of a map in which the terminal is located, a point cloud quantity of the first point cloud frame, a point cloud quantity of the historical point cloud frame, a point cloud density of the first point cloud frame, and a point cloud density of the historical point cloud frame, a grid size in the first plane, the grid size comprising a length and / or a width of each of the plurality of grids.

[0115] In a third aspect, an embodiment of the present application provides an electronic device, comprising a processor and a memory, the memory being configured to store a computer program, and the processor being configured to invoke the computer program stored in the memory, so that the electronic device implements the method described in any one of the first aspect.

[0116] In a fourth aspect, an embodiment of the present application provides a terminal, which comprises the positioning device of the second aspect or the electronic device of the third aspect.

[0117] Further, the terminal further comprises an IMU and a first sensor. The first sensor can be a probe device, a navigation system, or a wheel speed meter, etc.

[0118] Further, the terminal further comprises a second sensor. The second sensor can be a probe device, a navigation system, or a wheel speed meter, etc.

[0119] Optionally, the terminal can be a smart mobile terminal such as a vehicle, a drone, or a robot, etc.

[0120] In a fifth aspect, an embodiment of the present application provides a computer readable storage medium, which stores instructions. When the instructions are run on at least one processor, the method described in any one of the preceding first aspects is implemented.

[0121] In a sixth aspect, the present application provides a computer program product, which comprises computer instructions. When the instructions are run on at least one processor, the method described in any one of the preceding first aspects is implemented.

[0122] Optionally, the computer program product can be a software installation package. When the method described above needs to be used, the computer program product can be downloaded and executed on a computing device.

[0123] The technical solutions provided by the second to sixth aspects of the present application have the beneficial effects of the technical solutions of the first aspect, which will not be repeated here. BRIEF DESCRIPTION OF DRAWINGS

[0124] The drawings used in the description of the embodiments will be briefly described below.

[0125] Figure 1 is a schematic diagram of a positioning system architecture provided by an embodiment of the present application;

[0126] Figure 2 is a schematic diagram of parameters used in fusion positioning provided by an embodiment of the present application;

[0127] Figure 3 is a schematic diagram of a positioning method flow provided by an embodiment of the present application;

[0128] Figure 4 is a schematic diagram of a state quantity provided by an embodiment of the present application;

[0129] Figure 5 is a schematic diagram of an optimization process of a state quantity provided by an embodiment of the present application;

[0130] Figure 6 is a schematic diagram of another state quantity optimization process provided by an embodiment of the present application;

[0131] Figure 7 is a flowchart of a positioning method provided by an embodiment of the present application;

[0132] Figure 8 is a schematic diagram of a scene in which a detection device detects an object space provided by an embodiment of the present application;

[0133] Figure 9 is a schematic diagram of an object space provided by an embodiment of the present application;

[0134] Figure 10 is a schematic diagram of a first point cloud frame provided by an embodiment of the present application;

[0135] Figure 11 is a schematic diagram of a grid position of a sampling point provided by an embodiment of the present application;

[0136] Figure 12 is a schematic diagram of a process for determining a height map provided by an embodiment of the present application;

[0137] Figure 13 is a schematic diagram of a height map provided by an embodiment of the present application;

[0138] Figure 14 is a schematic diagram of second image data provided by an embodiment of the present application;

[0139] Figure 15 is a structural schematic diagram of a positioning device provided by an embodiment of the present application;

[0140] Figure 16 is a structural schematic diagram of an electronic device provided by an embodiment of the present application. DETAILED DESCRIPTION

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

[0142] For the convenience of understanding, the following illustrates some concepts related to the embodiments of the present application for reference. As described below:

[0143] 1. Inertial measurement unit (inertial measurement unit)

[0144] An inertial measurement unit is a device that measures the attitude angle (or angular rate) and acceleration of an object. Generally, an IMU contains an accelerometer and a gyroscope. Generally, the accelerometer detects the acceleration signal of the object in the independent three-axis coordinate system of the carrier, and the gyroscope detects the angular velocity signal of the carrier relative to the navigation coordinate system. The angular velocity and acceleration of the object in three-dimensional space are measured, and the attitude of the object is calculated based on the angular velocity and acceleration.

[0145] 2. Global Navigation Satellite System (GNSS)

[0146] The Global Navigation Satellite System, also known as the Global Satellite Navigation System, is an air-based radio navigation and positioning system that can provide users with all-weather 3D coordinates, speed, and time information at any location on Earth or near-Earth space.

[0147] 3. Detection device

[0148] The detection device is a device capable of detecting the space of an object, including but not limited to a LiDAR, a radar, or a depth camera.

[0149] 4. Kalman filtering

[0150] Kalman filtering is an algorithm that uses linear system state equations to perform optimal estimation of system states based on system input and output observation data.

[0151] The above explanations of technical terms can be applied in the following.

[0152] Currently, mobile terminals (referring to terminals with mobility, such as vehicles, robots, etc.) usually collect positioning systems and various sensors to perform positioning. Sensors such as IMUs, GNSSs, wheel speed sensors, radars, and LiDARs.

[0153] Since each sensor has its own output rhythm when outputting positioning data, the timestamps of the obtained data are usually different, so the timing of the multi-sensor output positioning data cannot be completely aligned. Generally, the positioning data of the multi-sensor is open-loop.

[0154] Since the timestamps of the sensor data entering the fusion positioning cannot be aligned, when the data of a sensor is added to the fusion positioning, the electronic device usually fuses the input sensor positioning data with the current positioning data obtained at the current timestamp. This makes it possible that when new sensor data is input at a certain time, the data near that time may jump with historical data.

[0155] Specifically, when new positioning data of a sensor is input at a certain time, the current pose is optimized based on the new positioning data of the sensor. For the current time, there can be a large difference between the pose before optimization and the pose after optimization. Before optimization, the transition from the historical pose to the current pose is smooth. However, due to the optimization of the current pose, it is highly likely that the optimized pose and the historical pose have a large difference, that is, a jump, which makes the fused positioning data not conducive to smooth driving of the terminal. In addition, since the historical pose is not optimized, the previous historical pose can be inaccurate. When a previous pose is needed in a certain processing process, it will also cause calculation errors and affect decision accuracy.

[0156] Therefore, embodiments of the present application provide a positioning method and related apparatus. The present application is based on the positioning data of the IMU, and combines the positioning data of the sensor (for example, the first sensor) for fusion positioning. The positioning data of the first sensor can optimize the positioning data of the IMU, and the optimization is performed in a time window, and the current positioning data and the historical positioning data of the IMU in the time window are optimized. In other words, in the optimization, the state quantity obtained by the IMU in the time window is optimized as a whole to form a closed loop. Therefore, by implementing the present solution, not only the accuracy of the current positioning data can be improved, but also the accuracy of the historical positioning data in the time window can be improved, and the change of the state quantity in time is more smooth, which is conducive to the driving stability of the terminal.

[0157] Further, the time window can be pushed forward (that is, a sliding window) as time changes. Therefore, the present solution can improve the correlation of state quantity optimization in multi-sensor data fusion, and ensure the real-time performance of pose optimization.

[0158] The system architecture to which embodiments of the present application are applied will be introduced below. It should be noted that the system architecture and business scenarios described in the present application are used to more clearly illustrate the technical solutions of the present application, and do not constitute a limitation on the technical solutions provided by the present application. Those skilled in the art can know that, as the system architecture evolves and new business scenarios appear, the technical solutions provided by the present application are also applicable to similar technical problems.

[0159] Please refer to Figure 1 , Figure 1Fig. 1 is a schematic diagram of an architecture of a positioning system according to an embodiment of the present application. The positioning system 10 comprises a positioning device 101, and one or more of an IMU 102, a GNSS 103, a lidar 104, and a wheel odometer 105. Optionally, the positioning system 10 can be located in an electronic device, which can be a standalone device such as a terminal, a network device, a server, etc., or a module in a standalone device such as a chip, an integrated circuit, etc. The terminal can include, but is not limited to, a vehicle, a movable robot, a drone, etc.

[0160] The following describes the modules in the positioning system 10.

[0161] The positioning device 101 is a device with computing capability, which can fuse the positioning data from multiple sensors. The multiple sensors include one or more of the IMU 102, the GNSS 103, the lidar 104, and the wheel odometer 105.

[0162] The IMU 102 is used to obtain velocity and / or acceleration. The acceleration can include angular acceleration, linear acceleration, etc., and the velocity can include angular velocity, linear velocity, etc. In some scenarios, the IMU 101 can include an accelerometer and a gyroscope. The accelerometer measures linear acceleration, and the gyroscope measures angular velocity. The linear acceleration can be integrated to obtain linear velocity, and the linear velocity can be further integrated to obtain displacement. The angular velocity can be integrated to obtain angle.

[0163] The GNSS 103 can determine one or more of longitude, latitude, or altitude, etc.

[0164] The lidar 104 can obtain point cloud data, which includes one or more sampling points. Each sampling point can include one or more of distance information, height information, intensity information, etc. Since the detection of the lidar 104 to the object space can be continuous, the point cloud data can also include data over time. In some scenarios, the point cloud data is stored or transmitted in the form of frames, which is referred to as point cloud frames. A point cloud frame is the point cloud obtained by the lidar completing a detection to the field of view of the object space, which can be regarded as a picture. Of course, the lidar 104 can be replaced by other detection devices such as a radar, a depth camera, etc.

[0165] The wheel odometer 105 can be used to determine the displacement of the terminal.

[0166] In the embodiment of the present application, the positioning device 101 tightly couples the positioning data of multiple sensors, and improves the positioning accuracy through mutual optimization. The positioning device 101 takes the positioning data from the IMU as the basis, combines the positioning data of other sensors, and improves the accuracy of the current fusion positioning data and the accuracy of the historical fusion positioning data in the fusion process, that is, the sliding window driving is used for fusion to form a closed loop.

[0167] The data of various sensors used in the positioning method of the embodiment of the present application is described below.

[0168] Please refer to Figure 2 , Figure 2 is a schematic diagram of parameters used in the fusion positioning provided by the embodiment of the present application, which shows the process of fusing the positioning data of multiple sensors through Kalman filtering. In the Kalman filtering, the positioning data obtained by the IMU is used as the input data in the prediction stage, and one or more of the positioning data obtained by the GNSS, the positioning data obtained by the wheel speed meter or the positioning data obtained by the LiDAR (an example of a detection device) is used as the input data in the update stage.

[0169] In a possible implementation manner, the IMU includes an accelerometer and a gyroscope, the accelerometer measures linear acceleration, and the gyroscope measures angular velocity. The fusion positioning system integrates the linear acceleration to obtain linear velocity, and further integrates the linear velocity to obtain displacement. The angular velocity is integrated to obtain angle.

[0170] The GNSS can be used to determine the longitude, latitude and altitude of the terminal, and the wheel speed meter gives the displacement information of the terminal by accumulating its own data.

[0171] The data obtained by the laser radar (LiDAR) detection can include multiple point cloud frames. The positioning data of the LiDAR can be obtained through processing, which can include one or more stages such as the following: the first stage is to match the current frame with the last frame to optimize the pose of the current point cloud frame; the second stage is to match the current frame with the map formed by the historical frames to further optimize the pose of the current point cloud frame; and the third stage is loop detection, if the current frame and the historical frames form a loop, the poses of all key frames are optimized. Of course, the third stage is executed when there is a loop, and can not be executed if there is no loop. Similarly, the LiDAR positioning data enters the update stage of the Kalman filtering, and is fused with other positioning data.

[0172] The method provided by the embodiment of the present application is described below.

[0173] Please refer to Figure 3 , Figure 3 is a flowchart of a positioning method provided by the embodiment of the present application. The method can be based on Figure 1The system architecture shown is implemented, for example, the method can be implemented by a positioning device 101 in a positioning system.

[0174] Figure 3 The positioning method shown comprises one or more steps in steps S301-S303.

[0175] Step S301: The positioning device obtains a first state quantity and a historical state quantity of the terminal according to first measurement data from the IMU.

[0176] Specifically, the positioning device is a device with data processing capability. For example, the positioning device can include a central processing unit (CPU), an electronic control unit (ECU), a domain controller (DC), etc. For another example, the positioning device is an electronic device or a module in an electronic device, and the electronic device includes but is not limited to a terminal, a network device, a server, etc. Among them, the terminal is, for example, a vehicle, a mobile robot, a drone, a roadside device, etc.

[0177] The IMU can include an accelerometer and a gyroscope, the accelerometer measures linear acceleration, and the gyroscope measures angular velocity. The first measurement data can include linear acceleration, angular velocity, etc. Alternatively, the first measurement data can be raw data read from a serial port or a chassis, and the positioning device can calculate acceleration, angular velocity, etc. according to a gyroscope measurement model, an accelerometer measurement model, etc. The raw data is used to further determine state quantities (described below) such as velocity, displacement, angle, angular velocity, etc.

[0178] The first state quantity, the historical state quantity, etc. are physical quantities describing the state of the terminal, and can include displacement. Further, the state quantity can also include one or more of angle, velocity, acceleration, angular velocity, angular velocity deviation, velocity deviation, acceleration, momentum, kinetic energy, angular momentum, etc. The state quantity has a corresponding timestamp, for example, the first state quantity can be the displacement of the terminal at the first time, i.e. the timestamp corresponding to the first state quantity indicates the first time. Optionally, the timestamp corresponding to the state quantity can be stored in a key frame, for example, the positioning data of the IMU can include multiple key frames, such as key frames I1, I2, I3, each key frame has a state quantity and a timestamp, for example, the timestamp of key frame I1 is i1, and the state quantity corresponding to key frame I1 is .

[0179] As a possible example, the time instant corresponding to the first state quantity can be the time instant when the measurement data of the state quantity is generated. In this case, the time instant can be stored or transmitted in the form of a time stamp. In the implementation process, the meaning represented by the time stamp can not necessarily be the time instant, for example, the time instant can also be replaced by the time when the data is collected, or the time when the data is output, and the specific implementation is subject to the specific implementation.

[0180] As a possible example, the first state quantity can be in the form of , wherein represents the first state quantity, i indicates the number (for example, the time stamp of the key frame), P represents the displacement, V represents the speed, R represents the angle, is the deviation of the angular velocity, is the deviation of the acceleration. Optionally, T is used to indicate the time, for example, the time stamp. Or T is used to indicate the map, for example, the grid map. Or, T is used to indicate the path.

[0181] The time stamp of the historical state quantity can be a state quantity located before the first state quantity. Similarly, the information contained in the historical state quantity can refer to the first state quantity.

[0182] Please refer to Figure 4 , Figure 4 is a schematic diagram of a state quantity provided by an embodiment of the present application. As a possible example, the positioning device obtains the following state quantities according to the first measurement data from the IMU: , , , , , , , and , and the corresponding time stamps are to . In the illustrated state quantities, if is the first state quantity (as shown in the figure, the first state quantity is the state quantity closest to the current time instant now), the historical state quantity is , , , , , , . Among them, , , , , , , and are state quantities in the same time window, that is, state quantities in the time window #1.

[0183] Step S302: The positioning device obtains a second state quantity of the terminal according to the second measurement data from the first sensor.

[0184] Specifically, the first sensor includes one or more of a detection device, a wheel speed meter, a navigation positioning system, etc. The detection device refers to a device with ranging and / or image detection capabilities, such as a vision-based sensor, a radar-based sensor, etc.

[0185] For example, when the first sensor is a detection device (e.g., a laser radar), the positioning device obtains the state quantity of the terminal according to the measurement data of the detection device. The calculation process can be referred to in the following description Figure 2 , which will be introduced in the following description Figure 7 of the illustrated embodiments.

[0186] Step S303: The positioning device optimizes the first state quantity and the historical state quantity according to the second state quantity, to obtain an optimized first state quantity and an optimized historical state quantity.

[0187] The timestamp of the second state quantity, the timestamp of the second state quantity, and the timestamp of the historical state quantity are located in the same time window. The width of the time window can be predefined, preconfigured, or determined by calculation. For example, the width of the time window can be 10 seconds (s), or the width of the time window can be 100 key frames.

[0188] As a possible implementation, the positioning device determines the timestamp (e.g., the first timestamp) of the second state quantity. The positioning data of the IMU near the first timestamp is calculated using the second state quantity, to obtain the positioning data of the IMU at the first timestamp. Based on the positioning data of the IMU at the first timestamp and the second state quantity, the positioning data of the IMU in a time window is optimized to obtain the fused positioning data. Since the positioning data of the IMU in the time window is uniformly optimized, the closed loop formed by optimizing the current and historical positioning data makes the pose curve more continuous and smooth.

[0189] As an exemplary optimization process, the historical state quantity includes a third state quantity, the third state quantity is a state quantity adjacent to the first state quantity in terms of timestamp, and the timestamp of the second state quantity is between the timestamp of the third state quantity and the timestamp of the first state quantity. The positioning device can perform interpolation calculation on the third state quantity and the first state quantity according to the timestamp of the second state quantity, to obtain a first intermediate state quantity, the timestamp of the first intermediate state quantity being the same as the timestamp of the second state quantity; obtain an error (or referred to as the error of the first intermediate state quantity) according to the first intermediate state quantity and the second state quantity; and optimize the first state quantity and the historical state quantity according to the error.

[0190] Please see Figure 5 , Figure 5 This is a schematic diagram illustrating a state variable optimization process provided in an embodiment of this application. The IMU's positioning data may include... , , , , , , , and .in, and Positioning data was obtained from the wheel speed sensor, for example, represented as... .in, The timestamp is located at and Between timestamps, for example, represented as t e6 The positioning device interpolates the IMU's positioning data to determine the IMU's location. This timestamp location data, for example, is represented as In other words, state variables and The timestamps are the same. The positioning device is based on... and The positioning data obtained from the IMU and the wheel speed meter are in Error at time According to the error optimization Location data of the IMU within the given time window.

[0191] For example, , , , , , , and If they belong to the same time window, such as time window #1, then based on the error... Optimize the above state variables, of course. It also falls within the aforementioned time window. For example, the optimized state variable is represented as follows: , , , , , , and Of course, the above multiple state variables correspond to The values ​​can be different, for example and In may be different.

[0192] As an exemplary optimization process, the positioning device inputs the first intermediate state quantity and the second state quantity into the target function and solves the target function to obtain the error of the first state quantity. When optimizing the first state quantity, the optimized first state quantity is obtained according to the first state quantity and the error of the first state quantity. Similarly, when optimizing the historical state quantity, the error of the historical state quantity is obtained by solving the target function; and the optimized historical state quantity is obtained according to the historical state quantity and the error of the historical state quantity.

[0193] Exemplarily, a target function is as follows:

[0194]

[0195] wherein, is a state quantity calculated according to the measurement data of the IMU, or the positioning data of the IMU. is a state quantity calculated according to the measurement data of the wheel speed meter, or the positioning data of the wheel speed meter. is a state quantity calculated according to the measurement data of the GNSS, or the positioning data of the GNSS. is a state quantity calculated according to the measurement data of the LiDAR, or the positioning data of the LiDAR.

[0196] If the first sensor is the wheel speed meter, when the positioning data of the wheel speed meter is input, the first term is calculated, i.e. Similarly, if the first sensor is the GNSS, when the positioning data of the GNSS is input, the intermediate term is calculated, i.e. Similarly, if the first sensor is the LiDAR, when the positioning data of the LiDAR is input, the intermediate term is calculated, i.e. In some scenarios, the positioning data of the wheel speed meter, the GNSS and the LiDAR may not be input at the same time. If the input time of the positioning data of the three is different, the positioning data is fused according to the input order and the IMU positioning data.

[0197] Further, the electronic device solves the target function so that the value of the target function is minimized. In the solving process, the IMU state quantity is updated, so that the IMU positioning data is optimized. The solving method can include Newton, Gauss-Newton or LM, etc. Exemplarily, the first state quantity is as follows: The optimization process includes the following:

[0198]

[0199] wherein, Exp(δφ) is the error of the angle, is the error of the displacement, an error of a velocity error, an error of an angular velocity error, an error of an acceleration error.

[0200] In some possible scenarios, the data used in the fusion positioning process can be data of multiple sensors. As one possible example, the positioning apparatus obtains a fourth state quantity according to third measurement data from the IMU, and a timestamp of the fourth state quantity is after a timestamp of the first state quantity. That is, as time goes on, the positioning data of the IMU continues to change forward, and the positioning apparatus determines the fourth state quantity after determining the first state quantity. Further, the positioning apparatus obtains a fifth state quantity of the terminal according to fourth measurement data from the second sensor, and a timestamp of the fifth state quantity is in the same time window as the timestamp of the fourth state quantity and the timestamp of the first state quantity. In other words, the positioning data of the second sensor is input and participates in the fusion positioning process. The positioning apparatus optimizes the state quantities in the time window in which the fifth state quantity is located according to the fifth state quantity. Since the fourth state quantity and the first state quantity are in the same time window, the fifth state quantity can be used to optimize the fourth state quantity and the first state quantity. Specifically, the positioning apparatus optimizes the first state quantity after optimization according to the fifth state quantity to obtain a second-optimized first state quantity, and optimizes the fourth state quantity according to the fifth state quantity to obtain an optimized fourth state quantity.

[0201] In the above embodiment, the second sensor is another sensor other than the first sensor. In other words, the positioning data of the IMU can be fused with multiple sensors to improve positioning accuracy.

[0202] See Figure 6 , Figure 6 is another schematic diagram of a state quantity optimization process provided in an embodiment of the present application. As can be seen, the positioning apparatus calculates measurement data from the IMU to obtain state quantities. For example, the displacement of the IMU on the x axis in a certain time period is x1, x2, x3,..., xn, xn+1, xn+2 respectively. If there is wheel speed meter positioning data, the IMU positioning data and the wheel speed meter positioning data are fused, and the current IMU and wheel speed meter positioning data are optimized, and the historical positioning data in the sliding window is also updated. If there is GNSS positioning data, the GNSS positioning data is fused with the fused positioning data of the optimized IMU and wheel speed meter, and the current positioning data of the three is optimized, and the historical positioning data in the sliding window is updated. If there is LiDAR positioning data, the LiDAR positioning data is fused with the fused positioning data of the previous three, and the current and historical positioning data are optimized. It should be noted that Figure 6The fusion sequence and quantity shown are only examples, and the wheel speed meter, GNSS and LiDAR positioning data and the IMU positioning data have no front-back fusion sequence. When the positioning data of a sensor is obtained, it is fused with the IMU positioning data. Optionally, the positioning data of the sensor can be more or less, and the general fusion principle does not change.

[0203] In Figure 3 In the embodiment shown, the positioning device can be based on the positioning data (such as the first state quantity, the historical state quantity) of the IMU, and the positioning data of the sensor (such as the first sensor) is fused for positioning. The positioning data of the first sensor can optimize the positioning data of the IMU, and the optimization is performed on the current positioning data and the historical positioning data of the IMU in a time window. In other words, the state quantity in the time window is optimized as a whole to form a closed loop. Therefore, by implementing the present solution, not only the accuracy of the current positioning data can be improved, but also the accuracy of the historical positioning data in the time window can be improved, and the change of the state quantity over time is more smooth, which is beneficial to the terminal driving.

[0204] Further, the time window can be pushed forward (i.e. sliding window) as time changes. Therefore, the present solution can improve the relevance of state quantity optimization when fusing multi-sensor data, and ensure the real-time performance of pose optimization.

[0205] The above positioning method involves the positioning data (such as the second state quantity or the fifth state quantity) of the detection device. As a possible embodiment, the positioning data of the detection device can be obtained based on image matching. Specifically, the measurement data of the detection device is input into the positioning device, and the positioning device generates an intensity map and a height map based on a two-dimensional grid map, using the intensity and height of the sampling points in the point cloud data. The historical point cloud data is saved in the grid map. When positioning, the intensity map and the height map of the current frame of point cloud are aligned with the intensity map and the height map of the historical frame, respectively, to determine the IDE positioning data of the detection device. In the above embodiment, the historical point cloud data is saved, and there is no downsampling, feature point extraction or feature point matching, which can greatly reduce the time for obtaining the positioning data and reduce the demand for computing power. Moreover, the present solution can be applied to mechanical LiDAR and solid-state LiDAR, and can ensure real-time performance with minimum computing power under the condition of meeting the positioning accuracy requirement.

[0206] The acquisition process of the positioning data of the detection device will be described below. Please refer to Figure 7 , Figure 7 is a flowchart of a positioning method provided by an embodiment of the present application, which can be applied to the positioning system described above. For example, the method can be implemented by the positioning device 101 in the positioning system.

[0207] Step S701: The positioning device acquires a plurality of point cloud frames from the detection device, the plurality of point cloud frames including a first point cloud frame and a historical point cloud frame before the first point cloud frame.

[0208] Specifically, the positioning device is a device with data processing capability. For example, the positioning device can include a central processing unit (CPU), an electronic control unit (ECU), a domain controller (DC), etc.

[0209] The detection device can detect the object space to obtain point cloud data. Since the detection of the object space by the detection device can be continuous, the point cloud data can also include data at multiple time stamps (or time points). In some scenarios, the point cloud data can be stored or transmitted in the form of frames, referred to as point cloud frames. A point cloud frame is a point cloud obtained by the laser radar completing a detection of the field of view of the object space, which can be regarded as a picture.

[0210] Please refer to Figure 8 , Figure 8 is a scene diagram provided by an embodiment of the present application, in which a plurality of objects (including living objects) can exist in the object space, such as object 1, object 2, object 3, etc. As viewed from the field of view of the detection device, the picture of the object space can be as shown in Figure 9 Since the detection is continuous, detection data corresponding to different time points can be obtained, such as a point cloud frame at t0 and a point cloud frame at t1.

[0211] The first point cloud frame obtained by the detection device for the object space can be as shown in Figure 10 . Among them, the first point cloud frame can be a point cloud frame, or a point cloud frame obtained by fusing a plurality of point cloud frames.

[0212] Each point cloud frame acquired by the positioning device includes a plurality of sampling points. Each sampling point in the plurality of sampling points corresponds to a coordinate position and an index information. The coordinate position can indicate the position of the sampling point in the coordinate system, wherein the coordinate system includes but is not limited to Cartesian coordinate system, polar coordinate system, or spherical coordinate system, etc. The index information includes but is not limited to one or more of height, intensity, reflectivity, etc. As Figure 10 , the first point cloud frame can include a sampling point P1, and the information of the sampling point P1 is in the form of , , , , wherein is a horizontal coordinate value, is a vertical coordinate value, is a height (or a vertical coordinate value), is an intensity.

[0213] Optionally, the positioning device can obtain the point cloud frame through communication, which can be wired or wireless, or through data copying or reading, etc. The specific implementation of obtaining the point cloud frame is not strictly limited herein.

[0214] Step S702: The positioning device projects the first point cloud frame onto a first plane to obtain first image data.

[0215] The first plane can be a horizontal plane or other plane. Of course, the plane herein is not necessarily a completely horizontal plane, i.e., the first plane can have ups and downs. For example, the first plane can be a longitudinal and transverse plane, or an X-Y plane.

[0216] The first image data indicates the distribution of the index information of the sampling points in the first point cloud frame. For example, the index information can be height, and the first image data can be a height map, which can indicate the distribution of the sampling points on the plane. For another example, the index information can be intensity, and the first image data can be an intensity map. For another example, the index information includes both intensity and height, and the first image data can include an intensity map and a height map. In other words, the z-axis of the point cloud is compressed to convert the three-dimensional point cloud into a two-dimensional image, and the intensity value and / or the height value become a feature of the image.

[0217] As a possible implementation, when projecting, the positioning device can determine the position of the sampling point on the first plane according to the coordinate position of the sampling point in the point cloud frame. The position on the first plane can correspond to an index value, which is determined by the index information of the sampling point at the position. The sampling points at different positions are different, and thus the distribution of the index information of the sampling points at different positions forms the first image data.

[0218] Optionally, the first plane can include a plurality of grids. For example, the first plane is a gridded plane. In this case, the first image data includes a plurality of grids and the index values (or the indication of the index values, such as light and dark conditions) corresponding to the plurality of grids, respectively.

[0219] The index information is taken as a height map as an example for description, which is also applicable to an intensity map.

[0220] For example, the information of the first sampling point can include the height of the first sampling point, the intensity of the first sampling point, and the position of the first sampling point on the first plane. , , ). The positioning device determines a grid map (the grid map can be regarded as a first plane), and the grid map includes a plurality of grids. The positioning device determines a grid position of the sampling point in the grid map according to the abscissa (or x value) and the ordinate (or y value) of the sampling point. Further, the positioning device forms a height map according to the average value of the height of the sampling point in the grid.

[0221] Optionally, the initial position of the grid map, or the initial position of the electronic device and the detection device in the grid map can be given by the fusion positioning.

[0222] Please refer to Figure 11 , Figure 11 is a schematic diagram of a grid position of a sampling point provided by an embodiment of the present application. Each grid is numbered by the horizontal position and the vertical position. The positioning device determines the grid position of the sampling point according to the abscissa and the ordinate of the sampling point. As shown in Figure 11 , according to the abscissa and ordinate values of the point p1 , , it is determined that the position of the point p1 falls into the grid (2, 5).

[0223] The positioning device obtains a height map according to the average value of the height of the sampling point in the grid. As shown in Figure 12 is a process schematic diagram for determining a height map provided by an embodiment of the present application. Two sampling points fall into the grid (2, 5). If the height values (i.e. z values) of the sampling points are 26.1 and 26.7 respectively, then the average value of the height of the sampling points in the grid (2, 5) is 26.4 (i.e. the index value of the grid). Similarly, according to the height values of the sampling points in other grids, the average values of the heights of the sampling points in the grids are obtained to form a height map. As shown in Figure 13 is a schematic diagram of a height map provided by an embodiment of the present application. Each grid corresponds to an average value of the height, thereby indicating the distribution of the height map of the sampling point in the x-y plane.

[0224] As a possible implementation, the size of the grid map can be determined according to one or more of the computing power, the size of the map, the number of point clouds, or the density of point clouds. The size includes the side length (such as the length and / or the width) of the grid, and the computing power is the computing power of the positioning device or the computing power of other devices.

[0225] In some scenarios, the size of the grid map can be determined by road testing, thereby maintaining the balance between the computing power and the accuracy under the condition of ensuring the real-time performance.

[0226] Step S703: The positioning device projects the historical point cloud frame to the first plane to obtain second image data.

[0227] The second image data indicates the distribution of index information of the sampling points in the historical point cloud frames. For example, the index information can be height, and the first image data can be a height map, which indicates the distribution of the sampling points on a plane. For another example, the index information can be intensity, and the first image data can be an intensity map. For another example, the index information includes both intensity and height, and the first image data can include an intensity map and a height map.

[0228] As a possible implementation, the positioning device creates a grid map according to the historical point cloud data, and obtains the second image data according to the grid positions of the sampling points in the historical point cloud frames in the grid map.

[0229] The following describes the height map as an example, and the intensity map is also applicable.

[0230] Please refer to Figure 14 , Figure 14 is a schematic diagram of the second image data provided by an embodiment of the present application. The point cloud data includes a plurality of point cloud frames. If the current obtained point cloud frame is #2, the historical point cloud frames are point cloud frames #0 and #1, and optionally also include point cloud frame #2. The positioning device can obtain height map #1 according to point cloud frame #0, and can obtain height map #2 according to point cloud frame #1.

[0231] In a possible implementation, the second image data is obtained by fusing the sampling points in a plurality of point cloud frames. Specifically, the positioning device determines the grid position of a historical sampling point (i.e., a sampling point in a historical point cloud frame) according to the horizontal and vertical coordinate values (x, y) of the sampling point, and then updates the height average of the point cloud in the grid to form the second image data.

[0232] For example, in the second image data, the height value of a certain grid can satisfy the following formula.

[0233]

[0234] wherein n represents the number of point clouds in the grid, is the intensity average of the point clouds in the grid, is the height of the new point cloud, is the latest height average. For example, for the grid (2, 5) in point cloud frame #2, the number of point clouds in the grid is 2, the intensity average of the point clouds in the grid is 26.4 (i.e., the current height value), if a sampling point in point cloud frame #2 falls into the grid (2, 5) and the height value is 26.5, then the latest height average of the grid (2, 5) is [(2 26.2+26.5) ÷ (2+1)=]26.3. In other words, for the current point cloud frame #2, it can be updated in the height map #1 corresponding to the point cloud frame #1 to obtain the height map #2. Similarly, the height map #1 can be obtained by updating the height map #0.

[0235] Further, for each sampling point in the point cloud frame falling into the grid (2, 5), the height value of the grid can be updated using the above formula. For example, if there are k sampling points in the point cloud frame #2 falling into a certain grid, the height value of the grid can satisfy the following formula:

[0236]

[0237] wherein n represents the number of point clouds in the grid, is the average intensity of the point clouds in the grid, is the height of the new point cloud, is the average value of the latest height. In the foregoing formula, n and k are integers and n and k are greater than or equal to 0.

[0238] Step S704: The positioning device matches the first image data and the second image data to obtain the second state quantity of the terminal.

[0239] In a possible implementation, the positioning device performs graphic matching, also referred to as graphic correspondence, on the first image data and the second image data.

[0240] Optionally, when matching, part of the grids in the first image data can be matched with part of the grids in the second image data. For example, given an initial position, ten rows above, below, left and right of the grid are taken, and a total of 441 grids are matched.

[0241] Optionally, the number of grids for matching can be predefined, preconfigured or determined according to actual conditions. For example, the number of grids for matching can be determined according to an example, a grid size, a positioning accuracy requirement, or the like.

[0242] As a possible implementation, the positioning device performs graphic alignment through a lucas-kanade algorithm. Specifically, the positioning device sets an objective function, matches the first image data and the second image data based on the objective function, and obtains the motion quantity data.

[0243] For example, a possible objective function is as follows:

[0244]

[0245] wherein T is a grid map created by a historical frame, including the second image data, I is a grid map created by a current point cloud frame, including the first image data, W is a transformation, and p is a motion quantity Optionally, x can be a state variable.

[0246] In one possible implementation, the initial motion amount of the positioning device in each iteration is given by the fused positioning, and the increment is estimated. To update exercise volume For example, regarding After performing a first-order Taylor expansion, the objective function is:

[0247]

[0248] in It is a gradient map formed by an intensity map or a height map, for After differentiating, setting the result to 0, we get:

[0249]

[0250]

[0251] The positioning device fuses the motion quantities derived from the intensity map and the height map to obtain the second state quantity. .

[0252] exist Figure 7 In the illustrated embodiment, the positioning device performs two-dimensional projection on the point cloud frame from the detection device to form a distribution image for a specific indicator. The distribution image of the current point cloud frame is then matched with the distribution images of historical point cloud frames to obtain the terminal's state value. Compared to methods using unprocessed point cloud frames to obtain positioning data, this significantly reduces computational load and improves positioning efficiency. Furthermore, matching with two-dimensional images shortens the time required to obtain the state value, allowing the second state value to be quickly fused with other positioning data, thus improving positioning accuracy.

[0253] The aforementioned method of obtaining state variables based on IMU measurement data is described below. Specifically, it may include one or more steps S1-S6.

[0254] Step S1: The positioning device acquires the raw data from the IMU.

[0255] Here, raw IMU data refers to the data output by the IMU. For example, the positioning device reads raw IMU data from a serial port or chassis.

[0256] Step S2: The positioning device calculates the gyroscope measurement model based on the angular velocity.

[0257]

[0258] in, is a test value of angular velocity, is a true value of angular velocity, is a bias of angular velocity, is a measurement noise of angular velocity. Optionally, the above parameters can be obtained based on an IMU coordinate system.

[0259] Step S3: The positioning device calculates an accelerometer measurement model according to the acceleration:

[0260]

[0261] wherein, is a measurement value of acceleration, is a bias of acceleration, is a measurement noise of acceleration. In the world coordinate system, is a true value of acceleration, is a gravity acceleration, is a rotation matrix. Optionally, the above parameters can be obtained based on an IMU coordinate system.

[0262] Step S4: The positioning device determines a rotation matrix related to an angle.

[0263] Specifically, the positioning device obtains the rotation matrix after a time ,

[0264]

[0265] wherein, is a change amount of time.

[0266] Step S5: After a time , the velocity and displacement are calculated.

[0267]

[0268]

[0269] Step S6: The first state quantity is determined. For example, the positioning device derives the state quantity of a key frame j from the state quantity of a key frame i, wherein i and j are numbers of the key frames.

[0270]

[0271]

[0272]

[0273] In the terminal coordinate system, the first state quantity is defined as . According to the above formula, the first state quantity of the terminal at any time can be calculated by using the interpolation principle.

[0274] The above describes the method of the embodiments of the present application in detail. In order to facilitate better implementation of the above scheme of the embodiments of the present application, the device of the embodiments of the present application is provided as follows.

[0275] It can be understood that the device provided by the embodiments of the present application, for example, an emotion recognition device, contains a hardware structure, a software module, or a combination of a hardware structure and a software module corresponding to the execution of each function, etc. in order to realize the functions in the above method embodiments.

[0276] Those skilled in the art should easily realize that, in combination with the units and steps of the examples described in the embodiments disclosed herein, the embodiments of the present application can be realized in the form of hardware or a combination of hardware and computer software. Whether a certain function is executed in the form of hardware or computer software driving hardware depends on the specific application and design constraints of the technical solution. Professional technicians can use different device implementation manners to realize the above-mentioned method embodiments in different use scenarios, and different implementation manners of the device should not be considered as beyond the scope of the embodiments of the present application.

[0277] The embodiments of the present application can divide the device into functional modules. For example, each functional module can be divided according to each function, or two or more functions can be integrated into one functional module. The above-mentioned integrated module can be realized in the form of hardware or in the form of a software functional module. It should be noted that the division of modules in the embodiments of the present application is illustrative, and is only a logical functional division. Actual implementation can have another division manner. For example, in the case of dividing each functional module of the device by integration, the present application lists several possible processing devices.

[0278] Please refer to Figure 15 , Figure 15 is a structural schematic diagram of a positioning device provided by the embodiments of the present application. The positioning device 150 can be the positioning device 101 shown in Figure 1 , or a device in the positioning device 101. The positioning device 150 is used to realize the positioning method shown in Figure 3 or Figure 7 .

[0279] The positioning device 150 can include a pose determination module 1501 and a pose optimization module 1502. Wherein, the detailed description of each unit is as follows:

[0280] The pose determination module 1501 is used to:

[0281] obtain a first state quantity of the terminal and a historical state quantity before the timestamp of the first state quantity according to the first measurement data from the inertial measurement unit IMU;

[0282] obtaining a second state quantity of the terminal according to second measurement data from the first sensor;

[0283] The pose optimization module 1502 is configured to optimize the first state quantity and the historical state quantity according to the second state quantity, to obtain an optimized first state quantity and an optimized historical state quantity, and the optimized first state quantity and the optimized historical state quantity are used to determine the position of the terminal.

[0284] The timestamps of the first state quantity, the second state quantity and the historical state quantity are located in the same time window.

[0285] In yet another possible implementation, the state quantity, such as the first state quantity, the second state quantity, the historical state quantity, etc., can include displacement. Further, the state quantity can also include one or more of angle, velocity, acceleration, angular velocity, angular velocity deviation, velocity deviation, acceleration, momentum, kinetic energy, angular momentum, etc.

[0286] In yet another possible implementation, the state quantity has a corresponding timestamp, for example, the first state quantity can be the displacement of the terminal at the first time.

[0287] In yet another possible implementation, the historical state quantity includes a third state quantity, the third state quantity is a state quantity adjacent to the timestamp of the first state quantity, and the timestamp of the second state quantity is located between the timestamp of the third state quantity and the timestamp of the first state quantity.

[0288] The pose optimization module 1502 is further configured to:

[0289] According to the timestamp of the second state quantity, the third state quantity and the first state quantity are calculated by interpolation to obtain a first intermediate state quantity, and the timestamp of the first intermediate state quantity is the same as the timestamp of the second state quantity.

[0290] According to the first intermediate state quantity and the second state quantity, the first state quantity and the historical state quantity are optimized.

[0291] In yet another possible implementation, the pose optimization module 1502 is further configured to:

[0292] The first intermediate state quantity and the second state quantity are input into an objective function.

[0293] The objective function is solved to obtain an error of the first state quantity.

[0294] According to the first state quantity and the error of the first state quantity, the optimized first state quantity is obtained.

[0295] solving the objective function to obtain an error of the historical state quantity;

[0296] obtaining an optimized historical state quantity according to the historical state quantity and the error of the historical state quantity.

[0297] In yet another possible implementation, the first sensor comprises one or more of a probing device, a wheel speed meter, a navigation positioning system, etc.

[0298] For example, the probing device can be a laser radar, such as a mechanical laser radar or a solid-state laser radar, etc.

[0299] In yet another possible implementation, the pose determination module 1501 is further configured to obtain a fourth state quantity according to third measurement data from the IMU, a timestamp of the fourth state quantity being after a timestamp of the first state quantity.

[0300] obtain a fifth state quantity of the terminal according to fourth measurement data from a second sensor, a timestamp of the fifth state quantity being in the same time window as the timestamp of the fourth state quantity and the timestamp of the first state quantity.

[0301] The pose optimization module 1502 is further configured to optimize state quantities in a time window in which the fifth state quantity is located according to the fifth state quantity.

[0302] In yet another possible implementation, the pose optimization module 1502 is further configured to optimize the first state quantity after optimization according to the fifth state quantity, to obtain a second-optimized first state quantity.

[0303] optimize the fourth state quantity according to the fifth state quantity, to obtain an optimized fourth state quantity.

[0304] In yet another possible implementation, the first sensor is a probing device, and the pose determination module 1501 is further configured to:

[0305] obtain the second measurement data from the probing device, the second measurement data comprising a plurality of point cloud frames, the plurality of point cloud frames comprising a first point cloud frame and a historical point cloud frame before the first point cloud frame, each point cloud frame in the plurality of point cloud frames comprising a plurality of sampling points, each sampling point in the plurality of sampling points corresponding to a coordinate position and index information, the index information comprising height and / or intensity;

[0306] project the first point cloud frame onto a first plane to obtain first image data, the first image data indicating a distribution of index information of sampling points in the first point cloud frame;

[0307] projecting the historical point cloud frame to the first plane to obtain second image data, the second image data indicating a distribution of index information of the sampling points in the historical point cloud frame; and matching the first image data and the second image data to obtain the second state quantity.

[0308] In yet another possible implementation, the coordinate position of the sampling point includes a horizontal coordinate value and a vertical coordinate value, and the first plane is a horizontal-vertical plane that is rasterized.

[0309] In yet another possible implementation, the first plane includes a plurality of grids, and the pose determination module 1501 is further configured to:

[0310] determine, according to the coordinate position of the sampling point in the first point cloud frame, a grid position of the sampling point in the first point cloud frame in the plurality of grids;

[0311] determine, according to the index information of the sampling point in each grid in the plurality of grids, an index value of each grid in the plurality of grids;

[0312] determine the first image data, the first image data including a plurality of grid corresponding index values.

[0313] In yet another possible implementation, the first plane includes a plurality of grids, and the pose determination module 1501 is further configured to:

[0314] determine, according to the coordinate position of the sampling point in the historical point cloud frame, a grid position of the sampling point in the historical point cloud frame in the plurality of grids;

[0315] determine, according to the index information of the sampling point in each grid in the plurality of grids, an index value of each grid in the plurality of grids;

[0316] determine the second image data, the second image data including a plurality of grid corresponding index values.

[0317] In yet another possible implementation, the pose determination module 1501 is further configured to:

[0318] determine, according to one or more of an operation capability of an operation device, a size of a map in which the terminal is located, a point cloud quantity of the first point cloud frame, a point cloud quantity of the historical point cloud frame, a point cloud density of the first point cloud frame, and a point cloud density of the historical point cloud frame, a grid size in the first plane, the grid size including a length and / or a width of each grid in the plurality of grids.

[0319] See Figure 16 , Figure 16This is a schematic diagram of the structure of an electronic device provided in an embodiment of this application. The electronic device 160 includes a processor 1601 and a memory 1602. Optionally, it includes a communication interface 1603. The processor 1601, memory 1602, and communication interface 1603 (optionally) are interconnected via a bus.

[0320] Processor 1601 can be one or more central processing units (CPUs). When processor 1601 is a CPU, the CPU can be a single-core CPU or a multi-core CPU.

[0321] The memory 1602 includes, but is not limited to, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM), or compact disc read-only memory (CD-ROM), which is used for related computer programs and data.

[0322] Communication interface 1603 is used to receive and send data.

[0323] The electronic device 160 is used to implement the aforementioned positioning method, for example... Figure 3 and / or Figure 7 The positioning method shown.

[0324] This application also provides a computer-readable storage medium storing a computer program that, when run on a network device,... Figure 3 or Figure 7 The method flow shown is thus implemented.

[0325] In this application, the term "multiple" in the embodiments refers to two or more objects; "and / or" describes the relationship between associated objects, indicating that three relationships can exist. For example, A and / or B can represent three cases: A alone, A and B simultaneously, and B alone, where A and B can be singular or plural; the character " / " generally indicates that the preceding and following associated objects are in an "or" relationship. Furthermore, unless otherwise stated, the term "first" in the embodiments of this application is only used for name identification and is not used to limit the order, sequence, priority, or importance of multiple objects. This rule also applies to "second," "third," and "fourth," etc.

[0326] The above merely provides the specific implementation of the present application, but the protection scope of the present application is not limited to this. Any person skilled in the art can easily think of various equivalent modifications or replacements within the technical range disclosed by the present application, and these modifications or replacements should be covered in the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. A positioning method, characterized by, The method comprises: obtaining a first state quantity of a terminal and a historical state quantity before a time stamp of the first state quantity according to first measurement data from an inertial measurement unit (IMU), the historical state quantity containing a third state quantity, the third state quantity being a state quantity adjacent to the first state quantity in time; obtaining a second state quantity of the terminal according to second measurement data from a first sensor, a time stamp of the second state quantity being between a time stamp of the third state quantity and a time stamp of the first state quantity; interpolating the third state quantity and the first state quantity according to the time stamp of the second state quantity to obtain a first intermediate state quantity, the time stamp of the first intermediate state quantity being the same as the time stamp of the second state quantity; optimizing the first state quantity and the historical state quantity according to the first intermediate state quantity and the second state quantity, the optimized first state quantity and the optimized historical state quantity being used to determine a position of the terminal; the time stamp of the first state quantity, the time stamp of the second state quantity and the time stamp of the historical state quantity being in the same time window.

2. The method of claim 1, wherein, The optimization of the first state quantity and the historical state quantity according to the first intermediate state quantity and the second state quantity comprises: inputting the first intermediate state quantity and the second state quantity into an objective function; solving the objective function to obtain an error of the first state quantity; obtaining the optimized first state quantity according to the first state quantity and the error of the first state quantity; solving the objective function to obtain an error of the historical state quantity; obtaining the optimized historical state quantity according to the historical state quantity and the error of the historical state quantity.

3. The method of claim 1, wherein, The first sensor contains one or more of a detection device, a wheel speed meter and a navigation positioning system, and the detection device contains a radar and / or a lidar.

4. The method according to any one of claims 1 to 3, characterized in that, The method further comprises: obtaining a fourth state quantity according to third measurement data from the IMU, a time stamp of the fourth state quantity being after the time stamp of the first state quantity; obtaining a fifth state quantity of the terminal according to fourth measurement data from a second sensor, a time stamp of the fifth state quantity being in the same time window as the time stamp of the fourth state quantity and the time stamp of the first state quantity; optimizing the optimized first state quantity according to the fifth state quantity to obtain a second-optimized first state quantity; optimizing the fourth state quantity according to the fifth state quantity to obtain an optimized fourth state quantity.

5. The method according to any one of claims 1 to 3, characterized in that, The first sensor is a detection device, and the obtaining of the second state quantity of the terminal according to the second measurement data from the first sensor comprises: obtaining the second measurement data from the detection device, the second measurement data containing a plurality of point cloud frames, the plurality of point cloud frames containing a first point cloud frame and a historical point cloud frame before the first point cloud frame, each point cloud frame in the plurality of point cloud frames containing a plurality of sampling points, each sampling point in the plurality of sampling points corresponding to a coordinate position and index information, the index information containing height and / or intensity. projecting the first point cloud frame to a first plane to obtain first image data, the first image data indicating distribution of index information of sampling points in the first point cloud frame; projecting the historical point cloud frame to the first plane to obtain second image data, the second image data indicating distribution of index information of sampling points in the historical point cloud frame; and matching the first image data and the second image data to obtain the second state quantity.

6. The method of claim 5, wherein, The first plane includes a plurality of grids, and the projecting the first point cloud frame to the first plane to obtain first image data comprises: determining, according to coordinate positions of the sampling points in the first point cloud frame, grid positions of the sampling points in the first point cloud frame in the plurality of grids; determining, according to index information of the sampling points in each grid in the plurality of grids, an index value of each grid in the plurality of grids; determining the first image data, the first image data including index values corresponding to the plurality of grids respectively.

7. An electronic device, comprising: The electronic device includes a processor and a memory, the memory being configured to store a computer program, and the processor being configured to invoke the computer program stored in the memory, so that the electronic device implements the method of any one of claims 1-6.

8. A vehicle characterized by comprising: The electronic device, the inertial measurement unit (IMU) and the first sensor as claimed in claim 7.

9. A computer-readable storage medium, characterized in that, The computer readable storage medium stores a computer program, and when the computer program runs on a computer, the computer executes the method of any one of claims 1-6.

Images