Positioning and map construction method, device, robot and computer storage medium

Abstract

The present invention relates to a positioning and map construction method, device, robot and computer storage medium, the method comprising: extracting straight line features from acquired point cloud data at time t to obtain a plurality of corresponding line segments; estimating the relative posture of the robot at time t compared to time t-1; performing line segment matching on a plurality of line segments corresponding to time t and a plurality of line segments obtained by extracting straight line features at time t-1 according to the relative posture to obtain the straight lines to which each line segment belongs; jointly optimizing the posture of the robot at each time and the two-line parameterized representation of all straight lines to obtain the posture of the robot at each time and the straight line parameters of all straight lines. Since the two-line parameterized representation is used when representing the straight line, the amount of calculation can be reduced as much as possible, thereby improving the calculation efficiency and shortening the calculation time.

Description

Technical Field

[0001] The present application belongs to the field of autonomous navigation, and specifically relates to a positioning and map building method, device, robot and computer storage medium. Background Art

[0002] During operation, a mobile robot needs to continuously acquire data in order to locate itself in real time. In addition, it also needs to construct a map of the surrounding environment based on the acquired data.

[0003] Generally speaking, mobile robots use SLAM (Simultaneous Localization And Mapping) algorithms to obtain and determine data for real-time positioning and map construction. Therefore, the quality of the SLAM algorithm directly determines the positioning accuracy and stability of the mobile robot during movement. Therefore, it is necessary to propose a stable, efficient and high-precision SLAM algorithm.

[0004] In the prior art, a SLAM algorithm is proposed that uses graph optimization technology to optimize straight line features and robot postures in real time. However, in this algorithm, six parameters corresponding to the straight line features are used to represent the straight line, so that the description of the straight line is not a minimum parameterized representation. Therefore, the subsequent calculation amount is large and the calculation efficiency is low, which further affects the efficiency of real-time positioning and map construction. Summary of the invention

[0005] In view of this, the purpose of the present application is to provide a positioning and map construction method, device, robot and computer storage medium, which can reduce the amount of calculation in the positioning and map construction process, thereby improving the efficiency of positioning and map construction, so as to improve the efficiency of subsequent real-time positioning and map construction.

[0006] The embodiment of the present application is implemented as follows:

[0007] In the first aspect, the embodiment of the present application provides a positioning and map construction method, the method comprising: extracting straight line features from the acquired point cloud data at time t to obtain a corresponding plurality of line segments; estimating the relative posture of the robot at time t compared to time t-1; performing line segment matching on the plurality of line segments corresponding to the time t and the plurality of line segments obtained by extracting straight line features at time t-1 according to the relative posture to obtain the straight lines to which each line segment belongs; jointly optimizing the posture of the robot at each time and the two-line parameterized representation of all straight lines to obtain the posture of the robot at each time and the straight line parameters of all straight lines. Since the two-line parameterized representation is used when representing the straight line, the amount of calculation for determining the data used for subsequent positioning and map construction can be reduced as much as possible, thereby improving the efficiency of map positioning and map construction, so as to improve the efficiency of subsequent real-time positioning and map construction.

[0008] In combination with the first aspect embodiment, in a possible implementation manner, estimating the relative posture of the robot at the time t compared to the time t-1 includes: setting the posture of the robot at the time t to Set the posture of the robot at the time t-1 to Set the rotation matrix and translation vector of the robot coordinate system relative to the world coordinate system at time t to be According to the pre-saved robot wheel encoder posture estimation algorithm, the relative posture of the time t compared to the time t-1 is calculated. in, The formula is satisfied: (x k ,y k ) represents the coordinates of the robot relative to the world coordinate system w at time t=k, represents the orientation of the robot relative to the world coordinate system w at time t=k, is the rotation matrix, is the translation vector, v is the linear velocity of the robot, ω is the angular velocity of the robot, and Δt is the time interval between the time t and the time t-1.

[0009] In combination with the embodiment of the first aspect, in a possible implementation manner, the line segment matching of the multiple line segments corresponding to the time t and the multiple line segments obtained by extracting straight line features at the time t-1 includes: determining that the robot coordinate system formed by the robot at the time t-1 is a reference coordinate system; according to the relative posture, converting the coordinates of the two endpoints of the line segment corresponding to the time t to the reference coordinate system to obtain the expression of the line segment corresponding to the time t in the reference coordinate system; matching the expression of the line segment corresponding to the time t in the reference coordinate system with the expressions of the multiple line segments corresponding to the time t-1.

[0010] In combination with the embodiment of the first aspect, in a possible implementation manner, before matching the expression of the line segment corresponding to the time t in the reference coordinate system with the expressions of the multiple line segments corresponding to the time t-1, the method further includes: for each line segment corresponding to the time t, according to a predetermined attribute feature and a pre-stored attribute feature threshold, filtering difference line segments from all line segments corresponding to the time t-1, and determining the remaining line segments corresponding to the time t-1 as candidate line segments corresponding to the line segment;

[0011] Correspondingly, matching the expression of the line segment corresponding to the time t in the reference coordinate system with the expressions of the multiple line segments corresponding to the time t-1 includes: for each line segment corresponding to the time t, matching the expression of the line segment in the reference coordinate system with the expression of the candidate line segment corresponding to the line segment. By reducing the number of line segments to be matched during line segment matching, the amount of calculation is reduced and the calculation efficiency is improved.

[0012] In combination with the embodiment of the first aspect, in a possible implementation, before matching the expression of the line segment in the reference coordinate system with the expression of the candidate line segment corresponding to the line segment, the method also includes: determining that the number of candidate line segments corresponding to the line segment is not zero; otherwise, the method also includes: determining that the line segment belongs to a new straight line.

[0013] In combination with the first aspect embodiment, in a possible implementation manner, matching the expression of the line segment corresponding to the time t in the reference coordinate system with the expressions of the multiple line segments corresponding to the time t-1 includes: for each line segment corresponding to the time t, forming a line segment pair with each single line segment corresponding to the time t-1, and according to a pre-constructed loss function Calculate the loss value of each line segment pair corresponding to the line segment; determine the line segment pair corresponding to the minimum loss value as belonging to the same straight line; where d1 and d2 are the distances from the two endpoints of the line segment to the other line segment in the line segment pair, d0 is the length of the overlapping area of ​​the two line segments in the line segment pair, is the length of the line segment, α is a pre-set control factor parameter, and d1, d2, d0, They are all determined by the expressions of the two segments in the pair.

[0014] In combination with the embodiment of the first aspect, in a possible implementation manner, the posture of the robot at each moment and the two-line parameterized representation of all straight lines are jointly optimized to obtain the posture of the robot at each moment and the straight line parameters of all straight lines, including: setting the expression of the posture of the robot at each moment; setting the expression of all straight lines obtained after the line segment matching; constructing a residual vector through the expression of the posture and the expression of all straight lines; obtaining the optimal estimation formula between the posture at each moment and all straight lines by minimizing the sum of squares of the residual vector; and iteratively solving the optimal estimation formula to obtain the posture at each moment and the straight line parameters of all straight lines.

[0015] In the second aspect, the embodiment of the present application provides a positioning and map construction device, which includes: an extraction module, an estimation module, a matching module and an optimization module. The extraction module is used to extract straight line features from the acquired point cloud data at time t to obtain a corresponding plurality of line segments; the estimation module is used to estimate the relative posture of the robot at time t compared to time t-1; the matching module is used to match the plurality of line segments corresponding to the time t and the plurality of line segments obtained by extracting straight line features at time t-1 according to the relative posture to obtain the straight lines to which each line segment belongs; the optimization module is used to jointly optimize the posture of the robot at each time and the two-line parameterized representation of all straight lines to obtain the posture of the robot at each time and the straight line parameters of all straight lines.

[0016] In combination with the second aspect embodiment, in a possible implementation manner, the estimation module is used to set the posture of the robot at the time t to Set the posture of the robot at the time t-1 to Set the rotation matrix and translation vector of the robot coordinate system relative to the world coordinate system at time t to be According to the pre-saved robot wheel encoder posture estimation algorithm, the relative posture of the time t compared to the time t-1 is calculated. in, and The formula is satisfied: (x k ,y k ) represents the coordinates of the robot relative to the world coordinate system w at time t=k, represents the orientation of the robot relative to the world coordinate system w at time t=k, is the rotation matrix, is the translation vector, v is the linear velocity of the robot, ω is the angular velocity of the robot, and Δt is the time interval between the time t and the time t-1.

[0017] In combination with the embodiment of the second aspect, in a possible implementation, the matching module is used to determine that the robot coordinate system formed by the robot at the time t-1 is a reference coordinate system; according to the relative posture, the coordinates of the two endpoints of the line segment corresponding to the time t are converted into the reference coordinate system to obtain the expression of the line segment corresponding to the time t in the reference coordinate system; the expression of the line segment corresponding to the time t in the reference coordinate system and the expressions of multiple line segments corresponding to the time t-1 are matched.

[0018] In combination with the second aspect embodiment, in a possible implementation manner, the device further includes a filtering module, which is used to filter the difference line segments from all line segments corresponding to the time t-1 according to a predetermined attribute feature and a pre-stored attribute feature threshold for each line segment corresponding to the time t, and determine the remaining line segments corresponding to the time t-1 as candidate line segments corresponding to the line segment;

[0019] Correspondingly, the matching module is used to match, for each line segment corresponding to the time t, the expression of the line segment in the reference coordinate system and the expression of the candidate line segment corresponding to the line segment.

[0020] In combination with the second aspect embodiment, in a possible implementation, the device also includes a determination module for determining that the number of candidate line segments corresponding to the line segment is not zero; when the determination module determines that, the device also includes an adding module for determining that the line segment belongs to a new straight line.

[0021] In combination with the second aspect embodiment, in a possible implementation, the matching module is used to, for each line segment corresponding to the time t, form a line segment pair with each single line segment corresponding to the time t-1, and according to a pre-constructed loss function Calculate the loss value of each line segment pair corresponding to the line segment; determine the line segment pair corresponding to the minimum loss value as belonging to the same straight line; where d1 and d2 are the distances from the two endpoints of the line segment to the other line segment in the line segment pair, d0 is the length of the overlapping area of ​​the two line segments in the line segment pair, is the length of the line segment, α is a pre-set control factor parameter, and d1, d2, d0, They are all determined by the expressions of the two segments in the pair.

[0022] In combination with the second aspect embodiment, in a possible implementation, the optimization module is used to set the expression of the posture of the robot at the various moments; set the expression of all straight lines obtained after the line segment matching; construct a residual vector through the expression of the posture and the expression of all straight lines; obtain the optimal estimation formula between the posture at the various moments and all straight lines by minimizing the sum of squares of the residual vectors; iteratively solve the optimal estimation formula to obtain the posture at the various moments and the straight line parameters of all straight lines.

[0023] In a third aspect, an embodiment of the present application further provides a robot, comprising: a memory and a processor, wherein the memory and the processor are connected; the memory is used to store programs; the processor calls the program stored in the memory to execute the method provided in the above-mentioned first aspect embodiment and / or any possible implementation method in combination with the first aspect embodiment.

[0024] In a fourth aspect, an embodiment of the present application further provides a non-volatile computer-readable storage medium (hereinafter referred to as computer storage medium), on which a computer program is stored. When the computer program is run by a computer, it executes the method provided by the above-mentioned first aspect embodiment and / or any possible implementation method in combination with the first aspect embodiment.

[0025] Other features and advantages of the present application will be described in the following description, and partly become apparent from the description, or understood by implementing the embodiments of the present application. The purpose and other advantages of the present application can be realized and obtained by the structures specifically pointed out in the written description and the drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can also be obtained based on these drawings without creative work. As shown in the drawings, the above and other purposes, features and advantages of the present application will be clearer. The same reference numerals indicate the same parts in all the drawings. The drawings are not deliberately scaled to the actual size, and the focus is on showing the main purpose of the present application.

[0027] Figure 1 A schematic diagram of the structure of a robot provided in an embodiment of the present application is shown.

[0028] Figure 2 A flow chart of a positioning and map building method provided in an embodiment of the present application is shown.

[0029] Figure 3 A schematic diagram of extracting straight line features based on a point cloud provided in an embodiment of the present application is shown.

[0030] Figure 4 A schematic diagram showing line segment matching provided in an embodiment of the present application is shown.

[0031] Figure 5 A structural block diagram of a positioning and map building device provided in an embodiment of the present application is shown.

[0032] Icons: 100 - robot; 110 - processor; 120 - memory; 130 - laser emission component; 400 - positioning and map building device; 410 - extraction module; 420 - estimation module; 430 - matching module; 440 - optimization module. DETAILED DESCRIPTION

[0033] The technical solutions in the embodiments of the present application will be described below in conjunction with the drawings in the embodiments of the present application.

[0034] It should be noted that similar numbers and letters represent similar items in the following figures, so once an item is defined in one figure, it does not need to be further defined and explained in the subsequent figures. At the same time, in the description of this application, relational terms such as "first", "second", etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply that there is any such actual relationship or order between these entities or operations. Moreover, the term "comprise" or any other variant thereof is intended to cover non-exclusive inclusion, so that the process, method, article or equipment including a series of elements includes not only those elements, but also includes other elements that are not clearly listed, or also includes elements inherent to such process, method, article or equipment. In the absence of more restrictions, the elements defined by the statement "comprise one..." do not exclude the existence of other identical elements in the process, method, article or equipment including the elements.

[0035] Furthermore, the term "and / or" in this application is merely a term used to describe the association relationship between associated objects, indicating that three relationships may exist. For example, A and / or B may represent three situations: A exists alone, A and B exist at the same time, and B exists alone.

[0036] In addition, the defects existing in the real-time positioning and map construction solutions in the prior art (large amount of calculation and low calculation efficiency) are the results obtained by the applicant after practice and careful research. Therefore, the process of discovering the above defects and the solutions proposed in the embodiments of this application for the above defects below should be recognized as the applicant's contribution to this application.

[0037] In order to solve the above problems, the embodiments of the present application provide a positioning and map construction method, device, robot and computer storage medium, which can reduce the amount of calculation in the calculation process and thus improve the calculation efficiency.

[0038] The technology can be implemented by using corresponding software, hardware or a combination of software and hardware. The following is a detailed introduction to the embodiments of the present application.

[0039] First, refer to Figure 1 To describe the robot 100 used to implement the positioning and map building method and device of the embodiment of the present application.

[0040] The robot 100 may include: a processor 110 , a memory 120 , and a laser emitting component 130 .

[0041] It should be noted that Figure 1 The components and structures of the robot 100 shown are merely exemplary and non-limiting. The robot 100 may also have other components and structures as required.

[0042] The processor 110, the memory 120, the laser emitting assembly 130, and other components that may appear in the robot 100 are directly or indirectly electrically connected to each other to achieve data transmission or interaction. For example, the processor 110, the memory 120, the laser emitting assembly 130, and other components that may appear may be electrically connected to each other via one or more communication buses or signal lines.

[0043] After being powered on, the laser emitting assembly 130 can emit laser light at a fixed frequency and receive reflected laser light after being reflected by external objects, thereby generating point cloud data corresponding to each laser emission moment.

[0044] In some embodiments, the laser emitting component 130 may be a multi-threaded laser component for emitting multi-threaded lasers; in other embodiments, the laser emitting component 130 may be a single-threaded laser component for emitting single-threaded lasers.

[0045] Optionally, since the data complexity of the point cloud data obtained based on a single-threaded laser is lower than the data complexity of the point cloud data obtained based on a multi-threaded laser, in order to reduce the amount of calculation and reduce the data complexity, a single-threaded laser can be directly used as the laser emitting component 130, so that the subsequently obtained point cloud data is composed of two-dimensional discrete points.

[0046] The memory 120 is used to store programs, for example, a program corresponding to the positioning and map building method described below or a positioning and map building device described below. Optionally, when the positioning and map building device is stored in the memory 120, the positioning and map building device includes at least one software function module that can be stored in the memory 120 in the form of software or firmware.

[0047] Optionally, the software function modules included in the positioning and mapping device may also be embedded in the operating system (OS) of the robot 100 .

[0048] The processor 110 is used to execute executable modules stored in the memory 120, such as software function modules or computer programs included in the positioning and mapping device. When the processor 110 receives the execution instruction, it can execute the computer program, for example, to perform: extracting straight line features from the acquired point cloud data at time t to obtain corresponding multiple line segments; estimating the relative posture of the robot at time t compared to time t-1; performing line segment matching on the multiple line segments corresponding to the time t and the multiple line segments obtained by extracting straight line features at time t-1 according to the relative posture, to obtain the straight lines to which each line segment belongs; jointly optimizing the posture of the robot at each time and the two-line parameterized representation of all straight lines to obtain the posture of the robot at each time and the straight line parameters of all straight lines.

[0049] Of course, the method disclosed in any embodiment of the present application can be applied to the processor 110 or implemented by the processor 110.

[0050] The following is an introduction to the positioning and map construction method provided by this application.

[0051] See also Figure 2 , the present application embodiment provides a positioning and map construction method applied to the robot 100. Figure 2 , taking the point cloud data composed of two-dimensional discrete points as an example, the embodiment of the present application is introduced.

[0052] Step S110: extracting straight line features from the acquired point cloud data at time t to obtain a plurality of corresponding line segments.

[0053] After the robot 100 is powered on and started, it can emit laser light through the laser emitting component 130 to obtain point cloud data that reflects the current environmental conditions.

[0054] At each subsequent moment, the robot 100 can successively obtain the point cloud data of the current moment through the laser emitting component 130.

[0055] The point cloud data includes several two-dimensional discrete points.

[0056] The two-dimensional discrete points in multiple planes generated by a single-line laser can be expressed by the formula In which, x represents the horizontal coordinate of the two-dimensional discrete point in the robot coordinate system o at the current moment, and y represents the vertical coordinate of the two-dimensional discrete point in the robot coordinate system o at the current moment. The expression P representing a two-dimensional discrete point includes two real variables, namely x and y.

[0057] Generally speaking, a first identification point and a second identification point are pre-set on the robot 100 .

[0058] At any time, the robot 100 can determine the robot coordinate system corresponding to the current moment by obtaining the position of the first identification point at the current moment and the position of the second identification point at the current moment.

[0059] For example, at any moment, the robot 100 uses the position of the first identification point as the coordinate origin, the straight line connecting the position of the second identification point and the position of the first identification point as the y-axis, and the straight line perpendicular to the y-axis as the x-axis, thereby determining the robot coordinate system corresponding to the current moment.

[0060] It is worth pointing out that, since the robot 100 is constantly moving, the robot coordinate system corresponding to each moment may be different. In addition, the robot coordinate system determined at the initial moment of starting the robot 100 is generally taken as the global world coordinate system w.

[0061] Among them, the first identification point can be the position of the geometric center point of the robot 100, or it can be a point corresponding to other positions of the robot 100, and the embodiment of the present application does not make specific limitations.

[0062] After obtaining the point cloud data, Figure 3 As shown, the robot 100 can extract a number of line segments L from the point cloud data by using a pre-stored known straight line feature extraction algorithm (such as a least squares algorithm). i ={n i ,s i , e i}, i=1…n.

[0063] in, represents the normal vector of the line where the line segment i is located, θ represents the angle between the normal vector of the line where the line segment is located and the x-axis in the robot coordinate system o at the current moment, and s i 、e i represents the two endpoints of line segment i, and n is a positive integer.

[0064] Step S120: estimating the relative posture of the robot at the time t compared to the time t-1.

[0065] The position of the robot 100 at time t is referred to as the posture of the robot 100 at time t.

[0066] Assume that the posture of robot 100 at time t is And assume that the posture of the robot 100 at the moment before the moment t (referred to as the moment t-1 in the present embodiment) is

[0067] Among them, (x k ,yk ) represents the coordinates of the robot relative to the world coordinate system w at time t=k, represents the orientation of the robot 100 relative to the world coordinate system w at time t=k, The expression T representing the posture includes three real variables, namely x, y,

[0068] In addition, it is assumed that the rotation matrix and translation vector of the robot coordinate system o of the robot 100 relative to the world coordinate system w at time t=k are in, is the rotation matrix, is the translation vector.

[0069] After the above-mentioned assumed parameters are present, the relative posture of the robot 100 at time t (t=k) compared to time t-1 can be estimated according to the pre-stored estimation algorithm of the relative posture of the robot at different times.

[0070] Among them, the estimation algorithm of the relative posture of the robot at different times may include, but is not limited to: the posture estimation algorithm of the robot wheel encoder, the matching algorithm of the iterative closest point (ICP) of the point cloud, etc. The specific type of estimation algorithm.

[0071] Taking the posture estimation algorithm of the robot wheel encoder as an example, according to this algorithm, the robot can calculate the relative posture at time t compared to time t-1

[0072] in, and The three satisfy the formula: v is the linear velocity of the robot 100 when it moves forward measured by the built-in wheel encoder, ω is the angular velocity of the robot 100 when it moves forward measured by the built-in wheel encoder, and △t is the time interval between time t and time t-1.

[0073] It is worth noting that there is no strict order of execution between the above step S110 and step S120. It is understandable that in some embodiments, step S120 can be executed before step S110, in other embodiments, step S120 can be executed after step S110, and in other embodiments, step S120 and step S110 can be executed at the same time.

[0074] Step S130: According to the relative posture, line segment matching is performed on the multiple line segments corresponding to the time t and the multiple line segments obtained by extracting straight line features at the time t-1 to obtain the straight lines to which the respective line segments belong.

[0075] Generally speaking, the laser emitting assembly 130 collects point cloud data at a rate of 10-50 frames per second in the environment. Therefore, for the same spatial scene, the difference between the point cloud data collected by adjacent frames is small. Under this premise, when it is necessary to build a map, in order to reduce the amount of calculation, it is necessary to match the line segments extracted at adjacent moments with each other, so as to associate the line segments belonging to the same straight line and determine them as the same straight line. In addition, it is also convenient to form a complete graph when building the map later, improving the accuracy and completeness of the construction.

[0076] Optionally, the process of line segment matching can refer to the following method.

[0077] Step S131: According to the relative posture, the coordinates of the two endpoints of the line segment corresponding to the time t are converted into the reference coordinate system to obtain an expression of the line segment corresponding to the time t in the reference coordinate system.

[0078] Assume that the current moment t = c, the previous moment t-1 = r, and the relative posture of the current moment to the previous moment The robot coordinate system formed by the robot 100 at the previous moment (time t-1) is used as the reference coordinate system.

[0079] Assume that a point in the robot coordinate system at the current moment is The rotation matrix of the robot 100 at the current moment relative to the previous moment and translation vector You can Transform the robot coordinate system into the reference coordinate system, the formula is: Can be abbreviated as in, For point Expression in the reference coordinate system.

[0080] The same method as above can be used to observe the two endpoints of the line segment at the current moment After transformation to the reference coordinate system, we get Correspondingly, the line segment extracted by the robot 100 at the current moment The expression after transformation to the reference coordinate system (the robot coordinate system corresponding to the previous moment) is: Among them, the transformation equation of the normal vector of the line segment is

[0081] Step S132: Match the expression of the line segment corresponding to the time t in the reference coordinate system with the expressions of multiple line segments corresponding to the time t-1.

[0082] In an optional implementation, the line segment corresponding to the current moment after the coordinate system transformation may be directly matched with the line segment corresponding to the previous moment.

[0083] In another optional implementation, before performing line segment matching, the line segments obtained at the previous moment may be filtered to remove the difference line segments in the line segments corresponding to the previous moment, and the remaining line segments are determined as candidate line segments.

[0084] Among them, for each line segment, there is a large difference between the corresponding difference line segment representation and the line segment, and the probability that the two belong to the same straight line is small.

[0085] If, before performing line segment matching, the difference line segments corresponding to the previous moment are screened out for the line segments corresponding to the current moment, then when matching the line segments corresponding to the current moment after the coordinate system transformation with the line segments corresponding to the previous moment, the line segments corresponding to the current moment after the coordinate system transformation and the candidate line segments corresponding to the previous moment can be directly matched, thereby reducing the amount of data when performing the matching operation.

[0086] The candidate line segments may be screened in the following manner.

[0087] For each line segment corresponding to the current moment, the difference line segments can be filtered from all line segments corresponding to the previous moment according to predetermined attribute features and pre-saved attribute feature thresholds, and the remaining line segments in the previous moment can be determined as candidate line segments corresponding to the line segment.

[0088] Optionally, for a line segment observed by the robot 100 at the current time c The expression in the reference coordinate system corresponding to the previous moment r is You can and each line segment observed by the robot 100 at the previous moment r The comparison is performed to determine the attribute features between the two line segments to be compared. The attribute features include but are not limited to at least one of the angle difference between the two line segments to be compared, the distance between the two line segments to be compared, and the overlap between the two line segments to be compared. Accordingly, the attribute feature thresholds corresponding to the above-listed attribute features are the angle difference threshold, the distance threshold, and the overlap threshold.

[0089] Optionally, the angle difference between the two line segments to be compared is Angle in the reference coordinate system and the line segment in the reference coordinate system Angle in the reference coordinate system The angle difference between If the angle difference exceeds a preset angle difference threshold (e.g., 5°), the two line segments to be compared are considered and The probability that they belong to the same straight line is small. for The difference line segment.

[0090] Optionally, the distance between the two line segments to be compared is The expressions of the two endpoints of in the reference coordinate system To the line segment in the reference coordinate system The distances are d1 and d2 respectively. As for d1 and d2, if the distance represented by any of them exceeds a preset distance threshold (e.g., 10 cm), then the two line segments to be compared are considered and The probability that they belong to the same straight line is small. for The difference line segment.

[0091] Optionally, the overlap degree d0 between the two line segments to be compared, the endpoints Projection onto a line Get the projection point d0 is the line segment and If d0 is less than the overlap threshold (e.g., 10 cm), the two line segments to be compared are considered and The probability that they belong to the same straight line is small. for The difference line segment.

[0092] The intuitive meaning of d1, d2, and d0 is as follows Figure 4 shown.

[0093] The following will introduce the process of line segment matching.

[0094] For each line segment corresponding to time t, the line segment and each single line segment corresponding to time t-1 form a line segment pair, and according to the pre-constructed loss function, the loss value of each line segment pair corresponding to the line segment is calculated, and then the line segment pair corresponding to the minimum loss value is determined to belong to the same straight line.

[0095] Assume that a line segment observed at the current time c is Its expression in the reference coordinate system is A line segment observed at the previous moment r is And in the current example, and Combine into line segment pairs.

[0096] Among them, the pre-constructed loss function is d1, d2 are the line segments The expressions of the two endpoints of in the reference coordinate system To the other line segment in the line segment pair d0 is the length of the overlapping area of ​​the two line segments in the line segment pair, For this line segment The length of , α is a preset control factor parameter (for example, it can be set to 0.5).

[0097] By repeating the above process of calculating the loss value, we can get a line segment observed at the current moment c as The loss value between the line segments observed at the previous moment r (if line segment filtering is performed, this is the loss value between the candidate line segments observed at the previous moment r). Subsequently, by comparing the size of each loss value, the line segment observed at the previous moment r corresponding to the minimum loss value is compared with At this time, the two line segments can be set with the same line identifier (eg, line ID) to indicate that the two line segments belong to the same line, and can be represented by the same expression.

[0098] In addition, if a line segment is observed at the current moment If the candidate line segment obtained is 0 when filtering the line segments observed at the previous moment r, it means that the line segment is part of the newly appeared straight line. In this case, Set a new line identifier. When a line segment belonging to the same line is generated, the new line identifier can be inherited to subsequent segments connected to the line segment. Line segments that belong to the same straight line.

[0099] When the line segments observed at the current moment are iteratively matched with the line segments observed at the previous moment by the above-mentioned line segment matching method at each moment, the line segments belonging to the same straight line at the first moment and the current moment can be associated.

[0100] In addition, since a two-dimensional line has only two degrees of freedom in a plane, namely y=kx+b, under the premise that the direction of the line is known and the distance from the line to the origin is known, the uniquely determined x and y can uniquely determine a line. Therefore, a straight line can be uniquely determined by two parameters.

[0101] In the embodiment of the present application, in order to reduce the amount of calculation, each straight line is represented by a two-straight line parameterization form: in is the angle corresponding to the normal vector of the straight line, ρ represents the distance from the origin of the coordinate system to the straight line, and the equation corresponding to the point on the straight line is: cos(θ)x+sin(θ)y=ρ.

[0102] Step S140: jointly optimizing the posture of the robot at each moment and the two-line parameterized representation of all the lines to obtain the posture of the robot at each moment and the line parameters of all the lines.

[0103] Assume that the robot trajectory that needs to be estimated by the robot 100 at the current moment includes the postures at n moments At the same time, suppose that there are m straight lines obtained after line segment matching at the current moment, and the m straight lines are expressed as

[0104] Assume that at time t=k, the relative posture of the robot 100 in the robot coordinate system at this moment relative to the world coordinate system is Assume that the i-th line in the map At time k, it is observed by robot 100, then The corresponding observation in the robot coordinate system O is Under ideal conditions, for a point on a straight line The point obtained by transforming it from the robot coordinate system to the world coordinate system The equation of the straight line should satisfy:

[0105]

[0106]

[0107] However, due to the noise in the point cloud data measurements and the estimation error of the robot posture, the observed coordinates and map straight line Residual constraints can be constructed between:

[0108]

[0109] Considering that each line has two endpoints s, e, a line can construct a residual vector consisting of two residuals:

[0110]

[0111] By minimizing the sum of squares of the residual vector, the optimal estimation formula between the posture at each moment and all straight lines can be obtained:

[0112] By iteratively solving the above optimal estimation formula, the posture at each moment and the line parameters of all lines can be obtained, thereby completing the trajectory estimation and map construction of the robot 100.

[0113] Among them, the solution to the nonlinear least squares problem can be solved by existing calculation methods, such as Gauss-Newton algorithm (Gauss-Newton), Levenberg-Marquardt algorithm (Levenberg-Marquardt), etc. for iterative solution. The specific solution method is not limited in the embodiments of the present application.

[0114] like Figure 5 As shown, the embodiment of the present application further provides a positioning and map building device 400 , which may include: an extraction module 410 , an estimation module 420 , a matching module 430 , and an optimization module 440 .

[0115] An extraction module 410 is used to extract straight line features from the acquired point cloud data at time t to obtain a corresponding plurality of line segments;

[0116] An estimation module 420, for estimating the relative posture of the robot at the time t compared to the time t-1;

[0117] A matching module 430 is used to perform line segment matching on the multiple line segments corresponding to the time t and the multiple line segments obtained by extracting the straight line features at the time t-1 according to the relative posture, so as to obtain the straight line to which each line segment belongs;

[0118] The optimization module 440 is used to jointly optimize the posture of the robot at each moment and the two-line parameterized representation of all the lines to obtain the posture of the robot at each moment and the line parameters of all the lines.

[0119] In a possible implementation, the estimation module 420 is used to set the posture of the robot at the time t to Set the posture of the robot at the time t-1 to Set the rotation matrix and translation vector of the robot coordinate system relative to the world coordinate system at time t to be According to the pre-saved robot wheel encoder posture estimation algorithm, the relative posture of the time t compared to the time t-1 is calculated. in, and The formula is satisfied: (x k ,y k) represents the coordinates of the robot relative to the world coordinate system w at time t=k, represents the orientation of the robot relative to the world coordinate system w at time t=k, is the rotation matrix, is the translation vector, v is the linear velocity of the robot, ω is the angular velocity of the robot, and Δt is the time interval between the time t and the time t-1.

[0120] In a possible implementation, the matching module 430 is used to determine that the robot coordinate system formed by the robot at the time t-1 is a reference coordinate system; according to the relative posture, the coordinates of the two endpoints of the line segment corresponding to the time t are converted into the reference coordinate system to obtain the expression of the line segment corresponding to the time t in the reference coordinate system; the expression of the line segment corresponding to the time t in the reference coordinate system and the expressions of multiple line segments corresponding to the time t-1 are matched.

[0121] In a possible implementation, the device further includes a filtering module, configured to filter, for each line segment corresponding to the time t, the difference line segments from all line segments corresponding to the time t-1 according to a predetermined attribute feature and a pre-stored attribute feature threshold, and determine the remaining line segments corresponding to the time t-1 as candidate line segments corresponding to the line segment;

[0122] Correspondingly, the matching module 430 is used to match, for each line segment corresponding to the time t, the expression of the line segment in the reference coordinate system and the expression of the candidate line segment corresponding to the line segment.

[0123] In a possible implementation, the device further includes a determination module for determining that the number of candidate line segments corresponding to the line segment is not zero; when the determination module determines that , the device further includes an adding module for determining that the line segment belongs to a new straight line.

[0124] In a possible implementation, the matching module 430 is configured to, for each line segment corresponding to the time t, form a line segment pair with each single line segment corresponding to the time t-1, and match the line segment pair according to a pre-constructed loss function. Calculate the loss value of each line segment pair corresponding to the line segment; determine the line segment pair corresponding to the minimum loss value as belonging to the same straight line; where d1 and d2 are the distances from the two endpoints of the line segment to the other line segment in the line segment pair, d0 is the length of the overlapping area of ​​the two line segments in the line segment pair, is the length of the line segment, α is a pre-set control factor parameter, and d1, d2, d0, They are all determined by the expressions of the two segments in the pair.

[0125] In one possible implementation, the optimization module 440 is used to set an expression for the posture of the robot at each moment; set an expression for all straight lines obtained after the line segment matching; construct a residual vector through the expression for the posture and the expression for all straight lines; obtain an optimal estimation formula between the posture at each moment and all straight lines by minimizing the sum of squares of the residual vector; and iteratively solve the optimal estimation formula to obtain the posture at each moment and straight line parameters of all straight lines.

[0126] The positioning and map building device 400 provided in the embodiment of the present application has the same implementation principle and technical effects as those of the aforementioned method embodiment. For the sake of brief description, for matters not mentioned in the device embodiment, reference may be made to the corresponding contents in the aforementioned method embodiment.

[0127] In addition, an embodiment of the present application further provides a computer storage medium on which a computer program is stored. When the computer program is executed by a computer, the steps included in the positioning and map building method as described above are executed.

[0128] In addition, an embodiment of the present invention further provides a robot, comprising a processor and a memory connected to the processor, wherein the memory stores a computer program, and when the computer program is executed by the processor, the robot executes the steps included in the positioning and map building method as described above. Figure 1 .

[0129] In summary, the positioning and map construction method, device, robot and computer storage medium proposed in the embodiments of the present invention extract straight line features from the acquired point cloud data at time t to obtain multiple corresponding line segments; estimate the relative posture of the robot at time t compared to time t-1; perform line segment matching on multiple line segments corresponding to the time t and multiple line segments obtained by extracting straight line features at time t-1 according to the relative posture to obtain the straight lines to which each line segment belongs; jointly optimize the posture of the robot at each time and the two-line parameterized representation of all straight lines to obtain the posture of the robot at each time and the straight line parameters of all straight lines. Since the two-line parameterized representation is used when representing the straight line, the amount of calculation during data calculation can be reduced as much as possible, thereby improving data acquisition efficiency and shortening calculation time.

[0130] It should be noted that the various embodiments in this specification are described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the various embodiments can be referenced to each other.

[0131] In several embodiments provided in the present application, it should be understood that the disclosed devices and methods can also be implemented in other ways. The device embodiments described above are merely schematic. For example, the flowcharts and block diagrams in the accompanying drawings show the possible architecture, functions and operations of the devices, methods and computer program products according to multiple embodiments of the present application. In this regard, each box in the flowchart or block diagram can represent a module, a program segment or a part of a code, and the module, a program segment or a part of a code contains one or more executable instructions for implementing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order from the order marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or flowchart, and the combination of boxes in the block diagram and / or flowchart can be implemented with a dedicated hardware-based system that performs a specified function or action, or can be implemented with a combination of dedicated hardware and computer instructions.

[0132] In addition, the functional modules in the various embodiments of the present application may be integrated together to form an independent part, or each module may exist separately, or two or more modules may be integrated to form an independent part.

[0133] If the functions are implemented in the form of software function modules and sold or used as independent products, they can be stored in a computer storage medium. Based on this understanding, the technical solution of the present application can be essentially or partly embodied in the form of a software product that contributes to the prior art. The computer software product is stored in a storage medium and includes several instructions for a computer device (which can be a personal computer, a laptop, a server, or a network device, etc.) to perform all or part of the steps of the methods described in each embodiment of the present application. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0134] The above description is only a specific implementation manner of the present application, but the protection scope of the present application is not limited thereto. Any technician familiar with the technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application.

Claims

1. A positioning and map construction method, characterized in that: The method comprises: At time t, straight line features are extracted from the acquired point cloud data to obtain corresponding multiple line segments; Estimate the relative posture of the robot at the time t compared to the time t-1; According to the relative posture, performing line segment matching on the multiple line segments corresponding to the time t and the multiple line segments obtained by performing line feature extraction at the time t-1 to obtain the straight line to which each line segment belongs; Jointly optimizing the posture of the robot at each moment and the two-line parameterized representation of all the lines to obtain the posture of the robot at each moment and the line parameters of all the lines; The estimating the relative posture of the robot at the time t compared to the time t-1 includes: Set the posture of the robot at time t to Set the posture of the robot at the time t-1 to in, represents real number xk, real number yk, real number The three-dimensional variables of the composition; Set the rotation matrix and translation vector of the robot coordinate system relative to the world coordinate system at time t to be According to the pre-saved robot wheel encoder posture estimation algorithm, the relative posture of the time t compared to the time t-1 is calculated. in, and The formula is satisfied: (x k ,y k ) represents the coordinates of the robot relative to the world coordinate system w at time t=k, represents the orientation of the robot relative to the world coordinate system w at time t=k, is the rotation matrix, is the translation vector, v is the linear velocity of the robot, ω is the angular velocity of the robot, and Δt is the time interval between the time t and the time t-1.

2. The method according to claim 1, characterized in that: The performing line segment matching on the multiple line segments corresponding to the time t and the multiple line segments obtained by extracting straight line features at the time t-1 includes: Determine the robot coordinate system formed by the robot at the time t-1 as the reference coordinate system; According to the relative posture, the coordinates of the two endpoints of the line segment corresponding to the time t are converted into the reference coordinate system to obtain an expression of the line segment corresponding to the time t in the reference coordinate system; The expression of the line segment corresponding to the time t in the reference coordinate system and the expressions of the multiple line segments corresponding to the time t-1 are matched.

3. The method according to claim 2, characterized in that Before matching the expression of the line segment corresponding to the time t in the reference coordinate system with the expressions of the plurality of line segments corresponding to the time t-1, the method further includes: For each line segment corresponding to the time t, according to a predetermined attribute feature and a pre-stored attribute feature threshold, filter the difference line segments from all the line segments corresponding to the time t-1, and determine the remaining line segments corresponding to the time t-1 as candidate line segments corresponding to the line segment; Correspondingly, matching the expression of the line segment corresponding to the time t in the reference coordinate system with the expressions of the plurality of line segments corresponding to the time t-1 includes: For each line segment corresponding to the time t, the expression of the line segment in the reference coordinate system and the expression of the candidate line segment corresponding to the line segment are matched.

4. The method according to claim 3, characterized in that Before matching the expression of the line segment in the reference coordinate system with the expression of the candidate line segment corresponding to the line segment, the method further includes: Determine that the number of candidate line segments corresponding to the line segment is not zero; Otherwise, the method further comprises: Determine if the line segment belongs to the new line.

5. The method according to claim 2 or 3, characterized in that: The matching of the expression of the line segment corresponding to the time t in the reference coordinate system with the expressions of the plurality of line segments corresponding to the time t-1 includes: For each line segment corresponding to the time t, the line segment and each single line segment corresponding to the time t-1 form a line segment pair, and according to the pre-constructed loss function Calculate the loss value of each line segment pair corresponding to the line segment; Determine the line segment pair corresponding to the minimum loss value as belonging to the same straight line; Among them, d1 and d2 are the distances from the two endpoints of the line segment to the other line segment in the line segment pair, and d0 is the length of the overlapping area of ​​the two line segments in the line segment pair. is the length of the line segment, α is a pre-set control factor parameter, and d1, d2, d0, They are all determined by the expressions of the two segments in the pair.

6. The method according to claim 1, characterized in that The posture of the robot at each moment and the two-line parameterized representation of all the lines are jointly optimized to obtain the posture of the robot at each moment and the line parameters of all the lines, including: Setting the expression of the posture of the robot at each moment; Set the expressions of all straight lines obtained after matching the line segments; Constructing a residual vector through the expression of the posture and the expressions of all the straight lines; By minimizing the sum of squares of the residual vectors, an optimal estimation formula between the posture at each moment and all the straight lines is obtained; The optimal estimation formula is iteratively solved to obtain the posture at each moment and the straight line parameters of all straight lines.

7. A positioning and map building device, characterized in that: The device comprises: An extraction module is used to extract straight line features from the acquired point cloud data at time t to obtain corresponding multiple line segments; An estimation module is used to estimate the relative posture of the robot at the time t compared to the time t-1; the estimation of the relative posture of the robot at the time t compared to the time t-1 includes: Set the posture of the robot at time t to Set the posture of the robot at the time t-1 to in, Represents a real number x k , real number y k , real numbers The three-dimensional variables of the composition; Set the rotation matrix and translation vector of the robot coordinate system relative to the world coordinate system at time t to be According to the pre-saved robot wheel encoder posture estimation algorithm, the relative posture of the time t compared to the time t-1 is calculated. in, and The formula is satisfied: (x k ,y k ) represents the coordinates of the robot relative to the world coordinate system w at time t=k, represents the orientation of the robot relative to the world coordinate system w at time t=k, is the rotation matrix, is the translation vector, v is the linear velocity of the robot, ω is the angular velocity of the robot, and Δt is the time interval between the time t and the time t-1; A matching module, configured to perform line segment matching on the multiple line segments corresponding to the time t and the multiple line segments obtained by extracting straight line features at the time t-1 according to the relative posture, so as to obtain the straight line to which each line segment belongs; The optimization module is used to jointly optimize the posture of the robot at each moment and the two-line parameterized representation of all the lines to obtain the posture of the robot at each moment and the line parameters of all the lines.

8. A robot, characterized in that: include: A memory and a processor, wherein the memory and the processor are connected; The memory is used to store programs; The processor calls the program stored in the memory to execute the method according to any one of claims 1 to 6.

9. A computer storage medium, characterized in that: A computer program is stored thereon, and when the computer program is executed by a computer, the method according to any one of claims 1 to 6 is executed.

Images