Method, device, mobile robot, and medium for estimating the position and posture of a mobile robot

Abstract

The present application relates to a method, an apparatus, a mobile robot, and a medium for estimating the position and posture of a mobile robot, the method including the steps of acquiring an environmental three-dimensional point cloud when the mobile robot performs a planar task, and determining a work plane point cloud from the environmental three-dimensional point cloud based on a predetermined height threshold, acquiring inertial measurement information of the mobile robot, and determining the position and posture of the mobile robot in a world coordinate system based on the inertial measurement information and the work plane point cloud.

Description

[Technical Field]

[0001] (Related Applications) This application claims priority to a Chinese patent application filed with the China Patent Office on December 30, 2022, application number 202211724857.9, entitled "Method, device, mobile robot and medium for estimating the position and posture of a mobile robot," the entire contents of which are incorporated herein by reference.

[0002] The present application relates to the technical field of robot position and orientation estimation, and more particularly to a method, an apparatus, a mobile robot, and a medium for estimating the position and orientation of a mobile robot. [Background technology]

[0003] When a mobile robot performs a task on a flat surface, the mobile robot's attitude angle is usually aligned with the working plane, reducing the robot's degrees of freedom and simplifying the complexity of solving the robot's motion problem. However, in real situations, the robot's operating environment may contain depressions, thresholds, etc., which may cause the robot to bump into and change its attitude, affecting the robot's ability to sense surrounding obstacles, resulting in inaccurate positioning and attitude of the robot and problems such as obstacle marking errors.

[0004] In the prior art, robot posture detection methods for performing planar tasks are usually obtained by calculating angle values ​​measured by an inertial measurement unit (IMU). However, low-cost IMUs often have relatively large error values, making it difficult to obtain an accurate robot posture on the working plane. Summary of the Invention

[0005] In view of the above circumstances, the embodiments of the present application provide a method, apparatus, mobile robot, and medium for estimating the position and attitude of a mobile robot, which combines point cloud data collected by a line laser and angle data collected by an inertial measurement unit to more accurately estimate the floor attitude angle of the mobile robot when the mobile robot moving in a planar motion encounters a depression or threshold on the working plane, and correct the positioning position and attitude of the mobile robot.

[0006] In a first aspect, an embodiment of the present application provides a method for estimating a position and posture of a mobile robot, the method comprising: acquiring an environmental 3D point cloud when the mobile robot is performing a planar task, and determining a work plane point cloud from the environmental 3D point cloud based on a preset height threshold; acquiring inertial measurement information of the mobile robot; and determining the position and orientation of the mobile robot in a world coordinate system based on the inertial measurement information and the work plane point cloud.

[0007] Optionally, in the above method, the mobile robot is equipped with a line laser module, the line laser module emits at least two line lasers, and the points of the at least two line lasers projected onto the work plane are not located on the same line; The step of acquiring an environmental three-dimensional point cloud when the mobile robot executes a planar task includes: Using the light beam emitted by the laser module, obtain an original point cloud of the laser line center in the image pixel coordinate system when the mobile robot performs a planar task; establishing a first constraint from the original point cloud of the laser line center based on a mapping from the camera coordinate system to an image pixel coordinate system; establishing a second constraint based on a line laser light plane equation in the camera coordinate system; combining the first constraint and the second constraint to determine the environment 3D point cloud.

[0008] Optionally, in the method, the step of determining a work plane point cloud from the environment 3D point cloud based on the preset height threshold comprises: determining whether a height value of each point in the environmental 3D point cloud is less than the height threshold, and if so, attributing the corresponding point cloud to a work plane of the mobile robot; Determining whether the number of points to be attributed to the work plane is greater than or equal to a predetermined number threshold, and if so, using the points belonging to the work plane of the mobile robot as the work plane point cloud.

[0009] Optionally, in the above method, the step of determining the position and posture of the mobile robot in a world coordinate system based on the inertial measurement information and the work plane point cloud comprises: constructing a first error term of an attitude estimation model based on the inertial measurement information, the first error term being determined based on a roll angle error term and a pitch angle error term; A second error term of the pose estimation model is constructed based on the flush condition of the work plane point cloud, and the second error term is determined based on the distance from each point cloud that falls on the work plane to the work plane; constructing a least squares optimization model as the pose estimation model based on the first error term and the second error term; Solving the pose estimation model to obtain roll and pitch angle estimates for the mobile robot in the world coordinate system.

[0010] Optionally, in the method, constructing a second error term of the pose estimation model based on a coplanar condition of the work plane point cloud comprises: determining a normal vector value of a work plane normal vector in a robot coordinate system using the roll angle estimated value and the pitch angle estimated value as variables; determining a work plane equation for the work plane in the robot coordinate system and a plurality of coefficient values ​​in the work plane equation based on the normal vector value; determining a distance value from each point of the work plane point cloud to the work plane based on the work plane equation and the plurality of coefficient values; determining the second error term based on a distance value from each of the points to the work plane.

[0011] Optionally, in the method, the environment 3D point cloud includes an obstacle point cloud, and the method further comprises: The method further includes a step of correcting the obstacle point cloud based on the determined position and posture of the mobile robot in the world coordinate system, and acquiring three-dimensional coordinates of the obstacle point cloud in the world coordinate system.

[0012] Optionally, in the above method, the step of modifying the obstacle point cloud based on the determined position and orientation of the mobile robot in the world coordinate system includes: determining a first transformation matrix from the camera coordinate system to the robot coordinate system; specifying a position and orientation of the mobile robot in a world coordinate system and a first coordinate of an axis in the world coordinate system based on the robot coordinate system; determining a second transformation matrix from the robot coordinate system to the world coordinate system based on the first coordinate and the designated axis unit coordinate of the world coordinate system; determining second coordinates of the obstacle point cloud in the world coordinate system based on the obstacle point cloud in the camera coordinate system, the first transformation matrix, and the second transformation matrix.

[0013] In a second aspect, an embodiment of the present application further provides an apparatus for estimating a position and posture of a mobile robot, the apparatus comprising: a first acquisition unit, which is used to acquire an environmental 3D point cloud when the mobile robot performs a planar task, and determine a work plane point cloud from the environmental 3D point cloud based on a preset height threshold; a second acquisition unit used to acquire inertial measurement information of the mobile robot; and a position and orientation estimation unit used to determine the position and orientation of the mobile robot in a world coordinate system based on the inertial measurement information and the work plane point cloud.

[0014] In a third aspect, an embodiment of the present application further provides a mobile robot, comprising a processor and a memory adapted to store executable instructions for said processor, wherein said processor is configured to perform any of the methods described above by executing the executable instructions.

[0015] In a fourth aspect, the present invention further provides a computer-readable storage medium having a computer program stored thereon, the computer program being executed by the processor to perform any of the above methods. The method of the present invention can achieve at least the following beneficial effects:

[0016] In this application, when a mobile robot performs a planar task, the environmental 3D point cloud and inertial measurement data of the environment in which the mobile robot is located are acquired, the environmental 3D point cloud is filtered based on a preset height threshold to acquire a work plane point cloud on the work plane, and the work plane point cloud and inertial measurement information are then combined to determine the position and orientation of the mobile robot in the world coordinate system. In this application, when detecting the orientation of the mobile robot, the environmental 3D point cloud data is added, and the position and orientation of the mobile robot are detected by combining the work plane point cloud and inertial measurement information, which can more accurately estimate the robot's floor attitude angle, particularly suitable for scenarios where the work plane encounters a depression or threshold. The algorithm in this application is simple, requires little computing power, and is highly practical.

[0017] The accompanying drawings described herein are used to provide a further understanding of the present application, constitute a part of the present application, and the illustrative embodiments and the description thereof are used for interpreting the present application and are not to be construed as an undue limitation of the present application. [Brief explanation of the drawings]

[0018] [Figure 1] 1 is a schematic flowchart of a method for estimating the position and posture of a mobile robot according to an embodiment of the present application; [Figure 2-a] FIG. 1 is a schematic diagram illustrating a method for attaching a line laser module to a mobile robot according to an embodiment of the present application. [Figure 2-b] 1 is a schematic diagram of a method for attaching a line laser module to a mobile robot according to an embodiment of the present application; [Figure 3-a] Schematic diagram showing the world coordinate system defined by the line laser module installation method in Figure 2-a or Figure 2-b [Figure 3-b] Schematic diagram showing the robot coordinate system defined by the line laser module installation method in Figure 2-a or Figure 2-b [Figure 4-a] FIG. 1 illustrates a scenario in which a large obstacle exists on the working surface of a mobile robot according to an embodiment of the present application. [Figure 4-b] FIG. 1 illustrates a scenario in which a large obstacle is present on the working surface of a mobile robot according to another embodiment of the present application. [Figure 5] 1 is a schematic flowchart of a method for estimating the position and orientation of a mobile robot according to another embodiment of the present application; [Figure 6] 1 is a schematic structural diagram of an apparatus for estimating the position and posture of a mobile robot according to an embodiment of the present application; [Figure 7] Schematic structural diagram of a mobile robot according to an embodiment of the present application. DETAILED DESCRIPTION OF THE INVENTION

[0019] In order to make the objectives, technical solutions and advantages of the present application clearer, the technical solutions of the present application will be clearly and completely described below with reference to specific embodiments of the present application and the corresponding accompanying drawings. Obviously, the described embodiments are only some of the embodiments of the present application, and not all of the embodiments. Based on the embodiments of the present application, other embodiments obtained by those skilled in the art without any creative work shall all be included in the protection scope of the present application.

[0020] Below, in conjunction with the accompanying drawings, the technical solutions provided by each embodiment of the present application are described in detail.

[0021] In the prior art, the posture detection method for a robot performing a planar task is usually obtained by calculating the angle values ​​measured by an inertial measurement unit, but this method has a large error, making it difficult to obtain an accurate posture of the robot on the working plane.

[0022] Therefore, the present application provides a method for estimating the position and posture of a mobile robot, and FIG. 1 is a schematic flowchart of a method for estimating the position and posture of a mobile robot according to one embodiment of the present application. As can be seen from FIG. 1, the method for estimating the position and posture of a mobile robot of the present application includes at least steps S110 to S130.

[0023] Step S110: Obtain an environmental 3D point cloud when the mobile robot performs a planar task, and determine a work plane point cloud from the environmental 3D point cloud based on a preset height threshold.

[0024] The present application is primarily applicable to scenarios in which a mobile robot performs a task on a flat surface, and is particularly applicable to scenarios in which a robot moving on a flat surface performs a task, such as a sweeping robot.

[0025] When a mobile robot performs a planar task, it acquires a three-dimensional point cloud of the environment in which it is located. The acquisition of the three-dimensional point cloud of the environment is performed using various types of laser radar or camera, etc. Specifically, the laser radar or camera is attached to a specified position on the mobile robot in a specific position and posture, and when the mobile robot performs a planar task, it acquires a three-dimensional point cloud of the environment around the mobile robot.

[0026] In some embodiments of the present application, a line laser sensor is selected to acquire a 3D point cloud of an environment from the standpoints of economy and effectiveness. Specifically, a line laser module is mounted on a mobile robot. There are various methods for mounting the line laser module on a mobile robot. FIG. 2-a is a schematic diagram illustrating a method for mounting a line laser module on a mobile robot according to an embodiment of the present application, and FIG. 2-b is a schematic diagram illustrating a method for mounting a line laser module on a mobile robot according to an embodiment of the present application. As can be seen from FIGS. 2-a and 2-b, methods for mounting the line laser module on a mobile robot include, but are not limited to, a front-cross combination of two vertical line lasers (FIG. 2-a) and a cross combination of horizontal and vertical line lasers (FIG. 2-b). To perform plane fitting using the line laser point cloud, the line laser points falling on the work plane must not all be collinear. Any method for mounting the line laser module that satisfies this condition is sufficient. The methods illustrated in FIGS. 2-a and 2-b are merely illustrative and do not limit the present application in any way. It should be noted that plane fitting cannot be performed using the environmental 3D point cloud reconstructed by the single horizontal line laser projection method.

[0027] The following will be explained using the installation method of the line laser module shown in Figure 2-a and Figure 2-b as an example, and the calculation method for both is the same.

[0028] First, the coordinate system definitions of this application will be explained. Figure 3-a shows a schematic diagram of the world coordinate system defined by the installation method of the line laser module of Figure 2-a or Figure 2-b, and Figure 3-b shows a schematic diagram of the robot coordinate system defined by the installation method of the line laser module of Figure 2-a or Figure 2-b. Referring to Figure 3-b, when the mobile robot tilts, the roll angle and pitch angle of the attitude angles of the mobile robot are respectively shown in Figure 3-b.

[0029] When a line laser emitted from a line laser module is projected onto a work plane, it is possible to restore environmental 3D point cloud information of the work plane environment in a camera coordinate system. Specifically, when the mobile robot performs a planar task, the light beam emitted by the laser module is used to obtain an original point cloud of the laser line center in an image pixel coordinate system, a first constraint is established from the original point cloud of the laser line center based on mapping from the camera coordinate system to the image pixel coordinate system, a second constraint is established based on a line laser light plane equation in the camera coordinate system, and the first constraint and the second constraint are combined to determine the environmental 3D point cloud.

[0030] When the mobile robot performs a planar task, the line laser module scans the environment, obtains the original point cloud p=(u, v) of the laser line center in the image pixel coordinate system, and calculates the environmental point cloud coordinates P of the original point cloud p=(u, v) in the camera coordinate system. C =(x, y, z) can be recovered, and specifically, based on the mapping K (shown in Equation 1) from the camera coordinate system to the image pixel coordinate system, the first constraint shown in the following Equation 2 is established.

[0031]

number

[0032]

number

[0033] Here, f represents a mapping function from the camera coordinate system to the image pixel coordinate system, and u0 and u0 represent an arbitrary original point group.

[0034] Since points from the line laser light plane must fall on the line laser light plane, the second constraint shown in the following Equation 3 is established from the line laser light plane equation in the camera coordinate system.

[0035]

number

[0036] where A laser , B laser , C laser , and D laser denotes the coefficients of the line laser light plane equation.

[0037] Finally, by combining and solving the first constraint and the second constraint, an environment 3D point cloud can be obtained, which includes the 3D coordinates (X, Y, Z) of multiple points, as shown in the following Equation 4:

[0038]

number

[0039] The restored 3D point cloud of the working plane is filtered to remove obstacle points on the working plane using a loose height threshold. If there are no large or numerous obstacles on the working plane, i.e., if the number of points belonging to the working plane exceeds a certain threshold, the robot working plane attitude angle can be estimated and solved; otherwise, the robot working plane attitude angle estimation is abandoned. Specifically, based on the work plane point cloud determined from the 3D point cloud of the working plane and a preset height threshold, the height value of each point cloud of the working plane is compared with the preset height threshold. If the height value of a point cloud is smaller than the height threshold, it is determined that the point cloud belongs to the mobile robot working plane. If the height value of a point cloud is higher than the height threshold, the point cloud is filtered, i.e., it is determined that the point cloud does not belong to the working plane. Furthermore, the points belonging to the working plane are counted to obtain a calculation value. If the calculation value is equal to or greater than the preset threshold, it indicates that a sufficient number of points fall on the working plane of the mobile robot, and all points falling on the working plane are treated as the working plane point cloud.

[0040] If there are large obstacles or many obstacles on the work plane, the number of point clouds falling onto the work plane will not be small, making it meaningless to perform position and posture estimation of the mobile robot. If the calculated value is less than a preset threshold value, it indicates that the number of point clouds falling onto the mobile robot's work plane is relatively small, and the restored environmental 3D point cloud of the work plane is abandoned, i.e., the current position and posture estimation of the mobile robot is abandoned.

[0041] Referring to Figures 4-a and 4-b, Figure 4-a shows a scenario in which a large obstacle exists on the work plane of a mobile robot according to one embodiment of the present application, and Figure 4-b shows a scenario in which a large obstacle exists on the work plane of a mobile robot according to another embodiment of the present application. As shown in Figures 4-a and 4-b, when there is a large obstacle in front of the mobile robot, most of the point clouds exceed the height threshold of the work plane, and the number of point clouds that fall on the work plane is insufficient, making it impossible to accurately estimate the work plane, so the mobile robot's position and posture estimation is abandoned at this time.

[0042] Step S120: Obtain inertial measurement information of the mobile robot.

[0043] The inertial measurement information of the mobile robot can be acquired by the inertial measurement unit (IMU) installed on the mobile robot. The inertial measurement information is mainly the roll angle measurement in the world coordinate system of the mobile robot. meas and the measured pitch angle meas The roll angle measurement measured by the inertial measurement unit meas and the measured pitch angle meas Regarding the above, if there are any unevenness such as depressions on the work plane, a relatively large error occurs in the attitude angle measured by the IMU.

[0044] Step S130: The position and orientation of the mobile robot in the world coordinate system are determined based on the inertial measurement information and the work plane point group.

[0045] Finally, the inertial measurement information and the work plane point cloud are combined to determine the position and posture of the mobile robot in the world coordinate system. The overall concept of position and posture estimation is as follows: The error terms of the roll angle and pitch angle measurements measured by the IMU are used as one constraint, and the work plane point cloud surface alignment condition obtained by the line laser point cloud reconstruction module is used as another constraint. The roll angle and pitch angle estimates of the mobile robot coordinate system relative to the world coordinate system are obtained by solving the problem using a combination of least squares and optimization, and the obtained roll angle and pitch angle estimates are used as the position and posture of the mobile robot in the world coordinate system.

[0046] Specifically, a first error term of an attitude estimation model is constructed based on the inertial measurement information, the first error term being one of the constraints, and the first error term including a roll angle error term and a pitch angle error term; a second error term of an attitude estimation model of the mobile robot is constructed based on the planar condition of the work plane point cloud, the second error term being another constraint, and the second error term including the distance from the point cloud falling on the work plane to the work plane; a least squares optimization model is constructed as the attitude estimation model based on the first error term and the second error term; and the attitude estimation model is solved to obtain estimated roll angle and pitch angle values ​​of the mobile robot in the world coordinate system.

[0047] In this application, the roll angle estimate roll opt and the estimated pitch angle opt The least squares optimization model is established using the independent variables, and the roll angle estimation value, opt and the estimated pitch angle opt is the goal to be solved in this application. In some embodiments of this application, the least squares optimization model is shown in Equation 5.

[0048]

number

[0049] As can be seen from Equation 5, the least squares optimization model TIFF2026501651000007.tif817 contains two error terms, the first error term and the second error term, where the first error term f imu (x) is related to the roll angle error term and the pitch angle error term, and the second error term f plane、i (x) relates to the distance from the point cloud falling on the work plane to said work plane.

[0050] First error term f imu For (x), the roll angle measurement obtained by the IMU is meas and the measured pitch angle meas and the estimated roll angle, opt and the estimated pitch angle opt The specific first error term f is determined from the difference between imu (x) is shown in equation (6).

[0051]

number

[0052] Second error term f imu (x) is determined based on the distance from each point cloud falling on the work plane to the work plane. During calculation, a second error term of the posture estimation model is constructed based on the plane alignment condition of the work plane point cloud, specifically including: using the roll angle estimate value and the pitch angle estimate value as variables, determining a normal vector value of a work plane normal vector in the robot coordinate system; determining a work plane equation for the work plane in the robot coordinate system and a number of coefficient values ​​in the work plane equation based on the normal vector value; determining a distance value from each point cloud of the work plane point cloud to the work plane based on the work plane equation and the number of coefficient values; and finally determining the second error term based on the distance value from each of the point clouds to the work plane.

[0053] Specifically, the roll angle estimate, roll, is calculated by using the flatness condition of the obtained work plane points as one of the constraints. opt and the estimated pitch angle optFrom the normal vector of the work plane (ground surface, etc.) in the robot coordinate system, Calculate the normal vector value for TIFF2026501651000009.tif64 TIFF2026501651000010.tif64 is shown in Equation 7.

[0054]

number

[0055] The working plane equation of the working plane in the robot coordinate system is Equation 8.

[0056]

number

[0057] where A, B, C, and D are the coefficients of the work plane equation.

[0058] By solving equation 8, the coefficients A, B, and C shown in equation (9) of the working plane equation are obtained.

[0059]

number

[0060] Furthermore, when the robot is on the work plane, the origin p of the robot coordinate system r =[x r ,y r ,z r ] T =[0,0,0] T also falls onto the work plane, and based on this, the coefficient D shown in equation (10) of the work plane equation is obtained.

[0061]

number

[0062] Each point in the work plane point cloud For TIFF2026501651000015.tif960, the distance value f from the point cloud to the work plane plane,i can be determined and the distance value is used as the second error term, as shown in Equation 11.

[0063]

number

[0064] A least squares optimization model is constructed as a posture estimation model for a mobile robot, and then TIFF2026501651000017.tif817 Estimate the roll angle of a mobile robot in the world coordinate system by minimizing roll opt and the estimated pitch angle opt can be obtained.

[0065] As can be seen from the method shown in Figure 1, in this application, when a mobile robot performs a planar task, the environmental 3D point cloud and inertial measurement data of the environment in which the mobile robot is located are acquired, the environmental 3D point cloud is filtered based on a preset height threshold to obtain a work plane point cloud located on the work plane, and the work plane point cloud and inertial measurement information are then combined to determine the position and orientation of the mobile robot in the world coordinate system. In this application, when detecting the orientation of the mobile robot, the environmental 3D point cloud data is added, and the work plane point cloud and inertial measurement information are combined to detect the position and orientation of the mobile robot. This allows for more accurate estimation of the robot's floor attitude angle, especially in scenarios where a depression or threshold is encountered on the work plane. The algorithm used in this application is simple, requires little computing power, and is highly practical.

[0066] In some embodiments of the present application, in the above method, the environment three-dimensional point cloud includes an obstacle point cloud, and the method further includes a step of modifying the obstacle point cloud based on the determined position and posture of the mobile robot in a world coordinate system, and obtaining three-dimensional coordinates of the obstacle point cloud in the world coordinate system.

[0067] Furthermore, after solving the accurate attitude angle of the mobile robot in the world coordinate system, the three-dimensional coordinates of the obstacle point cloud in the world coordinate system can be obtained by correcting the obstacle point cloud detected by the previous line laser module.

[0068] Specifically, in some embodiments of the present application, modifying the obstacle point cloud based on the determined position and posture of the mobile robot in the world coordinate system includes determining a first transformation matrix from the camera coordinate system to the robot coordinate system and a first coordinate of a designated axis of the robot coordinate system in the world coordinate system based on the position and posture of the mobile robot in the world coordinate system; determining a second transformation matrix from the robot coordinate system to the world coordinate system based on the first coordinate and a unit coordinate of a designated axis of the world coordinate system; and determining a second coordinate of the obstacle point cloud in the world coordinate system based on the obstacle point cloud in the camera coordinate system, the first transformation matrix, and the second transformation matrix.

[0069] The overall concept of obstacle correction is to transform the obtained obstacle point cloud data in the camera coordinate system into the world coordinate system by a first transformation matrix and a second transformation matrix, where the first transformation matrix is ​​the transformation matrix from the camera coordinate system to the robot coordinate system, and the second transformation matrix is ​​the transformation matrix from the robot coordinate system to the world coordinate system.

[0070] First, a first transformation matrix T from the camera coordinate system to the robot coordinate system is calculated. RC Determine the first transformation matrix T RC can be shown in Equation 12.

[0071]

number

[0072] Typically, the first transformation matrix T RC is known or can be known by knowing the hardware configuration of the mobile robot, where R RC and tRC is the first transformation matrix T RC is the element in the corresponding position.

[0073] As mentioned above, the estimated roll angle of the mobile robot in the world coordinate system is opt and the estimated pitch angle opt Therefore, the second transformation matrix from the robot coordinate system to the world coordinate system is obtained. First, the first coordinate of the robot coordinate axis Z in the world coordinate system is TIFF2026501651000019.tif75 can be determined, as shown in Equation 13:

[0074]

number

[0075] The unit vector of the Z axis of the world coordinate system itself is TIFF2026501651000021.tif719, first coordinate TIFF2026501651000022.tif75 and the unit vector of the Z axis of the world coordinate system itself The second transformation matrix T from the robot coordinate system to the world coordinate system is obtained by multiplying and crossing the two vectors in TIFF2026501651000023.tif719. WR is obtained and shown in equation (14).

[0076]

number

[0077] where R WR and t WR is the first transformation matrix T RC It is an element in the corresponding position.

[0078] Finally, the obstacle point cloud coordinates P in the camera coordinate system C = (x, y, z), the first transformation matrix T from the camera coordinate system to the robot coordinate system RCand the second transformation matrix T from the robot coordinate system to the world coordinate system WR In addition, the coordinates P of the obstacle point cloud in the world coordinate system w The specific algorithm is shown in Equation 15.

[0079]

number

[0080] After the correction, the obtained obstacle point cloud data will be more accurate.

[0081] FIG. 5 shows a schematic flowchart of a method for estimating the position and posture of a mobile robot according to another embodiment of the present application, and as can be seen from FIG. 5, this embodiment includes: When the mobile robot performs planar motion, the original point cloud collected by the line laser module and the inertial measurement information collected by the inertial measurement unit are simultaneously acquired.

[0082] Based on the inertial measurement information, roll and pitch angle measurements of the mobile robot in a world coordinate system are determined.

[0083] A three-dimensional point map of the environment is reconstructed based on the original point cloud.

[0084] For any point cloud in the environmental 3D point map, determine whether its height value is less than a preset height threshold, and if so, attribute the point cloud to the working plane of the mobile robot; if not, filter the point cloud, i.e., delete the point cloud, to obtain a planar working point cloud.

[0085] Determine whether the number of points in the planar working point cloud is greater than or equal to a preset number threshold; if not, abandon the current attitude estimation task; if yes, construct a first error term based on inertial measurement information, and construct a second error term based on the planar working point cloud.

[0086] An attitude estimation model is constructed based on the first error term and the second error term, and the attitude estimation model is solved to obtain estimated roll and pitch angles of the mobile robot in the world coordinate system.

[0087] The coordinates of the obstacle point cloud in the planar work point cloud in the camera coordinate system are obtained, and the obstacle point cloud is corrected and transformed based on the roll angle estimate and the pitch angle estimate to obtain the coordinate values ​​of the obstacle point cloud in the world coordinate system.

[0088] FIG. 6 shows a schematic structural diagram of an apparatus for estimating the position and posture of a mobile robot according to an embodiment of the present application. As can be seen from FIG. 6, the apparatus 600 for estimating the position and posture of a mobile robot includes: a first acquisition unit 610, which is used to acquire an environment 3D point cloud when the mobile robot performs a planar task, and determine a working plane point cloud from the environment 3D point cloud based on a preset height threshold; a second acquisition unit 620 used to acquire inertial measurement information of the mobile robot; and a position and orientation estimation unit 630 used to determine the position and orientation of the mobile robot in a world coordinate system based on the inertial measurement information and the work plane point cloud.

[0089] In some embodiments of the present application, in the above-mentioned device, the mobile robot is equipped with a line laser module, the line laser module emits at least two line lasers, and the points of the at least two line lasers projected onto the work plane are not located on the same line, and a first acquisition unit 610 is used to acquire an original point cloud of the laser line center in the image pixel coordinate system using the light beam emitted by the laser module when the mobile robot performs a planar task, and a first constraint is established from the original point cloud of the laser line center based on mapping from the camera coordinate system to the image pixel coordinate system, and a second constraint is established based on a line laser light plane equation in the camera coordinate system, and the first constraint and the second constraint are combined to determine the environmental three-dimensional point cloud.

[0090] In some embodiments of the present application, in the above-mentioned device, the first acquisition unit 610 determines whether the height value of each point cloud in the environment 3D point cloud is smaller than the height threshold, and if so, assigns the corresponding point cloud to the working plane of the mobile robot; determines whether the number of point clouds assigned to the working plane is greater than or equal to a predetermined number threshold, and if so, uses the point cloud belonging to the working plane of the mobile robot as the working plane point cloud; if not, uses the environment 3D point cloud for discarding.

[0091] In some embodiments of the present application, in the above-mentioned device, the position and attitude estimation unit 630 constructs a first error term of an attitude estimation model based on the inertial measurement information, where the first error term is determined based on a roll angle error term and a pitch angle error term; constructs a second error term of the attitude estimation model based on the flush condition of the work plane point cloud, where the second error term is determined based on the distance from each point cloud falling on the work plane to the work plane; constructs a least squares optimization model as the attitude estimation model based on the first error term and the second error term; and solves the attitude estimation model to obtain roll angle estimation values ​​and pitch angle estimation values ​​of the mobile robot in the world coordinate system.

[0092] In some embodiments of the present application, in the above-mentioned device, the position and attitude estimation unit 630 determines a normal vector value of a work plane normal vector in the robot coordinate system using the roll angle estimated value and the pitch angle estimated value as variables, determines a work plane equation for the work plane in the robot coordinate system and multiple coefficient values ​​in the work plane equation based on the normal vector value, determines a distance value from each point group of the work plane point group to the work plane based on the work plane equation and the multiple coefficient values, and is used to determine the second error term based on the distance value from each of the points to the work plane.

[0093] In some embodiments of the present application, in the above device, the environment 3D point cloud includes an obstacle point cloud, and the position and attitude estimation unit 630 is further used to modify the obstacle point cloud based on the determined position and attitude of the mobile robot in the world coordinate system, and obtain the 3D coordinates of the obstacle point cloud in the world coordinate system.

[0094] In some embodiments of the present application, in the above device, the position and orientation estimation unit 630 is used to determine a first transformation matrix from the camera coordinate system to the robot coordinate system and a first coordinate of a specified axis of the robot coordinate system in the world coordinate system based on the position and orientation of the mobile robot in the world coordinate system, determine a second transformation matrix from the robot coordinate system to the world coordinate system based on the first coordinate and a unit coordinate of a specified axis of the world coordinate system, and determine a second coordinate of the obstacle point cloud in the world coordinate system based on an obstacle point cloud in the camera coordinate system, the first transformation matrix, and the second transformation matrix.

[0095] It should be noted that the above-mentioned device for estimating the position and posture of a mobile robot can realize a method for estimating the position and posture of the mobile robot one by one, and the details will not be repeated here.

[0096] 7 is a schematic structural diagram of a mobile robot according to an embodiment of the present application. Referring to FIG. 7, at the hardware level, the mobile robot includes a processor, and optionally includes an internal bus, a network interface, and a memory. Here, the memory may include an internal memory such as a high-speed random-access memory (RAM), or a non-volatile memory such as at least one disk memory. Of course, the mobile robot may further include other hardware required for service.

[0097] The processor, the network interface, and the memory may be connected to each other via an internal bus, which may be an ISA (Industry Standard Architecture) bus, a PCI (Peripheral Component Interconnect) bus, or an EISA (Extended Industry Standard Architecture) bus, etc. The bus may be classified into an address bus, a data bus, a control bus, etc. For simplicity of illustration, only one bidirectional arrow is shown in FIG. 7, but this does not mean that there is only one bus or one type of bus.

[0098] The memory is used to store programs. Specifically, the programs may include program codes, which may include computer operation instructions. The memory may include internal memory and non-volatile memory, and provides instructions and data to the processor.

[0099] The processor loads the corresponding computer program from the non-volatile memory into the internal memory and executes it to form a device for estimating the position and orientation of a mobile robot at a logical level. The processor executes the program stored in the memory and is used to specifically perform the method.

[0100] The method performed by the apparatus for estimating the position and posture of a mobile robot disclosed in the embodiment shown in FIG. 6 of the present application can be applied to or implemented by a processor. The processor may be an integrated circuit chip with signal processing capabilities. During implementation, each step of the method may be achieved by an integrated logic circuit in hardware or instructions in software form in the processor. The processor may be a general-purpose processor, including a central processing unit (CPU), a network processor (NP), a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field-programmable gate array (FPGA) or other programmable logic device, a discrete gate or transistor logic device, or a discrete hardware assembly. Each method, step, and logical block diagram disclosed in the embodiment of the present application can be implemented or executed. The general-purpose processor may be a microprocessor, or any conventional processor. The steps of the method disclosed in the embodiments of the present application may be performed by a hardware decoding processor, or may be performed by a combination of hardware and software modules in the decoding processor. The software modules may be arranged in a storage medium well known in the art, such as a random access memory, a flash memory, a read-only memory, a programmable read-only memory or an electrically erasable programmable memory, a register, etc. The storage medium is arranged in the memory, and the processor reads the arrangement information in the memory and cooperates with the hardware to complete the steps of the method.

[0101] The mobile robot may further execute the method performed by the apparatus for estimating the position and posture of a mobile robot in FIG. 6, and realize the functions of the embodiment of the apparatus for estimating the position and posture of a mobile robot shown in FIG. 6, which will not be repeated in the embodiments of this application.

[0102] An embodiment of the present application further provides a computer-readable storage medium, the computer-readable storage medium storing one or more programs, the one or more programs including instructions, which, when executed by a mobile robot including a plurality of application programs, enable the mobile robot to perform, and specifically execute, the method performed by the apparatus for estimating the position and posture of a mobile robot in the embodiment shown in FIG. 6 .

[0103] It should be understood by those skilled in the art that embodiments of the present application may be provided as a method, system, or computer program product. Thus, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk memory, CD-ROM, optical memory, etc.) containing computer-usable program code.

[0104] The present application will be described with reference to flowcharts and / or block diagrams of methods, apparatuses (systems), and computer program products according to embodiments of the present application. It should be understood that each flow and / or block in the flowcharts and / or block diagrams, and combinations of flows and / or blocks in the flowcharts and / or block diagrams, may be implemented by computer program instructions. These computer program instructions are provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device, and the instructions, when executed by the processor of the computer or other programmable data processing device, produce a device for performing the function(s) specified in one or more of the flow(s) in the flowcharts and / or one or more of the block(s) in the block diagrams.

[0105] These computer program instructions may be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to operate in a particular manner, and the instructions stored in the computer-readable memory produce an article of manufacture that includes an instruction apparatus, which implements the functions specified in one or more flows of the flowcharts and / or one or more blocks of the block diagrams.

[0106] These computer program instructions may be loaded into a computer or other programmable data processing device, and a series of operation steps may be executed on the computer or other programmable device to realize computer processing, and the instructions executed on the computer or other programmable device may also realize functional steps specified in one or more flows of the flowcharts and / or one or more blocks of the block diagrams.

[0107] In a typical configuration, a computing device includes one or more processors (CPUs), input / output interfaces, network interfaces, and memory.

[0108] The memory may include forms of computer-readable media such as volatile memory, random access memory (RAM) and / or non-volatile memory such as read-only memory (ROM) or flash memory (flash RAM). Memory is one example of a computer-readable medium.

[0109] Computer-readable media include nonvolatile and volatile media, removable and non-removable media, and can store configuration information using any method or technology. Configuration information may be computer-readable instructions, data structures, program modules, or other data. Examples of computer storage media include phase-change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory, or other memory technologies, read-only optical disks (CD-ROMs), digital versatile disks (DVDs), or other optical storage devices, magnetic cartridge tape, magnetic tape disk storage devices, or other magnetic storage devices, or any other non-transferable media used to store configuration information accessible by a computing device. As defined herein, computer-readable media does not include transitory media, such as modulated data signals and carriers.

[0110] Additionally, "comprises," "has," or any other variation thereof, covers a non-exclusive inclusion, such that a process, method, article, or apparatus comprising a set of elements includes not only those elements but also other elements not expressly listed or inherent in the process, method, article, or apparatus. Unless more restrictive, an element defined by the phrase "comprises a..." does not exclude the presence of other identical elements in the process, method, article, or apparatus of that element.

[0111] Those skilled in the art will appreciate that the embodiments of the present application may be provided as a method, a system, or a computer program product. Thus, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware. Furthermore, the present application may take the form of a computer program product that may be embodied on one or more computer-usable storage media (including, but not limited to, disk memory, CD-ROM, optical memory, etc.) containing computer-usable program code.

[0112] The above is merely an example of the present application and is not intended to limit the present application. Those skilled in the art can make various modifications and variations to the present application. Any modifications, equivalent substitutions, improvements, etc. made without departing from the spirit and principles of the present application shall be included within the scope of the claims of the present application.

Claims

1. acquiring an environmental 3D point cloud when the mobile robot is performing a planar task, and determining a work plane point cloud from the environmental 3D point cloud based on a preset height threshold; acquiring inertial measurement information of the mobile robot; determining a position and orientation of the mobile robot in a world coordinate system based on the inertial measurement information and the work plane point cloud; A method for estimating the position and pose of a mobile robot.

2. a line laser module mounted on the mobile robot, the line laser module emitting at least two line lasers, and the points of the at least two line lasers projected onto the work plane are not located on the same line; The step of acquiring an environmental three-dimensional point cloud when the mobile robot executes a planar task includes: By using the light beam emitted by the laser module, an original point cloud of the laser line center in the image pixel coordinate system is obtained when the mobile robot performs a planar task; establishing a first constraint from the original point cloud of laser line centers based on a mapping from the camera coordinate system to an image pixel coordinate system; establishing a second constraint based on a line laser light plane equation in the camera coordinate system; and combining the first constraint and the second constraint to determine the environmental 3D point cloud.

3. determining a work plane point cloud from the environment 3D point cloud based on the preset height threshold, determining whether the height value of each point in the environmental 3D point cloud is less than the height threshold, and if so, attributing the corresponding point cloud to a working plane of the mobile robot; 3. A method for estimating the position and posture of a mobile robot as described in claim 2, comprising a step of determining whether the number of points to be attributed to the work plane is equal to or greater than a predetermined threshold number, and if so, using the points belonging to the work plane of the mobile robot as the work plane point cloud.

4. The step of determining the position and posture of the mobile robot in a world coordinate system based on the inertial measurement information and the work plane point cloud includes: constructing a first error term of an attitude estimation model based on the inertial measurement information, the first error term being determined based on a roll angle error term and a pitch angle error term; constructing a second error term of the pose estimation model based on a plane alignment condition of the work plane point cloud, the second error term being determined based on a distance from each point cloud falling on the work plane to the work plane; constructing a least squares optimization model as the pose estimation model based on the first error term and the second error term; and solving the pose estimation model to obtain roll and pitch angle estimates of the mobile robot in the world coordinate system.

5. constructing a second error term of the pose estimation model based on a flush condition of the work plane point cloud, determining a normal vector value of a work plane normal vector in a robot coordinate system using the roll angle estimated value and the pitch angle estimated value as variables; determining a work plane equation of the work plane in the robot coordinate system and a plurality of coefficient values ​​in the work plane equation based on the normal vector value; determining a distance value from each point of the work plane point cloud to the work plane based on the work plane equation and the plurality of coefficient values; and determining the second error term based on a distance value from each of the point clouds to the work plane.

6. The environmental three-dimensional point cloud includes an obstacle point cloud, and the method for estimating the position and posture of the mobile robot includes:

2. The method for estimating the position and posture of a mobile robot according to claim 1, further comprising the step of correcting the obstacle point cloud based on the determined position and posture of the mobile robot in the world coordinate system, and obtaining three-dimensional coordinates of the obstacle point cloud in the world coordinate system.

7. The step of correcting the obstacle point cloud based on the determined position and orientation of the mobile robot in the world coordinate system includes: determining a first transformation matrix from the camera coordinate system to the robot coordinate system; determining a position and orientation of the mobile robot in a world coordinate system and a first coordinate of a designated axis in the world coordinate system based on the robot coordinate system; determining a second transformation matrix from the robot coordinate system to the world coordinate system based on the first coordinate and the designated axis unit coordinate of the world coordinate system; and determining second coordinates of the obstacle point cloud in the world coordinate system based on the obstacle point cloud in the camera coordinate system, the first transformation matrix, and the second transformation matrix.

8. a first acquisition unit configured to acquire an environment 3D point cloud when the mobile robot performs a planar task, and determine a work plane point cloud from the environment 3D point cloud based on a preset height threshold; a second acquisition unit configured to acquire inertial measurement information of the mobile robot; and a position and orientation estimation unit configured to determine a position and orientation of the mobile robot in a world coordinate system based on the inertial measurement information and the work plane point cloud.

9. a processor; a memory configured to store executable instructions for the processor, A mobile robot, wherein the processor is configured to perform the method of any one of claims 1 to 7 by executing the executable instructions.

10. A computer-readable storage medium on which a computer program is stored, A computer-readable storage medium, the computer program being executed by a processor to perform the method of any one of claims 1 to 7.

Images