A method, apparatus, device and medium for motion control of a tracked mobile machine

Abstract

The application provides a motion control method, device, equipment and medium of a tracked mobile machine, a reference path composed of a plurality of path points is given, and a preview distance is set. A path point closest to the preview distance from the current position of the mobile machine is searched in the reference path, and is set as a preview point. A circular arc is constructed between the mobile machine and the preview point as a real-time tracking path of the mobile machine. The speed of the left and right tracks of the mobile machine is controlled to make the mobile machine always travel along the reference path. The application realizes the motion control of the tracked mobile machine, solves the problem that the existing motion control algorithm cannot be directly applied to the tracked mobile machine, and lays a certain technical foundation for unmanned driving and operation of the tracked mobile machine.

Description

Technical Field

[0001] This invention relates to the field of motion control technology, specifically to a motion control method, device, equipment, and medium for tracked mobile machinery. Background Technology

[0002] Mobile machinery has a wide range of applications in modern agriculture. With the continuous development of global technology, the intelligentization of mobile machinery has become a future development trend. The intelligentization of mobile machinery includes product intelligence, manufacturing intelligence, service intelligence, and management intelligence. Intelligent mobile machinery falls under the category of product intelligence, referring to the ability of mobile machinery to autonomously complete corresponding walking and operational tasks without human intervention.

[0003] Intelligent mobile machinery is generally considered to comprise five major modules: environmental perception, mobile machinery positioning, decision-making and planning, motion control, and automated operation. Among these, motion control is one of the foundations for achieving unmanned driving and automated operation of mobile machinery. Unlike ordinary passenger vehicles, mobile machinery often operates on unstructured roads, with complex and ever-changing operating scenarios, making existing motion control algorithms unsuitable for direct application.

[0004] Currently, intelligent mobile machinery motion control technology is mainly applied in the automotive field, with relatively little research on related technologies in the mobile machinery sector. Existing intelligent mobile machinery motion control technologies are mostly based on PID control, which suffers from problems such as difficulty in parameter tuning, poor generalization, and low control accuracy.

[0005] In view of the above, this application is hereby submitted. Summary of the Invention

[0006] In view of this, the purpose of the present invention is to provide a motion control method, device, equipment and medium for tracked mobile machinery, which can effectively solve the problems of existing intelligent mobile machinery motion control technology, which is mainly based on PID control, and has the problems of difficult parameter adjustment, poor generalization and low control accuracy.

[0007] This invention discloses a motion control method for tracked mobile machinery, comprising:

[0008] Acquire multiple path points transmitted by the RTK positioning device, generate a reference path, and set the aiming distance;

[0009] Obtain the current position of the mobile machinery, filter out the path point in the reference path that is closest to the pre-aiming distance to the current position of the mobile machinery, and set the path point as the pre-aiming point;

[0010] An arc is constructed between the mobile machinery and the pre-aiming point as the real-time tracking path of the mobile machinery;

[0011] The mobile machine tracking speed is obtained, and the magnitudes of the left track speed and the right track speed of the mobile machine are controlled according to the mobile machine tracking speed and the real-time tracking path, so that the mobile machine always travels along the reference path.

[0012] Preferably, a plurality of path points transmitted by the RTK positioning device are obtained, a reference path is generated, and a preview distance is set, specifically:

[0013] The RTK positioning device is called to dot the to-be-worked region, the longitude and latitude of each point are obtained, and a plurality of path points are generated;

[0014] A full-coverage path planning algorithm is called to process the path points, and a reference path is generated, wherein the reference path is composed of a plurality of path points;

[0015] A preset range limit and a preset initial value are obtained, a preview distance is generated, and the preview distance is dynamically adjusted according to a subsequent real-time travel speed of the mobile machine.

[0016] Preferably, a full-coverage path planning algorithm is called to process the path points, and a reference path is generated, specifically:

[0017] A to-be-worked region contour map is constructed according to the longitude and latitude of each point, and the coordinates of each point of the to-be-worked region contour map are determined;

[0018] The to-be-worked region contour map is preprocessed, and complex contours in the to-be-worked region contour map are simplified and smoothed;

[0019] A multi-objective global optimization cost function is constructed;

[0020] Based on the to-be-worked region contour map, a Headlands boundary is generated;

[0021] According to the multi-objective global optimization cost function and the Headlands boundary, a parallel line bundle is divided in a surrounded working region in the to-be-worked region contour map, or a parallel line bundle is divided in the surrounded working region in the to-be-worked region contour map according to a user-defined parallel line angle, and the parallel line bundle is processed according to the multi-objective global optimization cost function, to generate a parallel line distribution with an optimal angle;

[0022] A starting point of the parallel line bundle is defined, and the parallel line bundle is sorted according to a preset heuristic method;

[0023] A preset minimum turning radius of the mobile machine is obtained, a Dubins curve or a Reeds-Sheep curve is called to connect the parallel line bundle, the full-coverage path planning is completed, and a reference path is generated.

[0024] Preferably, the current position of the mobile machine is acquired, a path point in the reference path closest to the current position of the mobile machine by a distance equal to the preview distance is screened out, and the path point is set as the preview point, specifically:

[0025] A straight line equation is determined according to the preview distance as a search radius, and a formula is determined according to the straight line equation And the formula The distance between the next path point and the previous path point in the sequence of the reference path and the current position of the mobile machine and the straight line equation is solved Wherein, The current position of the mobile machine is The previous path point position is The next path point position is Each coefficient of the plane equation is The distance between the current position of the mobile machine and the straight line equation is

[0026] When it is determined that the search radius is less than the distance between the current position of the mobile machine and the straight line equation, a search failure signal is generated, and the search for the preview point is re-performed;

[0027] When it is determined that the search radius is equal to the distance between the current position of the mobile machine and the straight line equation, the preview point is the projection point position of the current position of the mobile machine on the straight line equation;

[0028] When it is determined that the search radius is greater than the distance between the current position of the mobile machine and the straight line equation, a circle is drawn with the current position of the mobile machine as the center and the search radius as the radius, and the intersection point of the circle and the straight line equation is calculated, wherein the intersection point in the same direction as the moving direction of the mobile machine is the preview point.

[0029] Preferably, a circular arc is constructed between the mobile machine and the preview point as the real-time tracking path of the mobile machine, specifically:

[0030] An angle between the mobile machine and the preview point is acquired, and the circular arc radius of the real-time tracking path of the mobile machine and the curvature radius of the real-time tracking path of the mobile machine are solved through a sine theorem calculation model according to the angle, the preview distance, the current position of the mobile machine and the preview point, the sine theorem calculation model being , Wherein, The angle between the mobile machine and the preview point is The preview distance is The circular arc radius of the real-time tracking path of the mobile machine is The curvature radius of the real-time tracking path of the mobile machine is

[0031] constructing a real-time tracking path of the mobile machine by using a radius of an arc of the real-time tracking path of the mobile machine.

[0032] Preferably, a tracking speed of the mobile machine is obtained, and magnitudes of left track speed and right track speed of the mobile machine are controlled according to the tracking speed of the mobile machine and the real-time tracking path, so that the mobile machine always travels along the reference path, specifically:

[0033] obtaining a tracking speed of the mobile machine, and controlling magnitudes of left track speed and right track speed of the mobile machine according to the tracking speed of the mobile machine and the real-time tracking path, so that the mobile machine always travels along the reference path, specifically: processing the tracking speed of the mobile machine, the radius of the arc of the real-time tracking path of the mobile machine and a lateral error between the position of the mobile machine and the preview point, and solving an angular velocity of the mobile machine around a center of the real-time tracking path, wherein, the angular velocity of the mobile machine around the center of the real-time tracking path, the tracking speed of the mobile machine, the lateral error between the position of the mobile machine and the preview point;

[0034] according to a straight-going formula of the mobile machine , a left-turn formula of the mobile machine , , and a right-turn formula of the mobile machine , calculating the angular velocity of the mobile machine around the center of the real-time tracking path, the radius of the arc of the real-time tracking path of the mobile machine and a half length of a vehicle width of the mobile machine, and solving speeds of left and right tracks of the mobile machine, wherein, the left track speed of the mobile machine, the right track speed of the mobile machine, the half length of the vehicle width of the mobile machine.

[0035] The application further discloses a motion control device of a tracked mobile machine, which comprises:

[0036] a data management unit configured to obtain a plurality of path points transmitted by an RTK positioning device, generate a reference path, and set a preview distance;

[0037] a preview point searching unit configured to obtain a current position of the mobile machine, screen a path point closest to the preview distance from the current position of the mobile machine in the reference path, and set the path point as a preview point;

[0038] a tracking path generating unit configured to construct an arc between the mobile machine and the preview point as a real-time tracking path of the mobile machine;

[0039] A motion control unit is configured to obtain a tracking speed of the mobile machine, and control magnitudes of left and right track speeds of the mobile machine according to the tracking speed and the real-time tracking path, so that the mobile machine always travels along the reference path.

[0040] The application further discloses a motion control device of a tracked mobile machine, which comprises a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor.

[0041] The application further discloses a readable storage medium, which stores a computer program capable of being executed by a processor of a device where the readable storage medium is located to implement the motion control method of the tracked mobile machine.

[0042] To sum up, the motion control method, device, equipment and medium of the tracked mobile machine provided by the embodiment have the following technical points: a reference path composed of a plurality of path points is given, and a preview distance is set. A path point closest to the preview distance from a current position of the mobile machine is searched in the reference path, and is set as a preview point. A circular arc between the mobile machine and the preview point is constructed as a real-time tracking path of the mobile machine. The mobile machine always travels along the reference path by controlling the left and right track speeds of the mobile machine. The application realizes the motion control of the tracked mobile machine, solves the problem that the existing motion control algorithm cannot be directly applied to the tracked mobile machine, and lays a certain technical foundation for unmanned driving and operation of the tracked mobile machine. Thus, the problem that the intelligent mobile machine motion control technology in the prior art is mainly PID control, and has the problems of difficult parameter adjustment, poor generalization, and low control precision is solved. BRIEF DESCRIPTION OF DRAWINGS

[0043] Figure 1 is a flowchart of a motion control method of a tracked mobile machine provided by an embodiment of the application.

[0044] Figure 2 is a module schematic diagram of a motion control device of a tracked mobile machine provided by an embodiment of the application. DETAILED DESCRIPTION

[0045] In order to make the purposes, technical solutions and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only some of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative work fall within the scope of protection of the present application. Therefore, the following detailed description of the embodiments of the present application provided in the drawings is not intended to limit the scope of the claimed present application, but only represents selected embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative work fall within the scope of protection of the present application.

[0046] The specific embodiments of the present application will be described in detail below with reference to the drawings.

[0047] The present application discloses a track-type mobile machine motion control method, device, equipment and medium, which at least solves the problems of the prior art to some extent.

[0048] Please refer to Figure 1 The first embodiment of the present application provides a track-type mobile machine motion control method, which can be executed by a motion control device (hereinafter referred to as a control device) of a track-type mobile machine, and in particular, by one or more processors in the control device to implement the following steps:

[0049] S101, acquiring a plurality of path points transmitted by an RTK positioning device, generating a reference path, and setting a preview distance;

[0050] Specifically, step S101 includes calling the RTK positioning device to dot the to-be-worked region, acquiring the longitude and latitude of each point, and generating a plurality of path points;

[0051] A full-coverage path planning algorithm is called to process the path points to generate a reference path, wherein the reference path is composed of a plurality of path points;

[0052] A preset range limit and a preset initial value are acquired to generate a preview distance, and the preview distance is dynamically adjusted according to a subsequent real-time travel speed of the mobile machine.

[0053] In particular, a full-coverage path planning algorithm is called to process the path points to generate a reference path, specifically:

[0054] A contour map of the to-be-worked region is constructed according to the longitude and latitude of each point, and the coordinates of each point of the contour map of the to-be-worked region are determined;

[0055] Preprocess the to-be-worked region contour map, and simplify and smooth the complex contour in the to-be-worked region contour map;

[0056] Construct a multi-target global optimization cost function;

[0057] Generate a Headlands boundary based on the to-be-worked region contour map;

[0058] According to the multi-target global optimization cost function and the Headlands boundary, perform parallel line bundle division on the surrounded working region in the to-be-worked region contour map, or perform parallel line bundle division on the surrounded working region in the to-be-worked region contour map according to a self-defined parallel line angle, and perform processing on the parallel line bundle according to the multi-target global optimization cost function to generate a parallel line distribution with an optimal angle;

[0059] Define a starting point of the parallel line bundle, and sort the parallel line bundle according to a preset heuristic method;

[0060] Obtain a preset minimum turning radius of a mobile machine, call a Dubins curve or a Reeds-Sheep curve to connect the parallel line bundle, complete the full-coverage path planning, and generate a reference path.

[0061] Currently, intelligent mobile machine motion control technology is mainly applied in the field of automobiles, and relatively little research is conducted in the field of mobile machines. Existing intelligent mobile machine motion control technology is mainly based on PID control, which has problems such as difficult parameter tuning, poor generalization, and low control precision.

[0062] In this embodiment, the control device can be a desktop computer, a notebook computer, a server, a workstation, or the like, which has a data processing and analysis capability. The control device can be installed with a corresponding operating system and application software, and the functions required in this embodiment can be realized through the combination of the operating system and the application software.

[0063] Specifically, in the embodiment, a reference path composed of a plurality of path points is given, and a preview distance is set. Specifically, first, the RTK positioning device is used to mark the to-be-operated area to obtain the latitude and longitude of each point. Second, a full-coverage path planning algorithm is used to generate a reference path composed of a plurality of path points. The to-be-operated area contour map is constructed by using the latitude and longitude of each point, and the coordinates of each point of the to-be-operated area contour map are determined. The to-be-operated area contour map is preprocessed to simplify and smooth part of the complex contour. A multi-objective global optimization cost function is constructed. The Headlands boundary is generated based on the to-be-operated area contour map. Based on the multi-objective global optimization cost function, the to-be-operated area surrounded by the Headlands boundary is divided into parallel line bundles. The optimal angle of the parallel line distribution is obtained according to the multi-objective global optimization cost function, or the to-be-operated area surrounded by the Headlands boundary is divided into parallel line bundles according to the user-defined parallel line angle. The starting point of the parallel line bundle is defined, and the preset heuristic method is used to sort the parallel line bundle. The minimum turning radius of the mobile machine is given, and the Dubins curve or Reeds-Sheep curve is used to connect the parallel line bundle to complete the full-coverage path planning. Finally, the range of the preview distance is limited, an initial value is given, and the preview distance is dynamically adjusted according to the subsequent real-time travel speed of the mobile machine.

[0064] In the embodiment, the to-be-operated area surrounded by the to-be-operated area contour map is divided into parallel line bundles according to the multi-objective global optimization cost function and the Headlands boundary. According to actual needs, the specific optimization cost function required can be selected from the multi-objective global optimization cost function. The multi-objective global optimization cost function is specifically:

[0065] The first optimization cost function is to maximize the field coverage, which is used to calculate the percentage of the field covered by the line bundle. The cost is a value between [0, 1], which is defined as a maximization problem. For a large area , suppose it intersects with a plurality of small areas , and the sum of the areas of the intersection parts is , then the maximization objective function is: , wherein is the total area of the large area , and represents the sum of the overlapping parts of all small areas and large areas, which can calculate the coverage degree of the large area by all small areas.

[0066] The second optimization cost function, the harness number minimization constraint, is used to minimize the number of harnesses. This objective function depends on the shape and area of ​​the site and the width of the robot. The number of harnesses is constrained by the following conditions: ,in, It is a given angle Number of scanned wires It is the area enclosed by the inner boundary of Headlands. Let be the robot's operating width. Then the objective function to be minimized is... .

[0067] The third optimization cost function is field overlap maximization. The field overlap function is used to calculate the area of ​​the overlap between one polygonal region and several other polygonal regions. An example is as follows: Let A be a polygon, B be multiple polygonal regions, and O be their overlapping portion, then we have: , , ,in, , , These represent the number of vertices of polygons A, B, and O, respectively. Let be the coordinates of the i-th vertex of polygon A, and N be the area of ​​the overlapping region calculated using Green's theorem. ,Right now: The objective function to be maximized is: .

[0068] The fourth optimization cost function minimizes the harness path length, which can be calculated using the following formula: ,in, Sum the lengths of all the wire bundles. It refers to the number of wire harnesses. It is the first The number of nodes on the parallel boundaries, It is the first The first parallel boundary 1 node It is a node and nodes Given the Euclidean norm, the objective function to be minimized is: .

[0069] In summary, users can select any one or more of the first to fourth optimization cost functions as needed, and simultaneously divide the enclosed work area into parallel line bundles. Alternatively, the enclosed work area can be divided into parallel line bundles based on a user-defined parallel line angle, where the user-defined parallel line angle is the angle manually defined to generate the parallel line bundles.

[0070] In this embodiment, the starting point of the parallel line bundle is defined, and the parallel line bundle is sorted using a preset heuristic method. This preset heuristic method includes the ox-plowing heuristic and the snake-like heuristic. The specific steps of the ox-plowing heuristic planning are as follows: First, state point definition: the map is divided into a finite number of state points. In this embodiment, only the starting and ending points of the parallel line bundles are used as state points; more state points can be defined by the user. The second step is state selection. A suitable starting state is selected on the map. In this example, the starting state point is the bottom left vertex of the polygon. In the current state, an adjacent state that has not been visited is selected as the next target state. The travel cost is calculated based on the distance between the two states and the estimated time and energy required to reach the target state. All adjacent states are traversed in turn, and the state with the smallest travel cost is selected as the next target state. After entering the next state, a forward or reverse (depending on the current state) path is established with that state as the origin until a boundary or obstacle is encountered. When it is impossible to continue, the starting point is returned and another path is built. This process is repeated until all state points have been traversed to complete the entire planning process, thereby achieving the sorting of the parallel line bundles.

[0071] The specific steps of the serpentine heuristic sorting are as follows: First, serpentine sorting: The parallel line bundles to be sorted are cyclically sorted, with an initial value of 1. The loop starts from the second element of the path array and operates on the array. The loop ends at the center of the array. Inside the loop, elements located between the current index i and i+1 are rotated to achieve the serpentine order. Second, serpentine turning: Elements from the i+1th element to the last element in the array are reversed to achieve the "snake's" turn. Finally, if the array length is odd, the elements located between i and i+1 are rotated again to complete the final step of the serpentine order sorting. This achieves the sorting of the parallel line bundles.

[0072] S102, obtain the current position of the mobile machinery, filter out the path point in the reference path that is closest to the pre-aiming distance to the current position of the mobile machinery, and set the path point as the pre-aiming point;

[0073] Specifically, step S102 includes: using the pre-aiming distance as the search radius, according to the linear equation... and formula Solve for the next and previous points in the sequence of the reference path, and the distance between the current position of the moving machinery and the equation of the straight line. ,in, The current position of the mobile machinery. The location of the previous path point. The location of the next path point. wherein, d is the distance between the current position of the mobile machine and the straight line equation, and wherein, d is the distance between the current position of the mobile machine and the straight line equation, and

[0074] When it is judged that the search radius is smaller than the distance between the current position of the mobile machine and the straight line equation, a search failure signal is generated, and the search of the preview point is re-performed;

[0075] When it is judged that the search radius is equal to the distance between the current position of the mobile machine and the straight line equation, the preview point is the projection point position of the current position of the mobile machine on the straight line equation;

[0076] When it is judged that the search radius is greater than the distance between the current position of the mobile machine and the straight line equation, a circle is drawn with the current position of the mobile machine as the center and the search radius as the radius, and the intersection point of the circle and the straight line equation is calculated, wherein the intersection point in the same direction as the moving direction of the mobile machine is the preview point.

[0077] Specifically, in the embodiment, a path point in the reference path closest to the current position of the mobile machine in the distance is searched and set as the preview point. First, a straight line equation of a next path point and a previous path point in the reference path sequence is solved with the preview distance as the search radius , and the distance between the current position of the mobile machine and the straight line equation is solved , specifically as follows:

[0078]

[0079] wherein, d is the distance between the current position of the mobile machine and the straight line equation, and is the current position of the mobile machine, is the position of the previous path point, is the position of the next path point, wherein, d is the distance between the current position of the mobile machine and the straight line equation, and is the distance between the current position of the mobile machine and the straight line equation;

[0080] When the search radius is smaller than the distance between the current position of the mobile machine and the straight line equation, it indicates that the search fails, and the search needs to be re-performed;

[0081] When the search radius is equal to the distance between the current position of the mobile machine and the straight line equation, the preview point position of the mobile machine is the projection point position of the current position of the mobile machine on the straight line equation;

[0082] When the search radius is greater than the distance between the current position of the mobile machine and the straight line equation, a circle is drawn with the current position of the mobile machine as the center and the search radius as the radius, and the intersection point of the circle and the straight line equation is obtained, wherein the intersection point in the same direction as the travel direction of the mobile machine is the preview point of the mobile machine, and the preview point search of the mobile machine is completed.

[0083] S103, a circular arc is constructed between the mobile machine and the preview point as the real-time tracking path of the mobile machine;

[0084] Specifically, step S103 includes: obtaining the included angle between the mobile machine and the preview point, and calculating the circular arc radius of the real-time tracking path of the mobile machine and the curvature radius of the real-time tracking path of the mobile machine by a sine theorem calculation model according to the included angle, the preview distance, the current position of the mobile machine and the preview point, wherein the sine theorem calculation model is , is the included angle between the mobile machine and the preview point, is the preview distance, is the circular arc radius of the real-time tracking path of the mobile machine, is the curvature radius of the real-time tracking path of the mobile machine;

[0085] The real-time tracking path of the mobile machine is constructed by using the circular arc radius of the real-time tracking path of the mobile machine.

[0086] Specifically, in the embodiment, a circular arc is constructed between the mobile machine and the preview point as the real-time tracking path of the mobile machine. Based on the current position of the mobile machine and the position of the preview point, the circular arc radius of the real-time tracking path of the mobile machine and the curvature radius of the tracking path are solved by the sine theorem in the case that the included angle between the mobile machine and the preview point and the preview distance are known, and the calculation model is specifically as follows:

[0087]

[0088]

[0089] is the included angle between the mobile machine and the preview point, is the preview distance, is the circular arc radius of the real-time tracking path of the mobile machine, is the curvature radius of the real-time tracking path of the mobile machine; and the real-time tracking path of the mobile machine is constructed by using the circular arc radius.

[0090] ​​S104, obtaining a mobile machine tracking speed, and controlling magnitudes of left and right track speeds of the mobile machine according to the mobile machine tracking speed and the real-time tracking path, so that the mobile machine always travels along the reference path.

[0091] Specifically, step S104 comprises: obtaining a mobile machine tracking speed, and solving an angular velocity of the mobile machine around a center of the real-time tracking path according to a formula Processing the mobile machine tracking speed, a radius of a circular arc of the real-time tracking path of the mobile machine, and a lateral error between the mobile machine position and the preview point, the angular velocity of the mobile machine around the center of the real-time tracking path is solved, wherein, the angular velocity of the mobile machine around the center of the real-time tracking path, the mobile machine tracking speed, the lateral error between the mobile machine position and the preview point.

[0092] According to a mobile machine straight-going formula , a mobile machine left-turn formula , , and a mobile machine right-turn formula , , the angular velocity of the mobile machine around the center of the real-time tracking path, the radius of the circular arc of the real-time tracking path of the mobile machine, and a half length of a vehicle width of the mobile machine are calculated, and speeds of left and right tracks of the mobile machine are solved, wherein, the left track speed of the mobile machine, the right track speed of the mobile machine, the half length of the vehicle width of the mobile machine.

[0093] Specifically, in the embodiment, the mobile machine always travels along the reference path by controlling the speeds of the left and right tracks of the mobile machine. Given the mobile machine tracking speed, the angular velocity of the mobile machine around the center of the real-time tracking path is solved in combination with the radius of the circular arc of the real-time tracking path of the mobile machine and the lateral error between the mobile machine position and the preview point, specifically as follows:

[0094]

[0095] wherein, the angular velocity of the mobile machine around the center of the real-time tracking path, the mobile machine tracking speed, the lateral error between the mobile machine position and the preview point.

[0096] Based on the angular velocity of the mobile machine around the center of the real-time tracking path, in combination with the radius of the circular arc of the real-time tracking path of the mobile machine and the half length of the width of the mobile machine, the left and right track speeds of the mobile machine are solved, specifically as follows:

[0097] The mobile machine moves straightly:

[0098]

[0099] The mobile machine turns left:

[0100]

[0101]

[0102] The mobile machine turns right:

[0103]

[0104]

[0105] wherein, is the left track speed of the mobile machine, is the right track speed of the mobile machine, is the half length of the width of the mobile machine.

[0106] In summary, the key points of the motion control method of the tracked mobile machine are to give a reference path composed of a plurality of path points and to set a preview distance. A path point closest to the preview distance from the current position of the whole vehicle is searched in the reference path, and is set as a preview point. A circular arc between the whole vehicle and the preview point is constructed as a real-time tracking path of the whole vehicle. The speeds of the left and right tracks of the whole vehicle are controlled to make the whole vehicle always travel along the reference path. The motion control of the tracked mobile machine is realized, the problem that the existing motion control algorithm cannot be directly applied to the tracked mobile machine is solved, and a certain technical foundation is laid for the unmanned driving and operation of the tracked mobile machine.

[0107] Please refer to Figure 2 The second embodiment of the present application provides a motion control device of a tracked mobile machine, comprising:

[0108] The data management unit 201 is used for acquiring a plurality of path points transmitted by an RTK positioning device, generating a reference path, and setting a preview distance;

[0109] The preview point searching unit 202 is used for acquiring the current position of the mobile machine, screening out a path point closest to the preview distance from the current position of the mobile machine in the reference path, and setting the path point as a preview point;

[0110] The tracking path generating unit 203 is configured to construct an arc as a real-time tracking path of the mobile machine between the mobile machine and the preview point.

[0111] The motion control unit 204 is configured to acquire a tracking speed of the mobile machine, and control magnitudes of left and right track belt speeds of the mobile machine according to the tracking speed of the mobile machine and the real-time tracking path, so that the mobile machine always travels along the reference path.

[0112] The third embodiment of the present application provides a motion control device of a tracked mobile machine, comprising a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor, and the processor implements the motion control method of the tracked mobile machine according to any one of the above embodiments when executing the computer program.

[0113] The fourth embodiment of the present application provides a readable storage medium, which stores a computer program capable of being executed by a processor of a device where the readable storage medium is located, to implement the motion control method of the tracked mobile machine according to any one of the above embodiments.

[0114] Exemplarily, the computer program in the third and fourth embodiments of the present application can be divided into one or more modules, which are stored in the memory and executed by the processor to complete the present application. The one or more modules can be a series of computer program instruction segments capable of completing a specific function, which are used to describe the execution process of the computer program in the motion control device of the tracked mobile machine. For example, the device in the second embodiment of the present application.

[0115] The processor can be a central processing unit (CPU), and can also be other general-purpose processors, digital signal processors (DSP), application specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor can be a microprocessor or the processor can also be any conventional processor and the like. The processor is the control center of the motion control method of the tracked mobile machine, and is connected with all parts of the motion control method of the tracked mobile machine through various interfaces and lines.

[0116] The memory can be configured to store the computer programs and / or modules, and the processor realizes various functions of the motion control method of the tracked mobile machine by running or executing the computer programs and / or modules stored in the memory and calling data stored in the memory. The memory can mainly include a program storage area and a data storage area, wherein the program storage area can store an operating system, at least one application program required by a function (such as a sound playing function, a text conversion function, etc.), and the like; and the data storage area can store data created according to use of the mobile phone (such as audio data, text message data, etc.), and the like. In addition, the memory can include a high-speed random access memory, and can also include a nonvolatile memory, for example, a hard disk, a memory, a plug-in hard disk, a Smart Media Card (SMC), a Secure Digital (SD) card, a Flash Card, at least one disk storage device, a flash memory device, or other volatile solid-state memory devices.

[0117] The modules implemented can be stored in a computer readable storage medium if they are implemented in the form of software function units and sold or used as independent products. Based on this understanding, all or part of the processes in the above-mentioned embodiment methods can also be completed by a computer program instructing related hardware, and the computer program can be stored in a computer readable storage medium. The computer program can implement the steps of each method embodiment when executed by a processor. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or some intermediate forms, etc. The computer readable medium can include any entity or device capable of carrying the computer program code, recording medium, U disk, mobile hard disk, magnetic disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signal, telecommunication signal, and software distribution medium, etc. It should be noted that the contents included in the computer readable medium can be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction, for example, in some jurisdictions, according to legislation and patent practice, the computer readable medium does not include electrical carrier signals and telecommunication signals.

[0118] It should be noted that the apparatus embodiments described above are merely illustrative, and the units described as separate units can or can not be physically separate, and the units shown as units can or can not be physical units, i.e., can be located in one place, or can be distributed to multiple network units. Part or all of the modules can be selected to achieve the purpose of the embodiment according to actual needs. In addition, the connection relationship between the modules in the apparatus embodiment provided by the present application indicates that there is a communication connection between them, which can be implemented as one or more communication buses or signal lines. Those skilled in the art can understand and implement it without creative labor.

[0119] The above is only the preferred embodiment of the present application, and the protection scope of the present application is not limited to the above-mentioned embodiments. Any technical solution falling within the concept of the present application shall fall within the protection scope of the present application.

Claims

1. A motion control method for a tracked mobile machine, characterized in that, include: Acquire multiple path points transmitted by the RTK positioning device, generate a reference path, and set the aiming distance; Obtain the current position of the mobile machinery, filter out the path point in the reference path that is closest to the pre-aiming distance from the current position of the mobile machinery, and set this path point as the pre-aiming point, specifically: Using the pre-aiming distance as the search radius, according to the linear equation and formula Solve for the next and previous points in the sequence of the reference path, and the distance between the current position of the moving machinery and the equation of the straight line. ,in, The current position of the mobile machinery. The location of the previous path point. The location of the next path point. Let be the coefficients of the equation of the line. The distance between the current position of the moving machinery and the equation of the straight line; When it is determined that the search radius is less than the distance between the current position of the mobile machinery and the straight line equation, a search failure signal is generated, and the search for the pre-aiming point is restarted; When it is determined that the search radius is equal to the distance between the current position of the mobile machinery and the straight line equation, the aiming point is the projection point of the current position of the mobile machinery onto the straight line equation; When it is determined that the search radius is greater than the distance between the current position of the mobile machinery and the straight line equation, a circle is drawn with the current position of the mobile machinery as the center and the search radius as the radius. The intersection point of the circle and the straight line equation is calculated. The intersection point with the same direction as the direction of travel of the mobile machinery is the pre-aiming point. An arc is constructed between the mobile machinery and the pre-aiming point as the real-time tracking path of the mobile machinery; The tracking speed of the mobile machinery is obtained, and the speeds of the left and right tracks of the mobile machinery are controlled based on the tracking speed and the real-time tracking path so that the mobile machinery always travels along the reference path.

2. The motion control method for a tracked mobile machine according to claim 1, characterized in that, Obtain multiple path points transmitted by the RTK positioning device, generate a reference path, and set the aiming distance, specifically: The RTK positioning device is used to mark points in the work area, obtain the latitude and longitude of each point, and generate multiple waypoints. The path points are processed by a full-coverage path planning algorithm to generate a reference path, wherein the reference path consists of multiple path points; The system acquires a preset range limit and a preset initial value, generates a pre-aiming distance, and dynamically adjusts the pre-aiming distance based on the subsequent real-time travel speed of the mobile machinery.

3. The motion control method for a tracked mobile machine according to claim 2, characterized in that, The full-coverage path planning algorithm is invoked to process the path points and generate a reference path, specifically as follows: A contour map of the area to be worked on is constructed based on the latitude and longitude of each point, and the coordinates of each point on the contour map of the area to be worked on are determined. The contour map of the area to be worked on is preprocessed to simplify and smooth the complex contours in the contour map of the area to be worked on. Construct a multi-objective global optimization cost function; Based on the outline map of the area to be worked on, generate the Headlands boundary; Based on the multi-objective global optimization cost function and the Headlands boundary, the work area enclosed in the outline map of the work area to be worked is divided into parallel line bundles, or the work area enclosed in the outline map of the work area to be worked is divided into parallel line bundles according to a custom parallel line angle, and the parallel line bundles are processed according to the multi-objective global optimization cost function to generate a parallel line distribution with the optimal angle. Define the starting point of the parallel wire bundles, and sort the parallel wire bundles according to a preset heuristic method; Obtain the preset minimum turning radius of the mobile machinery, connect the parallel wire harnesses using Dubins curves or Reeds-Sheep curves, complete the full-coverage path planning, and generate a reference path.

4. The motion control method for a tracked mobile machine according to claim 1, characterized in that, An arc is constructed between the moving machine and the pre-aiming point as the real-time tracking path for the moving machine, specifically as follows: The angle between the moving machine and the pre-aiming point is obtained. Based on the angle, the pre-aiming distance, the current position of the moving machine, and the pre-aiming point, the radius of curvature of the real-time tracking path of the moving machine and the radius of curvature of the real-time tracking path of the moving machine are solved using a sine theorem calculation model. The sine theorem calculation model is as follows: , ,in, The angle between the moving machine and the pre-aiming point. The aiming distance is... The radius of the arc for the real-time tracking path of the mobile machinery. The radius of curvature of the real-time tracking path of the mobile machinery; The real-time tracking path of the mobile machinery is constructed using the radius of the arc of the real-time tracking path.

5. The motion control method for a tracked mobile machine according to claim 4, characterized in that, The tracking speed of the mobile machinery is obtained, and the speeds of the left and right tracks of the mobile machinery are controlled based on the tracking speed and the real-time tracking path to ensure that the mobile machinery always travels along the reference path. Specifically: To obtain the tracking speed of the moving machine, according to the formula... The tracking speed of the mobile machinery, the radius of the arc of the real-time tracking path of the mobile machinery, and the lateral error between the position of the mobile machinery and the pre-aiming point are processed to solve for the angular velocity of the mobile machinery about the center of the real-time tracking path. Let be the angular velocity of the moving machine about the center of the real-time tracking path. The tracking speed of the moving machinery, The lateral error between the position of the moving machinery and the pre-aiming point; According to the formula for straight travel of mobile machinery Formula for left turn of mobile machinery , Formula for right turn of mobile machinery , The angular velocity of the mobile machinery around the center of the real-time tracking path, the radius of the arc of the real-time tracking path, and the half-length of the vehicle width of the mobile machinery are calculated to determine the speeds of the left and right tracks of the mobile machinery. For the speed of the left track of the mobile machinery, For the right track speed of the mobile machinery, The width and length of the mobile machinery are half of the vehicle's width.

6. A motion control device for tracked mobile machinery, characterized in that, include: The data management unit is used to acquire multiple path points transmitted by the RTK positioning device, generate a reference path, and set the pre-aiming distance; The aiming point search unit is used to obtain the current position of the mobile machinery, filter out the path point in the reference path whose distance from the current position of the mobile machinery is closest to the aiming distance, and set the path point as the aiming point. Specifically: Using the pre-aiming distance as the search radius, according to the linear equation and formula Solve for the next and previous points in the sequence of the reference path, and the distance between the current position of the moving machinery and the equation of the straight line. ,in, The current position of the mobile machinery. The location of the previous path point. The location of the next path point. Let be the coefficients of the equation of the line. The distance between the current position of the moving machinery and the equation of the straight line; When it is determined that the search radius is less than the distance between the current position of the mobile machinery and the straight line equation, a search failure signal is generated, and the search for the pre-aiming point is restarted; When it is determined that the search radius is equal to the distance between the current position of the mobile machinery and the straight line equation, the aiming point is the projection point of the current position of the mobile machinery onto the straight line equation; When it is determined that the search radius is greater than the distance between the current position of the mobile machinery and the straight line equation, a circle is drawn with the current position of the mobile machinery as the center and the search radius as the radius. The intersection point of the circle and the straight line equation is calculated. The intersection point with the same direction as the direction of travel of the mobile machinery is the pre-aiming point. The tracking path generation unit is used to construct an arc between the mobile machinery and the pre-aiming point as the real-time tracking path of the mobile machinery. The motion control unit is used to acquire the tracking speed of the mobile machinery, and control the speed of the left and right tracks of the mobile machinery based on the tracking speed and the real-time tracking path, so that the mobile machinery always travels along the reference path.

7. A motion control device for tracked mobile machinery, characterized in that, The system includes a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor, wherein the processor, when executing the computer program, implements a motion control method for a tracked mobile machine as described in any one of claims 1 to 5.

8. A readable storage medium, characterized in that, The device contains a computer program that can be executed by a processor of the device where the storage medium is located, to implement a motion control method for a tracked mobile machine as described in any one of claims 1 to 5.

Images