Hybrid control method of inspection robot in fixed point edge navigation mode

Abstract

The invention provides a hybrid control method of an inspection robot in a fixed point edge navigation mode. The hybrid control method comprises the following steps: judging the Euclidean distance or orientation deviation of a starting point and a target point, and judging a linear path or a curve path with a constraint term based on two-point interpolation; the transverse error and the course error of the linear path and the current pose are accurately calculated through the Stanley algorithm, the transverse error and the course error are combined to calculate a basic steering angle, the steering angle serves as a basic value, and the angular velocity Vr is finally output; a robot kinematics model and a trajectory tracking error model are firstly constructed for the curve path, hard constraints and soft constraints are added, then a multi-target cost function is constructed, and finally the optimal angular velocity Vr is output. According to the invention, when the robot runs along the preset point side path, accurate in-place between point positions can be efficiently completed, and stable operation can be maintained in a scene with dense equipment and harsh path constraint, so that the reliability and the operation efficiency of inspection operation in the special industry are improved.

Description

Technical Field

[0001] This invention relates to the field of robot path control technology, specifically a hybrid control method for an inspection robot in a fixed-point side navigation mode. Background Technology

[0002] Inspection robots in specialized industries such as subways and substations often employ a "point-edge mode" navigation based on a SLAM-built 2D map. This mode uses a pre-defined robot pose (x, y, r) as "points" and the lines connecting these points as "edges" (i.e., fixed feasible paths). This ensures that the robot does not intrude into lines or cross boundaries during inspections, guaranteeing the safety of the inspected objects. This mode places stringent demands on the efficiency and precision of the robot's point-to-point motion control. Especially when using a four-wheeled, four-rotor chassis, the higher mechanical flexibility and the greater emphasis on control precision and lateral control stability become even more critical. Summary of the Invention

[0003] To address the problems of existing technologies, this invention provides a hybrid control method for inspection robots in fixed-point-edge navigation mode. This method ensures that when the robot travels along a preset point-edge path, it can efficiently and accurately complete the positioning between points, and maintain stable operation in scenarios with dense equipment and strict path constraints, thereby improving the reliability and efficiency of inspection operations in special industries.

[0004] This invention provides a hybrid control method for an inspection robot in a fixed-point side navigation mode, comprising the following steps:

[0005] S1, determine the Euclidean distance and orientation deviation between the starting point and the target point;

[0006] S2 generates a path based on the starting point and the target point, using the Euclidean distance d between the starting point and the target point calculated in S1, as well as the angle difference between the two points. and Determine whether the distance d is greater than a threshold or the angle difference. and angle difference If one of the above judgment conditions is greater than the threshold, then a straight path L with the starting point as the starting point and the target point as the ending point is generated by linear interpolation. The orientation of the starting point and the target point is not considered in path L. Otherwise, a curved path S is generated by cubic Bézier curve interpolation. Path S satisfies the condition that the slope of the tangent at the starting point of the path is consistent with the orientation of the starting point s and the slope of the tangent at the ending point of the path is consistent with the orientation of the target point t. That is, path S considers the orientation of the starting point and the target point.

[0007] S3 utilizes the Stanley algorithm to accurately calculate the lateral and heading errors of the straight path relative to the current pose, and combines these two errors to calculate the basic steering angle. For the control and tracking of the straight path L in S2, the lateral error e between the straight path and the current pose is calculated using the projection distance formula from a point to a line, and then substituted into the Stanley formula. Calculate the robot's expected turning angle In the formula, k is the lateral bias gain used for sensitivity to the lateral bias response. To avoid abrupt changes in the angle when calculating the softening constant at low speeds, Given the current desired linear velocity of the robot, the desired rotation angle is finally calculated. With heading angle error The basic steering angle is calculated by adding them together. ;

[0008] S4, based on the basic steering angle Based on the baseline value, the PID algorithm corrects the remaining deviation and outputs the final angular velocity Vr;

[0009] S5, control and track the curved path S in S2, and construct the robot's kinematics model and trajectory tracking error model;

[0010] For the control and tracking of the curved path S in S2, firstly, the robot kinematic model and trajectory tracking error model are constructed (mainly including five variables: pose x, y, θ, lateral error cte, and heading error epsi). Then, hard constraints (such as maximum linear velocity, maximum angular velocity, etc.) and soft constraints (such as lateral deviation, heading deviation, rate of change of rotation angle, rate of change of linear velocity, etc.) are added respectively. Finally, a multi-objective cost function is constructed (mainly considering tracking accuracy, heading accuracy, minimum control output, and accurate positioning, etc.).

[0011] S6, through feedback correction and rolling execution by the MPC controller, obtains the optimal control quantity for multiple steps in the future, and finally outputs the optimal angular velocity Vr based on the system delay time;

[0012] Further improvements are made to the process of determining the Euclidean distance and orientation deviation between the starting point and the target point in step S1, as follows: Assume the current starting point is s( The target point is t( Then we have:

[0013] The Euclidean distance d between the starting point and the target point is ;

[0014] The angle difference between the orientations of the starting point and the target point and the angle of the line st connecting the starting point and the target point. and .

[0015] Further improvements are made, and the specific process of constructing the robot kinematic model and trajectory tracking error model in step S5 is as follows:

[0016] S5.1, construct the robot kinematics model and trajectory tracking error model, including five variables: pose x, y, θ, lateral error cte, and heading error epsi;

[0017] S5.2, add hard constraints and soft constraints respectively;

[0018] S5.3, construct a multi-objective cost function based on tracking accuracy, heading accuracy, minimum control output, and accurate positioning.

[0019] As a further improvement, the hard constraints in step S5.2 include the maximum linear velocity and the maximum angular velocity.

[0020] Further improvements include, in step S5.2, soft constraints including lateral deviation, heading deviation, rate of change of angle, and rate of change of linear velocity.

[0021] Further improvements include the design of the MPC controller in step S6 based on the kinematic model, trajectory tracking error model, and multi-objective cost function constructed in S5.

[0022] The beneficial effects of this invention are as follows: In complex environments such as subways and substations, it can combine the structural characteristics of a four-wheel drive robot with the strong constraints of fixed-point edge navigation. By integrating the advantages of multiple control strategies, it can ensure that when the robot travels along the preset point-edge path, it can not only efficiently complete the precise positioning between points, but also maintain stable operation in scenarios with dense equipment and strict path constraints, thereby improving the reliability and efficiency of inspection operations in special industries. Attached Figure Description

[0023] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0024] Figure 1 This is a schematic diagram of the process of the present invention. Detailed Implementation

[0025] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0026] See Figure 1 , Figure 1This is a flowchart illustrating an embodiment of a hybrid control method for a four-wheel-drive inspection robot in a fixed-point-side navigation mode provided by the present invention. The embodiment of the present invention provides a hybrid control method for a four-wheel-drive inspection robot in a fixed-point-side navigation mode, including steps S1 to S6:

[0027] S1 determines whether the Euclidean distance between the starting point and the target point is greater than a threshold or whether the orientation deviation is greater than a threshold.

[0028] In this embodiment of the invention, the current starting point s( ) and target point t( Calculate the Euclidean distance d = Then calculate the angle difference between the orientation of the starting point and the target point and the line st connecting the starting point and the target point. and .

[0029] S2 generates a path based on the starting point and the target point.

[0030] In this embodiment of the invention, the distance d calculated based on S1 and the angle difference between the two points are... and Determine whether the distance d is greater than a threshold (e.g., 2m) or the angle difference. and angle difference If either of the above conditions is greater than a threshold (e.g., 0.1 rad), then a straight path L is generated through linear interpolation, starting from the starting point and ending at the target point. Path L does not consider the orientation of the starting and target points. Conversely, if either condition is not met, a curved path S is generated through cubic Bézier curve interpolation. Path S satisfies the following conditions: the slope of the tangent at the starting point is consistent with the orientation of the starting point s, and the slope of the tangent at the ending point is consistent with the orientation of the target point t. In other words, path S considers the orientation of the starting and target points.

[0031] S3 utilizes the Stanley algorithm to accurately calculate the lateral and heading errors of the straight path relative to the current pose, and combines these two errors to calculate the basic steering angle. .

[0032] In this embodiment of the invention, for the control and tracking of the straight path L in S2, the lateral error e between the straight path and the current pose is calculated using the projection distance formula from a point to a line, and then substituted into the Stanley formula. Calculate the robot's expected turning angle In the formula, k is the lateral bias gain used for sensitivity to the lateral bias response. To avoid abrupt changes in the angle when calculating the softening constant at low speeds, Given the current desired linear velocity of the robot, the desired rotation angle is finally calculated. With heading angle error The basic steering angle is calculated by adding them together. ;

[0033] S4, based on the basic steering angle Based on the baseline value, the PID algorithm corrects the remaining deviation and outputs the final angular velocity Vr;

[0034] In this embodiment of the invention, the basic steering angle is calculated based on S3. PID control is used to correct the error, and the final angular velocity Vr is output.

[0035] S5, construct the robot's kinematics model and trajectory tracking error model.

[0036] In this embodiment of the invention, for the control and tracking of the curved path S in S2, firstly, a robot kinematic model and a trajectory tracking error model (mainly including five variables: pose x, y, θ, lateral error cte, and heading error epsi) are constructed. Then, hard constraints (such as maximum linear velocity, maximum angular velocity, etc.) and soft constraints (such as lateral deviation, heading deviation, rate of change of rotation angle, rate of change of linear velocity, etc.) are added respectively. Finally, a multi-objective cost function is constructed (mainly considering tracking accuracy, heading accuracy, minimum control output, and accurate positioning, etc.).

[0037] S6, through feedback correction and rolling execution by the MPC controller, obtains the optimal control quantity for multiple steps in the future, and finally outputs the optimal angular velocity Vr based on the system delay time.

[0038] In this embodiment of the invention, an MPC controller is designed based on the kinematic model, trajectory tracking error model, and multi-objective cost function constructed using S5. Through feedback correction and rolling execution by the controller, the optimal control sequence for multiple future time steps can be obtained. The optimal control sequence is selected based on the system delay, and the optimal angular velocity Vr is output.

[0039] This invention provides a hybrid control method for a four-wheel-drive inspection robot in a fixed-point side navigation mode. The method includes determining whether the Euclidean distance between the starting point and the target point is greater than a threshold, or whether the orientation deviation is greater than a threshold. Based on the determination conditions, the method further interpolates the starting point and the target point into a straight path or a curved path with constraints. For the straight path, the Stanley algorithm is used to accurately calculate the lateral and heading errors between the straight path and the current pose. These errors are combined to calculate the basic steering angle. Using the steering angle as the base value, a PID algorithm corrects the remaining deviation, ultimately outputting the angular velocity Vr. For the curved path, a robot kinematic model and a trajectory tracking error model are first constructed, with hard and soft constraints added. Then, a multi-objective cost function is constructed, including tracking accuracy, heading accuracy, minimum control output, and precise positioning. Finally, through feedback correction and rolling execution by the MPC controller, the optimal control quantity for multiple steps in the future is obtained. Finally, the optimal angular velocity Vr is output based on the system delay time.

[0040] In special and complex inspection environments such as subways and substations, the fixed-point-side navigation hybrid control method of the four-wheel drive inspection robot has shown significant advantages: it not only overcomes the pain points of low operation efficiency and poor operation stability under the traditional control method, but also ensures that when the robot moves along the preset point-side path, it can meet the operation requirements of precise docking between points, and adapt to complex scenarios with dense equipment and strict path constraints, providing effective support for improving the reliability and optimizing the efficiency of inspection operations in special industries.

[0041] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by hardware related to computer program instructions. The program can be stored in any computer-readable storage medium, and when executed, it can include the processes of the embodiments of the above methods. The storage medium can be a magnetic disk, optical disk, read-only memory (ROM), or random access memory (RAM), etc.

[0042] The various embodiments in this specification are described in a progressive manner. Similar or identical parts between embodiments can be referred to interchangeably. Each embodiment focuses on its differences from other embodiments. In particular, for the device embodiments, the above descriptions are merely preferred embodiments of the present invention. Since they are fundamentally similar to the method embodiments, the descriptions are relatively simple, and relevant parts can be referred to the descriptions of the method embodiments. The above descriptions are merely specific embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention, without departing from the principle of the present invention, should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A hybrid control method for a fixed-point edge navigation mode of a patrol robot, characterized by Comprise the following steps: S1, judge the Euclidean distance and the deviation of the starting point, target point; S2, generating a path according to the start point and the target point, the Euclidean distance d of the start point and the target point calculated according to S1 and the angle difference of the two points with , judging whether the distance d is greater than a threshold value or the angle difference with the angle difference one of which is greater than a threshold value, if one of the above judging conditions is met, generating a straight line path L with the start point as the start point and the target point as the end point through linear interpolation, the path L not considering the orientations of the start point and the target point; Conversely, the curve path S is generated by cubic Bezier curve interpolation, and the path S meets the conditions that the tangent slope of the path starting point is consistent with the direction of the starting point s, and the tangent slope of the path ending point is consistent with the direction of the target point t, that is, the path S considers the direction of the starting point and the target point; S3, use Stanley algorithm to accurately calculate the lateral error and heading error of the straight path and the current pose, and combine the two to calculate the basic steering angle ; For the control tracking of the straight line path L in S2, the lateral error e between the straight line path and the current pose is calculated by the projection distance formula from a point to a line, and substituted into the Stanley formula Calculate the desired turning angle of the robot , where k is the lateral deviation gain for the sensitivity of the response to the lateral deviation, is a softening constant to avoid sudden changes in the calculated turning angle when the speed is low, is the current desired linear speed of the robot, and finally the desired turning angle is added to the heading angle error to calculate the basic steering angle ; S4, with base steering angle The remaining deviation is corrected by a PID algorithm with base value, and the final angle speed Vr is output. S5, control tracking is performed on the curve path S in S2, and a robot kinematics model and a trajectory tracking error model are constructed; For the control tracking of the curve path S in S2, firstly, a robot kinematics model and a trajectory tracking error model (mainly including five variables of positions x, y, θ, lateral error cte, and heading error epsi) are constructed, then hard constraints (such as maximum linear velocity and maximum angular velocity) and soft constraints (such as lateral deviation, heading deviation, corner change rate, and linear velocity change rate) are added, and finally a multi-objective cost function (mainly considering tracking accuracy, heading accuracy, minimum control output, and accurate positioning) is constructed. 2.S6, feedback correction and rolling execution through the MPC controller, obtaining multi-step optimal control amount at future time, and finally outputting optimal angular velocity Vr according to system delay time.

3. The hybrid control method of the patrol robot in the fixed-point edge navigation mode according to claim 1, characterized in that: The process of judging the Euclidean distance and the orientation deviation of the starting point and the target point in step S1 is specifically as follows: assuming that the current starting point is s( ), and the target point is t( ), then there is: Euclidean distance d between start point, target point ; The angle difference between the orientation of the start point, the target point and the line st connecting the start point and the target point and .

4. The hybrid control method of the patrol robot in the fixed-point edge navigation mode according to claim 1, characterized in that: The specific process of constructing the robot kinematics model and the trajectory tracking error model in step S5 is as follows: S5.1, construct a robot kinematics model and a trajectory tracking error model, including five variables of positions x, y, θ, lateral error cte, and heading error epsi; S5.2, add hard constraints and soft constraints respectively; S5.3, construct a multi-objective cost function according to tracking accuracy, heading accuracy, minimum control output, and accurate positioning.

5. The hybrid control method of the patrol robot in the fixed-point edge navigation mode according to claim 3, characterized in that: The hard constraints in step S5.2 include maximum linear velocity and maximum angular velocity.

6. The hybrid control method of the patrol robot in the fixed-point edge navigation mode according to claim 3 or 4, characterized in that: The soft constraints in step S5.2 include lateral deviation, heading deviation, corner change rate, and linear velocity change rate.

7. The hybrid control method of the patrol robot in the fixed-point edge navigation mode according to claim 1, characterized in that: The MPC controller in step S6 is designed based on the kinematics model, the trajectory tracking error model, and the multi-objective cost function constructed in S5.

Images