CN109540022B - Method for planning and deciding path of flat-cabin robot based on TOF depth camera - Google Patents

Abstract

The invention relates to the technical field of navigation of granary robots, and provides a method for planning and deciding a route of a granary leveling robot based on a TOF depth camera, which comprises the steps of firstly obtaining three-dimensional image information of a local area of a grain stack, finding out all convex hulls in the local area, finding out a convex point with the highest height as a target point, wherein the corresponding convex hull is a convex hull to be leveled; then forming 9 squares by taking the starting point as the center, calculating the cost of moving from the starting point to the center point of each square around, selecting the point with the minimum cost as the point to be moved, and taking the point as a new starting point, searching the point to be moved next time until forming a local optimal path; and then moving to a target point along the local optimal path to perform leveling operation on the convex hull to be leveled, and performing the operation on the next local area until the whole granary is leveled. The invention has higher accuracy and adaptability in the complex environment of discontinuous terrain, and improves the autonomy of the leveling robot in the leveling operation.

Description

Method for planning and deciding path of flat-cabin robot based on TOF depth camera

Technical Field

The invention relates to the technical field of navigation of granary robots, in particular to a method for planning and deciding a path of a granary leveling robot based on a TOF depth camera.

Background

The grain storage is related to national security and social stability, and the innovation and development of the grain storage technology are concerned. As the last processing link of the grain entering the warehouse and entering the conventional storage stage, the leveling operation is crucial, and the leveling operation determines the flatness of the grain surface, which is directly related to the implementation effects of grain storage technologies such as ventilation, fumigation, grain temperature measurement and control during the grain storage period, thereby affecting the safety of the grain during the storage period.

At present, the main operation mode of the leveling operation is a manual mode, and the problems of low leveling speed, high labor intensity, grain deterioration and the like caused by manual operation cannot meet the requirements of the leveling operation and even can bring serious threats to the life safety of people. Therefore, the operation of the flat cabin requires the operation of a robot, namely, in the working process of the flat cabin, the grain higher than the plane is pushed to the place lower than the grain plane by the robot. At present, the robot has wide application in aspects of bin patrol, bin detection, bin reporting and the like of a granary, but the robot capable of working independently is not used in the operation of leveling, and the existing leveling robot needs to be controlled remotely by manpower, so that time and labor are wasted, and the leveling effect is not ideal; in a granary environment with a complex environment, the leveling robot is difficult to accurately reach a target position. The key of the leveling robot for accurately reaching the target position is a path planning and decision-making method. The existing leveling robot path planning and decision-making method has poor accuracy and adaptability in a discontinuous terrain environment, so that the autonomy of the leveling robot in leveling operation is not high enough.

Disclosure of Invention

Aiming at the problems in the prior art, the invention provides a method for planning and deciding a path of a leveling robot based on a TOF depth camera, which has higher accuracy and adaptability in a complex environment of a discontinuous terrain and improves the autonomy of the leveling robot in leveling operation.

The technical scheme of the invention is as follows:

a flat-cabin robot path planning and decision-making method based on a TOF depth camera is characterized by comprising the following steps:

step 1: installing the TOF depth camera on a leveling robot, placing the leveling robot in a granary, starting the TOF depth camera and the leveling robot, and establishing a three-dimensional rectangular coordinate system X-Y-Z;

step 2: identifying a grain pile in a grain bin through a TOF depth camera to obtain three-dimensional image information of a local area of the grain pile, and sending the three-dimensional image information to a leveling robot through the TOF depth camera; the local region is a field of view range of the TOF depth camera; the three-dimensional image information includes three-dimensional coordinate values (x) of each point P in a local area_P,y_P,h_P) Wherein x is_P、y_P、h_PRespectively, the coordinates of point P on axis X, Y, Z, h_PIs also the height of point P;

and step 3: the leveling robot processes the received three-dimensional image information, finds all convex hulls and valleys in a local area, and the vertexes of all the convex hulls form a convex point set B ═₁,B₂,B₃,...,B_MM is the number of convex hulls in a local area of the grain pile;

and 4, step 4: extracting the height value of each point in the bump set to form a height value set H ═ H₁,h₂,h₃,...,h_MFinding out the bump B with the highest height in the bump set_mAs target point, bump B_mThe corresponding convex hull is a convex hull to be leveled;

and 5: planning from a starting point A to a salient point B by taking a point A where the leveling robot is positioned as the starting point_mThe local optimal path comprises the following specific steps:

step 5.1: further analyzing the three-dimensional image information, and identifying the barrier region and the salient point removing B in the local region_mDefining a class function f (P) in other reachable areas except for the reachable area, and when the point P is the salient point B_mWhen f (p) is 1; when point P is located within the obstacle region, f (P) ═ 1; when the point P is located at the salient point removing point B_m(p) 0 when in other reachable regions than that;

step 5.2: forming a square area by taking the starting point A as the center, and dividing the square area into 9 squares with the length of n in half, so that the squares taking the starting point A as the center are middle squares, and the centers around the middle squares are points A_i8 squares; wherein, i is 1,2, 3.., 8;

step 5.3: if f (A)_i) Not equal to-1, the calculation moves from the starting point A to the point A_iAt the cost of f (i) ═ g (i) + h (i); wherein G (i) is a movement from the starting point A to the point A_iCost of (b), H (i) is from point A_iMove to point B_mCost of (g), (i) using starting point A and point A_iMeasured by the Manhattan distance between, H (i) using point A_iAnd point B_mMeasured by the manhattan distance between them, i.e.

If f (A)_i) Let f (i) be + ∞ -1; wherein x is_A、y_ARespectively the coordinates of point a on the X, Y axis,

are respectively point A_iAt the coordinates of the axis X, Y, the axis,

are respectively point B_mCoordinates at axis X, Y;

step 5.4: selecting point A with minimum cost_jAs a point to which the present movement is to be performed, F (j) min { F (1), F (2),.., F (8) };

step 5.5: if point A_jNot in the form of bumps B_mThen, with point A_jRepeating the method of step 5.2-step 5.4 for a new starting point, and determining the next moving point until reaching the convex point B_m(ii) a If point A_jIs a salient point B_mIf so, forming a local optimal path and entering the step 5.6;

step 5.6: the leveling robot moves to the convex point B according to the local optimal path_mCarrying out leveling operation on the convex hull to be leveled;

step 6: and (5) repeating the method of the step (2) to the step (5), and carrying out planning of the local optimal path and leveling operation on the next local area until the whole granary is leveled.

The step 4 comprises the following steps:

step 4.1: initialization h_max＝h₁、m＝1、i＝2；

Step 4.2: comparison h_maxAnd h_i: if h_max>h_iThen go to step 4.3; if h_max<h_iThen let h_max＝h_iI, then go to step 4.3;

step 4.3: if i is equal to M, go to step 4.4; if i is less than M, making i equal to i +1, and returning to the step 4.2;

step 4.4: salient point B_mNamely the bump with the highest height in the bump set.

The invention has the beneficial effects that:

according to the invention, the TOF depth camera is used as a sensor, and the depth information of an object is used as the input of an algorithm, so that the highest convex hull in a local area is accurately positioned, an optimal path reaching a target convex hull can be selected, and the working efficiency of the leveling is improved.

Drawings

Fig. 1 is a flowchart of a path planning and decision-making method of a flat-cabin robot based on a TOF depth camera according to the present invention.

Detailed Description

The invention will be further described with reference to the accompanying drawings and specific embodiments.

The invention aims to provide a method for planning and deciding a path of a leveling robot based on a TOF depth camera, which has higher accuracy and adaptability in a complex environment of a discontinuous terrain and improves the autonomy of the leveling robot in leveling operation.

Fig. 1 is a flowchart of a path planning and decision-making method of a flat-cabin robot based on a TOF depth camera according to the present invention. The invention discloses a method for planning and deciding a path of a horizontal warehouse robot based on a TOF depth camera, which is characterized by comprising the following steps of:

step 1: installing the TOF depth camera on a leveling robot, placing the leveling robot in a granary, starting the TOF depth camera and the leveling robot, and establishing a three-dimensional rectangular coordinate system X-Y-Z;

step 2: identifying a grain pile in a grain bin through a TOF depth camera to obtain three-dimensional image information of a local area of the grain pile, and sending the three-dimensional image information to a leveling robot through the TOF depth camera; the local region is a field of view range of the TOF depth camera; the three-dimensional image information includes three-dimensional coordinate values (x) of each point P in a local area_P,y_P,h_P) Wherein x is_P、y_P、h_PRespectively, the coordinates of point P on axis X, Y, Z, h_PIs also the height of point P;

and step 3: the leveling robot processes the received three-dimensional image information, finds all convex hulls and valleys in a local area, and the vertexes of all the convex hulls form a convex point set B ═₁,B₂,B₃,...,B_MM is the number of convex hulls in a local area of the grain pile;

and 4, step 4: extracting the height value of each point in the bump set to form a height value set H ═ H₁,h₂,h₃,...,h_MFinding out the bump B with the highest height in the bump set_mAs target point, bump B_mThe corresponding convex hull is a convex hull to be leveled;

and 5: planning from a starting point A to a salient point B by taking a point A where the leveling robot is positioned as the starting point_mThe local optimal path comprises the following specific steps:

step 5.1: further analyzing the three-dimensional image information, and identifying the barrier region and the salient point removing B in the local region_mDefining a class function f (P) in other reachable areas except for the reachable area, and when the point P is the salient point B_mWhen f (p) is 1; when point P is located within the obstacle region, f (P) ═ 1; when the point P is located at the salient point removing point B_m(p) 0 when in other reachable regions than that;

step 5.2: forming a square area by taking the starting point A as the center, and dividing the square area into 9 squares with the length of n in half, so that the squares taking the starting point A as the center are middle squares, and the centers around the middle squares are points A_i8 squares; wherein, i is 1,2, 3.., 8;

step 5.3: if f (A)_i) Not equal to-1, the calculation moves from the starting point A to the point A_iAt the cost of f (i) ═ g (i) + h (i); wherein G (i) is a movement from the starting point A to the point A_iCost of (b), H (i) is from point A_iMove to point B_mCost of (g), (i) using starting point A and point A_iMeasured by the Manhattan distance between, H (i) using point A_iAnd point B_mMeasured by the manhattan distance between them, i.e.

If f (A)_i) Let f (i) be + ∞ -1; wherein x is_A、y_ARespectively the coordinates of point a on the X, Y axis,

are respectively point A_iAt the coordinates of the axis X, Y, the axis,

are respectively point B_mCoordinates at axis X, Y;

step 5.4: selecting point A with minimum cost_jAs a point to which the present movement is to be performed, F (j) min { F (1), F (2),.., F (8) };

step 5.5: if point A_jNot in the form of bumps B_mThen, with point A_jRepeating the method of step 5.2-step 5.4 for a new starting point, and determining the next moving point until reaching the convex point B_m(ii) a If point A_jIs a salient point B_mIf so, forming a local optimal path and entering the step 5.6;

step 5.6: the leveling robot moves to the convex point B according to the local optimal path_mCarrying out leveling operation on the convex hull to be leveled;

step 6: and (5) repeating the method of the step (2) to the step (5), and carrying out planning of the local optimal path and leveling operation on the next local area until the whole granary is leveled.

In this embodiment, the step 4 includes the following steps:

step 4.1: initialization h_max＝h₁、m＝1、i＝2；

Step 4.2: comparison h_maxAnd h_i: if h_max>h_iThen go to step 4.3; if h_max<h_iThen let h_max＝h_iI, then go to step 4.3;

step 4.3: if i is equal to M, go to step 4.4; if i is less than M, making i equal to i +1, and returning to the step 4.2;

step 4.4: salient point B_mNamely the bump with the highest height in the bump set.

It is to be understood that the above-described embodiments are only a few embodiments of the present invention, and not all embodiments. The above examples are only for explaining the present invention and do not constitute a limitation to the scope of protection of the present invention. All other embodiments, which can be derived by those skilled in the art from the above-described embodiments without any creative effort, namely all modifications, equivalents, improvements and the like made within the spirit and principle of the present application, fall within the protection scope of the present invention claimed.

Claims (2)

1. A flat-cabin robot path planning and decision-making method based on a TOF depth camera is characterized by comprising the following steps:

step 1: installing the TOF depth camera on a leveling robot, placing the leveling robot in a granary, starting the TOF depth camera and the leveling robot, and establishing a three-dimensional rectangular coordinate system X-Y-Z;

step 2: identifying a grain pile in a grain bin through a TOF depth camera to obtain three-dimensional image information of a local area of the grain pile, and sending the three-dimensional image information to a leveling robot through the TOF depth camera; the local region is a field of view range of the TOF depth camera; the three-dimensional image information includes three-dimensional coordinate values (x) of each point P in a local area_P,y_P,h_P) Wherein x is_P、y_P、h_PRespectively, the coordinates of point P on axis X, Y, Z, h_PIs also the height of point P;

and step 3: the leveling robot processes the received three-dimensional image information, finds all convex hulls and valleys in a local area, and the vertexes of all the convex hulls form a convex point set B ═₁,B₂,B₃,...,B_MM is the number of convex hulls in a local area of the grain pile;

and 4, step 4: extracting the height value of each point in the bump set to form a height value set H ═ H₁,h₂,h₃,...,h_MFinding out the bump B with the highest height in the bump set_mAs target point, bump B_mThe corresponding convex hull is a convex hull to be leveled;

and 5: planning from a starting point A to a salient point B by taking a point A where the leveling robot is positioned as the starting point_mThe local optimal path comprises the following specific steps:

step 5.1: further analyzing the three-dimensional image information, and identifying the barrier region and the salient point removing B in the local region_mOther reachable regions than the defined class function f (P)) When the point P is a convex point B_mWhen f (p) is 1; when point P is located within the obstacle region, f (P) ═ 1; when the point P is located at the salient point removing point B_m(p) 0 when in other reachable regions than that;

step 5.2: forming a square area by taking the starting point A as the center, and dividing the square area into 9 squares with the length of n in half, so that the squares taking the starting point A as the center are middle squares, and the centers around the middle squares are points A_i8 squares; wherein, i is 1,2, 3.., 8;

step 5.3: if f (A)_i) Not equal to-1, the calculation moves from the starting point A to the point A_iAt the cost of f (i) ═ g (i) + h (i); wherein G (i) is a movement from the starting point A to the point A_iCost of (b), H (i) is from point A_iMove to point B_mCost of (g), (i) using starting point A and point A_iMeasured by the Manhattan distance between, H (i) using point A_iAnd point B_mMeasured by the manhattan distance between them, i.e.

If f (A)_i) Let f (i) be + ∞ -1; wherein x is_A、y_ARespectively the coordinates of point a on the X, Y axis,

are respectively point A_iAt the coordinates of the axis X, Y, the axis,

are respectively point B_mCoordinates at axis X, Y;

step 5.4: selecting point A with minimum cost_jAs a point to which the present movement is to be performed, F (j) min { F (1), F (2),.., F (8) };

step 5.5: if point A_jNot in the form of bumps B_mThen, with point A_jRepeating the method of step 5.2-step 5.4 for a new starting point, and determining the next moving point until reaching the convex point B_m(ii) a If point A_jIs a salient point B_mIf so, forming a local optimal path and entering the step 5.6;

step 5.6: the leveling robot moves to the convex point B according to the local optimal path_mCarrying out leveling operation on the convex hull to be leveled;

step 6: and (5) repeating the method of the step (2) to the step (5), and carrying out planning of the local optimal path and leveling operation on the next local area until the whole granary is leveled.

2. The TOF depth camera-based flat-binning robot path planning and decision method according to claim 1, wherein said step 4 comprises the following steps:

step 4.1: initialization h_max＝h₁、m＝1、i＝2；

Step 4.2: comparison h_maxAnd h_i: if h_max>h_iThen go to step 4.3; if h_max<h_iThen let h_max＝h_iI, then go to step 4.3;

step 4.3: if i is equal to M, go to step 4.4; if i is less than M, making i equal to i +1, and returning to the step 4.2;

step 4.4: salient point B_mNamely the bump with the highest height in the bump set.

Images