Radial basis function fitting-based terrain elevation map generation method and system

Abstract

The invention provides a terrain elevation map generation method and system based on radial basis function fitting, and the method comprises the steps: receiving historical point cloud information and odometer information, generating a local point cloud map, and carrying out the downsampling; dynamically generating a center point of the RBF based on the down-sampled local point cloud map and the ground fitting area; calculating a kernel matrix according to the down-sampled local point cloud map and the central point, and iteratively calculating the weight of the central point through GPU parallel acceleration to generate a terrain manifold; and calculating and outputting an elevation map through GPU parallel acceleration according to the terrain manifold. According to the method, the fitting speed is remarkably increased, the real-time performance of elevation map generation is ensured, and the map generation speed is greatly increased; the elevation information of the dynamic obstacle is updated while the elevation information of the static obstacle is reserved, and perception information is provided for movement, navigation and obstacle avoidance of the intelligent wheelchair; and an elevation map, a gradient map and a normal vector map with any resolution can be output.

Description

Technical Field

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

[0002] With the wide application of intelligent wheelchairs in the field of healthcare, especially the increasing demand for navigation in complex environments such as hospitals, nursing homes, and rehabilitation institutions. When performing assisted patient care tasks, intelligent wheelchairs must possess accurate, safe, and efficient autonomous navigation capabilities. To achieve this goal, intelligent wheelchairs need to accurately perceive and understand the structure of their surrounding environment, especially the elevation information of the ground and near-ground obstacles, which is crucial for the safe movement of intelligent wheelchairs in complex indoor and outdoor scenarios.

[0003] Currently, robot positioning and environmental perception technologies (such as Simultaneous Localization and Mapping, SLAM) have been widely applied in the fields of autonomous driving vehicles and drones. However, due to the particularity of the application scenarios of intelligent wheelchairs, such as diverse indoor floor materials, complex and variable outdoor terrains, and the frequent presence of dynamic personnel and equipment in the environment, traditional two-dimensional navigation maps can no longer meet the requirements of intelligent wheelchairs for fine environmental perception. Therefore, how to efficiently generate accurate, stable, and real-time terrain elevation maps has become one of the important issues for the autonomous navigation system of intelligent wheelchairs.

[0004] In the prior art, the methods for generating terrain elevation maps can be mainly divided into two categories: grid-based methods and probability model-based methods. Grid-based methods, such as Digital Elevation Model (DEM), usually divide the spatial area into regular grid cells, and generate the elevation information of the grid by projecting and statistically processing point cloud data. This method is simple to calculate and has high real-time performance, but it has problems such as fixed resolution and limited accuracy, and it is difficult to meet the needs of rapid changes in the local environment.

[0005] The patent document "Autonomous Mobile Device Positioning Method Based on Dynamic Loading of Point Cloud Map" (CN113375664B) discloses an autonomous mobile device positioning method based on dynamic loading of point cloud map, which eliminates the positioning deviation caused by human error during the movement matching of the carrier, improves the initial loading speed, and ensures that the movement speed and matching speed of the inspection robot remain stable during the movement process. However, the loam algorithm it uses lacks optimization for the accumulation of z-axis errors, and it is easy to accumulate height errors in relatively complex scenarios, resulting in sudden changes in the z-axis height of map construction.

[0006] Another type is the method based on probability models, such as Gaussian Process (GP) or kernel function method, which continuously estimates the elevation information of the ground from sparse or irregular point cloud data through probability inference methods. Although this method has high accuracy and strong adaptability to sparse data, its computational complexity is high and it is difficult to ensure real-time performance.

[0007] In addition, with the popularization of lidar and vision sensors, the amount of point cloud data has increased sharply, posing higher requirements for the real-time performance and computational efficiency of algorithms. Existing methods often rely on the central processing unit (CPU) for serial computing and cannot meet the real-time requirements of high-frequency and high-precision terrain perception for intelligent wheelchairs. Therefore, how to effectively utilize high-performance computing devices such as the graphics processing unit (GPU) to accelerate the generation process of terrain elevation maps has become an important research direction for improving the environmental perception performance of intelligent wheelchairs.

[0008] The patent document "A Multi-Floor Indoor Positioning Method Based on Radial Basis Function Network" (CN113543026A) discloses a method for positioning floors by using a radial basis function as a network model for height positioning, specifically for inferring the current floor where the robot is located in a multi-story building through deep learning. However, the weights of its radial basis function are solved through the gradient backpropagation principle of the deep learning network model, and it cannot reduce the Z-axis error in the positioning process.

[0009] In view of the above deficiencies of the existing technologies, a method for generating a terrain elevation map based on RBF radial basis function and accelerated by GPU parallelization is proposed to achieve high-precision and high-efficiency terrain perception and provide reliable support for the navigation and obstacle avoidance of intelligent wheelchairs in complex environments. Summary of the Invention

[0010] Aiming at the defects in the existing technologies, 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] A method for generating a terrain elevation map based on radial basis function fitting according to the present invention includes: Step S1, receiving historical point cloud information and odometer information, generating a local point cloud map and downsampling it; Step S2, dynamically generating the center points of RBF based on the downsampled local point cloud map and the ground fitting area; Step S3, calculating the kernel matrix according to the downsampled local point cloud map and the center points, and iteratively calculating the weights of the center points through GPU parallel acceleration to generate a terrain manifold; Step S4, calculating and outputting an elevation map through GPU parallel acceleration according to the terrain manifold.

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

[0013] The point cloud data includes the three-dimensional position points of environmental objects in the lidar coordinate system.

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

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

[0016] In step S2, from the downsampled local point cloud map the two-dimensional plane coordinates of the points corresponding to the X and Y axes are extracted from the i-th point 。

[0017] The ground fitting area is a rectangular area with a fitting resolution of δ.

[0018] The ground fitting area is traversed in steps of δ, and the coordinate positions of all two-dimensional plane points within the fitting area are enumerated and screened using the radius search algorithm of the KD tree with a radius r. The central points that pass the screening are recorded as a set ; All central points are traversed If there is a historical estimation result with a weight, the historical estimation result is loaded as a weight prior. If it is a new point, the weight is initialized to zero; Among them, 、 respectively represent the x and y axis coordinates of the two-dimensional coordinates of the k-th point; 、 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 central points and the corresponding weights are stored and indexed through a hash structure.

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

[0020]

[0021] For any point within the ground fitting area, calculate the predicted height:

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

[0023] Among them, represents a preset radial basis function; represents the two-dimensional plane distance in Euclidean space; represents the two-dimensional coordinates of the point cloud point in the downsampled local point cloud map; represents the downsampled local point cloud map 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 kernel function bandwidth parameter; 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.

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

[0025] The weight corresponding to the center point is solved and / or updated with the weight of the historical estimation result as the prior condition, based on the least squares principle and the Kalman filtering principle.

[0026] Preferably, the solution and / or update of the weight includes: When estimating the weight for the first time, the first frame of local point cloud map and the corresponding odometer information are received, and the least squares problem is solved:

[0027]

[0028]

[0029]

[0030] Among them, , represents the weight vector of the center point; 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 downsampled local point cloud map and is the Z-axis coordinate of the i-th point in it.

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

[0032]

[0033] Calculate the residual:

[0034] where, represents the state estimated in the previous round; represents the covariance matrix; Q represents the process noise covariance matrix; I represents the identity matrix; represents the observed information of the point cloud height in the downsampled local point cloud map at time t in it.

[0035] Calculate the Kalman gain matrix :

[0036] Update the weight and weight covariance according to the Kalman gain matrix and the residual :

[0037]

[0038] Finally, update to obtain the weight of the center point and store the corresponding center point through a hash structure; where, represents the covariance of the observation equation.

[0039] Preferably, in step S4, according to the weight of the center point, use the radial basis function fitting formula to calculate the elevation estimation value:

[0040]

[0041] where, K represents the total number of center points; represents the preset radial basis function; Indicates the corresponding center point weight; Indicates the two-dimensional coordinates of the j-th center point; Indicates the two-dimensional grid grid covering the area of the output elevation map; Indicates the grid center point coordinates of the m-th row and n-th column.

[0042] Divide all grid points by thread blocks, assign one grid point to each thread in the CUDA of the GPU, independently and parallelly calculate the elevation estimation value of each grid center point and record it in the form of a two-dimensional array, and output the elevation map.

[0043] A terrain elevation map generation system based on radial basis function fitting provided by the present invention includes: The first module receives historical point cloud information and odometer information, generates a local point cloud map and downsamples it; The second module dynamically generates the center points of the RBF based on the downsampled local point cloud map and the ground fitting area; The third module calculates the kernel matrix according to the downsampled local point cloud map and the center points, accelerates it in parallel through the GPU, iteratively calculates the weights of the center points, and generates a terrain manifold; The fourth module calculates and outputs the elevation map through parallel acceleration of the GPU according to the terrain manifold.

[0044] Preferably, in the first module, the historical point cloud data of the most recent continuous N frames and the odometer information are received, fused, and downsampled.

[0045] The point cloud data includes the three-dimensional position points of environmental objects in the lidar coordinate system.

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

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

[0048] In the second module, the two-dimensional plane coordinates of the points corresponding to the X-axis and Y-axis are extracted from the i-th point in the downsampled local point cloud map . .

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

[0050] Traverse the ground fitting area at a step size of δ, and enumerate the coordinate positions of two-dimensional plane points within all fitting areas , and use the radius search algorithm of the KD tree to screen with a radius r, and record the filtered center points as a set ; Traverse all center points , if there is a historical estimation result of the weight, load the historical estimation result as the weight prior, and if it is a newly added point, initialize the weight to zero; Among them, , respectively represent the x and y axis coordinates of the two-dimensional coordinates of the kth point; , 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; Store and index the spatial coordinates of the center points and the corresponding weights through a hash structure.

[0051] Preferably, GPU parallel computing is used in the third module, and the radial basis function kernel is:

[0052]

[0053] For any point within the ground fitting area, calculate the predicted height:

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

[0055] Among them, represents a preset radial basis function; represents the two-dimensional plane distance in the Euclidean space; represents the two-dimensional coordinates of the point cloud point in the downsampled local point cloud map; represents the downsampled local point cloud map the ith point cloud point in; represents the two-dimensional coordinates of the jth center point; K represents the total number of center points; σ represents the kernel function bandwidth parameter; denotes the weight corresponding to the j-th center point; denotes the element at the i-th row and j-th column of the kernel matrix A.

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

[0057] The weight corresponding to the center point is solved and / or updated with the weight of the historical estimation result as the prior condition, based on the least squares principle and the Kalman filtering principle.

[0058] Preferably, the solution and / or update of the weight includes: When initially estimating the weight, the first frame of the local point cloud map and the corresponding odometer information are received, and the least squares problem is solved:

[0059]

[0060]

[0061]

[0062] where , denotes the weight vector of the center point; denotes the weight of the i-th center point; z denotes the height observation vector of each point in the local point cloud map; denotes the downsampled local point cloud map the Z-axis coordinate of the i-th point in.

[0063] When there is a prior condition, the state transition equation and the observation equation are:

[0064]

[0065] Calculate the residual:

[0066] where denotes the state of the previous estimate; denotes the covariance matrix; Q denotes the process noise covariance matrix; I denotes the identity matrix; Represents the local point cloud map after downsampling at time t Observation information of the midpoint cloud height

[0067] Calculate the Kalman gain matrix :

[0068] Update the weights and weight covariance according to the Kalman gain matrix and the residual :

[0069]

[0070] Finally, update to obtain the weight of the center point And the corresponding center points are stored through a hash structure; Among them, Represents the covariance of the observation equation

[0071] Preferably, in the fourth module, according to the weight of the center point, the elevation estimation value is calculated by using the radial basis function fitting formula:

[0072]

[0073] Where K represents the total number of center points; Represents the preset radial basis function; Represents the corresponding center point Of the weight; Represents the two-dimensional coordinates of the j-th center point; Represents the two-dimensional grid grid of the output elevation map coverage area; Represents the grid center point coordinates of the m-th row and n-th column

[0074] Divide all grid points by thread blocks, allocate one grid point to each thread in CUDA of the GPU, independently and parallelly calculate the elevation estimation value of each grid center point and record it in the form of a two-dimensional array, and output the elevation map

[0075] Compared with the prior art, the present invention has the following beneficial effects: 1. The present invention uses RBF to quickly fit lidar point cloud data, and significantly improves the fitting speed through parallel computing of the GPU, ensuring the real-time generation of the elevation map

[0076] 2. Based on the Kalman filtering principle, the present invention updates the current elevation map information according to the prior of the historical elevation map, 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 intelligent wheelchairs.

[0077] 3. The present invention can output elevation maps, gradient maps, and normal vector maps with arbitrary resolutions, and when there are sufficient hardware CUDA cores, it can achieve parallel computing through GPU acceleration, greatly improving the map generation speed. BRIEF DESCRIPTION OF THE DRAWINGS

[0078] By reading the following detailed description of non-limiting embodiments with reference to the accompanying drawings, other features, objects, and advantages of the present invention will become more apparent: Figure 1 It is a schematic flowchart of a method for generating a terrain elevation map based on radial basis function fitting. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0079] The present invention will be described in detail below with reference to specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but do not limit the present invention in any form. It should be noted that those of ordinary skill in the art can make several changes and improvements without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

[0080] The present invention provides a method for generating a terrain elevation map based on radial basis function (RBF) fitting, which can accurately provide the elevation information of the surrounding environment in real time during the movement of an intelligent wheelchair, detect and effectively identify static and dynamic obstacle objects in the environment, so as to achieve efficient and accurate terrain perception of the intelligent wheelchair in a complex environment, and provide strong perception information support for the movement, navigation, and obstacle avoidance of the intelligent wheelchair. Specifically, taking Figure 1 as an example, it includes the following steps: Step S1: Receive the information of the most recent multiple frames of historical point cloud and odometer information, generate a local point cloud map and downsample it; Specifically, the most recent multiple frames of historical point cloud data and odometer information are received by a lidar, and they are fused to generate a local point cloud map, and the data is downsampled to improve the calculation efficiency, aiming to generate a local point cloud map to characterize the environmental structure near the current position of the intelligent wheelchair.

[0081] In more preferred examples, the odometer information of the intelligent wheelchair and the current frame of lidar point cloud data are received, where the odometer information provides the real-time pose information of the intelligent wheelchair in the world coordinate system, and the point cloud data records the three-dimensional positions of environmental objects in the lidar coordinate system.

[0082] To ensure data consistency, it is necessary to construct a transformation matrix using the current pose of the intelligent wheelchair to transform each point in the point cloud data from the radar coordinate system to the world coordinate system.

[0083] Let the current time be \(t\). The odometry information of the intelligent wheelchair at this time consists of a position vector and Euler angles, denoted as:

[0084] Among them, the position vector \(t\) t represents the translation at time \(t\), and the Euler angles represent the rotation angles around the \(x\), \(y\), and \(z\) axes at time \(t\) respectively. 、 、 represent the position vectors of the \(x\), \(y\), and \(z\) axes at time \(t\) respectively. Convert the Euler angles to a rotation matrix :

[0085] Among them, represents the rotation matrix corresponding to the rotation angle around the \(x\) axis, represents the rotation matrix corresponding to the rotation angle \(\theta\) around the \(y\) axis, represents the rotation matrix corresponding to the rotation angle \(\psi\) around the \(z\) axis.

[0086] According to the rotation matrix and the translation vector \(t\) t construct a transformation matrix \(T\) t :

[0087] Suppose \(M\) points are collected in the current frame. Then the current frame of point cloud 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:

[0088] Among them, represents the set of point clouds at time \(t\), represents the \(i\)-th point cloud sampled at time \(t\).

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

[0090] Perform the same operation on the point clouds of the most recent consecutive \(N\) frames in history, denoted as:

[0091] Among them, Denote the historical point cloud of the most recent j-th frame.

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

[0093] After the point cloud transformation and fusion at multiple moments are completed, the formed local point cloud map may have 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 perform downsampling processing on the local point cloud map, removing duplicate or overly dense points and reducing the subsequent data processing burden.

[0094] Specifically, the entire three-dimensional space is divided into several cubic voxels with side length l. Traverse all points in the local point cloud, and determine the voxel unit number to which each point belongs according to its three-dimensional coordinate values. For multiple points contained in the same voxel, only retain one representative point as the output point of the voxel, and this 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 and uniformly distributed representative points, forming the downsampled local point cloud map, denoted as .

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

[0096] Step S2: Dynamically generate the RBF center points for ground fitting based on the point cloud distribution of the local point cloud map and the ground fitting area; Specifically, according to the distribution characteristics of the point cloud in the downsampled local point cloud map and the pre-set ground fitting area, dynamically select and determine the position of the center points of the RBF suitable for fitting.

[0097] In more preferred examples, extract the two-dimensional plane coordinates of the i-th point corresponding to the X-axis and Y-axis from each point in the downsampled local point cloud map , use the downsampled point cloud data to construct a KD tree to accelerate the spatial search efficiency. Combining the pre-defined ground fitting area, use the radius search algorithm of the KD tree to quickly locate the positions where the RBF center points need to be placed, thereby dynamically generating a set of RBF center points for terrain fitting. ,

[0098] Set the predetermined ground fitting area as a rectangular area , and set the fitting resolution as δ. 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, and enumerate the coordinate positions of two-dimensional plane points within all fitting areas , where represents the x-axis coordinate of the two-dimensional coordinates of the k-th point, represents the y-axis coordinate of the two-dimensional coordinates of the k-th point. For each candidate position, perform a radius search within its neighborhood with a radius of r through a KD tree to determine whether there is a sufficient amount of point cloud data around this position

[0099] If there is point cloud support near a certain position , then retain this point as the RBF center point. Denote all the center points that pass the screening as a set:

[0100] After traversing all candidate points, for each center point (a total of K center points), if there is already a weight estimation result in the historical iteration, then load this historical weight as the weight prior for this round; if it is a newly added point, then initialize its weight to zero

[0101] Establish a mapping relationship using a hash function to efficiently store and real-time update the spatial positions of each RBF center point and their corresponding weight information. The spatial coordinates and corresponding weights of all center points are stored and indexed through a hash structure to support subsequent parallel acceleration and fast update operations. Finally, complete the selection and initialization of the dynamic RBF center points at the current moment

[0102] Step S3: Calculate the kernel matrix for fitting based on the local point cloud map and RBF center points. Based on the Kalman filter principle, through GPU parallel acceleration, iteratively calculate the weights of the RBF center points to generate a terrain manifold In more preferred examples, calculate the kernel matrix using the determined RBF center points and local point cloud map data, and implement parallel acceleration of the kernel matrix calculation and matrix calculation in the Kalman filter algorithm through a graphics processing unit (GPU). Compared with traditional optimization methods (such as sparse kernel techniques, neighbor truncation, iterative approximation, etc.), it can significantly improve the large-scale data processing speed while maintaining accuracy to efficiently solve the weight parameters of the RBF center points, thereby constructing an accurate terrain manifold

[0103] Specifically, based on the current local point cloud map and the positions of the already selected RBF center points, use the GPU parallel computing platform with the CUDA architecture to quickly and parallelly calculate the radial basis function values between each point cloud point and the RBF center points to construct the kernel matrix at the current moment

[0104] Define the radial basis function kernel as:

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

[0106] where represents the weight value corresponding to the j-th RBF center point.

[0107] Considering the characteristic that the weight parameter is updated over time, using the weight of the historical estimation result as the prior condition, based on the Kalman filtering principle, through high-efficiency matrix calculation libraries such as CuBLAS and Cusolver on the CUDA platform for parallel calculation, accelerating the implementation of complex matrix operations such as the Kalman gain matrix and covariance update matrix, and updating the weight parameter of the RBF center point in real time.

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

[0109] To accelerate the construction of the kernel matrix, introduce the GPU parallel computing framework, implement parallel computing using CUDA programming, and through a custom CUDA kernel function, distribute the calculation tasks of the Euclidean distance and kernel function value between the point cloud point and the RBF center point to multiple cores on the GPU for simultaneous execution. The GPU is suitable for large-scale matrix operations, especially the large number of parallel multiplication and addition operations involved in the RBF kernel matrix and weight solution. While completing all calculation tasks at one time, it avoids repeated iteration or redundant structure selection, thus greatly reducing the fitting time, having higher real-time performance and scalability, and being suitable for high-performance scenarios such as online mapping.

[0110] Next, solve it in two cases through the least - squares principle and the Kalman filter principle to process the estimation of weights. The weights of all RBF centers are represented by a vector as follows:

[0111] When initially estimating the weights: When receiving the first - frame local point - cloud map and the corresponding odometry, since there is no prior for the weights at this time, directly solve the least - squares problem:

[0112] where, is the height observation vector of each point in the local point - cloud map, that is, corresponding to the down - sampled local point - cloud map the -th point's Z - axis coordinate in it. For this least - squares problem, its analytical solution is:

[0113] When there is a historical prior: Update it using the Kalman filter method. Let the state estimated in the previous round be , and the covariance matrix be . The state - transition equation and the observation equation are as follows:

[0114]

[0115] where, Q is the process - noise covariance matrix, and I is the identity matrix, which is introduced due to the uncertainty in the self - pose estimation of the intelligent wheelchair during movement.

[0116] According to the Kalman - filter update steps, first calculate the residual:

[0117] where, represents the vector composed of the z - axis coordinates of all the point - cloud points in the down - sampled local point - cloud map at time t, which is the observation information of the point - cloud height.

[0118] During the generation process of the elevation map, based on the Kalman - filter principle, update the current elevation - map information according to the historical elevation - map prior, retain the elevation information of static obstacles while updating the elevation information of dynamic obstacles, and significantly improve the fitting speed through the parallel computing of the GPU to ensure the real - time nature of the elevation - map generation. Let the covariance of the observation equation be , which is introduced by the point - cloud observation noise of the current - frame local point - cloud map. Calculate the Kalman gain matrix:

[0119] Update the weights and weight covariance according to the Kalman gain matrix and the residuals:

[0120]

[0121] The finally updated β t is the estimated center point weight at the current moment. This estimated result, together with 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.

[0122] To accelerate the solution process of the weight vector β, a GPU parallel computing framework is introduced, and the parallel linear algebra library in the CUDA programming model is used to complete matrix multiplication and system of equations solving. The multiplication operation of the kernel matrix and the vector is implemented using CuBLAS, and the fast inversion and solution of the normal equations are implemented using Cusolver.

[0123] The updated weight results are stored efficiently again through the hash function as the initial conditions for the next iterative update, thereby generating a smooth and continuous terrain elevation fitting manifold. The parallel acceleration mechanism is a key means deployed under a high-performance computing architecture, greatly enhancing the timeliness of high-frequency updates in a dynamic environment.

[0124] Step S4: Output the elevation map near the intelligent wheelchair through GPU parallel acceleration calculation.

[0125] Specifically, based on the characteristic that the radial basis function can fit the manifold, the terrain manifold is fitted using the radial basis function, and based on the constructed terrain manifold, a detailed elevation map of the area near the intelligent wheelchair is output through GPU parallel computing.

[0126] In more preferred examples, using the calculated RBF center point weight parameters, for any position where height information needs to be queried, the elevation estimated value is calculated using the radial basis function fitting formula.

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

[0128] Let the area covered by the elevation map to be output be a two-dimensional grid mesh , where represents the grid center point coordinates of the m-th row and the n-th column. For each grid point , its estimated height is given by the following formula:

[0129] Among them, is a preset radial basis function, is the weight corresponding to the center point . Through the estimation of this weighted kernel function, the terrain elevation of the entire local area is continuously and smoothly reconstructed in space.

[0130] Since the generation of the elevation map involves traversing a large number of grid points and weighted calculations of kernel functions, to ensure real-time performance, the GPU parallel acceleration framework is also used to execute this process. All grid points are divided into thread blocks, and a grid point is assigned to each thread in CUDA to independently and parallelly calculate its estimated height. Through the GPU parallel computing architecture, the elevation estimation results of multiple query points are calculated in parallel at the same time, significantly improving the real-time performance and efficiency of elevation map generation and meeting the requirements of fast navigation and dynamic obstacle avoidance of intelligent wheelchairs.

[0131] Finally, the output elevation map records the elevation estimation values of each grid center point in the form of a two-dimensional array, constituting the local terrain model of the intelligent wheelchair at the current position, which is used for subsequent functional modules such as path planning, collision detection, and navigation control, providing a basic perception guarantee for the safe and autonomous movement of the intelligent wheelchair in a complex environment.

[0132] 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 manner of the terrain elevation map generation system based on radial basis function fitting.

[0133] According to a terrain elevation map generation system based on radial basis function fitting provided by the present invention, it includes: The first module, which receives historical point cloud information and odometer information, generates a local point cloud map and performs downsampling; The second module, which dynamically generates the center points of RBF based on the downsampled local point cloud map and the ground fitting area; The third module, which calculates the kernel matrix according to the downsampled local point cloud map and the center points, and iteratively calculates the weights of the center points through GPU parallel acceleration to generate a terrain manifold; The fourth module, which calculates and outputs the elevation map through GPU parallel acceleration according to the terrain manifold.

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

[0135] The point cloud data includes the three-dimensional position points of environmental objects in the lidar coordinate system.

[0136] The odometry information includes the real-time pose information of the target in the world coordinate system.

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

[0138] In the second module, the two-dimensional plane coordinates of the points corresponding to the X-axis and Y-axis are extracted from the i-th point in the downsampled local point cloud map 。 。

[0139] The ground fitting region is a rectangular region , and the fitting resolution is δ.

[0140] Traverse the ground fitting region in steps of δ, enumerate the coordinate positions of all two-dimensional plane points in the fitting region , and use the radius search algorithm of the KD tree to screen with a radius r. Record the central points that pass the screening as a set ; Traverse all central points . If there is a historical estimation result of the weight, load the historical estimation result as the weight prior. If it is a new point, initialize the weight to zero; Among them, 、 respectively represent the x and y axis coordinates of the two-dimensional coordinates of the k-th point; 、 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; Store and index the spatial coordinates of the central point and the corresponding weight through a hash structure.

[0141] In more preferred examples, in the third module, GPU parallel computing is used, and the radial basis function kernel is:

[0142]

[0143] For any point in the ground fitting region, calculate the predicted height:

[0144] The kernel matrix between each point cloud point and the central point in the downsampled local point cloud map is :

[0145] Among them, represents a preset radial basis function; represents the two-dimensional plane distance in Euclidean space; represents the two-dimensional coordinates of a point cloud point in the downsampled local point cloud map; represents the downsampled local point cloud map 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 kernel function bandwidth parameter; 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.

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

[0147] The weight corresponding to the center point is solved and / or updated with the weight of the historical estimation result as the prior condition, based on the least square principle and the Kalman filtering principle.

[0148] In more preferred examples, the solution and / or update of the weight includes: When initially estimating the weight, the first frame of local point cloud map and the corresponding odometer information are received, and the least square problem is solved:

[0149]

[0150]

[0151]

[0152] Among them, , represents the weight vector of the center point; represents the weight of the i-th center point; represents the height observation vector of each point in the local point cloud map; Represents the downsampled local point cloud map The Z-axis coordinate of the i-th point in

[0153] When there are prior conditions, the state transition equation And the observation equation Are:

[0154]

[0155] Calculate the residual:

[0156] Where, Represents the state estimated in the previous round; Represents the covariance matrix; Q represents the process noise covariance matrix; I represents the identity matrix; Represents the downsampled local point cloud map at time t Observation information of the point cloud height in

[0157] Calculate the Kalman gain matrix :

[0158] Update the weights and weight covariances according to the Kalman gain matrix and the residual :

[0159]

[0160] Finally, the weight of the center point is updated And the corresponding center points are stored through a hash structure; Where, Represents the covariance of the observation equation.

[0161] In more preferred examples, in the fourth module, according to the weights of the center points, the elevation estimate value is calculated using the radial basis function fitting formula:

[0162]

[0163] Where, K represents the total number of center points; Represents the preset radial basis function; Represents the corresponding center point Of the weight; represent the two-dimensional coordinates of the j-th center point; represent the two-dimensional grid grid covering the area of the output elevation map; represent the coordinates of the grid center point in the m-th row and n-th column.

[0164] Divide all grid points by thread blocks. In CUDA of GPU, each thread is assigned a grid point, and the elevation estimation value of each grid center point is calculated independently and in parallel and recorded in the form of a two-dimensional array, and the output elevation map is output.

[0165] In more preferred examples, verification is carried out on a hardware platform equipped with an Intel Core i5-12600KF processor and an NVIDIA RTX 3080 graphics card. Different sizes of regions of interest (ROIs) and radial basis function (RBF) parameters are set respectively, and the kernel matrix calculation time, weight solution time, total elevation map calculation time, and final fitting accuracy (mean absolute error) are recorded, and a comparative analysis of GPU and CPU multi-threading is carried out. The GPU acceleration scheme provided by the present invention has significantly shortened the calculation time, with the minimum acceleration ratio reaching 24.5 times and the maximum acceleration ratio exceeding 120 times. SLAM algorithms such as loam can introduce this system to reduce the z-axis error accumulation caused by complex environments and improve the positioning accuracy by observing the height error of the intelligent wheelchair.

[0166] In terms of accuracy, the GPU implementation is consistent with the CPU multi-threading results, and the difference in mean absolute error is within the millimeter level, without affecting the final elevation fitting accuracy. Especially in a large-scale high-density point cloud scene (ROI 6×6, RBF resolution 0.2m), the GPU can complete the elevation map calculation of 251,001 points in only 49.5 milliseconds, compared with 6016.07 milliseconds of CPU multi-threading, fully demonstrating the significant advantages in real-time and large-scale processing tasks.

[0167] Those skilled in the art know that in addition to implementing the system and its various devices, modules, and units provided by the present invention in the form of pure computer-readable program code, the method steps can be logically programmed to make 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 to achieve the same function. Therefore, the system and its various devices, modules, and units provided by the present invention can be considered as a kind of hardware component, and the devices, modules, and units included therein for implementing various functions can also be regarded as the structure within the hardware component; the devices, modules, and units for implementing various functions can also be regarded as both software modules for implementing the method and the structure within the hardware component.

[0168] The specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which does not affect the essence of the present invention. Without conflict, the embodiments of the present application and the features in the embodiments can be combined with each other arbitrarily.

Claims

1. A method for generating a terrain elevation map based on radial basis function fitting, characterized in that Including: Step S1: Receive historical point cloud information and odometer information, generate a local point cloud map and downsample it; Step S2: Dynamically generate the center points of the RBF based on the downsampled local point cloud map and the ground fitting area; Step S3: Calculate the kernel matrix according to the downsampled local point cloud map and the center points, accelerate through GPU parallel computing, iteratively calculate the weights of the center points, and generate a terrain manifold; Step S4: Calculate and output a height map through GPU parallel acceleration according to the terrain manifold.

2. The method for generating a topographic elevation map based on radial basis function fitting according to claim 1, wherein, In step S1, receive the historical point cloud data of the most recent continuous N frames and the odometer information, perform fusion processing, and perform downsampling processing; The point cloud data includes the three-dimensional position points of environmental objects in the lidar coordinate system; The odometer information includes the real-time pose information of the target in the world coordinate system; The downsampling process uses a voxel filtering method to obtain the downsampled local point cloud map ; In the step S2, the two-dimensional plane coordinates of the points corresponding to the X-axis and the Y-axis are extracted from the i-th point in the downsampled local point cloud map ; ; The ground fitting area is a rectangular area , and the fitting resolution is δ; Traverse the ground fitting area in steps of δ, and enumerate the coordinate positions of two-dimensional plane points in all fitting areas , and use the radius search algorithm of the KD tree to screen with a radius r, and record the screened center points as a set ; Traverse all center points , if there is a historical estimation result of weights, load the historical estimation result as the weight prior, and if it is a new point, initialize the weight to zero; wherein, and respectively represent the x and y axis coordinates of the two-dimensional coordinates of the k-th point; , represent the minimum and maximum x-axis boundary values of the ground fitting matrix, respectively; , respectively represent the minimum and maximum y-axis boundary values of the ground fitting matrix; Store and index the spatial coordinates of the center points and the corresponding weights 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, use GPU parallel computing, and the radial basis function kernel is: For any point within the ground fitting area , calculate the predicted height: The kernel matrix of each point cloud point in the downsampled local point cloud map and the center point is :[[]]END]] Among them, represents a preset radial basis function; d represents the two-dimensional plane distance in Euclidean space; represent the two-dimensional coordinates of the point cloud points in the downsampled local point cloud map; Indicates the i-th point cloud point in the downsampled local point cloud map; represent the two-dimensional coordinates of the j-th center point; K represents the total number of center points; σ represents the kernel function bandwidth parameter; denotes the weight corresponding to the j-th center point; Denote the element at 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 points and the kernel matrix runs simultaneously on multiple cores of the GPU, update the height map information according to the prior of the historical height map, and obtain the terrain manifold; The weights corresponding to the center points are solved and / or updated based on the weights of the historical estimation results as prior conditions, based on the least squares principle and the Kalman filter principle.

4. The method for generating a terrain elevation map based on radial basis function fitting according to claim 3, wherein The solution and / or update of the weights includes: When initially estimating the weights, receive the first frame of local point cloud map and the corresponding odometer information, and solve the least squares problem: Among them, , represents the weight vector of the center point; represent 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 are as follows: Calculate the residual: Among them, represents the state estimated in the previous round; denotes the covariance matrix; Q represents the process noise covariance matrix; I represents the identity matrix; Represents the downsampled local point cloud map at time t Observation information of the midpoint cloud height; Calculate the Kalman gain matrix : Update the weights and weight covariance based on the Kalman gain matrix and the residuals : Finally, update to obtain the weights of the center points Store them together with the corresponding center points through a hash structure; Among them, represents the covariance of the observation equation.

5. The method for generating a topographic elevation map based on radial basis function fitting according to claim 1, wherein In step S4, according to the weights of the center points, use the radial basis function fitting formula to calculate the height estimation value: Where, K represents the total number of center points; denote a preset radial basis function; Indicates the corresponding center point of the weight; represent the two-dimensional coordinates of the j-th center point; A two-dimensional grid mesh representing the coverage area of the output elevation map; denote the coordinates of the center point of the grid at the m-th row and the n-th column; Divide all grid points into thread blocks, allocate a grid point to each thread in CUDA of the GPU, independently and parallelly calculate the height estimation value of each grid center point and record it in the form of a two-dimensional array, and output the height map.

6. A terrain elevation map generation system based on radial basis function fitting, characterized in that, Including: The first module: Receive historical point cloud information and odometer information, generate a local point cloud map and downsample it; The second module: Dynamically generate the center points of the RBF based on the downsampled local point cloud map and the ground fitting area; The third module: Calculate the kernel matrix according to the downsampled local point cloud map and the center points, accelerate through GPU parallel computing, iteratively calculate the weights of the center points, and generate a terrain manifold; The fourth module: Calculate and output a height map through GPU parallel acceleration according to the terrain manifold.

7. The terrain elevation map generation system based on radial basis function fitting according to claim 6, wherein In the first module, receive the historical point cloud data of the most recent continuous N frames and the odometer information, perform fusion processing, and perform downsampling processing; The point cloud data includes the three-dimensional position points of environmental objects in the lidar coordinate system; The odometer information includes the real-time pose 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 ; Extract the two-dimensional plane coordinates of the points corresponding to the X-axis and Y-axis from the i-th point in the downsampled local point cloud map in the second module ; ; The ground fitting area is a rectangular area , and the fitting resolution is δ; Traverse the ground fitting area in steps of δ, and enumerate the coordinate positions of two-dimensional plane points in all fitting areas , and use the radius search algorithm of the KD tree to screen with a radius r, and record the screened center points as a set ; Traverse all center points , if there is a historical estimation result of the weight, load the historical estimation result as the weight prior. If it is a new point, initialize the weight to zero; Among them, and respectively represent the x-axis and y-axis coordinates of the two-dimensional coordinates of the k-th point; , 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; Store and index the spatial coordinates of the center points and the corresponding weights 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 third module, use GPU parallel computing, and the radial basis function kernel is: At any point within the ground fitting area , calculate the predicted height: The kernel matrix of each point cloud point in the downsampled local point cloud map and the center point is :[[]] Among them, represents a 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 i-th point cloud point in the downsampled local point cloud map ; Represent the two-dimensional coordinates of the j-th center point; K represents the total number of center points; σ represents the kernel function bandwidth parameter; Denote the weight corresponding to the j-th center point; Denote the element at 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 points and the kernel matrix runs simultaneously on multiple cores of the GPU. The elevation map information is updated according to the prior of the historical elevation map to obtain the terrain manifold; The weight corresponding to the center point is solved and / or updated based on the least squares principle and the Kalman filtering principle with the weight of the historical estimation result as the prior condition.

9. The terrain elevation map generation system based on radial basis function fitting according to claim 8, characterized in that The solution and / or update of the weight includes: When estimating the weight for the first time, the first frame of the local point cloud map and the corresponding odometer information are received, and the least squares problem is solved: Among them, , represents the weight vector of the center point; Denote 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 downsampled local point cloud map The Z-axis coordinate of the i-th point in When there are prior conditions, the state transition equation and the observation equation are as follows: Calculate the residual: Among them, represents the state estimated in the previous round; denotes the covariance matrix; Q represents the process noise covariance matrix; I represents the identity matrix; Represents the downsampled local point cloud map at time t Observation information of the midpoint cloud height; Calculate the Kalman gain matrix : Update the weights and weight covariance based on the Kalman gain matrix and the residuals : Finally update to obtain the weights of the center points Store them with the corresponding center points through a hash structure; Among them, 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 fourth module, according to the weight of the center point, the elevation estimation value is calculated using the radial basis function fitting formula: where, K represents the total number of center points; denote a preset radial basis function; Indicates the corresponding center point of the weight; represent the two-dimensional coordinates of the j-th center point; A two-dimensional grid mesh representing the coverage area of the output elevation map; Denote the grid center point coordinates of the m-th row and the n-th column; All grid points are divided by thread blocks. In CUDA of the GPU, each thread is assigned a grid point, and the elevation estimation value of each grid center point is calculated independently and in parallel and recorded in the form of a two-dimensional array, and the elevation map is output.

Images