Unmanned vehicle positioning method and system, terminal and storage medium

Abstract

The invention relates to the technical field of unmanned driving, and particularly provides an unmanned vehicle positioning method and system, a terminal and a storage medium, and the method comprises the steps: obtaining vehicle positioning data and system noise data collected by a plurality of sensors; constructing a prediction equation and an observation equation of vehicle positioning based on the vehicle positioning data and the system noise data; constructing a covariance matrix based on system noise data; and based on the prediction equation, the observation equation and the covariance matrix of vehicle positioning, calculating a Kalman gain by using a Kalman algorithm to obtain an optimal estimated value at the current moment. According to the invention, the positioning precision of the vehicle in a complex environment is effectively improved.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned driving technology, and in particular relates to an unmanned driving vehicle positioning method, system, terminal and storage medium. Background Art

[0002] With the rapid development of autonomous driving technology, vehicle positioning technology has become an important foundation for realizing unmanned driving. Accurate vehicle positioning is a prerequisite for ensuring the safe driving and correct decision-making of unmanned vehicles. At present, commonly used vehicle positioning technologies mainly rely on the global navigation satellite system (GNSS), inertial navigation system (IMU) and other sensors (such as lidar, camera). However, the complexity of the urban environment, such as the obstruction of high-rise buildings, the failure of satellite signals, and sudden traffic conditions, pose great challenges to vehicle positioning.

[0003] Although the existing vehicle positioning technology has reached a certain level of maturity, it still has the following major drawbacks in complex scenarios: First, it is highly dependent on a single sensor: Most unmanned vehicle positioning systems rely on a single GNSS (such as GPS, Beidou) as the main positioning source. However, GNSS is easily interfered by factors such as urban high-rise buildings and atmospheric conditions. Especially in areas such as urban canyons and tunnels, satellite signals may completely fail, resulting in reduced vehicle positioning accuracy or even positioning failure. Second, the drift error is large when the IMU works independently: the inertial navigation system (IMU) can provide vehicle motion information in a short period of time, but its error when working independently will accumulate over time (drift error), especially in the absence of GNSS signals, the accuracy of the IMU will drop rapidly. Third, the ability to perceive the environment is limited: using only GNSS or IMU cannot accurately perceive the dynamic environment around the vehicle, such as pedestrians, vehicles, traffic signals, etc. Although lidar and cameras can provide rich environmental information, these sensors themselves may also be interfered by factors such as light, weather, and obstructions in some scenarios. Fourth, the positioning accuracy is not high and the robustness is insufficient: the existing single sensor positioning solutions often have insufficient positioning accuracy and poor system robustness in complex environments and cannot cope with changes in various environmental conditions. Summary of the invention

[0004] In view of the problems in the prior art of strong dependence on a single sensor and large drift error when the IMU works independently, the present invention provides an unmanned vehicle positioning method, system, terminal and storage medium to solve the above technical problems.

[0005] In a first aspect, the present invention provides a method for positioning an unmanned vehicle, comprising: Obtain vehicle positioning data and system noise data collected by multiple sensors; Based on the vehicle positioning data and system noise data, the prediction equation and observation equation of vehicle positioning are constructed; Based on the system noise data, the covariance matrix is ​​constructed; Based on the prediction equation, observation equation and covariance matrix of vehicle positioning, the Kalman algorithm is used to calculate the Kalman gain to obtain the optimal estimate at the current moment.

[0006] Further, the vehicle positioning data collected by the multiple sensors includes first positioning data and second positioning data; The first positioning data includes vehicle position data collected by IMU, the second positioning data includes point cloud data collected by lidar, and the system noise data includes system state noise and system observation noise.

[0007] Furthermore, the prediction equation and observation equation of vehicle positioning are constructed based on the vehicle positioning data and the system noise data, including: Based on the first positioning data and system state noise, a prediction equation for vehicle positioning is constructed , the prediction equation for vehicle positioning is ,in, is the vehicle position at time k, is the optimal estimate of the vehicle position at time k-1, is the system state noise, is the known system input quantity; Based on the second positioning data and system observation noise, the observation equation is constructed , the observation equation is ,in, is the point cloud data of the laser radar at time k, Observe the noise of the system.

[0008] Furthermore, based on the system noise data, a covariance matrix is ​​constructed, including: Based on the system state noise, construct the covariance matrix of the system state noise , system state noise Subject to mean 0 and covariance matrix The Gaussian distribution of ~N(0, ); Based on the system observation noise, construct the covariance matrix of the system observation noise , system observation noise Subject to mean 0 and covariance matrix The Gaussian distribution of ~N(0, ).

[0009] Furthermore, based on the prediction equation, observation equation and covariance matrix of vehicle positioning, the Kalman algorithm is used to calculate the Kalman gain to obtain the optimal estimate at the current moment, including: The prediction equation is expanded using the first-order Taylor expansion to obtain ,in, is the prior estimate of the vehicle position at time k, is the optimal estimate of the vehicle position at time k-1; The observation equation is expanded using the first-order Taylor expansion to obtain ,in is the observed estimated value; right Calculate the first-order partial derivative and get the state transfer matrix ; right Calculate the first-order partial derivative and obtain the observation transfer matrix ; Based on the state transfer matrix , and the covariance matrix corresponding to k-1 moment , get the prior estimate of the covariance matrix at time k , = + ; Prior estimates based on the covariance matrix at time k , observation transfer matrix , , calculate the Kalman gain , = ,in, is the Kalman gain at time k, for The transposed matrix of Based on Kalman gain , a priori estimate of the vehicle position Get the best estimate at the current time , = .

[0010] In a second aspect, the present invention provides an unmanned vehicle positioning system, comprising: A data acquisition module, used to acquire vehicle positioning data and system noise data collected by multiple sensors; An equation building module, used to build prediction equations and observation equations for vehicle positioning based on vehicle positioning data and system noise data; A covariance matrix building module is used to build a covariance matrix based on system noise data; The optimal estimate calculation module is used to calculate the Kalman gain based on the prediction equation, observation equation and covariance matrix of vehicle positioning using the Kalman algorithm to obtain the optimal estimate at the current moment.

[0011] Further, the equation building module includes: A prediction equation building unit, used to build a prediction equation for vehicle positioning based on the first positioning data and system state noise , the prediction equation for vehicle positioning is ,in, is the vehicle position at time k, is the optimal estimate of the vehicle position at time k-1, is the system state noise, is the known system input quantity; An observation equation construction unit, used to construct an observation equation based on the second positioning data and the system observation noise , the observation equation is ,in, is the point cloud data of the laser radar at time k, Observe the noise of the system.

[0012] Furthermore, the covariance matrix building module includes: The first covariance matrix construction unit is used to construct the covariance matrix of the system state noise based on the system state noise , system state noise Subject to mean 0 and covariance matrix The Gaussian distribution of ~N(0, ); The second covariance matrix construction unit is used to construct the covariance matrix of the system observation noise based on the system observation noise , system observation noise Subject to mean 0 and covariance matrix The Gaussian distribution of ~N(0, ).

[0013] In a third aspect, a terminal is provided, including: processor, memory, wherein: The memory is used to store computer programs. The processor is used to call and run the computer program from the memory, so that the terminal executes the above-mentioned terminal method.

[0014] According to a fourth aspect, a computer storage medium is provided, wherein the computer-readable storage medium stores instructions, and when the instructions are executed on a computer, the computer executes the methods described in the above aspects.

[0015] The beneficial effect of the present invention is that the unmanned vehicle positioning method, system, terminal and storage medium provided by the present invention can combine the advantages of inertial measurement unit and laser radar point cloud data to perform vehicle positioning by fusing data from multiple sensors. IMU can provide high-frequency motion information, while laser radar provides accurate spatial data of the environment. By combining the two, the positioning accuracy of the vehicle in complex environments can be effectively improved when the GNSS signal is weak or interfered.

[0016] The unmanned vehicle positioning method, system, terminal and storage medium provided by the present invention use the Kalman filter algorithm to fuse the prediction equation and observation equation of vehicle positioning, and construct a covariance matrix based on noise data, which can better handle the uncertainty in sensor data. Even if there is noise in the data collected by the sensor, the Kalman filter can dynamically adjust the positioning result according to the noise characteristics, so that the vehicle can maintain high-precision positioning even in an environment with large noise interference, thereby enhancing the robustness of the system.

[0017] The unmanned vehicle positioning method, system, terminal and storage medium provided by the present invention have the advantage of strong real-time performance through the Kalman filter algorithm. During the driving process of the vehicle, the system can dynamically adjust the positioning result of the vehicle by continuously updating the predicted value and observed value of the vehicle position. Through the rapid iterative calculation of the state equation and the observation equation at each moment, the system can quickly obtain the optimal estimate value at the current moment, thereby ensuring the real-time positioning requirements of the unmanned vehicle and ensuring safe and efficient operation.

[0018] The unmanned vehicle positioning method, system, terminal and storage medium provided by the present invention linearize the prediction equation and the observation equation through the first-order Taylor expansion, and the system can adapt to the drawback that the motion state of the vehicle is nonlinear in a dynamic and complex driving environment. The linearization process enables the Kalman filter to be effectively applied to complex vehicle motion models, ensuring that the system can accurately track the position and posture changes of the vehicle.

[0019] The unmanned vehicle positioning method, system, terminal and storage medium provided by the present invention can effectively correct the accumulated error caused by the IMU by combining the observation data of the laser radar and using the Kalman filter to update the state. This observation calibration mechanism reduces the accumulation of positioning errors and further improves the stability of long-term positioning. In addition, the design principle of the present invention is reliable, the structure is simple, and it has a very broad application prospect. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, for ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0021] Figure 1 is a schematic flow chart of a system according to an embodiment of the present invention.

[0022] Figure 2 is a schematic block diagram of a method according to an embodiment of the present invention.

[0023] Figure 3 A schematic diagram of the structure of a terminal provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0024] In order to enable those skilled in the art to better understand the technical solutions in the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.

[0025] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art of the present invention. The terms used in the specification of the present invention herein are only for the purpose of describing specific embodiments and are not intended to limit the present invention.

[0026] The driverless car positioning method provided in the embodiment of the present invention is executed by a computer device, and accordingly, the driverless car positioning system runs in the computer device.

[0027] Figure 1 is a schematic flow chart of a method according to an embodiment of the present invention. Figure 1 The execution subject may be a driverless car positioning system. According to different requirements, the order of the steps in the flow chart may be changed, and some may be omitted.

[0028] To facilitate understanding of the present invention, the unmanned vehicle positioning method provided by the present invention is further described below based on the principle of the unmanned vehicle positioning method of the present invention and in combination with the process of positioning the unmanned vehicle in the embodiment.

[0029] Specifically, Figure 1 As shown, the unmanned vehicle positioning method includes: S1. Obtain vehicle positioning data and system noise data collected by multiple sensors.

[0030] The first positioning data includes vehicle position data collected by IMU, the second positioning data includes point cloud data collected by lidar, and the system noise data includes system state noise and system observation noise.

[0031] Specifically, in this step, the system collects the positioning data of the vehicle from different sensors and records the system noise at the same time. This is the basis of the positioning process, and the data comes from the following sensors: IMU sensor: The IMU sensor provides the vehicle's acceleration and angular velocity data every second. For example, the IMU provides acceleration information every 0.1 seconds. Assume that at time , the acceleration collected by the IMU sensor is , the angular velocity is s. This indicates that the vehicle is accelerating during a small turn, and these data constitute the first positioning data of the system.

[0032] LiDAR: For example, LiDAR can be mounted on top of a vehicle, scanning hundreds of thousands of point cloud data per second to help measure the distance between the vehicle and surrounding objects (such as buildings, roadblocks, etc.). , the laser radar reports that the distance between the vehicle and the building on the right is , which data constitutes the second positioning data.

[0033] S2. Based on the vehicle positioning data and system noise data, the prediction equation and observation equation of vehicle positioning are constructed.

[0034] Based on the first positioning data and system state noise, a prediction equation for vehicle positioning is constructed , the prediction equation for vehicle positioning is ,in, is the vehicle position at time k, is the optimal estimate of the vehicle position at time k-1, is the system state noise, is the known system input. Based on the second positioning data and system observation noise, the observation equation is constructed. , the observation equation is ,in, is the point cloud data of the laser radar at time k, Observe the noise of the system.

[0035] S3. Construct a covariance matrix based on the system noise data.

[0036] Based on the system state noise, construct the covariance matrix of the system state noise , system state noise Subject to mean 0 and covariance matrix The Gaussian distribution of ~N(0, ). Based on the system observation noise, the covariance matrix of the system observation noise is constructed , system observation noise Subject to mean 0 and covariance matrix The Gaussian distribution of ~N(0, ).

[0037] S4. Based on the prediction equation, observation equation and covariance matrix of vehicle positioning, the Kalman algorithm is used to calculate the Kalman gain to obtain the optimal estimate at the current moment.

[0038] The prediction equation is expanded using the first-order Taylor expansion to obtain ,in, is the prior estimate of the vehicle position at time k, is the optimal estimate of the vehicle position at time k-1. The observation equation is expanded using the first-order Taylor expansion to obtain ,in is the estimated value of the observation. Calculate the first-order partial derivative and get the state transfer matrix .right Calculate the first-order partial derivative and obtain the observation transfer matrix Based on the state transfer matrix , and the covariance matrix corresponding to k-1 moment , get the prior estimate of the covariance matrix at time k , = + . Based on the prior estimate of the covariance matrix at time k , observation transfer matrix , , calculate the Kalman gain , = ,in, is the Kalman gain at time k, for The transposed matrix of . Based on the Kalman gain , a priori estimate of the vehicle position Get the best estimate at the current time , = .

[0039] In some embodiments, the pluggable module material management system may include multiple functional modules composed of computer program segments. The computer program of each program segment in the pluggable module material management system may be stored in a memory of a computer device and executed by at least one processor to perform (see Figure 1 Description) Function of pluggable module material management.

[0040] In this embodiment, the pluggable module material management system can be divided into multiple functional modules according to the functions it performs, such as Figure 2 As shown. The functional modules of the system 200 may include: a data acquisition module 210, an equation construction module 220, a covariance matrix construction module 230 and an optimal estimate calculation module 240. The module referred to in the present invention refers to a series of computer program segments that can be executed by at least one processor and can complete fixed functions, which are stored in a memory. In this embodiment, the functions of each module will be described in detail in subsequent embodiments.

[0041] A data acquisition module, used to acquire vehicle positioning data and system noise data collected by multiple sensors; An equation building module, used to build prediction equations and observation equations for vehicle positioning based on vehicle positioning data and system noise data; A covariance matrix construction module is used to construct a covariance matrix based on system noise data; The optimal estimate calculation module is used to calculate the Kalman gain based on the prediction equation, observation equation and covariance matrix of vehicle positioning using the Kalman algorithm to obtain the optimal estimate at the current moment.

[0042] Optionally, as an embodiment of the present invention, the equation building module includes: A prediction equation building unit, used to build a prediction equation for vehicle positioning based on the first positioning data and system state noise , the prediction equation for vehicle positioning is ,in, is the vehicle position at time k, is the optimal estimate of the vehicle position at time k-1, is the system state noise, is the known system input quantity; An observation equation construction unit, used to construct an observation equation based on the second positioning data and the system observation noise , the observation equation is ,in, is the point cloud data of the laser radar at time k, Observe the noise of the system.

[0043] Optionally, as an embodiment of the present invention, the covariance matrix construction module includes: The first covariance matrix construction unit is used to construct the covariance matrix of the system state noise based on the system state noise , system state noise Subject to mean 0 and covariance matrix The Gaussian distribution of ~N(0, ); The second covariance matrix construction unit is used to construct the covariance matrix of the system observation noise based on the system observation noise , system observation noise Subject to mean 0 and covariance matrix The Gaussian distribution of ~N(0, ).

[0044] Figure 3 The present invention provides a schematic diagram of the structure of a terminal 300 provided in an embodiment of the present invention. The terminal 300 can be used to execute the unmanned vehicle positioning method provided in an embodiment of the present invention.

[0045] The terminal 300 may include: a processor 310, a memory 320 and a communication unit 330. These components communicate via one or more buses. Those skilled in the art will appreciate that the server structure shown in the figure does not limit the present invention, and it may be a bus structure or a star structure, and may include more or fewer components than shown in the figure, or combine certain components, or arrange the components differently.

[0046] The memory 320 can be used to store the execution instructions of the processor 310, and the memory 320 can be implemented by any type of volatile or non-volatile storage terminal or a combination thereof, such as static random access memory (SRAM), electrically erasable programmable read-only memory (EEPROM), erasable programmable read-only memory (EPROM), programmable read-only memory (PROM), read-only memory (ROM), magnetic memory, flash memory, magnetic disk or optical disk. When the execution instructions in the memory 320 are executed by the processor 310, the terminal 300 can perform some or all of the steps in the following method embodiments.

[0047] The processor 310 is the control center of the storage terminal, and uses various interfaces and lines to connect various parts of the entire electronic terminal. It runs or executes software programs and / or modules stored in the memory 320, and calls data stored in the memory to perform various functions of the electronic terminal and / or process data. The processor can be composed of an integrated circuit (IC), for example, it can be composed of a single packaged IC, or it can be composed of multiple packaged ICs with the same or different functions. For example, the processor 310 can only include a central processing unit (CPU). In the embodiment of the present invention, the CPU can be a single computing core or multiple computing cores.

[0048] The communication unit 330 is used to establish a communication channel so that the storage terminal can communicate with other terminals, receive user data sent by other terminals or send user data to other terminals.

[0049] The present invention also provides a computer storage medium, wherein the computer storage medium may store a program, and when the program is executed, the program may include some or all of the steps in each embodiment provided by the present invention. The storage medium may be a magnetic disk, an optical disk, a read-only memory (ROM) or a random access memory (RAM).

[0050] Therefore, by fusing data from multiple sensors, the system can combine the advantages of inertial measurement units and lidar point cloud data to locate the vehicle. IMU can provide high-frequency motion information, while lidar provides accurate spatial data of the environment. By combining the two, the positioning accuracy of the vehicle in complex environments can be effectively improved when the GNSS signal is weak or interfered.

[0051] The unmanned vehicle positioning method, system, terminal and storage medium provided by the present invention use the Kalman filter algorithm to fuse the prediction equation and observation equation of vehicle positioning, and construct a covariance matrix based on noise data, which can better handle the uncertainty in sensor data. Even if there is noise in the data collected by the sensor, the Kalman filter can dynamically adjust the positioning result according to the noise characteristics, so that the vehicle can maintain high-precision positioning even in an environment with large noise interference, thereby enhancing the robustness of the system.

[0052] The unmanned vehicle positioning method, system, terminal and storage medium provided by the present invention have the advantage of strong real-time performance through the Kalman filter algorithm. During the driving process of the vehicle, the system can dynamically adjust the positioning result of the vehicle by continuously updating the predicted value and observed value of the vehicle position. Through the rapid iterative calculation of the state equation and the observation equation at each moment, the system can quickly obtain the optimal estimate value at the current moment, thereby ensuring the real-time positioning requirements of the unmanned vehicle and ensuring safe and efficient operation.

[0053] The unmanned vehicle positioning method, system, terminal and storage medium provided by the present invention linearize the prediction equation and the observation equation through the first-order Taylor expansion, and the system can adapt to the drawback that the motion state of the vehicle is nonlinear in a dynamic and complex driving environment. The linearization process enables the Kalman filter to be effectively applied to complex vehicle motion models, ensuring that the system can accurately track the position and posture changes of the vehicle.

[0054] The unmanned vehicle positioning method, system, terminal and storage medium provided by the present invention can effectively correct the accumulated error caused by the IMU by combining the observation data of the laser radar and using the Kalman filter for state update. This observation calibration mechanism reduces the accumulation of positioning errors and further improves the stability of long-term positioning. The technical effects that can be achieved by this embodiment can be found in the description above and will not be repeated here.

[0055] Those skilled in the art can clearly understand that the technology in the embodiments of the present invention can be implemented by means of software plus a necessary general hardware platform. Based on this understanding, the technical solution in the embodiments of the present invention, in essence or in other words, the part that contributes to the prior art, can be embodied in the form of a software product, which is stored in a storage medium such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a disk or an optical disk, and other media that can store program codes, including several instructions for enabling a computer terminal (which can be a personal computer, a server, or a second terminal, a network terminal, etc.) to execute all or part of the steps of the methods described in each embodiment of the present invention.

[0056] In this specification, the same or similar parts between the various embodiments can be referred to each other. In particular, for the terminal embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the description in the method embodiment.

[0057] In the several embodiments provided by the present invention, it should be understood that the disclosed systems and methods can be implemented in other ways. For example, the system embodiments described above are only schematic. For example, the division of the modules is only a logical function division. There may be other division methods in actual implementation, such as multiple modules or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of systems or modules, which can be electrical, mechanical or other forms.

[0058] The modules described as separate components may or may not be physically separated, and the components shown as modules may or may not be physical modules, that is, they may be located in one place or distributed on multiple network modules. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0059] In addition, each functional module in each embodiment of the present invention may be integrated into one processing module, or each module may exist physically separately, or two or more modules may be integrated into one module.

[0060] Although the present invention has been described in detail by referring to the accompanying drawings and in combination with the preferred embodiments, the present invention is not limited thereto. Without departing from the spirit and essence of the present invention, a person of ordinary skill in the art may make various equivalent modifications or substitutions to the embodiments of the present invention, and these modifications or substitutions shall be within the scope of the present invention. Any person skilled in the art who is familiar with the present technical field can easily think of changes or substitutions within the technical scope disclosed by the present invention, and all of these shall be within the scope of protection of the present invention. Therefore, the scope of protection of the present invention shall be subject to the scope of protection of the claims.

Claims

1. A method for positioning an unmanned vehicle, characterized in that: include: Obtain vehicle positioning data and system noise data collected by multiple sensors; Based on the vehicle positioning data and system noise data, the prediction equation and observation equation of vehicle positioning are constructed; Based on the system noise data, the covariance matrix is ​​constructed; Based on the prediction equation, observation equation and covariance matrix of vehicle positioning, the Kalman algorithm is used to calculate the Kalman gain to obtain the optimal estimate at the current moment.

2. The method according to claim 1, characterized in that The vehicle positioning data collected by the multiple sensors include first positioning data and second positioning data; The first positioning data includes vehicle position data collected by IMU, the second positioning data includes point cloud data collected by lidar, and the system noise data includes system state noise and system observation noise.

3. The method according to claim 2, characterized in that The prediction equation and observation equation of vehicle positioning are constructed based on the vehicle positioning data and the system noise data, including: Based on the first positioning data and system state noise, a prediction equation for vehicle positioning is constructed , the prediction equation for vehicle positioning is ,in, is the vehicle position at time k, is the optimal estimate of the vehicle position at time k-1, is the system state noise, is the known system input quantity; Based on the second positioning data and system observation noise, the observation equation is constructed , the observation equation is ,in, is the point cloud data of the laser radar at time k, Observe the noise of the system.

4. The method according to claim 3, characterized in that Based on the system noise data, a covariance matrix is ​​constructed, including: Based on the system state noise, construct the covariance matrix of the system state noise , system state noise Subject to mean 0 and covariance matrix The Gaussian distribution of ~N(0, ); Based on the system observation noise, construct the covariance matrix of the system observation noise , system observation noise Subject to mean 0 and covariance matrix The Gaussian distribution of ~N(0, ).

5. The method according to claim 4, characterized in that Based on the prediction equation, observation equation and covariance matrix of vehicle positioning, the Kalman algorithm is used to calculate the Kalman gain to obtain the optimal estimate at the current moment, including: The prediction equation is expanded using the first-order Taylor expansion to obtain ,in, is the prior estimate of the vehicle position at time k, is the optimal estimate of the vehicle position at time k-1; The observation equation is expanded using the first-order Taylor expansion to obtain ,in is the observed estimated value; right Calculate the first-order partial derivative and get the state transfer matrix ; right Calculate the first-order partial derivative to obtain the observation transfer matrix ; Based on the state transfer matrix , and the covariance matrix corresponding to k-1 moment , get the prior estimate of the covariance matrix at time k , = + ; Prior estimates based on the covariance matrix at time k , observation transfer matrix , , calculate the Kalman gain , = ,in, is the Kalman gain at time k, for The transposed matrix of Based on Kalman gain , a priori estimate of the vehicle position Get the best estimate at the current time , = .

6. An unmanned vehicle positioning system, characterized in that: include: A data acquisition module, used to acquire vehicle positioning data and system noise data collected by multiple sensors; An equation building module, used to build prediction equations and observation equations for vehicle positioning based on vehicle positioning data and system noise data; A covariance matrix building module is used to build a covariance matrix based on system noise data; The optimal estimate calculation module is used to calculate the Kalman gain based on the prediction equation, observation equation and covariance matrix of vehicle positioning using the Kalman algorithm to obtain the optimal estimate at the current moment.

7. The system of claim 6, wherein the equation building module comprises: A prediction equation building unit, used to build a prediction equation for vehicle positioning based on the first positioning data and system state noise , the prediction equation for vehicle positioning is ,in, is the vehicle position at time k, is the optimal estimate of the vehicle position at time k-1, is the system state noise, is the known system input quantity; An observation equation construction unit, used to construct an observation equation based on the second positioning data and the system observation noise , the observation equation is ,in, is the point cloud data of the laser radar at time k, Observe the noise of the system.

8. The system according to claim 7, wherein the covariance matrix building module comprises: The first covariance matrix construction unit is used to construct the covariance matrix of the system state noise based on the system state noise , system state noise Subject to mean 0 and covariance matrix The Gaussian distribution of ~N(0, ); The second covariance matrix construction unit is used to construct the covariance matrix of the system observation noise based on the system observation noise , system observation noise Subject to mean 0 and covariance matrix The Gaussian distribution of ~N(0, ).

9. A terminal, characterized in that: include: A memory, used for storing a positioning program for an unmanned vehicle; A processor is used to implement the steps of the unmanned vehicle positioning method as described in any one of claims 1 to 5 when executing the unmanned vehicle positioning program.

10. A computer-readable storage medium storing a computer program, characterized in that: The readable storage medium stores an unmanned vehicle positioning program, and when the unmanned vehicle positioning program is executed by the processor, the steps of the unmanned vehicle positioning method according to any one of claims 1 to 5 are implemented.

Images