A convex hull model-based robot obstacle perception and autonomous obstacle avoidance method

Abstract

A robot obstacle perception and autonomous obstacle avoidance method based on a convex hull model is proposed. The method comprises: 1. Acquiring robot localization and agricultural scene information using LiDAR and differential RTK; 2. Separating ground point clouds and obstacle point clouds by comprehensively analyzing land slope conditions through a sector-based ground segmentation algorithm; 3. Performing cluster analysis on the obstacle point clouds using Euclidean clustering and Graham scanning to establish an obstacle convex hull model representation; and determining inter-frame relationships and motion prediction information of obstacles using a multi-feature nearest neighbor algorithm and Kalman prediction; 4. Establishing a finite state machine decision system, combined with environmental perception and prediction features, to provide reference path guidance; and 5. Using a spatiotemporal joint obstacle avoidance path planning algorithm based on MPC, establishing obstacle avoidance constraints based on scale transformation, and optimizing the solution to obtain the obstacle avoidance path and optimal control input. This application effectively improves the accuracy of obstacle detection and the robustness of the obstacle avoidance system, ensuring the efficiency and safety of robot operation.

Description

Technical Field

[0001] This invention relates to the field of intelligent agricultural equipment technology, specifically to a robot obstacle perception and autonomous obstacle avoidance method based on a convex hull model. Background Technology

[0002] As an important branch of agricultural mechanization and intelligentization, robot automatic navigation technology is of great significance in saving labor costs, reducing the safety risks of operators, reducing the dependence of agricultural production on agricultural labor, improving operational accuracy, and promoting intelligent agricultural management.

[0003] Robot navigation systems mainly consist of three modules: perception, decision-making, and control. Existing perception and decision-making planning mechanisms have weak adaptability to dynamic environmental changes. Robot operation paths are often pre-generated before operation, thus lacking effective mechanisms for handling emergencies. Their path planning and obstacle avoidance strategies lack flexibility and adaptability, making them unable to effectively cope with sudden changes in the environment and limiting the practical application of robots in complex environments. Therefore, designing highly intelligent and robust robot obstacle perception methods and safe, highly stable obstacle avoidance path planning methods tailored to the dynamic and static obstacle characteristics in agricultural scenarios is of significant scientific importance and practical value for promoting the automation and intelligence of agricultural equipment in my country and improving agricultural productivity.

[0004] The existing technologies are compared as follows:

[0005] Compare with Patent 1: Real-time Obstacle Avoidance Planning Method Based on A* Algorithm, Authorization Announcement Date: 2023 / 08 / 29, Authorization Announcement No.: CN113701780B. This invention calculates the obstacle avoidance target point based on the set obstacle size using a search algorithm, uses the A* algorithm for initial obstacle avoidance path planning, determines whether the newly sampled state point meets the robot's kinematic constraints by judging the maximum turning angle, and uses B-spline curves to fit the path.

[0006] Compared to the obstacle avoidance algorithms used in this method, the A* algorithm requires the creation of a local grid map, resulting in higher memory burden and processing pressure; the B-spline algorithm optimizes the position of control points in the curve, making it difficult to constrain the robot's kinematics. In contrast, the MPC optimization algorithm used in this method optimizes the robot's state and system input in the spatiotemporal domain. By establishing constraints through the robot's Ackerman kinematic model, it can better optimize the robot's running state, while improving the smoothness of the generated path and the smoothness of the power input, thus improving operational comfort and safety.

[0007] Comparing Patent 2: A Robot Obstacle Avoidance System and Dynamic Path Planning Method Based on Virtual Target Points and Virtual Gravitational Fields, Application Publication Date: 2023 / 06 / 23, Application Publication Number: CN116300911A. This method employs downsampling of lidar point clouds, selection of detection areas, removal of ground point clouds, clustering and sorting of obstacle point clouds, and utilizes an improved artificial potential field method to generate obstacle avoidance paths.

[0008] Compared to the obstacle perception algorithms used in this method, although both utilize LiDAR point clouds and perform preprocessing, ground segmentation, and clustering, the ground segmentation algorithm used in this patent is based on a planar model, fitting all points in the point cloud to a plane. In agricultural scenarios, the land generally has significant undulations, and there may also be local slopes. Simply fitting the ground with a planar model leads to a large amount of erroneous obstacle information from the ground. This method uses a sector-based segmentation algorithm, projecting the point cloud onto a distance-angle plane in different sectors and fitting the ground with piecewise polygonal lines. This effectively detects local ground undulations and identifies slopes exceeding a certain limit as obstacles.

[0009] This patent uniformly uses the OBB bounding box model to represent obstacles. Compared with the convex polygon representation used in this method, while the OBB bounding box model can extract obstacle height information, it loses a lot of obstacle feature information. In addition, in agricultural scenarios, many obstacles are not rectangular. Using convex polygon representation can better abstract the current obstacle feature information and provide reasonable obstacle avoidance constraints for subsequent obstacle avoidance algorithms.

[0010] Compared to the obstacle avoidance algorithm used in this method, the obstacle avoidance algorithm based on artificial potential fields used in this patent addresses the possibility of traditional artificial potential field methods getting stuck in local optima by setting virtual target points and adaptively generating multiple obstacle avoidance path clusters. This method can provide the robot with a virtual guiding force, thereby achieving local obstacle avoidance. However, this method also suffers from the problem of too many parameters, making optimization difficult. When the parameters are set inappropriately, operational safety cannot be guaranteed, and path oscillation problems also exist. For the robot's kinematic constraints and minimum turning radius, this method uses a method of first generating path clusters and then filtering them, which lacks flexibility. The obstacle avoidance planning algorithm based on MPC used in this method, guided by a finite vector machine decision system, can effectively ensure operational safety.

[0011] Compare with Patent 3: Active Safety Control Method, Intelligent Device, and Storage Medium for Vehicles, Authorization Announcement Date: 2024 / 10 / 01, Authorization Announcement Number: CN118514685B. This method collects sensor data frame sequences, including environmental perception features and vehicle state features; it inputs the data frame sequences into an active safety model based on an end-to-end model for processing to obtain the active safety control decision at the target time.

[0012] In contrast to the decision-making mechanism used in this method, the patent employs an end-to-end proactive safety decision-making model based on neural networks and attention mechanisms. Training this model relies on sample data and labeled information from multiple scenarios, placing high demands on the quantity and quality of the training set. Furthermore, using deep learning models for safety control lacks interpretability and is insufficiently adaptable to obstacle avoidance tasks in varying scenarios. This method, however, uses rule-based finite state machine decision-making. By adding rules, it can better handle low-probability events and extreme cases, resulting in higher reliability. Summary of the Invention

[0013] To address the above problems, this invention proposes a robot obstacle perception and autonomous obstacle avoidance method based on a convex hull model, aiming to solve the collision accidents that may occur when wheeled robots are moving, and improve the safety and efficiency of robot operations.

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

[0015] A robot obstacle perception and autonomous obstacle avoidance method based on a convex hull model, characterized by the following steps:

[0016] Step 1: Use the navigation information module to obtain the global task objective T for farmland operations. k k = 1, 2, ..., n, read the vertex coordinates of the polygonal work area; use differential RTK and IMU to obtain the robot's current localization X-axis. k and attitude information R k Use lidar to acquire the original point cloud P0;

[0017] Step 2: Preprocess the original point cloud P0 using point cloud statistical filtering and voxel filtering to remove some outliers and reduce the number of points. Then, use a ground segmentation algorithm based on sector fitting to detect ground slope conditions and separate the ground point cloud to obtain the ground point cloud P. g and obstacle point cloud P o ;

[0018] Step 3: Perform clustering, inter-frame correlation, and motion state prediction on the separated obstacle point cloud to obtain the obstacle set O represented by convex polygons. i And the risks encountered {σ,V,t} TC};

[0019] Step 4: Using a finite state machine-based decision-making algorithm, combined with the current robot motion state and encountered risks, calculate and generate the reference state S. r ;

[0020] Step 5: Using a spatiotemporal joint obstacle avoidance path planning algorithm based on MPC and scale transformation constraints, an optimized obstacle avoidance path is generated based on the reference path, and given in the form of robot state variables, acceleration and turning angle.

[0021] As a further improvement to the present invention, the sensor installation and operation method in step 1 is as follows:

[0022] The dual RTK antennas are horizontally arranged and installed on the top of the robot, 0.2 to 0.3 meters from the rear end. The inertial sensor is fixed to the center of the bottom of the robot's cockpit via a bracket, ensuring that the measurement axis is aligned with the robot's axis. The VLP16-line lidar is installed on the robot's engine cover, 0.1 to 0.2 meters from the front, via an adjustable bracket. The bracket angle is adjusted to ensure that the horizontal line of the lidar's center is parallel to the vehicle's chassis plane, and the original point cloud P0 of the farmland is obtained.

[0023] As a further improvement to the present invention, the point cloud preprocessing and ground segmentation algorithm in step 2 includes the following steps:

[0024] (3-1) Using coordinate transformation operations, the original farmland point cloud P0 in the vehicle coordinate system is transformed to the global coordinate system, resulting in the local point cloud P in the global coordinate system. 0.1 ;

[0025] (3-2) Using point cloud statistical filtering, for point cloud P 0.1 Each point p in i Calculate the mean μ and variance σ of the Euclidean distances between n points in the neighborhood of the point, remove the point cloud with Euclidean distances outside the range of μ-kσ and μ+kσ, and obtain the filtered point cloud P1.

[0026] (3-3) Filter the point cloud P1 according to the interval angle Δα. s Divide into n s The segment is divided into fan-shaped sections, and the point clouds in the corresponding segments are dimensionality-reduced and projected onto the corresponding sectors based on the distance d from the center point; according to the set interest space range (d... min ,d max The point cloud is cropped to obtain point cloud P2, which is then divided into n equally spaced points in each sector. b part;

[0027] (3-4) In each segment, take the first n... l Given a point (d, h) with the minimum height, find the average height h. aDetermine the coordinates of segment i. Fit a straight line l based on the coordinates of the midpoint cloud of the segment. i :h=k i *d+b i Ground point P is obtained by separating the points based on the equation of the straight line. gi Non-ground point P oi When the slope of the piecewise straight line is k i Greater than the allowable slope k th When fitting, the slopes of the straight lines in the preceding and following segments are used; iterate n times. iter Next, the ground point cloud P was finally segmented. g Non-ground obstacle point cloud P o .

[0028] As a further improvement to the present invention, step 3, which involves point cloud clustering, inter-frame correlation, and motion state prediction, includes the following steps:

[0029] (4-1) Construct a 3D KD tree using obstacle point clouds and set clustering parameters; use the Euclidean clustering algorithm to randomly select initial points, perform nearest neighbor search, add the searched points to the point set, repeat this process until no other points are added, and obtain several obstacle point cloud clusters {C i};

[0030] (4-2) Reduce the obstacle point cloud cluster to a 2D plane. For point clouds of different frames, use the Graham scanning algorithm to construct the convex hull of the obstacle point cloud and remove obstacles with a length l smaller than the threshold. th Establish the obstacle set by constructing a convex polygon of obstacles.

[0031] (4-3) Use the multi-feature nearest neighbor algorithm to match and track obstacles in the previous and next frames, calculate the difference function D(i,j), establish the difference correlation matrix D between the previous and next frames, and compare it with the set difference threshold D according to the nearest neighbor principle. th By comparison, matching obstacles are selected to establish a spatiotemporal joint obstacle set.

[0032]

[0033] Among them, A k ∈R m×2 and b k ∈R m×1 Let be the shape parameters of the convex polygon at time k, and m be the number of sides of the convex polygon;

[0034] (4-4) Based on the position of obstacles in the five frames before and after the spatiotemporal joint obstacle set, establish the minimum circumcircle. When the diameter of the circumcircle is less than the set judgment threshold d, it is regarded as a static obstacle; otherwise, it is a dynamic obstacle.

[0035] (4-5) Using the Kalman prediction algorithm, the motion of dynamic obstacles is predicted based on historical positions, and the deviation σ between the predicted trajectory and the current heading is obtained; the covariance ellipsoid volume V is used to measure the overall uncertainty.

[0036]

[0037] Where, λ i Let P be the motion prediction covariance matrix at time k. k The i-th eigenvalue is given, and Γ(·) is the gamma function. The expected collision time t is calculated based on the obstacle velocity and the robot's current velocity. TC .

[0038] As a further improvement to the present invention, the decision-making algorithm based on finite state machines in step 4 includes the following steps:

[0039] (5-1) Based on the robot's working environment and working objectives, establish a finite state machine that includes five motion path states (following, emergency braking, left turn, right turn, and reversing) and two speed sub-states (acceleration and deceleration);

[0040] (5-2) Establish the input and transition conditions for each motion state and velocity sub-state, set the termination state conditions and enter the initialization state;

[0041] (5-3) Based on the perceptual information {σ,V,t} obtained in step 3 TC} and current vehicle status S k Used as input to a state machine for state transitions;

[0042] (5-4) According to the given decision system frequency F d Determine the current machine state, calculate the reference target point, and generate the reference state S using the Dubins curve. r ;

[0043] (5-5) When the destination is detected, enter the termination state and issue a termination command.

[0044] Step 5 of the spatiotemporal joint obstacle avoidance path planning algorithm based on MPC and scale transformation constraints includes the following steps:

[0045] (6-1) Establish a kinematic model of the robot based on wheeled Ackerman steering, set the optimization time domain N, and set the state constraint S according to the physical constraints of farmland. lim Input constraints u are set according to the control system limits. lim and control input objective function J u ;

[0046] (6-2) According to reference state S r Define the state error Js Based on the current state, establish initial state constraints and set the terminal error objective function J. N Set the state and input smoothness objective function J xs and J us ;

[0047] (6-3) Obtain the obstacle set given in step 4 Obstacle avoidance constraints are established using a scaling transformation algorithm:

[0048]

[0049] (6-4) Based on the overall optimization objective J:

[0050] J = J N +J s +J u +J xs +J us

[0051] The optimization constraints include state constraints, input constraints, initial state constraints, and obstacle avoidance constraints. A nonlinear optimization problem is established, with the variables to be optimized being the state variable S and the input variable u. The IPOPT solver is used to optimize and solve the problem to obtain the optimized state variable and the input variable.

[0052] (6-5) Send the first optimized input to the executor for execution, update the system state, and repeat steps (6-2) to (6-4). Use the previous optimization result as the initial input to the optimizer for optimization and solution.

[0053] Beneficial effects:

[0054] This invention discloses a robot obstacle perception and autonomous obstacle avoidance method based on a convex hull model. The method uses a 16-line LiDAR combined with differential RTK to acquire robot localization and agricultural scene information; it separates ground point clouds and obstacle point clouds by comprehensively analyzing land slope conditions through a sector-based ground segmentation algorithm; it performs cluster analysis on the obstacle point clouds using Euclidean clustering and Graham scanning to establish obstacle convex hull representations; it determines inter-frame connections between obstacles using a multi-feature nearest neighbor algorithm and Kalman prediction; and it establishes a finite state machine decision system based on robot spatiotemporal information to guide a spatiotemporal joint obstacle avoidance path planning algorithm based on MPC, establishes obstacle avoidance constraints based on scale transformation, and optimizes the solution to obtain the obstacle avoidance path and optimal control input. This robot obstacle perception and avoidance method comprehensively considers the characteristics of obstacles in agricultural scenes, performs spatiotemporal joint detection and analysis of potential dynamic obstacles, effectively improves the accuracy of obstacle detection and the robustness of the obstacle avoidance system, and ensures the efficiency and safety of unmanned agricultural production operations. Attached Figure Description

[0055] Figure 1 This is a flowchart of the method disclosed in this invention;

[0056] Figure 2 This is a schematic diagram of the sensor installation and operation method disclosed in this invention;

[0057] Figure 3 This is a schematic diagram of the ground segmentation algorithm disclosed in this invention;

[0058] Figure 4 This is a flowchart of the point cloud convex hull algorithm disclosed in this invention;

[0059] Figure 5 This is a flowchart of the obstacle multi-feature nearest neighbor matching algorithm disclosed in this invention;

[0060] Figure 6 This is a schematic diagram of the classification of dynamic and static obstacles disclosed in this invention;

[0061] Figure 7 This is a schematic diagram of the finite state machine structure disclosed in this invention;

[0062] Figure 8 This is a schematic diagram of the MPC algorithm structure disclosed in this invention;

[0063] Figure 9 This is a schematic diagram of obstacle avoidance constraints based on scale transformation disclosed in this invention. Detailed Implementation

[0064] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments:

[0065] This invention discloses a robot obstacle perception and autonomous obstacle avoidance method based on a convex hull model. The implementation flowchart of the method is as follows. Figure 1 As shown, it includes the following steps:

[0066] Step 1: Use the navigation information module to obtain the global task objective T for farmland operations. k (k = 1, 2, ..., n), read the vertex coordinates of the polygonal work area and obtain the robot's current localization X at the current moment. k and attitude information R k ;

[0067] The navigation information module consists of a differential RTK module, an inertial sensor module, and a wireless data transmission module, and its installation method is as follows: Figure 2As shown, dual RTK antennas are horizontally arranged and fixed at a distance of 0.2–0.3 m from the rear end of the robot. The inertial sensor is fixed to the center of the top of the robot's cockpit via a bracket, ensuring that the measurement axis is aligned with the robot's axis. The wireless data transmission module is installed inside the cockpit and communicates with the host computer using antennas to obtain the overall farmland operation task objectives, including the segmented operation path, the latitude and longitude coordinates of the operation vertices, and the coordinates of the operation starting point. Through a data fusion algorithm, the RTK differential data and the inertial sensor acceleration data are fused to obtain the robot's relative position X relative to the operation starting point. k and posture R k .

[0068] Step 2: Obtain the original point cloud of farmland using LiDAR and perform point cloud preprocessing. Use an improved ground segmentation algorithm based on sector fitting to detect ground slope conditions and separate the ground point cloud. The specific steps include the following:

[0069] (2-1) The VLP16-line lidar is mounted on the robot engine cover at a distance of 0.1 to 0.2 m from the front using an adjustable bracket. At the same time, the bracket angle is adjusted to ensure that the horizontal line of the lidar center is parallel to the vehicle chassis plane, and the original point cloud P0 of the farmland is obtained.

[0070] (2-2) Based on position X k and posture R k An augmented rotation matrix T is established, and coordinate transformation operations are used to transform the original farmland point cloud in the vehicle coordinate system to the global coordinate system, resulting in a local point cloud P in the global coordinate system. 0.1 :

[0071]

[0072] (2-3) For point cloud P 0.1 A point p i Calculate the mean and variance of the Euclidean distance between the point and its n neighbors:

[0073]

[0074] Where d i For point p i The Euclidean distance between p and a point in its neighborhood. i A point in the neighborhood and p i If the Euclidean distance to point P is within the range (μ-kσ, μ+kσ), then the point is retained; otherwise, it is considered an outlier and removed. Traverse P 0.1 Statistical filtering is performed on all points to obtain point cloud P1.

[0075] (2-4) such as Figure 3As shown, the preprocessed lidar point cloud P1 is divided according to the interval angle Δα. s Divide into n s Segmented sector division:

[0076]

[0077] For each point p in point cloud P1 i =(x i ,y i ,h i According to the distance d from the center point i Dimensionally reduce and project to the corresponding sector, corresponding point p is =(d i ,h i ):

[0078]

[0079] Based on the set interest space range d lim =(d min ,d max The point cloud is cropped to obtain point cloud P2 = {p is}

[0080] (2-5) Divide the point cloud P2 into n equally spaced segments in each sector. b In each segment, take the first n... l Given a point (d, h) with the minimum height, find the average height h. a Determine the coordinates of segment i. i = 1, 2, ..., n b For each segment, a straight line is fitted based on the point cloud within that segment. For the i-th segment, let the fitted curve be l. i :h=k i *d+b i There exists a matrix-form error formula:

[0081] H i =D i ·θ i +∈ i

[0082] in, The height vector of all points; Represents the augmented distance matrix; θ i =[k i ,b i ] T Let be the parameter vector to be solved, ∈ i Let be the error vector. For minimizing the objective S(θ), we have:

[0083]

[0084] The parameter vector θ can be obtained by using the least squares algorithm formula:

[0085]

[0086] Based on the equation of the straight line, traverse all points in the segment, and when the height distance between a point and the line is greater than the judgment threshold h... th When it is determined to be a non-ground point, the ground point P is obtained. gi Non-ground point P oi When the slope ki of the piecewise straight line is greater than the allowable slope k th When using the slope of the straight line segment before and after the curve, the slope of the fitted curve is replaced with the slope of the straight line segment before and after the curve segment.

[0087] (2-6) Repeat steps (2-4) and (2-5) for the ground points, iterating n times. iter Next, the ground point cloud P was finally segmented. g Non-ground obstacle point cloud P o ;

[0088] Step 3: Perform clustering, inter-frame correlation, and motion state prediction on the separated obstacle point cloud to obtain the obstacle set O represented by convex polygons. i And the risks encountered {σ,V,t} TC The specific steps include the following:

[0089] (3-1) Construct a 3D KD tree using obstacle point clouds and set a clustering neighborhood distance threshold D. th Minimum number of points N min Using Euclidean clustering, initial points are randomly selected, and nearest neighbor search is performed. The searched points are added to the point set, and this process is repeated until no other points are added, resulting in several obstacle point cloud clusters.

[0090] C = {C i |i=1,2,…,m}

[0091] Where m is the number of obstacles.

[0092] (3-2) Reduce the obstacle set C to a 2D horizontal plane, and use the Graham scan algorithm to construct the convex hull of the obstacle point cloud. The algorithm flow is as follows: Figure 4 As shown. Finally, a convex polygon obstacle set is established.

[0093]

[0094] in, and n is the shape parameter of the convex polygon. iLet be the number of sides of the convex polygon. Perform the above operations on the point clouds of different frames to obtain the obstacle sets for each frame.

[0095] (3-3) Use the multi-feature nearest neighbor algorithm to match and track obstacles in the previous and next frames, and calculate the difference function D(i,j):

[0096] D(i,j)=ω1D dist +ω2D s +ω3D h +ω4D intensity

[0097] Where D dist Let D be the distance between the centroids of the two obstacles. s D represents the difference in obstacle area. h D represents the height difference of the obstacles. intensity Let D be the difference in average intensity of the obstacle point cloud. Assuming there are m and n obstacles in the preceding and following frames respectively, establish the difference correlation matrix D between the preceding and following frames:

[0098]

[0099] Based on the nearest neighbor principle and the set difference threshold D th By comparison, matching obstacles are selected. The matching algorithm process is as follows: Figure 5 As shown. Establish a spatiotemporal joint obstacle set.

[0100]

[0101] in, and Let n be the shape parameters of the convex polygon at time k. k Where N is the number of sides of the convex polygon, and N is the prediction time domain length;

[0102] (3-4) Based on the center positions of obstacles in the five frames before and after the spatiotemporal joint obstacle set, construct the minimum circumcircle, such as... Figure 6 As shown. When the diameter of the circumscribed circle is less than the set judgment threshold d. s When the obstacle is in a certain position, it is considered a static obstacle; otherwise, it is considered a dynamic obstacle.

[0103] (3-5) Using the Kalman prediction algorithm, motion prediction is performed on dynamic obstacles based on their historical locations. Assuming the dynamic obstacle motion model is a CA (Velocity Acceleration) model, the obstacle motion state vector X is defined as follows:

[0104] X = [x, y, v] x ,v y ,a x ,a y] T

[0105] Establish the state transition equation and observation equation by setting a sampling time interval Δt:

[0106] X k+1 =F·X k +w k

[0107] z k =H·X k +v k

[0108] Where F is the state transition matrix; H is the observation matrix; This represents process noise, indicating the jerk of the obstacle. The observation noise represents the uncertainty of the sensor's perception of obstacles.

[0109]

[0110] The Kalman prediction algorithm includes initialization, optimal estimation filtering, state prediction, and error prediction. For known obstacle observation data z1, z2, ..., z... N Predict the state at time k (k>N). And error covariance matrix P k ,have:

[0111]

[0112] Where F(k / N) is the state transition matrix from time N to time k, σ a Here is the process noise covariance matrix. Optimal state estimation at time N. It can be obtained by filtering the state variables at times 1 to N. This yields obstacle state estimation and state prediction.

[0113]

[0114] Find the intersection point c(x,y) of the predicted obstacle trajectory and the extension of the current heading, and calculate the deviation between the predicted trajectory and the current heading at the intersection point:

[0115]

[0116] Simultaneously, by predicting the covariance matrix P k The diagonal elements are used to calculate the uncertainty Var of the obstacle's trajectory under different state variables.

[0117] Var(x i ) = P ii i = 1, 2, ..., 6

[0118] Where, x i For any state in the obstacle's motion; the overall uncertainty is measured using the covariance ellipsoid volume V:

[0119]

[0120] Where, λ i Let P be the covariance matrix. k The i-th eigenvalue, Γ(·), is the gamma function.

[0121] Based on the obstacle's speed V o Robot's current speed V h The robot's current position r = (x0, y0) and the intersection point c = (x0, y0) of the predicted trajectory and the extension of the current heading. c ,y c The expected collision time t is calculated. TC :

[0122]

[0123] Step 4: Using a finite state machine-based decision-making algorithm, combined with the current robot motion state and encountered risks, calculate and generate the reference path state S. r Specifically, it includes the following steps:

[0124] (4-1) Based on the robot's operating environment and objectives, establish a finite state machine that includes five motion path states (following, emergency braking, left turn, right turn, and reversing) and two speed sub-states (acceleration and deceleration), such as... Figure 7 As shown;

[0125] (4-2) Establish the input and transition conditions for each motion state and velocity sub-state.

[0126] Set the expected collision time deceleration threshold and safety threshold when When, it indicates that the robot has entered a dangerous spacetime region and transitions to a deceleration state; when When the time is too short, it indicates that the distance to the obstacle is too close and there is a high risk of collision, so the system switches to emergency braking mode.

[0127] When the prediction uncertainty V is greater than the threshold V th When V reaches the threshold V, it indicates that the current obstacle's movement is highly random, and the system transitions to a deceleration state. th After a certain period of time, the transfer will enter an accelerated state.

[0128] when And V <V thWhen the system is in a relatively safe state, it can perform relatively aggressive maneuvers. If the obstacle angle σ > 0, it will turn left to accelerate and bypass it; otherwise, it will turn right to bypass it. An obstacle avoidance threshold is set. when When the obstacle has been bypassed, the system determines that it has been bypassed and transitions to the follow state.

[0129] (4-3) Based on the perceptual information {σ,V,t} obtained in step 3 TC} and current vehicle status S k Used as input to a state machine for state transitions;

[0130] (4-4) According to the given decision system frequency F d Determine the current state of the state machine and set the forward distance d. f Calculate the current robot's position on the global task path l i The projection point h on y = ax + b r =(x r ,y r ):

[0131]

[0132] Where r0 is the robot's current position vector. The forward vector d along the work path from the projection point is taken. f From the reference point, draw a perpendicular line to the obstacle to obtain the local target point. Connect the local target point and the current robot position using a Dubins curve to obtain the local reference path. Assign the target velocity v according to the acceleration and deceleration states. high v low and a max Obtain reference state S r ;

[0133] (4-5) When the distance to the target endpoint is detected to be less than the judgment threshold d th When the time comes, enter the termination state and issue the termination command F. e ;

[0134] Step 5: Use a spatiotemporal joint obstacle avoidance path planning algorithm based on MPC and scale transformation constraints. The structure of the MPC obstacle avoidance planning system is as follows: Figure 8 As shown, input the reference path S r The system predicts and estimates state errors using a predictive model, optimizes these errors, and generates an optimized obstacle avoidance path. The optimization results are presented in the form of robot state variables, acceleration, and steering angle. Specifically, the steps include:

[0135] (5-1) Establish a kinematic model of the robot based on wheeled Ackerman steering, with S = [x, y, v, θ] T Let u be a state variable, and be [a, δ].T For the input quantity, discretize it using Euler's formula:

[0136]

[0137] Define the optimization time domain N, and set state constraints S based on the physical limitations of the farmland. lim Input constraints u are set according to the control system limits. lim and control input objective function J u :

[0138]

[0139] Among them, w u To control the weight coefficients of the input objective function.

[0140] (5-2) Based on the reference state S given in step 4 r Define the state error J s :

[0141]

[0142] Based on the current state Establish initial state constraints:

[0143]

[0144] Take the last state point of the reference path As the final target point, the terminal error objective function J is set. N :

[0145]

[0146] Set the state and input smoothness objective function J xs and J us :

[0147]

[0148]

[0149] (5-3) Obtain the obstacle set given in step 4 Obstacle avoidance constraints are established using a scaling algorithm. The robot shape is represented by an axis-aligned bounding box, and the robot position p at time k is... k =[x k ,y k ] T and heading angle θ k The robot corresponds to the rectangular bounding box. The expression is as follows:

[0150]

[0151] in, For the heading angle θ k The corresponding rotation matrix, A∈R 4×2 and b∈R 4×1 For rectangular shape parameters, only related to the vehicle width l w Length l from the center point to the front and rear of the car a l b It is relevant and does not change over time.

[0152] Consider the obstacle set at time k

[0153]

[0154] Define the transformation coefficients α∈R ≥0 Scale the vehicle rectangle at time k:

[0155]

[0156] like Figure 9 As shown, for non-concentric rectangles, there always exists an α such that the transformed robot rectangle intersects the obstacle boundary at exactly one point, thus yielding a collision safety constraint based on scale transformation:

[0157]

[0158] (5-4) Based on the overall optimization objective and optimization constraints (including state constraints, input constraints, initial state constraints, obstacle avoidance constraints, and motion constraints), establish a nonlinear optimization problem:

[0159]

[0160] The IPOPT solver was used to optimize the solution, resulting in optimized state and input variables.

[0161] (5-5) Send the first optimized input to the executor for execution, update the system state, and repeat steps (5-2) to (5-4). Use the previous optimization result as the initial input to the optimizer for optimization, until the termination instruction F issued by the state machine is detected. e .

[0162] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any other way. Any modifications or equivalent changes made based on the technical essence of the present invention shall still fall within the scope of protection claimed by the present invention.

Claims

1. A robot obstacle perception and autonomous obstacle avoidance method based on a convex hull model, characterized in that, Includes the following steps: Step 1: Use the navigation information module to obtain the global task objective T for farmland operations. k k = 1, 2, ..., n, read the vertex coordinates of the polygonal work area; use differential RTK and IMU to obtain the robot's current localization X-axis. k and attitude information R k Use lidar to acquire the original point cloud P0; Step 2: Preprocess the original point cloud P0 using point cloud statistical filtering and voxel filtering to remove some outliers and reduce the number of points. Then, use a ground segmentation algorithm based on sector fitting to detect ground slope conditions and separate the ground point cloud to obtain the ground point cloud P. g and obstacle point cloud P o ; Step 3: Perform clustering, inter-frame correlation, and motion state prediction on the separated obstacle point cloud to obtain the obstacle set O represented by convex polygons. i And the risks encountered {σ, V, t} TC }; Step 4: Using a finite state machine-based decision-making algorithm, combined with the current robot motion state and encountered risks, calculate and generate the reference state S. r ; Step 5: Using a spatiotemporal joint obstacle avoidance path planning algorithm based on MPC and scale transformation constraints, an optimized obstacle avoidance path is generated based on the reference path, and given in the form of robot state variables, acceleration and turning angle.

2. The robot obstacle perception and autonomous obstacle avoidance method based on a convex hull model according to claim 1, characterized in that, The sensor installation and operation method in step 1 are as follows: The dual RTK antennas are horizontally arranged and installed on the top of the robot, 0.2 to 0.3 meters from the rear end. The inertial sensor is fixed to the center of the bottom of the robot's cockpit via a bracket, ensuring that the measurement axis is aligned with the robot's axis. The VLP16-line lidar is installed on the robot's engine cover, 0.1 to 0.2 meters from the front, via an adjustable bracket. The bracket angle is adjusted to ensure that the horizontal line of the lidar's center is parallel to the vehicle's chassis plane, and the original point cloud P0 of the farmland is obtained.

3. The robot obstacle perception and autonomous obstacle avoidance method based on a convex hull model according to claim 1, characterized in that, Step 2, the point cloud preprocessing and ground segmentation algorithm, includes the following steps: (3-1) Using coordinate transformation operations, the original farmland point cloud P0 in the vehicle coordinate system is transformed to the global coordinate system, resulting in the local point cloud P in the global coordinate system. 0.1 ; (3-2) Using point cloud statistical filtering, for point cloud P 0.1 Each point p in i Calculate the mean μ and variance σ of the Euclidean distances between n points in the neighborhood of the point, remove the point cloud with Euclidean distances outside the range of μ-kσ and μ+kσ, and obtain the filtered point cloud P1. (3-3) Filter the point cloud P1 according to the interval angle Δα. s Divide into n s Segmenting into fan shapes, and dividing the point cloud into corresponding segments. Dimensionally reduced projection is performed onto the corresponding sector based on the distance d from the center point; based on the set interest space range (d... min d max The point cloud is cropped to obtain point cloud P2, which is then divided into n equally spaced points in each sector. b part; (3-4) In each segment, take the first n... l Given a point (d, h) with the minimum height, find the average height h. a Determine the coordinates of segment i. Fit a straight line l based on the coordinates of the midpoint cloud of the segment. i h = k i *d+b i Ground point P is obtained by separating the points based on the equation of the straight line. gi Non-ground point P oi When the slope of the piecewise straight line is k i Greater than the allowable slope k th When fitting, the slopes of the straight lines in the preceding and following segments are used; iterate n times. iter Next, the ground point cloud P was finally segmented. g Non-ground obstacle point cloud P o .

4. The robot obstacle perception and autonomous obstacle avoidance method based on a convex hull model according to claim 1, characterized in that, Step 3, which involves point cloud clustering, inter-frame correlation, and motion state prediction, includes the following steps: (4-1) Construct a 3D KD tree using obstacle point clouds and set clustering parameters; use the Euclidean clustering algorithm to randomly select initial points, perform nearest neighbor search, add the searched points to the point set, repeat this process until no other points are added, and obtain several obstacle point cloud clusters {C i }; (4-2) Reduce the obstacle point cloud cluster to a 2D plane. For point clouds of different frames, use the Graham scanning algorithm to construct the convex hull of the obstacle point cloud and remove obstacles with a length l smaller than the threshold. th Establish the obstacle set by constructing a convex polygon of obstacles. (4-3) Use the multi-feature nearest neighbor algorithm to match and track obstacles in the previous and next frames, calculate the difference function D(i,j), establish the difference correlation matrix D between the previous and next frames, and compare it with the set difference threshold D according to the nearest neighbor principle. th By comparison, matching obstacles are selected to establish a spatiotemporal joint obstacle set. Among them, A k ∈R m×2 and b k ∈R m×1 Let be the shape parameters of the convex polygon at time k, and m be the number of sides of the convex polygon; (4-4) Based on the position of obstacles in the five frames before and after the spatiotemporal joint obstacle set, establish the minimum circumcircle. When the diameter of the circumcircle is less than the set judgment threshold d, it is regarded as a static obstacle; otherwise, it is a dynamic obstacle. (4-5) Using the Kalman prediction algorithm, the motion of dynamic obstacles is predicted based on historical positions, and the deviation σ between the predicted trajectory and the current heading is obtained; the covariance ellipsoid volume V is used to measure the overall uncertainty. Where, λ i Let P be the motion prediction covariance matrix at time k. k The i-th eigenvalue, Γ(·), is the gamma function. The expected collision time t is calculated based on the obstacle velocity and the robot's current velocity. TC .

5. The robot obstacle perception and autonomous obstacle avoidance method based on a convex hull model according to claim 1, characterized in that... The decision-making algorithm based on finite state machine in step 4 includes the following steps: (5-1) Based on the robot's working environment and working objectives, establish a finite state machine that includes five motion path states (following, emergency braking, left turn, right turn, and reversing) and two speed sub-states (acceleration and deceleration). (5-2) Establish the input and transition conditions for each motion state and velocity sub-state, set the termination state conditions and enter the initialization state; (5-3) Based on the perceptual information {σ, V, t} obtained in step 3 TC } and current vehicle status S k Used as input to a state machine for state transitions; (5-4) According to the given decision system frequency F d Determine the current machine state, calculate the reference target point, and generate the reference state S using the Dubins curve. r ; (5-5) When the destination is detected, enter the termination state and issue a termination command.

6. The robot obstacle perception and autonomous obstacle avoidance method based on a convex hull model according to claim 1, characterized in that, Step 5 of the spatiotemporal joint obstacle avoidance path planning algorithm based on MPC and scale transformation constraints includes the following steps: (6-1) Establish a kinematic model of the robot based on wheeled Ackerman steering, set the optimization time domain N, and set the state constraint S according to the physical constraints of farmland. lim Input constraints u are set according to the control system limits. lim and control input objective function J u ; (6-2) According to reference state S r Define the state error J s Based on the current state, establish initial state constraints and set the terminal error objective function J. N Set the state and input smoothness objective function J xs and J us ; (6-3) Obtain the obstacle set given in step 4 Obstacle avoidance constraints are established using a scaling transformation algorithm: (6-4) Based on the overall optimization objective J: J=J N +J s +J u +J xs +J us The optimization constraints include state constraints, input constraints, initial state constraints, and obstacle avoidance constraints. A nonlinear optimization problem is established, with the variables to be optimized being the state variable S and the input variable u. The IPOPT solver is used to optimize and solve the problem to obtain the optimized state variable and the input variable. (6-5) Send the first optimized input to the executor for execution, update the system state, and repeat steps (6-2) to (6-4). Use the previous optimization result as the initial input to the optimizer for optimization and solution.

Images