CN114063615A - Backing navigation control method and system for intelligent vehicle for spraying pesticide between ridges in shed - Google Patents

Backing navigation control method and system for intelligent vehicle for spraying pesticide between ridges in shed Download PDF

Info

Publication number
CN114063615A
CN114063615A CN202111299516.7A CN202111299516A CN114063615A CN 114063615 A CN114063615 A CN 114063615A CN 202111299516 A CN202111299516 A CN 202111299516A CN 114063615 A CN114063615 A CN 114063615A
Authority
CN
China
Prior art keywords
vehicle
grid
point
local
map
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202111299516.7A
Other languages
Chinese (zh)
Other versions
CN114063615B (en
Inventor
余伶俐
吴汉钊
许泽中
罗嘉威
赵于前
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Central South University
Original Assignee
Central South University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Central South University filed Critical Central South University
Priority to CN202111299516.7A priority Critical patent/CN114063615B/en
Publication of CN114063615A publication Critical patent/CN114063615A/en
Application granted granted Critical
Publication of CN114063615B publication Critical patent/CN114063615B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0234—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using optical markers or beacons
    • G05D1/0236—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using optical markers or beacons in combination with a laser
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
    • G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G05D1/0278—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using satellite positioning signals, e.g. GPS
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
    • G05D1/0289—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling with means for avoiding collisions between vehicles
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
    • G05D1/0291—Fleet control
    • G05D1/0293—Convoy travelling

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Optics & Photonics (AREA)
  • Electromagnetism (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Navigation (AREA)

Abstract

The invention discloses a reversing navigation control method and system for an intelligent vehicle for spraying pesticides among ridges in a greenhouse, wherein road environment information in the greenhouse is collected, and a global static grid map with an unchanged original point is constructed; sensing crops on two sides of a ridge in the greenhouse by a multi-line laser radar when the intelligent pesticide spraying vehicle backs up and drives out between the ridges, collecting barrier information when the intelligent vehicle backs up, constructing a local cost map with a vehicle as a center, projecting dynamic barriers on the local map in real time, and performing expansion treatment; determining a passable road boundary and generating a reversing center reference line; planning a local reversing track according to vehicle positioning information, reversing center reference line information, dynamic barrier constraint and vehicle kinematics constraint; and (3) extracting track points from the backing track, and generating angular acceleration and linear acceleration control commands by adopting PI controllers in the transverse direction and the longitudinal direction respectively. The invention can avoid collision with crops.

Description

Backing navigation control method and system for intelligent vehicle for spraying pesticide between ridges in shed
Technical Field
The invention relates to the field of intelligent agricultural in-shed robot navigation, in particular to a backing navigation control method and system for an intelligent inter-ridge pesticide spraying vehicle in a shed.
Background
Navigation control that backs a car between ridge in warmhouse booth is one of the important research content that does not come wisdom agricultural pesticide and sprays intelligent car, along with wisdom farm warmhouse booth's automation level is higher and higher, ridge length and density are bigger and larger, it is bigger and bigger to replace the demand that manpower realized the automatic pesticide that sprays between the ridge, especially to narrow the road that can pass through between the ridge, realize that the pesticide sprays intelligent car and back a car navigation and improve the automation level of operation between the ridge, guarantee that the pesticide sprays the coverage and have important value. However, in an actual scene, the following problems still exist to restrict the application of the backing navigation control of the intelligent vehicle for spraying pesticide between ridges in the shed.
(1) The invention discloses an S-shaped pesticide spraying navigation control method which is only suitable for scenes that a vehicle can freely run at the head and the tail of a field ridge, namely, the method only comprises a scene that the vehicle runs forwards, but cannot process a scene that one end of the field ridge is closed and the vehicle needs to run backwards. The controller in the invention adopts a vehicle dynamic model, so that the controller cannot run at high frequency;
(2) most of the traditional road boundary detection and identification methods are perception methods using vision or laser radars, and because the types of crops on two sides of a ridge can change along with planting requirements and the crops can continuously grow along with seasons, no determined boundary information exists on the road between ridges, and the difficulty in extracting and identifying the road boundary is increased;
(3) the traditional road center reference line generation method aiming at a specific park can be realized simply by adopting a GPS dotting marking mode, namely, an intelligent vehicle with centimeter-level RTK positioning accuracy is utilized to firstly drive on a preset road and record a GPS point sequence actually traveled by the vehicle, the GPS point sequence is preprocessed and then used as road center reference line information for automatic navigation of future vehicles, however, in a greenhouse, due to the uncertainty of planting requirements, the density and width of ridges can change along with the change, and the GPS point sequence is required to be recorded again on all ridges of the greenhouse every time the ridges change, so that the flexibility of GPS tracking is limited.
Disclosure of Invention
The invention aims to solve the technical problem that the prior art is insufficient, and provides a reversing navigation control method and system for an intelligent vehicle for spraying pesticides between ridges in a shed, so that the probability of collision between the intelligent vehicle and crops on two sides of the ridges when the intelligent vehicle reverses and runs between the ridges is reduced.
In order to solve the technical problems, the technical scheme adopted by the invention is as follows: a reversing navigation control method of an intelligent vehicle for spraying pesticides among ridges in a shed comprises the following steps:
s1, collecting road environment information in the greenhouse, and constructing a global static grid map with unchanged origin coordinates;
s2, acquiring barrier information when the intelligent vehicle runs in a reverse mode by sensing crops on two sides of a ridge in the greenhouse, intercepting a local grid map from a global static grid map, projecting dynamic barriers onto the local grid map in real time, performing expansion processing, and constructing a local cost map with a vehicle as a center;
s3, determining a passable road boundary according to the dynamic obstacle grid and the expansion grid information in the local cost map, and generating a reversing center reference line;
s4, planning a local reversing track according to the vehicle positioning information, the reversing center reference line information, the dynamic barrier constraint and the vehicle kinematics constraint;
and S5, extracting track points from the local reversing track, and generating angular acceleration and linear acceleration control commands by respectively adopting a PI controller in the transverse direction and the longitudinal direction of the vehicle so as to track the expected angular speed and linear speed of the track points.
The intelligent pesticide spraying vehicle can realize reversing navigation among semi-closed ridges, enhances the automation degree of the intelligent pesticide spraying vehicle, and improves the stability and robustness of autonomous driving among the ridges.
The specific implementation process of step S1 includes:
carrying out incremental detection sensing on the environment between ridges in the shed by using a multi-line laser radar;
recording the position of the intelligent vehicle when the intelligent vehicle is started as a mapping original point, conducting mapping by utilizing an SLAM algorithm while the intelligent vehicle senses the advancing, and storing a three-dimensional point cloud map when the environment is completely explored, wherein each point cloud point contains three-dimensional global coordinate information relative to the mapping original point (x, y, z);
eliminating point cloud information on the ground in the three-dimensional point cloud map, projecting the rest three-dimensional point clouds to the ground, converting the three-dimensional point cloud map into a two-dimensional global map only containing (x, y) coordinates, and storing the two-dimensional global map as a mapglobalAnd the two-dimensional coordinates of the map-building origin are marked as the global map origin (x)origin，yorigin)。
Step S1 establishes a global map of the driving environment of the intelligent vehicle in the shed, enhances the understanding of the intelligent vehicle on the environment in the shed, and improves the feasibility of inter-ridge navigation.
Preferably, the method further comprises the following steps:
according to the global map resolution parameter rglobalFor mapglobalRasterizing is carried out to generate a global static raster map costmapglobal；
Traversing two-dimensional global mapglobalAnd initializing the corresponding cost value to 0:
traversing two-dimensional global mapglobalThe two-dimensional global coordinate (x) of the point cloud is calculated from all the point cloud informationglobal，yglobal) Conversion to grid coordinates (x)obs，yobs) The cost value corresponding to the barrier grid is set to 1.
The method has the advantages that rasterization is carried out on the basis of the global map, corresponding global cost is set for each grid, time complexity of map operation is reduced, obstacles are divided by the obstacle grids, and the collision detection fineness is improved.
The specific implementation process of step S2 includes:
according to the preset parameters of the local cost map: width (m) and height (m) from a global static grid map costmap with the vehicle rear axle center as the origin of coordinates of the local cost mapglobalIn-cut local grid map costmap of expected sizelocal；
In costmaplocalUpdating the dynamic barrier of the upper computer, and writing the grid coordinate cost corresponding to the dynamic barrier of the real-time perception;
according to a preset expansion radius parameter RinflationAnd performing expansion processing on the dynamic barrier, writing in grid coordinate cost corresponding to the expansion processing, and generating a local cost map.
The invention dynamically performs projection and collision on the dynamic barrier in real time, thereby avoiding the influence on the barrier avoidance caused by the change of the types of crops at two sides of the ridge or the change of the growth of the crops along with seasons.
The updating process of the dynamic barrier comprises the following steps:
data of a multi-line laser radar observation source on the intelligent vehicle are collected in real time, and generated point cloud information is stored in a buffer queueobservationPerforming the following steps;
processing bufferobservationAccording to the mark position, judging the position of each point cloud information according to the mark position of the cache data, and if the point cloud information is marked as marking, pushing the point cloud information to a buffermarkingThe tail of the queue; if the point cloud information is marked as clear, pushing the point cloud information to bufferclearingThe tail of the queue;
to buffer in sequenceclearingThe point cloud information in the point cloud information is cleared, namely, an area between the vehicle and the point cloud information to be processed is marked as free, and the cost of the corresponding grid coordinate is set to be 0;
to buffer in sequencemarkingThe point cloud information in (1) executes marking operation and traverses buffermarkingRemoving the point cloud information with the distance from the sensor larger than a threshold value dmaxAnd z coordinate is greater than a threshold value zmaxThe screened dynamic barrier is converted from the global coordinate to the grid coordinate, and the cost of the barrier grid coordinate is set to be 1.
The updating step of the dynamic barrier simply and effectively classifies the laser point cloud, so that the laser point cloud is used for the collision detection process of backing navigation, and the barrier avoidance safety is improved.
Preferably, the specific implementation process for marking the region between the vehicle and the point cloud information to be processed as free includes:
traverse bufferclearingConverting all the point cloud information under the global coordinate into a grid coordinate system;
traversing the point cloud information after coordinate conversion, connecting the center of the rear axle of the vehicle with the point cloud point needing to be processed currently, recording the grid coordinate passing through the connecting line, and forming a free coordinate sequence setfree；
Traversal setfreeIf the coordinate is in the local cost map, the cost of the grid coordinate is set to be 0; if the grid coordinates are out of range of the local cost map, set processing continuesfreeThe next grid coordinate.
The processing process of the free area mark enables the intelligent vehicle to know the current drivable area, and the feasibility of autonomous navigation is enhanced.
The specific implementation process for performing expansion treatment on the dynamic barrier comprises the following steps:
by expanding the radius RinflationAnd global map resolution rglobalCalculating the number num of expansion gridsinflation；
Grid coordinate sequence set for obtaining obstacle contour pointcontour；
Traversal setcontourContour point grid coordinate (x) of (1)contour，ycontour) Connecting the original point of the local cost map and the coordinates of the contour points to form a vector OC;
expanding num in the direction pointed by the vector OC by taking the coordinates of the contour points as a starting pointinflationLength of the gridDegree;
the cost of the expansion grid coordinate is set to 1.
The obstacle avoidance is safer and more effective by expansion processing of the dynamic obstacle, the obstacle avoidance safety is improved, and the collision probability of the intelligent vehicle and the obstacle of the rear vehicle is reduced.
The specific implementation process of step S3 includes:
step 1, traversing grids with the cost of 1 in a local cost map, wherein the grids comprise static obstacles, dynamic obstacles and obstacle expansion grids, judging the position relation between each obstacle grid and a vehicle, and if the obstacle grids are on the left side of the vehicle, placing the obstacle grids into a left obstacle grid queue setleft(ii) a If the obstacle grid is on the right side of the vehicle, the obstacle grid is placed in the right obstacle grid queue setright；
Step 2, traversing the grid queue set of the left obstacleleftConverting the grid coordinates of the barrier grid into a global coordinate system with an unchanged origin, and then converting the grid coordinates into a local coordinate system with the vehicle as a center;
grid queue set for traversing right obstaclerightFirstly, converting grid coordinates of the obstacle grid into a global coordinate system with an unchanged origin, and then converting the grid coordinates into a local coordinate system with the vehicle as a center;
step 3, converting the set into the set under the global coordinate systemleftSorting according to the x coordinate of the barrier grid from large to small according to the number num of the road boundary pointsedgeFrom sequenced setleftMiddle selected top numedgeThe obstacle points form a road boundary queue edge closest to the left side of the vehicleleft；
Set for conversion into global coordinate systemrightSorting from small to large according to the x-coordinate of the obstacle grid, from sorted setrightMiddle selected top numedgeThe obstacle points form a road boundary queue edge closest to the right side of the vehicleright；
Step 4, sampling according to the road sampling intervaledgeTo edgeleft、edgerightAccording to sampleedgePerforming interval sampling to obtainThe arriving road boundary points are respectively put into a queue SEleft、SEright；
Step 5, first traverse SEleftTaking out the obstacle points obs to be processedleft(ii) a Then traverse SErightSelecting the distance obs on the y-axisleftNearest points obsrightAs a result of the matching;
step 6, the left and right obstacle points obs are processedleft、obsrightIs converted into a global coordinate system, and the coordinates (x) of the center point of the road are calculatedc，yc)；
And 7, putting the road center point into a backing center reference point queue refPath.
The method for generating the center reference line based on the barrier grid can effectively search the center point of the path, and avoids the filtering and removing processes of point cloud information.
The specific implementation process of step S4 includes:
step 1, traversing a reversing center reference point queue refPath, and selecting a point in the refPath which is closest to a current intelligent vehicle as a starting point of local track planning;
step 2, selecting the last point in the refPath as a terminal point of the local track planning;
step 3, processing dynamic obstacle collision constraint, and forming a point-shaped obstacle set by grids with obstaclespobs；
Step 4, converting the nonlinear least square problem of the local reversing planning into a multi-objective optimization problem, converting the multi-objective optimization problem into a graph optimization problem, and determining the top point and the edge of the graph;
step 5, calling the g2o framework to calculate the graph optimization problem to obtain an optimal local backing track optimalTraj, wherein the local backing track consists of a series of optimal state points posiAt a time interval of Δ TiComposition, i ═ 1, 2, … …, n: n is the number of state points in an optimal track;
optimalTraj＝(pose1，ΔT1，pose2，ΔT2，...，ΔTn-1，posen)；
wherein the optimum state point is defined by two-dimensional plane coordinates (x)i，yi) And a desired heading angle thetaiConsists of the following components:
posei＝(xi，yi，θi)；
step 6, carrying out feasibility check on the local reversing track, and executing step 7 if the local reversing track passes the feasibility check; if the check is not passed, a warning is given, and step 9 is executed;
step 7, processing the local backing track optimalTraj and utilizing two adjacent optimal state points (pos)iAt a time interval of Δ TiAnd calculating the speed of the current track point, wherein the calculation formula of the linear speed is as follows:
Figure BDA0003337890740000061
the calculation formula of the angular velocity is:
Figure BDA0003337890740000062
and 8, limiting the track points exceeding the maximum speed constraint:
Figure BDA0003337890740000063
step 9, judging whether the current vehicle running state is abnormal or not, and if the current vehicle running state is normal, executing step 10; if the planning fails, selecting a middle point in the refPath as a terminal point of the local track planning, and executing the step 1 and the steps 3-6 again; if the vehicle is detected to be in an oscillation state, namely the linear velocity and the angular velocity are both smaller than given values, the steps 1 to 6 are executed again;
step 10, judging whether an operation finishing point is reached, and finishing reversing navigation if the vehicle backs from the inter-ridge road to the main road in the shed; otherwise, re-executing the step 1 to the step 9.
The local reversing track generation method based on the optimization theory can effectively improve the smoothness and the stability of the local reversing track, and the optimal reversing track can meet a plurality of preset constraints and meet preset target requirements.
The specific implementation process of the step 3 comprises the following steps:
step 1), traversing all grid coordinates in the local cost map, and if the cost of the coordinates is 1, converting the grid coordinates into a global coordinate system;
step 2), traversing and converting the coordinates (x) of the obstacle points under the global coordinate systempobs，ypobs) Calculating the coordinates (x) of the center of the vehiclevel，yvel) As a starting point, (x)pobs，ypobs) Vector formed for endpoint
Figure BDA0003337890740000071
Is expressed as:
Figure BDA0003337890740000072
and 3), acquiring the current heading angle theta of the vehicle from the positioning information, wherein the current direction vector of the vehicle can be expressed as:
Figure BDA0003337890740000073
step 4, calculating
Figure BDA0003337890740000074
And
Figure BDA0003337890740000075
vector dot product of (a):
Figure BDA0003337890740000076
if dotProduct is positive, the obstacle point is in front of the vehicle and is not in the consideration range of the backing plan, and then the method is ended; if dotProduct is negative, indicating that the obstacle point is behind the vehicle,adding the obstacle point to the set of point-shaped obstacles setpobsIn (1).
Above-mentioned process can effectively reject the barrier point in vehicle the place ahead, has improved collision detection's efficiency.
Lateral acceleration control command avThe calculation formula is as follows:
av＝Kpvepv+Kiveiv；
longitudinal acceleration control command aωThe calculation formula is as follows:
aω＝Kpωepω+Kiωeiω；
wherein, Kpv、KivRespectively is a preset longitudinal speed position error coefficient and a longitudinal speed integral error coefficient; e.g. of the typepv＝vwp-vv；
Figure BDA0003337890740000077
vv、ωvVehicle longitudinal velocity and rotational angular velocity, respectively.
The calculation process of the control command is based on a model-free PID method, namely, dynamic modeling is not needed for the intelligent vehicle, the low-speed tracking control has better adaptability, and the method is suitable for both forward running and reverse running and has good adaptability and robustness.
The invention also provides a backing navigation control system of the intelligent vehicle for spraying pesticide between ridges in the shed, which comprises computer equipment; the computer device is configured or programmed for performing the steps of the method of the invention.
Compared with the prior art, the invention has the beneficial effects that: according to the method, a global static grid map with an unchanged original point is constructed by collecting road environment information in the greenhouse; sensing crops on two sides of a ridge in a greenhouse by a multi-line laser radar when a pesticide spraying intelligent vehicle backs up and drives out between the ridges, acquiring barrier information when the intelligent vehicle backs up, constructing a local cost map with a vehicle as a center, projecting dynamic barriers on the local map in real time and performing expansion treatment, increasing the safety margin of expansion detection by adding an additional expansion grid, and reducing the probability of collision between the intelligent vehicle and the crops on two sides of the ridge when the intelligent vehicle backs up and drives out between the ridges; according to the real-time dynamic barrier information, the passable road boundary is determined, the reversing center reference line is generated, and the real-time performance and the flexibility of the center reference line generation are improved; and planning a local reversing track according to the vehicle positioning information, the reversing center reference line information, the dynamic barrier constraint and the vehicle kinematics constraint. The local track is referred to the reversing center line, so that the local track is close to the reversing center line as much as possible, meanwhile, barrier constraint is considered again in local planning, the reversing driving safety is improved, the addition of vehicle kinematic constraint enables the local planning track to be smoother than the reversing center reference line, and the stability and the continuity of the control module are guaranteed; track points are extracted from the backing track, the PI controllers are respectively adopted in the transverse direction and the longitudinal direction to generate angular acceleration and linear acceleration control instructions so as to track the expected angular speed and the linear speed of the track points, the selection of the track points is adaptive to the vehicle speed, and the robustness of the control module to the backing vehicle speed is improved. The intelligent vehicle for spraying the pesticide between the ridges in the greenhouse can complete the task of reversing navigation between the narrow ridges of the greenhouse, avoid collision with crops, and improve the applicability and flexibility of the intelligent vehicle for spraying the pesticide between the ridges in the greenhouse.
Drawings
FIG. 1 is a block diagram of a reverse navigation process;
FIG. 2 is a schematic view of backing navigation between ridges;
FIG. 3 is a schematic diagram of global map conversion;
FIG. 4 is a schematic diagram of a local cost map;
FIG. 5 is a diagram of the effect of the reverse track;
fig. 6 is a graph of local track point distance deviation.
Detailed Description
The following describes embodiments of the present invention in detail, which are developed based on the technical solutions of the present invention, and give detailed implementation manners and specific operation procedures to further explain the technical solutions of the present invention.
The reversing navigation control method for the intelligent vehicle for spraying pesticide between ridges in the shed, provided by the embodiment of the invention, has the advantages that the complete reversing navigation process is shown in a figure 1, the reversing navigation schematic diagram between ridges is shown in a figure 2, and the method comprises the following steps:
the method comprises the following steps: collecting road environment information in the greenhouse, and constructing a global static grid map with unchanged origin coordinates:
step 1, carrying out incremental detection sensing on the environment between ridges in a shed by a pesticide spraying intelligent vehicle equipped with a multi-line laser radar;
in the embodiment, only for the backing scene between ridges in the shed, the intelligent pesticide spraying vehicle is a wire-controlled intelligent vehicle chassis which is provided with a multi-wire laser radar and can receive an acceleration control command based on a differential steering principle, and the laser radars are mounted at the front and the rear of the vehicle, wherein the front laser radar is used for the intelligent pesticide spraying vehicle to drive forwards to spray pesticides between the ridges, and the rear laser radar is used for the intelligent vehicle to drive backwards out of the ridges;
step 2, recording global positioning information when the intelligent vehicle is started as a mapping origin, mapping by using an SLAM algorithm while sensing the advancing of the vehicle, and storing the mapping as a three-dimensional point cloud map when the environment is completely explored, wherein each point cloud contains (x, y, z) three-dimensional global coordinate information relative to the mapping origin;
step 3, firstly, eliminating point cloud information on the ground in the three-dimensional point cloud map, then projecting the rest three-dimensional point clouds to the ground, further converting the three-dimensional point cloud map into a two-dimensional global map only containing (x, y) coordinates, and storing the two-dimensional global map as mapglobalAnd the two-dimensional coordinates of the map-building origin are marked as the global map origin (x)origin，yorigin) (ii) a The crops planted on two sides between ridges are represented as a series of point cloud information containing two-dimensional global coordinates; the driving road between ridges is represented as a free passing area without point cloud information;
step 4, according to the global map resolution parameter rglobalFor mapglobalRasterizing is carried out to generate a global static raster map costmapglobal. Wherein the coordinates (x) in the grid mapcell，ycell) With coordinates (x) in the global mapglobal，yglobal) The correspondence is expressed as:
Figure BDA0003337890740000091
wherein group down () is a downward rounding function, one grid coordinate corresponds to a plurality of global map coordinates from the above formula, and the global map is converted into a global static grid map as shown in fig. 3.
Step 5, traversing all grids in the global map, and initializing corresponding cost values to be 0: cost (x)cell，ycell)＝0；
Step 6, traversing the global mapglobalAccording to formula (1), two-dimensional global coordinates (x) of the point cloud are calculatedglobal，yglobal) Conversion to grid coordinates (x)obs，yobs) And setting the cost of the corresponding barrier grid as 1: cost (x)obs，yobs)＝1；
Step two: pesticide sprays intelligent vehicle through the other crops in ridge both sides in the multi-thread laser radar perception warmhouse booth when backing a car and rolling out between the ridge, gathers the barrier information when intelligent vehicle backs a car and runs, intercepts local grid map from the static grid map of global, with dynamic barrier real-time projection to local grid map on and carry out inflation treatment, the local cost map of vehicle as the center is used in the construction:
step 1, according to local cost map preset parameters: width (m) and height (m) of the global static grid map costmap with the center of the vehicle rear axle as the origin of coordinates of the local mapglobalIn-cut local grid map costmap of expected sizelocal；
Step 2, in costmaplocalUpdating the barrier, and writing in the grid coordinate cost corresponding to the real-time perceived dynamic barrier, wherein the specific steps of updating the dynamic barrier are as follows:
a1: collecting data of a multi-line laser radar observation source in real time, and storing generated point cloud information into a buffer queueobservationPerforming the following steps;
a2: processing bufferobservationThe cache data in (2) judges the position of each point cloud information according to the mark bit, if the point cloud information is marked as marking, the obstacle is marked in a local cost map during subsequent processing, and the point cloud information is pushed to a buffermarkingThe tail of the queue; if the point cloud information is marked as clear, the obstacle is indicated to be cleared from the local cost map during subsequent processing, and the point cloud information is pushed to the bufferclearingThe tail of the queue;
a3: to buffer in sequenceclearingThe point cloud information in (1) is cleared, namely, an area between the vehicle and the point cloud information to be processed is marked as free (namely a passable area), namely, the cost of the corresponding grid coordinate is set as 0: cost (x)obstacle，yobstacle) The specific steps of marking the passable area are as follows:
b1: traverse bufferclearingConverting all point cloud information under the global coordinate into a grid coordinate system according to a formula (1);
b2: traversing the point cloud information after coordinate conversion, connecting the origin of the local cost map (the center of the rear axle of the vehicle) with the point cloud point needing to be processed currently, recording the grid coordinate passing through the connecting line, and forming a free coordinate sequence setfree；
B3: traversal setfreeIf the coordinate is within the local cost map, the cost of the grid coordinate is set to 0: cost (x)free，yfree) 0; if the grid coordinates are out of range of the local cost map, set processing continuesfreeNext grid coordinate of (2);
a4, buffer in turnmarkingThe point cloud information in the process of (1) is marked, the point cloud information in the queue is traversed, and the distance between the point cloud information and the sensor is eliminated and is larger than a threshold value dmaxAnd z coordinate is greater than a threshold value zmaxE.g. setting dmax＝10[m]，zmax＝1[m]Then, the point clouds with the distance between the point clouds and the sensor larger than 10 meters and the point clouds with the z coordinate larger than 1 meter are removed, and the screened barrier is removedThe obstacle is converted from global coordinates to grid coordinates according to equation (1), and the cost of the obstacle grid coordinates is set to 1: cost (x)obstacle，yobstacle)＝1；
Step 3, according to a preset expansion radius parameter RinflationAnd performing expansion processing on the dynamic barrier in the step 2, and writing a grid coordinate cost corresponding to the expansion processing, wherein the specific steps of the expansion processing of the dynamic barrier are as follows:
c1 by expansion radius RinflationAnd global map resolution rglobalCalculating the number num of expansion gridsinflation：
Figure BDA0003337890740000101
C2, obtaining a grid coordinate sequence set of the outline points of the obstacleoutline；
C3, traverse setcontourContour point grid coordinate (x) of (1)contour，ycontour) Connecting the original point of the local cost map and the coordinates of the contour points to form a vector OC;
c4, expanding num in the direction of vector OC by using the coordinates of contour points as starting pointsinflationThe length of each grid;
c5, setting the cost of the corresponding expansion grid coordinate to 1: cost (x)inflation，yinflation)＝1；
The construction of the local cost map is completed by projecting the real-time obstacles and the expansion area to the local grid map, and as shown in fig. 4, the local cost map of an ideal intra-canopy inter-ridge environment is illustrated, in which the dot obstacles and the expansion grid occupy one grid size.
Step three: determining a passable road boundary according to the dynamic obstacle grid and the expansion grid information in the local cost map, and generating a backing center reference line:
step 1, traversing grids with the cost of 1 in the local cost map, including static obstacles, dynamic obstacles and obstacle expansion grids, and judging each obstacleThe position relation of the grid and the vehicle, if the obstacle grid is on the left side of the vehicle, the obstacle grid is put into the left obstacle grid queue setleft(ii) a If the obstacle grid is on the right side of the vehicle, then the obstacle grid is placed in the right obstacle grid queue setright；
Step 2, traversing the grid queue set of the left obstacleleftThe grid coordinates of the obstacle grid are first converted into a global coordinate system with the origin unchanged, and then converted into a local coordinate system with the vehicle as the center, because of setleftThe obstacles in the middle are all positioned on the left side of the vehicle, so the x-axis coordinates in the converted coordinates are all negative;
step 3, converting the set into the set under the global coordinate systemleftSorting according to the x coordinate of the barrier grid from large to small according to the number num of the road boundary pointsedgeFrom sequenced setleftMiddle selected top numedgeThe obstacle points form a road boundary queue edge closest to the left side of the vehicleleft；
Step 4, traversing right obstacle grid queue setrightThe grid coordinates of the obstacle grid are first converted into a global coordinate system with the origin unchanged, and then converted into a local coordinate system with the vehicle as the center, because of setleftThe obstacles in the middle are all positioned on the right side of the vehicle, so the x-axis coordinates in the converted coordinates are all positive;
step 5, converting the set into the set under the global coordinate systemrightSorting from small to large according to the x-coordinate of the obstacle grid, from sorted setrightMiddle selected top numedgeThe obstacle points form a road boundary queue edge closest to the right side of the vehicleright；
Step 6, sampling according to the road sampling intervaledgeTo edgeleft、edgerightAccording to sampleedgeCarrying out interval sampling, and respectively putting the road boundary points obtained by sampling into a queue SEleft、SEright；
Step 7, two-layer traversal is performed, first the SE is traversedleftTaking out the obstacle points obs to be processedleft(ii) a Then traverse SErightAccording to obsleftIs matched and the distance obs on the y-axis is selectedleftNearest points obsrightAs a result of the matching;
step 8, the left and right obstacle points obsleft、obsrightThe grid coordinate of (2) is converted into a global coordinate system, and the coordinate conversion formula is as follows:
Figure BDA0003337890740000121
step 9, calculating the coordinates (x) of the road center pointc，yc) The calculation method comprises the following steps:
Figure BDA0003337890740000122
step 10, putting the road center point into a backing center reference point queue refPath;
step four: planning a local reversing track according to vehicle positioning information, reversing center reference line information, dynamic barrier constraint and vehicle kinematics constraint, and specifically comprising the following steps:
step 1, traversing points closest to the vehicle in a backing center reference point queue refPath according to Euclidean distance according to points matched with the vehicle global positioning information, and using the points as starting points of local track planning;
step 2, selecting the last point in the refPath as a terminal point of the local track planning;
step 3, processing dynamic obstacle collision constraint, and forming a point-shaped obstacle set by grids with obstaclespobsBecause the invention is only used for backing navigation, only the obstacle point behind the vehicle is considered when generating the local backing track. The dynamic barrier processing steps in the local reversing track planning are as follows:
step D1, traversing all grid coordinates in the local cost map, and if the cost of the coordinates is 1, seating the grid
The target is transformed to the global coordinate system according to equation (2):
Figure BDA0003337890740000123
step D2, traversing the coordinates (x) of the obstacle point converted into the global coordinate systempobs，ypobs) Calculating the coordinates (x) of the center of the vehiclevel，yvel) As a starting point, (x)pobs，ypcbs) Vector formed for endpoint
Figure BDA0003337890740000124
Is expressed as:
Figure BDA0003337890740000131
step D3, obtaining the current heading angle θ of the vehicle from the positioning information, and the current direction vector of the vehicle can be represented as:
Figure BDA0003337890740000132
step D4, calculating
Figure BDA0003337890740000133
And
Figure BDA0003337890740000134
vector dot product of (a):
Figure BDA0003337890740000135
step D5, if dotProduct is positive, it indicates that the obstacle point is in front of the vehicle and is not in the consideration range of the reverse planning, so the following steps are skipped;
step D6, if dotProduct is negative, it indicates that the obstacle point is behind the vehicle, and adds the obstacle point to the point-shaped obstacle setpobsPerforming the following steps;
step 4, converting the nonlinear least square problem of local reversing planning into a multi-objective optimization problem, converting the multi-objective optimization problem into a graph optimization problem, determining the top point and the edge of a graph, and constructing the graph optimization problem by the specific method as follows:
e1, abstracting the variables to be optimized into vertexes in the graph optimization problem. The vertex is divided into two types, one is the configuration space coordinate of the locus point: (x, y, θ); the other type is a time interval delta T between every two adjacent track points, and the combination of the two adjacent track points forms a solving space of a local reversing planning problem;
e2, converting barrier constraint, vehicle integrity constraint, maximum speed constraint, maximum acceleration constraint and shortest time constraint into edges in graph optimization, wherein each edge can be connected with a plurality of vertexes, and the set of the vertexes and the edges form a hypergraph required by solving a graph optimization problem;
e3, punishing the vertexes exceeding the constraint by adopting a soft constraint mode, wherein the more the vertexes exceed the boundary, the stronger the punishment is;
the local reversing trajectory planning problem is converted into a graph optimization problem, so that the method is convenient and universal, and is beneficial to expansion, for example, if requirements on trajectory smoothness are required later, smoothness constraint can be directly modeled as an error item and added into an objective function, and then the error item is abstracted into an edge in a graph structure, so that the solution can be completed again;
step 5, calling the g2o frame to solve the planning problem to obtain the optimal local backing track optimalTraj,
the local reversing track is composed of a series of optimal state points (pos) and a time interval delta T:
optimalTraj＝(pose1，ΔT1，pose2，ΔT2，...，ΔTn-1，posen)
wherein each optimum state point consists of two-dimensional plane coordinates (x, y) and a desired heading angle theta:
pose＝(x，y，θ)
step 6, carrying out feasibility check on the local reversing track, and executing step 7 if the local reversing track passes the feasibility check; if the check is not passed, a warning is given, steps 7 and 8 are skipped, and step 9 is executed;
and 7, processing the local backing track optimalTraj, and calculating the speed of the current track point for two adjacent optimal poses (positions) and a time interval delta T each time, wherein the calculation formula of the linear speed is as follows:
Figure BDA0003337890740000141
the calculation formula of the angular velocity is:
Figure BDA0003337890740000142
and 8, processing the saturated speed value, and limiting the track points exceeding the maximum speed constraint:
Figure BDA0003337890740000143
step 9, judging whether the current vehicle running state is abnormal or not, and if the current vehicle running state is normal, executing step 10; if the planning fails, selecting a middle point in the refPath as an end point of the local track planning, and executing the step 1 and the step 3-6 again; if the vehicle is detected to be in an oscillation state, namely the linear velocity and the angular velocity are both smaller than given values, the steps 1-6 are executed again;
step 10, judging whether an operation finishing point is reached, and finishing reversing navigation if the vehicle backs from the inter-ridge road to the main road in the shed; otherwise, steps 1-9 are re-executed.
The backing track effect between ridges is shown in fig. 5, wherein the two sides of the intelligent vehicle are sequences of crop obstacle points on the ridges, and the central line is the generated backing track. As can be seen from the figure, the invention generates a safe and smooth backing track and lays a foundation for the backing track tracking control.
The experimental data of the reversing track are shown in table 1, the sampling interval is set to be 5 track points, and the distance d between each track point and the left road boundary is calculatedlAnd is expressed by negative values; then the distance d between the point and the right road boundary is calculatedrAnd is represented by a positive value. Finally will beThe two distances are added to obtain Δ d, i.e., Δ d ═ dl+dr. The smaller the value, the more the track point is located on the road middle line. The average value of delta d calculated from the table is 0.028, which shows that the local track is generated in the center of the road, and ensures the safety of track planning.
TABLE 1 Experimental data on the reversing trajectory of the present invention
Figure BDA0003337890740000144
Figure BDA0003337890740000151
Table 1 provides data for a portion of the local trace points by uniform sampling, and the curve shown in fig. 6 shows the distance deviation of all trace points on the local trace, and it can be seen from fig. 6 that all trace points are near the x-axis, the maximum value is 0.109, the minimum value is-0.1123, and the average value is 0.027. The result shows that the local track points all fall in the center of the road, and the safety of the intelligent vehicle backing navigation is ensured.
Step five: extracting track points from a local backing track, and generating angular acceleration and linear acceleration control instructions by adopting PI controllers in the transverse direction and the longitudinal direction respectively to track the expected angular velocity and the linear velocity of the track points:
step 1, acquiring global positioning information, and calculating a point closest to a vehicle in a local backing track optimalTraj: (x) clostpoint ═ xcp，ycp，θcp)；
Step 2, selecting a reversing tracking waypoint (x) in optimalTrajwp，ywp，θwp，vwp，ωwp) Selecting a waypoint which is far away from the closed point if the speed is fast; if the vehicle speed is slow, selecting a track point closer to the closed point;
step 3, reading the longitudinal speed v of the vehicle from the CAN bus of the vehiclevAnd angular velocity of rotation omegav；
Step 4, calculating the current position error of the longitudinal speed:
epv＝vwp-vv
step 5, calculating the accumulated error of the longitudinal speed:
Figure BDA0003337890740000152
step 6, according to the PI feedback control law, presetting a longitudinal speed position error coefficient KpvAnd a longitudinal velocity integral error coefficient KivCalculating a longitudinal acceleration control command:
av＝Kpvepv+Kiveiv
step 7, calculating the current position error of the rotation angular velocity:
epω＝ωwp-ωv
step 8, calculating the accumulated error of the longitudinal speed:
Figure BDA0003337890740000153
step 9, according to the PI feedback control law, presetting a rotation angular velocity position error coefficient KpωAnd rotation angular velocity integral error coefficient KiωCalculating a longitudinal acceleration control command:
aω＝Kpωepω+Kiωeiω。
experiments show that by adopting the scheme of the invention, an accurate reversing center reference line, a safe and smooth local reversing track and an inter-ridge reversing tracking task are generated, the feasibility of the reversing navigation control method for the intelligent vehicle for spraying pesticide between ridges in a shed is proved, and the obstacle avoidance safety is greatly improved.

Claims (10)

1. The reversing navigation control method of the intelligent vehicle for spraying pesticide between ridges in the shed is characterized by comprising the following steps of:
s1, collecting road environment information in the greenhouse, and constructing a global static grid map with unchanged origin coordinates;
s2, acquiring barrier information when the intelligent vehicle runs in a reverse mode by sensing crops on two sides of a ridge in the greenhouse, intercepting a local grid map from a global static grid map, projecting dynamic barriers onto the local grid map in real time, performing expansion processing, and constructing a local cost map with a vehicle as a center;
s3, determining a passable road boundary according to the dynamic obstacle grid and the expansion grid information in the local cost map, and generating a reversing center reference line;
s4, planning a local reversing track according to the vehicle positioning information, the reversing center reference line information, the dynamic barrier constraint and the vehicle kinematics constraint;
and S5, extracting track points from the local reversing track, and generating angular acceleration and linear acceleration control commands by respectively adopting a PI controller in the transverse direction and the longitudinal direction of the vehicle so as to track the expected angular speed and linear speed of the track points.
2. The method for controlling the backing navigation of the intelligent inter-ridge pesticide spraying vehicle in the shed according to claim 1, wherein the specific implementation process of the step S1 comprises the following steps:
carrying out incremental detection sensing on the environment between ridges in the shed by using a multi-line laser radar;
recording the position of the intelligent vehicle when the intelligent vehicle is started as a mapping original point, conducting mapping by utilizing an SLAM algorithm while the intelligent vehicle senses the advancing, and storing a three-dimensional point cloud map when the environment is completely explored, wherein each point cloud point contains three-dimensional global coordinate information relative to the mapping original point (x, y, z);
eliminating point cloud information on the ground in the three-dimensional point cloud map, projecting the rest three-dimensional point clouds to the ground, converting the three-dimensional point cloud map into a two-dimensional global map only containing (x, y) coordinates, and storing the two-dimensional global map as a mapglobalAnd the two-dimensional coordinates of the map-building origin are marked as the global map origin (x)origin，yorigin) (ii) a Preferably, the method further comprises the following steps:
according to the global map resolution parameter rglobalFor mapglobalRasterizing is carried out to generate a global static raster map costmapglobal；
Traversing two-dimensional global mapglobalAnd initializing the corresponding cost value to 0:
traversing two-dimensional global mapglobalThe two-dimensional global coordinate (x) of the point cloud is calculated from all the point cloud informationglobal，yglobal) Conversion to grid coordinates (x)obs，yobs) The cost value corresponding to the barrier grid is set to 1.
3. The method for controlling the backing navigation of the intelligent inter-ridge pesticide spraying vehicle in the shed according to claim 1, wherein the specific implementation process of the step S2 comprises the following steps:
according to the preset parameters of the local cost map: width (m) and height (m) from a global static grid map costmap with the vehicle rear axle center as the origin of coordinates of the local cost mapglobalIn-cut local grid map costmap of expected sizelocal；
In costmaplocalUpdating the dynamic barrier of the upper computer, and writing the grid coordinate cost corresponding to the dynamic barrier of the real-time perception;
according to a preset expansion radius parameter RinflationAnd performing expansion processing on the dynamic barrier, writing in grid coordinate cost corresponding to the expansion processing, and generating a local cost map.
4. The reversing navigation control method for the intelligent intra-canopy inter-ridge pesticide spraying vehicle according to claim 3, wherein the updating process of the dynamic barrier comprises the following steps:
data of a multi-line laser radar observation source on the intelligent vehicle are collected in real time, and generated point cloud information is stored in a buffer queueobservationPerforming the following steps;
processing bufferobservationAccording to the marker bit, judging the position of each point cloud information, if the point is positionedMarking the cloud information as marking, pushing the cloud information to buffermarkingThe tail of the queue; if the point cloud information is marked as clear, pushing the point cloud information to bufferclearingThe tail of the queue;
to buffer in sequenceclearingThe point cloud information in the point cloud information is cleared, namely, an area between the vehicle and the point cloud information to be processed is marked as free, and the cost of the corresponding grid coordinate is set to be 0;
to buffer in sequencemarkingThe point cloud information in (1) executes marking operation and traverses buffermarkingRemoving the point cloud information with the distance from the sensor larger than a threshold value dmaxAnd z coordinate is greater than a threshold value zmaxConverting the screened dynamic barrier from the global coordinate to a grid coordinate, and setting the cost of the barrier grid coordinate to 1;
preferably, the specific implementation process for marking the region between the vehicle and the point cloud information to be processed as free includes:
traverse bufferclearingConverting all the point cloud information under the global coordinate into a grid coordinate system;
traversing the point cloud information after coordinate conversion, connecting the center of the rear axle of the vehicle with the point cloud point needing to be processed currently, recording the grid coordinate passing through the connecting line, and forming a free coordinate sequence setfree；
Traversal setfreeIf the coordinate is in the local cost map, the cost of the grid coordinate is set to be 0; if the grid coordinates are out of range of the local cost map, set processing continuesfreeThe next grid coordinate.
5. The reversing navigation control method for the intelligent intra-canopy inter-ridge pesticide spraying vehicle according to claim 3, wherein the specific implementation process of performing expansion treatment on the dynamic barrier comprises the following steps:
by expanding the radius RinflationAnd global map resolution rglobalCalculating the number num of expansion gridsinflation；
Grid coordinate sequence set for obtaining obstacle contour pointcontour；
Traversal setcontourContour point grid coordinate (x) of (1)contour，ycontour) Connecting the original point of the local cost map and the coordinates of the contour points to form a vector OC;
expanding num in the direction pointed by the vector OC by taking the coordinates of the contour points as a starting pointinflationThe length of each grid;
the cost of the expansion grid coordinate is set to 1.
6. The method for controlling the backing navigation of the intelligent inter-ridge pesticide spraying vehicle in the shed according to claim 1, wherein the specific implementation process of the step S3 comprises the following steps:
step 1, traversing grids with the cost of 1 in a local cost map, wherein the grids comprise static obstacles, dynamic obstacles and obstacle expansion grids, judging the position relation between each obstacle grid and a vehicle, and if the obstacle grids are on the left side of the vehicle, placing the obstacle grids into a left obstacle grid queue setleft(ii) a If the obstacle grid is on the right side of the vehicle, the obstacle grid is placed in the right obstacle grid queue setright；
Step 2, traversing the grid queue set of the left obstacleleftConverting the grid coordinates of the barrier grid into a global coordinate system with an unchanged origin, and then converting the grid coordinates into a local coordinate system with the vehicle as a center;
grid queue set for traversing right obstaclerightFirstly, converting grid coordinates of the obstacle grid into a global coordinate system with an unchanged origin, and then converting the grid coordinates into a local coordinate system with the vehicle as a center;
step 3, converting the set into the set under the global coordinate systemleftSorting according to the x coordinate of the barrier grid from large to small according to the number num of the road boundary pointsedgeFrom sequenced setleftMiddle selected top numedgeThe obstacle points form a road boundary queue edge closest to the left side of the vehicleleft；
Set for conversion into global coordinate systemrightAccording to the barrier fenceThe x-coordinates of the grids are sorted from small to large, from the sorted setrightMiddle selected top numedgeThe obstacle points form a road boundary queue edge closest to the right side of the vehicleright；
Step 4, sampling according to the road sampling intervaledgeTo edgeleft、edgerightAccording to sampleedgeCarrying out interval sampling, and respectively putting the road boundary points obtained by sampling into a queue SEleft、SEright；
Step 5, first traverse SEleftTaking out the obstacle points obs to be processedleft(ii) a Then traverse SErightSelecting the distance obs on the y-axisleftNearest points obsrightAs a result of the matching;
step 6, the left and right obstacle points obs are processedleft、obsrightIs converted into a global coordinate system, and the coordinates (x) of the center point of the road are calculatedc，yc)；
And 7, putting the road center point into a backing center reference point queue refPath.
7. The method for controlling the backing navigation of the intelligent inter-ridge pesticide spraying vehicle in the shed according to claim 1, wherein the specific implementation process of the step S4 comprises the following steps:
step 1, traversing a reversing center reference point queue refPath, and selecting a point in the refPath which is closest to a current intelligent vehicle as a starting point of local track planning;
step 2, selecting the last point in the refPath as a terminal point of the local track planning;
step 3, processing dynamic obstacle collision constraint, and forming a point-shaped obstacle set by grids with obstaclespobs；
Step 4, converting the nonlinear least square problem of the local reversing planning into a multi-objective optimization problem, converting the multi-objective optimization problem into a graph optimization problem, and determining the top point and the edge of the graph;
step 5, calling the g2o framework to calculate the graph optimization problem to obtain the optimal local backing track optimalTraj, the local reversing track is composed of a series of optimal state points (pos)iAt a time interval of Δ TiComposition, i ═ 1, 2, … …, n: n is the number of state points in an optimal track;
optimalTraj＝(pose1，ΔT1，pose2，ΔT2，...，ΔTn-1，posen)；
wherein the optimum state point is defined by two-dimensional plane coordinates (x)i，yi) And a desired heading angle thetaiConsists of the following components:
posei＝(xi，yi，θi)；
step 6, carrying out feasibility check on the local reversing track, and executing step 7 if the local reversing track passes the feasibility check;
if the check is not passed, a warning is given, and step 9 is executed;
step 7, processing the local backing track optimalTraj and utilizing two adjacent optimal state points (pos)iAt a time interval of Δ TiAnd calculating the speed of the current track point, wherein the calculation formula of the linear speed is as follows:
Figure FDA0003337890730000051
the calculation formula of the angular velocity is:
Figure FDA0003337890730000052
and 8, limiting the track points exceeding the maximum speed constraint:
Figure FDA0003337890730000053
step 9, judging whether the current vehicle running state is abnormal or not, and if the current vehicle running state is normal, executing step 10; if the planning fails, selecting a middle point in the refPath as a terminal point of the local track planning, and executing the step 1 and the steps 3-6 again; if the vehicle is detected to be in an oscillation state, namely the linear velocity and the angular velocity are both smaller than given values, the steps 1 to 6 are executed again;
step 10, judging whether an operation finishing point is reached, and finishing reversing navigation if the vehicle backs from the inter-ridge road to the main road in the shed; otherwise, re-executing the step 1 to the step 9.
8. The reversing navigation control method for the intelligent inter-ridge pesticide spraying vehicle in the shed according to claim 7, wherein the specific implementation process of the step 3 comprises the following steps:
step 1), traversing all grid coordinates in the local cost map, and if the cost of the coordinates is 1, converting the grid coordinates into a global coordinate system;
step 2), traversing and converting the coordinates (x) of the obstacle points under the global coordinate systempobs，ypobs) Calculating the coordinates (x) of the center of the vehiclevel，yvel) As a starting point, (x)pobs，ypobs) Vector formed for endpoint
Figure FDA0003337890730000054
Figure FDA0003337890730000055
Is expressed as:
Figure FDA0003337890730000061
and 3), acquiring the current heading angle theta of the vehicle from the positioning information, wherein the current direction vector of the vehicle can be expressed as:
Figure FDA0003337890730000062
step 4, calculating
Figure FDA0003337890730000063
And
Figure FDA0003337890730000064
vector dot product of (a):
Figure FDA0003337890730000065
if dotProduct is positive, the obstacle point is in front of the vehicle and is not in the consideration range of the backing plan, and then the method is ended; if dotProduct is negative, indicating that the obstacle point is behind the vehicle, the obstacle point is added to the set of point-shaped obstacles setpobsIn (1).
9. The reversing navigation control method for the intelligent intra-canopy inter-ridge pesticide spraying vehicle according to claim 1, characterized in that a transverse acceleration control command avThe calculation formula is as follows:
av＝Kpvepv+Kiveiv；
longitudinal acceleration control command aωThe calculation formula is as follows:
aω＝Kpωepω+Kiωeiω；
wherein, Kpv、KivRespectively is a preset longitudinal speed position error coefficient and a longitudinal speed integral error coefficient; e.g. of the typepv＝vwp-vv；
Figure FDA0003337890730000066
vv、ωvVehicle longitudinal velocity and rotational angular velocity, respectively.
10. A backing navigation control system of an intelligent vehicle for spraying pesticide between ridges in a shed is characterized by comprising computer equipment; the computer device is configured or programmed for carrying out the steps of the method according to one of claims 1 to 9.
CN202111299516.7A 2021-11-04 2021-11-04 Reversing navigation control method and system of intelligent pesticide spraying vehicle among ridges in greenhouse Active CN114063615B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111299516.7A CN114063615B (en) 2021-11-04 2021-11-04 Reversing navigation control method and system of intelligent pesticide spraying vehicle among ridges in greenhouse

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111299516.7A CN114063615B (en) 2021-11-04 2021-11-04 Reversing navigation control method and system of intelligent pesticide spraying vehicle among ridges in greenhouse

Publications (2)

Publication Number Publication Date
CN114063615A true CN114063615A (en) 2022-02-18
CN114063615B CN114063615B (en) 2023-12-19

Family

ID=80273757

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111299516.7A Active CN114063615B (en) 2021-11-04 2021-11-04 Reversing navigation control method and system of intelligent pesticide spraying vehicle among ridges in greenhouse

Country Status (1)

Country Link
CN (1) CN114063615B (en)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114734482A (en) * 2022-04-06 2022-07-12 深圳市普渡科技有限公司 Method for testing road recognition function, computer device and storage medium
CN115251023A (en) * 2022-08-17 2022-11-01 陕西理工大学 Intelligent pesticide spraying method and system
CN117948984A (en) * 2024-03-26 2024-04-30 广东省科学院智能制造研究所 Path planning method of mobile operation robot in variable width traffic domain

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107688337A (en) * 2016-08-03 2018-02-13 苏州青飞智能科技有限公司 A kind of magnetic navigation unmanned vehicle system with the function that turns around automatically
CN109324630A (en) * 2018-09-19 2019-02-12 清远市飞凡创丰科技有限公司 A kind of unmanned plane pesticide spraying system and control method
CN109964596A (en) * 2019-04-01 2019-07-05 华南农业大学 A kind of direct sowing of rice apparatus and method based on intelligent robot
CN112461227A (en) * 2020-10-22 2021-03-09 新兴际华集团有限公司 Intelligent autonomous navigation method for polling wheel type chassis robot

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107688337A (en) * 2016-08-03 2018-02-13 苏州青飞智能科技有限公司 A kind of magnetic navigation unmanned vehicle system with the function that turns around automatically
CN109324630A (en) * 2018-09-19 2019-02-12 清远市飞凡创丰科技有限公司 A kind of unmanned plane pesticide spraying system and control method
CN109964596A (en) * 2019-04-01 2019-07-05 华南农业大学 A kind of direct sowing of rice apparatus and method based on intelligent robot
CN112461227A (en) * 2020-10-22 2021-03-09 新兴际华集团有限公司 Intelligent autonomous navigation method for polling wheel type chassis robot

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
BAIFAN CHEN;ZIXING CAI;ZHENG XIAO;JINXIA YU;LIMEI LIU: "Real-time Detection of Dynamic Obstacle Using Laser Radar", 《《2008 THE 9TH INTERNATIONAL CONFERENCE FOR YOUNG COMPUTER SCIENTISTS》》, pages 1728 - 1732 *
LINGLI YU ETAL.: "A Path Planning and Navigation Control System Design for Driverless Electric Bus", 《IEEE ACCESS》, pages 53960 - 53975 *
QINGCHUN FENG, BOWEN WANG, WANHAO ZHANG AND XIAOMING LI: "Development and Test of Spraying Robot for Anti-epidemic and Disinfection in Animal Housing", 《PROCEEDINGS OF THE 3RD WRC SYMPOSIUM ON ADVANCED ROBOTICS AND AUTOMATION 2021》, pages 24 - 29 *
凌雯雯: "水稻插秧机自主作业路径规划算法研究", 《中国优秀硕士学位论文全文数据库农业科技辑》农业科技辑》, pages 044 - 4 *
蒋蘋: "基于双源激光的田间作业机械导航定位系统研究", 《中国博士学位论文全文数据库 农业科技辑》, pages 044 - 1 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114734482A (en) * 2022-04-06 2022-07-12 深圳市普渡科技有限公司 Method for testing road recognition function, computer device and storage medium
CN114734482B (en) * 2022-04-06 2024-01-12 深圳市普渡科技有限公司 Road recognition function test method, computer device and storage medium
CN115251023A (en) * 2022-08-17 2022-11-01 陕西理工大学 Intelligent pesticide spraying method and system
CN115251023B (en) * 2022-08-17 2023-08-22 陕西理工大学 Intelligent pesticide spraying system
CN117948984A (en) * 2024-03-26 2024-04-30 广东省科学院智能制造研究所 Path planning method of mobile operation robot in variable width traffic domain
CN117948984B (en) * 2024-03-26 2024-05-31 广东省科学院智能制造研究所 Path planning method of mobile operation robot in variable width traffic domain

Also Published As

Publication number Publication date
CN114063615B (en) 2023-12-19

Similar Documents

Publication Publication Date Title
CN114063615A (en) Backing navigation control method and system for intelligent vehicle for spraying pesticide between ridges in shed
Gehrig et al. Collision avoidance for vehicle-following systems
Levinson et al. Towards fully autonomous driving: Systems and algorithms
Petrovskaya et al. Model based vehicle detection and tracking for autonomous urban driving
CN112015176B (en) Unmanned tractor field operation path planning method and device
CN113204236A (en) Intelligent agent path tracking control method
CN115525047B (en) Vehicle local track planning method and system with multi-type obstacle avoidance mode
WO2022165614A1 (en) Path construction method and apparatus, terminal, and storage medium
Liu et al. Reinforcement-tracking: An effective trajectory tracking and navigation method for autonomous urban driving
CN116839570B (en) Crop interline operation navigation method based on sensor fusion target detection
CN113515111B (en) Vehicle obstacle avoidance path planning method and device
CN115268443A (en) Robot obstacle avoidance path planning method
Menon et al. NBV-SC: Next best view planning based on shape completion for fruit mapping and reconstruction
Huang et al. An online multi-lidar dynamic occupancy mapping method
CN116331264A (en) Obstacle avoidance path robust planning method and system for unknown obstacle distribution
CN113859226B (en) Motion planning and automatic parking method based on reinforcement learning
CN112230638A (en) Parking path planning method and device for vehicle
CN112519783B (en) Method and system for generating bottom-up smooth track of intelligent driving
Schratter et al. LiDAR-based mapping and Localization for autonomous racing
CN115525054B (en) Method and system for controlling tracking of edge path of unmanned sweeper in large industrial park
Chen et al. From perception to control: an autonomous driving system for a formula student driverless car
Petrovskaya et al. Model based vehicle tracking in urban environments
Rasmussen et al. Trail following with omnidirectional vision
Dong et al. Path Planning Research for Outdoor Mobile Robot
CN113587946A (en) Visual navigation system and method for field agricultural machine

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant