Multi-sensor data fusion positioning navigation system for intelligent campus unmanned vehicle

Abstract

The invention relates to the technical field of urban planning management, and particularly discloses a multi-sensor data fusion positioning navigation system for an intelligent campus unmanned vehicle, which is characterized in that vehicle-mounted multi-line laser radar point cloud, IMU inertial navigation data and GNSS satellite positioning data are acquired; during mapping, an improved SLAM algorithm is adopted to generate a three-dimensional point cloud map, the three-dimensional point cloud map is converted into a two-dimensional grid map, a positioning mode is adaptively selected during navigation, and an optimal pose is solved through a multi-sensor data fusion algorithm; the path planning module generates hybrid navigation data including a global path sequence and a local obstacle avoidance trajectory based on the pose, a map and a target point; according to the process, a complete navigation closed loop from environment perception to motion execution is constructed through multi-source data deep fusion and space-time synchronization, positioning drifting and dynamic obstacles in a complex campus scene are effectively handled, and reliable technical support is provided for high-precision autonomous navigation of the intelligent campus unmanned vehicle.

Description

Technical Field

[0001] This invention belongs to the field of urban planning and management technology, and relates to a multi-sensor data fusion positioning and navigation system for unmanned vehicles in smart campuses. Background Technology

[0002] Multi-sensor data fusion positioning and navigation technology for unmanned vehicles in smart campuses is of paramount importance and necessity for achieving accurate, continuous, and robust autonomous operation in dynamic, semi-structured environments with mixed pedestrian and vehicle traffic. Smart campus scenarios combine indoor-outdoor connectivity, dense building clusters, and high-frequency pedestrian activity. Single sensors such as GNSS are susceptible to occlusion failure, LiDAR is affected by weather, and visual sensors are affected by lighting interference. Therefore, fusing data from multiple sources such as LiDAR, IMU, and GNSS can leverage their complementary strengths to provide stable pose estimation under various conditions. This is the cornerstone for ensuring the safe obstacle avoidance, path planning, and efficient completion of logistics and transportation tasks by unmanned vehicles.

[0003] However, current technology has obvious drawbacks: external parameter calibration is often an offline static process, which cannot compensate for mechanical drift during long-term operation, resulting in inaccurate coordinate unification; fusion algorithms (such as Kalman filtering) mostly use fixed noise models, which are difficult to adaptively adjust to cope with environmental fluctuations such as drastic changes in GNSS signals; thus limiting the practicality and reliability of the system. Summary of the Invention

[0004] In view of the problems existing in the prior art, the present invention provides a multi-sensor data fusion positioning and navigation system for unmanned vehicles in smart campuses to solve the above-mentioned technical problems.

[0005] To achieve the above and other objectives, the technical solution adopted by the present invention is as follows: This invention provides a multi-sensor data fusion positioning and navigation system for unmanned vehicles in smart campuses. The system includes a data acquisition module, a preprocessing module, a mapping and positioning module, a path planning module, and a motion control module. The data acquisition module is used to acquire environmental point cloud data output by the vehicle-mounted multi-line lidar, inertial navigation data output by the inertial measurement unit, and satellite positioning data output by the global satellite navigation system in real time. The preprocessing module is configured to receive environmental point cloud data, inertial navigation data and satellite positioning data, synchronize and align the above data according to a preset time reference, and unify the data of each sensor to the vehicle coordinate system based on a pre-calibrated extrinsic matrix, and output the spatiotemporally registered multi-source sensor data stream. The mapping and localization module is used to receive multi-source sensor data streams. In the mapping stage, it uses an improved simultaneous localization and mapping algorithm to generate a 3D point cloud map and convert it into 2D grid map data. In the navigation stage, it adaptively selects the localization mode according to the current environmental characteristics and calculates the optimal pose estimation data of the unmanned vehicle in the 2D grid map data based on the multi-sensor data fusion algorithm. The path planning module generates hybrid navigation data, including global path sequences and local obstacle avoidance trajectories, based on optimal pose estimation data, 2D grid map data, and target point data. The motion control module receives the hybrid navigation data, converts it into chassis motor drive commands through kinematic model calculations, and controls the autonomous vehicle's movement.

[0006] As described above, the multi-sensor data fusion positioning and navigation system for unmanned vehicles in smart campuses provided by this invention has at least the following beneficial effects: This invention employs an improved simultaneous localization and mapping (SMR) algorithm in the mapping and localization stages. This algorithm efficiently constructs and optimizes a 3D environment model during the mapping phase and intelligently converts it into a lightweight 2D grid map suitable for real-time navigation. Crucially, during navigation, this solution innovatively designs a mechanism that adaptively selects the optimal localization mode (such as feature matching localization, inertial-dominated localization, or fusion localization) based on multi-source information including real-time point cloud features, satellite signal quality, and inertial data confidence levels. It also continuously outputs the optimal probabilistic pose estimate of the vehicle in the 2D map through a multi-source data fusion algorithm. This adaptive strategy enables the system to intelligently handle continuous scene transitions from open elevated roads to satellite-denied tunnels, and from static structured roads to densely populated areas. It effectively avoids the localization jumps, divergences, or loss problems that easily occur with traditional single or fixed switching strategies during sudden environmental changes. This achieves robust localization with centimeter-level accuracy in complex, long-cycle operations, significantly improving the continuity and accuracy of the autonomous vehicle's environmental understanding and self-state awareness in real-world complex conditions.

[0007] This invention further utilizes a path planning module to generate, in real time, a hybrid navigation command that integrates the globally optimal path sequence and local dynamic obstacle avoidance trajectory, based on the aforementioned high-precision and robust optimal pose estimation and two-dimensional map, combined with global target points. The motion control module, based on a precise vehicle kinematics model, translates this command into smooth, continuous drive signals for the underlying actuators. This end-to-end closed-loop collaborative design, from high-quality perception and intelligent decision-making to precise execution, ensures that the navigation strategy can be dynamically optimized and fine-tuned according to the real-time environment and its own state. This integrated solution effectively overcomes problems in traditional systems such as driving jerks, trajectory oscillations, and insufficient passability in dense dynamic obstacles caused by inconsistent perception data, fluctuations in positioning results, or disconnect between the planning and control modules. This significantly improves the smoothness, safety, and intelligent decision-making level of the overall navigation of autonomous vehicles, providing core technical support for achieving high-level autonomous driving in all weather and all scenarios. Attached Figure Description

[0008] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced 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.

[0009] Figure 1 This is a schematic diagram showing the connections of the various modules in the system of the present invention. Detailed Implementation

[0010] The following description, in conjunction with the implementation of this invention, is merely an example and illustration of the concept of this invention. Those skilled in the art can make various modifications or additions to the specific embodiments described, or use similar methods to replace them, as long as they do not deviate from the inventive concept or exceed the scope defined in these claims, all of which should fall within the protection scope of this invention.

[0011] Please see Figure 1 As shown, a multi-sensor data fusion positioning and navigation system for unmanned vehicles in smart campuses includes a data acquisition module, a preprocessing module, a mapping and positioning module, a path planning module, and a motion control module. The data acquisition module is used to acquire environmental point cloud data output by the vehicle-mounted multi-line lidar, inertial navigation data output by the inertial measurement unit, and satellite positioning data output by the global satellite navigation system in real time. The preprocessing module is configured to receive environmental point cloud data, inertial navigation data and satellite positioning data, synchronize and align the above data according to a preset time reference, and unify the data of each sensor to the vehicle coordinate system based on a pre-calibrated extrinsic matrix, and output the spatiotemporally registered multi-source sensor data stream. The mapping and localization module is used to receive multi-source sensor data streams. In the mapping stage, it uses an improved simultaneous localization and mapping algorithm to generate a 3D point cloud map and convert it into 2D grid map data. In the navigation stage, it adaptively selects the localization mode according to the current environmental characteristics and calculates the optimal pose estimation data of the unmanned vehicle in the 2D grid map data based on the multi-sensor data fusion algorithm. The path planning module generates hybrid navigation data, including global path sequences and local obstacle avoidance trajectories, based on optimal pose estimation data, 2D grid map data, and target point data. The motion control module receives the hybrid navigation data, converts it into chassis motor drive commands through kinematic model calculations, and controls the autonomous vehicle's movement.

[0012] Preferably, the mapping and positioning module performs the following data processing steps when generating two-dimensional raster map data: The spatiotemporally registered environmental point cloud data is obtained, and outlier noise data is removed using a hybrid filtering algorithm; Ground point segmentation is performed on the filtered point cloud data based on preset height and slope thresholds to separate ground point cloud data from obstacle point cloud data. The obstacle point cloud data is projected onto a two-dimensional plane, and the point cloud density feature value in each grid cell is statistically analyzed during the projection process. The point cloud density feature value is compared with a preset occupancy probability threshold. When the feature value exceeds the threshold, the corresponding grid is marked as an obstacle occupancy state, and 3D to 2D grid map data containing occupancy state information is generated. The 2D grid map data is configured as the base layer for subsequent navigation path search.

[0013] In one specific embodiment, the spatiotemporally registered environmental point cloud dataset is obtained. ,in Let be the three-dimensional spatial coordinates of the i-th point, in meters, and N be the total number of point clouds. A hybrid filtering algorithm is used to remove outlier noise data. This algorithm first employs a radius filter, setting the search radius... The recommended value range is 0.05m to 0.15m, preferably 0.1m, to balance denoising effect and computational efficiency. Calculations are performed point-to-point. Centered on Number of neighboring points in the neighborhood of a sphere with radius ,like Less than the preset minimum neighbor threshold The recommended value range is 3 to 8, preferably 5. If this value is set, the point is identified as an outlier and removed, resulting in denoised point cloud data. Subsequently, voxel filtering downsampling is performed on the denoised point cloud data, with the voxel grid side length set. The recommended value range is 0.05m to 0.1m, preferably 0.08m. Calculate the average coordinates of all points within the falling pixel grid as the representative point of the grid, and output the filtered point cloud data. To reduce data redundancy and preserve geometric features, the filtered point cloud data is then segmented into ground points based on preset height and slope thresholds. A grid-based segmentation strategy is used to divide the horizontal plane into segments of size [missing information]. Grid cells, The recommended value range is 0.5m to 1.5m, preferably 1.0m. For point sets falling within the same grid cell, extract the minimum height value. As a local ground reference, calculate any point Height difference features The unit is meters, and the slope characteristics between this point and the lowest point in the grid are calculated simultaneously. The unit is radians, and a preset height threshold is set. The recommended value range is 0.1m to 0.25m, preferably 0.15m. The setting logic is to filter out minor road surface undulations and measurement noise by setting a preset slope threshold. The recommended value range is 0.15 rad to 0.35 rad (corresponding to approximately 8 to 20 degrees), with 0.26 rad (corresponding to 15 degrees) being the preferred value. The logic behind this setting is to distinguish between passable ramps and vertical obstacle walls. and If both conditions are met, the data is classified as ground point data; otherwise, it is classified as obstacle point data, thus separating the obstacle point cloud dataset. The obstacle point cloud data is then projected onto a two-dimensional plane to construct a resolution of [resolution missing]. raster map, The recommended value range is 0.05m to 0.2m, preferably 0.1m, to match the navigation accuracy requirements of autonomous vehicles. During the projection process, the point cloud density feature value within each grid cell is statistically analyzed. The calculation formula is: ,in This represents the point cloud density feature value of the m-th row and n-th column raster, in units of points per square meter. The number of obstacle points projected within the grid is a dimensionless integer; finally, the point cloud density feature value is compared with a preset occupancy probability threshold. Compare them. It is recommended to set the value based on the number of lidar lines and the ambient noise level, ranging from 5 to 30 lines per square meter, with 15 lines per square meter being preferred. The logic behind this setting is to distinguish between real obstacle clusters and occasional false noise detections. When the grid is in an occupied state, it is determined to be occupied and assigned a value of 1; otherwise, it is determined to be free and assigned a value of 0, generating a two-dimensional grid map data matrix containing occupancy status information.

[0014] Preferably, the mapping and positioning module includes an environmental state determination unit, which is used to monitor the eigenvalues ​​of the covariance matrix of the satellite positioning data in real time. When the eigenvalue of the covariance matrix is ​​less than the preset accuracy threshold, it is determined to be an outdoor environment mode, and the system activates the first fusion positioning channel based on factor graph optimization. When the eigenvalue of the covariance matrix is ​​greater than or equal to a preset accuracy threshold, it is determined to be an indoor or weak signal environment mode, and the system activates the second fusion positioning channel based on adaptive particle filtering.

[0015] In one specific embodiment, the raw positioning data sequence output by the global navigation satellite system receiver is obtained. Each data point Includes three-dimensional coordinates All units are meters. To evaluate the quality of the satellite signal at the current moment, a sliding window strategy is used to extract the positioning data from the most recent N epochs as a sample set, where N is the preset sliding window length. A value between 10 and 20 epochs is recommended, with 15 epochs being preferred. The logic behind this range is to smooth out random noise while promptly reflecting changes in signal quality. Next, a position covariance matrix is ​​constructed based on this sample set. The calculation formula is: ,in The mean location of the sample set is given in meters, T represents the matrix transpose, and the covariance matrix is ​​given. for The real symmetric matrix whose elements are all in square meters; subsequently, the covariance matrix... Perform eigenvalue decomposition and solve the characteristic equation. Three eigenvalues ​​were obtained Select the largest eigenvalue As the eigenvalue of the covariance matrix of satellite positioning data, this value reflects the variance of the positioning error ellipsoid along the direction of maximum extension, and is expressed in square meters; finally, Compared with the preset accuracy threshold Perform a comparison and preset a precision threshold. The recommended value range is 25 square meters to 100 square meters, preferably 50 square meters (corresponding to a standard deviation of approximately 7 meters). The logic behind this threshold setting is to distinguish between high-precision positioning in a wide-open outdoor environment and multipath effects and signal obstruction in indoor or densely populated urban areas. When the current environment is determined to be outdoor, and the satellite signal is good, the system activates the first fusion positioning channel based on factor graph optimization, using high-precision satellite data constraints to optimize the positioning result; when When the system determines that the current environment is indoor or in a weak signal environment mode, satellite data is unreliable or unavailable. The system then activates the second fusion positioning channel based on adaptive particle filtering and relies on lidar and inertial navigation data for relative positioning.

[0016] Preferably, the data processing procedure of the first fused positioning channel includes: Construct a factor graph model containing the pose vertices of the autonomous vehicle; The satellite positioning data is added as an absolute position constraint factor to the factor graph model; The inertial navigation data is pre-integrated to generate relative motion constraint factors, which are then added to the factor graph model. The environmental point cloud data is scanned and matched with the three-dimensional point cloud map to generate a laser odometry constraint factor, which is then added to the factor graph model. By minimizing the joint error function of all factors, the pose vertices in the factor graph model are solved nonlinearly, and the high-precision outdoor pose data is output as the optimal pose estimation data.

[0017] In one specific embodiment, the data processing procedure of the first fused positioning channel includes: firstly, constructing a factor graph model containing the pose vertices of the unmanned vehicle, wherein the state vector of the model is defined as... ,in It is a three-dimensional position vector, with units of meters, derived from the state recursion or initial value of the previous time step; This is a three-dimensional velocity vector, with units of meters per second; A quaternion representing attitude is a dimensionless mathematical expression; Accelerometer bias, in meters per square second; The gyroscope bias is expressed in radians per second. Next, the satellite positioning data is added as an absolute position constraint factor to the factor graph model to construct the satellite positioning residual term. The calculation formula is: ,in The position observation value output by the satellite receiver, in meters, represents the covariance matrix corresponding to this residual term. The information matrix is ​​obtained by directly outputting the precision factor or differential state solution from the satellite receiver, with units of square meters. The inertial navigation data is used as a weight in the optimization process. Simultaneously, the inertial navigation data undergoes pre-integration to generate relative motion constraint factors. This process is performed between adjacent keyframes i and j, utilizing the angular velocity of the inertial measurement unit. By integrating with the original acceleration data 'a', the change in relative position can be calculated. Relative velocity change and relative rotation quaternions Construct the inertial pre-integral residual term The residual combines the errors in position, velocity, and attitude, and its covariance matrix is... The noise model of the inertial sensor is used to calculate the discrete-time recursive values, with units corresponding to square meters, square meters per second, and radians squared, respectively. Subsequently, the environmental point cloud data is scanned and matched with the 3D point cloud map, specifically using an iterative nearest-point or normal distribution transformation algorithm to calculate the relative pose transformation of the current frame point cloud relative to the map. This is used to construct the laser odometry residual term. The calculation formula is: ,in The inverse operation of pose transformation is represented by log, which is the logarithmic mapping on the manifold that maps the transformation error to the Lie algebra space, and its information matrix is... It is recommended to dynamically set the fitting score based on the point cloud matching, with a suggested value range of 10 to 1000 diagonal matrix elements, preferably the reciprocal of the matching score, to adaptively adjust the weight of the laser constraint. Finally, by minimizing the joint error function of all factors, a nonlinear optimization solution is performed on the pose vertices in the factor graph model. The joint error function is defined as follows: ,in The Mahalanobis distance is a dimensionless scalar, and is solved iteratively using the Levenberg-Marquardt algorithm, with a preset damping factor. Since it is a dimensionless coefficient, it is recommended to set the initial value to 0. Its adjustment logic is to reduce the error when the iteration is successful. To accelerate convergence, a method approximating Gauss-Newton is used, but this is increased as the error rises. Convergence is guaranteed by approximating gradient descent by solving the incremental equation. Update the state vector, where H is the Hessian matrix and g is the gradient vector, until the increment... If the norm is less than the preset convergence threshold or the maximum number of iterations is reached, the outdoor high-precision pose data is output as the optimal pose estimation data.

[0018] Preferably, the data processing procedure of the second fused positioning channel includes: Initialize the particle set, and use the inertial navigation data as prediction input to update the prior pose distribution of each particle in the particle set; The environmental point cloud data of the current frame is matched with the two-dimensional raster map data in the likelihood domain to calculate the weight value of each particle. Resampling is performed based on particle weight values ​​to remove low-weight particles and duplicate high-weight particles. The weighted average of the resampled particle set is calculated, and the indoor pose estimation data is output as the optimal pose estimation data. The indoor pose estimation data and the inertial navigation data form a closed-loop correction relationship.

[0019] In one specific embodiment, the data processing procedure of the second fusion positioning channel first initializes a particle set containing N particles. ,in Let represent the pose assumption of the i-th particle at time t. These are two-dimensional plane coordinates, with the unit being meters. This is the heading angle, in radians. The normalized weight of the particles is a dimensionless coefficient. The recommended value for the number of particles N is between 500 and 2000, preferably 1000. This value is chosen to balance computational load and positioning accuracy, ensuring real-time operation on the embedded edge computing terminal. Using the inertial navigation data as prediction input, a motion model is constructed to update the prior pose distribution of each particle in the particle set. Specifically, this involves obtaining the angular velocity output by the inertial measurement unit. Given acceleration 'a', calculate the relative motion increment by time integration. Combined with the particle pose at the previous moment The state transition is calculated using the following formula: ,in This represents pose composition operations. From Gaussian distribution Mid-sampled random noise is used to simulate IMU drift and integration error; covariance matrix It is recommended to set the zero-bias instability parameter according to the IMU device datasheet to ensure that the predicted distribution covers the true pose; subsequently, acquire the environmental point cloud data of the current frame and assume the pose of each particle. As transformation parameters, the laser point cloud is transformed from the laser coordinate system to the map coordinate system, resulting in the transformed scan point set. The point set is then matched with the two-dimensional grid map data in the likelihood field to calculate the weight value of each particle. The matching process uses a pre-calculated likelihood field map, which stores the Euclidean distance from each grid cell to the nearest obstacle. The particle weight calculation formula is as follows: Where K is the number of valid scan points, which is a dimensionless integer. The distance from the k-th scan point in the i-th particle pose to the nearest obstacle in the map is expressed in meters. To observe the noise standard deviation, the unit is meters, and a value range of 0.05m to 0.2m is recommended, with 0.1m being preferred. The logic behind this setting is to balance positioning accuracy with tolerance to environmental noise (such as pedestrians and glass reflections). After calculating the weights of all particles, the number of effective particles is then calculated. ,when Less than the preset resampling threshold When using this value, it is recommended to set it to 0.5N. Resampling is performed based on the particle weights, employing a low-variance resampling algorithm to remove low-weight particles and replicate high-weight particles, causing the particle distribution to converge to near the true pose. Finally, the weighted average of the resampled particle set is calculated, and the pose output formula is: The indoor pose estimation data is output as the optimal pose estimation data.

[0020] Preferably, the route planning module includes a global route planning unit, which performs the following operations: In the two-dimensional grid map data, starting from the current optimal pose estimation data and ending at the target point data, the improved A* algorithm is used to search and generate initial path data containing a series of discrete waypoints. Extract the turning key point data from the initial path data, and use the Bézier curve algorithm to perform smooth interpolation calculations on the road segments before and after the key point data; Generate globally optimal path data with continuous curvature and transmit this data to the local path planning unit.

[0021] In one specific embodiment, the two-dimensional grid map data is acquired and abstracted into a directed graph model containing a set of nodes and a set of edges. The grid coordinates parsed from the current optimal pose estimation data are taken as the starting node S, and the grid coordinates parsed from the target point data are taken as the target node G. An improved A* algorithm is used to search and generate initial path data containing a series of discrete waypoints. This algorithm improves upon traditional cost functions... Based on this, a safety distance penalty term is introduced to construct an improved cost function. G(n) is the actual cost of moving from the starting node S to the current node n, in meters, obtained by accumulating the Euclidean distance between adjacent grid centers; H(n) is the heuristic estimated cost of moving from the current node n to the target node G, in meters, and it is recommended to use Manhattan distance or Euclidean distance for calculation. This is the Euclidean distance from the current node n to the nearest obstacle grid, in meters, which can be quickly calculated using a distance transformation algorithm; The preset safety penalty range coefficient is in meters. It is recommended to have a value range of 0.5 meters to 2.0 meters, preferably 1.0 meters. Its setting logic is to make the path prioritize the center route away from obstacles in narrow passages. The preset safety distance attenuation factor, in meters, is recommended to be between 0.2 and 0.5 meters, preferably 0.3 meters. It controls the rate at which the penalty term decays with increasing distance. The search direction is guided by minimizing F'(n), and the output is an initial path data sequence containing a series of zigzag points. Subsequently, key point extraction and smoothing are performed on the initial path data, and adjacent path vectors are calculated by traversing the path sequence. and The included angle The calculation formula is: When the included angle When the turning angle exceeds a preset turning threshold (recommended range is 15 to 45 degrees, preferably 30 degrees), the point is determined to be a critical turning point. A Bézier curve algorithm is used to perform smooth interpolation calculations on the road segments before and after the critical turning point data. A cubic Bézier curve is used to replace the bends near the critical turning point, and the curve equation is defined as follows: , where t is a normalization parameter, with a value range of [0,1] and is dimensionless; and These are the start and end points of the curve, respectively, in meters, taken from the path points before and after the key point; and For control points, the unit is meters, and they are defined by extending a preset distance along the tangent direction of the key points. Sure The recommended value range is 0.2 meters to 1.0 meters, preferably 0.5 meters, to generate a curve with continuous curvature that satisfies the kinematic constraints of the autonomous vehicle. Finally, the smoothed Bézier curve segment is spliced ​​with the original path to generate globally optimal path data with continuous curvature, and this data is transmitted to the local path planning unit.

[0022] Preferably, the path planning module further includes a local path planning unit, which performs the following operations: Based on the environmental point cloud data, a local cost map is constructed, which reflects the distribution of dynamic and static obstacles around the unmanned vehicle in real time. Based on the dynamic window algorithm, multiple sets of simulated velocity pairs are sampled in the velocity space, and the corresponding multiple predicted trajectory data are deduced. An evaluation function is constructed to score each predicted trajectory data. The evaluation function comprehensively considers the degree of fit between the trajectory and the global optimal path data, the distance to obstacles in the local cost map data, and the current driving speed. Select the velocity pair corresponding to the highest-scoring predicted trajectory data, and output local control command data containing linear velocity and angular velocity.

[0023] In one specific embodiment, local cost map data is constructed based on the environmental point cloud data. Specifically, this involves acquiring the 3D environmental point cloud collected by the LiDAR, projecting it onto a 2D plane with the center of the autonomous vehicle as the origin using a coordinate transformation matrix, and setting the grid map resolution. The recommended value range is 0.02 meters to 0.1 meters, preferably 0.05 meters. The logic behind this setting is to balance environmental perception accuracy with computational resource consumption. The projected obstacle grid is then expanded, with the expansion radius... The calculation formula is ,in The radius of the circumscribed circle of the autonomous vehicle is expressed in meters and is obtained directly from the vehicle's geometric parameters. The recommended value for the preset safety buffer distance is between 0.1 meters and 0.3 meters, preferably 0.2 meters. The necessity of this expansion process is to visualize the obstacle risk area, provide a safety boundary for path search, and generate a local cost map matrix that reflects the distribution of dynamic and static obstacles around the unmanned vehicle in real time. Subsequently, multiple simulated velocity pairs are generated by sampling in the velocity space based on the dynamic window algorithm. Based on the current motion state and dynamic constraints of the autonomous vehicle, the feasible velocity sampling space at the current moment is calculated, and the dynamic window is defined. for , where v and These are sampled values ​​of linear velocity and angular velocity, respectively, in meters per second and radians per second. and Given the current linear velocity and angular velocity, and These are the maximum acceleration and maximum deceleration, respectively, in meters per second squared. This is the maximum angular acceleration, expressed in radians per square second. To control the period, measured in seconds, the logic behind setting this window is to ensure that the sampling speed is within the vehicle's physical limits and braking capacity, and to operate within this window at a preset linear velocity resolution. (Recommended value: 0.01 m / s) and angular velocity resolution (It is recommended to use a value of 0.01 radians per second) Discretize the sampling to obtain multiple sets of simulated velocity pairs; Next, trajectory extrapolation is performed for each set of simulated speed pairs, assuming the autonomous vehicle travels within the predicted time. The inner part moves at a constant speed, and the predicted trajectory sequence is calculated using a kinematic model. The calculation formula is as follows: ,in Let be the coordinates and heading angle at step k, in meters and radians respectively. The simulation step size is in seconds, which generates multiple predicted trajectory data; subsequently, an evaluation function is constructed. Each predicted trajectory data point is scored, and the evaluation function uses a normalized weighted summation, where... The azimuth evaluation component is calculated using the following formula: The unit is radians. To predict the heading angle at the end of the trajectory, This is the azimuth angle from the center of the current autonomous vehicle to the target point. This component is used to guide the autonomous vehicle towards the target. The obstacle distance evaluation component is calculated using the following formula: The unit is meters, which represents the distance from each point on the predicted trajectory to the nearest obstacle in the local cost map. If the distance is less than the collision threshold, the trajectory score is set to zero. This component is used to ensure obstacle avoidance safety. For the speed evaluation component, the value of the simulated linear velocity v is directly taken, in meters per second, to encourage autonomous vehicles to travel at higher speeds under safe conditions; The normalized smoothing factor, These are the azimuth weight, range weight, and velocity weight, respectively, all of which are dimensionless coefficients. Recommended ranges are provided. The weighting logic prioritizes ensuring safe obstacle avoidance distance, followed by directional accuracy, and finally considers driving efficiency; ultimately, the speed corresponding to the highest-scoring predicted trajectory is selected. The output is local control command data containing linear velocity and angular velocity.

[0024] Preferably, when the preprocessing module performs time point synchronization alignment, it uses the acquisition and transmission time of the environmental point cloud data as the reference time axis; For the inertial navigation data with high frequency, the inertial data value corresponding to the reference time axis moment is calculated by linear interpolation. For the satellite positioning data with a low frequency, the nearest neighbor matching method is used to associate the satellite positioning data frame that is closest to the reference time axis. The synchronized data is packaged into multi-sensor data frames with a unified time label to ensure that the data processed by the subsequent fusion algorithm is in the same time segment.

[0025] In one specific embodiment, firstly, each frame of environmental point cloud data output by the lidar sensor is acquired, and then the hardware time point information in the data packet header is parsed. The reference time point, measured in seconds, is chosen because LiDAR point cloud data typically has the highest data density and is most sensitive to motion distortion; therefore, using it as the system's time reference maximizes the registration accuracy of subsequent fusion algorithms. Next, for the frequently accessed inertial navigation data, the system establishes a system in memory with a depth of... A circular buffer queue is recommended. It can store at least 200 milliseconds of data to cope with transmission latency. Linear interpolation is used to calculate the inertial data value corresponding to the reference time axis moment. Specifically, the process involves retrieving time points from the buffer queue. and , so that satisfaction Under the given conditions, use the linear interpolation formula The acceleration and angular velocity data at the current moment are calculated, where for The inertial measurement vector at each moment, measured in meters per square second or radians per second, is derived from the physical characteristic that inertial sensor data changes linearly over a very short time, effectively eliminating system errors introduced by inconsistent sampling times. Subsequently, for the lower-frequency satellite positioning data, the nearest neighbor matching method is used to associate the satellite positioning data frame closest to the reference time axis moment, calculating the satellite data time point. Compared with the reference time Time deviation Traverse the cache queue to find The smallest satellite data frame, in which a preset time correlation threshold is introduced. The recommended value range is 0.05 seconds to 0.2 seconds, preferably 0.1 seconds. The logic behind this threshold setting is to distinguish between valid synchronized data and outdated, stale data. If the minimum time deviation is greater than... If the satellite signal is deemed unavailable at the current moment, the data frame is discarded to prevent erroneous constraints from contaminating the positioning results. Finally, the synchronized environmental point cloud data, interpolated inertial navigation data, and matched satellite positioning data are encapsulated into a single data set containing a unified time label. The multi-sensor data frames ensure that the data processed by the subsequent fusion algorithm are at the same time segment, and output the time-synchronized sensor fusion data stream.

[0026] Preferably, the system adopts a cloud-edge-device collaborative architecture for data interaction and processing: The 3D point cloud map construction and global path planning calculation in the mapping and positioning module are deployed on a cloud server. The cloud computing power is used to process large-scale map data optimization tasks, and the optimized global map data is distributed. The real-time pose calculation and local path planning calculation in the positioning module are deployed on the vehicle edge computing terminal. The vehicle edge computing terminal uses locally acquired sensor data and global map data sent from the cloud to perform real-time matching and obstacle avoidance decisions. The motion control module is deployed on the vehicle chassis actuator and directly responds to the control commands output by the edge computing terminal.

[0027] Preferably, the motion control module has an embedded differential kinematics model that decouples the linear velocity and angular velocity in the local control command data; Based on the wheel spacing and wheel radius parameters of the unmanned vehicle, the target rotational speed data of the left and right drive wheels are calculated; By using a PID control algorithm to adjust the motor voltage in a closed loop, the actual wheel speed is made to follow the target speed data, thereby driving the unmanned vehicle to travel along the planned path.

[0028] In one specific embodiment, the 3D point cloud map construction and global path planning calculation in the mapping and positioning module are deployed on a cloud server. The cloud server receives environmental point cloud data and initial pose data uploaded by multiple unmanned vehicles, and uses high computing power to perform large-scale point cloud registration and loop closure detection to construct a 3D point cloud map of the entire campus. Based on this map data, global topology path planning data is generated. To reduce data transmission bandwidth and adapt to edge computing capabilities, the 3D point cloud map is projected and compressed into 2D grid map data containing occupancy probability information, and then distributed to the vehicle-mounted edge computing terminal via 5G or WiFi network. Next, the real-time pose calculation and local path planning calculation in the positioning module are deployed on the vehicle-mounted edge computing terminal. The vehicle-mounted edge computing terminal uses locally acquired LiDAR point cloud data, inertial navigation data, and cloud-distributed 2D grid map data for real-time scanning and matching to calculate the optimal pose estimation data at the current moment. It then combines locally perceived obstacle data to run a local path planning algorithm, outputting linear velocity v and angular velocity v. The local control command data is then received; subsequently, the motion control module is deployed on the microcontroller unit at the actuator end of the vehicle chassis, directly responding to the control commands output by the edge computing terminal. This module has an embedded differential kinematic model, and its specific operation involves receiving the linear velocity v (in meters per second) and angular velocity from the local control command data. (Unit: radians per second), based on the physical structural parameters of the autonomous vehicle, the data is decoupled and the target rotational speeds of the left and right drive wheels are calculated. The calculation formula is as follows: ,in and The target rotational speeds of the left and right drive wheels are respectively expressed in radians per second. L is the wheel spacing parameter of the unmanned vehicle, expressed in meters, obtained by measuring the vehicle chassis geometry. R is the wheel radius parameter, expressed in meters, obtained by measuring the wheel diameter. This formula is derived from the kinematic principle of differential drive. Its necessity lies in decomposing the overall planned speed of the vehicle into the independent rotational speeds of the two drive wheels. Finally, a closed-loop speed control system is constructed by using a PID control algorithm to regulate the motor voltage in a closed loop. The PID control law formula is as follows: ,in This represents the rotational speed error, expressed in radians per second. The encoder acquires the actual wheel speed in radians per second, and u(t) represents the output motor control voltage or PWM duty cycle in volts or a dimensionless percentage. These are the proportional, integral, and differential coefficients, respectively. The recommended value range is 0.5 to 2.0, and the logic behind this setting is to provide fast response capabilities. The recommended value range is 0.01 to 0.1, used to eliminate static error. The recommended value range is 0.001 to 0.01, which is used to suppress speed overshoot. Through PID control, the actual wheel speed can be made to follow the target speed data quickly and stably, thereby driving the unmanned vehicle to travel along the planned path.

[0029] It should be noted that the interval and threshold sizes are set for ease of comparison. The size of the threshold depends on the amount of sample data and the base number set by those skilled in the art for each set of sample data, as long as it does not affect the proportional relationship between the parameter and the quantized value. Furthermore, the above formulas are all dimensionless calculations, and the formulas are derived from software simulations using a large amount of collected data to obtain the most recent real-world results. The preset parameters in the formulas are set by those skilled in the art according to the actual situation.

[0030] It should be understood that in the various embodiments of this application, the order of the above-mentioned processes does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of this application.

[0031] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

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

Claims

1. A multi-sensor data fusion positioning and navigation system for unmanned vehicles in smart campuses, characterized in that, The system includes a data acquisition module, a preprocessing module, a mapping and positioning module, a path planning module, and a motion control module; The data acquisition module is used to acquire environmental point cloud data output by the vehicle-mounted multi-line lidar, inertial navigation data output by the inertial measurement unit, and satellite positioning data output by the global satellite navigation system in real time. The preprocessing module is configured to receive environmental point cloud data, inertial navigation data and satellite positioning data, synchronize and align the above data according to a preset time reference, and unify the data of each sensor to the vehicle coordinate system based on a pre-calibrated extrinsic matrix, and output the spatiotemporally registered multi-source sensor data stream. The mapping and localization module is used to receive multi-source sensor data streams. In the mapping stage, it uses an improved simultaneous localization and mapping algorithm to generate a 3D point cloud map and convert it into 2D grid map data. In the navigation stage, it adaptively selects the localization mode according to the current environmental characteristics and calculates the optimal pose estimation data of the unmanned vehicle in the 2D grid map data based on the multi-sensor data fusion algorithm. The path planning module generates hybrid navigation data, including global path sequences and local obstacle avoidance trajectories, based on optimal pose estimation data, 2D grid map data, and target point data. The motion control module receives the hybrid navigation data, converts it into chassis motor drive commands through kinematic model calculations, and controls the autonomous vehicle's movement.

2. The multi-sensor data fusion positioning and navigation system for unmanned vehicles in smart campuses as described in claim 1, characterized in that, The mapping and positioning module performs the following data processing steps when generating two-dimensional raster map data: The spatiotemporally registered environmental point cloud data is obtained, and outlier noise data is removed using a hybrid filtering algorithm; Ground point segmentation is performed on the filtered point cloud data based on preset height and slope thresholds to separate ground point cloud data from obstacle point cloud data. The obstacle point cloud data is projected onto a two-dimensional plane, and the point cloud density feature value in each grid cell is statistically analyzed during the projection process. The point cloud density feature value is compared with a preset occupancy probability threshold. When the feature value exceeds the threshold, the corresponding grid is marked as an obstacle occupancy state, and 3D to 2D grid map data containing occupancy state information is generated. The 2D grid map data is configured as the base layer for subsequent navigation path search.

3. The multi-sensor data fusion positioning and navigation system for unmanned vehicles in smart campuses as described in claim 1, characterized in that, The mapping and positioning module includes an environmental state determination unit, which is used to monitor the eigenvalues ​​of the covariance matrix of the satellite positioning data in real time. When the eigenvalue of the covariance matrix is ​​less than the preset accuracy threshold, it is determined to be an outdoor environment mode, and the system activates the first fusion positioning channel based on factor graph optimization. When the eigenvalue of the covariance matrix is ​​greater than or equal to a preset accuracy threshold, it is determined to be an indoor or weak signal environment mode, and the system activates the second fusion positioning channel based on adaptive particle filtering.

4. The multi-sensor data fusion positioning and navigation system for unmanned vehicles in smart campuses as described in claim 3, characterized in that, The data processing procedure of the first fusion positioning channel includes: Construct a factor graph model containing the pose vertices of the autonomous vehicle; The satellite positioning data is added as an absolute position constraint factor to the factor graph model; The inertial navigation data is pre-integrated to generate relative motion constraint factors, which are then added to the factor graph model. The environmental point cloud data is scanned and matched with the three-dimensional point cloud map to generate a laser odometry constraint factor, which is then added to the factor graph model. By minimizing the joint error function of all factors, the pose vertices in the factor graph model are solved nonlinearly, and the high-precision outdoor pose data is output as the optimal pose estimation data.

5. The multi-sensor data fusion positioning and navigation system for unmanned vehicles in smart campuses as described in claim 3, characterized in that, The data processing procedure of the second fusion positioning channel includes: Initialize the particle set, and use the inertial navigation data as prediction input to update the prior pose distribution of each particle in the particle set; The environmental point cloud data of the current frame is matched with the two-dimensional raster map data in the likelihood domain, and the weight value of each particle is calculated. Resampling is performed based on particle weight values ​​to remove low-weight particles and duplicate high-weight particles. The weighted average of the resampled particle set is calculated, and the indoor pose estimation data is output as the optimal pose estimation data. The indoor pose estimation data and the inertial navigation data form a closed-loop correction relationship.

6. The multi-sensor data fusion positioning and navigation system for unmanned vehicles in smart campuses as described in claim 1, characterized in that, The route planning module includes a global route planning unit, which performs the following operations: In the two-dimensional grid map data, starting from the current optimal pose estimation data and ending at the target point data, the improved A* algorithm is used to search and generate initial path data containing a series of discrete waypoints. Extract the turning key point data from the initial path data, and use the Bézier curve algorithm to perform smooth interpolation calculations on the road segments before and after the key point data; Generate globally optimal path data with continuous curvature and transmit this data to the local path planning unit.

7. The multi-sensor data fusion positioning and navigation system for unmanned vehicles in smart campuses as described in claim 6, characterized in that, The route planning module further includes a local route planning unit, which performs the following operations: Based on the environmental point cloud data, a local cost map is constructed, which reflects the distribution of dynamic and static obstacles around the unmanned vehicle in real time. Based on the dynamic window algorithm, multiple sets of simulated velocity pairs are sampled in the velocity space, and the corresponding multiple predicted trajectory data are deduced. An evaluation function is constructed to score each predicted trajectory data. The evaluation function comprehensively considers the degree of fit between the trajectory and the global optimal path data, the distance to obstacles in the local cost map data, and the current driving speed. Select the velocity pair corresponding to the highest-scoring predicted trajectory data, and output local control command data containing linear velocity and angular velocity.

8. The multi-sensor data fusion positioning and navigation system for unmanned vehicles in smart campuses as described in claim 1, characterized in that, When performing time-point synchronization alignment, the preprocessing module uses the acquisition and transmission time of the environmental point cloud data as the reference time axis. For the inertial navigation data with high frequency, the inertial data value corresponding to the reference time axis moment is calculated by linear interpolation. For the satellite positioning data with a low frequency, the nearest neighbor matching method is used to associate the satellite positioning data frame that is closest to the reference time axis. The synchronized data is packaged into multi-sensor data frames with a unified time label to ensure that the data processed by the subsequent fusion algorithm is in the same time segment.

9. The multi-sensor data fusion positioning and navigation system for unmanned vehicles in smart campuses as described in claim 1, characterized in that, This system adopts a cloud-edge-device collaborative architecture for data interaction and processing: The 3D point cloud map construction and global path planning calculation in the mapping and positioning module are deployed on a cloud server. The cloud computing power is used to process large-scale map data optimization tasks, and the optimized global map data is distributed. The real-time pose calculation and local path planning calculation in the positioning module are deployed on the vehicle edge computing terminal. The vehicle edge computing terminal uses locally acquired sensor data and global map data sent from the cloud to perform real-time matching and obstacle avoidance decisions. The motion control module is deployed on the vehicle chassis actuator and directly responds to the control commands output by the edge computing terminal.

10. The multi-sensor data fusion positioning and navigation system for unmanned vehicles in smart campuses as described in claim 9, characterized in that, The motion control module has a built-in differential kinematics model that decouples the linear velocity and angular velocity in the local control command data. Based on the wheel spacing and wheel radius parameters of the unmanned vehicle, the target rotational speed data of the left and right drive wheels are calculated; By using a PID control algorithm to adjust the motor voltage in a closed loop, the actual wheel speed is made to follow the target speed data, thereby driving the unmanned vehicle to travel along the planned path.

Images