Tracking control method of double-track robot

Abstract

The invention relates to the technical field of robots, in particular to a tracking control method for a double-track robot, and the method comprises the steps: calculating the transverse distance deviation and the course angle deviation of the robot through the positioning data of the current robot; and selecting different control strategies according to the transverse distance deviation and the course angle deviation. And when the robot is in a linear driving state, a double-ring PID regulator is called to carry out speed real-time control. If the triggering course deviation is too large or the transverse deviation distance is too large, corresponding control strategies are selected for adjustment. The above process is repeated until the track tracking of all the routes is completed. The adaptive capacity of the robot is enhanced especially for orchards, fields and other complex environments with non-pavement pavements, and compared with other complex control algorithms, the control strategy is simplified, the calculation complexity of the system is reduced, and rapid commercial use is facilitated.

Description

Technical Field

[0001] The present invention relates to the field of robot technology, and in particular to a tracking control method of a dual-track robot. Background Art

[0002] In the field of robot control, the dual-track chassis has unique advantages in application scenarios such as orchards and fields due to its good off-road performance and stability. However, there are many challenges in achieving high-precision tracking control of dual-track robots.

[0003] At present, there are many control algorithms that can be used for dual-track chassis, including fuzzy logic control, sliding mode control, adaptive control, model predictive control, reinforcement learning control, and neural network control. These control methods have their own characteristics, but they also have some limitations in practical applications. For example, although fuzzy logic control handles uncertainty and fuzziness well, it relies on expert knowledge and has complex rule settings; sliding mode control performs well in dealing with nonlinear systems and is robust, but it may produce chattering, affecting the control effect. In addition, although adaptive control, model predictive control, reinforcement learning control, and neural network control methods have high accuracy and adaptability in theory, they are still in the laboratory research stage and have not yet become large-scale methods adopted in the industry. The main reason is that their algorithms are complex and difficult to commercialize quickly.

[0004] In view of the control requirements of the dual-track chassis in practical applications, especially considering the characteristics of complex unpaved roads such as orchards and fields, the above control method is difficult to quickly deploy and achieve the expected control effect in a short time. Specifically, the terrain of orchards, fields and other environments is complex and changeable, which puts higher requirements on the tracking accuracy and stability of the robot. Therefore, in order to solve the problem of high-precision tracking control of dual-track robots on unpaved roads such as orchards and fields, it is necessary to invent a control method that is both fast and effective and easy to commercialize. Summary of the invention

[0005] The purpose of the present invention is to provide a tracking control method for a dual-track robot, which solves the problem that in the actual application of the dual-track chassis in complex unpaved roads such as orchards and fields, the existing control methods are difficult to quickly deploy and achieve the expected control effect in a short time.

[0006] To achieve the above object, the present invention provides a tracking control method for a dual-track robot, comprising the following steps:

[0007] Calculate the lateral distance deviation between the robot and the current route segment, as well as the angular deviation between the robot's current heading and the heading of the route segment, using the current robot positioning data.

[0008] Different control strategies can be selected through lateral distance deviation and heading angle deviation;

[0009] When the robot is in a straight-line driving state, the dual-loop PID regulator is called to perform real-time speed control;

[0010] When driving in a straight line, if the triggered heading deviation is too large or the lateral deviation distance is too large, the corresponding control strategy will be selected for adjustment, and this cycle will be repeated until the trajectory tracking of the entire route is completed.

[0011] Wherein, by using the current robot positioning data, the lateral distance deviation between the robot and the current route segment, and the angular deviation between the current heading of the robot and the heading of the route segment are calculated, and the steps further include:

[0012] Defined as positive on the left side of the route, negative on the right side of the route, and zero on the route;

[0013] Assume that the current robot position is C and the route segment is AB, that is, the lateral distance deviation is calculated as:

[0014] AB=dist(A,B),AC=dist(A,C),BC=dist(B,C)

[0015]

[0016] Among them, dist(X, Y) represents the distance between two points X and Y, Area represents the area of ​​triangle ABC, and h represents the height through point C.

[0017] Wherein, by using the current robot positioning data, the lateral distance deviation between the robot and the current route segment, and the angular deviation between the current heading of the robot and the heading of the route segment are calculated, and the steps further include:

[0018] Map the value range of the angle deviation to the interval [-180, 180].

[0019] Wherein, different control strategies are selected according to the lateral distance deviation and the heading angle deviation, and the steps further include:

[0020] The heading angle deviation is processed first, and then the lateral distance deviation.

[0021] Wherein, different control strategies are selected according to the lateral distance deviation and the heading angle deviation, and the steps further include:

[0022] If the robot's heading angle deviation exceeds 45 degrees, the robot stops and rotates in place to adjust the heading until the heading angle deviation is less than 5 degrees, and the adjustment is completed.

[0023] Wherein, different control strategies are selected according to the lateral distance deviation and the heading angle deviation, and the steps further include:

[0024] If the lateral deviation of the robot exceeds 0.4 meters, the outer track of the robot will be sent a much higher speed than the inner track, so that the robot moves closer to the route direction until the lateral deviation is less than 0.1 meters, and the adjustment is completed.

[0025] Wherein, when the robot is in a straight-line driving state, a dual-loop PID regulator is called to perform real-time speed control, and the steps further include:

[0026] The lateral distance deviation is input into the first-stage PID regulator, and the obtained output is inverted as the expectation of the second-stage PID regulator;

[0027] The heading deviation is input into the secondary PID regulator, and the output obtained is threshold-limited, and the result is used as the angular velocity of the robot;

[0028] The angular velocity is converted into the linear velocity of the dual tracks and superimposed on the fixed linear velocity of the robot to obtain the final left and right track speeds.

[0029] A tracking control method for a dual-track robot of the present invention uses the current robot positioning data to calculate the lateral distance deviation between the robot and the current route segment, as well as the angular deviation between the robot's current heading and the heading of the route segment. According to the lateral distance deviation and the heading angle deviation, different control strategies are selected, and the heading angle deviation is processed first, and then the lateral distance deviation is processed. When the robot is in a straight-line driving state, the dual-loop PID regulator is called to perform real-time speed control. If the triggered heading deviation is too large or the lateral deviation distance is too large, the corresponding control strategies are selected for adjustment. The above process is repeated until the trajectory tracking of all routes is completed. The integral, differential, differential terms and other parameters of the dual-loop PID are repeatedly and carefully adjusted, and finally a set of parameters with the best control effect is obtained, which not only ensures the rapid response of the entire system, but also ensures the stability of the system, especially for complex environments of non-paved roads such as orchards and fields, the adaptability of the robot is enhanced, compared with other complex control algorithms, the control strategy is simplified, the calculation complexity of the system is reduced, and it is convenient for rapid commercial use. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art are briefly introduced below.

[0031] Figure 1 4 is a flow chart of a tracking control method for a dual-track robot according to a first embodiment of the present invention.

[0032] Figure 2It is a flowchart of the steps of the tracking control method of the dual-track robot according to the first embodiment of the present invention. DETAILED DESCRIPTION

[0033] Embodiments of the present invention are described in detail below. Examples of the embodiments are shown in the accompanying drawings. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present invention, but should not be construed as limiting the present invention.

[0034] The first embodiment of the present application is:

[0035] See also Figure 1 and Figure 2 ,in, Figure 1 4 is a flow chart of a tracking control method for a dual-track robot according to a first embodiment of the present invention. Figure 2 It is a flowchart of the steps of the tracking control method of the dual-track robot according to the first embodiment of the present invention.

[0036] The present invention provides a tracking control method for a dual-track robot, comprising the following steps:

[0037] S101: Calculate the lateral distance deviation between the robot and the current route segment, and the angular deviation between the robot's current heading and the heading of the route segment, based on the current robot positioning data;

[0038] Specifically, the robot is equipped with a GNSS sensor, and the position of the robot is obtained from the GNSS sensor. The lateral distance deviation between the robot and the current route segment is calculated based on the current robot position. This distance needs to take the direction into consideration. It is defined here as positive on the left side of the route, negative on the right side of the route, and zero on the route.

[0039] Assume that the current robot position is C and the route segment is AB, where dist(X, Y) represents the distance between two points X and Y, Area represents the area of ​​triangle ABC, and h represents the height passing through point C, i.e., the lateral distance deviation:

[0040] AB=dist(A,B),AC=dist(A,C),BC=dist(B,C)

[0041]

[0042] The azimuth data provided by the current robot's combined satellite inertial navigation sensor is used to calculate the angular deviation between the robot's current heading and the heading of the route segment it is in, and the value range of this angle difference is mapped to the interval [-180, 180].

[0043] The azimuth angle solution is as follows:

[0044] Bearing=mod(180 / π*arctan2(y,x)+360,360);

[0045] y=sin(Δλ)*cos(□2), x=cos(□1)*sin(□2)-sin(□1)*cos(□2)*cos(Δλ);

[0046] □1 = starting point latitude (radians) = lat1*π / 180, □2 = end point latitude (radians) = lat2*π / 180;

[0047] Δλ = longitude difference (radians) = (lon2-lon1)*π / 180;

[0048] The azimuth calculated by this formula is: with due north as 0° and clockwise as positive, the return value range is [0°, 360°];

[0049] Angle_diff=(azimuth-bearing)>180? Angle_diff-360: (<-180?Angle_diff+360);

[0050] This maps the angle difference to [-180°, 180°].

[0051] S102: Select different control strategies according to the lateral distance deviation and the heading angle deviation;

[0052] Specifically, different control strategies are selected based on the lateral distance deviation and heading angle deviation, with the heading angle deviation being processed first and then the lateral distance deviation:

[0053] Angle control: If the robot's heading angle deviation exceeds 45 degrees, the robot stops and rotates in place to adjust the heading until the heading angle deviation is less than 5 degrees, and the adjustment ends.

[0054] Distance control: If the lateral deviation of the robot exceeds 0.4 meters, the outer track of the robot will be sent a much higher speed than the inner track, so that the robot moves closer to the route until the lateral deviation is less than 0.1 meters, and the adjustment is completed.

[0055] S103: When the robot is in a straight-line driving state, the dual-loop PID regulator is called to perform real-time speed control;

[0056] Specifically, when the robot is in a straight-line driving state, the dual-loop PID regulator will be called to perform real-time speed control. The specific method is to input the lateral distance deviation into the first-level PID regulator, and the output obtained is inverted as the expectation of the second-level PID regulator. At the same time, the heading deviation is input into the second-level PID regulator, and the output obtained is threshold-limited. This result is used as the angular velocity of the robot. At the same time, this angular velocity is converted into the linear velocity of the dual tracks and superimposed on the fixed linear velocity of the robot to obtain the final left and right track speeds.

[0057] Primary PID mathematical model:

[0058] Assume PID (setpoint, feedback), setpoint is the expected value, feedback is the feedback value, error = setpoint – feedback is the current error of the system;

[0059] Integral = error + integral is the system cumulative error;

[0060] Derivative = error – previous_error is the differential of this error;

[0061] Output=Kp*error+Ki*integral+Kd*derivate is PID output;

[0062] Double-loop PID mathematical model:

[0063] Dis_output = PID (0, dis_high), dis_high is the lateral deviation distance;

[0064] Ang_output = PID (-Dis_output, angle_diff), angle_diff is the angle deviation;

[0065] Ang_output>25?25: Ang_output, limit the output of dual-loop PID to no more than 25;

[0066] The purpose of PID control is to make the feedback infinitely close to the setpoint, so we set the setpoint = 0, then the system will make the robot's lateral deviation feedback = 0.

[0067] The formula for a PID (Proportional-Integral-Derivative) controller is a commonly used algorithm in control systems to calculate the output of the controller. A PID controller generates a control signal by calculating the proportional (P), integral (I), and derivative (D) parts of the error, which is the difference between the setpoint and the actual value. Here is the formula for a PID controller:

[0068]

[0069] in:

[0070] u(t) is the output of the controller;

[0071] K p is the proportional gain;

[0072] -e(t) is the error at time t, that is, the difference between the set value and the actual value;

[0073] K i is the integral gain, usually expressed as Where T i is the integration time constant;

[0074] e(t) i dt is the integral of the error, which represents the accumulation of past errors;

[0075] K d is the derivative gain, usually expressed as K d *T d , where T d is the differential time constant;

[0076] is the derivative of the error, which represents the rate of change of the error over time.

[0077] S104: When driving in a straight line, if the triggered heading deviation is too large or the lateral deviation distance is too large, the corresponding control strategies are selected for adjustment respectively, and the process is repeated until the trajectory tracking of the entire route is completed.

[0078] Specifically, when driving in a straight line, if the heading deviation is too large due to road conditions or other reasons, the above-mentioned angle control is entered for adjustment; if the lateral deviation distance is too large, the above-mentioned distance control is entered for adjustment; this cycle is repeated to complete the trajectory tracking of the entire route.

[0079] Using this method to control the dual-track chassis can make the lateral deviation accuracy within 10 cm and the steering accuracy within 5 degrees.

[0080] The integral, differential, and derivative terms of the dual-loop PID were adjusted repeatedly and meticulously, and finally a set of parameters with the best control effect was obtained, which not only ensured the rapid response of the entire system, but also ensured the stability of the system. In particular, the robot's adaptability is enhanced for complex environments such as orchards and fields without paved roads. Compared with other complex control algorithms, the control strategy is simplified, the computational complexity of the system is reduced, and it is easy to use commercially.

[0081] What is disclosed above is only one or more preferred embodiments of the present application, and cannot be used to limit the scope of rights of the present application. Ordinary technicians in this field can understand that all or part of the processes of implementing the above embodiments and equivalent changes made according to the claims of the present application are still within the scope covered by the present application.

Claims

1. A tracking control method for a dual-track robot, characterized in that: The following steps are involved: Calculate the lateral distance deviation between the robot and the current route segment, as well as the angular deviation between the robot's current heading and the heading of the route segment, using the current robot positioning data. Different control strategies can be selected through lateral distance deviation and heading angle deviation; When the robot is in a straight-line driving state, the dual-loop PID regulator is called to perform real-time speed control; When driving in a straight line, if the triggered heading deviation is too large or the lateral deviation distance is too large, the corresponding control strategy will be selected for adjustment, and this cycle will be repeated until the trajectory tracking of the entire route is completed.

2. The tracking control method of a dual-track robot according to claim 1, characterized in that: Calculating the lateral distance deviation between the robot and the current route segment, and the angular deviation between the robot's current heading and the heading of the route segment through the current robot positioning data, the steps further include: Defined as positive on the left side of the route, negative on the right side of the route, and zero on the route; Assume that the current robot position is C and the route segment is AB, that is, the lateral distance deviation is calculated as: AB=dist(A,B),AC=dist(A,C),BC=dist(B,C) Among them, dist(X, Y) represents the distance between two points X and Y, Area represents the area of ​​triangle ABC, and h represents the height through point C.

3. The tracking control method of a dual-track robot according to claim 1, characterized in that: Calculating the lateral distance deviation between the robot and the current route segment, and the angular deviation between the robot's current heading and the heading of the route segment through the current robot positioning data, the steps further include: Map the value range of the angle deviation to the interval [-180, 180].

4. The tracking control method of a dual-track robot according to claim 1, characterized in that: Different control strategies are selected according to the lateral distance deviation and the heading angle deviation, and the steps further include: The heading angle deviation is processed first, and then the lateral distance deviation.

5. The tracking control method of a dual-track robot as claimed in claim 4, characterized in that: Different control strategies are selected according to the lateral distance deviation and the heading angle deviation, and the steps further include: If the robot's heading angle deviation exceeds 45 degrees, the robot stops and rotates in place to adjust the heading until the heading angle deviation is less than 5 degrees, and the adjustment is completed.

6. The tracking control method of a dual-track robot according to claim 4, characterized in that: Different control strategies are selected according to the lateral distance deviation and the heading angle deviation, and the steps further include: If the lateral deviation of the robot exceeds 0.4 meters, the outer track of the robot will be sent a much higher speed than the inner track, so that the robot moves closer to the route direction until the lateral deviation is less than 0.1 meters, and the adjustment is completed.

7. The tracking control method of a dual-track robot according to claim 1, characterized in that: When the robot is in a straight-line driving state, the dual-loop PID regulator is called to perform real-time speed control, and the steps further include: The lateral distance deviation is input into the first-stage PID regulator, and the obtained output is inverted as the expectation of the second-stage PID regulator; The heading deviation is input into the secondary PID regulator, and the output obtained is threshold-limited, and the result is used as the angular velocity of the robot; The angular velocity is converted into the linear velocity of the dual tracks and superimposed on the fixed linear velocity of the robot to obtain the final left and right track speeds.

Images