Lofting robot navigation method based on total station

Abstract

The invention belongs to the technical field of automatic navigation, and discloses a lofting robot navigation method based on a total station, a lofting trolley of a lofting robot carries a mechanical arm adjusting device to run, the top of the mechanical arm adjusting device is provided with a prism, the bottom of the mechanical arm adjusting device is provided with a seal ink pressing mechanism, and the prism is tracked by the total station; the real-time coordinates of the prism are fed back, the lofting trolley carries out real-time deviation rectification according to the coordinates in the running process, the coarse positioning efficiency is improved, and then the prism is adjusted to be in the state of being perpendicular to the lofting ground through a third freedom degree adjusting assembly and a fourth freedom degree adjusting assembly; and then the seal ink pressing mechanism is adjusted to the position over the lofting point through the first freedom degree adjusting assembly and the second freedom degree adjusting assembly in combination with a target lofting point accurate positioning algorithm, and accurate lofting is achieved.

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] At present, the Global Navigation Satellite System (GNSS) includes one or more satellite constellations and their augmentation systems required to support specific tasks. The International Committee on Global Navigation Satellite Systems announced the four major satellite navigation system suppliers in the world, including GPS: the United States' Global Positioning System, which is currently the most widely used navigation and positioning technology; BDS: China's Beidou Satellite Navigation System, which has been rapidly developed and widely used in recent years; GLONASS: Russia's Global Navigation Satellite System; Galileo: Europe's Global Satellite Navigation System. These systems achieve precise positioning of targets through satellite signals, with all-weather and global coverage.

[0003] Inertial Navigation System (INS) The inertial navigation system is composed of a gyroscope, an acceleration sensor and software. It measures the angular velocity and acceleration data of the moving carrier and integrates these data over time to obtain the speed, position and attitude of the moving carrier. This system has the characteristics of strong autonomy, but it will produce cumulative errors after working for a long time.

[0004] Visual navigation Visual navigation mainly uses visual sensors (such as cameras) to obtain environmental images and recognize lane lines, road signs, etc. through image processing algorithms to achieve vehicle positioning. This technology has the advantages of low cost and rich information, but it does not work well in complex environments (such as strong light, backlight, and darkness).

[0005] LiDAR Navigation LiDAR navigation collects 3D point cloud data of the environment through LiDAR and matches it with the map data collected in advance to achieve accurate positioning of the vehicle. This technology has the advantages of high detection accuracy and long detection distance, but the equipment cost is relatively high.

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

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

[0008] 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.

[0009] 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: 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 to track the prism of the layout robot and feedback the real-time coordinates of the prism; Step 3: The control component automatically navigates the route of the layout robot's layout car according to the connection line between the front and rear layout points. The layout car performs real-time deviation correction during driving 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; Step 3-2: According to the deviation correction value and direction, the differential steering of the layout car is used to achieve deviation correction; 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 mechanical arm adjustment device of the layout robot to perform two precision positioning, adjust the prism to a state of being vertical to the layout ground, and ensure that the bottom surface of the seal pressing mechanism 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 and the second degree of freedom adjustment component of the mechanical arm adjustment device of the stakeout robot, perform two more precise positionings, and adjust the seal ink pressing mechanism to just 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; Step 6: Repeat steps 3 to 5 until all stakeout points are staked out with stamping.

[0010] Preferably, the layout robot includes a layout trolley, a mechanical arm adjustment device, a seal inking mechanism, a prism and a control component. The layout trolley, the mechanical arm adjustment device and the seal inking mechanism are electrically connected to the control component respectively. The mechanical 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 which are 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 arranged 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 respectively. The power component drives the two wheels on the same side to rotate synchronously through the linkage component.

[0011] Preferably, 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 straight line P1P2 is calculated as follows:

[0012] 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.

[0013] 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; When k<0, S<0, the deviation is corrected to the left, and when S>0, the deviation is corrected to the right, and the correction value is A.

[0014] Preferably, the step 3-2 is specifically as follows: Step 3-2-1: Calculate the speed difference of the wheels on both sides of the lofting trolley according to the wheelbase D of the lofting trolley and the current speed v0 of the lofting trolley; Step 3-2-2: Calculate the speed of the left wheel and the speed of the right wheel according to the speed difference of the wheels on both sides; Step 3-2-3: Determine the speed of the left wheel and the right wheel 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.

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

[0016] Preferably, the speed of the left wheel vl=v0-Δv=v0(1-η); The speed of the right wheel vr=v0+Δv=v0(1+η).

[0017] 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 ink pressing mechanism of the layout robot is parallel to the layout ground.

[0018] Preferably, the precise positioning algorithm using the target stakeout point in step 5 includes: Step 5-1: After adjusting the bottom surface of the seal inking mechanism 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 mechanical arm adjustment device 2 swings to the left at an angle α to obtain the second measurement point P6 (x6, y6); Step 5-3: Find the equation of the straight 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: Line P5P6 rotates left around point P5 by an angle (90°-α / 2) to obtain the equation of line P5P4. 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: According to the equation of the straight line P5P4 and the arm length R of the robot arm adjustment device 2, the point coordinates P4 (x4, y4) are obtained; y4=y5-Rsin(arctank6) x4=x5-Rcos(arctank6) Step 5-6: Point P2 (x2, y2) is the known destination point. Find the angle Δβ between the straight 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 mechanical 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.

[0019] Compared with the prior art, the advantages of this application are: (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 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 prism is tracked by the total station to 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 rough positioning. Then, the prism is adjusted to a state of being 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 be 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. (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 within plus or minus 30 degrees in front of the stakeout vehicle, and there is no dead zone; (3) The present application has developed a precise positioning algorithm for the target point of the stakeout. After the stakeout 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, i.e., the rotation origin, is obtained. Since the target point P2 is known, the angle Δβ between the straight lines P4P6 and P4P2, i.e., 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 stakeout trolley and the rotation origin, thereby obtaining the swing angle and elongation, with high calculation accuracy and fast speed. (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

[0020] Figure 1 , a schematic diagram of an application of a total station-based layout robot navigation method of the present application; Figure 2 , a schematic diagram of the structure of the layout robot of the present application; Figure 3 , Schematic diagram of the real-time correction algorithm of this application; Figure 4 , Schematic diagram of the real-time correction algorithm of this application; Figure 5 , Schematic diagram of the precise positioning algorithm of the target stakeout point of this application; Figure 6 , Schematic diagram of the algorithm for accurately positioning the target layout points of this application.

[0021] Description of reference numerals: 1. Stakeout trolley, 2. Mechanical arm adjustment device, 3. Stamp ink pressing mechanism, 4. Prism, 5. Total station; 1-1, cover, 1-2, wheel, 1-3, power assembly, 1-4, linkage assembly; 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

[0022] The specific implementation of the present application is described below in conjunction with embodiments: 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 under which this application can be implemented. 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 effects and purposes that can be achieved by this application.

[0023] like Figure 1 , 2 As shown, the present application discloses a total station-based layout robot navigation method, comprising the following steps: 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 travel route of the layout robot's layout vehicle 1 according to the connection line between the two layout points. The layout vehicle 1 performs real-time deviation correction during driving 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; Step 3-2: According to the deviation correction value and direction, the deviation correction is achieved by using the differential steering of the layout vehicle 1; Step 4: After the layout trolley 1 stops steadily, 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 are used to perform two precision positioning, and the prism 4 is adjusted to a state of being vertical to the layout ground to 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 precise positioning twice again, and adjust the seal ink pressing mechanism 3 to just above the stakeout point; observe the coordinates of the prism 4, and compare the error with the target stakeout point coordinates to see if it is within the range; if yes, execute the press-stamp stakeout; if not, repeat step 5; Step 6: Repeat steps 3 to 5 until all stakeout points are staked out with stamping.

[0024] like Figure 2 As shown, preferably, the layout robot includes 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 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 which are connected in sequence. The fifth degree of freedom adjustment component 2-5 is sleeved 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, and the axis of the prism 4 is coaxial with the axis of the seal ink pressing 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.

[0025] like Figure 3 As shown, preferably, 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 straight line P1P2 is calculated as follows:

[0026] 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.

[0027] 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; When k<0, S<0, the deviation is corrected to the left, and when S>0, the deviation is corrected to the right, and the correction value is A.

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

[0029] like Figure 4 As shown, preferably, 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 according to 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 according to 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 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.

[0030] Preferably, 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, and the speed difference Δv=η*v0, wherein η is the wheel speed difference.

[0031] Preferably, 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+η).

[0032] The trajectories of the left and right wheels in the same time Δt are two concentric circles: Left wheel track circumference: Cl=2πR1 Right wheel track circumference: C2=2πRr Δt=C1 / vl=C2 / vr 2πR1 / (v0(1-η))=2πRr / v0(1+η) Radius of two circles: Rr=R1+D R1 / (v0(1-η))=(R1+D) / v0(1+η) R1=D / (2(1 / η-1)) 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 an angle of γ, and adjusts the fourth degree of freedom adjustment component 2-4 to an angle of β, so that the axis of the prism 4 is adjusted to a vertical state with respect to the layout ground, ensuring that the bottom surface of the seal inking mechanism 3 of the layout robot is parallel to the layout ground.

[0033] like Figure 5 , 6 As shown, preferably, the target stakeout point precise positioning algorithm in step 5 includes: 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); Step 5-2: The lofting trolley 1 remains stationary, and the first degree of freedom adjustment component 2-1 of the mechanical arm adjustment device 2 swings to the left at an angle α to obtain the second measurement point P6 (x6, y6); Step 5-3: Find the equation of the straight 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: Line P5P6 rotates left around point P5 by an angle (90°-α / 2) to obtain the equation of line P5P4. 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: According to the equation of the straight line P5P4 and the arm length R of the robot arm adjustment device 2, the point coordinates P4 (x4, y4) are obtained; y4=y5-Rsin(arctank6) x4=x5-Rcos(arctank6) Step 5-6: Point P2 (x2, y2) is the known destination point. Find the angle Δβ between the straight 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 mechanical 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.

[0034] Example 1 This embodiment uses a total station-based layout robot navigation method of the present application to perform navigation layout, including the following steps: 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 travel route of the layout robot's layout vehicle 1 according to the connection line between the two layout points. The layout vehicle 1 performs real-time deviation correction during driving and stops near the layout point to achieve rough positioning. like Figure 3 , 4 As shown, 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; 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 straight line P1P2 is calculated as follows:

[0035] 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; Step 3-2: According to the deviation correction value and direction, the deviation correction is achieved by using the differential steering of the layout vehicle 1; Step 3-2-1: Calculate the speed difference between the wheels 1-2 on both sides of the lofting trolley 1 according to 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 according to 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 according to the deviation correction value and direction, obtain the speed after deviation correction, and adjust the speed of step 3-2-2 to the speed after deviation correction; Step 4: After the layout trolley 1 stops steadily, 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 are used to perform two precision positioning, and the prism 4 is adjusted to a state of being vertical to the layout ground to ensure that the bottom surface of the seal pressing mechanism 3 of the layout robot is parallel to the layout ground; 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, adjusts the third degree of freedom adjustment component 2-3 to an angle of γ, and adjusts the fourth degree of freedom adjustment component 2-4 to an angle of β, so that the axis of the prism 4 is adjusted to a vertical state with respect to the layout ground, and ensures 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 precise positioning twice again, and adjust the seal ink pressing mechanism 3 to just above the stakeout point; observe the coordinates of the prism 4, and compare the error with the target stakeout point coordinates to see if it is within the range; if yes, execute the press-stamp stakeout; if not, repeat step 5; like Figure 5 , 6 As shown, the target stakeout point precise positioning algorithm in step 5 includes: 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); Step 5-2: The lofting trolley 1 remains stationary, and the first degree of freedom adjustment component 2-1 of the mechanical arm adjustment device 2 swings to the left at an angle α to obtain the second measurement point P6 (x6, y6); Step 5-3: Find the equation of the straight 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: Line P5P6 rotates left around point P5 by an angle (90°-α / 2) to obtain the equation of line P5P4. 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: According to the equation of the straight line P5P4 and the arm length R of the robot arm adjustment device 2, the point coordinates P4 (x4, y4) are obtained; y4=y5-Rsin(arctank6) x4=x5-Rcos(arctank6) Step 5-6: Point P2 (x2, y2) is the known destination point. Find the angle Δβ between the straight lines P4P6 and P4P2: β1 = arctan((y6-y4) / (x6-x4)) β2=arctan((y2-y4) / (x2-x4)) The rightward swing angle of the first degree of freedom adjustment component 2-1 of the mechanical arm adjustment device 2 is: Δβ=β1-β2 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.

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

[0037] Comparative Example Inertial navigation is used. Inertial navigation relies on the accuracy of the gyroscope to calculate the direction and speed to calculate the distance. Its disadvantage is that the gyroscope has cumulative errors, and the cumulative errors increase with time. Each time the vehicle is staked out, the coordinate direction needs to be reported and input into the computer to achieve navigation. The preparation time is long and the accuracy is poor. It can only be applied to short-distance stakeout applications with low accuracy requirements.

[0038] Application Examples 10 points are staked out on a straight line with a spacing of 5 meters. Inertial navigation and the navigation method of this application are used at the same time. The experimental data are shown in the following table:

[0039] 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.

[0040] The navigation method of the present application can achieve millimeter-level high-precision positioning. It 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.

[0041] The present application discloses a layout robot navigation method based on a total station, wherein a layout trolley of the layout robot 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 on the bottom of the mechanical arm adjustment device, and the prism is tracked by the total station to feed back the real-time coordinates of the prism, and the layout trolley performs real-time deviation correction according to the coordinates during driving to improve the efficiency of coarse positioning, and then the prism is adjusted to a state of being vertical to the layout ground by using the third degree of freedom adjustment component and the fourth degree of freedom adjustment component, and then the seal and ink pressing mechanism is adjusted to be 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 precise positioning algorithm of the target layout point to achieve precise layout.

[0042] 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 within plus or minus 30 degrees in front of the stakeout vehicle, and there is no dead zone.

[0043] The present application develops an algorithm for accurately positioning 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 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. Based on the equation of the straight line P5P4 and the arm length R of the robotic arm adjustment device, the point coordinates P4, i.e., the rotation origin, are obtained. Since the target point P2 is known, the angle Δβ between the straight lines P4P6 and P4P2, i.e., 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 front direction of the layout trolley and the rotation origin, thereby obtaining the swing angle and elongation, with high calculation accuracy and fast speed.

[0044] 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.

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

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

[0047] 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 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: Using a total station (5) to track the prism (4) of the layout robot, and feeding back the real-time coordinates of the prism (4); Step 3: The control component automatically navigates the travel route of the layout vehicle (1) of the layout robot according to the connection line between the two layout points. The layout vehicle (1) performs real-time deviation correction during the travel 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; Step 3-2: According to the deviation correction value and direction, the deviation correction is achieved by using the differential steering of the layout vehicle (1); Step 4: After the layout vehicle (1) stops steadily, the total station (5) is used 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, and the prism (4) is adjusted to a state of being vertical to the layout ground surface, so as to ensure that the bottom surface of the seal pressing mechanism (3) of the layout robot is parallel to the layout ground surface; Step 5: Using the target stakeout point precise positioning algorithm, 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 precise positioning twice again, and adjust the seal ink pressing mechanism (3) to the position directly above the stakeout point; observe the coordinates of the prism (4), and compare the error with the coordinates of the target stakeout point to see if it is within the range; if yes, execute the press-stamp stakeout; if no, repeat step 5; Step 6: Repeat steps 3 to 5 until all stakeout points are staked out with stamping.

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 respectively electrically connected to the control component. 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); wheels (1-2) are respectively installed in pairs on two opposite sides of the cover (1-1) of the layout trolley (1); 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. A total station-based layout robot navigation method according to claim 2, characterized in that: 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 straight line P1P2 is calculated as follows: ； Step 3-1-3: Since the movement direction of the layout 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.

4. The total station-based layout robot navigation method according to claim 3, 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, and when S>0, the deviation is corrected to the right, and the correction value is A.

5. The total station-based layout robot navigation method according to claim 3, characterized in that: 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) according to 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) according to 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) 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.

6. The total station-based layout robot navigation method according to claim 5, 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), and the speed difference Δv=η*v0, wherein η is the wheel speed difference.

7. The total station-based layout robot navigation method according to claim 6, 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+η).

8. 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 a 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 of γ; the fourth degree of freedom adjustment component (2-4) is adjusted to an angle of β, so that the axis of the prism (4) is adjusted to a perpendicular state to the layout ground, thereby ensuring that the bottom surface of the seal ink pressing mechanism (3) of the layout robot is parallel to the layout ground.

9. The total station-based layout robot navigation method according to claim 5, characterized in that: 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 at an angle α to obtain the second measurement point P6 (x6, y6); Step 5-3: Find the equation of the straight 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: Line P5P6 rotates left around point P5 by an angle (90°-α / 2) to obtain the equation of line P5P4. 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: According to the equation of the straight line P5P4 and the arm length R of the robot arm adjustment device (2), the point coordinates P4 (x4, y4) are obtained; y4=y5-Rsin(arctank6); x4=x5-Rcos(arctank6); Step 5-6: Point P2 (x2, y2) is the known destination point. Find the angle Δβ between the straight 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 mechanical 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.

Images