CN116045976A - Pose determining method and related device, electronic equipment and storage medium - Google Patents

Pose determining method and related device, electronic equipment and storage medium Download PDF

Info

Publication number
CN116045976A
CN116045976A CN202310111826.4A CN202310111826A CN116045976A CN 116045976 A CN116045976 A CN 116045976A CN 202310111826 A CN202310111826 A CN 202310111826A CN 116045976 A CN116045976 A CN 116045976A
Authority
CN
China
Prior art keywords
state quantity
pose
point cloud
state
measurement unit
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202310111826.4A
Other languages
Chinese (zh)
Inventor
何云翔
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang Zero Run Technology Co Ltd
Original Assignee
Zhejiang Zero Run Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Zhejiang Zero Run Technology Co Ltd filed Critical Zhejiang Zero Run Technology Co Ltd
Priority to CN202310111826.4A priority Critical patent/CN116045976A/en
Publication of CN116045976A publication Critical patent/CN116045976A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

The application discloses a pose determining method, a related device, electronic equipment and a storage medium, wherein the pose determining method comprises the following steps: based on the state quantity of the inertial measurement unit, a Kalman filter equation is constructed, and based on the error state quantity, the state quantity of the inertial measurement unit is corrected to obtain a target state quantity of the inertial measurement unit; determining whether to update the pose state of the odometer based on a fusion result between the target state quantity and the point cloud data of the point cloud collector; responding to the pose state of the updated odometer, and adjusting a first observed quantity in the Kalman filtering equation to obtain a new Kalman filtering equation; based on the new Kalman filtering equation, predicting to obtain new error state quantity, and re-executing the steps and subsequent steps of correcting the state quantity of the inertial measurement unit based on the error state quantity until the pose state is determined to be unnecessary to update, and taking the latest pose state as the target pose of the odometer. By means of the scheme, the accuracy of the pose state can be improved.

Description

Pose determining method and related device, electronic equipment and storage medium
Technical Field
The present disclosure relates to the field of data processing technologies, and in particular, to a pose determining method and related device, an electronic device, and a storage medium.
Background
In recent years, laser-inertial navigation odometers (LiDAR-Inertial Odometry, LIO) have developed rapidly, and can be classified into graph optimization methods and kalman filter-based methods.
At present, real-time acceleration, angular velocity, position and other information are generally obtained according to an IMU (Inertial Measurement Unit ), an RTK (Real-time kinematic) and other sensors, a Kalman filtering model is constructed, the position relation at the next moment is predicted, the predicted value is corrected through accurate point cloud matching, a Kalman gain equation is updated, and a Real-time matching result is output and is overlapped to the pose at the last moment. However, as time is accumulated, the gesture superimposed track is longer and longer, because the random error of the IMU measurement value is continuously increased along with time, and the accuracy of the positioning result of the RTK is reduced due to factors such as shielding, so that the point cloud matching is invalid or fails, the system cannot update the Kalman gain equation, and finally the predicted gesture update is inaccurate. In view of this, how to improve the accuracy of the pose state is a problem to be solved.
Disclosure of Invention
The technical problem that this application mainly solves is to provide a position appearance determining method and relevant device, electronic equipment and storage medium, can improve the accuracy of position appearance state.
In order to solve the above technical problem, a first aspect of the present application provides a pose determining method, including: based on the state quantity of the inertial measurement unit, a Kalman filter equation is constructed, and based on the error state quantity, the state quantity of the inertial measurement unit is corrected to obtain a target state quantity of the inertial measurement unit; the error state quantity is obtained through prediction based on a Kalman filtering equation; determining whether to update the pose state of the odometer based on the fusion result between the target state quantity and the point cloud data of the point cloud collector; responding to the pose state of the updated odometer, and adjusting a first observed quantity in the Kalman filtering equation to obtain a new Kalman filtering equation; wherein, the first observed quantity characterizes the observed quantity corresponding to the inertial measurement unit in the Kalman filtering equation; based on the new Kalman filtering equation, predicting to obtain new error state quantity, and re-executing the step and subsequent steps of correcting the state quantity of the inertial measurement unit based on the error state quantity until the pose state is determined to be unnecessary to update, and taking the latest pose state as the target pose of the odometer.
In order to solve the technical problem, a second aspect of the present application provides a pose determining device, which includes a building module, an updating module, an adjusting module and a predicting module; the construction module is used for constructing a Kalman filter equation based on the state quantity of the inertial measurement unit, correcting the state quantity of the inertial measurement unit based on the error state quantity and obtaining a target state quantity of the inertial measurement unit; the error state quantity is obtained through prediction based on a Kalman filtering equation; the updating module is used for determining whether to update the pose state of the odometer based on the fusion result between the target state quantity and the point cloud data of the point cloud collector; the adjustment module is used for adjusting a first observed quantity in the Kalman filtering equation to obtain a new Kalman filtering equation in response to determining the pose state of the updated odometer; wherein, the first observed quantity characterizes the observed quantity corresponding to the inertial measurement unit in the Kalman filtering equation; the prediction module is used for predicting and obtaining a new error state quantity based on a new Kalman filtering equation, and re-executing the step and the subsequent steps of correcting the state quantity of the inertial measurement unit based on the error state quantity until the position and the posture are determined to be unnecessary to update, and taking the latest position and posture as the target position and posture of the odometer.
In order to solve the above technical problem, a third aspect of the present application provides an electronic device, which includes a memory and a processor coupled to each other, where the memory stores program instructions, and the processor is configured to execute the program instructions to implement the pose determining method in the first aspect.
In order to solve the above technical problem, a fourth aspect of the present application provides a computer-readable storage medium storing program instructions executable by a processor for implementing the pose determining method in the above first aspect.
According to the scheme, the Kalman filter equation is constructed based on the state quantity of the inertial measurement unit, the state quantity of the inertial measurement unit is corrected based on the error state quantity, the target state quantity of the inertial measurement unit is obtained, and the error state quantity is predicted based on the Kalman filter equation; determining whether to update the pose state of the odometer based on the fusion result between the target state quantity and the point cloud data of the point cloud collector; responding to the pose state of the updated odometer, and adjusting a first observed quantity in the Kalman filtering equation to obtain a new Kalman filtering equation, wherein the first observed quantity characterizes the observed quantity corresponding to the inertial measurement unit in the Kalman filtering equation; based on the new Kalman filtering equation, predicting to obtain new error state quantity, re-executing the step and subsequent steps of correcting the state quantity of the inertial measurement unit based on the error state quantity until the pose state is determined to be unnecessary to update, using the latest pose state as the target pose of the odometer, correcting the state quantity of the inertial measurement unit based on the error state quantity to obtain the target state quantity of the inertial measurement unit, contributing to improving the accuracy of the target state quantity of the inertial measurement unit, and based on the fusion result between the target state quantity and the point cloud data of the point cloud collector, determining whether to update the pose state of the odometer, adjusting a first observed quantity in the Kalman filter equation in response to determining the pose state of the odometer to obtain a new Kalman filter equation, and continuously updating to obtain the new Kalman filter equation. Therefore, the accuracy of the pose state can be improved.
It is to be understood that both the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the application.
Drawings
The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the application and, together with the description, serve to explain the technical aspects of the application.
FIG. 1 is a flow chart of an embodiment of a pose determination method of the present application;
FIG. 2 is a flow chart of another embodiment of a pose determination method of the present application;
FIG. 3 is a schematic diagram of a frame of an embodiment of a pose determination apparatus of the present application;
FIG. 4 is a schematic diagram of a framework of an embodiment of the electronic device of the present application;
FIG. 5 is a schematic diagram of a framework of one embodiment of a computer readable storage medium of the present application.
Detailed Description
The following describes the embodiments of the present application in detail with reference to the drawings.
In the following description, for purposes of explanation and not limitation, specific details are set forth such as the particular system architecture, interfaces, techniques, etc., in order to provide a thorough understanding of the present application.
The term "and/or" is herein merely an association relationship describing an associated object, meaning that there may be three relationships, e.g., a and/or B, may represent: a exists alone, A and B exist together, and B exists alone. In addition, the character "/" herein generally indicates that the front and rear associated objects are an "or" relationship. Further, "a plurality" herein means two or more than two. In addition, the term "at least one" herein means any one of a plurality or any combination of at least two of a plurality, for example, including at least one of A, B, C, and may mean including any one or more elements selected from the group consisting of A, B and C. "several" means at least one. The terms first, second and the like in the description and in the claims and in the above-described figures, are used for distinguishing between similar elements and not necessarily for describing a particular sequential or chronological order.
Referring to fig. 1, fig. 1 is a flow chart illustrating an embodiment of a pose determining method according to the present application.
Specifically, the method may include the steps of:
step S11: based on the state quantity of the inertial measurement unit, a Kalman filter equation is constructed, and based on the error state quantity, the state quantity of the inertial measurement unit is corrected to obtain the target state quantity of the inertial measurement unit.
In the embodiment of the disclosure, the error state quantity is predicted based on a Kalman filtering equation, and the Kalman filtering is to predict the system state by using a linear system state equation and observing data to obtain the error state quantity.
In one implementation scenario, the state quantity may be acquired by an Inertial Measurement Unit (IMU), and the acceleration v= (v) of the vehicle may be detected by the IMU, for example x ,v y ,v z ) And angular velocity ω= (ω) x ,ω y ,ω z ) Etc. Of course, information such as acceleration and angular velocity of the vehicle may be acquired by a gyroscope. The manner of acquiring the state quantity may be determined according to actual conditions, and is not particularly limited herein.
In one implementation, a Kalman filter equation is constructed based on the state quantities of the inertial measurement unit. Specifically, based on the state quantity of the inertial measurement unit, a kinematic equation and an error kalman filter equation are defined and initialized. Illustratively, taking noise of the inertial measurement unit readings as an error state variable of the kalman filter, the resulting continuous kinematic equation may be expressed as follows:
Figure BDA0004077528810000051
Figure BDA0004077528810000052
Figure BDA0004077528810000053
Figure BDA0004077528810000054
Figure BDA0004077528810000055
Figure BDA0004077528810000056
Wherein, in the state quantity
Figure BDA0004077528810000057
Representing the derivative of time, (. Cndot.) ∧ An antisymmetric matrix representation representing the vector, b a And b g Representing the random noise of the acceleration and inertia measurement unit. Further, since the readings of the state quantity are all discrete states, the discrete manner can be expressed as follows from the continuous kinematic equation:
δp(t+Δt)＝δp+δvΔt
Figure BDA0004077528810000058
Figure BDA0004077528810000059
δb g (t+Δt)＝δb g +η g
δb a (t+Δt)＝δb a +η a
δg(t+Δt)＝δg
after the Kalman filter equation is obtained, the error state quantity is obtained by prediction based on the Kalman filter equation. Illustratively, i is the measurement index of the inertial measurement unit frame, x is the state variable of the inertial measurement unit, and the error state equation can be expressed as:
Figure BDA00040775288100000510
where Q is the covariance matrix with respect to the error state noise w.
Further, based on the error state quantity, the state quantity of the inertial measurement unit is corrected, and the target state quantity of the inertial measurement unit is obtained. As a possible implementation, the error state quantity can be predicted by a kalman filter equation, and used as a fixed error state quantity, and the state quantity of the inertial measurement unit can be corrected by the fixed error state quantity. Different from the embodiment disclosed above, the state quantity of the inertial measurement unit may be pre-integrated to obtain a predicted state quantity, and the target state quantity of the inertial measurement unit may be obtained based on a difference between the predicted state quantity and the error state quantity. Illustratively, the jacobian matrix F may be calculated first, and may be expressed as:
Figure BDA0004077528810000061
On the basis, the state quantity of the inertial measurement unit is subjected to pre-integral calculation to obtain a predicted state quantity, and the predicted state quantity can be expressed as a self pose state, for example, can be expressed by 6-dimensional data (x, y, z, pitch angle, roll angle and yaw angle). Illustratively, the expression may be expressed as:
δx p ＝Fδx
P p ＝FPF T +Q
wherein the superscript P denotes a predicted state quantity, P p Representing the predicted covariance matrix and Q representing the system noise matrix. And obtaining the target state quantity of the inertial measurement unit based on the difference value between the predicted state quantity and the error state quantity, wherein it is understood that the predicted state quantity may have errors, so that the error state quantity is removed, and the accuracy of the target state quantity of the inertial measurement unit is further improved as much as possible. According to the mode, the state quantity of the inertial measurement unit is subjected to pre-integral calculation to obtain the predicted state quantity, the target state quantity of the inertial measurement unit is obtained based on the difference value between the predicted state quantity and the error state quantity, and the error state quantity is removed on the basis of obtaining the predicted state quantity, so that the purpose of correcting the state quantity of the inertial measurement unit is achieved, and the accuracy of the target state quantity is further improved as much as possible.
Step S12: and determining whether to update the pose state of the odometer based on a fusion result between the target state quantity and the point cloud data of the point cloud collector.
In one implementation, the point cloud data may be acquired by a point cloud acquisition device, which may employ, but is not limited to, a lidar, a binocular camera, and the like.
In one implementation scenario, to determine whether to update the pose state of the odometer, the target state quantity and the point cloud data may be fused to obtain fused data, and the obtained multi-frame fused data may be spliced to obtain the first spliced map. Further, the first spliced map and the latest point cloud data are matched to obtain a matching result, and whether to update the pose state of the odometer is determined based on whether the matching result is converged or not.
In another implementation scenario, unlike the foregoing embodiment, the point cloud data may be motion-compensated based on the target state quantity to obtain the target point cloud data, the motion distortion generated by the relative motion of the point cloud data following the device itself may be removed by performing motion compensation on the point cloud data according to the target state quantity, and the distortion may affect the accuracy of the measured point cloud data. And after the target point cloud data is obtained, obtaining a first spliced map based on the target point cloud data. As a possible implementation manner, the target point cloud data may be spliced to obtain a first spliced map, and after generating new target point cloud data, the new target point cloud data is updated to the first spliced map, so that the first spliced map is continuously updated along with the generation of the new target point cloud data. Different from the previous embodiment, whether the sliding window is filled is judged; discarding historical target point cloud data at the bottom of the sliding window in response to the sliding window being filled, and updating a second spliced map, wherein the second spliced map is spliced based on the historical target point cloud data in the sliding window before updating, so that a first spliced map is obtained; it can be understood that the historical target point cloud data exists in the sliding window, the historical target point cloud data is obtained before the sliding window is judged, and the historical target point cloud data existing in the sliding window is not generated at the same time, so that the historical target point cloud data at the bottom of the sliding window enters the sliding window first, and further the historical target point cloud data in the sliding window can be continuously updated along with the target point cloud data. Updating the second spliced map to obtain a first spliced map in response to the sliding window not being filled; specifically, when the sliding window is not filled, the target point cloud data is added to the second spliced map, so that the purpose of updating the second spliced map is achieved, and the first spliced map is obtained. According to the mode, whether the sliding window is filled is judged, so that how to update the second spliced map is determined, the first spliced map is obtained, the accuracy of the first spliced map is improved, and the instantaneity of the first spliced map is improved.
Further, the first spliced map and the latest point cloud data are matched, and whether the position and posture state of the odometer is updated or not is determined. As a possible implementation manner, the first spliced map and the latest point cloud data are matched, it can be understood that the first spliced map is obtained by splicing a plurality of target point cloud data, so that historical target point cloud data in the first spliced map and the latest point cloud data are matched, the ratio of successfully matched data in the target point cloud data is obtained, the ratio is compared with a preset threshold, when the ratio is greater than the preset threshold, the pose state of the odometer is updated, otherwise, the fusion result between the target state quantity and the point cloud data of the point cloud collector is re-executed, and whether the pose state of the odometer is updated or not and the subsequent steps are determined. Different from the previous embodiment, the first spliced map and the latest point cloud data are matched, and a matching result is obtained. It can be understood that the first spliced map is obtained by splicing based on the cloud data of the target point, the cloud data is updated based on the acquisition frequency of the cloud acquisition device, the first spliced map and the latest cloud data are matched to obtain a matching result, the matching result can be the successful match ratio or the successful match first numerical value, and the matching result can be determined according to the actual situation without specific limitation. And determining whether to update the pose state of the odometer based on whether the matching result is converged. And under the condition that the matching result is not converged, re-executing the fusion result between the target state quantity and the point cloud data of the point cloud collector, and determining whether to update the pose state of the odometer or not and the follow-up steps. And then, the matching result can be obtained by matching the first spliced map with the latest point cloud data, and whether the pose state of the odometer is updated or not is determined based on whether the matching result is converged or not, so that the improvement of the pose state of the odometer is facilitated. According to the method, the point cloud data are subjected to motion compensation based on the target state quantity to obtain the target point cloud data, fusion of the target state quantity and the point cloud data is facilitated, the first spliced map is obtained through the target point cloud data, instantaneity of the first spliced map is improved, the first spliced map is matched with the latest point cloud data, whether the odometer pose state is updated or not is determined, and accuracy of the odometer pose state is improved.
Step S13: and in response to determining the pose state of the updated odometer, adjusting the first observed quantity in the Kalman filtering equation to obtain a new Kalman filtering equation.
In an embodiment of the disclosure, the first observed quantity characterizes an observed quantity corresponding to the inertial measurement unit in the kalman filter equation. It may be appreciated that the first observed quantity may be a pose offset obtained by matching the point cloud data, and for example, the pose offset in the first spliced map and the latest point cloud data may be obtained, and the pose offset may be represented by 6-dimensional data (x, y, z, pitch angle, roll angle, yaw angle), that is, the pose offset between the latest point cloud data and the corresponding historical target point cloud data in the first spliced map.
In one implementation scenario, a first observed quantity in a Kalman filter equation is adjusted to obtain a new Kalman filter equation under the condition that the pose state of the updated odometer is determined. Specifically, in the case of determining the pose state of the updated odometer, the first observed quantity of the kalman filter equation may be adjusted by the matching result of the first stitched map and the latest point cloud data, that is, the accuracy (Fitness) of the matching result, and the first observed quantity may be a covariance matrix of the observed noise, and the higher the matching result accuracy is, the smaller the covariance matrix of the observed noise is, and the lower the matching result accuracy is, the larger the covariance matrix of the observed noise is. Illustratively, it may be represented as follows:
Figure BDA0004077528810000091
Where z is the observed data, N is the observed noise, and N is the covariance matrix of the observed noise.
In one implementation scenario, positioning data of the positioning device may be obtained, where the positioning data may be latitude and longitude information and attitude angle information of the device, and furthermore, the positioning device may employ, but is not limited to, an RTK gauge, a GPS gauge, and the like, which are not specifically limited herein. And based on the positioning data, adjusting a second observed quantity in the latest Kalman filtering equation, wherein the second observed quantity can be a covariance matrix of observed noise, and the second observed quantity characterizes the observed quantity corresponding to the positioning equipment in the Kalman filtering equation. The positioning device may be an RTK measuring instrument, and may determine a state bit output by the RTK measuring instrument, reduce a covariance matrix of the observation noise in a case that the state bit is a fixed solution, and increase the covariance matrix of the observation noise in a case that the state bit is a floating solution or a single-point solution. It can be understood that the state bit output by the RTK measuring instrument is a fixed solution representation and calculates the correct position coordinate, the precision is in the centimeter level, the state bit output by the RTK measuring instrument is a floating solution representation, the fixed solution is not calculated yet, the precision is in the centimeter-meter level, the state bit output by the RTK measuring instrument is a single-point solution representation RTK receiver which does not receive the differential signal, and the precision is in the meter level only by using the satellite positioning result as output. By adjusting the second observed quantity in the latest Kalman filter equation based on the positioning data, the accuracy of the second observed quantity in the latest Kalman filter equation is improved.
Step S14: based on the new Kalman filtering equation, predicting to obtain new error state quantity, and re-executing the steps and subsequent steps of correcting the state quantity of the inertial measurement unit based on the error state quantity until the pose state is determined to be unnecessary to update, and taking the latest pose state as the target pose of the odometer.
In one implementation scenario, detecting whether an observed quantity in a new Kalman filter equation is adjusted; in response to the observed quantity in the new Kalman filter equation having been adjusted, a new error state quantity is predicted based on the new Kalman filter equation. Specifically, whether the first observed quantity or the second observed quantity in the new kalman filter equation is adjusted can be detected, and if at least one of the first observed quantity and the second observed quantity is adjusted, a new error state quantity is predicted based on the new kalman filter equation. Illustratively, the Jacobian matrix of the observation equation as compared to the error state may be obtained first, and the expression may be expressed as follows:
Figure BDA0004077528810000101
further, the kalman gain matrix is calculated, and the new error state quantity is expressed as follows:
K＝P p H T (HP p H T +N) -1
δx＝K[z-h(x)]
P＝(I-KH)P p
where K is the kalman gain, P is the covariance matrix of the adjusted observation noise, δx is the new error state quantity. And in response to the observed quantity in the new Kalman filtering equation not being adjusted, re-executing a fusion result between the point cloud data based on the target state quantity and the point cloud collector, and determining whether to update the pose state of the odometer and the follow-up steps. Specifically, the fusion result between the point cloud data based on the target state quantity and the point cloud collector can be re-executed, whether the pose state of the odometer is updated or not is determined until a new error state quantity is obtained through prediction based on a new Kalman filtering equation, the state quantity of the inertial measurement unit is corrected through the new error state quantity, the target state quantity of the inertial measurement unit is obtained, and the pose state of the odometer is continuously circulated and updated. According to the mode, whether the observed quantity in the new Kalman filtering equation is adjusted or not is detected, whether the new error state quantity is predicted or not is further determined, and the accuracy of the new error state quantity is improved.
In one implementation scenario, a new error state quantity is predicted based on a new kalman filter equation, and the steps of correcting the state quantity of the inertial measurement unit and the subsequent steps based on the error state quantity are re-executed until the state of the pose is determined to be unnecessary to update, and the latest pose state is used as the target pose of the odometer.
According to the scheme, the Kalman filter equation is constructed based on the state quantity of the inertial measurement unit, the state quantity of the inertial measurement unit is corrected based on the error state quantity, the target state quantity of the inertial measurement unit is obtained, and the error state quantity is predicted based on the Kalman filter equation; determining whether to update the pose state of the odometer based on the fusion result between the target state quantity and the point cloud data of the point cloud collector; responding to the pose state of the updated odometer, and adjusting a first observed quantity in the Kalman filtering equation to obtain a new Kalman filtering equation, wherein the first observed quantity characterizes the observed quantity corresponding to the inertial measurement unit in the Kalman filtering equation; based on the new Kalman filtering equation, predicting to obtain new error state quantity, re-executing the step and subsequent steps of correcting the state quantity of the inertial measurement unit based on the error state quantity until the pose state is determined to be unnecessary to update, using the latest pose state as the target pose of the odometer, correcting the state quantity of the inertial measurement unit based on the error state quantity to obtain the target state quantity of the inertial measurement unit, contributing to improving the accuracy of the target state quantity of the inertial measurement unit, and based on the fusion result between the target state quantity and the point cloud data of the point cloud collector, determining whether to update the pose state of the odometer, adjusting a first observed quantity in the Kalman filter equation in response to determining the pose state of the odometer to obtain a new Kalman filter equation, and continuously updating to obtain the new Kalman filter equation. Therefore, the accuracy of the pose state can be improved.
Referring to fig. 2, fig. 2 is a schematic flow chart of another embodiment of the pose determining method of the present application, an IMU state quantity may be obtained through an IMU, where the IMU state quantity may include information such as acceleration, angular velocity, etc., a pre-integration calculation is performed on the IMU state quantity to obtain a predicted state quantity, and a target state quantity of an inertial measurement unit is obtained based on a difference value between the predicted state quantity and an error state quantity; and acquiring point cloud data through a laser radar, performing motion compensation on the point cloud data based on the target state quantity to obtain target point cloud data, removing dynamic point cloud based on the target point cloud data, and splicing the point cloud data from which the dynamic point cloud is removed to obtain a first spliced map. Specifically, judging whether the sliding window is filled; discarding historical target point cloud data at the bottom of the sliding window in response to the sliding window being filled, and updating a second spliced map to obtain a first spliced map, wherein the second spliced map is spliced based on the historical target point cloud data in the sliding window before updating; and in response to the sliding window not being filled, updating the second spliced map to obtain the first spliced map. Matching the first spliced map with the latest point cloud data to obtain a matching result, judging whether the matching result is converged, re-executing a fusion result between the point cloud data based on the target state quantity and the point cloud collector under the condition that the matching result is not converged, determining whether to update the pose state of the odometer, re-obtaining a new matching result and judging whether the new matching result is converged until the matching result is converged; and under the condition that the matching result is converged, updating the pose state of the odometer, and further adjusting the first observed quantity in the Kalman filtering equation. In addition, positioning data is obtained through the RTK, the positioning data can be longitude and latitude coordinates, a second observed quantity in a Kalman filtering equation is adjusted through the longitude and latitude coordinates, the second observed quantity can be a covariance matrix of observation noise, specifically, the longitude and latitude coordinates can be judged, the covariance matrix of the observation noise is reduced under the condition that the longitude and latitude coordinates are fixed solutions, and the covariance matrix of the observation noise is increased under the condition that the longitude and latitude coordinates are floating solutions or single-point solutions. It will be appreciated that a new Kalman filter equation may be obtained after the first or second observables have been adjusted in the Kalman filter equation. Further, whether the first observed quantity or the second observed quantity in the new kalman filter equation is adjusted or not can be detected, if at least one of the first observed quantity and the second observed quantity is adjusted, a new error state quantity is predicted based on the new kalman filter equation, and the step and the subsequent steps of correcting the state quantity of the inertial measurement unit based on the error state quantity are re-executed until the state of the pose is determined to be unnecessary to update, and the latest pose state is taken as the target pose of the odometer.
According to the scheme, the Kalman filter equation is constructed based on the state quantity of the inertial measurement unit, the state quantity of the inertial measurement unit is corrected based on the error state quantity, the target state quantity of the inertial measurement unit is obtained, and the error state quantity is predicted based on the Kalman filter equation; determining whether to update the pose state of the odometer based on the fusion result between the target state quantity and the point cloud data of the point cloud collector; responding to the pose state of the updated odometer, and adjusting a first observed quantity in the Kalman filtering equation to obtain a new Kalman filtering equation, wherein the first observed quantity characterizes the observed quantity corresponding to the inertial measurement unit in the Kalman filtering equation; based on the new Kalman filtering equation, predicting to obtain new error state quantity, re-executing the step and subsequent steps of correcting the state quantity of the inertial measurement unit based on the error state quantity until the pose state is determined to be unnecessary to update, using the latest pose state as the target pose of the odometer, correcting the state quantity of the inertial measurement unit based on the error state quantity to obtain the target state quantity of the inertial measurement unit, contributing to improving the accuracy of the target state quantity of the inertial measurement unit, and based on the fusion result between the target state quantity and the point cloud data of the point cloud collector, determining whether to update the pose state of the odometer, adjusting a first observed quantity in the Kalman filter equation in response to determining the pose state of the odometer to obtain a new Kalman filter equation, and continuously updating to obtain the new Kalman filter equation. Therefore, the accuracy of the pose state can be improved.
It will be appreciated by those skilled in the art that in the above-described method of the specific embodiments, the written order of steps is not meant to imply a strict order of execution but rather should be construed according to the function and possibly inherent logic of the steps.
Referring to fig. 3, fig. 3 is a schematic frame diagram of an embodiment of a pose determining apparatus according to the present application. The pose determination device 30 includes a construction module 31, an update module 32, an adjustment module 33, and a prediction module 34. The construction module 31 is configured to construct a kalman filter equation based on the state quantity of the inertial measurement unit, and correct the state quantity of the inertial measurement unit based on the error state quantity to obtain a target state quantity of the inertial measurement unit; the error state quantity is obtained through prediction based on a Kalman filtering equation; the updating module 32 is configured to determine whether to update the pose state of the odometer based on a fusion result between the target state quantity and the point cloud data of the point cloud collector; the adjustment module 33 is configured to adjust the first observed quantity in the kalman filter equation in response to determining to update the pose state of the odometer, so as to obtain a new kalman filter equation; wherein, the first observed quantity characterizes the observed quantity corresponding to the inertial measurement unit in the Kalman filtering equation; the prediction module 34 is configured to predict a new error state quantity based on a new kalman filter equation, and re-execute a step of correcting the state quantity of the inertial measurement unit based on the error state quantity and a subsequent step until it is determined that the pose state does not need to be updated, and take the latest pose state as a target pose of the odometer.
According to the scheme, on one hand, the state quantity of the inertial measurement unit is corrected based on the error state quantity, the accuracy of the target state quantity of the inertial measurement unit is improved, on the other hand, whether the pose state of the odometer is updated or not is determined based on the fusion result between the target state quantity and the point cloud data of the point cloud collector, and in response to the determination of the pose state of the odometer updating, the first observed quantity in the Kalman filter equation is adjusted to obtain a new Kalman filter equation, and further the new Kalman filter equation can be continuously updated, on the basis, the new Kalman filter equation is obtained, the new error state quantity is predicted based on the new Kalman filter equation, and the steps of correcting the state quantity of the inertial measurement unit and the subsequent steps are repeatedly executed based on the error state quantity until the pose state is determined to be unnecessary to update, and the latest pose state is used as the target pose of the odometer, so that errors can be reduced through repeated circulation. Therefore, the accuracy of the pose state can be improved.
In some disclosed embodiments, the update module 32 includes a fusion sub-module, a determination sub-module, and a match sub-module. The fusion sub-module is used for performing motion compensation on the point cloud data based on the target state quantity to obtain target point cloud data; the determining submodule is used for obtaining a first spliced map based on the cloud data of the target point; and the matching submodule is used for matching the first spliced map with the latest point cloud data and determining whether to update the position and posture state of the odometer.
Therefore, the target point cloud data are obtained by performing motion compensation on the point cloud data based on the target state quantity, fusion of the target state quantity and the point cloud data is facilitated, the first spliced map is obtained through the target point cloud data, instantaneity of the first spliced map is facilitated to be improved, the first spliced map is matched with the latest point cloud data, whether the odometer pose state is updated or not is determined, and accuracy of the odometer pose state is further improved.
In some disclosed embodiments, the determining submodule includes a judging unit for judging whether the sliding window is filled; discarding historical target point cloud data at the bottom of the sliding window in response to the sliding window being filled, and updating the second spliced map to obtain a first spliced map; the second spliced map is spliced based on historical target point cloud data in the sliding window before updating; and in response to the sliding window not being filled, updating the second spliced map to obtain the first spliced map.
Therefore, whether the sliding window is filled is judged, so that how to update the second spliced map is determined, the first spliced map is obtained, the accuracy of the first spliced map is improved, and the instantaneity of the first spliced map is improved.
In some disclosed embodiments, the matching submodule includes a matching unit and a determining unit. The matching unit is used for matching the first spliced map with the latest point cloud data to obtain a matching result; the determining unit is used for determining whether to update the pose state of the odometer based on whether the matching result is converged.
Therefore, the matching result is obtained by matching the first spliced map and the latest point cloud data, and whether the pose state of the odometer is updated or not is determined based on whether the matching result is converged or not, so that the pose state of the odometer is improved.
In some disclosed embodiments, the pose determination device 30 includes an acquisition module and a data adjustment module. The acquisition module is used for acquiring positioning data of the positioning equipment; the data adjustment module is used for adjusting the second observed quantity in the latest Kalman filtering equation based on the positioning data; wherein the second observed quantity characterizes an observed quantity corresponding to the positioning device in the Kalman filtering equation.
Therefore, by adjusting the second observed quantity in the latest kalman filter equation based on the positioning data, the accuracy of the second observed quantity in the latest kalman filter equation is facilitated to be improved.
In some disclosed embodiments, the prediction module 34 includes a detection sub-module for detecting whether the observed quantity in the new Kalman filter equation is adjusted; in response to the observed quantity in the new Kalman filtering equation being adjusted, predicting to obtain a new error state quantity based on the new Kalman filtering equation; and in response to the observed quantity in the new Kalman filtering equation not being adjusted, re-executing a fusion result between the point cloud data based on the target state quantity and the point cloud collector, and determining whether to update the pose state of the odometer and the follow-up steps.
Therefore, by detecting whether the observed quantity in the new Kalman filtering equation is adjusted, whether the new error state quantity is predicted is determined, and the accuracy of the new error state quantity is improved.
In some disclosed embodiments, the build module 31 includes a calculation sub-module and a determination sub-module. The calculating submodule is used for carrying out pre-integral calculation on the state quantity of the inertial measurement unit to obtain a predicted state quantity; the determining submodule is used for obtaining a target state quantity of the inertial measurement unit based on a difference value between the predicted state quantity and the error state quantity.
Therefore, the state quantity of the inertial measurement unit is pre-integrated to obtain a predicted state quantity, the target state quantity of the inertial measurement unit is obtained based on the difference value between the predicted state quantity and the error state quantity, and the error state quantity is removed on the basis of obtaining the predicted state quantity, so that the aim of correcting the state quantity of the inertial measurement unit is fulfilled, and the accuracy of the target state quantity is further improved as much as possible.
Referring to fig. 4, fig. 4 is a schematic frame diagram of an embodiment of the electronic device of the present application. The electronic device 40 comprises a memory 41 and a processor 42 coupled to each other, the memory 41 having stored therein program instructions, the processor 42 being adapted to execute the program instructions to implement the steps of any of the above-described pose determination method embodiments. In particular, electronic device 40 may include, but is not limited to: desktop computers, notebook computers, servers, cell phones, tablet computers, and the like, are not limited herein.
Specifically, the processor 42 is configured to control itself and the memory 41 to implement the steps in any of the above-described pose determination method embodiments. The processor 42 may also be referred to as a CPU (Central Processing Unit ). The processor 42 may be an integrated circuit chip having signal processing capabilities. The processor 42 may also be a general purpose processor, a digital signal processor (Digital Signal Processor, DSP), an application specific integrated circuit (Application Specific Integrated Circuit, ASIC), a Field programmable gate array (Field-Programmable Gate Array, FPGA) or other programmable logic device, discrete gate or transistor logic device, discrete hardware components. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like. In addition, the processor 42 may be commonly implemented by an integrated circuit chip.
In the above-mentioned scheme, the electronic device 40 may be configured to implement the steps in any one of the embodiments of the pose determining method, on the one hand, correct the state quantity of the inertial measurement unit based on the error state quantity, to obtain the target state quantity of the inertial measurement unit, so as to help to improve the accuracy of the target state quantity of the inertial measurement unit, and on the other hand, determine whether to update the pose state of the odometer based on the fusion result between the target state quantity and the point cloud data of the point cloud collector, and in response to determining to update the pose state of the odometer, adjust the first observed quantity in the kalman filter equation, so as to obtain a new kalman filter equation, and further, can continuously update to obtain a new kalman filter equation, on the basis of this new kalman filter equation, predict to obtain a new error state quantity, and re-execute the steps and subsequent steps based on the error state quantity, so as to correct the state quantity of the inertial measurement unit, until it is determined that the pose state does not need to be updated, so as to help to reduce the error by repeating the cycle. Therefore, the accuracy of the pose state can be improved.
Referring to fig. 5, fig. 5 is a schematic diagram illustrating an embodiment of a computer readable storage medium according to the present application. The computer readable storage medium 50 stores program instructions 51 executable by the processor, the program instructions 51 for implementing the steps in any of the above-described pose determination method embodiments.
In the above-mentioned aspect, the computer readable storage medium 50 may be configured to implement the steps in any one of the embodiments of the pose determining method, on the one hand, correct the state quantity of the inertial measurement unit based on the error state quantity, to obtain the target state quantity of the inertial measurement unit, so as to help improve the accuracy of the target state quantity of the inertial measurement unit, and on the other hand, determine whether to update the pose state of the odometer based on the fusion result between the target state quantity and the point cloud data of the point cloud collector, and in response to determining to update the pose state of the odometer, adjust the first observed quantity in the kalman filter equation to obtain a new kalman filter equation, so as to continuously update to obtain the new kalman filter equation, on the basis of this new kalman filter equation, predict to obtain the new error state quantity, and re-execute the steps and subsequent steps based on the error state quantity, correct the state quantity of the inertial measurement unit until it is determined that the pose state does not need to be updated, so as to help to reduce the error by repeating the cycle. Therefore, the accuracy of the pose state can be improved.
In some embodiments, functions or modules included in an apparatus provided by the embodiments of the present disclosure may be used to perform a method described in the foregoing method embodiments, and specific implementations thereof may refer to descriptions of the foregoing method embodiments, which are not repeated herein for brevity.
The foregoing description of various embodiments is intended to highlight differences between the various embodiments, which may be the same or similar to each other by reference, and is not repeated herein for the sake of brevity.
In the several embodiments provided in the present application, it should be understood that the disclosed methods and apparatus may be implemented in other manners. For example, the apparatus embodiments described above are merely illustrative, e.g., the division of modules or units is merely a logical functional division, and there may be additional divisions when actually implemented, e.g., multiple units or components may be combined or integrated into another system, or some features may be omitted or not performed. Alternatively, the coupling or direct coupling or communication connection shown or discussed with each other may be an indirect coupling or communication connection via some interfaces, devices or units, which may be in electrical, mechanical, or other forms.
The units described as separate units may or may not be physically separate, and units shown as units may or may not be physical units, may be located in one place, or may be distributed over a plurality of network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the embodiment.
In addition, each functional unit in each embodiment of the present application may be integrated in one processing unit, or each unit may exist alone physically, or two or more units may be integrated in one unit. The integrated units may be implemented in hardware or in software functional units.
The integrated units, if implemented in the form of software functional units and sold or used as stand-alone products, may be stored in a computer readable storage medium. Based on such understanding, the technical solution of the present application may be embodied essentially or in part or all or part of the technical solution contributing to the prior art or in the form of a software product stored in a storage medium, including several instructions to cause a computer device (which may be a personal computer, a server, or a network device, etc.) or a processor (processor) to perform all or part of the steps of the methods of the embodiments of the present application. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a random access Memory (RAM, random Access Memory), a magnetic disk, or an optical disk, or other various media capable of storing program codes.
If the technical scheme of the application relates to personal information, the product applying the technical scheme of the application clearly informs the personal information processing rule before processing the personal information, and obtains independent consent of the individual. If the technical scheme of the application relates to sensitive personal information, the product applying the technical scheme of the application obtains individual consent before processing the sensitive personal information, and simultaneously meets the requirement of 'explicit consent'. For example, a clear and remarkable mark is set at a personal information acquisition device such as a camera to inform that the personal information acquisition range is entered, personal information is acquired, and if the personal voluntarily enters the acquisition range, the personal information is considered as consent to be acquired; or on the device for processing the personal information, under the condition that obvious identification/information is utilized to inform the personal information processing rule, personal authorization is obtained by popup information or a person is requested to upload personal information and the like; the personal information processing rule may include information such as a personal information processor, a personal information processing purpose, a processing mode, and a type of personal information to be processed.

Claims (10)

1. The pose determining method is characterized by comprising the following steps of:
Based on the state quantity of the inertial measurement unit, constructing a Kalman filtering equation, and based on the error state quantity, correcting the state quantity of the inertial measurement unit to obtain a target state quantity of the inertial measurement unit; the error state quantity is predicted based on the Kalman filtering equation;
determining whether to update the pose state of the odometer based on the fusion result between the target state quantity and the point cloud data of the point cloud collector;
responding to the determination of updating the pose state of the odometer, and adjusting a first observed quantity in the Kalman filtering equation to obtain a new Kalman filtering equation; wherein the first observed quantity characterizes an observed quantity corresponding to the inertial measurement unit in the Kalman filtering equation;
and predicting to obtain a new error state quantity based on the new Kalman filtering equation, and re-executing the step and the subsequent steps of correcting the state quantity of the inertial measurement unit based on the error state quantity until the pose state is determined to be unnecessary to update, wherein the latest pose state is used as the target pose of the odometer.
2. The method of claim 1, wherein determining whether to update the pose state of the odometer based on a fusion result between the target state quantity and the point cloud data of the point cloud collector comprises:
Performing motion compensation on the point cloud data based on the target state quantity to obtain target point cloud data;
acquiring a first spliced map based on the target point cloud data;
and matching the first spliced map with the latest point cloud data, and determining whether to update the position and posture state of the odometer.
3. The method of claim 2, wherein the obtaining a first stitched map based on the target point cloud data comprises:
judging whether the sliding window is filled up;
discarding historical target point cloud data at the bottom of the sliding window in response to the sliding window being filled, and updating a second spliced map to obtain the first spliced map; the second spliced map is spliced based on the historical target point cloud data in the sliding window before updating;
and updating the second spliced map to obtain the first spliced map in response to the sliding window not being filled.
4. The method of claim 2, wherein the matching the first stitched map with the most current point cloud data to determine whether to update an odometer pose state comprises:
matching the first spliced map with the latest point cloud data to obtain a matching result;
And determining whether to update the pose state of the odometer based on whether the matching result is converged.
5. The method of claim 1, wherein prior to predicting a new state quantity of error based on the new kalman filter equation, the method further comprises:
acquiring positioning data of positioning equipment;
based on the positioning data, adjusting a second observed quantity in the latest Kalman filtering equation; and the second observed quantity characterizes the observed quantity corresponding to the positioning equipment in the Kalman filtering equation.
6. The method according to claim 1 or 5, wherein predicting a new error state quantity based on the new kalman filter equation comprises:
detecting whether the observed quantity in the new Kalman filtering equation is adjusted;
in response to the observed quantity in the new Kalman filtering equation having been adjusted, predicting a new error state quantity based on the new Kalman filtering equation;
and in response to the observed quantity in the new Kalman filtering equation not being adjusted, re-executing the fusion result between the target state quantity and the point cloud data of the point cloud collector, and determining whether to update the pose state of the odometer or not and the follow-up steps.
7. The method of claim 1, wherein correcting the state quantity of the inertial measurement unit based on the error state quantity to obtain the target state quantity of the inertial measurement unit comprises:
performing pre-integral calculation on the state quantity of the inertial measurement unit to obtain a predicted state quantity;
and obtaining the target state quantity of the inertial measurement unit based on the difference value between the predicted state quantity and the error state quantity.
8. A pose determination apparatus, characterized by comprising:
the construction module is used for constructing a Kalman filtering equation based on the state quantity of the inertial measurement unit, correcting the state quantity of the inertial measurement unit based on the error state quantity and obtaining the target state quantity of the inertial measurement unit; the error state quantity is predicted based on the Kalman filtering equation;
the updating module is used for determining whether to update the pose state of the odometer based on the fusion result between the target state quantity and the point cloud data of the point cloud collector;
the adjustment module is used for adjusting the first observed quantity in the Kalman filtering equation to obtain a new Kalman filtering equation in response to determining to update the pose state of the odometer; wherein the first observed quantity characterizes an observed quantity corresponding to the inertial measurement unit in the Kalman filtering equation;
And the prediction module is used for predicting and obtaining a new error state quantity based on the new Kalman filtering equation, and re-executing the step and the subsequent steps of correcting the state quantity of the inertial measurement unit based on the error state quantity until the pose state is determined to be unnecessary to update, and taking the latest pose state as the target pose of the odometer.
9. An electronic device comprising a memory and a processor coupled to each other, the memory having stored therein program instructions for executing the program instructions to implement the pose determination method according to any of claims 1 to 7.
10. A computer-readable storage medium, characterized in that program instructions executable by a processor for implementing the pose determination method according to any of claims 1 to 7 are stored.
CN202310111826.4A 2023-01-16 2023-01-16 Pose determining method and related device, electronic equipment and storage medium Pending CN116045976A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310111826.4A CN116045976A (en) 2023-01-16 2023-01-16 Pose determining method and related device, electronic equipment and storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310111826.4A CN116045976A (en) 2023-01-16 2023-01-16 Pose determining method and related device, electronic equipment and storage medium

Publications (1)

Publication Number Publication Date
CN116045976A true CN116045976A (en) 2023-05-02

Family

ID=86118161

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310111826.4A Pending CN116045976A (en) 2023-01-16 2023-01-16 Pose determining method and related device, electronic equipment and storage medium

Country Status (1)

Country Link
CN (1) CN116045976A (en)

Similar Documents

Publication Publication Date Title
CN111721289B (en) Vehicle positioning method, device, equipment, storage medium and vehicle in automatic driving
EP2400269B1 (en) Track information generating device, track information generating method, and computer-readable storage medium
EP1489381B1 (en) Method and apparatus for compensating for acceleration errors and inertial navigation system employing the same
CN112577521B (en) Combined navigation error calibration method and electronic equipment
CN112781586B (en) Pose data determination method and device, electronic equipment and vehicle
CN113916243A (en) Vehicle positioning method, device, equipment and storage medium for target scene area
CN110715659A (en) Zero-speed detection method, pedestrian inertial navigation method, device and storage medium
CN109059907A (en) Track data processing method, device, computer equipment and storage medium
CN113933818A (en) Method, device, storage medium and program product for calibrating laser radar external parameter
CN110637209B (en) Method, apparatus and computer readable storage medium having instructions for estimating a pose of a motor vehicle
KR20210013526A (en) Apparatus and method for terrain aided navigation using inertial position
CN113465628A (en) Inertial measurement unit data compensation method and system
KR20220058901A (en) Data processing methods and devices
CN110989619B (en) Method, apparatus, device and storage medium for locating objects
CN114396943A (en) Fusion positioning method and terminal
CN113566850B (en) Method and device for calibrating installation angle of inertial measurement unit and computer equipment
CN112946681B (en) Laser radar positioning method fusing combined navigation information
CN116242373A (en) High-precision navigation positioning method and system for fusing multi-source data
CN115143954B (en) Unmanned vehicle navigation method based on multi-source information fusion
CN115727871A (en) Track quality detection method and device, electronic equipment and storage medium
CN116182905A (en) Laser radar and combined inertial navigation space-time external parameter calibration method, device and system
CN114001730B (en) Fusion positioning method, fusion positioning device, computer equipment and storage medium
CN116045976A (en) Pose determining method and related device, electronic equipment and storage medium
CN115560744A (en) Robot, multi-sensor-based three-dimensional mapping method and storage medium
CN114915913A (en) UWB-IMU combined indoor positioning method based on sliding window factor graph

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination