A method, device, equipment and storage medium for positioning a traveling device

By combining inertial navigation and lidar multi-sensor fusion technology and combined with wheel speedometer data, the precise and autonomous positioning of the driving equipment is achieved, solving the problem of low positioning accuracy in the existing technology, and providing stable and accurate positioning information.

CN112880674BActive Publication Date: 2024-11-19CHONGQING LEISHEN INTELLIGENT SYSTEM TECHNOLOGY CO LTD
View PDF 9 Cites 0 Cited by

Patent Information

Application Number
CN202110084188.2
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2021-01-21
Publication Date
2024-11-19
Estimated Expiration
2041-01-21

AI Technical Summary

Technical Problem

The prior art has low accuracy in positioning results when locating driving equipment, especially when the environmental map is unknown or single sensor data acquisition, there are problems of uncertainty and low reliability.

Method used

Using combined inertial navigation and lidar multi-sensor fusion technology, the predicted state space vector and error covariance matrix of the driving equipment are determined through Kalman filter and normal distribution transformation algorithm, combined with wheel speedometer data, and iterative matching and error estimation are performed. , output stable and accurate positioning information.

Benefits of technology

It realizes accurate and autonomous positioning of driving equipment, improves the stability and accuracy of positioning information, and solves the problem of uncertainty in data of a single sensor.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN112880674B_ABST
    Figure CN112880674B_ABST
Patent Text Reader

Abstract

The embodiment of the present application discloses a positioning method, device, equipment and storage medium for a driving device. The method includes: determining a predicted state space vector and a predicted error covariance matrix according to the state data and initial filter parameters of the driving device; iteratively matching the prior map data and the point cloud data according to the general state space vector of the predicted state space vector, and determining the first pose information and the observation noise matrix; determining the second pose information and the measured speed information according to the pulse data collected by the wheel speed meter, and determining the wheel speed meter estimation error; determining the current state space vector and the current error covariance matrix according to the first pose information, the second pose information, the measured speed information, the observation noise matrix, the wheel speed meter estimation error and the predicted error covariance matrix, outputting the current state space vector, and resetting the error state space vector and the current error covariance matrix. The present application can provide stable and accurate positioning information for the driving device.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The embodiments of the present invention relate to the field of computer technology, and in particular to a positioning method, device, equipment and storage medium for a traveling device. Background Art

[0002] With the development of artificial intelligence technology, intelligent driving equipment (such as intelligent robots, automatic driving vehicles, etc.) is increasingly used in production and life. During the operation of driving equipment, real-time positioning is a key link in supervising driving equipment.

[0003] At present, when the existing technology locates the moving equipment, it is usually based on the known environment map, and a single sensor installed on the moving equipment is used to collect data to locate the moving equipment. However, there is a problem of low accuracy of the positioning result, which urgently needs to be improved. Summary of the invention

[0004] The embodiments of the present application provide a positioning method, apparatus, device and storage medium for a traveling device, which can enable the traveling device to achieve accurate autonomous positioning.

[0005] In a first aspect, an embodiment of the present application provides a positioning method for a traveling device, the method comprising:

[0006] Determine a predicted state space vector and a predicted error covariance matrix of the driving device according to the state data of the driving device collected by the combined inertial navigation and the initial filter parameters; wherein the predicted state space vector includes a general state space vector and an error state space vector;

[0007] Iteratively matching the prior map data and the point cloud data collected by the laser radar according to the general state space vector of the predicted state space vector, and determining the first position information and the observation noise matrix of the driving device according to the iterative matching result;

[0008] Determining second position information and measurement speed information of the traveling device according to pulse data collected by the wheel speed meter, and determining an estimation error of the wheel speed meter;

[0009] According to the first posture information, the second posture information, the measured speed information, the observation noise matrix, the wheel speed meter estimation error and the prediction error covariance matrix, the current state space vector and the current error covariance matrix of the driving device are determined, the current state space vector is output, and the error state space vector and the current error covariance matrix of the current state space vector are reset.

[0010] In a second aspect, an embodiment of the present application provides a positioning device for a traveling device, the device comprising:

[0011] A prediction module, used to determine a predicted state space vector and a predicted error covariance matrix of the driving device according to the state data of the driving device collected by the combined inertial navigation and the initial filter parameters; wherein the predicted state space vector includes a general state space vector and an error state space vector;

[0012] A matching module, used for iteratively matching the prior map data and the point cloud data collected by the laser radar according to the general state space vector of the predicted state space vector, and determining the first position information and the observation noise matrix of the driving device according to the iterative matching result;

[0013] A first determination module is used to determine the second position information and the measured speed information of the traveling device according to the pulse data collected by the wheel speed meter, and determine the estimation error of the wheel speed meter;

[0014] The second determination module is used to determine the current state space vector and the current error covariance matrix of the driving device according to the first posture information, the second posture information, the measured speed information, the observation noise matrix, the wheel speed meter estimation error and the prediction error covariance matrix, output the current state space vector, and reset the error state space vector and the current error covariance matrix of the current state space vector.

[0015] In a third aspect, an embodiment of the present application provides an electronic device, the electronic device comprising:

[0016] one or more processors;

[0017] A storage device for storing one or more programs;

[0018] When the one or more programs are executed by the one or more processors, the one or more processors implement the positioning method of the traveling device described in any embodiment of the present application.

[0019] In a fourth aspect, an embodiment of the present application provides a non-volatile readable storage medium, wherein the non-volatile readable storage medium stores computer executable instructions, wherein the computer executable instructions are used to implement the positioning method of the traveling device described in any embodiment of the present application.

[0020] The embodiment of the present application provides a positioning method, device, equipment and storage medium for a driving device, which determines the predicted state space vector and the predicted error covariance matrix of the driving device according to the state data and initial filter parameters of the driving device collected by the combined inertial navigation; iteratively matches the prior map data and the point cloud data collected by the laser radar according to the general state space vector of the predicted state space vector, and determines the first pose information and the observation noise matrix of the driving device according to the iterative matching result; determines the second pose information and the measured speed information of the driving device according to the pulse data collected by the wheel speed meter, and determines the wheel speed meter estimation error; determines the current state space vector and the current error covariance matrix of the driving device according to the first pose information, the second pose information, the measured speed information, the observation noise matrix, the wheel speed meter estimation error and the predicted error covariance matrix, outputs the current state space vector, and resets the error state space vector and the current error covariance matrix of the current state space vector. Executing the scheme of the present application can provide stable and accurate positioning information for the driving device.

[0021] It should be understood that the content described in this section is not intended to identify the key or important features of the embodiments of the present application, nor is it intended to limit the scope of the present application. Other features of the present application will become easily understood through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] The accompanying drawings are used to better understand the present solution and do not constitute a limitation of the present application.

[0023] Figure 1A A first flow chart of a positioning method for a traveling device provided in Embodiment 1 of the present application;

[0024] Figure 1B A schematic diagram of a motion model of a traveling device provided in Embodiment 1 of the present application;

[0025] Figure 2 A second flow chart of a positioning method for a traveling device provided in Embodiment 2 of the present application;

[0026] Figure 3A A third flow chart of a positioning method for a traveling device provided in Embodiment 3 of the present application;

[0027] Figure 3B A schematic diagram of a local map update of a positioning method for a mobile device provided in Embodiment 3 of the present application;

[0028] Figure 3C A schematic diagram of the principle of a positioning method for a traveling device provided in Embodiment 2 of the present application;

[0029] Figure 4 A schematic diagram of the structure of a positioning device for a traveling device provided in Embodiment 4 of the present application;

[0030] Figure 5 It is a block diagram of an electronic device used to implement a positioning method for a traveling device according to an embodiment of the present application. DETAILED DESCRIPTION

[0031] In order to make the purpose, technical solution and advantages of the embodiments of the present application clearer, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0032] Embodiment 1

[0033] Figure 1A A first flow chart of a positioning method for a traveling device provided in Embodiment 1 of the present application; Figure 1B A schematic diagram of the motion model of a traveling device provided in Example 1 of the present application. This embodiment is applicable to the situation where a traveling device is positioned in real time during its operation. A method for positioning a traveling device provided in this embodiment can be executed by a positioning device for a traveling device provided in an embodiment of the present application, and the device can be implemented by software and / or hardware and integrated in an electronic device that executes the method. In this embodiment, the electronic device can be a server device or a traveling device. If it is a traveling device, it can be an intelligent robot, an unmanned vehicle, etc.

[0034] See also Figure 1A The method of this embodiment includes but is not limited to the following steps:

[0035] S110 , determining a predicted state space vector and a predicted error covariance matrix of the traveling device according to the state data of the traveling device collected by the combined inertial navigation and the initial filter parameters.

[0036] Among them, the combined inertial navigation (Inertial Measurement Unit, IMU) is also called an inertial measurement unit, which includes at least an accelerometer and a gyroscope. The state data includes acceleration information and angular velocity information, wherein the acceleration information is collected by the accelerometer, and the angular velocity information is collected by the gyroscope. The filter of the embodiment of the present application can be a Kalman filter. Optionally, the Kalman filter can be a traditional Kalman filter or an error state Kalman filter. Preferably, the Kalman filter adopts an error state Kalman filter, and the error state Kalman filter can effectively avoid the shortcomings of parameter redundancy and singularity of the covariance matrix in the Kalman filter. The initial filter parameters refer to the initial parameters set for the Kalman filter, including initial positioning information (i.e., the initial posture information of the driving device), the initial error covariance matrix, the white noise variance of the accelerometer, the white noise variance of the gyroscope, the speed random walk variance, and the angle random walk variance.

[0037] In the embodiment of the present application, the predicted state space vector includes two parts: the general state space vector X and the error state space vector δX. The general state space vector X includes position P, velocity v, attitude (i.e., angle information) q, acceleration deviation a b and angular velocity deviation w b ; The error state space vector δX includes position error δP, velocity error δv, attitude error δq, acceleration deviation error δa b And angular velocity deviation error δw b As shown in the following formulas (1)-(2):

[0038] X=[P,v,q,a b ,w b ] T (1);

[0039] δX=[δP,δv,δq,δa b ,δw b ] T (2);

[0040] Specifically, formula (1) can be expressed by the following formula (3), and the general state space vector X in the predicted state space vector of the driving device can be predicted based on the following formula (3), that is, (P t , v t ,q t , a b(t) , w b(t) ).

[0041]

[0042] Among them, P t is the current position; Pt-1 is the position at the previous moment; v t-1 is the speed at the previous moment; Δt is the time interval between two adjacent data collections; R is the current posture rotation matrix from the previous moment to the current moment; a m is the current acceleration collected by the accelerometer in the combined inertial navigation; a b(t-1) is the acceleration deviation at the previous moment; g is the acceleration due to gravity; v t is the velocity at the current moment; q t is the posture at the current moment; q t-1 is the posture at the previous moment; m is the current angular velocity collected by the gyroscope in the combined inertial navigation system; w b(t) is the angular velocity deviation at the current moment; w b(t-1) is the angular velocity deviation at the previous moment; a b(t) is the acceleration deviation at the current moment.

[0043] Specifically, formula (2) can be expressed by the following formulas (4)-(5), and the error state space vector δX in the predicted state space vector of the driving device can be predicted based on the following formulas (4)-(5), that is, (δP, δv, δq, δa b(t) , δw b(t) ).

[0044]

[0045]

[0046] Among them, δP t is the position error at the current moment; δP t-1 is the position error at the previous moment; δv t-1 is the velocity error at the previous moment; Δt is the time interval between two adjacent data collections; δv t is the velocity error at the current moment; R is the current posture rotation matrix from the previous moment to the current moment; a m is the current acceleration collected by the accelerometer in the combined inertial navigation; a b(t-1) is the acceleration deviation at the previous moment; δq t-1 is the attitude error at the previous moment; δq t is the attitude error at the current moment; w m is the current angular velocity collected by the gyroscope in the combined inertial navigation system; w b(t-1) is the angular velocity deviation at the previous moment; δa b(t) is the acceleration deviation error at the current moment; δa b(t-1) is the acceleration deviation error at the previous moment; δw b(t) is the angular velocity deviation error at the current moment; δw b(t-1)is the angular velocity deviation error at the previous moment; V i is the accelerometer white noise; θ i Gyroscope white noise; A i is the random walk noise of the accelerometer; Ω i is the gyroscope random walk noise; I is the identity matrix; σ 2 a n is the white noise variance of the accelerometer, σ 2 w n is the white noise variance of the gyroscope, σ 2 a w is the velocity random walk variance and σ 2 w w is the angle random walk variance.

[0047] In the embodiment of the present application, the prediction error covariance matrix corresponding to the prediction state space vector can be calculated by the following (6)-(9):

[0048]

[0049]

[0050]

[0051]

[0052] Among them, F x is the state transfer matrix; I3 is the 3×3 unit matrix; Δt is the time interval between two adjacent data collections; R is the current posture rotation matrix from the previous moment to the current moment; a m is the current acceleration collected by the accelerometer in the combined inertial navigation; a b is the acceleration deviation; w m is the current angular velocity collected by the gyroscope in the combined inertial navigation system; w b is the angular velocity deviation; F i is the perturbation matrix; Q i is the noise matrix; σ 2 a n is the white noise variance of the accelerometer, σ 2 w n is the white noise variance of the gyroscope, σ 2 a w is the velocity random walk variance and σ 2 w w is the angle random walk variance; S is the current prediction error covariance matrix; S 上 is the previous forecast error covariance matrix.

[0053] S120. According to the general state space vector of the predicted state space vector, iteratively match the prior map data and the point cloud data collected by the laser radar, and determine the first position information and the observation noise matrix of the driving device according to the iterative matching result.

[0054] The driving device of the embodiment of the present application is also equipped with a laser radar, which is used to scan the point cloud data of the surrounding environment during the driving process of the driving device. The so-called prior map data is a high-precision environment map that has been constructed, and the prior map data can be pre-stored in the electronic device (such as the driving device).

[0055] Optionally, the embodiment of the present application uses a Normal Distance Transform (NDT) algorithm for iterative matching. In some embodiments, other iterative matching algorithms may also be used, which are not limited in the embodiment of the present application. This step iteratively matches the prior map data and the point cloud data collected by the lidar according to the general state space vector of the predicted state space vector, and determines the first position information of the driving device and the specific process of the observation noise matrix according to the iterative matching results. The following sub-steps can be implemented:

[0056] S1201. Convert the pose information in the general state space vector of the predicted state space vector from the inertial navigation coordinate system to the radar coordinate system, and use the pose information in the general state space vector of the predicted state space vector converted to the radar coordinate system as the initial pose matrix.

[0057] Specifically, before iteratively matching the prior map data and the point cloud data collected by the lidar, this substep needs to convert the pose information in the general state space vector of the predicted state space vector from the inertial navigation coordinate system to the radar coordinate system. The coordinate system conversion formula (10) is as follows:

[0058]

[0059] Among them, F I is the position and attitude information in the general state space vector of the predicted state space vector in the inertial navigation coordinate system, including position information and attitude information; L The position and posture information in the general state space vector of the predicted state space vector in the radar coordinate system, including position information and attitude information; is the transformation matrix from the inertial navigation coordinate system to the lidar coordinate system.

[0060] Furthermore, the pose information F in the general state space vector of the predicted state space vector converted to the radar coordinate system is L As the initial pose matrix, it should be noted that the initial pose matrix is ​​the predicted value (ie, initial value) of the first iterative matching.

[0061] S1202. Based on the normal distribution transformation algorithm and the initial pose matrix, perform a preset number of iterative matching on the prior map data and the point cloud data collected by the lidar to obtain a matching score corresponding to each iterative matching, and a target pose matrix associated with the matching score.

[0062] Among them, the embodiment of the present application can use the sum of the probability density matches of each position point obtained after matching the prior map data and each position point in the point cloud data collected by the lidar as the matching score of this iterative matching.

[0063] In a specific embodiment of the present application, the initial pose matrix is ​​substituted into the following probability density sum calculation formula (11), and the matching of the prior map data and the point cloud data is calculated based on the point cloud data and the prior map data collected by the laser radar. This embodiment requires a preset number of iterative matching calculations. During the multiple iterative matching processes, if the pose matrix can make the point cloud data match the prior map data well, then the matching score calculated based on formula (11) will be very large.

[0064] Specifically, assume that the point cloud data collected by the laser radar is A = {a0, ..., an}, where A is a three-dimensional point cloud data containing n points; the prior map data is B = {b0, ..., bn}, where B is the position coordinates of n points. First, the NDT algorithm is used to construct the prior map data B into a normal distribution of multidimensional variables. Secondly, based on the probability density sum calculation formula, based on the initial pose matrix (i.e., the predicted value of the iterative matching) and the preset number of iterations and iteration step, the prior map data and the point cloud data are iteratively matched for a preset number of times to obtain the iterative matching result corresponding to each iterative matching, i.e., the matching score and the target pose matrix associated with the matching score. Optionally, the NDT algorithm can use Newton optimization algorithm, least squares method, etc. to iteratively match the prior map data and point cloud data for a preset number of times. The matching score is calculated as follows:

[0065]

[0066]

[0067]

[0068] a i ′=T(a i ,F L ) (14);

[0069] Among them, score is the matching score, that is, the sum of probability density; a i′ is the coordinate value of the i-th point cloud in the point cloud data in the map coordinate system; d is the mean of the prior map data; n is the number of point clouds in the point cloud data (i.e., the number of location points in the prior map data); ∑ is the variance of the prior map data; b i is the coordinate value of the i-th point in the prior map data; a i is the coordinate value of the i-th point cloud in the point cloud data in the laser radar coordinate system; F L is the target pose matrix of the current iteration match (i.e., the predicted value of the next iteration match), where F L is the initial pose matrix; T is the conversion operator from the radar coordinate system to the map coordinate system. It should be noted that each time this sub-step performs iterative matching based on formula (11), a set of probability density sums (i.e., matching scores) and the target pose matrix F associated with the matching scores can be calculated. L .

[0070] S1203: Use the target pose matrix associated with the highest matching score in the iterative matching result as the first pose information of the driving device.

[0071] Specifically, the above steps can calculate a set of matching scores and the target pose matrix F associated with the matching scores based on each iterative matching. L This substep determines the score with the highest matching score from the matching scores obtained from multiple matching iterations. At this point, the matching between the prior map data and the point cloud data is the best. The target pose matrix F corresponding to the highest matching score is L As the first pose information.

[0072] S1204: Determine the matching confidence level according to the highest matching score in the iterative matching results.

[0073] In a specific embodiment of the present application, there may be a certain matching error when the point cloud data collected by the radar is iteratively matched with the prior map data. When determining the observation noise matrix corresponding to the first pose information obtained by iterative matching, it is necessary to first calculate the matching confidence based on the following formula (15), where the matching confidence represents the credibility of the positioning using the laser radar at the current moment.

[0074] P belief =1 / score 高 ×I 6×6 (15);

[0075] Among them, P belief Indicates the matching confidence; I 6×6 Represents the 6×6 identity matrix; score 高The highest matching score among the matching scores obtained by performing iterative matching for a preset number of times according to the above formula (11).

[0076] S1205. Determine an observation noise matrix according to the matching confidence and the rotation matrix.

[0077] In an embodiment of the present application, the observation noise matrix of the first pose information can be calculated based on the following formula (16)-formula (17).

[0078]

[0079]

[0080] Among them, R 3×3 represents the rotation matrix; R cur The rotation transformation matrix representing the first pose information; V lidar Represents the observation noise matrix corresponding to the first pose information.

[0081] S130: Determine the second position information and the measured speed information of the traveling device according to the pulse data collected by the wheel speed meter, and determine the estimation error of the wheel speed meter.

[0082] The vehicle in the embodiment of the present application is equipped with a wheel speed meter, which collects data in real time or at regular intervals during the operation of the vehicle. The pulse data collected by the wheel speed meter refers to the number of pulses per unit time. The motion model of the vehicle is a model built for the vehicle based on the motion principle of the vehicle, for example, Figure 1B The motion model of the traveling device shown in the figure, wherein Px and Py are the position coordinates of the traveling device in the horizontal direction of the X-axis and the Y-axis respectively. The second posture information refers to the current position information and posture information of the traveling device determined based on the data collected by the wheel speed meter, and the measured speed information refers to the speed of the traveling device in the X-axis and Y-axis directions determined based on the data collected by the wheel speed meter.

[0083] Optionally, the specific process of determining the second posture information and the measured speed information of the traveling device and determining the estimation error of the wheel speed meter according to the pulse data collected by the wheel speed meter in this step can be implemented by the following sub-steps:

[0084] S1301. Determine the position increment of the traveling device at the current moment relative to the previous moment and the wheel speed information at the current moment according to the pulse data collected by the wheel speed meter.

[0085] Among them, the wheel speed meter is a kind of wheel speed sensor, and the wheel speed meter includes a wheel speed meter encoder, and the wheel speed meter is used to measure the wheel speed of the target wheel of the traveling device. Specifically, the wheel speed meter is used to sense the rotation of the target wheel of the traveling device and output a voltage signal, so that the processing module of the traveling device processes the voltage signal and obtains corresponding pulse data.

[0086] In this embodiment, the traveling device is provided with an angle sensor, which is installed on the driving wheel and is used to collect the angle between the central axis of the traveling device and the X-axis direction. The wheel speed information of the traveling device at the current moment is determined by the pulse data collected by the wheel speed meter, wherein the wheel speed information includes the wheel speed of the left wheel and the wheel speed of the right wheel. In some embodiments, assuming that the radii of all left wheels and all right wheels of the traveling device are respectively the same, the step of determining the wheel speed information of the traveling device includes: calculating the wheel speed of the left wheel according to the pulse data collected by the wheel speed meter and the radius of the left wheel; calculating the wheel speed of the right wheel according to the pulse data collected by the wheel speed meter and the radius of the right wheel.

[0087] In this embodiment, the process of determining the position increment of the traveling device at the current moment relative to the previous moment is as follows: based on the working principle of the wheel speed meter and in combination with the motion model of the traveling device, the following formula (18) is used to calculate the position increment of the traveling device at the current moment relative to the previous moment based on the pulse data collected by the wheel speed meter.

[0088]

[0089] Where ΔD is the position increment of the current time relative to the previous time (i.e., within the adjacent time interval); Q is the wheel radius of the traveling device; R e is the resolution of the wheel speed meter encoder of the traveling device; ΔN is the pulse data collected by the wheel speed meter, that is, the number of pulses per unit time; α is the angle between the central axis of the traveling device and the X-axis direction.

[0090] S1302: Determine the angle increment of the traveling device at the current moment relative to the previous moment according to the wheel speed information at the current moment.

[0091] In the embodiment of the present application, after the above steps, the wheel speed information of the traveling device at the current moment is determined by the pulse data collected by the wheel speed meter, wherein the wheel speed information includes the wheel speed V of the right wheel. R and the left wheel speed V L Then, according to the wheel speed information at the current moment, the angle increment of the driving device at the current moment relative to the previous moment is calculated using the following formula (19).

[0092]

[0093] Where Δq represents the angle increment of the driving device at the current moment relative to the previous moment; V RIndicates the right wheel speed of the traveling equipment; V L represents the left wheel speed of the traveling device; E represents the wheelbase between the left and right wheels; Δt represents the time interval between two adjacent data collections.

[0094] S1303: Based on the motion model of the traveling device, and according to the posture information, position increment, and angle increment of the traveling device at the previous moment, determine the second posture information of the traveling device at the current moment.

[0095] In the embodiment of the present application, based on the motion model of the driving device, the following formula (20) can be used to obtain the current posture information of the driving device on the two-dimensional horizontal plane.

[0096]

[0097] Among them, (x t ,y t ,q t ) is the current position information of the driving device on the two-dimensional horizontal plane; (x t-1 ,y t-1 ,q t-1 ) is the posture information of the driving device on the two-dimensional horizontal plane at the previous moment; Δq is the angle increment of the driving device at the current moment relative to the previous moment (i.e. within the adjacent time interval); ΔD is the position increment of the driving device at the current moment relative to the previous moment (i.e. within the adjacent time interval).

[0098] It should be noted that what is calculated by the above formula (20) is the posture information of the traveling device in the two-dimensional horizontal plane at the current moment, that is, there is no posture information in the Z-axis direction. However, since the second posture information should include posture information in the three directions of the X-axis, Y-axis and Z-axis. Therefore, the embodiment of the present application needs to add the posture information in the Z-axis direction on the basis of the calculation result of the above formula (20) to obtain the second posture information of the traveling device at the current moment. Optionally, since the height information of the traveling device does not change much during the driving process, the posture information of the traveling device in the Z-axis direction at the current moment can use the posture information of the traveling device in the Z-axis direction at the previous moment, that is, on the basis of the calculation result of the above formula (20), add the posture information of the traveling device in the Z-axis direction at the previous moment, and you can get the second posture information of the traveling device at the current moment.

[0099] S1304: Determine the measured speed information of the traveling device at the current moment according to the wheel speed information and the angle increment at the current moment.

[0100] After the above steps, the measured speed information of the traveling device at the current moment is determined according to the wheel speed information of the traveling device at the current moment and the angle increment of the traveling device at the current moment relative to the previous moment. Specifically, the process of determining the measured speed information of the traveling device at the current moment is as follows: during the operation of the traveling device, the wheel speed information of the traveling device, i.e., the wheel speed information of the left and right wheels of the traveling device, is collected in real time or at regular intervals by a wheel speed meter. Based on the collected wheel speed information of the left and right wheels of the traveling device, the measured speed information of the current traveling device in the X-axis direction and the Y-axis direction is calculated, and the calculation formulas (21)-(22) of the measured speed information are as follows:

[0101]

[0102]

[0103] Among them, V R is the wheel speed information of the right wheel of the driving device at the current moment; V L is the wheel speed information of the left wheel of the driving device at the current moment; Δq is the angle increment of the driving device at the current moment relative to the previous moment; V x V is the measured speed information of the driving device in the X-axis direction at the current moment; y It is the measured speed information of the driving device in the Y-axis direction at the current moment.

[0104] It should be noted that the speed information calculated by the above formulas (21) and (22) is the measured speed information of the driving device in the two-dimensional horizontal plane at the current moment, that is, there is no measured speed information in the Z-axis direction. At this time, similar to adding the Z-axis direction posture information, the measured speed information of the driving device in the Z-axis direction at the previous moment can be used as the measured speed information in the Z-axis direction at the current moment. That is, V calculated by the above formulas (21) and (22) x 、V y , and the measured speed information of the traveling device in the Z-axis direction at the previous moment are collectively referred to as the measured speed information of the traveling device.

[0105] S1305: Determine a wheel speed meter estimation error according to an observed noise matrix of the driving device at a previous moment and a preset noise value.

[0106] In the embodiment of the present application, the calculation method of the wheel speed meter estimation error corresponding to the second posture information can be: when the driving device is positioned for the first time, the wheel speed meter estimation error P2 can be the observation noise matrix V calculated based on the above formula (17) during the first map matching. lidar , that is, P2 = V lidar ; When the driving device is not positioned for the first time (such as the second or third time), the observation noise matrix V determined in the last positioning can be lidarBased on , the preset noise value, such as the preset Gaussian white noise, is iterated to obtain the wheel speed meter estimation error P2 of this positioning. For example, when the driving device is positioned for the second time, the observation noise matrix V determined in the previous positioning can be lidar Based on this, the preset Gaussian white noise is iterated to obtain the wheel speed meter estimation error P2 for this positioning.

[0107] S140. Determine the current state space vector and the current error covariance matrix of the driving device according to the first posture information, the second posture information, the measured speed information, the observation noise matrix, the wheel speed meter estimation error and the prediction error covariance matrix, output the current state space vector, and reset the error state space vector and the current error covariance matrix of the current state space vector.

[0108] In an embodiment of the present application, this step can be implemented in four sub-steps: First, pre-process the first posture information, the second posture information, the measured speed information, the observation noise matrix, and the wheel speed meter estimation error to obtain the observation information and observation error of the traveling device. Second, determine the current state space vector of the traveling device based on the observation information, the observation error, and the prediction error covariance matrix. Third, determine the current error covariance matrix based on the observation error and the prediction error covariance matrix. Fourth, output the current state space vector, and reset the error state space vector and the current error covariance matrix of the current state space vector. It should be noted that the specific implementation process of these four sub-steps will be specifically explained in the following embodiments.

[0109] The technical solution provided in this embodiment determines the predicted state space vector and the predicted error covariance matrix of the driving device according to the state data of the driving device collected by the combined inertial navigation and the initial filter parameters; wherein the predicted state space vector includes a general state space vector and an error state space vector; according to the general state space vector of the predicted state space vector, the prior map data and the point cloud data collected by the laser radar are iteratively matched, and according to the iterative matching result, the first pose information and the observation noise matrix of the driving device are determined; according to the pulse data collected by the wheel speed meter, the second pose information and the measured speed information of the driving device are determined, and the wheel speed meter estimation error is determined; according to the first pose information, the second pose information, the measured speed information, the observation noise matrix, the wheel speed meter estimation error and the predicted error covariance matrix, the current state space vector and the current error covariance matrix of the driving device are determined, the current state space vector is output, and the error state space vector and the current error covariance matrix of the current state space vector are reset. The present application scheme utilizes the different characteristics of the combined inertial navigation and radar sensors to complement each other, and uses multi-sensor fusion technology for combined positioning, which solves the problem of uncertainty and low reliability of data using a single sensor in the prior art. Executing the solution of this application can provide stable and accurate positioning information for the traveling equipment.

[0110] Embodiment 2

[0111] Figure 2 A second flow chart of a method for positioning a traveling device provided in the second embodiment of the present application. The embodiment of the present application is optimized on the basis of the above embodiment, and the specific optimization is as follows: a detailed explanation is added on the process of determining the current state space vector and the current error covariance matrix of the traveling device, and the process of resetting the error state space vector of the current state space vector and the current error covariance matrix.

[0112] See also Figure 2 The method of this embodiment includes but is not limited to the following steps:

[0113] S201, determining a predicted state space vector and a predicted error covariance matrix of the traveling device according to the state data of the traveling device collected by the combined inertial navigation and the initial filter parameters.

[0114] S202. According to the general state space vector of the predicted state space vector, iteratively match the prior map data and the point cloud data collected by the laser radar, and determine the first position information and the observation noise matrix of the driving device according to the iterative matching result.

[0115] S203: Determine the second position information and the measured speed information of the traveling device according to the pulse data collected by the wheel speed meter, and determine the estimation error of the wheel speed meter.

[0116] S204, determine the relationship between the observation noise matrix and the first preset threshold and the second preset threshold. If the observation noise matrix is ​​greater than or equal to the first preset threshold, execute S205; if the observation noise matrix is ​​less than or equal to the second preset threshold, execute S206; if the observation noise matrix is ​​greater than the second preset threshold and less than the first preset threshold, execute S207.

[0117] Among them, the first preset threshold is greater than the second preset threshold.

[0118] In the embodiment of the present application, when the point cloud data collected by the radar is iteratively matched with the prior map data (i.e., map matching), mismatching may occur. If a matching error occurs, that is, the observation noise matrix V lidar If the observation noise matrix V is large, the fusion processing cannot be performed based on the first pose information. Therefore, it is necessary to determine lidar The relationship between the first preset threshold and the second preset threshold.

[0119] S205. Use the second posture information and the measured speed information as observation information, and use the wheel speed meter estimation error as observation error.

[0120] The observation information includes the position information (i.e., position information and posture information) and the measured speed information of the driving device. The observation error refers to the error information corresponding to the observation information.

[0121] In an embodiment of the present application, if the observation noise matrix satisfies the following formula (23) (that is, the observation noise matrix is ​​greater than or equal to the first preset threshold), it indicates that the first posture information obtained by iterative matching is inaccurate, then the second posture information and measured speed information determined according to the pulse data collected by the speedometer are used as the observation information of the driving device, and the speedometer estimation error is used as the observation error.

[0122] V lidar ≥V thre1 (twenty three);

[0123] Among them, V lidar is the observation noise matrix; V thre1 is the first preset threshold.

[0124] S206: Use the first position information and the measured velocity information as observation information, and use the observation noise matrix as observation error.

[0125] In an embodiment of the present application, if the observation noise matrix satisfies the following formula (24) (that is, the observation noise matrix is ​​less than or equal to the second preset threshold), it indicates that the iterative matching result at this time is relatively correct, and the first position information determined by the result of iterative matching of the prior map data and the point cloud data collected by the lidar and the measurement speed information determined by the pulse data collected by the wheel speed meter are used as the observation information of the driving device; the observation noise matrix determined by the result of iterative matching of the prior map data and the point cloud data collected by the lidar is used as the observation error of the driving device.

[0126] V lidar ≤V thre2 (twenty four);

[0127] Among them, V lidar is the observation noise matrix; V thre2 is the second preset threshold.

[0128] S207, determining the observation error according to the observation noise matrix and the wheel speed meter estimation error, and determining the observation information according to the first posture information, the second posture information, the measurement speed information, the observation error, the observation noise matrix and the wheel speed meter estimation error.

[0129] In an embodiment of the present application, if the observation noise matrix satisfies the following formula (25) (the observation noise matrix is ​​greater than the second preset threshold and less than the first preset threshold, where the first preset threshold is greater than the second preset threshold), it is necessary to determine the observation information and observation error of the driving device through fusion processing.

[0130] V thre2 <V lidar <V thre1 (25);

[0131] Among them, V lidar is the observation noise matrix; V thre1 is the first preset threshold; V thre2 is the second preset threshold.

[0132] Optionally, the observation error of the driving device can be determined based on the observation noise matrix and the wheel speed meter estimation error using the following formula (26).

[0133] P g =(V lidar -1 +P2 -1 ) -1 (26);

[0134] Among them, P g is the observation error; V lidaris the observation noise matrix corresponding to the first pose information; P2 represents the wheel speed meter estimation error corresponding to the second pose information.

[0135] Optionally, the specific process of determining the observation information according to the first pose information, the second pose information, the measured speed information, the observation error, the observation noise matrix and the wheel speed meter estimation error in this step can be implemented by the following sub-steps:

[0136] S2071. Perform weighted calculation on the first pose information and the second pose information according to the observation error, the observation noise matrix and the wheel speed meter estimation error to obtain the observed pose.

[0137] In the embodiment of the present application, the observation posture of the driving device can be determined using the following formula (27).

[0138] X g =P g *(V lidar -1 *X1+P2 -1 *X2) (27);

[0139] Among them, X1 represents the first pose information; X2 represents the second pose information; X g represents the observed posture; P2 represents the wheel speed meter estimation error corresponding to the second posture information; V lidar is the observation noise matrix corresponding to the first pose information; P g is the observation error.

[0140] S2072. Determine the observation information according to the observation posture and measurement speed information.

[0141] In the embodiment of the present application, the observation posture X of the driving device is obtained through step S2071. g , which includes position information and posture information. The measured speed information determined by the pulse data collected by the wheel speed meter and the observed posture X g Merge as observation information of driving equipment.

[0142] S208. Determine the error state space vector of the current state space vector according to the observation information, the observation error and the prediction error covariance matrix.

[0143] In an embodiment of the present application, observation information, observation error, prediction error covariance matrix and observation noise matrix can be input into the error state Kalman filter. Based on the working principle of the error state Kalman filter, the error state space vector can be obtained by combining the following formulas (28)-(30).

[0144] y=h(X t )+P g (28);

[0145] K=SH T (HSH T +P g ) -1 (29);

[0146] δX=K(yh(X t ))+P g (30);

[0147] Where y is the observation information (including position information, attitude information and measurement speed information); P g is the observation error; h(X t ) is the observation equation related to the state quantity; K is the gain matrix; S is the prediction error covariance matrix; H is the observation equation of the Jacobian matrix; δX is the error state space vector, where δX includes position error, velocity error, attitude angle error, acceleration deviation error and angular velocity deviation error.

[0148] S209 . Determine and output a general state space vector of the current state space vector of the traveling device according to the error state space vector of the current state space vector and the general state space vector of the state space vector of the traveling device at a previous moment.

[0149] The so-called general state space vector of the state space vector at the previous moment is the general state space vector (including position information, velocity information, attitude information, acceleration deviation information and angular velocity deviation information) determined by the driving device at the last positioning. This substep determines the general state space vector of the current state space vector at this positioning based on the error state space vector of the current state space vector and the general state space vector of the state space vector at the previous moment of the driving device.

[0150] In the embodiment of the present application, the general state space vector of the current state space vector can be calculated by formula (31):

[0151]

[0152] Among them, (P t , v t ,q t , a b(t) , w b(t) ) is the general state space vector of the current state space vector; (P t-1 , v t-1 ,q t-1 , a b(t-1) , w b(t-1) ) is the general state space vector of the state space vector at the previous moment; δP is the position error; δv is the velocity error; δq is the attitude angle error; δab is the acceleration deviation error; δw b is the angular velocity deviation error.

[0153] S210, determining a current error covariance matrix according to the prediction error covariance matrix and the observation noise matrix.

[0154] In the embodiment of the present application, after the general state space vector of the current state space vector is obtained through the above steps, the current error covariance matrix can be calculated by formula (32). Formula (32) is as follows:

[0155] S=(I-KH)S 上 (I-KH) T +KP g K T (32);

[0156] Among them, S is the current prediction error covariance matrix; S 上 is the covariance matrix of the last current prediction error; I is the identity matrix; K is the gain matrix; H is the observation equation of the Jacobian matrix; P g is the observation error.

[0157] S211. Reset the error state space vector of the current state space vector to zero.

[0158] In the embodiment of the present application, after determining the current state space vector and the current error covariance matrix of the driving device, it is necessary to reset the error state space vector of the current state space vector to zero. The advantage of this setting is that it can be used to calculate the location of the driving device at the next moment, making the state space vector of the driving device more accurate.

[0159] S212. Determine a reset matrix according to the attitude angle error in the unit matrix and the error state space vector of the predicted state space vector.

[0160] In an embodiment of the present application, the reset matrix is ​​determined by formula (33) based on the attitude angle error in the error state space vector of the unit matrix and the predicted state space vector.

[0161]

[0162] Wherein, δq is the attitude angle error in the error state space vector of the predicted state space vector; I6 is the 6×6 unit matrix; I is the unit matrix; and G is the reset matrix calculated by formula (33).

[0163] S213. Reset the current error covariance matrix according to the reset matrix.

[0164] In an embodiment of the present application, the reset matrix obtained according to formula (33) can be used to reset the current error covariance matrix through formula (34).

[0165] S′=GSG T (34);

[0166] Among them, S is the current error covariance matrix; S' is the reset error covariance matrix.

[0167] The technical solution provided by the present embodiment determines the relationship between the observation noise matrix and the first preset threshold and the second preset threshold, and obtains the observation information and the observation error by subdividing the types respectively; determines the error state space vector according to the observation information, the observation error and the prediction error covariance matrix; determines and outputs the general state space vector of the driving device according to the error state space vector and the general state space vector of the state space vector of the driving device at the previous moment; determines the current error covariance matrix according to the prediction error covariance matrix and the observation noise matrix, and resets the error state space vector to zero; resets the current error covariance matrix according to the unit matrix and the attitude angle error in the error state space vector. The solution of the embodiment of the present application determines the current state space vector and the current error covariance matrix of the driving device through the observation information and the observation error, and resets the error state space vector and the current error covariance matrix, obtains the precise positioning data of the driving device through the fusion algorithm, and further improves the accuracy of the autonomous positioning of the driving device.

[0168] Embodiment 3

[0169] Figure 3A A third flow chart of a positioning method for a traveling device provided in Embodiment 3 of the present application; Figure 3B A schematic diagram of a local map update of a positioning method for a mobile device provided in Embodiment 3 of the present application; Figure 3C A schematic diagram of the principle of a positioning method for a driving device provided in the second embodiment of the present application. The embodiment of the present application is optimized on the basis of the above embodiment, and the specific optimization is: adding the initialization process of the driving device and the detailed explanation of the updating process of the prior map.

[0170] refer to Figure 3A The method of this embodiment includes but is not limited to the following steps:

[0171] S310: Setting initial position information of the driving device according to the prior map data and the point cloud data collected by the laser radar.

[0172] The so-called initial posture information is an initial posture value set for the traveling device during the initialization phase of the traveling device. Optionally, the initial posture information is used as the posture information when the traveling device is first positioned.

[0173] Optionally, there are many ways to determine the initial posture information of the traveling device, which are not limited in this embodiment. For example, Method 1: Compare the prior map data with the point cloud data collected by the laser radar, parse the position of the point cloud data in the current map, and set the position as the initial posture information of the traveling device. Method 2: On the basis of the rough positioning of the initial posture information of the traveling device in Method 1 above, based on the prior map data and the laser radar data, an iterative matching algorithm (such as the NDT algorithm) is used to set the precise initial posture information of the traveling device. It should be noted that this step uses an iterative matching algorithm to calculate the initial posture information of the traveling device, which is similar to the implementation process of using the iterative matching algorithm to determine the first posture information of the traveling device in the above embodiment. The specific iterative matching process has been described in detail in the above embodiment and will not be described here.

[0174] S320, initializing according to the initial posture information and the inertial navigation parameters to obtain initial filter parameters.

[0175] The inertial navigation parameters may be the relevant noise parameter values ​​of the combined inertial navigation recorded in the inertial navigation parameter manual, such as the white noise variance σ of the accelerometer. 2 a n , the white noise variance of the gyroscope σ 2 w n , speed random walk variance σ 2 a w and the angular random walk variance σ 2 w w . In the embodiment of the present application, the initial filter parameters may include but are not limited to: initial positioning parameters, initial error covariance matrix and inertial navigation parameters. Specifically, the initial posture information of the driving device set in S310 can be used as the initial positioning parameters of the error state Kalman filter; the initial error covariance matrix of the error state Kalman filter is set to a preset value, preferably, the preset value can be a matrix with a larger value. At the same time, it is also necessary to set the inertial navigation parameters according to the combined inertial navigation parameter manual, that is, the white noise variance σ of the accelerometer 2 a n , the white noise variance of the gyroscope σ 2 w n , speed random walk variance σ 2 a w and the angular random walk variance σ 2 w w .

[0176] Preferably, before setting the initial posture information of the driving device, a judgment process of whether the driving device can receive data can be added. Specifically, if the driving device can receive all the data required for positioning (i.e., the status data collected by the combined inertial navigation, the point cloud data collected by the laser radar, and the wheel speed information collected by the wheel speed meter), the above-mentioned S310-S320 operations are performed, otherwise a maintenance request is generated.

[0177] S330, determining a predicted state space vector and a predicted error covariance matrix of the traveling device according to the state data of the traveling device collected by the combined inertial navigation and the initial filter parameters.

[0178] S340. According to the general state space vector of the predicted state space vector, iteratively match the prior map data and the point cloud data collected by the laser radar, and determine the first position information and the observation noise matrix of the driving device according to the iterative matching result.

[0179] S350: Determine the second position information and the measured speed information of the traveling device according to the pulse data collected by the wheel speed meter, and determine the estimation error of the wheel speed meter.

[0180] S360. Determine the current state space vector and the current error covariance matrix of the driving device according to the first posture information, the second posture information, the measured speed information, the observation noise matrix, the wheel speed meter estimation error and the prediction error covariance matrix, output the current state space vector, and reset the error state space vector and the current error covariance matrix of the current state space vector.

[0181] S370: If it is monitored that the driving distance of the driving device at the current moment reaches a preset distance relative to the last time a local map was constructed, a local map corresponding to the current moment is constructed based on the point cloud data collected by the laser radar.

[0182] In an embodiment of the present application, when the driving device is operating in a dynamic environment, radar map matching may be incorrect due to partial environmental changes. In addition, before the driving device operates, it is necessary to input the prior map data into the driving device. When the driving device may enter some scenes that are not in the prior map due to work needs, map matching cannot be performed at this time. In order to solve these problems, the embodiment of the present application can determine whether the driving distance at the current moment relative to the last time a local map was constructed has reached a preset distance after each positioning to determine the current position information. If the preset distance is reached, the prior map data needs to be updated. Before updating the prior map data, it is necessary to use the point cloud data scanned by the radar and the corresponding positioning information, and use a sliding window to maintain a fixed-size local map update size, that is, to build a real-time local map, that is, all local maps constructed are of the same size. Optionally, the preset distance can be two meters. For example Figure 3B As shown, the map data in the current environment is updated every two meters.

[0183] S380: Update the prior map data according to the local map corresponding to the current moment.

[0184] In the embodiment of the present application, the prior map is updated once according to the current positioning information and the local map. Specifically, the local map corresponding to the current moment is used as the standard, and the areas in the prior map that are inconsistent with the local map data corresponding to the current moment are replaced, or the areas that are not in the prior map are added according to the local map corresponding to the current moment, so as to obtain the updated prior map data.

[0185] It should be noted that the embodiment of the present application uses the updated prior map data as the map data for the current environment, and uses it in subsequent steps to iteratively match the prior map data and the point cloud data collected by the lidar, so as to achieve accurate positioning of the driving equipment in a dynamic or partially unknown environment.

[0186] For example, Figure 3CAs shown in the figure, it is assumed that the driving device is a mobile robot, which is equipped with a combined inertial navigation, a laser radar and a wheel speed meter. The state data of the mobile robot collected by the combined inertial navigation is first input into the state prediction module. The state prediction module can predict the predicted state space vector of the mobile robot and its corresponding prediction error covariance matrix based on the initial filter parameters of the error state Kalman filter; then the posture information in the general state space vector of the predicted state space vector predicted by the state prediction module is input into the iterative matching module. The iterative matching module will iteratively match the prior map data and the point cloud data collected by the laser radar based on the posture information to determine the first posture information and the observation noise matrix of the mobile robot; then the pulse data collected by the wheel speed meter is input into the speed measurement module. The block and the dead reckoning module determine the second posture information, measured speed information and wheel speed meter estimation error of the driving device; then the output results of the iterative matching module, the state prediction module, the speed measurement module and the dead reckoning module (i.e. the first posture information, the second posture information, the measured speed information, the observation noise matrix, the wheel speed meter estimation error and the prediction error covariance matrix) are transmitted to the update fusion module, which calculates the current state space vector and the current error covariance matrix of the mobile robot, and feeds the calculation results back to the state prediction module, so that the state prediction module resets the error state space vector and the current error covariance matrix of the current state space vector, so as to accurately determine the predicted state space vector and its corresponding predicted error covariance matrix in the next positioning. The general state space vector in the current state space vector is used as the state output. The embodiment of the present application can also determine whether the travel distance of the mobile robot at the current moment relative to the last time the local map was constructed has reached the preset distance based on the position information in the current state space vector of the mobile robot calculated by the update fusion module. If the preset distance is reached, the prior map data needs to be updated by the map update module. The next time the iterative matching module is used, iterative matching will be performed based on the updated prior map and the point cloud data collected by the laser radar. In addition, the embodiment of the present application will also update the measured speed information of the Z-axis direction at the current moment in the current state space vector of the mobile robot calculated by the fusion module as the measured speed information of the Z-axis direction at the next moment.

[0187] The technical solution provided in this embodiment sets the initial posture information of the driving device according to the prior map data and the point cloud data collected by the laser radar, and initializes the filter parameters, and then determines the predicted state space vector of the driving device and its corresponding prediction error covariance matrix based on the initialized initial filter parameters and the state data of the driving device collected by the combined inertial navigation; when the driving distance of the driving device reaches a preset distance, a local map is constructed according to the point cloud data collected by the laser radar, and the prior map is updated; according to the posture information in the general state space vector of the predicted state space vector, the updated prior map data and the point cloud data collected by the laser radar are iteratively matched to determine the first posture information and the observation noise matrix of the driving device; according to the pulse data collected by the wheel speed meter, the second posture information of the driving device, the measured speed information and the wheel speed meter estimation error are determined. Finally, according to the first pose information, the second pose information, the measured speed information, the observation noise matrix, the wheel speed meter estimation error and the prediction error covariance matrix, the current state space vector and the current error covariance matrix of the driving device are determined, the current state space vector is output, and the error state space vector and the current error covariance matrix of the current state space vector are reset for the next positioning. By accurately initializing the filter parameters and the initial positioning parameters, and updating the prior map data, the accuracy of the autonomous positioning of the driving device is greatly improved.

[0188] Embodiment 4

[0189] Figure 4 A schematic diagram of a positioning device for a traveling device provided in an embodiment of the present application is shown in FIG. Figure 4 As shown, the apparatus 400 may include:

[0190] The prediction module 410 is used to determine the predicted state space vector and the predicted error covariance matrix of the driving device according to the state data of the driving device collected by the combined inertial navigation and the initial filter parameters; wherein the predicted state space vector includes a general state space vector and an error state space vector.

[0191] The matching module 420 is used to iteratively match the prior map data and the point cloud data collected by the lidar according to the general state space vector of the predicted state space vector, and determine the first position information and observation noise matrix of the driving device according to the iterative matching result.

[0192] The first determination module 430 is used to determine the second posture information and the measured speed information of the traveling device according to the pulse data collected by the wheel speed meter, and determine the estimation error of the wheel speed meter.

[0193] The second determination module 440 is used to determine the current state space vector and the current error covariance matrix of the driving device according to the first posture information, the second posture information, the measured speed information, the observation noise matrix, the wheel speed meter estimation error and the prediction error covariance matrix, output the current state space vector, and reset the error state space vector and the current error covariance matrix of the current state space vector.

[0194] Furthermore, the above-mentioned matching module 420 can be specifically used to: convert the posture information in the general state space vector of the predicted state space vector from the inertial navigation coordinate system to the radar coordinate system, and use the posture information in the general state space vector of the predicted state space vector converted to the radar coordinate system as the initial posture matrix; based on the normal distribution transformation algorithm and the initial posture matrix, perform a preset number of iterative matching on the prior map data and the point cloud data collected by the lidar to obtain the matching score corresponding to each iterative matching, and the target posture matrix associated with the matching score.

[0195] Furthermore, the above-mentioned matching module 420 can also be specifically used to: use the target posture matrix associated with the highest matching score in the iterative matching result as the first posture information of the driving device; determine the matching confidence based on the highest matching score in the iterative matching result; determine the observation noise matrix based on the matching confidence and the rotation matrix.

[0196] Furthermore, the above-mentioned first determination module 430 can be specifically used to: determine the position increment of the traveling device at the current moment relative to the previous moment and the wheel speed information at the current moment based on the pulse data collected by the wheel speed meter; determine the angle increment of the traveling device at the current moment relative to the previous moment based on the wheel speed information at the current moment; based on the motion model of the traveling device, determine the second posture information of the traveling device at the current moment according to the posture information of the traveling device at the previous moment, the position increment and the angle increment; determine the measured speed information of the traveling device at the current moment according to the wheel speed information and the angle increment at the current moment; determine the estimation error of the wheel speed meter according to the observed noise matrix of the traveling device at the previous moment and the preset noise value.

[0197] Furthermore, the second determination module 440 may include: a first fusion unit, a second fusion unit and an error determination unit;

[0198] The first fusion unit is used to preprocess the first posture information, the second posture information, the measured speed information, the observation noise matrix and the wheel speed meter estimation error to obtain observation information and observation error.

[0199] The second fusion unit is used to determine the current state space vector of the driving device according to the observation information, the observation error and the prediction error covariance matrix.

[0200] The error determination unit is used to determine the current error covariance matrix according to the observation error and the prediction error covariance matrix.

[0201] Furthermore, the above-mentioned first fusion unit can be specifically used for: if the observation noise matrix is ​​greater than or equal to a first preset threshold, then the second posture information and the measured speed information are used as the observation information, and the wheel speed meter estimation error is used as the observation error; if the observation noise matrix is ​​less than or equal to a second preset threshold, then the first posture information and the measured speed information are used as the observation information, and the observation noise matrix is ​​used as the observation error; if the observation noise matrix is ​​greater than the second preset threshold and less than the first preset threshold, then the observation error is determined according to the observation noise matrix and the wheel speed meter estimation error, and the observation information is determined according to the first posture information, the second posture information, the measured speed information, the observation error, the observation noise matrix and the wheel speed meter estimation error; wherein, the first preset threshold is greater than the second preset threshold.

[0202] Furthermore, the above-mentioned first fusion unit can also be specifically used to: perform weighted calculation on the first posture information and the second posture information according to the observation error, the observation noise matrix and the wheel speed meter estimation error to obtain the observed posture; determine the observation information according to the observed posture and the measured speed information.

[0203] Furthermore, the above-mentioned second fusion unit can be specifically used to: determine the error state space vector of the current state space vector based on the observation information, the observation error and the prediction error covariance matrix; determine and output the general state space vector of the current state space vector of the driving device based on the error state space vector of the current state space vector and the general state space vector of the state space vector of the driving device at the previous moment.

[0204] Furthermore, the above-mentioned second determination module 440 can also be specifically used to: reset the error state space vector of the current state space vector to zero; determine the reset matrix based on the unit matrix and the attitude angle error in the error state space vector of the predicted state space vector; and reset the current error covariance matrix based on the reset matrix.

[0205] Furthermore, the positioning device of the above-mentioned driving equipment may also include: a map updating module;

[0206] The map update module is used to construct a local map corresponding to the current moment based on the point cloud data collected by the laser radar if it is monitored that the driving distance of the driving device at the current moment relative to the last time the local map was constructed reaches a preset distance; and update the prior map data based on the local map corresponding to the current moment.

[0207] Furthermore, the positioning device of the above-mentioned traveling equipment may further include: an initial information setting module;

[0208] The initial posture information of the driving device is set according to the prior map data and the point cloud data collected by the laser radar; and the initialization is performed according to the initial posture information and the inertial navigation parameters to obtain the initial filter parameters.

[0209] The positioning device for the traveling equipment provided in this embodiment can be applied to the positioning method for the traveling equipment provided in any of the above embodiments, and has corresponding functions and beneficial effects.

[0210] Embodiment 5

[0211] According to an embodiment of the present application, the present application also provides an electronic device and a readable storage medium.

[0212] like Figure 5 , is a block diagram of an electronic device according to a positioning method of a traveling device according to an embodiment of the present application. The electronic device is intended to represent various forms of digital computers, such as laptop computers, desktop computers, workbenches, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The electronic device may also represent various forms of mobile devices, such as personal digital processing, cellular phones, smart phones, wearable devices, and other similar computing devices. Optionally, the electronic device of the embodiment of the present application may be a server device, or a traveling device. If it is a traveling device, it may be an intelligent robot, an unmanned vehicle, etc. The components shown herein, their connections and relationships, and their functions are merely examples and are not intended to limit the implementation of the present application described and / or required herein.

[0213] like Figure 5As shown, the electronic device includes: one or more processors 510, a memory 520, and interfaces for connecting various components, including high-speed interfaces and low-speed interfaces. The various components are interconnected using different buses and can be installed on a common mainboard or installed in other ways as needed. The processor can process instructions executed in the electronic device, including instructions stored in or on the memory to display graphical information of the GUI on an external input / output device (such as a display device coupled to the interface). In other embodiments, if necessary, multiple processors and / or multiple buses can be used together with multiple memories and multiple memories. Similarly, multiple electronic devices can be connected, and each device provides some necessary operations (for example, as a server array, a group of blade servers, or a multi-processor system). Figure 5 A processor 510 is taken as an example.

[0214] The memory 520 is a non-transitory computer-readable storage medium provided in the present application. The memory stores instructions executable by at least one processor, so that the at least one processor executes the positioning method of the driving device provided in the present application. The non-transitory computer-readable storage medium of the present application stores computer instructions, which are used to cause a computer to execute the positioning method of the driving device provided in the present application.

[0215] The memory 520 is a non-transient computer-readable storage medium that can be used to store non-transient software programs, non-transient computer executable programs and modules, such as program instructions / modules corresponding to the positioning method of the driving device in the embodiment of the present application (for example, the attached Figure 4 The processor 510 executes various functional applications and data processing of the server by running the non-transient software programs, instructions and modules stored in the memory 520, that is, the positioning method of the driving device in the above method embodiment is implemented.

[0216] The memory 520 may include a program storage area and a data storage area, wherein the program storage area may store an operating system and applications required for at least one function; the data storage area may store data created based on the use of the electronic device located by the driving device, etc. In addition, the memory 520 may include a high-speed random access memory, and may also include a non-transient memory, such as at least one disk storage device, a flash memory device, or other non-transient solid-state storage device. In some embodiments, the memory 520 may optionally include a memory remotely disposed relative to the processor 510, and these remote memories may be connected to the electronic device located by the driving device via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0217] The electronic device of the positioning method of the driving device may further include: an input device 530 and an output device 540. The processor 510, the memory 520, the input device 530 and the output device 540 may be connected via a bus or other means. Figure 5 The example of connecting through bus is taken in the following.

[0218] The input device 530 can receive input digital or character information, and generate key signal input related to user settings and function control of the electronic device for positioning the traveling device, such as a touch screen, a keypad, a mouse, a track pad, a touch pad, an indicator rod, one or more mouse buttons, a trackball, a joystick and other input devices. The output device 540 may include a display device, an auxiliary lighting device (e.g., an LED) and a tactile feedback device (e.g., a vibration motor), etc. The display device may include, but is not limited to, a liquid crystal display (LCD), a light emitting diode (LED) display and a plasma display. In some embodiments, the display device may be a touch screen.

[0219] Various implementations of the systems and techniques described herein can be realized in digital electronic circuit systems, integrated circuit systems, dedicated ASICs (application specific integrated circuits), computer hardware, firmware, software, and / or combinations thereof. These various implementations can include: being implemented in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which can be a special purpose or general purpose programmable processor that can receive data and instructions from a storage system, at least one input device, and at least one output device, and transmit data and instructions to the storage system, the at least one input device, and the at least one output device.

[0220] These computer programs (also referred to as programs, software, software applications, or code) include machine instructions for programmable processors and can be implemented using high-level procedural and / or object-oriented programming languages, and / or assembly / machine languages. As used herein, the terms "machine-readable medium" and "computer-readable medium" refer to any computer program product, device, and / or means (e.g., disk, optical disk, memory, programmable logic device (PLD)) for providing machine instructions and / or data to a programmable processor, including a machine-readable medium that receives machine instructions as a machine-readable signal. The term "machine-readable signal" refers to any signal for providing machine instructions and / or data to a programmable processor.

[0221] To provide interaction with a user, the systems and techniques described herein can be implemented on a computer having: a display device (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor) for displaying information to the user; and a keyboard and pointing device (e.g., a mouse or trackball) through which the user can provide input to the computer. Other types of devices can also be used to provide interaction with the user; for example, the feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form (including acoustic input, voice input, or tactile input).

[0222] The systems and techniques described herein can be implemented in a computing system that includes backend components (e.g., as a data server), or a computing system that includes middleware components (e.g., an application server), or a computing system that includes frontend components (e.g., a user computer with a graphical user interface or a web browser through which a user can interact with implementations of the systems and techniques described herein), or a computing system that includes any combination of such backend components, middleware components, or frontend components. The components of the system can be interconnected by any form or medium of digital data communication (e.g., a communication network). Examples of communication networks include: a local area network (LAN), a wide area network (WAN), the Internet, and a blockchain network.

[0223] A computer system may include a client and a server. The client and the server are generally remote from each other and usually interact through a communication network. The relationship between the client and the server is generated by computer programs running on the corresponding computers and having a client-server relationship with each other. The server may be a cloud server, also known as a cloud computing server or cloud host, which is a host product in the cloud computing service system to solve the defects of difficult management and weak business scalability in traditional physical hosts and VPS services.

[0224] According to the technical solution of the embodiment of the present application, the predicted state space vector and the predicted error covariance matrix of the driving device are determined according to the state data of the driving device collected by the combined inertial navigation and the initial filter parameters; wherein the predicted state space vector includes a general state space vector and an error state space vector; according to the general state space vector of the predicted state space vector, the prior map data and the point cloud data collected by the laser radar are iteratively matched, and according to the iterative matching result, the first pose information and the observation noise matrix of the driving device are determined; according to the pulse data collected by the wheel speed meter, the second pose information and the measured speed information of the driving device are determined, and the wheel speed meter estimation error is determined; according to the first pose information, the second pose information, the measured speed information, the observation noise matrix, the wheel speed meter estimation error and the predicted error covariance matrix, the current state space vector and the current error covariance matrix of the driving device are determined, the current state space vector is output, and the error state space vector and the current error covariance matrix of the current state space vector are reset. The present application scheme utilizes the different characteristics of the combined inertial navigation and radar sensors to complement each other, and uses multi-sensor fusion technology for combined positioning, which solves the problem of uncertainty and low reliability of data using a single sensor in the prior art. Executing the solution of this application can provide stable and accurate positioning information for the traveling equipment.

[0225] It is worth noting that in the embodiment of the positioning device of the above-mentioned traveling equipment, the various units and modules included are only divided according to functional logic, but are not limited to the above-mentioned division, as long as the corresponding functions can be realized; in addition, the specific names of the functional units are only for the convenience of distinguishing each other, and are not used to limit the scope of protection of this application.

[0226] It should be understood that the various forms of processes shown above can be used to reorder, add or delete steps. For example, the steps recorded in this application can be executed in parallel, sequentially or in different orders, as long as the expected results of the technical solution disclosed in this application can be achieved, and this document is not limited here.

[0227] The above specific implementations do not constitute a limitation on the protection scope of this application. It should be understood by those skilled in the art that various modifications, combinations, sub-combinations and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions and improvements made within the spirit and principles of this application should be included in the protection scope of this application.

Claims

1. A positioning method for a traveling device, characterized in that: The method comprises: Determine a predicted state space vector and a predicted error covariance matrix of the driving device according to the state data of the driving device collected by the combined inertial navigation and the initial parameters of the filter; wherein the predicted state space vector includes a general state space vector and an error state space vector; Iteratively matching the prior map data and the point cloud data collected by the laser radar according to the general state space vector of the predicted state space vector, and determining the first position information and the observation noise matrix of the driving device according to the iterative matching result; Determining second position information and measurement speed information of the traveling device according to pulse data collected by the wheel speed meter, and determining an estimation error of the wheel speed meter; Determine a current state space vector and a current error covariance matrix of the traveling device according to the first posture information, the second posture information, the measured speed information, the observation noise matrix, the wheel speed meter estimation error and the prediction error covariance matrix, output the current state space vector, and reset the error state space vector of the current state space vector and the current error covariance matrix; The step of determining the first position information and the observation noise matrix of the driving device according to the iterative matching result includes: Using the target pose matrix associated with the highest matching score in the iterative matching result as the first pose information of the driving device; Determining a matching confidence level according to a highest matching score in the iterative matching results; The observation noise matrix is ​​determined according to the matching confidence and the rotation matrix.

2. The method according to claim 1, characterized in that The iterative matching of the prior map data and the point cloud data collected by the laser radar according to the general state space vector of the predicted state space vector comprises: Converting the pose information in the general state space vector of the predicted state space vector from the inertial navigation coordinate system to the radar coordinate system, and using the pose information in the general state space vector of the predicted state space vector converted to the radar coordinate system as an initial pose matrix; Based on the normal distribution transformation algorithm and the initial pose matrix, the prior map data and the point cloud data collected by the laser radar are iteratively matched for a preset number of times to obtain a matching score corresponding to each iterative matching and a target pose matrix associated with the matching score.

3. The method according to claim 1, characterized in that Determining the second position information and the measured speed information of the traveling device according to the pulse data collected by the wheel speed meter, and determining the wheel speed meter estimation error, comprises: Determine the position increment of the traveling device at the current moment relative to the previous moment and the wheel speed information at the current moment according to the pulse data collected by the wheel speed meter; Determining the angle increment of the traveling device at the current moment relative to the previous moment according to the wheel speed information at the current moment; Based on the motion model of the traveling device, and according to the posture information of the traveling device at a previous moment, the position increment, and the angle increment, determining second posture information of the traveling device at a current moment; Determining the measured speed information of the traveling device at the current moment according to the wheel speed information at the current moment and the angle increment; The wheel speed meter estimation error is determined according to the observed noise matrix of the traveling device at the last moment and the preset noise value.

4. The method according to claim 1, characterized in that: The method of determining a current state space vector and a current error covariance matrix of the traveling device according to the first posture information, the second posture information, the measured speed information, the observation noise matrix, the wheel speed meter estimation error and the prediction error covariance matrix comprises: Preprocessing the first posture information, the second posture information, the measured speed information, the observation noise matrix, and the wheel speed meter estimation error to obtain observation information and observation error; Determining a current state space vector of the driving device according to the observation information, the observation error and the prediction error covariance matrix; The current error covariance matrix is ​​determined according to the observation error and the prediction error covariance matrix.

5. The method according to claim 4, characterized in that The preprocessing of the first posture information, the second posture information, the measured speed information, the observation noise matrix and the wheel speed meter estimation error to obtain observation information and observation error includes: If the observation noise matrix is ​​greater than or equal to a first preset threshold, the second posture information and the measured speed information are used as the observation information, and the wheel speed meter estimation error is used as the observation error; If the observation noise matrix is ​​less than or equal to a second preset threshold, the first posture information and the measured speed information are used as the observation information, and the observation noise matrix is ​​used as the observation error; If the observation noise matrix is ​​greater than a second preset threshold and less than a first preset threshold, the observation error is determined based on the observation noise matrix and the wheel speed meter estimation error, and the observation information is determined based on the first posture information, the second posture information, the measurement speed information, the observation error, the observation noise matrix and the wheel speed meter estimation error; wherein the first preset threshold is greater than the second preset threshold.

6. The method according to claim 5, characterized in that The determining the observation information according to the first posture information, the second posture information, the measured speed information, the observation error, the observation noise matrix and the wheel speed meter estimation error comprises: Performing weighted calculation on the first posture information and the second posture information according to the observation error, the observation noise matrix and the wheel speed meter estimation error to obtain an observation posture; The observation information is determined according to the observation posture and the measurement speed information.

7. The method according to claim 4, characterized in that The determining the current state space vector of the driving device according to the observation information, the observation error and the prediction error covariance matrix includes: Determining an error state space vector of the current state space vector according to the observation information, the observation error, and the prediction error covariance matrix; According to the error state space vector of the current state space vector and the general state space vector of the state space vector of the driving device at a previous moment, a general state space vector of the current state space vector of the driving device is determined and output.

8. The method according to claim 1, characterized in thatThe resetting of the error state space vector of the current state space vector and the current error covariance matrix comprises: Resetting the error state space vector of the current state space vector to zero; Determine a reset matrix based on the identity matrix and the attitude angle error in the error state space vector of the predicted state space vector; The current error covariance matrix is ​​reset according to the reset matrix.

9. The method according to any one of claims 1 to 8, characterized in that: The method further comprises: If it is monitored that the driving distance of the driving device at the current moment relative to the last time the local map was constructed reaches a preset distance, a local map corresponding to the current moment is constructed based on the point cloud data collected by the laser radar; The priori map data is updated according to the local map corresponding to the current moment.

10. A positioning device for a traveling device, characterized in that: The device comprises: A prediction module, used to determine a predicted state space vector and a predicted error covariance matrix of the driving device according to the state data of the driving device collected by the combined inertial navigation and the initial parameters of the filter; wherein the predicted state space vector includes a general state space vector and an error state space vector; A matching module, used for iteratively matching the prior map data and the point cloud data collected by the laser radar according to the general state space vector of the predicted state space vector, and determining the first position information and the observation noise matrix of the driving device according to the iterative matching result; A first determination module is used to determine the second position information and the measured speed information of the traveling device according to the pulse data collected by the wheel speed meter, and determine the estimation error of the wheel speed meter; a second determination module, configured to determine a current state space vector and a current error covariance matrix of the traveling device according to the first posture information, the second posture information, the measured speed information, the observation noise matrix, the wheel speed meter estimation error and the prediction error covariance matrix, output the current state space vector, and reset the error state space vector of the current state space vector and the current error covariance matrix; The matching module is specifically used for: Using the target pose matrix associated with the highest matching score in the iterative matching result as the first pose information of the driving device; Determining a matching confidence level according to a highest matching score in the iterative matching results; The observation noise matrix is ​​determined according to the matching confidence and the rotation matrix.

11. An electronic device, characterized in that: The electronic device comprises: one or more processors; a memory for storing one or more programs, When the one or more programs are executed by the one or more processors, the one or more processors are able to execute the positioning method of the traveling device as described in any one of claims 1-9.

12. A non-volatile readable storage medium, characterized in that: The non-volatile readable storage medium stores computer-executable instructions, and the computer-executable instructions are used to enable the electronic device to execute the positioning method of the traveling device according to any one of claims 1-9.

Citation Information

Patent Citations

  • Robot positioning method and device based on SLAM (Simultaneous Localization And Mapping) of covariance intersection fusion

    CN110118556A

  • Integrated navigation method, integrated navigation device, electronic equipment and readable storage medium

    CN110174104A

  • Laser radar-based two-dimensional grid map construction method and system

    CN111427060A

  • Positioning method and device, automatic driving vehicle, electronic equipment and storage medium

    CN111649739A

  • Robot positioning evaluation method, robot and computer storage medium

    CN111708047A