Global path planning method based on four steering wheels

Abstract

The invention provides a global path planning method based on four steering wheels. The global path planning method comprises the following steps: S1, taking information such as a priori map and a vehicle body contour as input values of a global path planning algorithm module; s2, determining and setting a starting point, a target point and obstacle information of vehicle operation and a vehicle motion control model according to the priori map and the upper layer task; s3, an OPEN table and a CLOSE table are established; s4, establishing an evaluation function F (i) = G (i) + H (i); s5, putting a starting point in the OPEN table, selecting a node with the highest priority from the OPEN, taking the node as a father node, and putting the father node in a CLOSE table; and S6, calculating neighborhood nodes of the father node. According to the method, through cooperative arrangement of all the steps, curvilinear motion is considered, transverse movement and in-situ rotation of the robot are also considered, sample sets of other types can be customized, and compared with the prior art, more free and flexible all planning is achieved through the scheme.

Description

Technical Field

[0001] The present invention relates to the technical field related to path planning, and in particular to a global path planning method based on four steering wheels. Background Art

[0002] Automated Guided Vehicle (AGV) is an automated handling device that can travel along a predetermined path without manual navigation. As a highly automated means of transport, it embodies the advantages of safety, speed, efficiency, and low cost, and has gradually become a core component of the modern logistics system, which has greatly promoted the development of logistics and industrial automation.

[0003] Common driving methods on the market include independent steering wheels, differential gear drives, and interacting MECANUM wheels. Currently, differential gear drive is an automated driving technology used in automotive technology. This driving method cannot achieve arbitrary angular translation of the vehicle body and rotation around the geometric center of the vehicle body, and has high requirements for the openness of the site. Independent steering wheel drive has a variety of wheel system forms including single steering wheel drive, diagonal double steering wheel drive, rear drive double steering wheel drive, and four-steering wheel drive. Among them, single steering wheel and various forms of double steering wheel drives can adaptively find the instantaneous center of vehicle body speed during walking. Four-steering wheel AGV has greater The characteristics of load capacity and higher flexibility are gradually entering the field of vision of the logistics industry, but with it comes more complex trajectory tracking control and fault-tolerant control. Although the four-wheel chassis can bring flexible movement forms, its global planning algorithm has become more complex. At present, the path planning methods of AGV are mainly divided into three categories, namely search-based planning algorithms, sampling-based planning algorithms and reinforcement learning-based algorithms. Among them, the most widely used is the search-based path planning algorithm. The main graph search methods include: Dijkstra algorithm, Floyd algorithm, A*, D* and other algorithms, but the above algorithms do not consider the contour and kinematic constraints of the vehicle body;

[0004] Therefore, based on the above problems, the present invention provides a global path planning method based on four steering wheels. Summary of the invention

[0005] In view of the deficiencies in the prior art, the present invention provides a global path planning method based on four steering wheels, which solves the problems mentioned in the background technology.

[0006] To achieve the above objectives, the present invention is implemented through the following technical solutions:

[0007] A global path planning method based on four steering wheels comprises the following steps:

[0008] S1. Use the prior map, vehicle body contour and other information as input values ​​of the global path planning algorithm module;

[0009] S2. Determine and set the starting point, target point, obstacle information and vehicle motion control model of the vehicle according to the prior map and upper-level tasks;

[0010] S3. Create OPEN table and CLOSE table;

[0011] S4, establish an evaluation function F(i)=G(i)+H(i);

[0012] S5. Put the starting point in the OPEN table, select the node with the highest priority from OPEN and use it as the parent node, and put it in the CLOSE table;

[0013] S6. Calculate the neighboring nodes of the parent node.

[0014] Furthermore, in step S3, i represents the i-th node, G(i) represents the cost from the starting point to the i-th node, and H(i) represents the cost from the i-th node to the target point.

[0015] Furthermore, the step S5 is further divided into the following two steps:

[0016] S5.1: If the node is the end point, then trace the parent node step by step from the end point until it reaches the starting point; return the resulting path and the algorithm ends;

[0017] S5.2: If the node is not the end point, go to S6.

[0018] Furthermore, the step S6 is further divided into the following two steps:

[0019] S6.1: If the neighboring node m is in close_set, skip it and select the next neighboring node

[0020] S6.2: If the neighboring node m is not in the open_set either, calculate the evaluation function F, update the parent node of the m node, and add the node m to the open_set.

[0021] Furthermore, the motion model formulas of steps S6.1 and S6.2 are as follows:

[0022]

[0023] Where p is as follows, c and s in the formula are cos and sin functions:

[0024]

[0025] The inverse matrix of the P matrix is ​​P+ :

[0026]

[0027] where k = 4a 2 +4b 2 .

[0028] Furthermore, the simplified kinematic model of steps S6.1 and S6.2 is as follows:

[0029]

[0030] in

[0031] The present invention provides a global path planning method based on four steering wheels. Compared with the prior art, it has the following beneficial effects:

[0032] 1. A global path planning method based on four steering wheels provided by the present invention is used for autonomous navigation of a mobile robot. First, the upper system provides information of the scene, the starting point and the end point for path planning. Secondly, the scheme uses an improved lattice-based global path planning algorithm to search for a path and obtain a global path. Finally, motion control is performed according to the global path planning. The global path of the present invention takes into account the kinematic constraints of the vehicle body contour and the four steering wheels, and solves the problem of a four-steering wheel chassis out of control or control failure due to an improper global path.

[0033] 2. The global path planning method based on four steering wheels provided by the present invention not only takes into account curved motion, but also takes into account the lateral movement and on-site rotation of the robot, and other types of sample sets can be customized, thereby achieving more free and flexible global planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0035] Figure 1 A schematic diagram of a physical model of four steering wheels of the present invention is shown;

[0036] Figure 2 A schematic diagram of the kinematic model of four steering wheels of the present invention is shown;

[0037] Figure 3 A schematic diagram of a four-steering wheel lattice sample set of the present invention is shown;

[0038] Figure 4 A schematic diagram of a global path planning result 1 based on four steering wheels of the present invention is shown;

[0039] Figure 5 A schematic diagram of a global path planning result 2 based on four steering wheels of the present invention is shown; DETAILED DESCRIPTION

[0040] 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 are clearly and completely described. 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.

[0041] Example

[0042] In order to solve the technical problems in the background technology, a global path planning method based on four steering wheels is provided as follows:

[0043] Combination Figure 1-Figure 5 As shown, the present invention provides a global path planning method based on four steering wheels, comprising the following steps:

[0044] S1. Use the prior map, vehicle body contour and other information as input values ​​of the global path planning algorithm module;

[0045] S2. Determine and set the starting point, target point, obstacle information and vehicle motion control model of the vehicle according to the prior map and upper-level tasks;

[0046] S3. Create OPEN table and CLOSE table;

[0047] S4, establish an evaluation function F(i)=G(i)+H(i);

[0048] S5. Put the starting point in the OPEN table, select the node with the highest priority from OPEN and use it as the parent node, and put it in the CLOSE table;

[0049] S6. Calculate the neighboring nodes of the parent node.

[0050] In step S3, i represents the i-th node, G(i) represents the cost from the starting point to the i-th node, and H(i) represents the cost from the i-th node to the target point.

[0051] The step S5 is further divided into the following two steps:

[0052] S5.1: If the node is the end point, then trace the parent node step by step from the end point until it reaches the starting point; return the resulting path and the algorithm ends;

[0053] S5.2: If the node is not the end point, go to S6.

[0054] The step S6 is further divided into the following two steps:

[0055] S6.1: If the neighboring node m is in close_set, skip it and select the next neighboring node

[0056] S6.2: If the neighboring node m is not in the open_set either, calculate the evaluation function F, update the parent node of the m node, and add the node m to the open_set.

[0057] For step S6, see also the sampling curve Figure 3 As shown in the figure, the present invention not only considers the curved motion, but also the lateral movement and rotation of the robot, and can customize other types of sample sets to achieve more free and flexible global planning. The sample needs to consider the robot's kinematic model during its calculation process. The physical model of the four steering wheels and the kinematic map are shown in the figure. Figure 1 , 2 Therefore, the kinematic derivation is as follows.

[0058] The motion model formulas of steps S6.1 and S6.2 are as follows:

[0059]

[0060] Where p is as follows, c and s in the formula are cos and sin functions:

[0061]

[0062] The inverse matrix of the P matrix is ​​P + :

[0063]

[0064] where k = 4a 2 +4b 2 .

[0065] The simplified kinematic model of steps S6.1 and S6.2 is as follows:

[0066]

[0067] in

[0068] Furthermore, the F function in S6.2 is an evaluation function, which means the cost from the parent node to the current node, that is, the cost consumed in each step of the expansion process. The cost determines which trajectory to choose for execution. The cumulative cost function includes but is not limited to: path length cost, angle turning cost, lateral movement cost, backward cost, and distance obstacle cost. For the path length cost, the cost in the expansion process is to compare the cost of the Reed-Shepp curve, the Dubins curve, and the Manhattan distance, and take the maximum value of the three as the path length cost. Considering that unnecessary repeated turns and long-distance reversing are not recommended and expected, different penalty coefficients are set for expansion nodes with different attributes. Therefore, for the angle turning cost, the cost formula is g angle =k angle l angle , where k angle is the penalty coefficient for turning, l angle is the angle difference; for the retreat cost, the cost formula is g back =k back l back , where k back is the penalty coefficient for reversing, l back is the back-off distance; the penalty coefficient k means that when the state of a node does not meet expectations, the cost of the node is increased (k is set to a constant greater than 1), so that the path search algorithm is more inclined to choose other nodes, where the straight node is 1, the left and right turn nodes are constants greater than 1, the forward node is 1, and the backward node is a constant greater than 1;

[0069] In order to minimize the number of lateral shifts of the four steering wheels, a lateral shift cost is added, and the cost formula is g Shift =k Shift *(1 / l Shift ), where k Shift is the penalty coefficient for lateral movement. The larger its value is, the less likely the robot is to lateral move. Shift is the difference in lateral length;

[0070] In order to make the global path away from obstacles, the cost of distance obstacle is added and the idea of ​​grid map is introduced. The grid map is as follows Figure 4 , 5As shown, the map is in the form of a grid map. The grid size is set by the resolution, and the value of each grid (cell cost) ranges from 0 to 255. It is divided into three states: Occupied (with obstacles), Free (without obstacles), and Unknown Space. The surrounding grid of Occupied is expanded. The closer to the obstacle, the larger the corresponding grid value. Assuming that the sample path passes through a total of k nodes, for the i-th node, DM(i) is the obstacle distance value corresponding to the i-th node, and its expression is as follows:

[0071]

[0072] In the above formula, DisValue i is the grid value corresponding to the i-th node, K is the safety distance threshold greater than a certain threshold. The above formula can be understood as when the distance between the node and the obstacle is less than the safety distance threshold, a larger cost value is assigned, and when the distance between the node and the obstacle is greater than the safety distance threshold, the cost value decreases with the increase of the distance;

[0073] In order to verify the beneficial effect of the implementation in the path search stage compared with the existing technology, the present invention establishes a simulated obstacle map on the ROS Rviz platform, and the global map planned is as follows Figure 4 , 5 As shown in the figure, it can be seen that it can effectively suppress actions such as lateral movement, steering, and reversing.

[0074] Therefore, in summary, the present invention is used for autonomous navigation of mobile robots. First, the upper-level system provides scene information for path planning, information on the planning starting point and information on the planning end point. Secondly, an improved lattice-based global path planning algorithm is used to perform path search to obtain a global path. Finally, motion control is performed according to the global path planning. The global path designed by this scheme takes into account the kinematic constraints of the vehicle body contour and the four steering wheels, and solves the problem of loss of control or control failure of the four-steering wheel chassis due to an improper global path. At the same time, curved motion is taken into account, as well as the lateral movement and in-situ rotation of the robot. Other types of sample sets can be customized to achieve more free and flexible global planning.

[0075] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the presence of other identical elements in the process, method, article or device including the elements.

[0076] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A global path planning method based on four steering wheels, Features: The steps include: S1. Use the prior map, vehicle body contour and other information as input values ​​of the global path planning algorithm module; S2. Determine and set the starting point, target point, obstacle information and vehicle motion control model of the vehicle according to the prior map and upper-level tasks; S3. Create OPEN table and CLOSE table; S4, establish an evaluation function F(i)=G(i)+H(i); S5. Put the starting point in the OPEN table, select the node with the highest priority from OPEN and use it as the parent node, and put it in the CLOSE table; S6. Calculate the neighboring nodes of the parent node.

2. A global path planning method based on four steering wheels according to claim 1, Features: In step S3, i represents the i-th node, G(i) represents the cost from the starting point to the i-th node, and H(i) represents the cost from the i-th node to the target point.

3. A global path planning method based on four steering wheels according to claim 1, Features: The step S5 is further divided into the following two steps: S5.1: If the node is the end point, then trace the parent node step by step from the end point until it reaches the starting point; return the resulting path and the algorithm ends; S5.2: If the node is not the end point, go to S6.

4. A global path planning method based on four steering wheels according to claim 1, Features: The step S6 is further divided into the following two steps: S6.1: If the neighboring node m is in close_set, skip it and select the next neighboring node S6.2: If the neighboring node m is not in the open_set either, calculate the evaluation function F, update the parent node of the m node, and add the node m to the open_set.

5. A global path planning method based on four steering wheels according to claim 4, Features: The motion model formulas of steps S6.1 and S6.2 are as follows: Where p is as follows, c and s in the formula are cos and sin functions: The inverse matrix of the P matrix is ​​P + : where k = 4a 2 +4b 2 .

6. A global path planning method based on four steering wheels according to claim 5, Features: The simplified kinematic model of steps S6.1 and S6.2 is as follows: in

Images