A remote path planning method for patrollers combined with original images

Abstract

The present invention provides a patrol vehicle remote path planning method combined with original images, and the steps are as follows: 1) obtaining the starting point position, heading, moving target point, target heading, multiple pairs of navigation camera original images, and two pairs of front and rear obstacle avoidance camera original images; 2) generating point clouds from multiple pairs of navigation camera point clouds and two pairs of front and rear obstacle avoidance camera original images, synthesizing and splicing terrain and suitability analysis results, and then planning a passable path with the minimum point method according to the target point and heading constraints; 3) analyzing the stones on the front of the patrol vehicle, combining the original image of the obstacle avoidance camera and the three-dimensional point cloud image of the obstacle avoidance camera to determine the passability of the stones; stipulating the method of pressing over, bypassing or crossing over the obstacles, and proposing a corresponding detailed path planning method. The present invention realizes the response to close-range obstacles according to the actual state of on-track operation.

Description

Technical Field

[0001] The invention relates to a patrol vehicle remote path planning method combined with original images. Background Art

[0002] The Mars rover carried out my country's first Mars patrol and exploration mission, presenting the topography of the Utopia Plain on Mars to all mankind, increasing human knowledge of Mars. Although the terrain of the Utopia Plain is relatively flat overall, there are many rocks and sand pits, which brings certain difficulties to path planning. In remote planning, when there are rocks or pit obstacles close to the vehicle body and it is impossible to restore the three-dimensional terrain of the entire obstacle in three dimensions, the path planning method that achieves the required target point position and heading at the same time is an important technical means that needs to be applied in remote control.

[0003] The ground navigation planning and control subsystem is responsible for completing global environmental perception, planning navigation units and generating control strategies for patrol vehicle movement based on the acquisition of telemetry engineering values ​​and image data. As an important part of patrol vehicle remote operation, it also faces more severe technical challenges brought by the complexity of terrain in patrol detection missions, and even determines the success or failure of patrol detection missions to a certain extent.

[0004] A ground navigation planning and control method for a rover proposed in the context of the lunar exploration mission provides a process for planning a lunar navigation unit based on a navigation camera, including: sequential image stitching, three-dimensional terrain restoration, terrain passability analysis, path planning, and mobile strategy generation. This patent provides general steps for rover navigation planning. A method for determining the passability of complex terrain on the far side of the moon under restricted constraints proposed in the context of the lunar exploration mission determines the passability of navigation paths on complex lunar terrain, but does not provide a definite method for planning walking paths under existing conditions. For Mars exploration missions and facing adverse conditions such as close-range obstacles, it is urgent to adopt a suitable path planning method. Summary of the invention

[0005] The technical problem solved by the present invention is: to overcome the above-mentioned deficiencies of the prior art and provide a rover remote path planning method based on original images. The method is based on the judgment of obstacles and then selects a suitable method for remote path planning to complete the remote operation of the Mars rover.

[0006] The technical solution of the present invention is: a patrol vehicle remote path planning method combined with original images, comprising the following steps:

[0007] 1) Obtain the starting point position, heading, moving target point, target heading, multiple pairs of original images of the navigation camera, and two pairs of original images of the front and rear obstacle avoidance cameras;

[0008] 2) Generate point clouds from multiple pairs of navigation camera point clouds and two pairs of obstacle avoidance camera original images, synthesize and stitch terrain and suitability analysis results, and then plan a passable path using the minimum point method based on the target point and heading constraints;

[0009] 3) Analyze the rocks on the front of the patrol vehicle, and determine the passability of the rocks by combining the original image of the obstacle avoidance camera and the three-dimensional point cloud image of the obstacle avoidance camera; define the methods of pressing over, bypassing or crossing the obstacles, and propose corresponding detailed path planning methods.

[0010] In the step 1), the starting point refers to the position point in the north-east coordinate system with the photo-taking point as the origin; the heading refers to the direction of the vehicle head in the north-east coordinate system, with north-east being positive.

[0011] In the step 1), the target point refers to the target of the patroller's current movement, which is the position point in the north-east coordinate system with the photo-taking point as the origin; the target heading refers to the direction of the vehicle head after reaching the target position in the north-east coordinate system.

[0012] In the step 1), the images refer to a plurality of pairs of navigation camera sequence images acquired by the navigation camera, and a pair of front obstacle avoidance camera images and a pair of rear obstacle avoidance camera images acquired by the obstacle avoidance camera.

[0013] The specific process of step 2) is as follows:

[0014] 21) The navigation camera generates point cloud terrain 1 from the surrounding images, the front and rear obstacle avoidance cameras generate point cloud terrain 2, and the terrain 3 of the area under the vehicle. Terrains 1, 2, and 3 are added together to form a patchwork terrain for path planning; the patchwork terrain is subjected to a suitability analysis to obtain a suitability analysis graph, and the result is saved for path planning;

[0015] 22) The minimum point planning method is used to perform path planning on the point cloud map and suitability map.

[0016] The specific planning method of step 22) is:

[0017] a) Mark the starting point s0(x0,y0) and the end point p1(x1,y1) on the path planning patchwork terrain;

[0018] b) Using the method of turning in place + curvature movement + turning in place, a curve is connected between the starting point and the end point; the adjustable variables include the turning angle in place at the starting point (s0.a), curvature (s0.c), and the turning angle in place at the end point (s0.b); adjusting the above three variables, a path that meets multiple constraints is obtained on the suitability analysis diagram; turning in place is defined as the position of the vehicle body remains unchanged, but the direction of the vehicle head is changed; curvature movement means that the vehicle moves according to a fixed curvature;

[0019] c) If b) cannot obtain a trajectory, then add an intermediate point s1 in the middle, and the planning method is the same as b). S0 to S1, and S1 to P1 are connected by curves without turning on the spot; if there is still no feasible trajectory, continue to add intermediate points.

[0020] The specific requirements for satisfying multiple constraints are as follows:

[0021] i. The curve searches for the passable area in the suitability analysis diagram;

[0022] ii. The curvature is not greater than the curvature threshold;

[0023] iii. If there is a feasible trajectory when the starting point turns at 0°, it should be given priority.

[0024] In the step 3), the original image of the obstacle avoidance camera and the three-dimensional point cloud image of the obstacle avoidance camera are analyzed. When the stone is at the edge, the obstacle avoidance camera point cloud cannot be restored in three dimensions after it is generated. To obtain the stone height information, it is necessary to compare the size of the stone and the wheel from the original image.

[0025] The specific method of step 3) is:

[0026] 31) Combine the original image of the obstacle avoidance camera and the 3D point cloud image of the obstacle avoidance camera to determine the passability of the stone and obtain a feasible path planning principle; the judgment method for passability is: if the height of the stone does not exceed 1 / 4 of the wheel, it is considered that the stone can be crushed; if it exceeds 1 / 4 but not more than 2 / 4, it is necessary to first step back and then step over or bypass the stone. If it exceeds 2 / 4, it is necessary to first step back and then bypass;

[0027] 32) For the stones that can be crushed, the path planning is performed according to the minimum point planning method in step 22);

[0028] 33) To cross or bypass an obstacle under the wheel, the vehicle first retreats a certain distance, then replans the path with the current point as the starting point to bypass or cross the obstacle;

[0029] 34) When there is a stone at the front of the vehicle and it is impossible to obtain all the stone information through the point cloud image of the obstacle avoidance camera, the obstacle is bypassed and the path is planned according to the minimum point planning method in step 22). The height of the stone is obtained through the three-dimensional point cloud, and the threshold for overcoming the obstacle is defined as f0. If it exceeds f0, the obstacle must be bypassed, otherwise it can be crossed. Then the path is planned according to the minimum point planning method in step 22).

[0030] The specific process of step 33) is as follows:

[0031] 33a) First calculate the position of the stone and mark it on the suitability map; the coordinates of the left front wheel and the right front wheel in this system are (m0, -n0) and (m0, n0) respectively, and the heading of the vehicle is s0.h; the coordinates in the north-east coordinate system are or Add a stone obstacle under the left front wheel or right front wheel in the suitability map;

[0032] 33b) The retreat distance shall not exceed 2m, and the person shall retreat along the original trajectory; and the starting point position shall be recalculated;

[0033] 33c) Then plan the path according to the minimum point planning method in step 22).

[0034] Compared with the prior art, the present invention has the following advantages:

[0035] In traditional path planning, it is generally impossible to obtain three-dimensional point cloud information of stones under the wheels among stone obstacles at the front of the vehicle. There is also a situation where the three-dimensional point cloud of the stone cannot be completely restored in three dimensions. In order to obtain a feasible planned path, the present invention needs to combine the original image of the obstacle avoidance camera and the three-dimensional point cloud image of the obstacle avoidance camera to determine the passability of the stone, and stipulates methods of pressing over, bypassing or crossing over the obstacle, and proposes a corresponding detailed path planning method. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0037] The following is combined with Figure 1 The present invention is further described in detail with specific embodiments:

[0038] 1. Obtain the starting position s0(x,y), starting heading (s0.h), moving target point (p1(x,y)), target heading (p1.h), original image (multiple pairs of navigation images, two pairs of front and rear obstacle avoidance camera images)

[0039] The starting point is the position point in the north-east coordinate system with the photo point as the origin. The heading refers to the direction of the vehicle head in the north-east coordinate system (north-east is positive). The target point is the target of the patroller's movement this time, with the photo point as the origin and the position point in the north-east coordinate system. The target heading refers to the direction of the vehicle head in the north-east coordinate system after it is in place. The images are several pairs of navigation camera sequence images, one pair of front obstacle avoidance camera images, and one pair of rear obstacle avoidance camera images.

[0040] 2. Generate 3D point clouds from multiple pairs of navigation camera point clouds and two pairs of obstacle avoidance camera original images through 3D restoration, generate point clouds and synthesize them to form 3D point clouds for path planning, stitch terrain and suitability analysis results, and then plan a passable path on the ground with the least point method according to constraints such as target points and headings:

[0041] 21) The navigation camera generates point clouds for terrain 1, the front and rear obstacle avoidance cameras generate point clouds for terrain 2, and the area under the vehicle is terrain 3. Terrains 1, 2, and 3 are added together to form a patchwork terrain for path planning. Perform a suitability analysis on the patchwork terrain (existing patent, 201910364733.6), and save the results for path planning;

[0042] 22) Use the least point planning method for path planning

[0043] a) Mark the starting point s0(x0,y0) and the end point p1(x1,y1) on the terrain;

[0044] b) Use the method of turning on the spot + curvature movement + turning on the spot to connect a curve between the starting point and the end point; the adjustable variables include the starting point turning angle (s0.a), curvature (s0.c), and the end point turning angle (s0.b); adjust these three variables to obtain a path that meets multiple constraints on the suitability analysis diagram. The planning must meet the following requirements:

[0045] i. The area where the curve can be passed in the suitability analysis;

[0046] ii. The curvature is not greater than the curvature threshold (generally set between 0.35-0.5);

[0047] iii. If there is a feasible trajectory when the starting point turns at 0°, it should be preferred;

[0048] c) If b) cannot obtain a trajectory, then add an intermediate point s1 in the middle. The planning method is the same as b). S0 to S1, S1 to P1 are connected by curves without turning in place. If there is still no feasible trajectory, then add another intermediate point.

[0049] 3. Analyze the original image and 3D point cloud image of the obstacle avoidance camera. When the stone is at the edge, the point cloud of the obstacle avoidance camera cannot be restored in 3D. To obtain the height information of the stone, it is necessary to compare the size of the stone and the wheel from the original image.

[0050] 31) Combined with two-dimensional images to determine the passability of stones

[0051] Generally, it is impossible to obtain 3D point cloud information of the stone under the wheel. In this case, the judgment method is: if the height of the stone does not exceed 1 / 4 of the wheel, it is considered that the stone can be crushed; if it exceeds 1 / 4 but not more than 2 / 4, it is necessary to back up and then step over or bypass the stone. If it exceeds 2 / 4, it is necessary to back up and then bypass it.

[0052] 32) For rocks that can be crushed, plan the path according to the minimum point planning method in step 2;

[0053] 33) To cross or bypass an obstacle, the car needs to retreat a certain distance first, then replan the path with the current point as the starting point to bypass or cross the obstacle.

[0054] 33a) First calculate the position of the stone and mark it on the suitability map. In this system, the coordinates of the left front wheel and the right front wheel are (m0, -n0) and (m0, n0) respectively, and the heading of the vehicle is s0.h. The coordinates in the north-east coordinate system are or

[0055] Add a stone obstacle under the left or right front wheel in the suitability map;.

[0056] 33b) The retreat distance shall not exceed 2m, and the person shall retreat along the original trajectory; and the starting point position shall be recalculated;

[0057] 33c) Then plan the path according to the minimum point planning method in step 2;

[0058] 34) There are stones on the front of the vehicle.

[0059] 34a) Obtain the height of the stone through the 3D point cloud, and define the threshold as f0 (f0 = 16 cm). If it exceeds f0, the obstacle must be bypassed, otherwise it can be crossed. If the information of the stone is incomplete after 3D restoration, the obstacle is bypassed.

[0060] 34b) Then plan the path according to the minimum point planning method in step 2.

Claims

1. A patrol vehicle remote path planning method combining original images, characterized in that The steps include: 1) Obtain the starting point position, heading, moving target point, target heading, multiple pairs of original images of the navigation camera, and two pairs of original images of the front and rear obstacle avoidance cameras; 2) Synthesize and stitch the point clouds generated by multiple pairs of navigation camera point clouds and the original images of the front and rear obstacle avoidance cameras to obtain the terrain and suitability analysis results, and then plan the passable path with the minimum point method based on the target point and heading constraints; 3) Analyze the rocks on the front of the patrol vehicle, and determine the passability of the rocks by combining the original image of the obstacle avoidance camera and the 3D point cloud image of the obstacle avoidance camera; define the methods of passing over, bypassing or crossing the obstacles, and propose corresponding detailed path planning methods; In the step 3), the original image of the obstacle avoidance camera and the three-dimensional point cloud image of the obstacle avoidance camera are analyzed. When the stone is at the edge, the point cloud of the obstacle avoidance camera cannot be three-dimensionally restored after it is generated, and the height information of the stone is obtained. Therefore, the size of the stone and the wheel needs to be compared from the original image; The specific method of step 3) is: 31) Combine the original image of the obstacle avoidance camera and the 3D point cloud image of the obstacle avoidance camera to determine the passability of the stone and obtain a feasible path planning principle; the judgment method for passability is: if the height of the stone does not exceed 1 / 4 of the wheel, it is considered that the stone can be crushed; if it exceeds 1 / 4 but not more than 2 / 4, it is necessary to first step back and then step over or bypass the stone; if it exceeds 2 / 4, it is necessary to first step back and then bypass; 32) For the stones that can be crushed, the path planning is performed according to the minimum point planning method in step 22); 33) To cross or bypass an obstacle under the wheel, the vehicle first retreats a certain distance, then replans the path with the current point as the starting point to bypass or cross the obstacle; 34) When there is a stone at the front of the vehicle and it is impossible to obtain all the stone information through the point cloud image of the obstacle avoidance camera, the obstacle bypassing process is followed and the path planning is performed according to the minimum point planning method in step 22; The height of the stone is obtained through the three-dimensional point cloud, and the threshold value of the obstacle that can be pressed is defined as f0. If it exceeds f0, the obstacle must be bypassed, otherwise the obstacle can be crossed; then the path is planned according to the minimum point planning method in step 22); The specific process of step 33) is as follows: 33a) First calculate the position of the stone and mark it on the suitability map; the coordinates of the left front wheel and the right front wheel in this system are (m0, -n0) and (m0, n0) respectively, and the heading of the vehicle is s0.h; the coordinates in the north-east coordinate system are or Add a stone obstacle under the left front wheel or right front wheel in the suitability map; 33b) The retreat distance shall not exceed 2m, and the retreat shall be along the original trajectory; And recalculate the starting point position; 33c) Then plan the path according to the minimum point planning method in step 22).

2. The method for remote path planning of a patrol vehicle combined with original images according to claim 1, characterized in that: In the step 1), the starting point refers to the position point in the north-east coordinate system with the photo-taking point as the origin; the heading refers to the direction of the vehicle head in the north-east coordinate system, with north-east being positive.

3. The method for remote path planning of a patrol vehicle combined with original images according to claim 1, characterized in that: In the step 1), the target point refers to the target of the patroller's current movement, which is the position point in the north-east coordinate system with the photo-taking point as the origin; The target heading refers to the direction of the vehicle's front in the north-east coordinate system after it has arrived.

4. The method for remote path planning of a patrol vehicle combined with original images according to claim 1, characterized in that: In the step 1), the images refer to a plurality of pairs of navigation camera sequence images acquired by the navigation camera, and a pair of front obstacle avoidance camera images and a pair of rear obstacle avoidance camera images acquired by the obstacle avoidance camera.

5. The method for remote path planning of a patrol vehicle combined with original images according to claim 1, characterized in that: The specific process of step 2) is as follows: 21) The navigation camera generates point cloud terrain 1 from the surrounding images, the front and rear obstacle avoidance cameras generate point cloud terrain 2, and the terrain 3 of the area under the vehicle. The above terrains 1, 2, and 3 are added together to form a spliced ​​terrain for path planning; the spliced ​​terrain is subjected to suitability analysis to obtain a suitability analysis graph, and the result is saved for path planning; 22) The minimum point planning method is used to perform path planning on the point cloud map and suitability map.

6. The method for remote path planning of a patrol vehicle combined with original images according to claim 5, characterized in that: The specific planning method of step 22) is: a) Mark the starting point s0(x0,y0) and the end point p1(x1,y1) on the path planning patchwork terrain; b) Using the method of turning in place + curvature movement + turning in place, a curve is connected between the starting point and the end point; the adjustable variables include the turning angle in place at the starting point (s0.a), curvature (s0.c), and the turning angle in place at the end point (s0.b); adjusting the above three variables, a path that meets multiple constraints is obtained on the suitability analysis diagram; turning in place is defined as the position of the vehicle body remains unchanged, but the direction of the vehicle head is changed; curvature movement means that the vehicle moves according to a fixed curvature; c) If b) cannot obtain a trajectory, then add an intermediate point s1 in the middle, and the planning method is the same as b). S0 to S1, and S1 to P1 are connected by curves without turning on the spot; if there is still no feasible trajectory, continue to add intermediate points.

7. The method for remote path planning of a patrol vehicle combined with original images according to claim 6, characterized in that: The specific requirements for satisfying multiple constraints are as follows: i. The curve searches for the passable area in the suitability analysis diagram; ii. The curvature is not greater than the curvature threshold; iii. If there is a feasible trajectory when the starting point turns at 0°, it should be given priority.

Images