CN111309030B - Unmanned motion control simulation system and simulation method for tractor - Google Patents
Unmanned motion control simulation system and simulation method for tractor Download PDFInfo
- Publication number
- CN111309030B CN111309030B CN202010224577.6A CN202010224577A CN111309030B CN 111309030 B CN111309030 B CN 111309030B CN 202010224577 A CN202010224577 A CN 202010224577A CN 111309030 B CN111309030 B CN 111309030B
- Authority
- CN
- China
- Prior art keywords
- module
- path
- vehicle
- tractor
- ang
- 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.)
- Active
Links
- 238000004088 simulation Methods 0.000 title claims abstract description 37
- 238000000034 method Methods 0.000 title claims abstract description 34
- 230000000694 effects Effects 0.000 claims abstract description 10
- 230000003993 interaction Effects 0.000 claims description 14
- 230000008569 process Effects 0.000 claims description 7
- 230000008859 change Effects 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 230000007246 mechanism Effects 0.000 claims description 3
- 238000005316 response function Methods 0.000 claims description 3
- 238000010008 shearing Methods 0.000 claims description 3
- 230000004927 fusion Effects 0.000 claims description 2
- 238000012795 verification Methods 0.000 description 4
- 230000007547 defect Effects 0.000 description 3
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
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/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/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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Steering Control In Accordance With Driving Conditions (AREA)
Abstract
The invention discloses a tractor unmanned motion control simulation system and a simulation method thereof, wherein the simulation system comprises a user interface, a path planning module, a vehicle state issuing module, an executing module, a path tracking module and a log recording module; the user interface is connected with the path planning module, the path tracking module is respectively connected with the path planning module, the execution module and the vehicle state release module, the vehicle state release module is respectively connected with the path tracking module, the user interface and the log recording module, the execution module is respectively connected with the path tracking module and the log recording module, one end of the log recording module is connected with the vehicle state release module, and the other end of the log recording module is connected with the execution module; the invention can efficiently verify the effect of unmanned motion control of the tractor with low cost.
Description
Technical Field
The invention relates to a simulation system and a simulation method thereof, in particular to a tractor unmanned motion control simulation system and a simulation method thereof.
Background
At present, the operation mode of the tractor is mainly operated by manpower, and the operation mode has obvious defects in a scene of large-scale cultivation, such as: the operation intensity is high, the labor cost is high, the operation precision is difficult to guarantee, so that the introduction of an unmanned technique in a cultivation scene of a tractor is urgent, but the effectiveness and the optimality of an unmanned tractor algorithm are verified mainly by installing an unmanned system on a vehicle-mounted computer at present, the verification motion control effect is achieved by installing the system on the vehicle-mounted computer, the tractor is required to be driven from a garage to a test site, test conditions are set, repeated parking modification parameters are required to be carried out, the tractor is required to be driven back to the garage, the whole process is at least required to be 4 hours, and therefore, the efficiency of the unmanned tractor motion control algorithm is low, the cost is high, and the optimal result cannot be determined.
The unmanned tractor is different from the unmanned tractor of a vehicle running on a structured road, the working environment of the unmanned tractor needs to run on an unstructured road and run at a low speed, a Pure pursuit algorithm is frequently used in the aspect of low-speed vehicle path tracking, the Pure pursuit algorithm only ensures that the heading of the vehicle can reach a specified position and cannot ensure that the heading of the vehicle is consistent with a preset value when the vehicle reaches the specified position, and the tractor needs to reach the specified position in the cultivation process and ensure that the heading of the vehicle is consistent with the cultivation direction, so the Pure pursuit algorithm is limited when the unmanned tractor is applied to unmanned tractor motion control.
Disclosure of Invention
The invention aims to provide a tractor unmanned motion control simulation system and a simulation method thereof, which are used for verifying the effect of tractor unmanned motion control with high efficiency and low cost.
In order to achieve the above purpose, the present invention provides the following technical solutions:
The unmanned motion control simulation system of the tractor comprises a user interface, a path planning module, a vehicle state issuing module, an executing module, a path tracking module and a log recording module; the user interface is connected with the path planning module, the path tracking module is respectively connected with the path planning module, the execution module and the vehicle state release module, the vehicle state release module is respectively connected with the path tracking module, the user interface and the log recording module, the execution module is respectively connected with the path tracking module and the log recording module, one end of the log recording module is connected with the vehicle state release module, and the other end of the log recording module is connected with the execution module.
The record content of the log record module comprises real-time gestures of the vehicle, path planning content, motion control output commands such as steering angle, vehicle speed and control parameters.
The user interface comprises an interaction module and a display module, wherein the interaction module is connected with a path planning module, one end of the path planning module is connected with the interaction module, the other end of the path planning module is connected with a path tracking module and the display module, and the display module of the user interface is connected with a vehicle state issuing module.
The display module comprises a model file, a path display node, a vehicle state real-time display node and an operation environment display node, wherein the path display node is connected with the path planning module, one end of the vehicle state real-time display node is connected with the vehicle state release module, and the other end of the vehicle state real-time display node is connected with the model file.
The simulation method for unmanned motion control of the tractor comprises the following specific steps:
Step1, a user inputs a motion control method, physical parameters of a tractor, a farmland boundary and a cultivation mode into a system through an interaction module in a user interface;
step2, the system loads a model, plans a path, displays the path, tracks the path and records data according to user settings;
step 2.1, loading the generated model file and each functional module by the system, and displaying a model of the tractor in the Gazebo D dynamic simulator;
Step 2.1.1, forming a matching hole which is perpendicular to a rotating shaft in a vehicle body part through a stretching and shearing command in SolidWorks software;
Step 2.1.2, generating each part of the processed tractor three-dimensional model through a file export command in SolidWorks software, wherein the file comprises a file in stl format corresponding to a left wheel, a right wheel, a rotating shaft and a vehicle body;
2.1.3, configuring a file in urdf format corresponding to a tractor, wherein the coordinate center of the vehicle body is set as a rear axle center, the tire center of front wheels, the tire center of rear wheels and the steering axle center of front wheels are set as cylinder centers, the vehicle body is simplified into a cuboid, and four wheels are simplified into cylinders;
Step 2.2, a path planning module plans an optimal path according to the terrain and the cultivation mode input by a user, and the optimal path is issued through the path planning module;
2.3, subscribing the contents of path planning by the path display node, establishing a coordinate relation between the data point and the Odometry mile meter, and displaying the planned path in an interface by the system according to the set data point and a line display mode, wherein the line display mode comprises the size, the color and the thickness of a line;
step 2.4, the vehicle state issuing module obtains the model gesture through the model service function and issues the model gesture according to the appointed frequency;
step 2.5, a vehicle state real-time display module converts the vehicle coordinates and Odometry coordinates, and then the converted vehicle body posture is broadcasted by utilizing Odometry, and the running state of the tractor is displayed in real time in the display module;
step 2.6, the system executes a path tracking module part and outputs the turning angle of the steering wheel;
Step 2.7, after receiving the turning angle of the steering wheel, the executing module converts the turning angle into turning angles on the left side and the right side of the front wheel of the tire through the Ackerman structural characteristics, and further cultivation of the vehicle along the set position and the set course is realized;
Step 2.7.1, converting the obtained rotation angle Ang_turn into a rotation angle Ang_l at the left side of the front wheel and a rotation angle Ang_r at the right side of the front wheel according to the following formulas (1) and (2),
Formula (1)
Formula (2)
Description of the formula: wherein d is the wheel base, and l is the wheel base;
Step 2.7.2, converting the vehicle speed v into the speed v_l of the front wheels and the speed v_r of the rear wheels according to the proportional relation between the left-side angle Ang_l of the front wheels and the right-side angle Ang_r of the front wheels and the angle Ang_turn;
Step 2.7.3, deducing the relation between the turning radius of the left side and the right side of the rear wheel and the turning radius of the whole vehicle according to the Ackerman steering mechanism principle, and further solving the left speed v_hl of the rear wheel and the right speed v_hr of the rear wheel;
Step 2.8, recording real-time state of the vehicle, planning path data and path tracking process data by a recording module according to the requirement;
and step 3, the user judges whether the control method or parameters need to be changed according to the display effect and the recorded data, if so, the step1 is executed, and if not, the step is ended.
Aiming at the path tracking module in the step 2.6), the invention uses an algorithm for fusing a course error control algorithm and Pure pursuit, and the specific steps are as follows:
Step 2.6.1, judging whether the current vehicle position enters a response function and whether a planned path is acquired, if so, satisfying a path tracking starting condition, executing step 2.6.2, otherwise, executing step 2.6.1;
step 2.6.2, judging whether the current speed of the vehicle is negative, if so, adjusting the heading of the vehicle, if not, not adjusting, and searching a nearest distance point in a range of a route planning sequence point which can be seen forward by the current position of the vehicle, thereby determining a target point;
Step 2.6.3, calculating a transverse error and a distance error according to the posture of the current vehicle, the coordinates and the heading of a target point, then calculating a rotation angle Ang p_s required by the wheel when controlling the distance error by utilizing a Pure pursuit algorithm, and calculating a rotation angle Ang heading required by the wheel by utilizing a heading error control algorithm;
Step 2.6.4, judging whether the distance is larger than a distance threshold, if so, performing distance error control by utilizing Pure pursuit to enable the vehicle to advance towards a target point, otherwise, calculating a corresponding distance index Ps and a corresponding speed index Pv according to the line type of path tracking, calculating a weight w heading for controlling the course error through the following formula (3), calculating a weight w p_s for controlling the distance error through the formula (4), and then calculating a current angle Ang_current through the formula (5);
formula (3)
Formula (4)
Formula (5)
2.6.5, Aiming at the non-abrupt characteristic of steering, taking the average value of the last angle Ang_last and the current angle Ang_current to obtain an output angle Ang_out, and then calculating a wheel angle Ang_turn according to the wheel angle limit and the steering rate limit;
step 2.6.6, judging whether the current running direction of the vehicle is forward or reverse, if the current running direction is reverse, taking the negative value of Ang_out, and if the current running direction is not reverse, keeping unchanged;
And 2.6.7, judging whether a new path task is issued, if so, emptying the path data cache, executing the new task, otherwise, judging whether the new path task reaches the end point, if so, sending a stop command, otherwise, calculating the target point, and then executing the step 2.6.2.
Description of equation (3) in step 2.6.4:
In order to reach the target point quickly, since the operation speed of the tractor is low, the time left for the course error control is enough, so the course error control is unnecessary to intervene in the whole course, a distance threshold value, usually the length of the vehicle, can be set first, when the distance between the tractor and the target point is larger than the threshold value, only the distance error is considered, namely, the vehicle advances towards the direction of the target point at the moment, the target point can be reached quickly, when the distance between the tractor and the target point is smaller than the threshold value, the course error intervenes at the moment, the higher the specific gravity of the course error control is, the higher the weight is, the degree of the course error along with the distance can be realized by adjusting the magnitude of the power, and the magnitude of the power can be adjusted according to the road conditions such as the difference of straight running and the curve running. In the same distance case, the speed is different, so the time left for course error adjustment is also different, and therefore the weight of the speed needs to be introduced into the formula.
Description of equation (4) and equation (5) in step 2.6.4:
Since only one input value is available at the same time, either distance error control or heading error control is performed, or alternatively steering abrupt change may occur, and since heading error control and distance error control may be opposite, it is necessary to consider both distance error control and heading error control at the same time, mainly by giving different weights to distance error control and heading error control.
Compared with the prior art, the invention has the beneficial effects that:
1) Aiming at the defects of low efficiency, high cost and poor optimal effect of the unmanned algorithm for real vehicle verification, the invention realizes the effectiveness of the rapid verification algorithm by developing a set of unmanned motion control simulation system, avoids the interference caused by the real vehicle state, realizes the adjustment of single parameter conditions and realizes the optimal parameter adjustment in the shortest time.
2) The invention fuses the course error control algorithm and Pure pursuit so as to realize that the method can be applied to the unmanned system of the tractor with the Ackerman structure in a low-speed scene, and expands the applicability of the Pure pursuit algorithm.
Drawings
FIG. 1 is a block diagram of a tractor motion control simulation system of the present invention;
Fig. 2 is a flow chart of the tractor path tracking principle of the present invention.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
According to fig. 1, the present invention provides a technical solution:
The unmanned motion control simulation system of the tractor comprises a user interface, a path planning module, a vehicle state issuing module, an executing module, a path tracking module and a log recording module; the user interface is connected with the path planning module, the path tracking module is respectively connected with the path planning module, the execution module and the vehicle state release module, the path planning module transmits the planned path data to the path tracking module, and the path tracking module enables the vehicle to cultivate according to the content of path planning through the execution module and ensures cultivation speed and operation precision; the vehicle state publishing module is respectively connected with the path tracking module, the user interface and the log recording module, and publishes the current real-time position of the vehicle to the path tracking module and the display module so as to be convenient for the path tracking module and the display module to subscribe for use; the execution module is respectively connected with the path tracking module and the log recording module and provides an execution part required by the ackerman structure of the tractor so that the system can perform motion control simulation; the user interface can visually check the real vehicle state of the vehicle and the error between the vehicle and the planned path so as to facilitate simulation debugging, one end of the log recording module is connected with the vehicle state issuing module, the other end of the log recording module is connected with the execution module, the real-time vehicle state data output by the vehicle state issuing module and the vehicle execution command data output by the execution module are transmitted to the log recording module, and the log recording module mainly plays a role in facilitating positioning and analysis of problems and counting the accuracy of motion control, and the recorded content comprises the real-time gesture of the vehicle, the path planning content, the motion control output commands such as steering angle, vehicle speed and control parameters.
The user interface comprises an interaction module and a display module, wherein the interaction module is connected with the path planning module, and provides a user interaction part, and a user is required to input a motion control method, physical parameters of the tractor, farmland boundaries and cultivation modes; one end of the path planning module is connected with the interaction module, the other end of the path planning module is connected with the path tracking module and the display module, the path planning module receives the boundary of the cultivation land, the vehicle parameters and the cultivation mode information transmitted by the interaction module, and a fully covered cultivation path is generated through a path algorithm input by a user and is issued to the path tracking module and the display module; the display module of the user interface is connected with the vehicle state release module, and the display module can visually check the real vehicle state of the vehicle and the error between the vehicle and the planned path so as to facilitate simulation debugging.
The display module comprises a model file, a path display node, a vehicle state real-time display node and an operation environment display node, wherein the path display node is connected with the path planning module, the path display node receives path data sent by the path planning module and displays the path data in an interface, one end of the vehicle state real-time display node is connected with the vehicle state release module, the other end of the vehicle state real-time display node is connected with the model file, the vehicle state real-time display node receives the path data sent by the vehicle state release module, and the model file receives the vehicle state data transmitted by the vehicle state real-time display node and displays the vehicle state in the interface in real time.
According to fig. 2, the specific working process of the path tracking module in the invention is implemented: finding out a target point which meets the condition in a path planning sequence point in a path planning module according to the current posture of the vehicle, wherein the target point which meets the condition is a data point closest to the vehicle in the running direction of the vehicle, calculating a heading error through the current heading of the vehicle and the heading of the target point, calculating a distance error through the position coordinates of the vehicle and the coordinates of the target point, controlling the heading error through a heading error control algorithm, controlling the distance error through a Pure pursuit algorithm, outputting an Ang_current through a fusion algorithm, taking the average of the last corner Ang_last and the current corner Ang_current to obtain an output angle Ang_out, and calculating a wheel corner Ang_turn according to a wheel corner limit and a steering rate limit.
The invention can be applied to the unmanned system of the tractor with the Ackerman structure in a low-speed scene by fusing the course error control algorithm and the Pure pursuit algorithm.
The simulation method for unmanned motion control of the tractor comprises the following specific steps:
Step1, a user inputs a motion control method, physical parameters of a tractor, a farmland boundary and a cultivation mode into a system through an interaction module in a user interface;
step2, the system loads a model, plans a path, displays the path, tracks the path and records data according to user settings;
step 2.1, loading the generated model file and each functional module by the system, and displaying a model of the tractor in the Gazebo D dynamic simulator;
step 2.1.1, a matched hole which is formed in a vehicle body part and is perpendicular to a rotating shaft through a stretching and shearing command in SolidWorks software, so as to eliminate the influence of non-perpendicular steering shaft caused by camber angle and caster angle of a steering wheel;
Step 2.1.2, generating each part of the processed tractor three-dimensional model through a file export command in SolidWorks software, wherein the file comprises a file in stl format corresponding to a left wheel, a right wheel, a rotating shaft and a vehicle body;
2.1.3, configuring a file in urdf format corresponding to a tractor, wherein the coordinate center of a vehicle body is set as a rear axle center, the tire center of front wheels, the tire center of rear wheels and the steering axle center of front wheels are set as cylinder centers, the vehicle body is simplified into a cuboid, and four wheels are simplified into cylinders, so that the effect of reducing the operation load of the system is achieved;
Step 2.2, a path planning module plans an optimal path according to the terrain and the cultivation mode input by a user, and the optimal path is issued through the path planning module;
2.3, subscribing the contents of path planning by the path display node, establishing a coordinate relation between the data point and the Odometry mile meter, and displaying the planned path in an interface by the system according to the set data point and a line display mode, wherein the line display mode comprises the size, the color and the thickness of a line;
step 2.4, the vehicle state issuing module obtains the model gesture through the model service function and issues the model gesture according to the appointed frequency;
step 2.5, a vehicle state real-time display module converts the vehicle coordinates and Odometry coordinates, and then the converted vehicle body posture is broadcasted by utilizing Odometry, and the running state of the tractor is displayed in real time in the display module;
step 2.6, the system executes a path tracking module part and outputs the turning angle of the steering wheel;
Step 2.7, after receiving the turning angle of the steering wheel, the executing module converts the turning angle into turning angles on the left side and the right side of the front wheel of the tire through the Ackerman structural characteristics, and further cultivation of the vehicle along the set position and the set course is realized;
Step 2.7.1, converting the obtained rotation angle Ang_turn into a rotation angle Ang_l at the left side of the front wheel and a rotation angle Ang_r at the right side of the front wheel according to the following formulas (1) and (2),
Formula (1)
Formula (2)
Description of the formula: wherein d is the wheel base, and l is the wheel base;
Step 2.7.2, converting the vehicle speed v into the speed v_l of the front wheels and the speed v_r of the rear wheels according to the proportional relation between the left-side angle Ang_l of the front wheels and the right-side angle Ang_r of the front wheels and the angle Ang_turn;
Step 2.7.3, deducing the relation between the turning radius of the left side and the right side of the rear wheel and the turning radius of the whole vehicle according to the Ackerman steering mechanism principle, and further solving the left speed v_hl of the rear wheel and the right speed v_hr of the rear wheel;
Step 2.8, recording real-time state of the vehicle, planning path data and path tracking process data by a recording module according to the requirement;
and step 3, the user judges whether the control method or parameters need to be changed according to the display effect and the recorded data, if so, the step1 is executed, and if not, the step is ended.
Aiming at the path tracking module in the step 2.6), the invention uses an algorithm for fusing a course error control algorithm and Pure pursuit, and the specific steps are as follows:
Step 2.6.1, judging whether the current vehicle position enters a response function and whether a planned path is acquired, if so, satisfying a path tracking starting condition, executing step 2.6.2, otherwise, executing step 2.6.1;
step 2.6.2, judging whether the current speed of the vehicle is negative, if so, adjusting the heading of the vehicle, if not, not adjusting, and searching a nearest distance point in a range of a route planning sequence point which can be seen forward by the current position of the vehicle, thereby determining a target point;
Step 2.6.3, calculating a transverse error and a distance error according to the posture of the current vehicle, the coordinates and the heading of a target point, then calculating a rotation angle Ang p_s required by the wheel when controlling the distance error by utilizing a Pure pursuit algorithm, and calculating a rotation angle Ang heading required by the wheel by utilizing a heading error control algorithm;
Step 2.6.4, judging whether the distance is larger than a distance threshold, if so, performing distance error control by utilizing Pure pursuit to enable the vehicle to advance towards a target point, otherwise, calculating a corresponding distance index Ps and a corresponding speed index Pv according to the line type of path tracking, calculating a weight w heading for controlling the course error through the following formula (3), calculating a weight w p_s for controlling the distance error through the formula (4), and then calculating a current angle Ang_current through the formula (5);
formula (3)
Formula (4)
Formula (5)
2.6.5, Aiming at the non-abrupt characteristic of steering, taking the average value of the last angle Ang_last and the current angle Ang_current to obtain an output angle Ang_out, and then calculating a wheel angle Ang_turn according to the wheel angle limit and the steering rate limit;
step 2.6.6, judging whether the current running direction of the vehicle is forward or reverse, if the current running direction is reverse, taking the negative value of Ang_out, and if the current running direction is not reverse, keeping unchanged;
And 2.6.7, judging whether a new path task is issued, if so, emptying the path data cache, executing the new task, otherwise, judging whether the new path task reaches the end point, if so, sending a stop command, otherwise, calculating the target point, and then executing the step 2.6.2.
Description of equation (3) in step 2.6.4:
In order to reach the target point quickly, since the operation speed of the tractor is low, the time left for the course error control is enough, so the course error control is unnecessary to intervene in the whole course, a distance threshold value, usually the length of the vehicle, can be set first, when the distance between the tractor and the target point is larger than the threshold value, only the distance error is considered, namely, the vehicle advances towards the direction of the target point at the moment, the target point can be reached quickly, when the distance between the tractor and the target point is smaller than the threshold value, the course error intervenes at the moment, the higher the specific gravity of the course error control is, the higher the weight is, the degree of the course error along with the distance can be realized by adjusting the magnitude of the power, and the magnitude of the power can be adjusted according to the road conditions such as the difference of straight running and the curve running. In the same distance case, the speed is different, so the time left for course error adjustment is also different, and therefore the weight of the speed needs to be introduced into the formula.
Description of equation (4) and equation (5) in step 2.6.4:
Since only one input value is available at the same time, either distance error control or heading error control is performed, or alternatively steering abrupt change may occur, and since heading error control and distance error control may be opposite, it is necessary to consider both distance error control and heading error control at the same time, mainly by giving different weights to distance error control and heading error control.
The whole operation process needs about 4 minutes to complete the simulation of the motion control and check the effect of the motion control, the simulation efficiency is improved by more than 90 percent, and the cost is reduced by 99 percent because the simulation test only needs to run on the existing PC computer without extra expenditure.
The tractor unmanned motion control simulation system and the simulation method thereof aim at the defects of low efficiency, high cost and poor optimal effect of the real vehicle unmanned motion verification algorithm. The course error control algorithm and Pure pursuit are fused, so that the method can be applied to a tractor unmanned system with an Ackerman structure in a low-speed scene, and the applicability of the Pure pursuit algorithm is expanded.
Although embodiments of the present invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made therein without departing from the principles and spirit of the invention, the scope of which is defined in the appended claims and their equivalents.
Claims (9)
1. A simulation method for unmanned motion control of a tractor is characterized by comprising the following steps of: the method comprises the following specific steps:
Step1, a user inputs a motion control method, physical parameters of a tractor, a farmland boundary and a cultivation mode into a system through an interaction module in a user interface;
step 2, the system loads a model, plans a path, displays the path, tracks the path and records data according to user settings; the method comprises the following specific steps:
step 2.1, loading the generated model file and each functional module by the system, and displaying a model of the tractor in the Gazebo D dynamic simulator;
Step 2.2, a path planning module plans an optimal path according to the terrain and the cultivation mode input by a user, and the optimal path is issued through the path planning module;
2.3, subscribing the contents of path planning by the path display node, establishing a coordinate relation between the data point and the Odometry mile meter, and displaying the planned path in an interface by the system according to the set data point and a line display mode, wherein the line display mode comprises the size, the color and the thickness of a line;
step 2.4, the vehicle state issuing module obtains the model gesture through the model service function and issues the model gesture according to the appointed frequency;
step 2.5, a vehicle state real-time display module converts the vehicle coordinates and Odometry coordinates, and then the converted vehicle body posture is broadcasted by utilizing Odometry, and the running state of the tractor is displayed in real time in the display module;
step 2.6, the system executes a path tracking module part and outputs the turning angle of the steering wheel;
Step 2.7, after receiving the turning angle of the steering wheel, the executing module converts the turning angle into turning angles on the left side and the right side of the front wheel of the tire through the Ackerman structural characteristics, and further cultivation of the vehicle along the set position and the set course is realized;
Step 2.8, recording real-time state of the vehicle, planning path data and path tracking process data by a recording module according to the requirement;
and step 3, the user judges whether the control method or parameters need to be changed according to the display effect and the recorded data, if so, the step1 is executed, and if not, the step is ended.
2. The simulation method of unmanned motion control of a tractor according to claim 1, wherein: the specific steps of the step 2.1 are as follows:
Step 2.1.1, forming a matching hole which is perpendicular to a rotating shaft in a vehicle body part through a stretching and shearing command in SolidWorks software;
Step 2.1.2, generating each part of the processed tractor three-dimensional model through a file export command in SolidWorks software, wherein the file comprises a file in stl format corresponding to a left wheel, a right wheel, a rotating shaft and a vehicle body;
And 2.1.3, configuring a file in urdf format corresponding to the tractor, wherein the coordinate center of the vehicle body is set as a rear axle center, the tire center of the front and rear wheels and the steering axle center of the front wheel are set as cylinder centers, the vehicle body is simplified into a cuboid, and four wheels are simplified into cylinders.
3. The simulation method of unmanned motion control of a tractor according to claim 1, wherein: the course error control algorithm and Pure pursuit fusion algorithm are used by the path tracking module in the step 2.6), and the specific steps are as follows:
Step 2.6.1, judging whether the current vehicle position enters a response function and whether a planned path is acquired, if so, satisfying a path tracking starting condition, executing step 2.6.2, otherwise, executing step 2.6.1;
step 2.6.2, judging whether the current speed of the vehicle is negative, if so, adjusting the heading of the vehicle, if not, not adjusting, and searching a nearest distance point in a range of a route planning sequence point which can be seen forward by the current position of the vehicle, thereby determining a target point;
Step 2.6.3, calculating a transverse error and a distance error according to the posture of the current vehicle, the coordinates and the heading of a target point, then calculating a rotation angle Ang p_s required by the wheel when controlling the distance error by utilizing a Pure pursuit algorithm, and calculating a rotation angle Ang heading required by the wheel by utilizing a heading error control algorithm;
Step 2.6.4, judging whether the distance is larger than a distance threshold, if so, performing distance error control by utilizing Pure pursuit to enable the vehicle to advance towards a target point, otherwise, calculating a corresponding distance index Ps and a corresponding speed index Pv according to the line type of path tracking, calculating a weight w heading for controlling the course error through the following formula (3), calculating a weight w p_s for controlling the distance error through the formula (4), and then calculating a current angle Ang_current through the formula (5);
wp_s=1-wheading formula (4)
Ang_current=w p_s×Angp_s+wheading×Angheading formula (5)
2.6.5, Aiming at the non-abrupt characteristic of steering, taking the average value of the last angle Ang_last and the current angle Ang_current to obtain an output angle Ang_out, and then calculating a wheel angle Ang_turn according to the wheel angle limit and the steering rate limit;
step 2.6.6, judging whether the current running direction of the vehicle is forward or reverse, if the current running direction is reverse, taking the negative value of Ang_out, and if the current running direction is not reverse, keeping unchanged;
And 2.6.7, judging whether a new path task is issued, if so, emptying the path data cache, executing the new task, otherwise, judging whether the new path task reaches the end point, if so, sending a stop command, otherwise, calculating the target point, and then executing the step 2.6.2.
4. The simulation method of unmanned motion control of a tractor according to claim 1, wherein: the specific steps of step 2.7 are as follows:
Step 2.7.1, converting the obtained rotation angle Ang_turn into a rotation angle Ang_l at the left side of the front wheel and a rotation angle Ang_r at the right side of the front wheel according to the following formulas (1) and (2),
Cot (ang_l) =cot (ang_turn) +d2l equation (1)
Cot (ang_r) =cot (ang_turn) -d2l formula (2)
Description of the formula: wherein d is the wheel base, and l is the wheel base;
Step 2.7.2, converting the vehicle speed v into the speed v_l of the front wheels and the speed v_r of the rear wheels according to the proportional relation between the left-side angle Ang_l of the front wheels and the right-side angle Ang_r of the front wheels and the angle Ang_turn;
And 2.7.3, deducing the relation between the turning radius of the left side and the right side of the rear wheel and the turning radius of the whole vehicle according to the Ackerman steering mechanism principle, and further obtaining the left speed v_hl of the rear wheel and the right speed v_hr of the rear wheel.
5. A simulation method of unmanned motion control of a tractor according to claim 3, wherein: the formula (3) in the step 2.6.4 firstly sets a distance threshold, only considers the distance error when the distance between the tractor and the target point is larger than the threshold, intervenes the heading error when the distance between the tractor and the target point is smaller than the threshold, and the more the distance is, the greater the specific gravity of the heading error control is, the weight of the heading error control realizes the degree of the change of the heading error along with the distance by adjusting the magnitude of the power, the magnitude of the power is adjusted according to the different road conditions, and under the same distance condition, the speed is different, and the adjustment time of the heading error is also different;
regarding the formula (4) and the formula (5) in the step 2.6.4, the distance error control and the heading error control are considered at the same time, and are mainly implemented by giving weights different from those given to the distance error control and the heading error control.
6. The simulation system of the simulation method of unmanned motion control of a tractor according to claim 1, wherein: the system comprises a user interface, a path planning module, a vehicle state issuing module, an executing module, a path tracking module and a log recording module; the user interface is connected with the path planning module, the path tracking module is respectively connected with the path planning module, the execution module and the vehicle state release module, the vehicle state release module is respectively connected with the path tracking module, the user interface and the log recording module, the execution module is respectively connected with the path tracking module and the log recording module, one end of the log recording module is connected with the vehicle state release module, and the other end of the log recording module is connected with the execution module.
7. The unmanned motion control simulation system of a tractor of claim 6, wherein: the recording content of the log recording module comprises real-time gestures of the vehicle, path planning content, motion control output commands and control parameters.
8. The unmanned motion control simulation system of a tractor of claim 6, wherein: the user interface comprises an interaction module and a display module, wherein the interaction module is connected with a path planning module, one end of the path planning module is connected with the interaction module, the other end of the path planning module is connected with a path tracking module and the display module, and the display module of the user interface is connected with a vehicle state issuing module.
9. The tractor unmanned motion control simulation system of claim 8, wherein: the display module comprises a model file, a path display node, a vehicle state real-time display node and an operation environment display node, wherein the path display node is connected with the path planning module, one end of the vehicle state real-time display node is connected with the vehicle state release module, and the other end of the vehicle state real-time display node is connected with the model file.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010224577.6A CN111309030B (en) | 2020-03-26 | 2020-03-26 | Unmanned motion control simulation system and simulation method for tractor |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010224577.6A CN111309030B (en) | 2020-03-26 | 2020-03-26 | Unmanned motion control simulation system and simulation method for tractor |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111309030A CN111309030A (en) | 2020-06-19 |
| CN111309030B true CN111309030B (en) | 2024-05-24 |
Family
ID=71145967
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010224577.6A Active CN111309030B (en) | 2020-03-26 | 2020-03-26 | Unmanned motion control simulation system and simulation method for tractor |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111309030B (en) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104898687A (en) * | 2014-08-21 | 2015-09-09 | 宁波市鄞州麦谷农业科技有限公司 | Automatic driving system of crawler type combined harvester and control method |
| CN107479558A (en) * | 2017-09-22 | 2017-12-15 | 中国人民解放军63983部队 | Vehicle field paths planning method based on vehicle movement model |
| CN107992049A (en) * | 2017-12-20 | 2018-05-04 | 上海交通大学 | Paddy planter modularization automatic Pilot control device and its control method |
| CN110830547A (en) * | 2019-09-23 | 2020-02-21 | 厦门大学 | Unmanned information storage method and device based on block chain storage certificate |
| CN211741925U (en) * | 2020-03-26 | 2020-10-23 | 洛阳智能农业装备研究院有限公司 | Tractor unmanned motion control simulation system |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20080195268A1 (en) * | 2007-02-09 | 2008-08-14 | Novariant, Inc. | Implement control system and method of using same |
-
2020
- 2020-03-26 CN CN202010224577.6A patent/CN111309030B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104898687A (en) * | 2014-08-21 | 2015-09-09 | 宁波市鄞州麦谷农业科技有限公司 | Automatic driving system of crawler type combined harvester and control method |
| CN107479558A (en) * | 2017-09-22 | 2017-12-15 | 中国人民解放军63983部队 | Vehicle field paths planning method based on vehicle movement model |
| CN107992049A (en) * | 2017-12-20 | 2018-05-04 | 上海交通大学 | Paddy planter modularization automatic Pilot control device and its control method |
| CN110830547A (en) * | 2019-09-23 | 2020-02-21 | 厦门大学 | Unmanned information storage method and device based on block chain storage certificate |
| CN211741925U (en) * | 2020-03-26 | 2020-10-23 | 洛阳智能农业装备研究院有限公司 | Tractor unmanned motion control simulation system |
Non-Patent Citations (1)
| Title |
|---|
| 拖拉机自动驾驶及控制技术;刘成强，等;《农业工程》;20190430;第第9卷卷(第第4期期);期刊第87-91页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111309030A (en) | 2020-06-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109263639B (en) | Driving path planning method based on state grid method | |
| CN108609014B (en) | Transverse control method and device of intelligent vehicle | |
| EP3489624B1 (en) | Automatic calibration method of angle sensor for automatic drive control system of farm machine | |
| CN109799828B (en) | Transverse and longitudinal coordination control method for automatic driving vehicle | |
| CN109823393A (en) | A kind of intelligent driving Vehicle tracing control method | |
| Zhao et al. | Dynamic motion planning for autonomous vehicle in unknown environments | |
| CN108508760B (en) | Joint simulation method and system for multi-wheel independently-driven slip steering vehicle | |
| JPWO2019065409A1 (en) | Map generation method for autonomous driving simulator and autonomous driving simulator | |
| CN103823929A (en) | Method for testing performance of steering system of vehicle on basis of driver model | |
| CN106950953A (en) | A kind of automatic parking path trace and vehicle control system and method | |
| CN110850905B (en) | Electronic differential control method and system based on road surface condition and pre-control | |
| CN115042816B (en) | Path tracking method, device, apparatus, medium, and program | |
| CN101118500A (en) | Software emulation method of self-determination driving vehicle running process | |
| CN114115274A (en) | Agricultural wheeled tractor path tracking output feedback control strategy | |
| CN113051765A (en) | Intelligent driving vehicle road on-loop test method based on virtual scene change | |
| CN112193318A (en) | Vehicle path control method, device, equipment and computer readable storage medium | |
| CN116069037A (en) | Unmanned vehicle formation track planning control method, device, equipment and storage medium | |
| CN211741925U (en) | Tractor unmanned motion control simulation system | |
| CN113050660B (en) | Error compensation method, error compensation device, computer equipment and storage medium | |
| CN111309030B (en) | Unmanned motion control simulation system and simulation method for tractor | |
| JP4648335B2 (en) | System and program for performing virtual test for vehicle running | |
| CN110375786A (en) | A kind of scaling method, mobile unit and storage medium that sensor is joined outside | |
| CN115511901B (en) | Mine map processing method, device and system, computer equipment and storage medium | |
| Miao et al. | Research on decoupling control for the longitudinal and lateral dynamics of a tractor considering steering delay | |
| CN113625597B (en) | Method and device for controlling simulated vehicle, electronic equipment and storage medium |
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 |