CN114485643A - Environment sensing and high-precision positioning method for coal mine underground mobile robot - Google Patents

Environment sensing and high-precision positioning method for coal mine underground mobile robot Download PDF

Info

Publication number
CN114485643A
CN114485643A CN202210088155.XA CN202210088155A CN114485643A CN 114485643 A CN114485643 A CN 114485643A CN 202210088155 A CN202210088155 A CN 202210088155A CN 114485643 A CN114485643 A CN 114485643A
Authority
CN
China
Prior art keywords
mobile robot
coal mine
inertial navigation
representing
point cloud
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
CN202210088155.XA
Other languages
Chinese (zh)
Other versions
CN114485643B (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.)
Chongqing University of Technology
Original Assignee
Chongqing University of Technology
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 Chongqing University of Technology filed Critical Chongqing University of Technology
Priority to CN202210088155.XA priority Critical patent/CN114485643B/en
Publication of CN114485643A publication Critical patent/CN114485643A/en
Application granted granted Critical
Publication of CN114485643B publication Critical patent/CN114485643B/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/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/183—Compensation of inertial measurements, e.g. for temperature effects
    • G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
    • 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
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • 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/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • 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
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/497—Means for monitoring or calibrating

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The invention discloses a method for environment sensing and high-precision positioning of a mobile robot in a coal mine, which can realize accurate positioning of the mobile robot in the coal mine and can also quickly, accurately and stably complete environment sensing and high-precision positioning of the mobile robot fused with multiple sensors in the coal mine. The mobile robot collects surrounding environment information through a plurality of sensors carried by the mobile robot, the sensors are calibrated by adopting a calibration principle, the remote control robot runs under a coal mine at a constant speed, a running path of the remote control robot becomes a closed loop, the data of the sensors are fused by using a mature extended Kalman theory to obtain a consistent explanation of the pose of the mobile robot under the coal mine, and the special environment under the coal mine is positioned and mapped by an improved cartographer algorithm. The human resource cost is reduced, and the underground operation efficiency of the coal mine is improved.

Description

Environment sensing and high-precision positioning method for coal mine underground mobile robot
Technical Field
The invention relates to the field of positioning of underground mobile robots in coal mines, in particular to an environment sensing and high-precision positioning method for underground mobile robots in coal mines.
Background
As the first coal mining major country in the world, coal produced annually occupies more than half of the global coal yield, and meanwhile, Chinese coal safety accidents are also the top. Because the underground coal mine has complex shape, dim light and unknown working environment, the concentration of harmful gas and the temperature and the humidity of the environment cannot be observed in real time, and meanwhile, along with the increasing coal demand of people, coal safety accidents are promoted to frequently occur. And for the research of the mobile robot autonomous positioning system, great guarantee is provided for future unmanned mining and coal mine underground rescue work. Most of the existing mobile robot positioning technologies are influenced by the underground special environment of the coal mine, so that positioning is inaccurate, even the mobile robot is broken down to cause clamping stagnation of a positioning process, and a positioning period is prolonged.
According to the defects of the prior art, a mobile robot environment sensing and high-precision positioning method capable of accurately positioning a mobile robot in a coal mine underground and quickly, accurately and stably completing fusion of multiple sensors in the coal mine underground is needed.
Disclosure of Invention
Aiming at the defects in the prior art, the invention provides the environment sensing and high-precision positioning method for the underground mobile robot for the coal mine, which can be used for accurately positioning the sealwort in the coal mine and can also be used for quickly, accurately and stably finishing the environment sensing and high-precision positioning of the mobile robot for the fusion of multiple sensors in the coal mine.
In order to achieve the technical purpose, the technical scheme adopted by the invention is as follows:
the method adopts a plurality of sensors, wherein the plurality of sensors comprise an explosion-proof laser radar and a strapdown inertial navigation, and the explosion-proof laser radar and the strapdown inertial navigation are used for carrying out environment sensing and high-precision positioning on the coal mine underground mobile robot according to the following steps:
pretreatment: installing an explosion-proof laser radar and a strapdown inertial navigation system on a hydraulically-driven quadruped robot, connecting the explosion-proof laser radar and the strapdown inertial navigation system with a power supply, establishing connection with a mobile robot, and setting the initial pose of the robot under a coal mine;
s1: initializing, namely moving the coal mine underground to a starting point by the mobile robot, and establishing communication between the mobile robot and a computer;
s2: the mobile robot is static at an initial position, and the explosion-proof laser radar and the strapdown inertial navigation are respectively calibrated, wherein the explosion-proof laser radar is calibrated by adopting a minimum variance method, the strapdown inertial navigation consists of a gyroscope and an accelerometer, the gyroscope is calibrated by adopting a static position method, and the accelerometer is calibrated by adopting a twelve-position method;
s3: remotely controlling the mobile robot to move, performing closed-loop uniform motion in the underground environment of the coal mine according to a preset route, and finishing the motion from a starting point to the starting point;
s4: the mobile robot carries out combined filtering in various modes on original signals acquired by the explosion-proof laser radar, wherein the combined filtering comprises straight-through filtering, statistical filtering and voxel filtering fusion, outliers and noise are filtered, useful point cloud information is acquired, and the quality of point cloud acquired by the laser radar in the coal mine is improved;
s5: the mobile robot carries out point cloud hole restoration and restoration on the laser point cloud information through an image repairing algorithm on the preprocessed laser point cloud obtained in the last step;
s6: the mobile robot performs data fusion on the laser point cloud obtained in the step S5 and data acquired by strapdown inertial navigation, performs data fusion of multiple sensors in an extended Kalman filtering mode, uses data information of the strapdown inertial navigation as a predicted value, uses point cloud data repaired by a laser radar as an observed value, corrects prior information of the strapdown inertial navigation, and obtains an optimal pose estimation value of the underground coal mine mobile robot at the current moment;
s7: in the moving process of the remote control mobile robot, the mobile robot runs an improved cartographer algorithm to build a picture, fast search of closed-loop detection is realized through a branch-and-bound method, and the accumulated error of local picture building of a sensor is eliminated through loop-back detection to obtain high-precision positioning;
s8: and the remote control mobile robot returns to the initial position and waits for the next instruction.
As a preferred scheme of the invention, the difference value between the self coordinate system of the explosion-proof laser radar and the coordinate system of the required mobile robot is obtained, and the difference value between the self coordinate system of the explosion-proof laser radar and the coordinate system of the required mobile robot mainly comprises a rotation matrix and a translation matrix; the influence of the translation matrix on the final positioning result of the underground coal mine is relatively small and is ignored; the rotation matrix error can affect the final positioning and mapping result;
the specific calibration method for the rotation matrix comprises the following steps of placing the mobile robot on a road surface which is as flat as possible under a coal mine, enabling the distances from two sides of the mobile robot to a coal wall to be equal, and extracting data of the coal wall and data of data on the underground coal mine surface through a RANSAC random sampling consistency algorithm, wherein the specific calibration method specifically comprises the following steps:
1-P＝(1-Wn)k
wherein, P represents the probability that RANSAC algorithm produces useful result for underground positioning as P, W represents the probability that randomly selected points in the collected point cloud data set are local points each time, the collected noise point cloud is called local points, and the probability that at least one of the selected n points is a local point is 1-WnK represents the number of iterations;
the explosion-proof laser radar is still on the ground under a flat coal mine, the point cloud height information on all the ground is consistent under the ideal condition, however, the roll angle roll and the pitch angle pitch of the explosion-proof laser radar will affect the height value, and the roll angle roll and the pitch angle pitch need to be found in an angle compensation manner, so that the variance of the point cloud on the underground ground of the coal mine is the minimum, preferably close to 0, so as to obtain the roll angle and pitch angle, and in the same way, the explosion-proof laser radar is still on the ground under the flat coal mine, and the point cloud depth information of all coal walls is identical under the ideal condition, however, the yaw angle yaw of the explosion-proof laser radar affects the height value, and the yaw angle yaw needs to be found in an angle compensation mode to minimize the point cloud variance of the coal wall under the coal mine, and the point cloud variance is preferably close to 0, so that the yaw angle yaw is obtained, and the rotation matrix can be obtained.
As a preferred scheme of the invention, the calibration of the strapdown inertial navigation is divided into three parts, and the errors T are respectively installed0Error of scale factor S0Zero offset error z0Thus acceleration a of strapdown inertial navigationBAnd a gyroscope wBThe measurement error model of (2) is:
Figure BDA0003487967750000031
the superscript B represents the value after correction, the superscript a represents the value of the accelerometer, the superscript g represents the value of the gyroscope, and the superscript C represents the true measured value;
firstly, calibrating an accelerometer by adopting a twelve-position method, sequentially placing strapdown inertial navigation at corresponding positions of a mobile robot, and recording the output of the accelerometer at each position as follows:
Figure BDA0003487967750000032
wherein T is sampling time, and the accumulated output of each position is averaged to obtain
Figure BDA0003487967750000033
Figure BDA0003487967750000034
Representing the zero offset error, K, of the accelerometeraMatrix of scale coefficients representing an accelerometer, fbRepresenting specific force input along a strapdown inertial navigation axial direction, wherein X, Y and Z represent 3 coordinate axial directions, and i (i ═ 1,2,3., 12) represents a rotation sequence; is provided with
Figure BDA0003487967750000035
wherein ,
Figure BDA0003487967750000036
cumulative acceleration output, f, representing the x, y, z axesbxi,fbyi,fbziRepresenting proportional inputs along the x, y, z axes of the strapdown inertial navigation,
Figure BDA0003487967750000037
denotes the gradient change of acceleration in the x, y, z directions, aFx，aFy，aFzRepresenting the scale factor of the accelerometer, aAyx,aAzx，aAxy，aFzy，aAxz,aAyzThe table is the installation error coefficient of the accelerometer; thus, the acceleration can be recorded as a calibration value;
the calibration of the gyroscope adopts a static position method, namely, the zero offset of the gyroscope on the x axis, the y axis and the z axis is solved by utilizing the symmetrical relation among multiple positions; the coordinate transformation matrix between the four positions and the initial position is adopted as follows:
Figure BDA0003487967750000038
sequentially parking the strapdown inertial navigation at corresponding positions, and setting stop time, wherein the output of the gyroscope is respectively as follows:
Figure BDA0003487967750000041
where T is the sampling time, δ ω1δω2δω3δω4Representing the cumulative output value of angular velocity, K, for each positiongA matrix of scale coefficients representing the gyroscope,
Figure BDA0003487967750000042
indicating rotational angular velocity of the earthMagnitude of degree, epsilongZero bias for the gyroscope; the above formula is added to obtain:
Figure BDA0003487967750000043
Figure BDA0003487967750000044
wherein ,
Figure BDA0003487967750000045
therefore, the temperature of the molten metal is controlled,
Figure BDA0003487967750000046
and obtaining a calibration error model of the gyroscope.
As a preferred scheme of the present invention, the combined filtering algorithm adopted in step S4 specifically includes the following steps: the method comprises the steps of firstly using geometric sampling to carry out down-sampling processing on point cloud data in an underground coal mine to reduce calculated amount, then extracting point clouds in an x-axis direction, a y-axis direction and a z-axis positive direction in the underground coal mine through straight-through filtering, secondly using statistical filtering to remove outliers with average distances outside a standard range, and finally adopting a voxel filter to filter the point clouds.
As a preferred scheme of the present invention, a point cloud cavity is caused by the characteristic of laser signal absorption of the coal wall in the coal mine, the laser point cloud repairing algorithm adopted in step S5 adopts a triangular grid method and a least square method to repair the point cloud cavity, so that the curve fitting effect in the coal mine is better, specifically:
S-A1: firstly, smoothing the obtained point cloud data, specifically, establishing a fitting function on a local sub-domain U of a fitting region:
Figure BDA0003487967750000051
wherein a (x) ═ a1(x),a2(x)...am(x)]TFor the coefficients to be found, it is a function of the coordinates x, p (x) ═ p1(x), p2(x)]TCalled basis function, which is a polynomial of perfect order; the customary secondary radicals are [1, u, v, u2, v2, uv]TThen the above formula can be equivalent to the following:
Figure BDA0003487967750000052
minimizing the J function, w (x-x)i) Is node xiA weight function of; weight function w (x-x)i) Should have a tight support;
S-A2: calculating a normal vector of the cloud data of the underground coal mine point in the S-A1 step;
S-A3: establishing a cloud cavity characteristic plane of an underground point of a coal mine, and expressing the normal vector of the cavity characteristic plane as follows:
Figure BDA0003487967750000053
n represents the normal vector of the hole feature plane, NsumThe sum of the normal vectors of the triangular patches representing the boundaries of all holes; n iskThe normal vector of the kth triangular patch in the boundary grid array is taken as the vector; v is the number of patches in the boundary chain;
the base point O of the projection feature plane is expressed as:
Figure BDA0003487967750000054
m represents the number of vertices, piCoordinate values representing the ith vertex; the N and the O can be used for establishing a required coal mine underground hole characteristic plane;
S-A4: unreasonable triangular patches caused by dust influence under the coal mine are removed, and local characteristics are prevented from being lost;
S-A5: the method comprises the following steps of (1) avoiding the occurrence of irregular triangular patches in the subsequent hole grid repairing process, and preprocessing the hole boundary; solving an area side length coefficient Q to determine whether to process the patch;
Figure BDA0003487967750000055
alpha is a standardized parameter, h is the longest side length of the triangular patch, p is 1/2 of the perimeter of the triangular patch, and S is the area of the triangular patch; setting a threshold Q at the same time;
S-A6: extracting an irregular triangular plate surface according to the included angle value theta of the minimum included angle point on the hole characteristic plane; the grid growth termination condition is that the original holes can be completely covered by judging the newly added grids of the number of the boundary points, and the filling of the holes is finished;
S-A7: the least square surface fitting adjustment method utilizes the peak of the existing boundary mesh to adjust the peak of the repaired mesh;
Z(x,y)＝a00x0y0+a01x0y1+a02x0y2+a10x1y0+a11x1y1+a12x1y2+a20x2y0+a21x2y1+a22x2y2
aijfor a coefficient matrix of dimension p × q, least square fitting is to find the parameter aijMaking the objective function S in the following formula to be a minimum value;
Figure BDA0003487967750000061
f(xi,yi) Is a point (x)i,yi,zk) Corresponding to zkCoordinate values; omega (x, y, z) is a weight function and represents the contribution of the point to the whole curved surface, and n represents the point number of the curved surface to be fitted; solving for the S minimum, a must be satisfied as followsThe relationship is as follows:
Figure BDA0003487967750000062
the S function is used for obtaining the partial derivative and the shift term
Figure BDA0003487967750000063
Expressed as A, B and C: a, B, C, B, A-1C; and obtaining the curved surface parameters after least square fitting for adjusting the peak of the repaired hole mesh.
As a preferred scheme of the invention, in the process of walking of the remote control mobile robot, the explosion-proof laser radar and the strapdown inertial navigation are started to collect the underground environment information of the coal mine, and the information of the two sensors is fused by adopting an extended Kalman filtering mode to obtain the consistency explanation of the target to be measured; the method specifically comprises the following steps:
S-B1: establishing an inertial navigation state equation based on the IMU as a prediction:
Figure BDA0003487967750000064
the superscript W is expressed as a world coordinate system, the origin of the world coordinate system is the initial position of the mobile robot, the superscript b is the coordinate system of the strapdown inertial navigation, and P is expressed as the position of the strapdown inertial navigation coordinate system of the mobile robot; v is expressed as the driving speed of the strapdown inertial navigation coordinate system of the mobile robot, and q is expressed as the posture of the mobile robot expressed by quaternion;
Figure BDA0003487967750000065
expressed as the gyro error of the strapdown inertial navigation,
Figure BDA0003487967750000066
accelerometer error expressed as strapdown inertial navigation; the state equation under continuous time is a differential equation of state quantity, and specifically includes:
Figure BDA0003487967750000071
Figure BDA0003487967750000072
wherein the subscript a denotes the accelerometer correlation, the subscript g denotes the gyroscope correlation, abRepresenting strapdown inertial navigation
Measurement of an accelerometer, naRepresenting noise, omega, of a strapdown inertial navigation accelerometerbRepresenting measured values of a strapdown inertial navigation gyroscope, ngRepresents the noise of the strapdown inertial navigation gyroscope,
Figure BDA0003487967750000073
and
Figure BDA0003487967750000074
representing the random walk noise of the strapdown inertial navigation gyroscope and the accelerometer,
Figure BDA0003487967750000075
representing the conversion of a strapdown inertial navigation coordinate system into a world coordinate system;
writing a state equation under continuous time in the running process of the underground coal mine motor robot into the form of a matrix as follows:
Figure BDA0003487967750000076
wherein ,FtFor the current state quantity coefficient matrix, BtInputting a noise term coefficient matrix for the system, wherein M is an input noise matrix, covariance is Q, and D is a constant matrix;
discretizing the state equation under continuous time to obtain a recursion state equation of two adjacent times, namely:
xt＝Ft-1xt-1+Bt-1M+Dt-1
wherein, T is a Kalman filtering period:
Figure BDA0003487967750000077
Figure BDA0003487967750000078
Dt-1＝DT
S-B2: establishing a pose obtained by matching laser point clouds based on a laser radar observation equation
Figure BDA0003487967750000079
wherein ,
Figure BDA00034879677500000710
indicating the position of the mobile robot in the lidar coordinate system,
Figure BDA00034879677500000711
observing and representing the speed of the mobile robot under a laser radar coordinate system;
the system observations are expressed as:
Figure BDA0003487967750000081
the system observation equation under continuous time is as follows:
Figure BDA0003487967750000082
wherein ,q(vφ) Indicating that the euler angles are converted to quaternions,
Figure BDA0003487967750000083
the strapdown inertial navigation coordinate system is converted into the laser radar coordinate system,
Figure BDA0003487967750000084
indicating that the position in the lidar coordinate system is converted into the world coordinate system,
Figure BDA0003487967750000085
the expression converts quaternion under the laser radar coordinate system into a world coordinate system,
Figure BDA0003487967750000086
indicating the conversion of the position in the strapdown inertial navigation coordinate system into the world coordinate system, vplRepresenting the velocity in the lidar coordinate system,
Figure BDA0003487967750000087
a quaternion representing the strapdown inertial navigation coordinate system is converted into a world coordinate system;
writing the observation equation under continuous time into a matrix form:
z＝Gtx+CtN
wherein ,GtExpressed as a positioning system state quantity coefficient matrix; ctExpressed as a positioning system observation noise term coefficient matrix; n is an input noise matrix, and the observed noise covariance is R; the observation equation and the discretization observation equation under continuous time are consistent;
obtaining a prediction state according to a prediction equation as follows:
Figure BDA0003487967750000088
wherein ,
Figure BDA0003487967750000089
a predicted state value, F, representing the current timet-1State transition matrix representing the system, Bt-1Representing system control matrix coefficients, M representing system state transition matrix, Dt-1Representing a constant matrix, the predicted state covariance is:
Figure BDA00034879677500000810
wherein ,
Figure BDA00034879677500000811
expressed as the predicted system state covariance, QkRepresenting the noise covariance moment, the kalman gain is as follows:
Figure BDA00034879677500000812
the system update state equation is expressed as follows:
Figure BDA0003487967750000091
the updated system covariance is expressed as follows:
Figure BDA0003487967750000092
at this point, one cycle of state estimation is completed.
As a preferred embodiment of the present invention, the mobile robot positioning method with high precision comprises the following specific steps:
S-C1: the remote control mobile robot walks a circle under the coal mine, so that a running path of the mobile robot forms a closed loop, the operation is carried out by adopting a coal mine underground laser point cloud preprocessing method, the quality of the point cloud is improved, the positioning precision is improved, and the processing result is input into an improved cartographer algorithm to carry out coal mine underground image construction and high-precision positioning of the mobile robot;
S-C2: the point cloud registration adopts a cerees base to calculate the pose information of the point cloud scanned by the coal mine underground environment laser radar in the stored coal mine environment subgraph, and specifically comprises the following steps:
Figure BDA0003487967750000093
Figure BDA0003487967750000094
representing the pose transformation from the point cloud coordinate system l to the corresponding sub-image coordinate system s, BsmoothRepresenting that a laser point cloud preprocessing method is adopted to improve the point cloud quality;
S-C3: the remote control mobile robot does uniform motion under a coal mine, and the estimation of the current time position is established on the basis of the observation position matched with the last point cloud, and the method specifically comprises the following steps:
Figure BDA0003487967750000095
wherein ,T't and R'tIs the predicted position and predicted attitude at time T, Tt-1 and Rt-1The posterior position and the posterior attitude at the time T-1 are respectively, and the delta T is the translation from the time T-1 to the time T; Δ R is the amount of rotation from time t-1 to time t; t is the scanning time of the laser radar;
attitude estimation value R 'at current time t'tAnd the direction of gravity G at the present momenttAs follows:
Figure BDA0003487967750000096
wherein ,Rt-1Is the attitude of the mobile robot at time t-1, Gt-1Is the gravity direction of the mobile robot at the time of t-1, and has an initial value of [ 001%]T,
Figure BDA0003487967750000097
Where k is a constant, Δ timu_tIs the time interval of two IMU data;
the gravity direction of the mobile robot corrects the pose of the robot, and specifically comprises the following steps:
Figure BDA0003487967750000101
Vgt*Gt＝G′t
Rt＝R′t*Vgt
wherein ,
Figure BDA0003487967750000102
is a quaternion expression form V of the estimated attitude of the mobile robot at time tgtGravity direction G updated by linear velocity of mobile robottTo the direction of gravity G 'under the estimated pose'tThe amount of rotation of;
S-C4: a sparse pose adjustment method is adopted to eliminate accumulated errors of the sensor, a global map is modeled by a nonlinear least square method, and the speed of loop detection is accelerated by a branch-and-bound method; when the pose matching degree of the current point cloud is larger than a set threshold value, a loop is considered to be detected to form a constraint, and an optimal solution of the global pose is calculated by combining a nonlinear least square method;
the main idea of the branch-and-bound algorithm is to describe all possible poses as a tree, describing each node in the tree as:
N＝(cxcycθch)∈Z4
wherein ,cxcyLocation searched for node, cθFor the angle of the node search, chFor the height of a node, the algorithm is based on a given search height h0Branching a root node to a height h0And calculating the matching score of each child node in the C; and if the highest matching score in the child node set C is larger than the set score threshold value, detecting a loop, searching a leaf node with the largest upper bound in the child nodes with the highest score, and returning the pose of the leaf node, otherwise, not detecting the loop.
The invention has the technical effects that: the mobile robot collects the underground environment information of a coal mine through a plurality of carried sensors, firstly, the explosion-proof star laser radar and the strapdown inertial navigation are calibrated, then, the mobile robot is remotely controlled to travel a closed-loop route under the coal mine, the mobile robot keeps traveling at a constant speed as far as possible in the traveling process, the collected underground point cloud data of the coal mine is subjected to fusion filtering processing, outlier point clouds and noise point clouds are filtered, an improved cartographer algorithm is operated to construct an underground map of the coal mine, the accumulated error of a local constructed map is eliminated through loop detection, the loop detection speed is accelerated by a branch-and-bound method, the operation speed of a computer is high, and the positioning efficiency of the mobile robot is improved. The human resource cost is reduced, and the underground operation efficiency of the coal mine is improved.
Drawings
FIG. 1 is a flow chart of a method for environment sensing and high-precision positioning of a mobile robot in an underground coal mine;
FIG. 2 is a diagram of a newly added triangle;
FIG. 3 is a flow chart of the steps of a multi-sensor fusion algorithm.
Detailed Description
The technical solution of the present invention is further explained with reference to the drawings and the embodiments.
In the embodiment, the model of the explosion-proof laser radar is LR-16FIS-C1, and the model of strapdown inertial navigation LPMS-IG 1-485.
A method for sensing environment and positioning high precision of a coal mine underground mobile robot is disclosed, the technological process of the method is shown in figure 1, a plurality of sensors are adopted in the method, the plurality of sensors comprise an explosion-proof laser radar and a strapdown inertial navigation, and the explosion-proof laser radar and the strapdown inertial navigation perform the environment sensing and high precision positioning of the coal mine underground mobile robot according to the following steps:
pretreatment: installing an explosion-proof laser radar and a strapdown inertial navigation system on a hydraulically-driven quadruped robot, connecting the explosion-proof laser radar and the strapdown inertial navigation system with a power supply, establishing connection with a mobile robot, and setting the initial pose of the robot;
s1: and initializing, moving the robot to an initial point, and establishing communication between the robot and the computer.
S2: referring to fig. 1, it can be seen that the mobile robot is stationary at an initial position, and the explosion-proof laser radar and the strapdown inertial navigation are respectively calibrated, the explosion-proof laser radar is calibrated by using a minimum variance method, the strapdown inertial navigation is composed of a gyroscope and an accelerometer, the gyroscope is calibrated by using a static position method, and the accelerometer is calibrated by using a twelve-position method.
S3: and (3) remotely controlling the mobile robot to move, performing closed-loop uniform motion in the underground environment of the coal mine according to a preset route, and ending the motion from the starting point to the starting point.
S4: as can be seen from the combination of the figure 1, the mobile robot carries out combined filtering in various modes on the original signals collected by the explosion-proof laser radar, including direct filtering, statistical filtering and voxel filtering fusion modes, so that outliers and noise are filtered, useful point cloud information is obtained, and the point cloud quality collected by the laser radar in the coal mine is improved.
S5: and the mobile robot carries out point cloud hole restoration and restoration on the laser point cloud information through an image repairing algorithm on the preprocessed laser point cloud obtained in the last step.
S6: and (4) the mobile robot performs data fusion on the laser point cloud obtained in the step (S4) and data acquired by strapdown inertial navigation, performs data fusion of multiple sensors in an extended Kalman filtering mode, uses the data information of the strapdown inertial navigation as a predicted value, uses the point cloud data repaired by the laser radar as an observed value, corrects the prior information of the strapdown inertial navigation, and obtains the optimal pose estimation value of the underground coal mine mobile robot at the current moment.
S7: in the moving process of the remote control mobile robot, the mobile robot runs an improved cartographer algorithm to build a map, the fast search of closed-loop detection is realized through a branch-and-bound method, the accumulated error of the local map building of the sensor is eliminated through the loop-back detection, and the high-precision positioning is obtained.
S8: and the remote control mobile robot returns to the initial position and waits for the next instruction.
In the calibration method of the explosion-proof laser radar sensor in step S2 of this embodiment, a difference between the own coordinate system of the explosion-proof laser radar and the coordinate system of the required mobile robot is obtained, so as to obtain the measurement value of the laser radar in the coordinate system of the mobile robot, and unify the data of the multiple sensors, where the difference between the own coordinate system of the explosion-proof laser radar and the coordinate system of the required mobile robot is mainly composed of a rotation matrix and a translation matrix. The influence of the translation matrix on the final positioning result in the underground coal mine is relatively small and is ignored again. And the rotational matrix error affects the final positioning and mapping result.
The specific calibration method for the rotation matrix in this embodiment is to place the mobile robot on a road surface which is as flat as possible in the underground coal mine, so that distances from two sides of the mobile robot to the coal wall are equal, and extract point data on the coal wall and the underground coal mine surface through a random sample consensus algorithm, specifically:
1-P＝(1-Wn)k
wherein P represents the probability that RANSAC algorithm produces useful results for underground positioning as P, W represents the probability that randomly selected points in the collected point cloud data set at each time are local points, the local points are points playing a role in establishing a model, the collected noise point cloud is called as an local point, and the probability that at least one of the selected n points is the local point is 1-WnAnd k represents the number of iterations.
The calibration of the rotation matrix of the explosion-proof laser radar comprises three angles, namely a roll angle roll, a pitch angle pitch and a yaw angle yaw, the explosion-proof laser radar is static on the smooth underground ground of the coal mine, the height information of point clouds on all the ground is consistent under an ideal condition, but the roll angle roll and the pitch angle pitch of the explosion-proof laser radar influence the height value, the roll angle roll and the pitch angle pitch need to be found in an angle compensation mode to enable the variance of the point clouds on the underground ground of the coal mine to be minimum, and the variance is preferably close to 0, so that the roll angle roll and the pitch angle pitch are obtained, similarly, the explosion-proof laser radar is static on the smooth underground ground, the point cloud depth information of all the coal walls under the ideal condition is consistent, but the yaw angle yaw of the explosion-proof laser radar influences the height value, and the variance of the point clouds on the underground coal wall of the coal mine needs to be found in an angle compensation mode to enable the coal mine to be minimum, preferably, the yaw angle yaw is obtained by approximating to 0, and the rotation matrix can be obtained.
The strapdown inertial navigation sensor calibration method in this embodiment is respectively providedMounting error T0Error of scale factor S0Zero offset error Z0Thus acceleration a of strapdown inertial navigationBAnd a gyroscope wBThe measurement error model of (2) is: the measurement error model of the strapdown inertial navigation is as follows:
Figure BDA0003487967750000121
the superscript B represents the corrected value, the superscript a represents the accelerometer value, the superscript g represents the gyroscope value, and the superscript C represents the true measurement value.
The strapdown inertial navigation system comprises an accelerometer and a gyroscope, the accelerometer is calibrated by adopting a twelve-position method, the strapdown inertial navigation system is placed at the corresponding position of the mobile robot in sequence, and the output of the accelerometer at each position is recorded as follows:
Figure BDA0003487967750000122
where T is the sampling time, the cumulative output for each position is averaged to obtain
Figure BDA0003487967750000123
Figure BDA0003487967750000124
Representing the zero offset error, K, of the accelerometeraMatrix of scale coefficients representing an accelerometer, fbRepresenting specific force input along a strapdown inertial navigation axial direction, wherein X, Y and Z represent 3 coordinate axial directions, and i (i ═ 1,2,3., 12) represents a rotation sequence; is provided with
Figure BDA0003487967750000131
wherein ,
Figure BDA0003487967750000132
cumulative acceleration output, f, representing the x, y, z axesbxi,fbyi,fbziRepresenting proportional inputs along the x, y, z axes of the strapdown inertial navigation,
Figure BDA0003487967750000133
denotes the gradient change of acceleration in the x, y, z directions, aFx，aFy，aFzRepresenting the scale factor of the accelerometer, aAyx,aAzx，aAxy，aFzy，aAxz,aAyzThe table is the installation error coefficient of the accelerometer. Thus, the acceleration recording calibration value can be obtained.
The gyroscope is calibrated by adopting a static position method, namely, the zero offset of the gyroscope on the x axis, the y axis and the z axis is solved by utilizing the symmetrical relation among multiple positions. In this embodiment, a coordinate transformation matrix between four positions and an initial position is:
Figure BDA0003487967750000134
the strapdown inertial navigation is sequentially parked at corresponding positions, in this embodiment, the stop time is set to 5 hours, and then the gyroscope outputs are respectively:
Figure BDA0003487967750000135
wherein T is sampling time, delta omega1δω2δω3δω4Representing the cumulative output value of angular velocity, K, for each positiongA matrix of scale coefficients representing the gyroscope,
Figure BDA0003487967750000136
representing the magnitude of angular velocity of rotation of the earth, epsilongIs the zero bias of the gyroscope. The above formula is added to obtain:
Figure BDA0003487967750000137
Figure BDA0003487967750000138
wherein ,
Figure BDA0003487967750000139
therefore, the number of the first and second electrodes is increased,
Figure BDA0003487967750000141
and obtaining a calibration error model of the gyroscope.
In the high-precision positioning method for the coal mine underground mobile robot in the embodiment, the combined filtering algorithm adopted in the step S4 specifically includes:
in the embodiment, firstly, geometric sampling is used for carrying out down-sampling processing on cloud data of underground points of a coal mine, the calculated amount is reduced, then point cloud data in the directions of 2 meters at the left side and the right side of the x-axis direction of the coal mine, 10 meters at the front side and the back side of the y-axis direction and 2.5 meters at the positive direction of the z-axis are extracted through straight-through filtering, then outliers with the average distance outside a standard range are removed through statistical filtering, finally a voxel filter is adopted to create a three-dimensional voxel grid for the input point cloud data, then, in each voxel, the gravity centers of all points in the voxel are used for approximately displaying other points in the voxel, and therefore all points in the voxel are finally represented by one gravity center point.
In the embodiment, the high-precision positioning method for the mobile robot in the underground coal mine adopts a laser point cloud repairing algorithm, and combines a triangular grid method and a least square method to repair a point cloud cavity, so that the curved surface fitting effect in the underground coal mine is better, and the method specifically comprises the following steps:
S-A1: firstly, point cloud data is required to be smoothed, specifically, on a local subdomain U of a fitting region,
establishing a fitting function:
Figure BDA0003487967750000142
wherein a (x) ═ a1(x),a2(x)...am(x)]TFor the coefficients to be found, it is a function of the coordinates x, p (x) ═ p1(x), p2(x)]TReferred to as a basis function, which is a polynomial of order perfect. The customary secondary radicals are [1, u, v, u2, v2, uv]TThen the above formula can be equivalent to the following:
Figure BDA0003487967750000143
minimizing the J function, w (x-x)i) Is node xiThe weight function of (2). Weight function w (x-x)i) Should have a tight support.
S-A2: calculating a normal vector of the cloud data of the underground coal mine point in the S-A1 step, specifically:
(1) selecting the current point cloud pi(x, y, z); (2) finding point p by using kd-treeiForming a matrix A of m by 3 by the nearest m points; (3) calculating a covariance matrix A' A; (4) solving the eigenvalue lambda of the covariance matrix obtained in (3)1λ2λ3(ii) a (5) Take lambda1λ2λ3Minimum eigenvalue λ ofmin(ii) a (6) Calculating piCurvature of (2): lambda [ alpha ]min/(λ1+λ2+λ3) (ii) a Therefore, the triangularization representation result of the laser point cloud data in the coal mine is obtained.
S-A3: establishing a cloud cavity characteristic plane of a coal mine underground point, and expressing a normal vector of the cavity characteristic plane as follows:
Figure BDA0003487967750000151
n denotes the normal vector of the hole feature plane, NsumThe sum of the normal vectors of the triangular patches representing the boundaries of all holes; n iskThe normal vector of the kth triangular patch in the boundary grid array is taken as the vector; v is the number of patches in the boundary chain.
The base point O of the projection feature plane is expressed as:
Figure BDA0003487967750000152
m represents the number of vertices, piAnd a coordinate value indicating the ith vertex. And the N and the O can be used for establishing a required coal mine underground hole characteristic plane.
S-A4: and unreasonable triangular patches caused by dust influence under the coal mine are removed, so that local characteristics are prevented from being lost.
S-A5: and irregular triangular patches are prevented from appearing in the subsequent hole mesh repairing process, and hole boundaries are preprocessed. And solving an area side length coefficient Q to determine whether the patch is processed.
Figure BDA0003487967750000153
Figure BDA0003487967750000154
For a normalization parameter, h is the longest edge length of the triangle patch, p is 1/2 of the perimeter of the triangle patch, and S is the area of the triangle patch. In this embodiment, a threshold value of Q is taken as 0.25, and if Q is less than 0.25, it is taken as a triangular patch to be optimized, otherwise, optimization is not required.
S-A6: according to the included angle value theta of the minimum included angle point on the hole characteristic plane, the following conditions are divided:
(1)θ<90 DEG, directly connecting two adjacent boundary points of the point to form a new triangular patch Ai-1AiAi+1If the length of the added edge of the triangular patch is as | A in FIG. 2ai-1Ai+1|/Lavg＞1.5(LavgAverage boundary edge length, the same applies below), the triangular patch is edge-split, increasing vertex B1Is Ai-1Ai+1While using a triangular patch Ai-1B1Ai and AiB1Ai+1Replace the original newly added triangular patch Ai-1AiAi+1(ii) a If | Ai-1Ai+1|/LavgIf less than 1.5, adding new triangular patch Ai-1AiAi+1
(2) Theta is more than 90 DEG and less than or equal to 135 DEG, and the vertex B is increased as shown in figure 2B1，B1In Ai-1，Ai and Ai+1On the plane, | AiB1|＝(|Ai-1Ai+AiAi+1I))/2 and AiB1Is less than Ai-1AiAi+1Halving and newly adding triangular patch Ai-1B1Ai and AiB1Ai+1。
(3) Theta is more than 135 degrees and less than or equal to 200 degrees, as shown in figure 2c, the vertex B is added1 and B2，B1 and B2In Ai-1，Ai and Ai+1On the plane, | AiB1|＝|AiB2|＝(|Ai-1Ai+AiAi+1I))/2 and AiB1 and AiB2Is less than Ai-1AiAi+1Trisection, newly adding triangular patch Ai-1B1Ai，B1AiB2 and AiB2Ai+1。
(4) Theta ≧ 200 DEG, increasing vertex B as shown in FIG. 2d1 and B2，B1 and B2In Ai-1，Ai and Ai+1On the plane, a newly added triangular patch Ai-1B1Ai and AiB2Ai+1Are all regular triangles.
The grid growth termination condition is that the number of boundary points is judged, when the number of the remaining boundary points is equal to 3, the hole repairing is finished, a triangular patch formed by the remaining 3 vertexes is added, the original holes can be completely covered by the newly added grid, and the hole filling is finished.
S-A7: the least square surface fitting and adjusting method utilizes the peak of the existing boundary mesh to adjust the peak of the repaired mesh.
Z(x,y)＝a00x0y0+a01x0y1+a02x0y2+a10x1y0+a11x1y1+a12x1y2+a20x2y0+a21x2y1+a22x2y2
aijFor a coefficient matrix of dimension p × q, least square fitting is to find the parameter aijThe objective function S in the following equation is set to a minimum value.
Figure BDA0003487967750000161
f(xi,yi) Is a point (x)i,yi,zk) Corresponding to zkCoordinate values; ω (x, y, z) is a weight function, which represents the contribution of the point to the whole curved surface, and the values of the present embodiment are all 1, that is, the contribution of the point to the curved surface on the curved surface is considered to be equal; n represents the number of points of the surface to be fitted. Solving for the S minimum, a must satisfy the following relationship:
Figure BDA0003487967750000162
the S function is used for obtaining the partial derivative and the shift term
Figure BDA0003487967750000163
Expressed as A, B and C: a, B, C, B, A-1C. And obtaining the curved surface parameters after least square fitting for adjusting the peak of the repaired hole mesh.
With reference to fig. 3, it can be seen that the multi-sensor data fusion method starts the explosion-proof laser radar and the strapdown inertial navigation operation during the walking process of the remote control mobile robot, collects the environmental information of the underground coal mine, and fuses the information of the two sensors in the extended kalman filtering manner to obtain the consistency explanation of the target to be measured. The method specifically comprises the following steps:
S-B1: establishing an inertial navigation state equation based on the IMU as a prediction:
Figure BDA0003487967750000171
the superscript W is expressed as a world coordinate system, the origin of the world coordinate system is the initial position of the mobile robot, the superscript b is the coordinate system of the strapdown inertial navigation, and P is expressed as the position of the strapdown inertial navigation coordinate system of the mobile robot; v is expressed as the driving speed of the strapdown inertial navigation coordinate system of the mobile robot, and q is expressed as the posture of the mobile robot expressed by quaternion;
Figure BDA0003487967750000172
expressed as the gyroscope error of strapdown inertial navigation,
Figure BDA0003487967750000173
expressed as the accelerometer error of strapdown inertial navigation. The state equation under continuous time is a differential equation of state quantity, and specifically includes:
Figure BDA0003487967750000174
Figure BDA0003487967750000175
in the present embodiment, the subscript a represents the correlation value of the accelerometer, the subscript g represents the correlation value of the gyroscope, and abRepresenting measurements of strapdown inertial navigation accelerometers, naRepresenting noise, omega, of a strapdown inertial navigation accelerometerbRepresenting measured values of a strapdown inertial navigation gyroscope, ngRepresents the noise of the strapdown inertial navigation gyroscope,
Figure BDA0003487967750000176
and
Figure BDA0003487967750000177
representing the random walk noise of the strapdown inertial navigation gyroscope and the accelerometer,
Figure BDA0003487967750000178
the representation transforms the strapdown inertial navigation coordinate system into the world coordinate system.
Writing a state equation under continuous time in the running process of the underground coal mine motor robot into the form of a matrix as follows:
Figure BDA0003487967750000179
wherein ,FtFor the current state quantity coefficient matrix, BtA noise term coefficient matrix is input into the system, M is an input noise matrix, covariance is Q, and D is a constant matrix.
Discretizing the state equation under continuous time to obtain a recursion state equation of two adjacent times, namely:
xt＝Ft-1xt-1+Bt-1M+Dt-1
wherein, T is a Kalman filtering period:
Figure BDA0003487967750000181
Figure BDA0003487967750000182
Dt-1＝DT
S-B2: establishing a laser radar observation equation based pose obtained by matching laser point clouds in the embodiment
Figure BDA0003487967750000183
wherein ,
Figure BDA0003487967750000184
indicating excitation of a mobile robotThe position of the optical radar under the coordinate system,
Figure BDA0003487967750000185
the view represents the velocity of the mobile robot in the lidar coordinate system.
The system observations are expressed as:
Figure BDA0003487967750000186
the system observation equation under continuous time is as follows:
Figure BDA0003487967750000187
wherein ,q(vφ) Indicating that the euler angles are converted to quaternions,
Figure BDA0003487967750000188
the strapdown inertial navigation coordinate system is converted into the laser radar coordinate system,
Figure BDA0003487967750000189
indicating that the position in the lidar coordinate system is converted into the world coordinate system,
Figure BDA00034879677500001810
the expression converts quaternion under the laser radar coordinate system into a world coordinate system,
Figure BDA00034879677500001811
indicating the conversion of the position in the strapdown inertial navigation coordinate system into the world coordinate system, vplRepresenting the velocity in the lidar coordinate system,
Figure BDA00034879677500001812
and (4) converting quaternions in the strapdown inertial navigation coordinate system into a world coordinate system.
In this embodiment, the observation equation under continuous time is written in the form of a matrix:
z＝Gtx+CtN
wherein ,GtExpressed as a positioning system state quantity coefficient matrix; ctExpressed as a positioning system observation noise term coefficient matrix; n is the input noise matrix and the observed noise covariance is R. The observation equation and the discretized observation equation are consistent in continuous time. S-B3: in this embodiment, the prediction state obtained according to the prediction equation is:
Figure BDA0003487967750000191
wherein ,
Figure BDA0003487967750000192
a predicted state value, F, representing the current timet-1State transition matrix representing the system, Bt-1Representing system control matrix coefficients, M representing system state transition matrix, Dt-1A matrix of constants is represented.
S-B4: the predicted state covariance in this example is:
Figure BDA0003487967750000193
wherein ,
Figure BDA0003487967750000194
expressed as the predicted system state covariance, QkRepresenting the noise covariance moment.
S-B5: the kalman gain is as follows:
Figure BDA0003487967750000195
the system update state equation in this embodiment is expressed as follows:
Figure BDA0003487967750000196
the updated system covariance in this embodiment is expressed as follows:
Figure BDA0003487967750000197
at this point, one cycle of state estimation is completed.
In this embodiment, the specific steps of the mobile robot high-precision positioning are as follows:
S-C1: the remote control mobile robot walks a circle under a coal mine, so that a running path of the mobile robot forms a closed loop, real-time data under the coal mine is collected through an anti-explosion laser radar and a strapdown inertial navigation sensor which are carried in the walking process, the operation is carried out by adopting a coal mine underground laser point cloud preprocessing method, the quality of point cloud is improved, the positioning precision is improved, and a processing result is input into an improved cartographer algorithm to carry out coal mine underground map construction and high-precision positioning of the mobile robot.
S-C2: the point cloud registration adopts position and attitude information of the point cloud scanned by the coal mine underground environment laser radar in the stored coal mine environment subgraph based on a ceres library, and the ceres library converts a position and attitude solving problem into a nonlinear least square problem to accelerate the calculation speed, and specifically comprises the following steps:
Figure BDA0003487967750000198
Figure BDA0003487967750000199
representing the pose transformation from the point cloud coordinate system l system to the corresponding sub-image coordinate system s system, which transforms the point p in the preprocessed laser point cloudkConverting the laser point cloud coordinate system l system into a coal mine environment subgraph coordinate system s system; b issmoothAnd the laser point cloud preprocessing method is adopted to improve the point cloud quality. The good initial pose is beneficial to the creation of a local map, and the short-time estimation of the IMU can provide the good initial pose for a positioning algorithm.
S-C3: the remote control mobile robot does uniform motion under a coal mine, and the estimation of the current-moment position is established on the basis of the observation position matched with the previous point cloud, and the method specifically comprises the following steps:
Figure BDA0003487967750000201
wherein ,T′t and R′tIs the predicted position and predicted attitude at time T, Tt-1 and Rt-1The posterior position and the posterior attitude at the time T-1 are respectively, and the delta T is the translation from the time T-1 to the time T; Δ R is the amount of rotation from time t-1 to time t; and t is the scanning moment of the laser radar.
Attitude estimation value R 'at current time t'tAnd the direction of gravity G at the present momenttAs follows:
Figure BDA0003487967750000202
wherein ,Rt-1Is the attitude of the mobile robot at time t-1, Gt-1Is the gravity direction of the mobile robot at the time of t-1, and has an initial value of [ 001%]T,
Figure BDA0003487967750000203
Where k is a constant, Δ timu_tIs the time interval of two IMU data.
The gravity direction of the mobile robot corrects the pose of the robot, and specifically comprises the following steps:
Figure BDA0003487967750000204
Vgt*Gt＝G′t
Rt＝R′t*Vgt
wherein ,
Figure BDA0003487967750000205
is a movement at time tQuaternion expression form, V, of the estimated pose of the robotgtGravity direction G updated by linear velocity of mobile robottTo the direction of gravity G 'under the estimated pose'tThe amount of rotation of.
S-C4: in the local map construction in the S-C3 step, the pose of the current moment is calculated through the sum of the translation amount and the rotation amount, and the laser point cloud is only matched with the recently established sub-graph, so that the drift error is larger and larger along with the increase of the running distance of the coal mine underground mobile robot.
And a sparse pose adjustment method is adopted to eliminate the accumulated error of the sensor, a global map is modeled by a nonlinear least square method, and the speed of loop-back detection is accelerated by a branch-and-bound method. And when the pose matching degree of the current point cloud is changed to the set threshold value, a loop is considered to be detected to form a constraint, and the optimal solution of the global pose is calculated by combining a nonlinear least square method.
The main idea of the branch-and-bound algorithm is to describe all possible poses as a tree, describing each node in the tree as:
N＝(cxcycθch)∈Z4
wherein ,cxcyLocation searched for node, cθFor the angle of the node search, chFor the height of a node, the algorithm is based on a given search height h0Branching a root node to a height h0And calculating a matching score for each child node in C. And if the highest matching score in the child node set C is larger than the set score threshold value, detecting a loop, searching a leaf node with the largest upper bound in the child nodes with the highest score, and returning the pose of the leaf node, otherwise, not detecting the loop.
The working principle of the invention is as follows:
the multi-sensor fusion of the coal mine underground mobile robot comprises the following steps:
in the process of walking of the remote control mobile robot, starting the explosion-proof laser radar and the strapdown inertial navigation to work, collecting underground environment information of a coal mine, fusing information of two sensors in an extended Kalman filtering mode, taking collected data of the strapdown inertial navigation as Kalman algorithm prediction measurement, taking the collected data of the laser radar as observation measurement of the Kalman algorithm, correcting and updating the value of the prediction measurement through the value of optical measurement, and obtaining the consistency explanation of a measured target.
High-precision positioning of the coal mine underground mobile robot:
the remote control mobile robot walks in a coal mine for a circle, so that a traveling path of the mobile robot forms a closed loop, real-time data in the coal mine are acquired through an explosion-proof laser radar and a strapdown inertial navigation sensor which are carried in the traveling process, the operation is carried out by adopting a coal mine underground laser point cloud preprocessing method, the quality of point cloud is improved, the positioning precision is improved, and the processing result is input into an improved cartographer algorithm to construct and position the underground coal mine image of the mobile robot with high precision.
Finally, the above embodiments are only for illustrating the technical solutions of the present invention and not for limiting, although the present invention has been described in detail with reference to the preferred embodiments, it should be understood by those skilled in the art that modifications or equivalent substitutions may be made to the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention, and all of them should be covered in the claims of the present invention.

Claims (7)

1. The method for environment sensing and high-precision positioning of the coal mine underground mobile robot is characterized in that a plurality of sensors are adopted, the sensors comprise an explosion-proof laser radar and a strapdown inertial navigation system, and the explosion-proof laser radar and the strapdown inertial navigation system are used for environment sensing and high-precision positioning of the coal mine underground mobile robot according to the following steps:
pretreatment: installing an explosion-proof laser radar and a strapdown inertial navigation system on a hydraulically-driven quadruped robot, connecting the explosion-proof laser radar and the strapdown inertial navigation system with a power supply, establishing connection with a mobile robot, and setting the initial pose of the robot under a coal mine;
s1: initializing, namely moving the coal mine underground to a starting point by the mobile robot, and establishing communication between the mobile robot and a computer;
s2: the mobile robot is static at an initial position, and the explosion-proof laser radar and the strapdown inertial navigation are respectively calibrated, wherein the explosion-proof laser radar is calibrated by adopting a minimum variance method, the strapdown inertial navigation consists of a gyroscope and an accelerometer, the gyroscope is calibrated by adopting a static position method, and the accelerometer is calibrated by adopting a twelve-position method;
s3: remotely controlling the mobile robot to move, performing closed-loop uniform motion in the underground environment of the coal mine according to a preset route, and finishing the motion from a starting point to the starting point;
s4: the mobile robot carries out combined filtering in various modes on original signals collected by the explosion-proof laser radar, wherein the combined filtering comprises a direct filtering mode, a statistical filtering mode and a voxel filtering fusion mode, outliers and noise are filtered, useful point cloud information is obtained, and the quality of point cloud collected by the laser radar in the coal mine is improved;
s5: the mobile robot carries out point cloud hole restoration and restoration on the laser point cloud information through an image repairing algorithm on the preprocessed laser point cloud obtained in the last step;
s6: the mobile robot performs data fusion on the laser point cloud obtained in the step S5 and data acquired by strapdown inertial navigation, performs data fusion of multiple sensors in an extended Kalman filtering mode, uses data information of the strapdown inertial navigation as a predicted value, uses point cloud data repaired by a laser radar as an observed value, corrects prior information of the strapdown inertial navigation, and obtains an optimal pose estimation value of the underground coal mine mobile robot at the current moment;
s7: in the moving process of the remote control mobile robot, the mobile robot runs an improved cartographer algorithm to build a picture, fast search of closed-loop detection is realized through a branch-and-bound method, and the accumulated error of local picture building of a sensor is eliminated through loop-back detection to obtain high-precision positioning;
s8: and the remote control mobile robot returns to the initial position and waits for the next instruction.
2. The method for environmental perception and high-precision positioning of the coal mine underground mobile robot according to claim 1 is characterized in that a difference value between an own coordinate system of the explosion-proof laser radar and a coordinate system of the required mobile robot is obtained, and the difference value between the own coordinate system of the explosion-proof laser radar and the coordinate system of the required mobile robot mainly consists of a rotation matrix and a translation matrix; the influence of the translation matrix on the final positioning result of the underground coal mine is relatively small and is ignored; the rotation matrix error can affect the final positioning and mapping result;
the specific calibration method for the rotation matrix comprises the following steps of placing the mobile robot on a road surface which is as flat as possible under a coal mine, enabling the distances from two sides of the mobile robot to a coal wall to be equal, and extracting data of the coal wall and data of data on the underground coal mine surface through a RANSAC random sampling consistency algorithm, wherein the specific calibration method specifically comprises the following steps:
1-P＝(1-Wn)k
wherein, P represents the probability that RANSAC algorithm produces useful result for underground positioning as P, W represents the probability that randomly selected points in the collected point cloud data set are local points each time, the collected noise point cloud is called local points, and the probability that at least one of the selected n points is a local point is 1-WnK is the number of iterations;
the explosion-proof laser radar is still on the ground under a flat coal mine, the point cloud height information on all the ground is consistent under the ideal condition, however, the roll angle roll and the pitch angle pitch of the explosion-proof laser radar will affect the height value, and the roll angle roll and the pitch angle pitch need to be found in an angle compensation manner, so that the variance of the point cloud on the underground ground of the coal mine is the minimum, preferably close to 0, so as to obtain the roll angle and pitch angle, and in the same way, the explosion-proof laser radar is still on the ground under the flat coal mine, and the point cloud depth information of all coal walls is identical under the ideal condition, however, the yaw angle yaw of the explosion-proof laser radar affects the height value, and the yaw angle yaw needs to be found in an angle compensation mode to minimize the point cloud variance of the coal wall under the coal mine, and the point cloud variance is preferably close to 0, so that the yaw angle yaw is obtained, and the rotation matrix can be obtained.
3. The environment of claim 1 for the mobile robot in a coal mineThe sensing and high-precision positioning method is characterized in that the calibration of the strapdown inertial navigation is divided into three parts, and errors T are respectively installed0Error of scale factor S0Zero offset error Z0Thus acceleration a of strapdown inertial navigationBAnd a gyroscope wBThe measurement error model of (2) is:
Figure FDA0003487967740000021
the superscript B represents the value after correction, the superscript a represents the value of the accelerometer, the superscript g represents the value of the gyroscope, and the superscript C represents the true measured value;
firstly, calibrating an accelerometer by adopting a twelve-position method, sequentially placing strapdown inertial navigation at corresponding positions of a mobile robot, and recording the output of the accelerometer at each position as follows:
Figure FDA0003487967740000022
where T is the sampling time, the cumulative output for each position is averaged to obtain
Figure FDA0003487967740000023
Figure FDA0003487967740000024
Representing the zero offset error, K, of the accelerometeraMatrix of scale coefficients representing an accelerometer, fbRepresenting specific force input along a strapdown inertial navigation axial direction, wherein X, Y and Z represent 3 coordinate axial directions, and i (i ═ 1,2,3., 12) represents a rotation sequence; is provided with
Figure FDA0003487967740000031
wherein ,
Figure FDA0003487967740000032
representing x, y, z axesCumulative acceleration output value, fbxi,fbyi,fbziRepresenting proportional inputs along the x, y, z axes of the strapdown inertial navigation,
Figure FDA0003487967740000033
representing the change in the gradient of the accelerometer in the x, y, z directions, aFx，aFy，aFzRepresenting the scale factor of the accelerometer, aAyx,aAzx，aAxy，aFzy，aAxz,aAyzThe table is the installation error coefficient of the accelerometer; thus, the acceleration can be recorded as a calibration value;
the calibration of the gyroscope adopts a static position method, namely, the zero offset of the gyroscope on the x axis, the y axis and the z axis is solved by utilizing the symmetrical relation among multiple positions; the coordinate transformation matrix between the four positions and the initial position is adopted as follows:
Figure FDA0003487967740000034
sequentially parking the strapdown inertial navigation at corresponding positions, and setting stop time, wherein the output of the gyroscope is respectively as follows:
Figure FDA0003487967740000035
where T is the sampling time, δ ω1δω2δω3δω4Representing the cumulative output value of angular velocity, K, for each positiongA matrix of scale coefficients representing the gyroscope,
Figure FDA0003487967740000036
representing the magnitude of angular velocity of rotation of the earth, epsilongZero bias for the gyroscope; the above formula is added to obtain:
Figure FDA0003487967740000037
Figure FDA0003487967740000038
wherein ,
Figure FDA0003487967740000039
therefore, the temperature of the molten metal is controlled,
Figure FDA0003487967740000041
and obtaining a calibration error model of the gyroscope.
4. The method for environmental perception and high-precision positioning of the coal mine underground mobile robot according to claim 1, wherein the combined filtering algorithm adopted in the step S4 specifically comprises the following steps: the method comprises the steps of firstly using geometric sampling to carry out down-sampling processing on point cloud data in an underground coal mine to reduce calculated amount, then extracting point clouds in an x-axis direction, a y-axis direction and a z-axis positive direction in the underground coal mine through straight-through filtering, secondly using statistical filtering to remove outliers with average distances outside a standard range, and finally adopting a voxel filter to filter the point clouds.
5. The method for environmental awareness and high-precision positioning of the mobile robot in the coal mine well according to claim 4, wherein a point cloud cavity is caused by the characteristic of laser signal absorption of the coal mine well wall, and the laser point cloud repairing algorithm adopted in the step S5 adopts a triangular grid method and a least square method to repair the point cloud cavity, so that the curve fitting effect in the coal mine well is better, specifically:
S-A1: firstly, smoothing the obtained point cloud data, specifically, establishing a fitting function on a local sub-domain U of a fitting region:
Figure FDA0003487967740000042
wherein a (x) ═ a1(x),a2(x)...am(x)]TFor the coefficients to be found, it is a function of the coordinate x, p (x) ═ p1(x),p2(x)...pm(x)]TCalled basis function, which is a polynomial of perfect order; the customary secondary radicals are [1, u, v, u2,v2,uv]TThen the above formula can be equivalent to the following:
Figure FDA0003487967740000043
minimizing the J function, w (x-x)i) Is node xiA weight function of (a); weight function w (x-x)i) Should have a tight support;
S-A2: calculating a normal vector of the cloud data of the underground coal mine points in the S-A1 step;
S-A3: establishing a cloud cavity characteristic plane of an underground point of a coal mine, and expressing the normal vector of the cavity characteristic plane as follows:
Figure FDA0003487967740000051
n denotes the normal vector of the hole feature plane, NsumThe sum of the normal vectors of the triangular patches representing the boundaries of all holes; n iskThe normal vector of the kth triangular patch in the boundary grid array is taken as the vector; v is the number of patches in the boundary chain;
the base point O of the projection feature plane is expressed as:
Figure FDA0003487967740000052
m represents the number of vertices, piCoordinate values representing the ith vertex; the N and the O can be used for establishing a required coal mine underground hole characteristic plane;
S-A4: unreasonable triangular patches caused by dust influence under the coal mine are removed, and local characteristics are prevented from being lost;
S-A5: the method comprises the following steps of (1) avoiding the occurrence of irregular triangular patches in the subsequent hole grid repairing process, and preprocessing the hole boundary; solving an area side length coefficient Q to determine whether to process the patch;
Figure FDA0003487967740000053
alpha is a standardized parameter, h is the longest side length of the triangular patch, p is 1/2 of the perimeter of the triangular patch, and S is the area of the triangular patch; setting a threshold Q at the same time;
S-A6: extracting an irregular triangular plate surface according to the included angle value theta of the minimum included angle point on the hole characteristic plane; the grid growth termination condition is that the original holes can be completely covered by judging the newly added grids of the number of the boundary points, and the filling of the holes is finished;
S-A7: the least square surface fitting adjustment method utilizes the peak of the existing boundary mesh to adjust the peak of the repaired mesh;
Z(x,y)＝a00x0y0+a01x0y1+a02x0y2+a10x1y0+a11x1y1+a12x1y2+a20x2y0+a21x2y1+a22x2y2
aijfor a coefficient matrix of dimension p × q, least square fitting is to find the parameter aijMaking the objective function S in the following formula to be a minimum value;
Figure FDA0003487967740000054
f(xi,yi) Is a point (x)i,yi,zk) Corresponding to zkCoordinate values; omega (x, y, z) is a weight function and represents the contribution of the point to the whole curved surface, and n represents the point number of the curved surface to be fitted; solving for the S minimum, a must satisfy the following relationship:
Figure FDA0003487967740000055
the S function is used for obtaining the partial derivative and the shift term
Figure FDA0003487967740000061
Expressed as A, B and C: a, B, C, B, A-1C; and obtaining the curved surface parameters after least square fitting for adjusting the peak of the repaired hole mesh.
6. The method for environment sensing and high-precision positioning of the coal mine underground mobile robot according to claim 1 is characterized in that in the process of walking of the remote control mobile robot, an explosion-proof laser radar and a strapdown inertial navigation are started, underground environment information of the coal mine is collected, information of two sensors is fused in an extended Kalman filtering mode, and consistency explanation of a target to be measured is obtained; the method specifically comprises the following steps:
S-B1: establishing an inertial navigation state equation based on the IMU as a prediction:
Figure FDA0003487967740000062
the superscript W is expressed as a world coordinate system, the origin of the world coordinate system is the initial position of the mobile robot, the superscript b is the coordinate system of the strapdown inertial navigation, and P is expressed as the position of the strapdown inertial navigation coordinate system of the mobile robot; v is expressed as the driving speed of the strapdown inertial navigation coordinate system of the mobile robot, and q is expressed as the posture of the mobile robot expressed by quaternion;
Figure FDA0003487967740000063
expressed as the gyro error of the strapdown inertial navigation,
Figure FDA0003487967740000064
accelerometer error expressed as strapdown inertial navigation; the state equation under continuous time is a differential equation of state quantity, and specifically includes:
Figure FDA0003487967740000065
Figure FDA0003487967740000066
wherein the subscript a denotes the accelerometer correlation, the subscript g denotes the gyroscope correlation, abRepresenting measurements of strapdown inertial navigation accelerometers, naRepresenting noise, omega, of a strapdown inertial navigation accelerometerbRepresenting measured values of a strapdown inertial navigation gyroscope, ngRepresents the noise of the strapdown inertial navigation gyroscope,
Figure FDA0003487967740000067
and
Figure FDA0003487967740000068
representing the random walk noise of the strapdown inertial navigation gyroscope and the accelerometer,
Figure FDA0003487967740000071
representing the conversion of a strapdown inertial navigation coordinate system into a world coordinate system;
writing a state equation under continuous time in the running process of the underground coal mine motor robot into the form of a matrix as follows:
Figure FDA0003487967740000072
wherein ,FtFor the current state quantity coefficient matrix, BtInputting a noise term coefficient matrix for the system, wherein M is an input noise matrix, covariance is Q, and D is a constant matrix;
discretizing the state equation under continuous time to obtain a recursion state equation of two adjacent times, namely:
xt＝Ft-1xt-1+Bt-1M+Dt-1
wherein, T is a Kalman filtering period:
Figure FDA0003487967740000073
Figure FDA0003487967740000074
Dt-1＝DT
S-B2: establishing a pose obtained by matching laser point clouds based on a laser radar observation equation
Figure FDA0003487967740000075
wherein ,
Figure FDA0003487967740000076
indicating the position of the mobile robot in the lidar coordinate system,
Figure FDA0003487967740000077
observing and representing the speed of the mobile robot under a laser radar coordinate system;
the system observations are expressed as:
Figure FDA0003487967740000078
the system observation equation under continuous time is as follows:
Figure FDA0003487967740000079
wherein ,q(vφ) The representation converts the euler angles into quaternions,
Figure FDA00034879677400000710
the strapdown inertial navigation coordinate system is converted into the laser radar coordinate system,
Figure FDA00034879677400000711
indicating that the position in the lidar coordinate system is converted into the world coordinate system,
Figure FDA00034879677400000712
the expression converts quaternion under the laser radar coordinate system into a world coordinate system,
Figure FDA00034879677400000713
indicating the conversion of the position in the strapdown inertial navigation coordinate system into the world coordinate system, vplRepresenting the velocity in the lidar coordinate system,
Figure FDA00034879677400000714
a quaternion representing the strapdown inertial navigation coordinate system is converted into a world coordinate system;
writing the observation equation under continuous time into a matrix form:
z＝Gtx+CtN
wherein ,GtExpressed as a positioning system state quantity coefficient matrix; ctExpressed as a positioning system observation noise term coefficient matrix; n is an input noise matrix, and the observed noise covariance is R; the observation equation and the discretization observation equation under continuous time are consistent;
obtaining a prediction state according to a prediction equation as follows:
Figure FDA0003487967740000081
wherein ,
Figure FDA0003487967740000082
a predicted state value, F, representing the current timet-1State transition matrix representing the system, Bt-1Representing system control matrix coefficients, M representing system state transition matrix, Dt-1Representing a constant matrix, the predicted state covariance is:
Figure FDA0003487967740000083
wherein ,
Figure FDA0003487967740000084
expressed as the predicted system state covariance, QkRepresenting the noise covariance moment, the kalman gain is as follows:
Figure FDA0003487967740000085
the system update state equation is expressed as follows:
Figure FDA0003487967740000086
the updated system covariance is expressed as follows:
Figure FDA0003487967740000087
at this point, one cycle of state estimation is completed.
7. The method for environmental perception and high-precision positioning of the coal mine underground mobile robot according to claim 5 is characterized in that the method for high-precision positioning of the mobile robot comprises the following specific steps:
S-C1: the remote control mobile robot walks a circle under the coal mine, so that a running path of the mobile robot forms a closed loop, the operation is carried out by adopting a coal mine underground laser point cloud preprocessing method, the quality of the point cloud is improved, the positioning precision is improved, and the processing result is input into an improved cartographer algorithm to carry out coal mine underground image construction and high-precision positioning of the mobile robot;
S-C2: the point cloud registration adopts a cerees base to calculate the pose information of the point cloud scanned by the coal mine underground environment laser radar in the stored coal mine environment subgraph, and specifically comprises the following steps:
Figure FDA0003487967740000091
Figure FDA0003487967740000092
representing the pose transformation from the point cloud coordinate system l to the corresponding sub-image coordinate system s, BsmoothRepresenting that a laser point cloud preprocessing method is adopted to improve the point cloud quality;
S-C3: the remote control mobile robot does uniform motion under a coal mine, and the estimation of the current-moment position is established on the basis of the observation position matched with the previous point cloud, and the method specifically comprises the following steps:
Figure FDA0003487967740000093
wherein ,T′t and R′tIs the predicted position and predicted attitude at time T, Tt-1 and Rt-1The posterior position and the posterior attitude at the time T-1 are respectively, and the delta T is the translation from the time T-1 to the time T; Δ R is the amount of rotation from time t-1 to time t; t is the scanning time of the laser radar;
attitude estimation value R 'at current time t'tAnd the direction of gravity G at the present momenttAs follows:
Figure FDA0003487967740000094
wherein ,Rt-1Is the attitude of the mobile robot at time t-1, Gt-1Is the gravity direction of the mobile robot at the time of t-1, and has an initial value of [ 001%]T,
Figure FDA0003487967740000095
Where k is a constant, Δ timu_tIs the time interval of two IMU data;
the gravity direction of the mobile robot corrects the pose of the robot, and specifically comprises the following steps:
Figure FDA0003487967740000096
Vgt*Gt＝G′t
Rt＝R′t*Vgt
wherein ,
Figure FDA0003487967740000097
is a quaternion expression form V of the estimated attitude of the mobile robot at the time tgtGravity direction G updated by linear velocity of mobile robottTo the direction of gravity G 'under the estimated pose'tThe amount of rotation of;
S-C4: a sparse pose adjustment method is adopted to eliminate accumulated errors of the sensor, a global map is modeled by a nonlinear least square method, and the speed of loop detection is accelerated by a branch-and-bound method; when the pose matching degree of the current point cloud is larger than a set threshold value, a loop is considered to be detected to form a constraint, and an optimal solution of the global pose is calculated by combining a nonlinear least square method;
the main idea of the branch-and-bound algorithm is to describe all possible poses as a tree, describing each node in the tree as:
N＝(cxcycθch)∈Z4
wherein ,cxcyLocation searched for node, cθFor the angle of the node search, chFor the height of a node, the algorithm is based on a given search height h0Branching a root node to a height h0And calculating the matching score of each child node in the C; and if the highest matching score in the child node set C is larger than the set score threshold value, detecting a loop, searching a leaf node with the largest upper bound in the child nodes with the highest score, and returning the pose of the leaf node, otherwise, not detecting the loop.
CN202210088155.XA 2022-01-25 2022-01-25 Coal mine underground mobile robot environment sensing and high-precision positioning method Active CN114485643B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210088155.XA CN114485643B (en) 2022-01-25 2022-01-25 Coal mine underground mobile robot environment sensing and high-precision positioning method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210088155.XA CN114485643B (en) 2022-01-25 2022-01-25 Coal mine underground mobile robot environment sensing and high-precision positioning method

Publications (2)

Publication Number Publication Date
CN114485643A true CN114485643A (en) 2022-05-13
CN114485643B CN114485643B (en) 2023-09-12

Family

ID=81474076

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210088155.XA Active CN114485643B (en) 2022-01-25 2022-01-25 Coal mine underground mobile robot environment sensing and high-precision positioning method

Country Status (1)

Country Link
CN (1) CN114485643B (en)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117433518A (en) * 2023-12-18 2024-01-23 三一重型装备有限公司 Positioning control system, method, electronic device, storage medium and chip
CN117823741A (en) * 2024-03-06 2024-04-05 福建巨联环境科技股份有限公司 Pipe network non-excavation repairing method and system combined with intelligent robot
CN117848332A (en) * 2024-03-07 2024-04-09 北京理工大学前沿技术研究院 IMU noise elimination method for vehicle-mounted multi-source fusion high-precision positioning system
CN117823741B (en) * 2024-03-06 2024-05-31 福建巨联环境科技股份有限公司 Pipe network non-excavation repairing method and system combined with intelligent robot

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018102264A1 (en) * 2016-11-29 2018-06-07 Hrl Laboratories, Llc Opportunistic sensor fusion algorithm for autonomous guidance while drilling
CN112033400A (en) * 2020-09-10 2020-12-04 西安科技大学 Intelligent positioning method and system for coal mine mobile robot based on combination of strapdown inertial navigation and vision
CN113236363A (en) * 2021-04-23 2021-08-10 陕西陕煤黄陵矿业有限公司 Mining equipment navigation positioning method, system, equipment and readable storage medium
CN113608236A (en) * 2021-08-03 2021-11-05 哈尔滨智兀科技有限公司 Mine robot positioning and image building method based on laser radar and binocular camera

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018102264A1 (en) * 2016-11-29 2018-06-07 Hrl Laboratories, Llc Opportunistic sensor fusion algorithm for autonomous guidance while drilling
CN112033400A (en) * 2020-09-10 2020-12-04 西安科技大学 Intelligent positioning method and system for coal mine mobile robot based on combination of strapdown inertial navigation and vision
CN113236363A (en) * 2021-04-23 2021-08-10 陕西陕煤黄陵矿业有限公司 Mining equipment navigation positioning method, system, equipment and readable storage medium
CN113608236A (en) * 2021-08-03 2021-11-05 哈尔滨智兀科技有限公司 Mine robot positioning and image building method based on laser radar and binocular camera

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
马宏伟;王岩;杨林;: "煤矿井下移动机器人深度视觉自主导航研究", 煤炭学报, no. 06 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117433518A (en) * 2023-12-18 2024-01-23 三一重型装备有限公司 Positioning control system, method, electronic device, storage medium and chip
CN117823741A (en) * 2024-03-06 2024-04-05 福建巨联环境科技股份有限公司 Pipe network non-excavation repairing method and system combined with intelligent robot
CN117823741B (en) * 2024-03-06 2024-05-31 福建巨联环境科技股份有限公司 Pipe network non-excavation repairing method and system combined with intelligent robot
CN117848332A (en) * 2024-03-07 2024-04-09 北京理工大学前沿技术研究院 IMU noise elimination method for vehicle-mounted multi-source fusion high-precision positioning system
CN117848332B (en) * 2024-03-07 2024-05-03 北京理工大学前沿技术研究院 IMU noise elimination method for vehicle-mounted multi-source fusion high-precision positioning system

Also Published As

Publication number Publication date
CN114485643B (en) 2023-09-12

Similar Documents

Publication Publication Date Title
CN111207774B (en) Method and system for laser-IMU external reference calibration
CN113781582B (en) Synchronous positioning and map creation method based on laser radar and inertial navigation combined calibration
CN114485643B (en) Coal mine underground mobile robot environment sensing and high-precision positioning method
CN110726406A (en) Improved nonlinear optimization monocular inertial navigation SLAM method
CN110595503B (en) Self-alignment method of SINS strapdown inertial navigation system shaking base based on lie group optimal estimation
CN114018248B (en) Mileage metering method and image building method integrating code wheel and laser radar
CN114526745A (en) Drawing establishing method and system for tightly-coupled laser radar and inertial odometer
CN110929402A (en) Probabilistic terrain estimation method based on uncertain analysis
CN114019552A (en) Bayesian multi-sensor error constraint-based location reliability optimization method
CN113741503B (en) Autonomous positioning unmanned aerial vehicle and indoor path autonomous planning method thereof
CN115479598A (en) Positioning and mapping method based on multi-sensor fusion and tight coupling system
CN116642482A (en) Positioning method, equipment and medium based on solid-state laser radar and inertial navigation
CN113639722B (en) Continuous laser scanning registration auxiliary inertial positioning and attitude determination method
CN112985392B (en) Pedestrian inertial navigation method and device based on graph optimization framework
Wu et al. AFLI-Calib: Robust LiDAR-IMU extrinsic self-calibration based on adaptive frame length LiDAR odometry
Bai et al. Graph-optimisation-based self-calibration method for IMU/odometer using preintegration theory
Hussnain et al. Enhanced trajectory estimation of mobile laser scanners using aerial images
CN115562076B (en) Simulation system, method and storage medium for unmanned mine car
CN116625394A (en) Robot environment sensing and path optimizing system under unknown road condition
CN115797490A (en) Drawing construction method and system based on laser vision fusion
CN112489176B (en) Tightly-coupled graph building method fusing ESKF, g2o and point cloud matching
CN115930971B (en) Data fusion processing method for robot positioning and map building
CN115574816B (en) Bionic vision multi-source information intelligent perception unmanned platform
Liu et al. Error modelling and optimal estimation of laser scanning aided inertial navigation system in GNSS-denied environments
Sun et al. A Novel Localization Method for Indoor Mobile Robot Based on Adaptive Weighted Fusion

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