CN115597595A - Multi-line laser positioning method and positioning device, computer equipment and storage medium - Google Patents

Multi-line laser positioning method and positioning device, computer equipment and storage medium Download PDF

Info

Publication number
CN115597595A
CN115597595A CN202211496783.8A CN202211496783A CN115597595A CN 115597595 A CN115597595 A CN 115597595A CN 202211496783 A CN202211496783 A CN 202211496783A CN 115597595 A CN115597595 A CN 115597595A
Authority
CN
China
Prior art keywords
state
error
data
value
imu
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.)
Granted
Application number
CN202211496783.8A
Other languages
Chinese (zh)
Other versions
CN115597595B (en
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.)
Shanghai Xiangong Intelligent Technology Co ltd
Original Assignee
Shanghai Xiangong Intelligent 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 Shanghai Xiangong Intelligent Technology Co ltd filed Critical Shanghai Xiangong Intelligent Technology Co ltd
Priority to CN202211496783.8A priority Critical patent/CN115597595B/en
Publication of CN115597595A publication Critical patent/CN115597595A/en
Application granted granted Critical
Publication of CN115597595B publication Critical patent/CN115597595B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • G01C21/165—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 combined with non-inertial navigation instruments
    • G01C21/1652—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 combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • 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/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Navigation (AREA)

Abstract

The invention provides a multi-line laser positioning method, a positioning device, computer equipment and a storage medium, wherein the method comprises the following steps: s100, defining the state component of IMU data, system input data and system process noise, and establishing a system state model; step S200, initializing the model and establishing a covariance matrix of an error state; step S300 prediction phase: substituting the time interval data of IMU data and system input data into the initialized system state model, calculating the state prediction value of each IMU data, and updating the covariance matrix of the error state; step S400, calculating a residual error of a laser data point on a corresponding plane in the map point cloud under a current state predicted value according to the observation model, and if the residual error is not zero, judging that an error exists between the state predicted value and a true value, wherein the error is an error state; and carrying out iterative calculation by an error state Kalman filtering method until the value of the error state reaches the standard, and outputting a corresponding state component according to the value, thereby improving the positioning calculation precision.

Description

Multi-line laser positioning method and positioning device, computer equipment and storage medium
Technical Field
The invention relates to a robot positioning technology, in particular to a positioning method adopting tight coupling of multi-line laser and an IMU, a positioning device, computer equipment and a storage medium suitable for operating the method.
Background
When positioning is performed in a point cloud map constructed in advance by using a multi-line laser in an industrial scene, point cloud registration is generally performed by using an NDT (Normal Distribution point cloud algorithm), during registration, a nearest robot pose (namely, a position and an orientation of a robot in a current map coordinate system) is required to be used as an initial value, and after registration, a registration result and a milemeter are fused to obtain a current robot pose.
Therefore, the initial value of NDT registration comes from the pose of the robot after the odometer is fused, and if the vehicle slips, the NDT registration is inevitably influenced greatly, even registration failure is caused, and positioning loss is caused.
For the inventor, the prior art has proposed a "positioning method for tightly coupling a laser radar and an IMU" (chinese patent application No. 202010197086.7), which estimates the real-time motion state of a system by an IMU, extracts a plane feature by point cloud data of the laser radar for tracking, and corrects the motion state estimated by the IMU by using an observation model of the plane feature. In addition, the method utilizes the MSCKF framework to realize advantage complementation of the laser radar sensor and the IMU sensor, on one hand, the high-frequency motion estimation of the IMU can estimate the real-time state of the system, and on the other hand, the plane features extracted by the laser radar point cloud can be robustly tracked in the environment, so that the system state can be corrected at regular time. Therefore, the problem that the positioning is influenced by the slipping of the vehicle body can be solved to a certain extent.
However, the prior art also has some problems, such as: the plane detection algorithm adopted in the technology has higher complexity, if point cloud spherical projection is needed, the normal vector of each point is calculated, the growth clustering algorithm is executed, and then the PCA is used for calculating plane parameters. In addition, in the technology, the pose result is corrected by using the plane characteristics in a plurality of adjacent states in the sliding window as constraints, and the pose result is actually in a local positioning state, so that the positioning precision is improved to a certain extent, and the global consistent positioning cannot be ensured.
Disclosure of Invention
Therefore, the main objective of the present invention is to provide a method and a device for positioning a multi-line laser, a computer device, and a storage medium, so as to solve the above-mentioned drawbacks of the prior art.
In order to achieve the above object, according to a first aspect of the present invention, there is provided a multiline laser positioning method comprising the steps of:
step S100, defining the state components of IMU data, system input data and system process noise so as to establish a system state model according to the change of each state component caused by the system input data;
step S200, setting an initial value of a system state model to initialize the system state model and establishing a covariance matrix of an error state;
step S300 prediction phase: performing Kalman filtering standard prediction calculation on each received IMU data, wherein the method comprises the following steps: substituting the time interval data of IMU data and system input data into the initialized system state model, calculating the state predicted value of each IMU data, and updating the covariance matrix of the error state;
step S400, an observation stage: calculating a residual error of a laser data point on a corresponding plane in the map point cloud under the current state predicted value according to the observation model, and if the residual error is not zero, judging that an error exists between the state predicted value and a true value, wherein the error is an error state; and then, carrying out iterative calculation by an error state Kalman filtering method until the value of the error state reaches the standard, and outputting a corresponding state component according to the value of the error state reaching the standard.
In a possible preferred embodiment, in step S100, the defined status components of IMU data include: location of IMU in current map coordinate system
Figure 857947DEST_PATH_IMAGE001
And posture of the same
Figure 630731DEST_PATH_IMAGE002
Speed, velocity
Figure 577958DEST_PATH_IMAGE003
Angular velocity bias
Figure 221429DEST_PATH_IMAGE004
Acceleration bias
Figure 353333DEST_PATH_IMAGE005
Gravity vector under global coordinate system
Figure 957621DEST_PATH_IMAGE006
(ii) a And each of the above individual states is in a smooth flow pattern, combining all the states into one state variable:
Figure 833173DEST_PATH_IMAGE007
the composite flow pattern in which the state variable is located is:
Figure 596730DEST_PATH_IMAGE008
(ii) a Wherein
Figure 7857DEST_PATH_IMAGE009
For a particular orthogonal group of three dimensions, 3D rotations can be represented by the group,
Figure 489654DEST_PATH_IMAGE010
Is a 3-dimensional European space,
Figure 637739DEST_PATH_IMAGE011
Is 2-sphere, which is a hemisphere
Figure 396747DEST_PATH_IMAGE012
The definition is as follows:
Figure 604875DEST_PATH_IMAGE013
the system input data includes: angular velocity under IMU's own coordinate system
Figure 183755DEST_PATH_IMAGE014
Acceleration of the vehicle
Figure 401109DEST_PATH_IMAGE015
Then the system input data is:
Figure 873679DEST_PATH_IMAGE016
；
the system process noise includes: angular velocity measurement noise
Figure 393653DEST_PATH_IMAGE017
Acceleration measurement noise
Figure 850042DEST_PATH_IMAGE018
Bias random walk process noise of gyroscope
Figure 716761DEST_PATH_IMAGE019
Bias random walk process noise of accelerometer
Figure 574995DEST_PATH_IMAGE020
Then the system process noise is
Figure 124925DEST_PATH_IMAGE021
；
The system state model is as follows:
Figure 412818DEST_PATH_IMAGE022
wherein
Figure 971976DEST_PATH_IMAGE023
Is 18-dimensional Euclidean space, wherein
Figure 419137DEST_PATH_IMAGE024
Is the sampling period of the IMU.
In a possible preferred embodiment, the observation model in step S400 is:
Figure 280914DEST_PATH_IMAGE025
wherein the data points obtained from the lidar are
Figure 711896DEST_PATH_IMAGE026
，
Figure 684531DEST_PATH_IMAGE027
In order to be the true position of the object,
Figure 251778DEST_PATH_IMAGE028
for measuring noise, the points falling on the map point cloud plane satisfy the following conditions:
Figure 782992DEST_PATH_IMAGE029
Figure 170111DEST_PATH_IMAGE030
is a normal vector of the plane of the laser data point in the map coordinate system,
Figure 946437DEST_PATH_IMAGE031
the pose of the IMU under the map coordinate in the current state,
Figure 633770DEST_PATH_IMAGE032
is a transformation matrix of the laser coordinate system to the IMU coordinate system,
Figure 696404DEST_PATH_IMAGE033
converting points in a laser coordinate system into a map coordinate system,
Figure 711764DEST_PATH_IMAGE034
for a point in a map coordinate system on the plane,
Figure 885257DEST_PATH_IMAGE035
representing a vector on the plane;
setting IMU one sampling period
Figure 161517DEST_PATH_IMAGE036
As with the measurements in, the discrete process on the composite flow pattern is defined as:
Figure 535998DEST_PATH_IMAGE037
Figure 163288DEST_PATH_IMAGE038
the variation introduced by each IMU sampling period can be regarded as external disturbance superposed on a composite flow type system, and the use of the external disturbance
Figure 782882DEST_PATH_IMAGE039
Representing external disturbances superimposed on a complex flow pattern, use
Figure 179228DEST_PATH_IMAGE040
Representing a superposition operation in which the space in which the perturbation is located is the same as the tangential space structure.
In a possible preferred embodiment, the step of calculating the residual error of the laser data point falling on the corresponding plane in the map point cloud under the current state prediction value in step S400 includes:
traversing the current laser data point according to the pose of the IMU in the map
Figure 724610DEST_PATH_IMAGE041
And relative pose of laser and IMU
Figure 308038DEST_PATH_IMAGE042
Points in the laser coordinate system
Figure 354492DEST_PATH_IMAGE043
Transformation into the map coordinate system
Figure 480711DEST_PATH_IMAGE044
：
Figure 321628DEST_PATH_IMAGE045
Searching N map points closest to the laser data point, judging whether a plane can be formed or not, and if so, calculating the residual error of the laser data point on the plane.
In a possible preferred embodiment, the step S200 of setting an initial value of the system state model to initialize the system state model includes: setting the initial time askThe initial value of the system state model is:
Figure 533297DEST_PATH_IMAGE046
keeping static during initial positioning to obtain current position
Figure 117862DEST_PATH_IMAGE047
And posture
Figure 862702DEST_PATH_IMAGE048
Setting the speed as the initial positioning result
Figure 608941DEST_PATH_IMAGE049
Is arranged as
Figure 432541DEST_PATH_IMAGE050
Giving the angular velocity bias mean value in the current state
Figure 430584DEST_PATH_IMAGE051
Similarly calculating the mean value of the acceleration to assign
Figure 187187DEST_PATH_IMAGE052
Biasing the acceleration
Figure 245273DEST_PATH_IMAGE053
Is arranged as
Figure 290590DEST_PATH_IMAGE054
。
In a possible preferred embodiment, the step of establishing a covariance matrix of error states in step S200 comprises:
defining the covariance of the error state on the tangent space of the system state model, wherein the initial covariance is as follows:
Figure 92324DEST_PATH_IMAGE055
wherein
Figure 172275DEST_PATH_IMAGE056
Is one
Figure 525896DEST_PATH_IMAGE057
Of the square matrix of (1), components thereof
Figure 966498DEST_PATH_IMAGE058
Is one
Figure 430977DEST_PATH_IMAGE059
R is a rotation component; component(s) of
Figure 506381DEST_PATH_IMAGE060
Is one
Figure 30903DEST_PATH_IMAGE059
The square matrix of (A) is formed, p is a rotational component; component(s) of
Figure 926178DEST_PATH_IMAGE061
Is one
Figure 459927DEST_PATH_IMAGE059
V is a rotational component; component(s) of
Figure 389837DEST_PATH_IMAGE062
Is one
Figure 819682DEST_PATH_IMAGE059
The square matrix of (a) is obtained,
Figure 592465DEST_PATH_IMAGE063
is the rotational component; component(s) of
Figure 38228DEST_PATH_IMAGE064
Is one
Figure 947278DEST_PATH_IMAGE059
The square matrix of (A) is formed,
Figure 688969DEST_PATH_IMAGE065
is the rotational component; component(s) of
Figure 949049DEST_PATH_IMAGE066
Is one
Figure 699968DEST_PATH_IMAGE067
G is a rotational component;
Figure 401207DEST_PATH_IMAGE056
and the rest positions are all 0.
In a possible preferred embodiment, the step of performing iterative error state kalman filter computation includes: and calculating the current error state value by observing the error by taking the state predicted value obtained in the prediction stage as the start, updating the error state value into the state predicted value at the start of calculation if the value does not reach the standard, observing the error again and calculating the error state value at the moment, carrying out iterative calculation for multiple times until the error state value reaches the standard, and updating the covariance matrix of the error state by using the error state value reaching the standard.
In order to achieve the above object, according to a second aspect of the present invention, there is provided a multiline laser positioning apparatus comprising:
the storage unit is used for storing a program comprising the steps of the multi-line laser positioning method, and the program is used for the control unit, the laser scanning unit, the IMU measuring unit, the processing unit and the information transmission unit to be timely called and executed; the storage unit also stores map point cloud data;
wherein the control unit is configured to coordinate:
the IMU measuring unit is used for acquiring IMU data before each frame of laser data in a sampling period;
the processing unit is used for setting an initial value of the system state model to initialize the system state model and establishing a covariance matrix of an error state; the processing unit further brings angular velocity and acceleration data in the IMU data and corresponding time interval data of each IMU data into the initialized system state model, calculates a state prediction value of each IMU data, and updates a covariance matrix of an error state;
the laser scanning unit is used for scanning a scene to obtain laser point cloud data;
the processing unit is further used for calculating a residual error of a data point of the laser point cloud on a corresponding plane in the map point cloud under the current state predicted value according to the observation model, and if the residual error is not zero, judging that an error exists between the state predicted value and a true value, wherein the error is an error state; then, iterative calculation is carried out by an error state Kalman filtering method until the value of the error state reaches the standard, so as to output the corresponding state component according to the value;
and the information transmission unit is used for transmitting the state component data processed by the processing unit.
In order to achieve the above object, according to a third aspect of the present invention, there is provided a computer device, including a memory and a processor, wherein the memory stores a computer program, and the processor implements the steps of the multiline laser positioning method as described above when executing the computer program.
In order to achieve the above object, according to a fourth aspect of the present invention, there is further provided a computer readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the steps of the multiline laser positioning method according to any one of the above.
The multiline laser positioning method, the positioning device, the computer equipment and the computer readable storage medium provided by the invention have the following beneficial effects:
compared with the prior art, the method needs point cloud spherical projection, calculates the normal vector of each point, executes the growth clustering algorithm, and then calculates the plane parameters by using the PCA, only the nearest N map points, such as 5 points, of each point need to be searched in the scheme, and whether the 5 map points form a plane or not is evaluated, so that the complexity of the plane detection algorithm is reduced.
In addition, compared with the prior art, the position and orientation result is corrected by using the plane features in a plurality of adjacent states in the sliding window as constraints, and actually, the position and orientation result is also in a local positioning state, so that the positioning precision is improved to a certain extent, and the global consistent positioning cannot be ensured. The scheme directly uses plane features in the global map as constraints, so that the pose can be converged to a global optimal state, and a global consistency positioning result can be ensured.
On the other hand, compared with the posture in the prior art, the posture is expressed by using quaternion, the MSCKF framework is adopted for optimization calculation, normalization calculation is needed after the increment of the posture is updated in the calculation process, and the situation that the quaternion requirement is met is ensured, so that the solution precision is influenced by the process. The whole model of the scheme is established on the flow pattern, the flow pattern can be transformed into the reflux pattern by the increment on the corresponding tangent space through exponential mapping, the process has no precision loss, and the whole calculation precision is higher.
Drawings
The accompanying drawings, which are incorporated in and constitute a part of this application, illustrate embodiments of the invention and, together with the description, serve to explain the invention and not to limit the invention. In the drawings:
FIG. 1 is a schematic diagram of the steps of a multi-line laser positioning method of the present invention;
FIG. 2 is a logic diagram illustrating the steps of the multi-line laser positioning method of the present invention;
fig. 3 is a schematic structural diagram of the multi-line laser positioning device of the present invention.
Detailed Description
In order to make those skilled in the art better understand the technical solution of the present invention, the following will clearly and completely describe the specific technical solution of the present invention with reference to the embodiments to help those skilled in the art to further understand the present invention. It should be apparent that the embodiments described herein are only a few embodiments of the present invention, and not all embodiments. It should be noted that the embodiments and features of the embodiments in the present application can be combined with each other without departing from the inventive concept and without conflicting therewith by those skilled in the art. All other embodiments based on the embodiments of the present invention, which can be obtained by a person of ordinary skill in the art without any creative effort, shall fall within the disclosure and the protection scope of the present invention.
Furthermore, the terms "first," "second," "S1," "S2," and the like in the description and in the claims of the invention and in the drawings are used for distinguishing between similar elements and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used is interchangeable under appropriate circumstances such that the embodiments of the invention described herein are capable of operation in sequences other than those described herein. Also, the terms "including" and "having," as well as any variations thereof, in the present invention are intended to cover non-exclusive inclusions.
Interpretation of terms
IMU the method comprises the following steps: inertial Measurement unit, inertial Measurement unit.
Tightly coupling the laser with the IMU: the sensor data are jointly involved in the calculation process, and the internal parameters of the partial sensors are optimized as variables to be optimized in the calculation process. Corresponding to this is a loose coupling.
Flow pattern: the flow pattern is a topological space in which the local space is a linear space. The smooth flow pattern indicates that the tangent space (i.e., the local space) at any point of the topological space is unique, i.e., the surface of the flow pattern is smooth and continuous. The composite flow pattern is a composite topological space formed by a plurality of smooth flow patterns.
Reference documents:
1. B. M. Bell and F. W. Cathey, The iterated kalman filter update as a gauss-newton method, IEEE Trans. Automat. Contr., vol. 38, no. 2, pp.294–297, 1993.
the multiline laser positioning method provided by the invention is characterized in that a laser and IMU tight coupling mode is mainly adopted, a state to be solved is constructed on a composite flow pattern (Manifold), the angular speed and the acceleration of the IMU are used as control input, the degree of a plane point in current laser data falling on the plane in a point cloud map is used as an observation model, and an error state iteration Kalman filter is used for solving an optimal state, so that the position and the posture of a current robot in the map are obtained.
(one)
To this end, as shown in fig. 1 to fig. 2, a method for positioning a multi-line laser according to a first embodiment of the present invention includes steps S100 to S400:
step S100: and defining the state components of the IMU data, system input data and system process noise so as to establish a system state model according to the change of the system input data to each state component.
Specifically, the state component of the defined IMU data is a state to be solved in the present solution, and includes: of IMUs in the current map coordinate system (which may also be considered a global coordinate system):
position of
Figure 600284DEST_PATH_IMAGE068
、
Posture of the vehicle
Figure 895130DEST_PATH_IMAGE069
、
Speed of rotation
Figure 246477DEST_PATH_IMAGE070
、
Angular velocity offset
Figure 441703DEST_PATH_IMAGE071
、
Acceleration bias
Figure 853093DEST_PATH_IMAGE072
、
Gravity vector under global coordinate system
Figure 136700DEST_PATH_IMAGE073
。
And each individual state is on a smooth flow pattern, combining all states into one state variable:
Figure 760580DEST_PATH_IMAGE074
。
the composite flow pattern in which the state variable is located is:
Figure 842936DEST_PATH_IMAGE075
。
the system input data is in the IMU coordinate system:
angular velocity
Figure 658183DEST_PATH_IMAGE076
Acceleration of the vehicle
Figure 521097DEST_PATH_IMAGE077
、
The system input data is:
Figure 417509DEST_PATH_IMAGE078
。
wherein the system process noise is IMU:
angular velocity measurement noise
Figure 416689DEST_PATH_IMAGE079
、
Acceleration measurement noise
Figure 966619DEST_PATH_IMAGE080
、
Bias random walk process noise for gyroscopes
Figure 552715DEST_PATH_IMAGE081
、
Bias random walk process noise for accelerometers
Figure 252817DEST_PATH_IMAGE082
、
Then the system process noise is
Figure 965558DEST_PATH_IMAGE083
。
-system state model building concept
Assume an IMU for one sample period
Figure 92914DEST_PATH_IMAGE036
As with the measurements in, the discrete process on the complex flow pattern is defined as:
Figure 399262DEST_PATH_IMAGE084
Figure 762110DEST_PATH_IMAGE085
the change introduced by each IMU sampling period can be regarded as external disturbance superposed on a composite flow type system, and the disturbance is used when the space in which the disturbance is positioned is not necessarily the same as the tangential space structure on the flow type
Figure 968838DEST_PATH_IMAGE086
Indicating the superposition of external perturbations on the composite flow pattern. Use of
Figure 126150DEST_PATH_IMAGE040
Representing a superposition operation in which the space in which the perturbation is located is the same as the tangential space structure.
The following describes the changes that the lower IMU input brings for each state component.
For velocity
Figure 919794DEST_PATH_IMAGE087
To say, first, it is necessary toTransferring the measured data under the IMU self coordinate system to the global coordinate system,
Figure 696120DEST_PATH_IMAGE088
representing the acceleration value with bias and noise removed.
Since the read data of the IMU contains a gravity component,
Figure 383453DEST_PATH_IMAGE089
is a weight-bearing force component (arranged for the convenience of calculating partial derivatives later), and therefore
Figure 852612DEST_PATH_IMAGE090
Representing the elimination of the gravity component, the amount of change is:
Figure 992606DEST_PATH_IMAGE091
for position
Figure 74088DEST_PATH_IMAGE092
For example, the variation is:
Figure 350349DEST_PATH_IMAGE093
the acceleration component is ignored in actual use:
Figure 990409DEST_PATH_IMAGE094
for the attitude
Figure 352120DEST_PATH_IMAGE095
For example, the variation amount is:
Figure 470248DEST_PATH_IMAGE096
for IMU biasing there are:
Figure 866595DEST_PATH_IMAGE097
gravity force
Figure 411977DEST_PATH_IMAGE089
No change occurs:
Figure 526563DEST_PATH_IMAGE098
the system state model is thus organized:
Figure 946918DEST_PATH_IMAGE022
and then, the system state can be calculated by adopting a standard iterative extended Kalman filtering method.
Step S200: and setting an initial value of the system state model to initialize the system state model and establishing a covariance matrix of an error state.
Specifically, in an actual use scene, assuming that a point cloud map is already constructed in advance, an initial value of a system state needs to be set first, and an initial time is assumed to bek。
Then:
Figure 197771DEST_PATH_IMAGE099
wherein the initial position
Figure 648475DEST_PATH_IMAGE092
And posture
Figure 250357DEST_PATH_IMAGE100
Can be obtained by NDT or particle filtering and the like, which is the prior art and is not described herein in detail, so that the method in the current state
Figure 710289DEST_PATH_IMAGE092
And with
Figure 81227DEST_PATH_IMAGE101
Setting as an initial positioning result.
At the initial positioning, the vehicle is stationary and will be positioned
Figure 968412DEST_PATH_IMAGE102
Is arranged as
Figure 57591DEST_PATH_IMAGE103
. Angular velocity offset
Figure 568817DEST_PATH_IMAGE104
The initial value may be assigned to the angular velocity by calculating an average value of the angular velocity upon acquiring, for example, 20 (or more) stationary IMU data
Figure 794262DEST_PATH_IMAGE104
Similarly calculating the mean value of the acceleration to assign
Figure 117927DEST_PATH_IMAGE105
The bias of the acceleration being normally set to
Figure 163244DEST_PATH_IMAGE103
。
Then the covariance of the error state is defined over its tangent space, the initial covariance is:
Figure 89611DEST_PATH_IMAGE106
。
wherein the covariance of the position error, attitude error and velocity error is initialized as:
Figure 310508DEST_PATH_IMAGE107
。
the covariance of the angular velocity bias and acceleration bias errors is initialized as:
Figure 664129DEST_PATH_IMAGE108
。
the covariance of the gravity error is initialized as:
Figure 603266DEST_PATH_IMAGE109
。
step S300: in the prediction stage, kalman filtering standard prediction calculation is carried out on each received IMU data, and the method comprises the following steps: and substituting the time interval data of the IMU data and the system input data into the initialized system state model, calculating the state prediction value of each IMU data, and updating the covariance matrix of the error state.
Specifically, after the system initialization is completed, the received IMU data starts to be buffered until a frame of laser data is received. Because the frequency of the IMU data is usually much higher than the frequency of the laser data, before a frame of laser data is received, several IMU data are often cached, and each IMU data can be predicted once.
Namely, the Kalman filtering standard prediction process comprises the following steps:
Figure 707226DEST_PATH_IMAGE110
specific examples are as follows:
assume that the first IMU data is spaced apart by a time interval of
Figure 641684DEST_PATH_IMAGE036
Angular velocity of
Figure 166206DEST_PATH_IMAGE111
Acceleration of
Figure 327061DEST_PATH_IMAGE112
Then, there are:
Figure 595231DEST_PATH_IMAGE113
at this time
Figure 525141DEST_PATH_IMAGE114
。
Figure 627089DEST_PATH_IMAGE115
The expression means the disturbance of each component on the composite flow pattern cutting space, namely the integral process.
For component
Figure 399873DEST_PATH_IMAGE116
、
Figure 582986DEST_PATH_IMAGE117
、
Figure 757615DEST_PATH_IMAGE118
、
Figure 499306DEST_PATH_IMAGE119
To say that they belong to
Figure 759386DEST_PATH_IMAGE120
The perturbation can be calculated by direct superposition, namely:
Figure 244725DEST_PATH_IMAGE121
for component
Figure 539441DEST_PATH_IMAGE122
，
Figure 717612DEST_PATH_IMAGE123
Wherein:
Figure 199409DEST_PATH_IMAGE124
for component
Figure 721395DEST_PATH_IMAGE125
，
Figure 870617DEST_PATH_IMAGE126
Thus, the use of an IMU data can be calculated
Figure 750848DEST_PATH_IMAGE127
The state prediction value of (1).
Then the covariance matrix of the updated error state:
Figure 267411DEST_PATH_IMAGE128
。
for example, for the first IMU data
Figure 658334DEST_PATH_IMAGE129
。
Figure 740691DEST_PATH_IMAGE130
Figure 57403DEST_PATH_IMAGE131
Figure 90956DEST_PATH_IMAGE132
Figure 925051DEST_PATH_IMAGE133
Wherein
Figure 425696DEST_PATH_IMAGE134
Is a state
Figure 913309DEST_PATH_IMAGE135
Corresponds to the tangent space at that pointIs expressed above.
Figure 997939DEST_PATH_IMAGE136
Wherein:
Figure 963621DEST_PATH_IMAGE137
Figure 581422DEST_PATH_IMAGE138
Figure 567833DEST_PATH_IMAGE139
Figure 405339DEST_PATH_IMAGE140
wherein:
Figure 377974DEST_PATH_IMAGE141
。
so far, each IMU data can perform state prediction and calculate covariance of error states, and then after all the received IMUs before the current laser data are subjected to an over-prediction stage, an update procedure is required to correct the states to be optimal.
Step S400, an observation stage: calculating a residual error of a laser data point on a corresponding plane in the map point cloud under the current state predicted value according to the observation model, and if the residual error is not zero, judging that an error exists between the state predicted value and a true value, wherein the error is an error state; and then, carrying out iterative calculation by an error state Kalman filtering method until the value of the error state reaches the standard, and outputting a corresponding state component according to the value.
Specifically, assuming that the point cloud map is perfect enough, substantially consistent with the actual scene, and the scene is not changed, and secondly, the current location position is accurate enough, the point cloud collected at the position should be completely fitted with the point cloud map after being transformed into the map coordinate system. Generally, there are many planes in the environment, such as the ground, the wall, and objects whose surfaces contain planes, and obviously, under such assumption, the plane points in the collected laser point cloud should also fall on the corresponding planes in the map.
Assuming data points acquired from a laser
Figure 210801DEST_PATH_IMAGE142
Figure 977900DEST_PATH_IMAGE143
Is the true position.
Figure 896177DEST_PATH_IMAGE144
To measure the noise, the points lying on a plane satisfy the following condition:
Figure 162249DEST_PATH_IMAGE145
Figure 584003DEST_PATH_IMAGE146
is the normal vector of the plane of the point under the map coordinate system.
Figure 787583DEST_PATH_IMAGE147
The pose of the IMU under the map coordinate in the current state is obtained.
Figure 927577DEST_PATH_IMAGE148
The method is a transformation matrix from a laser coordinate system to an IMU coordinate system (the transformation matrix is determined after the sensor is installed, an accurate value can be obtained through calibration, and the calibration method belongs to the prior art and is not described herein again).
Figure 507594DEST_PATH_IMAGE149
Namely, points in the laser coordinate system are transformed into the map coordinate system.
Figure 49434DEST_PATH_IMAGE150
As points in a map coordinate system on the plane.
Figure 423915DEST_PATH_IMAGE151
Representing a vector on the plane.
The product is zero as known by the normal between any vector on the plane and the normal vector of the plane. In the actual situation, because both the map and the laser point cloud have errors, the plane normal vector and the vector calculation on the plane have errors, the product of multiplication of the two vectors is not zero, and if the current positioning position has deviation, the product is also not zero, and the observation model is defined as follows:
Figure 51205DEST_PATH_IMAGE152
each frame of laser data should have many plane points, and assuming there are m, the observation equation is:
Figure 64215DEST_PATH_IMAGE154
Figure 875176DEST_PATH_IMAGE155
to observe the noise.
In updating programs, states are further optimized using iterative methods, defining
Figure 724184DEST_PATH_IMAGE156
Is as followsjAnd estimating the state of the secondary iteration. When the temperature is higher than the set temperature
Figure 646003DEST_PATH_IMAGE157
When the update calculation is performed (i.e., just after the prediction stage has been performed, the first time the update calculation is performed),
Figure 37801DEST_PATH_IMAGE158
is the state estimation after the prediction phase is finished.
Will be firstjObserved residual of sub-iteration
Figure 878718DEST_PATH_IMAGE159
Is defined as:
Figure 857432DEST_PATH_IMAGE160
wherein
Figure 441997DEST_PATH_IMAGE161
In order to be able to take the value of the observation,
Figure 688302DEST_PATH_IMAGE162
is defined asjError between state estimate and true value of sub-iteration
Figure 700120DEST_PATH_IMAGE163
And (4) the resection space is represented.
Figure 664665DEST_PATH_IMAGE164
Figure 193867DEST_PATH_IMAGE165
Before the above calculation can be performed, it is necessary to solve the problem of how to screen out the points belonging to a certain plane (above) from the current laser data
Figure 419312DEST_PATH_IMAGE143
) And what the normal vector of the plane is (above)
Figure 336452DEST_PATH_IMAGE166
）。
The realization method comprises the following steps:
traversing the current laser data point, firstly according to the pose of the IMU in the map
Figure 286828DEST_PATH_IMAGE167
And relative pose of laser and IMU
Figure 213196DEST_PATH_IMAGE168
Points in the laser coordinate system
Figure 168514DEST_PATH_IMAGE143
Transformation into the map coordinate system
Figure 787714DEST_PATH_IMAGE169
：
Figure 461272DEST_PATH_IMAGE170
And then searching the nearest N map points to the point by using a KDTree (which may be implemented by using a KDTree or nanoflann in a common PCL library), preferably 5 map points as in this embodiment, and in order to avoid too close selected map points, downsampling the point cloud map with a resolution of 0.2 (implemented by using a VoxelGrid in the PCL library) is required.
Obtain the nearest 5 points
Figure 597855DEST_PATH_IMAGE171
Then, it is necessary to determine whether these 5 points can constitute a plane. Assuming that these 5 points are in a plane equation of
Figure 440303DEST_PATH_IMAGE172
On the plane of (a) then there are:
Figure 902508DEST_PATH_IMAGE173
solving this over-determined equation (which can be solved using Eigen) yields the coefficients for the plane.
Then each point is put into the equation one by one, the equation must be satisfied completely to identify a plane, and the normal vector of the plane is
Figure 594520DEST_PATH_IMAGE174
。
Due to the presence of errors, even if the points lie on a plane, the value calculated by the equation may not be
Figure 534795DEST_PATH_IMAGE175
Setting a threshold as long as in
Figure 166502DEST_PATH_IMAGE176
Nearby. In addition, in the iterative computation process, because there is an error between the estimated state and the true value, the point at this time does not fall on the plane, and the residual error at this time is:
Figure 534029DEST_PATH_IMAGE177
then, according to an iterative kalman update calculation (reference 1): and calculating a current error state value by observing errors starting from the state predicted value obtained in the prediction stage, if the value is greater than a threshold, updating the error state value into the state predicted value at the beginning of calculation, calculating the observation errors and the error state value at the moment again, performing iterative calculation for multiple times until the state errors reach the standard, updating the covariance matrix of the error state by using the solved state value, and outputting the current positioning position and posture according to the state value.
The realization method comprises the following steps:
the state of the error is such that,
Figure 447759DEST_PATH_IMAGE178
。
Figure 427609DEST_PATH_IMAGE179
is the error after each iteration, wherein:
Figure 274343DEST_PATH_IMAGE180
for different flow pattern types
Figure 547192DEST_PATH_IMAGE181
There are different calculation formulas:
Figure 948218DEST_PATH_IMAGE182
Figure 463250DEST_PATH_IMAGE183
the final update per iteration can be calculated as:
Figure 898911DEST_PATH_IMAGE184
after each iteration is completed, it is determined whether the current error is small enough, namely, if so
Figure 77083DEST_PATH_IMAGE185
If the value of each minute is less than 0.001 (namely a very small threshold), the current state is considered to be optimized and solved.
Figure 965404DEST_PATH_IMAGE186
Finally, the covariance of the error state is updated as follows:
Figure 644647DEST_PATH_IMAGE187
for different flow pattern types
Figure 182419DEST_PATH_IMAGE188
There are different calculation formulas:
Figure 531491DEST_PATH_IMAGE189
Figure 766164DEST_PATH_IMAGE190
and finishing the complete state error iterative extended Kalman filtering calculation. After receiving the next IMU data and laser data, the calculation is continued according to the above method, as shown in the flowchart of fig. 2.
Therefore, compared with other positioning methods, the initial value needs to be provided by combining the current odometer result before each calculation, and the scheme completely does not depend on the odometer, so that the slipping problem can be well solved.
On the other hand, compared with the similar prior art in the background technology, the scheme only needs to search the nearest 5 map points of each point, and whether the 5 map points form a plane or not is evaluated, so that the complexity of a plane detection algorithm is reduced.
Meanwhile, the scheme can directly use plane features in the global map as constraints, so that the pose can be converged to the global optimal state, and the global consistency positioning result can be ensured.
In addition, the whole model is established on the flow pattern, the flow pattern can be transformed into the reflux pattern by the increment on the corresponding tangent space through exponential mapping, the process has no precision loss, and the whole calculation precision is higher.
(II)
Referring to fig. 3, in a multi-line laser positioning method according to a first embodiment, the present invention further provides a multi-line laser positioning apparatus, which includes:
the storage unit is used for storing a program comprising the steps of the multi-line laser positioning method according to the first embodiment, so that the control unit, the laser scanning unit, the IMU measuring unit, the processing unit and the information transmission unit can be timely called and executed; the storage unit also stores map point cloud data.
Wherein the control unit is configured to coordinate:
and the IMU measuring unit is used for acquiring IMU data before each frame of laser data in a sampling period.
The processing unit is used for setting an initial value of the system state model so as to initialize the system state model and establish a covariance matrix of an error state; the processing unit further brings the angular velocity and acceleration data in the IMU data and the corresponding time interval data of each IMU data into the initialized system state model, calculates the state prediction value of each IMU data, and updates the covariance matrix of the error state.
And the laser scanning unit is used for scanning a scene to acquire laser point cloud data.
The processing unit is used for further calculating a residual error of a data point of the laser point cloud on a corresponding plane in the map point cloud under the current state predicted value according to the observation model, and judging that an error exists between the state predicted value and a true value if the residual error is not zero, wherein the error is an error state; and then, carrying out iterative calculation by an error state Kalman filtering method until the value of the error state reaches the standard, and outputting a corresponding state component according to the value.
And the information transmission unit is used for transmitting the state component data processed by the processing unit for positioning.
(III)
The invention further provides a computer device corresponding to the multiline laser positioning method of the first embodiment, which includes a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the steps of the multiline laser positioning method of the first embodiment.
(IV)
The invention further provides a computer-readable storage medium having a computer program stored thereon, where the computer program is executed by a processor to implement the steps of the multiline laser positioning method according to the first embodiment.
In summary, the preferred embodiments of the present invention disclosed above are only for the purpose of assisting the explanation of the present invention. The preferred embodiments are not intended to be exhaustive or to limit the invention to the precise embodiments disclosed. Obviously, many modifications and variations are possible in light of the above teaching. The embodiments were chosen and described in order to best explain the principles of the invention and the practical application, to thereby enable others skilled in the art to best utilize the invention. The invention is limited only by the claims and the full scope and equivalents thereof, and any modification, equivalent replacement, or improvement made within the spirit and principle of the invention should be included in the protection scope of the invention.
It will be appreciated by those skilled in the art that, in addition to implementing the system, apparatus and individual modules thereof provided by the present invention in purely computer readable program code means, the system, apparatus and individual modules thereof provided by the present invention can be implemented in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers and the like, all by logically programming the method steps. Therefore, the system, the apparatus, and the modules thereof provided by the present invention may be considered as a hardware component, and the modules included in the system, the apparatus, and the modules for implementing various programs may also be considered as structures in the hardware component; modules for performing various functions may also be considered to be both software programs for performing the methods and structures within hardware components.
In addition, all or part of the steps of the method according to the above embodiments may be implemented by a program instructing related hardware, where the program is stored in a storage medium and includes several instructions to enable a single chip, a chip, or a processor (processor) to execute all or part of the steps of the method according to 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), a magnetic disk or an optical disk, and other various media capable of storing program codes.
In addition, any combination of various different implementation manners of the embodiments of the present invention is also possible, and the embodiments of the present invention should be considered as disclosed in the embodiments of the present invention as long as the combination does not depart from the spirit of the embodiments of the present invention.

Claims (9)

1. A multiline laser positioning method comprises the following steps:
step S100, defining the state components of IMU data, system input data and system process noise so as to establish a system state model according to the change of each state component caused by the system input data;
step S200, setting an initial value of a system state model to initialize the system state model and establishing a covariance matrix of an error state;
step S300 prediction phase: performing Kalman filtering standard prediction calculation on each received IMU data, wherein the method comprises the following steps: substituting the time interval data of IMU data and system input data into the initialized system state model, calculating the state prediction value of each IMU data, and updating the covariance matrix of the error state;
step S400, an observation stage: calculating a residual error of a laser data point on a corresponding plane in the map point cloud under the current state predicted value according to the observation model, and if the residual error is not zero, judging that an error exists between the state predicted value and a true value, wherein the error is an error state; and then, carrying out iterative calculation by using an error state Kalman filtering method until the value of the error state reaches the standard, and outputting a corresponding state component according to the value of the error state reaching the standard, wherein in the step S100, the defined state component of the IMU data comprises: location of IMU in current map coordinate system
Figure 724650DEST_PATH_IMAGE002
And posture of the same
Figure 286389DEST_PATH_IMAGE004
Speed, velocity
Figure 634193DEST_PATH_IMAGE005
Angular velocityBiasing
Figure 840047DEST_PATH_IMAGE006
Acceleration bias
Figure 707640DEST_PATH_IMAGE008
Gravity vector under global coordinate system
Figure 606326DEST_PATH_IMAGE009
(ii) a And each of the above individual states is in a smooth flow pattern, combining all the states into one state variable:
Figure 175847DEST_PATH_IMAGE011
the composite flow pattern in which the state variable is located is:
Figure 185391DEST_PATH_IMAGE013
(ii) a Wherein
Figure 671605DEST_PATH_IMAGE014
For a particular orthogonal group of three dimensions, 3D rotations can be represented by the group,
Figure 475613DEST_PATH_IMAGE015
Is a 3-dimensional European space,
Figure 63589DEST_PATH_IMAGE016
Is 2-sphere, which is a hemisphere
Figure 611245DEST_PATH_IMAGE018
The system input data includes: angular velocity under IMU's own coordinate system
Figure 453431DEST_PATH_IMAGE019
Acceleration of the vehicle
Figure 693919DEST_PATH_IMAGE020
Then the system input data is:
Figure 378978DEST_PATH_IMAGE021
；
the system process noise includes: angular velocity measurement noise
Figure 120538DEST_PATH_IMAGE022
Acceleration measurement noise
Figure 941864DEST_PATH_IMAGE023
Bias random walk process noise of gyroscope
Figure 618833DEST_PATH_IMAGE024
Bias random walk process noise of accelerometer
Figure 902440DEST_PATH_IMAGE025
Then the system process noise is
Figure 57478DEST_PATH_IMAGE026
；
The system state model is as follows:
Figure 264468DEST_PATH_IMAGE027
wherein
Figure 971393DEST_PATH_IMAGE028
Is 18-dimensional Euclidean space, wherein
Figure 365465DEST_PATH_IMAGE029
Is the sampling period of the IMU.
2. The multiline laser positioning method according to claim 1, wherein the observation model in step S400 is:
Figure 933981DEST_PATH_IMAGE030
wherein the data points obtained from the lidar are
Figure 729899DEST_PATH_IMAGE032
，
Figure 607725DEST_PATH_IMAGE033
In order to be a true position of the object,
Figure 754672DEST_PATH_IMAGE035
for measuring noise, the points falling on the map point cloud plane satisfy the following conditions:
Figure 985934DEST_PATH_IMAGE036
Figure 10259DEST_PATH_IMAGE037
is a normal vector of the plane of the laser data point in the map coordinate system,
Figure 199932DEST_PATH_IMAGE039
the pose of the IMU under the map coordinate in the current state,
Figure 693230DEST_PATH_IMAGE040
is a transformation matrix from the laser coordinate system to the IMU coordinate system,
Figure 728182DEST_PATH_IMAGE041
the points in the laser coordinate system are transformed to the mapUnder the condition of the coordinate system,
Figure 498692DEST_PATH_IMAGE042
as points in a map coordinate system on the plane,
Figure 469053DEST_PATH_IMAGE044
representing a vector on the plane;
setting IMU one sampling period
Figure 325014DEST_PATH_IMAGE045
As with the measurements in, the discrete process on the complex flow pattern is defined as:
Figure 288291DEST_PATH_IMAGE046
Figure 913307DEST_PATH_IMAGE047
the variation introduced by each IMU sampling period can be regarded as external disturbance superposed on a composite flow type system, and the use of the external disturbance
Figure 444783DEST_PATH_IMAGE048
Representing external disturbances superimposed on a complex flow pattern, use
Figure 633712DEST_PATH_IMAGE049
Representing a superposition operation where the space in which the perturbation is located is the same as the tangent space structure.
3. The multiline laser positioning method of claim 2, wherein the step of calculating the residual error of the laser data point falling on the corresponding plane in the map point cloud under the current state prediction value in step S400 includes:
traverse the current laserLight data points according to IMU pose in map
Figure 541625DEST_PATH_IMAGE050
And relative pose of laser and IMU
Figure 411361DEST_PATH_IMAGE040
Points in the laser coordinate system
Figure 848158DEST_PATH_IMAGE052
Transformation into the map coordinate system
Figure 288498DEST_PATH_IMAGE054
：
Figure 203365DEST_PATH_IMAGE055
Searching N map points closest to the laser data point, judging whether a plane can be formed or not, and if so, calculating the residual error of the laser data point on the plane.
4. The multiline laser positioning method of claim 1, wherein the step S200 of setting an initial value of the system state model to initialize the system state model includes: setting an initial time as
Figure 927607DEST_PATH_IMAGE057
The initial value of the system state model is as follows:
Figure 800885DEST_PATH_IMAGE058
keeping still during initial positioning to obtain current position
Figure 853155DEST_PATH_IMAGE059
And posture
Figure 211193DEST_PATH_IMAGE060
Setting the speed as the initial positioning result
Figure 665308DEST_PATH_IMAGE061
Is arranged as
Figure 709487DEST_PATH_IMAGE063
Giving the angular velocity bias mean value in the current state
Figure 108107DEST_PATH_IMAGE064
Similarly calculating the mean value of the acceleration to assign
Figure 895935DEST_PATH_IMAGE065
Biasing the acceleration
Figure 79923DEST_PATH_IMAGE066
Is arranged as
Figure 295003DEST_PATH_IMAGE068
。
5. The multiline laser positioning method of claim 4 wherein the step of establishing a covariance matrix of error states in step S200 includes:
defining the covariance of the error state on the tangent space of the system state model, wherein the initial covariance is as follows:
Figure 915340DEST_PATH_IMAGE069
in which
Figure 506859DEST_PATH_IMAGE070
Is one
Figure 46818DEST_PATH_IMAGE071
Of the square matrix of (1), components thereof
Figure 167221DEST_PATH_IMAGE072
Is one
Figure 806012DEST_PATH_IMAGE073
R is a rotation component; component(s) of
Figure 935642DEST_PATH_IMAGE074
Is one
Figure 953277DEST_PATH_IMAGE073
P is a rotational component; component(s) of
Figure 385527DEST_PATH_IMAGE075
Is one
Figure 386981DEST_PATH_IMAGE073
V is a rotational component; component(s) of
Figure DEST_PATH_IMAGE076
Is one
Figure 648198DEST_PATH_IMAGE073
The square matrix of (A) is formed,
Figure 785918DEST_PATH_IMAGE077
is a rotational component; component(s) of
Figure 153183DEST_PATH_IMAGE078
Is one
Figure 376354DEST_PATH_IMAGE073
The square matrix of (A) is formed,
Figure DEST_PATH_IMAGE079
is the rotational component; component(s) of
Figure 706841DEST_PATH_IMAGE080
Is one
Figure DEST_PATH_IMAGE081
G is a rotation component;
Figure 433489DEST_PATH_IMAGE070
the remaining positions in (A) are all 0.
6. The multiline laser positioning method of claim 1 wherein the step of performing iterative error state kalman filtering comprises: and calculating the current error state value by observing the error by taking the state predicted value obtained in the prediction stage as the start, updating the error state value into the state predicted value at the start of calculation if the value does not reach the standard, observing the error again and calculating the error state value at the moment, carrying out iterative calculation for multiple times until the error state value reaches the standard, and updating the covariance matrix of the error state by using the error state value reaching the standard.
7. A multiline laser positioning device comprising:
a storage unit for storing a program comprising the steps of the multiline laser positioning method according to any one of claims 1 to 6 for timely retrieval and execution by the control unit, the laser scanning unit, the IMU measuring unit, the processing unit, the information transmission unit; the storage unit also stores map point cloud data;
wherein the control unit is configured to coordinate:
the IMU measuring unit is used for acquiring IMU data before each frame of laser data in a sampling period;
the processing unit is used for setting an initial value of the system state model to initialize the system state model and establishing a covariance matrix of an error state; the processing unit further brings angular velocity and acceleration data in the IMU data and corresponding time interval data of each IMU data into the initialized system state model, calculates a state prediction value of each IMU data, and updates a covariance matrix of an error state;
the laser scanning unit is used for scanning a scene to obtain laser point cloud data;
the processing unit is used for further calculating a residual error of a data point of the laser point cloud on a corresponding plane in the map point cloud under the current state predicted value according to the observation model, and judging that an error exists between the state predicted value and a true value if the residual error is not zero, wherein the error is an error state; then, iterative calculation is carried out by an error state Kalman filtering method until the value of the error state reaches the standard, so as to output the corresponding state component according to the value;
and the information transmission unit is used for transmitting the state component data processed by the processing unit.
8. A computer device comprising a memory and a processor, the memory storing a computer program, wherein the processor when executing the computer program performs the steps of the method according to any of claims 1 to 6.
9. A computer-readable storage medium, on which a computer program is stored which, when being executed by a processor, carries out the steps of the method according to any one of claims 1 to 6.
CN202211496783.8A 2022-11-28 2022-11-28 Multi-line laser positioning method and positioning device, computer equipment and storage medium Active CN115597595B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211496783.8A CN115597595B (en) 2022-11-28 2022-11-28 Multi-line laser positioning method and positioning device, computer equipment and storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211496783.8A CN115597595B (en) 2022-11-28 2022-11-28 Multi-line laser positioning method and positioning device, computer equipment and storage medium

Publications (2)

Publication Number Publication Date
CN115597595A true CN115597595A (en) 2023-01-13
CN115597595B CN115597595B (en) 2023-04-07

Family

ID=84852113

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211496783.8A Active CN115597595B (en) 2022-11-28 2022-11-28 Multi-line laser positioning method and positioning device, computer equipment and storage medium

Country Status (1)

Country Link
CN (1) CN115597595B (en)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112880674A (en) * 2021-01-21 2021-06-01 深圳市镭神智能系统有限公司 Positioning method, device and equipment of driving equipment and storage medium
US20210278549A1 (en) * 2018-08-23 2021-09-09 The Regents Of The University Of California Lane-level navigation system for ground vehicles with lidar and cellular signals
WO2022007437A1 (en) * 2020-07-04 2022-01-13 华为技术有限公司 Method for calibrating mounting deviation angle between sensors, combined positioning system, and vehicle
CN114217665A (en) * 2021-12-21 2022-03-22 清华大学 Camera and laser radar time synchronization method, device and storage medium

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20210278549A1 (en) * 2018-08-23 2021-09-09 The Regents Of The University Of California Lane-level navigation system for ground vehicles with lidar and cellular signals
WO2022007437A1 (en) * 2020-07-04 2022-01-13 华为技术有限公司 Method for calibrating mounting deviation angle between sensors, combined positioning system, and vehicle
CN112880674A (en) * 2021-01-21 2021-06-01 深圳市镭神智能系统有限公司 Positioning method, device and equipment of driving equipment and storage medium
CN114217665A (en) * 2021-12-21 2022-03-22 清华大学 Camera and laser radar time synchronization method, device and storage medium

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
韩月琪等: "过去状态估计之集合Kalman滤波扩充状态变量法研究", 《高原气象》 *

Also Published As

Publication number Publication date
CN115597595B (en) 2023-04-07

Similar Documents

Publication Publication Date Title
US9071829B2 (en) Method and system for fusing data arising from image sensors and from motion or position sensors
JP5219467B2 (en) Method, apparatus, and medium for posture estimation of mobile robot based on particle filter
US5363305A (en) Navigation system for a mobile robot
Fleps et al. Optimization based IMU camera calibration
JP6855524B2 (en) Unsupervised learning of metric representations from slow features
CN110986968B (en) Method and device for real-time global optimization and error loop judgment in three-dimensional reconstruction
US20220051031A1 (en) Moving object tracking method and apparatus
Michot et al. Bi-objective bundle adjustment with application to multi-sensor slam
Hashlamon A new adaptive extended Kalman filter for a class of nonlinear systems
CN114623817B (en) Self-calibration-contained visual inertial odometer method based on key frame sliding window filtering
Zhang et al. Vision-aided localization for ground robots
CN113960622A (en) Real-time positioning method and device fusing laser radar and IMU sensor information
CN114002725A (en) Lane line auxiliary positioning method and device, electronic equipment and storage medium
CN110375731A (en) A kind of mixing interacting multiple model filters method
KR20220058901A (en) Data processing methods and devices
CN115046545A (en) Positioning method combining deep network and filtering
Lupton et al. Removing scale biases and ambiguity from 6DoF monocular SLAM using inertial
CN117451032A (en) SLAM method and system of low-calculation-force and loose-coupling laser radar and IMU
Al Bitar et al. Neural networks aided unscented Kalman filter for integrated INS/GNSS systems
CN112991400A (en) Multi-sensor auxiliary positioning method for unmanned ship
CN115597595B (en) Multi-line laser positioning method and positioning device, computer equipment and storage medium
Poddar et al. Tuning of GPS aided attitude estimation using evolutionary algorithms
CN111811501B (en) Trunk feature-based unmanned aerial vehicle positioning method, unmanned aerial vehicle and storage medium
CN111812668B (en) Winding inspection device, positioning method thereof and storage medium
WO2024114593A1 (en) Multi-line laser positioning method and positioning apparatus, computer device, and storage medium

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
GR01 Patent grant
GR01 Patent grant
PE01 Entry into force of the registration of the contract for pledge of patent right
PE01 Entry into force of the registration of the contract for pledge of patent right

Denomination of invention: Multi line laser positioning method and positioning device, computer equipment, storage medium

Effective date of registration: 20230828

Granted publication date: 20230407

Pledgee: Bank of Communications Ltd. Shanghai New District Branch

Pledgor: Shanghai Xiangong Intelligent Technology Co.,Ltd.

Registration number: Y2023310000491