An indoor mobile carrier navigation positioning modeling and path planning method

Abstract

The application belongs to the technical field of mobile carrier navigation, and discloses an indoor mobile carrier navigation positioning modeling and path planning method, and the specific steps are as follows: step one, multi-source perception data synchronous acquisition preprocessing; step two, indoor layered composite environment modeling construction; step three, multi-factor fusion real-time high-precision positioning solution. The application realizes multi-source sensor hardware clock synchronization + space-time registration fusion modeling, builds a three-layer coupled composite map of grid-topology-semantic, adds a feature double closed loop verification mechanism, effectively suppresses positioning drift, adapts to weak texture, symmetry, long channel special scene; adopts a semantic weighted improved A* algorithm linkage dynamic window method, global path pruning denoising, local path spline smoothing, path no right angle sharp turn, adapts to carrier motion dynamics characteristics; dynamically sets the obstacle safety inflation buffer zone, avoids human-machine, carrier object collision, linkage global semantic no-entry area autonomous bypass.

Description

Technical Field

[0001] This invention belongs to the field of mobile carrier navigation technology, specifically a method for indoor mobile carrier navigation and positioning modeling and path planning. Background Technology

[0002] Indoor mobile vehicle navigation is a technology system designed for indoor mobile vehicles such as robots, AGVs, inspection vehicles, and warehouse forklifts. It enables autonomous positioning and path planning in enclosed spaces without GPS signals. Relying on indoor positioning sensors such as LiDAR, UWB, vision, and geomagnetism to obtain the real-time coordinates of the vehicle, and combining them with indoor maps to complete environmental modeling, the system avoids obstacles in real time, dynamically plans the optimal driving route, and controls the vehicle to autonomously reach the target point. It is suitable for scenarios such as factories, shopping malls, underground parking garages, and computer rooms.

[0003] Traditional indoor navigation and positioning modeling often relies on single laser / visual SLAM. However, long corridors, pure white walls, and symmetrical apartment layouts are prone to cumulative pose drift, resulting in scale distortion in mapping, low accuracy in closed-loop matching, and extremely poor correction precision. Furthermore, the different sampling frequencies and independent clocks of multiple sensors lead to inaccurate spatiotemporal alignment, causing continuous accumulation of fusion errors. This significantly reduces positioning stability in scenarios with dynamic pedestrian flow, temporary objects, and movable furniture. Path planning using native A* and basic artificial potential field methods results in high redundancy in node searching and traversal, long computation times on embedded terminals, and a high risk of getting stuck in local minima. Generated paths often have numerous right-angle inflection points and poor smoothness, lacking safety boundaries based on carrier dimensions, leading to a high probability of collisions in narrow passages. Modeling only constructs a two-dimensional geometric grid without semantic hierarchical annotation, resulting in fragmented global and local planning. Sudden environmental changes prevent millisecond-level replanning, leading to insufficient adaptability and operational robustness. Summary of the Invention

[0004] The purpose of this invention is to provide an indoor mobile vehicle navigation and positioning modeling and path planning method to solve the problems mentioned in the background art.

[0005] To achieve the above objectives, the present invention provides the following technical solution: a method for indoor mobile vehicle navigation and positioning modeling and path planning, the specific steps of which are as follows: Step 1: Synchronous Acquisition and Preprocessing of Multi-Source Sensing Data The timing and extrinsic parameter matrices of four types of vehicle-mounted sensors—LiDAR, vision camera, IMU inertial unit, and UWB base station—are uniformly calibrated. Multi-source sensing data are collected synchronously, and filtering and noise reduction, coordinate system transformation, and abnormal and invalid sensing data are eliminated. Step Two: Modeling and Constructing the Indoor Layered Composite Environment Based on preprocessed data, a three-layer composite map with the same coordinate system is constructed by coupling a two-dimensional raster map, a topological path network map, and a semantic function label map, and a map closed-loop correction and incremental update mechanism is configured. Step 3: Multi-factor fusion real-time high-precision positioning solution An adaptive weighted particle filter fusion model was built, which integrates four types of observations: map features, wheel mileage, UWB ranging, and visual features, and dynamically adapts to the real-time pose of the scene weight calculation carrier. Step 4: Initialize navigation start point, destination, and traffic constraints Enter the poses of the navigation start and end points, load the vehicle dynamics parameters, hierarchical safety distance parameters, and semantic global access constraints, and lock the navigation constraints after verifying the passability connectivity of the start and end points. Step 5: Adaptive Weighted Global Optimal Path Coarse Planning A multi-dimensional cost-improved A* algorithm is adopted, which combines distance, channel width, semantic congestion, and energy consumption cost factors to search for nodes and prunes redundant nodes to generate a global collision-free coarse path with inflection points. Step Six: Path Smoothing Optimization and Safety Expansion Layer Correction By eliminating redundant inflection points with small deviations in the coarse path, a continuous curvature trajectory is fitted using third-order B-splines, and a hierarchical obstacle safety expansion buffer is constructed to generate a compliant benchmark smooth navigation trajectory. Step 7: Dynamic Obstacle Perception and Local Real-Time Replanning The laser-vision dual recognition module predicts the trajectory of dynamic obstacles. When an obstacle intrudes into the baseline trajectory, the improved dynamic window method is used to carry out local detour replanning while keeping the global path unchanged. Step 8: Trajectory Tracking Closed-Loop Control and Map Iterative Update The vehicle's driving posture is controlled by a dual closed-loop PID controller to compensate for driving trajectory deviations; environmental data is incrementally collected throughout the navigation operation, and the three-layer composite map is iteratively updated to complete the closed loop of the entire navigation process.

[0006] As a preferred technical solution of the present invention, the multi-source sensing data synchronous acquisition and preprocessing in step one specifically includes: the main control board sends out a synchronous trigger pulse to unify the 10Hz sampling frequency of the four types of sensors, and completes the sensor hand-eye calibration and external parameter matrix calibration; synchronously acquires three-dimensional laser point cloud, RGB depth image, three-axis angular velocity / acceleration attitude data, and raw ranging data from four-point UWB base stations; uses adaptive statistical filtering to remove laser reflection pseudopoints and long-distance outliers, Gaussian filtering to remove image noise, builds an IMU temperature drift compensation model to correct static zero bias, uniformly converts all sensing data to the vehicle body coordinate system at the center of the chassis, and removes invalid data that exceeds the ranging threshold.

[0007] As a preferred technical solution of the present invention, the indoor layered composite environment modeling and construction described in step two specifically includes: the grid map uses the GMAPping-SLAM algorithm to divide the grid into 5cm precision grid units, and binarizes the passable grids and obstacle grids; the topology map uses indoor turning points, doors, and elevator entrances as topology nodes, fits passable straight paths between nodes as connecting edges, and binds the connecting edges to the channel width and slope attributes to simplify the global path logic; the semantic map relies on the indoor lightweight YOLO model to identify and label semantic tags for work areas, pedestrian passages, and restricted areas. The three layers of maps share the same world coordinate system and are equipped with a loop closure matching module. The carrier passes through repeated points, automatically corrects map distortion, and supports incremental overwriting and updating of newly added obstacles, forming an integrated iterative composite environment model.

[0008] As a preferred technical solution of the present invention, the multi-factor fusion real-time high-precision positioning solution in step three specifically includes: retrieving the pre-built composite map in step two, extracting laser plane features and visual corner features in real time to complete the initial positioning of map matching; relying on the vehicle-mounted wheel odometer to output the incremental mileage of the vehicle, and combining IMU attitude data to calculate the short-term trajectory to make up for the positioning gap during the feature missing period; introducing the UWB four-point ranging constraint equation to construct the global coordinate constraint boundary and eliminate the cumulative error of trajectory calculation; building a dynamic weighted particle filter fusion framework and setting scene adaptive weighting rules: increasing the weight of laser positioning by 65% ​​in open long corridors, increasing the weight of UWB wireless positioning in weak texture pure white areas, and increasing the weight of visual feature positioning in areas with crowd obstruction; setting a positioning convergence threshold, and outputting the result when the particle residual of a single iteration is less than 0.03m, and outputting the real-time six-bit real-time pose data of the vehicle's centimeter-level plane coordinates, heading angle, and driving speed, with a stable positioning frequency of 10Hz.

[0009] As a preferred technical solution of the present invention, the initialization of navigation start point, end point and traffic constraints in step four specifically includes: receiving scheduling instructions from the host computer or manual touch instructions, selecting passable grid points in the composite map, locking the pose of the navigation start point and the pose of the target end point, and synchronously matching the optimal orientation angle of the start and end points; inputting the inherent dynamic parameters of the vehicle: chassis circumscribed radius, maximum straight speed, maximum steering angular velocity, acceleration and deceleration response time, and minimum turning radius; configuring three types of safety avoidance distances in a graded manner: 0.3m basic safety distance for regular channels, 0.5m enhanced safety distance for narrow channels, and 0.8m avoidance safety distance for pedestrian intersections; batch importing semantic map fixed constraints: permanent restricted areas, time-limited closed channels, low-speed passage areas, and single-sided passage permission rules; simultaneously determining the connectivity of the start and end points, and when the point is in an obstacle grid or a restricted semantic area, a pop-up error is displayed and the point is reselected. After the verification is passed, all navigation constraint parameters are locked, and the path planning stage is entered.

[0010] As a preferred technical solution of the present invention, the adaptive weighted global optimal path coarse planning in step five specifically includes: improving the A* Euclidean distance heuristic function, adding a multi-dimensional weighted cost model, and adaptively iteratively adjusting the cost weights. Among them, the travel distance weight ensures the shortest path mileage, the channel width weight filters out narrow paths smaller than the carrier's travel width, and the semantic cost weight avoids high-frequency congestion areas, bumpy ground, and high-energy-consumption travel areas. The core cost formula of the improved A* algorithm is: F(n)=G(n)+H(n), where F(n) is the comprehensive travel cost of the node, G(n) is the actual travel cost from the starting point to the current node, and H(n) is the Euclidean distance heuristic cost from the current node to the destination. When the algorithm traverses the grid nodes, a node pruning rule is added to remove collinear, detour, and high-cost expansion nodes. The coarse path of global inflection points, composed of discrete inflection points, without global collision risk, with the optimal mileage, and compliant travel is output with the minimum global travel cost as the standard.

[0011] As a preferred technical solution of the present invention, the path smoothing optimization and safety expansion layer correction in step six specifically includes: traversing the global coarse path inflection point set, using the slope difference judgment method to eliminate redundant inflection points of the path with a deflection angle of less than 8°, and simplifying the number of path anchor points; using third-order B-spline curve interpolation to fit the remaining discrete inflection points, constraining the maximum curvature of the path to not exceed the minimum turning curvature of the carrier, eliminating 90° right-angle sharp turns and acute-angle turns, and ensuring the continuity of the trajectory curvature throughout the entire process; based on the radius of the carrier chassis, performing hierarchical corrosion expansion processing on the grid around the path: performing first-level expansion and distance extension around permanent obstacles, performing second-level dynamic expansion around temporary movable obstacles, and performing minimum basic expansion in open passage areas; verifying all points of the fitted trajectory, determining that the trajectory grid and the obstacle expansion grid have no intersection, and generating a benchmark smooth navigation trajectory that combines driving smoothness and all-domain passive collision avoidance capability.

[0012] As a preferred technical solution of the present invention, the dynamic obstacle perception and local real-time replanning in step seven specifically includes: relying on the forward detection of lidar and visual target recognition, a 3m×2m forward warning perception zone is delineated in front of the carrier, and three types of dynamic obstacles are classified and identified: pedestrians, transfer vehicles, and temporary stacks. The obstacle's movement speed, direction of travel, and duration of passage are calculated by measuring three consecutive frames of detection data, and the obstacle's 3-second trajectory is predicted to determine whether it has intruded into the baseline safe navigation trajectory. When an obstacle intrudes into the trajectory, a local path interval of 1.5m before and after is extracted and combined with the real-time vehicle speed and steering limits to quickly generate a detour sub-trajectory for replanning. The replanning only modifies the local section route, and the beginning and end points of the detour trajectory are connected to the original global baseline path. The replanning response delay is ≤120ms, and the detour is automatically returned to the original path after completion.

[0013] As a preferred technical solution of the present invention, the trajectory tracking closed-loop control and map iterative update described in step eight specifically include: using an outer loop pose deviation PID calculation and an inner loop power speed regulation dual closed-loop controller to control the lateral and heading deviations of the vehicle's travel within ±2cm; continuously collecting incremental point cloud and image data of the surrounding environment throughout the vehicle's travel process, comparing it with the original composite map, identifying new obstacles and removing similar obstacles, and automatically increasing the laser update weight in low-light environments at night; verifying the docking pose accuracy after navigation reaches the destination, and synchronously incrementally overwriting the three-layer composite map data to achieve multi-task continuous closed-loop navigation.

[0014] The beneficial effects of this invention are as follows: This invention achieves hardware clock synchronization and spatiotemporal registration fusion modeling for multi-source sensors, constructing a three-layer coupled composite map of grid, topology, and semantics. It adds a dual closed-loop verification mechanism based on features to effectively suppress positioning drift and adapt to special scenarios with weak textures, symmetry, and long channels. It employs a semantically weighted improved A* algorithm linked to a dynamic window method, global path pruning and noise reduction, local path spline smoothing, and paths without sharp right-angle turns, adapting to the dynamic characteristics of the carrier's motion. It dynamically sets hierarchical obstacle safety expansion buffer zones to avoid collisions between humans / machines and carrier objects, and links with a global semantic no-entry zone for autonomous detours. It establishes a global-local planning linkage channel, enabling immediate local replanning upon encountering dynamic obstacles, balancing positioning accuracy, traffic efficiency, and driving smoothness, and is suitable for all types of indoor carriers such as warehouse AGVs and building inspection robots. Attached Figure Description

[0015] Figure 1 This is a flowchart of the present invention. Detailed Implementation

[0016] 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 some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0017] like Figure 1 As shown in the figure, this embodiment of the invention provides a method for indoor mobile vehicle navigation and positioning modeling and path planning, the specific steps of which are as follows: Step 1: Synchronous Acquisition and Preprocessing of Multi-Source Sensing Data The timing and extrinsic parameter matrices of four types of vehicle-mounted sensors—LiDAR, vision camera, IMU inertial unit, and UWB base station—are uniformly calibrated. Multi-source sensing data are collected synchronously, and filtering and noise reduction, coordinate system transformation, and abnormal and invalid sensing data are eliminated. Step Two: Modeling and Constructing the Indoor Layered Composite Environment Based on preprocessed data, a three-layer composite map with the same coordinate system is constructed by coupling a two-dimensional raster map, a topological path network map, and a semantic function label map, and a map closed-loop correction and incremental update mechanism is configured. Step 3: Multi-factor fusion real-time high-precision positioning solution An adaptive weighted particle filter fusion model was built, which integrates four types of observations: map features, wheel mileage, UWB ranging, and visual features, and dynamically adapts to the real-time pose of the scene weight calculation carrier. Step 4: Initialize navigation start point, destination, and traffic constraints Enter the poses of the navigation start and end points, load the vehicle dynamics parameters, hierarchical safety distance parameters, and semantic global access constraints, and lock the navigation constraints after verifying the passability connectivity of the start and end points. Step 5: Adaptive Weighted Global Optimal Path Coarse Planning A multi-dimensional cost-improved A* algorithm is adopted, which combines distance, channel width, semantic congestion, and energy consumption cost factors to search for nodes and prunes redundant nodes to generate a global collision-free coarse path with inflection points. Step Six: Path Smoothing Optimization and Safety Expansion Layer Correction By eliminating redundant inflection points with small deviations in the coarse path, a continuous curvature trajectory is fitted using third-order B-splines, and a hierarchical obstacle safety expansion buffer is constructed to generate a compliant benchmark smooth navigation trajectory. Step 7: Dynamic Obstacle Perception and Local Real-Time Replanning The laser-vision dual recognition module predicts the trajectory of dynamic obstacles. When an obstacle intrudes into the baseline trajectory, the improved dynamic window method is used to carry out local detour replanning while keeping the global path unchanged. Step 8: Trajectory Tracking Closed-Loop Control and Map Iterative Update The vehicle's driving posture is controlled by a dual closed-loop PID controller to compensate for driving trajectory deviations; environmental data is incrementally collected throughout the navigation operation, and the three-layer composite map is iteratively updated to complete the closed loop of the entire navigation process.

[0018] This method integrates eight time-series linked navigation processes, relies on four types of sensors to collaboratively perceive indoor environmental data, builds a three-layer integrated composite environmental map to complete site modeling, combines adaptive particle filtering to complete vehicle localization, and uses an improved A* algorithm and dynamic window method to perform global path planning and local dynamic obstacle avoidance. Finally, it completes autonomous navigation through closed-loop vehicle control and map iteration. Compared with traditional indoor navigation, this solution abandons the single-sensor positioning and single-layer grid modeling mode, and connects the entire process of positioning, mapping, planning and vehicle control. It can adapt to complex indoor conditions such as long corridors, weak textured walls, dense crowds, and narrow passages, and takes into account positioning accuracy, driving smoothness and dynamic obstacle avoidance capabilities. It is suitable for the routine operation of various indoor mobile vehicles such as warehouse AGVs and building inspection robots.

[0019] The multi-source sensing data synchronous acquisition and preprocessing in step one specifically includes: the main control board sends out a synchronous trigger pulse to unify the 10Hz sampling frequency of the four types of sensors, and completes the sensor hand-eye calibration and external parameter matrix calibration; synchronously acquires three-dimensional laser point cloud, RGB depth image, three-axis angular velocity / acceleration attitude data, and raw ranging data from four-point UWB base stations; adaptive statistical filtering is used to remove laser reflection pseudopoints and long-distance outliers, Gaussian filtering is used to remove image noise, an IMU temperature drift compensation model is built to correct static zero bias, all sensing data are uniformly converted to the vehicle body coordinate system at the center of the chassis, and invalid data exceeding the ranging threshold are removed.

[0020] Smooth indoor walls and glass partitions can reflect LiDAR beams, generating invalid outliers and pseudo-point clouds. Fluctuations in indoor lighting can cause noise in camera images. Prolonged operation of the IMU can lead to temperature drift and continuous accumulation of attitude bias errors. In addition, the four types of sensors have independent sampling clocks at the factory, resulting in misaligned data acquisition timing and making direct fusion impossible. Therefore, by unifying the sensor sampling frequency, coupled with targeted filtering and noise reduction, error compensation, and a unified global coordinate reference, invalid ranging data exceeding limits can be eliminated. This ensures that the original data used for subsequent fusion modeling and positioning calculations are time-synchronized and error-controllable, improving the accuracy of subsequent mapping, positioning, and planning, and reducing data deviations caused by environmental interference from the source.

[0021] The indoor layered composite environment modeling and construction in step two specifically includes: the grid map uses the GMAPping-SLAM algorithm to divide the grid into 5cm precision grid cells, and binarizes the passable grid cells and obstacle grid cells; the topology map uses indoor turning points, doors, and elevator entrances as topology nodes, fits passable straight paths between nodes as connecting edges, and binds the connecting edges to the channel width and slope attributes to simplify the global path logic; the semantic map relies on the indoor lightweight YOLO model to identify and label semantic tags for work areas, pedestrian passages, and restricted areas. The three layers of maps share the same world coordinate system and are equipped with a loop closure matching module. The map distortion is automatically corrected when the carrier passes through repeated points, and incremental overwriting and updating are supported when new obstacles are added, forming an integrated and iterative composite environment model.

[0022] Traditional 2D raster maps can only distinguish obstacles and passageways, but cannot identify indoor functional areas. In long corridors and symmetrical apartment layouts, the maps are prone to distortion, and map errors cannot be corrected after the vehicle returns to its original position. This step solves the problem by splitting the map into three complementary layers: a 5cm high-precision raster adapted to the passage size of small to medium-sized indoor vehicles; a topological map that simplifies the indoor path structure, reducing the number of nodes traversed in subsequent path planning and lowering the onboard computing load; and a semantic map that delineates access permission areas, allowing direct avoidance of restricted work zones. Furthermore, the three layers of maps share the same coordinate system, and map distortion is automatically corrected when the vehicle passes through repeated points, solving the map drift and distortion problems in indoor weak texture and repetitive scenes. This makes the map suitable for routine mapping of indoor repetitive structures and weak texture sites.

[0023] The multi-factor fusion real-time high-precision positioning solution in step three specifically includes: retrieving the pre-built composite map from step two, extracting laser plane features and visual corner features in real time to complete map matching and initial positioning; relying on the vehicle-mounted wheel odometer to output the vehicle's mileage increment, and combining IMU attitude data to calculate short-term tracks to fill the positioning gaps during periods of feature loss; introducing UWB four-point ranging constraint equations to construct global coordinate constraint boundaries and eliminate accumulated errors in track calculation; building a dynamic weighted particle filter fusion framework and setting scene-adaptive weighting rules: increasing the weight of laser positioning by 65% ​​in open long corridors, increasing the weight of UWB wireless positioning in weak texture pure white areas, and increasing the weight of visual feature positioning in areas obscured by crowds; setting a positioning convergence threshold, outputting results when the particle residual in a single iteration is less than 0.03m, and outputting real-time six-bit real-time pose data of the vehicle's centimeter-level plane coordinates, heading angle, and driving speed, with a stable positioning frequency of 10Hz.

[0024] Fixed-weight fusion positioning cannot adapt to changing indoor environments: empty long corridors lack visual features, resulting in a significant decrease in visual positioning accuracy; pure white walls with weak textures lack laser features, leading to large laser positioning drift; crowds can obstruct the visual and laser detection field of view. This step adaptively adjusts the positioning weights of each sensor based on the indoor scene, and relies on the particle filter iterative convergence algorithm to constrain positioning errors, stably outputting complete six-bit pose data including planar coordinates, heading, and velocity, rather than just planar coordinates. It controls the carrier positioning error to the centimeter level in all scenarios, ensuring continuous and stable position output under any working conditions, without jump points or drift issues, and is suitable for all indoor operation scenarios such as warehouses, buildings, and computer rooms.

[0025] Step four, the initialization of navigation start point, end point, and traffic constraints, specifically includes: receiving scheduling instructions from the host computer or manual touch commands; selecting passable grid points within the composite map; locking the pose of the navigation start point and the target end point; synchronously matching the optimal orientation angle of the start and end points; inputting the inherent dynamic parameters of the vehicle: chassis circumscribed radius, maximum straight-line speed, maximum steering angular velocity, acceleration / deceleration response time, and minimum turning radius; configuring three types of safety avoidance distances: 0.3m basic safety distance for regular passages, 0.5m enhanced safety distance for narrow passages, and 0.8m avoidance safety distance for pedestrian intersections; batch importing semantic map-fixed constraints: permanent no-entry zones, time-limited closed passages, low-speed passage zones, and single-sided passage permission rules; simultaneously determining the connectivity of the start and end points; if a point is located in an obstacle grid or a no-entry semantic area, a pop-up error message is displayed, and a new point is selected; after successful verification, all navigation constraint parameters are locked, and the path planning phase begins.

[0026] Indoor traffic scenarios present significant risks due to human-computer interaction and varying traffic space dimensions. Maintaining a uniform safety distance can easily lead to scrapes or wasted traffic space. Furthermore, vehicle steering and driving parameters directly limit path accessibility. This step involves setting safety avoidance distances based on site scenario classifications, matching the vehicle's driving dynamics parameters, and verifying navigation start and end points in advance to avoid issues such as points falling on walls, equipment restricted areas, temporary barriers, or other impassable areas. It also intercepts invalid navigation tasks in advance to prevent operational malfunctions such as vehicle starting jams, stationary errors, and collisions with obstacles.

[0027] The adaptive weighted global optimal path coarse planning in step five specifically includes: improving the A* Euclidean distance heuristic function, adding a multi-dimensional weighted cost model, and adaptively iteratively adjusting the cost weights. Among these, the travel distance weight ensures the shortest path mileage, the channel width weight filters out narrow paths smaller than the carrier's travel width, and the semantic cost weight avoids high-frequency congestion areas, bumpy surfaces, and high-energy-consumption travel areas. The improved core cost formula of the A* algorithm is: F(n)=G(n)+H(n), where F(n) is the comprehensive travel cost of the node, G(n) is the actual travel cost from the starting point to the current node, and H(n) is the Euclidean distance heuristic cost from the current node to the destination. When the algorithm traverses the grid nodes, it adds node pruning rules to remove collinear, detour, and high-cost expansion nodes. The algorithm outputs a global inflection point coarse path composed of discrete inflection points, without global collision risk, with the optimal mileage, and compliant travel, based on the minimum global travel cost.

[0028] Traditional A* algorithms rely solely on spatial distance as a path selection criterion, often resulting in planned paths that traverse narrow passages, congested areas, and bumpy terrain. This leads to high energy consumption and a significant risk of collisions for the vehicle. Furthermore, the algorithm traverses numerous redundant grid nodes, resulting in lengthy computation times for airborne and embedded devices. In contrast, the improved A* algorithm introduces a multi-dimensional travel cost evaluation criterion, comprehensively considering factors such as distance, passage width, pedestrian flow, and energy consumption to select paths. By pruning nodes to eliminate invalid detours and collinear nodes, it prioritizes planning smooth, low-energy, and appropriately mileage inflection point paths, thereby improving the overall path rationality and reducing path planning computation time.

[0029] Specifically, step six, path smoothing optimization and safety expansion layer correction, includes: traversing the global coarse path inflection point set, using the slope difference judgment method to eliminate redundant inflection points with deflection angles less than 8°, and reducing the number of path anchor points; using third-order B-spline curve interpolation to fit the remaining discrete inflection points, constraining the maximum curvature of the path to not exceed the minimum turning curvature of the carrier, eliminating 90° right-angle sharp turns and acute-angle turns, and ensuring continuous curvature of the entire trajectory; based on the radius of the carrier chassis, performing hierarchical erosion and expansion processing on the surrounding grid of the path: performing first-level expansion around permanent obstacles, second-level dynamic expansion around temporary movable obstacles, and minimum basic expansion in open passage areas; verifying all points of the fitted trajectory, determining that the trajectory grid and the obstacle expansion grid have no intersection, and generating a benchmark smooth navigation trajectory that combines driving smoothness and all-domain passive collision avoidance capability.

[0030] Traditional methods simply remove inflection points without considering the minimum turning radius of the vehicle to fit the trajectory. This results in differential-driven vehicles experiencing steering jerking, slippage, and significant energy loss. Additionally, obstacles have uniform expansion dimensions, leading to wasted space near fixed walls and insufficient space near moving objects. By setting an 8° inflection point removal threshold and constraining the curvature of the spline curve to match the vehicle's steering capability, and by differentiating expansion collision avoidance levels for permanent, temporary, and open areas, a greater avoidance distance is provided for moving objects, comprehensively reducing the probability of scraping and collisions during vehicle operation.

[0031] Specifically, step seven, dynamic obstacle perception and local real-time replanning, includes: relying on lidar front-end detection and visual target recognition, a 3m×2m forward warning perception zone is delineated in front of the carrier; three types of dynamic obstacles are classified and identified: pedestrians, transfer vehicles, and temporary stacks; the obstacle's movement speed, direction of travel, and duration of passage are calculated using three consecutive frames of detection data; the obstacle's 3-second trajectory is predicted; and it is determined whether the obstacle intrudes into the baseline safe navigation trajectory. When an obstacle intrudes into the trajectory, a 1.5m local path interval is extracted before and after the obstacle, and combined with real-time vehicle speed and steering limits, a detour sub-trajectory is quickly generated for replanning. The replanning only modifies the local section route, and the beginning and end points of the detour trajectory are connected to the original global baseline path. The replanning response latency is ≤120ms, and the vehicle automatically returns to the original path after the detour is completed.

[0032] Traditional obstacle avoidance methods directly replan the global route after identifying obstacles, resulting in long detours and low operational efficiency. Artificial potential field-based obstacle avoidance algorithms are prone to causing the vehicle to get stuck at local points and spin in place. This step pre-defines the near-range early warning perception area, calculates the movement speed and direction of pedestrians, transport vehicles, and temporary debris in advance, and predicts the future trajectory of obstacles. Only local detours are optimized for the small sections of the path that are occupied, while the original global navigation route is retained throughout. The obstacle avoidance response delay is controlled to achieve fast and smooth detours. After the detour is completed, the system directly returns to the baseline navigation trajectory, ensuring the efficiency of navigation operations.

[0033] Specifically, step eight, trajectory tracking closed-loop control and map iterative update, includes: using an outer-loop pose deviation PID calculation and an inner-loop power speed regulation dual closed-loop controller to control the lateral and heading deviations of the vehicle's travel within ±2cm; continuously collecting incremental point cloud and image data of the surrounding environment throughout the vehicle's journey, comparing it with the original composite map, identifying new obstacles and removing similar obstacles, and automatically increasing the laser update weight in low-light environments at night; verifying the parking pose accuracy after navigation reaches the destination, and synchronously incrementally overwriting the three-layer composite map data to achieve multi-task continuous closed-loop navigation.

[0034] The outer loop compares the real-time positioning and pose of the vehicle with the planned baseline trajectory, performing PID differential calculations on lateral and heading deviations. The inner loop, combined with the chassis's dynamic response characteristics, calculates and adapts linear and angular velocity output commands to dynamically compensate for trajectory deviations caused by road slippage and mileage drift, keeping driving deviations within ±2cm. Relying on dual closed-loop PID control of the vehicle's driving posture, it compensates for driving deviations in real time, ensuring the vehicle follows the trajectory. In low-light environments at night, the weight of visual imaging is weakened, and the environment map is stably updated using laser point clouds. After navigation reaches the target point, the three-layer map of the entire domain is updated simultaneously, realizing a self-optimizing closed loop of navigation-vehicle control-mapping. It adapts to dynamic changes in the indoor environment and can directly take over the next set of navigation tasks, achieving continuous multi-task autonomous navigation operations.

[0035] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus.

[0036] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A method for indoor mobile vehicle navigation and positioning modeling and path planning, characterized in that, The specific steps are as follows: Step 1: Synchronous Acquisition and Preprocessing of Multi-Source Sensing Data The timing and extrinsic parameter matrices of four types of vehicle-mounted sensors—LiDAR, vision camera, IMU inertial unit, and UWB base station—are uniformly calibrated. Multi-source sensing data are collected synchronously, and filtering and noise reduction, coordinate system transformation, and abnormal and invalid sensing data are eliminated. Step Two: Modeling and Constructing the Indoor Layered Composite Environment Based on preprocessed data, a three-layer composite map with the same coordinate system is constructed by coupling a two-dimensional raster map, a topological path network map, and a semantic function label map, and a map closed-loop correction and incremental update mechanism is configured. Step 3: Multi-factor fusion real-time high-precision positioning solution An adaptive weighted particle filter fusion model was built, which integrates four types of observations: map features, wheel mileage, UWB ranging, and visual features, and dynamically adapts to the real-time pose of the scene weight calculation carrier. Step 4: Initialize navigation start point, destination, and traffic constraints Enter the poses of the navigation start and end points, load the vehicle dynamics parameters, hierarchical safety distance parameters, and semantic global access constraints, and lock the navigation constraints after verifying the passability connectivity of the start and end points. Step 5: Adaptive Weighted Global Optimal Path Coarse Planning A multi-dimensional cost-improved A* algorithm is adopted, which combines distance, channel width, semantic congestion, and energy consumption cost factors to search for nodes and prunes redundant nodes to generate a global collision-free coarse path with inflection points. Step Six: Path Smoothing Optimization and Safety Expansion Layer Correction By eliminating redundant inflection points with small deviations in the coarse path, a continuous curvature trajectory is fitted using third-order B-splines, and a hierarchical obstacle safety expansion buffer is constructed to generate a compliant benchmark smooth navigation trajectory. Step 7: Dynamic Obstacle Perception and Local Real-Time Replanning The laser-vision dual recognition module predicts the trajectory of dynamic obstacles. When an obstacle intrudes into the baseline trajectory, the improved dynamic window method is used to carry out local detour replanning while keeping the global path unchanged. Step 8: Trajectory Tracking Closed-Loop Control and Map Iterative Update The vehicle's driving posture is controlled by a dual closed-loop PID controller to compensate for driving trajectory deviations; environmental data is incrementally collected throughout the navigation operation, and the three-layer composite map is iteratively updated to complete the closed loop of the entire navigation process.

2. The indoor mobile carrier navigation and positioning modeling and path planning method according to claim 1, characterized in that, The multi-source sensing data synchronous acquisition and preprocessing described in Step 1 specifically includes: the main control board sending out a synchronous trigger pulse to unify the 10Hz sampling frequency of the four types of sensors, completing sensor hand-eye calibration and extrinsic parameter matrix calibration; synchronously acquiring three-dimensional laser point cloud, RGB depth image, three-axis angular velocity / acceleration attitude data, and raw ranging data from four-point UWB base stations; using adaptive statistical filtering to remove laser reflection pseudopoints and long-distance outliers, Gaussian filtering to remove image noise, building an IMU temperature drift compensation model to correct static zero bias, uniformly converting all sensing data to the vehicle body coordinate system at the center of the chassis, and removing invalid data that exceeds the ranging threshold.

3. The indoor mobile carrier navigation and positioning modeling and path planning method according to claim 1, characterized in that, Step two of the indoor layered composite environment modeling construction specifically includes: the grid map uses the GMAPping-SLAM algorithm to divide the grid into 5cm precision grid cells, and binarizes the passable grid cells and obstacle grid cells; the topology map uses indoor turning points, doors, and elevator entrances as topology nodes, fits passable straight paths between nodes as connecting edges, and binds the connecting edges to the channel width and slope attributes to simplify the global path logic; the semantic map relies on the indoor lightweight YOLO model to identify and label semantic tags for work areas, pedestrian passages, and restricted areas. The three layers of maps share the same world coordinate system and are equipped with a loop closure matching module. The map distortion is automatically corrected when the carrier passes through repeated points, and incremental overwriting and updating are supported when new obstacles are added, forming an integrated and iterative composite environment model.

4. The indoor mobile carrier navigation and positioning modeling and path planning method according to claim 1, characterized in that, Step 3, the multi-factor fusion real-time high-precision positioning solution, specifically includes: retrieving the pre-built composite map from Step 2, extracting laser plane features and visual corner features in real time to complete map matching and initial positioning; relying on the vehicle-mounted wheel odometer to output the vehicle's mileage increment, and combining IMU attitude data to calculate short-term tracks to fill the positioning gaps during periods of feature loss; introducing UWB four-point ranging constraint equations to construct global coordinate constraint boundaries and eliminate accumulated errors in track calculation; building a dynamic weighted particle filter fusion framework and setting scene-adaptive weighting rules: increasing the weight of laser positioning by 65% ​​in open long corridors, increasing the weight of UWB wireless positioning in weak texture pure white areas, and increasing the weight of visual feature positioning in areas obscured by crowds; setting a positioning convergence threshold, outputting results when the particle residual of a single iteration is less than 0.03m, and outputting real-time six-bit real-time pose data of the vehicle's centimeter-level plane coordinates, heading angle, and driving speed, with a stable positioning frequency of 10Hz.

5. The indoor mobile carrier navigation and positioning modeling and path planning method according to claim 1, characterized in that, Step four, which initializes the navigation start point, end point, and traffic constraints, specifically includes: receiving scheduling instructions from the host computer or manual touch commands; selecting passable grid points within the composite map; locking the navigation start point pose and target end point pose; and simultaneously matching the optimal orientation angles of the start and end points. It also involves inputting the inherent dynamic parameters of the vehicle: chassis circumscribed radius, maximum straight-line speed, maximum steering angular velocity, acceleration / deceleration response time, and minimum turning radius; configuring three types of safety avoidance distances: a basic safety distance of 0.3m for regular passages, a reinforced safety distance of 0.5m for narrow passages, and a safety distance of 0.8m for pedestrian crossings; batch importing semantic map-based fixed constraints: permanent no-entry zones, time-limited closed passages, low-speed passage zones, and single-sided passage permission rules; and simultaneously determining the connectivity of the start and end points. If a point is located in an obstacle grid or a no-entry semantic area, a pop-up error message is displayed, and a new point is selected. After successful verification, all navigation constraint parameters are locked, and the path planning phase begins.

6. The indoor mobile carrier navigation and positioning modeling and path planning method according to claim 1, characterized in that, The adaptive weighted global optimal path coarse planning described in step five specifically includes: improving the A* Euclidean distance heuristic function, adding a multi-dimensional weighted cost model, and adaptively iteratively adjusting the cost weights. Among these, the travel distance weight ensures the shortest path mileage, the channel width weight filters out narrow paths smaller than the carrier's travel width, and the semantic cost weight avoids high-frequency congestion areas, bumpy surfaces, and high-energy-consumption travel areas. The core cost formula of the improved A* algorithm is: F(n)=G(n)+H(n), where F(n) is the comprehensive travel cost of the node, G(n) is the actual travel cost from the starting point to the current node, and H(n) is the Euclidean distance heuristic cost from the current node to the destination. When the algorithm traverses the grid nodes, a node pruning rule is added to remove collinear, detour, and high-cost expansion nodes. The coarse path with global inflection points, composed of discrete inflection points, without global collision risk, with the optimal mileage, and compliant travel is output based on the minimum global travel cost.

7. The indoor mobile carrier navigation and positioning modeling and path planning method according to claim 1, characterized in that, Step six, specifically the path smoothing optimization and safety expansion layer correction, includes: traversing the global coarse path inflection point set, using the slope difference judgment method to eliminate redundant inflection points with deflection angles less than 8°, thus reducing the number of path anchor points; using third-order B-spline curve interpolation to fit the remaining discrete inflection points, constraining the maximum curvature of the path to not exceed the minimum turning curvature of the carrier, eliminating 90° right-angle sharp turns and acute-angle turns, and ensuring continuous curvature of the entire trajectory; based on the radius of the carrier chassis, performing hierarchical erosion and expansion processing on the surrounding grid of the path: performing first-level expansion around permanent obstacles, second-level dynamic expansion around temporary movable obstacles, and minimum basic expansion in open passage areas; verifying all points of the fitted trajectory, determining that the trajectory grid and the obstacle expansion grid have no intersection, and generating a benchmark smooth navigation trajectory that combines driving smoothness and all-domain passive collision avoidance capability.

8. The indoor mobile carrier navigation and positioning modeling and path planning method according to claim 1, characterized in that, Step seven, specifically the dynamic obstacle perception and local real-time replanning, includes: relying on lidar front-end detection and visual target recognition, a 3m×2m forward warning perception zone is delineated in front of the vehicle; three types of dynamic obstacles are classified and identified: pedestrians, transport vehicles, and temporary stacks; the obstacle's movement speed, direction of travel, and duration of passage are calculated using three consecutive frames of detection data; the obstacle's 3-second trajectory is predicted; and it is determined whether the obstacle intrudes into the baseline safe navigation trajectory. When an obstacle intrudes into the trajectory, a 1.5m local path interval is extracted before and after the obstacle, and combined with real-time vehicle speed and steering limits, a detour sub-trajectory is quickly generated for replanning. The replanning only modifies the local section route, and the beginning and end points of the detour trajectory are connected to the original global baseline path. The replanning response latency is ≤120ms, and the vehicle automatically returns to the original path after the detour is completed.

9. The indoor mobile carrier navigation and positioning modeling and path planning method according to claim 1, characterized in that, Step eight specifically includes: using an outer-loop pose deviation PID calculation and an inner-loop power speed regulation dual closed-loop controller to control the lateral and heading deviations of the vehicle's travel within ±2cm; continuously collecting incremental point cloud and image data of the surrounding environment throughout the vehicle's journey, comparing it with the original composite map, identifying new obstacles and removing similar obstacles, and automatically increasing the laser update weight in low-light environments at night; verifying the docking pose accuracy after navigation reaches the destination, and synchronously incrementally overwriting the three-layer composite map data to achieve multi-task continuous closed-loop navigation.

Images