Multi-dimensional perception fused Beidou enhanced navigation positioning method

Abstract

The invention discloses a multidimensional perception fused Beidou enhanced navigation positioning method, which belongs to the technical field of navigation positioning, and comprises the following steps: S1, respectively processing historical positioning data of a mobile carrier acquired by a Beidou satellite and historical environment perception data of the mobile carrier acquired by a plurality of sensors based on a logarithmic transformation function, acquiring a first observation equation of the historical positioning data and a second observation equation of the historical environment sensing data; s2, based on the first observation equation and the second observation equation, using a central difference method to obtain a Jacobian matrix in Kalman filtering; and S3, fusing the positioning data of the mobile carrier obtained by the Beidou satellite in real time and the environmental perception data of the mobile carrier obtained by the plurality of sensors in real time based on a Jacobian matrix in Kalman filtering to obtain fused data, and navigating and positioning the mobile carrier based on the fused data. The problem of low navigation and positioning accuracy caused by difficulty in accurately linearizing nonlinear data through a Kalman filtering algorithm is solved.

Description

Technical Field

[0001] The present invention relates to the field of navigation and positioning technology, and in particular to a Beidou enhanced navigation and positioning method with multi-dimensional perception fusion. Background Art

[0002] With the rapid progress of automatic navigation technology and unmanned aerial vehicle navigation technology, the role of positioning technology in many fields has become increasingly prominent and more critical. For example, the patent number is CN117760407A, which is a robot positioning and navigation system and method for multi-positioning sensor fusion, including a data processing module, a positioning module, a motion module, an identification module, a perception module, a wireless communication module and a signal control module; the data processing module is connected with the positioning module, the identification module, the perception module, the wireless communication module and the signal control module, and the positioning module is connected with the motion module; the positioning module includes a Beidou RTK positioning module, a laser navigation positioning module, an inertial navigation positioning module and a driver positioning module. The present invention uses a multi-positioning sensor fusion positioning and navigation system, adopts a Kalman filter algorithm to fuse multiple positioning data, combines the positioning advantages of each sensor, and outputs the fused positioning information, thereby improving the stability and accuracy of robot navigation. However, in the above scheme, when the Kalman filter algorithm is used to fuse multiple positioning data, due to the limitations of the Taylor expansion in the Kalman filter algorithm, it is difficult to accurately linearize the nonlinear data. Therefore, when the Kalman filter algorithm is used to fuse the data, there will be insufficient fusion, resulting in low navigation positioning accuracy. Summary of the invention

[0003] The Kalman filter algorithm in the prior art is difficult to accurately linearize nonlinear data, resulting in low navigation and positioning accuracy. The present invention provides a Beidou enhanced navigation and positioning method with multi-dimensional perception fusion, which makes full use of the scale transformation characteristics of the logarithmic transformation function, smoothes the extreme values ​​in the historical positioning data and the historical environmental perception data, reduces the scale of the data, and improves the linearity of the first observation equation and the second observation equation obtained. At the same time, the Jacobian matrix in the Kalman filter is accurately obtained by combining the numerical differential characteristics and local approximation ability of the central difference method, and the Jacobian matrix is ​​used to fuse the positioning data with the environmental perception data to obtain the fused data for navigation and positioning, which solves the problem of low navigation and positioning accuracy caused by the difficulty of the Kalman filter algorithm to accurately linearize nonlinear data, thereby significantly improving the navigation and positioning accuracy.

[0004] In order to solve the above technical problems, the present invention provides a Beidou enhanced navigation and positioning method with multi-dimensional perception fusion, comprising the following steps: S1: Based on a logarithmic transformation function, historical positioning data of a mobile carrier acquired by a Beidou satellite and historical environmental perception data of a mobile carrier acquired by a plurality of sensors are processed respectively to obtain a first observation equation of the historical positioning data and a second observation equation of the historical environmental perception data; S2: Based on the first observation equation and the second observation equation, the central difference method is used to obtain the Jacobian matrix in the Kalman filter; S3: Based on the Jacobian matrix in the Kalman filter, the positioning data of the mobile carrier obtained in real time by the Beidou satellite and the environmental perception data of the mobile carrier obtained in real time by several sensors are fused to obtain fused data, and the mobile carrier is navigated and positioned based on the fused data.

[0005] After adopting the above technical solution, the present invention has the following advantages: By integrating the positioning data of mobile carriers acquired in real time by Beidou satellites with the environmental perception data of mobile carriers acquired in real time by several sensors, the advantages of both are fully utilized to improve the accuracy and reliability of navigation positioning; Considering that historical positioning data and historical environmental perception data usually have different characteristics and scales, the logarithmic transformation function is used to smooth the extreme values ​​in the data, reduce the scale difference of the data, and improve the linearity of the first observation equation and the second observation equation. Since the accuracy of the Jacobian matrix directly affects the processing ability of the filtering algorithm for nonlinear data, and considering the limitations of the Taylor expansion in the existing Kalman filter algorithm and the numerical differential characteristics and local approximation ability of the central difference method, the central difference method is used instead of the Taylor expansion, which can accurately obtain the Jacobian matrix in the Kalman filter. The Jacobian matrix obtained by the central difference method is used to fuse the data. On the premise of ensuring the linearization of the nonlinear data, the data fusion is more sufficient, which significantly improves the navigation positioning accuracy. The problem of low navigation positioning accuracy caused by the difficulty of the Kalman filter algorithm to accurately linearize nonlinear data is solved.

[0006] Preferably, the S1 comprises: S11: Synchronize the historical positioning data of the mobile carrier acquired by the Beidou satellite with the historical environmental perception data of the mobile carrier acquired by a plurality of sensors to obtain the target historical positioning data and the target historical environmental perception data; S12: Based on the Beidou satellite characteristics and the logarithmic transformation function, the target historical positioning data is transformed to obtain a first positioning factor estimate, and a first observation equation of the historical positioning data is obtained based on the first positioning factor estimate. Based on the characteristics of several sensors and the logarithmic transformation function, the target historical environmental perception data is transformed to obtain a second positioning factor estimate, and a second observation equation of the historical environmental perception data is obtained based on the second positioning factor estimate.

[0007] Taking into account the difference between the time when Beidou satellites obtain historical positioning data and the time when several sensors obtain historical environmental perception data, in order to avoid the uniformity of the data, the data is processed synchronously to improve the data processing efficiency, thereby improving the robustness and accuracy of navigation positioning. Taking into account that historical positioning data and historical environmental perception data usually have different characteristics and scales, the logarithmic transformation function is used to smooth the extreme values ​​in the data, reduce the scale difference of the data, and improve the linearity of the obtained first observation equation and the second observation equation.

[0008] Preferably, the S11 includes: S111: Obtain a first timestamp of historical positioning data of a mobile carrier acquired by a Beidou satellite, and if a second timestamp corresponding to the first timestamp does not exist in the second timestamps of historical environmental perception data of the mobile carrier acquired by a plurality of sensors, remove the historical positioning data corresponding to the first timestamp from the historical positioning data of the mobile carrier acquired by the Beidou satellite to obtain target historical positioning data, and mark the historical environmental perception data corresponding to the second timestamp corresponding to the first timestamp that does not exist in the second timestamps of the historical environmental perception data of the mobile carrier acquired by the plurality of sensors as initial historical environmental perception data; S112: If there is at most one category of data in the initial historical environment perception data at the same second timestamp, it means that the preset requirements are not met, and the data that does not meet the preset requirements in the initial historical environment perception data is removed to obtain the target historical environment perception data.

[0009] Taking into account the difference between the time when Beidou satellites obtain historical positioning data and the time when several sensors obtain historical environmental perception data, in order to avoid the uniformity of the data, the data is processed synchronously, which improves the data processing efficiency and thus improves the robustness and accuracy of navigation positioning.

[0010] Preferably, in S12, the plurality of sensors include at least a laser radar, a camera, and an inertial measurement unit.

[0011] Preferably, in S12, transforming the target historical environment perception data to obtain the second positioning factor estimation value based on the plurality of sensor characteristics and the logarithmic transformation function includes: The logarithmic transformation function is y=log(x). When obtaining the estimated value of the second positioning factor of the laser radar, y is the estimated value of the second positioning factor of the laser radar, and x is the distance data obtained by the laser radar in the target historical environment perception data. When obtaining the estimated value of the second positioning factor of the camera, y is the estimated value of the second positioning factor of the camera, and x is the pixel data obtained by the camera in the target historical environment perception data. When obtaining the estimated value of the second positioning factor of the inertial measurement unit, y is the estimated value of the second positioning factor of the inertial measurement unit, and x is the motion data obtained by the inertial measurement unit in the target historical environment perception data. Considering that different sensors use different data to characterize navigation and positioning during navigation and positioning, based on several sensor characteristics and the logarithmic transformation function, the target historical environment perception data is transformed to obtain the estimated value of the second positioning factor, and the logarithmic transformation function is used to smooth the extreme values ​​in the data, reduce the scale difference of the data, improve the linearity of the data, facilitate subsequent data fusion, and indirectly improve the positioning accuracy.

[0012] Preferably, S2 includes: S21: randomly generate a disturbance value, obtain a first state variable and a second state variable based on the disturbance value, use a central difference method, input the first state variable and the second state variable into a first observation equation to obtain a first approximate value and a second approximate value, respectively, and input the first state variable and the second state variable into a second observation equation to obtain a third approximate value and a fourth approximate value, respectively; S22: Obtaining a Jacobian matrix in the Kalman filter based on the first approximation, the second approximation, the third approximation, and the fourth approximation.

[0013] Since the accuracy of the Jacobian matrix directly affects the filtering algorithm's ability to process nonlinear data, and considering the limitations of Taylor expansion in the existing Kalman filter algorithm and the numerical differentiation characteristics and local approximation ability of the central difference method, the central difference method is used instead of Taylor expansion. This can accurately obtain the Jacobian matrix in the Kalman filter, while ensuring that the nonlinear data is linearized, making data fusion more complete and significantly improving the navigation positioning accuracy.

[0014] Preferably, the S22 includes: Jacobian matrix f1(x1) is the first approximation, f1(x2) is the second approximation, f2(x1) is the third approximation, f2(x2) is the fourth approximation, x1 is the first state variable, x2 is the second state variable, σ1 is the first disturbance value among the disturbance values, and σ2 is the second disturbance value among the disturbance values.

[0015] Preferably, S3 includes: S31: inputting the positioning data of the mobile carrier acquired in real time by the Beidou satellite and the environmental perception data of the mobile carrier acquired in real time by a number of sensors into the Kalman filter, and using the Jacobian matrix to obtain the positioning estimation value and error covariance matrix of the mobile carrier; S32: Determine whether the error covariance matrix meets a preset condition. If so, fuse the positioning data and the environmental perception data based on the positioning estimation value to obtain fused data. If not, update the parameters of the Kalman filter and continue to execute S31.

[0016] Beneficial effects of this program: By integrating the positioning data of mobile carriers acquired in real time by Beidou satellites with the environmental perception data of mobile carriers acquired in real time by several sensors, the advantages of both are fully utilized to improve the accuracy and reliability of navigation positioning; Considering the difference between the time when Beidou satellites obtain historical positioning data and the time when several sensors obtain historical environmental perception data, in order to avoid the singleness of the data, the data is processed synchronously, which improves the data processing efficiency and thus improves the utilization of computing resources; Considering that historical positioning data and historical environmental perception data usually have different characteristics and scales, the logarithmic transformation function is used to smooth the extreme values ​​in the data, reduce the scale difference of the data, and improve the linearity of the first observation equation and the second observation equation. Since the accuracy of the Jacobian matrix directly affects the processing ability of the filtering algorithm for nonlinear data, and considering the limitations of the Taylor expansion in the existing Kalman filter algorithm and the numerical differential characteristics and local approximation ability of the central difference method, the central difference method is used instead of the Taylor expansion, which can accurately obtain the Jacobian matrix in the Kalman filter. The Jacobian matrix obtained by the central difference method is used to fuse the data. On the premise of ensuring the linearization of the nonlinear data, the data fusion is more sufficient, which significantly improves the navigation positioning accuracy. The problem of low navigation positioning accuracy caused by the difficulty of the Kalman filter algorithm to accurately linearize nonlinear data is solved.

[0017] The present invention also provides a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, it implements the steps of the Beidou enhanced navigation and positioning method with multi-dimensional perception fusion.

[0018] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the steps of the Beidou enhanced navigation and positioning method with multi-dimensional perception fusion are implemented. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Other features, objects and advantages of the present invention will become more apparent by reading the detailed description of non-limiting embodiments made with reference to the following drawings. The drawings are only for the purpose of illustrating preferred embodiments and are not to be considered as limiting the present invention. Also, the same reference symbols are used throughout the drawings to represent the same parts.

[0020] Figure 1 The present invention is a flowchart of the Beidou enhanced navigation and positioning method with multi-dimensional perception fusion. DETAILED DESCRIPTION

[0021] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific implementation method described herein is only an optimal embodiment of the present invention, which is only used to explain the present invention and does not limit the scope of protection of the present invention. All other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0022] Before discussing the exemplary embodiments in more detail, it should be mentioned that some exemplary embodiments are described as processes or methods depicted as flow charts. Although the flow charts describe the operations (or steps) as sequential processes, many of the operations (or steps) therein can be implemented in parallel, concurrently, or simultaneously. In addition, the order of the operations can be rearranged. The process can be terminated when its operation is completed, but can also have additional steps not included in the drawings; the process can correspond to a method, function, procedure, subroutine, subprogram, etc.

[0023] Embodiment 1: like Figure 1 As shown, the Beidou enhanced navigation and positioning method with multi-dimensional perception fusion includes the following steps: S1: Based on the logarithmic transformation function, the historical positioning data of the mobile carrier obtained by the Beidou satellite and the historical environmental perception data of the mobile carrier obtained by several sensors are processed separately to obtain the first observation equation of the historical positioning data and the second observation equation of the historical environmental perception data.

[0024] The S1 includes: S11: Synchronize the historical positioning data of the mobile carrier acquired by the Beidou satellite with the historical environmental perception data of the mobile carrier acquired by a plurality of sensors to obtain the target historical positioning data and the target historical environmental perception data; S12: Based on the Beidou satellite characteristics and the logarithmic transformation function, the target historical positioning data is transformed to obtain a first positioning factor estimate, and a first observation equation of the historical positioning data is obtained based on the first positioning factor estimate. Based on the characteristics of several sensors and the logarithmic transformation function, the target historical environmental perception data is transformed to obtain a second positioning factor estimate, and a second observation equation of the historical environmental perception data is obtained based on the second positioning factor estimate.

[0025] The S11 includes: S111: Obtain a first timestamp of historical positioning data of a mobile carrier acquired by a Beidou satellite, and if a second timestamp corresponding to the first timestamp does not exist in the second timestamps of historical environmental perception data of the mobile carrier acquired by a plurality of sensors, remove the historical positioning data corresponding to the first timestamp from the historical positioning data of the mobile carrier acquired by the Beidou satellite to obtain target historical positioning data, and mark the historical environmental perception data corresponding to the second timestamp corresponding to the first timestamp that does not exist in the second timestamps of the historical environmental perception data of the mobile carrier acquired by the plurality of sensors as initial historical environmental perception data; S112: If there is at most one category of data in the initial historical environment perception data at the same second timestamp, it means that the preset requirements are not met, and the data that does not meet the preset requirements in the initial historical environment perception data is removed to obtain the target historical environment perception data.

[0026] In S12, the plurality of sensors include at least a laser radar, a camera, and an inertial measurement unit.

[0027] In S12, the step of transforming the target historical environment perception data to obtain a second positioning factor estimation value based on the plurality of sensor characteristics and the logarithmic transformation function includes: The logarithmic transformation function is y=log(x). When obtaining the estimated value of the second positioning factor of the lidar, y is the estimated value of the second positioning factor of the lidar, and x is the distance data obtained by the lidar in the target historical environment perception data. When obtaining the estimated value of the second positioning factor of the camera, y is the estimated value of the second positioning factor of the camera, and x is the pixel data obtained by the camera in the target historical environment perception data. When obtaining the estimated value of the second positioning factor of the inertial measurement unit, y is the estimated value of the second positioning factor of the inertial measurement unit, and x is the motion data obtained by the inertial measurement unit in the target historical environment perception data.

[0028] In this embodiment, the historical positioning data of the mobile carrier acquired by the Beidou satellite includes the geographical location information of the mobile carrier at a specific time point, specifically the longitude, latitude, altitude information of the location of the mobile carrier and the timestamp of capturing the positioning data. Several sensors include laser radar, camera, and inertial measurement unit. The historical environmental perception data of the mobile carrier acquired by several sensors include the distance information acquired by the laser radar and the timestamp of acquiring the distance information, the image information of the surrounding environment of the mobile carrier acquired by the camera and the timestamp of acquiring the surrounding environment image information, and the angular velocity and angular velocity information acquired by the inertial measurement unit and the timestamp of acquiring the acceleration and angular velocity. One category of data refers to data acquired by sensors of the same category. For example, at a time point, only the camera in the sensor acquires data, which is one category of data. If at the same timestamp, the camera and laser radar in the sensor both acquire data, which is two categories of data. The preset requirement is that there are at least two categories of data in the initial historical environmental perception data at the same second timestamp. The first observation equation can be E pos =g(x)+∈ pos , where g(x) is the estimated value of the first positioning factor, x is the historical positioning data of the mobile carrier obtained by the Beidou satellite, ∈ pos is the first observation error, and the second observation equation can be E pod =D(x)+ρ pod , where D(x) is the estimated value of the second positioning factor, x is the environmental perception data of the mobile carrier obtained by several sensors, ∈ pos is the second observation error. Considering that historical positioning data and historical environmental perception data usually have different characteristics and scales, the logarithmic transformation function is used to smooth the extreme values ​​in the data, reduce the scale difference of the data, and improve the linearity of the first observation equation and the second observation equation.

[0029] S2: Based on the first observation equation and the second observation equation, the Jacobian matrix in the Kalman filter is obtained using the central difference method.

[0030] The S2 includes: S21: randomly generate a disturbance value, obtain a first state variable and a second state variable based on the disturbance value, use a central difference method, input the first state variable and the second state variable into a first observation equation to obtain a first approximate value and a second approximate value, respectively, and input the first state variable and the second state variable into a second observation equation to obtain a third approximate value and a fourth approximate value, respectively; S22: Obtaining a Jacobian matrix in the Kalman filter based on the first approximation, the second approximation, the third approximation, and the fourth approximation.

[0031] The S22 includes: Jacobian matrix f1(x1) is the first approximation, f1(x2) is the second approximation, f2(x1) is the third approximation, f2(x2) is the fourth approximation, x1 is the first state variable, x2 is the second state variable, σ1 is the first disturbance value among the disturbance values, and σ2 is the second disturbance value among the disturbance values.

[0032] In this embodiment, if the randomly generated first disturbance value σ1 is 0.01 and the second disturbance value σ2 is 0.02, the state estimation value is obtained using the first observation equation, the second observation equation, the historical positioning data and the historical environmental perception data. If the obtained state estimation value x0 = [1, 2], then x1 = [1+0.01, 2+0.02] = [1.01, 2.02], x2 = [1-0.01, 2-0.02] = [0.99, 1.98], and the obtained x1 and x2 are respectively input into the first observation equation f1(y) and the second observation equation f1(y). Equation f2(y), y is x1 or x2. Since the accuracy of the Jacobian matrix directly affects the filtering algorithm's ability to process nonlinear data, and considering the limitations of Taylor expansion in the existing Kalman filter algorithm and the numerical differentiation characteristics and local approximation ability of the central difference method, the central difference method is used instead of Taylor expansion. The Jacobian matrix in the Kalman filter can be accurately obtained, and the Jacobian matrix obtained by the central difference method is used to fuse the data. Under the premise of ensuring the linearization of nonlinear data, the data fusion is more sufficient, which significantly improves the navigation positioning accuracy.

[0033] S3: Based on the Jacobian matrix in the Kalman filter, the positioning data of the mobile carrier obtained in real time by the Beidou satellite and the environmental perception data of the mobile carrier obtained in real time by several sensors are fused to obtain fused data, and the mobile carrier is navigated and positioned based on the fused data.

[0034] The S3 includes: S31: inputting the positioning data of the mobile carrier acquired in real time by the Beidou satellite and the environmental perception data of the mobile carrier acquired in real time by a number of sensors into the Kalman filter, and using the Jacobian matrix to obtain the positioning estimation value and error covariance matrix of the mobile carrier; S32: Determine whether the error covariance matrix meets a preset condition. If so, fuse the positioning data and the environmental perception data based on the positioning estimation value to obtain fused data. If not, update the parameters of the Kalman filter and continue to execute S31.

[0035] The positioning data of the mobile carrier acquired in real time by the Beidou satellite and the environmental perception data of the mobile carrier acquired in real time by several sensors are processed to obtain the state estimation value again, and the first disturbance value and the second disturbance value are randomly generated again. The state estimation value, the first disturbance value and the second disturbance value acquired again are input into the expression of each element acquisition process of the Jacobian matrix, and then the Jacobian matrix of the positioning data of the mobile carrier acquired in real time by the Beidou satellite and the environmental perception data of the mobile carrier acquired in real time by several sensors is obtained. Based on the Jacobian matrix, the positioning estimation value and the error covariance matrix of the mobile carrier are obtained. The preset condition is set according to the requirements of different devices for positioning accuracy. If the position error in a certain direction is expected to be small, the preset condition is that a certain element of the error covariance matrix (representing the position error in a certain direction) is less than a certain threshold. The updated parameters of the Kalman filter include the state transfer matrix and the process noise covariance. By fusing the positioning data of the mobile carrier acquired in real time by the Beidou satellite and the environmental perception data of the mobile carrier acquired in real time by several sensors, the advantages of both are fully utilized, and the accuracy and reliability of navigation positioning are improved.

[0036] In this embodiment, it also includes a receiving module, a data processing module, a fusion module, a SLAM module and an optimization module. The receiving module is used to receive the positioning data from the Beidou satellite and the environmental perception data of various sensors. The design of the receiving module fully considers the integrity and anti-interference ability of the signal, and the receiving module can effectively filter out the multipath effect and other interference sources. The data processing module pre-processes the received positioning data and environmental perception data. The pre-processing process includes denoising, calibration and synchronization. An adaptive filter is used to dynamically adjust the filter parameters according to the change of environmental noise, thereby effectively removing environmental noise. The calibration process eliminates sensor errors by using a calibration algorithm to ensure the accuracy of each sensor data in the same coordinate system. The fusion module uses an improved Kalman filter to fuse multi-source data. The SLAM module is responsible for building an environmental map in real time. The SLAM module uses graph optimization technology to optimize the multi-source observation data as a constraint by constructing a factor graph model. The factor graph model can effectively represent the relationship between variables and use these relationships to optimize the system state. The optimization module post-processes the SLAM results and optimizes the pose estimation using an advanced nonlinear optimization algorithm, thereby reducing the cumulative error and improving the accuracy and robustness of the system. The design of the optimization module takes into account the limitation of computing resources to ensure that the optimization process can be completed under the real-time requirements. Based on the above-mentioned use of the receiving module, data processing module, fusion module, SLAM module and optimization module, extensive testing and verification were carried out to evaluate its performance in practical applications. The test environment includes a variety of complex scenes such as urban environment, forest and indoor environment. Urban environment test: In an urban environment with tall buildings, the system can effectively deal with signal shielding and multipath effects, and provide stable navigation and positioning services. Through the fusion of multi-sensor data, the system can maintain a high positioning accuracy when the signal is unstable. Forest environment test: In an environment with dense trees, through the fusion of multi-sensor data, the weakening problem of Beidou signal is overcome, and high-precision positioning is achieved. The high-precision ranging capability of the laser radar is particularly important in the forest environment, and it can effectively identify and avoid obstacles. Indoor environment test: In an indoor environment without satellite signals, the system relies on sensor data and SLAM technology to successfully realize the construction of environmental maps. The test of the indoor environment verifies the independent navigation capability of the present invention in the absence of GNSS signals, and the verification results are shown in Table 1. Table 1. Performance verification table of the present invention in different environments It means that compared with traditional GNSS and single sensors, the present invention has significant improvements in positioning accuracy and robustness, and can meet the needs of various complex application scenarios.

[0037] Embodiment 2: This embodiment also provides a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, it implements the steps of the Beidou enhanced navigation and positioning method with multi-dimensional perception fusion.

[0038] Embodiment 3: This embodiment also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the steps of the Beidou enhanced navigation and positioning method with multi-dimensional perception fusion are implemented.

[0039] The specific implementation described above is a preferred implementation of the Beidou enhanced navigation and positioning method with multi-dimensional perception fusion of the present invention, but it is not intended to limit the specific implementation scope of the present invention. The scope of the present invention includes but is not limited to this specific implementation. All equivalent changes made in accordance with the shape and structure of the present invention are within the protection scope of the present invention.

Claims

1. The Beidou enhanced navigation and positioning method based on multi-dimensional perception fusion is characterized by: The following steps are involved: S1: Based on a logarithmic transformation function, historical positioning data of a mobile carrier acquired by a Beidou satellite and historical environmental perception data of a mobile carrier acquired by a plurality of sensors are processed respectively to obtain a first observation equation of the historical positioning data and a second observation equation of the historical environmental perception data; S2: Based on the first observation equation and the second observation equation, the Jacobian matrix in the Kalman filter is obtained using the central difference method; S3: Based on the Jacobian matrix in the Kalman filter, the positioning data of the mobile carrier obtained in real time by the Beidou satellite and the environmental perception data of the mobile carrier obtained in real time by several sensors are fused to obtain fused data, and the mobile carrier is navigated and positioned based on the fused data.

2. The Beidou enhanced navigation and positioning method based on multi-dimensional perception fusion according to claim 1 is characterized in that: The S1 includes: S11: Synchronize the historical positioning data of the mobile carrier acquired by the Beidou satellite with the historical environmental perception data of the mobile carrier acquired by a plurality of sensors to obtain the target historical positioning data and the target historical environmental perception data; S12: Based on the Beidou satellite characteristics and the logarithmic transformation function, the target historical positioning data is transformed to obtain a first positioning factor estimate, and a first observation equation of the historical positioning data is obtained based on the first positioning factor estimate. Based on the characteristics of several sensors and the logarithmic transformation function, the target historical environmental perception data is transformed to obtain a second positioning factor estimate, and a second observation equation of the historical environmental perception data is obtained based on the second positioning factor estimate.

3. The Beidou enhanced navigation and positioning method with multi-dimensional perception fusion according to claim 2 is characterized in that: The S11 includes: S111: Obtain a first timestamp of historical positioning data of a mobile carrier acquired by a Beidou satellite, and if a second timestamp corresponding to the first timestamp does not exist in the second timestamps of historical environmental perception data of the mobile carrier acquired by a plurality of sensors, remove the historical positioning data corresponding to the first timestamp from the historical positioning data of the mobile carrier acquired by the Beidou satellite to obtain target historical positioning data, and mark the historical environmental perception data corresponding to the second timestamp corresponding to the first timestamp that does not exist in the second timestamps of the historical environmental perception data of the mobile carrier acquired by the plurality of sensors as initial historical environmental perception data; S112: If there is at most one category of data in the initial historical environment perception data at the same second timestamp, it means that the preset requirements are not met, and the data that does not meet the preset requirements in the initial historical environment perception data is removed to obtain the target historical environment perception data.

4. The Beidou enhanced navigation and positioning method with multi-dimensional perception fusion according to claim 2 is characterized in that: In S12, the plurality of sensors include at least a laser radar, a camera, and an inertial measurement unit.

5. The Beidou enhanced navigation and positioning method with multi-dimensional perception fusion according to claim 4 is characterized in that: In S12, the step of transforming the target historical environment perception data to obtain a second positioning factor estimation value based on the plurality of sensor characteristics and the logarithmic transformation function includes: The logarithmic transformation function is y=log(x). When obtaining the estimated value of the second positioning factor of the lidar, y is the estimated value of the second positioning factor of the lidar, and x is the distance data obtained by the lidar in the target historical environment perception data. When obtaining the estimated value of the second positioning factor of the camera, y is the estimated value of the second positioning factor of the camera, and x is the pixel data obtained by the camera in the target historical environment perception data. When obtaining the estimated value of the second positioning factor of the inertial measurement unit, y is the estimated value of the second positioning factor of the inertial measurement unit, and x is the motion data obtained by the inertial measurement unit in the target historical environment perception data.

6. The Beidou enhanced navigation and positioning method with multi-dimensional perception fusion according to claim 1 is characterized in that: The S2 includes: S21: randomly generate a disturbance value, obtain a first state variable and a second state variable based on the disturbance value, use a central difference method, input the first state variable and the second state variable into a first observation equation to obtain a first approximate value and a second approximate value, respectively, and input the first state variable and the second state variable into a second observation equation to obtain a third approximate value and a fourth approximate value, respectively; S22: Obtaining a Jacobian matrix in the Kalman filter based on the first approximation, the second approximation, the third approximation, and the fourth approximation.

7. The Beidou enhanced navigation and positioning method with multi-dimensional perception fusion according to claim 6 is characterized in that: The S22 includes: Jacobian matrix f1(x1) is the first approximation, f1(x2) is the second approximation, f2(x1) is the third approximation, f2(x2) is the fourth approximation, x1 is the first state variable, x2 is the second state variable, σ1 is the first disturbance value among the disturbance values, and σ2 is the second disturbance value among the disturbance values.

8. The Beidou enhanced navigation and positioning method with multi-dimensional perception fusion according to claim 1 is characterized in that: The S3 includes: S31: inputting the positioning data of the mobile carrier acquired in real time by the Beidou satellite and the environmental perception data of the mobile carrier acquired in real time by a number of sensors into the Kalman filter, and using the Jacobian matrix to obtain the positioning estimation value and error covariance matrix of the mobile carrier; S32: Determine whether the error covariance matrix meets a preset condition. If so, fuse the positioning data and the environmental perception data based on the positioning estimation value to obtain fused data. If not, update the parameters of the Kalman filter and continue to execute S31.

9. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the Beidou enhanced navigation and positioning method with multi-dimensional perception fusion as described in any one of claims 1 to 8 are implemented.

10. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the steps of the Beidou enhanced navigation and positioning method with multi-dimensional perception fusion as described in any one of claims 1 to 8 are implemented.

Images