Path planning method of storage intelligent vehicle based on mapping and navigation algorithm

Abstract

The invention belongs to the technical field of intelligent information, and particularly relates to a path planning method of a storage intelligent vehicle based on a mapping and navigation algorithm, which comprises the following steps: step 1, a dynamic radar scanning module detects and acquires environmental information in real time, and a mapping module combines a Gmapping algorithm and a Cartograph algorithm to complete the construction of a pre-mapping of a global map; step 2, a navigation module performs positioning and path planning by means of a Nav2 system, calculates a motion path and speed information of the storage intelligent vehicle, and generates a navigation instruction; and step 3, the communication module transmits the navigation instruction to the storage intelligent vehicle, the storage intelligent vehicle executes the navigation instruction and completes a predetermined motion task, and by means of efficient inter-module communication and dynamic path planning, the system can deal with various obstacles in a complex environment, ensures efficient and safe operation of the intelligent vehicle, and improves the safety of the intelligent vehicle. And the device is especially suitable for a storage environment requiring high precision and continuity.

Description

Technical Field

[0001] The present invention belongs to the field of intelligent information technology, and in particular relates to a path planning method for a warehousing intelligent vehicle based on mapping and navigation algorithms. Background Art

[0002] Warehousing and logistics warehouse smart vehicles are automated transportation equipment designed for warehousing and logistics environments, and can autonomously complete cargo handling, storage and transportation tasks. With the development of the logistics and warehousing industry, smart vehicles with mapping and navigation functions are becoming increasingly important in transportation. In complex environments, building accurate three-dimensional or two-dimensional warehouse maps and achieving precise positioning of warehouse smart vehicles have become a research hotspot. However, existing mapping and navigation algorithms face challenges in deployment, such as complex integration and difficulty in implementation. Conventional methods have difficulty achieving efficient positioning and path planning in complex environments. Therefore, there is an urgent need for a systematic solution that meets the operational needs of warehouse smart vehicles. Summary of the invention

[0003] The present invention aims to solve the problem of low efficiency in positioning and path planning of existing warehouse intelligent vehicles in complex environments, and proposes a path planning method for warehouse intelligent vehicles based on mapping and navigation algorithms.

[0004] In order to achieve the above-mentioned invention object, the technical solution adopted is as follows: A path planning method for a warehouse intelligent vehicle based on mapping and navigation algorithms comprises the following steps: Step 1: The dynamic radar scanning module detects and obtains environmental information in real time, and the mapping module combines the Gmapping algorithm and the Cartographer algorithm to complete the pre-map construction of the global map; Step 2: The navigation module uses the Nav2 system to perform positioning and path planning based on the pre-built map of the global map, calculates the movement path and speed information of the warehouse smart car, and generates navigation instructions; Step 3: The communication module transmits the navigation instruction to the warehouse smart vehicle, and the warehouse smart vehicle executes the navigation instruction to complete the predetermined motion task.

[0005] Optionally, the dynamic radar scanning module detects and acquires environmental information in real time, specifically including: The dynamic radar scanning module provides real-time ranging information and obstacle detection information, and compresses the three-dimensional point cloud information through the open source Linefitgroundsegmentation technology to achieve three-dimensional to two-dimensional laser data conversion.

[0006] Furthermore, the dynamic radar scanning module includes a 2D radar module and a 3D radar module. The 3D radar module obtains point cloud information, converts / Pointcloud into / Laserscan information, and divides the point cloud information obtained by the 3D radar module into N segments at equal intervals. Each segment is further subdivided into M Bins. By projecting all point cloud information of the current frame into the segment, the three-dimensional point coordinates p(x, y, z) are reduced to two-dimensional coordinates p'(d, z). In this process, the two-dimensional coordinates p'(d, z) corresponding to each point are obtained, and the smallest p'(d, z) in each bin is fitted to obtain a straight line z=kd+b, where k represents the slope of the road surface. The height from the point to the road surface is judged based on the straight line. If the height is less than a threshold, the point is determined to be a road surface point, otherwise it is an obstacle point.

[0007] Optionally, the mapping module combines Gmapping and Cartographer algorithms to complete the construction of the global map, specifically including: The Gmapping algorithm separates the problems of positioning and mapping in SLAM, namely:

[0008] in To control data, is the observed data, and m are positioning information and map information respectively. The algorithm includes four steps, namely sampling, weight calculation, resampling and map estimation. First, a particle estimate with a larger weight is obtained from the motion model, and then a proposed distribution is simulated based on the particle estimate. The weight is used to describe the difference between the target distribution and the proposed distribution. Then resampling is performed, and the maps fitted from different paths are integrated to generate a global map. The Cartographer algorithm is based on the optimized SLAM algorithm, which adopts the method of front-end local mapping, closed-loop detection, and back-end global mapping, and then uses global mapping to optimize the front-end local mapping.

[0009] Optionally, the navigation module uses the Nav2 system to perform positioning and path planning, calculates the movement path and speed information of the warehouse smart vehicle, and generates navigation instructions, specifically including: The navigation module uses the Nav2 system, combined with the AMCL algorithm, to calculate the online cost map and perform real-time positioning and path planning for the warehouse smart vehicle; The navigation module includes a global mapping module and a local mapping module. The global mapping module calls the service provided by the MapServer module in the ROS2 robot system based on the pre-map of the global map generated by the mapping module, and adjusts the parameters according to the preset anti-collision obstacle expansion distance and the vehicle wheel group scheme of the warehouse smart car; The local mapping module calculates the specific position of the warehouse smart car based on the obstacle distance obtained by the dynamic radar module scanning, plans the travel route of the warehouse smart car according to the positional relationship between the warehouse smart car and the target point, and generates navigation instructions.

[0010] Furthermore, the navigation module includes four service nodes: BTNavigatorServer, RecoveryServer, ControllerServer and PlannerServer, among which PlannerServer provides services related to the global mapping module, and controls the setting of the obstacle expansion map through the specific parameters of Costmap, thereby realizing the anti-collision function for static obstacles; ControllerServer provides services related to the local mapping module, and combines the obstacle information / Laserscan of the dynamic radar scanning module to effectively perform obstacle avoidance tasks for dynamic obstacles in the warehousing environment such as warehouse employees and other smart vehicles; RecoveryServer is responsible for handling fault conditions, BTNavigatorServer builds the overall behavior tree of the warehouse smart car, and is responsible for ultimately sending navigation instructions to the communication module.

[0011] Optionally, the communication module uses the USB virtual serial port of the RT-Thread framework to realize communication between the upper and lower computers, and parses the data frame according to the upper and lower computer communication protocol that supports CRC verification, and sends navigation instructions.

[0012] Compared with the prior art, the present invention provides a path planning method for a warehouse smart vehicle based on mapping and navigation algorithms. With efficient inter-module communication and dynamic path planning, the present invention can cope with various obstacles in complex environments and ensure efficient and safe operation of the smart vehicle. It is particularly suitable for warehouse environments that require high precision and continuity. BRIEF DESCRIPTION OF THE DRAWINGS

[0013] In order to more clearly illustrate one or more embodiments of the present specification 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 one or more embodiments of the present specification. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0014] Figure 1 It is a system structure diagram of the present invention. DETAILED DESCRIPTION

[0015] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with specific embodiments and with reference to the accompanying drawings.

[0016] The present invention will be further described below in conjunction with the accompanying drawings.

[0017] This embodiment provides a path planning method for a warehouse intelligent vehicle based on mapping and navigation algorithms. Figure 1 , including the following steps: Step 1: The dynamic radar scanning module detects and obtains environmental information in real time, and the mapping module combines the Gmapping algorithm and the Cartographer algorithm to complete the construction of the global map; Step 2: The navigation module uses the Nav2 system to perform positioning and path planning, calculates the movement path and speed information of the warehouse smart car, and generates navigation instructions; Step 3: The communication module transmits the navigation instruction to the warehouse smart vehicle, and the warehouse smart vehicle executes the navigation instruction to complete the predetermined motion task.

[0018] Specifically, the dynamic radar scanning module provides two deployment solutions based on the budgets and specific needs of different warehouse management manufacturers: 2D radar module and 3D radar module.

[0019] The 2D radar module has the advantage of being well compatible with the Nav2 system, with lower hardware costs and lower computing power requirements for the computing unit.

[0020] The 3D radar module mainly obtains point cloud information, and the point cloud needs to be compressed and converted from / Pointcloud to / Laserscan information. In this process, the open source linefit_ground_segmentation scheme is used. The specific method is to divide the 3D radar point cloud into N segments at equal intervals, and each segment is further subdivided into M Bins. By projecting all point clouds of the current frame into segments, the three-dimensional point coordinates p(x,y,z) are reduced to two-dimensional coordinates p'(d,z). In this process, the module obtains the two-dimensional coordinates p'(d,z) corresponding to each point, and fits the smallest p'(d,z) in each bin to obtain a straight line z=kd+b, where k represents the slope of the road surface. The module will judge the height from the point to the road surface based on the straight line. If the height is less than the threshold, the point is determined to be a road surface point, otherwise it is an obstacle point.

[0021] In response to the cost reduction needs of the logistics and warehousing industry, the mapping module adopts a deployment method that separates global mapping and navigation, also known as pre-mapping. This method greatly reduces the computing power requirements of terminal devices. The mapping module provides two pre-mapping solutions, Gmapping and Cartographer, which can adapt to a variety of scenarios based on the accuracy requirements of the map file, the mapping mode, and the software and hardware configuration.

[0022] The following is an explanation of the Gmapping solution: Gmapping is a SLAM algorithm that builds grid maps based on RBpf research. At the same time, RBpf is improved in the Gmapping algorithm. Its essence is to separate the problems of positioning and mapping in SLAM, namely:

[0023] in To control data, is the observed data, and m are positioning information and map information respectively. The algorithm includes four steps, namely sampling, weight calculation, resampling and map estimation. First, a particle estimate with a larger weight is obtained from the motion model, and then a proposed distribution is simulated based on the particle estimate. The weight is used to describe the difference between the target distribution and the proposed distribution, and then resampling is performed. The module integrates the maps fitted from different paths and generates the final map.

[0024] The following is an explanation of the Cartographer solution: This is based on an optimized SLAM algorithm, which uses a method of local mapping and closed-loop detection at the front end and global mapping at the back end, and then uses global mapping to optimize local mapping at the front end.

[0025] The navigation module adopts the Nav2 solution. First of all, considering the interface with other modules, the navigation module has two main mapping units: GlobalMapping (global mapping module) and LocalMapping (local mapping module). GlobalMapping calls the service provided by the MapServer module in the ROS2 robot system based on the map file generated by the global mapping module. The import of the global map can adjust the parameters according to the preset anti-collision obstacle expansion distance and the specific vehicle wheel set scheme. In view of the fact that most warehouses adopt a large flat-floor building model with less vertical movement, the two-dimensional plane map imported by Nav2 can meet most of the mapping needs. LocalMapping accurately calculates the specific location of the warehouse smart car based on the obstacle distance obtained by dynamic radar scanning. According to the positional relationship between the warehouse smart car and the target point, the navigation module plans the route of the warehouse smart car.

[0026] It is worth mentioning that the positioning method used by Nav2 is AMCL positioning. For distance sensors such as 2D radar and 3D radar, its role in the positioning process is to solve The calculation problem is how to consider the distance sensor information z after the location x of the warehouse smart car is given. If the observation data is given:

[0027] Then the following statement can be made:

[0028] In the above formula, x and m are the estimated location information of the warehouse smart car, and m is the map information. It can be expressed as the confidence level of the distance sensor information z being true given the location information and map information of the warehouse smart car. By evaluating the location of the warehouse smart car with high confidence, the location information of the warehouse smart car can be obtained.

[0029] Considering the operation mode of the specific internal algorithm, the navigation module contains four service nodes: BTNavigatorServer, RecoveryServer, ControllerServer and PlannerServer. Among them, PlannerServer provides services related to GlobalMapping, and controls the setting of the obstacle expansion map through the specific parameters of Costmap, so as to realize the anti-collision function for static obstacles. ControllerServer provides services related to LocalMapping, and combines the obstacle information / Laserscan of the dynamic radar scanning module to effectively perform the obstacle avoidance task of dynamic obstacles in the warehouse environment such as warehouse employees and other intelligent vehicles. RecoveryServer is responsible for handling fault conditions, such as algorithm restart when the scanning device fails temporarily, thereby enhancing the robustness of the overall algorithm. Finally, BTNavigatorServer builds the overall behavior tree of the robot and is responsible for sending speed information / cmd_vel to the communication module.

[0030] This smart car deployment solution adopts a host-server structure as a whole, and the communication between the host and server is realized through a virtual serial port device to realize USB interrupt reception and data parsing functions. In the thread initialization phase, a virtual serial port device is created through the RT-Thread kernel. When the host receives data, the virtual serial port device triggers an interrupt after the USB interface receives the data. The interrupt function is responsible for parsing the data and performing corresponding processing.

[0031] In terms of communication protocol, the linear speed method is used to control the movement of the warehouse smart car; the linear speed of the warehouse smart car in the x, y, and z directions is forced to be converted from the int32 type to the uint32 type, and then grouped into eight bits each to form a data frame with a total length of 13, and a CRC check is attached. Through this method, the lower computer receives and parses the running speed command sent by the upper computer through the USB virtual serial port device, thereby controlling the smart car to achieve motion obstacle avoidance and navigation functions. In order to achieve the above functions, a specific protocol scheme is specially formulated. Table 1 is an example of a general frame: Table 1 General frame examples

[0032] The data field is customized by the transmitted data packet. Table 2, Table 3 and Table 4 are examples of speed control, odometer closed loop and IMU data feedback respectively: Table 2 Speed ​​control frame example

[0033] Table 3 Odometry closed-loop frame example

[0034] Table 4 IMU data feedback example The lower computer reads the data from the drive wheel encoder to calculate the drive wheel speed, thereby inferring the odometer track. At the same time, the lower computer also reads the IMU sensor data installed on the vehicle body to achieve the fusion of odometer track data and IMU data, making the closed loop more accurate and reliable.

[0035] Through the above scheme, this smart car method can adapt to the complex environment of indoor warehousing scenarios, complete the tasks of mapping, positioning and navigation, and has the characteristics of easy deployment, low cost and strong adaptability.

[0036] It should be understood by those skilled in the art that the discussion of any of the above embodiments is merely illustrative and is not intended to imply that the scope of the present disclosure (including the claims) is limited to these examples. Within the scope of the present invention, the technical features in the above embodiments or in different embodiments may be combined, the steps may be implemented in any order, and there are many other variations of the different aspects of the present invention as described above, which are not provided in detail for the sake of simplicity.

[0037] The embodiments of the present invention are intended to cover all such substitutions, modifications and variations that fall within the broad scope of the appended claims. Therefore, any omissions, modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A path planning method for a warehouse intelligent vehicle based on mapping and navigation algorithms, characterized in that: The steps include: Step 1: The dynamic radar scanning module detects and obtains environmental information in real time, and the mapping module combines the Gmapping algorithm and the Cartographer algorithm to complete the pre-map construction of the global map; Step 2: The navigation module uses the Nav2 system to perform positioning and path planning based on the pre-built map of the global map, calculates the movement path and speed information of the warehouse smart car, and generates navigation instructions; Step 3: The communication module transmits the navigation instruction to the warehouse smart vehicle, and the warehouse smart vehicle executes the navigation instruction to complete the predetermined motion task.

2. The path planning method according to claim 1, characterized in that: The dynamic radar scanning module detects and obtains environmental information in real time, specifically including: The dynamic radar scanning module provides real-time ranging information and obstacle detection information, and compresses the three-dimensional point cloud information through the open source Linefitgroundsegmentation technology to achieve three-dimensional to two-dimensional laser data conversion.

3. The path planning method according to claim 2, characterized in that: The dynamic radar scanning module includes 2D radar deployment and 3D radar deployment. The 3D radar deployment obtains point cloud information, converts / Pointcloud into / Laserscan information, and divides the point cloud information obtained by the 3D radar into N segments at equal intervals. Each segment is further subdivided into M Bins. By projecting all point cloud information of the current frame into the segment, the three-dimensional point coordinates p(x, y, z) are reduced to two-dimensional coordinates p'(d, z). In this process, the two-dimensional coordinates p'(d, z) corresponding to each point are obtained, and the smallest p'(d, z) in each bin is fitted to obtain a straight line z=kd+b, where k represents the slope of the road surface. The height from the point to the road surface will be judged based on the straight line. If the height is less than a threshold, the point is determined to be a road surface point, otherwise it is an obstacle point.

4. The path planning method according to claim 1, characterized in that: The mapping module combines Gmapping and Cartographer algorithms to complete the construction of the global map, specifically including: The Gmapping algorithm separates the problems of positioning and mapping in SLAM, namely: in To control data, is the observed data, and m are positioning information and map information respectively. The algorithm includes four steps, namely sampling, weight calculation, resampling and map estimation. First, a particle estimate with a larger weight is obtained from the motion model, and then a proposed distribution is simulated based on the particle estimate. The weight is used to describe the difference between the target distribution and the proposed distribution. Then resampling is performed, and the maps fitted from different paths are integrated to generate a global map. The Cartographer algorithm is based on the optimized SLAM algorithm, which adopts the method of front-end local mapping, closed-loop detection, and back-end global mapping, and then uses global mapping to optimize the front-end local mapping.

5. The path planning method according to claim 1, characterized in that: The navigation module uses the Nav2 system to perform positioning and path planning, calculates the movement path and speed information of the warehouse smart car, and generates navigation instructions, specifically including: The navigation module uses the Nav2 system, combined with the AMCL algorithm, to calculate the online cost map and perform real-time positioning and path planning for the warehouse smart vehicle; The navigation module includes a global mapping module and a local mapping module. The global mapping module calls the service provided by the MapServer module in the ROS2 robot system based on the pre-map of the global map generated by the mapping module, and adjusts the parameters according to the preset anti-collision obstacle expansion distance and the vehicle wheel group scheme of the warehouse smart car; The local mapping module calculates the specific position of the warehouse smart car based on the obstacle distance obtained by the dynamic radar module scanning, plans the travel route of the warehouse smart car according to the positional relationship between the warehouse smart car and the target point, and generates navigation instructions.

6. The path planning method according to claim 5, characterized in that: The navigation module includes four service nodes: BTNavigatorServer, RecoveryServer, ControllerServer and PlannerServer, among which PlannerServer provides services related to the global mapping module, and controls the setting of the obstacle expansion map through the specific parameters of Costmap, so as to realize the anti-collision function of static obstacles; ControllerServer provides services related to the local mapping module, and combines the obstacle information / Laserscan of the dynamic radar scanning module to effectively perform the obstacle avoidance task of dynamic obstacles in the warehouse environment such as warehouse employees and other smart vehicles; RecoveryServer is responsible for handling fault conditions, and BTNavigatorServer builds the overall behavior tree of the warehouse smart car, and is responsible for sending navigation instructions to the communication module in the end.

7. The path planning method according to claim 1, characterized in that: The communication module uses the USB virtual serial port of the RT-Thread framework to realize the communication between the upper and lower computers, and parses the data frame according to the upper and lower computer communication protocol that supports CRC verification, and sends navigation instructions.

Images