Vehicle positioning and navigation method, forklift operation control method and related equipment

Abstract

The invention discloses a vehicle positioning navigation method, a forklift operation control method and related equipment, and the method comprises the steps: selecting a fixed point in an operation region as an anchor point, taking the anchor point as an original point of a global point cloud map, fusing IMU positioning data and laser point cloud data, and generating the global point cloud map; preset positioning steps are repeatedly executed until the vehicle arrives at a designated operation point: whether the vehicle is outdoor or indoor is judged based on RTK state data corresponding to the RTK positioning data, and an initial pose is generated based on a judgment result, the global point cloud map, the RTK positioning data and the laser point cloud data; fusing the IMU positioning data, the RTK positioning data and the laser point cloud data based on the initial pose or the real-time pose of the vehicle obtained in the previous cycle and a preset adaptive repositioning strategy to obtain the real-time pose of the vehicle; and judging whether the vehicle arrives at a specified operation point based on the real-time pose of the vehicle. According to the invention, high-precision positioning, self-adaptive repositioning and millimeter-level operation control of the vehicle in an indoor and outdoor mixed scene can be realized.

Description

Vehicle positioning and navigation methods, forklift operation control methods and related equipment Technical Field

[0001] This invention belongs to the field of robot mapping and localization, specifically relating to a vehicle positioning and navigation method, a forklift operation control method, and related equipment. Background Technology

[0002] With the rapid development of computer and sensor technologies, intelligent robots have been widely applied in various fields such as industry, transportation, and daily services. Among them, unmanned forklifts, as a typical application of intelligent robots, play a key handling role in intelligent warehousing and logistics.

[0003] To balance the cost of building a positioning system with operational flexibility, the mainstream positioning method in material handling scenarios currently relies on laser SLAM technology, which achieves positioning and navigation through environmental scanning mapping and real-time point cloud matching. However, this technology has the following significant drawbacks: insufficient positioning accuracy—laser positioning alone cannot meet the millimeter-level accuracy requirements for picking up pallets, easily leading to low operational efficiency and safety hazards; coordinate system drift—the closed relative map built by laser SLAM lacks absolute scale, and each mapping generates an independent coordinate system, resulting in inconsistent coordinates of work points and hindering multi-task collaborative planning; manual repositioning—the system needs to be initialized from the mapping starting point upon startup, and manual input of pose is required after an unexpected power outage, resulting in low levels of intelligence; and poor environmental adaptability—a single laser sensor is not robust enough in complex scenarios, sparse features in open outdoor areas easily lead to positioning loss, and stability is difficult to guarantee when switching between indoor and outdoor environments.

[0004] Chinese invention patent application CN 117760407 A discloses a robot positioning and navigation system and method based on multi-positioning sensor fusion. It proposes to constrain laser positioning results by introducing RTK positioning information and to improve the positioning accuracy and stability of the system by fusing odometer, inertial measurement unit, and BeiDou RTK positioning data through two extended Kalman filters, ensuring stable and reliable outdoor inspection of substation robots. However, this solution still cannot achieve the millimeter-level positioning accuracy required for forklift operations, and the coordinate system drifts when the starting point is used as the mapping origin and mapping is repeated.

[0005] Chinese invention patent application CN 119104056 A discloses a robot localization and navigation system and method based on multimodal information fusion. This method integrates motion information from depth cameras, BeiDou navigation, and IMU (Installation Unit) to calculate motion observation residuals and a nonlinear optimization function. The optimized variables are then input into an autotransformer PID collaborative control system to ensure positioning accuracy and real-time motion control. However, this scheme involves real-time fusion of visual and laser constraints, which is computationally complex and resource-intensive, and it does not mention a crucial relocation solution during the localization process.

[0006] The thesis from North China Electric Power University, titled "Research on Multi-Source Fusion Positioning Method for Indoor and Outdoor Complex Environments," discloses a method that fuses GPS / IMU information using Kalman filtering outdoors, and establishes an adaptive Monte Carlo positioning model indoors relying on visual odometry and IMU. During indoor-outdoor transitions, an interactive multi-model approach ensures smooth switching of positioning results, guaranteeing the stability of the joint indoor-outdoor positioning. However, this scheme fails to address the problem of repeated mapping drift, and indoor repositioning relies on particle convergence, which is time-consuming, inefficient, and prone to failure. Summary of the Invention

[0007] To address the aforementioned issues, this invention proposes a vehicle positioning and navigation method, a forklift operation control method, and related equipment, which can achieve high-precision positioning, adaptive repositioning, and millimeter-level operation control of vehicles (such as unmanned forklifts) in mixed indoor and outdoor scenarios, significantly reducing manual intervention and improving the reliability and intelligence of the entire positioning system.

[0008] To achieve the above-mentioned technical objectives and effects, the present invention is implemented through the following technical solution:

[0009] In a first aspect, the present invention provides a vehicle positioning and navigation method based on multi-source information fusion, comprising:

[0010] A fixed point within the work area is selected as the anchor point, and the anchor point is used as the origin of the global point cloud map. The IMU positioning data and laser point cloud data during the vehicle's movement are fused to generate a global point cloud map.

[0011] Repeat the preset positioning steps until the vehicle reaches the designated work point. The preset positioning steps include:

[0012] Based on the RTK status data corresponding to the RTK positioning data, it is determined whether the vehicle is outdoors or indoors, and based on the determination result, global point cloud map, RTK positioning data and laser point cloud data, an initial pose is generated;

[0013] Based on the initial pose or the real-time pose of the vehicle obtained in the previous cycle, and the preset adaptive relocation strategy, the vehicle's real-time pose is obtained by fusing IMU positioning data, RTK positioning data and laser point cloud data during the vehicle's motion.

[0014] Determine whether the vehicle has reached the designated work point based on the vehicle's real-time position and orientation.

[0015] In conjunction with the first aspect, optionally, the method for generating the global point cloud map includes:

[0016] Determine a fixed point in the work area as an anchor point and record the longitude, latitude, and altitude information (lon0, lat0, alt0) of the fixed point.

[0017] Place the vehicle at any location in the work area, and use the anchor point as the origin of the global point cloud map coordinate system. Solve the RTK positioning data (lon1,lat1,alt1,hdg1) corresponding to the current position of the vehicle to obtain the current pose (x1,y1,z1,yaw1) of the vehicle in the global point cloud map coordinate system. lon1,lat1,alt1,hdg1 represent the longitude, latitude, altitude, and orientation of the vehicle, respectively, and x1,y1,z1,yaw1 represent the X-axis coordinate, Y-axis coordinate, Z-axis coordinate, and heading, respectively.

[0018] Using the current pose (x1, y1, z1, yaw1) as the initial pose for mapping, the vehicle is controlled to traverse the work area, and IMU positioning data and laser point cloud data during vehicle movement are fused to generate a global point cloud map.

[0019] In conjunction with the first aspect, optionally, the step of using the current pose (x1, y1, z1, yaw1) as the initial pose for mapping, controlling the vehicle to traverse the work area, and fusing IMU positioning data and laser point cloud data during vehicle movement to generate a global point cloud map includes:

[0020] Receive the 3D laser point cloud under the current pose (x1, y1, z1, yaw1), and perform segmentation, filtering and distortion removal processing on the 3D laser point cloud. Combine the processed 3D laser point cloud with the current pose (x1, y1, z1, yaw1) to form the initial frame sub-image M1, and register the initial frame sub-image M1 with the global point cloud map.

[0021] The vehicle is controlled to begin moving. The real-time acquired IMU positioning data is integrated to obtain the predicted pose (x2, y2, z2, yaw2), where x2, y2, z2, and yaw2 represent the X-axis coordinate, Y-axis coordinate, Z-axis coordinate, and heading, respectively. The cumulative movement distance is then calculated. Greater than the set distance threshold, or cumulative orientation When the change is greater than the set orientation threshold, the three-dimensional laser point cloud under the predicted pose (x2, y2, z2, yaw2) is received, and the three-dimensional laser point cloud is segmented, filtered and deformed. The processed three-dimensional laser point cloud and the predicted pose (x2, y2, z2, yaw2) are combined to form a new frame of the preparatory sub-image M2.

[0022] The three-dimensional laser point cloud in the new frame's preparatory sub-image M2 and the previous frame's sub-image M1 is registered using nonlinear least squares optimization. Based on the registration result, the pose information in the preparatory sub-image M2 is updated and optimized to obtain the optimized sub-image M2'. Sub-image M2' is then registered to the global point cloud map.

[0023] Control the vehicle to move continuously within the work area and continuously register new sub-maps to the global point cloud map;

[0024] When the current position is determined to be a previously visited position based on the pose information in the subgraph, a loop closure detection is performed. The loop closure detection includes: performing nonlinear least squares optimization update on all subgraphs in the global point cloud map based on the overlap constraint of two subgraphs when the vehicle visits the same position in succession, so as to minimize the matching error of two subgraphs at the same position, and then registering the optimized and updated subgraphs in the global point cloud map.

[0025] Repeat the above steps until a global point cloud map of the entire work area is generated in the global point cloud map coordinate system with the anchor point as the origin, and the indoor area is marked.

[0026] In conjunction with the first aspect, optionally, the step of determining whether the vehicle is outdoors or indoors based on RTK status data corresponding to RTK positioning data, and generating an initial pose based on the determination result, the global point cloud map, the RTK positioning data, and the laser point cloud data, includes:

[0027] If the RTK state data is a stable solution, the vehicle is determined to be outdoors. The current RTK positioning data (lon3,lat3,alt3,hdg3) is directly calculated into the global point cloud map coordinate system with the anchor point as the origin to obtain the corresponding initial pose (x3,y3,z3,yaw3) and complete the outdoor autonomous relocalization. Here, lon3,lat3,alt3,hdg3 represent the vehicle's longitude, latitude, altitude, and orientation, respectively, and x3,y3,z3,yaw3 represent the X-axis coordinate, Y-axis coordinate, Z-axis coordinate, and heading, respectively.

[0028] If the RTK state data is an unstable solution state, the vehicle is determined to be indoors. Based on the current laser point cloud data, the branch and bound algorithm is used to match the indoor area marked on the global point cloud map to obtain the corresponding initial pose (x4, y4, z4, yaw4) and complete the indoor autonomous relocalization. Here, x4, y4, z4, and yaw4 represent the X-axis coordinate, Y-axis coordinate, Z-axis coordinate, and heading, respectively.

[0029] In conjunction with the first aspect, optionally, the step of matching the current laser point cloud data with the marked indoor area on the global point cloud map using a branch and bound algorithm to obtain the corresponding pose (x4, y4, z4, yaw4) includes:

[0030] The indoor areas marked on the global point cloud map are projected in two dimensions along the Z-axis according to the principle of gradually increasing resolution, forming raster map layer 1, raster map layer 2, ..., raster map layer N in sequence;

[0031] Starting from raster map layer N, the current laser point cloud data is projected according to the resolution of raster map layer N to obtain real-time point cloud projection raster M. N ;

[0032] Based on the resolution of the raster map layer N and the laser point cloud sensing radius, calculate the real-time point cloud projection raster M. N The angular resolution theta1 is matched with the rotation of the raster map layer N, theta1 = acos(1 - (Δ²) / (2R²)), where Δ is the resolution of the raster map layer N and R is the sensing radius of the laser point cloud;

[0033] Projecting real-time point cloud raster M N At the center, all cells of the raster map layer N are traversed and matched. During the traversal of each cell, the real-time point cloud is projected onto the raster M according to the angular resolution theta1. N The process involves rotating the cells one by one and then matching them with the grid map layer N to obtain a matching score, until all cells of the grid map layer N have been traversed.

[0034] Based on the matching score, determine the cell with the highest score in raster map layer N, select the area in raster map layer N-1 that overlaps with the cell with the highest score, and discard the other areas;

[0035] Using the same method, the real-time point cloud projection grid M is obtained. N-1 Using the angular resolution theta2, traverse and match all cells in the selected area of ​​raster map layer N-1;

[0036] Repeat the above operation until the best matching cell is found in the first layer of the raster map. The combination of the coordinates and orientation of the best matching cell in the global point cloud map is the corresponding initial pose (x4, y4, z4, yaw4).

[0037] In conjunction with the first aspect, optionally, the step of obtaining the real-time vehicle pose based on the initial pose or the real-time vehicle pose obtained in the previous cycle, and a preset adaptive relocalization strategy, by fusing IMU positioning data, RTK positioning data, and laser point cloud data during vehicle motion, includes:

[0038] Based on the initial pose or the real-time vehicle pose obtained in the previous cycle, the predicted pose Tpred corresponding to the current laser point cloud data is obtained by integrating the IMU positioning data.

[0039] If, within a preset radius threshold, the radius threshold is less than or equal to the laser point cloud sensing radius, and the current laser point cloud data contains other point cloud data besides the ground point cloud data, then it is determined that the point cloud matching pose update condition is met, and the NDT point cloud matching strategy is executed. The NDT point cloud matching strategy includes:

[0040] The global point cloud map is divided into voxels to obtain a number of voxels, and the normal distribution model of each voxel is calculated.

[0041] Based on the predicted pose Tpred, the current laser point cloud data is converted into a global point cloud map. Each point cloud falls into a different voxel and is fed into its respective normal distribution model to calculate the probability score Pi of each point cloud.

[0042] Summing the probability scores Pi of all point clouds yields Pa;

[0043] The predicted pose Tpred is continuously adjusted to obtain the updated pose Tupdate;

[0044] When Pa is at its maximum, the updated pose Tupdate is the optimal pose, and the optimal pose is used as the real-time pose of the vehicle.

[0045] If, within a preset radius threshold, the current laser point cloud data contains only ground point cloud data, it is determined that the point cloud matching pose update condition is not met, and an RTK fusion positioning strategy is executed. The RTK fusion positioning strategy includes:

[0046] The received RTK positioning data is solved into the global point cloud map coordinate system with the anchor point as the origin to obtain the corresponding current pose (x5, y5, z5, yaw5), where x5, y5, z5, and yaw5 represent the X-axis coordinate, Y-axis coordinate, Z-axis coordinate, and heading, respectively.

[0047] Perform extended Kalman filtering on the current pose (x5, y5, z5, yaw5) and the predicted pose Tpred to obtain the optimal pose, and use the optimal pose as the real-time pose of the vehicle.

[0048] Secondly, the present invention provides a method for controlling an unmanned forklift, comprising:

[0049] Based on the vehicle positioning and navigation method described in any one of the first aspects, control the unmanned forklift to run to the designated work point;

[0050] Based on the pallet image and depth point cloud data collected by the visual sensor when the unmanned forklift is located at the designated work point, the pose deviation between the unmanned forklift and the hole on the pallet is calculated.

[0051] Based on the posture deviation, a hierarchical control command is generated, and the hierarchical control command is used to control the electronic control driver to drive the corresponding actuator to perform the corresponding action and complete the forklift operation.

[0052] In conjunction with the second aspect, optionally, the calculation of the pose deviation between the unmanned forklift and the holes on the pallet based on the pallet image and depth point cloud data collected by the visual sensor when the unmanned forklift is located at the designated work point includes:

[0053] Acquire the stack image and depth point cloud data collected by the visual sensor, and perform downsampling and intensity filtering on the depth point cloud data to obtain processed depth camera data, wherein the depth camera data includes the stack image and the processed depth point cloud data;

[0054] The processed depth camera data is segmented into a stack region, and the stack plane is extracted using the RANSAC algorithm to obtain the stack plane.

[0055] Hole features are extracted from the planar surface of the pallet to identify holes;

[0056] The identified holes are morphologically optimized to smooth their outlines, resulting in optimized holes.

[0057] Based on the optimized holes, the center pixel coordinates of the holes in the camera coordinate system are identified, and the pose deviation between the unmanned forklift and the holes on the pallet in the camera coordinate system is calculated using the PnP algorithm.

[0058] Based on the extrinsic parameter transformation from the camera to the center of the unmanned forklift, the pose deviation between the unmanned forklift and the hole on the pallet in the vehicle coordinate system is obtained.

[0059] In conjunction with the second aspect, optionally, the step of generating hierarchical control commands based on the pose deviation, and using the hierarchical control commands to control the electronically controlled driver to drive the corresponding actuator to perform corresponding actions, includes:

[0060] Based on the pose deviation between the unmanned forklift and the hole on the pallet, inverse kinematics is performed. A graded motion strategy is generated based on the inverse kinematics results. The unmanned forklift executes the graded motion strategy as a whole. The graded motion strategy includes: first, coarse alignment is performed, and the forklift moves at a first percentage of the maximum speed; when the real-time pose deviation between the unmanned forklift and the hole on the pallet is less than the design threshold D1, fine alignment is performed, and the forklift moves at a second percentage of the maximum speed, executing incremental PID control until the real-time pose deviation between the unmanned forklift and the hole on the pallet is less than the design distance threshold D2, at which point the unmanned forklift stops moving; the second percentage is less than the first percentage.

[0061] The image of the pallet and the depth point cloud data collected by the visual sensor are acquired again, and the real-time pose deviation between the unmanned forklift and the hole on the pallet is calculated. When the real-time pose deviation between the unmanned forklift and the hole on the pallet is less than the distance threshold D3 required for picking up the goods, the unmanned forklift is controlled to move forward to pick up the goods.

[0062] Thirdly, the present invention provides a vehicle positioning and navigation system based on multi-source information fusion, comprising: a controller, and a laser sensor, an inertial measurement unit and an RTK positioning module connected to the controller, wherein the laser sensor, the inertial measurement unit and the RTK positioning module are all used to be installed on a vehicle;

[0063] The laser sensor is used to obtain laser point cloud data;

[0064] The inertial measurement unit is used to obtain IMU positioning data;

[0065] The RTK positioning module is used to obtain RTK positioning data;

[0066] The controller is configured to perform the method described in any one of the first aspects.

[0067] Fourthly, the present invention provides an unmanned forklift control system, including a controller, a laser sensor, an inertial measurement unit, an RTK positioning module, a vision sensor, an electronically controlled driver connected to the controller, and an actuator connected to the electronically controlled driver; the laser sensor, inertial measurement unit, RTK positioning module, vision sensor, electronically controlled driver, electronically controlled driver, and actuator are all used to be installed on the forklift.

[0068] The laser sensor is used to obtain laser point cloud data;

[0069] The inertial measurement unit is used to obtain IMU positioning data;

[0070] The RTK positioning module is used to obtain RTK positioning data;

[0071] The visual sensor is used to acquire images of the stack and depth point cloud data;

[0072] The controller is configured to perform the method according to any one of the second aspects.

[0073] Fifthly, the present invention provides a vehicle positioning and navigation system based on multi-source information fusion, including a storage medium and a processor;

[0074] The storage medium is used to store instructions;

[0075] The processor is configured to operate according to the instructions to perform the method according to any one of the first aspects.

[0076] In a sixth aspect, the present invention provides an unmanned forklift control system, including a storage medium and a processor;

[0077] The storage medium is used to store instructions;

[0078] The processor is configured to operate according to the instructions to perform the method according to any one of the second aspects.

[0079] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0080] This invention proposes a vehicle positioning and navigation method, a forklift operation control method, and related equipment, which can achieve high-precision positioning, adaptive repositioning, and millimeter-level operation control of vehicles (such as unmanned forklifts) in mixed indoor and outdoor scenarios, significantly reducing manual intervention and improving the reliability and intelligence of the entire positioning system. Attached Figure Description

[0081] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly described below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort, wherein:

[0082] Figure 1 is a schematic diagram of the structure of a driving operation mapping and positioning system according to an embodiment of the present invention;

[0083] Figure 2 is a flowchart of map construction and geographic coordinate system alignment according to an embodiment of the present invention;

[0084] Figure 3 is a flowchart of an autonomous relocation and fusion positioning and navigation method according to an embodiment of the present invention;

[0085] Figure 4 is a flowchart of a forklift operation control method according to an embodiment of the present invention. Detailed Implementation

[0086] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

[0087] Furthermore, if the embodiments of this invention involve descriptions such as "first" or "second," these descriptions are for descriptive purposes only and should not be construed as indicating or implying their relative importance or implicitly specifying the number of technical features indicated. Therefore, a feature defined with "first" or "second" may explicitly or implicitly include at least one of those features. Additionally, the technical solutions of the various embodiments can be combined with each other, but this must be based on the ability of those skilled in the art to implement them. If the combination of technical solutions is contradictory or impossible to implement, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed by this invention.

[0088] Example 1

[0089] This invention provides a vehicle positioning and navigation method based on multi-source information fusion, comprising the following steps:

[0090] (1) Select a fixed point within the work area as the anchor point, and use the anchor point as the origin of the global point cloud map. Integrate the IMU positioning data and laser point cloud data during the vehicle movement process to generate a global point cloud map.

[0091] (2) Repeat the preset positioning steps until the vehicle reaches the designated work point. The preset positioning steps include:

[0092] Based on the RTK status data corresponding to the RTK positioning data, it is determined whether the vehicle is outdoors or indoors, and based on the determination result, global point cloud map, RTK positioning data and laser point cloud data, an initial pose is generated;

[0093] Based on the initial pose or the real-time pose of the vehicle obtained in the previous cycle, and the preset adaptive relocation strategy, the vehicle's real-time pose is obtained by fusing IMU positioning data, RTK positioning data and laser point cloud data during the vehicle's motion.

[0094] Determine whether the vehicle has reached the designated work point based on the vehicle's real-time position and orientation.

[0095] Based on the above solution, high-precision positioning, adaptive repositioning, and millimeter-level operation control of vehicles (such as unmanned forklifts) in mixed indoor and outdoor scenarios can be achieved, significantly reducing manual intervention and improving the reliability and intelligence of the entire positioning system.

[0096] In one specific embodiment of the present invention, the method for generating the global point cloud map includes:

[0097] Determine a fixed point in the work area as an anchor point and record the longitude, latitude, and altitude information (lon0, lat0, alt0) of the fixed point.

[0098] Place the vehicle at any location in the work area, and use the anchor point as the origin of the global point cloud map coordinate system. Solve the RTK positioning data (lon1,lat1,alt1,hdg1) corresponding to the current position of the vehicle to obtain the current pose (x1,y1,z1,yaw1) of the vehicle in the global point cloud map coordinate system. lon1,lat1,alt1,hdg1 represent the longitude, latitude, altitude, and orientation of the vehicle, respectively, and x1,y1,z1,yaw1 represent the X-axis coordinate, Y-axis coordinate, Z-axis coordinate, and heading, respectively.

[0099] Using the current pose (x1, y1, z1, yaw1) as the initial pose for mapping, the vehicle is controlled to traverse the work area, and IMU positioning data and laser point cloud data during vehicle movement are fused to generate a global point cloud map.

[0100] In the above scheme, the mapping drift problem is solved by anchor points during the process of building a global point cloud map, aligning the laser mapping with the geographic coordinate system, ensuring the consistency of repeated mapping and positioning coordinate system, and supporting multi-task collaboration and historical data reuse.

[0101] In one specific embodiment of the present invention, the step of using the current pose (x1, y1, z1, yaw1) as the initial pose for mapping, controlling the vehicle to traverse the work area, and fusing IMU positioning data and laser point cloud data during vehicle movement to generate a global point cloud map includes:

[0102] Receive the 3D laser point cloud under the current pose (x1, y1, z1, yaw1), and perform segmentation, filtering and distortion removal processing on the 3D laser point cloud. Combine the processed 3D laser point cloud with the current pose (x1, y1, z1, yaw1) to form the initial frame sub-image M1, and register the initial frame sub-image M1 with the global point cloud map.

[0103] The vehicle is controlled to begin moving. The real-time acquired IMU positioning data is integrated to obtain the predicted pose (x2, y2, z2, yaw2), where x2, y2, z2, and yaw2 represent the X-axis coordinate, Y-axis coordinate, Z-axis coordinate, and heading, respectively. The cumulative movement distance is then calculated. Greater than the set distance threshold, or cumulative orientation When the change is greater than the set orientation threshold, the three-dimensional laser point cloud under the predicted pose (x2, y2, z2, yaw2) is received, and the three-dimensional laser point cloud is segmented, filtered and deformed. The processed three-dimensional laser point cloud and the predicted pose (x2, y2, z2, yaw2) are combined to form a new frame of the preparatory sub-image M2.

[0104] The three-dimensional laser point cloud in the new frame's preparatory sub-image M2 and the previous frame's sub-image M1 is registered using nonlinear least squares optimization. Based on the registration result, the pose information in the preparatory sub-image M2 is updated and optimized to obtain the optimized sub-image M2'. Sub-image M2' is then registered to the global point cloud map.

[0105] Control the vehicle to move continuously within the work area and continuously register new sub-maps to the global point cloud map;

[0106] When the current position is determined to be a previously visited position based on the pose information in the subgraph, a loop closure detection is performed. The loop closure detection includes: performing nonlinear least squares optimization update on all subgraphs in the global point cloud map based on the overlap constraint of two subgraphs when the vehicle visits the same position in succession, so as to minimize the matching error of two subgraphs at the same position, and then registering the optimized and updated subgraphs in the global point cloud map.

[0107] Repeat the above steps until a global point cloud map of the entire work area is generated in the global point cloud map coordinate system with the anchor point as the origin, and the indoor area is marked.

[0108] In one specific embodiment of the present invention, the step of determining whether the vehicle is outdoors or indoors based on RTK status data corresponding to RTK positioning data, and generating an initial pose based on the determination result, global point cloud map, RTK positioning data, and laser point cloud data, includes:

[0109] If the RTK state data is in a stable solution state, it is determined that the vehicle is outdoors. The current RTK positioning data (lon3,lat3,alt3,hdg3) is directly solved into the global point cloud map coordinate system with the anchor point as the origin to obtain the corresponding initial pose (x3,y3,z3,yaw3) and complete the outdoor autonomous relocalization. Here, lon3,lat3,alt3,hdg3 represent the vehicle's longitude, latitude, altitude, and orientation, respectively, and x3,y3,z3,yaw3 represent the X-axis coordinate, Y-axis coordinate, Z-axis coordinate, and heading, respectively.

[0110] If the RTK state data is an unstable solution state, the vehicle is determined to be indoors. Based on the current laser point cloud data, the branch and bound algorithm is used to match the indoor area marked on the global point cloud map to obtain the corresponding initial pose (x4, y4, z4, yaw4) and complete the indoor autonomous relocalization. Here, x4, y4, z4, and yaw4 represent the X-axis coordinate, Y-axis coordinate, Z-axis coordinate, and heading, respectively.

[0111] In one specific embodiment of the present invention, the step of matching the current laser point cloud data with the indoor area marked on the global point cloud map based on the branch and bound algorithm to obtain the corresponding pose (x4, y4, z4, yaw4) includes:

[0112] The indoor areas marked on the global point cloud map are projected in two dimensions along the Z-axis according to the principle of gradually increasing resolution, forming raster map layer 1, raster map layer 2, ..., raster map layer N in sequence;

[0113] Starting from raster map layer N, the current laser point cloud data is projected according to the resolution of raster map layer N to obtain real-time point cloud projection raster M. N ;

[0114] Based on the resolution of the raster map layer N and the laser point cloud sensing radius, calculate the real-time point cloud projection raster M. N The angular resolution theta1 is matched with the rotation of the raster map layer N, theta1 = acos(1 - (Δ²) / (2R²)), where Δ is the resolution of the raster map layer N and R is the sensing radius of the laser point cloud;

[0115] Projecting real-time point cloud raster M N At the center, all cells of the raster map layer N are traversed and matched. During the traversal of each cell, the real-time point cloud is projected onto the raster M according to the angular resolution theta1. N The process involves rotating the cells one by one and then matching them with the grid map layer N to obtain a matching score, until all cells of the grid map layer N have been traversed.

[0116] Based on the matching score, determine the cell with the highest score in raster map layer N, select the area in raster map layer N-1 that overlaps with the cell with the highest score, and discard the other areas;

[0117] Using the same method, the real-time point cloud projection grid M is obtained. N-1 Using the angular resolution theta2, traverse and match all cells in the selected area of ​​raster map layer N-1;

[0118] Repeat the above operation until the best matching cell is found in the first layer of the raster map. The combination of the coordinates and orientation of the best matching cell in the global point cloud map is the corresponding initial pose (x4, y4, z4, yaw4).

[0119] In one specific embodiment of the present invention, the step of obtaining the real-time vehicle pose based on the initial pose or the real-time vehicle pose obtained in the previous cycle, and a preset adaptive relocalization strategy, by fusing IMU positioning data, RTK positioning data, and laser point cloud data during vehicle motion, includes:

[0120] Based on the initial pose or the real-time vehicle pose obtained in the previous cycle, the predicted pose Tpred corresponding to the current laser point cloud data is obtained by integrating the IMU positioning data.

[0121] If, within a preset radius threshold, the radius threshold is less than or equal to the laser point cloud sensing radius (preferably less than the laser point cloud sensing radius to improve data validity), and the current laser point cloud data contains other point cloud data besides the ground point cloud data, then it is determined that the point cloud matching pose update condition is met, and the NDT point cloud matching strategy is executed. The NDT point cloud matching strategy includes:

[0122] The global point cloud map is divided into voxels to obtain a number of voxels, and the normal distribution model of each voxel is calculated.

[0123] Based on the predicted pose Tpred, the current laser point cloud data is converted into a global point cloud map. Each point cloud falls into a different voxel and is fed into its respective normal distribution model to calculate the probability score Pi of each point cloud.

[0124] Summing the probability scores Pi of all point clouds yields Pa;

[0125] The predicted pose Tpred is continuously adjusted to obtain the updated pose Tupdate;

[0126] When Pa is at its maximum, the updated pose Tupdate is the optimal pose, and the optimal pose is used as the real-time pose of the vehicle.

[0127] If, within a preset radius threshold, the current laser point cloud data contains only ground point cloud data, it is determined that the point cloud matching pose update condition is not met, and an RTK fusion positioning strategy is executed. The RTK fusion positioning strategy includes:

[0128] The received RTK positioning data is solved into the global point cloud map coordinate system with the anchor point as the origin to obtain the corresponding current pose (x5, y5, z5, yaw5), where x5, y5, z5, and yaw5 represent the X-axis coordinate, Y-axis coordinate, Z-axis coordinate, and heading, respectively.

[0129] Perform extended Kalman filtering on the current pose (x5, y5, z5, yaw5) and the predicted pose Tpred to obtain the optimal pose, and use the optimal pose as the real-time pose of the vehicle.

[0130] In the above scheme, through indoor / outdoor scene recognition and adaptive relocation switching mechanisms, no manual intervention is required after a power outage and restart. Simultaneously, by selecting an indoor map area and employing a branch-and-bound algorithm, numerous invalid matches between real-time point clouds and prior maps are avoided during indoor relocation, optimizing computational efficiency. Furthermore, an extended Kalman filter algorithm is used to fuse data from RTK satellite positioning (RTK positioning data), inertial measurement unit navigation positioning (IMU positioning data), and laser navigation positioning (laser point cloud data), combining the positioning advantages of various sensors to ensure the reliability and accuracy of navigation for the unmanned forklift when operating indoors and outdoors. The vehicle's real-time pose is used in conjunction with the navigation algorithm to achieve vehicle navigation.

[0131] The vehicle positioning and navigation method of the present invention will be described in detail below with reference to a specific implementation method.

[0132] As shown in Figure 2, the vehicle positioning and navigation method based on multi-source information fusion specifically includes the following steps:

[0133] (1) Map construction and alignment with geographic coordinate system, the specific process is as follows:

[0134] A fixed point is determined in the work area, and the longitude, latitude, and altitude information (lon0, lat0, alt0) of the fixed point in the WGS-84 (World Geodetic System 1984) coordinate system are recorded. This fixed point is used as the anchor point. In the subsequent global point cloud map construction and positioning and navigation work, the anchor point is used as the origin of the coordinate system, and the east / north direction is used as the X / Y axis. Once the anchor point is determined, it will not be changed arbitrarily.

[0135] After controlling the unmanned forklift (in this embodiment, the vehicle is set as an unmanned forklift) to move to any outdoor location and then stop, the mapping function is activated;

[0136] The system receives the current position of the unmanned forklift recorded by the RTK positioning module, determines the longitude, latitude, altitude, and orientation information (lon1,lat1,alt1,hdg1) of the unmanned forklift, uses the anchor point as the origin of the Northeast Tianditu coordinate system, and calculates the current attitude (x1,y1,z1,yaw1) of the unmanned forklift in the Northeast Tianditu coordinate system. x1,y1,z1,yaw1 represent the X-axis, Y-axis, Z-axis coordinates and heading, respectively. The system initializes the mapping pose of the unmanned forklift using this attitude (x1,y1,z1,yaw1).

[0137] The mapping program for the unmanned forklift is initiated, controlling its movement to traverse the work area and generate a global point cloud map, specifically including:

[0138] Using (x1, y1, z1, yaw1) as the initial pose value, the system receives and processes the 3D laser point cloud in real time, performing segmentation, filtering, and distortion removal on the 3D laser point cloud. The processed 3D laser point cloud is then combined with the corresponding initial pose value (x1, y1, z1, yaw1) to form an initial frame sub-image M1, which is then registered with the global point cloud map.

[0139] The unmanned forklift is controlled to start moving. The IMU positioning data output by the inertial measurement unit is integrated to obtain the predicted pose (x2, y2, z2, yaw2). When the cumulative movement distance is... Greater than a threshold, such as 5 meters, or cumulative orientation If the change exceeds a threshold, such as 45 degrees, the 3D laser point cloud with the pose (x2, y2, z2, yaw2) will be received and segmented, filtered, and distortion-reduced. The processed 3D laser point cloud and the corresponding pose (x2, y2, z2, yaw2) will be combined to form a new frame's preliminary sub-image M2.

[0140] With the goal of maximizing the overlap of repeated point clouds between two adjacent sub-images, the 3D laser point clouds in the new frame's preliminary sub-image M2 and the previous frame's sub-image M1 are registered using nonlinear least-squares optimization. Based on the registration result, the pose information in the preliminary sub-image M2 is updated and optimized to obtain the optimized sub-image M2'. Sub-image M2' is then registered as the new frame's sub-image with the global point cloud map.

[0141] The unmanned forklift is continuously moved within the work area, constantly registering new subgraphs with the global point cloud map. When it detects a return to a previously visited location, specifically if the Euclidean distance between the current pose and a historical pose is less than a set threshold, it indicates a return to a previously visited location. Loop closure detection is then performed. Based on the overlap constraints of two subgraphs when the forklift visits the same location, all subgraphs are updated again using nonlinear least squares optimization to minimize the matching error between two subgraphs at the same location. The optimized and updated subgraphs are then re-registered back into the global point cloud map.

[0142] Repeat the above steps until a global point cloud map of the entire indoor and outdoor working area is generated in the Northeast Tianditu coordinate system with the anchor point as the origin.

[0143] Mark indoor map areas in the global point cloud map and save relevant data information of the global point cloud map.

[0144] (2) Adaptive relocation and multi-source information fusion positioning and navigation

[0145] As shown in Figure 3, the vehicle positioning and navigation method based on multi-source information fusion also includes adaptive relocation and multi-source information fusion positioning and navigation, and the specific process is as follows:

[0146] During the unmanned forklift positioning and navigation operation, the positioning and navigation function can be activated by turning on the machine at any location.

[0147] The positioning and navigation algorithm autonomously loads the global point cloud map and synchronously receives RTK positioning data, RTK status data corresponding to the RTK positioning data, IMU positioning data, and real-time laser point cloud data;

[0148] The positioning and navigation algorithm initiates the adaptive relocation function algorithm;

[0149] Based on RTK state data, the system autonomously determines the current environment of the unmanned forklift. If the RTK state data is in a stable state, it is determined that the unmanned forklift is outdoors. The current RTK positioning data (lon3,lat3,alt3,hdg3) is directly calculated into the Northeast-Sky coordinate system with the origin as the anchor point to obtain the initial pose (x3,y3,z3,yaw3). This coordinate system is perfectly matched with the global point cloud map, thus achieving autonomous relocalization outdoors.

[0150] If the RTK state data is an unstable solution, it is determined that the unmanned forklift is indoors. Using real-time indoor laser point cloud data collected by the laser sensor, and relying on a branch-and-bound algorithm, it is matched with the indoor area marked on the global point cloud map to achieve autonomous indoor relocalization. Specifically:

[0151] The marked indoor map area is projected in two dimensions along the Z-axis at the lowest resolution to form the finest raster map layer A.

[0152] The finest raster layer A is scaled up by a factor of 2 each time, generating a multi-layered pyramid map with resolutions ranging from fine to coarse. For example, in a three-layer pyramid map, the bottommost finest raster layer A has a resolution of 0.5m, the middle raster layer B has a resolution of 1m, and the topmost coarsest raster layer C has a resolution of 2m. Correspondingly, the coarsest raster layer C has the fewest cells.

[0153] Starting from the topmost raster map layer C, the laser point cloud of the current frame is projected according to the resolution of raster map layer C to obtain the real-time point cloud projection raster Mc.

[0154] Based on the resolution of the current map layer and the sensing radius of the laser point cloud, calculate the angular resolution theta1 of the rotation matching between the real-time point cloud projection grid Mc and the current grid map layer C, theta1 = acos(1 - (Δ²) / (2R²)) (where: Δ is the resolution of the current map layer, and R is the sensing radius of the laser point cloud (i.e. the maximum sensing radius range set by the lidar).

[0155] Using the center of the real-time point cloud projection grid Mc of the current point cloud, all cells of the raster map layer C are traversed and matched. When traversing each cell, the real-time point cloud projection grid Mc is rotated successively according to the angular resolution theta1, and then matched and calculated with the raster map layer C to obtain the matching score, until all cells of the current layer have been traversed.

[0156] Based on the matching score, the cell with the highest score in raster map layer C is determined. The area in raster map layer B that overlaps with this cell is selected, and other areas are discarded. The new real-time point cloud projection raster Mb and angular resolution theta2 are calculated using the same method. Within the selected area of ​​raster map layer B, all cells are traversed and matched.

[0157] Repeat the above operation until the best matching cell and orientation are found in the finest raster map layer A. The combination of the cell's coordinates and orientation in the global point cloud map is the correct initial pose (x4, y4, z4, yaw4), ultimately achieving indoor autonomous relocalization.

[0158] After the autonomous repositioning of the unmanned forklift is completed, the initial pose returned from the repositioning and the work point pose issued by the scheduling command are used as the start and end points, respectively, to initiate the driving positioning and navigation phase, specifically as follows:

[0159] Based on the initial pose (x3, y3, z3, yaw3) or initial pose (x4, y4, z4, yaw4) obtained from the relocalization, the predicted pose T corresponding to the current frame of laser point cloud data is obtained by integrating the IMU localization data. pred ;

[0160] If the richness of real-time point cloud features is determined, and point cloud data other than ground data exists within the radius threshold, it is determined that the point cloud matching pose update condition is met, and the NDT point cloud matching algorithm is executed:

[0161] First, the global point cloud map is divided into voxels, and the normal distribution model of each voxel is calculated.

[0162] Based on the predicted pose T pred The real-time laser point cloud data is converted into a global point cloud map. Each point cloud falls into a different voxel and is fed into its own normal distribution model to calculate the probability score Pi of each point cloud.

[0163] Summing the probability scores Pi of all real-time point clouds yields Pa. The predicted pose T is then continuously fine-tuned using the algorithm. pred， Get the updated pose T update， When Pa is at its maximum, the updated pose T is... update It refers to the optimal posture, which is used as the vehicle's real-time pose for external output.

[0164] If the real-time point cloud contains only ground point cloud data within the radius threshold, it is determined that the point cloud matching pose update condition is not met, and RTK fusion localization is performed:

[0165] Receive RTK positioning data and convert it into a pose (x5, y5, z5, yaw5) in a northeast-sky coordinate system with the anchor point as the origin. Then, combine the pose (x5, y5, z5, yaw5) with the predicted pose T. pred Perform extended Kalman filtering to obtain the optimal attitude, and use the optimal attitude as the vehicle's real-time pose for external output.

[0166] Repeat the above process until the unmanned forklift reaches the designated work point.

[0167] Example 2

[0168] This invention provides a method for controlling an unmanned forklift, comprising:

[0169] Based on the vehicle positioning and navigation method described in any one of Embodiment 1, control the unmanned forklift to run to the designated work point;

[0170] The pose deviation between the unmanned forklift and the holes on the pallet is calculated based on the pallet images and depth point cloud data collected by the visual sensor when the unmanned forklift is located at the designated work point.

[0171] The controller generates hierarchical control commands based on the posture deviation, and uses the hierarchical control commands to control the electronically controlled driver to drive the corresponding actuator to perform the corresponding action and complete the forklift operation.

[0172] In the above scheme, the scanning perception of the vision sensor and the hierarchical control and fine-tuning of the controller are used to perform secondary positioning and alignment, which realizes a leap in positioning accuracy and ensures the accuracy and safety of the unmanned forklift in the picking operation.

[0173] Specifically, the calculation of the pose deviation between the unmanned forklift and the holes on the pallet based on the pallet image and depth point cloud data collected by the visual sensor when the unmanned forklift is located at the designated work point includes:

[0174] Upon arrival at the work site, the vision sensor is activated; in practice, a depth camera may be selected as the vision sensor.

[0175] The visual sensor acquires images of the stack and depth point cloud data, and performs downsampling and intensity filtering on the depth point cloud data to obtain processed depth camera data (i.e., stack images and processed depth point cloud data).

[0176] The processed depth camera data is segmented into a stack region, and the stack plane is extracted using the RANSAC algorithm to obtain the stack plane.

[0177] Hole features are extracted from the planar surface of the stack to identify holes on the stack. Specifically, this includes hole identification using OpenCV.

[0178] Furthermore, the identified holes are morphologically optimized to smooth their outlines.

[0179] Based on the optimized holes, the center pixel coordinates of the holes in the camera coordinate system are identified, and the pose deviation between the unmanned forklift and the holes on the pallet in the camera coordinate system is calculated using the PnP algorithm.

[0180] Based on the external parameter transformation from the camera to the center of the unmanned forklift, the pose deviation between the unmanned forklift and the hole on the pallet in the vehicle coordinate system is obtained.

[0181] Based on the pose deviation between the unmanned forklift and the hole on the pallet, inverse kinematics is performed. A hierarchical motion strategy is generated based on the inverse kinematics results, and the unmanned forklift executes this strategy. The hierarchical motion strategy includes: first, coarse alignment, moving at 50% of maximum speed, with the path trajectory directly interpolated as a straight line; second, when the real-time pose deviation between the unmanned forklift and the hole on the pallet is less than the design threshold D1, fine alignment begins, moving at 20% of maximum speed, and incremental PID control is executed; finally, motion control stops when the real-time pose deviation between the unmanned forklift and the hole on the pallet is less than the design distance threshold D2.

[0182] The visual sensor is used again to collect images of the pallet and depth point cloud data. The real-time pose deviation between the unmanned forklift and the hole on the pallet is calculated. When the real-time pose deviation between the unmanned forklift and the hole on the pallet is less than the required distance threshold D3 for picking up the goods, the unmanned forklift is controlled to move forward to pick up the goods.

[0183] Example 3

[0184] This invention provides a vehicle positioning and navigation system based on multi-source information fusion, including: a controller, and a laser sensor, an inertial measurement unit, and an RTK positioning module connected to the controller, wherein the laser sensor, the inertial measurement unit, and the RTK positioning module are all used to be installed on a vehicle;

[0185] The laser sensor is used to obtain laser point cloud data;

[0186] The inertial measurement unit is used to obtain IMU positioning data;

[0187] The RTK positioning module is used to obtain RTK positioning data;

[0188] The controller is configured to perform the method described in any one of Embodiment 1.

[0189] In the specific implementation process, the laser sensor and RTK positioning module are installed on the top of the vehicle body, and the inertial measurement unit is installed on the horizontal surface of the vehicle body; as shown in Figure 1, the laser sensor includes a 16-line or higher lidar and a data processing unit. The lidar scans the environment by emitting multi-threaded laser beams and converts environmental features into three-dimensional point cloud data.

[0190] The inertial measurement unit includes a three-axis accelerometer, a three-axis gyroscope, and a magnetometer. It can be used to detect the acceleration, angular velocity, and orientation of the forklift in space in real time. By integrating the above information, it can provide high-frequency pose prediction data.

[0191] The RTK positioning module includes a pair of BeiDou high-precision antennas, a BeiDou RTK receiver, and a data processing unit. Mounted on the top of the forklift, the RTK positioning module receives multi-frequency satellite signals through the BeiDou high-precision antennas. The BeiDou RTK receiver then uses the carrier phase differential positioning principle to execute the RTK algorithm to obtain the position of the main antenna in space. A positioning baseline is formed by the installation distance between the two antennas, and the relative position vector between the two antennas is accurately calculated to determine the orientation of the forklift. Ultimately, centimeter-level absolute positioning of the unmanned forklift is achieved, and this positioning can be used to establish a geographic coordinate system benchmark.

[0192] The control processor is internally equipped with a data acquisition module (for acquiring data output from the laser sensor, vision sensor module, inertial measurement unit and RTK positioning module), a laser mapping algorithm (for constructing a global point cloud map), and a positioning and navigation module (for executing the positioning and navigation algorithm to obtain the real-time pose of the vehicle).

[0193] Example 4

[0194] This invention provides an unmanned forklift control system, including a controller, a laser sensor, an inertial measurement unit, an RTK positioning module, a vision sensor, an electronically controlled driver connected to the controller, and an actuator connected to the electronically controlled driver; the laser sensor, inertial measurement unit, RTK positioning module, vision sensor, electronically controlled driver, and actuator are all used to be installed on the forklift.

[0195] The laser sensor is used to obtain laser point cloud data;

[0196] The inertial measurement unit is used to obtain IMU positioning data;

[0197] The RTK positioning module is used to obtain RTK positioning data;

[0198] The visual sensor is used to acquire images of the stack and depth point cloud data;

[0199] The controller is configured to perform the method described in embodiment 2.

[0200] In the specific implementation process, the laser sensor and RTK positioning module are installed on the top of the vehicle body, the vision sensor is installed on the working surface of the fork, and the inertial measurement unit is installed on the horizontal surface of the vehicle body; as shown in Figure 1, the laser sensor includes a 16-line or higher lidar and a data processing unit. The lidar scans the environment by emitting multi-threaded laser beams and converts the environmental features into three-dimensional point cloud data.

[0201] The visual sensing module includes an RGB-D depth camera and an image preprocessing unit. The RGB-D depth camera acquires images of the stack and pixel depth information to obtain the spatial pose of the object relative to itself, achieving sub-millimeter-level feature recognition and relative pose detection.

[0202] The inertial measurement unit includes a three-axis accelerometer, a three-axis gyroscope, and a magnetometer. It can be used to detect the acceleration, angular velocity, and orientation of the forklift in space in real time. By integrating the above information, it can provide high-frequency pose prediction data.

[0203] The RTK positioning module includes a pair of BeiDou high-precision antennas, a BeiDou RTK receiver, and a data processing unit. Mounted on the top of the forklift, the RTK positioning module receives multi-frequency satellite signals through the BeiDou high-precision antennas. The BeiDou RTK receiver then uses the carrier phase differential positioning principle to execute the RTK algorithm to obtain the position of the main antenna in space. A positioning baseline is formed by the installation distance between the two antennas, and the relative position vector between the two antennas is accurately calculated to determine the orientation of the forklift. Ultimately, centimeter-level absolute positioning of the unmanned forklift is achieved, and this positioning can be used to establish a geographic coordinate system benchmark.

[0204] The control processor internally includes a data acquisition module (for acquiring data output from the laser sensor, vision sensor module, inertial measurement unit, and RTK positioning module), a laser mapping algorithm (for constructing a global point cloud map), a positioning and navigation module (for executing the positioning and navigation algorithm to obtain the real-time pose of the vehicle), an image recognition module (for connecting to the depth camera and executing the image recognition algorithm to obtain the holes on the pallet), and a motion control and signal module (for connecting to the electronic control driver). According to the requirements of different task stages, it processes data and runs corresponding functional algorithms to ultimately obtain the precise pose of the unmanned forklift and issue control commands.

[0205] An electronically controlled actuator is used to execute millimeter-level fine-tuning commands to precisely control the actuator.

[0206] The actuator specifically includes an omnidirectional active steering wheel and two forward drive support wheels, enabling high degrees of freedom of movement and flexible steering in place.

[0207] Example 5

[0208] Based on the same inventive concept as Embodiment 1, this embodiment of the invention provides a vehicle positioning and navigation system based on multi-source information fusion, including a storage medium and a processor;

[0209] The storage medium is used to store instructions;

[0210] The processor is configured to operate according to the instructions to execute the method according to any one of Embodiment 1.

[0211] Example 6

[0212] Based on the same inventive concept as in Embodiment 2, this embodiment of the invention provides an unmanned forklift control system, including a storage medium and a processor;

[0213] The storage medium is used to store instructions;

[0214] The processor is configured to operate according to the instructions to execute the method described in Embodiment 2.

[0215] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0216] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in one or more blocks of the flowchart illustrations and / or one or more blocks of the block diagrams.

[0217] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means that implement the functions specified in one or more flowcharts and / or one or more block diagrams.

[0218] These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process, such that the instructions, which execute on the computer or other programmable apparatus, provide steps for implementing the functions specified in one or more flowcharts and / or one or more block diagrams.

[0219] The embodiments of the present invention have been described above with reference to the accompanying drawings. However, the present invention is not limited to the specific embodiments described above. The specific embodiments described above are merely illustrative and not restrictive. Those skilled in the art can make many other forms under the guidance of the present invention without departing from the spirit and scope of the claims. All of these forms are within the protection scope of the present invention.

[0220] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely illustrative of the principles of the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed. The scope of protection of this invention is defined by the appended claims and their equivalents.

Claims

1. A vehicle positioning and navigation method based on multi-source information fusion, characterized in that, include: A fixed point within the work area is selected as the anchor point, and the anchor point is used as the origin of the global point cloud map. The IMU positioning data and laser point cloud data during the vehicle's movement are fused to generate a global point cloud map. Repeat the preset positioning steps until the vehicle reaches the designated work point. The preset positioning steps include: determining whether the vehicle is outdoors or indoors based on RTK status data corresponding to RTK positioning data, and generating an initial pose based on the determination result, global point cloud map, RTK positioning data and laser point cloud data. Based on the initial pose or the real-time pose of the vehicle obtained in the previous cycle, and the preset adaptive relocation strategy, the vehicle's real-time pose is obtained by fusing IMU positioning data, RTK positioning data and laser point cloud data during the vehicle's movement; and the vehicle's real-time pose is used to determine whether the vehicle has reached the designated work point.

2. The vehicle positioning and navigation method based on multi-source information fusion according to claim 1, characterized in that: The method for generating the global point cloud map includes: determining a fixed point as an anchor point in the work area and recording the longitude, latitude, and altitude information (lon0, lat0, alt0) of the fixed point; placing the vehicle at any position in the work area, using the anchor point as the origin of the global point cloud map coordinate system, and calculating the current pose (x1, y1, z1, yaw1) of the vehicle in the global point cloud map coordinate system from the RTK positioning data (lon1, lat1, alt1, hdg1) corresponding to the current position of the vehicle, where lon1, lat1, alt1, and hdg1 represent the longitude, latitude, altitude, and orientation of the vehicle, respectively, and x1, y1, z1, and yaw1 represent the X-axis coordinate, Y-axis coordinate, Z-axis coordinate, and heading, respectively; using the current pose (x1, y1, z1, yaw1) as the initial pose for mapping, controlling the vehicle to traverse the work area, and fusing the IMU positioning data and laser point cloud data during the vehicle's movement to generate a global point cloud map.

3. The vehicle positioning and navigation method based on multi-source information fusion according to claim 2, characterized in that: The process of using the current pose (x1, y1, z1, yaw1) as the initial pose for mapping, controlling the vehicle to traverse the work area, and fusing IMU positioning data and laser point cloud data during vehicle movement to generate a global point cloud map includes: receiving a 3D laser point cloud under the current pose (x1, y1, z1, yaw1), segmenting, filtering, and distortion-reducing the 3D laser point cloud, combining the processed 3D laser point cloud with the current pose (x1, y1, z1, yaw1) to form an initial frame sub-image M1, and registering the initial frame sub-image M1 with the global point cloud map; controlling the vehicle to start moving, integrating the real-time acquired IMU positioning data to obtain the predicted pose (x2, y2, z2, yaw2), where x2, y2, z2, and yaw2 represent the X-axis coordinate, Y-axis coordinate, Z-axis coordinate, and heading, respectively. When the cumulative movement distance... Greater than the set distance threshold, or cumulative orientation When the change exceeds a set orientation threshold, the system receives a 3D laser point cloud with the predicted pose (x2, y2, z2, yaw2), segments, filters, and removes distortion from this 3D laser point cloud. The processed 3D laser point cloud is then combined with the predicted pose (x2, y2, z2, yaw2) to form a new frame's preliminary sub-image M2. Nonlinear least-squares optimization registration is performed between the new frame's preliminary sub-image M2 and the 3D laser point cloud in the previous frame's sub-image M1. Based on the registration result, the pose information in the preliminary sub-image M2 is updated and optimized to obtain the optimized sub-image M2'. Sub-image M2' is then registered with the global point cloud map. The system controls the vehicle to... The work area is constantly moving, continuously registering new sub-maps to the global point cloud map. When the pose information in the sub-map indicates that the current position is a previously visited position, a loop closure detection is performed. The loop closure detection includes: based on the overlap constraint of two sub-maps when the vehicle visits the same position successively, performing nonlinear least squares optimization update on all sub-maps in the global point cloud map to minimize the matching error between two sub-maps at the same position, and then registering the optimized and updated sub-maps to the global point cloud map. The above steps are repeated until a global point cloud map of the entire work area is generated in the global point cloud map coordinate system with the anchor point as the origin, and the indoor area is marked.

4. The vehicle positioning and navigation method based on multi-source information fusion according to claim 1, characterized in that: The process of determining whether the vehicle is outdoors or indoors based on RTK state data corresponding to RTK positioning data, and generating an initial pose based on the determination result, global point cloud map, RTK positioning data, and laser point cloud data, includes: if the RTK state data is a stable solution state, then the vehicle is determined to be outdoors, and the current RTK positioning data (lon3, lat3, alt3, hdg3) is directly solved into the global point cloud map coordinate system with the anchor point as the origin to obtain the corresponding initial pose (x3, y3, z3, yaw3), completing the outdoor autonomous relocalization, wherein lon3, lat3, alt ... lt3 and hdg3 represent the vehicle's longitude, latitude, altitude, and orientation, respectively, while x3, y3, z3, and yaw3 represent the X-axis coordinate, Y-axis coordinate, Z-axis coordinate, and heading, respectively. If the RTK state data is an unstable solution, the vehicle is determined to be indoors. Based on the current laser point cloud data, relying on the branch and bound algorithm, it is matched with the indoor area marked on the global point cloud map to obtain the corresponding initial pose (x4, y4, z4, yaw4), thus completing the indoor autonomous relocalization. Here, x4, y4, z4, and yaw4 represent the X-axis coordinate, Y-axis coordinate, Z-axis coordinate, and heading, respectively.

5. The vehicle positioning and navigation method based on multi-source information fusion according to claim 4, characterized in that: The process of matching the current laser point cloud data with the marked indoor area on the global point cloud map using a branch and bound algorithm to obtain the corresponding pose (x4, y4, z4, yaw4) includes: projecting the marked indoor area on the global point cloud map along the Z-axis in a two-dimensional manner according to the principle of gradually increasing resolution, forming raster map layer 1, raster map layer 2, ..., raster map layer N in sequence; starting from raster map layer N, projecting the current laser point cloud data according to the resolution of raster map layer N to obtain the real-time point cloud projection raster M. N Based on the resolution of the raster map layer N and the sensing radius of the laser point cloud, calculate the real-time point cloud projection raster M. N The angular resolution theta1 is matched with the rotation of the raster map layer N, theta1 = acos(1 - (Δ²) / (2R²)), where Δ is the resolution of the raster map layer N and R is the sensing radius of the laser point cloud; the real-time point cloud is projected onto the raster M. N At the center, all cells of the raster map layer N are traversed and matched. During the traversal of each cell, the real-time point cloud is projected onto the raster M according to the angular resolution theta1. N The process involves successive rotations, followed by matching calculations with raster map layer N to obtain matching scores. This process continues until all cells in raster map layer N have been traversed. Based on the matching scores, the cell with the highest score in raster map layer N is identified. The region in raster map layer N-1 that overlaps with the cell with the highest score is selected, while other regions are discarded. The real-time point cloud projection raster M is then obtained using the same method. N-1 With the angular resolution theta2, in the selected area of ​​raster map layer N-1, all cells are traversed and matched; repeat the above operation until the most matching cell is found in raster map layer 1. The combination of the coordinates and orientation of the most matching cell in the global point cloud map is the corresponding initial pose (x4, y4, z4, yaw4).

6. The vehicle positioning and navigation method based on multi-source information fusion according to claim 1, characterized in that: The process of obtaining the real-time vehicle pose based on the initial pose or the previous cycle, and a preset adaptive relocalization strategy, by fusing IMU positioning data, RTK positioning data, and laser point cloud data during vehicle motion, includes: integrating the IMU positioning data to obtain the predicted pose Tpred corresponding to the current laser point cloud data based on the initial pose or the real-time vehicle pose obtained from the previous cycle; if, within a preset radius threshold (less than or equal to the laser point cloud sensing radius), the current laser point cloud data contains other point cloud data besides the ground point cloud data, then it is determined that the point cloud matching pose update condition is met, and the NDT point cloud matching strategy is executed. The NDT point cloud matching strategy includes: dividing the global point cloud map into voxels to obtain several voxels, and calculating the normal distribution model of each voxel; based on the predicted pose Tpred, converting the current laser point cloud data into the global point cloud map, with each point cloud falling into a different voxel and its corresponding normal distribution model. Calculate the probability score Pi for each point cloud; sum the probability scores Pi of all point clouds to obtain Pa; continuously adjust the predicted pose Tpred to obtain the updated pose Tupdate; when Pa is at its maximum, the updated pose Tupdate is the optimal pose, and the optimal pose is used as the real-time pose of the vehicle; if within a preset radius threshold, the current laser point cloud data only contains ground point cloud data, it is determined that the point cloud matching pose update condition is not met, and an RTK fusion positioning strategy is executed. The RTK fusion positioning strategy includes: solving the received RTK positioning data into a global point cloud map coordinate system with the anchor point as the origin to obtain the corresponding current pose (x5, y5, z5, yaw5), where x5, y5, z5, and yaw5 represent the X-axis coordinate, Y-axis coordinate, Z-axis coordinate, and heading, respectively; performing extended Kalman filtering on the current pose (x5, y5, z5, yaw5) and the predicted pose Tpred to obtain the optimal pose, and using the optimal pose as the real-time pose of the vehicle.

7. A method for controlling an unmanned forklift, characterized in that, include: Based on the vehicle positioning and navigation method according to any one of claims 1-6, control the unmanned forklift to run to the designated work point; Based on the pallet image and depth point cloud data collected by the vision sensor when the unmanned forklift is located at the designated work point, the pose deviation between the unmanned forklift and the holes on the pallet is calculated; according to the pose deviation, hierarchical control commands are generated, and the hierarchical control commands are used to control the electronic control driver to drive the corresponding actuator to perform the corresponding action to complete the forklift operation.

8. The unmanned forklift control method according to claim 7, characterized in that, The method for calculating the pose deviation between the unmanned forklift and the holes on the pallet based on the pallet image and depth point cloud data collected by the visual sensor when the unmanned forklift is located at a designated work point includes: acquiring the pallet image and depth point cloud data collected by the visual sensor, and performing downsampling and intensity filtering on the depth point cloud data to obtain processed depth camera data, wherein the depth camera data includes the pallet image and the processed depth point cloud data; segmenting the pallet region on the processed depth camera data, extracting the pallet plane using the RANSAC algorithm to obtain the pallet plane; extracting hole features from the pallet plane to identify the holes; performing morphological optimization on the identified holes to smooth the hole contours to obtain optimized holes; identifying the center pixel coordinates of the holes in the camera coordinate system based on the optimized holes, and calculating the pose deviation between the unmanned forklift and the holes on the pallet in the camera coordinate system using the PnP algorithm; and obtaining the pose deviation between the unmanned forklift and the holes on the pallet in the vehicle coordinate system based on the extrinsic parameter transformation from the camera to the vehicle center of the unmanned forklift.

9. The unmanned forklift control method according to claim 8, characterized in that, The step of generating hierarchical control commands based on the pose deviation and using the hierarchical control commands to control the electronic control driver to drive the corresponding actuator to perform corresponding actions includes: performing inverse kinematics based on the pose deviation between the unmanned forklift and the hole on the pallet; generating a hierarchical motion strategy based on the inverse kinematics result; and executing the hierarchical motion strategy as a whole. The hierarchical motion strategy includes: first performing coarse alignment and moving at a first percentage of the maximum speed; when the real-time pose deviation between the unmanned forklift and the hole on the pallet is less than the design threshold D1, starting fine alignment and moving at a second percentage of the maximum speed, executing incremental PID control until the real-time pose deviation between the unmanned forklift and the hole on the pallet is less than the design distance threshold D2, controlling the unmanned forklift to stop moving; the second percentage is less than the first percentage; acquiring the pallet image and depth point cloud data collected by the visual sensor again, calculating the real-time pose deviation between the unmanned forklift and the hole on the pallet; and when the real-time pose deviation between the unmanned forklift and the hole on the pallet is less than the required distance threshold D3 for picking up the goods, controlling the unmanned forklift to move forward and pick up the goods.

10. A vehicle positioning and navigation system based on multi-source information fusion, characterized in that, include: A controller, and a laser sensor, an inertial measurement unit, and an RTK positioning module connected to the controller, wherein the laser sensor, inertial measurement unit, and RTK positioning module are all for mounting on a vehicle; the laser sensor is used to acquire laser point cloud data; the inertial measurement unit is used to acquire IMU positioning data; the RTK positioning module is used to acquire RTK positioning data; the controller is configured to perform the method of any one of claims 1-6.

11. An unmanned forklift control system, characterized in that: The device includes a controller, a laser sensor, an inertial measurement unit (IMU), an RTK positioning module, a vision sensor, an electronically controlled driver (ECU), and an actuator connected to the controller; the laser sensor, IMU, RTK positioning module, vision sensor, ECU, and actuator are all mounted on a forklift; the laser sensor is used to acquire laser point cloud data; the IMU is used to acquire IMU positioning data; the RTK positioning module is used to acquire RTK positioning data; the vision sensor is used to acquire pallet images and depth point cloud data; the controller is configured to perform the method according to any one of claims 7-9.

12. A vehicle positioning and navigation system based on multi-source information fusion, characterized in that, It includes a storage medium and a processor; the storage medium is used to store instructions; the processor is used to operate according to the instructions to perform the method according to any one of claims 1-6.

13. An unmanned forklift control system, characterized in that: It includes a storage medium and a processor; the storage medium is used to store instructions; the processor is used to operate according to the instructions to perform the method according to any one of claims 7-9.

Images