Intelligent crawler-type mobile machinery fusion motion control tracking method and device

Abstract

The invention provides an intelligent crawler-type mobile machinery fusion motion control tracking method and device, and relates to the technical field of motion control tracking. According to the method, vertex coordinates of an operation area are obtained, and an initial reference path is generated by using a full-coverage path planning algorithm; and a cubic spline interpolation technology is adopted to carry out optimization and smoothing processing on the path. In the aspect of a control strategy, a self-adaptive Pure controller and a model predictive control controller are combined, and the speed and the pose are accurately controlled respectively. The speed and steering information is integrated and output through the fusion controller, and efficient tracking of the crawler-type machine along the planned path is achieved. Through the multi-controller fusion and path optimization technology, the adaptability and operation precision of the crawler-type machinery are effectively improved, the computing resource consumption is reduced, and technical support is provided for wide application of the intelligent crawler-type machinery. The objective of the invention is to improve path tracking precision and system efficiency in a complex environment.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of motion control tracking, in particular to a kind of intelligent tracked mobile machinery fusion motion control tracking method and device. BACKGROUND

[0002] In modern agriculture, industry and special operation field, tracked mobile machinery because of its excellent ground adhesion performance and obstacle crossing ability, has become an indispensable important equipment.For example, in agriculture, tracked machinery can effectively deal with complex and changeable terrain, realize efficient operation;In the field of mining and construction, its strong passability enables it to complete transportation and construction tasks in harsh environments;In the field of military and disaster relief, the high adaptability of tracked machinery is the key factor to ensure the smooth progress of the task.

[0003] However, with the continuous expansion of application scenarios and the increasing requirements of technology, the existing motion control technology of tracked mobile machinery gradually exposes some problems to be solved.On the one hand, the traditional path tracking method mainly relies on simple control algorithm, lacks real-time perception and dynamic adjustment ability to complex environment.This leads to the fact that when facing complex terrain and dynamic environment, tracked machinery is easy to deviate from the planned path, and the path tracking accuracy is difficult to meet the high-precision operation demand.For example, in precision agriculture, the accuracy requirement of operation path is very high, and the traditional method often cannot achieve ideal operation effect.

[0004] On the other hand, the existing control system mostly adopts single control strategy, such as simple PID control or control based on preset mode.This single control mode cannot effectively integrate multiple sensor data and complex environmental information, resulting in slow response or insufficient stability of the system when facing complex tasks.For example, in complex environments such as mines, tracked machinery needs to be dynamically adjusted according to real-time terrain and obstacle information, and single control strategy is difficult to realize such flexible control demand.

[0005] In addition, the traditional control system also has a big problem in resource consumption.Due to the lack of efficient optimization mechanism, the system often needs to consume a large amount of computing resources in the running process, which not only reduces the running efficiency of the system, but also increases the energy consumption and hardware cost of the equipment.Therefore, developing an intelligent motion control tracking method that can realize high-precision path tracking, multi-source information fusion and efficient resource utilization is of great significance to improve the operation performance and adaptability of tracked mobile machinery.

[0006] In view of this, the present application is proposed. SUMMARY

[0007] The present application provides an intelligent tracked mobile machinery fusion motion control tracking method and device, which can at least partially improve the above problems.

[0008] To achieve the above object, the present application adopts the following technical solutions: The intelligent tracked mobile machine fusion motion control tracking method comprises: Obtaining coordinate data in a work area, performing full coverage path planning processing on the coordinate data in the work area, generating a snake-shaped full coverage planning path, and performing interpolation smoothing processing on the snake-shaped full coverage planning path by using a cubic spline interpolation algorithm; Obtaining pose information of the tracked mobile machine, preprocessing the pose information based on the snake-shaped full coverage planning path, and distributing the obtained speed information to an adaptive Pure Pursuit controller for speed control and distributing the pose information to an MPC controller for pose control; The fusion controller is used to combine the speed information and steering information after speed control and pose control processing, and output fusion control instructions to the tracked mobile machine; The tracked mobile machine is controlled to track the snake-shaped full coverage planning path according to the fusion control instructions.

[0009] The present application also provides an intelligent tracked mobile machine fusion motion control tracking device, which comprises: A path planning unit is configured to obtain coordinate data in a work area, perform full coverage path planning processing on the coordinate data in the work area, generate a snake-shaped full coverage planning path, and perform interpolation smoothing processing on the snake-shaped full coverage planning path by using a cubic spline interpolation algorithm; A preprocessing unit is configured to obtain pose information of the tracked mobile machine, preprocess the pose information based on the snake-shaped full coverage planning path, and distribute the obtained speed information to an adaptive Pure Pursuit controller for speed control and distribute the pose information to an MPC controller for pose control; A merging unit is configured to use a fusion controller to combine the speed information and steering information after speed control and pose control processing, and output fusion control instructions to the tracked mobile machine; A tracking unit is configured to control the tracked mobile machine to track the snake-shaped full coverage planning path according to the fusion control instructions.

[0010] In summary, the intelligent tracked mobile machine fusion motion control tracking method aims to solve the problems of insufficient path tracking accuracy, single control system and poor adaptability of existing tracked mobile machines. The method obtains the vertex position coordinates of the working area, generates a reference path using the full coverage path planning algorithm, and uses the cubic spline interpolation method to interpolate and smooth the path, thereby generating an optimized path. In the control process, the current state and path information of the tracked mobile machine are preprocessed, the speed information is assigned to the adaptive Pure Pursuit controller for speed control, and the pose information is assigned to the Model Predict Control (MPC) controller for pose control. The fusion controller combines the processed speed information and steering information to output the control amount, and the tracked mobile machine tracks the full coverage planning path according to the fusion control instruction.

[0011] The present application realizes the cooperative control of speed and pose by fusing adaptive Pure Pursuit and MPC controllers, effectively improving the path tracking accuracy and adaptability of tracked mobile machines in complex environments. Compared with traditional single control methods, this method realizes the fusion control tracking of adaptive Pure Pursuit and MPC, effectively improving the tracking accuracy of tracked agricultural machines during the entire path tracking process; it can better cope with complex terrain and dynamic environment, reduce system computing resource consumption, and improve work efficiency. In addition, through cubic spline interpolation and smoothing processing, the path planning is further optimized to ensure that the tracked machine can complete the work task more smoothly and efficiently. Compared with traditional MPC control, the speed control does not go through the optimization processing of the MPC controller, greatly reducing the system computing resource consumption. This method provides more efficient and accurate technical support for the unmanned driving and work of tracked mobile machines, and has wide application prospects. BRIEF DESCRIPTION OF DRAWINGS

[0012] Figure 1 is a flowchart of the intelligent tracked mobile machine fusion motion control tracking method provided by the first embodiment of the present application; Figure 2 is a schematic diagram of the fusion control system of the intelligent tracked mobile machine fusion motion control tracking method provided by the embodiment of the present application; Figure 3 is a module schematic diagram of the intelligent tracked mobile machine fusion motion control tracking device provided by the second embodiment of the present application. DETAILED DESCRIPTION

[0013] In order to make the objects, technical solutions and advantages of the present application clearer, further detailed description will be made to the present application with reference to the embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application, and are not used to limit the present application.

[0014] Referring to Figure 1 , Figure 2 The first embodiment of the present application discloses a fusion motion control tracking method for intelligent tracked mobile machinery, which can be executed by a fusion motion control tracking device (hereinafter referred to as a tracking device) for intelligent tracked mobile machinery, and in particular, by one or more processors in the tracking device to implement the following method: S1, obtaining coordinate data in a work area, performing full coverage path planning processing on the coordinate data in the work area, generating a snake-shaped full coverage planning path, and performing interpolation smoothing processing on the snake-shaped full coverage planning path using a cubic spline interpolation algorithm; Specifically, step S1 further comprises: determining a work area, and obtaining three-dimensional coordinates at the vertex of the convex field work area collected by the vehicle-mounted RTK; calculating the three-dimensional coordinates using a preset offline full coverage path planning algorithm to generate a snake-shaped full coverage planning path, wherein the offline full coverage path planning algorithm requires input from the starting coordinate point and back to the starting coordinate point when three-dimensional coordinates are input, so as to form a complete closed area; The snake-shaped full coverage planning path is stored in a csv format, which includes three-dimensional coordinates and a yaw angle.

[0015] The snake-shaped full coverage planning path is loaded into a cubic spline interpolation algorithm in an offline form, and interpolation smoothing processing is performed on the snake-shaped full coverage planning path; The cubic spline interpolation algorithm is used to interpolate and smooth the X-axis coordinate, Y-axis coordinate and yaw angle, and the interpolated data is filtered using a Savitzky-Golay filter.

[0016] In this embodiment, the work area is first accurately planned. Through the vehicle-mounted RTK (real-time motion) positioning system, three-dimensional coordinate data of the vertex of the work area is obtained. These coordinate data are the basis for subsequent path planning, which can ensure the accuracy and reliability of the path. The high-precision positioning capability of the RTK system provides accurate geographic reference for path planning.

[0017] Subsequently, the preset offline full coverage path planning algorithm is used to process the three-dimensional coordinate data. The algorithm can generate a snake-shaped full coverage planning path, ensuring efficient and no-missing coverage of the tracked mobile machine in the working area. The path planning algorithm starts from the starting coordinate point and finally returns to the starting coordinate point when the input coordinates are input, forming a complete closed area. This closed loop path design can effectively avoid work omission and improve work efficiency. The generated snake-shaped full coverage planning path is stored in csv format, which not only contains the three-dimensional coordinates of the path points, but also contains the yaw angle information. This data format is convenient for subsequent processing and analysis, and also provides rich information support for the motion control of the machine.

[0018] In order to further optimize the smoothness and traceability of the path, a cubic spline interpolation algorithm is used to interpolate and smooth the snake-shaped full coverage planning path. The algorithm interpolates and smooths the X-axis coordinate, Y-axis coordinate and yaw angle of the path point, making the path smoother and facilitating the tracking control of the tracked mobile machine. The intelligent mobile machine moves on the horizontal ground, and the vertical height is ignored. In the interpolation process, a suitable smoothing factor is set to balance the smoothness and tracking accuracy of the path. In the interpolation process, the smoothing factor is set to 21.

[0019] The interpolated data is further filtered by a Savitzky-Golay filter with a window size of 51. This filter can effectively remove noise and abnormal points in the path data, further improving the smoothness and reliability of the path. Through this filtering process, the path can better adapt to the motion characteristics of the machine in actual application, reducing the vibration and control error caused by the non-smooth path.

[0020] Through the above steps, the generated snake-shaped full coverage planning path not only covers the entire working area, but also is smoother and easier to track after interpolation and smoothing. This path planning method combines high-precision positioning data and advanced algorithm processing, effectively improving the working performance and path tracking accuracy of the tracked mobile machine in complex terrain and dynamic environment.

[0021] S2, obtaining the pose information of the tracked mobile machine, preprocessing the pose information based on the snake-shaped full coverage planning path, and distributing the obtained speed information to the adaptive Pure Pursuit controller for speed control, and distributing the pose information to the MPC controller for pose control; Specifically, step S2 further comprises: obtaining the pose information of the tracked mobile machine in real time according to the preset RTK positioning system, and inputting the pose information into the vehicle-mounted computing platform of the tracked mobile machine; Based on the serpentine full coverage planning path after interpolation and smoothing, the target point is judged to determine whether the target point is obtained, whether the target point has been reached, and whether the path information of the serpentine full coverage planning path is complete; When it is determined that the target point has been acquired but not reached, and the path information is complete, the speed information at this time is assigned to the adaptive Pure Pursuit controller, and the posture information is assigned to the MPC controller.

[0022] The adaptive Pure Pursuit controller is used as a speed controller to receive speed information from the vehicle-mounted computing platform, and to determine the adaptive forward-looking distance and calculate the speed. The adaptive forward-looking distance calculation steps are specifically as follows: Calculate the current curvature of the path point using the formula: ,in, a 、 b 、 c is the side length of the triangle formed by three consecutive points on the path, s is the semiperimeter of the triangle formed by three consecutive points, A is the area of ​​the triangle, k Real-time tracking of path curvature for tracked mobile machinery; Determine the basic foresight distance , the speed factor of the adaptive Pure Pursuit controller and the curvature factor of the MPC controller To solve, the formula is: ,in, is the speed impact factor, is the current speed of the crawler mobile machinery, The maximum moving speed that a crawler mobile machine can achieve. is the curvature influence factor, Real-time tracking of the absolute value of path curvature for crawler mobile machinery; Adaptive foresight distance for the adaptive Pure Pursuit controller The calculation formula is: ; Curvature versus expected speed based on real-time tracking path of crawler mobile machinery The calculation formula is: , is the influence coefficient of the curvature of the tracking path; The difference between the current speed and the target speed of the crawler mobile machinery is calculated. When it is judged that the difference is within the preset limit range, the current speed is output. When it is judged that the difference exceeds the limit range, acceleration limit and speed output are performed.

[0023] The MPC controller is used as a pose controller, receives pose information from a vehicle-mounted computing platform, constructs a state transition variable equation of a tracked mobile machine kinematics model, optimizes and solves the equation, and extracts angular velocity; The tracked mobile machine kinematics model is constructed based on a differential model, and the formula is: wherein, is the lateral displacement of the tracked mobile machine, is the longitudinal displacement of the tracked mobile machine, is the steering angle of the tracked mobile machine, is the angular velocity of the tracked mobile machine; The state transition variable equation of the tracked mobile machine is constructed based on the kinematics model, and the state of the machine at k+1 is predicted based on the state of the machine at k, and the formula is: wherein, is the x-axis position of the state variable transition equation at k+1, is the x-axis position of the state variable transition equation at k, is the y-axis position of the state variable transition equation at k+1, is the y-axis position of the state variable transition equation at k, is the speed of the tracked mobile machine at k, is the heading angle at k+1, is the heading angle at k, is the angular velocity of the tracked mobile machine at k, is the lateral error at k+1, is the heading error at k+1, is a reference trajectory function, is the angle error between the current heading angle and the tangent direction of the reference trajectory, is the expected heading angle at k; The pose controller is optimized and solved, including the generation of a reference trajectory, the construction of a cost function, the setting of constraint conditions, and the solution of a quadratic programming; wherein, the cost function is a function constructed for the lateral error, the heading error, the angular velocity, the acceleration, the angular velocity change rate, and the acceleration change rate during the movement of the tracked mobile machine, and the formula is: ; wherein, is the weight coefficient of the lateral error, is the weight coefficient of the heading error, is the weight coefficient of the angular velocity, is the weight coefficient of the acceleration, is the weight coefficient of the angular velocity change rate, is the weight coefficient of the acceleration change rate, is the MPC prediction time domain length, is the lateral error at time t, is the expected lateral error, e is the expected heading error, is the angular velocity at time t, is the acceleration at time t+1, is the acceleration at time t; The quadratic programming solves the optimal steering control quantity and outputs the control quantity.

[0024] In this embodiment, the position and posture information of the crawler mobile machinery is obtained in real time through a preset RTK positioning system. The RTK positioning system can provide high-precision positioning data to ensure the precise positioning of the machinery in complex environments. These position and posture information are then input into the on-board computing platform of the crawler mobile machinery to provide data support for subsequent control decisions. Based on the serpentine full coverage planning path after interpolation and smoothing, the target point is judged and processed. This process includes judging whether the target point has been acquired, whether the target point has been reached, and whether the path information of the serpentine full coverage planning path is complete. This judgment mechanism can ensure that the machine always maintains accurate identification and tracking of the target point during the path tracking process, avoiding operation interruption or deviation due to incomplete path information or loss of target points.

[0025] If the target point is determined to have been acquired but not reached, and the path information is complete, the speed information is assigned to the adaptive Pure Pursuit controller for speed control, while the posture information is assigned to the MPC (Model Predictive Control) controller for posture control. This fusion controller combines control via the adaptive Pure Pursuit controller and the MPC controller. This clearly defined control strategy leverages the strengths of both controllers to achieve coordinated control of speed and posture.

[0026] The adaptive Pure Pursuit controller, acting as a speed controller, receives speed information from the onboard computing platform and performs adaptive foresight distance determination and speed calculations. First, it calculates the curvature of the path point. Using a specific formula, it calculates the side length, semi-perimeter, and area of ​​the triangle formed by three consecutive points on the path. This gives the curvature of the tracked mobile machine's real-time tracking path. This curvature calculation provides an important geometric basis for subsequent speed adjustments.

[0027] Next, the base foresight distance is determined, and the speed factor of the adaptive Pure Pursuit controller and the curvature factor of the MPC controller are solved. Using specific formulas, the speed and curvature factors are calculated, combining the current speed and maximum travel speed of the crawler machine, as well as the absolute value of the curvature of the real-time tracked path. The introduction of these factors enables the controller to dynamically adjust control parameters based on the actual operating status of the machine and path characteristics, improving control adaptability and flexibility.

[0028] The adaptive Pure Pursuit controller further calculates the adaptive look-ahead distance and the desired speed based on the curvature of the tracked mobile machine's real-time tracking path. A specific formula, combined with the influence coefficient of the tracking path's curvature, calculates the desired speed. This process fully considers the impact of the path's geometric characteristics on speed, ensuring that the machine maintains a reasonable speed on paths of varying curvature, thereby improving path tracking accuracy and stability.

[0029] Finally, the difference between the crawler machine's current speed and the target speed is calculated. If the difference is within a preset limit, the current speed is output directly. If the difference exceeds the limit, acceleration is limited and the speed output is applied. This acceleration limiting mechanism effectively prevents excessive acceleration during speed adjustment, thereby reducing vibration and shock, and improving the machine's service life and operational stability.

[0030] Simultaneously, the MPC controller, acting as a posture controller, receives posture information from the onboard computing platform and constructs and optimizes the state transition variable equations for the tracked mobile machine's kinematic model, extracting angular velocity. This kinematic model is constructed based on a differential velocity model, using specific formulas to describe the relationships between the machine's lateral displacement, longitudinal displacement, steering angle, and angular velocity. This kinematic model accurately reflects the machine's motion characteristics and provides a theoretical foundation for posture control.

[0031] Based on the kinematic model, a state-variable transfer equation for a tracked mobile machine is constructed. The machine's state at time k is used to predict its state at time k+1. Using a specific formula, the state-variable transfer equation at time k+1 is calculated to determine the x-axis position, y-axis position, heading angle, lateral error, and heading error. This state prediction method can predict the machine's motion trends in advance, providing accurate input data for optimization solutions.

[0032] The pose controller is optimized, including the generation of the reference trajectory, the construction of the cost function, the setting of the constraint conditions, and the quadratic programming solving. Among them, the cost function comprehensively considers the lateral error, the heading error, the angular velocity, the acceleration, the angular velocity change rate, and the acceleration change rate during the movement of the tracked mobile machine, and assigns a corresponding weight coefficient to each factor. The cost function is constructed through a specific formula, so that the controller can find the optimal control strategy in multi-objective optimization.

[0033] Finally, the optimal steering control amount is solved by quadratic programming, and the control amount is output. This optimization solving process can comprehensively consider the kinematic characteristics of the machine and the path planning requirements, generate optimal pose control instructions, and thus realize accurate tracking of the machine on a complex path.

[0034] S3, using a fusion controller to merge the speed information and steering information processed by the speed control and pose control, and outputting a fusion control instruction to the tracked mobile machine; Specifically, step S3 further includes: using a fusion controller to optimize the speed information and steering information processed by the speed control and pose control to obtain a final speed control signal and a steering control signal; The optimized speed control signal and steering control signal are merged to output a unified control amount, and a fusion control instruction is obtained, thereby ensuring smooth operation of the intelligent mobile machine in a complex environment.

[0035] In this embodiment, the fusion controller receives speed information and steering information processed from speed control and pose control. These information has certain adaptability and accuracy after being processed by adaptive Pure Pursuit controller and MPC controller. The adaptive Pure Pursuit controller calculates reasonable speed instructions according to the curvature of the path and the speed state of the machine; and the MPC controller optimizes accurate steering instructions according to the pose information and kinematic model of the machine. The combination of these two parts of information provides a comprehensive control basis for the fusion controller.

[0036] The fusion controller optimizes these information. The optimization process considers various operating requirements of the tracked mobile machine in complex environments, such as terrain changes, dynamic obstacles, etc. Through comprehensive analysis of the speed and steering information, the fusion controller can adjust the weight of each control signal to adapt to different working scenarios. For example, in complex terrain, more stable steering control may be needed to maintain the balance of the machine; while in flat terrain, the weight of speed control can be appropriately increased to improve work efficiency. This optimization process can effectively improve the adaptability and stability of the machine in complex environments, and reduce the instability that may occur due to the dominance of a single control signal.

[0037] After optimization, the fusion controller combines the optimized speed control signal and steering control signal. This process is not simply a signal superposition, but through a complex algorithm, the two signals are integrated into a unified control quantity. This unified control quantity contains all the necessary information of speed and steering, which can be directly recognized and executed by the control system of the tracked mobile machine. This merging method avoids conflicts and interference between multiple control signals, ensures the clarity and consistency of control instructions, and further improves the operation efficiency and reliability of the machine.

[0038] Finally, the fusion controller outputs the fusion control instruction to the tracked mobile machine. This fusion control instruction is a unified instruction that integrates speed and steering information, which can guide the tracked mobile machine to run smoothly in complex environments. After receiving the fusion control instruction, the internal control system of the tracked mobile machine adjusts the drive system and steering mechanism of the track according to the instruction, so that the motion trajectory of the machine is consistent with the planned path as much as possible. This fusion control method not only improves the accuracy of path tracking, but also enhances the stability and adaptability of the machine in complex environments, providing strong technical support for the intelligent operation of the tracked mobile machine.

[0039] S4, controlling the tracked mobile machine to track the serpentine full coverage planning path according to the fusion control instruction.

[0040] Specifically, in this embodiment, after receiving the fusion control instruction output by the fusion controller, the internal control system of the tracked mobile machine accurately tracks the full coverage planning path. The fusion control instruction contains the optimized speed control signal and steering control signal, which are processed by the fusion controller to ensure smooth operation of the machine in complex environments. In this way, the machine can accurately adjust its motion state according to real-time path planning and control instructions, so as to efficiently and stably travel along the serpentine full coverage planning path.

[0041] The serpentine full coverage planning path is carefully designed, taking into account factors such as the number of parallel paths, work coverage rate, and total length of work path, which can ensure efficient and non-missing coverage of the tracked mobile machine in the work area, ensuring high work coverage rate and low repetition rate. This path planning method combines the topographic features and work requirements of the work area, and is generated by a full coverage path planning algorithm, and is further smoothed by three times of spline interpolation and smoothing, further improving the smoothness and traceability of the path. The tracked mobile machine travels along such a path, which can effectively avoid work omission, improve work efficiency, and reduce mechanical vibration and control errors caused by uneven paths.

[0042] During path tracking, the tracked mobile machine utilizes its internal control system and sensors to obtain its pose information in real-time and compares it with the planned path. Based on the current position deviation, the control system calculates the required adjustment amount and achieves path tracking by controlling the drive system and steering mechanism of the tracked belt. The speed and steering information provided by the fusion controller play an important role in this process, as they provide precise instructions for the machine's motion control, ensuring that the machine's motion trajectory is as consistent as possible with the planned path.

[0043] In simple terms, after receiving the fusion control instructions, the tracked mobile machine uses its motion control system to obtain the current position and attitude information using the RTK sensor, and compares it with the planned path in real-time. Based on the current position deviation, the control system calculates the required adjustment amount and achieves path tracking by controlling the drive system and steering mechanism of the tracked belt. In this process, the speed and steering information provided by the fusion controller are used to adjust the motion speed and steering angle of the tracked belt, ensuring that the machine's motion trajectory is as consistent as possible with the planned path.

[0044] This path tracking method has significant benefits. First, it effectively improves the path tracking accuracy of the tracked mobile machine, making its operation in complex terrain and dynamic environments more stable and reliable. Second, through the optimization and adjustment of fusion control instructions, the machine can better adapt to different working scenarios, improving work efficiency and quality. In addition, the design of the snake-shaped full-coverage planning path takes into account factors such as work coverage and total path length, ensuring that the machine can efficiently complete the work task while reducing unnecessary energy consumption and resource waste In summary, the intelligent tracked mobile machine fusion motion control tracking method aims to solve the problems of insufficient path tracking accuracy, single control system, and poor adaptability of tracked machines in complex environments in existing technology. This method significantly improves the work performance and intelligence level of tracked machines through a series of innovative control strategies and technical means.

[0045] Specifically, first, this method obtains the vertex position coordinates of the work area through the full-coverage path planning algorithm and generates a snake-shaped full-coverage planning path. This path planning method can ensure that the tracked mobile machine achieves efficient and non-missing coverage in the work area, significantly improving work efficiency. At the same time, the cubic spline interpolation algorithm is used to interpolate and smooth the path, further optimizing the smoothness and traceability of the path, reducing the vibration and control error caused by the non-smooth path during the machine's operation, and improving the stability and accuracy of the work.

[0046] In terms of control strategy, the method introduces a fusion control scheme of adaptive Pure Pursuit controller and Model Predictive Control (MPC) controller. The adaptive Pure Pursuit controller is responsible for speed control, which can dynamically adjust the speed according to the motion state of the machine and the characteristics of the path by real-time calculation of path curvature and adaptive look-ahead distance, ensuring that the machine maintains a reasonable speed on paths of different curvatures, thereby improving the accuracy and stability of path tracking. The MPC controller focuses on pose control, which can accurately control the steering and pose of the tracked machine by constructing the kinematic model and state transition equation of the tracked machine, and combining with the optimization algorithm, so as to maintain a good motion posture in complex environments.

[0047] Through the fusion controller, the method optimizes and combines the signals processed by speed control and pose control, and outputs unified fusion control instructions. This fusion control method not only fully utilizes the advantages of the two controllers, but also avoids the limitations of single control strategy, significantly improving the adaptability and stability of the system. The tracked machine tracks the path along the serpentine full coverage planning path according to the fusion control instructions, which can effectively cope with the changes of complex terrain and dynamic environment, and realize high-precision and high-efficiency operation.

[0048] In short, the key points of the intelligent tracked mobile machine fusion motion control tracking method are to obtain the vertex position coordinates of the working area, to perform full coverage path planning on the working area, and to generate a reference path. The reference path is interpolated and smoothed by using the cubic spline interpolation method. The current tracked mobile machine state and path information are preprocessed. The speed information is assigned to the adaptive Pure Pursuit controller for speed control, and the pose information is assigned to the MPC controller for pose control. The fusion controller combines and outputs the processed speed information and steering information. The tracked mobile machine receives the fusion control instructions and tracks the full coverage planning path. It realizes intelligent tracked mobile machine fusion motion control, solves the problems of insufficient path tracking accuracy of tracked mobile machines, single control system and poor adaptability, and lays a certain technical foundation for unmanned driving and operation of tracked mobile machines.

[0049] Compared with the prior art, the method has significant benefits. Through path planning optimization and multi-controller fusion control, the path tracking accuracy of the tracked machine is greatly improved, which can maintain a stable running state in complex environments. At the same time, the optimized control strategy effectively reduces the consumption of system computing resources, improves the response speed and running efficiency of the system. In addition, the method also provides a solid technical support for the unmanned driving and operation of tracked machines, has a wide application prospect, and is especially suitable for complex operation scenes such as agriculture, mining, construction, military and disaster relief.

[0050] Referring to Figure 3 The second embodiment of the present application provides a smart tracked mobile machine fusion motion control tracking device, comprising: A path planning unit 101 is configured to acquire coordinate data in a work area, perform full-coverage path planning processing on the coordinate data in the work area, generate a snake-shaped full-coverage planning path, and perform interpolation smoothing processing on the snake-shaped full-coverage planning path by using a cubic spline interpolation algorithm. A preprocessing unit 102 is configured to acquire pose information of a tracked mobile machine, perform preprocessing on the pose information based on the snake-shaped full-coverage planning path, and distribute the obtained speed information to an adaptive Pure Pursuit controller for speed control and distribute the pose information to an MPC controller for pose control. A merging unit 103 is configured to perform control quantity merging on the speed information and steering information processed by the speed control and the pose control by using a fusion controller, and output a fusion control instruction to the tracked mobile machine. A tracking unit 104 is configured to control the tracked mobile machine to perform path tracking along the snake-shaped full-coverage planning path according to the fusion control instruction.

[0051] The above describes the preferred embodiments of the present application. It should be noted that those skilled in the art can make several improvements and refinements without departing from the principles of the present application, and these improvements and refinements are also considered within the protection scope of the present application.

Claims

1. An intelligent crawler-type mobile machine fusion motion control tracking method, characterized in that: include: Acquire coordinate data within the operating area, perform full coverage path planning processing on the coordinate data within the operating area, generate a serpentine full coverage planning path, and use a cubic spline interpolation algorithm to perform interpolation and smoothing processing on the serpentine full coverage planning path; Acquire the position and posture information of the crawler mobile machine, pre-process the position and posture information based on a serpentine full coverage path planning, and assign the obtained speed information to an adaptive Pure Pursuit controller for speed control, and assign the position and posture information to an MPC controller for position control; A fusion controller is used to combine the speed information and steering information processed by speed control and posture control, and output a fusion control instruction to the crawler mobile machine; The crawler-type mobile machine is controlled to perform path tracking along the serpentine full coverage planned path according to the fusion control instruction.

2. The intelligent crawler-type mobile machine fusion motion control tracking method according to claim 1 is characterized in that: Obtain the coordinate data within the working area, perform full coverage path planning on the coordinate data within the working area, and generate a serpentine full coverage planning path, specifically: Determine the working area and obtain the three-dimensional coordinates of the vertices of the convex field working area collected by the vehicle-mounted RTK; Calculating the three-dimensional coordinates using a preset offline full-coverage path planning algorithm to generate a serpentine full-coverage planned path, wherein the offline full-coverage path planning algorithm, when inputting the three-dimensional coordinates, needs to input from a starting coordinate point and then return to the starting coordinate point to form a complete closed area; The serpentine full coverage planning path is stored in CSV format, which includes three-dimensional coordinates and yaw angles.

3. The intelligent crawler-type mobile machine fusion motion control and tracking method according to claim 1 is characterized in that: The cubic spline interpolation algorithm is used to perform interpolation and smoothing processing on the serpentine full coverage planning path, specifically: The serpentine full coverage planning path is loaded into a cubic spline interpolation algorithm in an offline form, and an interpolation smoothing process is performed on the serpentine full coverage planning path; The cubic spline interpolation algorithm is used to interpolate and smooth the X-axis coordinates, Y-axis coordinates, and yaw angles, and the interpolated data is filtered using a Savitzky-Golay filter.

4. The intelligent crawler-type mobile machine fusion motion control and tracking method according to claim 1 is characterized in that: The position and posture information of the crawler mobile machine is obtained, and the position and posture information is preprocessed based on the serpentine full coverage planning path, specifically: Acquire the position and posture information of the crawler mobile machine in real time according to a preset RTK positioning system, and input the position and posture information into the on-board computing platform of the crawler mobile machine; Based on the serpentine full coverage planning path after interpolation and smoothing, the target point is judged to determine whether the target point is obtained, whether the target point has been reached, and whether the path information of the serpentine full coverage planning path is complete; When it is determined that the target point has been acquired but not reached, and the path information is complete, the speed information at this time is assigned to the adaptive Pure Pursuit controller, and the posture information is assigned to the MPC controller.

5. The intelligent crawler-type mobile machine fusion motion control and tracking method according to claim 1 is characterized in that: The obtained speed information is assigned to the adaptive Pure Pursuit controller for speed control, and the posture information is assigned to the MPC controller for posture control, specifically: The adaptive Pure Pursuit controller is used as a speed controller to receive speed information from the vehicle-mounted computing platform, and to determine the adaptive forward-looking distance and calculate the speed. The adaptive forward-looking distance calculation steps are specifically as follows: Calculate the current curvature of the path point using the formula: ,in, a 、 b 、 c is the side length of the triangle formed by three consecutive points on the path, s is the semiperimeter of the triangle formed by three consecutive points, A is the area of ​​the triangle, k Real-time tracking of path curvature for tracked mobile machinery; Determine the basic foresight distance , the speed factor of the adaptive Pure Pursuit controller and the curvature factor of the MPC controller To solve, the formula is: ,in, is the speed impact factor, is the current speed of the crawler mobile machinery, The maximum moving speed that a crawler mobile machine can achieve. is the curvature influence factor, Real-time tracking of the absolute value of path curvature for crawler mobile machinery; Adaptive foresight distance for the adaptive Pure Pursuit controller The calculation formula is: ; Curvature versus expected speed based on real-time tracking path of crawler mobile machinery The calculation formula is: , is the influence coefficient of the curvature of the tracking path; The difference between the current speed and the target speed of the crawler mobile machinery is calculated. When it is judged that the difference is within the preset limit range, the current speed is output. When it is judged that the difference exceeds the limit range, acceleration limit and speed output are performed.

6. The intelligent crawler-type mobile machine fusion motion control and tracking method according to claim 5 is characterized in that: Also includes: The MPC controller is used as a posture controller to receive posture information from the vehicle-mounted computing platform, and to construct and optimize the state transition variable equations and extract the angular velocity of the kinematic model of the crawler mobile machine. The kinematic model of the crawler mobile machinery is constructed based on the differential model, and its formula is: ,in, is the lateral displacement of the crawler mobile machinery, is the longitudinal displacement of the crawler mobile machinery, is the steering angle of the crawler mobile machinery, is the angular velocity of the crawler mobile machinery; Based on the kinematic model, the state transfer variable equation of the crawler mobile machine is constructed, and the mechanical state at time k is used to predict the mechanical state at time k+1. The formula is: ,in, is the x-axis position of the state variable transfer equation at time k+1, is the x-axis position of the state variable transfer equation at time k, is the y-axis position of the state variable transfer equation at time k+1, is the y-axis position of the state variable transfer equation at time k, is the speed of the crawler mobile machine at time k, is the heading angle at time k+1, is the heading angle at time k, is the angular velocity of the crawler mobile machine at time k, is the lateral error at time k+1, is the heading error at time k+1, is the reference trajectory function, is the angle error between the current heading angle and the tangent direction of the reference trajectory, is the expected heading angle at time k; Optimize and solve the posture controller, including the generation of reference trajectory, construction of cost function, setting of constraints and solving quadratic programming; Among them, the cost function is to construct a function of the lateral error, heading error, angular velocity, acceleration, angular velocity change rate, and acceleration change rate during the movement of the crawler mobile machinery. Its formula is: ； in, is the weight coefficient of the lateral error, is the weight coefficient of heading error, is the weight coefficient of angular velocity, is the weight coefficient of acceleration, is the weight coefficient of the angular velocity change rate, is the weight coefficient of the acceleration change rate, is the MPC prediction time domain length, is the lateral error at time t, is the expected lateral error, e is the expected heading error, is the angular velocity at time t, is the acceleration at time t+1, is the acceleration at time t; The quadratic programming solves the optimal steering control quantity and outputs the control quantity.

7. The intelligent crawler-type mobile machine fusion motion control and tracking method according to claim 1 is characterized in that: A fusion controller is used to combine the speed information and steering information processed by speed control and posture control, and output the fusion control instructions to the crawler mobile machine. Specifically: Use the fusion controller to optimize the speed information and steering information processed by speed control and posture control to obtain the final speed control signal and steering control signal; The optimized speed control signal and steering control signal are combined to output a unified control quantity and obtain a fusion control instruction, thereby ensuring the smooth operation of intelligent mobile machinery in complex environments.

8. An intelligent crawler-type mobile mechanical fusion motion control tracking device, characterized in that: include: A path planning unit is used to obtain coordinate data within the working area, perform full coverage path planning processing on the coordinate data within the working area, generate a serpentine full coverage planning path, and use a cubic spline interpolation algorithm to perform interpolation and smoothing processing on the serpentine full coverage planning path; a preprocessing unit for acquiring position and posture information of the crawler mobile machine, preprocessing the position and posture information based on a serpentine full coverage planning path, and assigning the obtained speed information to an adaptive Pure Pursuit controller for speed control, and assigning the position and posture information to an MPC controller for position and posture control; a merging unit for merging the speed information and steering information processed by the speed control and the posture control by using a fusion controller, and outputting a fusion control instruction to the crawler mobile machine; A tracking unit is used to control the crawler-type mobile machinery to perform path tracking along the serpentine full coverage planned path according to the fusion control instruction.

Images