CN111309030A - Tractor unmanned motion control simulation system and simulation method thereof - Google Patents
Tractor unmanned motion control simulation system and simulation method thereof Download PDFInfo
- Publication number
- CN111309030A CN111309030A CN202010224577.6A CN202010224577A CN111309030A CN 111309030 A CN111309030 A CN 111309030A CN 202010224577 A CN202010224577 A CN 202010224577A CN 111309030 A CN111309030 A CN 111309030A
- Authority
- CN
- China
- Prior art keywords
- module
- path
- vehicle
- tractor
- distance
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 39
- 238000004088 simulation Methods 0.000 title claims abstract description 32
- 230000000694 effects Effects 0.000 claims abstract description 10
- 230000003993 interaction Effects 0.000 claims description 14
- 230000008569 process Effects 0.000 claims description 11
- 230000008859 change Effects 0.000 claims description 6
- 238000012935 Averaging Methods 0.000 claims description 4
- 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
- 238000003971 tillage Methods 0.000 claims description 3
- 238000012795 verification Methods 0.000 description 6
- 230000007547 defect Effects 0.000 description 3
- 238000012360 testing method Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 238000009313 farming Methods 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
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/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 simulation system and a simulation method for controlling unmanned movement of a tractor, which comprises a user interface, a path planning module, a vehicle state publishing module, an execution module, a path tracking module and a log recording module, wherein the user interface is used for displaying the path planning 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 publishing module, the vehicle state publishing 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 publishing module, and the other end of the log recording module is connected with the execution module; the invention can verify the effect of the unmanned motion control of the tractor with high efficiency and low cost.
Description
Technical Field
The invention relates to a simulation system and a simulation method thereof, in particular to a simulation system and a simulation method for controlling unmanned movement of a tractor.
Background
At present, the operation mode of the tractor is mainly operated by manpower, and the operation mode has obvious defects in a large-scale cultivation scene, such as: the operation intensity is high, the labor cost is high, and the operation precision is difficult to guarantee, so that the introduction of the unmanned technology into the cultivation scene of the tractor is urgent, but currently, aiming at the unmanned tractor algorithm, the effectiveness and the optimality of the unmanned tractor algorithm are mainly verified by installing an unmanned system on a vehicle-mounted computer in a real vehicle, the verification of the motion control effect requires that the tractor is driven from a garage to a test site, the test condition is set, the vehicle is repeatedly parked to modify parameters, then the tractor is driven back to the garage, and the whole process requires at least 4 hours, so that the efficiency of the real vehicle verification of the unmanned tractor motion control algorithm is low, the cost is high, and the optimal result cannot be determined.
The tractor is unmanned, different from the unmanned driving of a vehicle running on a structured road, the working environment of the tractor needs to run on an unstructured road and run at a low speed, and in the aspect of low-speed vehicle path tracking, a Purpursuit algorithm is frequently used, but the Purpursuit algorithm can only ensure that the vehicle can reach a specified position and cannot ensure that the heading of the vehicle is consistent with the preset heading when the vehicle reaches the specified position, and the tractor not only needs to reach the specified position in the farming process but also needs to ensure that the heading of the vehicle is consistent with the direction of farming, so the Pure pursuit algorithm is limited when being applied to the unmanned movement control of the tractor.
Disclosure of Invention
The invention aims to provide a simulation system and a simulation method for controlling the unmanned movement of a tractor, which can verify the effect of the unmanned movement control of the tractor with high efficiency and low cost.
In order to achieve the purpose, the invention provides the following technical scheme:
a tractor unmanned motion control simulation system comprises a user interface, a path planning module, a vehicle state issuing module, an execution 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 publishing module, the vehicle state publishing 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 publishing module, and the other end of the log recording module is connected with the execution module.
The log recording module records the real-time attitude of the vehicle, the path planning content, the motion control output command such as a steering angle, a vehicle speed and a control parameter.
The user interface comprises an interaction module and a display module, the interaction module is connected with the 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 the path tracking module and the display module, and the display module of the user interface is connected with the vehicle state publishing 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, 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 publishing module, and the other end of the vehicle state real-time display node is connected with the model file.
The simulation method for controlling the unmanned movement of the tractor comprises the following specific steps:
step 1, 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 the setting of a user;
step 2.1, the system loads the generated model file and each function module, and displays the model of the tractor in a Gazebo3D dynamic simulator;
step 2.1.1, forming a matching hole vertical to a rotating shaft on a vehicle body part through a stretching and shearing command in SolidWorks software;
2.1.2, generating parts of the processed three-dimensional model of the tractor through an export file command in SolidWorks software, wherein the parts comprise left and right wheels, a rotating shaft and a stl format file corresponding to the tractor body;
step 2.1.3, configuring files in a urdf format corresponding to the tractor, wherein a coordinate center of a tractor body is set as a center of a rear shaft, centers of tires of front and rear wheels and a steering shaft of the front wheel are set as centers of cylinders, the tractor body is simplified into a cuboid, and four wheels are simplified into cylinders;
2.2, the path planning module plans an optimal path according to the terrain and cultivation mode input by the user and issues the optimal path through the path planning module;
step 2.3, the path display node subscribes the content of path planning, further establishes the coordinate relationship between the data point and the odometer, and displays the planned path in an interface 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 acquires the model attitude through the model service function and issues the model attitude according to the specified frequency;
2.5, converting the vehicle coordinates and the odometer coordinates by a vehicle state real-time display module, broadcasting the converted vehicle body posture by using the odometer, and displaying the running condition of the tractor in real time in the display module;
step 2.6, the system executes the path tracking module part and outputs the rotation angle of the steering wheel;
step 2.7, the execution module receives the rotation angle of the steering wheel and converts the rotation angle into rotation angles on the left side and the right side of a front wheel of the tire through Ackerman structural characteristics so as to realize vehicle tillage along a set position and a set course;
step 2.7.1, converting the acquired corner Ang _ turn into a corner Ang _ l at the left side of the front wheel and a corner Ang _ r at the right side of the front wheel according to the following formula (1) and formula (2),
Description of the formula: wherein d is the wheel base and l is the wheel base;
2.7.2, converting the vehicle speed v into the speed v _ l of the front wheel and the speed v _ r of the rear wheel according to the left corner Ang _ l of the front wheel and the proportional relation between the right corner Ang _ r and the corner Ang _ turn of the front wheel;
2.7.3, deducing the relation between the turning radius of the left side and the turning radius of the whole vehicle of the rear wheel by 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, the recording module records the real-time state of the vehicle, the planning path data and the path tracking process data according to the requirement;
and 3, judging whether the control method or the parameters need to be changed or not by the user according to the display effect and the recorded data, if so, executing the step 1, and if not, finishing.
Aiming at the path tracking module in the step 2.6), the invention uses an algorithm for fusing a course error control algorithm and a Pure pursuit, and the method comprises the following specific steps:
step 2.6.1, judging whether the current vehicle position enters a response function or not and whether a planned path is acquired or not, if so, meeting the path tracking starting condition, and 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 course of the vehicle, if not, not adjusting, and then searching a nearest distance point in a path planning sequence point range which can be seen from the front at the current position of the vehicle so as to determine a target point;
step 2.6.3, calculating a lateral error and a distance error according to the attitude of the current vehicle and the coordinates and the heading of the target point, and then calculating a turning angle Ang required by the wheel when the distance error is controlled by using the Pure pursuit algorithm p_s Calculating the required turning angle Ang of the wheel by using a course error control algorithm heading ；
Step 2.6.4, judging whether the distance is greater than a distance threshold value, if so, utilizing Pure pursuit to carry out distance error control to enable the vehicle to move forward towards a target point, otherwise, calculating a corresponding distance index Ps and a corresponding speed index Pv according to a path tracking line type, and calculating a weight w for controlling the course error through the following formula (3) heading Calculating the weight w for controlling the distance error by the formula (4)p_sThen, the current angle Ang is calculated by using the formula (5)_current；
Step 2.6.5, aiming at improving steering stability and aiming at the non-abrupt change characteristic of steering, averaging the last turning angle Ang _ last and the current turning angle Ang _ current to obtain an output angle Ang _ out, and then calculating a wheel turning angle Ang _ turn according to the limit of the wheel turning angle and the limit of the steering rate;
step 2.6.6, judging whether the current advancing direction of the vehicle is advancing or backing, if so, taking a negative value for Ang _ out, and if not, keeping the value unchanged;
and 2.6.7, judging whether a new path task is issued, if so, emptying the path data cache and executing the new task, otherwise, judging whether the destination is reached, if so, sending a stop command, otherwise, calculating a target point, and then executing the step 2.6.2.
Description on equation (3) in step 2.6.4:
in order to quickly reach a target point, because the operation speed of the tractor is low, the time left for the course error control is enough, and therefore the course error control does not need to intervene in the whole process, a distance threshold value can be set firstly, usually the vehicle length is set, when the distance between the tractor and the target point is greater than the threshold value, only the distance error is considered at the moment, namely the vehicle advances towards the direction of the target point at the moment, the target point can be quickly reached, when the distance between the tractor and the target point is less than the threshold value, the course error intervenes at the moment, when the distance is closer, the proportion of the course error control is larger, the weight can realize the degree of the course error changing along with the distance by adjusting the power, and the power can be adjusted according to the difference of road conditions such as straight driving. Similarly, if the speed is different, the time left for the heading error adjustment is also different, so that the weight of the speed needs to be introduced into the formula.
Description on equations (4) and (5) in step 2.6.4:
because only one input value exists at the same time, steering sudden change may exist in the process of either distance error control or course error control or alternative control, and because the course error control and the distance error control may be opposite, the distance error control and the course error control need to be considered at the same time and are realized mainly by giving different weights to the distance error control and the course 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 real vehicle verification unmanned algorithm, 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 a single parameter condition and realizes the optimization of the parameter adjustment in the shortest time.
2) The invention integrates the course error control algorithm and the Pure pursuit, thereby realizing the application to the unmanned system of the tractor with the Ackerman structure in the low-speed scene and expanding the applicability of the Pure pursuit algorithm.
Drawings
FIG. 1 is an architectural diagram of a tractor motion control simulation system of the present invention;
fig. 2 is a schematic flow diagram of the tractor path tracking of the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
According to fig. 1, the present invention provides a technical solution:
a tractor unmanned motion control simulation system comprises a user interface, a path planning module, a vehicle state issuing module, an execution 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 publishing module, the path planning module transmits planned path data to the path tracking module, and the path tracking module enables the vehicle to cultivate according to the content of the 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 facilitate the subscription and use of the path tracking module and the display module; the execution module is respectively connected with the path tracking module and the log recording module and provides an execution part required by an Ackerman structure of the tractor so as to facilitate the system to carry out 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, the log recording module is mainly used for facilitating positioning, analyzing problems and counting the accuracy of motion control, and the recording content comprises the real-time posture of the vehicle, path planning content and 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 a tractor, a farmland boundary and a cultivation mode; 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 cultivated land block, the vehicle parameters and the cultivation mode information transmitted by the interaction module, and generates a full-coverage cultivated path through a path algorithm input by a user and distributes the full-coverage cultivated path to the path tracking module and the display module; the display module of the user interface is connected with the vehicle state publishing 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 publishing 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 publishing module, and the model file receives 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 according to the present invention is implemented as follows: the method comprises the steps of finding out a target point meeting conditions in a path planning sequence point in a path planning module according to the current posture of a vehicle, wherein the target point meeting the conditions is a data point which is closest to the vehicle in the driving direction of the vehicle, calculating a course error according to the current course of the vehicle and the target point course, calculating a distance error according to a position coordinate of the vehicle and a target point coordinate, controlling a navigation error by using a course error control algorithm, controlling the distance error by using a Pure pursuit algorithm, outputting Ang _ current according to a fusion algorithm, averaging the last turn angle Ang _ last time and the current turn angle Ang _ current to obtain an output angle Ang _ out, and calculating a wheel turn angle Ang _ turn according to a wheel turn angle limit and a steering rate limit.
The invention realizes the application 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 controlling the unmanned movement of the tractor comprises the following specific steps:
step 1, 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 the setting of a user;
step 2.1, the system loads the generated model file and each function module, and displays the model of the tractor in a Gazebo3D dynamic simulator;
step 2.1.1, forming a matching hole perpendicular to a rotating shaft on a vehicle body part through a stretching and shearing command in SolidWorks software so as to eliminate the influence of the out-tilt angle of a steering wheel and the non-perpendicularity of the steering shaft caused by the back tilt angle;
2.1.2, generating parts of the processed three-dimensional model of the tractor through an export file command in SolidWorks software, wherein the parts comprise left and right wheels, a rotating shaft and a stl format file corresponding to the tractor body;
step 2.1.3, configuring files in a urdf format corresponding to the tractor, wherein the coordinate center of a tractor body is set as the center of a rear shaft, the centers of tires of front and rear wheels and the center of a steering shaft of the front wheel are set as the centers of cylinders, the tractor 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;
2.2, the path planning module plans an optimal path according to the terrain and cultivation mode input by the user and issues the optimal path through the path planning module;
step 2.3, the path display node subscribes the content of path planning, further establishes the coordinate relationship between the data point and the odometer, and displays the planned path in an interface 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 acquires the model attitude through the model service function and issues the model attitude according to the specified frequency;
2.5, converting the vehicle coordinates and the odometer coordinates by a vehicle state real-time display module, broadcasting the converted vehicle body posture by using the odometer, and displaying the running condition of the tractor in real time in the display module;
step 2.6, the system executes the path tracking module part and outputs the rotation angle of the steering wheel;
step 2.7, the execution module receives the rotation angle of the steering wheel and converts the rotation angle into rotation angles on the left side and the right side of a front wheel of the tire through Ackerman structural characteristics so as to realize vehicle tillage along a set position and a set course;
step 2.7.1, converting the acquired corner Ang _ turn into a corner Ang _ l at the left side of the front wheel and a corner Ang _ r at the right side of the front wheel according to the following formula (1) and formula (2),
Description of the formula: wherein d is the wheel base and l is the wheel base;
2.7.2, converting the vehicle speed v into the speed v _ l of the front wheel and the speed v _ r of the rear wheel according to the left corner Ang _ l of the front wheel and the proportional relation between the right corner Ang _ r and the corner Ang _ turn of the front wheel;
2.7.3, deducing the relation between the turning radius of the left side and the turning radius of the whole vehicle of the rear wheel by 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, the recording module records the real-time state of the vehicle, the planning path data and the path tracking process data according to the requirement;
and 3, judging whether the control method or the parameters need to be changed or not by the user according to the display effect and the recorded data, if so, executing the step 1, and if not, finishing.
Aiming at the path tracking module in the step 2.6), the invention uses an algorithm for fusing a course error control algorithm and a Pure pursuit, and the method comprises the following specific steps:
step 2.6.1, judging whether the current vehicle position enters a response function or not and whether a planned path is acquired or not, if so, meeting the path tracking starting condition, and 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 course of the vehicle, if not, not adjusting, and then searching a nearest distance point in a path planning sequence point range which can be seen from the front at the current position of the vehicle so as to determine a target point;
step 2.6.3, calculating a lateral error and a distance error according to the attitude of the current vehicle and the coordinates and the heading of the target point, and then calculating a turning angle Ang required by the wheel when the distance error is controlled by using the Pure pursuit algorithm p_s Calculating the required turning angle Ang of the wheel by using a course error control algorithm heading ；
Step 2.6.4, judging whether the distance is greater than a distance threshold value, if so, utilizing Pure pursuit to carry out distance error control to enable the vehicle to move forward towards a target point, otherwise, calculating a corresponding distance index Ps and a corresponding speed index Pv according to a path tracking line type, and calculating a weight w for controlling the course error through the following formula (3) heading Calculating the weight w for controlling the distance error by the formula (4)p_sThen, the current angle Ang is calculated by using the formula (5)_current；
Step 2.6.5, aiming at improving steering stability and aiming at the non-abrupt change characteristic of steering, averaging the last turning angle Ang _ last and the current turning angle Ang _ current to obtain an output angle Ang _ out, and then calculating a wheel turning angle Ang _ turn according to the limit of the wheel turning angle and the limit of the steering rate;
step 2.6.6, judging whether the current advancing direction of the vehicle is advancing or backing, if so, taking a negative value for Ang _ out, and if not, keeping the value unchanged;
and 2.6.7, judging whether a new path task is issued, if so, emptying the path data cache and executing the new task, otherwise, judging whether the destination is reached, if so, sending a stop command, otherwise, calculating a target point, and then executing the step 2.6.2.
Description on equation (3) in step 2.6.4:
in order to quickly reach a target point, because the operation speed of the tractor is low, the time left for the course error control is enough, and therefore the course error control does not need to intervene in the whole process, a distance threshold value can be set firstly, usually the vehicle length is set, when the distance between the tractor and the target point is greater than the threshold value, only the distance error is considered at the moment, namely the vehicle advances towards the direction of the target point at the moment, the target point can be quickly reached, when the distance between the tractor and the target point is less than the threshold value, the course error intervenes at the moment, when the distance is closer, the proportion of the course error control is larger, the weight can realize the degree of the course error changing along with the distance by adjusting the power, and the power can be adjusted according to the difference of road conditions such as straight driving. Similarly, if the speed is different, the time left for the heading error adjustment is also different, so that the weight of the speed needs to be introduced into the formula.
Description on equations (4) and (5) in step 2.6.4:
because only one input value exists at the same time, steering sudden change may exist in the process of either distance error control or course error control or alternative control, and because the course error control and the distance error control may be opposite, the distance error control and the course error control need to be considered at the same time and are realized mainly by giving different weights to the distance error control and the course error control.
The whole operation process can complete the simulation of the motion control and check the effect of the motion control within about 4min, the simulation efficiency is improved by over 90 percent, and the cost is reduced by 99 percent because the simulation test only needs to be operated on the existing PC computer without extra expenditure.
By implementing the tractor unmanned motion control simulation system and the simulation method thereof, aiming at the defects of low efficiency, high cost and poor optimal effect of the real vehicle verification unmanned algorithm, 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 condition and realizes the optimization of parameter adjustment in the shortest time. The course error control algorithm and the Pure pursuit are fused, so that the method can be applied to the unmanned system of the tractor with the 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 appreciated by those skilled in the art that changes, modifications, substitutions and alterations can be made in these embodiments without departing from the principles and spirit of the invention, the scope of which is defined in the appended claims and their equivalents.
Claims (10)
1. The utility model provides a tractor unmanned motion control simulation system which characterized in that: the system comprises a user interface, a path planning module, a vehicle state publishing module, an execution 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 publishing module, the vehicle state publishing 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 publishing module, and the other end of the log recording module is connected with the execution module.
2. The tractor unmanned motion control simulation system of claim 1, wherein: the log recording module records the real-time attitude of the vehicle, the path planning content, the motion control output command such as a steering angle, a vehicle speed and a control parameter.
3. The tractor unmanned motion control simulation system of claim 1, wherein: the user interface comprises an interaction module and a display module, the interaction module is connected with the 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 the path tracking module and the display module, and the display module of the user interface is connected with the vehicle state publishing module.
4. The tractor unmanned motion control simulation system of claim 3, 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, 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 publishing module, and the other end of the vehicle state real-time display node is connected with the model file.
5. A simulation method for controlling the unmanned movement of a tractor is characterized in that: the method comprises the following specific steps:
step 1, 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 the setting of a user;
and 3, judging whether the control method or the parameters need to be changed or not by the user according to the display effect and the recorded data, if so, executing the step 1, and if not, finishing.
6. The method of simulating tractor unmanned motion control of claim 5, wherein: the system carries out the method of loading the model, planning the path, displaying the path, tracing the path and recording the data according to the user setting, and the specific steps are as follows:
step 2.1, the system loads the generated model file and each function module, and displays the model of the tractor in a Gazebo3D dynamic simulator;
2.2, the path planning module plans an optimal path according to the terrain and cultivation mode input by the user and issues the optimal path through the path planning module;
step 2.3, the path display node subscribes the content of path planning, further establishes the coordinate relationship between the data point and the odometer, and displays the planned path in an interface 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 acquires the model attitude through the model service function and issues the model attitude according to the specified frequency;
2.5, converting the vehicle coordinates and the odometer coordinates by a vehicle state real-time display module, broadcasting the converted vehicle body posture by using the odometer, and displaying the running condition of the tractor in real time in the display module;
step 2.6, the system executes the path tracking module part and outputs the rotation angle of the steering wheel;
step 2.7, the execution module receives the rotation angle of the steering wheel and converts the rotation angle into rotation angles on the left side and the right side of a front wheel of the tire through Ackerman structural characteristics so as to realize vehicle tillage along a set position and a set course;
and 2.8, recording the real-time state of the vehicle, the planning path data and the path tracking process data by a recording module according to the requirement.
7. The method of simulating tractor unmanned motion control of claim 6, wherein: the specific steps of step 2.1 are as follows:
step 2.1.1, forming a matching hole vertical to a rotating shaft on a vehicle body part through a stretching and shearing command in SolidWorks software;
2.1.2, generating parts of the processed three-dimensional model of the tractor through an export file command in SolidWorks software, wherein the parts comprise left and right wheels, a rotating shaft and a stl format file corresponding to the tractor body;
and 2.1.3, configuring files in a urdf format corresponding to the tractor, wherein the coordinate center of the tractor body is set as the center of a rear shaft, the centers of tires of front and rear wheels and the center of a steering shaft of the front wheel are set as the centers of cylinders, the tractor body is simplified into a cuboid, and the four wheels are simplified into a cylinder.
8. The method of simulating tractor unmanned motion control of claim 6, wherein: the path tracking module in the step 2.6) uses an algorithm for fusing a course error control algorithm and a Pure pursuit, and the specific steps are as follows:
step 2.6.1, judging whether the current vehicle position enters a response function or not and whether a planned path is acquired or not, if so, meeting the path tracking starting condition, and 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 course of the vehicle, if not, not adjusting, and then searching a nearest distance point in a path planning sequence point range which can be seen from the front at the current position of the vehicle so as to determine a target point;
2.6.3, according to the current vehicle postureAnd calculating the transverse error and the distance error according to the coordinates and the course of the target point, and then calculating the turning angle Ang required by the wheel when controlling the distance error by using the Pure pursuit algorithm p_s Calculating the required turning angle Ang of the wheel by using a course error control algorithm heading ；
Step 2.6.4, judging whether the distance is greater than a distance threshold value, if so, utilizing Pure pursuit to carry out distance error control to enable the vehicle to move forward towards a target point, otherwise, calculating a corresponding distance index Ps and a corresponding speed index Pv according to a path tracking line type, and calculating a weight w for controlling the course error through the following formula (3) heading Calculating the weight w for controlling the distance error by the formula (4)p_sThen, the current angle Ang is calculated by using the formula (5)_current；
Step 2.6.5, aiming at improving steering stability and aiming at the non-abrupt change characteristic of steering, averaging the last turning angle Ang _ last and the current turning angle Ang _ current to obtain an output angle Ang _ out, and then calculating a wheel turning angle Ang _ turn according to the limit of the wheel turning angle and the limit of the steering rate;
step 2.6.6, judging whether the current advancing direction of the vehicle is advancing or backing, if so, taking a negative value for Ang _ out, and if not, keeping the value unchanged;
and 2.6.7, judging whether a new path task is issued, if so, emptying the path data cache and executing the new task, otherwise, judging whether the destination is reached, if so, sending a stop command, otherwise, calculating a target point, and then executing the step 2.6.2.
9. The method of simulating tractor unmanned motion control of claim 6, wherein: the specific steps of step 2.7 are as follows:
step 2.7.1, converting the acquired corner Ang _ turn into a corner Ang _ l at the left side of the front wheel and a corner Ang _ r at the right side of the front wheel according to the following formula (1) and formula (2),
Description of the formula: wherein d is the wheel base and l is the wheel base;
2.7.2, converting the vehicle speed v into the speed v _ l of the front wheel and the speed v _ r of the rear wheel according to the left corner Ang _ l of the front wheel and the proportional relation between the right corner Ang _ r and the corner Ang _ turn of the front wheel;
and 2.7.3, deducing the relation between the turning radius of the left side and the turning radius of the right side of the rear wheel and the turning radius of the whole vehicle by 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.
10. The method of simulating tractor unmanned motion control of claim 8, wherein: step 2.6.4, according to the formula (3), firstly, a distance threshold value is set, when the distance between the tractor and a target point is greater than the threshold value, only the distance error is considered at the moment, when the distance between the tractor and the target point is less than the threshold value, the course error intervenes at the moment, the closer the distance is, the greater the proportion of course error control is, the degree of course error change along with the distance is realized by adjusting the weight of the power, the power is adjusted according to road conditions such as difference of straight running and curve running, under the same distance condition, the speed is different, and the time for adjusting the course error is 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 the distance error control and the heading error control are realized mainly by giving different weights to the distance error control and the heading error control.
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 true CN111309030A (en) | 2020-06-19 |
| CN111309030B 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 (6)
| 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 |
| 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 |
-
2020
- 2020-03-26 CN CN202010224577.6A patent/CN111309030B/en active Active
Patent Citations (6)
| 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 |
| 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 |
|---|
| 刘成强，等: "拖拉机自动驾驶及控制技术", 《农业工程》, vol. 9, no. 4, 30 April 2019 (2019-04-30), pages 87 - 91 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111309030B (en) | 2024-05-24 |
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 | |
| Yang et al. | An optimal goal point determination algorithm for automatic navigation of agricultural machinery: Improving the tracking accuracy of the Pure Pursuit algorithm | |
| CN108508760B (en) | Joint simulation method and system for multi-wheel independently-driven slip steering vehicle | |
| Sabelhaus et al. | Using continuous-curvature paths to generate feasible headland turn manoeuvres | |
| CN109823393A (en) | A kind of intelligent driving Vehicle tracing control method | |
| CN107092266B (en) | A kind of locomotive Trajectory Tracking Control method | |
| CN108820042A (en) | A kind of automatic Pilot method and device | |
| CN109388140B (en) | Improved pure tracking control method for ground vehicle path tracking | |
| CN113359710B (en) | LOS theory-based agricultural machinery path tracking method | |
| CN108007417A (en) | A kind of agricultural machinery automatic driving control system angular transducer automatic calibration method | |
| CN109283843B (en) | Path-changing trajectory planning method based on fusion of polynomial and particle swarm optimization | |
| CN104590259A (en) | Track searching method and system | |
| CN111158379B (en) | Steering wheel zero-bias self-learning unmanned vehicle track tracking method | |
| CN211741925U (en) | Tractor unmanned motion control simulation system | |
| CN101118500A (en) | Software emulation method of self-determination driving vehicle running process | |
| 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 | |
| CN107992039B (en) | Trajectory planning method based on flow field in dynamic environment | |
| CN111309030A (en) | Tractor unmanned motion control simulation system and simulation method thereof | |
| CN112069559B (en) | Method for determining inner wheel difference area range of right turn at intersection of large vehicles and application of method | |
| Miao et al. | Research on decoupling control for the longitudinal and lateral dynamics of a tractor considering steering delay | |
| Zhou et al. | Optimization of autonomous driving state control of low energy consumption pure electric agricultural vehicles based on environmental friendliness | |
| CN113914406B (en) | Electric loader path tracking system and control method | |
| CN115655311A (en) | Ackerman robot odometer calibration method based on scanning matching |
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 |