A laser PSD pose recognition method for a geological model pipeline robot

Abstract

The application discloses a kind of laser PSD pose recognition methods of geological model pipeline robot.The method comprises: installing fixed support at the entrance of pipeline tunneling channel and placing pipeline robot, establishing the global reference coordinate system of fixed support and the initial tail coordinate system of the tail of pipeline robot, and then obtaining initial homogeneous transformation matrix;Install laser ranging unit to synchronously collect moving distance and light point offset when pipeline robot carries out pipeline tunneling;Establish pose recognition model based on pose solution constraint, output the optimal homogeneous transformation matrix of the actual pose of pipeline robot after fitting, and finally identify the pose deviation of pipeline robot to carry out real-time adjustment.The method of the present application can accurately fit and solve the pose of the robot in the small pipeline of the geological model, fully utilize the advantages of high resolution, high stability and fast response speed of PSD position sensor, and provide a new solution for dynamic pose recognition of small pipeline robot.

Description

Technical Field

[0001] This invention relates to a method for recognizing the pose of a pipeline robot, and specifically to a laser PSD pose recognition method for a geological model pipeline robot. Background Technology

[0002] In the field of pipeline robots, recognizing and detecting the robot's pose has become an indispensable part. Many subsequent tasks of pipeline robots, such as decision-making for control modules to issue travel or correction commands, pipeline inspection, and 3D pipeline reconstruction, all require the robot's pose data. Currently, there are various methods for pipeline robot localization and pose recognition. For example, the method described in CN120063253A, "A Pipeline Robot Localization Method Based on Adaptive Extended Kalman Filter," uses Kalman filtering to iteratively predict and correct the pose state of the pipeline robot to obtain its pose data. Another method, CN119779266A, "A Beidou, Inertial Navigation, and Vision Collaboration Method for Underground Pipeline Robot Localization," utilizes a collaborative approach involving an inertial measurement unit, a visual sensor, and a Beidou positioning sensor to achieve precise positioning of the pipeline robot.

[0003] Although much research has been conducted on localization and pose recognition methods for pipeline robots, relatively little research has been done on their application in the localization and pose recognition of robots within geological model pipelines. Geological models are spatial representations of underground strata, structures, lithology, and physical properties, abstracted, simplified, and quantified based on geological data and related theories. These models are used to reconstruct underground geological conditions and support geological research, resource exploration, and engineering design. Current pose detection methods face challenges in the specific environment of geological models: 1. Vision- or laser-based SLAM technology suffers from decreased pose accuracy due to the lack of obvious features in the excavated pipeline; 2. Acoustic-based pose detection technology faces measurement difficulties because sound waves cannot penetrate the geological model itself; 3. While laser target-based detection technology can locate the robot's position within the pipeline, it has limitations in detecting the robot's posture accuracy. Summary of the Invention

[0004] To address the problems existing in the background art, this invention provides a laser PSD pose recognition method for a geological model pipeline robot. This invention addresses the shortcomings of pose recognition in geological model pipeline robots and overcomes the limitations of the background art in geological model pipelines.

[0005] The technical solution adopted in this invention is: The laser PSD pose recognition method for a geological model pipeline robot of the present invention includes: Step 1: Install a fixed support at the entrance of the pipeline tunnel and place the pipeline robot. Establish the global reference coordinate system of the fixed support and the initial tail coordinate system of the pipeline robot, and then obtain the initial homogeneous transformation matrix between the global reference coordinate system and the initial tail coordinate system.

[0006] Step 2: Install several laser ranging units on the fixed support and the tail of the pipeline robot to simultaneously collect the moving distance and spot offset of the pipeline robot when it is tunneling the pipeline.

[0007] Step 3: Establish a pose recognition model based on pose calculation constraints. Input the travel distance of the pipeline robot and the offset of the light spot into the pose recognition model. After fitting, output the optimal homogeneous transformation matrix of the actual pose of the pipeline robot. Finally, identify the pose deviation of the pipeline robot for real-time adjustment.

[0008] In step one, the global reference coordinate system O1 of the fixed support takes the center position of the fixed support as its origin. The Z1 axis of the global reference coordinate system faces into the pipeline tunnel and is parallel to the preset tunneling direction. The X1 axis is vertically upward, and the Y1 axis is a horizontal direction perpendicular to the preset tunneling direction, which can be to the left or right. When the pipeline robot is initially positioned at the entrance of the pipeline tunnel, its central axis is parallel to the preset tunneling direction. The initial tail coordinate system O2 of the pipeline robot takes the center position of the tail position as its origin. The Z2, X2, and Y2 axes of the initial tail coordinate system O2 are in the same direction as the Z1, X1, and Y1 axes of the global reference coordinate system, respectively, thus obtaining the initial homogeneous transformation matrix. It is determined through pre-calibration.

[0009] In step two, the pipeline robot moves forward in a straight line along the planned channel axis in the preset tunneling direction. The origin of the initial tail coordinate system O2 of the pipeline robot is located on the straight line in the preset tunneling direction. When the pipeline robot is tunneling, the travel distance of the pipeline robot is obtained in real time through each laser ranging unit, and an ideal tail coordinate system O3 is established. The origin of the ideal tail coordinate system O3 is located on the planned channel axis. It is determined by moving the origin of the initial tail coordinate system O2 along the planned channel axis to determine the current travel distance. The Z3, X3, and Y3 axes of the ideal tail coordinate system O3 are in the same direction as the Z2, X2, and Y2 axes of the initial tail coordinate system O2, respectively. That is, the Z2 and Z3 axes are on the same straight line, thereby obtaining the ideal homogeneous transformation matrix between the ideal tail coordinate system O3 and the initial tail coordinate system O2. An ideal homogeneous transformation matrix contains only translations along the Z-axis.

[0010] In step two, the laser ranging unit includes a laser ranging sensor and a two-dimensional PSD (Position Sensitive Detector) position sensor. The laser ranging sensors of each laser ranging unit are evenly spaced on the fixed support facing the entrance of the pipeline tunnel. The two-dimensional PSD position sensors of each laser ranging unit are evenly spaced at the tail of the pipeline robot. Each laser ranging sensor and its respective two-dimensional PSD position sensor are arranged facing each other. The laser emission direction of each laser ranging sensor is parallel to the preset tunneling direction of the pipeline robot. When the pipeline robot is in the initial position, the laser line emitted by each laser ranging sensor is perpendicularly incident on the center position of the photosensitive surface of its respective two-dimensional PSD position sensor.

[0011] In step two, when the pipeline robot is excavating the pipeline, the moving distance of the pipeline robot includes the moving distance of each group of laser ranging units, which is obtained by collecting the moving distance between the laser ranging sensor and the two-dimensional PSD position sensor; the light spot offset of the pipeline robot includes the light spot offset of each group of laser ranging units, which is obtained by collecting the offset of the incident light spot on the photosensitive surface of the two-dimensional PSD position sensor relative to the center of the photosensitive surface of the laser line emitted by the laser ranging sensor of each group of laser ranging units.

[0012] In step three, the pose recognition model is as follows: in, K The number of laser ranging units; It is a rotation matrix; When the pipeline robot does not deviate, the first k The direction vector of the photosensitive plane normal vector of a two-dimensional PSD position sensor in the initial tail coordinate system O2 of the pipeline robot; For the first k The three-dimensional coordinates of the laser point of a laser rangefinder in the global reference coordinate system O1. =[ , and The first k The x and y coordinates of the laser point emitted by a laser rangefinder in the global reference coordinate system O1. For the first k The distance traveled is collected by a laser rangefinder sensor. This is the matrix transpose. For the first k The coordinates of the center of the photosensitive surface of a two-dimensional PSD position sensor in the global reference coordinate system O1 under the current actual pose state.

[0013] In step three, the pose calculation constraints are as follows: in, This is the initial homogeneous transformation matrix; It is an ideal homogeneous transformation matrix; Let O be the yaw homogeneous transformation matrix between the ideal tail coordinate system O3 and the actual tail coordinate system O4 at the current position of the pipeline robot. For the first k The coordinates of the center of the photosensitive surface of a two-dimensional PSD position sensor in the actual tail coordinate system O4; For the first k The homogeneous transformation matrix of the local coordinate system of a two-dimensional PSD position sensor relative to the initial tail coordinate system O2 of the pipeline robot; and The first k The offset of the incident light spot on the photosensitive surface of the two-dimensional PSD position sensor in the x and y directions relative to the center of the photosensitive surface, as emitted by a laser rangefinder.

[0014] The optimal yaw homogeneous transformation matrix is ​​obtained by fitting the pose recognition model using an optimization algorithm. As the optimal homogeneous transformation matrix, based on the optimal homogeneous transformation matrix and the ideal homogeneous transformation matrix. The pose deviation of the pipeline robot at its current position is identified, thereby adjusting the Z4 axis of the actual tail coordinate system O4 to be consistent with the Z3 axis of the ideal tail coordinate system O3.

[0015] The beneficial effects of this invention are: 1. The present invention adopts a split layout in which the laser emitting unit is installed on the fixed support at the inlet end and the two-dimensional PSD position sensor is installed at the tail of the robot. It eliminates the need to integrate large detection equipment in narrow pipes. The layout of the three two-dimensional PSD position sensors occupies very little space and can be adapted to ultra-small tunneling pipes with geological models. It solves the core pain point of existing solutions that are limited by installation space and difficult to adapt to small pipes.

[0016] 2. The laser detection scheme based on a fixed reference at the pipeline inlet of this invention does not suffer from the feature loss and accuracy reduction problem of SLAM technology, nor does it suffer from the zero drift and cumulative error problems of inertial navigation and odometer schemes. It can maintain stable detection accuracy even after long-distance tunneling. At the same time, by constructing a dedicated mathematical model for pose calculation and fitting the optimal actual coordinate system with a mathematical optimization algorithm, it can accurately calculate the robot's full-dimensional pose data, giving full play to the advantages of high resolution and high stability of the two-dimensional PSD position sensor.

[0017] 3. The pose calculation process of this invention has a clear logic. Through mathematical models, it can quickly complete the calculation and output of pose data with high computational efficiency. It can adapt to the high-frequency detection requirements of real-time robot control. At the same time, the solution is quick to install and calibrate. It can be widely used in geological model experiments, automatic tunneling of small pipelines, underground pipeline inspection and other fields, and has great promotional value. Attached Figure Description

[0018] Figure 1 This is a flowchart of the method of the present invention; Figure 2 This is a design drawing of the pose recognition platform for a pipeline robot; Figure 3 This is a schematic diagram of the coordinate transformation relationship of the pipeline robot; Figure 4 This is a simulation result diagram of the method of the present invention. Detailed Implementation

[0019] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. Obviously, the specific embodiments described herein are only for explaining the present invention and are not intended to limit the present invention. Any simple modifications made to the following embodiments based on the technical essence of the present invention shall still fall within the protection scope of the present invention. The technical features involved in the various embodiments of the present invention can be combined with each other as long as they do not conflict with each other.

[0020] like Figure 1 As shown, the laser PSD pose recognition method for the geological model pipeline robot of the present invention is as follows: like Figure 2 As shown, a fixed support is installed at the entrance of the pipeline tunnel and a pipeline robot is placed there. A global reference coordinate system for the fixed support and an initial tail coordinate system for the pipeline robot are established. Then, the initial homogeneous transformation matrix between the global reference coordinate system and the initial tail coordinate system is obtained. Specifically, as shown... Figure 3 As shown, the global reference coordinate system O1 of the fixed support takes the center position of the fixed support as its origin. The Z1 axis of the global reference coordinate system faces into the pipeline tunnel and is parallel to the preset tunneling direction. The X1 axis is vertically upward, and the Y1 axis is a horizontal direction perpendicular to the preset tunneling direction, which can be to the left or right. When the pipeline robot is initially positioned at the entrance of the pipeline tunnel, its central axis is parallel to the preset tunneling direction. The initial tail coordinate system O2 of the pipeline robot takes the center position of the tail position as its origin. The Z2, X2, and Y2 axes of the initial tail coordinate system O2 are in the same direction as the Z1, X1, and Y1 axes of the global reference coordinate system, respectively, thus obtaining the initial homogeneous transformation matrix. It is determined through pre-calibration.

[0021] Several laser ranging units are installed on the fixed support and at the tail of the pipeline robot to synchronously collect the robot's travel distance and laser spot offset during pipeline excavation. Specifically, the pipeline robot moves in a straight line along the planned channel axis in the preset excavation direction. The origin of the initial tail coordinate system O2 of the pipeline robot is located on the straight line of the preset excavation direction. When the pipeline robot is excavating, the travel distance of the pipeline robot is obtained in real time through each laser ranging unit, and an ideal tail coordinate system O3 is established. The origin of the ideal tail coordinate system O3 is located on the planned channel axis, determined by moving the origin of the initial tail coordinate system O2 along the planned channel axis. The Z3, X3, and Y3 axes of the ideal tail coordinate system O3 are in the same direction as the Z2, X2, and Y2 axes of the initial tail coordinate system O2, respectively, i.e., the Z2 and Z3 axes are on the same straight line. Thus, the ideal homogeneous transformation matrix between the ideal tail coordinate system O3 and the initial tail coordinate system O2 is obtained. An ideal homogeneous transformation matrix contains only translations along the Z-axis.

[0022] The laser ranging unit includes a laser ranging sensor and a two-dimensional PSD (Position Sensitive Detector) sensor. The laser ranging sensors of each laser ranging unit are evenly spaced on a fixed support facing the entrance of the pipeline tunnel. The two-dimensional PSD sensors of each laser ranging unit are evenly spaced at the tail of the pipeline robot. Each laser ranging sensor and its corresponding two-dimensional PSD sensor are arranged facing each other, and the laser emission direction of each laser ranging sensor is parallel to the preset tunneling direction of the pipeline robot. When the pipeline robot is in its initial position, the laser line emitted by each laser ranging sensor is perpendicularly incident on the center of the photosensitive surface of its corresponding two-dimensional PSD sensor. Specifically, three laser ranging units can be set up, with the three two-dimensional PSD sensors distributed in an inverted equilateral triangle at the tail of the pipeline robot: two horizontally arranged at the upper right and upper left of the tail center, and one directly below the tail center. Initially, the installation parameters of the laser ranging sensor and the two-dimensional PSD sensor need to be calibrated.

[0023] In practical implementation, the fixed support of this invention is fixedly installed at the entrance of the pipeline tunneling channel of the geological model, serving as the global reference carrier for the entire pose recognition system. The mounting surface of the fixed support is perpendicular to the axis of the planned pipeline travel channel. After installation, horizontal and vertical calibration is required to ensure that the reference surface has no tilt deviation. The laser ranging unit uses three laser ranging sensors that integrate laser emission and laser ranging functions, which are fixedly installed on the mounting surface of the fixed support. The installation positions of the three laser ranging sensors correspond one-to-one with the two-dimensional PSD position sensor at the tail of the robot. After installation, the emission direction of the three laser ranging sensors needs to be calibrated to ensure that the laser emission direction of all laser ranging sensors is parallel to the axis of the planned travel route of the pipeline robot, and that when the pipeline robot is in the initial zero position, the three laser beams can be perpendicularly incident on the center position of the photosensitive surface of the corresponding two-dimensional PSD position sensor. Three 2D PSD position sensors are fixedly installed on the tail end face of the pipeline robot. The three 2D PSD position sensors are distributed in an inverted equilateral triangle at the tail of the robot. Specifically, two 2D PSD position sensors are horizontally arranged at the upper right and upper left of the center of the tail of the pipeline robot, and one 2D PSD position sensor is arranged directly below the center of the tail of the pipeline robot. During installation, it is necessary to ensure that the photosensitive surface of all 2D PSD position sensors is perpendicular to the planned travel direction of the pipeline robot and perpendicular to the laser incident direction of the laser rangefinder, so as to ensure that the laser spot and the geometric center of the photosensitive surface are completely coincident in the initial state.

[0024] When the pipeline robot is excavating a pipeline, the robot's movement distance includes the movement distance of each set of laser ranging units, which is obtained by collecting the movement distance between the laser ranging sensor and the 2D PSD position sensor. The laser spot offset of the pipeline robot includes the laser spot offset of each set of laser ranging units, which is obtained by collecting the offset of the incident laser spot on the photosensitive surface of the 2D PSD position sensor relative to the center of the photosensitive surface, where the laser line emitted by the laser ranging sensor of each set of laser ranging units is located. During movement, the laser line emitted by the laser ranging sensor will always be incident on the photosensitive surface of the 2D PSD position sensor, with only the position shifting. If it is not incident, the position of the pipeline robot needs to be adjusted so that the laser line is incident on the photosensitive surface before pose recognition is performed.

[0025] In practice, a global reference coordinate system is first established at the center of the upper surface of the fixed support at the pipe inlet. ,definition The positive axis is the planned travel direction of the pipeline robot, which coincides with the pipeline design axis. The positive direction of the axis is vertically upward. The positive axis is horizontal to the right, conforming to the right-hand rule. This coordinate system serves as the fixed reference coordinate system for the entire system and remains unchanged throughout the entire process. Subsequently, an initial tail coordinate system is established at the center of the tail end face at the initial zero position of the pipeline robot. This coordinate system is rigidly connected to the robot body and moves synchronously with the robot. When the robot is in the initial zero position, the initial tail coordinate system... The axes of each coordinate system and the global reference coordinate system The corresponding axes are parallel, and the initial homogeneous transformation matrix between the two coordinate systems is determined. The matrix parameters are determined by the system installation location and calibration results, and are stored in the system as fixed parameters after calibration.

[0026] After completing the basic coordinate system construction, the sensor installation parameters were calibrated. First, the three laser rangefinders were calibrated in the global reference coordinate system. The installation coordinates below determine the location of the laser point emitted by each laser rangefinder. Fixed X and Y coordinates in a coordinate system Where k=1,2,3, corresponding to 3 laser rangefinders, after calibration, these parameters are stored in the system as fixed parameters. Then, the 3 two-dimensional PSD position sensors are calibrated in the initial tail coordinate system. The installation location is determined, and the center of each 2D PSD position sensor is located at... Coordinates in the coordinate system, and the local coordinate system of each PSD relative to the initial tail coordinate system. homogeneous transformation matrix Simultaneously calibrating the robot when there is no deviation. k The plane normal vector of a two-dimensional PSD position sensor in the initial tail coordinate system Downward direction After the above parameters are calibrated, they are all stored in the system as fixed parameters. After calibration, the tail coordinate system corresponding to the robot's current ideal pose is defined as follows: This coordinate system is the theoretical coordinate system after the robot travels along the planned axis without any offset, and it is relative to the initial tail coordinate system. The homogeneous transformation matrix is This matrix only includes translation along the Z-axis, with the translation distance being the average of the data collected by the three laser rangefinders. The average value is only reflected in... In the meantime, the tail coordinate system corresponding to the robot's actual pose is defined as follows: The core solution objective of this invention is the actual tail coordinate system of the robot. Tail coordinate system relative to ideal pose homogeneous transformation matrix .

[0027] As the robot moves along the planned path, three laser rangefinders on the fixed support simultaneously collect real-time distance data. The three sets of ranging data are subjected to mean filtering, and the average value is taken as the actual travel distance of the robot. Simultaneously, the three travel distances are stored in the system for subsequent pose calculation. At the same time, three laser rangefinders continuously emit laser beams, which are incident on the photosensitive surface of the corresponding two-dimensional PSD position sensor at the robot's tail. The two-dimensional PSD position sensor collects the photocurrent signals from the four electrodes in real time, and through built-in calculation circuits and position calculation formulas, outputs the two-dimensional offset of the laser incident point relative to the center of the photosensitive surface. In this embodiment, the formula for calculating the position of the light spot of the two-dimensional PSD position sensor is as follows: Let the width of the photosensitive area of ​​the two-dimensional PSD be M, and the height be H. The four electrodes are located at the four corners of the photosensitive area. Let the current on the upper left, upper right, lower left, and lower right electrodes be... , , and The position of the light spot on the x-axis is: The position of the light spot on the y-axis is: Using the center of the photosensitive surface as the zero point, the offset of the light spot relative to the center is calculated. The system completes the acquisition and output of offset data. It timestamps and synchronizes the laser ranging data and PSD offset data, filters and denoises the acquired raw data, removes outliers, and then inputs the valid data into the pose calculation mathematical model.

[0028] When calculating the pose, first define the unknown parameter θ = [ , , , , , ],in Let be the rotation vector. Let be the quantities of the rotation vector in the x, y, and z directions, respectively. This can be converted into a 3×3 rotation matrix R, where t = It is a translation vector. Let x, y, and z be the quantities of the translation vector in the x, y, and z directions, respectively. The homogeneous transformation matrix to be determined is: = The pose recognition model based on pose calculation constraints is established simultaneously as follows: in, KThe number of laser ranging units, in specific implementation K =3; It is a rotation matrix; When the pipeline robot does not deviate, the first k The direction vector of the photosensitive plane normal vector of a two-dimensional PSD position sensor in the initial tail coordinate system O2 of the pipeline robot; For the first k The three-dimensional coordinates of the laser point of a laser rangefinder in the global reference coordinate system O1. =[ , and The first k The x and y coordinates of the laser point emitted by a laser rangefinder in the global reference coordinate system O1. For the first k The distance traveled is collected by a laser rangefinder sensor. This is the matrix transpose. For the first k The coordinates of the center of the photosensitive surface of a two-dimensional PSD position sensor in the global reference coordinate system O1 under the current actual pose state.

[0029] The specific constraints for pose calculation are as follows: in, The initial homogeneous transformation matrix is ​​determined by the installation location; It is an ideal homogeneous transformation matrix, determined by the data from the laser rangefinder sensor; The yaw homogeneous transformation matrix between the ideal tail coordinate system O3 and the actual tail coordinate system O4 at the current position of the pipeline robot is determined by the installation position of the two-dimensional PSD position sensor. For the first k The coordinates of the center of the photosensitive surface of a two-dimensional PSD position sensor in the actual tail coordinate system O4; For the first k The homogeneous transformation matrix of the local coordinate system of each two-dimensional PSD position sensor relative to the initial tail coordinate system O2 of the pipeline robot. Specifically, the local coordinate system of the two-dimensional PSD position sensor is based on the center of the photosensitive surface of each two-dimensional PSD position sensor as the origin, and the three-axis direction is consistent with the three-axis direction of the global reference coordinate system O1. and The first k The offset of the incident light spot on the photosensitive surface of the two-dimensional PSD position sensor in the x and y directions relative to the center of the photosensitive surface, as emitted by a laser rangefinder.

[0030] The travel distance and spot offset of the pipeline robot are input into the pose recognition model, and the optimal yaw homogeneous transformation matrix is ​​obtained by fitting the pose recognition model through an optimization algorithm. As the optimal homogeneous transformation matrix, based on the optimal homogeneous transformation matrix and the ideal homogeneous transformation matrix. The pose deviation of the pipeline robot at its current position is identified, thereby adjusting the Z4 axis of the actual tail coordinate system O4 to be consistent with the Z3 axis of the ideal tail coordinate system O3.

[0031] In practice, a nonlinear optimization algorithm is used to solve the above model. The robot's zero-position state with no offset is used as the initial iteration value. Real-time data is substituted into the objective function and constraints, and the optimal solution corresponding to the minimum residual is obtained through iterative optimization fitting. This is the optimal homogeneous transformation matrix corresponding to the robot's actual pose. The robot's rotational and translational deviations relative to its ideal pose are obtained, which are the robot's real-time pose data and pose deviation data. The robot's real-time travel distance is output synchronously, thus completing the entire pose recognition process.

[0032] The method of this invention achieves real-time, high-precision pose recognition of pipeline robots in small pipelines of geological models through three core steps: system installation and calibration and coordinate system construction, synchronous data acquisition of sensors, and pose calculation model construction and fitting solution. This provides core data support for robot movement control and automatic correction.

[0033] like Figure 4 As shown, to verify the effectiveness of the method of the present invention, simulation verification was carried out simultaneously in this embodiment, obtaining real pose data and pose data calculated by the method of the present invention. The data output results include... The rotation matrix and translation vector are used in the simulation. A geological model pipeline robot simulation environment is built during the simulation process to simulate multi-dimensional pose deviations such as horizontal offset, vertical offset, yaw, pitch, and roll generated during the robot's movement. Sensor noise is also added to simulate actual working conditions. Then, following the method flow of this invention, the entire process of system calibration, data acquisition, and pose calculation is simulated, synchronously recording the robot's actual pose data and the pose data calculated by the method of this invention. Simulation results show that the pose data calculated by the method of this invention highly overlaps with the robot's actual pose data. The translational deviation calculation accuracy is better than 0.75mm, and the angular deviation calculation accuracy is better than 0.15°, with no cumulative error. It can maintain stable high-precision detection results even during long-distance tunneling, fully verifying the effectiveness and reliability of the method of this invention.

[0034] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made without departing from the scope and spirit of the described embodiments should be included within the protection scope of the present invention.

Claims

1. A laser PSD pose recognition method for a geological model pipeline robot, characterized in that, include: Step 1: Install a fixed support at the entrance of the pipeline tunnel and place the pipeline robot. Establish the global reference coordinate system of the fixed support and the initial tail coordinate system of the pipeline robot, and then obtain the initial homogeneous transformation matrix between the global reference coordinate system and the initial tail coordinate system. Step 2: Install several laser ranging units on the fixed support and the tail of the pipeline robot to synchronously collect the moving distance and spot offset of the pipeline robot when it is tunneling the pipeline. Step 3: Establish a pose recognition model based on pose calculation constraints. Input the travel distance of the pipeline robot and the offset of the light spot into the pose recognition model. After fitting, output the optimal homogeneous transformation matrix of the actual pose of the pipeline robot. Finally, identify the pose deviation of the pipeline robot for real-time adjustment.

2. The laser PSD pose recognition method for a geological model pipeline robot according to claim 1, characterized in that: In step one, the global reference coordinate system O1 of the fixed support takes the center position of the fixed support as its origin. The Z1 axis of the global reference coordinate system faces into the pipeline tunnel and is parallel to the preset tunneling direction. The X1 axis is vertically upward, and the Y1 axis is a horizontal direction perpendicular to the preset tunneling direction. When the pipeline robot is initially positioned at the entrance of the pipeline tunnel, its central axis is parallel to the preset tunneling direction. The initial tail coordinate system O2 of the pipeline robot takes the center position of the tail position as its origin. The Z2, X2, and Y2 axes of the initial tail coordinate system O2 are in the same direction as the Z1, X1, and Y1 axes of the global reference coordinate system, respectively, thus obtaining the initial homogeneous transformation matrix. .

3. The laser PSD pose recognition method for a geological model pipeline robot according to claim 1, characterized in that: In step two, the pipeline robot moves forward in a straight line along the planned channel axis in the preset tunneling direction. The origin of the initial tail coordinate system O2 of the pipeline robot is located on the straight line in the preset tunneling direction. When the pipeline robot is tunneling, the travel distance of the pipeline robot is obtained in real time through each laser ranging unit, and an ideal tail coordinate system O3 is established. The origin of the ideal tail coordinate system O3 is located on the planned channel axis. It is determined by moving the origin of the initial tail coordinate system O2 along the planned channel axis to determine the current travel distance. The Z3, X3, and Y3 axes of the ideal tail coordinate system O3 are in the same direction as the Z2, X2, and Y2 axes of the initial tail coordinate system O2, respectively. Thus, the ideal homogeneous transformation matrix between the ideal tail coordinate system O3 and the initial tail coordinate system O2 is obtained. .

4. The laser PSD pose recognition method for a geological model pipeline robot according to claim 1, characterized in that: In step two, the laser ranging unit includes a laser ranging sensor and a two-dimensional PSD position sensor. The laser ranging sensors of each laser ranging unit are installed at intervals on the side of the fixed support facing the entrance of the pipeline tunnel. The two-dimensional PSD position sensors of each laser ranging unit are installed at intervals at the tail of the pipeline robot. Each laser ranging sensor and its respective two-dimensional PSD position sensor are arranged facing each other. The laser emission direction of each laser ranging sensor is parallel to the preset tunneling direction of the pipeline robot. When the pipeline robot is in the initial position, the laser line emitted by each laser ranging sensor is perpendicularly incident on the center position of the photosensitive surface of its respective two-dimensional PSD position sensor.

5. The laser PSD pose recognition method for a geological model pipeline robot according to claim 4, characterized in that: In step two, when the pipeline robot is excavating the pipeline, the moving distance of the pipeline robot includes the moving distance of each group of laser ranging units, which is obtained by collecting the moving distance between the laser ranging sensor and the two-dimensional PSD position sensor; the light spot offset of the pipeline robot includes the light spot offset of each group of laser ranging units, which is obtained by collecting the offset of the incident light spot on the photosensitive surface of the two-dimensional PSD position sensor relative to the center of the photosensitive surface of the laser line emitted by the laser ranging sensor of each group of laser ranging units.

6. The laser PSD pose recognition method for a geological model pipeline robot according to claim 5, characterized in that: In step three, the pose recognition model is as follows: in, K The number of laser ranging units; It is a rotation matrix; When the pipeline robot does not deviate, the first k The direction vector of the photosensitive plane normal vector of a two-dimensional PSD position sensor in the initial tail coordinate system O2 of the pipeline robot; For the first k The three-dimensional coordinates of the laser point of a laser rangefinder in the global reference coordinate system O1. =[ , and The first k The x and y coordinates of the laser point emitted by a laser rangefinder in the global reference coordinate system O1. For the first k The distance traveled is collected by a laser rangefinder sensor. This is the matrix transpose. For the first k The coordinates of the center of the photosensitive surface of a two-dimensional PSD position sensor in the global reference coordinate system O1 under the current actual pose state.

7. The laser PSD pose recognition method for a geological model pipeline robot according to claim 6, characterized in that: In step three, the pose calculation constraints are as follows: in, This is the initial homogeneous transformation matrix; It is an ideal homogeneous transformation matrix; Let O be the yaw homogeneous transformation matrix between the ideal tail coordinate system O3 and the actual tail coordinate system O4 at the current position of the pipeline robot. For the first k The coordinates of the center of the photosensitive surface of a two-dimensional PSD position sensor in the actual tail coordinate system O4; For the first k The homogeneous transformation matrix of the local coordinate system of a two-dimensional PSD position sensor relative to the initial tail coordinate system O2 of the pipeline robot; and The first k The offset of the incident light spot on the photosensitive surface of the two-dimensional PSD position sensor in the x and y directions relative to the center of the photosensitive surface; The optimal yaw homogeneous transformation matrix is ​​obtained by fitting the pose recognition model using an optimization algorithm. As the optimal homogeneous transformation matrix, based on the optimal homogeneous transformation matrix and the ideal homogeneous transformation matrix. The pose deviation of the pipeline robot at its current position is identified, thereby adjusting the Z4 axis of the actual tail coordinate system O4 to be consistent with the Z3 axis of the ideal tail coordinate system O3.

Images