A navigation method for layout robot based on total station

Abstract

The present application belongs to the field of automatic navigation technology, and discloses a layout robot navigation method based on a total station, wherein the layout robot's layout trolley is equipped with a mechanical arm adjustment device for travel, a prism is arranged on the top of the mechanical arm adjustment device, and a seal and ink pressing mechanism is arranged at the bottom of the mechanical arm adjustment device, and the total station is used to track the prism and feed back the real-time coordinates of the prism. The layout trolley performs real-time deviation correction according to the coordinates during travel to improve the efficiency of coarse positioning, and then uses the third degree of freedom adjustment component and the fourth degree of freedom adjustment component to adjust the prism to a state of being vertical to the layout ground, and then uses the first degree of freedom adjustment component and the second degree of freedom adjustment component in combination with the target layout point precise positioning algorithm to adjust the seal and ink pressing mechanism to be directly above the layout point to achieve precise layout.

Description

Technical Field

[0001] The present application belongs to the field of automatic navigation technology, and specifically relates to a layout robot navigation method based on a total station. Background Art

[0002] Currently, the Global Navigation Satellite System (GNSS) comprises one or more satellite constellations and the augmentation systems required to support specific operations. The International Committee on Global Navigation Satellite Systems (ICG) lists the four major global satellite navigation system providers: GPS (the United States' Global Positioning System, currently the most widely used navigation and positioning technology); BDS (China's BeiDou Navigation Satellite System, which has seen rapid development and widespread adoption in recent years); GLONASS (Russia's Global Navigation Satellite System); and Galileo (Europe's Global Navigation Satellite System). These systems use satellite signals to achieve precise positioning of targets, offering all-weather, global coverage.

[0003] Inertial Navigation System (INS)

[0004] An inertial navigation system, comprised of gyroscopes, accelerometers, and software, measures a moving object's angular velocity and acceleration and integrates this data over time to determine its velocity, position, and attitude. While this system offers significant autonomy, it can also generate cumulative errors over time.

[0005] Visual navigation

[0006] Visual navigation primarily uses visual sensors (such as cameras) to capture environmental images and uses image processing algorithms to identify lane lines and road signs, thereby achieving vehicle positioning. This technology has the advantages of low cost and rich information, but it is not very effective in complex environments (such as strong sunlight, backlight, and darkness).

[0007] LiDAR navigation

[0008] LiDAR navigation uses a LiDAR sensor to collect 3D point cloud data of the environment and match it with pre-collected map data to accurately locate the vehicle. This technology has the advantages of high detection accuracy and long detection range, but the equipment cost is relatively high.

[0009] Combined navigation

[0010] The current combined navigation can only achieve coarse positioning, at the centimeter level, and has strict requirements on terrain and usage conditions.

[0011] The above navigation technologies have low accuracy, and the highest accuracy can only reach the centimeter level. It is impossible to achieve accurate positioning at the millimeter level and cannot be applied to layout operations with high precision requirements. Summary of the Invention

[0012] The purpose of this application is to solve the problems existing in the prior art and to provide a total station-based layout robot navigation method.

[0013] In order to solve the technical problem, the technical solution of the present application is: a total station-based layout robot navigation method, comprising the following steps:

[0014] Step 1: According to the layout line type file, import the layout point coordinate file into the control component of the layout robot;

[0015] Step 2: Use the total station to track the prism of the layout robot and feedback the real-time coordinates of the prism;

[0016] Step 3: The control component automatically navigates the route of the stakeout robot's trolley based on the line connecting the two stakeout points. The trolley makes real-time deviation corrections during driving and stops near the stakeout point to achieve coarse positioning.

[0017] The real-time deviation correction includes:

[0018] Step 3-1: Calculate the correction value and direction based on the known starting point P1, destination point P2 and current point P3;

[0019] Step 3-2: Based on the deviation correction value and direction, use the differential steering of the layout car to achieve deviation correction;

[0020] Step 4: After the layout trolley stops steadily, use the total station to cooperate with the third degree of freedom adjustment component and the fourth degree of freedom adjustment component of the layout robot's mechanical arm adjustment device to perform two precise positionings, adjust the prism to a state perpendicular to the layout ground, and ensure that the bottom surface of the layout robot's seal and ink pressing mechanism is parallel to the layout ground;

[0021] Step 5: Use the target stakeout point precise positioning algorithm to adjust the first degree of freedom adjustment component and the second degree of freedom adjustment component of the robotic arm adjustment device of the stakeout robot, perform two more precise positionings, and adjust the seal and ink pressing mechanism to be directly above the stakeout point; observe the prism coordinates and compare them with the target stakeout point coordinates to see if the error is within the range; if yes, execute the press-stamp stakeout; if not, repeat step 5;

[0022] Step 6: Repeat steps 3 to 5 until all stakeout points are staked out with stamps.

[0023] Preferably, the layout robot includes a layout trolley, a robotic arm adjustment device, a seal inking mechanism, a prism and a control component. The layout trolley, the robotic arm adjustment device and the seal inking mechanism are electrically connected to the control component respectively. The robotic arm adjustment device includes a first degree of freedom adjustment component, a second degree of freedom adjustment component, a third degree of freedom adjustment component, a fourth degree of freedom adjustment component and a fifth degree of freedom adjustment component connected in sequence. The fifth degree of freedom adjustment component is mounted inside the fourth degree of freedom adjustment component. The bottom of the fifth degree of freedom adjustment component is connected to the seal inking mechanism. A prism is provided on the top of the fourth degree of freedom adjustment component. The axis of the prism is coaxial with the axis of the seal inking mechanism. Wheels are installed on two opposite sides of the cover of the layout trolley, and the power component drives the two wheels on the same side to rotate synchronously through the linkage component.

[0024] Preferably, the step 3-1 is specifically as follows:

[0025] Given the starting point P1 (x1, y1), the destination point P2 (x2, y2), and the current point P3 (x3, y3);

[0026] Step 3-1-1: The general equation of the straight line P1P2 is:

[0027] y=kx+b

[0028] in:

[0029] k=(y2-y1) / (x2-x1);

[0030] b=y1-x1*k or b=y2-x2*k;

[0031] Step 3-1-2: The distance S from the current point P3 (x3, y3) to the line P1P2 is calculated as follows:

[0032]

[0033] Step 3-1-3: Since the movement direction of the layout car is consistent with the direction of the straight line P1P2, the correction value and direction of P1→P2 are defined according to k and S.

[0034] Preferably, when k>0, S<0, the deviation is corrected to the right, S>0, the deviation is corrected to the left, and the deviation correction value is A;

[0035] When k<0, S<0, the deviation is corrected to the left; when S>0, the deviation is corrected to the right, and the correction value is A.

[0036] Preferably, the step 3-2 is specifically as follows:

[0037] Step 3-2-1: Calculate the speed difference between the wheels on both sides of the trolley based on the wheelbase D of the trolley and the current speed v0 of the trolley;

[0038] Step 3-2-2: Calculate the speed of the left wheel and the speed of the right wheel based on the speed difference of the wheels on both sides;

[0039] Step 3-2-3: Determine the speed of the left and right wheels based on the correction value and direction, obtain the speed after correction, and adjust the speed in step 3-2-2 to the speed after correction.

[0040] Preferably, the step 3-2-1 is specifically as follows: given the wheelbase D of the lofting trolley and the current speed v0 of the lofting trolley, the speed difference Δv=η*v0, where η is the wheel speed difference.

[0041] Preferably, the speed of the left wheel vl=v0-Δv=v0(1-η);

[0042] The speed of the right wheel vr=v0+Δv=v0(1+η).

[0043] Preferably, step 4 is specifically as follows: using a total station to track the prism of the layout robot, when the axis of the prism is not perpendicular to the layout ground, transmitting a signal to the control component of the layout robot, the control component adjusts the third degree of freedom adjustment component and the fourth degree of freedom adjustment component of the robotic arm adjustment device, adjusts the third degree of freedom adjustment component to an angle of γ, and adjusts the fourth degree of freedom adjustment component to an angle of β, so that the axis of the prism is adjusted to a vertical state with the layout ground, ensuring that the bottom surface of the seal and ink pressing mechanism of the layout robot is parallel to the layout ground.

[0044] Preferably, the target stakeout point precise positioning algorithm used in step 5 includes:

[0045] Step 5-1: After adjusting the bottom surface of the seal inking mechanism to be parallel to the layout ground, arbitrarily select the first measuring point P5 (x5, y5);

[0046] Step 5-2: The lofting trolley 1 remains stationary, and the first degree of freedom adjustment component 2-1 of the manipulator adjustment device 2 swings to the left by an angle α to obtain the second measurement point P6 (x6, y6);

[0047] Step 5-3: Find the equation of the line P5P6:

[0048] y=k5x+b5

[0049] in:

[0050] k5=(y6-x6) / (x5-x5)=tan(θ1);

[0051] b5=y5-x5*k5, or b5=y6-x6*k5;

[0052] θ1=arctan(k5);

[0053] Step 5-4: Rotate the line P5P6 to the left around point P5 by an angle (90°-α / 2) to obtain the equation of the line P5P4. The line P5P4 passes through point P5 (x5, y5):

[0054] y=k6(x-x5)+y5=tan(θ1+90°-α / 2)(x-x5)+y5

[0055] in:

[0056] k6=tan(θ2)=tan(θ1+90°-α / 2);

[0057] Step 5-5: Obtain the coordinates of point P4 (x4, y4) based on the equation of straight line P5P4 and the arm length R of the robot adjustment device 2;

[0058] y4=y5-Rsin(arctank6)

[0059] x4=x5-Rcos(arctank6)

[0060] Step 5-6: Point P2 (x2, y2) is the known destination point. Find the angle Δβ between lines P4P6 and P4P2:

[0061] β1=arctan((y6-y4) / (x6-x4))

[0062] β2=arctan((y2-y4) / (x2-x4))

[0063] Δβ=β1-β2

[0064] The first degree of freedom adjustment component (2-1) of the robot arm adjustment device (2) swings rightward at an angle Δβ;

[0065] Step 5-7: Calculate the elongation ΔR of the second degree of freedom adjustment component 2-2:

[0066] R1=P4P6;

[0067] R2=root((y2-y4) 2 +(x2-x4) 2 )

[0068] ΔR=R2-R1

[0069] Complete two precise positionings to meet the layout requirements.

[0070] Compared with the prior art, the advantages of this application are:

[0071] (1) The present application discloses a navigation method for a layout robot based on a total station. The layout robot's layout trolley is equipped with a mechanical arm adjustment device for travel. A prism is provided on the top of the mechanical arm adjustment device, and a seal and ink pressing mechanism is provided on the bottom of the mechanical arm adjustment device. The prism is tracked by the total station and the real-time coordinates of the prism are fed back. The layout trolley performs real-time deviation correction according to the coordinates during travel to improve the efficiency of coarse positioning. Then, the prism is adjusted to a state vertical to the layout ground by using the third degree of freedom adjustment component and the fourth degree of freedom adjustment component. Then, the seal and ink pressing mechanism is adjusted to the position directly above the layout point by using the first degree of freedom adjustment component and the second degree of freedom adjustment component in combination with the target layout point precise positioning algorithm to achieve precise layout.

[0072] (2) This application calculates the deviation correction value and direction based on the adjacent stakeout points and the current stakeout point, and performs differential steering on the stakeout vehicle based on the deviation correction value and direction to achieve real-time deviation correction. This application's stakeout vehicle has a wide working range, and the working range is plus or minus 30 degrees in front of the stakeout vehicle, and there is no dead zone;

[0073] (3) This application has developed a precise positioning algorithm for the target point of the layout. After the layout trolley stops, the first measurement point P5 is arbitrarily selected, and then the second measurement point P6 is obtained by swinging at an angle of α to obtain the equation of the straight line P5P6. The straight line P5P6 is then rotated 90°-α / 2 to the left around the point P5 to obtain the equation of the straight line P5P4. Then, based on the equation of the straight line P5P4 and the arm length R of the robot arm adjustment device, the point coordinate P4, that is, the rotation origin, is obtained. Since the target point P2 is known, the angle Δβ between the straight lines P4P6 and P4P2, that is, the swing angle of the first degree of freedom adjustment component, and the elongation ΔR of the second degree of freedom adjustment component can be calculated. This process can quickly calculate the head direction of the layout trolley and the rotation origin, thereby obtaining the swing angle and elongation, with high calculation accuracy and fast speed.

[0074] (4) The navigation method of this application is simple to operate. The layout data is imported using a table and sent to the layout vehicle via WIFI. Remote communication can be achieved within a range of up to 3 kilometers. BRIEF DESCRIPTION OF THE DRAWINGS

[0075] Figure 1 , a schematic diagram of an application of a total station-based layout robot navigation method of the present application;

[0076] Figure 2 , a schematic diagram of the structure of the layout robot of this application;

[0077] Figure 3 , Schematic diagram of the real-time correction algorithm of this application;

[0078] Figure 4 , Schematic diagram of the real-time correction algorithm of this application;

[0079] Figure 5 , Schematic diagram of the precise positioning algorithm of the target stakeout point of this application;

[0080] Figure 6 , Schematic diagram of the precise positioning algorithm for the target stakeout points of this application.

[0081] Description of reference numerals:

[0082] 1. Stakeout trolley, 2. Robotic arm adjustment device, 3. Seal ink pressing mechanism, 4. Prism, 5. Total station;

[0083] 1-1, cover, 1-2, wheel, 1-3, power assembly, 1-4, linkage assembly;

[0084] 2-1, first degree of freedom adjustment component, 2-2, second degree of freedom adjustment component, 2-3, third degree of freedom adjustment component, 2-4, fourth degree of freedom adjustment component, 2-5, fifth degree of freedom adjustment component. DETAILED DESCRIPTION

[0085] The following describes the specific implementation of this application in conjunction with examples:

[0086] It should be noted that the structures, proportions, sizes, etc. shown in this specification are only used to match the contents disclosed in the specification for people familiar with this technology to understand and read, and are not used to limit the conditions for the implementation of this application. Any structural modification, change in proportional relationship or adjustment of size should still fall within the scope of the technical content disclosed in this application without affecting the efficacy and purpose that can be achieved by this application.

[0087] like Figure 1 、 2 As shown, the present application discloses a total station-based layout robot navigation method, comprising the following steps:

[0088] Step 1: According to the layout line type file, import the layout point coordinate file into the control component of the layout robot;

[0089] Step 2: Use the total station 5 to track the prism 4 of the layout robot and feed back the real-time coordinates of the prism 4;

[0090] Step 3: The control component automatically navigates the route of the stakeout robot's stakeout trolley 1 based on the line connecting the two stakeout points. The stakeout trolley 1 performs real-time deviation correction during driving and stops near the stakeout point to achieve coarse positioning.

[0091] The real-time deviation correction includes:

[0092] Step 3-1: Calculate the correction value and direction based on the known starting point P1, destination point P2 and current point P3;

[0093] Step 3-2: Based on the deviation correction value and direction, use the differential steering of the layout vehicle 1 to achieve deviation correction;

[0094] Step 4: After the layout trolley 1 stops steadily, use the total station 5 to cooperate with the third degree of freedom adjustment component 2-3 and the fourth degree of freedom adjustment component 2-4 of the mechanical arm adjustment device 2 of the layout robot to perform two precise positionings, adjust the prism 4 to a state perpendicular to the layout ground, and ensure that the bottom surface of the seal pressing mechanism 3 of the layout robot is parallel to the layout ground;

[0095] Step 5: Use the target stakeout point precise positioning algorithm to adjust the first degree of freedom adjustment component 2-1 and the second degree of freedom adjustment component 2-2 of the robotic arm adjustment device 2 of the stakeout robot, perform two more precise positionings, and adjust the seal and ink pressing mechanism 3 to be directly above the stakeout point; observe the coordinates of the prism 4 and compare them with the coordinates of the target stakeout point to see if the error is within the range; if yes, execute the press-stamp stakeout; if not, repeat step 5;

[0096] Step 6: Repeat steps 3 to 5 until all stakeout points are staked out with stamps.

[0097] like Figure 2 As shown, preferably, the layout robot includes a layout trolley 1, a robotic arm adjustment device 2, a seal inking mechanism 3, a prism 4 and a control component. The layout trolley 1, the robotic arm adjustment device 2 and the seal inking mechanism 3 are electrically connected to the control component respectively. The robotic arm adjustment device 2 includes a first degree of freedom adjustment component 2-1, a second degree of freedom adjustment component 2-2, a third degree of freedom adjustment component 2-3, a fourth degree of freedom adjustment component 2-4 and a fifth degree of freedom adjustment component 2-5 connected in sequence. The fifth degree of freedom adjustment component 2-5 is mounted inside the fourth degree of freedom adjustment component 2-4. The bottom of the fifth degree of freedom adjustment component 2-5 is connected to the seal inking mechanism 3. The top of the fourth degree of freedom adjustment component 2-4 is provided with a prism 4, and the axis of the prism 4 is coaxial with the axis of the seal inking mechanism 3; the cover 1-1 of the layout trolley 1 is respectively provided with wheels 1-2 on two opposite sides, and the power component 1-3 drives the two wheels 1-2 on the same side to rotate synchronously through the linkage component 1-4.

[0098] like Figure 3 As shown, preferably, the step 3-1 is specifically as follows:

[0099] Given the starting point P1 (x1, y1), the destination point P2 (x2, y2), and the current point P3 (x3, y3);

[0100] Step 3-1-1: The general equation of the straight line P1P2 is:

[0101] y=kx+b

[0102] in:

[0103] k=(y2-y1) / (x2-x1);

[0104] b=y1-x1*k or b=y2-x2*k;

[0105] Step 3-1-2: The distance S from the current point P3 (x3, y3) to the line P1P2 is calculated as follows:

[0106]

[0107] Step 3-1-3: Since the movement direction of the layout car 1 is consistent with the direction of the straight line P1P2, the correction value and direction of P1→P2 are defined according to k and S.

[0108] Preferably, when k>0, S<0, the deviation is corrected to the right, S>0, the deviation is corrected to the left, and the deviation correction value is A;

[0109] When k<0, S<0, the deviation is corrected to the left; when S>0, the deviation is corrected to the right, and the correction value is A.

[0110] The deviation correction value is a set value, which is generally set at plus or minus 0.3 km / h.

[0111] like Figure 4 As shown, preferably, the step 3-2 is specifically as follows:

[0112] Step 3-2-1: Calculate the speed difference between the wheels 1-2 on both sides of the lofting trolley 1 based on the wheelbase D of the lofting trolley 1 and the current speed v0 of the lofting trolley 1;

[0113] Step 3-2-2: Calculate the speed of the left wheel 1-2 and the speed of the right wheel 1-2 according to the speed difference between the wheels 1-2 on both sides;

[0114] Step 3-2-3: Determine the speed of the left wheel 1-2 and the right wheel 1-2 according to the correction value and direction, obtain the speed after correction, and adjust the speed of step 3-2-2 to the speed after correction.

[0115] Preferably, the step 3-2-1 is specifically as follows: given the wheelbase D of the lofting trolley 1 and the current speed v0 of the lofting trolley 1 , the speed difference Δv=η*v0, where η is the wheel speed difference.

[0116] Preferably,

[0117] The speed of the left wheel 1-2 is vl=v0-Δv=v0(1-η);

[0118] The speed of the right wheel 1-2 is vr=v0+Δv=v0(1+η).

[0119] The trajectories of the left and right wheels in the same time Δt are two concentric circles:

[0120] Left wheel track circumference:

[0121] Cl=2πR1

[0122] Right wheel track circumference:

[0123] C2=2πRr

[0124] Δt=C1 / vl=C2 / vr

[0125] 2πR1 / (v0(1-η))=2πRr / v0(1+η)

[0126] Radius of two circles: Rr=R1+D

[0127] R1 / (v0(1-η))=(R1+D) / v0(1+η)

[0128] R1=D / (2(1 / η-1))

[0129] Preferably, step 4 is specifically as follows: using the total station 5 to track the prism 4 of the layout robot, when the axis of the prism 4 is not perpendicular to the layout ground, transmitting a signal to the control component of the layout robot, the control component adjusts the third degree of freedom adjustment component 2-3 and the fourth degree of freedom adjustment component 2-4 of the robotic arm adjustment device 2, adjusts the third degree of freedom adjustment component 2-3 to the γ angle, and adjusts the fourth degree of freedom adjustment component 2-4 to the β angle, so that the axis of the prism 4 is adjusted to a vertical state with the layout ground, ensuring that the bottom surface of the seal pressing mechanism 3 of the layout robot is parallel to the layout ground.

[0130] like Figure 5 、 6 As shown, preferably, the target stakeout point precise positioning algorithm in step 5 includes:

[0131] Step 5-1: After adjusting the bottom surface of the seal inking mechanism 3 to be parallel to the lofted ground, arbitrarily select the first measuring point P5 (x5, y5);

[0132] Step 5-2: The lofting trolley 1 remains stationary, and the first degree of freedom adjustment component 2-1 of the manipulator adjustment device 2 swings to the left by an angle α to obtain the second measurement point P6 (x6, y6);

[0133] Step 5-3: Find the equation of the line P5P6:

[0134] y=k5x+b5

[0135] in:

[0136] k5=(y6-x6) / (x5-x5)=tan(θ1);

[0137] b5=y5-x5*k5, or b5=y6-x6*k5;

[0138] θ1=arctan(k5);

[0139] Step 5-4: Rotate the line P5P6 to the left around point P5 by an angle (90°-α / 2) to obtain the equation of the line P5P4. The line P5P4 passes through point P5 (x5, y5):

[0140] y=k6(x-x5)+y5=tan(θ1+90°-α / 2)(x-x5)+y5

[0141] in:

[0142] k6=tan(θ2)=tan(θ1+90°-α / 2);

[0143] Step 5-5: Obtain the coordinates of point P4 (x4, y4) based on the equation of straight line P5P4 and the arm length R of the robot adjustment device 2;

[0144] y4=y5-Rsin(arctank6)

[0145] x4=x5-Rcos(arctank6)

[0146] Step 5-6: Point P2 (x2, y2) is the known destination point. Find the angle Δβ between lines P4P6 and P4P2:

[0147] β1=arctan((y6-y4) / (x6-x4))

[0148] β2=arctan((y2-y4) / (x2-x4))

[0149] Δβ=β1-β2

[0150] The first degree of freedom adjustment component (2-1) of the robot arm adjustment device (2) swings rightward at an angle Δβ;

[0151] Step 5-7: Calculate the elongation ΔR of the second degree of freedom adjustment component 2-2:

[0152] R1=P4P6;

[0153] R2=root((y2-y4) 2 +(x2-x4) 2 )

[0154] ΔR=R2-R1

[0155] Complete two precise positionings to meet the layout requirements.

[0156] Example 1

[0157] This embodiment uses a total station-based layout robot navigation method of the present application to perform navigation layout, including the following steps:

[0158] Step 1: According to the layout line type file, import the layout point coordinate file into the control component of the layout robot;

[0159] Step 2: Use the total station 5 to track the prism 4 of the layout robot and feed back the real-time coordinates of the prism 4;

[0160] Step 3: The control component automatically navigates the route of the stakeout robot's stakeout trolley 1 based on the line connecting the two stakeout points. The stakeout trolley 1 performs real-time deviation correction during driving and stops near the stakeout point to achieve coarse positioning.

[0161] like Figure 3 、 4 As shown, the real-time deviation correction includes:

[0162] Step 3-1: Calculate the correction value and direction based on the known starting point P1, destination point P2 and current point P3;

[0163] Given the starting point P1 (x1, y1), the destination point P2 (x2, y2), and the current point P3 (x3, y3);

[0164] Step 3-1-1: The general equation of the straight line P1P2 is:

[0165] y=kx+b

[0166] in:

[0167] k=(y2-y1) / (x2-x1);

[0168] b=y1-x1*k or b=y2-x2*k;

[0169] Step 3-1-2: The distance S from the current point P3 (x3, y3) to the line P1P2 is calculated as follows:

[0170]

[0171] Step 3-1-3: Since the movement direction of the lofting trolley 1 is consistent with the direction of the straight line P1P2, the correction value and direction of P1→P2 are defined according to k and S;

[0172] Step 3-2: Based on the deviation correction value and direction, use the differential steering of the layout vehicle 1 to achieve deviation correction;

[0173] Step 3-2-1: Calculate the speed difference between the wheels 1-2 on both sides of the lofting trolley 1 based on the wheelbase D of the lofting trolley 1 and the current speed v0 of the lofting trolley 1;

[0174] Step 3-2-2: Calculate the speed of the left wheel 1-2 and the speed of the right wheel 1-2 according to the speed difference between the wheels 1-2 on both sides;

[0175] Step 3-2-3: Determine the speed of the left wheel 1-2 and the right wheel 1-2 according to the correction value and direction, obtain the speed after correction, and adjust the speed of step 3-2-2 to the speed after correction;

[0176] Step 4: After the layout trolley 1 stops steadily, use the total station 5 to cooperate with the third degree of freedom adjustment component 2-3 and the fourth degree of freedom adjustment component 2-4 of the mechanical arm adjustment device 2 of the layout robot to perform two precise positionings, adjust the prism 4 to a state perpendicular to the layout ground, and ensure that the bottom surface of the seal pressing mechanism 3 of the layout robot is parallel to the layout ground;

[0177] The prism 4 of the layout robot is tracked by the total station 5. When the axis of the prism 4 is not perpendicular to the layout ground, a signal is transmitted to the control component of the layout robot. The control component adjusts the third degree of freedom adjustment component 2-3 and the fourth degree of freedom adjustment component 2-4 of the mechanical arm adjustment device 2. The third degree of freedom adjustment component 2-3 is adjusted to an angle γ, and the fourth degree of freedom adjustment component 2-4 is adjusted to an angle β. This adjusts the axis of the prism 4 to a perpendicular state with respect to the layout ground, thereby ensuring that the bottom surface of the seal pressing mechanism 3 of the layout robot is parallel to the layout ground.

[0178] Step 5: Use the target stakeout point precise positioning algorithm to adjust the first degree of freedom adjustment component 2-1 and the second degree of freedom adjustment component 2-2 of the robotic arm adjustment device 2 of the stakeout robot, perform two more precise positionings, and adjust the seal and ink pressing mechanism 3 to be directly above the stakeout point; observe the coordinates of the prism 4 and compare them with the coordinates of the target stakeout point to see if the error is within the range; if yes, execute the press-stamp stakeout; if not, repeat step 5;

[0179] like Figure 5 、 6 As shown, the target stakeout point precise positioning algorithm in step 5 includes:

[0180] Step 5-1: After adjusting the bottom surface of the seal inking mechanism 3 to be parallel to the lofted ground, arbitrarily select the first measuring point P5 (x5, y5);

[0181] Step 5-2: The lofting trolley 1 remains stationary, and the first degree of freedom adjustment component 2-1 of the manipulator adjustment device 2 swings to the left by an angle α to obtain the second measurement point P6 (x6, y6);

[0182] Step 5-3: Find the equation of the line P5P6:

[0183] y=k5x+b5

[0184] in:

[0185] k5=(y6-x6) / (x5-x5)=tan(θ1);

[0186] b5=y5-x5*k5, or b5=y6-x6*k5;

[0187] θ1=arctan(k5);

[0188] Step 5-4: Rotate the line P5P6 to the left around point P5 by an angle (90°-α / 2) to obtain the equation of the line P5P4. The line P5P4 passes through point P5 (x5, y5):

[0189] y=k6(x-x5)+y5=tan(θ1+90°-α / 2)(x-x5)+y5

[0190] in:

[0191] k6=tan(θ2)=tan(θ1+90°-α / 2)

[0192] Step 5-5: Obtain the coordinates of point P4 (x4, y4) based on the equation of straight line P5P4 and the arm length R of the robot adjustment device 2;

[0193] y4=y5-Rsin(arctank6)

[0194] x4=x5-Rcos(arctank6)

[0195] Step 5-6: Point P2 (x2, y2) is the known destination point. Find the angle Δβ between lines P4P6 and P4P2:

[0196] β1=arctan((y6-y4) / (x6-x4))

[0197] β2=arctan((y2-y4) / (x2-x4))

[0198] The rightward swing angle of the first degree of freedom adjustment component 2-1 of the robot arm adjustment device 2 is:

[0199] Δβ=β1-β2

[0200] Step 5-7: Calculate the elongation ΔR of the second degree of freedom adjustment component 2-2:

[0201] R1=P4P6;

[0202] R2=root((y2-y4)2 +(x2-x4) 2 )

[0203] ΔR=R2-R1

[0204] Complete two precise positionings to meet the layout requirements.

[0205] Step 6: Repeat steps 3 to 5 until all stakeout points are staked out with stamps.

[0206] Comparative Example

[0207] Inertial navigation relies on gyroscope accuracy to calculate direction and speed, and thus distance. However, its disadvantage is that the gyroscope has cumulative errors, which increase over time. Each stakeout operation requires the vehicle's coordinates and direction to be entered into the computer for navigation. This leads to long preparation times and poor accuracy, making it suitable only for short-distance stakeouts with low precision requirements.

[0208] Application Examples

[0209] Ten points were staked out on a straight line with a spacing of 5 meters. Both inertial navigation and the navigation method of this application were used. The experimental data are shown in the following table:

[0210]

[0211] It can be seen from the above table that the accuracy of inertial navigation increases exponentially with the increase of the stakeout distance, while the total station navigation method is not affected by the stakeout distance and the accuracy is always maintained within 2 mm, which greatly improves the stakeout accuracy.

[0212] The navigation method of this application can achieve millimeter-level high-precision positioning, is simple to use, and has no stringent requirements. It overcomes the problem of low accuracy of visual navigation, the problem that lidar navigation cannot obtain real-time position, and the problem that hybrid navigation can only achieve coarse positioning (centimeter level) and has stringent requirements on terrain and usage conditions.

[0213] The present application discloses a layout robot navigation method based on a total station, wherein the layout robot's layout trolley is equipped with a mechanical arm adjustment device for travel, a prism is arranged on the top of the mechanical arm adjustment device, and a seal and ink pressing mechanism is arranged at the bottom of the mechanical arm adjustment device, the total station is used to track the prism, and the real-time coordinates of the prism are fed back, and the layout trolley performs real-time deviation correction according to the coordinates during travel to improve the efficiency of coarse positioning, and then uses the third degree of freedom adjustment component and the fourth degree of freedom adjustment component to adjust the prism to a state of being vertical to the layout ground, and then uses the first degree of freedom adjustment component and the second degree of freedom adjustment component in combination with the target layout point precise positioning algorithm to adjust the seal and ink pressing mechanism to be directly above the layout point to achieve precise layout.

[0214] This application calculates the correction value and direction based on the adjacent stakeout points and the current stakeout point, and performs differential steering on the stakeout vehicle based on the correction value and direction to achieve real-time correction. The stakeout vehicle of this application has a wide working range, and the working range is plus or minus 30 degrees in front of the stakeout vehicle, and there is no dead zone.

[0215] The present application develops a precise positioning algorithm for the target layout point. After the layout trolley stops, the first measuring point P5 is arbitrarily selected, and then the second measuring point P6 is obtained by swinging at an angle α to obtain the equation of the straight line P5P6. The straight line P5P6 is then rotated 90°-α / 2 to the left around the point P5 to obtain the equation of the straight line P5P4. Based on the equation of the straight line P5P4 and the arm length R of the robotic arm adjustment device, the point coordinates P4, that is, the rotation origin, are obtained. Since the target point P2 is known, the angle Δβ between the straight lines P4P6 and P4P2, that is, the swing angle of the first degree of freedom adjustment component, and the elongation ΔR of the second degree of freedom adjustment component can be calculated. This process can quickly calculate the head direction of the layout trolley and the rotation origin, thereby obtaining the swing angle and elongation, with high calculation accuracy and fast speed.

[0216] The navigation method of this application is simple to operate. The layout data is imported using a table and sent to the layout vehicle via Wi-Fi. Remote communication within a range of up to 3 to 5 kilometers can be achieved.

[0217] In addition, the layout robot described in this application can also cooperate with GNSS to realize rough layout lines.

[0218] The preferred embodiments of the present application are described in detail above, but the present application is not limited to the above embodiments. Various changes can be made within the knowledge of ordinary technicians in this field without departing from the purpose of the present application.

[0219] Many other changes and modifications can be made without departing from the concept and scope of the present application. It should be understood that the present application is not limited to the specific embodiments, and the scope of the present application is defined by the appended claims.

Claims

1. A total station-based layout robot navigation method, characterized in that: The following steps are involved: Step 1: According to the layout line type file, import the layout point coordinate file into the control component of the layout robot; Step 2: Use the total station (5) to track the prism (4) of the layout robot and feed back the real-time coordinates of the prism (4); Step 3: The control component automatically navigates the layout robot's layout trolley (1) according to the line connecting the two layout points. The layout trolley (1) performs real-time deviation correction during the driving process and stops near the layout point to achieve rough positioning. The real-time deviation correction includes: Step 3-1: Calculate the correction value and direction based on the known starting point P1, destination point P2 and current point P3; The step 3-1 is specifically as follows: Given the starting point P1 (x1, y1), the destination point P2 (x2, y2), and the current point P3 (x3, y3); Step 3-1-1: The general equation of the straight line P1P2 is: y=kx+b; in: k=(y2-y1) / (x2-x1); b=y1-x1*k or b=y2-x2*k; Step 3-1-2: The distance S from the current point P3 (x3, y3) to the line P1P2 is calculated as follows: ； Step 3-1-3: Since the movement direction of the lofting trolley (1) is consistent with the direction of the straight line P1P2, the correction value and direction of P1→P2 are defined according to k and S; Step 3-2: Based on the deviation correction value and direction, the deviation correction is achieved by using the differential steering of the layout trolley (1); The step 3-2 is specifically as follows: Step 3-2-1: Calculate the speed difference between the wheels (1-2) on both sides of the lofting trolley (1) based on the wheelbase D of the lofting trolley (1) and the current speed v0 of the lofting trolley (1); Step 3-2-2: Calculate the speed of the left wheel (1-2) and the speed of the right wheel (1-2) based on the speed difference between the wheels (1-2) on both sides; Step 3-2-3: Determine the speed of the left wheel (1-2) and the right wheel (1-2) based on the correction value and direction, obtain the speed after correction, and adjust the speed of step 3-2-2 to the speed after correction; Step 4: After the layout trolley (1) stops steadily, use the total station (5) to cooperate with the third degree of freedom adjustment component (2-3) and the fourth degree of freedom adjustment component (2-4) of the mechanical arm adjustment device (2) of the layout robot to perform two precision positionings, adjust the prism (4) to a state perpendicular to the layout ground, and ensure that the bottom surface of the seal pressing mechanism (3) of the layout robot is parallel to the layout ground; Step 5: Use the target stakeout point precise positioning algorithm to adjust the first degree of freedom adjustment component (2-1) and the second degree of freedom adjustment component (2-2) of the mechanical arm adjustment device (2) of the stakeout robot, perform two more precise positionings, and adjust the seal ink pressing mechanism (3) to be just above the stakeout point; observe the coordinates of the prism (4), and compare them with the coordinates of the target stakeout point to see if the error is within the range; if yes, execute the press-stamp stakeout; if not, repeat step 5; The target stakeout point precise positioning algorithm in step 5 includes: Step 5-1: After adjusting the bottom surface of the seal ink pressing mechanism (3) to be parallel to the lofted ground, arbitrarily select the first measuring point P5 (x5, y5); Step 5-2: The lofting trolley (1) remains stationary, and the first degree of freedom adjustment component (2-1) of the robotic arm adjustment device (2) swings to the left by an angle α to obtain the second measurement point P6 (x6, y6); Step 5-3: Find the equation of the line P5P6: y=k5x+b5; in: k5=(y6-x6) / (x5-x5)=tan(θ1); b5=y5-x5*k5, or b5=y6-x6*k5; θ1=arctan(k5); Step 5-4: Rotate the line P5P6 to the left around point P5 by an angle (90°-α / 2) to obtain the equation of the line P5P4. The line P5P4 passes through point P5 (x5, y5): y=k6(x-x5)+y5=tan(θ1+90°-α / 2)(x-x5)+y5; in: k6=tan(θ2)=tan(θ1+90°-α / 2); Step 5-5: Based on the equation of the straight line P5P4 and the arm length R of the robotic arm adjustment device (2), obtain the coordinates of the point P4 (x4, y4); y4=y5-Rsin(arctank6); x4=x5-Rcos(arctank6); Step 5-6: Point P2 (x2, y2) is the known destination point. Find the angle Δβ between lines P4P6 and P4P2: β1=arctan((y6-y4) / (x6-x4)); β2=arctan((y2-y4) / (x2-x4)); Δβ=β1-β2; The first degree of freedom adjustment component (2-1) of the robot arm adjustment device (2) swings rightward at an angle Δβ; Step 5-7: Calculate the elongation ΔR of the second degree of freedom adjustment component (2-2): R1=P4P6; R2=root((y2-y4) 2 +(x2-x4) 2 ); ΔR=R2-R1; Complete two precise positionings to meet the layout requirements; Step 6: Repeat steps 3 to 5 until all stakeout points are staked out with stamps.

2. A total station-based layout robot navigation method according to claim 1, characterized in that: The layout robot comprises a layout trolley (1), a mechanical arm adjustment device (2), a seal ink pressing mechanism (3), a prism (4) and a control component. The layout trolley (1), the mechanical arm adjustment device (2) and the seal ink pressing mechanism (3) are electrically connected to the control component respectively. The mechanical arm adjustment device (2) comprises a first degree of freedom adjustment component (2-1), a second degree of freedom adjustment component (2-2), a third degree of freedom adjustment component (2-3), a fourth degree of freedom adjustment component (2-4) and a fifth degree of freedom adjustment component (2-5) which are connected in sequence. The degree of freedom adjustment component (2-5) is mounted inside the fourth degree of freedom adjustment component (2-4); the bottom of the fifth degree of freedom adjustment component (2-5) is connected to the seal ink pressing mechanism (3); the top of the fourth degree of freedom adjustment component (2-4) is provided with a prism (4); the axis of the prism (4) is coaxial with the axis of the seal ink pressing mechanism (3); the housing (1-1) of the lofting trolley (1) is provided with wheels (1-2) on two opposite sides, respectively; the power component (1-3) drives the two wheels (1-2) on the same side to rotate synchronously through the linkage component (1-4).

3. The total station-based layout robot navigation method according to claim 1, characterized in that: When k>0, S<0, the deviation is corrected to the right, S>0, the deviation is corrected to the left, and the deviation correction value is A; When k<0, S<0, the deviation is corrected to the left; when S>0, the deviation is corrected to the right, and the correction value is A.

4. The total station-based layout robot navigation method according to claim 1, characterized in that: The step 3-2-1 is specifically as follows: given the wheelbase D of the lofting trolley (1), the current speed v0 of the lofting trolley (1), the speed difference Δv=η*v0, where η is the wheel speed difference.

5. The total station-based layout robot navigation method according to claim 1, characterized in that: The speed of the left wheel (1-2) is vl=v0-Δv=v0(1-η); The speed of the right wheel (1-2) is vr=v0+Δv=v0(1+η).

6. The total station-based layout robot navigation method according to claim 1, characterized in that: The step 4 specifically comprises: using a total station (5) to track the prism (4) of the layout robot; when the axis of the prism (4) is not perpendicular to the layout ground, transmitting a signal to the control component of the layout robot; the control component adjusts the third degree of freedom adjustment component (2-3) and the fourth degree of freedom adjustment component (2-4) of the mechanical arm adjustment device (2); adjusting the third degree of freedom adjustment component (2-3) to an angle γ; and adjusting the fourth degree of freedom adjustment component (2-4) to an angle β; adjusting the axis of the prism (4) to a perpendicular state to the layout ground, thereby ensuring that the bottom surface of the seal pressing mechanism (3) of the layout robot is parallel to the layout ground.

Images