High-precision positioning and deviation rectifying method, system and medium integrating vision and laser
Technical Field
The invention relates to the technical field of cleaning robot software algorithms, in particular to a vision and laser fusion high-precision positioning and deviation rectifying method, a system and a medium, and more particularly relates to a vision and laser fusion high-precision positioning and deviation rectifying method and a system based on a hotel cleaning robot.
Background
The invention discloses a hotel cleaning robot, which is a service robot for cleaning a hotel room toilet, realizes intelligent cleaning combining autonomous navigation and autonomous cleaning functions, has accurate cleaning capability which cannot be achieved by people, can reasonably plan cleaning paths and cleaning steps for different objects or different cleaning areas, and has a certain arrival error due to navigation, so that the error can influence the overall cleaning precision of the robot to a certain extent.
Patent document CN114158984A (application number: 202111581831.9) discloses a cleaning robot including: the robot comprises a controller function module, a mechanical arm function module, a navigation system module, a vision system module, a clamping jaw function module, a sweeper function module and a UI (user interface) module; the controller function module comprises a master controller and a plurality of slave controllers, the master controller is connected with the plurality of slave controllers, and the controller is connected with the other function modules; the mechanical arm functional module is used for executing cleaning action; the navigation system module is used for taking charge of the advancing and positioning functions of the robot; the visual system module is used for realizing mode recognition, indoor environment reconstruction and auxiliary positioning; the clamping jaw function module is used for installing and fixing a cleaning tool and is arranged at the tail end of the mechanical arm function module; the sweeper functional module is used for executing sweeping actions; the UI interface module is used for providing a visual operation interface.
Disclosure of Invention
Aiming at the defects in the prior art, the invention aims to provide a high-precision positioning and deviation rectifying method, system and medium with vision and laser fusion.
The invention provides a high-precision positioning and deviation rectifying method based on vision and laser fusion, which comprises the following steps:
step S1: the cleaning robot moves to a designated cleaning point position by using the laser radar;
step S2: the cleaning robot scans a cleaning object by using a camera to obtain a 3D model of the object;
step S3: matching the 3D model obtained by scanning with a previously established 3D model of the cleaning object meeting the preset requirement, and calculating the visual point position with the best matching degree;
step S4: controlling the robot to correspondingly move by using the corresponding position relation between the vision and the center of the chassis of the robot;
step S5: after the robot finishes moving, acquiring the 3D model of the cleaning object again, calculating the matching degree of the 3D model of the current cleaning object and the 3D model of the cleaning object meeting the preset requirement, judging whether the error meets the preset requirement, and when the error does not meet the preset requirement, repeatedly triggering the steps S2 to S5 until the error meets the preset requirement;
the 3D model of the cleaning object meeting the preset requirement is obtained by placing the cleaning robot at a preset position through a cleaning robot camera.
Preferably, the step S1 adopts:
step S1.1: establishing a 2d grid map used for corresponding navigation by using a laser radar;
step S1.2: and based on the established 2d grid map, positioning by using a laser radar of the cleaning robot to reach the designated cleaning point.
Preferably, the step S3 adopts: and iteratively completing the matching of the 3D model obtained by current scanning and the previously established 3D model of the cleaning object meeting the preset requirement based on an icp registration algorithm, thereby obtaining the visual point location with the best matching degree.
Preferably, the step S4 adopts: and calculating the distance and the angle of the center of the robot chassis relative to the cleaning object by using the corresponding position relation between the vision and the center of the robot chassis, and controlling the robot to move correspondingly.
The invention provides a vision and laser fused high-precision positioning and deviation rectifying system, which comprises:
module M1: the cleaning robot moves to a designated cleaning point position by using the laser radar;
module M2: the cleaning robot scans a cleaning object by using a camera to obtain a 3D model of the object;
module M3: matching the 3D model obtained by scanning with a previously established 3D model of the cleaning object meeting the preset requirement, and calculating the visual point position with the best matching degree;
module M4: controlling the robot to move correspondingly by using the corresponding position relation between the vision and the center of the robot chassis;
module M5: after the robot finishes moving, the 3D model of the cleaning object is obtained again, the matching degree of the 3D model of the current cleaning object and the 3D model of the cleaning object meeting the preset requirement is calculated, whether the error meets the preset requirement or not is judged, and when the error does not meet the preset requirement, the module M2 to the module M5 are triggered repeatedly until the error meets the preset requirement;
the 3D model of the cleaning object meeting the preset requirement is obtained by placing the cleaning robot at a preset position through a cleaning robot camera.
Preferably, the module M1 employs:
module M1.1: establishing a 2d grid map used for corresponding navigation by using a laser radar;
module M1.2: and based on the established 2d grid map, positioning by using a laser radar of the cleaning robot to reach the designated cleaning point.
Preferably, the module M3 adopts: and iteratively completing the matching of the 3D model obtained by current scanning and the previously established 3D model of the clean object meeting the preset requirement based on an icp registration algorithm, thereby obtaining the visual point location with the best matching degree.
Preferably, the module M4 employs: and calculating the distance and the angle of the center of the robot chassis relative to the cleaning object by using the corresponding position relation between the vision and the center of the robot chassis, and controlling the robot to move correspondingly.
According to the present invention, a computer-readable storage medium is provided, in which a computer program is stored, which computer program, when being executed by a processor, carries out the steps of the method as described above.
The present invention provides a cleaning robot including: a controller;
the controller comprises the computer readable storage medium storing the computer program, or the controller comprises the vision and laser fused high-precision positioning deviation correcting system.
Compared with the prior art, the invention has the following beneficial effects:
1. the visual positioning and deviation rectifying device can also avoid the collision problem caused by the fact that the 2D laser navigation cannot detect the space obstacle;
2. the arrival precision of the cleaning target point is improved, and the overall stability and accuracy of cleaning by the cleaning robot are greatly improved;
3. the defect of lack of 2D laser characteristic points is made up by using abundant image characteristics of vision, and the auxiliary positioning before the robot arrives at a point is facilitated;
4. the 3D modeling can be carried out on the object to be cleaned in advance through vision, so that the navigation to the point is facilitated, the current object to be cleaned can be quickly identified and matched, and the final robot positioning and deviation correction can be realized.
Drawings
Other features, objects and advantages of the invention will become more apparent upon reading of the detailed description of non-limiting embodiments with reference to the following drawings:
fig. 1 is a flow chart of a high-precision positioning and deviation rectifying method based on vision and laser fusion of a hotel cleaning robot.
Detailed Description
The present invention will be described in detail with reference to specific examples. The following examples will assist those skilled in the art in further understanding the invention, but are not intended to limit the invention in any way. It should be noted that it would be obvious to those skilled in the art that various changes and modifications can be made without departing from the spirit of the invention. All falling within the scope of the present invention.
Example 1
The invention provides a high-precision positioning and deviation rectifying method integrating vision and laser aiming at the problem that a certain point arriving error exists when a hotel cleaning robot navigates to a point, and the method makes use of rich image characteristics of the vision to make up for the deficiency of 2D laser characteristic points and is beneficial to auxiliary positioning before the robot arrives at the point;
the walking position of the robot can be effectively controlled through vision so as to avoid unnecessary collision in the space;
the 3D modeling can be carried out on the object needing to be cleaned in advance through vision, so that the object needing to be cleaned can be rapidly identified and matched after the object is navigated to a point, and the final robot positioning and deviation correction can be realized.
The invention provides a high-precision positioning and deviation rectifying method based on vision and laser fusion, which comprises the following steps of:
step 1: establishing a 2d grid map used for corresponding navigation by using a two-dimensional laser radar;
step 2: marking each room point location and each cleaning point location in the room on a map;
and 3, step 3: with the issuing of the cleaning task, the robot starts to go from a linen room, goes to a target guest room toilet and reaches a designated cleaning point;
and 4, step 4: scanning a clean object by using a camera, matching the scanned 3D model with a previously established 3D model of the clean object, and calculating a visual point location with the best matching degree, so that the distance and the angle between the center of the chassis of the robot and the clean object are calculated by using the corresponding position relation between vision and the center of the chassis of the robot, and finally the robot is controlled to move correspondingly;
specifically, calculating by an icp registration algorithm to obtain a visual point with the best matching degree; the ICP is an Iterative Closest Point algorithm (ICP), and the aim is to perform Point cloud registration, where Point cloud registration refers to inputting two Point clouds (source) and (target), and outputting a rotational-translational transformation to make the coincidence degree of the two Point clouds as high as possible.
Specifically, the calculated visual point position with the best matching degree is used for calculating the position deviation between the current camera and the camera for acquiring the 3D model of the cleaning object built in advance, and then the position deviation is converted to the center of the chassis to calculate the position deviation of the center of the chassis.
And 5: due to the movement error of the robot chassis, the cleaning requirement can be met only by moving the robot for many times, and therefore, the operation of the step 4 is carried out again after the robot moves until the error reaches a certain error threshold range.
The invention provides a vision and laser fused high-precision positioning and deviation rectifying system, which comprises:
module 1: establishing a 2d grid map used for corresponding navigation by using a two-dimensional laser radar;
and (3) module 2: marking each room point location and each cleaning point location in the room on a map;
and a module 3: with the issuing of the cleaning task, the robot starts to go from a linen room, goes to a target guest room toilet and reaches a designated cleaning point;
and a module 4: scanning a clean object by using a camera, matching the scanned 3D model with a previously established 3D model of the clean object, and calculating a visual point location with the best matching degree, so that the distance and the angle between the center of the chassis of the robot and the clean object are calculated by using the corresponding position relation between vision and the center of the chassis of the robot, and finally the robot is controlled to move correspondingly;
specifically, calculating by an icp registration algorithm to obtain a visual point location with the best matching degree; ICP is an Iterative Closest Point (ICP) algorithm, which aims to perform Point cloud registration, and Point cloud registration refers to inputting two Point clouds (source) and (target) and outputting a rotational-translational transformation so that the degree of coincidence of the two Point clouds is as high as possible.
Specifically, the calculated visual point position with the best matching degree is used for calculating the position deviation between the current camera and the camera for acquiring the 3D model of the cleaning object built in advance, and then the position deviation is converted to the center of the chassis to calculate the position deviation of the center of the chassis.
And a module 5: due to the movement error of the robot chassis, the cleaning requirement can be met only by moving the robot for many times, so that the robot can operate the module 4 again after the movement is completed until a certain error threshold range is reached.
It is known to those skilled in the art that, in addition to implementing the system, apparatus and its various modules provided by the present invention in pure computer readable program code, the system, apparatus and its various modules provided by the present invention can be implemented in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers and the like by completely programming the method steps. Therefore, the system, the device and the modules thereof provided by the present invention can be considered as a hardware component, and the modules included in the system, the device and the modules thereof for implementing various programs can also be considered as structures in the hardware component; modules for performing various functions may also be considered to be both software programs for performing the methods and structures within hardware components.
The foregoing description of specific embodiments of the present invention has been presented. It is to be understood that the present invention is not limited to the specific embodiments described above, and that various changes or modifications may be made by one skilled in the art within the scope of the appended claims without departing from the spirit of the invention. The embodiments and features of the embodiments of the present application may be combined with each other arbitrarily without conflict.