Robot obstacle sensing and autonomous obstacle avoidance method based on convex hull model

Abstract

The invention discloses a robot obstacle sensing and autonomous obstacle avoidance method based on a convex hull model. The method comprises the following steps: 1, acquiring robot positioning and agricultural scene information by using a laser radar and differential RTK; 2, comprehensively analyzing a land slope condition through a sector-based ground segmentation algorithm, and separating ground point clouds from obstacle point clouds; 3, performing clustering analysis on the obstacle point cloud by using a Euclidean clustering method and a Graham scanning method, and establishing an obstacle convex hull model for representation; using a multi-feature nearest neighbor algorithm and Kalman prediction to determine obstacle inter-frame relation and motion prediction information; 4, establishing a finite-state machine decision-making system, and providing reference path guidance in combination with environment perception prediction features; and 5, based on an MPC-based space-time joint obstacle avoidance path planning algorithm, establishing an obstacle avoidance constraint based on scale transformation, and performing optimization solution to obtain an obstacle avoidance path and optimal control input. According to the invention, the accuracy of obstacle detection and the robustness of an obstacle avoidance system are effectively improved, and the high efficiency and safety of the robot during operation are ensured.

Description

Technical Field

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

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

[0003] The robot navigation system is mainly composed of three modules: perception, decision-making and control. The existing perception and decision-making planning mechanisms have weak adaptability to dynamic changes in the environment. The robot's operation path is often pre-generated before the operation, so there is a lack of effective response mechanism for emergencies. Its path planning and obstacle avoidance strategies lack flexibility and adaptability, and thus cannot effectively respond to sudden changes in the environment, limiting the actual application effect of the robot in complex environments. Therefore, designing a highly intelligent and robust robot obstacle perception method and a safe and highly stable obstacle avoidance path planning method based on the characteristics of dynamic and static obstacles in agricultural scenes has important scientific significance and practical use value for promoting the automation and intelligence of my country's agricultural equipment and improving agricultural productivity.

[0004] The existing technology comparison is as follows:

[0005] Comparative Patent 1: Real-time obstacle avoidance planning method based on A-star algorithm, authorization announcement date, 2023 / 08 / 29, authorization announcement date number: CN113701780B. Based on the set obstacle size, this invention calculates the obstacle avoidance target point through a search algorithm, uses the A-star algorithm for initial obstacle avoidance path planning, determines whether the new sampled state point meets the robot kinematic constraints by judging the maximum steering angle, and uses the B-spline curve to fit the path.

[0006] Compared with the obstacle avoidance algorithm used in this method, the use of the A-star algorithm requires the establishment of a local grid map, which brings higher memory burden and processing pressure; the use of the B-spline algorithm optimization algorithm is to optimize the position of the control points in the curve, which is difficult to constrain the robot's kinematics. The MPC optimization algorithm used in this method optimizes the robot state and system input in the time and space domain. By establishing constraints through the robot's Ackerman kinematic model, the robot's running state can be better optimized, while improving the smoothness of the generated path and the smoothness of the power input, thereby improving the comfort and safety of the operation.

[0007] Comparative 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 uses the laser radar point cloud downsampling, detection area selection, ground point cloud removal, obstacle point cloud clustering and sorting, and uses the improved artificial potential field method to generate an obstacle avoidance path.

[0008] Compared with the obstacle perception algorithm used in this method, although both use LiDAR point clouds, and both perform preprocessing, ground segmentation, and clustering, the ground segmentation algorithm used in this patent is based on a plane model, which fits all points in the point cloud into a plane. In agricultural scenes, the land generally has large undulations, and there may also be certain slopes in some parts. Only using a plane to fit the ground will result in a large amount of erroneous obstacle information from the ground. This method uses a sector-based segmentation algorithm, which projects the point cloud onto the distance-angle plane in different sectors, and uses segmented polylines to fit the ground, which can effectively detect local undulations of the ground, and when the slope exceeds the limit, it is considered an obstacle.

[0009] This patent uniformly uses the OBB bounding box model to describe obstacles. Compared with the convex polygon representation used in this method, the OBB bounding box model can extract obstacle height information, but a large amount of obstacle feature information is lost. At the same time, in agricultural scenes, a large number of obstacles do not exist in rectangular form. 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 with the obstacle avoidance algorithm used in this method, the obstacle avoidance algorithm based on artificial potential field used in this patent sets a virtual target point and adaptively generates multiple obstacle avoidance path clusters in response to the situation that the traditional artificial potential field method may fall into the local optimum. This method can provide a virtual guiding force for the robot, thereby achieving local obstacle avoidance. However, this method also has the problem of too many parameters and difficulty in optimization. When the parameters are set unreasonably, the operation safety cannot be guaranteed, and there is also the problem of path oscillation. For the robot's kinematic constraints and minimum turning radius, the method used in this method is to generate path clusters first and then screen them, which lacks flexibility. The obstacle avoidance planning algorithm based on MPC adopted in this method can effectively ensure operation safety through the guidance of the finite vector machine decision system.

[0011] Comparative Patent 3: Active safety control method, intelligent device and storage medium for vehicles, authorization announcement date: 2024 / 10 / 01, authorization announcement number: CN118514685. This method collects sensor data frame sequences, including environmental perception features and vehicle status features; inputs the data frame sequence into an active safety model based on an end-to-end model for processing, and obtains an active safety control decision at the target moment.

[0012] Compared with the decision-making mechanism adopted by this method, the patent uses an end-to-end active safety decision-making model based on neural networks and attention mechanisms. The training of this model is based on sample data and annotation information of multiple scenarios, which places high demands on the quantity and quality of the training set. At the same time, the use of deep learning models for safety control lacks interpretability and is not adaptable enough to variable scene obstacle avoidance tasks. This method uses a rule-based finite state machine decision. By adding rules, it can better deal with the occurrence of low-probability events and extreme cases, and has higher reliability. Summary of the invention

[0013] In order to solve the above problems, the present invention proposes a robot obstacle perception and autonomous obstacle avoidance method based on a convex hull model, which aims to solve the collision accidents that may occur during the movement of the wheeled robot and improve the robot's operation safety and work efficiency.

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

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

[0016] Step 1: Use the navigation information module to obtain the global task target T of the farmland operation k , k = 1, 2, ..., n, read the vertex coordinates of the polygonal working area; use differential RTK and IMU to obtain the robot's own positioning X at the current moment k and attitude information R k ;Use LiDAR to obtain the original point cloud P0;

[0017] Step 2: Use point cloud statistical filtering and voxel filtering to preprocess the original point cloud P0, remove some outliers and reduce the number of point clouds, use the ground segmentation algorithm based on fan fitting to detect the ground slope conditions, separate the ground point cloud, and obtain the ground point cloud P g and obstacle point cloud P o ;

[0018] Step 3: Cluster, inter-frame association and motion state prediction are performed on the separated obstacle point cloud to obtain the obstacle set O represented by convex polygons. i and will encounter risks {σ,V,t TC};

[0019] Step 4: Use the decision-making algorithm based on the finite state machine to calculate and generate the reference state S based on the current robot motion state and the risk encountered. r ;

[0020] Step 5: Use the spatiotemporal joint obstacle avoidance path planning algorithm based on MPC and scale transformation constraints to generate an optimized obstacle avoidance path based on the reference path, which is given in the form of robot state quantity, acceleration and steering angle.

[0021] As a further improvement of the present invention, the sensor installation and working method in step 1 is:

[0022] The dual RTK antennas are horizontally arranged and installed on the top of the robot, 0.2 to 0.3 m away from the rear end. The inertial sensor is fixed at the center of the bottom of the robot cockpit through a bracket to ensure that the measuring axis is consistent with the robot axis. The VLP16-line laser radar is installed on the robot engine cover through an adjustable angle bracket, 0.1 to 0.2 m away from the front side. At the same time, the bracket angle is adjusted to ensure that the center horizontal line of the laser radar remains parallel to the chassis plane of the vehicle, and the original point cloud P0 of the farmland is read.

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

[0024] (3-1) Use coordinate transformation operation to transform the original point cloud P0 of the farmland in the vehicle coordinate system to the global coordinate system to obtain 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 Every point p i , calculate the mean μ and variance σ of the Euclidean distance of n points in the point area, remove the point cloud whose Euclidean distance is outside the range of μ-kσ,μ+kσ, and obtain the filtered point cloud P1;

[0026] (3-3) The filtered point cloud P1 is divided into two parts according to the interval angle Δα. s The division into n s Segment sector segmentation, the point cloud in the corresponding segment is projected to the corresponding sector according to the distance d from the center point; according to the set interest space range (d min ,d max ) point cloud is cropped to obtain point cloud P2, which is divided into n equally spaced points in each sector. b part;

[0027] (3-4) In each segment, take the first n l points with the smallest height (d,h), find the average height h a, determine the coordinate point of segment i Fitting straight line l according to the coordinate points of the segmented point cloud i :h=k i *d+b i , according to the straight line equation, we can separate the ground point P gi and non-ground point P oi , when the slope of the piecewise straight line k i Greater than the allowable slope k th When , the slope of the straight line of the front and back segments is used for fitting; iterate n iter Finally, the ground point cloud P is segmented. g and non-ground obstacle point cloud P o .

[0028] As a further improvement of the present invention, the point cloud clustering, inter-frame association and motion state prediction in step 3 include the following steps:

[0029] (4-1) Use the obstacle point cloud to build a 3D KD tree and set clustering parameters; use the Euclidean clustering algorithm to randomly select initial points, perform nearest neighbor search, add the search 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, use the Graham scanning algorithm to build the convex hull of the obstacle point cloud for different frame point clouds, and remove the points with a length less than the threshold l th The edges of the obstacle are used to create a convex polygon obstacle set.

[0031] (4-3) Use the multi-feature nearest neighbor algorithm to match and track the obstacles in the front and back frames, calculate the difference function D(i, j), establish the front and back frame difference correlation matrix D, and according to the nearest neighbor principle, compare it with the set difference threshold D th Compare, select matching obstacles, and establish a spatiotemporal joint obstacle set

[0032]

[0033] Among them, A k ∈R m×2 and b k ∈R m×1 is the shape parameter of the convex polygon at time k, and m is the number of edges of the convex polygon;

[0034] (4-4) According to the obstacle positions in the five frames before and after the spatiotemporal joint obstacle set, a minimum circumscribed circle is established. When the diameter of the circumscribed circle is less than the set judgment threshold d, it is considered a static obstacle, otherwise it is a dynamic obstacle;

[0035] (4-5) Using the Kalman prediction algorithm, the motion of the dynamic obstacle is predicted based on the historical position, 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] Among them, λ i is the k-time motion prediction covariance matrix P k The expected collision time t is calculated based on the obstacle speed and the current speed of the robot. TC .

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

[0039] (5-1) According to the robot's working environment and working objectives, a finite state machine is established, which includes five motion path states: following, emergency braking, left turn, right turn, and reverse, and two speed sub-states: acceleration and deceleration;

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

[0041] (5-3) According to the perception information {σ,V,t TC} and the current vehicle state S k As the state machine input, it performs state transfer;

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

[0043] (5-5) When it is detected that the end point has been reached, the termination state is entered and a termination instruction is issued.

[0044] 6. The robot obstacle perception and autonomous obstacle avoidance method based on convex hull model according to claim 1, characterized in that the spatiotemporal joint obstacle avoidance path planning algorithm based on MPC and scale transformation constraints in step 5 comprises the following steps:

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

[0046] (6-2) According to the reference state S r , define the state error J s , according to the current state, establish the 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 Establish obstacle avoidance constraints through the scale transformation algorithm:

[0048]

[0049] (6-4) According to the overall optimization goal J:

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

[0051] and optimization constraints, wherein the optimization constraints include state constraints, input constraints, initial state constraints, and obstacle avoidance constraints, and a nonlinear optimization problem is established, wherein the variables to be optimized are the state quantity S and the input quantity u, and the IPOPT solver is used to optimize and solve them to obtain the optimized state quantity and input quantity;

[0052] (6-5) The first optimized input quantity is sent to the actuator for execution, the system state is updated, and steps (6-2) to (6-4) are repeated, using the last optimization result as the initial input of the optimizer to perform optimization solution.

[0053] Beneficial effects:

[0054] The present invention discloses a robot obstacle perception and autonomous obstacle avoidance method based on convex hull model. The method uses a 16-line laser radar, combined with differential RTK to obtain robot positioning and agricultural scene information; through a sector-based ground segmentation algorithm, the land slope conditions are comprehensively analyzed to separate the ground point cloud and the obstacle point cloud; the obstacle point cloud is clustered and analyzed by the Euclidean clustering method and the Graham scanning method, and the obstacle convex hull representation is established; the obstacle inter-frame connection is determined by the multi-feature nearest neighbor algorithm and Kalman prediction; the robot's spatiotemporal information is integrated to establish a finite state machine decision system, guide the spatiotemporal joint obstacle avoidance path planning algorithm based on MPC, establish an obstacle avoidance constraint based on scale transformation, and optimize the solution to obtain the obstacle avoidance path and the optimal control input. The robot obstacle perception and obstacle avoidance method comprehensively considers the characteristics of agricultural scene obstacles, performs spatiotemporal joint detection and analysis for possible 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. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 is a flow chart of the method disclosed in the present invention;

[0056] Figure 2 It is a schematic diagram of the installation and working mode of the sensor disclosed in the present invention;

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

[0058] Figure 4 It is a flow chart of the point cloud convex hull algorithm disclosed in the present invention;

[0059] Figure 5 It is a flow chart of the obstacle multi-feature nearest neighbor matching algorithm disclosed in the present invention;

[0060] Figure 6 It is a schematic diagram of dynamic and static obstacle classification disclosed in the present invention;

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

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

[0063] Fig. 9 It is a schematic diagram of the obstacle avoidance constraint based on scale transformation disclosed in the present invention. DETAILED DESCRIPTION

[0064] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments:

[0065] The present invention discloses a robot obstacle perception and autonomous obstacle avoidance method based on a convex hull model. The implementation flow chart of the method is as follows: Figure 1 As shown, the following steps are included:

[0066] Step 1: Use the navigation information module to obtain the global task target T of the farmland operation k (k=1,2,…,n), read the vertex coordinates of the polygonal working area, and obtain the robot's own positioning 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. The installation method is as follows: Figure 2 As shown in the figure, the dual RTK antennas are arranged horizontally and installed at a fixed position on the top of the robot at a distance of 0.2 to 0.3 m from the rear end; the inertial sensor is fixed at the center of the top of the robot cockpit through a bracket to ensure that the measuring axis is consistent with the robot axis; the wireless data transmission module is installed in the cockpit and uses the antenna to communicate with the host computer to obtain the global farmland operation task objectives, including the segmented operation path, the longitude and latitude coordinates of the operation vertex, and the coordinates of the operation starting point; through the data fusion algorithm, the RTK differential data and the inertial sensor acceleration data are fused to obtain the relative position X of the robot itself relative to the operation starting point k and posture R k .

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

[0069] (2-1) Install the VLP16 laser radar on the robot engine cover 0.1 to 0.2 m from the front side through an adjustable angle bracket. At the same time, adjust the bracket angle to ensure that the center horizontal line of the laser radar is parallel to the vehicle chassis plane, and read the original point cloud P0 of the farmland;

[0070] (2-2) According to the position X k and posture R k , establish the augmented rotation matrix T, use the coordinate transformation operation to transform the original point cloud of the farmland in the vehicle coordinate system to the global coordinate system, and obtain the local point cloud P in the global coordinate system 0.1 :

[0071]

[0072] (2-3) For point cloud P 0.1 A point p in i , calculate the mean and variance of the Euclidean distance between it and the n points in its neighborhood:

[0073]

[0074] where d i For point p i The Euclidean distance to a point in its neighborhood. If p i A point in the neighborhood and p i If the Euclidean distance of is within the range of (μ-kσ,μ+kσ), the point is retained; otherwise, the point is considered an outlier and removed. 0.1 All points in the matrix are statistically filtered to obtain point cloud P1.

[0075] (2-4) Figure 3 As shown, the preprocessed laser radar point cloud P1 is divided into s The division into n s Segment sector segmentation:

[0076]

[0077] For each point p in the point cloud P1 i =(x i ,y i ,h i ), according to the distance d from the center point i Perform dimensionality reduction projection to the corresponding sector, corresponding to point p is =(d i ,h i ):

[0078]

[0079] According to 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 points in each sector. b In each segment, take the first n l points with the smallest height (d,h), find the average height h a , determine the coordinate point of segment i i=1,2,…,n b ; Each segment fits a straight line according to the point cloud in the segment. For the i-th segment, let the fitting curve be l i :h=k i *d+b i , there is a matrix error formula:

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

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

[0083]

[0084] Through the least squares algorithm formula, the parameter vector θ can be solved:

[0085]

[0086] According to the straight line equation, traverse all the points in the segment, when the height distance between the point and the straight line is greater than the judgment threshold h th When it is judged as a non-ground point, the ground point P is obtained. gi and non-ground point P oi ; When the segmented straight line slope ki is greater than the allowable slope slope k th When , the slope of the straight line of the front and rear segments is used instead of the slope of the fitting curve;

[0087] (2-6) Repeat steps (2-4) and (2-5) for the ground points, iterating n iter Finally, the ground point cloud P is segmented. g and non-ground obstacle point cloud P o ;

[0088] Step 3: Cluster, inter-frame association and motion state prediction are performed on the separated obstacle point cloud to obtain the obstacle set O represented by convex polygons. i and will encounter risks {σ,V,t TC}, specifically including the following steps:

[0089] (3-1) Use the obstacle point cloud to build a 3D KD tree and set the clustering neighborhood distance threshold D th , minimum number of points N min ; Use the Euclidean clustering algorithm to randomly select the initial point, perform the nearest neighbor search, add the search point to the point set, repeat this process until no other points are added, and obtain 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 scanning algorithm to build the convex hull of the obstacle point cloud. The algorithm flow is as follows: Figure 4 As shown. Finally, the obstacle convex polygon obstacle set is established

[0093]

[0094] in, and is the convex polygon shape parameter, n i is the number of convex polygon edges. Perform the above operations on point clouds in different frames to obtain obstacle sets in different frames.

[0095] (3-3) Use the multi-feature nearest neighbor algorithm to match and track the 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 is the distance between the centers of mass of the two obstacles, D s is the obstacle area difference, D h is the height difference of the obstacle, D intensity is the difference in the average intensity of the obstacle point cloud. Assuming that there are m and n obstacles in the previous and next frames respectively, the difference correlation matrix D of the previous and next frames is established:

[0098]

[0099] According to the nearest neighbor principle, the difference with the set threshold D th Compare and select matching obstacles. The matching algorithm process is as follows Figure 5 Establishing a spatiotemporal joint obstacle set

[0100]

[0101] in, and is the convex polygon shape parameter at time k, n k is the number of convex polygon edges, N is the length of the prediction time domain;

[0102] (3-4) According to the center position of the obstacle in the five frames before and after the spatiotemporal joint obstacle set, the minimum circumscribed circle is established, such as Figure 6When the diameter of the circumscribed circle is less than the set judgment threshold d s When , it is considered as a static obstacle, otherwise it is a dynamic obstacle;

[0103] (3-5) Use the Kalman prediction algorithm to predict the motion of dynamic obstacles based on historical positions. Assuming that the dynamic obstacle motion model is the CA (Velocity Acceleration) model, define the obstacle motion state vector X:

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

[0105] Set the sampling time interval Δt to establish the state transfer equation and observation equation:

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

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

[0108] Among them, F is the state transfer matrix; H is the observation matrix; is the process noise, which represents the jerk of the obstacle; is the observation noise, which represents the uncertainty of the sensor's perception of obstacles.

[0109]

[0110] The process of 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 the error covariance matrix P k ,have:

[0111]

[0112] Among them, F(k / N) is the state transfer matrix from time N to time k, σ a is the process noise covariance matrix. The optimal state estimate at time N It can be obtained by filtering the state quantity at time 1 to N. Obstacle state estimation and state prediction are obtained:

[0113]

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

[0115]

[0116] At the same time, by predicting the covariance matrix P k Diagonal elements, calculate the uncertainty Var of the obstacle's motion trajectory in different state quantities:

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

[0118] Among them, x i is any state of the obstacle motion state; the covariance ellipsoid volume V is used to measure the overall uncertainty:

[0119]

[0120] Among them, λ i is the covariance matrix P k The i-th eigenvalue of , Γ(·) is the gamma function.

[0121] According to the obstacle speed V o 、Current speed of the robot V h , the robot's current position r = (x0, y0) and the intersection of the predicted trajectory and the extension line of the current heading c = (x c ,y c ), calculate the expected collision time t TC :

[0122]

[0123] Step 4: Use the decision-making algorithm based on the finite state machine to calculate and generate the reference path state S based on the current robot motion state and the risk encountered r , specifically including the following steps:

[0124] (4-1) According to the robot's working environment and working objectives, a finite state machine is established, which includes five motion path states: following, emergency braking, left turn, right turn, and reverse, and two speed sub-states: acceleration and deceleration. Figure 7 As shown;

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

[0126] Set the time-to-collision deceleration threshold and safety threshold when When , it means that the robot has entered the dangerous space-time area and transferred to the deceleration sub-state; when When the vehicle is too close to an obstacle, there is a high risk of collision, and the vehicle switches to emergency braking mode.

[0127] When the prediction uncertainty V is greater than the threshold V th When V returns to the threshold value V th After a certain period of time, the transfer enters the accelerated state.

[0128] when And V <V th When , it means that the system is in a relatively safe state and can perform relatively aggressive actions. If the angle σ>0, choose to turn left and accelerate to bypass the obstacle, otherwise turn right to bypass the obstacle; Set the obstacle avoidance judgment threshold when , it is determined that the current obstacle has been bypassed and the state is transferred to the following state.

[0129] (4-3) According to the perception information {σ,V,t TC} and the current vehicle state S k As the state machine input, it performs state transfer;

[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 in the global operation path l i : Projection point h on y=ax+b r =(x r ,y r ):

[0131]

[0132] Among them, r0 is the current position vector of the robot. f The reference point is drawn, and the vertical line intersects with the obstacle to obtain the local target point. The local target point and the current robot position are connected using the Dubins curve to obtain the local reference path. The target speed v is assigned according to the acceleration and deceleration state. high 、v low and a max , obtain the reference state S r ;

[0133] (4-5) When the distance to the end point of the operation target is detected to be less than the judgment threshold d th When the state is terminated, the termination instruction F is issued. e ;

[0134] Step 5: Use the spatiotemporal joint obstacle avoidance path planning algorithm based on MPC and scale transformation constraints. The MPC obstacle avoidance planning system structure is as follows: Figure 8 As shown, input reference path S r , the prediction model is used to predict the estimated state error, and the error is optimized to generate an optimized obstacle avoidance path. The optimization result is given in the form of robot state quantity, acceleration and steering angle. The specific steps include:

[0135] (5-1) Establish a robot kinematic model based on wheeled Ackerman steering, with S = [x, y, v, θ] T is the state quantity, u=[a,δ] T As input quantity, use Euler formula for discretization:

[0136]

[0137] Set the optimization time domain N, and set the state quantity constraint S according to the physical limitations of the farmland lim , set the input constraint u according to the control system constraints lim , and the control input objective function J u :

[0138]

[0139] Among them, w u Enter the objective function weight coefficient for control.

[0140] (5-2) According to the reference state S given in step 4 r , define the state error J s :

[0141]

[0142] According to the current status Establish initial state constraints:

[0143]

[0144] Take the last state point of the reference path As the final target point, set the terminal error objective function J 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 through the scale transformation algorithm. The robot shape is represented by an axis-aligned bounding box. For the robot position p at time k k =[x k ,y k ] T and heading angle θ k , the robot corresponds to the rectangular bounding box It is expressed as:

[0150]

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

[0152] Consider the set of obstacles at time k

[0153]

[0154] Define the transformation coefficient α∈R ≥0 , scale the vehicle rectangle at time k:

[0155]

[0156] like Fig. 9 As shown in the figure, for non-concentric rectangles, there always exists α, so that the transformed robot rectangle has only one intersection with the obstacle boundary, and the collision safety constraint based on scale transformation can be obtained:

[0157]

[0158] (5-4) According to the overall optimization goal and optimization constraints (including state constraints, input constraints, initial state constraints, obstacle avoidance constraints, and motion constraints), a nonlinear optimization problem is established:

[0159]

[0160] Use IPOPT solver to optimize and solve it, and obtain the optimized state quantity and input quantity;

[0161] (5-5) The first optimized input is sent to the actuator for execution, the system state is updated, and steps (5-2) to (5-4) are repeated. The last optimization result is used as the initial input of the optimizer to perform optimization and solve until the termination instruction F issued by the state machine is detected. e .

[0162] The above description is only a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or equivalent change made based on the technical essence of the present invention still falls within the scope of protection required by the present invention.

Claims

1. A robot obstacle perception and autonomous obstacle avoidance method based on a convex hull model, characterized in that: The following steps are involved: Step 1: Use the navigation information module to obtain the global task target T of the farmland operation k , k = 1, 2, ..., n, read the vertex coordinates of the polygonal working area; use differential RTK and IMU to obtain the robot's own positioning X at the current moment k and attitude information R k ;Use LiDAR to obtain the original point cloud P0; Step 2: Use point cloud statistical filtering and voxel filtering to preprocess the original point cloud P0, remove some outliers and reduce the number of point clouds, use the ground segmentation algorithm based on fan fitting to detect the ground slope conditions, separate the ground point cloud, and obtain the ground point cloud P g and obstacle point cloud P o ; Step 3: Cluster, inter-frame association and motion state prediction are performed on the separated obstacle point cloud to obtain the obstacle set O represented by convex polygons. i and will encounter risk {σ, V, t TC }; Step 4: Use the decision-making algorithm based on the finite state machine to calculate and generate the reference state S based on the current robot motion state and the risk encountered. r ; Step 5: Use the spatiotemporal joint obstacle avoidance path planning algorithm based on MPC and scale transformation constraints to generate an optimized obstacle avoidance path based on the reference path, which is given in the form of robot state quantity, acceleration and steering angle.

2. The robot obstacle perception and autonomous obstacle avoidance method based on convex hull model according to claim 1, characterized in that: The sensor installation and working mode 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 m away from the rear end. The inertial sensor is fixed at the center of the bottom of the robot cockpit through a bracket to ensure that the measuring axis is consistent with the robot axis. The VLP16-line laser radar is installed on the robot engine cover through an adjustable angle bracket, 0.1 to 0.2 m away from the front side. At the same time, the bracket angle is adjusted to ensure that the center horizontal line of the laser radar remains parallel to the chassis plane of the vehicle, and the original point cloud P0 of the farmland is read.

3. The robot obstacle perception and autonomous obstacle avoidance method based on convex hull model according to claim 1, characterized in that: The point cloud preprocessing and ground segmentation algorithm in step 2 includes the following steps: (3-1) Use coordinate transformation operation to transform the original point cloud P0 of the farmland in the vehicle coordinate system to the global coordinate system to obtain 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 Every point p i , calculate the mean μ and variance σ of the Euclidean distance of n points in the point area, remove the point cloud whose Euclidean distance is outside the range of μ-kσ,μ+kσ, and obtain the filtered point cloud P1; (3-3) The filtered point cloud P1 is divided into two parts according to the interval angle Δα. s The division into n s Segment fan segmentation, the corresponding point cloud in the segment According to the distance d from the center point, the dimensionality reduction projection is performed to the corresponding sector; according to the set interest space range (d min , d max ) point cloud is cropped to obtain point cloud P2, which is divided into n equally spaced points in each sector. b part; (3-4) In each segment, take the first n l points with the smallest height (d, h), find the average height h a , determine the coordinate point of segment i Fitting straight line l according to the coordinate points of the segmented point cloud i :h=k i *d+b i , according to the straight line equation, we can separate the ground point P gi and non-ground point P oi , when the slope of the piecewise straight line k i Greater than the allowable slope k th When , the slope of the straight line of the front and back segments is used for fitting; iterate n iter Finally, the ground point cloud P is segmented. g and non-ground obstacle point cloud P o .

4. The robot obstacle perception and autonomous obstacle avoidance method based on convex hull model according to claim 1, characterized in that: Point cloud clustering, inter-frame association, and motion state prediction in step 3 include the following steps: (4-1) Use the obstacle point cloud to build a 3D KD tree and set clustering parameters; use the Euclidean clustering algorithm to randomly select initial points, perform nearest neighbor search, add the search 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, use the Graham scanning algorithm to build the convex hull of the obstacle point cloud for different frame point clouds, and remove the points with a length less than the threshold l th The edges of the obstacle are used to create a convex polygon obstacle set. (4-3) Use the multi-feature nearest neighbor algorithm to match and track the obstacles in the front and back frames, calculate the difference function D(i, j), establish the front and back frame difference correlation matrix D, and according to the nearest neighbor principle, compare it with the set difference threshold D th Compare, select matching obstacles, and establish a spatiotemporal joint obstacle set Among them, A k ∈R m×2 and b k ∈R m×1 is the shape parameter of the convex polygon at time k, and m is the number of edges of the convex polygon; (4-4) According to the obstacle positions in the five frames before and after the spatiotemporal joint obstacle set, a minimum circumscribed circle is established. When the diameter of the circumscribed circle is less than the set judgment threshold d, it is considered a static obstacle, otherwise it is a dynamic obstacle; (4-5) Using the Kalman prediction algorithm, the motion of the dynamic obstacle is predicted based on the historical position, 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: Among them, λ i is the k-time motion prediction covariance matrix P k The expected collision time t is calculated based on the obstacle speed and the current speed of the robot. TC .

5. The robot obstacle perception and autonomous obstacle avoidance method based on convex hull model according to claim 1, characterized in that ,The decision algorithm based on the finite state machine in step 4 includes the following steps: (5-1) According to the robot working environment and working objectives, a finite state machine is established that includes five motion path states of following, emergency braking, left turn, right turn, and reverse and two speed sub-states of acceleration and deceleration; (5-2) Establish the input and transition conditions of each motion state and speed sub-state, set the termination state conditions and enter the initialization state; (5-3) According to the perception information {σ, V, t TC } and the current vehicle state S k As the state machine input, perform state transfer; (5-4) According to the given decision system frequency F d , determine the current machine state, calculate the reference target point, and use the Dubins curve to generate the reference state S r ; (5-5) When it is detected that the end point has been reached, the termination state is entered and a termination instruction is issued.

6. The robot obstacle perception and autonomous obstacle avoidance method based on convex hull model according to claim 1, characterized in that: The spatiotemporal joint obstacle avoidance path planning algorithm based on MPC and scale transformation constraints in step 5 includes the following steps: (6-1) Establish a robot kinematic model based on wheeled Ackerman steering, set the optimization time domain N, and set the state constraint S according to the physical limitations of the farmland. lim , according to the control system constraints set input constraints u lim , and the control input objective function J u ; (6-2) According to the reference state S r , define the state error J s , according to the current state, establish the 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 Establish obstacle avoidance constraints through the scale transformation algorithm: (6-4) According to the overall optimization goal J: J=J N +J s +J u +J xs +J us and optimization constraints, wherein the optimization constraints include state constraints, input constraints, initial state constraints, and obstacle avoidance constraints, and a nonlinear optimization problem is established, wherein the variables to be optimized are the state quantity S and the input quantity u, and the IPOPT solver is used to optimize and solve them to obtain the optimized state quantity and input quantity; (6-5) The first optimized input quantity is sent to the actuator for execution, the system state is updated, and steps (6-2) to (6-4) are repeated, using the last optimization result as the initial input of the optimizer to perform optimization solution.

Images