Path tracking method, system and equipment of four-wheel inspection robot and medium

Abstract

The invention discloses a path tracking method, system and device for a four-wheel inspection robot and a medium, and the method comprises the steps: introducing a target point into an RRT path extension algorithm, and obtaining a basic path from a starting point to an ending point; optimizing the basic path to obtain a final path of the four-wheel inspection robot; based on an obstacle avoidance function, taking the minimum collision index and the minimum distance between the position of the four-wheel inspection robot and the final path as targets, and constructing an obstacle avoidance target function; solving the obstacle avoidance target function to obtain front wheel rotation angle information of the four-wheel inspection robot; and controlling the four-wheel inspection robot to advance according to the front wheel rotation angle information of the four-wheel inspection robot. According to the method, the system, the equipment and the medium, the robot can continue to follow a given path to travel after obstacle avoidance.

Description

Technical Field

[0001] The invention belongs to the field of control algorithms and relates to a path tracking method, system, equipment and medium for a four-wheel inspection robot. Background Art

[0002] As an important component of the smart electric field, intelligent inspection robots have been widely used in the power field. The power plant environment is generally a fixed environment with a pre-known map. Therefore, the robot inspection route navigation can be divided into two tasks, namely path planning and path tracking. After planning a feasible path on the map, the robot can complete the navigation task by following the path.

[0003] Inspection robots are mostly four-wheel robots with strong stability. They can carry sensors and mechanical arms and have good adaptability to different road environments. Due to the characteristics of wheeled robots, they mainly track the path by controlling the steering of the front wheels, so it is necessary to establish a model for their movement to achieve tracking control.

[0004] For the above solutions to work smoothly, one of the most basic requirements is that there are no obstacles on the robot's route. However, there are often some obstacles in the field environment that are difficult to predict in advance, so the robot needs to have the ability to avoid obstacles. A widely used solution is to replan the robot's path. The robot uses its own sensors to identify and map the environment, and plans a collision-free path in the real-time environment with obstacles it identifies, so that the robot can safely drive to its destination.

[0005] But one problem with this is that since the robot is constantly replanning its path to avoid obstacles, it will continue to make the best choice of the path to the target point, which makes it difficult for the robot to follow a predetermined basic route. In actual operations, we often hope that the robot can return to the originally planned basic route after avoiding obstacles. In this way, we can design a reasonable path planning algorithm, or even directly give a reasonable robot driving path without planning, so that the robot can follow the path, making the task more reasonable and the process smoother.

[0006] Therefore, a method for achieving obstacle avoidance in the path tracking stage is proposed. When the robot recognizes that it is a certain distance away from an obstacle, it implements obstacle avoidance action. When there is no obstacle or it is beyond a certain distance from the obstacle, it continues to follow the given path and finally reaches the destination. Summary of the invention

[0007] The purpose of the present invention is to overcome the shortcomings of the above-mentioned prior art and provide a path tracking method, system, device and medium for a four-wheel inspection robot. The method, system, device and medium can enable the robot to continue to follow a given path after avoiding obstacles.

[0008] To achieve the above object, the present invention discloses a path tracking method for a four-wheel inspection robot, comprising:

[0009] The target point is introduced into the RRT (Rapidly-exploring RandomTrees) path expansion algorithm to obtain the basic path from the starting point to the end point;

[0010] Optimizing the basic path to obtain the final path of the four-wheel inspection robot;

[0011] Based on the obstacle avoidance function, an obstacle avoidance objective function is constructed with the goal of minimizing the collision index and minimizing the distance between the position of the four-wheel inspection robot and the final path;

[0012] Solving the obstacle avoidance objective function to obtain the front wheel turning angle information of the four-wheel inspection robot;

[0013] The four-wheel inspection robot is controlled to move according to the front wheel turning angle information of the four-wheel inspection robot.

[0014] The path tracking method of the four-wheel inspection robot of the present invention is further improved in that:

[0015] Furthermore, the process of optimizing the basic path to obtain the final path of the four-wheel inspection robot is as follows:

[0016] The basic path is parameterized using Bessel to make the path curvature continuous, and then a maximum curvature constraint is imposed to obtain the final path of the four-wheel inspection robot.

[0017] Furthermore, the obstacle avoidance objective function is:

[0018]

[0019] in, J obs Represents the probability of a collision.

[0020] Furthermore, the obstacle avoidance function is:

[0021]

[0022] The present invention discloses a path tracking system for a four-wheel inspection robot, comprising:

[0023] A calculation module is used to introduce the target point into the RRT path extension algorithm to obtain a basic path from the starting point to the end point;

[0024] An optimization module, used to optimize the basic path to obtain a final path of the four-wheel inspection robot;

[0025] A construction module is used to construct an obstacle avoidance objective function based on an obstacle avoidance function, with the goal of minimizing the collision index and minimizing the distance between the position of the four-wheel inspection robot and the final path;

[0026] A solving module, used to solve the obstacle avoidance objective function and obtain the front wheel turning angle information of the four-wheel inspection robot;

[0027] The control module is used to control the movement of the four-wheel inspection robot according to the front wheel turning angle information of the four-wheel inspection robot.

[0028] The path tracking system of the four-wheel inspection robot of the present invention is further improved in that:

[0029] Furthermore, the process of optimizing the basic path to obtain the final path of the four-wheel inspection robot is as follows:

[0030] The basic path is parameterized using Bessel to make the path curvature continuous, and then a maximum curvature constraint is imposed to obtain the final path of the four-wheel inspection robot.

[0031] Furthermore, the obstacle avoidance objective function is:

[0032]

[0033] in, J obs Represents the probability of a collision.

[0034] Furthermore, the obstacle avoidance function is:

[0035]

[0036] The present invention discloses a computer device, comprising a memory, a processor and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the steps of the path tracking method of the four-wheel inspection robot are implemented.

[0037] The present invention discloses a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the path tracking method of the four-wheel inspection robot are implemented.

[0038] The present invention has the following beneficial effects:

[0039] During specific operation, the path tracking method, system, device and medium of the four-wheel inspection robot described in the present invention are based on an obstacle avoidance function, with the goal of minimizing the collision index and minimizing the distance between the position of the four-wheel inspection robot and the final path, to construct an obstacle avoidance target function, solve the obstacle avoidance target function, and obtain the front wheel turning angle information of the four-wheel inspection robot, so that the four-wheel inspection robot can continue to follow the given path after avoiding obstacles. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] The accompanying drawings constituting a part of the present invention are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the accompanying drawings:

[0041] Figure 1 This is a schematic diagram of the algorithm for path pruning operation;

[0042] Figure 2 The figure is a flow chart of the method of the present invention. DETAILED DESCRIPTION

[0043] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0044] In the description of the present invention, it should be understood that the terms “include” and “comprises” indicate the presence of described features, wholes, steps, operations, elements and / or components, but do not exclude the presence or addition of one or more other features, wholes, steps, operations, elements, components and / or collections thereof.

[0045] It should also be understood that the terms used in the present specification are only for the purpose of describing specific embodiments and are not intended to limit the present invention. As used in the present specification and the appended claims, unless the context clearly indicates otherwise, the singular forms "a", "an" and "the" are intended to include plural forms.

[0046] It should be further understood that the term "and / or" used in the present specification and the appended claims refers to any combination of one or more of the associated listed items and all possible combinations, and includes these combinations. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. In addition, the character " / " in the present invention generally indicates that the associated objects are in an "or" relationship.

[0047] It should be understood that, although the terms first, second, third, etc. may be used to describe preset ranges, etc. in the embodiments of the present invention, these preset ranges should not be limited to these terms. These terms are only used to distinguish preset ranges from each other. For example, without departing from the scope of the embodiments of the present invention, the first preset range may also be referred to as the second preset range, and similarly, the second preset range may also be referred to as the first preset range.

[0048] The word "if" as used herein may be interpreted as "at the time of" or "when" or "in response to determining" or "in response to detecting", depending on the context. Similarly, the phrases "if it is determined" or "if (stated condition or event) is detected" may be interpreted as "when it is determined" or "in response to determining" or "when detecting (stated condition or event)" or "in response to detecting (stated condition or event)", depending on the context.

[0049] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all of the embodiments. The components of the embodiments of the present invention described and shown in the drawings here can usually be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present invention provided in the drawings is not intended to limit the scope of the claimed invention, but merely represents selected embodiments of the present invention. Based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0050] Various structural schematic diagrams of the embodiments disclosed in the present invention are shown in the accompanying drawings. These figures are not drawn to scale, and some details are magnified and some details may be omitted for the purpose of clear expression. The shapes of various regions and layers shown in the figures and the relative sizes and positional relationships therebetween are only exemplary, and may deviate in practice due to manufacturing tolerances or technical limitations, and those skilled in the art may additionally design regions / layers with different shapes, sizes, and relative positions according to actual needs.

[0051] Embodiment 1

[0052] refer to Figure 1 and Figure 2 The path tracking method of the four-wheel inspection robot of the present invention comprises the following steps:

[0053] 1) Improved RRT path planning algorithm

[0054] Considering the problem that the traditional RRT algorithm selects points too randomly in the map space, the target point is introduced into the RRT path expansion algorithm, so that more nodes grow toward the target and the path search speed is accelerated. The formula of the target-oriented RRT tree growth method is obtained as follows:

[0055]

[0056] The growth direction of the node is determined by the random point r and the target point g. The growth direction of the RRT tree is controlled by adjusting the weights of the random amount and the target-oriented amount. The optimal weights of a set of structured environments are obtained through experiments.

[0057] In addition, due to the working characteristics of the inspection robot, in the generation of the path, more attention is often paid to the smoothness and fluency of the path, and the accuracy requirements for the path points are relatively low. Therefore, the path generated by RRT is pruned to reduce redundant nodes.

[0058] Based on this, a basic path from the starting point to the end point is obtained, but the path generated by RRT is a broken line and cannot be directly applied to the robot. It needs to be parameterized. A trajectory with continuous curvature is obtained. Bezier curves are used to parameterize the path, and a maximum curvature constraint is imposed on it to prevent the path from turning beyond the maximum radius of the robot's turn.

[0059] 2) MPC path tracking algorithm with obstacle avoidance

[0060] The four-wheeled robot involved is a front-wheel steering robot, and the lateral load offset and rear wheel changes are not considered. It is assumed that the left and right wheels are subjected to the same force. The basic dynamic model of the robot is established with reference to the single-track model, and the model predictive control algorithm (MPC) is solved in the state space form. The core is to improve the model predictive control algorithm (MPC) by introducing the distance to the obstacle in the objective function. J obs Represents the possibility of collision, and finally we get:

[0061]

[0062] The optimal solution is essentially to minimize the collision index and minimize the position difference between the robot position and the given tracking path. Therefore, the objective function of obstacle avoidance mainly considers the relationship between the distance between the robot and the obstacle and the control increment. The distance between the robot and the obstacle is solved using xy coordinates and reverse, and the following obstacle avoidance function is obtained:

[0063]

[0064] Finally, the objective function with constraints is solved, and the front wheel angle of the robot is used as the output to control the robot to move along the input path. When the robot is far away from the obstacle, the obstacle avoidance objective function has little effect on the objective function of the entire MPC algorithm, and path tracking becomes a general MPC solution problem.

[0065] Embodiment 2

[0066] The path tracking system of the four-wheel inspection robot of the present invention comprises:

[0067] A calculation module is used to introduce the target point into the RRT path extension algorithm to obtain a basic path from the starting point to the end point;

[0068] An optimization module, used to optimize the basic path to obtain a final path of the four-wheel inspection robot;

[0069] A construction module is used to construct an obstacle avoidance objective function based on an obstacle avoidance function, with the goal of minimizing the collision index and minimizing the distance between the position of the four-wheel inspection robot and the final path;

[0070] A solving module, used to solve the obstacle avoidance objective function and obtain the front wheel turning angle information of the four-wheel inspection robot;

[0071] The control module is used to control the movement of the four-wheel inspection robot according to the front wheel turning angle information of the four-wheel inspection robot.

[0072] As an implementation mode of the present invention, the process of optimizing the basic path to obtain the final path of the four-wheel inspection robot is:

[0073] The basic path is parameterized using Bessel to make the path curvature continuous, and then a maximum curvature constraint is imposed to obtain the final path of the four-wheel inspection robot.

[0074] As an implementation mode of the present invention, the obstacle avoidance objective function is:

[0075]

[0076] in, J obs Represents the probability of a collision.

[0077] As an implementation mode of the present invention, the obstacle avoidance function is:

[0078]

[0079] The division of modules in the embodiments of the present application is schematic and is only a logical function division. There may be other division methods in actual implementation. In addition, each functional module in each embodiment of the present application may be integrated into a processor, or may exist physically separately, or two or more modules may be integrated into one module. The above-mentioned integrated modules may be implemented in the form of hardware or in the form of software functional modules.

[0080] Embodiment 3

[0081] A computer device comprises a memory, a processor and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the steps of the path tracking method of the four-wheel inspection robot are implemented, for example, comprising: introducing a target point into an RRT path extension algorithm to obtain a basic path from a starting point to an end point; optimizing the basic path to obtain a final path of the four-wheel inspection robot; constructing an obstacle avoidance target function based on an obstacle avoidance function with the goal of minimizing a collision index and minimizing a distance between a position of the four-wheel inspection robot and the final path; solving the obstacle avoidance target function to obtain front wheel turning angle information of the four-wheel inspection robot; controlling the four-wheel inspection robot to move according to the front wheel turning angle information of the four-wheel inspection robot; the process of optimizing the basic path to obtain the final path of the four-wheel inspection robot is: optimizing parameters of the basic path to obtain a trajectory with continuous curvature; parameterizing the trajectory using Bessel, and then applying a maximum curvature constraint to obtain the final path of the four-wheel inspection robot. The memory may include a memory, such as a high-speed random access memory, and may also include a non-volatile memory, such as at least one disk memory, etc. The processor, network interface, and memory are interconnected through an internal bus, and the internal bus may be an industrial standard architecture bus, a peripheral component interconnection standard bus, an extended industrial standard architecture bus, etc. The bus may be divided into an address bus, a data bus, a control bus, etc. The memory is used to store programs. Specifically, the program may include a program code, and the program code includes computer operation instructions. The memory may include a memory and a non-volatile memory, and provide instructions and data to the processor.

[0082] Embodiment 4

[0083] A computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the path tracking method of the four-wheel inspection robot are implemented, for example, including: introducing a target point into the RRT path expansion algorithm to obtain a basic path from the starting point to the end point; optimizing the basic path to obtain the final path of the four-wheel inspection robot; constructing an obstacle avoidance objective function based on an obstacle avoidance function with the goal of minimizing the collision index and minimizing the distance between the position of the four-wheel inspection robot and the final path; solving the obstacle avoidance objective function to obtain the front wheel turning angle information of the four-wheel inspection robot; controlling the four-wheel inspection robot to move according to the front wheel turning angle information of the four-wheel inspection robot; the process of optimizing the basic path to obtain the final path of the four-wheel inspection robot is: optimizing the parameters of the basic path to obtain a trajectory with continuous curvature; using Bessel to parameterize the trajectory, and then applying a maximum curvature constraint to obtain the final path of the four-wheel inspection robot. Specifically, the computer-readable storage medium includes, but is not limited to, for example, volatile memory and / or non-volatile memory. The volatile memory may include a random access memory (RAM) and / or a cache memory (cache), etc. The non-volatile memory may include a read-only memory (ROM), a hard disk, a flash memory, an optical disk, a magnetic disk, etc.

[0084] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented in one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that include computer-usable program code.

[0085] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0086] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0087] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0088] Those skilled in the art will readily appreciate other embodiments of the present invention after considering the specification and disclosure of the invention. This application is intended to cover any variations, uses or adaptations of the present invention that follow the general principles of the present invention and include common knowledge or customary techniques in the art that are not disclosed by the present invention. The specification and examples are to be considered exemplary only, and the true scope and spirit of the present invention are indicated by the following claims.

[0089] It should be understood that the present invention is not limited to the exact construction that has been described above and shown in the drawings and that various modifications and changes may be made without departing from the scope thereof. The scope of the present invention is limited only by the appended claims.

[0090] The above description is only a preferred embodiment of the present invention and does not limit the present invention in any way. Any simple modification, change and equivalent structural change made to the above embodiment based on the technical essence of the present invention still falls within the protection scope of the technical solution of the present invention.

Claims

1. A path tracking method for a four-wheel inspection robot, characterized in that: include: The target point is introduced into the RRT path extension algorithm to obtain the basic path from the starting point to the end point; Optimizing the basic path to obtain the final path of the four-wheel inspection robot; Based on the obstacle avoidance function, an obstacle avoidance objective function is constructed with the goal of minimizing the collision index and minimizing the distance between the position of the four-wheel inspection robot and the final path; Solving the obstacle avoidance objective function to obtain the front wheel turning angle information of the four-wheel inspection robot; The four-wheel inspection robot is controlled to move according to the front wheel turning angle information of the four-wheel inspection robot.

2. The path tracking method of the four-wheel inspection robot according to claim 1, characterized in that: The process of optimizing the basic path to obtain the final path of the four-wheel inspection robot is as follows: The basic path is parameterized using Bessel to make the path curvature continuous, and then a maximum curvature constraint is imposed to obtain the final path of the four-wheel inspection robot.

3. The path tracking method of the four-wheel inspection robot according to claim 1, characterized in that: The obstacle avoidance objective function is: in, J obs Represents the probability of a collision.

4. The path tracking method of the four-wheel inspection robot according to claim 1, characterized in that: The obstacle avoidance function is:

5. A path tracking system for a four-wheel inspection robot, characterized in that: include: A calculation module is used to introduce the target point into the RRT path extension algorithm to obtain a basic path from the starting point to the end point; An optimization module, used to optimize the basic path to obtain a final path of the four-wheel inspection robot; A construction module is used to construct an obstacle avoidance objective function based on an obstacle avoidance function, with the goal of minimizing the collision index and minimizing the distance between the position of the four-wheel inspection robot and the final path; A solving module, used to solve the obstacle avoidance objective function and obtain the front wheel turning angle information of the four-wheel inspection robot; The control module is used to control the movement of the four-wheel inspection robot according to the front wheel turning angle information of the four-wheel inspection robot.

6. The path tracking system of the four-wheel inspection robot according to claim 5, characterized in that: The process of optimizing the basic path to obtain the final path of the four-wheel inspection robot is as follows: The basic path is parameterized using Bessel to make the path curvature continuous, and then a maximum curvature constraint is imposed to obtain the final path of the four-wheel inspection robot.

7. The path tracking system of the four-wheel inspection robot according to claim 5, characterized in that: The obstacle avoidance objective function is: in, J obs Represents the probability of a collision.

8. The path tracking system of the four-wheel inspection robot according to claim 5, characterized in that: The obstacle avoidance function is:

9. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the steps of the path tracking method of the four-wheel inspection robot as described in any one of claims 1-4 are implemented.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the path tracking method of the four-wheel inspection robot as described in any one of claims 1 to 4 are implemented.

Images