CN112577500A - Positioning and map construction method and device, robot and computer storage medium - Google Patents

Abstract

The invention relates to a positioning and map construction method, a positioning and map construction device, a robot and a computer storage medium, wherein the method comprises the following steps: performing linear feature extraction on the acquired point cloud data at the time t to obtain a plurality of corresponding line segments; estimating the relative attitude of the robot at the time t compared with the time t-1; according to the relative posture, performing line segment matching on a plurality of line segments corresponding to the t moment and a plurality of line segments obtained by performing line feature extraction at the t-1 moment to obtain a line to which each line segment belongs; and carrying out joint optimization on the postures of the robot at each moment and the two-straight-line parametric representation forms of all straight lines to obtain the postures of the robot at each moment and the straight line parameters of all straight lines. When representing straight lines, a two-straight-line parametric representation form is adopted, so that the calculation amount can be reduced as much as possible, the calculation efficiency can be improved, and the calculation time can be shortened.

Description

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

technical field

The present application belongs to the field of autonomous navigation, and specifically relates to a positioning and map construction method, device, robot and computer storage medium.

Background technique

During the operation of the mobile robot, it needs to continuously obtain data so as to locate itself in real time. In addition, it also needs to construct a map of the surrounding environment according to the obtained data.

Generally speaking, the mobile robot obtains and determines the data for real-time localization and map construction through the SLAM (Simultaneous Localization And Mapping) algorithm. Therefore, it is necessary to propose a stable, efficient and high-precision SLAM algorithm.

In the prior art, a SLAM algorithm that uses graph optimization technology to optimize the line feature and the robot attitude in real time is proposed. However, in this algorithm, six parameters corresponding to the line feature are used to represent the line, so that the description of the line is not The minimum parameterized representation, therefore, results in a large amount of subsequent calculations and low computational efficiency, which further affects the efficiency of real-time positioning and map construction.

SUMMARY OF THE INVENTION

In view of this, the purpose of this 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 process of positioning and map construction, thereby improving the efficiency of positioning and map construction, so as to improve the subsequent real-time Efficiency of positioning and map building.

The embodiments of the present application are implemented as follows:

In the first aspect, an embodiment of the present application provides a method for positioning and constructing a map, the method includes: extracting linear features from the acquired point cloud data at time t to obtain a plurality of corresponding line segments; The relative posture at time t compared to time t-1; according to the relative posture, the line segments corresponding to the time t and the line segments obtained by performing linear feature extraction at the time t-1 are processed. Match to obtain the straight line to which each line segment belongs; perform joint optimization on the posture of the robot at each moment and the parametric representation of the two straight lines of all straight lines to obtain the posture of the robot at each moment and the straight line parameters of all straight lines . Since the two-line parametric representation is used to represent the straight line, the calculation amount of the data determined for the subsequent implementation of 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 building.

设置所述机器人在所述t-1时刻的姿态为

设置所述机器人在所述t时刻的机器人坐标系相对于世界坐标系的旋转矩阵及平移向量分别为

根据预先保存的机器人轮式编码器姿态估计算法，计算得到所述t时刻相较于所述t-1时刻的相对姿态

其中，

之间满足公式：

(x_k，y_k)表示所述机器人在t＝k时刻时相对于世界坐标系w的坐标，

表示所述机器人在t＝k时刻时相对于世界坐标系w的朝向，

为所述旋转矩阵，

为所述平移向量，v为所述机器人的线速度，ω为所述机器人的角速度，△t为所述t时刻与所述t-1时刻之间的时间间隔。With reference to the embodiment of the first aspect, in a possible implementation, the 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 for

Set the posture of the robot at the time t-1 as

The rotation matrix and translation vector of the robot coordinate system relative to the world coordinate system of the robot at the time t are set as

According to the pre-saved robot wheel encoder attitude estimation algorithm, the relative attitude of the time t compared to the time t-1 is calculated and obtained

in,

The formula is satisfied between:

(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.

With reference to the embodiment of the first aspect, in a possible implementation manner, the line segment matching is performed on a plurality of line segments corresponding to the time t and a plurality of line segments obtained by performing linear feature extraction at the time t-1 , including: determining the robot coordinate system formed by the robot at the time t-1 as the 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 coordinate system In the reference coordinate system, the expression of the line segment corresponding to the time t under the reference coordinate system is obtained; the expression of the line segment corresponding to the time t under the reference coordinate system and the line segment corresponding to the time t The expressions of multiple line segments corresponding to time -1 are matched.

With reference to the embodiment of the first aspect, in a possible implementation manner, in the expression of the line segment corresponding to the time t in the reference coordinate system and a plurality of expressions corresponding to the time t-1 Before the expression of the line segment is matched, 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, from the line corresponding to the time t-1. Filter the difference line segments from all the line segments, and determine the remaining line segments corresponding to the time t-1 as the candidate line segments corresponding to the line segment;

Correspondingly, the matching 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 includes: for the line segment corresponding to the time t For each line segment corresponding to the moment, 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. By reducing the number of line segments that need to be matched during line segment matching, the amount of calculation is reduced and the calculation efficiency is improved.

With reference to the embodiment of the first aspect, in a possible implementation, before 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, the method The method further includes: determining that the number of candidate line segments corresponding to the line segment is not zero; otherwise, the method further includes: determining that the line segment belongs to a new straight line.

计算与该线段对应的各个线段对的损失值；将所述损失值最小时所对应的线段对确定为属于同一直线；其中，d₁，d₂为该线段的两个端点到线段对中的另一线段的距离，d₀是线段对中两线段的重合区域的长度，

为该线段的长度，α是预先设定的控制因子参数，且d₁，d₂，d₀，

均由线段对中的两线段的表达式来确定。With reference to the embodiment of the first aspect, in a possible implementation manner, the expression of the line segment corresponding to the time t in the reference coordinate system and the multiple line segments corresponding to the time t-1 Matching the expression of the

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; wherein, d ₁ , d ₂ are the two end points of the line segment to the line segment pair. The distance of another line segment, d ₀ 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 preset control factor parameter, and d ₁ , d ₂ , d ₀ ,

Both are determined by the expressions of the two line segments in the line segment pair.

In combination with the embodiment of the first aspect, in a possible implementation, the robot's posture at each moment and the two-line parametric representation of all straight lines are jointly optimized to obtain the robot at each moment The attitude of the robot and the straight line parameters of all straight lines, including: setting the expression of the attitude of the robot at each moment; setting the expression of all the straight lines obtained after the matching of the line segments; through the expression of the attitude and The expressions of all the straight lines are used to construct residual vectors; by minimizing the sum of the squares of the residual vectors, the optimal estimation formula between the posture at each moment and all the straight lines is obtained; The estimation formula is iteratively solved to obtain the posture at each moment and the straight line parameters of all straight lines.

In a second aspect, an embodiment of the present application provides a positioning and map construction device, the device includes: an extraction module, an estimation module, a matching module, and an optimization module. The extraction module is used to perform linear feature extraction on the acquired point cloud data at time t to obtain a plurality of corresponding line segments; the estimation module is used to estimate the relative position of the robot at time t compared to time t-1. Attitude; a matching module, configured to perform line segment matching on a plurality of line segments corresponding to the time t and a plurality of line segments obtained by performing linear feature extraction at the time t-1 according to the relative posture, to obtain each line segment The line to which it belongs; the optimization module is used to jointly optimize the posture of the robot at each moment and the two-line parameterized representations of all the lines to obtain the posture of the robot at the time and the line parameters of all the lines.

设置所述机器人在所述t-1时刻的姿态为

设置所述机器人在所述t时刻的机器人坐标系相对于世界坐标系的旋转矩阵及平移向量分别为

根据预先保存的机器人轮式编码器姿态估计算法，计算得到所述t时刻相较于所述t-1时刻的相对姿态

其中，

与

之间满足公式：

(x_k，y_k)表示所述机器人在t＝k时刻时相对于世界坐标系w的坐标，

表示所述机器人在t＝k时刻时相对于世界坐标系w的朝向，

为所述旋转矩阵，

为所述平移向量，v为所述机器人的线速度，ω为所述机器人的角速度，△t为所述t时刻与所述t-1时刻之间的时间间隔。With reference to the embodiment of the second aspect, in a possible implementation, the estimation module is configured to set the posture of the robot at the time t to be

Set the posture of the robot at the time t-1 as

The rotation matrix and translation vector of the robot coordinate system relative to the world coordinate system of the robot at the time t are set as

According to the pre-saved robot wheel encoder attitude estimation algorithm, the relative attitude of the time t compared to the time t-1 is calculated and obtained

in,

and

The formula is satisfied between:

(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.

With reference to the embodiment of the second aspect, in a possible implementation, the matching module is configured to determine the robot coordinate system formed by the robot at the time t-1 as a reference coordinate system; attitude, convert the coordinates of the two endpoints of the line segment corresponding to the time t into the reference coordinate system, and 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 the multiple line segments corresponding to the time t-1 are matched.

With reference to the embodiment of the second aspect, in a possible implementation manner, the apparatus further includes a filtering module, for each line segment corresponding to the time t, according to a predetermined attribute feature and a pre-saved attribute feature Threshold, filter difference line segments from all 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, the matching module is configured to, for each line segment corresponding to the time t, match the expression of the line segment in the reference coordinate system with the expression of the candidate line segment corresponding to the line segment.

With reference to the embodiment of the second aspect, in a possible implementation manner, the apparatus further includes a determination module configured to determine that the number of candidate line segments corresponding to the line segment is not zero; when the determination module determines that it is The apparatus also includes an adding module for determining that the line segment belongs to a new straight line.

计算与该线段对应的各个线段对的损失值；将所述损失值最小时所对应的线段对确定为属于同一直线；其中，d₁，d₂为该线段的两个端点到线段对中的另一线段的距离，d₀是线段对中两线段的重合区域的长度，

为该线段的长度，α是预先设定的控制因子参数，且d₁，d₂，d₀，

均由线段对中的两线段的表达式来确定。With reference to the embodiment of the second aspect, in a possible implementation manner, the matching module is configured to, for each line segment corresponding to the time t, compare the line segment with each single line segment corresponding to the time t-1. Line segments form line-segment pairs, and according to a pre-built 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; wherein, d ₁ , d ₂ are the two end points of the line segment to the line segment pair. The distance of another line segment, d ₀ 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 preset control factor parameter, and d ₁ , d ₂ , d ₀ ,

Both are determined by the expressions of the two line segments in the line segment pair.

With reference to the embodiment of the second aspect, in a possible implementation, the optimization module is used to set the expression of the posture of the robot at the various moments; set all straight lines obtained after the line segment matching by the expression of the attitude and the expressions of all the straight lines, construct a residual vector; by minimizing the sum of squares of the residual vector, the difference between the attitude at each moment and all the straight lines is obtained The optimal estimation formula between the two 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.

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

In a fourth aspect, the embodiments of the present application further provide a non-volatile computer-readable storage medium (hereinafter referred to as a computer storage medium), on which a computer program is stored, and the computer program executes the above-mentioned first aspect when the computer program is run by a computer. Embodiments and/or methods provided in conjunction with any possible implementation manner of the embodiments of the first aspect.

Other features and advantages of the present application will be set forth in the description which follows, and, in part, will be apparent from the description, or may be learned by practice of the embodiments of the present application. The objectives and other advantages of the application may be realized and attained by the structure particularly pointed out in the written description and drawings.

Description of drawings

In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the accompanying drawings required in the embodiments will be briefly introduced below. Obviously, the drawings in the following description are only some of the present application. In the embodiments, for those of ordinary skill in the art, other drawings can also be obtained according to these drawings without any creative effort. The above and other objects, features and advantages of the present application will be more apparent from the accompanying drawings. The same reference numerals refer to the same parts throughout the drawings. The drawings are not intentionally scaled to actual size, and the emphasis is on illustrating the subject matter of the present application.

FIG. 1 shows a schematic structural diagram of a robot provided by an embodiment of the present application.

FIG. 2 shows a flowchart of the method for positioning and building a map provided by an embodiment of the present application.

FIG. 3 shows a schematic diagram of linear feature extraction according to a point cloud provided by an embodiment of the present application.

FIG. 4 shows a schematic diagram of line segment matching provided by an embodiment of the present application.

FIG. 5 shows a structural block diagram of the positioning and map building apparatus provided by the embodiment of the present application.

Icons: 100-robot; 110-processor; 120-memory; 130-laser emission component; 400-location and map construction device; 410-extraction module; 420-estimation module; 430-matching module; 440-optimization module.

Detailed ways

The technical solutions in the embodiments of the present application will be described below with reference to the accompanying drawings in the embodiments of the present application.

It should be noted that like numerals and letters refer to like items in the following figures, so once an item is defined in one figure, it does not require further definition and explanation in subsequent figures. Meanwhile, 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 these entities or that there is any such actual relationship or sequence between operations. Moreover, the term "comprising" or any other variation thereof is intended to cover non-exclusive inclusion, whereby a process, method, article or device comprising a series of elements includes not only those elements, but also other elements not expressly listed, Or also include elements inherent to such a process, method, article or apparatus. Without further limitation, an element qualified by the phrase "comprising a..." does not preclude the presence of additional identical elements in a process, method, article or apparatus that includes the element.

Furthermore, the term "and/or" in this application is only an association relationship to describe related objects, indicating that there can be three kinds of relationships, for example, A and/or B, it can mean that A exists alone, and A and B exist at the same time. B, there are three cases of B alone.

In addition, the defects of 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 above-mentioned defects are found The process and the solutions to the above-mentioned defects proposed by the embodiments of the present application hereinafter should be regarded as contributions made by the applicant to the present application.

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, thereby improving the calculation efficiency.

The technology can be implemented by corresponding software, hardware and combination of software and hardware. The embodiments of the present application are described in detail below.

First, referring to FIG. 1 , a robot 100 for implementing the method and apparatus for positioning and map construction according to the embodiment of the present application will be described.

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

It should be noted that the components and structures of the robot 100 shown in FIG. 1 are only exemplary and not restrictive, and the robot 100 may also have other components and structures as required.

The processor 110 , the memory 120 , the laser emitting component 130 and other components that may be present in the robot 100 are directly or indirectly electrically connected to each other to realize data transmission or interaction. For example, the processor 110, the memory 120, the laser emitting component 130 and other possible components may be electrically connected to each other through one or more communication buses or signal lines.

After the laser emitting component 130 is powered on, it can emit laser light at a fixed frequency, and receive the reflected laser light after the foreign object reflects the laser light, so as to generate point cloud data corresponding to each laser emission moment.

Wherein, in some embodiments, the laser emitting assembly 130 may be a multi-threaded laser assembly for emitting multi-threaded lasers; in other embodiments, the laser emitting assembly 130 may also be a single-threaded laser assembly for emitting laser light Single thread laser.

Optionally, since the data complexity of point cloud data obtained based on a single-threaded laser is lower than that of point cloud data obtained by multi-threaded lasers, for the purpose of reducing the amount of calculation and data complexity, it is possible to directly A single-threaded laser is used as the laser emitting component 130, so that the point cloud data obtained subsequently is composed of two-dimensional discrete points.

The memory 120 is used for storing programs, for example, a program corresponding to a positioning and map construction method appearing later or a positioning and map construction apparatus appearing later. Optionally, when the location and map construction apparatus is stored in the memory 120, the location and map construction apparatus includes at least one software function module that can be stored in the memory 120 in the form of software or firmware.

Optionally, the software function modules included in the positioning and map building apparatus may also be solidified in an operating system (operating system, OS) of the robot 100 .

The processor 110 is configured to execute executable modules stored in the memory 120, such as software function modules or computer programs included in the positioning and map construction apparatus. When the processor 110 receives the execution instruction, it can execute the computer program, for example, perform: extracting linear features from the acquired point cloud data at time t to obtain a plurality of corresponding line segments; Compared with the relative posture at time t-1; according to the relative posture, perform line segment matching on a plurality of line segments corresponding to the time t and a plurality of line segments obtained by performing linear feature extraction at the time t-1 to obtain The lines to which each line segment belongs; jointly optimize the posture of the robot at each moment and the two-line parameterized representations of all the lines to obtain the posture of the robot at the various moments and the linear parameters of all the lines.

Certainly, the method disclosed in any embodiment of the present application may be applied to the processor 110 or implemented by the processor 110 .

The following will introduce the positioning and map construction methods provided by the present application.

Referring to FIG. 2 , an embodiment of the present application provides a positioning and map construction method applied to the above-mentioned robot 100 . The embodiments of the present application will be described below with reference to FIG. 2 , taking the point cloud data composed of two-dimensional discrete points as an example.

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

After the robot 100 is powered on and starts, the laser emitting component 130 can emit laser light to the outside, so as to acquire point cloud data used to reflect the environmental conditions at the current moment.

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

In the point cloud data, several two-dimensional discrete points are included.

来表示。其中，x表示二维离散点在当前时刻的机器人坐标系o中的横坐标，y表示二维离散点在当前时刻的机器人坐标系o中的纵坐标。

表示二维离散点的表达式P中包括2个实数变量，即x与y。The two-dimensional discrete points in multiple planes produced by a single-line laser can be expressed by the formula

To represent. Among them, x represents the abscissa of the two-dimensional discrete point in the robot coordinate system o at the current moment, and y represents the ordinate 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.

Generally speaking, a first identification point and a second identification point are preset on the robot 100 .

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

For example, at any moment, the robot 100 takes the position of the first identification point as the origin of the coordinates, the line connecting the position of the second identification point and the position of the first identification point is the y-axis, and the vertical The straight line on the y-axis is the x-axis, thereby determining the robot coordinate system corresponding to the current moment.

It is worth noting that since the robot 100 is constantly moving, the coordinate systems of the robot corresponding to each moment may be different. In addition, generally, the robot coordinate system determined at the initial time of startup of the robot 100 is used as the global world coordinate system w.

The first identification point may be a position where the geometric center point of the robot 100 is located, or may be a point corresponding to other positions of the robot 100, which is not specifically limited in this embodiment of the present application.

After obtaining the point cloud data, as shown in FIG. 3 , the robot 100 can extract several line segments L _i ={n _i from the point cloud data through a pre-saved known linear feature extraction algorithm (such as the least squares algorithm), s _i , e _i }, i=1...n.

表示线段i所在直线的法向量，θ表示线段所在直线的法向量与当前时刻的机器人坐标系o中的x轴的夹角，s_i、e_i表示线段i的两个端点，n为正整数。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, s _i , e _i represent the two endpoints of the line segment i, n is a positive integer .

Step S120: Estimate the relative posture of the robot at the time t compared to the time t-1.

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

且假设机器人100在t时刻的前一时刻(在本申请实施例中称之为t-1时刻)的姿态为

Assume that the posture of the robot 100 at time t is

And it is assumed that the posture of the robot 100 at the moment before time t (referred to as time t-1 in the embodiment of the present application) is:

表示机器人100在t＝k时刻时，相对于世界坐标系w的朝向，

表示姿态的表达式T中包括3个实数变量，即x、y、

Among them, (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 100 relative to the world coordinate system w at time t=k,

The expression T that represents the attitude includes three real variables, namely x, y,

其中，

为旋转矩阵，

为平移向量。In addition, it is assumed that the rotation matrix and translation vector of the robot coordinate system o relative to the world coordinate system w of the robot 100 at time t=k are respectively:

in,

is the rotation matrix,

is the translation vector.

After the above-mentioned parameters are assumed, 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 for the relative posture of the robot at different times.

的估计算法的具体类型。Wherein, the estimation algorithm for 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 point cloud iterative closest point (ICP, Iterative Closest Point), and the like. In this embodiment of the present application, the relative pose pair is not limited

The specific type of estimation algorithm for .

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

与

三者之间满足公式：

v为机器人100通过内置的轮式编码器测得的自身在前进时的线速度，ω为机器人100通过内置的轮式编码器测得的自身在前进时的角速度，△t为t时刻与t-1时刻之间的时间间隔。in,

and

The formula between the three is satisfied:

v is the linear velocity of the robot 100 when it is moving forward measured by the built-in wheel encoder, ω is the angular velocity of the robot 100 when it is moving forward measured by the built-in wheel encoder, Δt is the time between t and t -1 Time interval between moments.

It is worth noting that there is no strict sequence of execution between the above steps S110 and S120. It can be understood that, in some embodiments, step S120 may be executed before step S110, in other embodiments, step S120 may be executed after step S110, and in other embodiments, step S120 and step S110 may be executed simultaneously.

Step S130: Perform line segment matching on the line segments corresponding to the time t and the line segments obtained by performing the linear feature extraction at the time t-1 according to the relative posture, to obtain the lines to which each line segment belongs.

Generally speaking, the laser emitting component 130 collects point cloud data at a rate of 10-50 frames per second in the environment. Therefore, for the same spatial scene, there are differences between the point cloud data collected in several adjacent frames. smaller. Under this premise, when a map needs to be constructed, for the purpose of reducing 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 the same straight line . In addition, it is also convenient to form a complete graph when the map is subsequently constructed, so as to improve the accuracy and completeness of the construction.

Optionally, the process of line segment matching may refer to the following method.

Step S131: According to the relative posture, convert the coordinates of the two end points of the line segment corresponding to the time t into the reference coordinate system, and obtain the line segment corresponding to the time t in the reference coordinate system. expression.

且以机器人100在前一时刻(t-1时刻)时所形成的机器人坐标系为参考坐标系。Among them, it is assumed that the current time t=c, the previous time t-1=r, the relative posture of the current time relative to the previous time

And take the robot coordinate system formed by the robot 100 at the previous time (time t-1) as the reference coordinate system.

通过机器人100在当前时刻相对于前一时刻的旋转矩阵

及平移向量

可以将

变换到机器人坐标系为参考坐标系，公式为：

可以简写为

其中，

为点

在参考坐标系下的表达式。Suppose a point in the robot coordinate system at the current moment is

Through the rotation matrix of the robot 100 at the current moment relative to the previous moment

and translation vector

can

Transform to the robot coordinate system as the reference coordinate system, the formula is:

can be abbreviated as

in,

for points

Expression in the reference coordinate system.

变换到参考坐标系后，得到

相应的，当前时刻机器人100所提取出的线段

变换到参考坐标系(前一时刻对应的机器人坐标系)后的表达式为：

其中，线段的法向量的变换方程为

The current moment can be observed at the two endpoints of the line segment in the same way as above

After transforming 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

Step S132: Match 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.

In an optional implementation manner, the line segment corresponding to the current moment after the coordinate system transformation can be directly matched with the line segment corresponding to the previous moment.

In another optional implementation, before performing line segment matching, the line segment obtained at the previous moment may also be filtered, so as to filter out the difference line segments in the line segment corresponding to the previous moment, and filter the remaining line segments. The line segment is determined as the candidate line segment.

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.

If before performing line segment matching, the line segment corresponding to the current moment is screened for the line segment corresponding to the previous moment. When the corresponding line segment is matched, the line segment corresponding to the current moment and the candidate line segment corresponding to the previous moment after the coordinate system transformation can be directly matched, thereby reducing the amount of data when performing the matching operation.

Among them, the candidate line segments can be screened by referring to the following methods.

For each line segment corresponding to the current moment, the difference line segment can be filtered from all the line segments corresponding to the previous moment according to the pre-determined attribute feature and the pre-stored attribute feature threshold, and the remaining line segments in the previous moment can be determined as The candidate line segment corresponding to this line segment.

在前一时刻r所对应的参考坐标系下的表达式

可以将

与机器人100在前一时刻r所观测到的每一条线段

进行比对，从而确定待比对的两条线段之间的属性特征。其中，属性特征包括但不限于：待比对的两线段的角度差、待比对的两线段之间的距离、待比对的两线段之间的重合度中的至少一个。相应的，与上述所列举的属性特征所对应的属性特征阈值分别为角度差阈值、距离阈值以及重合度阈值。Optionally, for a certain line segment observed by the robot 100 at the current moment c

The expression in the reference coordinate system corresponding to the previous moment r

can

and each line segment observed by the robot 100 at the previous time r

Alignment is performed to determine the attribute characteristics 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 degree of coincidence between the two line segments to be compared. Correspondingly, the attribute feature thresholds corresponding to the above-listed attribute features are an angle difference threshold, a distance threshold, and a coincidence threshold, respectively.

在参考坐标系里的角度

和参考坐标系里线段

在参考坐标系里的角度

之间的角度差

如果该角度差超过预先设定的角度差角度差阈值(例如5°)，则认为待比对的两线段

与

之间属于同一直线的概率较小，

为

的差异线段。Optionally, the angle difference between the two line segments to be compared is

angle in the reference frame

and line segments in the reference coordinate system

angle in the reference frame

angle difference between

If the angle difference exceeds the preset angle difference angle difference threshold (for example, 5°), it is considered that the two line segments to be compared are

and

The probability of belonging to the same straight line is small,

for

difference line segment.

的两端点在参考坐标系内的表达式

到参考坐标系里的线段

的距离，分别为d₁，d₂。其中

就d₁、d₂而言，其中任一个的所表征的距离超过预先设置的距离阈值(例如10厘米)，则认为待比对的两线段

与

之间属于同一直线的概率较小，

为

的差异线段。Optionally, the distance between the two line segments to be compared is

Expressions of both ends of , in the reference coordinate system

to the line segment in the reference coordinate system

The distances are d ₁ and d ₂ , respectively. in

As far as d ₁ and d ₂ are concerned, if the represented distance of any one of them exceeds a preset distance threshold (for example, 10 cm), it is considered that the two line segments to be compared are

and

The probability of belonging to the same straight line is small,

for

difference line segment.

投影到直线

上得到投影点

d₀即为线段

和

的重合区域的长度。如果d₀小于重合度阈值(例如10厘米)，则认为待比对的两线段

与

之间属于同一直线的概率较小，

为

的差异线段。Optionally, the degree of coincidence d ₀ between the two line segments to be compared, the endpoint

Project to line

get the projected point on

d ₀ is the line segment

and

the length of the overlapping region. If d ₀ is less than the coincidence threshold (for example, 10 cm), the two line segments to be compared are considered

and

The probability of belonging to the same straight line is small,

for

difference line segment.

The intuitive meaning of d ₁ , d ₂ , and d ₀ is shown in Fig. 4 below.

The process of line segment matching will be introduced below.

For each line segment corresponding to time t, a line segment pair is formed between the line segment and each single line segment corresponding to time t-1, and the loss value of each line segment pair corresponding to the line segment is calculated according to the pre-built loss function, and then The pair of line segments corresponding to the smallest loss value is determined to belong to the same straight line.

其在参考坐标系内的表达式为

前一时刻r所观测到的某一线段为

且当前举例时，将

与

组合成线段对。Suppose 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 the current example, will

and

Combine into line segment pairs.

d₁，d₂为该线段

的两个端点在参考坐标系内的表达式

分别到线段对中的另一线段

的距离，d₀是线段对中两线段的重合区域的长度，

为该线段

的长度，α是预先设定的控制因子参数(例如可以设置为0.5)。Among them, the pre-built loss function is

d ₁ , d ₂ are the line segments

The expression of the two endpoints of , in the reference coordinate system

to the other segment of the segment pair, respectively

The distance of d ₀ is the length of the overlapping area of the two line segments in the line segment pair,

for the line segment

The length of α is a preset control factor parameter (for example, it can be set to 0.5).

与前一时刻r所观测到的各条线段(若进行了线段过滤，则此处为前一时刻r所观测到的各条候选线段)之间的损失值。后续，通过比较各损失值之间的大小，将损失值最小时所对应的属于前一时刻r所观测到的线段与

确定为属于同一直线。此时，可以将两线段设置同样的直线标识(例如直线ID)，用于表征两者属于同一直线，可用同一个表达式来进行表示。By repeating the above process of calculating the loss value, a certain line segment observed at the current moment c can be obtained as

The loss value between each line segment observed at the previous time r (if line segment filtering is performed, here is each candidate line segment observed at the previous time 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

determined to belong to the same straight line. At this time, the same line identifier (eg, line ID) can be set for the two line segments to indicate that the two line segments belong to the same line, which can be represented by the same expression.

若在针对其对前一时刻r所观测到的各条线段进行线段过滤时，最终得到的候选线段为0，此时说明该线段

为新出现的直线的一部分，此时，可以为该线段

设置一个全新的直线标识。当后续时刻有与该线段

属于同一直线的线段产生时，可以将该全新的直线标识继承给后续与该线段

属于同一直线的线段。In addition, if a line segment is observed for the current moment

If the line segment filtering is performed on each line segment observed at the previous time r, the final candidate line segment is 0, which means that the line segment is 0.

is a part of the new line, at this time, it can be the line segment

Sets a brand new line identity. When there is a line segment related to the

When a line segment belonging to the same line is generated, the new line ID can be inherited to the subsequent line segment with the line segment.

Line segments that belong to the same straight line.

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

In addition, since a two-dimensional straight line has only 2 degrees of freedom in the plane, that is, y=kx+b, on the premise that the orientation of the straight line is known and the distance from the straight line to the origin is known, the unique x and y can be uniquely determined. Straight line, therefore, a straight line can be uniquely determined by 2 parameters.

其中

为直线法向量对应的角度，ρ表示坐标系原点到直线的距离，点在直线上对应的方程为：cos(θ)x+sin(θ)y＝ρ。In the embodiment of the present application, in order to reduce the amount of calculation, each obtained straight line is represented by a two-line parametric representation:

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=ρ.

Step S140: Jointly optimize the posture of the robot at each moment and the two-line parameterized representations of all straight lines to obtain the posture of the robot at each moment and the linear parameters of all the straight lines.

同时，假设当前时刻经过线段匹配后所得到的直线有m条，且m条直线表示为

Assume that the robot trajectory that the robot 100 needs to estimate at the current moment includes the poses at n moments

At the same time, it is assumed that there are m straight lines obtained after line segment matching at the current moment, and the m straight lines are expressed as

假设地图中第i条直线

在k时刻被机器人100观测到，那么

在机器人坐标系O中对应的观测量为

在理想状态下，对于直线上的一个点

将其通过机器人坐标系变换到世界坐标系后得到的点

应满足直线方程：Assuming that at time t=k, the position of the robot 100 in the robot coordinate system at this moment relative to the world coordinate system is

Suppose the i-th straight line in the map

Observed by the robot 100 at time k, then

The corresponding observation in the robot coordinate system O is

Ideally, for a point on the line

The point obtained by transforming it from the robot coordinate system to the world coordinate system

The straight line equation should be satisfied:

和地图直线

之间可构建残差约束：However, due to the noise in the point cloud data measurements and the estimation error of the robot pose, the observed coordinates

straight line with the map

Residual constraints can be constructed between:

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

By minimizing the sum of squares of the residual vector, the optimal estimation formula between the pose at each moment and all the straight lines can be obtained

By iteratively solving the above optimal estimation formula, the posture at each moment and the straight line parameters of all straight lines can be obtained, so as to complete the trajectory estimation and map construction of the robot 100 .

Among them, the nonlinear least squares problem can be solved iteratively by using existing calculation methods, such as Gauss-Newton algorithm (Gauss-Newton), Levenberg-Marquardt algorithm (Levenberg-Marquardt), etc. This application The specific solution method is not limited in the embodiment.

As shown in FIG. 5 , an embodiment of the present application further provides a positioning and map construction apparatus 400 . The positioning and map construction apparatus 400 may include: an extraction module 410 , an estimation module 420 , a matching module 430 and an optimization module 440 .

The extraction module 410 is used to perform linear feature extraction on the acquired point cloud data at time t to obtain a plurality of corresponding line segments;

Estimation module 420, for estimating the relative posture of the robot at time t compared to time t-1;

The matching module 430 is configured to perform line segment matching on a plurality of line segments corresponding to the time t and a plurality of line segments obtained by performing linear feature extraction at the time t-1 according to the relative posture, to obtain the belonging of each line segment. straight line;

The optimization module 440 is configured to jointly optimize the posture of the robot at each moment and the two-line parameterized representations of all straight lines to obtain the posture of the robot at each moment and the linear parameters of all the straight lines.

设置所述机器人在所述t-1时刻的姿态为

设置所述机器人在所述t时刻的机器人坐标系相对于世界坐标系的旋转矩阵及平移向量分别为

根据预先保存的机器人轮式编码器姿态估计算法，计算得到所述t时刻相较于所述t-1时刻的相对姿态

其中，

与

之间满足公式：

(x_k，y_k)表示所述机器人在t＝k时刻时相对于世界坐标系w的坐标，

表示所述机器人在t＝k时刻时相对于世界坐标系w的朝向，

为所述旋转矩阵，

为所述平移向量，v为所述机器人的线速度，ω为所述机器人的角速度，△t为所述t时刻与所述t-1时刻之间的时间间隔。In a possible implementation manner, the estimation module 420 is configured to set the posture of the robot at the time t to be

Set the posture of the robot at the time t-1 as

The rotation matrix and translation vector of the robot coordinate system relative to the world coordinate system of the robot at the time t are set as

According to the pre-saved robot wheel encoder attitude estimation algorithm, the relative attitude of the time t compared to the time t-1 is calculated and obtained

in,

and

The formula is satisfied between:

(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.

In a possible implementation manner, the matching module 430 is configured to determine the robot coordinate system formed by the robot at the time t-1 as a reference coordinate system; The coordinates of the two end points of the line segment corresponding to the time t are converted into the reference coordinate system, and the expression of the line segment corresponding to the time t in the reference coordinate system is obtained; The expressions in the reference coordinate system and the expressions of the multiple line segments corresponding to the time t-1 are matched.

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

Correspondingly, the matching module 430 is configured to, for each line segment corresponding to the time t, match the expression of the line segment in the reference coordinate system with the expression of the candidate line segment corresponding to the line segment.

In a possible implementation manner, the apparatus further includes a determining module, configured to determine that the number of candidate line segments corresponding to the line segment is not zero; when the determining module determines that it is, the apparatus further includes an adding module, which uses to determine that the line segment belongs to a new line.

计算与该线段对应的各个线段对的损失值；将所述损失值最小时所对应的线段对确定为属于同一直线；其中，d₁，d₂为该线段的两个端点到线段对中的另一线段的距离，d₀是线段对中两线段的重合区域的长度，

为该线段的长度，α是预先设定的控制因子参数，且d₁，d₂，d₀，

均由线段对中的两线段的表达式来确定。In a possible implementation manner, the matching module 430 is configured to, for each line segment corresponding to the time t, form a line segment pair with the line segment and each single line segment corresponding to the time t-1, and According to a pre-built 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; wherein, d ₁ , d ₂ are the two end points of the line segment to the line segment pair. The distance of another line segment, d ₀ 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 preset control factor parameter, and d ₁ , d ₂ , d ₀ ,

Both are determined by the expressions of the two line segments in the line segment pair.

In a possible implementation manner, the optimization module 440 is configured 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; The expression of the attitude and the expressions of all the straight lines are used to construct a residual vector; by minimizing the sum of squares of the residual vector, the optimal estimation formula between the attitude at each moment and all the straight lines is obtained. ; Iteratively solve the optimal estimation formula to obtain the posture at each moment and the straight line parameters of all straight lines.

The positioning and map construction device 400 provided by the embodiments of the present application has the same implementation principle and technical effects as the foregoing method embodiments. For the sake of brief description, the parts not mentioned in the device embodiments may be referred to in the foregoing method embodiments. corresponding content.

In addition, an embodiment of the present application further provides a computer storage medium, where a computer program is stored on the computer storage medium, and when the computer program is run by a computer, the steps included in the above positioning and map construction method are executed.

In addition, an embodiment of the present invention also provides a robot, including a processor and a memory connected to the processor, where a computer program is stored in the memory, and when the computer program is executed by the processor, the robot is made to Perform the steps involved in the positioning and map building method described above. The schematic diagram of the structure of the robot can be seen in FIG. 1 .

To sum up, the method, device, robot and computer storage medium for positioning and map construction proposed in the embodiments of the present invention perform linear feature extraction on the acquired point cloud data at time t to obtain a plurality of corresponding line segments; The relative posture at the time t compared to the time t-1; according to the relative posture, a plurality of line segments corresponding to the time t and a plurality of line segments obtained by performing linear feature extraction at the time t-1 Line segment matching is performed to obtain the straight line to which each line segment belongs; 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 all straight lines. the line parameters. Since a two-line parameterized representation is used when representing a straight line, the amount of computation during data computation can be reduced as much as possible, thereby improving data acquisition efficiency and shortening computation time.

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. For the same and similar parts among the various embodiments, refer to each other Can.

In the several embodiments provided in this application, it should be understood that the disclosed apparatus and method may also be implemented in other manners. The apparatus embodiments described above are merely illustrative, for example, the flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality and possible implementations of apparatuses, methods and computer program products according to various embodiments of the present application. operate. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code that contains one or more functions for implementing the specified logical function(s) executable instructions. It should also be noted that, in some alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It is also noted that each block of the block diagrams and/or flowchart illustrations, and combinations of blocks in the block diagrams and/or flowchart illustrations, can be implemented in dedicated hardware-based systems that perform the specified functions or actions , or can be implemented in a combination of dedicated hardware and computer instructions.

In addition, each functional module in each embodiment of the present application may be integrated together to form an independent part, or each module may exist independently, or two or more modules may be integrated to form an independent part.

If the functions are implemented in the form of software function modules and sold or used as independent products, they may be stored in a computer storage medium. Based on this understanding, the technical solution of the present application can be embodied in the form of a software product in essence, or the part that contributes to the prior art or the part of the technical solution. The computer software product is stored in a storage medium, including Several instructions are used to cause a computer device (which may be a personal computer, a notebook computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, Read-Only Memory (ROM, Read-Only Memory), Random Access Memory (RAM, Random Access Memory), magnetic disk or optical disk and other media that can store program codes .

The above are only specific embodiments of the present application, but the protection scope of the present application is not limited to this. should be covered within the scope of protection of this application.

Claims (10)

1. a positioning and map construction method, is characterized in that, described method comprises: Perform linear feature extraction on the acquired point cloud data at time t to obtain a plurality of corresponding line segments; Estimate the relative posture of the robot at the time t compared to the time t-1; According to the relative posture, line segment matching is performed on a plurality of line segments corresponding to the time t and a plurality of line segments obtained by performing linear feature extraction at the time t-1 to obtain the straight line to which each line segment belongs; The posture of the robot at each moment and the two-line parameterized representations 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. 2. method according to claim 1, is characterized in that, the relative posture of described estimated robot at described t moment compared to t-1 moment, comprises: Set the posture of the robot at the time t as

Set the posture of the robot at the time t-1 as

The rotation matrix and translation vector of the robot coordinate system relative to the world coordinate system of the robot at the time t are set as

According to the pre-saved robot wheel encoder attitude estimation algorithm, the relative attitude of the time t compared to the time t-1 is calculated and obtained

in,

and

The formula is satisfied between:

(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. 3 . The method according to claim 1 , wherein the line segment matching is performed on a plurality of line segments corresponding to the time t and a plurality of line segments obtained by performing linear feature extraction at the time t-1, 3 . include: determining that the robot coordinate system formed by the robot at the time t-1 is the reference coordinate system; According to the relative attitude, the coordinates of the two end points 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 the multiple line segments corresponding to the time t-1 are matched. 4. The method according to claim 3, wherein, in the expression of the line segment corresponding to the time t in the reference coordinate system and a plurality of line segments corresponding to the time t-1 Before matching the expression, the method further includes: For each line segment corresponding to the time t, the difference line segment is filtered from all the line segments corresponding to the time t-1 according to a predetermined attribute feature and a pre-stored attribute feature threshold, and the difference line segment is compared with the time t- The remaining line segments corresponding to time 1 are determined as candidate line segments corresponding to the line segment; Correspondingly, the matching of 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 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. 5 . The method according to claim 4 , wherein, 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 comprises: 6 . include: It is determined that the number of candidate line segments corresponding to the line segment is not zero; Otherwise, the method further includes: Make sure that the line segment belongs to the new line. 6. The method according to claim 3 or 4, characterized in that, the expression of the line segment corresponding to the time t in the reference coordinate system and a plurality of expressions corresponding to the time t-1 Line segment expressions are matched, including: For each line segment corresponding to the time t, a line segment pair is formed between the line segment and each single line segment corresponding to the time t-1, and according to the pre-built loss function

Calculate the loss value of each line segment pair corresponding to the line segment; Determining the pair of line segments corresponding to the minimum loss value as belonging to the same straight line; Among them, d ₁ , d ₂ are the distances from the two end points of the line segment to the other line segment in the line segment pair, d ₀ 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 preset control factor parameter, and d ₁ , d ₂ , d ₀ ,

Both are determined by the expressions of the two line segments in the line segment pair. 7. The method according to claim 1, characterized in that, by jointly optimizing the posture of the robot at each moment and the two-line parameterized representations of all straight lines, the robot's posture at each moment is obtained. Attitude and line parameters for all lines, including: Set the expression of the attitude of the robot at the various moments; Set the expressions of all straight lines obtained after the line segment is matched; According to the expression of the attitude and the expression of all the straight lines, a residual vector is constructed; By minimizing the sum of squares of the residual vector, the 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. 8. A positioning and map construction device, wherein the device comprises: The extraction module is used to perform linear feature extraction on the acquired point cloud data at time t to obtain a plurality of corresponding line segments; an estimation module for estimating the relative posture of the robot at the time t compared to the time t-1; The matching module is configured to perform line segment matching on the multiple line segments corresponding to the time t and the multiple line segments obtained by performing linear feature extraction at the time t-1 according to the relative posture, and obtain the corresponding line segment to which each line segment belongs. straight line; The optimization module is used to jointly optimize the posture of the robot at each moment and the two-line parameterized representations of all straight lines to obtain the posture of the robot at each moment and the linear parameters of all the straight lines. 9. A robot, comprising: a memory and a processor, wherein the memory is connected to the processor; the memory is used to store programs; The processor invokes a program stored in the memory to perform the method of any of claims 1-7. 10. A computer storage medium, characterized in that a computer program is stored thereon, and the computer program executes the method according to any one of claims 1-7 when the computer program is run by a computer.

Images