Satellite positioning and orientation automatic lineation robot control method

Abstract

The invention discloses a satellite positioning and orientation automatic lineation robot control method, which relates to the technical field of automation control and comprises the following steps: S1, generating high-precision positioning data by fusing a satellite positioning and orientation technology and an inertial navigation technology; the satellite positioning data and the inertial navigation data are fused through a Kalman filtering algorithm. According to the automatic lineation robot control method, the satellite positioning and orientation technology and the inertial navigation technology are fused, data fusion is carried out through the Kalman filtering algorithm, high-precision positioning data can be generated, a solid foundation is provided for follow-up pattern planning and path planning, and the automatic lineation robot control method is suitable for large-scale popularization and application. The working efficiency of the lineation robot is improved, the lineation precision is remarkably improved, the accuracy and consistency of pattern drawing are ensured, a complex pattern is decomposed into simple pattern units through the pattern planning software module, path planning is conducted through a cubic spline interpolation algorithm, and the lineation process is simplified.

Description

Technical Field

[0001] The present invention relates to the technical field of automatic control, and specifically relates to a control method for an automatic line-drawing robot with satellite positioning and orientation. Background Art

[0002] In the technical field of automatic control, line-drawing robots are increasingly widely used, especially showing great potential in road markings, airport runway markings, and large-scale construction site planning. Traditional line-drawing robots mainly rely on preset paths or manual operations to work, which is not only inefficient but also difficult to ensure line-drawing accuracy. With the progress of technology, the combination of satellite positioning and orientation technology and inertial navigation technology provides a new solution for the automatic control of line-drawing robots. This combination can generate high-precision positioning data, enabling the line-drawing robot to autonomously navigate and complete precise line-drawing tasks in complex environments. Through advanced control algorithms and software modules, the line-drawing robot can achieve automatic decomposition of the target pattern, path planning, and precise drawing, greatly improving work efficiency and line-drawing accuracy.

[0003] However, the existing control technology for line-drawing robots still has some problems and deficiencies. On the one hand, single satellite positioning or inertial navigation technology is easily interfered with in complex environments, resulting in inaccurate positioning data and thus affecting line-drawing accuracy. On the other hand, existing line-drawing robots often lack flexibility and adaptability when facing complex patterns and dynamic environments, and it is difficult to achieve efficient and precise line-drawing. In addition, for path deviation and error correction during the line-drawing process, existing technologies also lack effective real-time adjustment algorithms, making it difficult to guarantee the line-drawing quality. In response to this, we propose a control method for an automatic line-drawing robot with satellite positioning and orientation. Summary of the Invention

[0004] To solve the above technical problems, a control method for an automatic line-drawing robot with satellite positioning and orientation is provided, and this technical solution solves the above problems.

[0005] To achieve the above object, the technical solution adopted by the present invention is as follows:

[0006] A control method for an automatic line-drawing robot with satellite positioning and orientation, comprising the following steps:

[0007] S1. By fusing satellite positioning and orientation technology with inertial navigation technology, high-precision positioning data is generated; the satellite positioning data and inertial navigation data are fused through the Kalman filter algorithm, and the specific formula is:

[0008]

[0009] Wherein, is the state estimate value after fusion, K kis the Kalman gain, Z k is the satellite positioning observation value, H is the observation matrix, is the predicted state value;

[0010] S2. Based on the fused data, the target pattern is decomposed into multiple simple graphic units by the pattern planning software module. The decomposition process includes:

[0011] Conduct topological analysis on the complex pattern according to the national standard GB5768.3 - 2009, and extract three types of basic units: straight line segments, arc segments, and broken line segments. Each unit is defined by the starting point coordinates (x s , y s ), the ending point coordinates (x e , y e ) and the radius of curvature r;

[0012] S3. The motion control software module generates path planning instructions based on the current fused positioning data and the decomposed graphic units. The path planning adopts the cubic spline interpolation algorithm, and the interpolation function is:

[0013] S(x) = a i + b i (x - x i ) + c i (x - x i ) 2 + d i (x - x i ) 3 (x i ≤ x ≤ x i + 1)

[0014] where a i , b i , c i , d i are the interpolation coefficients, which are determined by the boundary conditions and the continuous derivative constraints;

[0015] S4. The motor control software module drives the robot to perform the drawing action according to the path planning instructions, and feeds back the device attitude angles (θ x , θ y ) in real time through the attitude sensor to correct the path deviation; when the deviation exceeds the threshold δ max = 1 cm, trigger the adjustment algorithm, and the adjustment frequency is more than 10 times per second;

[0016] S5. After the drawing is completed, verify the pattern accuracy through the laser rangefinder to ensure that the error of the straight line segment is ≤ 1.5 cm per kilometer and the local error of the complex pattern is ≤ 1 cm.

[0017] Preferably, the fusion process of the satellite positioning and orientation technology and the inertial navigation technology in step S1 further includes:

[0018] The carrier phase observations φ L1 and φ L2 are obtained by a multi-frequency GNSS receiver, and combined with the angular velocity ω and acceleration a output by an inertial measurement unit (IMU) to construct a state vector

[0019] X = [x, y, z, v x , v y , v z , θ x , θ y , θ z T

[0020] Dynamic error compensation is achieved through an extended Kalman filter (EKF);

[0021] The IMU data sampling frequency is 100 Hz, the GNSS data update frequency is 10 Hz, and the fusion period is 10 ms;

[0022] When the satellite signal is lost, the inertial navigation system operates independently, and the error accumulation model is:

[0023]

[0024] where ∈ v and ∈ a are the drift errors of velocity and acceleration, and are compensated by a quadratic polynomial fitted from historical data.

[0025] Preferably, the initialization process of integrated positioning and orientation in step S1 includes:

[0026] The initial coordinates (x 0 , y 0 ) are obtained through 10 minutes of GNSS static observations, with an accuracy better than 2 cm;

[0027] When initializing the inertial navigation system, the zero-velocity correction algorithm is executed, and the formula is:

[0028]

[0029] where ω k and a k are the original IMU data in a stationary state, and N = 1000 is the number of sampling points;

[0030] After initialization, the output integrated positioning data update frequency is 50 Hz.

[0031] Preferably, the complexity classification rule of the graphic unit in step S2 is:

[0032] Define the complexity coefficient ​Where L is the total length of the graphic unit, R is the minimum radius of curvature, and N is the number of turning points;

[0033] When C ≤ 10, the drawing speed is v 1 = 1.2 m / s; when 10 < C ≤ 30, the speed is v 2 = 0.8 m / s; when C > 30, the speed is v 3 = 0.5 m / s The drawing speed and accuracy satisfy the relationship:

[0034] ∈ = k·v -1.5

[0035] Where k = 0.05 is the experimental fitting constant, and ∈ is the upper limit of the allowable error.

[0036] Preferably, the pattern planning software module in step S2 includes two connection modes:

[0037] The connection modes include forward connection and reverse connection: Forward connection: When the included angle Δθ between the tangent directions of adjacent graphic units is ≤ 15°, a continuous transition algorithm is adopted, and the path curvature change rate is limited to:

[0038]

[0039] Reverse connection: When Δθ > 15°, the robot pauses and relocates, and continues to draw after the positioning error satisfies δ ≤ 0.5 cn;

[0040] The coordinates of the connection points are smoothed by a Bezier curve, and the distance d between the control points satisfies:

[0041]

[0042] Where L prev and L next are the lengths of adjacent units.

[0043] Preferably, the pattern planning software module in step S2 supports dynamic priority adjustment:

[0044] When an external obstacle is detected, the current task is paused and an obstacle avoidance path is generated. The radius R of the obstacle avoidance path obs ≥ 1.5 m;

[0045] When resuming drawing, recalculate the decomposition scheme of the remaining graphic units and preferentially process the unaffected units;

[0046] The radius of curvature R of the obstacle avoidance path satisfies:

[0047]

[0048] Where v is the current drawing speed and μ is the friction coefficient.

[0049] Preferably, the path planning of the motion control software module in step S3 further includes:

[0050] Segment-wise optimization is performed on each graphic unit, with the length of each segment being l i ≤ 0.5 m, and the acceleration constraint of each segment is calculated:

[0051] a max = μ·g·cos(θ x )·cos(θ y )

[0052] where μ = 0.8 is the friction coefficient between the tire and the ground, and g = 9.8 m / s 2 ;

[0053] During path planning, the speed curve adopts a trapezoidal acceleration - constant speed - deceleration model. During the acceleration and deceleration phases, t acc = t dec = 0.5 s, and the maximum acceleration a max = 1.0 m / s 2 .

[0054] Preferably, the motor control software module in step S4 adopts a closed-loop control strategy:

[0055] The error between the motor speed n and the target speed n ref is adjusted by a PID controller, and the control law is:

[0056]

[0057] where K p = 2.5, K i = 0.1, K d = 0.05, and the sampling period T s = 10 ms;

[0058] The angle α of the steering motor is fed back by an optical encoder, with a resolution ≤ 0.01° and a repeat positioning accuracy ≤ 0.1°.

[0059] Preferably, the path deviation in step S4 is corrected by a deviation adjustment algorithm;

[0060] The specific deviation adjustment algorithm is as follows:

[0061] The Euclidean distance deviation of the current drawing point (x t , y t ) is calculated in real time:

[0062]

[0063] If δ > δ max, a velocity correction amount is generated according to the robot kinematic model:

[0064]

[0065] where K p = 0.8, K d = 0.2 are the proportional-derivative coefficients;

[0066] Meanwhile, adjust the motor steering angle α:

[0067]

[0068] where θ z is the current heading angle, and the corrected motor control command is sent to the drive module through the CAN bus.

[0069] Preferably, the verification method of the laser rangefinder in step S5 includes:

[0070] Lay out 10 detection points along the drawn pattern, measure each point 3 times and take the average value, and calculate the root mean square error:

[0071]

[0072] If RMSE > 1 cm, trigger the automatic repair process, and the repair path adopts the spiral coverage algorithm, with a pitch p = 0.2 m and a coverage width w = 0.05 m.

[0073] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0074] The automatic scribing robot control method proposed by the present invention can generate high-precision positioning data by integrating satellite positioning and orientation technology with inertial navigation technology and using the Kalman filtering algorithm for data fusion, providing a solid foundation for subsequent pattern planning and path planning. This not only improves the working efficiency of the scribing robot, but also significantly improves the scribing accuracy, ensuring the accuracy and consistency of the drawn pattern. By decomposing complex patterns into simple graphic units through the pattern planning software module and adopting the cubic spline interpolation algorithm for path planning, the scribing process is effectively simplified and the technical difficulty is reduced. At the same time, the drawing speed is dynamically adjusted according to the complexity of the graphic unit, further improving the drawing efficiency and accuracy. The motor control software module adopts a closed-loop control strategy to correct the path deviation in real time, ensuring the stability and reliability of the scribing process. BRIEF DESCRIPTION OF THE DRAWINGS

[0075] Figure 1 is the flowchart of the method of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0076] The following description is used to disclose the present invention so that those skilled in the art can implement the present invention. The preferred embodiments in the following description are only examples, and those skilled in the art can think of other obvious variations.

[0077] Referring to Figure 1 as shown, a control method for an automatic line-drawing robot for satellite positioning and orientation includes the following steps:

[0078] S1. Generate high-precision positioning data by fusing satellite positioning and orientation technology with inertial navigation technology; the satellite positioning data and inertial navigation data are fused through the Kalman filtering algorithm, and the specific formula is:

[0079]

[0080] Where is the fused state estimate value, K k is the Kalman gain, Z k is the satellite positioning observation value, H is the observation matrix, is the predicted state value;

[0081] S2. Based on the fused data, decompose the target pattern into multiple simple graphic units through the pattern planning software module, and the decomposition process includes:

[0082] Conduct topological analysis on the complex pattern according to the national standard GB5768.3-2009, extract three types of basic units: straight line segments, arc segments, and broken line segments, and each unit is defined by the starting point coordinates (x s , y s ), the ending point coordinates (x e , y e ) and the radius of curvature r;

[0083] S3. The motion control software module generates path planning instructions according to the current fused positioning data and the decomposed graphic units. The path planning adopts the cubic spline interpolation algorithm, and the interpolation function is:

[0084] S(x) = a i + b i (x - x i ) + c i (x - x i ) 2 + d i (x - x i ) 3 (x i ≤ x ≤ xi + 1)

[0085] Where a i , b i , c i , d iis the interpolation coefficient, which is determined by boundary conditions and continuous derivative constraints;

[0086] S4. The motor control software module drives the robot to perform the drawing action according to the path planning instruction, and real-time feedbacks the device attitude angles (θ x , θ y ) through the attitude sensor, and corrects the path deviation; when the deviation exceeds the threshold δ max = 1 cm, trigger the adjustment algorithm, and the adjustment frequency is more than 10 times per second;

[0087] S5. After the drawing is completed, verify the pattern accuracy through the laser rangefinder to ensure that the error per kilometer of the straight line segment ≤ 1.5 cm and the local error of the complex pattern ≤ 1 cm.

[0088] 1. Initialization and data fusion of the positioning and orientation system

[0089] (1) Hardware configuration

[0090] The robot is equipped with a multi-frequency GNSS receiver (supporting systems such as GPS and Beidou), a six-axis IMU (including three-axis gyroscopes and three-axis accelerometers), a laser rangefinder, and a CAN bus communication module. (2)

[0092] Initialization process:

[0093] GNSS static calibration: The robot stands still for 10 minutes, and collects GNSS carrier phase observations φ L1 , φ L2 , and calculates the initial coordinates (x 0 , y 0 ) through the least squares method, with an accuracy ≤ 2 cm.

[0094] IMU zero velocity correction: Collect 1000 groups of IMU data in a stationary state, and calculate the angular velocity bias and the acceleration bias

[0095]

[0096] (3) Data fusion: Use the extended Kalman filter (EKF) to fuse GNSS and IMU data, and the state vector is X = [x, y, z, v x , v y , v z , θ x , θ y , θ z T , and the prediction model is:

[0097]

[0098] ​Among them, F is the state transition matrix, u k is the control input, w k is the process noise; the observation model is:

[0099]

[0100] H is the observation matrix, v k is the observation noise, and the fusion frequency is 50Hz.

[0101] 2. Decomposition and path planning of complex patterns

[0102] (1) Pattern decomposition rules:

[0103] According to the national standard GB5768.3-2009, the road markings are decomposed into three basic units: straight line segments, arc segments, and broken line segments. Each unit is defined by the starting point (x s , y s ), the ending point (x e , y e ), the radius of curvature r, and the tangent direction angle θ.

[0104] Complexity calculation: Calculate the complexity coefficient (L is the length, R is the minimum radius of curvature, and N is the number of turning points) for each unit, and allocate the drawing speed according to the C value:

[0105]

[0106] (2) Path planning algorithm:

[0107] Use the cubic spline interpolation method to generate a smooth path, and the interpolation function is:

[0108] S(x) = a i + b i (x - x i ) + c i (x - x i ) 2 + d i (x - x i ) 3 (x i ≤ x ≤ x i+1 )

[0109] The coefficients are solved through boundary conditions (position, velocity, and acceleration continuity).

[0110] Segment optimization: The length of each path segment ≤ 0.5m, and the acceleration limit is:

[0111] a max = 0.8·g·cos(θ x )·cos(θ y)(g = 9.8 m / s 2 )

[0112] 3. Real - time plotting and deviation correction control

[0113] (1) Motion control process:

[0114] The motor control module receives the path - planning instruction and drives the wheeled chassis to move according to the trapezoidal speed curve (acceleration for 0.5 s → constant speed → deceleration for 0.5 s).

[0115] The steering angle α is fed back by the photoelectric encoder, with a resolution ≤ 0.01°, and the control law is:

[0116]

[0117] (2) Deviation adjustment:

[0118] Real - time calculate the Euclidean distance deviation between the current coordinates (x c , y c ) and the target point (x t , y t ).

[0119] If δ > 1 cm, trigger PID adjustment:

[0120]

[0121] The adjustment frequency ≥ 10 Hz to ensure error convergence.

[0122] 4. Graphic connection and exception handling

[0123] (1) Co - direction connection: When the included angle Δθ between the tangent directions of adjacent units ≤ 15°, use the Bezier curve for smooth transition, and the control - point spacing is:

[0124]

[0125] Reverse connection: When Δθ > 15° or there is a bend, the robot pauses and relocates. After the positioning error ≤ 0.5 cm, it continues to draw. Obstacle avoidance processing (3)

[0126] When an obstacle is detected, generate an obstacle - avoidance path, and the radius of curvature satisfies: After completion, resume the original task according to the priority.

[0127] 5. Precision verification and repair mechanism

[0128] (1) Laser detection: Arrange 10 detection points along the marking line, measure each point 3 times and take the average value, and calculate the root - mean - square error:

[0129]

[0130] Rework strategy: If RMSE > 1 cm, start spiral coverage rework with pitch p = 0.2 m and coverage width w = 0.05 m until the error meets the standard.

[0131] 6. Implementation effect

[0132] Through the above method, the measured error per kilometer of the straight road markings is ≤ 1.5 cm, and the local error of complex patterns is ≤ 1 cm. The drawing efficiency is more than 3 times higher than that of manual work. This method can be widely applied to scenarios such as municipal roads and highways.

[0133] The above shows and describes the basic principles, main features and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited by the above embodiments. What is described in the above embodiments and the specification is only the principle of the present invention. Without departing from the spirit and scope of the present invention, the present invention will have various changes and improvements, and these changes and improvements fall within the scope of the present invention claimed. The scope of protection claimed by the present invention is defined by the appended claims and their equivalents.

Claims

1. A satellite positioning and orientation automatic marking robot control method, characterized in that: It includes the following steps: S1. Generate high-precision positioning data by fusing satellite positioning and orientation technology with inertial navigation technology; the satellite positioning data and inertial navigation data are fused through the Kalman filtering algorithm, and the specific formula is: in, is the estimated value of the fused state, K k is the Kalman gain, Z k is the satellite positioning observation value, H is the observation matrix, is the predicted state value; S2. Based on the fused data, decompose the target pattern into multiple simple graphic units through the pattern planning software module, and the decomposition process includes: According to the national standard GB5768.3-2009, the complex pattern is topologically analyzed and three basic units, straight line segment, arc segment and broken line segment, are extracted. Each unit is composed of the starting point coordinates (x s ,y s )、end point coordinates (x e ,y e ) and the radius of curvature r are defined; S3. The motion control software module generates path planning instructions according to the current fused positioning data and the decomposed graphic units. Cubic spline interpolation algorithm is used for path planning, and the interpolation function is: S(x)=a i +b i (x-x i )+c i (x-x i ) 2 +d i (x-x i ) 3 (x i ≤x≤xi+1) Among them, a i ,b i ,c i ,d i is the interpolation coefficient, which is determined by the boundary conditions and continuous derivative constraints; S4, the motor control software module drives the robot to perform drawing actions according to the path planning instructions, and feeds back the device attitude angle (θ x ,θ y ), correct the path deviation; when the deviation exceeds the threshold δ max = 1cm, the adjustment algorithm is triggered, and the adjustment frequency is more than 10 times per second; S5. After the drawing is completed, verify the pattern accuracy through a laser rangefinder to ensure that the error per kilometer of the straight line segment is ≤1.5 cm, and the local error of the complex pattern is ≤1 cm.

2. The control method of a satellite-based automatic marking robot according to claim 1, characterized in that: The fusion process of satellite positioning and orientation technology and inertial navigation technology in step S1 further includes: Using a multi-frequency GNSS receiver to obtain carrier phase observations φ L1 and φ L2 , combined with the angular velocity ω and acceleration a output by the inertial measurement unit (IMU), construct the state vector X=[x,y,z,v x ,v y ,v z ,θ x ,θ y ,θ z ] T Implement dynamic error compensation through an Extended Kalman Filter (EKF); The IMU data sampling frequency is 100 Hz, the GNSS data update frequency is 10 Hz, and the fusion period is 10 ms; When the satellite signal is lost, the inertial navigation system operates independently, and the error accumulation model is: Among them, ∈ v and ∈ a The drift error of velocity and acceleration is compensated by fitting a quadratic polynomial based on historical data.

3. The control method of a satellite-based automatic marking robot according to claim 1, characterized in that: The initialization process of the fused positioning and orientation in step S1 includes: Obtain the initial coordinates (x0, y0) with an accuracy better than 2 cm through GNSS static observation for 10 minutes; When initializing the inertial navigation system, execute the zero-velocity correction algorithm, and the formula is: Among them, ω k and a k is the original data of IMU in static state, N=1000 is the number of sampling points; After initialization, the output fused positioning data update frequency is 50 Hz.

4. The control method of a satellite-based automatic marking robot according to claim 1, characterized in that: The complexity classification rules of the graphic units in step S2 are: Defining the complexity factor Where L is the total length of the graphic unit, R is the minimum radius of curvature, and N is the number of turning points; When C≤10, the drawing speed is v1 = 1.2 m / s; when 10 < C≤30, the speed is v2 = 0.8 m / s; when C>30, the speed is v3 = 0.5 m / s. The drawing speed and accuracy satisfy the relational expression: ∈=k·v -1.5 Among them, k = 0.05 is the experimental fitting constant, and ∈ is the upper limit of the allowable error.

5. The control method of a satellite-based automatic marking robot according to claim 1, characterized in that: The pattern planning software module in step S2 includes two connection modes: The connection modes include forward connection and reverse connection: Forward connection: When the included angle Δθ between the tangent directions of adjacent graphic units is ≤15°, a continuous transition algorithm is adopted, and the path curvature change rate is limited to: Reverse connection: When Δθ>15°, the robot pauses and relocates, and continues to draw after the positioning error satisfies δ≤0.5 cm; The connection point coordinates are smoothed through a Bezier curve, and the control point spacing d satisfies: Among them, L prev and L next is the length of adjacent units.

6. The control method of a satellite-based automatic marking robot according to claim 1, characterized in that: The pattern planning software module in step S2 supports dynamic priority adjustment: When an external obstacle is detected, the current task is paused and an obstacle avoidance path is generated. The obstacle avoidance path radius is R obs ≥1.5m; When resuming drawing, recalculate the decomposition scheme of the remaining graphic units and give priority to processing the unaffected units; The curvature radius R of the obstacle avoidance path satisfies: Among them, v is the current drawing speed, and μ is the friction coefficient.

7. The control method of a satellite-based automatic marking robot according to claim 1, characterized in that: The path planning of the motion control software module in step S3 further includes: Each graphic unit is optimized in segments, and the length of each segment is l i ≤0.5m, and calculate the acceleration constraints of each segment: a max =μ·g·cos(θ x )·cos(θ y ) Among them, μ = 0.8 is the friction coefficient between the tire and the ground, g = 9.8m / s 2 ; When planning the path, the speed curve adopts a trapezoidal acceleration-constant speed-deceleration model, and the acceleration and deceleration sections are t acc =t dec =0.5s, maximum acceleration a max =1.0m / s 2 .

8. The control method of a satellite-based automatic marking robot according to claim 1, characterized in that: The motor control software module in step S4 adopts a closed-loop control strategy: Motor speed n and target speed n ref The error is adjusted by the PID controller, and the control law is: Among them, K p =2.5, K i =0.1, K d =0.05, sampling period T s =10ms; The steering motor angle α is fed back through an optical encoder, with a resolution ≤0.01° and a repeat positioning accuracy ≤0.1°.

9. The control method of a satellite-based automatic marking robot according to claim 1, characterized in that: The path deviation in step S4 is corrected through a deviation adjustment algorithm; The specific deviation adjustment algorithm is: Real-time calculation of the current drawing point (x t ,y t )’s Euclidean distance deviation: If δ>δ max , then the speed correction is generated according to the robot kinematic model: Where K p =0.8, K d =0.2 is the proportional-differential coefficient; At the same time, adjust the motor steering angle α: Among them, θ z is the current heading angle, and the corrected motor control command is sent to the drive module through the CAN bus.

10. The control method of a satellite-based automatic marking robot according to claim 1, characterized in that: The verification method of the laser rangefinder in step S5 includes: Ten detection points are arranged along the drawn pattern, and each point is measured 3 times to take the average value, and the root mean square error is calculated: If RMSE>1cm, the automatic rework process is triggered, and the rework path adopts a spiral covering algorithm with a pitch of p=0.2m and a covering width of w=0.05m.

Images