A method, device, equipment and storage medium for positioning a traveling device
By combining inertial navigation and lidar data fusion, using Kalman filter and iterative matching algorithm, the precise autonomous positioning of the driving equipment is achieved, and the problem of low positioning accuracy in the existing technology is solved, and the stability of the positioning results is improved. reliability.
Patent Information
- Application Number
- CN202110082436.X
- 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
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.
Using a combination of inertial navigation and lidar sensor data fusion method, the predicted state space vector and current state space vector of the driving device are determined through the Kalman filter and iterative matching algorithm, and the prior map data and point cloud data are used for accuracy. Match, output stable and accurate pose information.
It realizes accurate and autonomous positioning of the driving equipment, improves the accuracy and reliability of positioning results, and solves the problem of data uncertainty of a single sensor.
Smart Images

Figure CN112880664B_ABST
Abstract
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, according to the state data of the driving device collected by the combined inertial navigation and the initial filter parameters, a predicted state space vector of the driving device and a predicted error covariance matrix corresponding to the predicted state space vector; 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 posture information in the general state space vector of the predicted state space vector, and determining the matching posture information and the observation noise matrix of the driving device according to the iterative matching result;
[0008] According to the matching posture information, the predicted error covariance matrix corresponding to the predicted state space vector and the observed noise 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.
[0009] In a second aspect, an embodiment of the present application provides a positioning device for a traveling device, the device comprising:
[0010] A prediction module, used to determine a predicted state space vector of the driving device and a predicted error covariance matrix corresponding to the predicted state space vector 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;
[0011] A matching module, used for iteratively matching the prior map data and the point cloud data collected by the laser radar according to the posture information in the general state space vector of the predicted state space vector, and determining the matching posture information and observation noise matrix of the driving device according to the iterative matching result;
[0012] A determination module is used to determine the current state space vector and the current error covariance matrix of the driving device based on the matching posture information, the predicted error covariance matrix corresponding to the predicted state space vector and the observation noise 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.
[0013] In a third aspect, an embodiment of the present application provides an electronic device, the electronic device comprising:
[0014] one or more processors;
[0015] A storage device for storing one or more programs;
[0016] 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.
[0017] 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.
[0018] The embodiment of the present application provides a positioning method, device, equipment and storage medium for a driving device. According to the state data of the driving device collected by the combined inertial navigation and the initial filter parameters, the predicted state space vector of the driving device and the predicted error covariance matrix corresponding to the predicted state space vector are determined; according to the posture information in 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 matching posture information and the observation noise matrix of the driving device are determined; according to the matching posture information, the predicted error covariance matrix and the observation noise matrix corresponding to the predicted state space vector, 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 scheme of the embodiment of the present application is based on the data fusion collected by the combined inertial navigation and laser radar multiple sensors to obtain the final posture information of the driving device, which can provide a stable and accurate state space vector for the driving device.
[0019] 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
[0020] The accompanying drawings are used to better understand the present solution and do not constitute a limitation of the present application.
[0021] Figure 1 A first flow chart of a positioning method for a traveling device provided in Embodiment 1 of the present application;
[0022] Figure 2A A second flow chart of a positioning method for a traveling device provided in Embodiment 2 of the present application;
[0023] Figure 2B A schematic diagram of the principle of a positioning method for a traveling device provided in Embodiment 2 of the present application;
[0024] Figure 2C A schematic diagram of a motion model of a traveling device provided in Embodiment 2 of the present application;
[0025] Figure 3 A third flow chart of a positioning method for a traveling device provided in Embodiment 3 of the present application;
[0026] Figure 4 A schematic diagram of the structure of a positioning device for a traveling device provided in Embodiment 4 of the present application;
[0027] 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
[0028] 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.
[0029] Embodiment 1
[0030] Figure 1 A first flow chart of a method for positioning a traveling device provided in the first embodiment of the present application. This embodiment is applicable to situations where a traveling device is positioned in real time during its operation. The 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, which can be implemented in 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.
[0031] See also Figure 1 The method of this embodiment includes but is not limited to the following steps:
[0032] S110 , determining a predicted state space vector of the traveling device and a prediction error covariance matrix corresponding to the predicted state space vector according to the state data of the traveling device collected by the combined inertial navigation and the initial filter parameters.
[0033] 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.
[0034] 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):
[0035] X=[P,v,q,a b ,w b ] T (1);
[0036] δX=[δP,δv,δq,δa b ,δw b ] T (2);
[0037] 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) ).
[0038]
[0039] Among them, P t is the current position; P t-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 last 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.
[0040] 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) ).
[0041]
[0042]
[0043] 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) V is the angular velocity deviation error at the previous moment; 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.
[0044] 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):
[0045]
[0046]
[0047]
[0048] S=F x S 上 F x T +F i Q i F i T (9);
[0049] 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.
[0050] S120. According to the posture information in 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 lidar, and determine the matching posture information and observation noise matrix of the driving device according to the iterative matching result.
[0051] Among them, the posture information in the general state space vector of the predicted state space vector includes the position information P and posture information q of the driving device. 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 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 an electronic device (such as a driving device).
[0052] 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 by the embodiment of the present application. This step iteratively matches the prior map data and the point cloud data collected by the lidar based on the posture information in the general state space vector of the predicted state space vector, and determines the matching posture information of the driving device and the specific process of the observation noise matrix based on the iterative matching results. The following sub-steps can be implemented:
[0053] 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.
[0054] 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:
[0055]
[0056] 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.
[0057] 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.
[0058] 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.
[0059] 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.
[0060] 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.
[0061] Specifically, assume that the point cloud data collected by the laser radar is A = {a0, ..., a n}, where A is the 3D point cloud data containing n points; the prior map data is B = {b0, ..., b n}, where B is the position coordinate of n points. First, the NDT algorithm is used to construct the prior map data B into a normal distribution of multidimensional variables. Secondly, through the probability density sum calculation formula, based on the initial pose matrix (i.e., the predicted value of 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:
[0062]
[0063]
[0064]
[0065] a i ′=T(a i ,F L ) (14);
[0066] 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 Lis 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 .
[0067] S1203: Use the target posture matrix associated with the highest matching score in the iterative matching result as the matching posture information of the driving device.
[0068] 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 matching pose information.
[0069] S1204: Determine the matching confidence level according to the highest matching score in the iterative matching results.
[0070] 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. The matching error is mainly used to perform data fusion on the posture information in the general state space vector of the predicted state space vector and the matching posture information. When determining the observation noise matrix corresponding to the matching posture 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 positioning using the laser radar at the current moment.
[0071] P belief =1 / score 高 ×I 6×6 (15);
[0072] 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).
[0073] S1205. Determine an observation noise matrix according to the matching confidence and the rotation matrix.
[0074] In an embodiment of the present application, the observation noise matrix of the matching posture information can be calculated based on the following formula (16)-formula (17).
[0075]
[0076]
[0077] Among them, R 3×3 represents the rotation matrix; R cur Represents the rotation transformation matrix of the matching pose information; V lidar Represents the observation noise matrix corresponding to the matching pose information.
[0078] S130. Determine the current state space vector and the current error covariance matrix of the driving device according to the matching posture information, the predicted error covariance matrix corresponding to the predicted state space vector, and the observed noise 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.
[0079] Optionally, according to the matching posture information, the prediction error covariance matrix corresponding to the prediction state space vector and the observation noise matrix, the current state space vector and the current error covariance matrix of the driving device are determined, and the current state space vector is output. Optionally, the current error covariance matrix can also be output. This step can be explained in detail through the following sub-steps:
[0080] S1301. Determine the error state space vector of the current state space vector according to the matching posture information, the prediction error covariance matrix corresponding to the prediction state space vector, and the observation noise matrix.
[0081] In an embodiment of the present application, the matching posture information, the prediction error covariance matrix corresponding to the predicted state space vector and the observation noise matrix can be input into the Kalman filter. Based on the working principle of the Kalman filter, the error state space vector can be obtained by combining the following formulas (18)-(20).
[0082] y=h(X t )+V lidar (18);
[0083] K=SH T (HSH T +V lidar ) -1 (19);
[0084] δX=K(yh(X))+V lidar (20);
[0085] Among them, y contains the matching pose information F L (including position information and posture information); V lidar is the observation noise matrix corresponding to the matching pose information; h(Xt ) 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.
[0086] S1302: 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.
[0087] 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.
[0088] In the embodiment of the present application, the general state space vector of the current state space vector can be calculated by formula (21):
[0089]
[0090] 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; δa b is the acceleration deviation error; δw b is the angular velocity deviation error.
[0091] S1303: Determine a current error covariance matrix according to the prediction error covariance matrix and the observation noise matrix corresponding to the prediction state space vector.
[0092] 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 (22). Formula (22) is as follows:
[0093] S=(I-KH)S 上 (I-KH) T +KV lidar K T (twenty two);
[0094] Among them, S is the current prediction error covariance matrix; S 上 is the covariance matrix of the current prediction error of the last time; I is the identity matrix; K is the gain matrix; H is the observation equation of the Jacobian matrix; V lidar is the observation noise matrix corresponding to the matching pose information.
[0095] S1304: Reset the error state space vector of the current state space vector to zero.
[0096] In an embodiment of the present application, through the above steps, the current state space vector and the current error covariance matrix of the driving device are determined according to the matching posture information, the prediction error covariance matrix corresponding to the prediction state space vector, and the observation noise matrix. Then, the error state space vector of the current state space vector needs to be reset to zero. The advantage of this setting is that it can be used to calculate the positioning of the driving device at the next moment, making the state space vector of the driving device more accurate.
[0097] S1305. 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.
[0098] In an embodiment of the present application, the reset matrix is determined by formula (23) based on the attitude angle error in the error state space vector of the unit matrix and the predicted state space vector.
[0099]
[0100] 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 (23).
[0101] S1306. Reset the current error covariance matrix according to the reset matrix.
[0102] In an embodiment of the present application, the reset matrix obtained according to formula (23) can be used to reset the current error covariance matrix through formula (24).
[0103] S′=GSG T (twenty four);
[0104] Among them, S is the current error covariance matrix; S' is the reset error covariance matrix.
[0105] The technical solution provided in this embodiment determines the predicted state space vector of the driving device and the predicted error covariance matrix corresponding to the predicted state space vector according to the state data of the driving device collected by the combined inertial navigation and the initial filter parameters; iteratively matches the prior map data and the point cloud data collected by the laser radar according to the posture information in the general state space vector of the predicted state space vector, and determines the matching posture information and the observation noise matrix of the driving device according to the iterative matching results; determines the current state space vector and the current error covariance matrix of the driving device according to the matching posture information, the prediction error covariance matrix and the observation noise matrix corresponding to the predicted state space vector, 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. The present application scheme utilizes the different characteristics of the combined inertial navigation and radar sensors to complement each other, uses multi-sensor fusion technology for combined positioning, and solves the problem of uncertainty and low reliability of the data using a single sensor in the prior art. The implementation of the present application scheme can enable the driving device to achieve accurate autonomous positioning, which has a strong engineering application value.
[0106] Embodiment 2
[0107] Figure 2A A second flow chart of a positioning method for a traveling device provided in Embodiment 2 of the present application; Figure 2B A schematic diagram of the principle of a positioning method for a traveling device provided in Embodiment 2 of the present application; Figure 2C This is a schematic diagram of the motion model of the driving device provided in the second embodiment of the present application. The present embodiment is optimized on the basis of the above embodiment, and the specific optimization is: adding a description of the zero-speed correction process for the positioning result of the driving device. Specifically, Figure 2A-2C As shown, the method includes:
[0108] S210 , determining a predicted state space vector of the traveling device and a prediction error covariance matrix corresponding to the predicted state space vector according to the state data of the traveling device collected by the combined inertial navigation and the initial filter parameters.
[0109] The predicted state space vector includes a general state space vector and an error state space vector.
[0110] S220. According to the posture information in 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 lidar, and determine the matching posture information and observation noise matrix of the driving device according to the iterative matching result.
[0111] S230. Determine the current state space vector and the current error covariance matrix of the driving device according to the matching posture information, the predicted error covariance matrix corresponding to the predicted state space vector, and the observed noise matrix; and reset the error state space vector and the current error covariance matrix of the current state space vector.
[0112] S240: Determine the measured speed information of the traveling device according to the wheel speed information collected by the wheel speed meter.
[0113] 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, obtains corresponding pulse data, and measures the wheel speed of the target wheel according to the frequency change of the pulse data. In this embodiment, the traveling device includes a left wheel and a right wheel, and the wheel speed information collected by the wheel speed meter includes the wheel speed of the left wheel and the wheel speed of the right wheel of the traveling device. The measured speed information can be the speed information of the traveling device calculated based on the wheel speed information collected by the wheel speed meter.
[0114] Specifically, the process of determining the measured speed information is as follows: during the operation of the traveling device, the wheel speed information of the traveling device, that is, the wheel speed information of the left and right wheels of the traveling device, is collected in real time or at a fixed time through 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. Figure 2B , the motion model of the traveling device shown in the figure, where P x and P y are the position coordinates of the driving device on the horizontal X-axis and Y-axis respectively; E is the wheelbase between the left and right wheels; V x is the measured velocity information in the X-axis direction; V y is the measured velocity information in the Y-axis direction; W is the angular velocity. The calculation formulas (19)-(21) for the measured velocity information are as follows:
[0115]
[0116]
[0117]
[0118] Where W is the angular velocity of the driving device; E is the wheelbase between the left and right wheels; Δt represents the time interval between two consecutive data collections; V R is the wheel speed information of the right wheel; V L is the wheel speed information of the left wheel; V x is the measured velocity information in the X-axis direction; Vy is the measured speed information in the Y-axis direction. In addition, V x and V y The combined speed information is called the speed measurement information of the driving device.
[0119] S250: If the measured speed information is zero, perform zero-speed correction on the speed information in the current state space vector of the driving device, output the current state space vector after zero-speed correction, and reset the current error covariance matrix.
[0120] Optionally, a wheel speed meter is a sensor commonly used in traveling equipment, and its data update frequency is high and its static characteristics are good, so the speed data obtained by the wheel speed meter is the most accurate. When the measured speed information calculated based on the wheel speed information collected by the wheel speed meter is zero, it means that the current real speed information of the traveling equipment is zero. At this time, it is necessary to correct the speed information in the current state space vector determined in S230 to zero, and then reset the current error covariance matrix to infinitesimal.
[0121] For example, Figure 2C As shown in the figure, assuming that the driving device is a mobile robot, the mobile robot 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 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. Matching, determine the matching posture information and observation noise matrix of the mobile robot; then transmit the output results of the iterative matching module and the state prediction module (i.e. matching posture information, prediction error covariance matrix and observation noise matrix) to the update fusion module, which calculates the current state space vector and 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 current error covariance matrix of the current state space vector, so as to accurately determine the predicted state space vector and its corresponding prediction error covariance matrix for the next positioning. Finally, the measured speed information determined based on the wheel speed information of the wheel speed meter is also required. If the measured speed information is zero, the speed information in the current state space vector determined by the update fusion module needs to be corrected at zero speed, and the final current state space vector is output.
[0122] The scheme of the embodiment of the present application determines the predicted state space vector and predicted 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, and iteratively matches the prior map data and the point cloud data collected by the laser radar according to the posture information in the general state space vector of the predicted state space vector, and determines the matching posture information and observation noise matrix of the driving device; and then determines the current state space vector and the current error covariance matrix of the driving device based on the matching posture information, the predicted error covariance matrix and the observation noise, and resets the error state space vector and the current error covariance matrix of the current state space vector. Finally, if the measured speed information of the driving device is determined to be zero based on the wheel speed information collected by the wheel speed meter, the speed information of the current state space vector of the driving device is corrected at zero speed, and the current error covariance matrix is reset. The scheme of the embodiment of the present application is based on the use of combined inertial navigation and laser radar, and adopts Kalman filter, iterative matching algorithm, and fusion algorithm to obtain the precise positioning data of the driving device, and then corrects the speed information in the positioning data based on the wheel speed meter, which further improves the accuracy of autonomous positioning of the driving device.
[0123] Embodiment 3
[0124] Figure 3 The third flow chart of a method for positioning a mobile device provided in the third embodiment of the present application is as follows: the present embodiment is optimized on the basis of the above embodiment, and the specific optimization is as follows: a detailed explanation of the initialization process of the mobile device is added.
[0125] refer to Figure 3 The method of this embodiment includes but is not limited to the following steps:
[0126] 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.
[0127] 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.
[0128] 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 in the above embodiment to determine the matching posture information of the traveling device. The specific iterative matching process has been described in detail in the above embodiment and will not be described here.
[0129] S320, initializing according to the initial posture information and the inertial navigation parameters to obtain initial filter parameters.
[0130] 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 Kalman filter; the initial error covariance matrix of the 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 .
[0131] 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.
[0132] S330: Determine a predicted state space vector of the traveling device and a prediction error covariance matrix corresponding to the predicted state space vector according to the state data of the traveling device collected by the combined inertial navigation and the initial filter parameters.
[0133] S340. According to the posture information in 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 lidar, and determine the matching posture information and observation noise matrix of the driving device according to the iterative matching result.
[0134] S350. Determine the current state space vector and the current error covariance matrix of the driving device according to the matching posture information, the predicted error covariance matrix corresponding to the predicted state space vector, and the observed noise 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.
[0135] Preferably, this embodiment can also determine the measured speed information of the traveling device based on the wheel speed information collected by the speedometer; if the measured speed information is zero, the speed information in the current state space vector of the traveling device determined in S350 is corrected to zero speed, and the current error covariance matrix is reset.
[0136] 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 predicted error covariance matrix based on the initialized initial filter parameters and the state data of the driving device collected by the combined inertial navigation, and iteratively matches the prior map data and the point cloud data collected by the laser radar according to the posture information in the general state space vector of the predicted state space vector, and determines the matching posture information and observation noise matrix of the driving device. Finally, according to the matching posture information, the prediction error covariance matrix and the observation noise 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, the accuracy of autonomous positioning of the driving device is greatly improved.
[0137] It should be understood that the various forms of processes shown in the above-mentioned various method embodiments can be reordered, and steps can be added or deleted. 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 does not limit them here.
[0138] Embodiment 4
[0139] 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:
[0140] A prediction module 410 is used to determine a predicted state space vector of the driving device and a predicted error covariance matrix corresponding to the predicted state space vector 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;
[0141] A matching module 420, configured to iteratively match the prior map data and the point cloud data collected by the laser radar according to the pose information in the general state space vector of the predicted state space vector, and determine the matching pose information and the observation noise matrix of the driving device according to the iterative matching result;
[0142] Determination module 430 is used to determine the current state space vector and the current error covariance matrix of the driving device based on the matching posture information, the predicted error covariance matrix corresponding to the predicted state space vector and the observation noise 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.
[0143] Furthermore, the matching module 420 is specifically used for:
[0144] 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;
[0145] 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.
[0146] Furthermore, the matching module 420 is also specifically used for:
[0147] Using the target pose matrix associated with the highest matching score in the iterative matching result as the matching pose information of the traveling device;
[0148] Determining a matching confidence level according to a highest matching score in the iterative matching results;
[0149] The observation noise matrix is determined according to the matching confidence and the rotation matrix.
[0150] Furthermore, the above-mentioned determination module 430 is specifically used for:
[0151] Determine an error state space vector of the current state space vector according to the matching posture information, a prediction error covariance matrix corresponding to the prediction state space vector, and the observation noise matrix;
[0152] 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;
[0153] The current error covariance matrix is determined according to the matching posture information, the prediction error covariance matrix corresponding to the predicted state space vector and the observation noise matrix.
[0154] Furthermore, the above-mentioned determination module 430 is also specifically used for:
[0155] Resetting the error state space vector of the current state space vector to zero;
[0156] Determine the reset matrix based on the unit matrix and the attitude angle error;
[0157] The current error covariance matrix is reset according to the reset matrix.
[0158] Furthermore, the above device also includes:
[0159] A speed calculation module, used to determine the measured speed information of the traveling device according to the wheel speed information collected by the wheel speed meter;
[0160] The zero-speed correction module is used to perform zero-speed correction on the speed information in the current state space vector of the traveling device if the measured speed information is zero, and reset the current error covariance matrix.
[0161] Furthermore, the above device also includes
[0162] 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.
[0163] 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.
[0164] 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.
[0165] Embodiment 5
[0166] According to an embodiment of the present application, the present application also provides an electronic device and a readable storage medium.
[0167] like Figure 5 , is a block diagram of an electronic device according to a positioning method for 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 can also represent various forms of mobile devices, such as personal digital processing, cellular phones, smart phones, wearable devices, and other similar computing devices. 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. Optionally, if the electronic device is a server device, the server can obtain data collected by various sensors (such as wheel speed meters, combined inertial navigation, and laser radar) on the traveling device to perform the positioning method for the traveling device according to an embodiment of the present application. If the electronic device is a traveling device, such as an intelligent robot, an unmanned vehicle, etc., then the traveling device processing includes, in addition to the devices introduced below (such as the processor 510 and the memory 520, etc.), also includes: a combined inertial navigation for collecting traveling device status data; a laser radar for collecting point cloud data of the surrounding environment of the traveling device; and a wheel speed meter for collecting wheel speed information of the traveling device.
[0168] 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 connected to each other using different buses and can be installed on a common mainboard or installed in other ways as needed. The processor 510 can process instructions executed in the electronic device, including instructions stored in the memory or on the memory 520 to display the 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 510 and / or multiple buses can be used together with multiple memories 520 and multiple memories 520. 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.
[0169] The memory 520 stores instructions executable by at least one processor, so that the at least one processor 510 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 enable the electronic device to execute the positioning method of the driving device provided in the present application.
[0170] 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 electronic device 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.
[0171] 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 application programs required for at least one function; the data storage area may store data created by the use of electronic devices based on the positioning of 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 devices. In some embodiments, the memory 520 may optionally include a memory remotely arranged relative to the processor 510, and these remote memories may be connected to the electronic device that executes the positioning method of 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.
[0172] 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.
[0173] 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.
[0174] Various embodiments of the methods and devices described herein can be implemented in digital electronic circuit systems, integrated circuit systems, dedicated ASICs (application specific integrated circuits), computer hardware, firmware, software, and / or combinations thereof. These various embodiments can be implemented by 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 dedicated or general programmable processor that can receive data and instructions from a memory 520, at least one input device, and at least one output device, and transmit data and instructions to the memory 520, the at least one input device, and the at least one output device.
[0175] 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.
[0176] 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).
[0177] 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.
[0178] 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.
[0179] 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, according to the state data of the driving device collected by the combined inertial navigation and the initial parameters of the filter, a predicted state space vector of the driving device and a predicted error covariance matrix corresponding to the predicted state space vector; 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 posture information in the general state space vector of the predicted state space vector, and determining the matching posture information and the observation noise matrix of the driving device according to the iterative matching result, wherein the iterative matching result includes a matching score corresponding to each iterative matching, and a target posture matrix associated with the matching score; Determine the current state space vector and the current error covariance matrix of the driving device according to the matching posture information, the prediction error covariance matrix corresponding to the prediction state space vector and the observation noise 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; The step of determining the matching posture 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 matching pose information of the traveling 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 posture information in the general state space vector of the predicted state space vector includes: 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 The method of determining the current state space vector and the current error covariance matrix of the driving device according to the matching posture information, the prediction error covariance matrix corresponding to the prediction state space vector, and the observation noise matrix, and outputting the current state space vector comprises: Determine an error state space vector of the current state space vector according to the matching posture information, a prediction error covariance matrix corresponding to the prediction state space vector, and the observation noise matrix; 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; The current error covariance matrix is determined according to the prediction error covariance matrix corresponding to the prediction state space vector and the observation noise matrix.
4. The method according to claim 3, characterized in that The 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; A reset matrix is determined according to the unit matrix and the attitude angle error in the error state space vector of the predicted state space vector; and the current error covariance matrix is reset according to the reset matrix.
5. The method according to any one of claims 1 to 4, characterized in that: The method further comprises: Determining the measured speed information of the traveling device according to the wheel speed information collected by the wheel speed meter; If the measured speed information is zero, zero-speed correction is performed on the speed information in the current state space vector of the traveling device, and the current error covariance matrix is reset.
6. The method according to claim 5, characterized in that The method further comprises: Setting initial position information of the driving device according to the prior map data and the point cloud data collected by the laser radar; Initialization is performed according to the initial posture information and the inertial navigation parameters to obtain the initial parameters of the filter.
7. A positioning device for a traveling device, characterized in that: The device comprises: A prediction module, used to determine a predicted state space vector of the driving device and a predicted error covariance matrix corresponding to the predicted state space vector 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, configured to iteratively match the prior map data and the point cloud data collected by the laser radar according to the pose information in the general state space vector of the predicted state space vector, and determine the matching pose information and the observation noise matrix of the driving device according to the iterative matching result, wherein the iterative matching result includes a matching score corresponding to each iterative matching and a target pose matrix associated with the matching score; A determination module, configured to determine a current state space vector and a current error covariance matrix of the driving device according to the matching posture information, a prediction error covariance matrix corresponding to the prediction state space vector, and the observation noise 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; The matching module is also specifically used for: Using the target pose matrix associated with the highest matching score in the iterative matching result as the matching pose information of the traveling 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.
8. 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 enabled to execute the positioning method of the traveling device as described in any one of claims 1-6.
9. 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-6.
Citation Information
Patent Citations
Mobile robot local self-positioning method and facility
CN108931245A
Positioning method, system, and device based on millimeter wave radar and storage medium
CN109031269A
Positioning method and system based on Kalman filtering, equipment and storage medium
CN109883423A
Mobile robot positioning method
CN110927740A
Vehicle motion state determination method and device and electronic device
CN111102978A
