Trajectory planning method, system, equipment and medium for autonomous exploration of unmanned vehicles

Abstract

The present invention proposes a trajectory planning method, system, device and medium for autonomous exploration of an unmanned vehicle. The method includes: acquiring point cloud information and a grid map to update a two-dimensional grid set and a grid state, and determining a target strategy for a target trajectory according to a state change; discretizing a control input at a first moment according to forward kinematic sampling to generate a trajectory point at a second moment; constructing an evaluation index of a trajectory point according to a cost function and a heuristic function to determine a target trajectory point at the second moment; and generating a target trajectory according to the target trajectory point and the target strategy.

Description

Technical Field

[0001] The present invention relates to the field of robot control technology, and in particular to a trajectory planning method, system, equipment and medium for autonomous exploration of an unmanned vehicle. Background Art

[0002] Existing mobile unmanned vehicles are often used in the field of autonomous exploration to perform mapping tasks, rescue missions, and exploration of unknown areas. As the complexity of application scenarios increases, higher requirements are placed on the exploration efficiency of unmanned vehicles. The exploration strategy of unmanned vehicles has a particularly important impact on the exploration efficiency.

[0003] The existing unmanned vehicle exploration strategies mainly include frontier grid-based exploration strategies and sampling-based exploration strategies. Although the above two existing exploration strategies have their own advantages, they still have defects. For example, the frontier grid-based exploration strategy needs to move back and forth in a single area, which makes it difficult to fully search all areas. Although the acquisition-based exploration strategy can effectively explore a single area, it cannot meet the needs of exploring all areas. Especially when the exploration environment area is further increased, the above exploration strategies will be severely hindered by the huge amount of calculations and cannot meet the high requirements of existing technologies for unmanned vehicle exploration strategies.

[0004] Therefore, there is an urgent need for a trajectory planning method for autonomous exploration of unmanned vehicles to solve the problem that the above exploration strategies cannot meet the actual exploration needs. Summary of the invention

[0005] In view of the deficiencies in the prior art, the present invention proposes a trajectory planning method, system, device and medium for autonomous exploration of an unmanned vehicle.

[0006] The first aspect of the present invention discloses a trajectory planning method for autonomous exploration of an unmanned vehicle, comprising:

[0007] Obtain point cloud information and grid map to update the two-dimensional grid set and grid status, and determine the target strategy of the target trajectory according to the status change;

[0008] Discretize the control input at the first moment according to the forward kinematic sampling to generate the trajectory points at the second moment;

[0009] Construct an evaluation index of the trajectory point according to the cost function and the heuristic function, and determine the target trajectory point at the second moment;

[0010] Generate a target trajectory based on the target trajectory points and the target strategy.

[0011] In an optional embodiment, the step of acquiring point cloud information and a grid map to update a two-dimensional grid set and a grid state includes:

[0012] Controlling the radar to obtain point cloud information and using a grid map to display the regional environment, taking the point cloud information as input and incrementally mapping it into the grid map;

[0013] The global grid map is continuously updated based on the point cloud information to confirm all the two-dimensional grid sets in the plane of the area where the unmanned vehicle is located, and the unknown grid sets S in the grid sets are recorded. unknow and the known grid set S know , and the union of grids with unknown states and known states is the set S of all two-dimensional grids;

[0014] For a known grid set S know The known grid states in are classified into obstacle grids and free grids, and the obstacle grid set S occu and the free grid set S free The union of is the known grid set S know .

[0015] In an optional embodiment, the target strategy for determining the target trajectory according to the state change includes: redefining the grid whose grid state is updated according to the point cloud information and recorded as a free grid, selecting a free grid adjacent to at least one unknown grid in the unknown grid set as a boundary grid, and recording the boundary grid set S frontier ;

[0016] The boundary grid set S frontier And the unknown grid set S unknow As the first constraint condition, the obstacle grid set S occu and the free grid set S free The union of is the known grid set S know As the second constraint condition, the known grid set is equivalent to the two-dimensional grid set S as the third constraint condition, and a target strategy is generated according to the first constraint condition, the second constraint condition and the third constraint condition to select the trajectory points of the target trajectory.

[0017] In an optional embodiment, the step of discretizing the control input quantity at the first moment according to forward kinematic sampling includes:

[0018] Determine the discrete time node Δt at the first moment t, through the speed discrete coefficient τ 1,i , τ 2,j The first control input is discretized to obtain an input sequence, and the second control input at the first time t is formed according to the elements of the input sequence. The first control input is expressed as:

[0019] I t ={(v i ,w j )}, where v irepresents the i-th input speed at the first moment t, w j represents the j-th input angular velocity at the first moment t, and the number of the first control input quantities is n;

[0020] The discretized first control input is expressed as:

[0021] v i =v m i n +τ 1,i ·(v max -v m i n ),τ 1,i ∈[0,1];

[0022] w j =w m i n +τ 2,j ·(w max -w m i n ),τ 2,j ∈[0,1];

[0023] Among them, v min Indicates the minimum speed of the input, v max Indicates the maximum speed of the input quantity, w min Represents the minimum angular velocity of the input, w max Represents the maximum angular velocity of the input, τ 1,i The discrete coefficient of velocity, τ 2,j The discrete coefficient representing the angular velocity;

[0024] The input sequence is expressed as:

[0025] v=[v0,v1,...,v n-1 ];

[0026] w=[w0,w1,...,w n-1 ];

[0027] Wherein, v represents the velocity element corresponding to the forward trajectory point output by the discretization processing of the first control input, w represents the angular velocity element corresponding to the forward trajectory point output by the discretization processing of the first control input, and n represents that the first control input generates n different input sequence elements at the first time t, and the input sequence elements include the velocity and angular velocity of the forward trajectory point of the unmanned vehicle;

[0028] The second control input is expressed as:

[0029] I t (i,j)={(V i ,W j )|V i ∈v,W j ∈w};

[0030] Among them, I t (i, j) represents the second control input, V i represents the i-th velocity element in the input sequence, W j Represents the jth speed element in the input sequence, where the number of second control inputs is n 2 .

[0031] In an optional embodiment, generating the trajectory point at the second moment includes:

[0032] Determine the first position of the unmanned vehicle at the first time t, according to the determined discrete time node Δt and the second control input I t (i, j) obtaining the second posture of the unmanned vehicle at the second time t+1, and obtaining the trajectory points of the unmanned vehicle from the first time t to the second time t+1 according to the first posture and the second posture;

[0033] The first posture is expressed as: P t (p t ,θ t ), where p t represents the position coordinates of the unmanned vehicle at the first moment t in global coordinates, θ t represents the turning angle of the unmanned vehicle at the first moment t;

[0034] The second posture is expressed as: P t+1 (p t+1 ,θ t+1 ), the calculation formula of the second posture is:

[0035] P t+1 (p t+1 ,θ t+1 )=P t (p t ,θ t )+Δt·I t (i, j), where Δt represents a discrete time node, and Δt is directly proportional to the exploration duration, and inversely proportional to the exploration efficiency.

[0036] In an optional embodiment, constructing an evaluation index of the trajectory point according to the cost function and the heuristic function to determine the target trajectory point at the second moment includes:

[0037] The dynamic cost function is constructed according to the minimum number of the second control input before obtaining the second posture, and the dynamic cost function is expressed as:

[0038]

[0039] Among them, α1 and α2 are weight factors;

[0040] The safe distance of the trajectory point is defined according to the distance check between the unknown grid and the obstacle grid, and the collision cost function is constructed by the safe distance. The collision cost function is expressed as:

[0041]

[0042] Among them, P t+1 represents the second posture, e represents the natural constant, d c Indicates the safety distance, d real Indicates the actual detection distance;

[0043] The boundary heuristic function is constructed according to the preset area search gain to minimize the number of round trip searches of the unmanned vehicle. The boundary heuristic function is expressed as:

[0044]

[0045] in, Denotes the second pose P t+1 The distance from the search boundary in the x-axis direction of the two-dimensional plane, Denotes the second pose P t+1 The distance from the search boundary in the y-axis direction of the two-dimensional plane;

[0046] The number of unknown grids observable by the unmanned vehicle at the second pose is used as the information gain to construct a gain heuristic function, which is expressed as:

[0047] g info =GetInfo(P t+1 ,S frontier )

[0048] Among them, GetInfo(P t+1 ,S frontier ) represents the number of boundary grids observable by the unmanned vehicle at the second position, S frontier Represents a boundary grid element in a boundary grid collection;

[0049] Corresponding weights are set for the dynamic cost function, collision cost function, boundary heuristic function, and gain heuristic function, and an evaluation index function of the state quantity of the unmanned vehicle in the second posture at the second moment is constructed to evaluate the optional trajectory points in the second posture. The evaluation index function is expressed as:

[0050] f total,i,j (P t+1 )=w1f dyn (I t (i,j))+w2f colli (P t+1 )+w3g barr (P t+1 )-w4g info (P t+1 )

[0051] Among them, w1 represents the weight of the dynamics cost function, w2 represents the weight of the collision cost function, w3 represents the weight of the boundary heuristic function, and w4 represents the weight of the gain heuristic function.

[0052] In an optional embodiment, generating a target trajectory according to the target trajectory point and the target strategy includes:

[0053] According to the optional trajectory points filtered by the evaluation index function, the trajectory point with the smallest value is selected as the next target trajectory point of the unmanned vehicle, and the target trajectory point is added to the path queue;

[0054] The global grid map is traversed until the trajectory points of the path queue satisfy the first constraint, the second constraint and the third constraint in the target strategy, and then the target path is obtained from the path queue.

[0055] The second aspect of the present invention discloses a trajectory planning system for autonomous exploration of an unmanned vehicle, the system comprising:

[0056] Map exploration module, used to obtain point cloud information and grid map to update the two-dimensional grid set and grid status, and determine the target strategy of the target trajectory according to the status change;

[0057] A viewpoint sampling module, used to discretize the control input at the first moment according to the forward kinematic sampling, and generate the trajectory points at the second moment;

[0058] A cost calculation module is used to construct an evaluation index of the trajectory point according to the cost function and the heuristic function, and determine the target trajectory point at the second moment;

[0059] The trajectory generation module is used to generate a target trajectory according to the target trajectory points and the target strategy.

[0060] In an optional embodiment, the map exploration module includes:

[0061] An information acquisition module, used to control the radar to obtain point cloud information, and to use a grid map to display the regional environment, taking the point cloud information as input and incrementally mapping it to the grid map;

[0062] The global grid map is continuously updated based on the point cloud information to confirm all the two-dimensional grid sets in the plane of the area where the unmanned vehicle is located, and the unknown grid sets S in the grid sets are recorded. unknow and the known grid set S know , and the union of grids with unknown states and known states is the set S of all two-dimensional grids;

[0063] For a known grid set S know The known grid states in are classified into obstacle grids and free grids, and the obstacle grid set S occu and the free grid set S free The union of is the known grid set S know .

[0064] In an optional embodiment, the map exploration module includes:

[0065] The strategy setting module is used to redefine the grids that update the grid state according to the point cloud information and record them as free grids, select at least one free grid adjacent to an unknown grid in the unknown grid set as a boundary grid, and record the boundary grid set S frontier ;

[0066] The boundary grid set S frontier And the unknown grid set S unknow As the first constraint condition, the obstacle grid set S occu and the free grid set S free The union of is the known grid set S know As the second constraint condition, the known grid set is equivalent to the two-dimensional grid set S as the third constraint condition, and a target strategy is generated according to the first constraint condition, the second constraint condition and the third constraint condition to select the trajectory points of the target trajectory.

[0067] In an optional embodiment, the viewpoint sampling module includes:

[0068] The forward sampling module is used to determine the discrete time node Δt at the first moment t, through the speed discrete coefficient τ 1,i , τ 2,j The first control input is discretized to obtain an input sequence, and the second control input at the first time t is formed according to the elements of the input sequence. The first control input is expressed as:

[0069] I t ={(v i ,w j )}, where v i represents the i-th input speed at the first moment t, w j represents the j-th input angular velocity at the first moment t, and the number of the first control input quantities is n;

[0070] The discretized first control input is expressed as:

[0071] v i =v m i n +τ 1,i ·(v max -v m i n ),τ 1,i ∈[0,1];

[0072] w j =w m i n +τ 2,j ·(w max -w m i n ),τ 2,j ∈[0,1];

[0073] Among them, v min Indicates the minimum speed of the input, v max Indicates the maximum speed of the input quantity, w min Represents the minimum angular velocity of the input, w max Represents the maximum angular velocity of the input, τ 1,i The discrete coefficient of velocity, τ 2,j The discrete coefficient representing the angular velocity;

[0074] The input sequence is expressed as:

[0075] v=[v0,v1,...,v n-1 ];

[0076] w=[w0,w1,...,w n-1 ];

[0077] Wherein, v represents the velocity element corresponding to the forward trajectory point output by the discretization processing of the first control input, w represents the angular velocity element corresponding to the forward trajectory point output by the discretization processing of the first control input, and n represents that the first control input generates n different input sequence elements at the first time t, and the input sequence elements include the velocity and angular velocity of the forward trajectory point of the unmanned vehicle;

[0078] The second control input is expressed as:

[0079] I t (i,j)={(V i ,W j )|Vi ∈v,W j ∈w};

[0080] Among them, I t (i, j) represents the second control input, V i represents the i-th velocity element in the input sequence, W j Represents the jth speed element in the input sequence, where the number of second control inputs is n 2 .

[0081] In an optional embodiment, the viewpoint sampling module includes:

[0082] The sampling submodule is used to determine the first position of the unmanned vehicle at the first time t, according to the determined discrete time node Δt and the second control input quantity I t (i, j) obtaining the second posture of the unmanned vehicle at the second time t+1, and obtaining the trajectory points of the unmanned vehicle from the first time t to the second time t+1 according to the first posture and the second posture;

[0083] The first posture is expressed as: P t (p t ,θ t ), where p t represents the position coordinates of the unmanned vehicle at the first moment t in global coordinates, θ t represents the turning angle of the unmanned vehicle at the first moment t;

[0084] The second posture is expressed as: P t+1 (p t+1 ,θ t+1 ), the calculation formula of the second posture is:

[0085] P t+1 (p t+1 ,θ t+1 )=P t (p t ,θ t )+Δt·I t (i, j), where Δt represents a discrete time node, and Δt is directly proportional to the exploration duration, and inversely proportional to the exploration efficiency.

[0086] In an optional embodiment, the cost calculation module includes:

[0087] The cost calculation submodule is used to construct a dynamic cost function according to the minimum number of the second control input before obtaining the second posture. The dynamic cost function is expressed as:

[0088]

[0089] Among them, α1 and α2 are weight factors;

[0090] The safe distance of the trajectory point is defined according to the distance check between the unknown grid and the obstacle grid, and the collision cost function is constructed by the safe distance. The collision cost function is expressed as:

[0091]

[0092] Among them, P t+1 represents the second posture, e represents the natural constant, d c Indicates the safety distance, d real Indicates the actual detection distance;

[0093] The boundary heuristic function is constructed according to the preset area search gain to minimize the number of round trip searches of the unmanned vehicle. The boundary heuristic function is expressed as:

[0094]

[0095] in, Denotes the second pose P t+1 The distance from the search boundary in the x-axis direction of the two-dimensional plane, Denotes the second pose P t+1 The distance from the search boundary in the y-axis direction of the two-dimensional plane;

[0096] The number of unknown grids observable by the unmanned vehicle at the second pose is used as the information gain to construct a gain heuristic function, which is expressed as:

[0097] g info =GetInfo(P t+1 ,S frontier );

[0098] Among them, GetInfo(P t+1 ,S frontier ) represents the number of boundary grids observable by the unmanned vehicle at the second position, S frontier Represents a boundary grid element in a boundary grid collection;

[0099] Corresponding weights are set for the dynamic cost function, collision cost function, boundary heuristic function, and gain heuristic function, and an evaluation index function of the state quantity of the unmanned vehicle in the second posture at the second moment is constructed to evaluate the optional trajectory points in the second posture. The evaluation index function is expressed as:

[0100] f total,i,j (P t+1 )=w1f dyn (I t (i,j))+w2f colli (P t+1)+w3g barr (P t+1 )-w4g info (P t+1 )

[0101] Among them, w1 represents the weight of the dynamics cost function, w2 represents the weight of the collision cost function, w3 represents the weight of the boundary heuristic function, and w4 represents the weight of the gain heuristic function.

[0102] In an optional embodiment, the trajectory generation module includes:

[0103] The trajectory generation submodule is used to select the trajectory point with the smallest value as the next target trajectory point of the unmanned vehicle according to the optional trajectory points filtered by the evaluation index function, and add the target trajectory point to the path queue;

[0104] The global grid map is traversed until the trajectory points of the path queue satisfy the first constraint, the second constraint and the third constraint in the target strategy, and then the target path is obtained from the path queue.

[0105] The third aspect of the present invention discloses a trajectory planning device for autonomous exploration of an unmanned vehicle, comprising:

[0106] at least one processor, and

[0107] a memory communicatively connected to the at least one processor; wherein,

[0108] The memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the trajectory planning method for autonomous exploration of an unmanned vehicle as described in any one of the first aspects of the present invention.

[0109] The fourth aspect of the present invention discloses a computer-readable storage medium, which stores computer-executable instructions, and the computer-executable instructions are used to enable a computer to execute the trajectory planning method for autonomous exploration of an unmanned vehicle as described in any one of the items disclosed in the first aspect of the present invention.

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

[0111] The present invention proposes a trajectory planning method, system, device and medium for autonomous exploration of an unmanned vehicle, wherein the method includes obtaining point cloud information and a grid map to update a two-dimensional grid set and a grid state, determining a target strategy for a target trajectory according to a state change; discretizing a control input at a first moment according to forward kinematic sampling to generate a trajectory point at a second moment; constructing an evaluation index of a trajectory point according to a cost function and an heuristic function to determine a target trajectory point at the second moment; and generating a target trajectory according to the target trajectory point and the target strategy. A plurality of trajectory points are obtained by adopting an unmanned vehicle kinematic forward sampling method, and a trajectory point evaluation index is established in order to fully explore the space, and the optimal trajectory point is selected to accelerate the exploration efficiency of the unknown space, and a search area boundary heuristic item is introduced to guide the unmanned vehicle to fully explore the boundary area to avoid a large range of return. Since the trajectory points are obtained by discrete forward kinematics, the trajectory points generated thereby can be directly run without wasting a large amount of resources for trajectory optimization, and can generate an unmanned vehicle trajectory that can be quickly executed by the unmanned vehicle to fully explore the unknown environment in real time, while solving the problems of insufficient exploration and low exploration efficiency, and can meet the high requirements of existing unmanned vehicle exploration, and has a high application value in the field of robot control. BRIEF DESCRIPTION OF THE DRAWINGS

[0112] 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 labor.

[0113] Figure 1 A flow chart of a trajectory planning method for autonomous exploration of an unmanned vehicle of the present invention;

[0114] Figure 2 Schematic diagram of the trajectory planning system for autonomous exploration of the unmanned vehicle of the present invention. DETAILED DESCRIPTION

[0115] In order to make the purpose, technical solutions and advantages of the present application clearer, the present application is described and illustrated below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not intended to limit the present application. Based on the embodiments provided in the present application, all other embodiments obtained by ordinary technicians in the field without making creative work are within the scope of protection of the present application.

[0116] Obviously, the drawings described below are only some examples or embodiments of the present application. For ordinary technicians in this field, the present application can also be applied to other similar scenarios based on these drawings without creative work. In addition, it can also be understood that although the efforts made in this development process may be complicated and lengthy, for ordinary technicians in this field related to the content disclosed in this application, some changes in design, manufacturing or production based on the technical content disclosed in this application are just conventional technical means, and should not be understood as insufficient content disclosed in this application.

[0117] Reference to "embodiments" in this application means that a particular feature, structure, or characteristic described in conjunction with the embodiments may be included in at least one embodiment of the present application. The appearance of the phrase in various locations in the specification does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment that is mutually exclusive with other embodiments. It is explicitly and implicitly understood by those of ordinary skill in the art that the embodiments described in this application may be combined with other embodiments without conflict.

[0118] Unless otherwise defined, the technical terms or scientific terms involved in this application should be understood by people with ordinary skills in the technical field to which this application belongs. The words "one", "a", "a", "the" and the like involved in this application do not indicate a quantitative limitation, and may represent the singular or plural. The terms "include", "comprise", "have" and any of their variations involved in this application are intended to cover non-exclusive inclusions; for example, a process, method, system, product or device that includes a series of steps or modules (units) is not limited to the listed steps or units, but may also include steps or units that are not listed, or may also include other steps or units inherent to these processes, methods, products or devices. The words "connect", "connected", "coupled" and the like involved in this application are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. The "multiple" involved in this application refers to two or more. "And / or" describes the association relationship of associated objects, indicating that there may be three relationships, for example, "A and / or B" can represent: A exists alone, A and B exist at the same time, and B exists alone. The character " / " generally indicates that the objects before and after are in an "or" relationship. The terms "first", "second", "third", etc. involved in this application are only used to distinguish similar objects and do not represent a specific ordering of the objects.

[0119] It should be noted that the current exploration strategies of unmanned vehicles are mainly divided into two categories. The first strategy is an exploration strategy based on the frontier grid. The frontier point is defined as a free grid with at least one obstacle grid. Information evaluation is performed on the boundary grids, and a greedy algorithm is used to guide the unmanned vehicle to conduct autonomous exploration until all boundaries are explored. The second strategy is a sampling-based exploration strategy, which uses a random sampling method to sample trajectory points and expand the search tree. Information evaluation is performed on the tree nodes to obtain the next expansion node until the expansion ends when there is no frontier.

[0120] Specifically, the above two methods have their own advantages in the exploration strategy of unmanned vehicles, but in the actual exploration process, the exploration strategy based on the frontier grid is a large environment composed of separate areas. Since the unmanned vehicle needs to move back and forth between areas, it is difficult to fully explore the area. The sampling-based exploration strategy shows great potential and can effectively explore a single area, but its disadvantage is that it cannot explore all areas, so it may be hindered in a large environment due to its huge computational workload.

[0121] Furthermore, in response to the defects of existing unmanned vehicle autonomous exploration strategies, the present invention proposes a trajectory planning method for unmanned vehicle autonomous exploration to meet the high requirements of existing exploration. A plurality of trajectory points are obtained by adopting the unmanned vehicle kinematic forward sampling method. In order to fully explore the space, a trajectory point evaluation index is established, the optimal trajectory point is selected to accelerate the exploration efficiency of the unknown space, and a search area boundary heuristic item is introduced to guide the unmanned vehicle to fully explore the boundary area to avoid large-scale return. Since the trajectory points are obtained by discrete forward kinematics, the trajectory points generated thereby can be directly run without wasting a lot of resources for trajectory optimization. The unmanned vehicle trajectory that can be quickly executed by the unmanned vehicle to fully explore the unknown environment can be generated in real time, while solving the problems of insufficient exploration and low exploration efficiency. It can meet the high requirements of existing unmanned vehicle exploration and has high application value in the field of robot control. The specific technical solutions will be explained one by one in the embodiments below.

[0122] Example 1

[0123] See also Figure 1 The embodiment of the present invention discloses a trajectory planning method for autonomous exploration of an unmanned vehicle, including:

[0124] 101. Obtain point cloud information and grid map to update the two-dimensional grid set and grid state, and determine the target strategy of the target trajectory according to the state change;

[0125] In an optional embodiment, the step of acquiring point cloud information and a grid map to update a two-dimensional grid set and a grid state includes:

[0126] Controlling the radar to obtain point cloud information and using a grid map to display the regional environment, taking the point cloud information as input and incrementally mapping it into the grid map;

[0127] The global grid map is continuously updated based on the point cloud information to confirm all the two-dimensional grid sets in the plane of the area where the unmanned vehicle is located, and the unknown grid sets S in the grid sets are recorded. unknow and the known grid set S know , and the union of grids with unknown states and known states is the set S of all two-dimensional grids;

[0128] For a known grid set S know The known grid states in are classified into obstacle grids and free grids, and the obstacle grid set S occu and the free grid set S free The union of is the known grid set S know .

[0129] Specifically, laser radar or other radar sensors can be installed on unmanned vehicles, mobile robots and other equipment to collect object information to present a series of scattered points with accurate angle and distance information, thereby obtaining point cloud information, where each point of the point cloud data can contain coordinates in a three-dimensional space, and the grid map of the exploration area can be obtained by means of map scanning and remote sensing technology. It is easy for technicians in this field to know that the plane of the global map can be divided into several grids in the space scanned by the laser radar, and the present application mainly solves the problem of trajectory points in exploration planning. Therefore, the coordinates in the Z-axis direction in the point cloud data are not considered, and only the entire The local map is represented as a two-dimensional grid to facilitate the subsequent exploration of the target trajectory points and reduce the computational consumption. The global map can be regarded as a number of two-dimensional grids. As the unmanned vehicle explores, the collected point cloud information is continuously divided into grid states by incremental projection. The area that the unmanned vehicle has not explored does not obtain point cloud information, so the grid state is unknown, and the explored area is known. Therefore, when the state of all two-dimensional grids becomes a known grid, the exploration task is completed. In order to facilitate the further planning of the unmanned vehicle's trajectory points, the obstacles in the known grids are identified, and the grids with obstacles are recorded as obstacle grids S. occu , and the grid without obstacles is recorded as free grid S free , thereby ensuring that the exploration process generates efficient and comprehensive exploration trajectory points as much as possible.

[0130] In an optional embodiment, the target strategy for determining the target trajectory according to the state change includes: redefining the grid whose grid state is updated according to the point cloud information and recorded as a free grid, selecting a free grid adjacent to at least one unknown grid in the unknown grid set as a boundary grid, and recording the boundary grid set S frontier ;

[0131] The boundary grid set S frontier And the unknown grid set S unknow As the first constraint condition, the obstacle grid set S occu and the free grid set S free The union of is the known grid set S know As the second constraint condition, the known grid set is equivalent to the two-dimensional grid set S as the third constraint condition, and a target strategy is generated according to the first constraint condition, the second constraint condition and the third constraint condition to select the trajectory points of the target trajectory.

[0132] Specifically, in the actual exploration process of the unmanned vehicle, due to the change in the state of the recognition grid, a boundary grid will appear between the unknown grid area and the known grid area. Setting the boundary grid can enable the unmanned vehicle to explore the boundary grid as a goal until all unknown grids are explored, and the exploration task can be completed. It can be understood that when the two-dimensional grid set of the global map is in the initial state, the number of unknown grids is equal to the number of all two-dimensional grids, and the set of known grids and boundary grids is an empty set. Therefore, the exploration problem of the unmanned vehicle is modeled, and a target strategy that needs to be satisfied is added to the solved unmanned vehicle trajectory point queue. When the sets of unknown grids and boundary grids are both empty sets, and the union of the sets of obstacle grids and free grids is equal to the set of known grids, and the set of known grids is equal to the set of two-dimensional grids, the initial goal of the exploration task, that is, to completely explore the regional environment, can be completed. However, in order to meet the high requirements of existing unmanned vehicle exploration, the generated trajectory queue needs to be further screened. The specific screening process is shown in the implementation method below.

[0133] 102. Discretize the control input at the first moment according to the forward kinematics sampling to generate the trajectory points at the second moment;

[0134] In an optional embodiment, the step of discretizing the control input quantity at the first moment according to forward kinematic sampling includes:

[0135] Determine the discrete time node Δt at the first moment t, through the speed discrete coefficient τ 1,i , τ 2,j The first control input is discretized to obtain an input sequence, and the second control input at the first time t is formed according to the elements of the input sequence. The first control input is expressed as:

[0136] I t ={(v i ,w j )}, where v i represents the i-th input speed at the first moment t, w j represents the j-th input angular velocity at the first moment t, and the number of the first control input quantities is n;

[0137] The discretized first control input is expressed as:

[0138] v i =v m i n +τ 1,i ·(v max -v m i n ),τ 1,i ∈[0,1];

[0139] w j =w m i n +τ 2,j ·(w max -w m i n ),τ 2,j ∈[0,1];

[0140] Among them, v min Indicates the minimum speed of the input, v max Indicates the maximum speed of the input quantity, w min Represents the minimum angular velocity of the input, w max Represents the maximum angular velocity of the input, τ 1,i The discrete coefficient of velocity, τ 2,j The discrete coefficient representing the angular velocity;

[0141] The input sequence is expressed as:

[0142] v=[v0,v1,...,v n-1 ];

[0143] w=[w0,w1,...,w n-1 ];

[0144] Wherein, v represents the velocity element corresponding to the forward trajectory point output by the discretization processing of the first control input, w represents the angular velocity element corresponding to the forward trajectory point output by the discretization processing of the first control input, and n represents that the first control input generates n different input sequence elements at the first time t, and the input sequence elements include the velocity and angular velocity of the forward trajectory point of the unmanned vehicle;

[0145] The second control input is expressed as:

[0146] I t (i,j)={(V i ,W j )|Vi ∈v,W j ∈w};

[0147] Among them, I t (i, j) represents the second control input, V i represents the i-th velocity element in the input sequence, W j Represents the jth speed element in the input sequence, where the number of second control inputs is n 2 .

[0148] In an optional embodiment, generating the trajectory point at the second moment includes:

[0149] Determine the first position of the unmanned vehicle at the first time t, according to the determined discrete time node Δt and the second control input I t (i, j) obtaining the second posture of the unmanned vehicle at the second time t+1, and obtaining the trajectory points of the unmanned vehicle from the first time t to the second time t+1 according to the first posture and the second posture;

[0150] The first posture is expressed as: P t (p t ,θ t ), where p t represents the position coordinates of the unmanned vehicle at the first moment t in global coordinates, θ t represents the turning angle of the unmanned vehicle at the first moment t;

[0151] The second posture is expressed as: P t+1 (p t+1 ,θ t+1 ), the calculation formula of the second posture is:

[0152] P t+1 (p t+1 ,θ t+1 )=P t (p t ,θ t )+Δt·I t (i, j), where Δt represents a discrete time node, and Δt is directly proportional to the exploration duration, and inversely proportional to the exploration efficiency.

[0153] Specifically, for the exploration problem, the trajectory points must not only consider the collision information, but the selection of trajectory points determines the efficiency of the exploration. Here, a method based on the forward kinematics sampling of the unmanned vehicle to generate viewpoints is proposed. The discrete unmanned vehicle control quantity is used to obtain the next state. The viewpoints generated this time are consistent with the kinematics of the unmanned vehicle, and trajectory optimization can be directly executed by the unmanned vehicle without spending a huge amount of computation.

[0154] Furthermore, before sampling the trajectory points, the position information and control input of the unmanned vehicle need to be input. The position of the unmanned vehicle is determined by the coordinates of the unmanned vehicle in the first pose and the body angle of the unmanned vehicle at the corresponding moment, and the control input is expressed as the speed and angular velocity of the unmanned vehicle at the first pose. The difference in speed and angular velocity will affect the position and angle of the generated second pose, so that the position of the unmanned vehicle at the next moment can be continuously calculated. The viewpoint of the unmanned vehicle at any time can be obtained according to the pose, and the exploration situation of the unmanned vehicle can be determined in real time, especially for the setting of discrete time nodes. If precise search is required, time nodes with smaller values ​​can be defined to improve the exploration efficiency of the unmanned vehicle by limiting the exploration time. Otherwise, the discrete time nodes are adjusted according to the exploration needs to achieve the purpose of maximizing exploration efficiency and full exploration.

[0155] Furthermore, when n speed parameters and angular velocity parameters of the first control inputs are used as inputs, since the control inputs of the unmanned vehicle are discretized, the generated input queue contains n speed inputs and n angular velocity inputs. Therefore, the number of second control inputs generated at the first moment t is a combination of n speed inputs and n angular velocity inputs, that is, the number of second control inputs is n. 2 , where the value of n is adjusted according to the actual situation and exploration needs of the exploration. In the specific implementation method below, some evaluation indicators will be set to balance the value of n, so as to achieve the purpose of maximizing the exploration efficiency and sufficient exploration. After obtaining the second control input between the first moment and the second moment, the position of the unmanned vehicle at the second moment (i.e., the next moment) can be calculated according to the position of the unmanned vehicle at the first moment and the defined discrete time nodes. It can be understood that the first moment is the current moment, and the second moment is the next moment of the current moment. The specific time interval is adjusted according to the needs of the exploration strategy for the increased actual value of the unit time. Therefore, the optional position of the unmanned vehicle at the next moment can be obtained by the above method at any time, thereby avoiding the problem of insufficient exploration.

[0156] 103. Construct an evaluation index of the trajectory point according to the cost function and the heuristic function, and determine the target trajectory point at the second moment;

[0157] In an optional embodiment, constructing an evaluation index of the trajectory point according to the cost function and the heuristic function to determine the target trajectory point at the second moment includes:

[0158] The dynamic cost function is constructed according to the minimum number of the second control input before obtaining the second posture, and the dynamic cost function is expressed as:

[0159]

[0160] Among them, α1 and α2 are weight factors;

[0161] The safe distance of the trajectory point is defined according to the distance check between the unknown grid and the obstacle grid, and the collision cost function is constructed by the safe distance. The collision cost function is expressed as:

[0162]

[0163] Among them, P t+1 represents the second posture, e represents the natural constant, d c Indicates the safety distance, d real Indicates the actual detection distance;

[0164] The boundary heuristic function is constructed according to the preset area search gain to minimize the number of round trip searches of the unmanned vehicle. The boundary heuristic function is expressed as:

[0165]

[0166] in, Denotes the second pose P t+1 The distance from the search boundary in the x-axis direction of the two-dimensional plane, Denotes the second pose P t+1 The distance from the search boundary in the y-axis direction of the two-dimensional plane;

[0167] The number of unknown grids observable by the unmanned vehicle at the second pose is used as the information gain to construct a gain heuristic function, which is expressed as:

[0168] g info =GetInfo(P t+1 ,S frontier )

[0169] Among them, GetInfo(P t+1 ,S frontier ) represents the number of boundary grids observable by the unmanned vehicle at the second position, S frontier Represents a boundary grid element in a boundary grid collection;

[0170] Corresponding weights are set for the dynamic cost function, collision cost function, boundary heuristic function, and gain heuristic function, and an evaluation index function of the state quantity of the unmanned vehicle in the second posture at the second moment is constructed to evaluate the optional trajectory points in the second posture. The evaluation index function is expressed as:

[0171] f total,i,j (P t+1 )=w1f dyn (I t (i,j))+w2f colli (P t+1 )+w3g barr (Pt+1 )-w4g info (P t+1 )Wherein, w1 represents the weight of the dynamics cost function, w2 represents the weight of the collision cost function, w3 represents the weight of the boundary heuristic function, and w4 represents the weight of the gain heuristic function.

[0172] Specifically, in step 103, the posture of the unmanned vehicle at the second moment (i.e., the next moment) is solved, that is, several optional unmanned vehicle viewpoints can be obtained. In order to further optimize the feasibility of the trajectory, it is necessary to comprehensively consider the kinematic cost, safety cost, search efficiency and information gain of the trajectory point. For the problem of kinematic cost, a dynamic cost function is defined to penalize the input of the unmanned vehicle. The purpose of this function is to hope that the unmanned vehicle can obtain the next state (posture at the next moment) with the least input as possible, thereby improving the efficiency of exploration. For the problem of safety cost, in order to ensure the safety of the path point, a safety distance d is defined c , and check the nearest unknown sang unkown 、Obstacle Sang occu , assuming that the distance between the two types of shanges closest to the unmanned vehicle is d real , the penalty cost function can be used to avoid collisions between unmanned vehicles, thereby improving the safety of exploration. For the search efficiency problem, the regional search gain is set to guide the unmanned vehicle to fully search the current area, avoid the phenomenon of round-trip search to improve the search efficiency. For the information gain problem, in order to explore as many unknown grids as possible, the number of unknown grids that can be observed without obstruction within the effective range of the sensor in the second posture is used as the information gain to avoid the problem of insufficient exploration as much as possible. Finally, the above function is constructed as a unified evaluation index to evaluate the second posture of the unmanned vehicle at the second moment to select the optimal trajectory point. It should be noted that the weight assigned to each function can be adjusted according to the actual situation. For example, for different environmental areas, whether it focuses more on exploration efficiency or exploration safety, technicians in this field can make adjustments.

[0173] 104. Generate a target trajectory according to the target trajectory points and the target strategy.

[0174] In an optional embodiment, generating a target trajectory according to the target trajectory point and the target strategy includes:

[0175] According to the optional trajectory points filtered by the evaluation index function, the trajectory point with the smallest value is selected as the next target trajectory point of the unmanned vehicle, and the target trajectory point is added to the path queue;

[0176] The global grid map is traversed until the trajectory points of the path queue satisfy the first constraint, the second constraint and the third constraint in the target strategy, and then the target path is obtained from the path queue.

[0177] Specifically, in the exploration phase, each next-moment trajectory point of the unmanned vehicle is screened to select the optimal trajectory point and save it in the path queue. This method sets a target strategy that needs to meet the exploration of the entire area. When the generated trajectory points are added to the path queue one by one and finally meet the target strategy, the trajectory queue is the optimal target path for the unmanned vehicle to explore autonomously. This process adjusts the unmanned vehicle in real time to generate an unmanned vehicle trajectory that can be directly executed to fully explore the unknown environment. The trajectory point can be directly run without wasting a lot of resources for trajectory optimization, which can meet the high requirements of existing unmanned vehicle exploration and has high application value in the field of robot control.

[0178] like Figure 2 As shown, the second aspect of the present invention discloses a trajectory planning system for autonomous exploration of an unmanned vehicle, the system comprising:

[0179] Map exploration module, used to obtain point cloud information and grid map to update the two-dimensional grid set and grid status, and determine the target strategy of the target trajectory according to the status change;

[0180] A viewpoint sampling module, used to discretize the control input at the first moment according to the forward kinematic sampling, and generate the trajectory points at the second moment;

[0181] A cost calculation module is used to construct an evaluation index of the trajectory point according to the cost function and the heuristic function, and determine the target trajectory point at the second moment;

[0182] The trajectory generation module is used to generate a target trajectory according to the target trajectory points and the target strategy.

[0183] In an optional embodiment, the map exploration module includes:

[0184] An information acquisition module, used to control the radar to obtain point cloud information, and to use a grid map to display the regional environment, taking the point cloud information as input and incrementally mapping it to the grid map;

[0185] The global grid map is continuously updated based on the point cloud information to confirm all the two-dimensional grid sets in the plane of the area where the unmanned vehicle is located, and the unknown grid sets S in the grid sets are recorded. unknow and the known grid set S know , and the union of grids with unknown states and known states is the set S of all two-dimensional grids;

[0186] For a known grid set S know The known grid states in are classified into obstacle grids and free grids, and the obstacle grid set S occu and the free grid set S free The union of is the known grid set S know.

[0187] In an optional embodiment, the map exploration module includes:

[0188] The strategy setting module is used to redefine the grids that update the grid state according to the point cloud information and record them as free grids, select at least one free grid adjacent to an unknown grid in the unknown grid set as a boundary grid, and record the boundary grid set S frontier ;

[0189] The boundary grid set S frontier And the unknown grid set S unknow As the first constraint condition, the obstacle grid set S occu and the free grid set S free The union of is the known grid set S know As the second constraint condition, the known grid set is equivalent to the two-dimensional grid set S as the third constraint condition, and a target strategy is generated according to the first constraint condition, the second constraint condition and the third constraint condition to select the trajectory points of the target trajectory.

[0190] In an optional embodiment, the viewpoint sampling module includes:

[0191] The forward sampling module is used to determine the discrete time node Δt at the first moment t, through the speed discrete coefficient τ 1,i , τ 2,j The first control input is discretized to obtain an input sequence, and the second control input at the first time t is formed according to the elements of the input sequence. The first control input is expressed as:

[0192] I t ={(v i ,w j )}, where v i represents the i-th input speed at the first moment t, w j represents the j-th input angular velocity at the first moment t, and the number of the first control input quantities is n;

[0193] The discretized first control input is expressed as:

[0194] v i =v m i n +τ 1,i ·(v max -v m i n ),τ 1,i ∈[0,1];

[0195] wj =w m i n +τ 2,j ·(w max -w m i n ),τ 2,j ∈[0,1];

[0196] Among them, v min Indicates the minimum speed of the input, v max Indicates the maximum speed of the input quantity, w min Represents the minimum angular velocity of the input, w max Represents the maximum angular velocity of the input, τ 1,i The discrete coefficient of velocity, τ 2,j The discrete coefficient representing the angular velocity;

[0197] The input sequence is expressed as:

[0198] v=[v0,v1,...,v n-1 ];

[0199] w=[w0,w1,...,w n-1 ];

[0200] Wherein, v represents the velocity element corresponding to the forward trajectory point output by the discretization processing of the first control input, w represents the angular velocity element corresponding to the forward trajectory point output by the discretization processing of the first control input, and n represents that the first control input generates n different input sequence elements at the first time t, and the input sequence elements include the velocity and angular velocity of the forward trajectory point of the unmanned vehicle;

[0201] The second control input is expressed as:

[0202] I t (i,j)={(V i ,W j )|V i ∈v,W j ∈w};

[0203] Among them, I t (i, j) represents the second control input, V i represents the i-th velocity element in the input sequence, W j Represents the jth speed element in the input sequence, where the number of second control inputs is n 2 .

[0204] In an optional embodiment, the viewpoint sampling module includes:

[0205] The sampling submodule is used to determine the first position of the unmanned vehicle at the first time t, according to the determined discrete time node Δt and the second control input quantity I t (i, j) obtaining the second posture of the unmanned vehicle at the second time t+1, and obtaining the trajectory points of the unmanned vehicle from the first time t to the second time t+1 according to the first posture and the second posture;

[0206] The first posture is expressed as: P t (p t ,θ t ), where p t represents the position coordinates of the unmanned vehicle at the first moment t in global coordinates, θ t represents the turning angle of the unmanned vehicle at the first moment t;

[0207] The second posture is expressed as: P t+1 (p t+1 ,θ t+1 ), the calculation formula of the second posture is:

[0208] P t+1 (p t+1 ,θ t+1 )=P t (p t ,θ t )+Δt·I t (i, j), where Δt represents a discrete time node, and Δt is directly proportional to the exploration duration, and inversely proportional to the exploration efficiency.

[0209] In an optional embodiment, the cost calculation module includes:

[0210] The cost calculation submodule is used to construct a dynamic cost function according to the minimum number of the second control input before obtaining the second posture. The dynamic cost function is expressed as:

[0211]

[0212] Among them, α1 and α2 are weight factors;

[0213] The safe distance of the trajectory point is defined according to the distance check between the unknown grid and the obstacle grid, and the collision cost function is constructed by the safe distance. The collision cost function is expressed as:

[0214]

[0215] Among them, P t+1 represents the second posture, e represents the natural constant, d c Indicates the safety distance, d real Indicates the actual detection distance;

[0216] The boundary heuristic function is constructed according to the preset area search gain to minimize the number of round trip searches of the unmanned vehicle. The boundary heuristic function is expressed as:

[0217]

[0218] in, Denotes the second pose P t+1 The distance from the search boundary in the x-axis direction of the two-dimensional plane, Denotes the second pose P t+1 The distance from the search boundary in the y-axis direction of the two-dimensional plane;

[0219] The number of unknown grids observable by the unmanned vehicle at the second pose is used as the information gain to construct a gain heuristic function, which is expressed as:

[0220] g info =GetInfo(P t+1 ,S frontier );

[0221] Among them, GetInfo(P t+1 ,S frontier ) represents the number of boundary grids observable by the unmanned vehicle at the second position, S frontier Represents a boundary grid element in a boundary grid collection;

[0222] Corresponding weights are set for the dynamic cost function, collision cost function, boundary heuristic function, and gain heuristic function, and an evaluation index function of the state quantity of the unmanned vehicle in the second posture at the second moment is constructed to evaluate the optional trajectory points in the second posture. The evaluation index function is expressed as:

[0223] f total,i,j (P t+1 )=w1f dyn (I t (i,j))+w2f colli (P t+1 )+w3g barr (P t+1 )-w4g info (P t+1 )

[0224] Among them, w1 represents the weight of the dynamics cost function, w2 represents the weight of the collision cost function, w3 represents the weight of the boundary heuristic function, and w4 represents the weight of the gain heuristic function.

[0225] In an optional embodiment, the trajectory generation module includes:

[0226] The trajectory generation submodule is used to select the trajectory point with the smallest value as the next target trajectory point of the unmanned vehicle according to the optional trajectory points filtered by the evaluation index function, and add the target trajectory point to the path queue;

[0227] The global grid map is traversed until the trajectory points of the path queue satisfy the first constraint, the second constraint and the third constraint in the target strategy, and then the target path is obtained from the path queue.

[0228] The third aspect of the present invention discloses a trajectory planning device for autonomous exploration of an unmanned vehicle, comprising:

[0229] at least one processor, and

[0230] a memory communicatively connected to the at least one processor; wherein,

[0231] The memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the trajectory planning method for autonomous exploration of an unmanned vehicle as described in any one of the first aspects of the present invention.

[0232] The computer device may be a terminal, and the computer device includes a processor, a memory, a network interface, a display screen and an input device connected through a system bus. Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The network interface of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, a trajectory planning method for autonomous exploration of an unmanned vehicle is implemented. The display screen of the computer device may be a liquid crystal display screen or an electronic ink display screen, and the input device of the computer device may be a touch layer covering the display screen, or a button, trackball or touchpad provided on the housing of the computer device, or an external keyboard, touchpad or mouse, etc.

[0233] The fourth aspect of the present invention discloses a computer-readable storage medium, which stores computer-executable instructions, and the computer-executable instructions are used to enable a computer to execute the trajectory planning method for autonomous exploration of an unmanned vehicle as described in any one of the items disclosed in the first aspect of the present invention.

[0234] Those of ordinary skill in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be implemented by instructing related hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it may include the processes of the embodiments of the trajectory planning method for autonomous exploration by an unmanned vehicle as described above. Among them, any reference to memory, storage, database or other media used in the embodiments provided in this application may include non-volatile and / or volatile memory. Non-volatile memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory may include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in many forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0235] Alternatively, if the above modules of the present invention are implemented in the form of software function modules and sold or used as independent products, they can also be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the embodiment of the present invention can be essentially or partly reflected in the form of a software product that contributes to the relevant technology. The computer software product is stored in a storage medium and includes several instructions for a computer device (which can be a personal computer, terminal, or network device, etc.) to execute all or part of the methods of each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as mobile storage devices, RAM, ROM, magnetic disks or optical disks.

[0236] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A trajectory planning method for autonomous exploration of an unmanned vehicle, characterized in that: The method comprises: Obtain point cloud information and grid map to update the two-dimensional grid set and grid status, and determine the target strategy of the target trajectory according to the status change; Discretize the control input at the first moment according to the forward kinematic sampling to generate the trajectory points at the second moment; Construct an evaluation index of the trajectory point according to the cost function and the heuristic function, and determine the target trajectory point at the second moment; Generate a target trajectory based on the target trajectory points and the target strategy.

2. The trajectory planning method for autonomous exploration of an unmanned vehicle according to claim 1, characterized in that: The step of obtaining point cloud information and a grid map and updating a two-dimensional grid set and a grid state includes: Controlling the radar to obtain point cloud information and using a grid map to display the regional environment, taking the point cloud information as input and incrementally mapping it into the grid map; The global grid map is continuously updated based on the point cloud information to confirm all the two-dimensional grid sets in the plane of the area where the unmanned vehicle is located, and the unknown grid sets S in the grid sets are recorded. unknow and the known grid set S know , and the union of grids with unknown states and known states is the set S of all two-dimensional grids; For a known grid set S know The known grid states in are classified into obstacle grids and free grids, and the obstacle grid set S occu and the free grid set S free The union of is the known grid set S know .

3. The trajectory planning method for autonomous exploration of an unmanned vehicle according to claim 2, characterized in that: The target strategy for determining the target trajectory according to the state change includes: redefining the grid whose grid state is updated according to the point cloud information and recorded as a free grid, selecting a free grid adjacent to at least one unknown grid in the unknown grid set as a boundary grid, and recording the boundary grid set S frontier ; The boundary grid set S frontier And the unknown grid set S unknow As the first constraint condition, the obstacle grid set S occu and the free grid set S free The union of is the known grid set S know As the second constraint condition, the known grid set is equivalent to the two-dimensional grid set S as the third constraint condition, and a target strategy is generated according to the first constraint condition, the second constraint condition and the third constraint condition to select the trajectory points of the target trajectory.

4. The trajectory planning method for autonomous exploration of an unmanned vehicle according to claim 1, characterized in that: The control input quantity of the first moment is discretized according to forward kinematic sampling, including: Determine the discrete time node Δt at the first moment t, through the speed discrete coefficient τ 1,i , τ 2，j The first control input is discretized to obtain an input sequence, and the second control input at the first time t is formed according to the elements of the input sequence. The first control input is expressed as: I t ={(v i , w j )), where v i represents the i-th input speed at the first moment t, w j represents the j-th input angular velocity at the first moment t, and the number of the first control input quantities is n; The discretized first control input is expressed as: v i =v min +τ 1,i ·(v max -v min ),τ 1，i ∈[0,1]; w j =w min +τ 2，j ·(w max -w min ),τ 2，j ∈[0,1]; Among them, v min Indicates the minimum speed of the input, v max Indicates the maximum speed of the input quantity, w min Represents the minimum angular velocity of the input, w max Represents the maximum angular velocity of the input, τ 1，i The discrete coefficient of velocity, τ 2，j The discrete coefficient representing the angular velocity; The input sequence is expressed as: v=[v0,v1,…,v n-1 ]; w=[W0,w1,...,w n-1 ]; Wherein, v represents the velocity element corresponding to the forward trajectory point output by the discretization processing of the first control input, w represents the angular velocity element corresponding to the forward trajectory point output by the discretization processing of the first control input, and n represents that the first control input generates n different input sequence elements at the first time t, and the input sequence elements include the velocity and angular velocity of the forward trajectory point of the unmanned vehicle; The second control input is expressed as: I t (i,j)={(V i ,W j )|V i ∈v,W j ∈w}; Among them, I t (i, j) represents the second control input, V i represents the i-th velocity element in the input sequence, W j Represents the jth speed element in the input sequence, where the number of second control inputs is n 2 .

5. The trajectory planning method for autonomous exploration of an unmanned vehicle according to claim 4, characterized in that: The generating of the trajectory point at the second moment includes: Determine the first position of the unmanned vehicle at the first time t, according to the determined discrete time node Δt and the second control input I t (i, j) obtaining a second posture of the unmanned vehicle at a second time t+1, and obtaining the trajectory points of the unmanned vehicle from the first time t to the second time t+1 according to the first posture and the second posture; The first posture is expressed as: P t (p t ,θ t ), where p t represents the position coordinates of the unmanned vehicle at the first moment t in global coordinates, θ t represents the turning angle of the unmanned vehicle at the first moment t; The second posture is expressed as: P t+1 (p t+1 ,θ t+1 ), the calculation formula of the second posture is: P t+1 (p t+1 ,θ t+1 )=P t (p t ,θ t )+Δt·I t (i, j), where Δt represents a discrete time node, and Δt is directly proportional to the exploration duration, and inversely proportional to the exploration efficiency.

6. The trajectory planning method for autonomous exploration of an unmanned vehicle according to claim 5, characterized in that: The step of constructing an evaluation index of the trajectory point according to the cost function and the heuristic function to determine the target trajectory point at the second moment includes: The dynamic cost function is constructed according to the minimum number of the second control input before obtaining the second posture, and the dynamic cost function is expressed as: Among them, α1 and α2 are weight factors; The safe distance of the trajectory point is defined according to the distance check between the unknown grid and the obstacle grid, and the collision cost function is constructed by the safe distance. The collision cost function is expressed as: Among them, P t+1 represents the second posture, e represents the natural constant, d c Indicates the safety distance, d real Indicates the actual detection distance; The boundary heuristic function is constructed according to the preset area search gain to minimize the number of round trip searches of the unmanned vehicle. The boundary heuristic function is expressed as: in, Denotes the second pose P t+1 The distance from the search boundary in the x-axis direction of the two-dimensional plane, Denotes the second pose P t+1 The distance from the search boundary in the y-axis direction of the two-dimensional plane; The number of unknown grids observable by the unmanned vehicle at the second pose is used as the information gain to construct a gain heuristic function, which is expressed as: g info =GetInfo(P t+1 ,S frontier ) Among them, GetInfo(P t+1 , S frontier ) represents the number of boundary grids observable by the unmanned vehicle at the second position, S frontier Represents a boundary grid element in a boundary grid collection; Corresponding weights are set for the dynamic cost function, collision cost function, boundary heuristic function, and gain heuristic function, and an evaluation index function of the state quantity of the unmanned vehicle in the second posture at the second moment is constructed to evaluate the optional trajectory points in the second posture. The evaluation index function is expressed as: f total,i,j (P t+1 )=w1f dyn (I t (i,j))+w2f colli (P t+1 )+w s g barr (P t+1 )-w4g info (P t+1 ) Among them, w1 represents the weight of the dynamics cost function, w2 represents the weight of the collision cost function, w3 represents the weight of the boundary heuristic function, and w4 represents the weight of the gain heuristic function.

7. The trajectory planning method for autonomous exploration of an unmanned vehicle according to claim 6, characterized in that: The generating a target trajectory according to the target trajectory point and the target strategy comprises: According to the optional trajectory points filtered by the evaluation index function, the trajectory point with the smallest value is selected as the next target trajectory point of the unmanned vehicle, and the target trajectory point is added to the path queue; The global grid map is traversed until the trajectory points of the path queue satisfy the first constraint, the second constraint and the third constraint in the target strategy, and then the target path is obtained from the path queue.

8. A trajectory planning system for autonomous exploration of an unmanned vehicle, characterized in that: The system comprises: Map exploration module, used to obtain point cloud information and grid map to update the two-dimensional grid set and grid status, and determine the target strategy of the target trajectory according to the status change; A viewpoint sampling module, used to discretize the control input at the first moment according to the forward kinematic sampling, and generate the trajectory points at the second moment; A cost calculation module is used to construct an evaluation index of the trajectory point according to the cost function and the heuristic function, and determine the target trajectory point at the second moment; The trajectory generation module is used to generate a target trajectory according to the target trajectory points and the target strategy.

9. A trajectory planning device for autonomous exploration of an unmanned vehicle, characterized in that: include: at least one processor, and a memory communicatively connected to the at least one processor; wherein, The memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the trajectory planning method for autonomous exploration of an unmanned vehicle as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer-executable instructions, and the computer-executable instructions are used to enable a computer to execute the trajectory planning method for autonomous exploration of an unmanned vehicle as described in any one of claims 1 to 7.

Images