A multi-machine docking positioning system construction and relative posture measurement method

Abstract

The present invention relates to a method for constructing a multi-machine docking and positioning system and measuring relative posture, comprising the following steps: firstly, according to the docking task requirements, the multi-robot system is divided into a docking end robot and a docked end robot, a laser ranging sensor array is deployed on the first docking surface of the docking end robot, a docking positioning mark is deployed on the second docking surface of the docked end robot, and the geometric parameters of the sensor array and the docking positioning mark are designed. A multi-machine docking and positioning coordinate system is established, and the docking end robot and the docked end robot are guided to the docking area. Then, the distance intensity data of the laser ranging sensor array is obtained, data noise processing and abnormal state judgment are performed, and the relative posture between robots is calculated. The method of the present invention solves the problem that visual sensors are easily affected by environmental interference and the measurement accuracy of general laser radars is limited, and is suitable for posture adjustment and multi-machine alignment operations during processing, assembly and transportation.

Description

Technical Field

[0001] The invention belongs to the field of intelligent manufacturing and robotics technology, and specifically relates to a multi-machine docking positioning system construction and a relative posture measurement method. Background Art

[0002] In modern industry and service fields, the development of robotics has greatly promoted the process of automated production and efficient operation. Especially in scenarios where multiple robots need to work together, such as assembly lines, warehousing logistics, and unmanned vehicle formations, precise positioning and docking technology has become one of the key technologies. By working together, multi-robot systems can improve the flexibility, reliability, and production efficiency of the system. However, achieving precise docking and collaborative operation between multiple robots faces many technical challenges, especially the problems of positioning accuracy and docking reliability.

[0003] At present, the commonly used multi-robot docking methods mainly rely on visual sensors and mechanical sensors. Visual sensors, such as cameras, can provide rich environmental information and realize robot positioning and docking through image processing algorithms. However, visual sensors are limited by factors such as lighting conditions and environmental occlusion, and are easily disturbed in complex environments, affecting positioning accuracy. In addition, the image processing algorithm has a large amount of calculation and poor real-time performance, and is not suitable for docking tasks that require high real-time performance. Mechanical sensors, such as contact sensors, achieve docking between robots through actual contact. Although this method is simple and reliable, there is a risk of wear and damage, especially in high-frequency docking operations. In addition, the accuracy of mechanical sensors is limited, and it is difficult to meet the needs of high-precision docking. For example, the Chinese invention patent with publication number CN116572008A proposes a docking assembly of low-pressure turbine rotors based on visual measurement and force feedback.

[0004] Laser ranging sensors have gradually become a research hotspot in the field of robot positioning and navigation due to their high precision, fast response, and immunity to ambient light. Laser ranging sensors can accurately measure the distance and position of objects by emitting laser beams and receiving reflected signals. Using laser ranging sensors for precise positioning of multi-robot docking can achieve high-precision non-contact positioning, providing a reliable technical guarantee for multi-robot collaborative operations.

[0005] In a multi-robot docking system, a key issue is how to use the data from laser range sensors for accurate positioning and docking. Since different robots may have changes in relative position and posture during the docking process, the data from a single sensor is often difficult to meet the needs of accurate positioning. Therefore, it is usually necessary to deploy a laser range sensor array at the docking end to obtain comprehensive position information through multi-point measurement.

[0006] On the other hand, the deployment of docking positioning marks at the docked end can significantly improve the recognition accuracy and reliability of the sensor through positioning marks with specific shapes and structures. In practical applications, the reasonable design and layout of the laser ranging sensor array and docking positioning marks, and the establishment of effective algorithms and methods for data processing and analysis are key technologies for achieving precise docking of multiple robots. Summary of the invention

[0007] The purpose of the present invention is to solve the problems raised in the above-mentioned background technology. The present invention proposes a multi-machine docking positioning system construction and relative posture measurement method. Through the positioning method combining a laser ranging sensor array with a positioning mark block, the problems that visual sensors are susceptible to environmental interference and mechanical sensors have limited accuracy are solved. The system and method of the present invention are not only suitable for assembly and processing processes in industrial manufacturing, but also can be applied to logistics warehousing, intelligent transportation, drone formations and other fields, and have broad application prospects and promotion value.

[0008] In order to achieve the above object, the present invention adopts the following technical solutions: including:

[0009] Step 1: Deployment of multi-machine docking and positioning system: Calculate the design parameters of the laser ranging sensor array and the docking positioning mark according to the multi-machine docking and positioning system parameter design method; Deploy the laser ranging sensor array on the first docking surface of the docking end robot according to the multi-machine docking and positioning system deployment method, and deploy the docking positioning mark on the second docking surface of the docked end robot, wherein the docking positioning mark is composed of a triangular positioning mark block and limit reflective marks installed on both sides thereof; Electrically connect the laser ranging sensor to the controller;

[0010] Step 2: Construction of multi-machine docking positioning coordinate system: Take the geometric center of the first docking surface of the docking robot as the origin, the X axis perpendicular to the first docking surface inward, and the Y axis parallel to the first docking surface forward to establish the first coordinate system; take the geometric center of the docked robot as the origin, the X1 axis perpendicular to the second docking surface outward, and the Y1 axis parallel to the second docking surface forward to establish the second coordinate system;

[0011] Step 3: Initialization guidance of multi-machine docking positioning: guide the docking robot and the docked robot to the docking area, and ensure that the docked robot is within the measurement range of the docking robot;

[0012] Step 4: Laser ranging sensor array data processing: Use the laser ranging sensor array on the docking end robot to collect the distance intensity data of the docking positioning mark of the docking end robot in real time, use the array ranging data processing method to process the data noise, and obtain the laser ranging sensor filter data sequence, including the measurement filter data of all laser ranging sensors;

[0013] Step 5: Determination of abnormal state of array ranging data: For the laser ranging sensor filtered data sequence, the relative attitude angle between the robots is calculated according to the data sequence quadratic difference method and the relative attitude angle estimation method. If the relative attitude angle is greater than the working acute angle α of the triangular positioning mark block, it is determined that the attitude deviation is too large, and then return to step 3 for reinitialization; if the relative attitude angle is not greater than the working acute angle α of the triangular positioning mark block, continue to determine whether the laser ranging sensor array is located between the two limit reflective marks according to the laser echo intensity threshold method. If it is located between the two limit reflective marks, go to step 6; if any laser ranging sensor detects a limit reflective mark, return to step 3 for reinitialization;

[0014] Step 6: Calculation of relative posture measurement of multi-machine docking: According to the parity of the number n of the laser ranging sensor array, determine the number K of laser ranging sensors aimed at the triangulation positioning mark block. If n is an odd number, K is 1; if n is an even number, K is 2; identify the laser ranging sensor currently aligned with the triangulation positioning mark block according to the data sequence quadratic difference method. If n is an odd number, record the measurement filter data of the laser ranging sensor m aimed at the triangulation positioning mark block as The relative posture calculation formula between robots is:

[0015]

[0016] in is the measurement filter data of the adjacent sensor on the left side of the mth laser ranging sensor, is the measurement filter data of the adjacent sensor on the right side of the mth laser ranging sensor, s is the target distance from the geometric center of the docked robot to the second docking surface, α is the angle between the long right-angle side and the hypotenuse of the triangulation mark block, D is the distance between the laser ranging sensors, and l is the length of the long right-angle side of the triangulation mark block;

[0017] If n is an even number, the measurement filter data of the laser ranging sensor M1 aligned with the triangulation positioning mark block is The measurement filter data of the laser ranging sensor aimed at the triangulation positioning marker block is Then the relative posture data between robots is:

[0018]

[0019] in is the measurement filtering data of the adjacent sensor on the left side of the Mth laser ranging sensor, is the measurement filtering data of the adjacent sensor on the right side of the M+1th laser ranging sensor, s is the target distance from the geometric center of the docked robot to the second docking surface, α is the angle between the long right-angled side and the hypotenuse of the triangulated positioning mark block, D is the distance between the laser ranging sensors, and l is the length of the long right-angled side of the triangulated positioning mark block.

[0020] Furthermore, the parameter design method of the multi-machine docking positioning system includes:

[0021] Step 1.1: Parameter initialization: According to the length dimension M1 of the first docking surface of the docking end robot and the length q of the laser ranging sensor, determine the docking surface measurement coefficient J, J∈(0,1), the length tolerance t of the long right-angle side l , positioning mark spacing tolerance t m , minimum butt spacing L d , take the initial value of the number of laser ranging sensor arrays n (n≥3) as 3;

[0022] Step 1.2: Array spacing design: solve the minimum laser ranging sensor array spacing D that satisfies equation (3);

[0023] JM1≤L j =nq+(n-1)D (18)

[0024] Step 1.3: Design of the spacing between positioning marks: The butt positioning mark is composed of a triangular positioning mark block and its two sides with a distance of d r The installed limit reflective marking composition is designed to meet the positioning marking spacing d of formula (4) r ;

[0025] L j <d r <L j +t m (19)

[0026] Step 1.4: Design of triangular positioning marker parameters: The triangular positioning marker is in the shape of a non-isosceles right triangle. The length of the right-angle side l of the triangular positioning marker that satisfies formula (5) is designed as follows:

[0027] D<l≤D+t l (20)

[0028] The maximum value l of the length of the long right angle side l max and the minimum value l min Substitute into equation (6) to find the minimum value of the acute angle α between the long right-angled side and the hypotenuse: min and the maximum value α max :

[0029]

[0030] Determine the minimum value of the acute angle α min and the maximum value α max Whether it satisfies formula (7),

[0031] 10°≤α<45° (22)

[0032] If satisfied, the multi-machine docking positioning system parameter design method is terminated; if not satisfied, n+1 is added and the process returns to step 1.2.

[0033] Furthermore, the multi-machine docking positioning system deployment method includes:

[0034] Step 2.1: With the midpoint of the first docking surface of the docking end robot as the center, the laser ranging sensor array is symmetrically arranged at equal intervals according to a predetermined spacing D, ensuring that the laser ranging sensor array is at the same horizontal height;

[0035] Step 2.2: Deploy docking positioning marks on the second docking surface of the docked robot, and deploy triangular positioning mark blocks at the center of the second docking surface. The triangular positioning mark blocks are installed on the second side surface of the docked robot through the long right-angled sides. The two sides of the triangular positioning mark blocks are spaced d apart. r Install limit reflective markers at the correct position to ensure that the triangular positioning mark block, limit reflective markers and laser ranging sensor array are at the same level.

[0036] Furthermore, the array ranging data processing method includes:

[0037] Step 3.1: Use the laser ranging sensor array on the docking end robot to collect the distance data d between the two docking surfaces in real time i , and the laser echo intensity I i , i = 1 to n;

[0038] Step 3.2: Use Kalman filter algorithm to remove distance data d i Noise in:

[0039] Step 3.2.1: Establish the state space model of the laser ranging sensor, including

[0040] Equation of state:

[0041] x k =Ax k-1 +w k-1 (twenty three)

[0042] Observation equation:

[0043] z k =Hx k +v k (twenty four)

[0044] Where x k Represents the state vector at time k, that is, the filtered value of the array ranging data x k-1 represents the state vector at time k-1, A is the state transfer matrix, w krepresents the process noise at time k-1, which obeys a Gaussian distribution with a mean of zero and a covariance matrix of Q. k is the observation vector at time k, i.e., the measurement value d of the array ranging data i , H is the observation matrix, v k represents the observation noise at time k, which obeys a Gaussian distribution with a mean of zero and a covariance matrix of R;

[0045] Step 3.2.2: Initialize Kalman filter algorithm parameters, including:

[0046] Initial array ranging data filtering value Set the initial measurement value z0 = d i , i=1~n

[0047] Initial covariance matrix P 0|0 , the state transfer matrix A and the observation matrix H are both set to the identity matrix I;

[0048] Step 3.2.3: Based on the state at the previous moment, predict the state at the current moment, including:

[0049] Status prediction:

[0050]

[0051] Error covariance prediction:

[0052]

[0053] In the formula represents the prior state estimate at time k-1, represents the prior state estimate at time k, P k-1|k-1 represents the prior error covariance at time k-1, P k|k-1 represents the prior error covariance at time k;

[0054] Step 3.2.4: Update the state using the current observation value, including:

[0055] Kalman gain:

[0056] K k =P k|k-1 H T (HP k|k-1 H T +R) -1 =P k|k-1 (P k|k-1 +R) -1 (27)

[0057] Array ranging data filtering value:

[0058]

[0059] In the formula, K k is the Kalman filter gain, is the filtered value of the array ranging data at time k, z k is the measured value of the array ranging data at time k, the laser ranging sensor filtered data sequence Filtered value from array ranging data composition.

[0060] Furthermore, the data sequence quadratic difference method includes:

[0061] Step 4.1: First calculate the difference e between adjacent data in the laser ranging sensor filter data sequence i , Then calculate the adjacent difference e i+1 and e i The difference between them is recorded as the quadratic difference E i , E i =e i+1 -e i , i=1~n-1;

[0062] Step 4.2: Set the difference threshold δ, and identify the laser ranging sensor that is currently aligned with the triangulation mark block according to the parity of the number of laser ranging sensors n:

[0063] If n is an odd number, the quadratic difference exceeding the difference threshold δ is E m-1 , then the mth laser ranging sensor is aligned with the triangulation positioning mark block;

[0064] If n is an even number, the two quadratic differences exceeding the difference threshold δ are recorded as E M-2 and E M-1 , then the Mth and M+1th laser ranging sensors are aligned with the triangulation positioning mark block.

[0065] Furthermore, the relative attitude angle estimation method comprises:

[0066] According to the data series quadratic difference result E i The relative posture angle θ between robots is calculated by the parity of the number of laser ranging sensors n:

[0067] If n is an odd number, the relative attitude angle θ between robots is:

[0068]

[0069] in is the measurement filter data of the adjacent sensor on the left side of the mth laser ranging sensor, is the measurement filter data of the adjacent sensor on the right side of the mth laser ranging sensor, and D is the sensor distance;

[0070] If n is an even number, the relative attitude angle θ between robots is:

[0071]

[0072] in The measurement data of the sensor adjacent to the left side of the Mth laser ranging sensor, is the measurement data of the adjacent sensor on the right side of the M+1th laser ranging sensor, and D is the distance between the laser ranging sensors.

[0073] Furthermore, the laser echo intensity threshold method includes:

[0074] Step 5.1: Set the laser echo intensity threshold I m , the laser echo intensity of a single laser ranging sensor at time t0 is

[0075] Step 5.2: According to the laser echo intensity Determine whether the laser distance sensor array is located between two limit reflective marks. It is determined that the laser distance measuring sensor is located between the two limit reflective marks; otherwise, the laser distance measuring sensor exceeds the limit reflective mark.

[0076] Compared with the prior art, the present invention has at least the following beneficial effects or advantages:

[0077] The present invention provides a multi-machine docking positioning system and a relative posture measurement method, which significantly improves the docking accuracy and reliability. First, the multiple robots are divided into docking end robots and docked end robots, and laser ranging sensor arrays and docking positioning marks are deployed on their respective docking surfaces. Secondly, the present invention designs a precise positioning system, and after obtaining the measurement data of the laser ranging sensor array, the data is filtered and abnormal state judgment is made, which effectively removes the measurement noise and improves the accuracy and effectiveness of the data. It solves the problem that visual sensors are easily affected by environmental interference and mechanical sensors are limited in accuracy. It has significant advantages in the automation, real-time and accuracy of docking tasks, and provides reliable technical guarantees for complex multi-robot collaboration. BRIEF DESCRIPTION OF THE DRAWINGS

[0078] The present invention will be further described in detail below with reference to the accompanying drawings:

[0079] Figure 1 The present invention is a flow chart of a multi-machine docking and positioning system construction and relative posture measurement method.

[0080] Figure 2Flowchart of parameter design method for multi-machine docking positioning system.

[0081] Figure 3 This is the deployment diagram of the multi-machine docking and positioning system.

[0082] Figure 4 Construct a graph for the multi-machine docking positioning coordinate system.

[0083] Figure 5 This is the data processing flow chart of the laser ranging sensor array.

[0084] Figure 6 Schematic diagram of docking relative posture data calculation. DETAILED DESCRIPTION

[0085] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments completed by the inventor based on the technical solution provided by the present invention. However, this implementation case is not intended to limit the present invention. All similar methods and similar variations of the present invention should be included in the protection scope of the present invention.

[0086] like Figure 1 As shown, the method for constructing a multi-machine docking positioning system and measuring relative posture of the present invention specifically includes the following steps:

[0087] First, the multi-machine docking and positioning system is deployed: the design parameters of the laser ranging sensor array and the docking positioning mark are calculated according to the multi-machine docking and positioning system parameter design method; the laser ranging sensor array is deployed on the first docking surface of the docking-end robot according to the multi-machine docking and positioning system deployment method, and the docking positioning mark is deployed on the second docking surface of the docked-end robot, and the docking positioning mark is composed of a triangular positioning mark block and limit reflective marks installed on both sides thereof; the laser ranging sensor is electrically connected to the controller.

[0088] Secondly, the multi-machine docking positioning coordinate system is constructed: the first coordinate system is established with the geometric center of the first docking surface of the docking end robot as the origin, the X-axis perpendicular to the first docking surface inward, and the Y-axis parallel to the first docking surface forward; the second coordinate system is established with the geometric center of the docked end robot as the origin, the X1-axis perpendicular to the second docking surface outward, and the Y1-axis parallel to the second docking surface forward.

[0089] Perform the multi-machine docking positioning initialization guidance again: guide the docking robot and the docked robot to the docking area, and ensure that the docked robot is within the measurement range of the docking robot.

[0090] The laser ranging sensor array data is processed again: the laser ranging sensor array on the docking robot is used to collect the distance intensity data of the docking positioning mark of the docking robot in real time, and the array ranging data processing method is used to process the data noise to obtain the laser ranging sensor filtered data sequence, including the measurement filtered data of all laser ranging sensors.

[0091] Then, the abnormal state of the array ranging data is judged: for the laser ranging sensor filtered data sequence, the relative attitude angle between the robots is calculated according to the data sequence quadratic difference method and the relative attitude angle estimation method. If the relative attitude angle is greater than the working acute angle α of the triangular positioning mark block, it is judged that the attitude deviation is too large, and the process returns to step 3 for reinitialization; if the relative attitude angle is not greater than the working acute angle α of the triangular positioning mark block, the laser echo intensity threshold method is continued to be used to determine whether the laser ranging sensor array is located between the two limit reflective marks. If it is located between the two limit reflective marks, the process goes to step 6; if any laser ranging sensor detects a limit reflective mark, the process returns to step 3 for reinitialization.

[0092] Finally, the relative posture measurement calculation of multi-machine docking is performed: according to the parity of the number n of the laser ranging sensor array, the number K of laser ranging sensors aimed at the triangulation positioning mark block is determined. If n is an odd number, K is 1; if n is an even number, K is 2; the laser ranging sensor currently aligned with the triangulation positioning mark block is identified according to the data sequence quadratic difference method. If n is an odd number, the measurement filter data of the laser ranging sensor m aimed at the triangulation positioning mark block is recorded as The relative posture calculation formula between robots is:

[0093]

[0094] in is the measurement filter data of the adjacent sensor on the left side of the mth laser ranging sensor, is the measurement filter data of the adjacent sensor on the right side of the mth laser ranging sensor, s is the target distance from the geometric center of the docked robot to the second docking surface, α is the angle between the long right-angle side and the hypotenuse of the triangulation mark block, D is the distance between the laser ranging sensors, and l is the length of the long right-angle side of the triangulation mark block;

[0095] If n is an even number, the measurement filter data of the laser ranging sensor M1 aligned with the triangulation positioning mark block is The measurement filter data of the laser ranging sensor aimed at the triangulation positioning marker block is Then the relative posture data between robots is:

[0096]

[0097] in is the measurement filtering data of the adjacent sensor on the left side of the Mth laser ranging sensor, is the measurement filtering data of the adjacent sensor on the right side of the M+1th laser ranging sensor, s is the target distance from the geometric center of the docked robot to the second docking surface, α is the angle between the long right-angled side and the hypotenuse of the triangulated positioning marker block, D is the distance between the laser ranging sensors, l is the length of the long right-angled side of the triangulated positioning marker block, and the obtained relative posture error of the multi-machine docking is output to the processor for subsequent processing.

[0098] like Figure 2 , 3 As shown in the figure, the deployment of the multi-machine docking positioning system mainly includes the following steps:

[0099] First, according to the length dimension M1=60cm of the first docking surface 5 of the docking end robot and the length q=2cm of the laser ranging sensor 2, the docking surface measurement coefficient J=0.25 and the length tolerance t l 3cm, t m The minimum docking distance L is 2cm. d =7cm, and the initial value of the number of laser ranging sensor arrays n (n≥3) is taken as 3.

[0100] Secondly, according to formula (33), the minimum laser ranging sensor array spacing D = 10 cm is solved, and the total length of the laser ranging sensor array is L j =26.

[0101] JM1≤L j =nq+(n-1)D (33)

[0102] Again, design the positioning mark spacing d that satisfies formula (34) r =27cm.

[0103] L j <d r <L j +t m (34)

[0104] Then, the length of the long right-angled side l = 12 cm of the triangular positioning mark block that satisfies formula (35) is designed, and the maximum value 13 and the minimum value 10 of the long right-angled side l are substituted into formula (36) respectively to solve the minimum value 28.3° and the maximum value 35.0° of the acute angle α included by the long right-angled side and the hypotenuse, so α = 30° is selected. The shape of the triangular positioning mark block is a non-isosceles right triangle. Since n is an odd number, it is guaranteed that one and only one laser beam falls on the hypotenuse of the triangular positioning mark block.

[0105] D<l≤D+t l (35)

[0106]

[0107] The parameter design of the multi-machine docking and positioning system has been completed.

[0108] Finally, with the midpoint of the first docking surface 5 of the docking end robot as the center, the laser ranging sensor array 2 is symmetrically arranged at equal intervals according to a predetermined spacing D, ensuring that the laser ranging sensor array 2 is at the same horizontal height, and the docking positioning marks 1 and 3 are deployed on the second docking surface 4 of the docked end robot, and the triangular positioning mark block 1 is deployed at the center of the second docking surface 4. The triangular positioning mark block 1 is installed on the second side surface 4 of the docked end robot through a long right-angled side, and the two sides of the triangular positioning mark block 1 are spaced d apart. r Install the limit reflective mark 3 at the position to ensure that the triangular positioning mark block 1 and the limit reflective mark 3 are at the same level.

[0109] like Figure 3 As shown in the figure, the construction of the multi-machine docking positioning coordinate system mainly includes the following steps:

[0110] The first coordinate system is established with the geometric center of the first docking surface of the docking end robot as the origin, the X-axis perpendicular to the first docking surface inward, and the Y-axis parallel to the first docking surface forward; the second coordinate system is established with the geometric center of the docked end robot as the origin, the X1-axis perpendicular to the second docking surface outward, and the Y1-axis parallel to the second docking surface forward.

[0111] like Figure 4 As shown, the laser ranging sensor array data processing mainly includes the following steps:

[0112] First, the laser ranging sensor array on the docking end robot is used to collect the distance data d between the two docking surfaces in real time. i =[20.1032,11.1604,23.5274], and laser echo intensity I i =[24.887,21.477,18.389].

[0113] Secondly, the Kalman filter algorithm is used to remove the distance data d i The noise in the

[0114] A1: Kalman filter algorithm parameter initialization, the initial array ranging data filtering value Set the initial measurement value z0 = d i , i = 1 ~ n, initial covariance matrix P 0|0 , the state transfer matrix A, and the observation matrix H are all set to the unit matrix I.

[0115] A2: Based on the state and control input at the previous moment, predict the state at the current moment, including:

[0116] Status prediction: Error covariance prediction:

[0117] P kk-1 =P k-1k-1 +Q (37)

[0118] A3: Use the current observation value to update the state and calculate the Kalman gain:

[0119] K k =P kk-1 (P kk-1 +R) -1 (38)

[0120] Array ranging data filtering value:

[0121]

[0122] Output is filtered value of array ranging data The laser ranging sensor filter data sequence composed of i = 1 to n, where

[0123] The determination of abnormal state of array ranging data mainly includes the following steps:

[0124] First, the serial number of the laser ranging sensor falling on the triangulation positioning mark block is obtained according to the data sequence quadratic difference method, and the difference e between adjacent data in the laser ranging sensor filter data sequence is calculated. i , e i =[-8.9263,12.2006], then calculate the difference between the adjacent difference values ​​e, record it as the quadratic difference E, E1=21.1269, then set the difference threshold δ=10, since n is an odd number and E1=21.1269, it is determined that the second laser ranging sensor falls on the triangulation positioning mark block.

[0125] Secondly, the relative attitude angle θ between robots is calculated according to the relative attitude angle estimation method. Since n is an odd number, the relative attitude angle between robots is The relative attitude angle is less than or equal to the working acute angle α of the triangular positioning marker block.

[0126] Finally, the laser echo intensity threshold method is used to determine whether the laser ranging sensor array is located between the two limit reflective marks. First, the laser echo intensity threshold I is set. m =2500, due to the laser echo intensity of the laser ranging sensor I i =[24.887,21.477,18.389] are all less than I m , it is determined that the laser ranging sensor is located between the two limit reflective marks, and the multi-machine docking relative posture measurement calculation is entered.

[0127] like Figure 5 As shown in the figure, the relative posture measurement calculation of multi-machine docking mainly includes the following steps:

[0128] First, determine the number of laser sensor arrays n=3, then K=1, and according to the data sequence quadratic difference method, it can be identified that the laser ranging sensor aimed at the triangulation positioning mark block is sensor No. 2, and the measurement data corresponding to sensor No. 2 is d2=11.1237.

[0129] Secondly, according to the relative posture calculation formula between robots

[0130]

[0131] in,

[0132] It can be calculated that the relative attitude angle error of the multi-machine docking is θ=9.3°, the relative position error of the multi-machine docking is ΔX=20.2812cm, ΔY=4.1521cm. Finally, the obtained relative posture error of the multi-machine docking is output to the processor for subsequent processing.

[0133] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A method for constructing a multi-machine docking positioning system and measuring relative posture, characterized in that: include: Step 1: Deployment of multi-machine docking and positioning system: Calculate the design parameters of the laser ranging sensor array and docking positioning mark according to the multi-machine docking and positioning system parameter design method; According to the multi-machine docking positioning system deployment method, a laser ranging sensor array is deployed on the first docking surface of the docking end robot, and a docking positioning mark is deployed on the second docking surface of the docked end robot, wherein the docking positioning mark is composed of a triangular positioning mark block and limit reflective marks installed on both sides thereof; the laser ranging sensor is electrically connected to the controller; Step 2: Construction of multi-machine docking positioning coordinate system: Take the geometric center of the first docking surface of the docking robot as the origin, the X axis perpendicular to the first docking surface inward, and the Y axis parallel to the first docking surface forward to establish the first coordinate system; take the geometric center of the docked robot as the origin, the X1 axis perpendicular to the second docking surface outward, and the Y1 axis parallel to the second docking surface forward to establish the second coordinate system; Step 3: Initialization guidance of multi-machine docking positioning: guide the docking robot and the docked robot to the docking area, and ensure that the docked robot is within the measurement range of the docking robot; Step 4: Laser ranging sensor array data processing: Use the laser ranging sensor array on the docking end robot to collect the distance intensity data of the docking positioning mark of the docking end robot in real time, use the array ranging data processing method to process the data noise, and obtain the laser ranging sensor filter data sequence, including the measurement filter data of all laser ranging sensors; Step 5: Determination of abnormal state of array ranging data: For the laser ranging sensor filtered data sequence, the relative attitude angle between robots is calculated according to the data sequence quadratic difference method and the relative attitude angle estimation method. If the relative attitude angle is greater than the working acute angle α of the triangular positioning mark block, it is determined that the attitude deviation is too large, and then return to step 3 for reinitialization; if the relative attitude angle is not greater than the working acute angle α of the triangular positioning mark block, then determine whether the laser ranging sensor array is located between the two limit reflective marks according to the laser echo intensity threshold method. If it is located between the two limit reflective marks, enter step 6; if any laser ranging sensor detects a limit reflective mark, then return to step 3 for reinitialization; Step 6: Calculation of relative posture measurement of multi-machine docking: According to the parity of the number n of the laser ranging sensor array, determine the number K of laser ranging sensors aimed at the triangulation positioning mark block. If n is an odd number, K is 1; if n is an even number, K is 2; identify the laser ranging sensor currently aligned with the triangulation positioning mark block according to the data sequence quadratic difference method. If n is an odd number, record the measurement filter data of the laser ranging sensor m aimed at the triangulation positioning mark block as The relative posture calculation formula between robots is: in is the measurement filter data of the adjacent sensor on the left side of the mth laser ranging sensor, is the measurement filtering data of the adjacent sensor on the right side of the mth laser ranging sensor, s is the length of the line from the intersection of the line connecting the geometric centers of the two docking robots and the first docking surface to the geometric center of the docking end robot, α is the angle between the long right-angle side and the hypotenuse of the triangular positioning mark block, D is the distance between the laser ranging sensors, and l is the length of the long right-angle side of the triangular positioning mark block; If n is an even number, the measurement filter data of the laser ranging sensor M1 aligned with the triangulation positioning mark block is The measurement filter data of the laser ranging sensor aimed at the triangulation positioning marker block is Then the relative posture data between robots is: in is the measurement filtering data of the adjacent sensor on the left side of the Mth laser ranging sensor, is the measurement filtering data of the adjacent sensor on the right side of the M+1th laser ranging sensor, s is the length of the line from the intersection of the line connecting the geometric centers of the two docking robots and the first docking surface to the geometric center of the docking end robot, α is the angle between the long right-angled side and the hypotenuse of the triangular positioning mark block, D is the distance between the laser ranging sensors, and l is the length of the long right-angled side of the triangular positioning mark block.

2. According to claim 1, a method for constructing a multi-machine docking positioning system and measuring relative posture, characterized in that: The multi-machine docking positioning system parameter design method comprises: Step 1.1: Parameter initialization: According to the length dimension M1 of the first docking surface of the docking end robot and the length q of the laser ranging sensor, determine the docking surface measurement coefficient J, J∈(0,1), the length tolerance t of the long right-angle side l , positioning mark spacing tolerance t m , minimum butt spacing L d , take the initial value of the number of laser ranging sensor arrays n (n≥3) as 3; Step 1.2: Array spacing design: Solve the minimum laser ranging sensor array spacing D that satisfies equation (3): JM1≤L j =nq+(n-1)D (3) Step 1.3: Design of the spacing between positioning marks: The butt positioning mark is composed of a triangular positioning mark block and its two sides with a distance of d r The installed limit reflective marking composition is designed to meet the positioning marking spacing d of formula (4) r : L j <d r <L j +t m (4) Step 1.4: Design of triangular positioning marker parameters: The triangular positioning marker is in the shape of a non-isosceles right triangle. The length of the right-angle side l of the triangular positioning marker that satisfies formula (5) is designed as follows: D<l≤D+t l (5) The maximum value l of the length of the long right angle side l max and the minimum value l min Substitute into equation (6) to find the minimum value of the acute angle α between the long right-angled side and the hypotenuse: min and the maximum value α max : Determine the minimum value of the acute angle α min and the maximum value α max Whether it satisfies formula (7), 10°≤α＜45° (7) If satisfied, the multi-machine docking positioning system parameter design method is terminated; if not satisfied, n+1 is added and the process returns to step 1.

2.

3. According to claim 1, a method for constructing a multi-machine docking positioning system and measuring relative posture, characterized in that: The multi-machine docking positioning system deployment method comprises: Step 2.1: With the midpoint of the first docking surface of the docking end robot as the center, the laser ranging sensor array is symmetrically arranged at equal intervals according to a predetermined spacing D, ensuring that the laser ranging sensor array is at the same horizontal height; Step 2.2: Deploy docking positioning marks on the second docking surface of the docked robot. First, deploy a triangular positioning mark block at the center of the second docking surface. The triangular positioning mark block is installed on the second side of the docked robot through a long right-angled side. Then, place a triangular positioning mark block on both sides of the triangular positioning mark block at a distance of d. r Install limit reflective markers at the correct position to ensure that the triangular positioning mark block, limit reflective markers and laser ranging sensor array are at the same level.

4. According to claim 1, a method for constructing a multi-machine docking positioning system and measuring relative posture, characterized in that: The array ranging data processing method comprises: Step 3.1: Use the laser ranging sensor array on the docking end robot to collect the distance data d between the two docking surfaces in real time i , and the laser echo intensity I i , i = 1 to n; Step 3.2: Use Kalman filter algorithm to remove distance data d i Noise in: Step 3.2.1: Establish the state space model of the laser ranging sensor, including: Equation of state: x k =Ax k-1 +w k-1 (8) Observation equation: z k =Hx k +v k (9) Where x k Represents the state vector at time k, that is, the filtered value of the array ranging data x k-1 represents the state vector at time k-1, A is the state transfer matrix, w k-1 represents the process noise at time k-1, which obeys a Gaussian distribution with a mean of zero and a covariance matrix of Q. k is the observation vector at time k, i.e., the measurement value d of the array ranging data i , H is the observation matrix, v k represents the observation noise at time k, which obeys a Gaussian distribution with a mean of zero and a covariance matrix of R; Step 3.2.2: Initialize Kalman filter algorithm parameters, including: Initial array ranging data filtering value Set the initial measurement value z0 = d i , i=1~n Initial covariance matrix P 0|0 , the state transfer matrix A and the observation matrix H are both set to the identity matrix I; Step 3.2.3: Based on the state at the previous moment, predict the state at the current moment, including: Status prediction: Error covariance prediction: P k|k-1 =AP k-1|k-1 From T +Q=P k-1|k-1 +Q (11) In the formula represents the prior state estimate at time k-1, represents the prior state estimate at time k, P k-1|k-1 represents the prior error covariance at time k-1, P k|k-1 represents the prior error covariance at time k; Step 3.2.4: Update the state using the current observation value, including: Kalman gain: K k =P k|k-1 H T (HP k|k-1 H T +R) -1 =P k|k-1 (P k|k-1 +R) -1 (12) Array ranging data filtering value: In the formula, K k is the Kalman filter gain, is the filtered value of the array ranging data at time k, z k is the measured value of the array ranging data at time k, the laser ranging sensor filtered data sequence Filtered value from array ranging data composition.

5. According to claim 1, a method for constructing a multi-machine docking positioning system and measuring relative posture, characterized in that: The data sequence quadratic difference method comprises: Step 4.1: First calculate the difference e between adjacent data in the laser ranging sensor filter data sequence i , Then calculate the adjacent difference e i+1 and e i The difference between them is recorded as the quadratic difference E i , E i =e i+1 -e i , i=1~n-1; Step 4.2: Set the difference threshold δ, and identify the laser ranging sensor that is currently aligned with the triangulation mark block according to the parity of the number of laser ranging sensors n: If n is an odd number, the quadratic difference exceeding the difference threshold δ is E m-1 , then the mth laser ranging sensor is aligned with the triangulation positioning mark block; If n is an even number, the two quadratic differences exceeding the difference threshold δ are recorded as E M-2 and E M-1 , then the Mth and M+1th laser ranging sensors are aligned with the triangulation positioning mark block.

6. According to claim 1, a method for constructing a multi-machine docking positioning system and measuring relative posture, characterized in that: The relative attitude angle estimation method comprises: According to the data series quadratic difference result E i The relative posture angle θ between robots is calculated by the parity of the number of laser ranging sensors n: If n is an odd number, the relative attitude angle θ between robots is: in is the measurement filter data of the adjacent sensor on the left side of the mth laser ranging sensor, is the measurement filter data of the adjacent sensor on the right side of the mth laser ranging sensor, and D is the sensor distance; If n is an even number, the relative attitude angle θ between robots is: in The measurement data of the sensor adjacent to the left side of the Mth laser ranging sensor, is the measurement data of the adjacent sensor on the right side of the M+1th laser ranging sensor, and D is the distance between the laser ranging sensors.

7. According to claim 1, a method for constructing a multi-machine docking positioning system and measuring relative posture, characterized in that: The laser echo intensity threshold method comprises: Step 5.1: Set the laser echo intensity threshold I m , the laser echo intensity of a single laser ranging sensor at time t0 is Step 5.2: According to the laser echo intensity Determine whether the laser distance sensor array is located between the two limit reflective marks. It is determined that the laser distance measuring sensor is located between the two limit reflective marks; otherwise, the laser distance measuring sensor exceeds the limit reflective mark.

Images