Simulation test platform and test method for orchard vehicle autonomous navigation algorithm

Abstract

The application provides a simulation test platform and a test method for an orchard vehicle autonomous navigation algorithm, which are used for testing and evaluating the overall performance of a navigation path generation and tracking algorithm in an orchard vehicle autonomous navigation algorithm based on a LiDAR under different orchard planting parameters and initial states of the vehicle. The test method comprises the following steps: an orchard map configuration step, a radar configuration and measurement step, a path generation and tracking step, and a simulation execution step. The test platform design method provided by the application can shorten the research and development cycle of the orchard vehicle autonomous navigation algorithm, reduce the research and development cost, effectively test and evaluate the algorithm performance under different orchard planting parameters and initial states of the vehicle, and meet the test requirements of the orchard vehicle autonomous navigation algorithm.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation technology, particularly autonomous navigation for orchard vehicles, and more specifically, to a simulation test platform design method for autonomous navigation algorithms for orchard vehicles. Background Technology

[0002] Autonomous navigation technology for orchard vehicles is one of the core technologies for achieving refined management of orchards. It is widely used in orchard production processes such as plant protection, weeding, and harvesting, and can effectively improve the accuracy and efficiency of orchard operations and reduce the labor intensity of operators.

[0003] The invention of LiDAR (Light Detection and Ranging) offers advantages such as high ranging accuracy, high resolution, and strong anti-interference capabilities, and is widely used in orchard vehicle autonomous navigation and environmental perception. LiDAR emits laser pulses in various directions through rotating optical components, enabling area scanning and contour measurement. During installation, the LiDAR scanning surface is aligned parallel to the ground to acquire contour information of the tree trunks surrounding the vehicle.

[0004] Research on autonomous navigation technology for orchard vehicles requires hardware such as lidar and orchard vehicles, as well as extensive field testing to evaluate the navigation performance of the algorithm under different planting environments and initial vehicle conditions. To shorten the development cycle and reduce costs, researchers use simulation technology to model the orchard working environment, sensor measurements, and vehicle movement to verify the performance of the autonomous navigation algorithm for orchard vehicles.

[0005] However, existing simulation tests of autonomous navigation algorithms for orchard vehicles focus on testing navigation path tracking algorithms, lacking overall performance testing of navigation path generation and tracking algorithms, as well as performance evaluation of algorithms under different environments. Summary of the Invention

[0006] The purpose of this invention is to address the problem of testing radar-based autonomous navigation path generation and tracking algorithms for orchard vehicles. It proposes a simulation testing platform and method for testing and evaluating the overall performance of LiDAR-based autonomous navigation path generation and tracking algorithms for orchard vehicles under different orchard planting parameters and initial vehicle conditions.

[0007] The technical solution of this invention is:

[0008] This invention provides a simulation testing method for an autonomous navigation algorithm for orchard vehicles, the method comprising the following steps:

[0009] S1. Orchard map configuration steps: Based on the orchard planting parameters entered by the user, generate the center position and diameter of the tree trunk, establish an orchard planting model based on the tree trunk information to simulate the orchard environment, and output the orchard map M.

[0010] S2. Radar Configuration and Measurement Steps: Based on the radar parameters entered by the user, a LiDAR measurement model is established. This measurement model can be based on the world coordinate system xO at time t. W The vehicle pose under y is obtained to obtain the vehicle coordinate system xO. V Polar coordinates of LiDAR measurement points under y-axis;

[0011] S3. Path Generation and Tracking Steps: Based on the polar coordinates of the LiDAR measurement points at time t, the navigation algorithm under test is first used to calculate the navigation path, and the navigation parameters at time t are output. Then, the navigation algorithm under test is used to guide the vehicle to follow the navigation path, and the expected speeds of the left and right wheels at time t are output.

[0012] S4. Simulation Execution Steps: On the orchard map configured in step S1, perform measurements according to the LiDAR measurement model configured in step S2. Configure vehicle operation parameters based on the navigation parameters generated by the navigation algorithm to be tested in step S3 and the expected vehicle speeds of the left and right wheels. Execute at time Δt to obtain the vehicle pose at time t+Δt. Return to S2 to continue execution and obtain the simulation result, i.e., the vehicle trajectory.

[0013] Furthermore, S1 specifically refers to:

[0014] S1-1, Orchard planting parameters entered by the user, including number of rows N1, number of trees N2, row spacing D1, tree spacing D2, and trunk diameter D3;

[0015] S1-2 generates the trunk center coordinates (x, y) using the following formula based on the planting parameters. T (k, j), y T (k, j) and trunk diameter d T (k, j):

[0016]

[0017] Where: k represents the tree trunk row number, j represents the tree trunk plant number, ε1 represents the random perturbation of the tree trunk planting position in the x-axis direction, ε2 represents the random perturbation of the tree trunk planting position in the y-axis direction, and ε3 represents the random perturbation of the tree trunk diameter.

[0018] S1-3. Establish an orchard planting model to simulate the orchard environment. Use black dots on a white background to represent tree trunks, and determine the center coordinates of the tree trunks (x, y, y). T y T and trunk diameter dT In the world coordinate system xO W Draw and output the orchard map M under y.

[0019] Furthermore, in S1-2: for simulating a curved tree row, the center row coordinates of the trunk of the j-th tree in the i-th row are calculated using the following formula:

[0020]

[0021] Where: λ represents the wavelength of the sinusoidal perturbation, and A represents the amplitude of the sinusoidal perturbation;

[0022] To simulate missing tree rows, N3 integers in the range [1, N2] are randomly generated for each row as the missing tree numbers.

[0023] Furthermore, S2 specifically refers to:

[0024] S2-1. User inputs radar parameters, including the maximum measurement range r of LiDAR. max Scanning angle range θ range 1. Scan angular resolution Δθ and starting scan angle θ0; 2. User input of world coordinate system xO W Initial vehicle pose (x) under y V (0), y V (0), α V (0));

[0025] S2-2. Based on the radar parameters, establish a LiDAR measurement model to calculate the number of scan lines N and the scan angle θ of each scan line. i :

[0026]

[0027] θ i =θ0 + Δθ(i-1)

[0028] Where: N represents the total number of scan lines, and i represents the scan line number;

[0029] Let the starting position of the LiDAR scan line be the same as the vehicle's center of gravity O. V Overlap, at time t, the i-th scan line is in the world coordinate system xO W The coordinates under y are x L y L :

[0030]

[0031] Where: r is the scan line radius; (x V (t), y V (t), α V (t) represents the world coordinate system xO at time t.W Vehicle position under y;

[0032] For each scan line, let r start from 0 and increase at intervals Δr, and then perform the scan;

[0033] S2-3, In the orchard map, when the scan line coordinates (x... L y L When a pixel at position () is black, it is assumed that the scan line has touched the tree trunk, and the extreme diameter of the scan line at this time is recorded as the measured value r. i * The measurement distance r returned by the i-th scan line is obtained using the following formula. i :

[0034] r i (t)=r i * +ε L

[0035] Where: ε L This refers to random measurement errors in LiDAR.

[0036] If the i-th scan line does not find a tree trunk point within the orchard map or the maximum measurement distance, let r i (t)=r max ;

[0037] Finally, the coordinates in the vehicle coordinate system xO are obtained. V polar coordinates of LiDAR measurement point at time t under y

[0038] Furthermore, the interval Δr is less than or equal to the size of a single pixel in the fruit tree map.

[0039] Furthermore, in S4, configuring the vehicle operating parameters specifically includes:

[0040] S4-1, Motor Control: Establish a motor model to simulate motor torque changes, and use a PID controller to control the left and right motors, based on the desired vehicle speeds of the left and right wheels at time t. and actual vehicle speed (v) L (t), v R (t)), to obtain the motor torque (τ) at time t. L (t), τ R (t));

[0041] S4-2, Vehicle Dynamics Control: Establish a two-wheel differential drive vehicle dynamics model to simulate vehicle speed changes, based on the left and right motor torques (τ) at time t. L (t), τ R (t)), to obtain the actual vehicle speed (v) of the left and right wheels at time t. L (t), vR (t));

[0042] S4-3, Vehicle Kinematics Control: Establish a vehicle kinematics model to simulate vehicle pose changes, based on the actual vehicle speeds (v) of the left and right wheels at time t. L (t), v R (t)), obtain xO in the world coordinate system at time t. W Vehicle pose (x) V (t), y V (t), α V (t));

[0043] Let S4-1 to S4-3 run for a time Δt, and obtain the vehicle pose (x) at time t+Δt. V (t + Δt), y V (t+Δt), α V (t+Δt)), and then start executing from S2.

[0044] Furthermore, in S4-1, the motor control specifically includes:

[0045] S4-1a, Based on the expected speeds of the left and right wheels at time t and actual vehicle speed (v) L (t), v R (t)), calculate the speed error (e) of the left and right motors. L (t), e R (t)):

[0046]

[0047] S4-1b, Calculate the control voltage (u) of the left and right motors using the PID parameter tuning method. L (t), u R (t)):

[0048]

[0049] Where: K P K I and K D These are the proportional, integral, and derivative parameters preset by the PID parameter tuning method; They represent e respectively L (t) and e R The differential of (t);

[0050] S4-1c, Using the linear saturation method, the amplitude of the control voltage for the left and right motors is limited to [-u]. max u max [Inside, that is, when |u] L (t)|>|u max |or|uR (t)|>|u max |time, let|u L (t)|=|u max |, |u R (t)|=|u max | The voltage direction remains unchanged;

[0051] S4-1d, control voltage (u) using left and right motors L (t), u R (t) is the input variable, and the speeds of the left and right motors (n) are the input variables. L (t), n R (t) is the state variable, and the torque (τ) of the left and right motors at time t is obtained according to the motor model below. L (t), τ R (t)):

[0052]

[0053] Where: k e k c J a and R a These are the motor back electromotive force coefficient, motor torque coefficient, motor moment of inertia, and motor armature resistance, respectively. Representing n respectively R (t), n L The differential of (t).

[0054] Furthermore, in S4-2, the vehicle dynamics control specifically refers to: based on the left and right motor torques (τ) at time t. L (t), τ R (t) The actual vehicle speeds (v) of the left and right wheels at time t are obtained using the following vehicle dynamics model. L (t), v R (t)):

[0055]

[0056] Where: D represents the vehicle track width, D' represents the distance between the vehicle's center of gravity and the geometric centers of the two drive wheels, m represents the vehicle mass, J represents the vehicle's moment of inertia, and r' ... V Indicates the wheel radius. They represent v respectively L (t), v R The differential of (t).

[0057] Furthermore, in S4-3, the vehicle kinematic control specifically refers to: based on the actual vehicle speed (v) of the left and right wheels at time t. L (t), v R(t)), the following vehicle kinematics model is used to obtain xO in the world coordinate system at time t. W Vehicle pose (x) V (t), y V (t), α V (t)):

[0058]

[0059] in: Don't be x V (t), y V (t), α V The differential of (t).

[0060] A simulation test platform for an autonomous navigation algorithm for orchard vehicles, based on the simulation test method for the same algorithm, comprises:

[0061] Orchard Map Configuration Unit: Based on the orchard planting parameters entered by the user, it generates the center position and diameter of the tree trunk, establishes an orchard planting model based on the tree trunk information to simulate the orchard environment, and outputs the orchard map M;

[0062] Radar configuration and measurement unit: Based on the radar parameters entered by the user, a LiDAR measurement model is established. This measurement model can be based on the world coordinate system xO at time t. W The vehicle pose under y is obtained to obtain the vehicle coordinate system xO. V Polar coordinates of LiDAR measurement points under y-axis;

[0063] Path generation and tracking unit: Based on the polar coordinates of the LiDAR measurement points at time t, it first calculates the navigation path using the navigation algorithm under test and outputs the navigation parameters at time t. Then, the navigation algorithm under test is used to guide the vehicle to travel along the navigation path, and the expected speeds of the left and right wheels at time t are output.

[0064] Simulation execution unit: Configures vehicle operating parameters based on the configured orchard map, LiDAR measurement model, navigation parameters, and desired speeds of the left and right wheels, executes the simulation, and obtains the simulation results, i.e., the vehicle's operating trajectory.

[0065] The beneficial effects of this invention are:

[0066] The simulation testing platform and testing method of this invention realize virtual orchard measurement and vehicle motion process simulation by constructing an orchard planting model, a LiDAR measurement model, a vehicle kinematics model, a vehicle dynamics model and a motor control model. It can test and evaluate the algorithm performance under different orchard planting parameters and vehicle initial state.

[0067] The simulation test platform design method for orchard vehicle autonomous navigation algorithm proposed in this invention can shorten the R&D cycle and reduce the R&D cost of orchard vehicle autonomous navigation algorithm, effectively test and evaluate the algorithm performance under different orchard planting parameters and vehicle initial state, and meet the testing requirements of orchard vehicle autonomous navigation algorithm.

[0068] Other features and advantages of the present invention will be described in detail in the following detailed description section. Attached Figure Description

[0069] The above and other objects, features and advantages of the present invention will become more apparent from the more detailed description of exemplary embodiments of the invention in conjunction with the accompanying drawings, wherein the same reference numerals generally represent the same components in the exemplary embodiments of the invention.

[0070] Figure 1 A flowchart of the simulation testing method of the present invention is shown.

[0071] Figure 2 A schematic diagram of the LiDAR measurement process in the embodiment is shown.

[0072] Figure 3 A schematic diagram of LiDAR measurement data from an embodiment is shown.

[0073] Figure 4 A schematic diagram of a differential drive vehicle in an embodiment is shown.

[0074] Figure 5 A schematic diagram of vehicle trajectory and deviation analysis in the embodiment is shown. Detailed Implementation

[0075] Preferred embodiments of the invention will now be described in more detail with reference to the accompanying drawings. While preferred embodiments of the invention are shown in the drawings, it should be understood that the invention can be implemented in various forms and should not be limited to the embodiments set forth herein.

[0076] A simulation testing method for an autonomous navigation algorithm for orchard vehicles, such as... Figure 1 As shown, the method includes the following steps:

[0077] S1. Orchard map configuration steps: Based on the orchard planting parameters entered by the user, generate the center position and diameter of the tree trunk, establish an orchard planting model based on the tree trunk information to simulate the orchard environment, and output the orchard map M.

[0078] S1-1, Orchard planting parameters entered by the user, including number of rows N1, number of trees N2, row spacing D1, tree spacing D2, and trunk diameter D3;

[0079] S1-2 generates the trunk center coordinates (x, y) using the following formula based on the planting parameters.T (k, j), y T (k, j) and trunk diameter d T (k, j):

[0080]

[0081] Where: k represents the tree trunk row number, j represents the tree trunk plant number, ε1 represents the random perturbation of the tree trunk planting position in the x-axis direction, ε2 represents the random perturbation of the tree trunk planting position in the y-axis direction, and ε3 represents the random perturbation of the tree trunk diameter.

[0082] To simulate a curved tree row, the center row coordinates of the trunk of the j-th tree in the i-th row are calculated using the following formula:

[0083]

[0084] Where: λ represents the wavelength of the sinusoidal perturbation, and A represents the amplitude of the sinusoidal perturbation;

[0085] To simulate missing tree rows, N3 integers in the range [1, N2] are randomly generated for each row as the missing tree numbers.

[0086] S1-3. Establish an orchard planting model to simulate the orchard environment. Use black dots on a white background to represent tree trunks, and determine the center coordinates of the tree trunks (x, y, y). T y T and trunk diameter d T In the world coordinate system xO W Draw and output the orchard map M under y.

[0087] S2. Radar Configuration and Measurement Steps: Based on the radar parameters entered by the user, a LiDAR measurement model is established. This measurement model can be based on the world coordinate system xO at time t. W The vehicle pose under y is obtained to obtain the vehicle coordinate system xO. V The polar coordinates of the LiDAR measurement point under y; S2 specifically refers to:

[0088] S2-1. User inputs radar parameters, including the maximum measurement range r of LiDAR. max Scanning angle range θ range 1. Scan angular resolution Δθ and starting scan angle θ0; 2. User input of world coordinate system xO W Initial vehicle pose (x) under y V (0), y V (0), α V (0));

[0089] S2-2. Based on the radar parameters, establish a LiDAR measurement model to calculate the number of scan lines N and the scan angle θ of each scan line. i :

[0090]

[0091] θ i =θ0 + Δθ(i-1)

[0092] Where: N represents the total number of scan lines, and i represents the scan line number;

[0093] Let the starting position of the LiDAR scan line be the same as the vehicle's center of gravity O. V Overlap, at time t, the i-th scan line is in the world coordinate system xO W The coordinates under y are x L y L :

[0094]

[0095] Where: r is the scan line radius; (x V (t), y V (t), α V (t) represents the world coordinate system xO at time t. W Vehicle position under y;

[0096] For each scan line, let r start from 0 and increase at intervals Δr, and perform the scan; the interval Δr is less than or equal to the size of a single pixel in the fruit tree map;

[0097] S2-3, In the orchard map, when the scan line coordinates (x... L y L When a pixel at position () is black, it is assumed that the scan line has touched the tree trunk, and the extreme diameter of the scan line at this time is recorded as the measured value r. i * ,like Figure 2 , 3 As shown, a random measurement error ε of LiDAR is introduced. L The measured distance r returned by the i-th scan line i for:

[0098] r i (t)=r i * +ε L

[0099] Where: ε L This refers to random measurement errors in LiDAR.

[0100] If the i-th scan line does not find a tree trunk point within the orchard map or the maximum measurement distance, let r i (t)=r max ;

[0101] Finally, the coordinates in the vehicle coordinate system xO are obtained. Vpolar coordinates of LiDAR measurement point at time t under y

[0102] S3. Path Generation and Tracking Steps: Based on the polar coordinates of the LiDAR measurement points at time t, the navigation algorithm under test is first used to calculate the navigation path, and the navigation parameters at time t are output. Then, the navigation algorithm under test is used to guide the vehicle to follow the navigation path, and the expected speeds of the left and right wheels at time t are output.

[0103] S4. Simulation Execution Steps: On the orchard map configured in step S1, perform measurements according to the LiDAR measurement model configured in step S2. Configure vehicle operation parameters based on the navigation parameters generated by the navigation algorithm to be tested in step S3 and the expected vehicle speeds of the left and right wheels. Execute at time Δt to obtain the vehicle pose at time t+Δt. Return to S2 to continue execution and obtain the simulation result, i.e., the vehicle trajectory.

[0104] Specifically, configuring vehicle operating parameters includes:

[0105] S4-1, Motor Control: Establish a motor model to simulate motor torque changes, and use a PID controller to control the left and right motors, based on the desired vehicle speeds of the left and right wheels at time t. and actual vehicle speed (v) L (t), v R (t)), to obtain the motor torque (τ) at time t. L (t), τ R (t));

[0106] The motor control is specifically as follows:

[0107] S4-1a, Based on the expected speeds of the left and right wheels at time t and actual vehicle speed (v) L (t), v R (t)), calculate the speed error (e) of the left and right motors. L (t), e R (t)):

[0108]

[0109] S4-1b, Calculate the control voltage (u) of the left and right motors using the PID parameter tuning method. L (t), u R (t)):

[0110]

[0111] Where: K P K I and K DThese are the proportional, integral, and derivative parameters preset by the PID parameter tuning method; They represent e respectively L (t) and e R The differential of (t);

[0112] S4-1c, Using the linear saturation method, the amplitude of the control voltage for the left and right motors is limited to [-u]. max u max [Inside, that is, when |u] L (t)|>|u max |or|u R (t)|>|u max |time, let|u L (t)|=|u max |, |u R (t)|=|u max | The voltage direction remains unchanged;

[0113] S4-1d, control voltage (u) using left and right motors L (t), u R (t) is the input variable, and the speeds of the left and right motors (n) are the input variables. L (t), n R (t) is the state variable, and the torque (τ) of the left and right motors at time t is obtained according to the motor model below. L (t), τ R (t)):

[0114]

[0115] Where: k e k c J a and R a These are the motor back electromotive force coefficient, motor torque coefficient, motor moment of inertia, and motor armature resistance, respectively. Representing n respectively R (t), n L The differential of (t).

[0116] S4-2, Vehicle Dynamics Control: Based on the left and right motor torques (τ) at time t L (t), τ R (t) The actual vehicle speeds (v) of the left and right wheels at time t are obtained using the following vehicle dynamics model. L (t), v R (t)):

[0117]

[0118] Where: D represents the vehicle track width, d represents the distance between the vehicle's center of mass and the geometric centers of the two drive wheels, m represents the vehicle mass, J represents the vehicle's moment of inertia, and r... V Indicates the wheel radius. They represent v respectively L (t), v R The differential of (t).

[0119] S4-3, Vehicle Kinematics Control: Based on the actual vehicle speed (v) of the left and right wheels at time t. L (t), v R (t)), the following vehicle kinematics model is used to obtain xO in the world coordinate system at time t. W Vehicle pose (x) V (t), y V (t), α V (t)):

[0120]

[0121] in: Don't be x V (t), y V (t), α V The differential of (t).

[0122] Let S4-1 to S4-3 run for a time Δt, and obtain the vehicle pose (x) at time t+Δt. V (t + Δt), y V (t+Δt), α V (t+Δt)), such as Figure 4 As shown, execution then begins from S2.

[0123] A simulation test platform for autonomous navigation algorithms for orchard vehicles, executing the aforementioned simulation test method.

[0124] Let the orchard map pixel size be 10mm, and the LiDAR detection interval Δr = 5mm. The orchard planting parameters are set as follows: number of rows N1 = 5, number of trees N2 = 10, row spacing D1 = 4m, tree spacing D2 = 2m, trunk diameter D3 = 0.12m, random perturbations ε1 and ε2 of the trunk position both follow a uniform distribution U(-0.2m, 0.2m), and random perturbations ε3 of the trunk diameter follow a uniform distribution U(-0.03m, 0.03m).

[0125] LiDAR parameters are set as follows: Maximum measurement distance r max =15m, scanning angle range θ range =270°, scanning angular resolution Δθ = 0.25°, initial scanning angle θ0 = -45°, measurement error ε LThe vehicle follows a uniform distribution U(-30mm, 30mm). Vehicle parameters are set as follows: wheelbase D = 0.5m, center-of-gravity distance d = 0.2m, vehicle mass m = 5kg, moment of inertia J = 2.5kg·m. 2 The wheel radius r = 0.04m. The motor parameters are set as follows: motor limiting voltage u... max =1.7V, motor back electromotive force coefficient k e =132.4, torque coefficient k c =1.2×10 -4 Motor rotational inertia J a =1.6×10 -4 kg·m 2 armature resistance R a =50Ω. PID parameter settings are as follows: K P =8,K I =0.9, K D =0.2.

[0126] A simulation test platform was built using the mathematical analysis software MATLAB 2018b to test and evaluate the orchard path generation and extraction algorithm based on 2D LiDAR. Four model parameters were selected as experimental factors: curve tree row amplitude A, row missing tree number N3, initial lateral deviation of the vehicle, and initial yaw angle. Single-factor experiments were conducted for each parameter.

[0127] The specific values ​​are shown in Table 1. When conducting an experiment on a certain experimental factor, the other experimental factors are set to 0. Let the navigation time interval Δt = 0.5s. The single-factor experimental results obtained using navigation deviation visualization are shown in Table 1. The vehicle trajectory is as follows: Figure 5 As shown, with the increase of the curve tree row amplitude, the vehicle has a larger initial lateral deviation and yaw angle. At the same time, the goodness of fit of the curve tree row using straight lines decreases, resulting in reduced navigation accuracy and increased lag. When A = 1.0m, the average value of |d| reaches 15.19cm, and the standard deviation reaches 14.38cm. With the increase of the number of missing trees in the row, the accuracy of the tree row fitting decreases, and there is a larger |d| at the missing tree locations. When the vehicle has an initial lateral deviation and yaw angle, there is a large |d| in the initial stage of navigation. Then the vehicle trajectory can converge to the navigation path relatively quickly, thereby eliminating the influence of the initial deviation.

[0128] Experimental results show that the simulation test platform is reliable. When the amplitude of the curve tree row, the number of missing trees in the row, and the initial deviation increase, the performance of the autonomous navigation algorithm decreases, which is consistent with the actual situation.

[0129] Table 1 Results of the single-factor experiment

[0130]

[0131]

[0132] The various embodiments of the present invention have been described above. These descriptions are exemplary and not exhaustive, nor are they limited to the disclosed embodiments. Many modifications and variations will be apparent to those skilled in the art without departing from the scope and spirit of the described embodiments.

Claims

1. A simulation test method for an orchard vehicle autonomous navigation algorithm, characterized in that, The method comprises the following steps: S1, orchard map configuration step: generating a tree trunk center position and a tree trunk diameter according to user-entered orchard planting parameters, establishing an orchard planting model based on the tree trunk information to simulate an orchard environment, and outputting an orchard map M; S2, radar configuration and measurement step: according to the radar parameters entered by the user, a LiDAR measurement model is established, which can obtain the polar coordinates of the LiDAR measurement points under the vehicle coordinate system xO W y according to the vehicle pose under the world coordinate system xO V y at time t. S3, path generation and tracking step: according to the polar coordinates of the LiDAR measurement points at time t, first use the navigation algorithm to be tested to calculate the navigation path, output the navigation parameters at time t Then use the navigation algorithm to be tested to track the path and guide the vehicle to drive according to the navigation path, output the left and right wheel expected vehicle speed at time t S4, simulation execution step: performing measurement on the orchard map configured in step S1 according to the LiDAR measurement model configured in step S2, configuring vehicle running parameters according to the navigation parameters generated by the to-be-tested navigation algorithm in step S3 and the expected vehicle speed of the left and right wheels, executing at time t+Δt, obtaining the vehicle pose at time t+Δt, returning to S2 for continuous execution, and obtaining a simulation result, i.e., a vehicle running trajectory; The S1 is specifically: S1-1, user-entered orchard planting parameters, including row number N1, plant number N2, row spacing D1, plant spacing D2, and tree trunk diameter D3; S1-2 According to the planting parameters, the following formula is used to generate the trunk center coordinates (x T (k,j),y T (k,j)) and the trunk diameter d T (k,j): Wherein: k represents the row number of the tree trunk, j represents the plant number of the tree trunk, ε1 represents the random disturbance of the x-axis direction tree trunk planting position, ε2 represents the random disturbance of the y-axis direction tree trunk planting position, and ε3 represents the random disturbance of the tree trunk diameter; S1-3, a plantation model of orchard is established to simulate the environment of orchard, a black dot on white background represents a tree trunk, according to the trunk center coordinate (x T ,y T ) and the trunk diameter d T , a map of orchard M is drawn and output in the world coordinate system xO W y In S1-2, to simulate a curved tree row, the tree trunk center row coordinate of the i-th row and the j-th plant of the fruit tree is as follows: Wherein: λ represents the wavelength of the sinusoidal disturbance, and A represents the amplitude of the sinusoidal disturbance; To simulate a tree-lacking row, N3 integers in the range [1, N2] are randomly generated for each row as tree-lacking plant numbers; In S4, configuring vehicle running parameters specifically includes: S4-1, motor control: establish motor model to simulate motor torque change, adopt PID controller to realize left and right motor control, based on left and right wheel expected speed at t moment and actual speed (v L (t),v R (t)) at t moment, obtain left and right motor torque (τ L (t),τ R (t)) at t moment; S4-2, Vehicle dynamics control: Establish two-wheel differential drive vehicle dynamics model to simulate vehicle speed change, based on left and right motor torque (τ L (t),τ R (t)) at t time, obtain left and right wheel actual speed (v L (t),v R (t)) at t time; S4-3, Vehicle kinematics control: establish vehicle kinematics model to simulate vehicle pose change, based on actual vehicle speed (v L (t),v R (t)) of left and right wheels at t time, obtain vehicle pose (x W y) in world coordinate system at t time, x V (t),y V (t),α V (t)); Let S4-1 to S4-3 run for Δt time, and the vehicle pose (x V (t+Δt),y V (t+Δt),α V (t+Δt)) at time t+Δt is obtained, and then the execution is started again from S2.

2. The method for simulation test of orchard-oriented vehicle autonomous navigation algorithm according to claim 1, characterized in that, The S2 is specifically: S2-1, the user enters radar parameters, including the maximum measurement distance r of the LiDAR max , the scanning angle range θ range , the scanning angle resolution Δθ and the initial scanning angle θ0; the user enters the initial vehicle pose (x W (0), y V (0), α V (0)) in the world coordinate system xO V y; S2-2, according to the radar parameter, establish a LiDAR measurement model to calculate the number N of scanning lines and the scanning angle θ of each scanning line i : θ i = θ0+ Δθ(i-1) Wherein: N represents the total number of scanning lines, and i represents the number of scanning lines; Let the starting position of the LiDAR scan line and the vehicle mass center O V coincide W The coordinates of the i-th scan line at time t in the world coordinate system xO L y are x L : where: r is the polar radius of the scan line; (x V (t),y V (t),α V (t)) represents the vehicle pose in the world coordinate system xO W y at time t. For each scanning line, let r start from 0 and increase by an interval Δr, and perform scanning; S2-3, in the orchard map, when the scan line coordinate (x L ,y L ) is a black pixel, it is considered that the scan line touches the trunk, and the scan line polar radius at this time is recorded as the measured value r i * , and the following formula is used to obtain the measured distance r i returned by the i-th scan line: r i (t) = r i * + ε L where: ε L is the LiDAR random measurement error; If the ith scan line does not find a trunk point within the orchard map range or the maximum measurement distance range, let r i (t) = r max ; Finally, the LiDAR measurement point polar coordinates under the vehicle coordinate system xOy at time t are obtained V ​ 3. The method for simulation test of orchard-oriented vehicle autonomous navigation algorithm according to claim 2, characterized in that, The interval Δr is less than or equal to the size of a single pixel of the fruit tree map.

4. The method for simulation test of orchard-oriented vehicle autonomous navigation algorithm according to claim 1, characterized in that, In S4-1, motor control is specifically: S4-1a, calculating the left and right motor speed error (e and the actual vehicle speed (v L (t), v R (t)) at the time t, and calculating the left and right motor speed error (e L (t), e R (t)): S4-1b, the PID parameter tuning method is adopted to calculate the left and right motor control voltages (u L (t),u R (t)) wherein: Kp, Ki, and Kd are PID parameters preset by the PID parameter tuning method; P Kp, Ki, and Kd I are PID parameters preset by the PID parameter tuning method; D Kp, Ki, and Kd respectively represent the differential of e L (t) and e R (t); S4-1c, the linear saturation method is used to limit the left and right motor control voltage amplitude in [-u max , u max ], that is, when |u L (t)|>|u max | or |u R (t)|>|u max |, |u L (t)|=|u max |, |u R (t)|=|u max |, and the voltage direction is unchanged; S4-1d, left and right motor control voltages (u L (t), u R (t)) as input variables, left and right motor speeds (n L (t), n R (t)) as state variables, and left and right motor torques (τ L (t), τ R (t)) according to the following motor model: wherein: k e , k c , J a and R a are motor back electromotive force coefficient, motor torque coefficient, motor moment of inertia and motor armature resistance, respectively; denote the differential of n R (t), n L (t), respectively.

5. The method for simulation test of orchard-oriented vehicle autonomous navigation algorithm according to claim 4, characterized in that, In the S4-2, the vehicle dynamics control is specifically: based on the left and right motor torques (τ L (t),τ R (t)) at time t, the actual left and right wheel speeds (v L (t),v R (t)) at time t are obtained using the following vehicle dynamics model: where: D represents the vehicle wheel base, d represents the distance between the vehicle mass center and the geometric center of the two driving wheels, m represents the vehicle mass, J represents the vehicle moment of inertia, r V represents the wheel radius, respectively represent the differential of v L (t), v R (t).

6. The method for simulation test of orchard-oriented vehicle autonomous navigation algorithm according to claim 5, characterized in that, In the S4-3, the vehicle kinematics control is specifically: based on the actual vehicle speeds (v L (t),v R (t)) of the left and right wheels at the time t, a vehicle kinematics model is used to obtain the vehicle pose (x W y) in the world coordinate system at the time t, that is, the vehicle pose (x V (t),y V (t),α V (t)) in the world coordinate system at the time t: wherein: else x V (t), y V (t), a V derivative of (t).

7. A simulation test platform for orchard vehicle autonomous navigation algorithm based on the simulation test method for orchard vehicle autonomous navigation algorithm according to any one of claims 1-6, characterized in that, The platform comprises: An orchard map configuration unit: generating a tree trunk center position and a tree trunk diameter according to user-entered orchard planting parameters, establishing an orchard planting model based on the tree trunk information to simulate an orchard environment, and outputting an orchard map M; Radar configuration and measurement unit: according to the radar parameters entered by the user, a LiDAR measurement model is established, which can obtain the polar coordinates of the LiDAR measurement points under the vehicle coordinate system xO W y according to the vehicle pose under the world coordinate system xO V y at time t. Path generation and tracking unit: according to the polar coordinates of the LiDAR measurement points at time t, first adopt the navigation algorithm to be tested to calculate the navigation path, output the navigation parameters at time t Then use the navigation algorithm to be tested for path tracking to guide the vehicle to drive according to the navigation path, output the left and right wheel expected vehicle speed at time t A simulation execution unit: configuring vehicle running parameters according to the configured orchard map, LiDAR measurement model, navigation parameters, and expected vehicle speed of the left and right wheels, performing simulation, and obtaining a simulation result, i.e., a vehicle running trajectory.

Images