WO2021249387A1 - 一种视觉地图构建方法和移动机器人 - Google Patents
一种视觉地图构建方法和移动机器人 Download PDFInfo
- Publication number
- WO2021249387A1 WO2021249387A1 PCT/CN2021/098881 CN2021098881W WO2021249387A1 WO 2021249387 A1 WO2021249387 A1 WO 2021249387A1 CN 2021098881 W CN2021098881 W CN 2021098881W WO 2021249387 A1 WO2021249387 A1 WO 2021249387A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- map
- pose
- map node
- node
- current
- Prior art date
Links
- 230000000007 visual effect Effects 0.000 title claims abstract description 27
- 238000010276 construction Methods 0.000 title claims abstract description 20
- 238000005259 measurement Methods 0.000 claims abstract description 141
- 238000000034 method Methods 0.000 claims description 98
- 239000011159 matrix material Substances 0.000 claims description 80
- 238000012937 correction Methods 0.000 claims description 57
- 238000005457 optimization Methods 0.000 claims description 39
- 238000004590 computer program Methods 0.000 claims description 10
- 230000017105 transposition Effects 0.000 claims description 4
- 230000008569 process Effects 0.000 description 17
- 230000033001 locomotion Effects 0.000 description 14
- 238000010586 diagram Methods 0.000 description 8
- 230000006870 function Effects 0.000 description 4
- 238000012804 iterative process Methods 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000001960 triggered effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
- G05D1/0253—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting relative motion information from a plurality of images taken successively, e.g. visual odometry, optical flow
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Definitions
- This application relates to the technical field of visual navigation, and in particular, to a method for constructing a visual map and a mobile robot.
- the mobile robot When the mobile robot performs visual navigation, it collects images of the surrounding environment through the camera device, and completes the position determination and path recognition of the mobile robot based on the images. Visual navigation needs to rely on visual maps to complete.
- the visual map includes several map nodes, and each map node includes at least the pose of the map node. Among them, the pose of the map node represents the pose of the mobile robot when it is located at the map node. Taking a map based on ground texture as an example, the map node includes the pose of the map node and the texture information of the texture point image at the map node.
- the image registration is performed, the current pose of the mobile robot is calculated, and the ground texture-based Positioning navigation.
- a camera device is used to record the effective texture information of the ground during the mapping process and correspond to the current position coordinates to form a texture map.
- the embodiments of the present application provide a method for constructing a visual map and a mobile robot to improve the accuracy of the pose of the map node in the visual map.
- the embodiment of the present application provides a method for constructing a visual map, and the method includes:
- the mobile robot is controlled to travel based on the laser real-time positioning and map-building SLAM map, and the position and posture of each map node is obtained based on the SLAM navigation and positioning method.
- the optimization includes, for any two map nodes in the map nodes that meet the distance less than or equal to the first distance threshold: obtaining the poses between the two map nodes according to the poses of the map nodes.
- the first measurement value is obtained by obtaining the second measurement value of the pose between the two map nodes according to the inertial sensor data, and based on the error between the first measurement value and the second measurement value, the inertial sensor data of the two map nodes
- the posture of the optimized map node is used as the posture of the map node to be built.
- the map nodes that need to be constructed for the map to be built are set in the following manner: obtain a laser SLAM map, set a travel route based on the laser SLAM map, and set map nodes according to a set interval;
- the control of the mobile robot to travel based on the laser SLAM map includes:
- Control the mobile robot to collect the pose and inertial sensing data of the map node when it is at each map node. After the pose and inertial sensing data of all the map nodes are collected, perform the optimization of the acquired pose of the map node A step of.
- the mobile robot is controlled to travel based on the laser SLAM map according to the travel route set by the map node that needs to be constructed according to the map to be built, and the pose of each map node is acquired based on the SLAM navigation and positioning method,
- Record inertial sensor data at each map node including:
- the mobile robot is controlled to travel, and the laser SLAM map is established based on the laser SLAM navigation and positioning method during the travel, and the current location information of the mobile robot is determined according to the current laser SLAM map ,
- the mobile robot reaches the map node, collect the current inertial sensor data;
- the third map node and the current map node are taken as any two map nodes that meet the distance less than or equal to the first distance threshold to optimize the pose of the current map node, and the current map node is added after the optimization is completed.
- the collection of map nodes has been optimized
- the unoptimized fourth map node whose distance from the current map node is less than or equal to the first distance threshold, and use the fourth map node and the current map node as any two that meet the distance less than or equal to the first distance threshold.
- the map node is used to optimize the pose of the two map nodes, and after the optimization is completed, the fourth map node and the current map node are added to the optimized map node set;
- the acquiring the first measurement value of the pose between the two map nodes according to the pose of the map node includes:
- the acquiring the second measurement value of the position between the two map nodes according to the inertial sensor data includes:
- the projection of the first distance in the x direction at the first angle and the first distance r at the first angle are obtained.
- the inertial sensor data of the two map nodes is used as the constraint of the poses of the two map nodes, and the poses of the two map nodes are at least
- the pose of a map node is optimized, including:
- the pose of the first map node obtained based on the SLAM navigation and positioning method is used as the initial value of the pose of the first map node, and the pose of the second map node obtained based on the SLAM navigation and positioning method is used as the initial value of the pose of the second map node value;
- the least square method is used to iteratively obtain the optimized pose of the first map node and the second map node.
- the least square method is used to iteratively obtain the optimized pose of the first map node and the position of the second map node.
- Posture including:
- the iteration end condition includes: reaching the set number of iterations, the error is less than the set error threshold, the correction amount is less than one of the set correction threshold, or any combination thereof.
- the correction amount is calculated in the following manner:
- the Jacobian matrix is determined according to the current pose of the first map node and the current pose of the second map node.
- the any two map nodes are the optimized third map node and the current map node to be optimized
- the acquiring the first measurement value of the pose between the two map nodes according to the pose of the map node includes:
- the acquiring the second measurement value of the pose between the two map nodes according to the inertial sensor data includes:
- the projection of the first distance in the first angle in the x direction, and the first distance in the first angle The projection in the y direction and the first angle;
- the inertial sensor data of the two map nodes is used as the constraint of the poses of the two map nodes, and the poses of the two map nodes are at least
- the pose of a map node is optimized, including:
- the least square method is used to iteratively obtain the optimized pose of the current map node.
- using the least squares method to iteratively obtain the optimized pose of the current map node includes:
- the iteration end condition includes: reaching the set number of iterations, the error is less than the set error threshold, the correction amount is less than one of the set correction threshold, or any combination thereof.
- the correction amount is calculated in the following manner:
- the Jacobian matrix is determined according to the current value of the pose of the current map node.
- the map to be built is a ground texture map, and after each map node records the inertial sensor data, the method further includes:
- Said using the posture of the optimized map node as the posture of the map node of the map to be built includes:
- the texture map and the laser SLAM map are combined into one.
- the embodiment of the present application also provides a mobile robot, including:
- SLAM positioning module based on the SLAM navigation and positioning method to obtain the pose of each map node
- Acquisition module record inertial sensor data at each map node
- the map node pose optimization module for any two map nodes in the map node whose distance is less than or equal to the first distance threshold: obtain the first measurement value between the two map nodes according to the pose of the map node, and obtain according to the inertial sensor data
- the second measurement value between the two map nodes is based on the error between the first measurement value and the second measurement value, and the inertial sensor data of the two map nodes is used as the constraint of the poses of the two map nodes, Optimizing the pose of at least one map node among the poses of the two map nodes;
- the map generation module takes the position of the optimized map node as the position of the map node to be built.
- the visual map construction solution uses laser real-time positioning and map construction as auxiliary means, collects the movement information of the mobile robot at each map node through inertial sensors, and uses the position and posture of the map node obtained based on the SLAM navigation and positioning method Based on the first measurement value of the map node's position and the second measurement value obtained by the inertial sensor, the error between the first measurement value and the second measurement value is constructed, and the position of the map node is optimized through graph optimization to improve
- the pose accuracy of the map node is reduced, the workload and implementation period of the visual map construction process are reduced, and the quality of the texture map and the texture positioning accuracy are improved for the texture map based on the ground mark.
- FIG. 1 is a schematic diagram of a process for constructing a texture map according to Embodiment 1 of the application.
- Fig. 2 is a schematic diagram of a map including laser SLAM map information and texture map information.
- FIG. 3 is a schematic diagram of a process of constructing a texture map according to the second embodiment of the application.
- Figure 4 is a schematic diagram of a mobile robot.
- laser SLAM Simultaneous Localization And Mapping, instant positioning and map construction
- SLAM Simultaneous Localization And Mapping, instant positioning and map construction
- a method for constructing a visual map includes:
- control the mobile robot to travel based on the laser SLAM map, obtain the pose of each map node based on the SLAM navigation and positioning method, and record the inertial sensor data on each map node;
- the optimization includes, for any two map nodes in the map nodes that meet the distance less than or equal to the first distance threshold: obtaining the poses between the two map nodes according to the poses of the map nodes.
- the first measurement value is to obtain the second measurement value of the pose between the two map nodes according to the inertial sensor data, and based on the error between the first measurement value and the second measurement value, the inertial sensor data of the two map nodes is used as the two Constraints on the pose of the map node to optimize the pose of at least one map node among the poses of the two map nodes;
- the posture of the optimized map node is taken as the posture of the map node of the map to be built.
- the above-mentioned mobile robot may be an AGV (Automated Guided Vehicle), which is also called an AGV trolley.
- AGV Automated Guided Vehicle
- the above-mentioned mobile robot may also be a robot capable of moving in other forms.
- the mobile robot in the embodiment of the present application is equipped with an imaging device, and the mobile robot may also be equipped with an inertial sensor, a laser sensor, and the like.
- each map node corresponds to an area with a certain range in the actual environment.
- each map node may also be called a texture point.
- the images collected by the camera mounted on the mobile robot can also be called texture point images accordingly.
- the above-mentioned travel route is set according to the map nodes that need to be constructed for the map to be constructed.
- the control robot is controlled to follow the above-mentioned travel route.
- the execution subject of the embodiment of the present application may be the mobile robot itself.
- each step in the embodiment of the present application may be executed by the mobile robot itself.
- the execution subject of the embodiment of the present application may also be a server that can communicate with the mobile robot, that is, each step in the embodiment of the present application may be executed by the server.
- the laser SLAM navigation and positioning method mentioned in the embodiment of the present application can be understood as a method for navigation and positioning based on the laser SLAM map to obtain the positioning information of the mobile robot.
- the laser SLAM navigation and positioning method is also referred to as the SLAM navigation and positioning method for short.
- Pose includes position and posture, where the position can be expressed in horizontal and vertical coordinates, and the posture can be the angle that the mobile robot has rotated relative to the reference position.
- the horizontal direction may be a direction where a horizontal line in a horizontal plane where the mobile robot is located
- a vertical direction may be a direction perpendicular to the horizontal direction in the horizontal plane.
- the above-mentioned reference position may be preset.
- a texture map For ease of understanding, the construction of a texture map will be described as an example below. It should be understood that this application is not limited to constructing a texture map, and other visual maps constructed by capturing images of the surrounding environment by a camera device are applicable. In addition, for ease of description, the following will take a mobile robot as the execution subject as an example for description.
- FIG. 1 is a schematic diagram of a process of constructing a texture map in Embodiment 1 of the application.
- the map construction method includes the following steps 101-105. Steps 101-105 are performed by the mobile robot.
- Step 101 The mobile robot obtains a laser SLAM map of the environment covered by the texture map to be built.
- the laser SLAM map refers to the SLAM map constructed based on the operating environment data of the mobile robot collected by the laser sensor.
- the laser SLAM map can be loaded into a mobile robot with a laser SLAM positioning function and an inertial sensor.
- a laser SLAM map can be constructed based on a mobile robot with SLAM navigation and positioning functions.
- control the mobile robot to automatically move within the environment covered by the texture map to be built, or manually move the mobile robot to obtain the data collected by the laser sensor mounted on the mobile robot, and establish the laser SLAM based on the data collected by the laser sensor map.
- This process can be implemented using existing laser SLAM algorithms, for example, algorithms such as cartographer and hector.
- Step 102 The mobile robot sets the map nodes to be constructed for the texture map to be constructed.
- the mobile robot can determine the travel route of the mobile robot according to the laser SLAM map; combined with the determined travel route, set a series of discrete n map nodes at a set interval; n is a natural number greater than 1.
- the above-mentioned set distance may be 0.5 meters, 0.4 meters, and so on.
- the above-mentioned travel route may be set according to the laser SLAM map.
- the travel route can be set according to the connection state of the path in the laser SLAM map, and the travel route can be set according to the content of the environment covered by the laser SLAM map.
- the aforementioned travel route covers all map nodes set in this step.
- Step 103 the mobile robot controls the mobile robot to move through the laser SLAM navigation based on the laser SLAM map according to the set travel route, and determines the current position of the mobile robot according to the laser SLAM map.
- the mobile robot moves to the setting set in step 102
- the ground texture image is collected by the camera device at the map node, and the current distance and the angle of rotation obtained by the inertial sensor at the map node are recorded.
- the mobile robot uses the data collected by the inertial sensor to obtain the distance and the angle that the mobile robot has moved from the previous map node to the current map node.
- the above-mentioned inertial sensor may be an odometer, or optionally, it may be a wheel odometer.
- Inertial sensors can collect data during the working process, and the collected data can be considered as the movement information of the mobile robot, reflecting the movement of the mobile robot.
- the foregoing current distance represents the distance traveled by the mobile robot from the previous map node to the current map node.
- the rotated angle indicates the angle that the mobile robot has moved from the previous map node to the current map node.
- the mobile robot is equipped with two left and right wheels, so that the mobile robot can move forward and backward by turning the tires of the left and right wheels, and can also adjust the angle of the mobile robot relative to the reference position by turning the tires of the left and right wheels to adjust
- the direction of travel that is, the number of turns of the tires of the left and right wheels can reflect the distance traveled by the mobile robot, and the number of turns of the tires of the left and right wheels can also reflect the angles of the mobile robot.
- the data recorded by the wheel odometer may include: the number of turns of the tires of the left and right wheels respectively.
- the number of turns of the left and right wheels can be recorded as the number of turns of the tire, and because the number of turns of the left and right wheels when the mobile robot rotates If they are not equal, the average of the number of turns of the left and right wheels can be recorded as the number of turns of the tire, and then the current distance can be calculated according to the number of turns of the tire.
- the left and right wheels are used as the inner wheel and the outer wheel respectively.
- the number of turns of the inner wheel and the outer wheel are generally different. At this time, the difference between the number of turns of the inner wheel and the number of turns of the outer wheel can also be used. That is, the difference between the number of turns of the left and right wheels calculates the above-mentioned turning angle.
- Step 103 is repeatedly executed until the mobile robot has finished traveling along the travel route and the pose and ground texture images of all the set map nodes are collected.
- the pose of the map node is obtained based on the positioning information determined by the laser SLAM map, and the collected ground texture image is used as the texture information of the map node to generate the map node.
- the position of the mobile robot can be determined according to the laser SLAM map, so that when the mobile robot moves to a map node, it can be based on the current position of the mobile robot and The position that has been determined before, the pose of the mobile robot is obtained, for example, the coordinates of the current position of the mobile robot, the angle of rotation, etc. are obtained.
- the pose of the map node is obtained.
- the previously determined position may be: the determined position of the mobile robot at any map node that the mobile robot has moved through.
- any map node i denoted as Use this as the initial value for optimization.
- the x direction can represent the horizontal direction
- the y direction can represent the vertical direction.
- the posture may be the angle that the mobile robot has rotated relative to the reference position.
- the pose of the map node obtained in step 103 contains noise, so optimization processing is required to meet the needs of high-precision positioning of the mobile robot.
- Step 104 The mobile robot uses the motion information between any two map nodes recorded by the inertial sensor as a constraint between the poses of the two map nodes, and uses the poses of the two map nodes as the initial value.
- the pose of is optimized by least squares, and the final pose of the two map nodes is obtained.
- the inertial sensor mounted on the mobile robot can record the movement information that characterizes the movement of the mobile robot from one map node to another, such as the distance moved, the angle rotated, etc.
- first map node A and second map node B whose distance is less than or equal to the first distance threshold as an example.
- the above-mentioned first map node A and second map node B may be map nodes whose poses have not been optimized.
- the circumference of the tire can be calculated based on the tire radius of the left and right wheels, and then the product of the circumference of the tire and the number of turns the tire has turned is used as the first distance r.
- the distance difference between the left and right wheels of the mobile robot can be calculated based on the number of turns of the left and right wheels and the circumference of the left and right wheels.
- An angle ⁇ r For example, calculate the ratio between the aforementioned wheelbase and the distance difference, and calculate the angle that the mobile robot has turned based on the ratio.
- x A , y A represent the coordinates of the mobile robot in the x direction and the y direction when the mobile robot is at the first map node A
- ⁇ A represents the posture of the mobile robot at the first map node A
- x B , y B represent The coordinates of the mobile robot in the x direction and the y direction when the mobile robot is at the second map node B
- ⁇ B represents the posture of the mobile robot when the mobile robot is at the second map node B.
- the first measurement value ⁇ 1 of the mobile robot from the first map node A to the second map node B can be obtained.
- the first measurement value ⁇ 1 includes: the second map node B and the first map node B.
- the coordinate difference ⁇ x of the map node A in the x direction, the coordinate difference ⁇ y in the y direction, and the posture difference ⁇ the mathematical expression is:
- the second measurement value ⁇ ′ 1 of the mobile robot from the first map node A to the second map node B can be obtained through the data recorded by the wheel odometer, and the second measurement value ⁇ ′ 1 includes: the first distance r is The projection ⁇ x r of the first angle ⁇ r in the x direction, the first distance r is the projection ⁇ y r of the first angle ⁇ r in the y direction, and the first angle ⁇ r , the mathematical expression is:
- the first distance r and the first angle ⁇ r that the mobile robot has turned from the first map node A to the second map node B are determined according to the inertial sensor data, and then according to the first distance r and the first angle ⁇ r can obtain the above-mentioned second measurement value.
- the first measurement value and the second measurement value Due to limitations such as measurement accuracy, it is difficult for the first measurement value and the second measurement value to be exactly the same.
- the first distance threshold is 0.5 meters, so the measured value of the short-distance inner wheel odometer can be drawn to be accurate.
- the measured value based on the laser SLAM navigation and positioning method contains a lot of noise. Therefore, the positioning information obtained by the mobile robot at the first map node A and the second map node B can be adjusted to make the first measurement value conform to the second measurement value as much as possible.
- the error of positioning based on the laser SLAM navigation and positioning method is: the difference between the first measurement value and the second measurement value, and the mathematical expression is:
- the covariance matrix ⁇ of the second measurement value can be obtained based on the second measurement value, so that the posture of the first map node A and the posture of the second map node B can be optimized by using the least square method.
- the optimization may be performed with the above-mentioned first distance r and the first angle ⁇ r as constraints.
- the above-mentioned initial value of the pose includes the pose of the first map node A and the pose of the second map node B obtained based on the laser SLAM map.
- the first map node A in the initial pose The pose is used as the current value of the first map node A
- the pose of the second map node B in the initial pose is used as the current value of the second map node B.
- obtaining the pose of the first map node A and the pose of the second map node B based on the laser SLAM map may be based on the laser SLAM map and obtaining the first map node A based on the laser SLAM navigation and positioning method The pose and the pose of the second map node B.
- Step 1042 the mobile robot calculates the current first measurement value according to formula 1 according to the current pose value of the first map node A and the second map node B; according to the first distance and the first angle recorded by the odometer , Calculate the second measured value according to Equation 2.
- the first distance and the first angle are determined according to the data collected by the inertial sensor, and the second measurement value is calculated according to Equation 2.
- Step 1043 The mobile robot calculates the current error according to Equation 3 based on the current first measurement value and the second measurement value.
- Step 1044 The mobile robot judges whether the iteration end condition is satisfied.
- the current value of the pose of the first map node A and the current value of the second map node B are taken as the final result and saved.
- the above correction amount is calculated by the following method:
- the transpose matrix of the Jacobian matrix the inverse matrix of the covariance matrix of the second measurement value, the Jacobian matrix, and the product of the correction equal to the transpose matrix of the Jacobian matrix and the current error
- the negative number of the product expressed mathematically as:
- J is the Jacobian matrix, calculated according to the current value of the pose of the map node
- ⁇ is the covariance matrix of the second measured value, which is a 3 ⁇ 3 matrix
- e is the current measured value obtained according to the current estimated value
- the error of, the correction amount ⁇ P used for iteration can be obtained by the above formula.
- the revised estimated value P 1 is used as the current value of P.
- the iteration end condition may be that the set number of iterations is reached, the error e is less than the set error threshold, the correction amount is less than one of the set correction threshold, or any combination thereof.
- the above iterative process completes the graph optimization of the poses of the two map nodes, thereby obtaining the accurate poses of the two map nodes.
- Step 104 is repeatedly executed until all map nodes are optimized.
- Step 105 The mobile robot saves the final pose and texture information of each map node to obtain a texture map based on the ground texture.
- the texture map can be combined with the laser SLAM map to provide selectivity for the map based on the positioning of the mobile robot.
- the laser SLAM map is selected in the first area suitable for SLAM positioning
- the texture map is selected in the second area suitable for texture positioning.
- FIG. 2 is a schematic diagram of a map including a laser SLAM map and a texture map, where the discrete points are map nodes of the texture map, and the object outline (continuous part) comes from the laser SLAM map.
- the aforementioned texture information may be collected at each map node.
- the aforementioned texture information may be a ground texture image collected by a camera on a mobile machine.
- the mobile robot after the mobile robot completes following the planned path and has generated all map nodes, it selects any two map nodes that meet the first distance threshold and that has not been optimized for graph optimization. In this way, when n When it is an even number, the optimization times for n map nodes is n/2. When n is an odd number, for the last map node, any map node that has been optimized for the first distance threshold can be selected to form two map nodes In this way, the optimization times of n map nodes are (n+1)/2, which is beneficial to improve the efficiency of optimization.
- FIG. 3 is a schematic diagram of a process of constructing a texture map according to Embodiment 2 of the present application.
- the mobile robot supports laser SLAM map building, and is equipped with an inertial sensor and a downward-looking camera device for collecting ground texture.
- the map construction method includes the following steps 301-306. The following steps 301-306 are performed by the mobile robot.
- Step 301 The mobile robot sets a travel path and map nodes according to the environmental range covered by the texture map to be built, where the map nodes are set at a set interval.
- the mobile robot controls the mobile robot to automatically move according to the travel path, or manually moves the mobile robot according to the travel path.
- the laser SLAM map is established through the laser SLAM navigation and positioning technology, and the current position of the mobile robot is determined according to the current laser SLAM map .
- the downward camera device is triggered to collect the map texture at the map node and record the current movement information of the mobile robot, that is, the inertial sensor data.
- Step 303 The mobile robot judges whether there is a first map node whose distance from the current map node is less than or equal to a set first distance threshold in the currently optimized map node set.
- step 304 If yes, go to step 304.
- the pose of the two map nodes is used as the initial value of the pose, and the pose of each of the two map nodes is optimized by least squares to obtain the final pose of the two map nodes.
- the details are the same as step 104.
- the second map node and the current map node are added to the optimized map node set.
- the mobile robot regards the first map node and the current map node as a pair of map nodes, and uses the motion information between the first map node and the current map node as the constraint between the poses of the two map nodes, and the current map
- the pose of the node is the initial value of the pose of the current map node, and the current map node is optimized by least squares to obtain the final pose of the current map node, thereby realizing the pose optimization of the current map node.
- the pose of map node B ⁇ B (x B , y B , ⁇ B ); assuming that the inertial sensor is a wheel odometer, the data recorded by the wheel odometer can be obtained from the first map node A to the current map node B The first distance r and the first angle ⁇ r rotated.
- the first measurement value ⁇ 1 of the mobile robot from the first map node A to the current map node B can be obtained, and the first measurement value ⁇ 1 includes: the current map node B and the first map node
- the mathematical expression is:
- the second measurement value ⁇ ′ 1 of the mobile robot from the first map node A to the current map node B can be obtained through the data recorded by the wheel odometer.
- the second measurement value ⁇ ′ 1 includes: The projection ⁇ x r of an angle ⁇ r in the x direction, the first distance r is the projection ⁇ y r of the first angle ⁇ r in the y direction, and the first angle ⁇ r , the mathematical expression is:
- the first distance r and the first angle ⁇ r that the mobile robot has turned from the first map node A to the current map node B are determined according to the inertial sensor data, and then according to the first distance r and the first angle ⁇ r can get the above-mentioned second measurement value.
- the first measurement value and the second measurement value Due to limitations such as measurement accuracy, it is difficult for the first measurement value and the second measurement value to be exactly the same.
- the first distance threshold is 0.5 meters, so it can be drawn that the measured value of the short-distance inner wheel odometer is accurate, and The measured value based on the laser SLAM navigation and positioning method contains a lot of noise. Therefore, the positioning information obtained by the mobile robot at node B of the current map can be adjusted so that the first measurement value matches the second measurement value as much as possible.
- the covariance matrix ⁇ of the second measurement value can be obtained based on the second measurement value, so that the posture of the current map node B can be optimized by using the least square method.
- the optimization when optimizing the posture of the current map node B, the optimization may be performed with the foregoing first distance and first angle as constraints.
- the initial value of the pose of the current map node B obtained based on the laser SLAM map is used as the current value of the pose of the current map node B.
- Step 3042 the mobile robot calculates the first measurement value according to formula 4 according to the pose of the first map node A and the current value of the current map node B's pose; according to the first distance and the first angle recorded by the odometer, according to formula 5 Calculate the second measurement value.
- the first distance and the first angle are determined according to the data collected by the inertial sensor, and the second measurement value is calculated according to Equation 5.
- step 3043 the mobile robot calculates the current error according to Equation 6 based on the current first measurement value and the second measurement value.
- Step 3044 The mobile robot judges whether the iteration end condition is satisfied.
- the current value of the pose of node B of the current map is taken as the final result and saved. Add the current map node B to the optimized map node set.
- the above correction amount is calculated by the following method:
- the transpose matrix of the Jacobian matrix the inverse matrix of the covariance matrix of the second measurement value, the Jacobian matrix, and the product of the correction equal to the transpose matrix of the Jacobian matrix and the current error
- the negative number of the product expressed mathematically as:
- J is the Jacobian matrix, calculated according to the current value of the current map node B's pose
- ⁇ is the covariance matrix of the second measurement value, which is a 3 ⁇ 3 matrix
- e is the current value obtained according to the current estimated value.
- the correction amount ⁇ P used for iteration can be obtained by the above formula.
- Taking the corrected estimated value P 1 as the current value of P specifically includes: taking the pose of the current map node B in P 1 as the current pose value ⁇ B of the second map node B, and returning to step 3042.
- the iteration end condition may be that the set number of iterations is reached, the error e is less than the set error threshold, the correction amount is less than one of the set correction threshold, or any combination thereof.
- the above iterative process completes the graph optimization of the pose of the current map node, so as to obtain the accurate pose of the current map node.
- the iterative process only needs to solve three unknowns, which facilitates the convergence of the iteration and improves the calculation speed.
- step 305 the mobile robot judges whether the path of travel is over, and the pose and texture information of all the set map nodes have been collected, if yes, execute step 306, otherwise, return to step 302.
- Step 306 the same as step 105, the mobile robot saves the final pose of each map node and the ground texture information collected at each map node to obtain a texture map based on the ground texture.
- mobile robots can combine texture maps with laser SLAM maps.
- the graph optimization process can be executed in parallel with the walking collection step of step 302, that is, while the mobile robot is traveling along the planned path, graph optimization is performed on the currently generated map nodes, which is beneficial to improve the construction of the map.
- Efficiency and, in the process of graph optimization, when there is no available optimized map node, two map nodes can be optimized at the same time, when there are available optimized map nodes, only the current map node can be optimized to improve The optimized convergence speed improves the overall performance of the optimization.
- the above-mentioned unavailable optimized map node may mean that there is no map node in the optimized map node set whose distance from the current map node is less than or equal to the set first distance threshold. Since there is no optimized map node available, in this case, the map nodes whose two poses have not been optimized and whose distance is less than or equal to the first distance threshold can be optimized, so that the two can be optimized in the same optimization process. Optimization of a map node.
- the mobile robot includes:
- the control walking module is used to control the mobile robot to travel based on the real-time laser positioning and map-building SLAM map according to the travel route set by the map node that needs to be constructed according to the map to be built;
- the SLAM positioning module is used to obtain the pose of each map node based on the SLAM navigation and positioning method
- Acquisition module used to record inertial sensor data at each map node
- the map node pose optimization module is used for any two map nodes in the map node whose distance is less than or equal to the first distance threshold: obtain the first measurement value between the two map nodes according to the pose of the map node, and according to the inertial sensor Data acquisition of the second measurement value between the two map nodes, based on the error between the first measurement value and the second measurement value, using the inertial sensor data of the two map nodes as the position and orientation of the two map nodes Constraint to optimize the pose of at least one map node among the poses of the two map nodes;
- the map generation module is used to output the optimized position of the map node as the position of the map node to be built.
- the mobile robot further includes:
- the node setting module is used to obtain the laser SLAM map, set the travel route based on the laser SLAM map, and set the map nodes according to the set distance;
- the walking control module is specifically used to control the mobile robot to collect the posture and inertial sensing data of the map node when it is at each map node, and trigger the map after the posture and inertial sensing data of all map nodes are collected.
- Node pose optimization module is specifically used to control the mobile robot to collect the posture and inertial sensing data of the map node when it is at each map node, and trigger the map after the posture and inertial sensing data of all map nodes are collected.
- the walking control module is specifically used to control the movement of the mobile robot according to the travel route set by the map node that needs to be constructed according to the map to be built, and it is established based on the laser SLAM navigation and positioning method during the travel.
- Laser SLAM map ;
- the collection module is specifically configured to determine the current position of the mobile robot according to the current laser SLAM map, and collect the current inertial sensor data when the mobile robot reaches the map node;
- the SLAM positioning module is specifically used to determine whether there is a third map node whose distance from the current map node is less than or equal to a set first distance threshold in the currently optimized map node set;
- the third map node and the current map node are taken as any two map nodes that meet the distance less than or equal to the first distance threshold to optimize the pose of the current map node, and the current map node is added after the optimization is completed.
- the collection of map nodes has been optimized
- the unoptimized fourth map node whose distance from the current map node is less than or equal to the first distance threshold, and use the fourth map node and the current map node as any two that meet the distance less than or equal to the first distance threshold.
- the map node is used to optimize the pose of the two map nodes, and after the optimization is completed, the fourth map node and the current map node are added to the optimized map node set;
- the map node pose optimization module is specifically used for when the any two map nodes are the first map node and the second map node that are not optimized, according to the SLAM-based navigation positioning method
- the SLAM-based navigation positioning method According to the acquired pose of the first map node and the pose of the second map node, the difference between the coordinates of the second map node and the first map node in the x direction, the coordinate difference in the y direction, and the attitude difference are obtained;
- the first distance from the first map node to the second map node and the first angle of rotation recorded in the inertial sensor data, the projection of the first distance in the first angle in the x direction, and the first distance in the first angle The projection in the y direction and the first angle;
- the pose of the first map node obtained based on the SLAM navigation and positioning method is used as the initial value of the pose of the first map node, and the second map node obtained based on the SLAM navigation and positioning method
- the pose of is used as the initial value of
- the map node pose optimization module is specifically configured to use the initial value of the first map node's pose as the current value of the first map node's pose, and set the initial value of the second map node's pose The value is used as the current pose value of the second map node; the current first measurement value is calculated according to the current pose value of the first map node and the current pose value of the second map node; the current first measurement value is calculated according to the current first measurement value and the second measurement Value, calculate the current error; determine whether the iteration end condition is satisfied; if so, use the current pose value of the first map node and the current pose value of the second map node as the optimized result; otherwise, calculate a correction amount, Add the correction amount to the current pose value of the first map node and the current pose value of the second map node after correction, and add the corrected pose of the first map node and the position of the second map node. Take the pose as the current value of the respective pose, and then return to execute the step of calculating the
- the iteration end condition includes: reaching the set number of iterations, the error is less than the set error threshold, the correction amount is less than one of the set correction threshold, or any combination thereof.
- the correction amount is calculated in the following manner:
- the Jacobian matrix is determined according to the current pose value of the first map node and the current pose value of the second map node.
- the map node pose optimization module is specifically used for when the any two map nodes are the optimized third map node and the current map node to be optimized, according to the optimized third According to the pose of the map node and the pose of the current map node obtained based on the SLAM navigation and positioning method, the difference between the coordinates of the current map node and the third map node in the x direction, the coordinate difference in the y direction, and the attitude difference are obtained; The first distance from the third map node to the current map node and the first angle of rotation recorded in the inertial sensor data, the projection of the first distance in the x direction at the first angle, and the first distance at the first angle in the first angle The projection in the y direction and the first angle; the pose of the current map node is used as the initial value of the pose of the current map node; the first distance and the first angle are used as the pose of the third map node and the position of the current map node Pose constraint: Based on the error between the first measurement value and
- the map node pose optimization module is specifically configured to use the initial value of the pose of the current map node as the current value of the pose of the current map node; according to the pose of the third map node and the current map The current value of the node's pose, calculate the current first measurement value; according to the current first measurement value and the second measurement value, calculate the current error; determine whether the iteration end condition is met; if so, the current value of the current map node's pose As the result of optimization; otherwise, calculate a correction amount, add the correction amount to the current value of the pose of the current map node, use the corrected pose of the current map node as the current value of the pose, and then return to execute the basis The step of calculating the current first measurement value of the pose of the third map node and the current value of the pose of the current map node;
- the iteration end condition includes: reaching the set number of iterations, the error is less than the set error threshold, the correction amount is less than one of the set correction threshold, or any combination thereof.
- the correction amount is calculated in the following manner:
- the Jacobian matrix is determined according to the current value of the pose of the current map node.
- the collection module is further configured to, if the map to be built is a ground texture map, after the inertial sensor data is recorded at each map node, the ground texture information is collected at each map node and saved ;
- the map generation module is specifically used to save the optimized pose of each map node and the ground texture information collected at each map node to obtain a texture map based on the ground texture; and synthesize the texture map and the laser SLAM map into one.
- a mobile robot including a memory and a processor, wherein the memory stores a computer program, and the processor can be used to execute the computer program to implement the The steps of building a visual map.
- the memory may include random access memory (Random Access Memory, RAM), and may also include non-volatile memory (Non-Volatile Memory, NVM), such as at least one disk storage.
- NVM non-Volatile Memory
- the memory may also be at least one storage device located far away from the foregoing processor.
- the above-mentioned processor can be a general-purpose processor, including a central processing unit (CPU), a network processor (Network Processor, NP), etc.; it can also be a digital signal processor (Digital Signal Processing, DSP), a dedicated integrated Circuit (Application Specific Integrated Circuit, ASIC), Field-Programmable Gate Array (Field-Programmable Gate Array, FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components.
- CPU central processing unit
- NP Network Processor
- DSP Digital Signal Processing
- ASIC Application Specific Integrated Circuit
- FPGA Field-Programmable Gate Array
- FPGA Field-Programmable Gate Array
- the embodiment of the present application also provides a computer-readable storage medium in which a computer program is stored, and the computer program is executed by a processor to implement the above-mentioned map construction steps.
- a computer program product containing instructions is also provided, which when running on a computer, causes the computer to execute the steps of the visual map construction method described in the embodiment of the present application.
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
一种视觉地图构建方法和移动机器人,包括,按照根据待建地图所需要构建的地图节点设定的行进路线,控制移动机器人基于激光SLAM地图行进,基于SLAM导航定位方式获取每个地图节点的位姿,在每个地图节点记录惯性传感器数据,对于地图节点中满足距离小于等于第一距离阈值的任意两地图节点:根据地图节点的位姿获取两地图节点之间位姿的第一测量值,根据惯性传感器数据获取两地图节点之间位姿的第二测量值,基于第一测量值与第二测量值之间的误差,以两地图节点的惯性传感器数据作为约束,对至少一个地图节点的位姿进行优化;将优化后的地图节点的位姿作为待建地图的地图节点的位姿。提高了视觉地图中地图节点的位姿精度。
Description
本申请要求于2020年6月8日提交中国专利局、申请号为202010510451.5发明名称为“一种视觉地图构建方法和移动机器人”的中国专利申请的优先权,其全部内容通过引用结合在本申请中。
本申请涉及视觉导航技术领域,特别地,涉及一种视觉地图构建方法和移动机器人。
移动机器人进行视觉导航时,通过摄像装置对周围环境进行图像采集,并基于图像完成移动机器人的位置确定和路径识别。视觉导航需要依赖视觉地图完成。视觉地图中包括若干地图节点,每个地图节点至少包括地图节点的位姿。其中,地图节点的位姿表示位于该地图节点时移动机器人的位姿。以基于地面纹理的地图为例,地图节点包括地图节点的位姿和地图节点处纹理点图像的纹理信息。移动机器人移动经过某地图节点的时候,根据摄像装置所采集图像的纹理信息和该地图节点处纹理点图像的纹理信息,进行图像配准,解算出移动机器人的当前位姿,实现基于地面纹理的定位导航。
为此,在视觉地图的建图过程中,需要获得每个地图节点的精确位姿,以保证后续基于视觉地图对移动机器人进行定位导航的精度。以基于地面纹理构建地图为例,建图过程中用摄像装置将地面的有效纹理信息记录下来,并且和当前的位置坐标对应,形成一张纹理地图。
发明内容
本申请实施例提供了一种视觉地图构建方法和移动机器人,以提高视觉地图中地图节点的位姿的精度。
本申请实施例提供了一种视觉地图构建方法,该方法包括,
按照根据待建地图所需要构建的地图节点设定的行进路线,控制移动机器人基于激光即时定位与地图构建SLAM地图行进,基于SLAM导航定位方式获取每个地图节点的位姿,在每个地图节点记录惯性传感器数据;
对获取的地图节点的位姿进行优化,该优化包括,对于地图节点中满足距离小于等于第一距离阈值的任意两地图节点:根据地图节点的位姿获取所述两地图节点之间位姿的第一测量值,根据惯性传感器数据获取所述两地图节点之间位姿的第二测量值,基于第一测量值与第二测量值之间的误差,以所述两地图节点的惯性传感器数据作为所述两地图节点的位姿的约束,对所述两地图节点的位姿中至少一个地图节点的位姿进行优化;
将优化后的地图节点的位姿作为待建地图的地图节点位姿。
本申请的一个实施例中,按照以下方式设定所述待建地图所需要构建的地图节点:获取激光SLAM地图,基于激光SLAM地图设定行进路线,并按照设定的间距设置地图节点;
所述控制移动机器人基于激光SLAM地图行进,包括:
控制移动机器人处于各地图节点时采集地图节点的位姿以及惯性传感数据,直至所有地图节点的位姿以及惯性传感数据都被采集后,执行所述对获取的地图节点的位姿进行优化的步骤。
本申请的一个实施例中,所述按照根据待建地图所需要构建的地图节点设定的行进路线,控制移动机器人基于激光SLAM地图行进,基于SLAM导航定位方式获取每个地图节点的位姿,在每个地图节 点记录惯性传感器数据,包括:
按照根据待建地图所需要构建的地图节点设定的行进路线,控制移动机器人行进,在行进过程中基于激光SLAM导航定位方式建立激光SLAM地图,并根据当前激光SLAM地图确定移动机器人的当前位置信息,当移动机器人到达地图节点时,采集当前惯性传感器数据;
判断当前已优化地图节点集合中是否有与当前地图节点之间距离小于等于设定的第一距离阈值的第三地图节点;
如果有,则将第三地图节点和当前地图节点作为所述满足距离小于等于第一距离阈值的任意两地图节点,以进行当前地图节点的位姿优化,并在完成优化后将当前地图节点加入已优化地图节点集合;
否则,选择与当前地图节点之间的距离小于等于第一距离阈值的未被优化的第四地图节点,将第四地图节点和当前地图节点作为所述满足距离小于等于第一距离阈值的任意两地图节点,以进行两地图节点的位姿优化,并在完成优化后将第四地图节点和当前地图节点加入已优化地图节点集合;
判断行进路径是否结束、且所有设定的地图节点的位姿都被采集,如果是,则执行所述将优化后的地图节点的位姿作为待建地图的地图节点的位姿的步骤,否则,返回执行所述按照待建地图所需要构建的地图节点和所设定的行进路线,控制移动机器人基于激光SLAM地图行进的步骤。
本申请的一个实施例中,当所述任意两地图节点均为未被优化的第一地图节点和第二地图节点时,
所述根据地图节点的位姿获取所述两地图节点之间位姿的第一测量值,包括:
根据基于SLAM导航定位方式获取的第一地图节点的位姿和第二地图节点的位姿,得到第二地图节点与第一地图节点在x方向的坐标之差、y方向的坐标之差、以及姿态之差;
所述根据惯性传感器数据获取所述两地图节点之间位置的第二测量值,包括:
根据惯性传感器数据中记录的从第一地图节点到第二地图节点的第一距离以及转过的第一角度,得到第一距离以第一角度在x方向上的投影、第一距离r以第一角度在y方向上的投影、以及第一角度;
所述基于第一测量值与第二测量值之间的误差,以所述两地图节点的惯性传感器数据作为所述两地图节点的位姿的约束,对所述两地图节点的位姿中至少一个地图节点的位姿进行优化,包括:
将基于SLAM导航定位方式获取的第一地图节点的位姿作为第一地图节点的位姿初始值,将基于SLAM导航定位方式获取的第二地图节点的位姿作为第二地图节点的位姿初始值;
以第一距离和第一角度作为所述第一地图节点的位姿和第二地图节点的位姿的约束;
基于第一测量值与第二测量值之间的误差,采用最小二乘法,迭代求取优化后的第一地图节点的位姿和第二地图节点的位姿。
本申请的一个实施例中,所述基于第一测量值与第二测量值之间的误差,采用最小二乘法,迭代求取优化后的第一地图节点的位姿和第二地图节点的位姿,包括:
将第一地图节点的位姿初始值作为第一地图节点的位姿当前值,将第二地图节点的位姿初始值作为第二地图节点的位姿当前值;
根据第一地图节点的位姿当前值和第二地图节点的位姿当前值,计算当前第一测量值;
根据当前第一测量值与第二测量值,计算当前误差;
判断是否满足迭代结束条件;
如果是,则将第一地图节点的位姿当前值和第二地图节点的位姿当前值作为优化后的结果;
否则,计算一修正量,将修正量分别与第一地图节点的位姿当前值、以及第二地图节点的位姿当前 值相加,将修正后的第一地图节点的位姿、以及修正后的第二地图节点的位姿作为各自的位姿当前值,然后返回执行所述根据第一地图节点的位姿当前值和第二地图节点的位姿当前值,计算当前第一测量值的步骤;
所述迭代结束条件包括,达到设定的迭代次数、所述误差小于设定的误差阈值、修正量小于设定的修正阈值之一或其任意的组合。
本申请的一个实施例中,所述修正量通过以下方式计算:
对当前误差求第一地图节点的位姿和第二地图节点的位姿的偏导数,得到雅克比矩阵;
根据克比矩阵的转置矩阵、第二测量值的协方差矩阵的逆矩阵、雅克比矩阵、以及修正量的乘积等于雅克比矩阵的转置矩阵与当前误差乘积的负数,确定修正量;
其中,所述雅克比矩阵根据第一地图节点的位姿当前和第二地图节点的位姿当前值确定。
本申请的一个实施例中,所述任意两地图节点为已优化的第三地图节点和待优化的当前地图节点,
所述根据地图节点的位姿获取所述两地图节点之间位姿的第一测量值,包括:
根据已优化的第三地图节点的位姿和基于SLAM导航定位方式获取的当前地图节点的位姿,得到当前地图节点与第三地图节点在x方向的坐标之差、y方向的坐标之差、以及姿态之差;
所述根据惯性传感器数据获取所述两地图节点之间位姿的第二测量值,包括:
根据惯性传感器数据中记录的从第三地图节点到当前地图节点的第一距离以及转过的第一角度,得到第一距离以第一角度在x方向上的投影、第一距离以第一角度在y方向上的投影、以及第一角度;
所述基于第一测量值与第二测量值之间的误差,以所述两地图节点的惯性传感器数据作为所述两地图节点的位姿的约束,对所述两地图节点的位姿中至少一个地图节点的位姿进行优化,包括:
将当前地图节点的位姿作为当前地图节点位姿初始值;
以第一距离和第一角度作为第三地图节点的位姿和当前地图节点的位姿的约束;
基于第一测量值与第二测量值之间的误差,采用最小二乘法,迭代求取优化后的当前地图节点的位姿。
本申请的一个实施例中,所述基于第一测量值与第二测量值之间的误差,采用最小二乘法,迭代求取优化后的当前地图节点的位姿,包括,
将当前地图节点的位姿初始值作为当前地图节点的位姿当前值;
根据第三地图节点的位姿和当前地图节点的位姿当前值,计算当前第一测量值;
根据当前第一测量值与第二测量值,计算当前误差;
判断是否满足迭代结束条件;
如果是,则将当前地图节点的位姿当前值作为优化后的结果;
否则,计算一修正量,将修正量与当前地图节点的位姿当前值相加,将修正后的当前地图节点的位姿作为位姿当前值,然后返回执行所述根据第一地图节点的位姿和当前地图节点的位姿当前值,计算当前第一测量值的步骤;
所述迭代结束条件包括:达到设定的迭代次数、所述误差小于设定的误差阈值、修正量小于设定的修正阈值之一或其任意的组合。
本申请的一个实施例中,所述修正量通过以下方式计算:
对当前误差求当前地图节点的位姿的偏导数,得到雅克比矩阵,
根据克比矩阵的转置矩阵、第二测量值的协方差矩阵的逆矩阵、雅克比矩阵、以及修正量的乘积等于雅克比矩阵的转置矩阵与当前误差乘积的负数,确定修正量;
其中,所述雅克比矩阵根据当前地图节点的位姿当前值确定。
本申请的一个实施例中,所述待建地图为地面纹理地图,所述在每个地图节点记录惯性传感器数据之后,还包括:
在每个地图节点采集地面纹理信息,并保存;
所述将优化后的地图节点的位姿作为待建地图的地图节点的位姿,包括:
将各个地图节点优化后的位姿以及在各个地图节点采集的地面纹理信息进行保存,得到基于地面纹理的纹理地图;
将纹理地图与激光SLAM地图合成为一体。
本申请实施例还提供一种移动机器人,包括,
控制行走模块,按照根据待建地图所需要构建的地图节点设定的行进路线,控制移动机器人基于激光即时定位与地图构建SLAM地图行进;
SLAM定位模块,基于SLAM导航定位方式获取每个地图节点位姿;
采集模块,在每个地图节点记录惯性传感器数据;
地图节点位姿优化模块,对于地图节点中满足距离小于等于第一距离阈值的任意两地图节点:根据地图节点的位姿获取所述两地图节点之间的第一测量值,根据惯性传感器数据获取所述两地图节点之间的第二测量值,基于第一测量值与第二测量值之间的误差,以所述两地图节点的惯性传感器数据作为所述两地图节点的位姿的约束,对所述两地图节点的位姿中至少一个地图节点的位姿进行优化;
地图生成模块,将优化后的地图节点的位姿作为待建地图的地图节点位姿。
本申请实施例提供的视觉地图构建方案,以激光即时定位与地图构建为辅助手段,通过惯性传感器采集移动机器人在各个地图节点处的运动信息,利用基于SLAM导航定位方式获得的地图节点的位姿的第一测量值、基于惯性传感器获取的地图节点的位姿的第二测量值,构建第一测量值与第二测量值之间的误差,通过图优化对地图节点的位姿进行优化,提高了地图节点的位姿精度,降低了视觉地图构建过程的工作量、实施周期,对于基于地面标识的纹理地图,提高了纹理地图的质量和纹理定位精度。
为了更清楚地说明本申请实施例和现有技术的技术方案,下面对实施例和现有技术中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本申请的一些实施例,本领域普通技术人员来讲还可以根据这些附图获得其他的附图。
图1为本申请实施例一的构建纹理地图的一种流程示意图。
图2为包括激光SLAM地图信息和纹理地图信息的一种地图的示意图。
图3为本申请实施例二的构建纹理地图的一种流程示意图。
图4为移动机器人的一种示意图。
为使本申请的目的、技术方案、及优点更加清楚明白,以下参照附图并举实施例,对本申请进一步详细说明。显然,所描述的实施例仅仅是本申请一部分实施例,而不是全部的实施例。本领域普通技术人员基于本申请中的实施例所获得的所有其他实施例,都属于本申请保护的范围。
本申请实施例以激光SLAM(Simultaneous Localization And Mapping,即时定位与地图构建)导航定位为辅助手段,建立视觉地图,并采用基于SLAM的导航定位方式获得的地图节点的位姿的第一测量值、基于惯性传感器获取的地图节点的位姿的第二测量值,构建第一测量值与第二测量值之间的误差,通过图优化对地图节点的位姿进行优化。
基于此,本申请的一个实施例中,提供了一种视觉地图构建方法,该方法包括:
按照根据待建地图所需要构建的地图节点设定的行进路线,控制移动机器人基于激光SLAM地图行进,基于SLAM导航定位方式获取每个地图节点的位姿,在每个地图节点记录惯性传感器数据;
对获取的地图节点的位姿进行优化,该优化包括,对于地图节点中满足距离小于等于第一距离阈值的任意两地图节点:根据地图节点的位姿获取所述两地图节点之间位姿的第一测量值,根据惯性传感器数据获取所述两地图节点之间位姿的第二测量值,基于第一测量值与第二测量值之间的误差,以两地图节点的惯性传感器数据作为两地图节点的位姿的约束,对两地图节点的位姿中至少一个地图节点的位姿进行优化;
将优化后的地图节点的位姿作为待建地图的地图节点的位姿。
应用本申请实施例提供的方案构建视觉地图,不仅提高了地图节点的位姿精度,还降低了视觉地图构建过程的工作量、实施周期。
其中,上述移动机器人可以是AGV(Automated Guided Vehicle),也称为AGV小车。当然,上述移动机器人也可以是其他形态能够移动的机器人。
另外,本申请实施例中的移动机器人搭载有摄像装置,移动机器人还可以搭载有惯性传感器、激光传感器等。
上述地图节点对应于实际环境中具有一定范围的区域。在构建纹理地图的情况下,各个地图节点也可以称为纹理点。移动机器人处于一个地图节点时,移动机器人搭载的摄像装置采集的图像也可以相应的称为纹理点图像。
上述行进路线是按照待构建地图所需要构建的地图节点设定的,控制机器人行进时,控制机器人按照上述行进路线行进。
可选的,本申请实施例的执行主体可以是移动机器人自身。也就是说,本申请实施例中各个步骤可以是由移动机器人自身执行的。另外,本申请实施例的执行主体也可以是能够与移动机器人通信的服务器,也就是说,本申请实施例中各个步骤可以是由服务器执行的。
本申请实施例中所提及的激光SLAM导航定位方式,可以理解为:基于激光SLAM地图进行导航定位,从而得到移动机器人的定位信息的方式。本申请中,也将激光SLAM导航定位方式简称为SLAM导航定位方式。
位姿包括位置和姿态,其中,位置可以以水平方向以及垂直方向的坐标表示,姿态可以是移动机器人相对于基准位置转过的角度等。其中,上述水平方向可以是移动机器人所在水平面内水平线所在的方向,垂直方向可以是上述水平面内与上述水平方向相垂直的方向。上述基准位置可以是预先设定的。
为便于理解,以下将以构建纹理地图为例来说明,所应理解的是,本申请不限于构建纹理地图,其它通过摄像装置对周围环境进行图像采集来构建的视觉地图都可以适用。另外,为便于说明,以下将以移动机器人为执行主体为例进行说明。
实施例一
参见图1,图1为本申请实施例一中构建纹理地图的一种流程示意图。该地图构建方法包括以下步骤101-105。步骤101-105是由移动机器人执行的。
步骤101,移动机器人获取待建纹理地图所覆盖环境范围的激光SLAM地图。
其中,激光SLAM地图是指:基于激光传感器采集的移动机器人的运行环境数据,构建的SLAM地图。
当待建纹理地图所覆盖环境范围的激光SLAM地图已存在时,则可以将该激光SLAM地图加载至一具有激光SLAM定位功能、且具有惯性传感器的移动机器人中。
如果尚无激光SLAM地图,则可以基于具有SLAM导航定位功能的移动机器人构建激光SLAM地图。在构建激光SLAM地图过程中,控制移动机器人自动在待建纹理地图所覆盖环境范围内移动,或者人工移动移动机器人,获得移动机器人搭载的激光传感器采集的数据,基于激光传感器采集的数据建立激光SLAM地图。该过程可以使用既有激光SLAM算法实现,例如,cartographer、hector等算法。
步骤102,移动机器人设定待建纹理地图所需要构建的地图节点。
可选的,移动机器人可以根据激光SLAM地图确定移动机器人的行进路线;结合所确定的行进路线,按照设定间隔设定一连串离散的n个地图节点;n为大于1的自然数。另外,还可以对所设定的n个地图节点进行检查,删除重复和/或间距小于设定间距的地图节点。例如,上述设定间距可以是0.5米、0.4米等。
其中,上述行进路线可以是根据激光SLAM地图设定的。例如,可以根据激光SLAM地图中路径的连通状态设定行进路线,可以根据激光SLAM地图所覆盖环境的内容设定行进路线等。
一种情况下,上述行进路线覆盖本步骤中设定的所有地图节点。
步骤103,移动机器人控制移动机器人基于激光SLAM地图按照设定的行进路线通过激光SLAM导航进行移动,并根据激光SLAM地图确定移动机器人所处的当前位置,当移动机器人移动到步骤102所设定的每个地图节点时,在地图节点处通过摄像装置进行地面纹理图像采集,并记录地图节点处通过惯性传感器获得的当前的距离和转过的角度。
也就是说,本步骤中移动机器人通过惯性传感器采集的数据,获得移动机器人从上一地图节点移动至当前地图节点移动过的距离和转过的角度。
其中,上述惯性传感器可以是里程计,可选的,可以是轮式里程计。
惯性传感器在工作过程中可以采集数据,所采集的数据可以认为是移动机器人的运动信息,反映移动机器人的运动情况。
上述当前的距离表示移动机器人从上一地图节点移动到当前地图节点移动过的距离。转过的角度表示移动机器人从上一地图节点移动到当前地图节点转过的角度。
一种情况下,移动机器人安装有左右两个轮,这样移动机器人可以通过转动左右轮的轮胎进而前后移动,还可以通过转动左右轮的轮胎调整移动机器人相对于基准位置转过的角度,进而调整行进方向,也就是,左右轮的轮胎转过的圈数可以反映移动机器人行进的距离,另外,左右轮的轮胎转过的圈数也可以反映移动机器人转过的角度。基于此,在上述惯性传感器为轮式里程计的情况下,轮式里程计所记录的数据可以包括:左右轮的轮胎分别转过的圈数。这种情况下,由于移动机器人前后移动时,左右轮转过的圈数相等,可以将左右轮转过的圈数记为轮胎转过的圈数,又由于移动机器人转动时,左右轮转过的圈数不相等,这时可以将左右轮转过圈数的平均值记为轮胎转过的圈数,然后根据轮胎转过的圈数 计算上述当前的距离。另外,移动机器人转动时,左右轮分别作为内侧轮和外侧轮,内侧轮和外侧轮转过的圈数一般不同,这时可以根据内侧轮转过的圈数与外侧轮转过的圈数之差,也就是,左右轮转过的圈数之差计算上述转过的角度。
反复执行步骤103,直至移动机器人沿行进路线行进结束、且所有已设定地图节点的位姿和地面纹理图像都被采集。
一种情况下,地图节点的位姿是根据激光SLAM地图所确定的定位信息获得的,将所采集的地面纹理图像作为该地图节点的纹理信息,从而生成地图节点。
可选的,由于移动机器人每移动到一个地图节点处,均可以根据激光SLAM地图确定出移动机器人的位置,这样当移动机器人移动到一个地图节点处时,可以根据移动机器人当前所处的位置以及之前已经确定出的位置,获得移动机器人的位姿,例如,获得移动机器人当前所处的位置坐标、转过的角度等。从而也就获得了地图节点的位姿。一种实现方式中,上述之前已经确定出的位置可以是:在移动机器人已经移动经过的任一地图节点处,确定出的移动机器人的位置。
对于任一地图节点i的位姿,记为
以此作为优化的初始值。其中,
表示移动机器人处于地图节点i时在x方向的位置、以及在y方向的位置。其中,x方向可以表示水平方向,y方向可以表示垂直方向。
表示移动机器人处于地图节点i时的姿态,例如,该姿态可以是移动机器人相对于基准位置转过的角度。
由于SLAM定位误差、移动机器人的惯性传感器记录的运动信息误差的影响,步骤103所获得的地图节点的位姿含有噪声,因此需要进行优化处理,以满足移动机器人高精度定位的需要。
步骤104,移动机器人将惯性传感器记录的任意两个地图节点之间的运动信息作为该两地图节点的位姿之间的约束,以该两地图节点的位姿为初始值,对该两地图节点的位姿进行最小二乘优化,得到该两地图节点的最终位姿。
移动机器人搭载的惯性传感器可以记录表征移动机器人从一个地图节点移动到另一个地图节点的运动信息,例如,移动过的距离,转动过的角度等。
在该步骤中,以任意两个满足距离小于等于第一距离阈值的第一地图节点A和第二地图节点B为例。一种情况下,上述第一地图节点A和第二地图节点B可以是位姿均未被优化过的地图节点。
在第一地图节点A处采集地面纹理图像时,移动机器人基于激光SLAM导航定位方式获得了第一地图节点A的位姿ξ
A=(x
A,y
A,θ
A);移动机器人移动到第二地图节点B处,基于激光SLAM导航定位方式获得了第二地图节点B的位姿ξ
B=(x
B,y
B,θ
B);假设惯性传感器为轮式里程计,通过轮式里程计记录的数据可以得到从第一地图节点A到第二地图节点B的第一距离r、和转过的第一角度Δθ
r。其中,第一距离r通过轮式里程计记录的轮胎转过的圈数和已知的轮胎半径得到,转过的第一角度Δθ
r通过轮式里程计记录的左右轮转过的圈数和已知的轮胎半径而得到。
例如,可以根据左右轮的轮胎半径计算轮胎的周长,然后将轮胎的周长与轮胎转过的圈数乘积作为上述第一距离r。可以基于左右轮分别转过的圈数和左右轮的周长计算移动机器人左右轮转过的距离差,然后基于上述距离差和左右轮之间的轮距,计算移动机器人转过的角度,作为第一角度Δθ
r。例如,计算上述轮距与距离差间的比值,基于该比值计算移动机器人转过的角度。
其中,上述x
A,y
A表示移动机器人在第一地图节点A处时在x方向和y方向的坐标,θ
A表示移动机器人在第一地图节点A处时的姿态,x
B,y
B表示移动机器人在第二地图节点B处时在x方向和y方向的坐标,θ
B表示移动机器人在第二地图节点B处时的姿态。
这样,通过激光SLAM导航定位方式,可以得到移动机器人从第一地图节点A到第二地图节点B的第一测量值Δξ
1,该第一测量值Δξ
1包括:第二地图节点B与第一地图节点A在x方向的坐标之差Δx、在y方向的坐标之差Δy、以及姿态之差Δθ,数学式表达为:
Δξ
1=(Δx,Δy,Δθ)=(x
B-x
A,y
B-y
A,θ
B-θ
A) (式1)
另外,通过轮式里程计记录的数据可以得到移动机器人从第一地图节点A到第二地图节点B的第二测量值Δξ′
1,该第二测量值Δξ′
1包括:第一距离r以第一角度Δθ
r在x方向上的投影Δx
r,第一距离r以第一角度Δθ
r在y方向上的投影Δy
r,以及第一角度Δθ
r,数学式表达为:
Δξ′
1=(Δx
r,Δy
r,Δθ
r)=(r·cosΔθ
r,r·sinΔθ
r,Δθ
r) (式2)
由此,得到惯性传感器记录的任意两个地图节点之间的运动信息与该两地图节点的位姿之间的约束关系。
也就是说,移动机器人从第一地图节点A到第二地图节点B的第一距离r以及转过的第一角度Δθ
r是根据惯性传感器数据确定的,进而根据第一距离r以及第一角度Δθ
r可以得到上述第二测量值。
由于测量精度等限制,第一测量值和第二测量值难以完全相同。鉴于第一地图节点A与第二地图节点B之间的距离小于等于第一距离阈值,例如,第一距离阈值为0.5米,因此可以拟定短距离内轮式里程计的测量值是准确的,而基于激光SLAM导航定位方式所得的测量值含有较大的噪声。因此可以调整移动机器人在第一地图节点A和第二地图节点B所得的定位信息,使第一测量值尽量符合第二测量值。
因此,基于激光SLAM导航定位方式进行定位的误差为:第一测量值与第二测量值之间的差值,数学表达式为:
e=Δξ′
1-Δξ
1 (式3)
显然,e是关于ξ
A、ξ
B的一个函数。为此,可以求e关于ξ
A=(x
A,y
A,θ
A)和ξ
B=(x
B,y
B,θ
B)的偏导数,组成雅克比矩阵:
此外,基于第二测量值可以得到第二测量值的协方差矩阵Σ,如此就可以利用最小二乘法对第一地图节点A的姿态和第二地图节点B的姿态进行优化。
可选的,对第一地图节点A的姿态和第二地图节点B的姿态进行优化时,可以以上述第一距离r和第一角度Δθ
r为约束进行优化。
另外,由于角度信息的存在,求解第一地图节点A的位姿和第二地图节点B的位姿成为求解非线性的方程。因此,可以采用迭代求解的方法实现,可以包括以下步骤1041-1044。
步骤1041,移动机器人将基于激光SLAM地图所获得的第一地图节点A的位姿ξ
A=(x
A,y
A,θ
A)、第二地图节点B的位姿ξ
B=(x
B,y
B,θ
B)作为位姿初始值,记为P。
也就是,初始状态下,上述位姿初始值包括基于激光SLAM地图获得的第一地图节点A的位姿和 第二地图节点B的位姿,这时,初始位姿中第一地图节点A的位姿作为第一地图节点A的位姿当前值,初始位姿中第二地图节点B的位姿作为第二地图节点B的位姿当前值。
可选的,基于激光SLAM地图获得第一地图节点A的位姿和第二地图节点B的位姿,可以是在激光SLAM地图的基础上,基于激光SLAM导航定位方式获得第一地图节点A的位姿和第二地图节点B的位姿。
步骤1042,移动机器人根据第一地图节点A的位姿当前值、第二地图节点B的位姿当前值,按照式1计算当前第一测量值;按照里程计记录的第一距离和第一角度,按照式2计算第二测量值。
也就是说,本步骤中按照惯性传感器采集的数据确定第一距离和第一角度,并按照式2计算第二测量值。
步骤1043,移动机器人基于当前第一测量值和第二测量值,按照式3计算当前误差。
步骤1044,移动机器人判断是否满足迭代结束条件。
如果是,则将第一地图节点A的位姿当前值以及第二地图节点B的位姿当前值作为最终结果,并保存。
否则,为了减小误差,计算基于当前P的修正量δP,以得到修正后的估计值P
1,其中,P
1=P+δP。
上述计算P
1时,可以分解为两部分。一部分是:将第一地图节点A的位姿当前值与修正量δP中与第一地图节点A相对应的信息相加,另一部分是:将第二地图节点B的位姿当前值与修正量δP中与第二地图节点B相对应的信息相加。
通过以下方式计算得到上述修正量:
对当前e求关于ξ
A=(x
A,y
A,θ
A)和ξ
B=(x
B,y
B,θ
B)的偏导数,得到雅克比矩阵J。
根据高斯-牛顿最小二乘法,有:雅克比矩阵的转置矩阵、第二测量值的协方差矩阵的逆矩阵、雅克比矩阵、以及修正量的乘积等于雅克比矩阵的转置矩阵与当前误差乘积的负数,用数学式表达为:
J
TΣ
-1J·δP=-J
T·e
其中,J为雅克比矩阵,根据地图节点的位姿当前值计算得到,Σ为第二测量值的协方差矩阵,其为3×3的矩阵,e为按照当前估计值所得到的当前测量值的误差,通过上式可求得用于迭代的修正量δP。
将修正后的估计值P
1作为P的当前值。具体包括:将P
1中第一地图节点A的位姿作为第一地图节点A的位姿当前值ξ
A、将P
1中第二地图节点B的位姿作为第二地图节点B的位姿当前值ξ
B,返回执行步骤1042。
所述迭代结束条件可以是,达到设定的迭代次数、误差e小于设定的误差阈值、修正量小于设定的修正阈值之一或其任意的组合。
上述迭代过程完成了两个地图节点的位姿的图优化,从而获得了两个地图节点准确的位姿。
反复执行步骤104,直至所有的地图节点被优化。
步骤105,移动机器人将各个地图节点的最终位姿、以及纹理信息进行保存,得到基于地面纹理的纹理地图。另外,可以将纹理地图与激光SLAM地图合成为一体,以为移动机器人定位时所基于的地图提供可选择性。例如,在适于SLAM定位的第一区域选择激光SLAM地图,在适于纹理定位的第二区域选择纹理地图。参见图2所示,图2为包括激光SLAM地图和纹理地图的一种地图的示意图,其中,离散的点为纹理地图的地图节点,物体轮廓(连续的部分)来自激光SLAM地图。
其中,上述纹理信息可以是在每个地图节点采集的。可选的,上述纹理信息可以是移动机器上的摄 像装置采集的地面纹理图像。
本实施例中,移动机器人按照拟定的路径行进完成、且已生成所有的地图节点之后,再从中选择符合第一距离阈值的未进行图优化的任意两地图节点来进行图优化,这样,当n为偶数时,n个地图节点的优化次数为n/2,当n为奇数时,对于最后的地图节点,可以选择符合第一距离阈值的已进行图优化的任一地图节点来组成两地图节点,这样n个地图节点的优化次数为(n+1)/2,有利于提高优化的效率。
实施例二
参见图3,图3为本申请实施例二的构建纹理地图的一种流程示意图。在该实施例中,移动机器人支持激光SLAM地图建图,并安装有惯性传感器和安装有用于采集地面纹理的下视摄像装置。该地图构建方法包括以下步骤301-306。以下步骤301-306是由移动机器人执行的。
步骤301,移动机器人根据待建纹理地图所覆盖环境范围设定行进路径和地图节点,其中,地图节点按照设定的间隔设置。
步骤302,移动机器人控制移动机器人自动按照行进路径移动,或按照行进路径人工移动移动机器人,在移动过程中通过激光SLAM导航定位技术建立激光SLAM地图,并根据当前激光SLAM地图确定移动机器人当前的位置。
当移动机器人达到设定的地图节点时,触发下视摄像装置采集该地图节点处的地图纹理,并记录移动机器人的当前运动信息,即惯性传感器数据。
步骤303,移动机器人判断当前已优化地图节点集合中是否有与当前地图节点之间距离小于等于设定的第一距离阈值的第一地图节点。
如果有,则执行步骤304。
否则,选择与当前地图节点之间的距离小于等于第一距离阈值的未被优化的第二地图节点,然后,将第二地图节点与当前地图节点之间的运动信息作为该两地图节点的位姿之间的约束,以该两地图节点的位姿为位姿初始值,对该两地图节点中的每一地图节点的位姿进行最小二乘优化,得到该两地图节点的最终位姿。具体与步骤104相同。并将第二地图节点和当前地图节点加入已优化地图节点集合。
步骤304,移动机器人将第一地图节点和当前地图节点作为一对地图节点,将第一地图节点与当前地图节点之间的运动信息作为该两地图节点的位姿之间的约束,以当前地图节点的位姿为当前地图节点的位姿初始值,对该当前地图节点进行最小二乘优化,得到当前地图节点最终位姿,从而实现对当前地图节点的位姿优化。
在该步骤中,第一地图节点A的位姿已被优化,记为ξ
A=(x
A,y
A,θ
A);当前地图节点B处,移动机器人根据激光SLAM导航定位方式获得了当前地图节点B的位姿ξ
B=(x
B,y
B,θ
B);假设惯性传感器为轮式里程计,通过轮式里程计记录的数据可以得到从第一地图节点A到当前地图节点B的第一距离r、和转过的第一角度Δθ
r。
这样,通过激光SLAM导航定位方式,可以得到移动机器人从第一地图节点A到当前地图节点B的第一测量值Δξ
1,该第一测量值Δξ
1包括:当前地图节点B与第一地图节点A在x方向的坐标之差Δx,在y方向的坐标之差Δy、以及姿态之差Δθ,数学式表达为:
Δξ
1=(Δx,Δy,Δθ)=(x
B-x
A,y
B-y
A,θ
B-θ
A) (式4)
另外,通过轮式里程计记录的数据可以得到移动机器人从第一地图节点A到当前地图节点B的第 二测量值Δξ′
1,该第二测量值Δξ′
1包括:第一距离r以第一角度Δθ
r在x方向上的投影Δx
r,第一距离r以第一角度Δθ
r在y方向上的投影Δy
r,以及第一角度Δθ
r,数学式表达为:
Δξ′
1=(Δx
r,Δy
r,Δθ
r)=(r·cosΔθ
r,r·sinΔθ
r,Δθ
r) (式5)
由此,得到惯性传感器记录的第一地图节点与当前地图节点之间的运动信息与该两地图节点的位姿之间的约束关系。
也就是说,移动机器人从第一地图节点A到当前地图节点B的第一距离r以及转过的第一角度Δθ
r是根据惯性传感器数据确定的,进而根据第一距离r以及第一角度Δθ
r可以得到上述第二测量值。
由于测量精度等限制,第一测量值和第二测量值难以完全相同。鉴于第一地图节点A与当前图节点B之间的距离小于等于第一距离阈值,例如,第一距离阈值为0.5米,因此可以拟定短距离内轮式里程计的测量值是准确的,而基于激光SLAM导航定位方式所得的测量值含有较大的噪声。因此可以调整移动机器人在当前地图节点B所得的定位信息,使第一测量值尽量符合第二测量值。
因此,基于激光SLAM导航定位方式进行定位的误差为:第一测量值与第二测量值之间的差值,数学表达式为:e=Δξ′
1-Δξ
1 (式6)。
显然,e是关于ξ
B的一个函数。为此,可以求e关于当前地图节点B的位姿ξ
B=(x
B,y
B,θ
B)的偏导数,组成雅克比矩阵:
此外,基于第二测量值可以得到第二测量值的协方差矩阵Σ,如此就可以利用最小二乘法对当前地图节点B的姿态进行优化。
可选的,对当前地图节点B的姿态进行优化时,可以以上述第一距离和第一角度为约束进行优化。
另外,由于角度信息的存在,求解当前地图节点B的位姿成为求解非线性的方程。因此,可以采用迭代求解的方法实现,具体包括以下步骤3041-3044。
步骤3041,移动机器人将当前地图节点B的位姿ξ
B=(x
B,y
B,θ
B)作为当前地图节点B的位姿初始值,记为P。
初始状态下,将基于激光SLAM地图获得的当前地图节点B的位姿初始值作为当前地图节点B的位姿当前值。
步骤3042,移动机器人根据第一地图节点A的位姿、当前地图节点B的位姿当前值,按照式4计算第一测量值;按照里程计记录的第一距离和第一角度,按照式5计算第二测量值。
也就是说,本步骤中按照惯性传感器采集的数据确定第一距离和第一角度,并按照式5计算第二测量值。
步骤3043,移动机器人基于当前第一测量值和第二测量值,按照式6计算当前误差。
步骤3044,移动机器人判断是否满足迭代结束条件。
如果是,则将当前地图节点B的位姿当前值作为最终结果,并保存。将当前地图节点B加入已优化地图节点集合。
否则,为了减小误差,计算基于当前P的修正量δP,以得到修正后的估计值P
1,其中,P
1=P+δP。
上述计算P
1时,仅包含一部分:将当前地图节点B的位姿当前值与δP相加。
通过以下方式计算得到上述修正量:
对当前e求关于ξ
B=(x
B,y
B,θ
B)的偏导数,得到雅克比矩阵J。
根据高斯-牛顿最小二乘法,有:雅克比矩阵的转置矩阵、第二测量值的协方差矩阵的逆矩阵、雅克比矩阵、以及修正量的乘积等于雅克比矩阵的转置矩阵与当前误差乘积的负数,用数学式表达为:
J
TΣ
-1J·δP=-J
T·e
其中,J为雅克比矩阵,根据当前地图节点B的位姿当前值计算得到,Σ为第二测量值的协方差矩阵,其为3×3的矩阵,e为按照当前估计值所得到的当前测量值的误差,通过上式可求得用于迭代的修正量δP。
将修正后的估计值P
1作为P的当前值,具体包括:将P
1中当前地图节点B的位姿作为第二地图节点B的位姿当前值ξ
B,返回执行步骤3042。
所述迭代结束条件可以是,达到设定的迭代次数、误差e小于设定的误差阈值、修正量小于设定的修正阈值之一或其任意的组合。
上述迭代过程完成了当前地图节点的位姿的图优化,从而获得当前地图节点准确的位姿,该迭代过程只需求解三个未知数,从而有利于迭代的收敛,提高计算的速度。
步骤305,移动机器人判断行进路径是否结束、且所有设定的地图节点的位姿和纹理信息都被采集,如果是,则执行步骤306,否则,返回执行步骤302。
步骤306,与步骤105相同,移动机器人将各个地图节点的最终位姿、以及在各个地图节点采集的地面纹理信息进行保存,得到基于地面纹理的纹理地图。另外,移动机器人可以将纹理地图与激光SLAM地图合成为一体。
在本实施例中,图优化过程可以与步骤302的行走采集步骤并行执行,即,在移动机器人按照拟定的路径行进的同时,对于当前已生成的地图节点进行图优化,有利于提高构建地图的效率;并且,在图优化过程中,无可用的已优化地图节点时,可同时对两个地图节点进行优化,当有可用的已优化地图节点时,可仅对当前地图节点进行优化,以提高优化的收敛速度,从而提高了优化的整体性能。
一种情况下,上述无可用的已优化地图节点可以是指:已优化地图节点集合中不存在与当前地图节点之间距离小于等于设定的第一距离阈值的地图节点。由于无可用的已优化地图节点,这种情况下,可以对两个位姿均未被优化过、且距离小于等于第一距离阈值的地图节点进行优化,这样在同一优化过程中可以实现对两个地图节点优化。
参见图4,图4为移动机器人的一种示意图。该移动机器人包括:
控制行走模块,用于按照根据待建地图所需要构建的地图节点设定的行进路线,控制移动机器人基于激光即时定位与地图构建SLAM地图行进;
SLAM定位模块,用于基于SLAM导航定位方式获取每个地图节点位姿;
采集模块,用于在每个地图节点记录惯性传感器数据;
地图节点位姿优化模块,用于对于地图节点中满足距离小于等于第一距离阈值的任意两地图节点:根据地图节点的位姿获取所述两地图节点之间的第一测量值,根据惯性传感器数据获取所述两地图节点之间的第二测量值,基于第一测量值与第二测量值之间的误差,以所述两地图节点的惯性传感器数据作为所述两地图节点的位姿的约束,对所述两地图节点的位姿中至少一个地图节点的位姿进行优化;
地图生成模块,用于将优化后的地图节点的位姿作为待建地图的地图节点位姿输出。
本申请的一个实施例中,所述移动机器人还包括:
节点设定模块,用于获取激光SLAM地图,基于激光SLAM地图设定行进路线,并按照设定的间距设置地图节点;
所述控制行走模块,具体用于控制移动机器人处于各地图节点时采集地图节点的位姿以及惯性传感数据,直至所有地图节点的位姿以及惯性传感数据都被采集后,触发所述地图节点位姿优化模块。
本申请的一个实施例中,所述控制行走模块,具体用于按照根据待建地图所需要构建的地图节点设定的行进路线,控制移动机器人行进,在行进过程中基于激光SLAM导航定位方式建立激光SLAM地图;
所述采集模块,具体用于根据当前激光SLAM地图确定移动机器人的当前位置,当移动机器人到达地图节点时,采集当前惯性传感器数据;
所述SLAM定位模块,具体用于判断当前已优化地图节点集合中是否有与当前地图节点之间距离小于等于设定的第一距离阈值的第三地图节点;
如果有,则将第三地图节点和当前地图节点作为所述满足距离小于等于第一距离阈值的任意两地图节点,以进行当前地图节点的位姿优化,并在完成优化后将当前地图节点加入已优化地图节点集合;
否则,选择与当前地图节点之间的距离小于等于第一距离阈值的未被优化的第四地图节点,将第四地图节点和当前地图节点作为所述满足距离小于等于第一距离阈值的任意两地图节点,以进行两地图节点的位姿优化,并在完成优化后将第四地图节点和当前地图节点加入已优化地图节点集合;
判断行进路径是否结束、且所有设定的地图节点的位姿都被采集,如果是,则执行所述将优化后的地图节点的位姿作为待建地图的地图节点的位姿的步骤,否则,触发所述控制行走模块。
本申请的一个实施例中,所述地图节点位姿优化模块,具体用于当所述任意两地图节点均为未被优化的第一地图节点和第二地图节点时,根据基于SLAM导航定位方式获取的第一地图节点的位姿和第二地图节点的位姿,得到第二地图节点与第一地图节点在x方向的坐标之差、在y方向的坐标之差、以及姿态之差;根据惯性传感器数据中记录的从第一地图节点到第二地图节点的第一距离以及转过的第一角度,得到第一距离以第一角度在x方向上的投影、第一距离以第一角度在y方向上的投影、以及第一角度;将基于SLAM导航定位方式获取的第一地图节点的位姿作为第一地图节点的位姿初始值,将基于SLAM导航定位方式获取的第二地图节点的位姿作为第二地图节点的位姿初始值;以第一距离和第一角度作为所述第一地图节点的位姿和第二地图节点的位姿的约束;基于第一测量值与第二测量值之间的误差,采用最小二乘法,迭代求取优化后的第一地图节点的位姿和第二地图节点的位姿。
本申请的一个实施例中,所述地图节点位姿优化模块,具体用于将第一地图节点的位姿初始值作为第一地图节点的位姿当前值,将第二地图节点的位姿初始值作为第二地图节点的位姿当前值;根据第一地图节点的位姿当前值和第二地图节点的位姿当前值,计算当前第一测量值;根据当前第一测量值与第二测量值,计算当前误差;判断是否满足迭代结束条件;如果是,则将第一地图节点的位姿当前值和第二地图节点的位姿当前值作为优化后的结果;否则,计算一修正量,将修正量分别与第一地图节点的位姿当前值、以及修正后的第二地图节点的位姿当前值相加,将修正后的第一地图节点的位姿、以及第二地图节点的位姿作为各自的位姿当前值,然后返回执行所述根据第一地图节点的位姿当前值和第二地图节点的位姿当前值,计算当前第一测量值的步骤;
所述迭代结束条件包括:达到设定的迭代次数、所述误差小于设定的误差阈值、修正量小于设定的修正阈值之一或其任意的组合。
本申请的一个实施例中,所述修正量通过以下方式计算:
对当前误差求第一地图节点的位姿和第二地图节点的位姿的偏导数,得到雅克比矩阵;
根据雅克比矩阵的转置矩阵、第二测量值的协方差矩阵的逆矩阵、雅克比矩阵、以及修正量的乘积等于雅克比矩阵的转置矩阵与当前误差乘积的负数,确定修正量;
其中,所述雅克比矩阵根据第一地图节点的位姿当前值和第二地图节点的位姿当前值确定。
本申请的一个实施例中,所述地图节点位姿优化模块,具体用于当所述任意两地图节点为已优化的第三地图节点和待优化的当前地图节点时,根据已优化的第三地图节点的位姿和基于SLAM导航定位方式获取的当前地图节点的位姿,得到当前地图节点与第三地图节点在x方向的坐标之差、y方向的坐标之差、以及姿态之差;根据惯性传感器数据中记录的从第三地图节点到当前地图节点的第一距离以及转过的第一角度,得到第一距离以第一角度在x方向上的投影、第一距离以第一角度在y方向上的投影、以及第一角度;将当前地图节点的位姿作为当前地图节点的位姿初始值;以第一距离和第一角度作为第三地图节点的位姿和当前地图节点的位姿的约束;基于第一测量值与第二测量值之间的误差,采用最小二乘法,迭代求取优化后的当前地图节点的位姿。
本申请的一个实施例中,所述地图节点位姿优化模块,具体用于将当前地图节点的位姿初始值作为当前地图节点的位姿当前值;根据第三地图节点的位姿和当前地图节点的位姿当前值,计算当前第一测量值;根据当前第一测量值与第二测量值,计算当前误差;判断是否满足迭代结束条件;如果是,则将当前地图节点的位姿当前值作为优化后的结果;否则,计算一修正量,将修正量与当前地图节点的位姿当前值相加,将修正后的当前地图节点的位姿作为位姿当前值,然后返回执行所述根据第三地图节点的位姿和当前地图节点的位姿当前值,计算当前第一测量值的步骤;
所述迭代结束条件包括:达到设定的迭代次数、所述误差小于设定的误差阈值、修正量小于设定的修正阈值之一或其任意的组合。
本申请的一个实施例中,所述修正量通过以下方式计算:
对当前误差求当前地图节点的位姿的偏导数,得到雅克比矩阵;
根据雅克比矩阵的转置矩阵、第二测量值的协方差矩阵的逆矩阵、雅克比矩阵、以及修正量的乘积等于雅克比矩阵的转置矩阵与当前误差乘积的负数,确定修正量;
其中,所述雅克比矩阵根据当前地图节点的位姿当前值确定。
本申请的一个实施例中,所述采集模块,还用于若所述待建地图为地面纹理地图,在每个地图节点记录惯性传感器数据之后,在每个地图节点采集地面纹理信息,并保存;
所述地图生成模块,具体用于将各个地图节点优化后的位姿以及在各个地图节点采集的地面纹理信息进行保存,得到基于地面纹理的纹理地图;将纹理地图与激光SLAM地图合成为一体。
本申请的一个实施例中,还提供了一种移动机器人,所述移动机器人包括有存储器和处理器,其中,存储器存储有计算机程序,处理器可用于执行计算机程序以实现本申请实施例所述的视觉地图构建的步骤。
存储器可以包括随机存取存储器(Random Access Memory,RAM),也可以包括非易失性存储器(Non-Volatile Memory,NVM),例如至少一个磁盘存储器。可选的,存储器还可以是至少一个位于远 离前述处理器的存储装置。
上述的处理器可以是通用处理器,包括中央处理器(Central Processing Unit,CPU)、网络处理器(Network Processor,NP)等;还可以是数字信号处理器(Digital Signal Processing,DSP)、专用集成电路(Application Specific Integrated Circuit,ASIC)、现场可编程门阵列(Field-Programmable Gate Array,FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
本申请实施例还提供了一种计算机可读存储介质,所述存储介质内存储有计算机程序,所述计算机程序被处理器执行时实现上述地图构建步骤。
本申请的一个实施例中,还提供了一种包含指令的计算机程序产品,当其在计算机上运行时,使得计算机执行本申请实施例中所述的视觉地图构建方法的步骤。
对于移动机器人、存储介质、计算机程序产品实施例而言,由于其基本相似于方法实施例,所以描述的比较简单,相关之处参见方法实施例的部分说明即可。
在本文中,诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来,而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且,术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下,由语句“包括一个……”限定的要素,并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
以上所述仅为本申请的较佳实施例而已,并不用以限制本申请,凡在本申请的精神和原则之内,所做的任何修改、等同替换、改进等,均应包含在本申请保护的范围之内。
Claims (13)
- 一种视觉地图构建方法,该方法包括:按照根据待建地图所需要构建的地图节点设定的行进路线,控制移动机器人基于激光即时定位与地图构建SLAM地图行进,基于SLAM导航定位方式获取每个地图节点的位姿,在每个地图节点记录惯性传感器数据;对获取的地图节点的位姿进行优化,该优化包括,对于地图节点中满足距离小于等于第一距离阈值的任意两地图节点:根据地图节点的位姿获取所述两地图节点之间位姿的第一测量值,根据惯性传感器数据获取所述两地图节点之间位姿的第二测量值,基于第一测量值与第二测量值之间的误差,以所述两地图节点的惯性传感器数据作为所述两地图节点的位姿的约束,对所述两地图节点的位姿中至少一个地图节点的位姿进行优化;将优化后的地图节点的位姿作为待建地图的地图节点的位姿。
- 如权利要求1所述的方法,其中,按照以下方式设定所述待建地图所需要构建的地图节点:获取激光SLAM地图,基于激光SLAM地图设定行进路线,并按照设定的间距设置地图节点;所述控制移动机器人基于激光SLAM地图行进,包括:控制移动机器人处于各地图节点时采集地图节点的位姿以及惯性传感数据,直至所有地图节点的位姿以及惯性传感数据都被采集后,执行所述对获取的地图节点的位姿进行优化的步骤。
- 如权利要求1所述的方法,其中,所述按照根据待建地图所需要构建的地图节点设定的行进路线,控制移动机器人基于激光SLAM地图行进,基于SLAM导航定位方式获取每个地图节点的位姿,在每个地图节点记录惯性传感器数据,包括:按照根据待建地图所需要构建的地图节点设定的行进路线,控制移动机器人行进,在行进过程中基于激光SLAM导航定位方式建立激光SLAM地图,并根据当前激光SLAM地图确定移动机器人的当前位置,当移动机器人到达地图节点时,采集当前惯性传感器数据;判断当前已优化地图节点集合中是否有与当前地图节点之间距离小于等于设定的第一距离阈值的第三地图节点;如果有,则将第三地图节点和当前地图节点作为所述满足距离小于等于第一距离阈值的任意两地图节点,以进行当前地图节点的位姿优化,并在完成优化后将当前地图节点加入已优化地图节点集合;否则,选择与当前地图节点之间的距离小于等于第一距离阈值的未被优化的第四地图节点,将第四地图节点和当前地图节点作为所述满足距离小于等于第一距离阈值的任意两地图节点,以进行两地图节点的位姿优化,并在完成优化后将第四地图节点和当前地图节点加入已优化地图节点集合;判断行进路径是否结束、且所有设定的地图节点的位姿都被采集,如果是,则执行所述将优化后的地图节点的位姿作为待建地图的地图节点的位姿的步骤,否则,返回执行所述按照待建地图所需要构建的地图节点和所设定的行进路线,控制移动机器人基于激光SLAM地图行进的步骤。
- 如权利要求1至3中任一项所述的方法,其中,当所述任意两地图节点均为未被优化的第一地图节点和第二地图节点时,所述根据地图节点的位姿获取所述两地图节点之间位姿的第一测量值,包括:根据基于SLAM导航定位方式获取的第一地图节点的位姿和第二地图节点的位姿,得到第二地图节点与第一地图节点在x方向的坐标之差、在y方向的坐标之差、以及姿态之差;所述根据惯性传感器数据获取所述两地图节点之间位姿的第二测量值,包括:根据惯性传感器数据中记录的从第一地图节点到第二地图节点的第一距离以及转过的第一角度,得到第一距离以第一角度在x方向上的投影、第一距离以第一角度在y方向上的投影、以及第一角度;所述基于第一测量值与第二测量值之间的误差,以所述两地图节点的惯性传感器数据作为所述两地图节点的位姿的约束,对所述两地图节点的位姿中至少一个地图节点的位姿进行优化,包括:将基于SLAM导航定位方式获取的第一地图节点的位姿作为第一地图节点的位姿初始值,将基于SLAM导航定位方式获取的第二地图节点的位姿作为第二地图节点的位姿初始值;以第一距离和第一角度作为所述第一地图节点的位姿和第二地图节点的位姿的约束;基于第一测量值与第二测量值之间的误差,采用最小二乘法,迭代求取优化后的第一地图节点的位姿和第二地图节点的位姿。
- 如权利要求4所述的方法,其中,所述基于第一测量值与第二测量值之间的误差,采用最小二乘法,迭代求取优化后的第一地图节点的位姿和第二地图节点的位姿,包括:将第一地图节点的位姿初始值作为第一地图节点的位姿当前值,将第二地图节点的位姿初始值作为第二地图节点的位姿当前值;根据第一地图节点的位姿当前值和第二地图节点的位姿当前值,计算当前第一测量值;根据当前第一测量值与第二测量值,计算当前误差;判断是否满足迭代结束条件;如果是,则将第一地图节点的位姿当前值和第二地图节点的位姿当前值作为优化后的结果;否则,计算一修正量,将修正量分别与第一地图节点的位姿当前值、以及第二地图节点的位姿当前值相加,将修正后的第一地图节点的位姿、以及修正后的第二地图节点的位姿作为各自的位姿当前值,然后返回执行所述根据第一地图节点的位姿当前值和第二地图节点的位姿当前值,计算当前第一测量值的步骤;所述迭代结束条件包括:达到设定的迭代次数、所述误差小于设定的误差阈值、修正量小于设定的修正阈值之一或其任意的组合。
- 如权利要求5所述的方法,其中,所述修正量通过以下方式计算:对当前误差求第一地图节点的位姿和第二地图节点的位姿的偏导数,得到雅克比矩阵;根据雅克比矩阵的转置矩阵、第二测量值的协方差矩阵的逆矩阵、雅克比矩阵、以及修正量的乘积等于雅克比矩阵的转置矩阵与当前误差乘积的负数,确定修正量;其中,所述雅克比矩阵根据第一地图节点的位姿当前值和第二地图节点的位姿当前值确定。
- 如权利要求3所述的方法,其中,当所述任意两地图节点为已优化的第三地图节点和待优化的当前地图节点时,所述根据地图节点的位姿获取所述两地图节点之间位姿的第一测量值,包括:根据已优化的第三地图节点的位姿和基于SLAM导航定位方式获取的当前地图节点的位姿,得到当前地图节点与第三地图节点在x方向的坐标之差、y方向的坐标之差、以及姿态之差;所述根据惯性传感器数据获取所述两地图节点之间位姿的第二测量值,包括:根据惯性传感器数据中记录的从第三地图节点到当前地图节点的第一距离以及转过的第一角度,得到第一距离以第一角度在x方向上的投影、第一距离以第一角度在y方向上的投影、以及第一角度;所述基于第一测量值与第二测量值之间的误差,以所述两地图节点的惯性传感器数据作为所述两地图节点的位姿的约束,对所述两地图节点的位姿中至少一个地图节点的位姿进行优化,包括:将当前地图节点的位姿作为当前地图节点的位姿初始值;以第一距离和第一角度作为第三地图节点的位姿和当前地图节点的位姿的约束;基于第一测量值与第二测量值之间的误差,采用最小二乘法,迭代求取优化后的当前地图节点的位姿。
- 如权利要求7所述的方法,其中,所述基于第一测量值与第二测量值之间的误差,采用最小二乘法,迭代求取优化后的当前地图节点的位姿,包括:将当前地图节点的位姿初始值作为当前地图节点的位姿当前值;根据第三地图节点的位姿和当前地图节点的位姿当前值,计算当前第一测量值;根据当前第一测量值与第二测量值,计算当前误差;判断是否满足迭代结束条件;如果是,则将当前地图节点的位姿当前值作为优化后的结果;否则,计算一修正量,将修正量与当前地图节点的位姿当前值相加,将修正后的当前地图节点的位姿作为位姿当前值,然后返回执行所述根据第三地图节点的位姿和当前地图节点的位姿当前值,计算当前第一测量值的步骤;所述迭代结束条件包括:达到设定的迭代次数、所述误差小于设定的误差阈值、修正量小于设定的修正阈值之一或其任意的组合。
- 如权利要求8所述的方法,其中,所述修正量通过以下方式计算:对当前误差求当前地图节点的位姿的偏导数,得到雅克比矩阵;根据雅克比矩阵的转置矩阵、第二测量值的协方差矩阵的逆矩阵、雅克比矩阵、以及修正量的乘积等于雅克比矩阵的转置矩阵与当前误差乘积的负数,确定修正量;其中,所述雅克比矩阵根据当前地图节点的位姿当前值确定。
- 如权利要求1所述的方法,其中,所述待建地图为地面纹理地图,所述在每个地图节点记录惯性传感器数据之后,还包括:在每个地图节点采集地面纹理信息,并保存;所述将优化后的地图节点的位姿作为待建地图的地图节点的位姿,包括:将各个地图节点优化后的位姿以及在各个地图节点采集的地面纹理信息进行保存,得到基于地面纹理的纹理地图;将纹理地图与激光SLAM地图合成为一体。
- 一种移动机器人,包括,控制行走模块,按照根据待建地图所需要构建的地图节点设定的行进路线,控制移动机器人基于激光即时定位与地图构建SLAM地图行进;SLAM定位模块,基于SLAM导航定位方式获取每个地图节点位姿;采集模块,在每个地图节点记录惯性传感器数据;地图节点位姿优化模块,对于地图节点中满足距离小于等于第一距离阈值的任意两地图节点:根据地图节点的位姿获取所述两地图节点之间位姿的第一测量值,根据惯性传感器数据获取所述两地图节点 之间位姿的第二测量值,基于第一测量值与第二测量值之间的误差,以所述两地图节点的惯性传感器数据作为所述两地图节点的位姿的约束,对所述两地图节点的位姿中至少一个地图节点的位姿进行优化;地图生成模块,将优化后的地图节点的位姿作为待建地图的地图节点位姿。
- 一种计算机可读存储介质,所述存储介质内存储有计算机程序,所述计算机程序被处理器执行时实现如权利要求1至10任一所述视觉地图构建方法的步骤。
- 一种移动机器人,所述移动机器人包括:存储器和处理器,其中,存储器存储有计算机程序,处理器可用于执行计算机程序以实现权利要求1至10任一所述视觉地图构建方法的步骤。
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010510451.5 | 2020-06-08 | ||
CN202010510451.5A CN113835422B (zh) | 2020-06-08 | 2020-06-08 | 一种视觉地图构建方法和移动机器人 |
Publications (1)
Publication Number | Publication Date |
---|---|
WO2021249387A1 true WO2021249387A1 (zh) | 2021-12-16 |
Family
ID=78845351
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
PCT/CN2021/098881 WO2021249387A1 (zh) | 2020-06-08 | 2021-06-08 | 一种视觉地图构建方法和移动机器人 |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN113835422B (zh) |
WO (1) | WO2021249387A1 (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114543797A (zh) * | 2022-02-18 | 2022-05-27 | 北京市商汤科技开发有限公司 | 位姿预测方法和装置、设备、介质 |
CN114608554A (zh) * | 2022-02-22 | 2022-06-10 | 北京理工大学 | 一种手持slam设备以及机器人即时定位与建图方法 |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114463429B (zh) * | 2022-04-12 | 2022-08-16 | 深圳市普渡科技有限公司 | 机器人、地图创建方法、定位方法及介质 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20130158865A1 (en) * | 2011-12-15 | 2013-06-20 | Electronics And Telecommunications Research Institute | Method and apparatus for estimating position of moving object |
CN105631017A (zh) * | 2015-12-29 | 2016-06-01 | 福州华鹰重工机械有限公司 | 离线坐标校准和地图创建的方法及装置 |
CN105783913A (zh) * | 2016-03-08 | 2016-07-20 | 中山大学 | 一种融合车载多传感器的slam装置及其控制方法 |
CN108253958A (zh) * | 2018-01-18 | 2018-07-06 | 亿嘉和科技股份有限公司 | 一种稀疏环境下的机器人实时定位方法 |
CN109084732A (zh) * | 2018-06-29 | 2018-12-25 | 北京旷视科技有限公司 | 定位与导航方法、装置及处理设备 |
CN109682373A (zh) * | 2018-12-28 | 2019-04-26 | 中国兵器工业计算机应用技术研究所 | 一种无人平台的感知系统 |
Family Cites Families (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109933056B (zh) * | 2017-12-18 | 2022-03-15 | 九阳股份有限公司 | 一种基于slam的机器人导航方法以及机器人 |
JP6974189B2 (ja) * | 2018-01-16 | 2021-12-01 | 株式会社豊田中央研究所 | 地図作成装置 |
CN108319976B (zh) * | 2018-01-25 | 2019-06-07 | 北京三快在线科技有限公司 | 建图方法及装置 |
CN109556596A (zh) * | 2018-10-19 | 2019-04-02 | 北京极智嘉科技有限公司 | 基于地面纹理图像的导航方法、装置、设备及存储介质 |
CN110375738B (zh) * | 2019-06-21 | 2023-03-14 | 西安电子科技大学 | 一种融合惯性测量单元的单目同步定位与建图位姿解算方法 |
CN110706248B (zh) * | 2019-08-20 | 2024-03-12 | 广东工业大学 | 一种基于slam的视觉感知建图方法及移动机器人 |
CN110553652B (zh) * | 2019-10-12 | 2022-06-24 | 上海高仙自动化科技发展有限公司 | 机器人多传感器融合定位方法及其应用 |
CN111076733B (zh) * | 2019-12-10 | 2022-06-14 | 亿嘉和科技股份有限公司 | 一种基于视觉与激光slam的机器人室内建图方法及系统 |
-
2020
- 2020-06-08 CN CN202010510451.5A patent/CN113835422B/zh active Active
-
2021
- 2021-06-08 WO PCT/CN2021/098881 patent/WO2021249387A1/zh active Application Filing
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20130158865A1 (en) * | 2011-12-15 | 2013-06-20 | Electronics And Telecommunications Research Institute | Method and apparatus for estimating position of moving object |
CN105631017A (zh) * | 2015-12-29 | 2016-06-01 | 福州华鹰重工机械有限公司 | 离线坐标校准和地图创建的方法及装置 |
CN105783913A (zh) * | 2016-03-08 | 2016-07-20 | 中山大学 | 一种融合车载多传感器的slam装置及其控制方法 |
CN108253958A (zh) * | 2018-01-18 | 2018-07-06 | 亿嘉和科技股份有限公司 | 一种稀疏环境下的机器人实时定位方法 |
CN109084732A (zh) * | 2018-06-29 | 2018-12-25 | 北京旷视科技有限公司 | 定位与导航方法、装置及处理设备 |
CN109682373A (zh) * | 2018-12-28 | 2019-04-26 | 中国兵器工业计算机应用技术研究所 | 一种无人平台的感知系统 |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114543797A (zh) * | 2022-02-18 | 2022-05-27 | 北京市商汤科技开发有限公司 | 位姿预测方法和装置、设备、介质 |
CN114543797B (zh) * | 2022-02-18 | 2024-06-07 | 北京市商汤科技开发有限公司 | 位姿预测方法和装置、设备、介质 |
CN114608554A (zh) * | 2022-02-22 | 2022-06-10 | 北京理工大学 | 一种手持slam设备以及机器人即时定位与建图方法 |
CN114608554B (zh) * | 2022-02-22 | 2024-05-03 | 北京理工大学 | 一种手持slam设备以及机器人即时定位与建图方法 |
Also Published As
Publication number | Publication date |
---|---|
CN113835422B (zh) | 2023-09-29 |
CN113835422A (zh) | 2021-12-24 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
WO2021249387A1 (zh) | 一种视觉地图构建方法和移动机器人 | |
US11988781B2 (en) | Extrinsic calibration method of multiple 3D LiDAR sensors for autonomous navigation system | |
CN107160395B (zh) | 地图构建方法及机器人控制系统 | |
JP6445995B2 (ja) | センサデータの空間的集約を用いる適応マッピング | |
WO2021139590A1 (zh) | 基于蓝牙与slam的室内定位导航装置及其方法 | |
Wang et al. | A simple and parallel algorithm for real-time robot localization by fusing monocular vision and odometry/AHRS sensors | |
CN111220153B (zh) | 基于视觉拓扑节点和惯性导航的定位方法 | |
WO2019136714A1 (zh) | 一种基于3d激光的地图构建方法及系统 | |
JP2020530569A (ja) | 車両センサの較正及び位置特定 | |
WO2017008454A1 (zh) | 一种机器人的定位方法 | |
US7747348B2 (en) | Method and apparatus for using rotational movement amount of mobile device and computer-readable recording medium for storing computer program | |
CN103412565A (zh) | 一种具有全局位置快速估计能力的机器人及其定位方法 | |
US20230083965A1 (en) | Map construction method, apparatus and storage medium | |
JP6827642B2 (ja) | 車輪付き車両用自己較正センサシステム | |
CN114018248B (zh) | 一种融合码盘和激光雷达的里程计方法与建图方法 | |
CN114526745A (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
TW202036030A (zh) | 資訊處理裝置以及移動機器人 | |
WO2021233452A1 (zh) | 一种地图更新方法以及装置 | |
He et al. | Camera-odometer calibration and fusion using graph based optimization | |
Wen et al. | GNSS/LiDAR integration aided by self-adaptive Gaussian mixture models in urban scenarios: An approach robust to non-Gaussian noise | |
CN113639722B (zh) | 连续激光扫描配准辅助惯性定位定姿方法 | |
Hu et al. | A small and lightweight autonomous laser mapping system without GPS | |
WO2022161271A1 (zh) | 斜坡定位校正方法、装置、机器人及可读存储介质 | |
CN113483762A (zh) | 一种位姿优化方法及设备 | |
CN117015719A (zh) | 用于确定刚体的运动状态的方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
121 | Ep: the epo has been informed by wipo that ep was designated in this application |
Ref document number: 21822885 Country of ref document: EP Kind code of ref document: A1 |
|
NENP | Non-entry into the national phase |
Ref country code: DE |
|
122 | Ep: pct application non-entry in european phase |
Ref document number: 21822885 Country of ref document: EP Kind code of ref document: A1 |