A PID-based track tracking control method and system for crawler walking device

The PID-based track tracking control method and system for crawler walking devices addresses environmental uncertainties by real-time data processing and trajectory correction, enhancing control performance and object pickup accuracy.

CN118884814BActive Publication Date: 2024-12-03CNNC FUJIAN FUQING NUCLEAR POWER
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202411374989.2
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2024-09-30
Publication Date
2024-12-03
Estimated Expiration
2044-09-30

AI Technical Summary

Technical Problem

Existing PID-based crawler walking devices face uncertainties due to environmental changes such as different terrains and friction coefficients, leading to poor trajectory tracking and reduced ability to pick up foreign objects accurately.

Method used

A PID-based track tracking control method and system that includes real-time data acquisition, posture and position data calculation, and trajectory correction in a three-dimensional coordinate system, using modules for attitude deviation, position deviation, and trajectory error calculation to adapt to complex environments.

Benefits of technology

Improves control performance and enhances the probability of accurately tracking trajectories and picking up foreign objects by compensating for posture and position deviations in diverse terrains and load conditions.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118884814B_ABST
    Figure CN118884814B_ABST
Patent Text Reader

Abstract

The present invention relates to the field of trajectory tracking control, and in particular to a PID-based track tracking control method and system for a crawler walking device. The method comprises: obtaining a pre-planned path and a control speed of the crawler walking device; obtaining real-time posture data and real-time position data of the crawler walking device; obtaining a pre-planned path curve according to the pre-planned path, and discretizing the pre-planned path curve into a plurality of target points; calculating a posture deviation value and a position deviation value; calculating a trajectory error value based on the posture deviation value and the position deviation value; and correcting the trajectory of the crawler walking device based on the trajectory error value. The device comprises a data acquisition module, a target point discretization module, a posture deviation value calculation module, a position deviation value calculation module, a trajectory error value calculation module, a trajectory correction module, and a control module. The present invention can effectively improve the control performance, adapt to various complex environmental factors, and improve the probability of picking up foreign objects by a crawler walking device based on PID.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of trajectory tracking control, and in particular to a PID-based track tracking control method and system for a crawler walking device. Background Art

[0002] The crawler walking device is a common mechanical system, which is widely used in various engineering and military fields. In order to achieve accurate and stable trajectory tracking control, it is necessary to install appropriate sensors for the crawler walking device and design appropriate control algorithms; for complex crawler walking device systems, it is even more necessary to establish an accurate mathematical model to help design an effective trajectory tracking control method. However, in actual scenarios, the existing technology often faces environmental changes such as different terrains, friction coefficients and loads. These changes will bring uncertainties to the trajectory tracking control and may lead to a decrease in control performance.

[0003] In nuclear power applications, PID-based crawler walking devices need to accurately pick up foreign objects under various complex environments, friction coefficients, and load changes. However, the PID control method often faces many uncertainties in this context, resulting in poor trajectory tracking effects, trajectory tracking deviations, and inability to accurately pick up foreign objects. Summary of the invention

[0004] The technical problem to be solved by the present invention is to provide a PID-based crawler walking device trajectory tracking control method and system, which can effectively improve the control performance, adapt to various complex environmental factors, and improve the probability of picking up foreign objects by the PID-based crawler walking device.

[0005] The present invention provides a PID-based track tracking control method for a crawler walking device, comprising the following steps:

[0006] Step S1: obtaining a pre-planned path and a control speed of a crawler walking device;

[0007] Step S2: Acquire real-time posture data and real-time position data of the crawler walking device;

[0008] Step S3: according to the pre-planned path, obtaining a pre-planned path curve, and discretizing the pre-planned path curve into a plurality of target points;

[0009] Step S4: Calculate the posture deviation value and the position deviation value;

[0010] Step S5: Calculate the trajectory error value based on the posture deviation value and the position deviation value;

[0011] Step S6: Correct the track of the crawler walking device based on the track error value.

[0012] In a specific embodiment of the present invention, the step S1 further includes establishing a trajectory tracking three-dimensional coordinate system based on the acquired pre-planned path and taking the path starting point as the coordinate origin, wherein the three-dimensional coordinates of the path starting point are .

[0013] In a specific embodiment of the present invention, in step S2, the real-time posture data includes the real-time rotation angle of the crawler walking device, and the rotation angle is obtained by a gyroscope; the real-time position data is the three-dimensional coordinates of the real-time position of the crawler walking device.

[0014] In a specific embodiment of the present invention, the step S3 specifically includes:

[0015] S301: based on n moments, pre-planned path curves and controlled speeds, obtaining real-time position data of the crawler walking device at each moment, wherein the real-time position data is the three-dimensional coordinates of the real-time position of the crawler walking device;

[0016] S302: The time points at n moments are matched one by one with the real-time position data corresponding to the moments, as n target points between the starting point and the end point of the path.

[0017] In a specific embodiment of the present invention, in step S4, the specific steps of calculating the posture deviation value include:

[0018] S401, obtain the three-dimensional coordinates of n target points, where the three-dimensional coordinates of the i-th target point are ;

[0019] S402: Assume that the control speed of the crawler walking device is V, and calculate the direction vector between the i-th target point and the i-1-th target point. , the control update speed calculation formula of the crawler walking device is:

[0020] ,

[0021] in, is the specific force measured by the accelerometer, is the Coriolis acceleration, is the centripetal acceleration about the earth, is the attitude matrix, is the acceleration due to gravity, is the harmful acceleration, [t m-1 ,t m ] time period, integrating the contrast calculation formula, we get:

[0022] ,

[0023] in, and t m With t m-1 The n-system velocity of the epoch body, T = t m- t m-1 The velocity increment caused by the specific force in time, T = t m- t m-1 The velocity increment caused by harmful acceleration in time, where It is expressed as:

[0024] ;

[0025] When the rotation effect and stroke error effect are added, the speed increment The calculation formula is: ,

[0026] in, For rotation error compensation, To compensate for the stroke error, is the velocity increment measured by the accelerometer;

[0027] S403, the crawler walking device rotates at a preset angle at the i-th target point , where atan2() is the inverse tangent function.

[0028] In a specific embodiment of the present invention, the specific step of calculating the posture deviation value in step S4 also includes:

[0029] S404: Using a gyroscope to obtain the real-time rotation angle of the crawler walking device at the i-th target point ;

[0030] S405, the attitude deviation value of the crawler walking device at the i-th target point is:

[0031] .

[0032] In a specific embodiment of the present invention, the specific steps of calculating the position deviation value in step S5 include:

[0033] S501, obtaining the three-dimensional coordinates of the real-time position of the crawler walking device at the i-th moment ;

[0034] S502, the position deviation value of the crawler walking device at the i-th moment

[0035] .

[0036] In a specific embodiment of the present invention, the calculation of the trajectory error value in S6 includes: calculating the trajectory error value of the crawler walking device at the i-th target point ,

[0037] Where b is the coefficient of posture deviation value, and c is the coefficient of position deviation value.

[0038] The present invention also provides a PID-based track tracking control system for a crawler walking device, comprising:

[0039] A data acquisition module is used to obtain the pre-planned path and control speed of the crawler walking device; obtain the real-time posture data and real-time position data of the crawler walking device;

[0040] A target point discretization module is used to obtain a pre-planned path curve according to a pre-planned path, and discretize the pre-planned path curve into multiple target points;

[0041] An attitude deviation value calculation module is used to calculate the attitude deviation value;

[0042] A position deviation value calculation module, used for calculating the position deviation value;

[0043] A trajectory error value calculation module calculates the trajectory error value based on the attitude deviation value and the position deviation value;

[0044] A trajectory correction module, used for correcting the trajectory of the crawler walking device based on the trajectory error value;

[0045] The control module is used to control the operation of each module.

[0046] Compared with the prior art, the PID-based track tracking control method and system of the crawler walking device of the present invention collects posture data and position data in real time, calculates the trajectory error value, and corrects the track of the crawler walking device in a three-dimensional coordinate system, providing precise trajectory tracking control when dealing with different terrains, friction coefficients and load changes. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 A method flow chart showing a PID-based track tracking control method for a crawler walking device according to the present invention;

[0048] Figure 2 The module composition diagram of the PID-based crawler walking device trajectory tracking control system. DETAILED DESCRIPTION

[0049] In order to further understand the present invention, the embodiments of the present invention are described below in conjunction with examples, but it should be understood that these descriptions are only for further illustrating the features and advantages of the present invention, rather than limiting the present invention.

[0050] The embodiment of the present invention discloses a track tracking control method of a crawler walking device based on PID, such as Figure 1 As shown, the following steps are included:

[0051] Step S1: obtaining a pre-planned path and a control speed of a crawler walking device;

[0052] The method also includes establishing a trajectory tracking three-dimensional coordinate system based on the acquired pre-planned path and taking the starting point of the path as the coordinate origin, wherein the three-dimensional coordinates of the starting point of the path are .

[0053] Step S2: Acquire real-time posture data and real-time position data of the crawler walking device;

[0054] The real-time posture data includes the real-time rotation angle of the crawler walking device, and the rotation angle is obtained by a gyroscope; the real-time position data is the three-dimensional coordinates of the real-time position of the crawler walking device.

[0055] Step S3: according to the pre-planned path, obtaining a pre-planned path curve, and discretizing the pre-planned path curve into a plurality of target points;

[0056] Specifically include:

[0057] S301: based on n moments, pre-planned path curves and controlled speeds, obtaining real-time position data of the crawler walking device at each moment, wherein the real-time position data is the three-dimensional coordinates of the real-time position of the crawler walking device;

[0058] S302: The time points at n moments are matched one by one with the real-time position data corresponding to the moments, as n target points between the starting point and the end point of the path.

[0059] Step S4: Calculate the posture deviation value and the position deviation value;

[0060] The specific steps for calculating the attitude deviation value include:

[0061] S401, obtain the three-dimensional coordinates of n target points, where the three-dimensional coordinates of the i-th target point are ;

[0062] S402: Assume that the control speed of the crawler walking device is V, and calculate the direction vector between the i-th target point and the i-1-th target point. , the control update speed calculation formula of the crawler walking device is:

[0063] ,

[0064] in, is the specific force measured by the accelerometer, is the Coriolis acceleration, is the centripetal acceleration about the earth, is the attitude matrix, is the acceleration due to gravity, is the harmful acceleration, [t m-1 ,t m ] time period, integrating the contrast calculation formula, we get:

[0065] ,

[0066] in, and t m With t m-1 The n-system velocity of the epoch body, T = t m- t m-1 The velocity increment caused by the specific force in time, T = t m- t m-1 The velocity increment caused by harmful acceleration in time, where It is expressed as:

[0067] ;

[0068] When the rotation effect and stroke error effect are added, the speed increment The calculation formula is: ,

[0069] in, For rotation error compensation, To compensate for the stroke error, is the velocity increment measured by the accelerometer;

[0070] S403, the crawler walking device rotates at a preset angle at the i-th target point , where atan2() is the inverse tangent function.

[0071] The specific steps of calculating the attitude deviation value also include:

[0072] S404: Using a gyroscope to obtain the real-time rotation angle of the crawler walking device at the i-th target point ;

[0073] S405, the attitude deviation value of the crawler walking device at the i-th target point is:

[0074] .

[0075] Step S5: Calculate the trajectory error value based on the posture deviation value and the position deviation value;

[0076] The specific steps for calculating the position deviation value include:

[0077] S501, obtaining the three-dimensional coordinates of the real-time position of the crawler walking device at the i-th moment ;

[0078] S502, the position deviation value of the crawler walking device at the i-th moment is:

[0079] .

[0080] Step S6: Correct the track of the crawler walking device based on the track error value.

[0081] Calculating the trajectory error value includes: calculating the trajectory error value of the crawler walking device at the i-th target point ,

[0082] Where b is the coefficient of posture deviation value, and c is the coefficient of position deviation value.

[0083] The embodiment of the present invention also discloses a track tracking control system of a crawler walking device based on PID, such as Figure 2 As shown, including:

[0084] A data acquisition module is used to obtain the pre-planned path and control speed of the crawler walking device; obtain the real-time posture data and real-time position data of the crawler walking device;

[0085] A target point discretization module is used to obtain a pre-planned path curve according to a pre-planned path, and discretize the pre-planned path curve into multiple target points;

[0086] An attitude deviation value calculation module is used to calculate the attitude deviation value;

[0087] A position deviation value calculation module, used for calculating the position deviation value;

[0088] A trajectory error value calculation module calculates the trajectory error value based on the attitude deviation value and the position deviation value;

[0089] A trajectory correction module, used for correcting the trajectory of the crawler walking device based on the trajectory error value;

[0090] The control module is used to control the operation of each module.

[0091] The above embodiments are only used to help understand the method and core idea of ​​the present invention. It should be noted that, for those skilled in the art, several improvements and modifications can be made to the present invention without departing from the principles of the present invention, and these improvements and modifications also fall within the scope of protection of the claims of the present invention.

[0092] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A PID-based track tracking control method for a crawler walking device, characterized in that: The following steps are involved: Step S1: obtaining a pre-planned path and a control speed of a crawler walking device; Step S2; Obtain real-time posture data and real-time position data of the crawler walking device; Step S3: according to the pre-planned path, obtaining a pre-planned path curve, and discretizing the pre-planned path curve into a plurality of target points; Step S4: Calculate the posture deviation value and the position deviation value; The specific steps of calculating the attitude deviation value include: using the gyroscope to obtain the real-time rotation angle of the crawler walking device at the i-th target point , then the attitude deviation value of the crawler walking device at the i-th target point is: ; is the preset rotation angle of the i-th target point, The real-time rotation angle at the i-th target point, To control the update speed, is the speed increment; Step S5: Calculate the trajectory error value based on the posture deviation value and the position deviation value; The specific steps for calculating the position deviation value include: S501, obtaining the three-dimensional coordinates of the real-time position of the crawler walking device at the i-th moment ; S502, the position deviation value of the crawler walking device at the i-th moment is: ; Step S6: Correcting the track of the crawler walking device based on the track error value; Calculating the trajectory error value includes: calculating the trajectory error value of the crawler walking device at the i-th target point , Where b is the coefficient of posture deviation value, and c is the coefficient of position deviation value.

2. The PID-based crawler walking device trajectory tracking control method according to claim 1 is characterized in that: The step S1 also includes establishing a trajectory tracking three-dimensional coordinate system based on the acquired pre-planned path and taking the path starting point as the coordinate origin, wherein the three-dimensional coordinates of the path starting point are .

3. The PID-based crawler walking device trajectory tracking control method according to claim 1, characterized in that: In step S2, the real-time posture data includes the real-time rotation angle of the crawler walking device, and the rotation angle is obtained by a gyroscope; the real-time position data is the three-dimensional coordinates of the real-time position of the crawler walking device.

4. The PID-based crawler walking device trajectory tracking control method according to claim 1, characterized in that: The step S3 specifically includes: S301: based on n moments, pre-planned path curves and controlled speeds, obtaining real-time position data of the crawler walking device at each moment, wherein the real-time position data is the three-dimensional coordinates of the real-time position of the crawler walking device; S302: The time points at n moments are matched one by one with the real-time position data corresponding to the moments, as n target points between the starting point and the end point of the path.

5. The PID-based track tracking control method for crawler walking device according to claim 1, characterized in that: In step S4, the specific steps of calculating the posture deviation value also include: S401, obtain the three-dimensional coordinates of n target points, where the three-dimensional coordinates of the i-th target point are ; S402: Assume that the control speed of the crawler walking device is V, and calculate the direction vector between the i-th target point and the i-1-th target point. , the control update speed calculation formula of the crawler walking device is: , in, To control the update speed, is the specific force measured by the accelerometer, is the Coriolis acceleration, is the centripetal acceleration about the earth, is the attitude matrix, is the gravitational acceleration, n is the number of target points, i is a single target point, is the harmful acceleration, [t m-1 ,t m ] time period, integrating the contrast calculation formula, we get: , in, and t m With t m-1 The n-system velocity of the epoch body, T = t m- t m-1 The velocity increment caused by the specific force in time, T = t m- t m-1 The velocity increment caused by harmful acceleration in time, where It is expressed as: ; When the effects of rotation and paddling error are added, the speed increment The calculation formula is: , in, For rotation error compensation, To compensate for the paddling error, is the velocity increment measured by the accelerometer; S403, the crawler walking device rotates at a preset angle at the i-th target point , where atan2() is the inverse tangent function.

6. A track tracking control system for a crawler walking device based on PID, characterized in that: include: A data acquisition module, used to obtain the pre-planned path and control speed of the crawler walking device; Obtain real-time posture data and real-time position data of the crawler walking device; A target point discretization module is used to obtain a pre-planned path curve according to a pre-planned path, and discretize the pre-planned path curve into multiple target points; An attitude deviation value calculation module is used to calculate the attitude deviation value; The specific steps of calculating the attitude deviation value include: using the gyroscope to obtain the real-time rotation angle of the crawler walking device at the i-th target point , then the attitude deviation value of the crawler walking device at the i-th target point is: ; A position deviation value calculation module, used for calculating the position deviation value; The specific steps of calculating the position deviation value include: obtaining the three-dimensional coordinates of the real-time position of the crawler walking device at the i-th moment , then the position deviation value of the crawler walking device at the i-th moment is: ; A trajectory error value calculation module calculates the trajectory error value based on the attitude deviation value and the position deviation value; The trajectory correction module is used to correct the trajectory of the crawler walking device based on the trajectory error value; calculating the trajectory error value includes: calculating the trajectory error value of the crawler walking device at the i-th target point , where b is the coefficient of posture deviation value, and c is the coefficient of position deviation value; The control module is used to control the operation of each module.