CN112630745A - Environment mapping method and device based on laser radar - Google Patents
Environment mapping method and device based on laser radar Download PDFInfo
- Publication number
- CN112630745A CN112630745A CN202011551838.1A CN202011551838A CN112630745A CN 112630745 A CN112630745 A CN 112630745A CN 202011551838 A CN202011551838 A CN 202011551838A CN 112630745 A CN112630745 A CN 112630745A
- Authority
- CN
- China
- Prior art keywords
- pose
- equipment
- environment
- frame
- current
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000013507 mapping Methods 0.000 title claims abstract description 50
- 238000000034 method Methods 0.000 title claims abstract description 43
- 230000008859 change Effects 0.000 claims description 42
- 230000007613 environmental effect Effects 0.000 claims description 18
- 238000001514 detection method Methods 0.000 claims description 7
- 230000008569 process Effects 0.000 description 11
- 238000006073 displacement reaction Methods 0.000 description 9
- 230000006870 function Effects 0.000 description 6
- 238000010276 construction Methods 0.000 description 5
- 238000012545 processing Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 239000011159 matrix material Substances 0.000 description 3
- 230000009466 transformation Effects 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000019771 cognition Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000012216 screening Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
Landscapes
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Traffic Control Systems (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
The application relates to an environment mapping method and device based on a laser radar, self-moving equipment and a storage medium, belonging to the technical field of intelligent navigation, wherein the method comprises the following steps: acquiring an ith environment frame by using a laser radar, and determining feature points in the ith environment frame; selecting pose values one by one in the estimated pose range of the current equipment as the pose to be detected of the equipment; determining a discrete grid corresponding to each characteristic point according to a preset grid mapping mode and the pose to be detected of the equipment; calculating the sum of preset values of the discrete grids corresponding to all the feature points, and correspondingly storing the sum of the preset values and the pose to be detected of the equipment, wherein the preset values are used for reflecting probability values of the feature points which do not exist in the discrete grids; selecting a target pose value with the minimum sum of corresponding preset values from all pose values, and setting the target pose value as the current actual pose of the equipment; and constructing a current local environment map according to the current actual pose of the equipment and the characteristic points in the ith environment frame.
Description
Technical Field
The application relates to the technical field of intelligent navigation, in particular to an environment mapping method and device based on a laser radar, a self-moving device and a storage medium.
Background
With the gradual development and improvement of machine intelligent technology, the self-moving equipment gradually becomes common intelligent equipment in production and life. The current self-moving equipment can be applied to various scenes such as families, markets, factories, outdoors and the like, self-plans a traveling route according to actual needs in different scenes, and performs self-movement of the equipment based on the planned traveling route.
In particular, the user may input the destination point on the self-moving device, or the self-moving device may autonomously determine the destination point based on a preset configuration. And then, the self-mobile equipment can firstly determine the map coordinates corresponding to the destination point through a built-in environment map of the current scene, and then plan all routes reaching the map coordinates according to all passable paths recorded in the environment map. Furthermore, the self-moving device can determine an actual optimal route according to a user instruction or a preset routing rule, map the actual optimal route into an actual scene, and then guide the user or automatically go to a destination.
In the process of implementing the present application, the inventors found that the above-mentioned technology has at least the following problems:
when the self-moving is realized, the self-moving device is generally required to have the capability of analyzing the surrounding environment and autonomously make a decision according to the cognition of the environment so as to complete the self-moving task. Therefore, the self-moving device needs to be capable of identifying the scene environment, establishing a corresponding scene map, and further positioning the position of the device and planning a moving path. For outdoor positioning and navigation, the GPS can be considered, but for indoor environment, due to its higher complexity, the GPS cannot make a corresponding judgment on the indoor complex environment. Therefore, an effective identification method suitable for various different scene environments is needed.
Disclosure of Invention
In order to effectively and accurately identify different scene environments, the embodiment of the application provides an environment mapping method and device based on a laser radar, a self-moving device and a storage medium. The technical scheme is as follows:
in a first aspect, an embodiment of the present application provides an environment mapping method based on a laser radar, where the method includes:
acquiring an ith environment frame by using a laser radar, and determining feature points in the ith environment frame;
selecting pose values one by one in the estimated pose range of the current equipment as the pose to be detected of the equipment;
determining a discrete grid corresponding to each feature point according to a preset grid mapping mode and the pose to be detected of the equipment;
calculating the sum of preset values of the discrete grids corresponding to all the feature points, and correspondingly storing the sum of the preset values and the pose to be detected of the equipment, wherein the preset values are used for reflecting probability values of the discrete grids without the feature points;
selecting a target pose value with the minimum sum of corresponding preset values from all pose values, and setting the target pose value as the current actual pose of the equipment;
and constructing a current local environment map according to the current actual pose of the equipment and the characteristic points in the ith environment frame.
Based on the technical scheme, the laser radar is used for continuously acquiring the environment frames, the local environment maps corresponding to the environment frames are continuously constructed, and finally the local environment maps of the multiple frames are spliced and combined, so that the global environment map with higher accuracy and perfect scene details can be obtained in different scenes.
Optionally, the method further includes:
and determining the estimated pose range of the current equipment according to the equipment pose change rate, the scanning frequency of the laser radar and the actual pose of the equipment corresponding to the (i-1) th environmental frame.
Based on the technical scheme, on the basis of the actual pose of the previous frame of equipment, the pose change process possibly occurring in the equipment is considered, and the estimated pose range which is more reasonable and accurate can be obtained.
Optionally, the determining the discrete grid corresponding to each feature point according to a preset grid mapping mode and the pose to be detected of the device includes:
determining a relative map pose of each feature point according to the pose to be detected of the equipment and the relative equipment pose of each feature point;
and determining the discrete grid corresponding to each feature point according to a preset grid mapping mode and the relative map pose.
Based on the technical scheme, the relative device pose of the feature points is converted into the relative map pose by using the device pose, so that the discrete grids corresponding to the feature points can be obtained.
Optionally, the method further includes:
determining all target discrete grids corresponding to the feature points of the (i-1) th environmental frame, and setting preset values of all the target discrete grids as 0;
and setting the minimum Euclidean distance value from each other discrete grid to any one target discrete grid as a preset value of the other discrete grids.
Based on the technical scheme, the preset values of the discrete grids are updated according to the mapping relation between the feature points in the previous frame and the discrete grids, and the Euclidean distance is used as the preset values of the discrete grids, so that assignment updating of the discrete grids can be completed quickly and efficiently.
Optionally, the method further includes:
and if the discrete grid a corresponding to the target characteristic point in the (i-1) th environment frame is different from the discrete grid b corresponding to the target characteristic point in the (i-2) th environment frame, setting the discrete grid corresponding to the target characteristic point as the discrete grid b.
Based on the technical scheme, when the discrete grids corresponding to the feature points in the two frames are different, the discrete grids corresponding to the feature points in the previous frame are selected as the standard, so that the condition of larger deviation of the final environment map caused by continuous accumulation of errors in the map building process can be effectively reduced.
Optionally, before constructing the current local environment map according to the current actual pose of the device and the feature points in the ith environment frame, the method further includes:
acquiring a pose change record of the equipment in the current frame through a pose sensor preset on the equipment;
and adjusting the current actual pose of the equipment according to the pose change record and the actual pose of the equipment corresponding to the (i-1) th environment frame.
Based on the technical scheme, the actual pose of the equipment is adjusted by using the pose change record of the equipment, so that the accuracy of the adjusted actual pose of the equipment can be improved, and the influence caused by algorithm errors is reduced.
Optionally, the setting the target pose value to be before the current actual pose of the device further includes:
and if the sum of the preset values is larger than the specified early warning threshold value, stopping constructing the local environment map corresponding to the current frame, and adjusting the estimated pose range of the current device according to the pose change record of the device in the ith environment frame and the actual pose of the device corresponding to the (i-1) th environment frame.
Based on the technical scheme, when the mapping relation between the feature point and the discrete grid has larger deviation, the estimated pose range is redefined by using the pose change record and the actual pose of the equipment in the previous frame, so that the influence of errors on image construction can be reduced.
In a second aspect, an embodiment of the present application further provides an environment mapping apparatus based on a laser radar, where the apparatus includes:
the environment scanning module is used for acquiring an ith environment frame by using a laser radar and determining a feature point in the ith environment frame;
the pose selection module is used for selecting pose values one by one in the estimated pose range of the current equipment as the pose to be detected of the equipment;
the grid mapping module is used for determining a discrete grid corresponding to each feature point according to a preset grid mapping mode and the pose to be detected of the equipment;
the pose detection module is used for calculating the sum of preset values of the discrete grids corresponding to all the feature points and correspondingly storing the sum of the preset values and the pose to be detected of the equipment, wherein the preset values are used for reflecting probability values of the feature points which do not exist in the discrete grids;
the pose determining module is used for selecting a target pose value with the minimum sum of corresponding preset values from all pose values and setting the target pose value as the current actual pose of the equipment;
and the map building module is used for building a current local environment map according to the current actual pose of the equipment and the characteristic points in the ith environment frame.
Optionally, the pose selection module is further configured to:
and determining the estimated pose range of the current equipment according to the equipment pose change rate, the scanning frequency of the laser radar and the actual pose of the equipment corresponding to the (i-1) th environmental frame.
Optionally, the mesh mapping module is specifically configured to:
determining a relative map pose of each feature point according to the pose to be detected of the equipment and the relative equipment pose of each feature point;
and determining the discrete grid corresponding to each feature point according to a preset grid mapping mode and the relative map pose.
Optionally, the mesh mapping module is further configured to:
determining all target discrete grids corresponding to the feature points of the (i-1) th environmental frame, and setting preset values of all the target discrete grids as 0;
and setting the minimum Euclidean distance value from each other discrete grid to any one target discrete grid as a preset value of the other discrete grids.
Optionally, the mesh mapping module is further configured to:
and if the discrete grid a corresponding to the target characteristic point in the (i-1) th environment frame is different from the discrete grid b corresponding to the target characteristic point in the (i-2) th environment frame, setting the discrete grid corresponding to the target characteristic point as the discrete grid b.
Optionally, the pose determination module is further configured to:
acquiring a pose change record of the equipment in the current frame through a pose sensor preset on the equipment;
and adjusting the current actual pose of the equipment according to the pose change record and the actual pose of the equipment corresponding to the (i-1) th environment frame.
Optionally, the map building module is further configured to:
and if the sum of the preset values is larger than the specified early warning threshold value, stopping constructing the local environment map corresponding to the current frame, and adjusting the estimated pose range of the current device according to the pose change record of the device in the ith environment frame and the actual pose of the device corresponding to the (i-1) th environment frame.
In a third aspect, there is provided a self-moving device comprising a processor and a memory, the memory having stored therein at least one instruction, at least one program, set of codes, or set of instructions, which is loaded and executed by the processor to implement the lidar based environment mapping method according to the first aspect.
In a fourth aspect, there is provided a computer readable storage medium having stored therein at least one instruction, at least one program, a set of codes, or a set of instructions, which is loaded and executed by a processor to implement the lidar based environment mapping method according to the first aspect.
In summary, the present application has the following beneficial effects:
by adopting the environment mapping method based on the laser radar, the self-moving equipment can continuously scan surrounding environment frames through the laser radar, and then the actual pose of the equipment corresponding to each frame is deduced by utilizing the mapping condition of the characteristic points and the discrete grids in the environment frames, so that the actual pose of each characteristic point in the current frame is determined on the basis of the actual pose of the equipment, and the construction of the local environment map is completed. Therefore, the laser radar is used for continuously acquiring the environment frames, the local environment maps corresponding to the environment frames are continuously constructed, and finally, the local environment maps of the multiple frames are spliced and combined, so that the global environment map with higher accuracy and perfect scene details can be obtained in different scenes.
Drawings
FIG. 1 is a flowchart of an environment mapping method based on lidar in an embodiment of the present application;
FIG. 2 is a schematic diagram of an estimated pose range of a self-moving device in an embodiment of the present application;
fig. 3 is a schematic diagram of a discrete grid labeled with preset values in an embodiment of the present application;
fig. 4 is a schematic structural diagram of an environment mapping device based on a laser radar in the embodiment of the present application.
Detailed Description
In order to make the objects, technical solutions and advantages of the present application more apparent, the present application is further described in detail below with reference to fig. 1-4 and the embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the present application and are not intended to limit the present application.
The embodiment of the application provides an environment map building method based on a laser radar, which can be applied to self-moving equipment (which can be simply referred to as equipment in the follow-up process), wherein the self-moving equipment can be any intelligent equipment with the functions of map building, positioning, navigation and moving, such as an intelligent guiding robot, an intelligent transportation robot and the like. The map building function can be realized by detecting the environment in the scene of the self-moving equipment through an environment detection device arranged on the self-moving equipment and creating an environment map of the current scene based on a detection result, the positioning function can be realized by detecting the nearby environment characteristics through the environment detection device by the self-moving equipment and determining the pose of the self-moving equipment in the environment map according to the environment characteristics, the navigation function can be realized by planning the self-moving equipment to move a route between the self-moving equipment and a destination point in the environment map according to the pose of the self-moving equipment and the coordinates of the destination point, and the moving function can be realized by generating moving parameters according to the moving route and driving the moving device according to the moving parameters. In view of the mapping function, the embodiment provides an environment mapping method which utilizes a laser radar for detection and can be applied to different application scenarios.
The process flow shown in fig. 1 will be described in detail below with reference to specific embodiments, and the contents may be as follows:
101, acquiring an ith environment frame by using a laser radar, and determining feature points in the ith environment frame.
In implementation, when the mobile device starts to build a map, the position of the mobile device may be set as a coordinate origin, the laser radar is used to continuously scan the surrounding environment of the device in the current scene to obtain a plurality of continuous environment frames, and the plurality of environment frames are used to build an initial local environment map. And then, the self-moving device can move in the current scene, and continuously utilizes the surrounding environment of the laser radar scanning device in the moving process, so that local environment maps at different positions and angles can be continuously generated by utilizing the environment frames obtained by scanning. Specifically, taking the process of creating the local environment map by using the ith environment frame as an example, the self-moving device may first scan the surrounding environment by using the laser radar to obtain the ith environment frame including the point cloud data of the object around the current device, and then may determine all the feature points included in the ith environment frame according to the preset feature point screening rule. The feature point filtering rule may be set by a technician on the self-moving device according to actual scene needs, which is not limited in this embodiment.
And 102, selecting pose values one by one in the estimated pose range of the current equipment as the pose to be detected of the equipment.
In implementation, in the process of creating the local environment map for the ith environment frame, the self-moving device needs to determine the current pose of the self-moving device first, and then perform positioning of the surrounding objects in the scene environment based on the current pose of the self-moving device. Therefore, when the self-moving equipment acquires the ith environment frame, one pose value can be selected one by one in the estimated pose range of the current equipment to serve as the current pose of the self-moving equipment, and the self-moving equipment can also be called as the pose to be detected of the equipment. Here, the estimated pose range of the device may be a value range of poses that all devices may possess, derived from the mobile device according to historical pose information of the device. It is worth mentioning that the pose xi of the mobile device can have values in 6 dimensions, and specifically, the pose xi can include 3 coordinate values "X, Y, Z" on an X axis, a Y axis and a Z axis in a three-dimensional space coordinate system, and 3 angle values "rx, ry, rz" corresponding to three euler angles of a roll angle, a pitch angle and a heading angle.
Referring to fig. 2, the size of the estimated pose range can be defined as the maximum displacement on the X-axis, Y-axis, and Z-axis in the three-dimensional space coordinate system:
and the maximum deviation of three Euler angles of roll angle, pitch angle and course angle:
=4 °, and the selection step length in the estimated pose range may be set to be displacement Δ s =0.05m and angle Δ θ =0.2 °, that is, the coordinate difference of two pose values selected in the estimated pose range in any axial direction should not be less than Δ s, or the angle difference of any euler angle should not be less than Δ θ, and the selection step length is set to be equal to the displacement Δ s =0.05m and the angle Δ θ =0.2 °, and the selection step length is set to be equal to the coordinate difference of two pose
,
,
，
,
,
Then there may be a value set of the estimated pose range corresponding to the ith environmental frame:
103, determining the discrete grid corresponding to each feature point according to a preset grid mapping mode and the pose to be detected of the equipment.
In implementation, after determining all the feature points in the ith environment frame, the self-moving device may determine, for each feature point, a discrete grid corresponding to the feature point in combination with the pose to be detected of the device according to a preset grid mapping manner. The discrete grid may be obtained by dividing the current scene environment into a plurality of continuous and non-overlapping three-dimensional grids according to a preset grid size, and may be represented as follows: and M is rX multiplied by rY multiplied by rZ, so that the discrete grid corresponding to the characteristic point is determined, namely the three-dimensional coordinate of the characteristic point is determined to be in the space corresponding to the discrete grid.
And 104, calculating the sum of preset values of the discrete grids corresponding to all the feature points, and correspondingly storing the sum of the preset values and the pose to be detected of the equipment.
The preset value substitution reflects the probability value of the absence of the feature points in each discrete grid.
In implementation, after the self-moving device divides the space around the device into a plurality of discrete grids, each discrete grid may be assigned with a value (which may be referred to as a preset value of the discrete grid), and the specific value may be referred to as a probability value of the presence of the feature point in each discrete grid, where a smaller preset value indicates a higher probability of the presence of the feature point in the discrete grid, and otherwise, the smaller preset value indicates a lower probability. Therefore, after the discrete grids corresponding to each feature point in the ith environment frame are determined, the sum of preset values of the discrete grids corresponding to all the feature points can be calculated, and the calculated sum of the preset values can be stored in correspondence with the pose to be detected of the equipment. It is to be understood that each pose value in the estimated pose range mentioned in step 102 may correspond to a sum of preset values.
And 105, selecting a target pose value with the minimum sum of corresponding preset values from all pose values, and setting the target pose value as the current actual pose of the equipment.
In implementation, after the mobile device completes storage of the sum of the preset values corresponding to each pose value in the estimated pose range, a target pose value with the minimum sum of the corresponding preset values can be selected from all the pose values, and then the target pose value is set as the current device actual pose. It is easy to understand that the sum of the preset values is minimum, that is, the discrete grid corresponding to the feature point in the current environment frame is represented, the probability of the feature point existing on the whole is higher, and the mapping condition of the feature point and the discrete grid is more expected, so that the corresponding target pose value can be considered to be more in line with the actual pose of the mobile device.
And 106, constructing a current local environment map according to the current actual pose of the equipment and the characteristic points in the ith environment frame.
In implementation, after the actual pose of the device in the ith environment frame is set, the pose of each feature point relative to the actual scene environment can be determined by taking the actual pose of the device as a reference and combining the poses of the feature points in the ith environment frame relative to the self-moving device, so that the current local environment map can be constructed based on the poses of the feature points relative to the actual scene environment. Furthermore, the self-moving device can splice and combine all local environment maps to generate an overall environment map in the current application scene.
Further, the determination method of the estimated pose range of the current device may specifically be as follows: and determining the estimated pose range of the current equipment according to the pose change rate of the equipment, the scanning frequency of the laser radar and the actual pose of the equipment corresponding to the (i-1) th environmental frame.
In implementation, when the estimated pose range of the ith environment frame device is determined, the self-moving device can firstly conduct referenceAnd determining the interval duration of two frames by the scanning frequency of the laser radar, calculating the maximum range of the actual pose change of the equipment between the two frames by referring to the pose change rate of the equipment, and further superposing the maximum range on the basis of the actual pose of the equipment corresponding to the (i-1) th environmental frame to determine the estimated pose range of the current equipment. For example, the interval between two frames is 1s, the pose change rate of the device is the displacement change rate V =0.2m/s, the angle change rate is theta =4 °/s, and the actual pose of the device corresponding to the i-1 th environmental frame is
Then estimate the pose range as
。
For step 103, the determination method of the discrete grid corresponding to the feature point may specifically be as follows: determining a relative map pose of each feature point according to the pose to be detected of the equipment and the relative equipment pose of each feature point; and determining the discrete grid corresponding to each characteristic point according to a preset grid mapping mode and a relative map pose.
In implementation, after determining the feature points in the ith environment frame, the self-moving device may calculate a pose (which may be referred to as a relative device pose) of each feature point with respect to the self-moving device, where the relative device pose is a pose value of the feature point in a three-dimensional space coordinate system constructed with the self-moving device as an origin and a laser radar emission direction as an X axis. In detail, if the relative device pose of the feature point k is
The pose of the device to be measured is
Then, the pose to be detected can be firstly based on the equipment
Determining a coordinate transformation matrix from a mobile device relative to a map origin
Including a rotation matrix
And a displacement vector
Thereby making it possible to use a coordinate transformation matrix
Carrying out coordinate transformation on the characteristic point k to generate a relative map pose of the characteristic point k
. And then, the self-moving equipment can determine the discrete grids corresponding to each feature point according to a preset grid mapping mode and the relative map poses of the feature points. For example, the three-dimensional space coordinates are valued at
Is mapped to discrete grid 1.
Further, the assignment process for discrete grids may refer to the following processes: determining all target discrete grids corresponding to the feature points of the (i-1) th environmental frame, and setting the preset values of all the target discrete grids as 0; and setting the minimum Euclidean distance value of each other discrete grid to any target discrete grid as a preset value of the other discrete grids.
In implementation, after the actual pose of the device corresponding to the i-1 th environment frame is determined, the self-moving device can update the preset value of the discrete grid based on the feature point relative map pose of the i-1 th environment frame. Specifically, the preset values of all target discrete grids corresponding to the feature points may be set to 0, for other discrete grids except the target discrete grid, the euclidean distance values between the other discrete grids and the adjacent target discrete grid may be calculated, and then the minimum euclidean distance value is set to the preset values of the other discrete grids. Referring to fig. 3, taking a plane coordinate system as an example, the numerical value marked in the discrete grid is the preset value of the discrete grid.
Further, when updating the preset value of the discrete grid, the following processing may exist: and if the discrete grid a corresponding to the target characteristic point in the (i-1) th environment frame is different from the discrete grid b corresponding to the target characteristic point in the (i-2) th environment frame, setting the discrete grid corresponding to the target characteristic point as the discrete grid b.
In implementation, when determining the discrete grid corresponding to the feature point in the i-1 th environmental frame, if it is found that the target feature point also exists in the i-2 th environmental frame, and the discrete grid b corresponding to the target feature point in the i-2 th environmental frame does not belong to the same discrete grid as the discrete grid a corresponding to the target feature point in the i-1 th environmental frame, the discrete grid corresponding to the target feature point in the i-1 th environmental frame may be set as the discrete grid b.
In another embodiment, the finally determined actual pose of the device may be adjusted according to the pose change condition recorded by the device, and the corresponding processing may be as follows: acquiring a pose change record of the equipment in the current frame through a pose sensor preset on the equipment; and adjusting the current actual pose of the equipment according to the pose change record and the actual pose of the equipment corresponding to the (i-1) th environment frame.
In implementation, a pose sensor can be arranged on the self-moving device in advance and used for measuring the change situation of the pose in the traveling process of the self-moving device. The pose sensor may include at least a displacement sensing assembly for registering a change in displacement of the device and a gyroscope sensor for registering a change in orientation of the device, such that the measured pose change includes at least a change in displacement and a change in orientation. Based on the method, when the self-moving device scans a plurality of environment frames through the laser radar, the self-moving device can simultaneously acquire the pose change record of the device in each frame by using the preset pose sensor, namely at least the displacement change record and the orientation change record of the device in each frame are acquired. Furthermore, when the ith environment frame is generated by scanning from the mobile device, a pose change record of the device in the ith environment frame can be obtained first, then the pose change record is superposed by taking the actual pose of the device corresponding to the (i-1) th environment frame as a reference, so as to obtain a motion estimated pose of the device corresponding to the ith environment frame, and finally the motion estimated pose of the device obtained in the step 105 can be used for adjusting the actual pose of the device. The specific adjustment mode can adopt a mode of setting weights, namely different weights are given to the actual pose of the equipment and the estimated pose of the motion of the equipment, then summation is carried out, and the pose value obtained by summation is used as the final actual pose of the equipment.
In another embodiment, the existence of the error may be identified by a minimum value of a sum of preset values corresponding to each frame, and the corresponding processing may be as follows: and if the sum of the preset values is larger than the specified early warning threshold value, stopping constructing the local environment map corresponding to the current frame, and adjusting the estimated pose range of the current device according to the pose change record of the device in the ith environment frame and the actual pose of the device corresponding to the (i-1) th environment frame.
In implementation, after the target pose value with the minimum sum of the preset values is selected from all the pose values by the mobile device, whether the minimum value of the preset sum is larger than the early warning threshold value or not can be judged firstly. If the difference is larger than the preset threshold, the fact that the position and posture of the detected feature point in the current frame are deviated to a large degree on the whole is shown, so that the relevant data of the current frame can be discarded, and the construction of the local environment map corresponding to the current frame is stopped. Furthermore, the self-moving device can call the pose change record of the device in the ith environment frame and the actual pose of the device corresponding to the (i-1) th environment frame, estimate the possible pose of the device corresponding to the current frame based on the pose change record and the actual pose of the device, and further adjust the estimated pose range of the current device by taking the possible pose of the device as a reference. Therefore, the self-moving equipment can calculate the sum of preset values corresponding to each pose value in the adjusted estimated pose range again.
By adopting the environment mapping method based on the laser radar, the self-moving equipment can continuously scan surrounding environment frames through the laser radar, and then the actual pose of the equipment corresponding to each frame is deduced by utilizing the mapping condition of the characteristic points and the discrete grids in the environment frames, so that the actual pose of each characteristic point in the current frame is determined on the basis of the actual pose of the equipment, and the construction of the local environment map is completed. Therefore, the laser radar is used for continuously acquiring the environment frames, the local environment maps corresponding to the environment frames are continuously constructed, and finally, the local environment maps of the multiple frames are spliced and combined, so that the global environment map with higher accuracy and perfect scene details can be obtained in different scenes.
Based on the same technical concept, an embodiment of the present application further provides an environment map building apparatus based on a laser radar, as shown in fig. 4, the apparatus includes:
an environment scanning module 401, configured to acquire an ith environment frame by using a laser radar, and determine a feature point in the ith environment frame;
a pose selecting module 402, configured to select pose values one by one as a pose to be detected of the device in an estimated pose range of the current device;
the grid mapping module 403 is configured to determine a discrete grid corresponding to each feature point according to a preset grid mapping manner and the pose to be detected of the device;
a pose detection module 404, configured to calculate a sum of preset values of the discrete grids corresponding to all the feature points, and store the preset value sum and the pose to be detected of the device, where the preset value represents a probability value that no feature point exists in each discrete grid;
a pose determining module 405, configured to select a target pose value with a minimum sum of corresponding preset values from all pose values, and set the target pose value as a current actual pose of the device;
and a map building module 406, configured to build a current local environment map according to the current actual pose of the device and the feature points in the ith environment frame.
Optionally, the pose selection module 402 is further configured to:
and determining the estimated pose range of the current equipment according to the equipment pose change rate, the scanning frequency of the laser radar and the actual pose of the equipment corresponding to the (i-1) th environmental frame.
Optionally, the mesh mapping module 403 is specifically configured to:
determining a relative map pose of each feature point according to the pose to be detected of the equipment and the relative equipment pose of each feature point;
and determining the discrete grid corresponding to each feature point according to a preset grid mapping mode and the relative map pose.
Optionally, the mesh mapping module 403 is further configured to:
determining all target discrete grids corresponding to the feature points of the (i-1) th environmental frame, and setting preset values of all the target discrete grids as 0;
and setting the minimum Euclidean distance value from each other discrete grid to any one target discrete grid as a preset value of the other discrete grids.
Optionally, the mesh mapping module 403 is further configured to:
and if the discrete grid a corresponding to the target characteristic point in the (i-1) th environment frame is different from the discrete grid b corresponding to the target characteristic point in the (i-2) th environment frame, setting the discrete grid corresponding to the target characteristic point as the discrete grid b.
Optionally, the pose determination module 405 is further configured to:
acquiring a pose change record of the equipment in the current frame through a pose sensor preset on the equipment;
and adjusting the current actual pose of the equipment according to the pose change record and the actual pose of the equipment corresponding to the (i-1) th environment frame.
Optionally, the map building module 406 is further configured to:
and if the sum of the preset values is larger than the specified early warning threshold value, stopping constructing the local environment map corresponding to the current frame, and adjusting the estimated pose range of the current device according to the pose change record of the device in the ith environment frame and the actual pose of the device corresponding to the (i-1) th environment frame.
The self-moving equipment can continuously scan surrounding environment frames through the laser radar, and then deduces the actual pose of the equipment corresponding to each frame by utilizing the mapping condition of the feature points and the discrete grids in the environment frames, thereby determining the actual pose of each feature point in the current frame on the basis of the actual pose of the equipment to complete the construction of the local environment map. Therefore, the laser radar is used for continuously acquiring the environment frames, the local environment maps corresponding to the environment frames are continuously constructed, and finally, the local environment maps of the multiple frames are spliced and combined, so that the global environment map with higher accuracy and perfect scene details can be obtained in different scenes.
An embodiment of the present application further provides a self-moving device, which includes a processor and a memory, where the memory stores at least one instruction, at least one program, a set of codes, or a set of instructions, and the at least one instruction, the at least one program, the set of codes, or the set of instructions is loaded and executed by the processor to implement the processing according to steps 101 to 106.
It will be understood by those skilled in the art that all or part of the steps for implementing the above embodiments may be implemented by hardware, or may be implemented by a program instructing relevant hardware, where the program may be stored in a computer-readable storage medium, and the above-mentioned storage medium may be a read-only memory, a magnetic disk or an optical disk, etc.
The foregoing is a preferred embodiment of the present application and is not intended to limit the scope of the application in any way, and any features disclosed in this specification (including the abstract and drawings) may be replaced by alternative features serving equivalent or similar purposes, unless expressly stated otherwise. That is, unless expressly stated otherwise, each feature is only an example of a generic series of equivalent or similar features.
Claims (10)
1. An environment mapping method based on laser radar is characterized by comprising the following steps:
acquiring an ith environment frame by using a laser radar, and determining feature points in the ith environment frame;
selecting pose values one by one in the estimated pose range of the current equipment as the pose to be detected of the equipment;
determining a discrete grid corresponding to each feature point according to a preset grid mapping mode and the pose to be detected of the equipment;
calculating the sum of preset values of the discrete grids corresponding to all the feature points, and correspondingly storing the sum of the preset values and the pose to be detected of the equipment, wherein the preset values are used for reflecting probability values of the discrete grids without the feature points;
selecting a target pose value with the minimum sum of corresponding preset values from all pose values, and setting the target pose value as the current actual pose of the equipment;
and constructing a current local environment map according to the current actual pose of the equipment and the characteristic points in the ith environment frame.
2. The method of claim 1, further comprising:
and determining the estimated pose range of the current equipment according to the equipment pose change rate, the scanning frequency of the laser radar and the actual pose of the equipment corresponding to the (i-1) th environmental frame.
3. The method according to claim 1, wherein the determining the discrete grid corresponding to each feature point according to a preset grid mapping mode and the pose to be detected of the device comprises:
determining a relative map pose of each feature point according to the pose to be detected of the equipment and the relative equipment pose of each feature point;
and determining the discrete grid corresponding to each feature point according to a preset grid mapping mode and the relative map pose.
4. The method of claim 1, further comprising:
determining all target discrete grids corresponding to the feature points of the (i-1) th environmental frame, and setting preset values of all the target discrete grids as 0;
and setting the minimum Euclidean distance value from each other discrete grid to any one target discrete grid as a preset value of the other discrete grids.
5. The method of claim 4, further comprising:
and if the discrete grid a corresponding to the target characteristic point in the (i-1) th environment frame is different from the discrete grid b corresponding to the target characteristic point in the (i-2) th environment frame, setting the discrete grid corresponding to the target characteristic point as the discrete grid b.
6. The method of claim 1, wherein before constructing a current local environment map according to the current device actual pose and the feature points in the ith environment frame, further comprising:
acquiring a pose change record of the equipment in the current frame through a pose sensor preset on the equipment;
and adjusting the current actual pose of the equipment according to the pose change record and the actual pose of the equipment corresponding to the (i-1) th environment frame.
7. The method of claim 1, wherein the setting the target pose value to be prior to the current device actual pose, further comprises:
and if the sum of the preset values is larger than the specified early warning threshold value, stopping constructing the local environment map corresponding to the current frame, and adjusting the estimated pose range of the current device according to the pose change record of the device in the ith environment frame and the actual pose of the device corresponding to the (i-1) th environment frame.
8. An environment mapping device based on laser radar, the device comprising:
the environment scanning module is used for acquiring an ith environment frame by using a laser radar and determining a feature point in the ith environment frame;
the pose selection module is used for selecting pose values one by one in the estimated pose range of the current equipment as the pose to be detected of the equipment;
the grid mapping module is used for determining a discrete grid corresponding to each feature point according to a preset grid mapping mode and the pose to be detected of the equipment;
the pose detection module is used for calculating the sum of preset values of the discrete grids corresponding to all the feature points and correspondingly storing the sum of the preset values and the pose to be detected of the equipment, wherein the preset values are used for reflecting probability values of the feature points which do not exist in the discrete grids;
the pose determining module is used for selecting a target pose value with the minimum sum of corresponding preset values from all pose values and setting the target pose value as the current actual pose of the equipment;
and the map building module is used for building a current local environment map according to the current actual pose of the equipment and the characteristic points in the ith environment frame.
9. A self-moving device, comprising a processor and a memory, wherein the memory has stored therein at least one instruction, at least one program, set of codes, or set of instructions, which is loaded and executed by the processor to implement the lidar based environment mapping method according to any of claims 1 to 7.
10. A computer readable storage medium having stored therein at least one instruction, at least one program, a set of codes, or a set of instructions, which is loaded and executed by a processor to implement the method of mapping a lidar based environment according to any of claims 1 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011551838.1A CN112630745A (en) | 2020-12-24 | 2020-12-24 | Environment mapping method and device based on laser radar |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011551838.1A CN112630745A (en) | 2020-12-24 | 2020-12-24 | Environment mapping method and device based on laser radar |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN112630745A true CN112630745A (en) | 2021-04-09 |
Family
ID=75324523
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011551838.1A Pending CN112630745A (en) | 2020-12-24 | 2020-12-24 | Environment mapping method and device based on laser radar |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112630745A (en) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107677279A (en) * | 2017-09-26 | 2018-02-09 | 上海思岚科技有限公司 | It is a kind of to position the method and system for building figure |
| CN108401461A (en) * | 2017-12-29 | 2018-08-14 | 深圳前海达闼云端智能科技有限公司 | Three-dimensional mapping method, device and system, cloud platform, electronic equipment and computer program product |
| WO2019169540A1 (en) * | 2018-03-06 | 2019-09-12 | 斯坦德机器人（深圳）有限公司 | Method for tightly-coupling visual slam, terminal and computer readable storage medium |
| CN110849374A (en) * | 2019-12-03 | 2020-02-28 | 中南大学 | Underground environment positioning method, device, equipment and storage medium |
| CN110866496A (en) * | 2019-11-14 | 2020-03-06 | 合肥工业大学 | Robot positioning and mapping method and device based on depth image |
| WO2020233724A1 (en) * | 2019-05-23 | 2020-11-26 | 全球能源互联网研究院有限公司 | Visual slam-based grid operating environment map construction method and system |
-
2020
- 2020-12-24 CN CN202011551838.1A patent/CN112630745A/en active Pending
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107677279A (en) * | 2017-09-26 | 2018-02-09 | 上海思岚科技有限公司 | It is a kind of to position the method and system for building figure |
| CN108401461A (en) * | 2017-12-29 | 2018-08-14 | 深圳前海达闼云端智能科技有限公司 | Three-dimensional mapping method, device and system, cloud platform, electronic equipment and computer program product |
| WO2019169540A1 (en) * | 2018-03-06 | 2019-09-12 | 斯坦德机器人（深圳）有限公司 | Method for tightly-coupling visual slam, terminal and computer readable storage medium |
| WO2020233724A1 (en) * | 2019-05-23 | 2020-11-26 | 全球能源互联网研究院有限公司 | Visual slam-based grid operating environment map construction method and system |
| CN110866496A (en) * | 2019-11-14 | 2020-03-06 | 合肥工业大学 | Robot positioning and mapping method and device based on depth image |
| CN110849374A (en) * | 2019-12-03 | 2020-02-28 | 中南大学 | Underground environment positioning method, device, equipment and storage medium |
Non-Patent Citations (2)
| Title |
|---|
| 康俊民;赵祥模;徐志刚;: "基于特征几何关系的无人车轨迹回环检测", 中国公路学报, no. 01 * |
| 胡向勇;洪程智;吴世全;: "基于关键帧的点云建图方法", 热带地貌, no. 01 * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11127203B2 (en) | Leveraging crowdsourced data for localization and mapping within an environment | |
| US10278333B2 (en) | Pruning robot system | |
| US20210232845A1 (en) | Information processing apparatus, information processing method, and storage medium | |
| EP3629052A1 (en) | Data collecting method and system | |
| KR101539270B1 (en) | sensor fusion based hybrid reactive motion planning method for collision avoidance and autonomous navigation, recording medium and mobile robot for performing the method | |
| JP5018458B2 (en) | Coordinate correction method, coordinate correction program, and autonomous mobile robot | |
| US20180100740A1 (en) | Method and apparatus for planning path | |
| KR102075844B1 (en) | Localization system merging results of multi-modal sensor based positioning and method thereof | |
| KR20180047572A (en) | Method for building a grid map with mobile robot unit | |
| CN111487960A (en) | Mobile robot path planning method based on positioning capability estimation | |
| Teixeira et al. | Autonomous aerial inspection using visual-inertial robust localization and mapping | |
| CN111856499B (en) | Map construction method and device based on laser radar | |
| US12002237B2 (en) | Position coordinate derivation device, position coordinate derivation method, position coordinate derivation program, and system | |
| JP5728564B2 (en) | Robot system and map updating method | |
| CN112964263B (en) | Automatic drawing establishing method and device, mobile robot and readable storage medium | |
| JP6594008B2 (en) | Mobile control device, landmark, and program | |
| US20220187845A1 (en) | Method for estimating positioning of moving object by using big cell grid map, recording medium in which program for implementing same is stored, and computer program stored in medium in order to implement same | |
| CN112630745A (en) | Environment mapping method and device based on laser radar | |
| CN115962787A (en) | Map updating method, map updating apparatus, map automatic driving control method, map automatic driving control apparatus, map automatic driving control medium, and vehicle | |
| US20220282987A1 (en) | Information processing system, information processing device, and information processing method | |
| KR20210003065A (en) | Method and system for collecting data | |
| KR20200043329A (en) | Method and system for collecting data | |
| Canh et al. | Multisensor data fusion for reliable obstacle avoidance | |
| CN111504337A (en) | POI orientation determining method and device | |
| US20230400860A1 (en) | Enabling mobile robots for autonomous missions |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |