Method and system for generating terrain elevation map based on radial basis function fitting

Abstract

The present invention provides a method and system for generating terrain elevation maps based on radial basis function fitting, comprising: receiving historical point cloud information and odometer information, generating a local point cloud map, and downsampling; dynamically generating the center point of the RBF based on the downsampled local point cloud map and the ground fitting area; calculating a kernel matrix based on the downsampled local point cloud map and the center point, iteratively calculating the weight of the center point through GPU parallel acceleration, and generating a terrain manifold; and outputting an elevation map based on the terrain manifold through GPU parallel acceleration. The present invention significantly improves fitting speed, ensures real-time elevation map generation, and significantly increases map generation speed; retains static obstacle elevation information while updating dynamic obstacle elevation information, providing perception information for intelligent wheelchair movement, navigation, and obstacle avoidance; and can output elevation maps, gradient maps, and normal vector maps of arbitrary resolution.

Description

Technical Field

[0001] The present invention belongs to the technical field of positioning and environmental perception, and in particular, relates to a method and system for generating a terrain elevation map based on radial basis function fitting. Background Art

[0002] With the widespread use of smart wheelchairs in healthcare, the need for navigation is becoming increasingly prominent, especially in complex environments such as hospitals, nursing homes, and rehabilitation facilities. Smart wheelchairs must possess precise, safe, and efficient autonomous navigation capabilities when assisting with patient care. To achieve this, smart wheelchairs must accurately perceive and understand the structure of their surroundings, particularly the elevation of the ground and near-ground obstacles. This is crucial for safe navigation in complex indoor and outdoor scenarios.

[0003] Currently, robotic positioning and environmental perception technologies (such as Simultaneous Localization and Mapping (SLAM)) are widely used in autonomous vehicles and drones. However, due to the unique application scenarios of smart wheelchairs, such as the diverse indoor floor materials, complex and changing outdoor terrain, and the frequent presence of people and equipment in the environment, traditional two-dimensional navigation maps are no longer sufficient for the precise environmental perception required by smart wheelchairs. Therefore, how to efficiently generate accurate, stable, and real-time terrain elevation maps has become a key issue in smart wheelchair autonomous navigation systems.

[0004] Existing methods for generating terrain elevation maps can be categorized into two main categories: grid-based methods and probabilistic model-based methods. Grid-based methods, such as digital elevation models (DEMs), typically divide a spatial area into regular grid cells and generate grid elevation information by projecting and statistically processing point cloud data. While this method offers computational simplicity and high real-time performance, it suffers from fixed resolution and limited accuracy, making it difficult to adapt to rapidly changing local environments.

[0005] Patent document "Positioning Method for Autonomous Mobile Devices Based on Dynamic Loading of Point Cloud Maps" (CN113375664B) discloses a positioning method for autonomous mobile devices based on dynamic loading of point cloud maps. This method eliminates positioning deviations caused by human error during carrier movement and matching, improves initial loading speed, and ensures that the inspection robot's movement and matching speeds remain stable during movement. However, the loam algorithm used in this method lacks optimization for z-axis error accumulation, making it prone to accumulating height errors in complex scenarios, resulting in sudden changes in z-axis height during map construction.

[0006] Another type of method is based on probabilistic models, such as Gaussian processes (GP) or kernel functions. These methods use probabilistic inference to continuously estimate ground elevation information from sparse or irregular point cloud data. While these methods offer high accuracy and adaptability to sparse data, they are computationally complex and struggle to guarantee real-time performance.

[0007] Furthermore, with the widespread adoption of LiDAR and vision sensors, the volume of point cloud data has increased dramatically, placing higher demands on real-time algorithms and computational efficiency. Existing methods often rely on serial computations within the central processing unit (CPU), which cannot meet the high-frequency, high-precision terrain perception requirements of smart wheelchairs. Therefore, effectively utilizing high-performance computing devices like graphics processing units (GPUs) to accelerate the generation of terrain elevation maps has become a key research direction for improving the environmental perception performance of smart wheelchairs.

[0008] The patent document "A Multi-Story Indoor Positioning Method Based on a Radial Basis Function Network" (CN113543026A) discloses a method for height positioning using radial basis functions as a network model. Specifically, it uses deep learning to infer a robot's current floor in a multi-story building. However, the weights of the radial basis functions are calculated using the gradient backpropagation principle of the deep learning network model, which does not reduce the Z-axis error during positioning.

[0009] In view of the shortcomings of the above-mentioned existing technologies, a terrain elevation map generation method based on RBF radial basis function and GPU parallel acceleration is proposed to achieve high-precision and high-efficiency terrain perception, providing reliable support for the navigation and obstacle avoidance of smart wheelchairs in complex environments. Summary of the Invention

[0010] In view of the defects in the prior art, the purpose of the present invention is to provide a method and system for generating a terrain elevation map based on radial basis function fitting.

[0011] According to the present invention, a method for generating a terrain elevation map based on radial basis function fitting is provided, comprising:

[0012] Step S1: Receive historical point cloud information and odometer information, generate a local point cloud map and downsample;

[0013] Step S2: dynamically generate the center point of the RBF based on the downsampled local point cloud map and the ground fitting area;

[0014] Step S3: Calculate the kernel matrix based on the downsampled local point cloud map and the center point, iteratively calculate the weight of the center point through GPU parallel acceleration, and generate the terrain manifold;

[0015] Step S4: Output the elevation map through GPU parallel acceleration calculation based on the terrain manifold.

[0016] Preferably, in step S1, the historical point cloud data and odometer information of the most recent N consecutive frames are received, fused, and downsampled.

[0017] The point cloud data includes three-dimensional position points of environmental objects in a laser radar coordinate system.

[0018] The odometer information includes real-time position information of the target in the world coordinate system.

[0019] The downsampling process uses a voxel filtering method to obtain a downsampled local point cloud map .

[0020] In step S2, the local point cloud map after downsampling is obtained. Extract the two-dimensional plane coordinates of the points corresponding to the X-axis and the Y-axis from the i-th point in .

[0021] The ground fitting area is a rectangular area , the fitting resolution is δ.

[0022] Traverse the ground fitting area in steps of δ and enumerate the coordinate positions of all two-dimensional plane points in the fitting area , the radius search algorithm of KD tree is used to filter with radius r, and the center points that pass the screening are recorded as the set ;

[0023] Traverse all center points If there are historical weight estimation results, the historical estimation results are loaded as weight priors. If it is a new point, the weight is initialized to zero.

[0024] in, 、 Respectively represent the x-axis and y-axis coordinates of the two-dimensional coordinates of the k-th point;

[0025] 、 Respectively represent the minimum and maximum x-axis boundary values of the ground fitting matrix;

[0026] 、 Respectively represent the minimum and maximum y-axis boundary values of the ground fitting matrix;

[0027] The spatial coordinates of the center point and the corresponding weight are stored and indexed through a hash structure.

[0028] Preferably, in step S3, GPU parallel computing is used, and the radial basis function kernel is:

[0029]

[0030]

[0031] For any point in the ground fitting area , calculate the predicted height:

[0032]

[0033] The kernel matrix of each point cloud point and the center point in the downsampled local point cloud map is :

[0034]

[0035] in, Represents the preset radial basis function;

[0036] Represents the two-dimensional plane distance in Euclidean space;

[0037] Represents the two-dimensional coordinates of the point cloud points in the local point cloud map after downsampling;

[0038] Represents the local point cloud map after downsampling The i-th point cloud point in ;

[0039] Represents the two-dimensional coordinates of the j-th center point;

[0040] K represents the total number of center points;

[0041] σ represents the bandwidth parameter of the kernel function;

[0042] Represents the weight corresponding to the j-th center point;

[0043] Represents the element in the i-th row and j-th column of the kernel matrix A.

[0044] The calculation of the Euclidean distance between the point cloud points and the center point and the kernel matrix is run simultaneously on multiple cores on the GPU. The elevation map information is updated according to the historical elevation map prior to obtain the terrain manifold.

[0045] The weight corresponding to the central point is solved and / or updated based on the least squares principle and the Kalman filter principle, taking the weight of the historical estimation result as a priori condition.

[0046] Preferably, the solving and / or updating of the weights includes:

[0047] When estimating the weights for the first time, we receive the first frame of the local point cloud map and the corresponding odometry information and solve the least squares problem:

[0048]

[0049]

[0050]

[0051]

[0052] in, , represents the weight vector of the center point;

[0053] Represents the weight of the i-th center point;

[0054] z represents the height observation vector of each point in the local point cloud map;

[0055] Represents the local point cloud map after downsampling The Z-axis coordinate of the i-th point in .

[0056] When there are prior conditions, the state transition equation and the observation equation for:

[0057]

[0058]

[0059] Compute the residuals:

[0060]

[0061] in, Indicates the status of the previous round of estimation;

[0062] represents the covariance matrix;

[0063] Q represents the process noise covariance matrix;

[0064] I represents the identity matrix;

[0065] Represents the local point cloud map after downsampling at time t Observation information of midpoint cloud height.

[0066] Calculate the Kalman gain matrix :

[0067]

[0068] Update weights and weight covariance based on Kalman gain matrix and residual :

[0069]

[0070]

[0071] Finally, the weight of the center point is updated The corresponding center point is stored through a hash structure;

[0072] in, represents the covariance of the observation equation.

[0073] Preferably, in step S4, the elevation estimate is calculated using a radial basis function fitting formula according to the weight of the center point:

[0074]

[0075]

[0076] Where K represents the total number of center points;

[0077] Represents the preset radial basis function;

[0078] Indicates the corresponding center point The weight of

[0079] Represents the two-dimensional coordinates of the j-th center point;

[0080] A two-dimensional raster grid representing the area covered by the output elevation map;

[0081] Represents the coordinates of the center point of the grid at row m and column n.

[0082] All grid points are divided into thread blocks. Each thread in the GPU's CUDA is assigned a grid point. The elevation estimate of each grid center point is calculated independently and in parallel and recorded in the form of a two-dimensional array to output the elevation map.

[0083] According to the present invention, a terrain elevation map generation system based on radial basis function fitting is provided, comprising:

[0084] The first module receives historical point cloud information and odometer information, generates a local point cloud map and downsamples it;

[0085] The second module dynamically generates the center point of the RBF based on the downsampled local point cloud map and the ground fitting area;

[0086] The third module calculates the kernel matrix based on the downsampled local point cloud map and the center point, and uses GPU parallel acceleration to iteratively calculate the weight of the center point to generate the terrain manifold;

[0087] The fourth module outputs the elevation map through GPU parallel acceleration calculation based on the terrain manifold.

[0088] Preferably, the first module receives the most recent N consecutive frames of historical point cloud data and odometer information, performs fusion processing, and performs downsampling processing.

[0089] The point cloud data includes three-dimensional position points of environmental objects in a laser radar coordinate system.

[0090] The odometer information includes real-time position information of the target in the world coordinate system.

[0091] The downsampling process uses a voxel filtering method to obtain a downsampled local point cloud map .

[0092] The second module is based on the downsampled local point cloud map Extract the two-dimensional plane coordinates of the points corresponding to the X-axis and the Y-axis from the i-th point in .

[0093] The ground fitting area is a rectangular area , the fitting resolution is δ.

[0094] Traverse the ground fitting area in steps of δ and enumerate the coordinate positions of all two-dimensional plane points in the fitting area , the radius search algorithm of KD tree is used to filter with radius r, and the center points that pass the screening are recorded as the set ;

[0095] Traverse all center points If there are historical weight estimation results, the historical estimation results are loaded as weight priors. If it is a new point, the weight is initialized to zero.

[0096] in, 、 Respectively represent the x-axis and y-axis coordinates of the two-dimensional coordinates of the k-th point;

[0097] 、 Respectively represent the minimum and maximum x-axis boundary values of the ground fitting matrix;

[0098] 、 Respectively represent the minimum and maximum y-axis boundary values of the ground fitting matrix;

[0099] The spatial coordinates of the center point and the corresponding weight are stored and indexed through a hash structure.

[0100] Preferably, the third module uses GPU parallel computing, and the radial basis function kernel is:

[0101]

[0102]

[0103] For any point in the ground fitting area , calculate the predicted height:

[0104]

[0105] The kernel matrix of each point cloud point and the center point in the downsampled local point cloud map is :

[0106]

[0107] in, Represents the preset radial basis function;

[0108] Represents the two-dimensional plane distance in Euclidean space;

[0109] Represents the two-dimensional coordinates of the point cloud points in the downsampled local point cloud map;

[0110] Represents the local point cloud map after downsampling The i-th point cloud point in ;

[0111] Represents the two-dimensional coordinates of the j-th center point;

[0112] K represents the total number of center points;

[0113] σ represents the bandwidth parameter of the kernel function;

[0114] Represents the weight corresponding to the j-th center point;

[0115] Represents the element in the i-th row and j-th column of the kernel matrix A.

[0116] The calculation of the Euclidean distance between the point cloud points and the center point and the kernel matrix is run simultaneously on multiple cores on the GPU. The elevation map information is updated according to the historical elevation map prior to obtain the terrain manifold.

[0117] The weight corresponding to the central point is solved and / or updated based on the least squares principle and the Kalman filter principle, taking the weight of the historical estimation result as a priori condition.

[0118] Preferably, the solving and / or updating of the weights includes:

[0119] When estimating the weights for the first time, we receive the first frame of the local point cloud map and the corresponding odometry information and solve the least squares problem:

[0120]

[0121]

[0122]

[0123]

[0124] in, , represents the weight vector of the center point;

[0125] Represents the weight of the i-th center point;

[0126] z represents the height observation vector of each point in the local point cloud map;

[0127] Represents the local point cloud map after downsampling The Z-axis coordinate of the i-th point in .

[0128] When there are prior conditions, the state transition equation and the observation equation for:

[0129]

[0130]

[0131] Compute the residuals:

[0132]

[0133] in, Indicates the status of the previous round of estimation;

[0134] represents the covariance matrix;

[0135] Q represents the process noise covariance matrix;

[0136] I represents the identity matrix;

[0137] Represents the local point cloud map after downsampling at time t Observation information of midpoint cloud height.

[0138] Calculate the Kalman gain matrix :

[0139]

[0140] Update weights and weight covariance based on Kalman gain matrix and residual :

[0141]

[0142]

[0143] Finally, the weight of the center point is updated The corresponding center point is stored through a hash structure;

[0144] in, represents the covariance of the observation equation.

[0145] Preferably, in the fourth module, the elevation estimate is calculated using a radial basis function fitting formula according to the weight of the center point:

[0146]

[0147]

[0148] Where K represents the total number of center points;

[0149] Represents the preset radial basis function;

[0150] Indicates the corresponding center point The weight of

[0151] Represents the two-dimensional coordinates of the j-th center point;

[0152] A two-dimensional raster grid representing the area covered by the output elevation map;

[0153] Represents the coordinates of the center point of the grid at row m and column n.

[0154] All grid points are divided into thread blocks. Each thread in the GPU's CUDA is assigned a grid point. The elevation estimate of each grid center point is calculated independently and in parallel and recorded in the form of a two-dimensional array to output the elevation map.

[0155] Compared with the prior art, the present invention has the following beneficial effects:

[0156] 1. The present invention uses RBF to quickly fit the lidar point cloud data, and significantly improves the fitting speed through GPU parallel computing, ensuring the real-time generation of elevation maps.

[0157] 2. Based on the Kalman filter principle, the present invention updates the current elevation map information according to the historical elevation map prior, retains the elevation information of static obstacles while updating the elevation information of dynamic obstacles, and provides perception information for the movement, navigation and obstacle avoidance of smart wheelchairs.

[0158] 3. The present invention can output elevation maps, gradient maps and normal vector maps of any resolution, and when there are sufficient hardware CUDA cores, parallel computing can be achieved through GPU acceleration, greatly improving the map generation speed. BRIEF DESCRIPTION OF THE DRAWINGS

[0159] Other features, objects and advantages of the present invention will become more apparent upon reading the detailed description of non-limiting embodiments with reference to the following drawings:

[0160] Figure 1 Schematic diagram of the flow of the terrain elevation map generation method based on radial basis function fitting. DETAILED DESCRIPTION

[0161] The present invention will be described in detail below with reference to specific embodiments. The following examples will help those skilled in the art to further understand the present invention, but are not intended to limit the present invention in any form. It should be noted that, for those skilled in the art, several changes and improvements can be made without departing from the scope of the present invention. These all fall within the scope of protection of the present invention.

[0162] The present invention provides a method for generating terrain elevation maps based on radial basis function (RBF) fitting, which can provide real-time and accurate elevation information of the surrounding environment during the movement of the smart wheelchair, detect and effectively identify static and dynamic obstacles in the environment, so as to achieve efficient and accurate terrain perception of the smart wheelchair in complex environments, and provide strong perception information support for the movement, navigation and obstacle avoidance of the smart wheelchair. Figure 1 For example, the following steps are included:

[0163] Step S1: Receive the most recent multiple frames of historical point cloud information and odometer information, generate a local point cloud map, and downsample;

[0164] Specifically, the laser radar receives the most recent multiple frames of historical point cloud data and odometry information, and fuses them to generate a local point cloud map. The data is downsampled to improve computational efficiency, aiming to generate a local point cloud map to characterize the environmental structure near the current position of the smart wheelchair.

[0165] In more preferred examples, the odometer information of the smart wheelchair and the current frame of lidar point cloud data are received, wherein the odometer information provides real-time posture information of the smart wheelchair in the world coordinate system, and the point cloud data records the three-dimensional position of environmental objects in the lidar coordinate system.

[0166] To ensure data consistency, it is necessary to use the current posture of the smart wheelchair to construct a transformation matrix to convert each point in the point cloud data from the radar coordinate system to the world coordinate system.

[0167] Assume that the current time is t, and the odometer information of the smart wheelchair at this time is composed of the position vector and Euler angle, which can be expressed as:

[0168]

[0169] Among them, the position vector t t Represents the translation at time t, Euler angle They represent the rotation angles around the x, y, and z axes at time t, respectively. 、 、 Represents the position vectors of x, y, and z axes at time t. Convert Euler angles to rotation matrices :

[0170]

[0171] in, Indicates the rotation angle around the x-axis The corresponding rotation matrix, represents the rotation matrix corresponding to the rotation angle θ around the y-axis, represents the rotation matrix corresponding to the rotation angle ψ around the z-axis,

[0172]

[0173] According to the rotation matrix and the translation vector t t Construct the transformation matrix T t :

[0174]

[0175] Assume that the current frame collects M points, then the point cloud of the current frame can be represented as a set, and each element in the set corresponds to the three-dimensional coordinates of the corresponding point in the current frame in the radar coordinate system:

[0176]

[0177] in, represents the set of point clouds at time t, Represents the i-th point cloud sampled at time t.

[0178] Use the rotation matrix and position vector to transform the point in the current frame to the world coordinate system:

[0179]

[0180] Perform the same operation on the most recent N consecutive frames of point cloud in history, which are recorded as:

[0181]

[0182] in, Represents the historical point cloud of the most recent j-th frame.

[0183] By performing spatial transformation and superposition processing on the point cloud data of the most recent consecutive frames in history, a local point cloud map of the area near the location of the smart wheelchair is generated. :

[0184]

[0185] After completing the point cloud conversion and fusion at multiple times, a local point cloud map is formed. There may be problems such as uneven point density or redundant data. To improve the efficiency and stability of subsequent calculations, reduce data redundancy, and improve the efficiency of subsequent calculations, the voxel grid downsampling method is used to downsample the local point cloud map, remove duplicate or overly dense points, and reduce the burden of subsequent data processing.

[0186] Specifically, the entire three-dimensional space is divided into a number of cubic voxels with a side length of l. All points in the local point cloud are traversed, and the voxel unit number to which each point belongs is determined based on the three-dimensional coordinate value of each point. For multiple points contained in the same voxel, only one representative point is retained as the output point of the voxel, and the representative point is taken as the average position of all points in the voxel. After this operation, the original point cloud is compressed into a set of sparse, evenly distributed representative points, forming a downsampled local point cloud map, which is recorded as .

[0187] The downsampling process can effectively preserve the geometric features of the original environment while significantly reducing the number of point clouds and reducing the computational complexity of subsequent processing algorithms.

[0188] Step S2: dynamically generate RBF center points for fitting the ground based on the point cloud distribution of the local point cloud map and the ground fitting area;

[0189] Specifically, according to the downsampled local point cloud map The distribution characteristics of the point cloud and the pre-set ground fitting area are used to dynamically select the center point position of the RBF suitable for fitting.

[0190] In more preferred embodiments, the local point cloud map after downsampling Extract the two-dimensional plane coordinates of the i-th point corresponding to the X-axis and Y-axis from each point in , using downsampled point cloud data to build a KD tree to accelerate spatial search efficiency. Combined with the pre-defined ground fitting area, the KD tree radius search algorithm is used to quickly locate the location where the RBF center point needs to be placed, thereby dynamically generating the RBF center point set for terrain fitting.

[0191] Assume that the predetermined ground fitting area is a rectangular area , and set the fitting resolution to δ. Traverse the entire fitting area with a step size of δ. The rectangular area, resolution, and step size are set according to the hardware platform and actual environment. Enumerate the coordinate positions of all two-dimensional plane points in the fitting area. ,in, Represents the x-axis coordinate of the two-dimensional coordinate of the k-th point, Represents the y-axis coordinate of the k-th point's two-dimensional coordinate. For each candidate position, a radius search is performed within its neighborhood using a KD tree with a radius of r to determine whether there is a sufficient amount of point cloud data around the position.

[0192] If a location If there is a point cloud support nearby, then retain the point as the RBF center point. All the center points that pass the screening are recorded as a set:

[0193]

[0194] After traversing all candidate points, for each center point (a total of K center points), if there is a weight estimation result in the historical iteration, the historical weight is loaded as the weight prior of this round; if it is a new point, its weight is initialized to zero.

[0195] A hash function is used to establish a mapping relationship, efficiently storing and updating the spatial location of each RBF center point and its corresponding weight information in real time. The spatial coordinates and corresponding weights of all center points are stored and indexed using a hash structure to support subsequent parallel acceleration and fast update operations. Finally, the dynamic RBF center point selection and initialization are completed at the current moment.

[0196] Step S3: Calculate the kernel matrix for fitting based on the local point cloud map and the RBF center points. Based on the Kalman filter principle and GPU parallel acceleration, iteratively calculate the weights of the RBF center points to generate the terrain manifold.

[0197] In more preferred examples, the kernel matrix is calculated using the determined RBF center point and local point cloud map data, and the parallel acceleration of the kernel matrix calculation and the matrix calculation in the Kalman filter algorithm is achieved through the graphics processing unit (GPU). Compared with traditional optimization methods (such as sparse kernel techniques, nearest neighbor truncation, iterative approximation, etc.), it can significantly improve the large-scale data processing speed while maintaining accuracy, so as to efficiently solve the weight parameters of the RBF center point, thereby constructing an accurate terrain manifold.

[0198] Specifically, based on the current local point cloud map and the position of the selected RBF center point, the CUDA architecture GPU parallel computing platform is used to quickly and in parallel calculate the radial basis function values between each point cloud point and the RBF center point to construct the kernel matrix at the current moment.

[0199] The radial basis function kernel is defined as:

[0200]

[0201] in, , represents the two-dimensional plane distance in Euclidean space, Represents the two-dimensional coordinates of the predicted point, Indicates the two-dimensional coordinates of the j-th RBF center point, σ is the kernel function bandwidth parameter. In the pre-set fitting area, that is, the ground fitting area, for any point , calculate the predicted height to form the terrain manifold, and the predicted height is expressed as:

[0202]

[0203] in, Indicates the weight value corresponding to the j-th RBF center point.

[0204] Taking into account the characteristics of weight parameters being updated over time, the weights of historical estimation results are used as prior conditions. Based on the Kalman filter principle, parallel computing is performed through efficient matrix computing libraries such as CuBLAS and Cusolver on the CUDA platform to accelerate the implementation of complex matrix operations such as the Kalman gain matrix and covariance update matrix, and the weight parameters of the RBF center point are updated in real time.

[0205] To quickly fit the lidar point cloud data using RBF, in order to obtain a good fitting result, it is necessary to estimate the weight of the RBF center point based on the local point cloud map information. Is the current downsampled local point cloud map The i-th point cloud point in is the RBF center point set No. A center point. There are M points in There are K points in the kernel matrix of the point cloud and RBF center point. , the element in row i and column j of A is:

[0206]

[0207] To accelerate the construction of the kernel matrix, a GPU parallel computing framework was introduced, and CUDA programming was used for parallel computing. Through custom CUDA kernel functions, the calculation of the Euclidean distance between point cloud points and RBF center points and the kernel function value was distributed to multiple cores on the GPU for simultaneous execution. GPUs are suitable for large-scale matrix operations, especially the large number of parallel multiplication and addition operations involved in solving the RBF kernel matrix and weights. While completing all computational tasks in one go, they avoid repeated iterations or redundant structure selection, significantly reducing fitting time. This provides higher real-time performance and scalability, making it suitable for high-performance scenarios such as online mapping.

[0208] Next, we will use the least squares principle and Kalman filter principle to solve the problem in two cases and estimate the weights. Use a vector To express:

[0209]

[0210] When estimating weights for the first time: corresponding to the first frame of the local point cloud map and the corresponding odometry, there is no prior knowledge of the weights, and the least squares problem is solved directly:

[0211]

[0212] in, is the height observation vector of each point in the local point cloud map, that is Corresponding downsampled local point cloud map Middle The Z-axis coordinate of the point. For this least squares problem, the analytical solution is:

[0213]

[0214] When there is a historical prior: use the Kalman filter method to update. Suppose the state of the previous round of estimation is , the covariance matrix is , the state transfer equation and observation equation are as follows:

[0215]

[0216]

[0217] Among them, Q is the process noise covariance matrix, I is the identity matrix, and the uncertainty of the smart wheelchair's own posture estimation during movement is introduced.

[0218] According to the Kalman filter update step, the residual is first calculated:

[0219]

[0220] in, Represents the downsampled local point cloud map at time t The vector composed of the z-axis coordinates of all point cloud points in is the observation information of the point cloud height.

[0221] In the process of generating the elevation map, based on the Kalman filter principle, the current elevation map information is updated according to the historical elevation map prior, the elevation information of static obstacles is retained while the elevation information of dynamic obstacles is updated, and the parallel computing of GPU is used to significantly improve the fitting speed and ensure the real-time generation of the elevation map. Assume that the covariance of the observation equation is , introduced by the point cloud observation noise of the local point cloud map of the current frame. Calculate the Kalman gain matrix:

[0222]

[0223] Update weights and weight covariance based on the Kalman gain matrix and residuals:

[0224]

[0225]

[0226] The final updated β t This is the center point weight estimate at the current moment. The estimated result and the corresponding center point position will be written into the hash structure, and the updated value will be used for subsequent elevation map estimation and iterative processing.

[0227] To accelerate the solution of the weight vector β, a GPU parallel computing framework was introduced, using the parallel linear algebra library in the CUDA programming model to perform matrix multiplication and solve the system of equations. CuBLAS was used to implement kernel matrix and vector multiplication operations, and Cusolver was used to quickly invert and solve the normal equations.

[0228] The updated weights are efficiently stored again using a hash function and serve as the initial conditions for the next iterative update, generating a smooth and continuous terrain elevation fitting manifold. Parallel acceleration is a key enabler for deployment within high-performance computing architectures, significantly enhancing the timeliness of high-frequency updates in dynamic environments.

[0229] Step S4: Output the elevation map near the smart wheelchair through GPU parallel accelerated calculation.

[0230] Specifically, based on the characteristic of radial basis function that it can fit manifolds, radial basis function is used to fit the terrain manifold. Based on the constructed terrain manifold, a detailed elevation map of the area near the smart wheelchair is output through GPU parallel computing.

[0231] In more preferred embodiments, the calculated RBF center point weight parameters are used to calculate the elevation estimate of any location where height information needs to be queried using a radial basis function fitting formula.

[0232] Specifically, after obtaining the radial basis function center point and its weight estimation value at the current moment, a CUDA architecture GPU parallel computing platform is used to generate a terrain elevation map of the area near the smart wheelchair.

[0233] Assume that the area covered by the elevation map to be output is a two-dimensional grid ,in, Represents the coordinates of the grid center point in the mth row and nth column. For each grid point , whose estimated height is given by:

[0234]

[0235] in, is the preset radial basis function, The corresponding center point The weight of the weighted kernel function is used to estimate the terrain elevation of the entire local area continuously and smoothly in space.

[0236] Because elevation map generation involves traversing a large number of grid points and performing weighted kernel calculations, the GPU parallel acceleration framework is also utilized to ensure real-time performance. All grid points are divided into thread blocks, and in CUDA, each thread is assigned a grid point and independently and parallelly computes its estimated height. This GPU parallel computing architecture allows for simultaneous parallel calculation of elevation estimates for multiple query points, significantly improving the real-time performance and efficiency of elevation map generation, meeting the requirements for rapid navigation and dynamic obstacle avoidance for intelligent wheelchairs.

[0237] Finally, the output elevation map records the elevation estimate of each grid center point in the form of a two-dimensional array, forming a local terrain model of the smart wheelchair at its current location. This model is used in subsequent functional modules such as path planning, collision detection, and navigation control, providing basic perception guarantees for the safe and autonomous movement of smart wheelchairs in complex environments.

[0238] The present invention also provides a terrain elevation map generation system based on radial basis function fitting. The terrain elevation map generation system based on radial basis function fitting can be implemented by executing the process steps of the terrain elevation map generation method based on radial basis function fitting. That is, those skilled in the art can understand the terrain elevation map generation method based on radial basis function fitting as a preferred implementation of the terrain elevation map generation system based on radial basis function fitting.

[0239] According to the present invention, a terrain elevation map generation system based on radial basis function fitting is provided, comprising:

[0240] The first module receives historical point cloud information and odometer information, generates a local point cloud map and downsamples it;

[0241] The second module dynamically generates the center point of the RBF based on the downsampled local point cloud map and the ground fitting area;

[0242] The third module calculates the kernel matrix based on the downsampled local point cloud map and the center point, and uses GPU parallel acceleration to iteratively calculate the weight of the center point to generate the terrain manifold;

[0243] The fourth module outputs the elevation map through GPU parallel acceleration calculation based on the terrain manifold.

[0244] In more preferred examples, the first module receives the most recent N consecutive frames of historical point cloud data and odometer information, performs fusion processing, and performs downsampling processing.

[0245] The point cloud data includes three-dimensional position points of environmental objects in a laser radar coordinate system.

[0246] The odometer information includes real-time position information of the target in the world coordinate system.

[0247] The downsampling process uses a voxel filtering method to obtain a downsampled local point cloud map .

[0248] The second module is based on the downsampled local point cloud map Extract the two-dimensional plane coordinates of the points corresponding to the X-axis and the Y-axis from the i-th point in .

[0249] The ground fitting area is a rectangular area , the fitting resolution is δ.

[0250] Traverse the ground fitting area in steps of δ and enumerate the coordinate positions of all two-dimensional plane points in the fitting area , the radius search algorithm of KD tree is used to filter with radius r, and the center points that pass the screening are recorded as the set ;

[0251] Traverse all center points If there are historical weight estimation results, the historical estimation results are loaded as weight priors. If it is a new point, the weight is initialized to zero.

[0252] in, 、 Respectively represent the x-axis and y-axis coordinates of the two-dimensional coordinates of the k-th point;

[0253] 、 Respectively represent the minimum and maximum x-axis boundary values of the ground fitting matrix;

[0254] 、 Respectively represent the minimum and maximum y-axis boundary values of the ground fitting matrix;

[0255] The spatial coordinates of the center point and the corresponding weight are stored and indexed through a hash structure.

[0256] In more preferred embodiments, the third module utilizes GPU parallel computing, and the radial basis function kernel is:

[0257]

[0258]

[0259] For any point in the ground fitting area , calculate the predicted height:

[0260]

[0261] The kernel matrix of each point cloud point and the center point in the downsampled local point cloud map is :

[0262]

[0263] in, Represents the preset radial basis function;

[0264] Represents the two-dimensional plane distance in Euclidean space;

[0265] Represents the two-dimensional coordinates of the point cloud points in the local point cloud map after downsampling;

[0266] Represents the local point cloud map after downsampling The i-th point cloud point in ;

[0267] Represents the two-dimensional coordinates of the j-th center point;

[0268] K represents the total number of center points;

[0269] Represents the kernel function bandwidth parameter;

[0270] Represents the weight corresponding to the j-th center point;

[0271] Represents the element in the i-th row and j-th column of the kernel matrix A.

[0272] The calculation of the Euclidean distance between the point cloud points and the center point and the kernel matrix is run simultaneously on multiple cores on the GPU. The elevation map information is updated according to the historical elevation map prior to obtain the terrain manifold.

[0273] The weight corresponding to the central point is solved and / or updated based on the least squares principle and the Kalman filter principle, taking the weight of the historical estimation result as a priori condition.

[0274] In more preferred embodiments, the solving and / or updating of the weights includes:

[0275] When estimating the weights for the first time, we receive the first frame of the local point cloud map and the corresponding odometry information and solve the least squares problem:

[0276]

[0277]

[0278]

[0279]

[0280] in, , represents the weight vector of the center point;

[0281] Represents the weight of the i-th center point;

[0282] Represents the height observation vector of each point in the local point cloud map;

[0283] Represents the local point cloud map after downsampling The Z-axis coordinate of the i-th point in .

[0284] When there are prior conditions, the state transition equation and the observation equation for:

[0285]

[0286]

[0287] Compute the residuals:

[0288]

[0289] in, Indicates the status of the previous round of estimation;

[0290] represents the covariance matrix;

[0291] Q represents the process noise covariance matrix;

[0292] I represents the identity matrix;

[0293] Represents the local point cloud map after downsampling at time t Observation information of midpoint cloud height.

[0294] Calculate the Kalman gain matrix :

[0295]

[0296] Update weights and weight covariance based on Kalman gain matrix and residual :

[0297]

[0298]

[0299] Finally, the weight of the center point is updated The corresponding center point is stored through a hash structure;

[0300] in, represents the covariance of the observation equation.

[0301] In more preferred embodiments, the fourth module calculates the elevation estimate using a radial basis function fitting formula based on the weight of the center point:

[0302]

[0303]

[0304] Where K represents the total number of center points;

[0305] Represents the preset radial basis function;

[0306] Indicates the corresponding center point The weight of

[0307] Represents the two-dimensional coordinates of the j-th center point;

[0308] A two-dimensional raster grid representing the area covered by the output elevation map;

[0309] Represents the coordinates of the center point of the grid at row m and column n.

[0310] All grid points are divided into thread blocks. Each thread in the GPU's CUDA is assigned a grid point. The elevation estimate of each grid center point is calculated independently and in parallel and recorded in the form of a two-dimensional array to output the elevation map.

[0311] In more preferred examples, verification was conducted on a hardware platform equipped with an Intel Core i5-12600KF processor and an NVIDIA RTX 3080 graphics card. Different region of interest (ROI) sizes and radial basis function (RBF) parameters were set, and the kernel matrix calculation time, weight solution time, total elevation map calculation time, and final fitting accuracy (mean absolute error) were recorded. A comparative analysis of GPU and CPU multi-threading was also conducted. The system GPU acceleration solution provided by the present invention significantly shortened the calculation time, with a minimum acceleration ratio of 24.5 times and a maximum acceleration ratio of more than 120 times. SLAM algorithms such as loam can be introduced into this system to reduce the accumulation of z-axis errors caused by complex environments and improve positioning accuracy by observing the height error of the smart wheelchair.

[0312] In terms of accuracy, the GPU implementation matched the CPU multi-threaded results, with mean absolute errors within millimeters, which did not affect the final elevation fitting accuracy. In particular, in a large-scale, high-density point cloud scenario (ROI 6×6, RBF resolution 0.2m), the GPU was able to complete the elevation map calculation for 251,001 points in just 49.5 milliseconds, compared to 6016.07 milliseconds for the CPU multi-threaded approach, fully demonstrating its significant advantages in real-time and large-scale processing tasks.

[0313] Those skilled in the art will appreciate that, in addition to implementing the system and its various devices, modules, and units provided by the present invention in purely computer-readable program code, it is entirely possible to implement the same functions of the system and its various devices, modules, and units provided by the present invention in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers by logically programming the method steps. Therefore, the system and its various devices, modules, and units provided by the present invention can be considered a hardware component, and the devices, modules, and units included therein for implementing various functions can also be considered as structures within the hardware component; the devices, modules, and units for implementing various functions can also be considered as both software modules implementing the method and structures within the hardware component.

[0314] The above describes specific embodiments of the present invention. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art may make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. The embodiments of this application and the features in the embodiments may be combined with each other in any manner unless there is a conflict.

Claims

1. A method for generating a terrain elevation map based on radial basis function fitting, characterized in that: include: Step S1: Receive historical point cloud information and odometer information, generate a local point cloud map and downsample; Step S2: dynamically generate the center point of the RBF based on the downsampled local point cloud map and the ground fitting area; Step S3: Calculate the kernel matrix based on the downsampled local point cloud map and the center point, iteratively calculate the weight of the center point through GPU parallel acceleration, and generate the terrain manifold; Step S4: Outputting an elevation map through GPU parallel accelerated calculation based on the terrain manifold; In step S3, GPU parallel computing is used, and the radial basis function kernel is: in, Represents the preset radial basis function; d represents the two-dimensional plane distance in Euclidean space; Represents the two-dimensional coordinates of the point cloud points in the local point cloud map after downsampling; σ represents the kernel function bandwidth parameter.

2. The method for generating a terrain elevation map based on radial basis function fitting according to claim 1, wherein: In step S1, the historical point cloud data and odometer information of the most recent N consecutive frames are received, fused, and further downsampled; The point cloud data includes three-dimensional position points of environmental objects in the laser radar coordinate system; The odometer information includes real-time position information of the target in the world coordinate system; The downsampling process uses a voxel filtering method to obtain a downsampled local point cloud map ; In step S2, the local point cloud map after downsampling is obtained. Extract the two-dimensional plane coordinates of the points corresponding to the X-axis and the Y-axis from the i-th point in ; The ground fitting area is a rectangular area , the fitting resolution is δ; Traverse the ground fitting area in steps of δ and enumerate all possible two-dimensional coordinate positions of the center point , the radius search algorithm of KD tree is used to filter with radius r, and the center points that pass the screening are recorded as the set ; Traverse all center points If there are historical weight estimation results, the historical estimation results are loaded as weight priors. If it is a new point, the weight is initialized to zero. in, 、 Respectively represent the minimum and maximum x-axis boundary values of the ground fitting matrix; 、 Respectively represent the minimum and maximum y-axis boundary values of the ground fitting matrix; The spatial coordinates of the center point and the corresponding weight are stored and indexed through a hash structure.

3. The method for generating a terrain elevation map based on radial basis function fitting according to claim 1, wherein: In step S3, any point in the ground fitting area is , calculate the predicted height: The kernel matrix of each point cloud point and the center point in the downsampled local point cloud map is : in, Represents the local point cloud map after downsampling The i-th point cloud point in ; Represents the two-dimensional coordinates of the j-th center point; K represents the total number of center points; Represents the weight corresponding to the j-th center point; Represents the element in the i-th row and j-th column of the kernel matrix A; The calculation of the Euclidean distance between the point cloud points and the center point and the kernel matrix is run simultaneously on multiple cores on the GPU. The elevation map information is updated based on the historical elevation map prior to obtain the terrain manifold; The weight corresponding to the central point is solved and / or updated based on the least squares principle and the Kalman filter principle, taking the weight of the historical estimation result as a priori condition.

4. The method for generating a terrain elevation map based on radial basis function fitting according to claim 3, wherein: The solving and / or updating of the weights includes: When estimating the weights for the first time, we receive the first frame of the local point cloud map and the corresponding odometry information and solve the least squares problem: in, Represents the weight of the i-th center point; z represents the height observation vector of each point in the local point cloud map; Represents the local point cloud map after downsampling The Z-axis coordinate of the i-th point in ; When there are prior conditions, the state transition equation and the observation equation for: Compute the residuals: in, Indicates the status of the previous round of estimation; represents the covariance matrix; Q represents the process noise covariance matrix; I represents the identity matrix; Represents the local point cloud map after downsampling at time t Observation information of midpoint cloud height; Calculate the Kalman gain matrix : Update weights and weight covariance based on Kalman gain matrix and residual : Finally, the weight of the center point is updated The corresponding center point is stored through a hash structure; in, represents the covariance of the observation equation.

5. The method for generating a terrain elevation map based on radial basis function fitting according to claim 1, wherein: In step S4, the radial basis function fitting formula is used to calculate the elevation estimate according to the weight of the center point: Where K represents the total number of center points; Represents the preset radial basis function; Indicates the corresponding center point The weight of Represents the two-dimensional coordinates of the j-th center point; A two-dimensional raster grid representing the area covered by the output elevation map; Represents the coordinates of the grid center point in the mth row and nth column; All grid points are divided into thread blocks. Each thread in the GPU's CUDA is assigned a grid point. The elevation estimate of each grid center point is calculated independently and in parallel and recorded in the form of a two-dimensional array to output the elevation map.

6. A terrain elevation map generation system based on radial basis function fitting, characterized in that: include: Module M1 receives historical point cloud information and odometer information, generates a local point cloud map and downsamples it; Module M2 dynamically generates the center point of the RBF based on the downsampled local point cloud map and the ground fitting area; Module M3 calculates the kernel matrix based on the downsampled local point cloud map and the center point, and iteratively calculates the weight of the center point through GPU parallel acceleration to generate the terrain manifold; Module M4, outputs elevation map through GPU parallel acceleration calculation based on terrain manifold; The module M3 uses GPU parallel computing, and the radial basis function kernel is: in, Represents the preset radial basis function; d represents the two-dimensional plane distance in Euclidean space; Represents the two-dimensional coordinates of the point cloud points in the downsampled local point cloud map; σ represents the kernel function bandwidth parameter.

7. The terrain elevation map generation system based on radial basis function fitting according to claim 6, characterized in that: The module M1 receives the historical point cloud data and odometer information of the most recent N consecutive frames, performs fusion processing, and further performs downsampling processing; The point cloud data includes three-dimensional position points of environmental objects in the laser radar coordinate system; The odometer information includes real-time position information of the target in the world coordinate system; The downsampling process uses a voxel filtering method to obtain a downsampled local point cloud map ; The module M2 is a local point cloud map after downsampling Extract the two-dimensional plane coordinates of the points corresponding to the X-axis and the Y-axis from the i-th point in ; The ground fitting area is a rectangular area , the fitting resolution is δ; Traverse the ground fitting area in steps of δ and enumerate all possible two-dimensional coordinate positions of the center point , the radius search algorithm of KD tree is used to filter with radius r, and the center points that pass the screening are recorded as the set ; Traverse all center points If there are historical weight estimation results, the historical estimation results are loaded as weight priors. If it is a new point, the weight is initialized to zero. in, 、 Respectively represent the minimum and maximum x-axis boundary values of the ground fitting matrix; 、 Respectively represent the minimum and maximum y-axis boundary values of the ground fitting matrix; The spatial coordinates of the center point and the corresponding weight are stored and indexed through a hash structure.

8. The terrain elevation map generation system based on radial basis function fitting according to claim 6, characterized in that: In the module M3, any point in the ground fitting area is , calculate the predicted height: The kernel matrix of each point cloud point and the center point in the downsampled local point cloud map is : in, Represents the local point cloud map after downsampling The Point cloud points; Represents the two-dimensional coordinates of the j-th center point; K represents the total number of center points; Represents the weight corresponding to the j-th center point; Represents the element in the i-th row and j-th column of the kernel matrix A; The calculation of the Euclidean distance between the point cloud points and the center point and the kernel matrix is run simultaneously on multiple cores on the GPU. The elevation map information is updated based on the historical elevation map prior to obtain the terrain manifold; The weight corresponding to the central point is solved and / or updated based on the least squares principle and the Kalman filter principle, taking the weight of the historical estimation result as a priori condition.

9. The terrain elevation map generation system based on radial basis function fitting according to claim 8, characterized in that: The solving and / or updating of the weights includes: When estimating the weights for the first time, we receive the first frame of the local point cloud map and the corresponding odometry information and solve the least squares problem: in, Represents the weight of the i-th center point; z represents the height observation vector of each point in the local point cloud map; Represents the local point cloud map after downsampling The Z-axis coordinate of the i-th point in ; When there are prior conditions, the state transition equation and the observation equation for: Compute the residuals: in, Indicates the status of the previous round of estimation; represents the covariance matrix; Q represents the process noise covariance matrix; I represents the identity matrix; Represents the local point cloud map after downsampling at time t Observation information of midpoint cloud height; Calculate the Kalman gain matrix : Update weights and weight covariance based on Kalman gain matrix and residual : Finally, the weight of the center point is updated The corresponding center point is stored through a hash structure; in, represents the covariance of the observation equation.

10. The terrain elevation map generation system based on radial basis function fitting according to claim 6, characterized in that: In the module M4, the radial basis function fitting formula is used to calculate the elevation estimate according to the weight of the center point: Where K represents the total number of center points; Represents the preset radial basis function; Indicates the corresponding center point The weight of Represents the two-dimensional coordinates of the j-th center point; A two-dimensional raster grid representing the area covered by the output elevation map; Represents the coordinates of the grid center point in the mth row and nth column; All grid points are divided into thread blocks. Each thread in the GPU's CUDA is assigned a grid point. The elevation estimate of each grid center point is calculated independently and in parallel and recorded in the form of a two-dimensional array to output the elevation map.

Images