A passable map generation method for indoor multi-story buildings

Abstract

The application discloses a kind of passable map generation methods for indoor multi-storey building, comprising the following steps: laser radar, inertial sensor and wheel speed odometer are installed to unmanned vehicle;Coordinate system is established based on laser radar, inertial sensor and wheel speed odometer, and error state is obtained based on the coordinate system;Measurement model is constructed, and the multi-storey building is measured by measurement model, obtains point cloud data and unmanned vehicle pose data;The data update of unmanned vehicle pose data is carried out by the error state obtained, and update pose data is obtained;According to the point cloud data and update pose data, passable map is generated.This method uses the way of tight coupling multi-sensor fusion to carry out positioning, can effectively improve the robustness and positioning accuracy of system, for the structure degradation environment such as corridor, tunnel, etc., using the way of tight coupling multi-sensor fusion can effectively solve the problem of environment degradation.

Description

Technical Field

[0001] This invention belongs to the field of path planning technology, and in particular relates to a method for generating accessible maps for multi-story indoor buildings. Background Technology

[0002] Traditional 3D point cloud maps project real-time point clouds or keyframes onto a global coordinate system based on the pose provided by the localization module. This results in poor readability of point cloud maps in indoor scenes, making it difficult for map users to extract crucial information. While 2D maps offer clear structure and high readability, they are unusable when autonomous vehicles are operating on sloping ground or in multi-story environments. Furthermore, traditional LiDAR and IMU / wheel speed odometer fusion methods first obtain an initial pose through the LiDAR matching process, then fuse this pose with the IMU and odometer data. This is loosely coupled fusion; when the LiDAR is in a degraded environment, the point cloud matching may be incorrect. Loosely coupled fusion will continue to use the incorrect matching results, inevitably reducing positioning accuracy. Summary of the Invention

[0003] To address the aforementioned technical problems, this invention proposes a method for generating accessible maps for multi-story indoor buildings, in order to solve the problem of positioning failure in degraded environments.

[0004] To achieve the above objectives, the present invention provides a method for generating accessible maps for multi-story indoor buildings, comprising the following steps:

[0005] Install lidar, inertial sensors, and wheel speed odometers on the driverless vehicle;

[0006] A coordinate system is established based on lidar, inertial sensors, and wheel speed odometers, and the error status is obtained based on the coordinate system.

[0007] A measurement model is constructed, and the multi-story building is measured using the measurement model to obtain point cloud data and autonomous vehicle pose data;

[0008] The autonomous vehicle pose data is updated by obtaining the error state;

[0009] A passable map is generated based on the point cloud data and the updated pose data.

[0010] Preferably, the method for establishing a coordinate system includes:

[0011] Establish a lidar coordinate system, an inertial sensor coordinate system, a wheel speed odometer coordinate system, and a global coordinate system, and transform the lidar coordinate system and the wheel speed odometer coordinate system into the inertial sensor coordinate system;

[0012] The origin of the lidar coordinate system is located at the lidar scanning center. The X-axis points in the forward scanning direction of the lidar, the Z-axis is vertically upward along the lidar rotation axis, and the Y-axis points to the left of the lidar.

[0013] The origin of the inertial sensor coordinate system is located at the center of the inertial sensor, the X-axis points directly forward of the autonomous vehicle, the Z-axis points vertically upward, and the Y-axis points to the left of the autonomous vehicle.

[0014] The origin of the wheel speed odometer coordinate system is located at the center of the autonomous vehicle, the X-axis points directly forward of the autonomous vehicle, the Z-axis points vertically upward, and the Y-axis points to the left of the autonomous vehicle.

[0015] The global coordinate system is the inertial sensor coordinate system at the start of the measurement.

[0016] Preferably, the method for obtaining the error state includes:

[0017] The system input is obtained through the outputs of the gyroscope and the accelerometer.

[0018] The system noise is obtained by measuring the noise of the gyroscope and accelerometer and the random walk process of the gyroscope and accelerometer;

[0019] The pose transformation relationship is obtained based on the system input and system noise.

[0020] The relationship of error quantities is derived by deriving the relationship of pose transformation, and the error state is obtained based on the relationship of error quantities.

[0021] Preferably, the method for constructing the measurement model includes constructing a wheel speed-mileage measurement model and constructing a lidar measurement model;

[0022] The method for constructing a wheel speed odometer measurement model includes: obtaining wheel speed odometer measurement information through the velocity of the inertial sensor coordinate system in the global coordinate system; obtaining observation values ​​based on the measurement information; obtaining the wheel speed odometer observation matrix based on the observation values; obtaining the wheel speed odometer observation equation by combining the observation matrix with measurement noise; and constructing a wheel speed odometer measurement model based on the wheel speed odometer observation equation.

[0023] Preferably, the method for constructing the lidar measurement model includes:

[0024] A multi-story building is scanned by a lidar to obtain a three-dimensional point cloud. A feature point set is obtained from the three-dimensional point cloud. The feature point set is combined with the lidar observation values ​​to obtain the lidar observation equation. A lidar measurement model is constructed based on the lidar observation equation.

[0025] Preferably, the process of generating a passable map includes:

[0026] Accessible areas are obtained by detecting accessible areas based on point cloud data. Accessible areas are combined with updated pose data to construct a accessible map. Accessible area detection includes ground accessible area detection and stair detection.

[0027] The process of detecting passable areas on the ground includes: cropping the point cloud data according to the installation height of the lidar to obtain cropped point cloud data; dividing the point cloud data into fan-shaped segments from a top-down view, with the segmentation angle being the horizontal resolution of the lidar; converting the points in the area from a rectangular coordinate system to a cylindrical coordinate system; sorting the rays at the same segmentation angle according to their radius to obtain a sequential dataset; comparing the slopes of three adjacent points of a ray to obtain the slopes of the three adjacent points; and determining whether the detected area is a passable area on the ground based on the slopes.

[0028] Preferably, the staircase detection process includes:

[0029] The dataset of non-ground accessible areas is filtered out, and the non-ground accessible area dataset is converted from cylindrical coordinate system to rectangular coordinate system to obtain point cloud set. Based on the riser, tread and stair width of the stair, the point cloud set is used to search for stair plane to obtain initial stair plane. Then, the initial stair plane is filtered by line fitting to obtain stair region.

[0030] Preferably, the process of generating a passable map includes:

[0031] A raster map is constructed based on octomap. Octree map ray tracing is performed on the point cloud of the passable area to obtain the newly added and disappeared graticules in the global passable area raster map. The newly added and disappeared graticules are combined with the pose of the unmanned vehicle in the global frame to generate the passable map.

[0032] Compared with the prior art, the present invention has the following advantages and technical effects:

[0033] The method for generating accessible maps for multi-story indoor buildings described in this invention effectively solves the problem of reduced accuracy after environmental degradation by using a tightly coupled approach and combining measurements from three sensors. Furthermore, the method uses an octree map ray tracing method for map updates, which effectively reduces the amount of data stored in the map. Attached Figure Description

[0034] The accompanying drawings, which form part of this application, are used to provide a further understanding of this application. The illustrative embodiments and descriptions of this application are used to explain this application and do not constitute an undue limitation of this application. In the drawings:

[0035] Figure 1 This is a flowchart of the method for generating a passable map according to an embodiment of the present invention;

[0036] Figure 2 This is a diagram showing the coordinate system and position of the sensor in an embodiment of the present invention;

[0037] Figure 3 This is a schematic diagram of the sector division according to an embodiment of the present invention;

[0038] Figure 4 This is a schematic diagram of the staircase features according to an embodiment of the present invention;

[0039] Figure 5 This is a schematic diagram of the point cloud projection of the staircase according to an embodiment of the present invention. Detailed Implementation

[0040] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.

[0041] It should be noted that the steps shown in the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and although a logical order is shown in the flowchart, in some cases the steps shown or described may be executed in a different order than that shown here.

[0042] Example 1

[0043] like Figure 1 As shown, this invention proposes a method for generating accessible maps for multi-story indoor buildings, comprising the following steps:

[0044] Install lidar, inertial sensors, and wheel speed odometers on the driverless vehicle;

[0045] A coordinate system is established based on lidar, inertial sensors, and wheel speed odometers, and the error status is obtained based on the coordinate system.

[0046] A measurement model is constructed, and the multi-story building is measured using the measurement model to obtain point cloud data and autonomous vehicle pose data;

[0047] The autonomous vehicle pose data is updated by obtaining the error state;

[0048] A passable map is generated based on the point cloud data and the updated pose data.

[0049] Further optimization schemes, such as the constructed autonomous vehicle coordinate system Figure 2 As shown,

[0050] Where the lidar coordinate system F at time t L,tThe coordinate system is defined as follows: the origin of the coordinate system is located at the center of the lidar scan, the X-axis points in the forward scanning direction of the lidar, the Z-axis is vertically upward along the lidar rotation axis, and the Y-axis points to the left of the lidar. The X, Y, and Z axes conform to the right-hand rule. The inertial measurement unit (IMU) outputs the angular velocity (gyroscope) and specific force (accelerometer output) of its own coordinate system at the current moment. Let ω be the output of the gyroscope at time t. m,t The accelerometer output is a m,t The coordinate system is the IMU system F at time t. I,t The wheel speed odometer outputs the current forward speed of the vehicle body, generally in coordinate system F. O,t Located at the center of the vehicle body, the wheel speed odometer outputs v at time t. odo,t The forward velocity of the autonomous vehicle at time t is usually expressed as s. o v odo,t , where s o The scale factor for the wheel speed odometer is F, the global coordinate system. G IMU coordinate system F defined at time 0 I,0 .

[0051] Further optimization schemes, the relationships between the sensors are as follows:

[0052] Since the various sensors are installed in different locations, their coordinate systems do not completely coincide. Let the coordinate systems and positions of the various sensors at time t be as follows: Figure 2 As shown, all data processing in this paper is performed in the IMU coordinate system.

[0053] Let the external parameters between the wheel speed odometer and the lidar and the IMU be respectively... and These represent the transformation matrices from the coordinate systems of the two sensors to the IMU system.

[0054]

[0055]

[0056] in Let represent the rotation matrix and translation vector from the wheel speed odometer system to the IMU system, respectively. Let represent the rotation matrix and translation vector from the lidar system to the IMU system, respectively.

[0057] When the speed measurement s of the wheel speed odometer is obtained at time t. o v odo,t At this time, the speed of switching to the IMU system is It can be divided into translational velocity. and rotational speed

[0058]

[0059]

[0060] ω t Represents the angular velocity at time t. Represents a three-dimensional vector ω t If the antisymmetric matrix is ​​ω t =[ω tx ω ty ω tz ] T ,but

[0061]

[0062] so

[0063]

[0064] When the lidar feature points are obtained at time t When converting it to the IMU coordinate system, we get

[0065]

[0066] A further optimization scheme defines the operators as follows:

[0067] Let M be the space of SO(3) manifolds, and let its corresponding tangent space be T. Then T is the space formed by three-dimensional antisymmetric matrices (or three-dimensional vectors). Let the three-dimensional vector space be R. 3 .definition and The operators are as follows:

[0068]

[0069]

[0070]

[0071]

[0072]

[0073]

[0074] Where R∈M, r∈T, Exp is the exponential mapping from the tangent space T to SO(3) defined by the Rodrigues formula.

[0075]

[0076] ln(·) is the inverse mapping of Exp(·).

[0077] Further optimization schemes, the error state calculation method is as follows:

[0078] Define the following state variables and Let be the rotation matrix, translation vector, and 3D velocity of the IMU frame relative to the global coordinate system. ω b a These are the zero bias values ​​for the gyroscope and accelerometer, respectively.

[0079] Let the system input be u, then

[0080]

[0081] Where ω m For the output of the gyroscope, a m This is the output of the accelerometer.

[0082] Assuming the system noise is all white noise from a Gaussian process, the system noise model is as follows:

[0083]

[0084] Where n ω n a For the measurement noise of gyroscopes and accelerometers, n bω n ba This refers to the random walk process of the gyroscope and accelerometer.

[0085] Based on the motion model of the IMU, let the system input and noise at time i be u, respectively. i With w i Then the pose transformation relationship from time i to time i+1 is:

[0086]

[0087] Where, Δt i =t i+1 -t i Let i be the time step from time i to time i+1.

[0088]

[0089] This article selects the error quantity of the state. As an estimator for Kalman filtering, then The definition is as follows:

[0090]

[0091] in This represents the angular error of the IMU coordinate system. These represent the error values ​​of each state variable, which are the state variable minus the true value.

[0092] Based on the relationship between x at time i and time i+1, the relationship between the error quantities at the two times can be derived through the following steps:

[0093] First, the process noise w i Set it to 0, and use the predicted value at time i. Predict the value at time i+1

[0094]

[0095] Then, based on the best estimate of the state at time i, x i Derive the optimal state estimate x at time i+1. i+1

[0096]

[0097] Then the error state at time i+1 can be expressed as:

[0098]

[0099] Where F x With F w The derivation process is as follows:

[0100] First, according to Equation 18, we know...

[0101]

[0102] Similarly, we can conclude that:

[0103]

[0104] Linearizing the nonlinear parts of the two equations above, we obtain:

[0105]

[0106] Among them, J r To approximate the Jacobian matrix by right multiplying by BCH,

[0107]

[0108]

[0109] at this time, It can be represented as

[0110]

[0111] At this point, F can be obtained. x With F w They are respectively:

[0112]

[0113]

[0114] Let Q = ww T Let P be the covariance matrix of the noise w, then the covariance matrix of the error state is P. i+1 for:

[0115] P i+1 =F x P i F x T +F w QF w T 31

[0116] Further optimization schemes, the wheel speed mileage measurement model construction method is as follows:

[0117] Let the measured information output by the wheel speed odometer at time i+1 be s. o v odo,i+1 From the equation, we can see that the velocity of the IMU coordinate system in the global coordinate system at that moment is... for:

[0118]

[0119] Then s o v odo,i+1 Represented by the following state variables:

[0120]

[0121] Each state variable is represented by an error state variable.

[0122]

[0123]

[0124]

[0125] Therefore s o v odo,i+1 It can be represented as:

[0126]

[0127] s o v odo,i+1 Linearization yields:

[0128]

[0129] Let the observed values for:

[0130]

[0131] Then the observation matrix of the wheel speed odometer for:

[0132]

[0133] Let the measurement noise of the wheel speed odometer be n. O The observation equation is then expressed as follows:

[0134]

[0135] A wheel speed and mileage measurement model is constructed based on the observation equation.

[0136] The construction process of the lidar measurement model is as follows:

[0137] The feature point set obtained based on the three-dimensional lidar scanning method The corresponding feature set in the map point cloud is when When it is a planar feature, v j Let be the normal vector of its corresponding plane, and let the coordinates of the plane's center be . when When it is an edge feature, v j This indicates the edge direction of its corresponding edge, and the coordinates of the edge center are...

[0138] For any feature point The residual between it and the map features is:

[0139]

[0140] Representing the residuals using state variables and linearizing them yields:

[0141]

[0142] Ideally, the residual value should be 0, but in practice, the measurement noise n is taken into account. f,j Therefore, the characteristic residual can be expressed as follows:

[0143]

[0144] Let the observed values for:

[0145]

[0146] The observation equation is then:

[0147]

[0148] in The observation matrix is ​​represented as follows:

[0149]

[0150] When taking measurements of all feature points, let

[0151]

[0152] Therefore, the observation equation of lidar is expressed as follows:

[0153]

[0154] A lidar measurement model is constructed based on the lidar observation equation.

[0155] Further optimization measures will be implemented to update the status of autonomous vehicles.

[0156] Based on the obtained error state equation of the IMU and the error measurement equation of the wheel speed history meter / LiDAR, the filter gain K at time i+1 is, according to the Kalman filter gain equation,...

[0157]

[0158] Where P i+1 Let i+1 be the prediction covariance matrix of the error state. ( or Let R be the observation matrix, where R = n L n L T Or R = n O n O T ) is the covariance matrix of the measurement noise.

[0159] According to the error state Kalman filter procedure, the predicted error state value at time i+1 is considered to be 0. Therefore, the estimated error state value at time i+1 is... for:

[0160]

[0161] State estimation at time i+1 Updated to:

[0162]

[0163] State estimation at time i+1 The covariance matrix is ​​updated as follows:

[0164]

[0165] Further optimizations include the following steps in generating a passable map:

[0166] Accessible areas are obtained by detecting accessible areas based on point cloud data. Accessible areas are combined with updated pose data to construct a accessible map. Accessible area detection includes ground accessible area detection and stair detection.

[0167] This application uses a ray geometry method for detecting accessible ground areas.

[0168] set up The original point cloud in the lidar coordinate system at time i needs to be segmented into ground points and non-ground obstacle point clouds. Therefore, the point cloud needs to be cropped according to the installation height of the lidar to obtain the cropped point cloud. This can be expressed as:

[0169]

[0170] h1 and h2 are the set clipping height thresholds.

[0171] Obtain the cropped point cloud Next, the point cloud is divided into fan-shaped sections from a top-down view, such as... Figure 3 As shown.

[0172] Will They were respectively assigned to different regions S0, S1...S j ...S 2π / α α is the segmentation angle, typically the horizontal resolution of the lidar. Now, let's consider a fan-shaped region S... j Taking point p within the area as an example, we perform passable area detection. Sj (x Sj ,y Sj ,z Sj Data structure g described by converting Cartesian coordinates to cylindrical coordinates col (r col ,θ col ,z Sj ), where r col With θ col They are respectively:

[0173]

[0174]

[0175] Since the set segmentation angle α is relatively small, S can be... j The points within the data are considered as a straight line. Rays with the same included angle are sorted according to their radii to obtain dataset G. col .

[0176] At this time G col It can be represented as

[0177] G col ={g col,0 ,g col,1 ...g col,j ...g col,m |r col,0 <r col,1 <...r col,j <...r col,m 57

[0178] Let p col,j p col,j-1 p col,j-2 For three adjacent points of the ray, n j-1 For p col,j-2 With p col,j-1 The vector between two points, n j For p col,j-1 With p col,j The vector between two points, then p col,j With p col,j-1 slope β j for:

[0179]

[0180] At this point, determining whether the slope between the two points before and after the ray is greater than a certain angle threshold ε will determine whether the point is a passable area on the ground. Therefore, S j Ground points in the region can be represented as:

[0181]

[0182] Therefore, the passable ground area P of the current scanned point cloud can be obtained. ground for:

[0183]

[0184] Firstly, because the scanning range of LiDAR is relatively wide, processing all point clouds within a single frame results in a large amount of data. Therefore, the data set is obtained by filtering the area directly in front of the LiDAR facing the stairs. It can be represented as:

[0185]

[0186] Will The point cloud set P is obtained by converting the cylindrical coordinate system to the rectangular coordinate system. up After that, P up Perform a staircase plan search. During the search, pay attention to the following characteristics: A single-step staircase consists of a riser (P). r With tread P t The staircase consists of two parts, with the normals to the riser and tread planes being parallel and perpendicular to the X-axis of the lidar, respectively. Typically, the width of the staircase is less than w, P. r With P t The height extension is less than h r Or the depth extension is less than d r ,like Figure 4 As shown.

[0187] Regarding the above feature P up Select a suitable set of planes P from the options. plane ={P plane,1 ,P plane,2 ,...P plane,j ,...P plane,l}, where l is the number of planes. At this time, P plane It will contain some point clouds of interfering planes, so the point cloud of the stairs needs to be screened a second time.

[0188] First, we need to extract the staircase plan point cloud P from the initial search. plane Projecting onto the XZ-axis plane yields a point cloud set P. line ={p line (x line ,y line ,z line )|y line =0,p line ∈P plane},like Figure 5 As shown.

[0189] Depend on Figure 5 It can be seen that the point cloud P is obtained by projecting onto the XZ axis plane. line It is approximated as a straight line, therefore the least squares method is used to approximate the point cloud P. line The expression for the line obtained by line fitting is:

[0190] A line x+B line z+C line =0 62

[0191] Where A line B line C line To obtain the parameters for the fitted straight line, the fitted straight line is used to perform a secondary screening of the staircase plane.

[0192] First, obtain P. planeThe coordinates of the center point n of each plane plane,j =(x plane,j ,y plane,j ,z plane,j ), j = 1, 2, ..., l, calculate n plane,j Distance d to the fitted line line,j for:

[0193]

[0194] d line,j Less than the set distance threshold d th Point cloud P for stairs stair P stair It can be represented as:

[0195] P stair ={P plane,j |d line,j <d th P plane,j ∈P plane 64

[0196] Where the distance threshold d th Related to the characteristics of the staircase, it is usually less than 1m.

[0197] Based on the point cloud of passable areas in a complex multi-story environment at time i The pose of the IMU coordinate system in the global coordinate system at time i. The passable area scanned in real time is converted to a global system to obtain... It can be represented as:

[0198]

[0199] Let be the extrinsic parameter matrix of the lidar and IMU. This yields the point cloud of the globally accessible region at time i. Afterwards, Projected onto a raster map, the newly added and disappearing point clouds are detected using ray tracing.

[0200] Projecting the point cloud of the passable region in each frame onto an octree forms a global octree map On. When the global passable region scanned at time i is obtained... Combine it with O n The process is performed to obtain the newly added and reduced raster areas.

[0201] Assumption for One point in the middle, Let {v} be the position of the lidar in the global coordinate system at time i.j}for and The laser passes through a set of j grid cells. Based on the octomap grid probability update principle, the grid uncertainty is updated to obtain:

[0202]

[0203] in Let be the uncertainty corresponding to this observation. From the above formula, we can see that when... When it is large, It has high sensitivity to environmental changes, allowing for rapid detection of these changes, but it is susceptible to noise interference. When the size is small, the sensitivity to environmental changes is low. Environmental change detection is relatively stable at this time, but the detection speed is slow. Ray tracing of the point cloud of all traversable areas at the current moment yields a global traversable area octree map O. n The newly added grid {V inc,i} and the disappearing grid {V dis,i}:

[0204] {V inc,i}={v i |l(o v,i )>0.5,l(o v,i-1 )<0.5} 67

[0205] {V dis,i}={v i |l(o v,i ) < 0.5, l(o v,i-1 )>0.5} 68

[0206] At this point, the globally passable area octree map O n The state of each grid cell is also updated. When map data needs to be transmitted from the autonomous vehicle to other terminals, each map transmission only requires transmitting O. n The part that changes in the middle {V change Position and pose of the autonomous vehicle in the global system Where {V change} can be represented as:

[0207] {V change,i}={{V inc},{V dis 69

[0208] Based on the initial transmission data and the changes made each time, a map of the passable area is constructed using the autonomous vehicle's pose in the global system.

[0209] In summary, the significant advantages of this invention compared to existing technologies are summarized as follows:

[0210] 1) This invention can effectively solve the problem of environmental degradation through multi-sensor fusion;

[0211] 2) This invention utilizes an octree for map updating, which enables incremental map updates and effectively reduces the amount of map data storage.

[0212] 3) This invention adopts a tightly coupled fusion approach, which integrates the IMU / wheel speed odometer with the original feature point cloud data. During the point cloud matching process, the measurement information provided by the IMU and wheel speed odometer is considered, and a comprehensive objective function is constructed for the three to obtain more accurate pose estimation results.

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

Claims

1. A method for generating accessible maps for multi-story indoor buildings, characterized in that, Includes the following steps: Install lidar, inertial sensors, and wheel speed odometers on the driverless vehicle; A coordinate system is established based on lidar, inertial sensors, and wheel speed odometers, and the error status is obtained based on the coordinate system. The method for establishing a coordinate system includes: Establish a lidar coordinate system, an inertial sensor coordinate system, a wheel speed odometer coordinate system, and a global coordinate system, and transform the lidar coordinate system and the wheel speed odometer coordinate system into the inertial sensor coordinate system; The origin of the lidar coordinate system is located at the lidar scanning center. The X-axis points in the forward scanning direction of the lidar, the Z-axis is vertically upward along the lidar rotation axis, and the Y-axis points to the left of the lidar. The origin of the inertial sensor coordinate system is located at the center of the inertial sensor, the X-axis points directly forward of the autonomous vehicle, the Z-axis points vertically upward, and the Y-axis points to the left of the autonomous vehicle. The origin of the wheel speed odometer coordinate system is located at the center of the autonomous vehicle, the X-axis points directly forward of the autonomous vehicle, the Z-axis points vertically upward, and the Y-axis points to the left of the autonomous vehicle. The global coordinate system is the inertial sensor coordinate system at the moment the autonomous vehicle starts operating; The method for obtaining the error state includes: The system input is obtained through the outputs of the gyroscope and the accelerometer. The system noise is obtained by measuring the noise of the gyroscope and accelerometer and the random walk process of the gyroscope and accelerometer; The pose transformation relationship is obtained based on the system input and system noise. The relationship of error quantities is derived by deriving the relationship of pose transformation, and the error state is obtained based on the relationship of error quantities; A measurement model is constructed, and the multi-story building is measured using the measurement model to obtain point cloud data and autonomous vehicle pose data; The method for constructing the measurement model includes constructing a wheel speed-mileage measurement model and constructing a lidar measurement model; The method for constructing a wheel speed odometer measurement model includes: obtaining wheel speed odometer measurement information through the velocity of the inertial sensor coordinate system in the global coordinate system; obtaining observation values ​​based on the measurement information; obtaining the observation matrix of the wheel speed odometer based on the observation values; obtaining the observation equation of the wheel speed odometer by combining the observation matrix with measurement noise; and constructing a wheel speed odometer measurement model based on the observation equation of the wheel speed odometer. The method for constructing the lidar measurement model includes: A multi-story building is scanned by a lidar to obtain a three-dimensional point cloud. A feature point set is obtained from the three-dimensional point cloud. The feature point set is combined with the lidar observation values ​​to obtain the lidar observation equation. A lidar measurement model is constructed based on the lidar observation equation. The autonomous vehicle pose data is updated by obtaining the error state; A passable map is generated based on the point cloud data and the updated pose data; The process of generating a passable map includes: Accessible areas are obtained by detecting accessible areas based on point cloud data. Accessible areas are combined with updated pose data to construct a accessible map. Accessible area detection includes ground accessible area detection and stair detection. The staircase detection process includes: The dataset of non-ground accessible areas is filtered out, and the non-ground accessible area dataset is converted from cylindrical coordinate system to rectangular coordinate system to obtain point cloud set. Based on the riser, tread and stair width of the stair, the point cloud set is used to search for stair plane to obtain initial stair plane. Then, the initial stair plane is filtered by line fitting to obtain stair region.

2. The method for generating accessible maps for multi-story indoor buildings according to claim 1, characterized in that, The process of detecting passable areas on the ground includes: cropping the point cloud data according to the installation height of the lidar to obtain cropped point cloud data; dividing the point cloud data into fan-shaped segments from a top-down view, with the segmentation angle being the horizontal resolution of the lidar; converting the points in the area from a rectangular coordinate system to a cylindrical coordinate system; sorting the rays at the same segmentation angle according to their radius to obtain a sequential dataset; comparing the slopes of three adjacent points of a ray to obtain the slopes of the three adjacent points; and determining whether the detected area is a passable area on the ground based on the slopes.

3. The method for generating accessible maps for multi-story indoor buildings according to claim 1, characterized in that, The process of generating a passable map includes: A raster map is constructed based on octomap. Octree map ray tracing is performed on the point cloud of the passable area to obtain newly added and disappeared graticules in the global passable area raster map. The newly added and disappeared graticules are combined with the pose of the unmanned vehicle in the global frame to generate the passable map.

Images