Wheeled robot motion planning method, device, electronic equipment and medium

Abstract

The present invention relates to a wheeled robot motion planning method, device, electronic device and medium, belonging to the field of robot path planning technology, wherein the method comprises: obtaining a grid map of an unknown target scene; generating an initial path and constructing a search tree based on the starting point and target point of the target wheeled robot in the grid map, initializing the search tree, and the root node of the search tree is the target point; obtaining a local target point and a local starting point; instantiating the search tree from the local target point to the local starting point to obtain the local path of the target wheeled robot; based on the local path until the local target point coincides with the target point, obtaining the global optimal path of the target wheeled robot. The present invention adopts a reverse BIT* algorithm to reversely generate a search tree starting from the target point. When encountering a dynamic object, only the path around the dynamic object needs to be adjusted, and the entire path does not need to be replanned, thereby improving the efficiency of path planning.

Description

Technical Field

[0001] The present invention relates to the technical field of robot path planning, and in particular to a wheeled robot motion planning method, device, electronic equipment and medium. Background Art

[0002] With the continuous improvement of the level of industrial intelligence and the implementation of various intelligent technologies such as voice recognition, machine vision, machine learning, automatic navigation and positioning, the scale of the robotics industry has grown rapidly. With its advantages of strong interactivity, strong environmental adaptability and a wide range of application areas, it has become an important part of economic development, people's lives, and social governance. It is widely used in mobile measurement, military reconnaissance, disaster relief and other fields. The market size is close to 100 billion, bringing tremendous changes to production and life.

[0003] The autonomous navigation system is a prerequisite for intelligent robots to independently perform various tasks. The motion planning system based on the environment perception module and the positioning and attitude module is one of the core of the autonomous navigation system. The purpose is to plan a motion trajectory that can avoid obstacles in the drivable area. It is specifically divided into two stages: path planning and trajectory optimization. Path planning is to find the safe and feasible shortest collision-free geometric path in a working environment with obstacles. However, most of the existing path planning modules have contradictions between availability, safety and planning efficiency.

[0004] In warehouses, parks, shopping malls and other known environments with prior maps, intelligent robot path planning often adopts methods such as setting inspection points and map matching. These path planning methods are often customized according to each scene. Most systems can only be used in a single scene and need to be constantly updated and maintained. However, in application fields such as mobile measurement and military reconnaissance, there are a large number of complex unknown scenes without prior maps or inspection points. At the same time, such scenes contain a large number of dynamic objects and have strong time-varying characteristics. Therefore, it is difficult for robot planning systems for specific environments to achieve usable motion planning.

[0005] In the existing technology, the D* series of algorithms are often used in the path planning stage for complex unknown scenarios. Based on the reverse incremental idea, the algorithm performs a reverse search from the target point to the starting point. When dynamic obstacles appear in the environment or the path deviates, the algorithm can dynamically update the path and has a strong ability to adapt to complex environments. However, during the search process, the algorithm searches for expanded nodes layer by layer, and a large number of irrelevant nodes are expanded, resulting in low search efficiency and difficulty in meeting both security and real-time requirements.

[0006] In summary, in complex and unknown environments, there are situations where dynamic objects are difficult to predict and there is no prior drivable area, which leads to problems such as long planning time and difficulty in identifying dynamic obstacles in the current path planning module. Therefore, the prior art lacks a motion planning method to plan the path of a wheeled robot, thereby improving the efficiency of path planning. Summary of the invention

[0007] In view of this, it is necessary to provide a wheeled robot motion planning method, device, electronic device and medium to solve the problem of low calculation efficiency of path planning of wheeled robots in the prior art.

[0008] In order to solve the above problems, the present invention provides a wheeled robot motion planning method, comprising:

[0009] Get a grid map of the target unknown scene;

[0010] Generate an initial path based on the starting point and target point of the target wheeled robot in the grid map and construct a search tree, initialize the search tree, and the root node of the search tree is the target point;

[0011] Get the local target point and the local starting point. The local target point is the intersection of the radar scanning boundary and the initial path, and the local starting point is the current position of the target wheeled robot.

[0012] Instantiating a search tree from a local target point to a local starting point to obtain a local path of the target wheeled robot;

[0013] Based on the local path until the local target point coincides with the target point, a global optimal path of the target wheeled robot is obtained.

[0014] In a possible implementation, obtaining a grid map of an unknown target scene includes:

[0015] The radar on the target wheeled robot is used to obtain the grid map of the target unknown scene based on the improved ray method.

[0016] In a possible implementation, the method of acquiring a grid map of the target unknown scene by using a radar on the target wheeled robot based on an improved ray method includes:

[0017] Obtain point cloud data of the target unknown scene;

[0018] Obtain obstacle points and ground points of the point cloud data based on an improved ray method;

[0019] Performing feature point registration on the obstacle point and the ground point to obtain edge points and plane points;

[0020] Obtaining initial positioning information of the target wheeled robot based on the edge points and the plane points;

[0021] Optimize the initial positioning information to obtain the position and posture of the target wheeled robot;

[0022] A grid map of the target unknown scene is obtained based on the position and posture of the target wheeled robot.

[0023] In a possible implementation, generating an initial path based on a starting point and a target point of the target wheeled robot in a grid map and constructing a search tree, and initializing the search tree, includes:

[0024] Generate an initial path based on the starting point and the target point of the target wheeled robot in the grid map;

[0025] A search tree is constructed in the grid map according to the starting point and the target point of the target wheeled robot, and the tree edge queue and the node queue of the search tree are initialized.

[0026] In a possible implementation, instantiating a search tree from a local target point to a local starting point to obtain a local path of the target wheeled robot includes:

[0027] The target range between the local target point and the local starting point is sampled by random sampling, and the point connected to the sampling point is determined according to the minimum cost value of the tree node queue to form the current optimal edge, which is the local path of the target wheeled robot.

[0028] In a possible implementation, the method of sampling the target range between the local target point and the local starting point by random sampling, and determining the point connected to the sampling point according to the minimum cost value of the tree node queue to form the current optimal edge, includes the following steps:

[0029] Step 1: Sampling the target range between the local target point and the local starting point, adding the sampling point to the sampling point set, and adding the node set to the tree node queue;

[0030] Step 2: Determine whether the tree edge queue and the tree node queue are empty. If so, execute step 3; otherwise, execute step 5.

[0031] Step 3: Determine whether a feasible path is generated, if so, execute step 4;

[0032] Step 4: When a feasible path is generated, the elliptical sampling area is updated based on the cost required to reach the starting point of the generated path, sampling points are obtained and the sampling point set is updated, and search tree nodes and search tree edges are pruned;

[0033] Step 5: Estimate the minimum cost of tree nodes and tree edges, and determine whether the minimum cost of tree nodes is less than or equal to the minimum cost of tree edges. If so, execute step 6;

[0034] Step 6: Determine the vertex with the minimum estimated cost in the tree node queue, obtain all sampling points that meet the constraints within the search radius, and add the edge to the tree edge queue;

[0035] Step 7: After determining the edge with the smallest estimated cost in the tree edge queue, delete it from the tree edge queue;

[0036] Step 8: Determine whether the estimated cost of this edge is greater than or equal to the current path cost. If so, execute step 9; otherwise, execute step 10.

[0037] Step 9: Clear the tree edge queue and tree node queue;

[0038] Step 10: Determine whether the cost of the path is reduced after passing through the edge, if so, execute step 11;

[0039] Step 11: Determine whether the node corresponding to the edge is in the tree node queue of the search tree, if so, execute step 12, if not, execute step 13;

[0040] Step 12: Rewire the search tree;

[0041] Step 13: Expand the tree edges of the search tree;

[0042] Step 14: Repeat steps 2 to 13 until the search tree is connected to the local starting point of the target wheeled robot.

[0043] In a possible implementation manner, the method further includes: processing the global optimal path by using a curve fitting method to obtain a motion trajectory of the target wheeled robot.

[0044] On the other hand, the present invention also provides a wheeled robot motion planning device, comprising:

[0045] A grid map acquisition module is used to obtain a grid map of an unknown target scene;

[0046] An initialization module is used to generate an initial path based on the starting point and target point of the target wheeled robot in the grid map and construct a search tree, and initialize the search tree, wherein the root node of the search tree is the target point;

[0047] A local target point and local starting point acquisition module is used to acquire a local target point and a local starting point. The local target point is the intersection of the radar scanning boundary and the initial path, and the local starting point is the current position of the target wheeled robot.

[0048] A local path acquisition module, which instantiates a search tree from a local target point to a local starting point to obtain a local path of the target wheeled robot;

[0049] The global optimal path acquisition module is used to obtain the global optimal path of the target wheeled robot based on the local path until the local target point coincides with the target point.

[0050] On the other hand, the present invention also provides an electronic device, including a memory and a processor, wherein:

[0051] The memory is used to store programs;

[0052] The processor is coupled to the memory and is used to execute the program stored in the memory to implement the steps in a wheeled robot motion planning method described in any one of the above implementations.

[0053] On the other hand, the present invention also provides a computer-readable storage medium for storing computer-readable programs or instructions, which, when executed by a processor, can implement the steps in a wheeled robot motion planning method described in any of the above-mentioned implementation methods.

[0054] The beneficial effect of the present invention is as follows: a wheeled robot motion planning method provided by the present invention first obtains a grid map of an unknown target scene, then generates an initial path based on the starting point and target point of the target wheeled robot in the grid map and constructs a search tree, initializes the search tree, the root node of the search tree is the target point, further obtains a local target point and a local starting point, the local target point is the intersection of a radar scanning boundary and the initial path, the local starting point is the current position of the target wheeled robot, further instantiates a search tree from the local target point to the local starting point to obtain the local path of the target wheeled robot, and finally obtains the global optimal path of the target wheeled robot based on the local path until the local target point coincides with the total target point. A reverse BIT* algorithm is used to generate a search tree in reverse from a target point. Compared with the basic BIT* algorithm, when a dynamic obstacle appears, the basic BIT* algorithm needs to replan the entire path to adapt to changes in the position of the dynamic object, and may need to make frequent adjustments to bypass the obstacle, resulting in reduced real-time performance of the path planning algorithm. However, the reverse BIT* algorithm used in the present invention generates a search tree in reverse from the target point. When a dynamic obstacle appears, the dynamic obstacle is used as the local target point, and only the path near the dynamic obstacle needs to be recalculated. This can adapt to changes in the position of the dynamic obstacle more quickly, avoid recalculating the entire path, and improve the computational efficiency of path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 A flowchart of an embodiment of a wheeled robot motion planning method provided by the present invention;

[0056] Figure 2A schematic diagram of an embodiment of a wheeled robot motion planning method provided by the present invention;

[0057] Figure 3 This is a schematic diagram of a point cloud segmentation algorithm based on an improved ray method according to an embodiment of the present invention;

[0058] Figure 4 For the present invention Figure 1 A schematic flow chart of an embodiment of S102;

[0059] Figure 5 A reverse BIT* algorithm planning flow chart of an embodiment of the present invention;

[0060] Figure 6 A schematic flow chart of an embodiment of a wheeled robot motion planning device provided by the present invention;

[0061] Figure 7 A schematic structural diagram of an embodiment of an electronic device provided by the present invention. DETAILED DESCRIPTION

[0062] The preferred embodiments of the present invention are described in detail below in conjunction with the accompanying drawings, wherein the accompanying drawings constitute a part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, but are not used to limit the scope of the present invention.

[0063] The present invention provides a wheeled robot motion planning method, device, electronic equipment and medium, which are described below respectively.

[0064] Figure 1 A schematic flow chart of an embodiment of the wheeled robot motion planning method provided by the present invention is as follows: Figure 1 As shown, the wheeled robot motion planning method includes:

[0065] S101, obtaining a grid map of a target unknown scene;

[0066] S102, generating an initial path based on the starting point and target point of the target wheeled robot in the grid map and constructing a search tree, initializing the search tree, and the root node of the search tree is the target point;

[0067] S103, obtaining a local target point and a local starting point, where the local target point is the intersection of the radar scanning boundary and the initial path, and the local starting point is the current position of the target wheeled robot;

[0068] S104, instantiating a search tree from the local target point to the local starting point to obtain a local path of the target wheeled robot;

[0069] S105. Obtain a global optimal path of the target wheeled robot based on the local path until the local target point coincides with the target point.

[0070] Compared with the prior art, the present embodiment provides a wheeled robot motion planning method, which first obtains a grid map of an unknown target scene, then generates an initial path based on the starting point and target point of the target wheeled robot in the grid map and constructs a search tree, initializes the search tree, and the root node of the search tree is the target point. The local target point and the local starting point are further obtained, the local target point is the intersection of the radar scanning boundary and the initial path, the local starting point is the current position of the target wheeled robot, and the search tree is further instantiated from the local target point to the local starting point to obtain the local path of the target wheeled robot. Finally, based on the local path until the local target point coincides with the total target point, the global optimal path of the target wheeled robot is obtained. The present invention adopts a reverse BIT* algorithm to reversely generate a search tree starting from the target point. When a dynamic obstacle appears and re-planning is required, sufficient prior information is available to improve the computational efficiency of re-planning.

[0071] It should be noted that the application scenarios of the present invention are complex unknown scenarios where there are a large number of no prior maps or checkpoints, such as mobile measurement or military reconnaissance. It can be understood that the present invention can be applied not only to mobile measurement and military reconnaissance, but also to complex unknown scenarios where there are a large number of no prior maps or checkpoints.

[0072] It should be noted that the reverse BIT* algorithm used in steps S102 to S105 of the embodiment of the present invention is a traditional BIT* algorithm that establishes a search tree with the starting point as the root node. The BIT* algorithm tends to explore nodes close to the starting point in the early stage of planning. Therefore, when dynamic obstacles appear and re-planning is required, the efficiency of the BIT* algorithm is low, resulting in reduced real-time performance of the path planning algorithm. The reverse BIT* algorithm used in the present invention generates a search tree in reverse from the target point. When dynamic obstacles appear and re-planning is required, it has sufficient prior information, which can improve the computational efficiency of re-planning.

[0073] It should be noted that in step S102, an initial path is generated based on the starting point and target point of the target wheeled robot in the grid map and a search tree is constructed. The starting point is the global starting point of the reverse BIT* algorithm, and the target point is the global target point of the reverse BIT* algorithm.

[0074] In some embodiments of the present invention, Figure 2 The figure shows a schematic diagram of an embodiment of a wheeled robot motion planning method provided by the present invention. First, the radar on the target wheeled robot is used to obtain a grid map of the target unknown scene, and then the path of the target wheeled robot is planned. Finally, the path is trajectory optimized to obtain the optimal path of the wheeled robot.

[0075] In some embodiments of the present invention, in step S101, obtaining a grid map of a target unknown scene includes:

[0076] The radar on the target wheeled robot is used to obtain the grid map of the target unknown scene based on the improved ray method.

[0077] In some embodiments of the present invention, the method of acquiring a grid map of a target unknown scene using a radar on a target wheeled robot based on an improved ray method includes:

[0078] Obtain point cloud data of the target unknown scene;

[0079] Obtain obstacle points and ground points of the point cloud data based on an improved ray method;

[0080] Performing feature point registration on the obstacle point and the ground point to obtain edge points and plane points;

[0081] Obtaining initial positioning information of the target wheeled robot based on the edge points and the plane points;

[0082] Optimize the initial positioning information to obtain the position and posture of the target wheeled robot;

[0083] A grid map of the target unknown scene is obtained based on the position and posture of the target wheeled robot.

[0084] In a specific embodiment of the present invention, Figure 3 The figure shows the principle diagram of the point cloud segmentation algorithm based on the improved ray method in an embodiment of the present invention. The improved ray method is used to segment the ground laser point cloud for the undulating road conditions in complex unknown scenes. In the ground point cloud judgment link, a local dynamic adjustment slope threshold algorithm is introduced, and the local point cloud distribution characteristics are obtained by using the angle relationship of the local point cloud. The local slope threshold is set according to the point cloud distribution characteristics, and then the point cloud segmentation is realized in the undulating road scene in the unknown environment, and the segmented point cloud data is used to establish a grid map. The specific steps are as follows:

[0085] Step 1: Sector division of laser point cloud and sorting of point clouds within sectors: divide the radar field of view into several sector-shaped areas according to the radar resolution, and group the point clouds in the same sector-shaped area into a set. Calculate the horizontal distance from each point in the set to the radar origin, and sort the point clouds in ascending order of horizontal distance.

[0086] Step 2: Based on the point cloud set obtained in the first step, the improved ray method is used to screen the obstacle points and ground points of the laser point clouds in all sectors according to the global slope threshold and the local slope threshold: the angle between the point cloud and the adjacent point cloud and the vertical angle between the point cloud and the origin of the radar coordinate system are calculated. When one of the two angles is greater than the global slope threshold, the point cloud is judged to be an obstacle point; the local slope threshold is calculated, and the ground point is judged by combining the tangent value of the angle between the point cloud and the adjacent point cloud and the tangent value of the vertical angle of the point cloud.

[0087] Step 3: Based on the obstacle points and ground points obtained in the second step, feature point registration is performed, the curvature of the point cloud is calculated, and it is divided into edge points and plane points accordingly.

[0088] Step 4: Based on the edge points and plane points extracted in step 3, the motion between the two frames is restored by the ICP algorithm to obtain the initial positioning information, where the constraint is to minimize the distance from the feature point to the edge line and edge surface.

[0089] Step 5: Considering that the pose information obtained in the fourth step is not accurate, the feature points are aligned to the local point cloud map to optimize the pose information.

[0090] Step 6: Integrate the pose information obtained in step 4 and step 5. The frequency of step 5 is higher but the accuracy is lower, while the frequency of step 6 is lower but the accuracy is higher. After integration, high-frequency and high-precision pose information and point cloud maps can be obtained.

[0091] Step 7: Use Octomap to convert the point cloud map into an occupancy grid map.

[0092] In some embodiments of the present invention, in step S102, as Figure 4 As shown, the initial path is generated based on the starting point and the target point of the target wheeled robot in the grid map and a search tree is constructed, and the search tree is initialized, including:

[0093] S401, generating an initial path based on the starting point and target point of the target wheeled robot in the grid map;

[0094] S402: construct a search tree in the grid map according to the starting point and the target point of the target wheeled robot, and initialize the tree edge queue and the node queue of the search tree.

[0095] Specifically, it can be understood that the search tree of this embodiment always builds a search tree for the target point and starting point of the target wheeled robot. In the initial state, the starting point can be understood as the local starting point of the target wheeled robot, and the target point can be understood as the local target point of the target wheeled robot.

[0096] In some embodiments of the present invention, the instantiating a search tree from the local target point to the local starting point to obtain the local path of the target wheeled robot includes:

[0097] The target range between the local target point and the local starting point is sampled by random sampling, and the point connected to the sampling point is determined according to the minimum cost value of the tree node queue to form the current optimal edge, which is the local path of the target wheeled robot.

[0098] In some embodiments of the present invention, Figure 5 The reverse BIT* algorithm planning flow chart of an embodiment of the present invention is shown, wherein the target range between the local target point and the local starting point is sampled by random sampling, and the point connected to the sampling point is determined according to the minimum cost value of the tree node queue to form the current optimal edge, including the following steps:

[0099] Step 1: Sampling the target range between the local target point and the local starting point, adding the sampling point to the sampling point set, and adding the node set to the tree node queue;

[0100] Step 2: Determine whether the tree edge queue and the tree node queue are empty. If so, execute step 3; otherwise, execute step 5.

[0101] Step 3: Determine whether a feasible path is generated, if so, execute step 4;

[0102] Step 4: When a feasible path is generated, the elliptical sampling area is updated based on the cost required to reach the starting point of the generated path, sampling points are obtained and the sampling point set is updated, and search tree nodes and search tree edges are pruned;

[0103] Step 5: Estimate the minimum cost of tree nodes and tree edges, and determine whether the minimum cost of tree nodes is less than or equal to the minimum cost of tree edges. If so, execute step 6;

[0104] Step 6: Determine the vertex with the minimum estimated cost in the search tree node queue, obtain all sampling points that meet the constraints within the search radius, and add the edge to the tree edge queue;

[0105] Step 7: After determining the edge with the smallest estimated cost in the search tree edge queue, delete it from the tree edge queue;

[0106] Step 8: Determine whether the estimated cost of this edge is greater than or equal to the current path cost. If so, execute step 9; otherwise, execute step 10.

[0107] Step 9: Clear the tree edge queue and tree node queue;

[0108] Step 10: Determine whether the cost of the path is reduced after passing through the edge, if so, execute step 11;

[0109] Step 11: Determine whether the node corresponding to the edge is in the tree node queue of the search tree, if so, execute step 12, if not, execute step 13;

[0110] Step 12: Rewire the search tree;

[0111] Step 13: Expand the tree edges of the search tree;

[0112] Step 14: Repeat steps 2 to 13 until the search tree is connected to the local starting point of the target wheeled robot.

[0113] In a specific embodiment of the present invention, in view of the impact of dynamic obstacles in complex unknown scenes, a reverse BIT* (Batch Informed Trees, BIT*) algorithm is used in the grid map for path planning. The basic BIT* algorithm tends to explore nodes close to the starting point in the early stage of planning. Therefore, when dynamic obstacles appear and re-planning is required, the efficiency of the BIT* algorithm is low, resulting in reduced real-time performance of the path planning algorithm. The reverse BIT* algorithm used in the present invention generates a search tree in reverse from the target point. When dynamic obstacles appear and re-planning is required, it has sufficient prior information, which can improve the computational efficiency of re-planning. The specific steps are as follows:

[0114] In the first step, based on the coordinate information of the starting point and the target point in the grid map, an initial path is generated using a cubic polynomial, and the local target point is set as the intersection of the laser radar scanning boundary and the initial trajectory to obtain the coordinate information;

[0115] The second step is to initialize the ordered search tree node queue With ordered search tree edge queue It is used to maintain the generated search tree information. The first element of the two queues has the minimum cost, and the target point is set as the root of the search tree.

[0116] In the third step, at the start time, the robot moves forward according to the initial path obtained in the first step, and searches the tree node queue in an ordered manner based on the local target point obtained in the first step and the initialized tree node queue in the second step. With ordered search tree edge queue , generate a search tree from the local target point to the local starting point to obtain the local path, and then the robot modifies the motion path based on the local path. Repeat the above steps until the local target point coincides with the total target point. Specifically, it includes the following steps:

[0117] Step 3: Determine the ordered queue and Is the status of empty? If so, execute step 32; otherwise, execute step 34;

[0118] Step 32: determine whether a feasible path is generated, if so, execute step 33;

[0119] Step 33: When generating a feasible path, the cost required to reach the starting point based on the generated path Update the elliptical sampling area, obtain the sampling points and update the sampling point set , prune search tree nodes and search tree edges;

[0120] Step 34: Estimate the minimum cost value of the tree node and the tree edge, and determine whether the minimum cost value of the tree node is less than or equal to the minimum cost value of the tree edge. If so, execute step 35;

[0121] Step 35: Find the ordered search tree node queue The vertex with the minimum estimated cost , and get the search radius All sampling points that satisfy the following formula are Add to the ordered queue middle;

[0122]

[0123] In the formula, From the starting point to the node The estimated cost of For Node Estimated cost to the target point; For slave nodes To Node Estimated cost of distance;

[0124] Step 36: Pop up the ordered search tree edge queue The edge with the smallest estimated cost After that, and from Deleted;

[0125] Step 37: Determine whether the estimated cost of this edge is greater than or equal to the current path cost. If so, execute step 38; otherwise, execute step 39;

[0126] Step 38: Clear the ordered queue and ;

[0127] Step 39: Determine if the path passes through the edge whether the subsequent cost value is reduced, if so, executing step 30;

[0128] Step 30: Determine whether the node corresponding to the edge is in the ordered search tree node queue If yes, execute step 31, if no, execute step 32;

[0129] Step 31: Rewire the search tree;

[0130] Step 32: Expand the tree edge;

[0131] Step 33: Repeat the above process until the search tree is connected to the local starting point.

[0132] In the fourth step, as the robot continues to move forward, the laser radar scanning range continues to approach the target point, the local starting point and the local target point are continuously updated, and the local path planning is continuously completed until the local target point coincides with the overall target point, completing the path planning task.

[0133] In some embodiments of the present invention, the method further includes: processing the global optimal path by means of curve fitting to obtain a motion trajectory of the target wheeled robot.

[0134] In a specific embodiment of the present invention, the global path of the target wheeled robot is obtained in step S102, and the trajectory of the global path needs to be optimized. In this embodiment, based on the trajectory expression of the discrete points of the global path obtained in step S102, and according to the trajectory smoothness and other design optimization targets, the quadratic penalty term is solved, and finally the solver is used to obtain continuous and complete trajectory information. The specific steps are as follows:

[0135] Use cubic uniform B-spline to design trajectory expression;

[0136] Based on the trajectory expression, and taking into account the incompleteness of the wheeled robot, the smoothness and feasibility of the trajectory, the quadratic penalty term of the objective function is designed and the unconstrained quadratic programming problem is constructed;

[0137] Solve the quadratic optimization problem and obtain continuous trajectory information.

[0138] In order to better implement a wheeled robot motion planning method in an embodiment of the present invention, based on a wheeled robot motion planning method, correspondingly, as Figure 6 As shown, an embodiment of the present invention further provides a wheeled robot motion planning device, a wheeled robot motion planning device 600 comprising:

[0139] A grid map acquisition module 601 is used to acquire a grid map of a target unknown scene;

[0140] Initialization module 602, used to generate an initial path based on the starting point and target point of the target wheeled robot in the grid map and construct a search tree, initialize the search tree, and the root node of the search tree is the target point;

[0141] A local target point and local starting point acquisition module 603 is used to acquire a local target point and a local starting point, the local target point is the intersection of the radar scanning boundary and the initial path, and the local starting point is the current position of the target wheeled robot;

[0142] A local path acquisition module 604 instantiates a search tree from a local target point to a local starting point to obtain a local path of the target wheeled robot;

[0143] The global optimal path acquisition module 605 is used to obtain the global optimal path of the target wheeled robot based on the local path until the local target point coincides with the target point.

[0144] The wheeled robot motion planning device 600 provided in the above embodiment can implement the technical solution described in the above embodiment of a wheeled robot motion planning method. The specific implementation principles of the above modules or units can refer to the corresponding contents in the above embodiment of a wheeled robot motion planning method, which will not be repeated here.

[0145] like Figure 7 As shown, the present invention also provides an electronic device 700. The electronic device 700 includes a processor 701, a memory 702 and a display 703. Figure 7 Only some components of the electronic device 700 are shown, but it should be understood that it is not required to implement all of the components shown, and more or fewer components may be implemented instead.

[0146] In some embodiments, the processor 701 may be a central processing unit (CPU), a microprocessor or other data processing chip, used to run program codes or process data stored in the memory 702, such as a wheeled robot motion planning method in the present invention.

[0147] In some embodiments, the processor 701 may be a single server or a server group. The server group may be centralized or distributed. In some embodiments, the processor 701 may be local or remote. In some embodiments, the processor 701 may be implemented in a cloud platform. In some embodiments, the cloud platform may include a private cloud, a public cloud, a hybrid cloud, a community cloud, a distributed cloud, an internal cloud, a multi-cloud, etc., or any combination thereof.

[0148] In some embodiments, the memory 702 may be an internal storage unit of the electronic device 700, such as a hard disk or memory of the electronic device 700. In other embodiments, the memory 702 may also be an external storage device of the electronic device 700, such as a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, etc., equipped on the electronic device 700.

[0149] Furthermore, the memory 702 may include both an internal storage unit of the electronic device 700 and an external storage device. The memory 702 is used to store application software installed in the electronic device 700 and various data.

[0150] In some embodiments, the display 703 may be an LED display, a liquid crystal display, a touch-sensitive liquid crystal display, an OLED (Organic Light-Emitting Diode) touch device, etc. The display 703 is used to display information on the electronic device 700 and to display a visual user interface. The components 701-703 of the electronic device 700 communicate with each other via a system bus.

[0151] In one embodiment, when the processor 701 executes a wheeled robot motion planning program in the memory 702, the following steps may be implemented:

[0152] Get a grid map of the target unknown scene;

[0153] Generate an initial path based on the starting point and target point of the target wheeled robot in the grid map and construct a search tree, initialize the search tree, and the root node of the search tree is the target point;

[0154] Get the local target point and the local starting point. The local target point is the intersection of the radar scanning boundary and the initial path, and the local starting point is the current position of the target wheeled robot.

[0155] Instantiating a search tree from a local target point to a local starting point to obtain a local path of the target wheeled robot;

[0156] Based on the local path until the local target point coincides with the target point, a global optimal path of the target wheeled robot is obtained.

[0157] It should be understood that: when the processor 701 executes a wheeled robot motion planning program in the memory 702, in addition to the above functions, other functions can also be implemented. For details, please refer to the description of the corresponding method embodiment above.

[0158] Furthermore, the embodiment of the present invention does not specifically limit the type of the electronic device 700 mentioned, and the electronic device 700 may be a portable electronic device such as a mobile phone, a tablet computer, a personal digital assistant (PDA), a wearable device, a laptop computer, etc. Exemplary embodiments of portable electronic devices include but are not limited to portable electronic devices equipped with IOS, Android, Microsoft or other operating systems. The above-mentioned portable electronic devices may also be other portable electronic devices, such as a laptop computer with a touch-sensitive surface (e.g., a touch panel). It should also be understood that in some other embodiments of the present invention, the electronic device 700 may not be a portable electronic device, but a desktop computer with a touch-sensitive surface (e.g., a touch panel).

[0159] Those skilled in the art will appreciate that all or part of the processes of the above-mentioned embodiments can be implemented by instructing related hardware through a computer program, and the program can be stored in a computer-readable storage medium, wherein the computer-readable storage medium is a disk, an optical disk, a read-only storage memory, or a random access memory, etc.

[0160] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

Claims

1. A wheeled robot motion planning method, characterized in that: include: Get the grid map of the target unknown scene; Generate an initial path based on the starting point and target point of the target wheeled robot in the grid map and construct a search tree, initialize the search tree, and the root node of the search tree is the target point; Get the local target point and the local starting point. The local target point is the intersection of the radar scanning boundary and the initial path, and the local starting point is the current position of the target wheeled robot. Based on the reverse BIT* algorithm, a search tree is instantiated from the local target point to the local starting point to obtain the local path of the target wheeled robot; Based on the local path until the local target point coincides with the target point, a global optimal path of the target wheeled robot is obtained; Among them, the search tree is instantiated from the local target point to the local starting point based on the reverse BIT* algorithm, including: In the first step, based on the coordinate information of the starting point and the target point in the grid map, an initial path is generated using a cubic polynomial, and the local target point is set as the intersection of the laser radar scanning boundary and the initial trajectory to obtain the coordinate information; The second step is to initialize the ordered search tree node queue With ordered search tree edge queue It is used to maintain the generated search tree information. The first element of the two queues has the minimum cost, and the target point is set as the root of the search tree. In the third step, at the start time, the robot moves forward according to the initial path obtained in the first step, and searches the tree node queue in an ordered manner based on the local target point obtained in the first step and the initialized tree node queue in the second step. With ordered search tree edge queue , a search tree is generated from the local target point to the local starting point to obtain the local path, and then the robot modifies the motion path based on the local path; the above steps are repeated until the local target point coincides with the total target point, which specifically includes the following steps: Step 3: Determine the ordered queue and Is the status of empty? If so, execute step 32; otherwise, execute step 34; Step 32: determine whether a feasible path is generated, if so, execute step 33; Step 33: When generating a feasible path, the cost required to reach the starting point based on the generated path Update the elliptical sampling area, obtain the sampling points and update the sampling point set , prune search tree nodes and search tree edges; Step 34: Estimate the minimum cost value of the tree node and the tree edge, and determine whether the minimum cost value of the tree node is less than or equal to the minimum cost value of the tree edge. If so, execute step 35; Step 35: Find the ordered search tree node queue The vertex with the minimum estimated cost , and get the search radius All sampling points that satisfy the following formula are Add to the ordered queue middle; In the formula, From the starting point to the node The estimated cost of For Node The estimated cost to reach the target point; For slave nodes To Node Estimated cost of distance; Step 36: Pop up the ordered search tree edge queue The edge with the smallest estimated cost After that, and from Deleted; Step 37: Determine whether the estimated cost of this edge is greater than or equal to the current path cost. If so, execute step 38; otherwise, execute step 39; Step 38: Clear the ordered queue and ; Step 39: Determine if the path passes through the edge whether the subsequent cost value is reduced, if so, executing step 30; Step 30: Determine whether the node corresponding to the edge is in the ordered search tree node queue If yes, execute step 31, if no, execute step 32; Step 31: Rewire the search tree; Step 32: Expand the tree edge; Step 33: Repeat the above process until the search tree is connected to the local starting point.

2. The wheeled robot motion planning method according to claim 1, characterized in that: The step of obtaining a grid map of an unknown target scene includes: The radar on the target wheeled robot is used to obtain the grid map of the target unknown scene based on the improved ray method.

3. The wheeled robot motion planning method according to claim 2, characterized in that: The method of using the radar on the target wheeled robot to obtain a grid map of the target unknown scene based on the improved ray method includes: Obtain point cloud data of the target unknown scene; Obtain obstacle points and ground points of the point cloud data based on an improved ray method; Performing feature point registration on the obstacle point and the ground point to obtain edge points and plane points; Obtaining initial positioning information of the target wheeled robot based on the edge points and the plane points; Optimize the initial positioning information to obtain the initial position and posture of the target wheeled robot; A grid map of the target unknown scene is obtained based on the position and posture of the target wheeled robot.

4. The wheeled robot motion planning method according to claim 1, characterized in that: Also includes: The global optimal path is processed by curve fitting to obtain the motion trajectory of the target wheeled robot.

5. A wheeled robot motion planning device, characterized in that: include: A grid map acquisition module is used to obtain a grid map of an unknown target scene; An initialization module is used to generate an initial path based on the starting point and target point of the target wheeled robot in the grid map and construct a search tree, and initialize the search tree, wherein the root node of the search tree is the target point; A local target point and local starting point acquisition module is used to acquire a local target point and a local starting point. The local target point is the intersection of the radar scanning boundary and the initial path, and the local starting point is the current position of the target wheeled robot. A local path acquisition module, used to instantiate a search tree from a local target point to a local starting point based on a reverse BIT* algorithm to obtain a local path of the target wheeled robot; A global optimal path acquisition module, used to obtain a global optimal path of the target wheeled robot based on the local path until the local target point coincides with the target point; Among them, the search tree is instantiated from the local target point to the local starting point based on the reverse BIT* algorithm, including: In the first step, based on the coordinate information of the starting point and the target point in the grid map, an initial path is generated using a cubic polynomial, and the local target point is set as the intersection of the laser radar scanning boundary and the initial trajectory to obtain the coordinate information; The second step is to initialize the ordered search tree node queue With ordered search tree edge queue It is used to maintain the generated search tree information. The first element of the two queues has the minimum cost, and the target point is set as the root of the search tree. In the third step, at the start time, the robot moves forward according to the initial path obtained in the first step, and searches the tree node queue in an ordered manner based on the local target point obtained in the first step and the initialized tree node queue in the second step. With ordered search tree edge queue , a search tree is generated from the local target point to the local starting point to obtain the local path, and then the robot modifies the motion path based on the local path; the above steps are repeated until the local target point coincides with the total target point, which specifically includes the following steps: Step 3: Determine the ordered queue and Is the status of empty? If so, execute step 32; otherwise, execute step 34; Step 32: determine whether a feasible path is generated, if so, execute step 33; Step 33: When generating a feasible path, the cost required to reach the starting point based on the generated path Update the elliptical sampling area, obtain the sampling points and update the sampling point set , prune search tree nodes and search tree edges; Step 34: Estimate the minimum cost value of the tree node and the tree edge, and determine whether the minimum cost value of the tree node is less than or equal to the minimum cost value of the tree edge. If so, execute step 35; Step 35: Find the ordered search tree node queue The vertex with the minimum estimated cost , and get the search radius All sampling points that satisfy the following formula are Add to the ordered queue middle; In the formula, From the starting point to the node The estimated cost of For Node The estimated cost to reach the target point; For slave nodes To Node Estimated cost of distance; Step 36: Pop up the ordered search tree edge queue The edge with the smallest estimated cost After that, and from Deleted; Step 37: Determine whether the estimated cost of this edge is greater than or equal to the current path cost. If so, execute step 38; otherwise, execute step 39; Step 38: Clear the ordered queue and ; Step 39: Determine if the path passes through the edge whether the subsequent cost value is reduced, if so, executing step 30; Step 30: Determine whether the node corresponding to the edge is in the ordered search tree node queue If yes, execute step 31, if no, execute step 32; Step 31: Rewire the search tree; Step 32: Expand the tree edge; Step 33: Repeat the above process until the search tree is connected to the local starting point.

6. An electronic device, characterized in that: comprising a memory and a processor, wherein: The memory is used to store programs; The processor is coupled to the memory and is used to execute the program stored in the memory to implement the steps in the wheeled robot motion planning method described in any one of claims 1 to 4 above.

7. A computer-readable storage medium, characterized in that: Used to store computer-readable programs or instructions, which, when executed by a processor, can implement the steps in a wheeled robot motion planning method as described in any one of claims 1 to 4 above.

Images