Multi-sensor fusion guiding robot positioning system

Abstract

The invention provides a multi-sensor fusion guiding robot positioning system which comprises a positioning module which integrates a PTZ camera, a total station, a robot and a central control module. The central module comprises a channel operation point obtaining module, the position of a channel operation point is calculated through the channel position obtained through cooperation of a PTZ camera and a total station, and accurate positioning of the channel is achieved. The guiding module is used for positioning the position of each feature point at the tail end of the robot through a PTZ camera; measuring three-dimensional coordinates of each feature point through a total station, and establishing a conversion relation between a robot coordinate system and a total station coordinate system; and converting the coordinate data of the channel operation point into robot coordinate data, and enabling the robot to move to the corresponding operation point for operation. The whole process is highly automatic, the integration level is high, and manual intervention and measurement errors are remarkably reduced.

Description

TECHNICAL FIELD

[0001] The present application relates to robot channel positioning technology, and in particular to a multi-sensor fusion guided robot positioning system. BACKGROUND

[0002] In the field of industrial automation, construction and precision manufacturing, the precision and efficiency of robot operation are crucial. Traditional robot operation usually relies on pre-programmed fixed paths or manual teaching to determine the operation point position. However, this method has obvious limitations in the face of complex, dynamic or high-precision tasks: human intervention is required: manual measurement and teaching are required, which is inefficient and prone to human error; poor adaptability: re-calibration is required when the environment changes or the task is adjusted, and the flexibility is insufficient; precision is limited: traditional measurement methods (such as tape measure, laser range finder) are difficult to meet the demand of millimeter level or even higher precision.

[0003] To overcome the above problems, total station assisted robot positioning has been introduced in the prior art. The total station can obtain the three-dimensional coordinates of the calibration target center point through angle and distance measurement, but the process of its cooperation with the robot still has the following shortcomings:

[0004] Inadequate data fusion: the coordinate data collected by the total station needs to be processed through complex post-processing before it can be used by the robot system, and the coordination is poor;

[0005] Coordinate system conversion error: the calibration process of the robot base coordinate system and the total station coordinate system is complicated, and the calibration error will be accumulated in the operation point positioning;

[0006] Low degree of automation: there are multiple manual interventions in the chain from measurement to execution, and the controllability is poor. SUMMARY

[0007] The present application provides a multi-sensor fusion guided robot positioning system to overcome the shortcomings of the prior art.

[0008] To achieve the above purpose, the multi-sensor fusion guided robot positioning system of the present application comprises: a central control module and a robot; the multi-sensor comprises: a PTZ camera and a total station; the central control module comprises:

[0009] A channel operation point acquisition module acquires the position of the channel through the PTZ camera and the total station to calculate the position of the channel operation point;

[0010] A guide module positions each feature point at the end of the robot through the PTZ camera; measures the three-dimensional coordinates of each feature point through the total station to establish the conversion relationship between the robot coordinate system and the total station coordinate system; converts the channel operation point coordinate data into robot coordinate data, and moves the robot to the corresponding operation point for operation.

[0011] Preferably, the trench operation point acquisition module comprises:

[0012] a positioning module, configured to acquire a region where the trench is located by the PTZ camera, to coarsely position the trench, and to synchronize the position of the total station to the direction of the trench, to finely position the trench by the total station based on the image position coordinates of the PTZ camera;

[0013] a data analysis module, configured to scan edge points of the trench, to acquire three-dimensional data of the edge points, to fit a profile of the trench, to obtain a central axis of the trench, to calculate geometric parameters of the trench, and to calculate and generate coordinate data of required operation points.

[0014] Preferably, before acquiring the region where the target trench is located, the positioning module is configured to calibrate the zoom of the PTZ camera, and the steps comprise:

[0015] placing a standard calibration target with an actual physical size S in front of the PTZ camera, measuring a distance D from the front end of the lens of the PTZ camera to the calibration target by a laser range finder, c adjusting the optical zoom value Z of the PTZ camera to an initial position Z1, taking a clear picture of the standard calibration target, measuring the diameter of the calibration target in the image by least-enclosing-circle fitting, and obtaining a pixel size Ps1;

[0016] adjusting the zoom value to a new position Z2, and measuring a new pixel size Ps2 again;

[0017] repeating the above process to change the value of Z, collecting data pairs (Z i , Ps i ), and calculating an effective focal length f i corresponding to each data point, i.e.: wherein subscript c is a coordinate system related to the PTZ camera;

[0018] performing quadratic polynomial fitting on the data points (Z i , f i ) to obtain coefficients a, b, and c, and obtaining an effective focal length formula f(Z) = a·Z 2 +b·Z+c under the current optical zoom value Z; wherein a is a quadratic term coefficient of the nonlinear variation rate of the effective focal length with the zoom position, b is a linear term coefficient, and c is a focal length value in the wide-angle segment.

[0019] Preferably, before acquiring the region where the target trench is located, the positioning module is configured to establish a conversion matrix of the coordinate system between the PTZ camera and the total station, and the specific steps comprise:

[0020] A standard calibration target is placed in the field of view of the PTZ camera and the total station, and the center point data of the calibration target is obtained from multiple different positions and angles; the transformation matrix between the PTZ camera and the total station coordinate system is calculated by using the nine-point calibration method; the total station is moved to the center point of the calibration target for distance measurement;

[0021] The total station obtains the three-dimensional coordinate P ts,i of the center point of the calibration target in the total station coordinate system by obtaining the three-dimensional observation data of the total station coordinate system ts,i , ts,i ts,i , ts,i ts,i , ts,i , and the three-dimensional observation data of the total station coordinate system includes the slant distance D ts , the horizontal angle Hz and the vertical angle V, and the calculation formula of the three-dimensional coordinate of the center point of the calibration target is:

[0022] T ts,i = -D ts *sin(Hz), Y ts,i = -D ts *cos(Hz), Z ts,i = D ts *cos(V),

[0023] The PTZ camera aligns the center point of the calibration target by controlling the pan-tilt head, adjusts the optical zoom value to control the clear center point of the calibration target in the image; when the image is clear, the current horizontal angle P, vertical angle T and optical zoom value Z are recorded; the pixel size of the current center point of the calibration target in the image is accurately measured by fitting the minimum circumscribed circle of the image; wherein the subscript ts represents the coordinate point of the total station;

[0024] According to the actual physical size S of the center point of the calibration target, the pixel size Ps in the image and the current optical zoom value, the distance D c of the center point of the calibration target to the PTZ camera is calculated:

[0025]

[0026] f(Z) is the effective focal length at the current optical zoom value Z, which is a function obtained by camera zoom calibration, and the calculation formula is:

[0027] f(Z) = a·Z 2 +b·Z+c;

[0028] Combined with the horizontal angle P, the vertical angle T and the distance D c of the center point of the calibration target, the coordinate calculation formula X c,i = -D c *sin(P), Y​​c,i = -D c *cos(P), Z c,i = D c *cos(T) to calculate the coordinate point P c,i = (X c,i , Y c,i , Z c,i ) ;

[0029] A plurality of pairs of corresponding three-dimensional points {P c,i , P ts,i} are used to calculate the best conversion matrix R ts←c by the Kabsch algorithm. The subscript c is the coordinate system related to the PTZ camera.

[0030] Preferably, in the positioning module, the step of coarsely positioning the channel in the channel area comprises:

[0031] Channel searching step:

[0032] The system state is set to target searching, the working states of the PTZ camera and the total station are monitored, and the interface is updated to display to the operator that the current is in the target searching stage and show the real-time video picture or search progress;

[0033] The central control module further comprises a visual processing module, which acquires a video stream from the PTZ camera in real time and pre-processes the video stream;

[0034] After entering the target searching stage, the system controls the PTZ camera to adjust the optical zoom value to the field of view of the minimum focal length, and the PTZ camera head starts to move according to the predetermined path from the preset starting position, and the visual processing module acquires and processes each frame of picture transmitted by the PTZ camera in real time; a target detection model is used to scan the image content to preliminarily identify the features of the channel; if the system preliminarily detects a suspected target, the autonomous search motion of the PTZ camera is immediately paused, a target confirmation process is triggered, and the PTZ camera is controlled to increase the optical zoom value;

[0035] The system uses a target detection model to identify the channel for the enlarged image, if the channel is successfully identified in the current frame and the confidence meets the requirements, the position of the channel in the current target enlarged image is acquired; if the identification fails, the optical zoom value is reduced to continue the search motion; when the target is successfully confirmed, the system locks the current view angle and enters the next stage, and if the target is not found in the entire search process, the search failure is reported;

[0036] Channel coarse positioning step:

[0037] When the target channel is detected, the PTZ camera head is controlled to rotate based on the target coordinates and the image coordinates to move the target center point closer to the image center point;

[0038] When the system confirms that the target channel has been identified, all movements of the PTZ camera head are stopped, and the current view is kept stable; the image coordinates and the PTZ camera state are recorded, and the system state is updated as the target is locked; the laser point position of the total station is moved to the image center point through the transformation matrix between the PTZ camera coordinate system and the total station coordinate system.

[0039] Preferably, the specific steps of controlling the PTZ camera head to rotate to move the target center point closer to the image center point include:

[0040] calculating the target bounding box center point and the pixel deviation of the image center point ; and

[0041] The pixel deviation is converted into the horizontal angle increment ΔP = Δu·msg and the vertical angle increment ΔT = Δv·msg that the PTZ head needs to rotate relative to the picture by using the intrinsic parameters, the current focal length and the distortion correction data of the PTZ camera; wherein, msg is the pixel size;

[0042] The calculated ΔP and ΔT are sent to the control interface of the PTZ camera to control the PTZ camera head to rotate smoothly; after the PTZ camera head rotates, the system reacquires the image, calculates the deviation of the target center from the image center again, and repeats the above process until the deviation of the target from the image center is less than a preset threshold.

[0043] Preferably, when the channel is precisely positioned in the positioning module, the position coordinates of the total station laser point in the image are identified, and the H rotate , V rotate rotational relative amount of the total station to measure the target point is acquired and controlled, and the specific steps include:

[0044] The current PTZ pose of the PTZ camera and the pixel coordinates of the extracted channel corner points are used to calculate the radian value required for the total station laser point to move to the corresponding position, and the calculation formula is:

[0045]

[0046] wherein, D ts is the slant distance of the current total station pointing to the channel, msg is the pixel angle size under the current focal length, H rotate , V rotate is the radian value increment of the total station; is the coordinate point of the laser point; is the coordinate point of the corner; the subscript ts represents the coordinate point of the total station;

[0047] After the total station is rotated, if there is a deviation between the laser point and the corner point, the coordinate data of the laser point and each corner point are recalculated. If there is no deviation between the laser point and the corner point, the edge of the groove is scanned.

[0048] Preferably, the specific steps of obtaining the pixel coordinates of the target edge point by the total station include:

[0049] When the laser point moves to the four corner points in sequence, a detailed scanning task is performed at each corner point: the total station performs a horizontal and vertical scan of the target area. Before the scan begins, the system uses the PTZ camera to lock the current image and extracts the two main edge lines of the groove in the image through the edge detection algorithm; the pixel points on the two edge lines are obtained. Perform least squares fitting to obtain the corresponding straight line equation in the image pixel space to obtain the slope k pixel , the equation of the line is:

[0050] Among them, b pixel is: intercept; the slope of the fitted line is: Where n is the total number of edge pixels;

[0051] Take the average of the slopes of the two edge lines as the main direction of the channel in the image and obtain a two-dimensional direction vector d img =[1,k pixel ], k pixel is the slope; through the rotation matrix R ts←c Project the two-dimensional direction and transform it into the total station coordinate system to obtain an initial three-dimensional space direction vector v slop_initial ; Taking the current scanning point as the origin, v slop_initial A local coordinate system is established for the main axis direction. Based on this coordinate system and the preset scanning step Δs, the system predicts the three-dimensional coordinates of the next scanning point. When scanning the first point v slope_k =v slop_initial , k is the predicted scanning point;

[0052] When the total station is moved to the predicted position, a measurement is immediately performed to obtain the actual landing point P of the laser in three-dimensional space. k+1 , by comparing the deviation between the predicted position and the actual position, the prediction error vector is calculated:

[0053] Use Kalman filtering to estimate the state of the channel direction vector and calculate the latest three-dimensional space direction vector And adjust the scanning trajectory.

[0054] Preferably, the specific steps of obtaining the coordinate data of the target edge point include:

[0055] The total station is controlled to move its laser point to the preset groove edge region according to the calculated rotation relative amount; when the laser ranging system moves to the specified scanning position, a fine scanning process is started;

[0056] When the fine scanning process is performed, the total station performs point position detection on the current region by continuously emitting a laser beam, collects time and angle information of the returned signal, and calculates three-dimensional coordinates of each laser echo point.

[0057] Preferably, in the data analysis module, the specific steps of fitting the groove profile include:

[0058] The shape of the groove cross section is restored using three-dimensional data, and if the edge of the groove is regular, the RANSAC algorithm is used to remove outliers;

[0059] The least square method is used to determine the final model parameters: if the quality of the three-dimensional data and the outliers meet the requirements, the model is fitted using the least square method, and if the edge of the groove is complex, the NURBS fitting method is used to reconstruct the surface;

[0060] After fitting, the fitting residual and the mean square error are calculated.

[0061] Preferably, the step of calculating the geometric parameters of the groove includes:

[0062] Along the central axis of the groove, sampling is performed at preset points, and a cross-sectional plane is established at each preset point position, and based on the cross-sectional plane, the boundary intersection points A i (x1,y1,z1),B i (x2,y2,z2) are obtained, and based on the two boundary intersection points, the groove width of the current cross section is calculated, that is:

[0063]

[0064] The depth of the groove is obtained through the vertical distance from the edge point to the reference plane at the bottom of the groove;

[0065] The length of the groove is obtained by measuring the distance from the starting point P0(x0,y0,z0) to the terminal point P n (x n ,y n ,z n ) of the central axis:

[0066]

[0067] If the center axis is a spatial curve, i.e., r(s)=[x(s),y(s),z(s)], s∈[0,1], wherein r(s) is a parameterized spatial curve that precisely depicts the three-dimensional spatial coordinates (x(s),y(s),z(s)) of each point on the center axis of the target object through a parameter s varying from 0 to 1, the arc length of the center axis is calculated, i.e.,

[0068]

[0069] Preferably, in the data analysis module, the specific steps of calculating and generating the coordinate data of the required work points are as follows: the coordinate data of each work point is generated on the fitted center axis according to a preset interval, and the calculation formula is as follows:

[0070]

[0071] Wherein, the set includes N work points, the index i takes values in the range [i,N], and (X ts,i ,Y ts,i ,Z ts,i ) is the coordinate point of the i-th work point.

[0072] Preferably, in the guiding module, the step of positioning the positions of each feature point at the end of the robot includes:

[0073] Controlling the PTZ camera to turn and zoom to control the field of view to cover the area of the feature point at the end of the robot, and acquiring the corresponding grayscale image I(x,y);

[0074] Based on the high contrast, specific shape and high brightness of the feature point, setting a brightness T, and performing binarization based on the brightness, setting the pixels higher than T as white and the pixels lower than T as black, to generate a binarized picture B(x,y):

[0075]

[0076] Extracting the boundaries of all connected regions in the binarized picture, and screening according to the expected pixel area range [A min ,A max ] occupied by the feature point under the current optical zoom, to remove the regions of noise or non-target objects;

[0077] Calculating the length and width of the minimum circumscribed rectangle of the light spot according to the features of the feature point rectangle, and judging whether the length and width are within the preset range; if both the length and the width are within the preset range, the current feature point is successfully recognized;

[0078] For each successfully recognized feature point contour, the accurate center pixel coordinates (u c,i ,vc,i ), specifically:

[0079] The zeroth moment M 00 =∑ x ∑ y I(x,y), the first moment M 10 =∑ x ∑ y x·I(x,y).M 01 =∑ x ∑ y y·I(x,y), x, y are pixel coordinates, and the accurate center pixel coordinates u c =M 10 / M 00 , V=M 01 / M 00 .

[0080] Preferably, when the total station measures the three-dimensional coordinates of each feature point, the calculated feature point image coordinates (u c,i , v c,i ) are used as a guide to control the total station to move to the corresponding feature point, the total station aims the laser beam at the center of the corresponding feature point, and three-dimensional coordinate measurement is performed.

[0081] Preferably, in the guide module, the specific steps of establishing the conversion relationship between the robot coordinate system and the total station coordinate system include:

[0082] The mobile robot moves to each position to record the coordinate points {P r,1 ,…,P r,n}, the subscript r represents the physical quantity of the robot robot, and the total station is simultaneously moved to each feature point position of the robot to record the coordinate points {P ts,1 ,…,P ts,n};

[0083] The centers of the robot coordinate system and the total station coordinate system are calculated, and each point is subtracted from the center of gravity of the corresponding point set to obtain the centralized point set A covariance matrix is constructed T is a transposition operation that transposes the rows and columns of a matrix, and singular value decomposition H=U∑V is performed on the covariance matrix T , because U and V only change the orientation without changing the shape, U and V are orthogonal matrices, and a rotation matrix R TS←r =UV T is calculated, V T is the transpose of V.

[0084] The multi-sensor fusion guided robot positioning system provided by the application has the advantages of:

[0085] 1. The present application is aimed at high-precision automatic operation of a specific target "channel", integrating a PTZ camera, a high-precision total station, an industrial robot, and a central control module. By combining the advantages of each device, a sub-millimeter level of three-dimensional coordinate measurement accuracy is achieved, the entire process is highly automated, the integration degree is high, manual intervention and measurement errors are significantly reduced, and the high precision, repeatability, and efficiency of the operation are ensured.

[0086] 2. In the central control module, the positioning module uses the wide-angle view of the PTZ camera and advanced vision processing technology to achieve preliminary positioning of the location of the channel, thereby efficiently determining the approximate orientation of the target "channel". Based on the approximate orientation information of the target "channel", the total station is guided to accurately point to the target channel area through the coordination module, and then the PTZ camera system is used to collect the three-dimensional coordinate data of the key feature points (channel corner points) and laser scanning points of the channel in real time. Combined with the calibration relationship between the PTZ camera and the total station, the motion parameters required for the total station to accurately scan the channel edge and center line are calculated, thereby establishing a high-precision coordination working mechanism between the total station and the PTZ camera.

[0087] 3. The total station scans the edge points of the channel according to the calculated path, obtaining the three-dimensional data of each edge point, which provides data basis for the precise positioning of the channel. According to the three-dimensional spatial data of each edge point, after processing, filtering, fitting, etc., the accurate three-dimensional geometric model of the channel is reconstructed, and the precise three-dimensional coordinates of all preset operation points are calculated, finally realizing the high-precision positioning measurement of the channel, providing guarantee for the subsequent installation operation.

[0088] 4. The calibrated coordinate system conversion relationship is used to seamlessly convert the total station data to the robot base coordinate system, eliminating system errors and realizing coordinate system unification. BRIEF DESCRIPTION OF DRAWINGS

[0089] Figure 1 A flowchart of the positioning module acquiring the channel position in the multi-sensor fusion guided robot positioning system provided by the present application;

[0090] Figure 2 A flowchart of the multi-sensor fusion guided robot positioning system startup and initialization provided by the present application;

[0091] Figure 3 A schematic diagram of a standard positioning target in the multi-sensor fusion guided robot positioning system provided by the present application;

[0092] Figure 4 A flowchart of target search and coarse positioning in the multi-sensor fusion guided robot positioning system provided by the present application;

[0093] Figure 5 A path diagram of a channel recognized when searching for a target channel in the multi-sensor fusion guided robot positioning system provided by the present application;

[0094] Figure 6 A channel diagram recognized when searching for a target channel in the multi-sensor fusion guided robot positioning system provided by the present application;

[0095] Figure 7 A flowchart of coordinating the working of the total station and the PTZ camera in the multi-sensor fusion guided robot positioning system provided by the present application;

[0096] Figure 8 A diagram of the position of the laser points recognized in the recognized channel image in the multi-sensor fusion guided robot positioning system provided by the present application;

[0097] Figure 9 A diagram of the position of the channel corner points recognized in the recognized channel image in the multi-sensor fusion guided robot positioning system provided by the present application;

[0098] Figure 10 A flowchart of processing the scanning data in the multi-sensor fusion guided robot positioning system provided by the present application;

[0099] Figure 11 A diagram of fitting the edge points to fit the channel profile in the multi-sensor fusion guided robot positioning system provided by the present application;

[0100] Figure 12 A flowchart of guiding the robot to accurately move to each task point position to perform a task based on the channel position of the total station in the multi-sensor fusion guided robot positioning system provided by the present application. DETAILED DESCRIPTION

[0101] The present application is described herein with reference to specific embodiments thereof which are illustrated in the accompanying drawings. These embodiments are described in detail so as to enable those skilled in the art to best utilize the application, and to enable practitioners thereof to make and use the application in such best manner. The present application is capable of other embodiments and of being practiced or being carried out in various ways. Variations and modifications of the present application can be made thereto without departing from the spirit or scope of the application. It is to be understood that features of the embodiments described herein can be combined, modified or subtracted in any manner without departing from the spirit or scope of the application.

[0102] Parameter definition: P: horizontal rotation angle of PTZ camera. T: vertical rotation angle of PTZ camera. Z: optical zoom value of PTZ camera. Hz: horizontal angle measured by total station. V: vertical angle measured by total station. i: index of calibration point, which takes the value of 1, 2, …, 9. ts: as a subscript, it represents a physical quantity or coordinate system related to the total station. c: as a subscript, it represents a physical quantity or coordinate system related to the PTZ camera. S: actual physical size of the standard calibration target. D: physical distance of the center point of the calibration target. X, Y, Z: coordinate points. K: intrinsic matrix of PTZ camera. J: distortion coefficient of PTZ camera. f(Z): effective focal length function, relationship between focal length and zoom value. msg: pixel angle size. Δu, Δv: horizontal / vertical pixel deviation of the center point of the calibration target from the image center point. ΔP, ΔT: horizontal / vertical rotation angle increment of PTZ camera. Coordinate points of the target model recognition box. Coordinate points of the current image center point. Coordinate points of the corner point. Coordinate points of the laser point. Pixel points on the edge line. rotate , V rotate : increment of total station radian value. W: channel width. L: channel length.

[0103] The present application provides a multi-sensor fusion guided robot positioning system (hereinafter referred to as "positioning system"), wherein the multi-sensor includes a PTZ camera and a total station. The multi-sensor fusion guided robot positioning system includes a central control module and a robot; the central control module includes a channel operation point acquisition module, which calculates the position of the channel operation point by the channel position acquired by the PTZ camera and the total station. A guiding module positions each feature point at the end of the robot through the PTZ camera; measures the three-dimensional coordinates of each feature point through the total station, establishes the conversion relationship between the robot coordinate system and the total station coordinate system; converts the channel operation point coordinate data into robot coordinate data, and the robot moves to the corresponding operation point for operation.

[0104] The channel operation point acquisition module includes a positioning module for coarse positioning of the channel by acquiring the channel area through the PTZ camera; synchronizing the position of the total station to the channel direction, and based on the image position coordinates of the PTZ camera, the channel is precisely positioned by the total station. A data analysis module is used to scan the edge points of the channel, acquire the three-dimensional data of the edge points, fit the channel profile, obtain the channel center axis, calculate the geometric parameters of the channel, and calculate and generate the coordinate data of the required operation points.

[0105] The present application is aimed at high-precision automatic operation of a specific target "tunnel", integrating a spherical pan-tilt-zoom camera (PTZ camera), a high-precision total station, an industrial robot, and a central control module. The PTZ camera is responsible for wide-range and rapid target search and preliminary identification; the total station provides sub-millimeter-level three-dimensional coordinate measurement accuracy; the robot performs precise physical operation; and the central control module is responsible for task scheduling, data management, device collaboration, and motion control. The present application realizes positioning measurement-computation-execution full-process automation through deep collaboration of multiple sensors (i.e., the total station and the PTZ camera) and the robot, reduces human intervention to the minimum, and improves operation accuracy to a sub-millimeter level.

[0106] Specifically, in the tunnel operation point acquisition module, the positioning module utilizes the wide-angle field of view of the PTZ camera and advanced vision processing technology to achieve preliminary positioning of the location of the tunnel, thereby efficiently determining the approximate orientation of the target "tunnel". Based on the approximate orientation information of the target "tunnel", the total station is guided by the collaboration module to accurately point to the target tunnel area, and then the PTZ camera system is used to collect three-dimensional coordinate data of key feature points (tunnel corner points) and laser scanning points of the tunnel in real time. Combined with the calibration relationship between the PTZ camera and the total station, the motion parameters required for the total station to accurately scan the edge and center line of the tunnel are calculated, thereby establishing a high-precision collaboration working mechanism between the total station and the PTZ camera. The total station scans the edge points of the tunnel according to the calculated path, obtaining three-dimensional data of each edge point, which provides data basis for precise positioning of the tunnel. According to the three-dimensional spatial data of each edge point, after processing, filtering, fitting, etc., the accurate three-dimensional geometric model of the tunnel is reconstructed, and the precise three-dimensional coordinates of all preset operation points are calculated, finally realizing high-precision positioning measurement of the tunnel. The positioning system utilizes the conversion relationship between the calibrated robot coordinate system and the total station coordinate system to convert the operation point coordinates to the robot coordinate system, and controls the robot to accurately move to each operation point position to perform tasks. The whole process is highly automated, significantly reducing human intervention and measurement errors, and ensuring high precision, repeatability, and efficiency of the operation.

[0107] As shown in Figure 1 , it is a flowchart of the positioning module of the present application when acquiring tunnel operation points. When positioning, the positioning system needs to start the system and perform initialization operations, as shown in Figure 2 , the specific steps include:

[0108] Step 1.1: Software system startup and self-checking:

[0109] Start the main control software deployed on the central control module. After the software is started, first, a series of initialization procedures are performed, including: loading configuration files (containing device parameters, calibration data, algorithm models, etc.), checking necessary software libraries and dependencies, initializing the log system, and creating a user interaction interface. Subsequently, self-checking of internal modules is performed to confirm that each core function module (vision processing, total station interface, industrial robot interface, data management, etc.) is in a ready state. The self-checking results will be recorded, and if serious errors are found, the startup process will be interrupted and reported to the user.

[0110] Step 1.2: PTZ camera device initialization:

[0111] The main control software communicates with the PTZ camera through a preset network communication interface to send initialization instructions:

[0112] (1) Establish a network connection with the PTZ camera according to the IP address and port in the configuration file;

[0113] (2) Perform a homing operation on the PTZ camera's gimbal to reach a known zero point or initial position;

[0114] (3) Set or restore default camera parameters such as resolution, frame rate, exposure mode, day-night transition, etc.;

[0115] (4) Query device status and clear possible error states;

[0116] After successful initialization, the PTZ camera should be able to respond to subsequent control instructions and provide video streams.

[0117] Step 1.3: Total station device initialization:

[0118] The software establishes a connection with the total station through a serial port. Send initialization instructions:

[0119] (1) Set communication parameters;

[0120] (2) Establish a communication connection;

[0121] (3) Check device status and obtain information such as power, tilt compensator status, etc.;

[0122] (4) Set measurement mode (such as precision measurement mode, tracking mode);

[0123] (5) Activate or check the laser pointer;

[0124] (6) Call the total station's built-in automatic calibration program to ensure measurement accuracy;

[0125] (7) Initialize the platform adjustment device to ensure that the total station is in a horizontal state.

[0126] After initialization is completed, the total station is ready to receive measurement and control instructions.

[0127] Before the positioning module acquires the area where the target slot is located, i.e. after the system initialization is completed, zoom calibration is performed, and the steps include:

[0128] A standard calibration target with an actual physical size of S is placed in front of the PTZ camera, and the distance D from the front end of the PTZ camera lens to the calibration target is accurately measured by a laser range finder c The optical zoom value Z of the PTZ camera is adjusted to an initial position Z1, a clear picture of the standard calibration target is taken, the diameter of the calibration target in the image is measured by using the minimum circumscribed circle fitting, and the pixel size Ps1 is obtained.

[0129] The zoom value is adjusted to a new position Z2, and the new pixel size Ps2 is measured again.

[0130] This process is repeated, the value of Z is systematically changed, a series of data pairs (Z i ,Ps i ) are collected, and thus the effective focal length f i of each data point can be obtained by using the formula , where the subscript c is the coordinate system related to the PTZ camera, and the quadratic polynomial fitting is performed on the data points (Z i ,f i ) to obtain the quadratic term coefficient a, the linear term coefficient b, and the focal length value c in the wide-angle segment, and the effective focal length formula f(Z) = a·Z 2 +b·Z+c under the current optical zoom value Z is obtained. As shown in Figure 3 .

[0131] In this embodiment, after zoom positioning, the conversion matrix of the coordinate system between the PTZ camera and the total station is established, which provides a guarantee for the accurate positioning of the slot by the total station; the specific steps include:

[0132] The standard calibration target is placed in the common field of view of the PTZ camera and the total station, and the center point data of the calibration target is obtained from multiple different positions and angles; the transformation matrix between the PTZ camera and the total station coordinate system is calculated by using the nine-point calibration method; and the total station is moved to the center point of the calibration target for distance measurement.

[0133] The total station obtains the three-dimensional observation data of its own coordinate system to obtain the three-dimensional coordinates P ts,i =(X ts,i ,Y ts,i ,Z ts,i ) of the center point of the calibration target in the total station (X ts,i ,Y ts,iZ ts,i ), the three-dimensional observation data of the total station itself coordinate system includes: slant distance D ts , horizontal angle Hz, vertical angle V, and the calculation formula of the three-dimensional coordinates of the calibration target center point is:

[0134] X ts,i =-D ts *sin(Hz), Y ts,i =-D ts *cos(Hz), Z ts,i =D ts *cos(V),

[0135] Similarly, the PTZ camera aligns the calibration target center point by controlling the pan-tilt-zoom, adjusts the optical zoom value to make it clearly visible in the image center position. When the image is clearly visible, record the current horizontal angle P, vertical angle T and optical zoom value Z; by fitting the minimum circumscribed circle to the image, the pixel size of the current calibration target center point in the image is accurately measured;

[0136] According to the actual physical size S of the calibration target center point, the pixel size s in the image and the current optical zoom value, the distance D c from the calibration target center point to the PTZ camera is calculated:

[0137]

[0138] f(Z) is the effective focal length under the current optical zoom value Z, which is a function obtained by camera zoom calibration, and its calculation formula is:

[0139] f(Z) = a Z 2 +b Z+c;

[0140] Combined with the horizontal angle P, vertical angle T and distance D c of the calibration target center point, the coordinate point P c,i =(X c,i ,Y c,i ,Z c,i ) of the camera in the total station coordinate system is calculated through the coordinate calculation formula X c,i =-D c *sin(P), Y c,i =-D c *cos(P), Z c,i =D c *cos(T);

[0141] Using a plurality of pairs of corresponding three-dimensional points {P c,i , P ts,i}, the best conversion matrix R ts←cThe number of three-dimensional points is preferably calculated using 9 pairs.

[0142] After the conversion matrix of the PTZ camera coordinate system and the total station coordinate system is completed, a communication link is established between the devices, and testing is performed. The specific process is as follows:

[0143] The central control module sends a simple state query instruction to each device and expects to receive a correct response. If the communication test fails (no response, response timeout, response error), the system automatically attempts to resend the instruction to reestablish the connection after a short delay. If the maximum number of retries is exceeded, the system displays explicit error information and possible solutions (such as checking the cable, restarting the device) to the operator. At the same time, detailed error information (timestamp, error code, device identification) is recorded. Before the communication is successfully established, the system is prevented from entering the subsequent main process that requires device cooperation, i.e., the positioning and device cooperation corresponding processes.

[0144] As shown in FIG. 2, the steps of searching for a slot and coarsely positioning the slot in the target slot area by the positioning module include: Figure 4

[0145] Step S2.1: Slot search step:

[0146] Step S2.11: Enter the main control process and the state monitoring process.

[0147] The system state is set to "target search in progress", the working state of the PTZ camera, the total station, and the industrial robot, and the running state of the central control module and the communication link are monitored, and the interface is updated to display to the operator that the current is in the target search stage, and the real-time video picture or search progress is displayed;

[0148] Step S2.12: PTZ camera real-time video stream acquisition and preprocessing. The central control module further includes a visual processing module that acquires a real-time video stream from the PTZ camera and pre-processes the video stream;

[0149] ​Specifically, the visual processing module starts to continuously receive real-time video data stream from the PTZ camera. For the target recognition of the channel, the YOLOv8n model pre-trained on large public datasets such as COCO is used as the basis, and based on this pre-trained model, transfer learning and fine-tuning are performed using the self-built dataset. To adapt to the input requirements of the target recognition algorithm, the received compressed video data is decoded into original image frames. Then, based on the calibrated PTZ camera intrinsic matrix K and distortion coefficient J, the image is corrected for distortion to eliminate geometric distortion caused by the lens. Subsequently, the image color space is converted to the BGR format required by the model, and the channel order is adjusted according to the requirements of the deep learning framework used. To meet the model's requirement for a fixed input size (640x640), the image is first scaled proportionally, and then the size is standardized through padding or cropping, while preserving as much original image information as possible. Subsequently, the pixel values are normalized to an appropriate numerical range to improve the stability and inference efficiency of the model. Finally, the image frames are input into the target detection model for subsequent analysis and recognition.

[0150] Step S2.13: Start the model-based visual target recognition automatic search mode (channel matching).

[0151] Specifically, after entering the target search phase, the positioning system will start the preset automatic search strategy, fully utilizing the wide coverage capability of the PTZ camera. The positioning system controls the PTZ camera to adjust the optical zoom value to the minimum focal length field of view, maximizing the coverage of a single frame, thereby achieving rapid initial scanning of the wide working area.

[0152] Starting from the preset starting position, the PTZ camera gimbal moves according to the predetermined path; that is, it performs a zigzag scan at a slow speed, thereby covering the entire monitoring area without omission and ensuring sufficient image capture time at each location. As shown in Figure 5 .

[0153] At the same time and in real time, the visual processing module acquires and processes each frame of image transmitted by the PTZ camera; a lightweight target detection model preliminarily trained for faster processing is used to quickly scan the image content and preliminarily identify the features of the channel; if the system preliminarily detects a suspected target, the system immediately suspends the autonomous search motion of the PTZ camera, triggers the target confirmation process, and controls the PTZ camera to increase the optical zoom value to an appropriate focal length; the system uses a more accurate target detection model to identify the channel based on the enlarged image; if the channel is successfully identified in the current frame and the confidence meets the requirements, the position of the channel in the current target enlarged image is obtained, including the coordinates of the bounding box, the pixel coordinates of the center point, and the pose of the target; if the identification fails, the optical zoom value is reduced and the search motion is continued;

[0154] When the target is successfully confirmed, the positioning system locks the current viewing angle and enters the next stage. If the target cannot be found during the entire search process, the search failure is reported. The approximate position of the slot obtained by coarse positioning is as follows: Figure 6 shown.

[0155] Step 2.2: Groove rough positioning steps:

[0156] Step 2.21: After the target channel is detected, the PTZ camera is controlled to rotate based on the target coordinates and the image coordinates to move the target center closer to the image center. The specific steps include:

[0157] (1) Calculate the center point of the target bounding box With the center of the image Pixel deviation and

[0158] (2) Using the internal parameters, current focal length and distortion correction data of the PTZ camera, the pixel deviation is converted into the horizontal angle increment ΔP = Δu·msg and the vertical angle increment ΔT = Δv·msg that the PTZ camera needs to rotate relative to the picture.

[0159] (3) The calculated ΔP and ΔT are sent to the PTZ camera's control interface to control the smooth rotation of the PTZ camera's pan / tilt. After the PTZ camera's pan / tilt rotates, the system reacquires the image and recalculates the deviation between the target center and the image center. The above process is repeated until the deviation of the target from the image center is less than a preset threshold. During this process, if the target size is too small, the system controls the PTZ camera to perform optical zoom to magnify the target area to provide clearer details.

[0160] Step 2.22: Once the system confirms it has identified the target channel, it stops all PTZ camera movement to maintain a stable viewing angle. The system then records the PTZ camera status and calculates the distance Dc from the PTZ camera to the target channel using the optical zoom value. The system status is then updated to "Target Locked." Using the transformation matrix between the PTZ camera coordinate system and the total station coordinate system, the total station's laser point is moved to the center of the image. The PTZ camera maintains the current viewing angle until receiving new commands.

[0161] Among them, when obtaining the relative rotation amount of the scanning motion, the key feature points are first extracted, including: the pixel coordinates of the four corner points of the target "groove" in the image And the pixel coordinates of the laser spot projected by the total station Where i is the index of the calibration point.

[0162] like Figure 7As shown, when the slot is fine positioned in the positioning module, the position coordinates of the total station laser point in the image are recognized, and the H rotate , rotate relative rotation amount, specifically:

[0163] Step 3.1: Using the current PTZ pose of the PTZ camera and the pixel coordinates of the extracted slot corner points, the radian value required for the total station laser point to move to the corresponding position is calculated. The calculation formula is:

[0164]

[0165] where D ts is the accurate slant range of the current total station pointing to the slot, msg is the pixel angle size under the current focal length, which defines the actual angle covered by a single pixel in the horizontal and vertical directions; H rotate , V rotate : the increment of the total station radian value. As shown in Figure 8 , 9 .

[0166] Step 3.2: After the total station is rotated, if there is a deviation between the laser point and the corner point, the laser point and the corner point coordinate data are recalculated. If there is no deviation between the laser point and the corner point, the slot edge scanning is performed.

[0167] As shown in Figure 10 , 11 , the specific steps to obtain the coordinate data of the target edge point include:

[0168] Step 4.1: When the laser point moves to the four corner points in turn, a fine scanning task is performed at each corner point position. In this process, the total station performs scanning in two dimensions of horizontal and vertical directions on the target area. Before scanning starts, the system will lock the current image using the PTZ camera, and extract the two main edge lines of the slot in the image through the edge detection algorithm. A series of pixel points on the two edge lines are fitted by the least squares method to obtain the corresponding straight line equation in the image pixel space , so as to calculate the corresponding slope k pixel . Where b pixel is the intercept, and the slope of the fitted straight line is: where n is the total number of edge pixel points.

[0169] Step 4.2: Take the average of the slopes of the two edge lines as the main direction of the slot in the image. Thus, a two-dimensional direction vector d img = [1, k pixel ] is obtained. Through the already calibrated rotation matrix R ts←cThe two-dimensional direction is approximately projected and converted to the total station coordinate system to obtain an initial three-dimensional space direction vector v slop_initial This vector represents the general trend of the channel. With the current scanning point as the origin, v slop_initial is the main axis direction, a local coordinate system is established, and according to this coordinate system and the preset scanning step Δs, the system predicts the three-dimensional coordinates of the next scanning point When scanning the first point, v slope_k =v slop_initial . k is the predicted scanning point.

[0170] Step 4.3: When the total station is moved to the predicted position, immediately perform a high-precision measurement to obtain the actual landing point P k+1 of the laser in three-dimensional space, and calculate the prediction error vector Then use the Kalman filter in the library to estimate the state of the direction vector of the channel to obtain the latest three-dimensional space direction vector and adjust the subsequent scanning trajectory. To ensure that the laser point is always inside the channel, the system uses a jump detection algorithm to continuously monitor the three-dimensional coordinates between consecutive measurement points. Once a significant mutation that exceeds the preset threshold is detected, the system will continue to collect two additional data points to prevent false positives caused by accidental noise.

[0171] Before the data analysis module fits the channel profile, the original scanning data needs to be checked and denoised. The specific process is as follows:

[0172] First, after transmitting three-dimensional data to the data management and storage module in the central control module through the total station interface, the system does not immediately perform geometric fitting processing, but first performs data format and integrity checking to ensure that each point has complete three-dimensional coordinates and no format errors or data loss.

[0173] Then, the coordinate range and validity are checked, and points outside the preset spatial boundary or containing abnormal values such as null values are removed. In the outlier removal stage, the system uses the density-based radius method to remove abnormal points. To simplify the data structure and improve the efficiency of subsequent processing, the system processes the three-dimensional data as follows: First, by unifying local dense points as representative points, the total size of the point cloud is reduced. Then, the simplified three-dimensional data is indexed in space using an octree structure to construct an efficient data access form, thereby supporting fast and accurate spatial queries and neighborhood searches.

[0174] In this embodiment, the specific steps of the data analysis module fitting the channel profile include:

[0175] If the edge of the channel is regular, the RANSAC algorithm with strong robustness is used to remove a small amount of noise points;

[0176] A more accurate least square method is used to determine the final model parameters: if the quality and noise points of the three-dimensional data meet the requirements, i.e., the fitting is good and the noise points are few, the least square method is used to fit the model, if the edge of the channel is complex, the NURBS advanced fitting method is used to fit the two-dimensional model for multiple cross sections, and the curved surface is reconstructed;

[0177] After the fitting is completed, the fitting residual and mean square error are calculated, and the fitting parameters are corrected through the obtained fitting residual and mean square error.

[0178] In this embodiment, the step of calculating the geometric parameters of the channel by the data analysis module includes:

[0179] Along the central axis of the channel, sampling is performed at a preset point, and a cross-sectional plane is established at each preset point position, which is perpendicular to the direction of the axis. Based on the cross-sectional plane, the boundary intersection points A i (x1,y1,z1),B i (x2,y2,z2) are obtained, based on the two boundary intersection points, the channel width of the current cross section is calculated, i.e.:

[0180]

[0181] The depth of the channel is obtained through the vertical distance from the edge point to the reference plane of the channel bottom;

[0182] The length of the channel is obtained by measuring the distance of the central axis from the starting point P0(x0,y0,z0) to the terminal point P n (x n ,y n ,z n ):

[0183]

[0184] If the central axis is a spatial curve, i.e. r(s)=[x(s),y(s),z(s)],s∈[0,1], the arc length of the central axis is calculated, i.e.

[0185]

[0186] wherein r(s) is a parameterized spatial curve, which accurately depicts the three-dimensional spatial coordinates (x(s),y(s),z(s)) of each point on the central axis of the target object through a parameter s changing from 0 to 1,

[0187] In this embodiment, the data analysis module calculates and generates the coordinate data of the required work points. The specific steps are as follows: on the fitted center axis, the coordinate data of each work point is generated according to the preset interval, and the calculation formula is:

[0188]

[0189] where the set includes N work points, the index i has a value range of [i, N], and (X ts,i , Y ts,i , Z ts,i ) is the coordinate point of the i-th work point.

[0190] As shown in Figure 12 , based on the precise coordinates of the channel and the precise three-dimensional coordinates of all the generated preset work points, the robot is guided to perform precise work, and the specific steps include:

[0191] Step 5.1: Visual search and positioning of mechanical arm end feature points (reflective stickers).

[0192] In order to accurately calibrate the current position of the robot, the system needs to locate a plurality of high-reflectivity reflective stickers (positioning feature points) installed on the robot end effector. The PTZ camera is turned and zoomed to cover the area where the reflective stickers are installed on the robot end, and a grayscale image I(x, y) is obtained.

[0193] Since the reflective stickers usually have high contrast, specific shape and high brightness, a suitable brightness T is set, and binarization is performed based on brightness. Pixels higher than T are set to white, and pixels lower than T are set to black, to generate a binary image The boundaries of all connected regions in the binary image are extracted, and according to the expected pixel area range [A min , A max ] that the reflective stickers may occupy under the current optical zoom, noise or non-target object regions are removed.

[0194] According to the characteristics of the reflective sticker rectangle, the length and width of the minimum circumscribed rectangle of the light spot are calculated, and it is judged whether the length and width are within the preset range. For each successfully recognized reflective sticker contour, the accurate center pixel coordinates (u c,i , v c,i ) in the image are calculated, and the calculation process is as follows:

[0195] First, the zero-order moment M 00 =∑ x ∑ y I(x,y) is calculated, and the first-order moment M 10 =∑ x ∑ yx I(x, y) M 01 =∑ x ∑ y y I(x, y), x, y are pixel coordinates, in order to obtain the accurate center pixel coordinates u c = M 10 / M 00 , v = M 01 / M 00 .

[0196] Step 5.2: Total station accurately measures the three-dimensional coordinates of the retroreflective sticker.

[0197] Using the calculated retroreflective sticker image coordinates (u c,i , v c,i ) as a guide, control the total station to move to the retroreflective sticker position, to ensure that the laser point moves to the center point, after moving to the point, start the total station ATR function again, automatically fine search and lock the center of the retroreflective sticker, accurately aim the laser beam at the center of the retroreflective sticker, and perform a high-precision three-dimensional coordinate measurement.

[0198] Step 5.3: Establish the conversion relationship between the robot mechanical arm base coordinate system and the total station coordinate system.

[0199] Move the robot mechanical arm to each position to record the coordinate points {P r,1 ,…,P r,n}, where the subscript r represents the coordinate points of the robot, and simultaneously move the total station to each retroreflective sticker position of the robot mechanical arm to record the coordinate points {P ts,1 ,…,P ts,n}, where the subscript ts represents the coordinate points of the total station.

[0200] Calculate the robot mechanical arm coordinate system N is the total number of coordinate systems, i is the index, and the value range is from 1 to N and the center of gravity of the total station coordinate system , and then subtract the center of gravity of each point set from the center of gravity to obtain the centralized point set Construct a 3x3 covariance matrix T is a transpose operation that transposes the rows and columns of the matrix, and singular value decomposition H = U∑V T is performed on the covariance matrix, U and V are 3x3 orthogonal matrices of different orientations, from which the rotation matrix R TS←r = UV T can be calculated, and V T is the transpose of V.

[0201] Step 5.4: Coordinate conversion of target work points and control of mechanical arm movement to work points.

[0202] The rotation matrix obtained in the last step is used to convert the coordinates of all target slot work points calculated by the positioning module into the robot arm coordinate system. The robot arm control interface module receives the converted target work point coordinates, plans and executes the movement of the robot arm, and accurately moves the end to the currently specified target slot work point, preparing to perform the task.

[0203] Obviously, the described embodiments are only part of the embodiments of the present application, not all. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative labor shall fall within the scope of protection of the present application.

Claims

1. Multi-sensor fusion guided robot positioning system, characterized by: include: Central control module and robots; The multi-sensor includes: a PTZ camera and a total station; the central control module includes: The trough operation point acquisition module calculates the position of the trough operation point by using the PTZ camera and the total station to obtain the trough position; The guidance module locates the position of each feature point at the end of the robot through the PTZ camera; measures the three-dimensional coordinates of each feature point through the total station, and establishes the conversion relationship between the robot coordinate system and the total station coordinate system; converts the coordinate data of the groove operation point into the robot coordinate data, and the robot moves to the corresponding operation point to perform the operation.

2. The multi-sensor fusion guided robot positioning system according to claim 1, characterized in that: The slot operation point acquisition module includes: The positioning module is used to obtain the area where the trough is located through the PTZ camera and perform rough positioning of the trough; synchronize the position of the total station to the direction of the trough, and perform fine positioning of the trough through the total station based on the image position coordinates of the PTZ camera; The data analysis module is used to scan the edge points of the groove, obtain the three-dimensional data of the edge points, fit the groove contour, obtain the center axis of the groove, calculate the geometric parameters of the groove, and calculate and generate the coordinate data of the required operating points.

3. The multi-sensor fusion guided robot positioning system according to claim 2, characterized in that: Before the positioning module obtains the area where the target channel is located, it is necessary to establish a conversion matrix between the coordinate systems of the PTZ camera and the total station. The specific steps include: Place a standard calibration target within the common field of view of the PTZ camera and the total station, and obtain the calibration target center point data from multiple different positions and angles; use the nine-point calibration method to calculate the transformation matrix between the PTZ camera and the total station coordinate system; move the total station to the calibration target center point for distance measurement; The total station obtains the three-dimensional coordinates P of the calibration target center point i in the total station by obtaining the three-dimensional observation data of its own coordinate system. ts,i =(X ts,i ,Y ts,i ,Z ts,i )(X ts,i ,Y ts,i ,Z ts,i ), the three-dimensional observation data of the total station's own coordinate system includes: slope distance D ts , horizontal angle Hz, vertical angle V, the calculation formula for the three-dimensional coordinates of the calibration target center point is: X ts,i =-D ts *sin(Hz),Y ts,i =-D ts *cos(Hz),Z ts,i =D ts *cos(V), The PTZ camera uses its pan / tilt control to align with the center point of the calibration target, and adjusts the optical zoom value to make the center point of the calibration target at the center of the image clear. When the image is clear, the current horizontal angle P, vertical angle T, and optical zoom value Z are recorded. The pixel size of the current calibration target center point in the image is measured by fitting the minimum circumscribed circle of the image. The subscript ts represents the coordinate point of the total station. Calculate the distance D from the center of the calibration target to the PTZ camera based on the actual physical size S of the center of the calibration target, the pixel size Ps in the image, and the current optical zoom value. c : f(Z) is the effective focal length at the current optical zoom value Z, a function obtained through camera zoom calibration, and its calculation formula is: f(Z)=a·Z 2 +b·Z+c; Combined with the horizontal angle P, vertical angle T and distance D of the calibration target center point c , by calculating the coordinate formula X c,i =-D c *sin(P),Y c,i =-D c *cos(P), Z c,i =D c *cos(T) calculates the coordinate point P c,i =(X c,i ,Y c,i ,Z c,i ); where a is the quadratic coefficient of the nonlinear rate of change of the effective focal length with the zoom position, b is the linear coefficient, and c is the focal length value in the wide-angle range; Use multiple pairs of corresponding 3D points {P c,i , P ts,i }, calculate the optimal transformation matrix R through the Kabsch algorithm ts←c , subscript c is the coordinate system related to the PTZ camera.

4. The multi-sensor fusion guided robot positioning system according to claim 2, characterized in that: In the positioning module, the step of roughly positioning the groove in the groove area includes: Slot search steps: Set the system status to target search, monitor the working status of the PTZ camera and total station, and update the interface to show the operator that it is currently in the target search phase and display the real-time video image or search progress; The central control module also includes: a visual processing module, which obtains video streams from the PTZ camera in real time and pre-processes the video streams; After entering the target search phase, the system controls the PTZ camera to adjust the optical zoom value to the field of view of the minimum focal length. Starting from the preset starting position, the PTZ camera pan and tilt moves along a predetermined path, and the visual processing module acquires and processes each frame transmitted by the PTZ camera in real time; the target detection model is used to scan the image content and preliminarily identify the characteristics of the groove; if the system preliminarily detects a suspected target, the autonomous search movement of the PTZ camera is immediately suspended, the target confirmation process is triggered, and the PTZ camera is controlled to increase the optical zoom value; the system uses the mark detection model to identify the groove in the magnified image. If the groove is successfully identified in the current frame and the confidence level meets the requirements, its position in the current target magnified image is obtained; if the identification fails, the optical zoom value is reduced to continue the search movement; when the target is successfully confirmed, the system locks the current viewing angle and enters the next phase. If the target is not found during the entire search process, the search failure is reported; Groove rough positioning steps: When the target channel is detected, the PTZ camera is controlled to rotate based on the target coordinates and the image coordinates to move the target center point closer to the image center point; When the system confirms that the target channel has been identified, it stops all movement of the PTZ camera pan / tilt to maintain a stable viewing angle. The PTZ camera status is recorded, and the distance Dc from the PTZ camera to the target channel is calculated using the optical zoom value. The system status is updated to target locked. The total station's laser point position is moved to the center of the image using the transformation matrix between the PTZ camera coordinate system and the total station coordinate system. The specific steps of controlling the PTZ camera pan / tilt to rotate so as to move the target center point closer to the image center point include: Calculate the center point of the target bounding box With the center of the image Pixel deviation and Using the PTZ camera's internal parameters, current focal length, and distortion correction data, the pixel deviation is converted into the horizontal angle increment ΔP = Δu·msg and the vertical angle increment ΔT = Δv·msg that the PTZ head needs to rotate relative to the image. Where msg is the pixel size. The calculated ΔP and ΔT are sent to the control interface of the PTZ camera to control the smooth rotation of the PTZ camera pan / tilt. After the PTZ camera pan / tilt rotates, the system reacquires the image, recalculates the deviation between the target center and the image center, and repeats the above process until the deviation of the target at the image center is less than the preset threshold.

5. The multi-sensor fusion guided robot positioning system according to claim 2, characterized in that: When the positioning module accurately positions the channel, it identifies the position coordinates of the total station laser point in the image, obtains and controls the total station to measure the target point H rotate 、V rotate Rotate the relative amount, the specific steps include: Using the current PTZ posture of the PTZ camera and the pixel coordinates of the extracted groove corner points, the radian value required for the total station laser point to move to the corresponding position is calculated. The calculation formula is: Among them, D ts is the slope distance of the current total station pointing to the channel, msg is the pixel angle size at the current focal length, H rotate , V rotate is the increment of the total station's arc value; is the coordinate point of the laser point; is the coordinate point of the corner point; the subscript ts represents the coordinate point of the total station; After the total station is rotated, if there is a deviation between the laser point and the corner point, the coordinate data of the laser point and each corner point are recalculated. If there is no deviation between the laser point and the corner point, the edge of the groove is scanned.

6. The multi-sensor fusion guided robot positioning system according to claim 2, characterized in that: In the data analysis module, the specific steps of fitting the groove profile include: The shape of the channel cross section is restored using 3D data. If the edge of the channel is regular, the RANSAC algorithm is used to remove noise. The least squares method is used to determine the final model parameters: if the 3D data quality and noise meet the requirements, the least squares method is used to fit the model. If the groove edge is complex, the NURBS fitting method is used to reconstruct the surface. After the fitting is completed, the fitting residuals and mean square error are calculated.

7. The multi-sensor fusion guided robot positioning system according to claim 2, characterized in that: In the data analysis module, the specific steps for calculating and generating the coordinate data of the required operating points are as follows: on the fitted central axis, the coordinate data of each operating point is generated according to the preset spacing, and the calculation formula is: Among them, the collection Including N operating points, the value range of index i is [i, N], (X ts,i ,Y ts,i ,Z ts,i ) is the coordinate point of the i-th operating point.

8. The multi-sensor fusion guided robot positioning system according to claim 1, characterized in that: In the guidance module, the step of locating the positions of the characteristic points of the robot end includes: Control the PTZ camera to turn and zoom, control the field of view to cover the area of ​​the feature points at the end of the robot, and obtain the corresponding grayscale image I(x,y); Based on the high contrast, specific shape and high brightness of the feature points, a brightness T is set, and the pixels above T are set to white, and the pixels below T are set to black to generate a binary image B(x,y): Extract the boundaries of all connected regions in the binary image, and calculate the pixel area range occupied by the expected feature points under the current optical zoom [A min ,A max ] to filter out noise or non-target areas; Calculate the length and width of the smallest circumscribed rectangle of the light spot based on the features of the feature point rectangle, and determine whether the length and width are within the preset range; if both are within the preset range, the current feature point is successfully identified; For each successfully identified feature point contour, calculate its exact center pixel coordinates (u c,i ,v c,i ), specifically: Find the zero-order moment M 00 =∑ x ∑ y I(x,y), first-order moment M 10 =∑ x ∑ y x·I(x,y).M 01 =∑ x ∑ y y·I(x,y), x, y are pixel coordinates, and then calculate the precise center pixel coordinate u c =M 10 / M 00 , v=M 01 / M 00 .

9. The multi-sensor fusion guided robot positioning system according to claim 1, characterized in that: When the total station measures the three-dimensional coordinates of each feature point, the calculated feature point image coordinates (u c,i ,v c,i ) as a guide to control the total station to move to the corresponding feature point. The total station aims the laser beam at the center of the corresponding feature point to perform three-dimensional coordinate measurement.

10. The multi-sensor fusion guided robot positioning system according to claim 1, characterized in that: In the guidance module, the specific steps of establishing the conversion relationship between the robot coordinate system and the total station coordinate system include: Move the robot to each location to record the coordinate points {P r,1 ,…,P r,n }, where the subscript r represents the coordinate point of the robot, and the total station is moved to each feature point of the robot to record the coordinate point {P ts,1 ,…,P ts,n }, where the subscript ts represents the coordinate point of the total station; Calculate the robot coordinate system N is the total number of coordinate systems, i is the index, ranging from 1 to N and the total station coordinate system The center of gravity of each point is subtracted from the center of gravity of its corresponding point set to obtain the centralized point set Construct a covariance matrix T is the transpose operation, which permutes the rows and columns of the matrix and performs singular value decomposition H=U∑V on the covariance matrix T , U and V are orthogonal matrices of different orientations, from which the rotation matrix R is calculated TS←r =UV T , V T is the transpose of V.

Images