Multi-source sensor high-precision positioning method and device and electronic equipment

Abstract

The invention discloses a multi-source sensor high-precision positioning method and device and electronic equipment, and relates to the technical field of combination of an intelligent driving technology and a navigation technology, and the method comprises the steps: obtaining a first integral pose matrix and a first observation pose matrix of a vehicle at a first moment, obtaining a second integral pose matrix and a second observation pose matrix of the vehicle at a second moment; calculating a pose error matrix among the first integral pose matrix, the first observation pose matrix, the second integral pose matrix and the second observation pose matrix; when it is determined that the pose error matrix does not meet the first preset error requirement, a second covariance of a second observation pose matrix is determined according to a first covariance of a second integral pose matrix; and inputting the second covariance, the first covariance, the second integral pose matrix and the second observation pose matrix into a filter, and outputting a target pose of the vehicle at the second moment. Therefore, high-precision positioning is ensured.

Description

Technical Field

[0001] The present application relates to the technical field of the combination of intelligent driving technology and navigation technology, and in particular to a multi-source sensor high-precision positioning method, device and electronic equipment. Background Art

[0002] At present, high-precision positioning methods usually use a multi-sensor fusion solution of Global Navigation Satellite System (GNSS) + Inertial Measurement Unit (IMU) or GNSS + IMU + wheel speed combination. The principle is to use GNSS information to correct the IMU integral posture so that the integral posture error converges. However, when GNSS is blocked, the signal quality will deteriorate. At this time, GNSS information not only cannot correct the IMU integral posture, but will increase the integral posture error, thereby reducing the vehicle positioning accuracy. Summary of the invention

[0003] The present application provides a multi-source sensor high-precision positioning method, device and electronic equipment, which can solve the problem that the positioning accuracy of the existing high-precision positioning method is reduced when the GNSS is blocked.

[0004] In a first aspect, the present application provides a multi-source sensor high-precision positioning method, the method comprising:

[0005] Respectively obtaining a first integral pose matrix and a first observed pose matrix of the vehicle at a first moment, and a second integral pose matrix and a second observed pose matrix of the vehicle at a second moment;

[0006] Calculate a pose error matrix among the first integral pose matrix, the first observation pose matrix, the second integral pose matrix, and the second observation pose matrix;

[0007] When it is determined that the pose error matrix does not meet a first preset error requirement, determining a second covariance of the second observation pose matrix according to the first covariance of the second integral pose matrix;

[0008] The second covariance, the first covariance, the second integral pose matrix and the second observation pose matrix are input into a filter, and the target pose of the vehicle at the second moment is output.

[0009] Through the above method, based on the calculated posture error matrix, it is possible to determine whether the GNSS is blocked. When it is determined that the GNSS is blocked, the second covariance of the second observation posture matrix is ​​adjusted to reduce its weight in the filtering, thereby reducing the disturbance of the GNSS to the vehicle positioning posture and ensuring high-precision positioning of the vehicle.

[0010] In one possible design, calculating a pose error matrix between the first integral pose matrix, the first observation pose matrix, the second integral pose matrix, and the second observation pose matrix includes:

[0011] Calculating an integral relative pose matrix between the first integral pose matrix and the second integral pose matrix, and calculating an observed relative pose matrix between the first observed pose matrix and the second observed pose matrix;

[0012] The pose error matrix is ​​calculated based on the integrated relative pose matrix and the observed relative pose matrix.

[0013] By using the above method, the pose error matrix is ​​calculated, and then based on judging whether the pose error matrix is ​​a unit matrix, it can be determined whether the GNSS is blocked.

[0014] In one possible design, determining that the pose error matrix does not meet a first preset error requirement includes:

[0015] When the posture error matrix is ​​a non-unit matrix, determining an error rotation matrix and an error translation vector in the posture error matrix;

[0016] When the error rotation matrix does not meet the second preset error requirement, and / or the error translation vector does not meet the third preset error requirement, it is determined that the posture error matrix does not meet the first preset error requirement.

[0017] In a possible design, the error rotation matrix does not meet the second preset error requirement, including:

[0018] Converting the error rotation matrix into a Lie algebra;

[0019] When the absolute value of any element in the Lie algebra is greater than or equal to a preset radian value, it is determined that the error rotation matrix does not meet the second preset error requirement.

[0020] In a possible design, the error translation vector does not meet the third preset error requirement, including:

[0021] When the absolute value of any element in the error translation vector is greater than or equal to a preset threshold, it is determined that the error translation vector does not meet the third preset error requirement.

[0022] In one possible design, determining the second covariance of the second observation pose matrix according to the first covariance of the second integral pose matrix includes:

[0023] Obtaining a real-time differential positioning RTK result of the vehicle;

[0024] When the RTK result is a fixed solution, determining the second covariance to be a first preset multiple of the first covariance; or

[0025] When the RTK result is a floating point solution, determining the second covariance to be a second preset multiple of the first covariance, wherein the second preset multiple is greater than the first preset multiple; or

[0026] When the RTK result is a differential solution, the second covariance is determined to be a third preset multiple of the first covariance, wherein the third preset multiple is greater than the second preset multiple.

[0027] In one possible design, the integral pose matrix and the observation pose matrix are both composed of a rotation matrix and a translation vector, wherein the rotation matrix represents the rotation of the vehicle from the carrier coordinate system to the world coordinate system, and the translation vector represents the translation of the vehicle from the carrier coordinate system to the world coordinate system.

[0028] In a second aspect, the present application provides a multi-source sensor high-precision positioning device, the device comprising:

[0029] An acquisition module, used to respectively acquire a first integral pose matrix and a first observation pose matrix of the vehicle at a first moment, and a second integral pose matrix and a second observation pose matrix of the vehicle at a second moment;

[0030] A calculation module, used to calculate a pose error matrix between the first integral pose matrix, the first observation pose matrix, the second integral pose matrix and the second observation pose matrix;

[0031] A determination module, configured to determine a second covariance of the second observation pose matrix according to the first covariance of the second integral pose matrix when it is determined that the pose error matrix does not meet a first preset error requirement;

[0032] A filtering positioning module is used to input the second covariance, the first covariance, the second integral pose matrix and the second observation pose matrix into a filter, and output the target pose of the vehicle at the second moment.

[0033] In a possible design, the computing module is specifically used for:

[0034] Calculating an integral relative pose matrix between the first integral pose matrix and the second integral pose matrix, and calculating an observed relative pose matrix between the first observed pose matrix and the second observed pose matrix;

[0035] The pose error matrix is ​​calculated based on the integrated relative pose matrix and the observed relative pose matrix.

[0036] In a possible design, the determining module is specifically used to:

[0037] When the posture error matrix is ​​a non-unit matrix, determining an error rotation matrix and an error translation vector in the posture error matrix;

[0038] When the error rotation matrix does not meet the second preset error requirement, and / or the error translation vector does not meet the third preset error requirement, it is determined that the posture error matrix does not meet the first preset error requirement.

[0039] In a possible design, the determining module is specifically used to:

[0040] Converting the error rotation matrix into a Lie algebra;

[0041] When the absolute value of any element in the Lie algebra is greater than or equal to a preset radian value, it is determined that the error rotation matrix does not meet the second preset error requirement.

[0042] In a possible design, the determining module is specifically used to:

[0043] When the absolute value of any element in the error translation vector is greater than or equal to a preset threshold, it is determined that the error translation vector does not meet the third preset error requirement.

[0044] In a possible design, the determining module is specifically used to:

[0045] Obtaining a real-time differential positioning RTK result of the vehicle;

[0046] When the RTK result is a fixed solution, determining the second covariance to be a first preset multiple of the first covariance; or

[0047] When the RTK result is a floating point solution, determining the second covariance to be a second preset multiple of the first covariance, wherein the second preset multiple is greater than the first preset multiple; or

[0048] When the RTK result is a differential solution, the second covariance is determined to be a third preset multiple of the first covariance, wherein the third preset multiple is greater than the second preset multiple.

[0049] In a third aspect, the present application provides an electronic device, including:

[0050] Memory, used to store computer programs;

[0051] The processor is used to implement the steps of the multi-source sensor high-precision positioning method of the first aspect when executing the computer program stored in the memory.

[0052] In a fourth aspect, the present application provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the multi-source sensor high-precision positioning method of the first aspect are implemented.

[0053] Based on the above multi-source sensor high-precision positioning method, the calculated pose error matrix can determine whether the GNSS is blocked. When it is determined that the GNSS is blocked, the second covariance of the second observation pose matrix is ​​adjusted to reduce its weight in the filtering, thereby reducing the disturbance of the GNSS to the vehicle positioning pose and ensuring the high-precision positioning of the vehicle.

[0054] The technical effects that can be achieved in each of the above-mentioned second to fourth aspects and each of the above-mentioned aspects refer to the technical effects that can be achieved in the above-mentioned first aspect or various possible schemes in the first aspect, and will not be repeated here. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 A flowchart of a multi-source sensor high-precision positioning method provided in an embodiment of the present application;

[0056] Figure 1a A logical schematic diagram of a posture error matrix calculation provided in an embodiment of the present application;

[0057] Figure 1b A logical schematic diagram of determining a second covariance provided in an embodiment of the present application;

[0058] Figure 2 A schematic diagram of the structure of a multi-source sensor high-precision positioning device provided in an embodiment of the present application;

[0059] Figure 3 A schematic diagram of the structure of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0060] In order to make the purpose, technical solutions and advantages of the present application clearer, the present application will be further described in detail below in conjunction with the accompanying drawings. The specific operating methods in the method embodiments can also be applied to device embodiments or system embodiments. It should be noted that in the description of the present application, "multiple" is understood as "at least two". "And / or" describes the association relationship of associated objects, indicating that three relationships may exist. For example, A and / or B can represent: A exists alone, and A and B exist, and B exists alone. A is connected to B, which can represent: A is directly connected to B and A is connected to B through C. In addition, in the description of the present application, words such as "first" and "second" are only used to distinguish the purpose of description, and cannot be understood as indicating or implying relative importance, nor can they be understood as indicating or implying order.

[0061] In the embodiments of the present application, the collection, storage, use, processing, transmission, provision and disclosure of user personal information involved are in compliance with the provisions of relevant laws and regulations and do not violate public order and good morals.

[0062] In order to facilitate understanding by those skilled in the art, the technical terms involved in the embodiments of the present application are first explained.

[0063] (1) Inertial Measurement Unit (IMU), which is used to measure the three-axis attitude angle (or angular velocity) and acceleration of an object.

[0064] (2) Global Navigation Satellite System (GNSS), also known as the Global Satellite Navigation System, is an air-based radio navigation and positioning system that can provide users with all-weather three-dimensional coordinates, speed and time information at any location on the Earth's surface or in near-Earth space.

[0065] (3) GNSS-Real Time Kinematic (RTK) positioning technology is a real-time dynamic positioning technology based on GNSS carrier phase observations. It can provide real-time three-dimensional positioning results of the measuring station in a specified coordinate system.

[0066] (4) Rodriguez rotation formula is a formula for calculating the new vector obtained by rotating a vector around a rotation axis by a given angle in three-dimensional space. This formula uses the original vector, the rotation axis and their cross product as the frame to represent the rotated vector.

[0067] (5) Kalman filtering is an algorithm that uses the linear system state equation and the system input and output observation data to optimally estimate the system state.

[0068] At present, high-precision positioning methods usually use a multi-sensor fusion solution of Global Navigation Satellite System (GNSS) + Inertial Measurement Unit (IMU) or GNSS + IMU + wheel speed combination. The principle is to use GNSS information to correct the IMU integral posture so that the integral posture error converges. However, when GNSS is blocked, the signal quality will deteriorate. At this time, GNSS information not only cannot correct the IMU integral posture, but will increase the integral posture error, thereby reducing the vehicle positioning accuracy.

[0069] In order to solve the above problems, a multi-source sensor high-precision positioning method provided in an embodiment of the present application can determine whether the GNSS is blocked by calculating the first integral pose matrix, the first observation pose matrix, the second integral pose matrix, and the pose error matrix between the second observation pose matrix. When it is determined that the GNSS is blocked, the second covariance of the second observation pose matrix is ​​adjusted to reduce its weight in filtering, thereby reducing the disturbance of the GNSS to the vehicle positioning pose and ensuring the high-precision positioning of the vehicle. Among them, the method and device described in the embodiment of the present application are based on the same technical concept. Since the principles of the problems solved by the method and the device are similar, the embodiments of the device and the method can refer to each other, and the repeated parts will not be repeated.

[0070] To further illustrate the technical solution provided by the embodiment of the present application, this is described in detail below in conjunction with the accompanying drawings and specific implementation methods. Although the embodiment of the present application provides the method operation steps as shown in the following embodiments or drawings, more or fewer operation steps may be included in the method based on routine or no creative labor. In the steps where there is no necessary causal relationship logically, the execution order of these steps is not limited to the execution order provided by the embodiment of the present application. The method may be executed or executed in parallel in the actual processing process or when the device is executed in accordance with the method sequence shown in the embodiment or drawings.

[0071] Figure 1 This is a flowchart of a multi-source sensor high-precision positioning method provided in an embodiment of the present application. The process can be executed by a multi-source sensor high-precision positioning device, which can be implemented by software, hardware, or a combination of software and hardware to ensure high-precision positioning of the vehicle when the GNSS is blocked. Figure 1 As shown, the process includes the following steps:

[0072] S11, respectively obtaining a first integral pose matrix and a first observed pose matrix of the vehicle at a first moment, and a second integral pose matrix and a second observed pose matrix of the vehicle at a second moment;

[0073] Optionally, the integral pose matrix is ​​obtained from the vehicle's own sensors (e.g., IMU, wheel speed sensor, IMU + wheel speed sensor), and the observation pose matrix is ​​obtained from GNSS. And the integral pose matrix T I and the observation pose matrix T G They are composed of the rotation matrix R and the translation vector r The rotation matrix R represents the rotation of the vehicle from the carrier coordinate system to the world coordinate system, and the translation vector r represents the translation of the vehicle from the carrier coordinate system to the world coordinate system.

[0074] For example, the first moment is t k The second moment is t k+1 .t k At time t, the IMU measures the rotation of the vehicle from the carrier coordinate system to the world coordinate system as the unit matrix, k The first integral pose matrix at the moment The rotation matrix and translation vector They are Then the first integral pose matrix t k+1 At time t, the IMU measures the rotation of the vehicle from the carrier coordinate system to the world coordinate system as 30 degrees around the x-axis. k+1 The second integral pose matrix at time The rotation matrix and translation vector They are Then the second integral pose matrix

[0075] At the same time, t k At time t, the GNSS measured the vehicle's rotation from the carrier coordinate system to the world coordinate system to be 0.1 degrees around the x-axis. k The first observation pose matrix at time The rotation matrix and translation vector They are Then the first observation pose matrix t k+1 At time t, the GNSS measured that the vehicle rotates from the carrier coordinate system to the world coordinate system by 29.9 degrees around the x-axis. k+1 The second observation pose matrix at time The rotation matrix and translation vector They are Then the first observation pose matrix

[0076] (For ease of description, the first moment is denoted as t k , the second moment is recorded as t k+1 , the first integral pose matrix is ​​recorded as The first observation pose matrix is ​​recorded as The second integral pose matrix is ​​recorded as The second observation pose matrix is ​​recorded as )

[0077] S12, calculating a pose error matrix among the first integral pose matrix, the first observation pose matrix, the second integral pose matrix, and the second observation pose matrix;

[0078] like Figure 1a As shown, it is a logical schematic diagram of a posture error matrix calculation provided in an embodiment of the present application, which specifically includes the following steps:

[0079] 101a: Calculate the first integral pose matrix With the second integral pose matrix The integral relative pose matrix between The specific calculation formula is as follows:

[0080]

[0081] 102a: Calculate the first observation pose matrix With the second observation pose matrix The relative pose matrix between the observations The specific calculation formula is as follows:

[0082]

[0083] 103a: Based on the first relative pose matrix and the second relative pose matrix The pose error matrix ΔT is calculated, and the specific calculation formula is as follows:

[0084]

[0085] For example According to formula (1), we can get

[0086] at the same time, According to formula (2), we can get Furthermore, according to And formula (3) to calculate

[0087] In the embodiment of the present application, since the integral relative posture error is small in a short time, it can be considered that Close to the true value, if t k+1 If the GNSS signal is good at this moment, is also close to the true value. At this time, based on and The calculated pose error matrix is ​​approximately equal to the identity matrix. k+1 When the GNSS signal is interfered, The value is large, based on and The calculated pose error matrix is ​​a non-unit matrix.

[0088] S13, when it is determined that the pose error matrix does not meet the first preset error requirement, determining a second covariance of the second observation pose matrix according to the first covariance of the second integral pose matrix;

[0089] like Figure 1b As shown, it is a logical schematic diagram of determining a second covariance provided in an embodiment of the present application, which specifically includes the following steps:

[0090] 101b: Determine whether the pose error matrix is ​​a unit matrix. If so, proceed to 105b; if not, proceed to 102b.

[0091] 102b: Determine the error rotation matrix ΔR and the error translation vector Δr in the posture error matrix ΔT.

[0092] For example, but

[0093] 103b: Determine whether the error rotation matrix ΔR meets the second error requirement, and whether the error translation vector Δr meets the third error requirement. If the error rotation matrix does not meet the second preset error requirement, and / or the error translation vector does not meet the third preset error requirement, that is, the posture error matrix does not meet the first preset error requirement, then proceed to 104b; if the error rotation matrix meets the second preset error requirement, and the error translation vector meets the third preset error requirement, that is, the posture error matrix meets the first preset error requirement, then proceed to 105b.

[0094] Optionally, the error rotation matrix does not meet the second preset error requirement, including: converting the error rotation matrix ΔR into a Lie algebra Δρ, specifically, ΔR can be converted into Δρ by a Rodrigues rotation formula. When the absolute value of any element in Δρ is greater than or equal to a preset radian value, it is determined that ΔR does not meet the second preset error requirement, wherein the dimension between the Lie algebra and the error translation vector is the same.

[0095] For example, the default radian value is 0.02 radians. The converted Wherein, Δρ includes element 1 being -0.0035, element 2 being 0.0, and element 3 being 0.0. Since the absolute values ​​of elements 1, 2, and 3 in Δρ are all less than the preset radian value of 0.02, it is determined that ΔR meets the second preset error requirement.

[0096] The error translation vector does not satisfy the third preset error requirement includes: when the absolute value of any element in the error translation vector Δr is greater than or equal to a preset threshold, determining that Δr does not satisfy the third preset error requirement.

[0097] For example, the preset threshold is 0.05m. Wherein, Δr includes element 1 being 0.0, element 2 being -0.000156, and element 3 being 0.010524. Since the absolute values ​​of elements 1, 2, and 3 in Δr are all less than the preset threshold value 0.05, it is determined that Δr meets the third preset error requirement.

[0098] It should be noted that the present application does not limit the specific numerical values ​​of the above-mentioned preset arc value and preset threshold value, and the specific numerical values ​​may be determined according to the circumstances.

[0099] Therefore, if any element in Δρ is less than the preset radian value, and the absolute value of any element in Δr is less than the preset threshold, it is considered that the Gnss signal is good and can be used normally. If any element in Δρ is greater than or equal to the preset radian value, or the absolute value of any element in Δr is greater than or equal to the preset threshold, it is considered that the Gnss signal is interfered.

[0100] 104b: Determine a second covariance of the second observation pose matrix based on the first covariance of the second integral pose matrix.

[0101] First, a real-time differential positioning RTK result of the vehicle is obtained based on the GNSS-RTK technology. When the RTK result is a fixed solution, the second covariance is determined to be a first preset multiple (for example, 20 times) of the first covariance, wherein the fixed solution indicates that the integer ambiguity has been resolved and the measurement has been initialized; or

[0102] When the RTK result is a floating point solution, determining the second covariance to be a second preset multiple (e.g., 50 times) of the first covariance, wherein the floating point solution indicates that the integer ambiguity has been resolved and the measurement has not been initialized, and the second preset multiple is greater than the first preset multiple; or

[0103] When the RTK result is a differential solution, the second covariance is determined to be a third preset multiple (for example, 100 times) of the first covariance, wherein the differential solution is characterized by a signal, but the accuracy of the intersection data is very low due to various reasons, and the third preset multiple is greater than the above-mentioned second preset multiple.

[0104] 105b: Obtain the second covariance of the second observation pose matrix.

[0105] It should be noted that the first preset multiple, the second preset multiple, and the third preset multiple are not limited in the embodiments of the present application, and the specific values ​​may be determined according to the circumstances.

[0106] S14, input the second covariance, the first covariance, the second integral pose matrix and the second observation pose matrix into the filter, and output the target pose of the vehicle at the second moment.

[0107] Optionally, the filter may be a Kalman filter, or other filters having the same function as the Kalman filter. The specific filter depends on the circumstances and is not specifically limited here.

[0108] By using the above-mentioned multi-source sensor high-precision positioning method, the first integral pose matrix, the first observation pose matrix, the second integral pose matrix and the pose error matrix between the second observation pose matrix are calculated, so as to determine whether the GNSS is blocked. When it is determined that the GNSS is blocked, the second covariance of the second observation pose matrix is ​​adjusted to reduce its weight in the filtering, thereby reducing the disturbance of the GNSS to the vehicle positioning pose and ensuring the high-precision positioning of the vehicle.

[0109] Based on the same inventive concept, the present application also provides a multi-source sensor high-precision positioning device, such as Figure 2 FIG. 1 is a schematic diagram of a high-precision positioning device with multiple source sensors provided in an embodiment of the present application, the device comprising:

[0110] An acquisition module 21 is used to respectively acquire a first integral pose matrix and a first observation pose matrix of the vehicle at a first moment, and a second integral pose matrix and a second observation pose matrix of the vehicle at a second moment;

[0111] A calculation module 22, used to calculate a pose error matrix between the first integral pose matrix, the first observation pose matrix, the second integral pose matrix and the second observation pose matrix;

[0112] A determination module 23, configured to determine a second covariance of the second observation pose matrix according to the first covariance of the second integral pose matrix when it is determined that the pose error matrix does not meet a first preset error requirement;

[0113] The filtering and positioning module 24 is used to input the second covariance, the first covariance, the second integral pose matrix and the second observation pose matrix into a filter, and output the target pose of the vehicle at the second moment.

[0114] In a possible design, the calculation module 22 is specifically used for:

[0115] Calculating an integral relative pose matrix between the first integral pose matrix and the second integral pose matrix, and calculating an observed relative pose matrix between the first observed pose matrix and the second observed pose matrix;

[0116] The pose error matrix is ​​calculated based on the integrated relative pose matrix and the observed relative pose matrix.

[0117] In a possible design, the determining module 23 is specifically used to:

[0118] When the posture error matrix is ​​a non-unit matrix, determining an error rotation matrix and an error translation vector in the posture error matrix;

[0119] When the error rotation matrix does not meet the second preset error requirement, and / or the error translation vector does not meet the third preset error requirement, it is determined that the posture error matrix does not meet the first preset error requirement.

[0120] In a possible design, the determining module 23 is specifically used to:

[0121] Converting the error rotation matrix into a Lie algebra;

[0122] When the absolute value of any element in the Lie algebra is greater than or equal to a preset radian value, it is determined that the error rotation matrix does not meet the second preset error requirement.

[0123] In a possible design, the determining module 23 is specifically used to:

[0124] When the absolute value of any element in the error translation vector is greater than or equal to a preset threshold, it is determined that the error translation vector does not meet the third preset error requirement.

[0125] In a possible design, the determining module 23 is specifically used to:

[0126] Obtaining a real-time differential positioning RTK result of the vehicle;

[0127] When the RTK result is a fixed solution, determining the second covariance to be a first preset multiple of the first covariance; or

[0128] When the RTK result is a floating point solution, determining the second covariance to be a second preset multiple of the first covariance, wherein the second preset multiple is greater than the first preset multiple; or

[0129] When the RTK result is a differential solution, the second covariance is determined to be a third preset multiple of the first covariance, wherein the third preset multiple is greater than the second preset multiple.

[0130] In one possible design, the integral pose matrix and the observation pose matrix are both composed of a rotation matrix and a translation vector, wherein the rotation matrix represents the rotation of the vehicle from the carrier coordinate system to the world coordinate system, and the translation vector represents the translation of the vehicle from the carrier coordinate system to the world coordinate system.

[0131] It should be noted here that the above-mentioned device provided in the embodiment of the present application can implement all the method steps in the above-mentioned method embodiment and can achieve the same technical effect. The parts and beneficial effects of this embodiment that are the same as the method embodiment will not be described in detail here.

[0132] Based on the same inventive concept, an electronic device is also provided in the embodiment of the present application, and the electronic device can realize the function of the aforementioned multi-source sensor high-precision positioning device, referring to Figure 3 , the electronic device comprises:

[0133] At least one processor 31, and a memory 32 connected to the at least one processor 31. The specific connection medium between the processor 31 and the memory 32 is not limited in the embodiment of the present application. Figure 3 In the example, the processor 31 and the memory 32 are connected via the bus 30. The bus 30 is Figure 3 The connection between other components is shown by bold lines, and is not intended to be limiting. The bus 30 can be divided into an address bus, a data bus, a control bus, etc. For ease of representation, Figure 3 Only one thick line is used in the figure, but it does not mean that there is only one bus or one type of bus. Alternatively, the processor 31 can also be called a controller, and there is no limitation on the name.

[0134] In the embodiment of the present application, the memory 32 stores instructions that can be executed by at least one processor 31. The at least one processor 31 can execute the multi-source sensor high-precision positioning method discussed above by executing the instructions stored in the memory 32. The processor 31 can implement Figure 2 The functions of each module in the device shown.

[0135] Among them, the processor 31 is the control center of the device, and can use various interfaces and lines to connect the various parts of the entire control device. By running or executing instructions stored in the memory 32 and calling data stored in the memory 32, the various functions of the device and processing data, the device can be monitored as a whole.

[0136] In a possible design, the processor 31 may include one or more processing units, and the processor 31 may integrate an application processor and a modem processor, wherein the application processor mainly processes an operating system, a user interface, and application programs, and the modem processor mainly processes wireless communications. It is understandable that the modem processor may not be integrated into the processor 31. In some embodiments, the processor 31 and the memory 32 may be implemented on the same chip, and in some embodiments, they may also be implemented separately on separate chips.

[0137] The processor 31 can be a general-purpose processor, such as a central processing unit (CPU), a digital signal processor, an application-specific integrated circuit, a field programmable gate array or other programmable logic device, a discrete gate or transistor logic device, a discrete hardware component, and can implement or execute the various methods, steps and logic block diagrams disclosed in the embodiments of the present application. A general-purpose processor can be a microprocessor or any conventional processor, etc. The steps of the multi-source sensor high-precision positioning method disclosed in the embodiments of the present application can be directly embodied as a hardware processor to be executed, or can be executed by a combination of hardware and software modules in the processor.

[0138] The memory 32 is a non-volatile computer-readable storage medium that can be used to store non-volatile software programs, non-volatile computer executable programs and modules. The memory 32 may include at least one type of storage medium, such as a flash memory, a hard disk, a multimedia card, a card-type memory, a random access memory (Random Access Memory, RAM), a static random access memory (Static Random Access Memory, SRAM), a programmable read-only memory (Programmable Read Only Memory, PROM), a read-only memory (Read Only Memory, ROM), an electrically erasable programmable read-only memory (Electrically Erasable Programmable Read-Only Memory, EEPROM), a magnetic memory, a disk, an optical disk, etc. The memory 32 is any other medium that can be used to carry or store a desired program code in the form of an instruction or data structure and can be accessed by a computer, but is not limited thereto. The memory 32 in the embodiment of the present application can also be a circuit or any other device that can realize a storage function, for storing program instructions and / or data.

[0139] By programming the processor 31, the code corresponding to the multi-source sensor high-precision positioning method described in the above embodiment can be fixed into the chip, so that the chip can execute Figure 1 The steps of the multi-source sensor high-precision positioning method of the embodiment shown are as follows: How to design and program the processor 31 is a technique known to those skilled in the art and will not be described in detail here.

[0140] Based on the same inventive concept, an embodiment of the present application further provides a storage medium, which stores computer instructions. When the computer instructions are executed on a computer, the computer executes the multi-source sensor high-precision positioning method discussed above.

[0141] In some possible implementations, various aspects of the multi-source sensor high-precision positioning method provided by the present application can also be implemented in the form of a program product, which includes program code. When the program product is run on an apparatus, the program code is used to enable the control device to execute the steps of the multi-source sensor high-precision positioning method according to various exemplary embodiments of the present application described above in this specification.

[0142] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented in one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that include computer-usable program code.

[0143] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0144] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0145] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0146] Obviously, those skilled in the art can make various changes and modifications to the present application without departing from the spirit and scope of the present application. Thus, if these modifications and variations of the present application fall within the scope of the claims of the present application and their equivalents, the present application is also intended to include these modifications and variations.

Claims

1. A high-precision positioning method using multi-source sensors. It is characterized in that The method comprises: Respectively obtaining a first integral pose matrix and a first observed pose matrix of the vehicle at a first moment, and a second integral pose matrix and a second observed pose matrix of the vehicle at a second moment; Calculate a pose error matrix among the first integral pose matrix, the first observation pose matrix, the second integral pose matrix, and the second observation pose matrix; When it is determined that the pose error matrix does not meet a first preset error requirement, determining a second covariance of the second observation pose matrix according to the first covariance of the second integral pose matrix; The second covariance, the first covariance, the second integral pose matrix and the second observation pose matrix are input into a filter, and the target pose of the vehicle at the second moment is output.

2. The method according to claim 1, It is characterized in that The calculating of the first integral pose matrix, the first observation pose matrix, the second integral pose matrix, and a pose error matrix between the second observation pose matrix comprises: Calculating an integral relative pose matrix between the first integral pose matrix and the second integral pose matrix, and calculating an observed relative pose matrix between the first observed pose matrix and the second observed pose matrix; The pose error matrix is ​​calculated based on the integrated relative pose matrix and the observed relative pose matrix.

3. The method according to claim 1, It is characterized in that The determining that the posture error matrix does not meet a first preset error requirement includes: When the posture error matrix is ​​a non-unit matrix, determining an error rotation matrix and an error translation vector in the posture error matrix; When the error rotation matrix does not meet the second preset error requirement, and / or the error translation vector does not meet the third preset error requirement, it is determined that the posture error matrix does not meet the first preset error requirement.

4. The method according to claim 3, It is characterized in that The error rotation matrix does not meet the second preset error requirement, including: Converting the error rotation matrix into a Lie algebra; When the absolute value of any element in the Lie algebra is greater than or equal to a preset radian value, it is determined that the error rotation matrix does not meet the second preset error requirement.

5. The method according to claim 3, It is characterized in that The error translation vector does not meet the third preset error requirement, including: When the absolute value of any element in the error translation vector is greater than or equal to a preset threshold, it is determined that the error translation vector does not meet the third preset error requirement.

6. The method according to claim 1, It is characterized in that The determining the second covariance of the second observation pose matrix according to the first covariance of the second integral pose matrix includes: Obtaining a real-time differential positioning RTK result of the vehicle; When the RTK result is a fixed solution, determining the second covariance to be a first preset multiple of the first covariance; or When the RTK result is a floating point solution, determining the second covariance to be a second preset multiple of the first covariance, wherein the second preset multiple is greater than the first preset multiple; or When the RTK result is a differential solution, the second covariance is determined to be a third preset multiple of the first covariance, wherein the third preset multiple is greater than the second preset multiple.

7. The method according to claim 1, It is characterized in that Both the integral pose matrix and the observation pose matrix are composed of a rotation matrix and a translation vector, wherein the rotation matrix represents the rotation of the vehicle from the carrier coordinate system to the world coordinate system, and the translation vector represents the translation of the vehicle from the carrier coordinate system to the world coordinate system.

8. A multi-source sensor high-precision positioning device, It is characterized in that The device comprises: An acquisition module, used to respectively acquire a first integral pose matrix and a first observation pose matrix of the vehicle at a first moment, and a second integral pose matrix and a second observation pose matrix of the vehicle at a second moment; A calculation module, used to calculate a pose error matrix between the first integral pose matrix, the first observation pose matrix, the second integral pose matrix and the second observation pose matrix; A determination module, configured to determine a second covariance of the second observation pose matrix according to the first covariance of the second integral pose matrix when it is determined that the pose error matrix does not meet a first preset error requirement; A filtering positioning module is used to input the second covariance, the first covariance, the second integral pose matrix and the second observation pose matrix into a filter, and output the target pose of the vehicle at the second moment.

9. An electronic device, It is characterized in that include: Memory, used to store computer programs; A processor, configured to implement the method steps of any one of claims 1 to 7 when executing the computer program stored in the memory.

10. A computer-readable storage medium, It is characterized in that The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method steps of any one of claims 1 to 7 are implemented.

Images