CN114581519A - Laser autonomous positioning mapping method for quadruped robot in cross-country environment - Google Patents

Abstract

The invention discloses a quadruped robot laser autonomous positioning mapping method in a cross-country environment, which comprises the steps of obtaining laser radar original point cloud in a field environment, extracting less than 100 three-dimensional feature points on the basis of the original point cloud for registration, focusing alternative three-dimensional feature points into edge points and plane points, and extracting the edge points and the plane points according to the values; after the three-dimensional characteristic points are obtained, registering the two frames of point clouds; establishing a geometric relation between the related point clouds, performing positioning work of the inspection device, and performing motion compensation on distortion generated by laser radar motion; the redundant parameters are effectively processed through an L-M algorithm, and the back end carries out nonlinear optimization state estimation by utilizing the L-M algorithm; when a certain number of point clouds are accumulated, the map can be built, and the point cloud data is integrated into a world map.

Description

Laser autonomous positioning mapping method for quadruped robot in cross-country environment

Technical Field

The invention belongs to the technical field of unmanned systems, and particularly relates to a laser autonomous positioning mapping method for a quadruped robot in a cross-country environment, a storage medium and electronic equipment.

Background

With the maturity of the Simultaneous Localization and Mapping (SLAM) technology of robots, more and more scholars are dedicated to applying the SLAM technology to field unmanned system inspection tasks, for example, n.b. joseph and others test several SLAM algorithms for desert inspectors, and propose a three-dimensional landform model generated by utilizing the SLAM algorithms to design an inspector reachable domain; affan Shaukat et al uses the obstacles detected by the binocular camera of the patrol instrument to form a feature combination, and designs a complete autonomous positioning algorithm under an EKF-SLAM framework, and the method is worth taking reference that the navigation road sign is regarded as a feature point combination representing the obstacle, and compared with the traditional positioning method, the success rate of feature recognition is improved; mark Woods and the like analyze and summarize the working mode and environment of a Mars rover in the past generation, design a Mars desert field autonomous positioning experiment based on an SLAM technology aiming at a future Mars surface patrol task (ExoMars2.0) in the European and aviation administration, and verify the effectiveness of an SLAM algorithm under the working conditions of long-term and insufficient communication of the Mars rover.

The SLAM problem can be simply summarized as how to position the robot in an unknown environment and use the measurement information of the sensor to perform self-positioning and surrounding environment description. Dissanayake et al established an initial framework for mobile robot navigation in the last 80 th century, proposed Kalman Filter based KF-SLAM navigation algorithms, and S.Thrun et al reviewed KF-SLAM algorithms. Montemello et al proposed a famous Fast-SLAM algorithm in 2002, proposed the idea of using Particle filter (Particle filter) to estimate the robot state, extended Kalman filtering to estimate map features, and referred to Rao-Blackwellize estimation structure to attribute EKF map estimates to individual particles. Montemerlo and S.Thrun further perfected the FastSLAM algorithm in 2003, and proposed that the Probability Density of measurement (Probability Density Function) is introduced to re-weight in the state prediction process, and the new FastSLAM 2.0 algorithm can achieve better estimation effect even when 1 particle is used. In recent years, with the improvement of computer computing power, the SLAM method based on graph optimization gradually becomes a research hotspot, and some excellent algorithms such as TreeMap, TORO, HOG _ MAN, G2O and the like are all superior to the SLAM method based on a filter system, but the calculation amount of the methods is significantly increased along with the increase of time, and the methods are still in offline optimization processing at present, and for the problem, Kaess et al propose an incremental smooth graph building method imam (incremental Smoothing and mapping). When calculating the nonlinear optimization problem, the iSAM carries out QR decomposition on the information matrix, and when new measurement information arrives, the algorithm updates the information matrix in an incremental manner by applying the matrix information of the previous step, so that recalculation of all related elements is avoided, and the algorithm efficiency is improved.

The core of the SLAM algorithm is to control accumulated errors through repeated feature re-observation (re-visualization), and a peripheral map can be output while accurate positioning is carried out, so that the SLAM algorithm has potential application value in field exploration navigation tasks. However, the existing autonomous positioning mapping method lacks robustness in a single field environment, so that the state estimation error rate is high, and the detection accuracy needs to be improved.

Disclosure of Invention

The purpose of the invention is as follows: in order to overcome the defects, the invention aims to provide a quadruped robot laser autonomous positioning mapping method in a cross-country environment, which can enable the robot to autonomously perform pose state estimation, positioning and navigation on the surrounding environment, improve the target detection precision and complete the patrol exploration task of the quadruped robot in the field environment.

The technical scheme is as follows: in order to achieve the technical effects, the invention provides a laser autonomous positioning mapping method for a quadruped robot in a cross-country environment, which comprises the following steps: the real-time positioning and mapping are decomposed into two threads, wherein the first thread is a high-frequency thread, the patrol device motion estimation is mainly carried out at 10Hz, IMU mileage information is fused, and the second thread is a low-frequency thread and is mainly responsible for local mapping;

the specific mapping method comprises the following steps:

s1: acquiring laser radar original point cloud under a field environment, extracting less than 100 three-dimensional feature points on the basis of the original point cloud for registration, focusing the alternative three-dimensional feature points into edge points and plane points, and extracting the edge points and the plane points according to the value of c;

s2: after the three-dimensional characteristic points are obtained, registering the two frames of point clouds;

s3: establishing a geometric relation between the related point clouds, positioning the inspection device, and performing motion compensation on a distortion problem generated by the motion of the laser radar;

s4: redundant parameters are effectively processed through a Levenberg-Marquardt algorithm, nonlinear optimization state estimation is carried out at the rear end by utilizing the Levenberg-Marquardt algorithm, and the Levenberg-Marquardt algorithm is not sensitive to an over-parameterization problem and can effectively process the redundant parameter problem, so that the condition that an optimization function falls into a local minimum value is greatly reduced;

s5: when a certain number of point clouds are accumulated, mapping can be carried out, the pose of the laser radar in a world coordinate system is accurately estimated, the point cloud data are integrated into a world map, and the operation frequency of an algorithm adopted for mapping is lower than that of a mileage calculation method.

According to the laser autonomous positioning mapping method for the quadruped robot in the off-road environment, less than 100 three-dimensional feature points are extracted for registration on the basis of the original point cloud in S1, the number of the selected points is small, the processing of large-scale point cloud by similar ICP algorithm can be effectively avoided, the data accuracy is improved, and meanwhile, the workload can be reduced; .

The invention relates to a laser autonomous positioning mapping method for a quadruped robot in a cross-country environment, which specifically comprises the following steps of extracting edge points and plane points according to the value of c in step S1:

wherein i is p_kA point of (1), p_kFor the point cloud obtained by one scan of the laser scanner, i ∈ p_kS is a set of continuous points near i in the same line scanning of the laser scanner, the S comprises half of points on two sides of i, and | S | is the number of the points;

modulo the normalized vector sum of points in i and S;

the modulus of the edge point vector sum is large, while the modulus of the plane point vector sum is small;

the points in the scan are sorted based on the c-value, and then the largest c-value and the smallest c-value are selected as feature points, wherein the largest c-value is an edge point, and the smallest c-value is a plane point.

In the autonomous laser positioning and mapping method for the quadruped robot in the off-road environment, when the point i is selected as the edge point or the plane point in the process of extracting the edge point and the plane point through the value of c in S1, the following conditions are also required to be met:

in order to uniformly distribute the characteristic points, the data obtained by each scanning needs to be uniformly divided into four regions, each region contains 2 edge points and 4 plane points at most, the number of the selected edge points or plane points cannot exceed the maximum value of the sub-region, the points in 10 points in the neighborhood around the point i cannot be selected again, and the point i cannot be on a plane which is approximately parallel to the laser beam and cannot be on the boundary of the shielded region;

assuming that point a is on a curved surface at an angle to the laser beam and point B is on a curved surface substantially parallel to the laser beam, point B is an unreliable laser echo and is not selected as a feature point, a real line segment is an object observable by the laser, and point a is on the boundary of the occlusion region and may be detected as an edge point, but if viewed from a different angle, the occlusion region may change and become observable, point a is not a significant edge point and may not be selected as a feature point.

The invention relates to a laser autonomous positioning mapping method for a quadruped robot in a cross-country environment, which comprises the following specific steps of registering two frames of point clouds in step S2:

1)：t_kdenotes the time at which the k-th scan starts, t_k+1The point cloud p obtained by the scanning is used as the scanning end time_kProjection to time stamp t after distortion correction_k+1The above step (1);

2): recording the point cloud after projection as p_k；

3): p will be used at the k +1 scan_kAnd newly acquired point cloud p_k+1Performing registration to estimate the movement of the lidar;

4): at the end of the k-th scan, p is added_kReprojection of time stamp t_k+1To obtain p_kWherein p is_kIs the point cloud perceived during the kth scan;

5): during the k +1 th scan, p_kAnd a newly perceived point cloud p_k+1Together for estimating the movement of the lidar;

6): for p_k+1Extracting characteristic points from the laser radar point cloud, and recording epsilon_k+1And H_k+1Respectively, the edge points and the plane points of the collection;

7): then find from p_kHas an edge line of as ∈_k+1Finding a plane as H according to the corresponding relation of the midpoints_k+1The corresponding relation of the midpoints;

8): transform using current estimate will be epsilon_k+1And H_k+1Re-projecting to coordinate system of scanning starting point, recording

And

the point set is re-projected, so that the pose transformation information of all points in the point cloud of the (k + 1) th frame can be obtained;

9): for the

And

will be at each point in

The corresponding relation is found out in the step (A),

the characteristic points of (2) are stored in a 3D KD-tree, which is commonly used as a range search and a nearest neighbor search.

The invention relates to a quadruped robot laser autonomous positioning mapping method in a cross-country environment, which is characterized in that two frames of point clouds are registered in step S2, and the specific method for registering three-dimensional edge feature points is as follows:

let i be

At the point of one of (a) and (b),

two points need to be found to represent the edge line, first at

Finding the point nearest to i, marking as j, then finding the point nearest to i in two continuous scans of the adjacent line of j, marking as l, then (j, l) forming the corresponding relation of i, wherein the nearest neighbor point is selected and used as the nearest neighbor index of KD-tree, in order to avoid the situation that the three points of i, j and l are collinear to cause no solution, j and l must come from the scans of different lines, and for the point

If (j, l) is the corresponding edge line,

the distance of the point to the edge line can be calculated as:

wherein

And

the coordinates of the points i, j and L in { L } respectively, and finding an edge line as

J is the point closest to the feature point, at

Finding a first line representing the same scan as j, a second line representing two consecutive scans, finding another point l on the second line for finding the correspondence of the edge line, and representing the correspondence as (j, l); geometric constraint relation constructed by three points i, j and l, d_εIs the distance from the point to the edge line.

The invention relates to a quadruped robot laser autonomous positioning mapping method in a cross-country environment, which is characterized in that two frames of point clouds are registered in step S2, and the specific method for registering three-dimensional plane feature points is as follows: let i be

At the point of one of (a) and (b),

find three points to represent a plane, first at

Finding the point nearest to i, marking as j, then marking the other two points as l and m, which are the points nearest to i, i is in the same line scanning with j, m is the point in two continuous scans of the adjacent line of j, thus ensuring that the three points are non-collinear, and calculating the distance between the plane point and the corresponding plane for the point

If (j, l, m) is the corresponding plane,

the point-to-plane distance is then:

wherein

Is the coordinate of the mid-point m of { L }, find a plane as

J is the point closest to the feature point and can be in

Finding out the points l and m, and respectively finding out other two points l and m on different lines, wherein the corresponding relation is expressed as (j, l, m); the geometric constraint relation constructed by four points of i, j, l and m in the process of matching the characteristics of the plane points, d_HIs the point-to-plane distance;

after the corresponding relation of the characteristic points is obtained, the positioning work of the patrol instrument is carried out, the carrier is assumed to have constant angular speed and linear speed in a short time in the scanning process of the laser radar, the linear interpolation posture conversion can be carried out on the points received at different times in the scanning process,

wherein, t_k+1Is the time at which the (k + 1) th scan starts, t_k+2To be the time at which the scanning is finished,

is [ t ]_k+1,t_k+2]The laser radar attitude change between the two modes,

including six-degree-of-freedom rigid movement of the laser radar,

wherein t is_x,t_yAnd t_zIs a translation along the x-, y-and z-axes of { L }, and θ_x,θ_yAnd theta_zIs the angle of rotation, t, following the right hand rule_iIs the time stamp of the point i,

is [ t ]_k+1,t_i]And the attitude of the laser radar is changed.

The invention relates to a quadruped robot laser autonomous positioning mapping method under a cross-country environment, wherein a geometric relationship is established between related point clouds in step S3, so that a specific method for performing motion compensation comprises the following steps:

from the transformation in equation (4), it can be derived,

wherein,

is epsilon_k+1Or H_k+1The coordinates of the middle point i are,

is that

And

the corresponding point of (i) in (1),

is that

R is a rotation matrix defined by the Rodrigues equation, facilitating derivation:

in the above formula, θ is the rotation amplitude

ω is a unit vector representing the direction of rotation

Omega is

Is determined by the skew-symmetric matrix of (a),

by combining equation (2) and equations (4) - (8), ε can be obtained_k+1The edge points in (b) and the corresponding edge lines,

similarly, in combination with formula (3) and formulas (4) - (8), can be at H_k+1Another geometrical relationship is established between the plane points in (a) and the corresponding planes,

for epsilon_k+1Or H_k+1The iteration of equations (8) and (9) for each feature point in (a) can obtain a non-linear function,

where each row of f corresponds to a feature point and d contains the corresponding distance. In this case, equation (11) has only one unknown quantity

Then, a Levenberg-Marquardt method is used for iterative optimization, and the aim is to find an optimal pose transformation

The distance d is minimized in such a way that,

first, calculate the relative

The Jacobian matrix of f, denoted J, where

Then, d is minimized to zero by iteration through equation (12),

where λ is a penalty factor determined by Levenberg-Marquardt.

The invention relates to a quadruped robot laser autonomous positioning mapping method under a cross-country environment, wherein in the step S4, a Levenberg-Marquardt algorithm is used for nonlinear optimization state estimation at the rear end, and the specific steps are as follows:

1): assuming that a nonlinear least square problem is researched, an optimization function is set, and an iteration step size of a Levenberg-Marquardt algorithm is defined:

where f (x) is an optimization function, the Levenberg-Marquardt algorithm defines an iteration step size of,

h_lm＝-(J^TJ+μI)^-1g,g＝J^Tf and mu is more than or equal to 0 (14)

Where, f ═ f (x), J ═ J (x), is the jacobian matrix of f, μ is a penalty factor, and it has the following effects:

(1) when μ > 0, all coefficient matrices are positive, which ensures that h_lmRepresents the descending direction of f (x), i.e. each step h_lmWill result in a decrease in the value of f (x);

(2) when the value of μ is large, it is preferable that,

h_lmdegenerating to an iteration step size of a steepest descent (steepbest) algorithm;

(3) if the value of mu is small or equal to zero,

h_lm＝-(J^TJ)^-1g (16)

h_lmthe method has the advantages that the method is degraded into the iteration step length of the Gauss-Newton algorithm, is suitable for the final stage of iteration, is very close to the optimal solution, and avoids the oscillation of the steepest descent;

it follows that μ affects both the direction of descent of the function and the size of the iteration step, and in order to minimize the optimization function, μ should be set to a small value at the start of the iteration and the size of μ and the matrix a should be set to₀＝J^T(x₀)J(x₀) The size of the middle element is related to that:

where tau is selected by the user on a case-by-case basis,

is a matrix A₀The elements of (1);

2): in an iterative process, the confidence domain method is a common method for updating μ, and defines a gain ratio Q to control the update, as follows:

here, F (x) -F (x + h)_lm) Is F (x) the actual reduction value of iteration at step k, L (h)_lm) Is F (x + h)_lm) Is a function of the linear approximation of (c),

denominator L (0) -L (h)_lm) A decreasing value for the linear model prediction, which is constantly zero;

if Q is larger, indicate L (h)_lm) Is F (x + h)_lm) Better approximation can reduce mu, so that the algorithm is closer to the gauss-newton algorithm in the next iteration, if Q is very small or negative, the approximation effect is poor, mu needs to be increased to reduce the iteration step length, and at the moment, the algorithm is closer to the steepest descent method, specifically:

(1) when Q > 0, this iteration is valid

(2) When Q is less than or equal to 0, the iteration is invalid, and a parameter v is introduced to update:

μ＝μ×v，v＝2×v (21)

and finally, judging whether to finish iteration according to an iteration termination condition, wherein the method has two specific forms:

(1) one is that the gradient g satisfies

||g||_∞≤ε₁ (22)

Here epsilon₁A defined threshold, positive, meaning that when the rate of iterative descent is almost zero, it is considered to have converged to near the extreme point;

(2) alternatively, when the amount of change in the variable x is too small, the iteration is considered to converge,

||x_new-x||＜ε₂(||x||+ε₂) (23)

here,. epsilon₂A positive number greater than zero, defined by the user;

3): defining a maximum number of iterations k_maxWhen the iteration number k is more than or equal to k_maxThe iteration is stopped to avoid the situation of infinite loop.

The invention relates to a quadruped robot laser autonomous positioning mapping method in a cross-country environment, which can map when accumulating a certain number of point clouds in step S5 and blend the point cloud data into a world map, and the specific method is as follows:

at the end of the (k + 1) th frame of point cloud scanning, a frame of undistorted point cloud can be obtained through a laser radar odometer,

and a pose change

It is contained in [ t_k+1,t_k+2]The mapping algorithm will be paired under the world coordinate system { W }, due to the movement of the laser radar in between

And (6) carrying out registration.

Herein is defined as Q_kFor the projection of all point clouds accumulated to the k-th frame under W,

is the last time t of the kth scanning of the laser radar on the map_k+1The mapping algorithm will follow the output of the laser radar odometer

From t_k+1Extension to t_k+2Thereby obtaining

Then pass through

Handle

Projected into the world coordinate System { W }, denoted

Then through registration

And Q_kTo further optimize radar pose

The invention relates to a laser autonomous positioning mapping method for a quadruped robot in a cross-country environment, which can optimize the position and pose of a radar

The method comprises the following steps:

representing the pose of the radar on a map

It is generated by the mapping algorithm at the kth frame;

radar motion between k +1 th frames

It is calculated by a mileage calculation method;

through

And

the distorted point cloud issued by the mileage calculation method can be mapped on a map and defined as

And matched to an existing map Q_kThe above step (1);

the definition and use of the mapping node for the feature points are the same as those of a mileage calculation method, but the number of the mapping node is more than ten times that of the mileage calculation method, so that a covariance matrix is introduced to find the corresponding relation of the feature points:

firstly, Q is put in_kStored in a cube with a side length of 10m, will be combined with

Extracting points in the intersected cube and storing the points in the 3D KD-tree;

then, using a 3D KD-tree to index, and finding a group of point cloud data of the adjacent characteristic points, and recording the point cloud data as S'; according to the characteristic of the covariance matrix, the coordinate distribution of the three-dimensional points of one area and the eigenvalue and the eigenvector of the covariance matrix formed by the coordinate distribution can be known to have a certain relation;

then calculating a covariance matrix of S', recording the covariance matrix as M, and respectively recording the eigenvalue and the eigenvector of M as V and V; for a line approximated by V, which contains a feature value significantly larger than the other two, the corresponding feature vector is the line vector of the midpoint of S';

for S 'on an approximate plane, V contains two larger eigenvalues, the third eigenvalue is significantly smaller, and the eigenvector associated with the smallest eigenvalue in E is the normal vector of the plane on which the midpoint of S' lies. Determining the position of the edge line or plane through the geometric center of S';

thus, an edge line corresponding to a lower edge point of the world coordinate system is found, and a plane corresponding to the plane point is found;

next, selecting two points on the edge line, selecting three points on the plane, calculating the distance by using the formula same as the formula (2) and the formula (3), and then performing iterative optimization, wherein the method used in the method is the same as the method used in the odometer node;

finally, the obtained optimal transformation is utilized

Will be provided with

Superposing the map again to the map;

to evenly distribute the points, the size of the map cloud will be reduced by a voxel grid filter, the voxel size being a 5cm cube.

The technical scheme shows that the invention has the following beneficial effects:

1. the autonomous laser positioning and mapping method for the quadruped robot in the cross-country environment can autonomously estimate the pose state of the laser radar in the cross-country environment, enables the robot to autonomously estimate, position and navigate the pose state of the surrounding environment, improves the target detection precision, and completes the patrol exploration task of the quadruped robot in the cross-country environment.

2. The laser autonomous positioning and mapping method for the quadruped robot in the off-road environment, provided by the invention, has the advantages that the motion distortion generated by point cloud is assumed in the scanning process of the laser radar, the carrier has constant angular velocity and linear velocity in a short time, the linear interpolation attitude transformation can be carried out on points received at different times in scanning, the motion compensation is carried out, and the problem that the points in the point cloud can generate motion distortion along with the fluctuation motion of the laser radar when a vehicle cannot run stably due to a rugged road condition by adopting real-time mileages is solved.

Drawings

FIG. 1 is a flowchart for autonomous positioning mapping of a quadruped robot based on a laser radar in the invention;

FIG. 2 is a schematic diagram of feature point screening according to the present invention;

FIG. 3 is a schematic diagram of the time history of the point cloud reprojection according to the present invention;

FIG. 4 is a schematic diagram of the edge point feature matching process of the present invention, wherein (a) shows three points being non-collinear, and (b) shows the geometric relationship of the members;

FIG. 5 is a schematic diagram of a planar point feature matching process in the present invention, wherein (a) shows three points being non-collinear, and (b) shows a geometric relationship of the structure;

FIG. 6 is a schematic diagram of a laser radar mapping algorithm process according to the present invention;

FIG. 7 is a diagram illustrating the mapping result of the lidar-based system of the present invention.

Detailed Description

The invention is further elucidated with reference to the drawings and the embodiments.

Examples

Reference will now be made in detail to embodiments of the present invention, examples of which are illustrated in the accompanying drawings, wherein like or similar reference numerals refer to the same or similar elements or elements having the same or similar function throughout. The embodiments described below with reference to the drawings are illustrative and intended to be illustrative of the invention and are not to be construed as limiting the invention.

In the description of the present invention, it is to be understood that the terms "center", "longitudinal", "lateral", "length", "width", "thickness", "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "clockwise", "counterclockwise", and the like, indicate orientations and positional relationships based on those shown in the drawings, and are used only for convenience of description and simplicity of description, and do not indicate or imply that the device or element being referred to must have a particular orientation, be constructed and operated in a particular orientation, and thus, are not to be considered as limiting the present invention.

Furthermore, the terms "first", "second" and "first" are used for descriptive purposes only and are not to be construed as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. In the description of the present invention, unless otherwise specified, "a plurality" means two or more unless explicitly defined otherwise.

In the present invention, unless otherwise expressly specified or limited, the terms "mounted," "connected," "secured," and the like are to be construed broadly and can, for example, be fixedly connected, detachably connected, or integrally connected; can be mechanically or electrically connected; they may be connected directly or indirectly through intervening media, or they may be interconnected between two elements. The specific meanings of the above terms in the present invention can be understood by those skilled in the art according to specific situations.

In the present invention, unless otherwise expressly stated or limited, "above" or "below" a first feature means that the first and second features are in direct contact, or that the first and second features are not in direct contact but are in contact with each other via another feature therebetween. Also, the first feature being "on," "above" and "over" the second feature includes the first feature being directly on and obliquely above the second feature, or merely indicating that the first feature is at a higher level than the second feature. A first feature being "under," "below," and "beneath" a second feature includes the first feature being directly under and obliquely below the second feature, or simply meaning that the first feature is at a lesser elevation than the second feature.

A laser autonomous positioning mapping method of a quadruped robot in a cross-country environment is shown in the figure, and comprises the following steps: the real-time positioning and mapping are decomposed into two threads, wherein the first thread is a high-frequency thread, the patrol device motion estimation is mainly carried out at 10Hz, IMU mileage information is fused, and the second thread is a low-frequency thread and is mainly responsible for local mapping;

the specific mapping method comprises the following steps:

s1: acquiring laser radar original point cloud under a field environment, extracting less than 100 three-dimensional feature points on the basis of the original point cloud for registration, focusing the alternative three-dimensional feature points into edge points and plane points, and extracting the edge points and the plane points according to the value of c;

s2: after the three-dimensional characteristic points are obtained, registering the two frames of point clouds;

s3: establishing a geometric relation between the related point clouds, positioning the inspection device, and performing motion compensation on a distortion problem generated by the motion of the laser radar;

s4: redundant parameters are effectively processed through a Levenberg-Marquardt algorithm, and nonlinear optimization state estimation is carried out at the rear end by utilizing the Levenberg-Marquardt algorithm;

s5: when a certain number of point clouds are accumulated, mapping can be carried out, the pose of the laser radar in a world coordinate system is accurately estimated, point cloud data are fused into a world map, and the operation frequency of an algorithm adopted for mapping is lower than that of a mileage calculation method.

In the method for establishing a map by autonomous laser positioning of a quadruped robot in an off-road environment in this embodiment, the specific method for extracting edge points and plane points through the value of c in step S1 is as follows:

wherein i is p_kA point of (1), p_kFor the point cloud obtained by one scan of the laser scanner, i ∈ p_kS is a set of continuous points near i in the same line scanning of the laser scanner, the S comprises half of points on two sides of i, and | S | is the number of the points;

modulo the normalized vector sum of points in i and S;

the modulus of the edge point vector sum is large, while the modulus of the plane point vector sum is small;

the points in the scan are sorted based on the c-value, and then the largest c-value and the smallest c-value are selected as feature points, wherein the largest c-value is an edge point, and the smallest c-value is a plane point.

In the laser autonomous positioning mapping method for the quadruped robot in the off-road environment described in this embodiment, in the process of extracting the edge point and the plane point by using the value of c in S1, when the point i is selected as the edge point or the plane point, the following conditions need to be satisfied:

in order to uniformly distribute the characteristic points, the data obtained by each scanning needs to be uniformly divided into four regions, each region contains 2 edge points and 4 plane points at most, the number of the selected edge points or plane points cannot exceed the maximum value of the sub-region, the points in 10 points in the neighborhood around the point i cannot be selected again, and the point i cannot be on a plane which is approximately parallel to the laser beam and cannot be on the boundary of the shielded region;

assuming that point a is on a curved surface at an angle to the laser beam and point B is on a curved surface substantially parallel to the laser beam as shown in fig. 2, point B is an unreliable laser echo and is not selected as a feature point, (B) a real line segment is an object observable by the laser, and point a is on the boundary of an occlusion region (a dotted line segment) and may be detected as an edge point, but if viewed from a different angle, the occlusion region may change and become observable, and point a is not a significant edge point and is not selected as a feature point.

After the point cloud features are extracted and stored in the map, the further scanned point clouds of the same name need to be effectively matched, and after the feature points are obtained in step S2, a specific method for registering the two frames of point clouds is as follows:

1)：t_kdenotes the time at which the k-th scan starts, t_k+1The point cloud p obtained by the scanning is used as the scanning end time_kProjection to time stamp t after distortion correction_k+1The above step (1);

2): the projected point cloud is recorded as figure 3

3): will be used at the k +1 th scan

And newly acquired point cloud p_k+1Performing registration to estimate the movement of the lidar;

4): as shown in fig. 3, where the first line segment represents the point cloud p perceived during the k-th scan_kAt the end of the kth scan, p is added_kReprojection of time stamp t_k+1To obtain

(second line segment) where p_kIs the point cloud perceived during the kth scan;

5): during the (k + 1) th scan, p_kAnd a newly perceived point cloud p_k+1(third line segments) together used to estimate the movement of the lidar;

6): for p_k+1Extracting characteristic points from the laser radar point cloud, and recording epsilon_k+1And H_k+1Respectively, the edge points and the plane points of the image are collected;

7): then find out from

Has an edge line of as ∈_k+1Finding a plane as H according to the corresponding relation of the midpoints_k+1The corresponding relation of the midpoints;

8): to facilitate processing and correction of lidar motion distortion, epsilon is transformed using the current estimate_k+1And H_k+1Projecting the coordinate system of the scanning starting point again to record

And

the point set is re-projected, so that pose transformation information of all points in the point cloud of the (k + 1) th frame can be obtained;

9): for the

And

will be at each point in

The corresponding relation is found out in the step (A),

the characteristic points of (2) are stored in a 3D KD-tree, which is commonly used as a range search and a nearest neighbor search.

In this embodiment, in step S2, two frames of point clouds are registered, and the specific method for registering the three-dimensional edge feature points is as follows:

let i be

At the point of one of (a) and (b),

two points need to be found to represent the edge line, first at

Finding out the point nearest to i, marking as j, then finding out the point nearest to i in two continuous scans of the adjacent line of j, marking as l, then forming the corresponding relation of i (j, l), wherein the nearest neighbor point is selected by using the nearest neighbor index of KD-tree, in order to avoid that the three points of i, j and l are collinear to cause no solution, j and l must come from the scans of different lines, and for the point

If (j, l) is the corresponding edge line,

the distance of the point to the edge line can be calculated as:

wherein

And

the coordinates of points i, j and L in { L }, respectively, as shown in FIG. 4, where (a) finds an edge line as

J is the point closest to the feature point, at

Finding a first line representing the same scan as j and a second line representing two consecutive scans, finding another point l on the second line in order to find the correspondence of the edge line in (a), and representing the correspondence as (j, l); geometric constraint relation constructed by three points i, j and l, d_εIs the distance from the point to the edge line.

In this embodiment, the two frames of point clouds are registered in step S2, and the specific method for registering the three-dimensional plane feature points is as follows:

as shown in FIG. 5, let i be

At the point of one of (a) and (b),

find three points to represent a plane, first at

Find the point closest to i, denoted as j, then, note the other two points as l and m, which are the points nearest to i, l is in the same line scan as j, m is the point in two consecutive scans of the adjacent line to j, which ensures thatThe three points are non-collinear, and similarly, the distance between a plane point and a corresponding plane is calculated, and for the point

If (j, l, m) is the corresponding plane,

the point-to-plane distance is then:

wherein

Is the coordinate of the mid-point m of { L }, as shown in FIG. 5, the process in (a) finds a plane as

J is the point closest to the feature point and can be in

Then finding out another two points l and m on different lines of the third line and the fourth line respectively, wherein the corresponding relation is expressed as (j, l, m); the geometric constraint relationship constructed by i, j, l, m four points in the plane point feature matching process is shown as (b) in fig. 5, d_HIs the point-to-plane distance;

after the corresponding relation of the characteristic points is obtained, the positioning work of the patrol instrument is carried out, the carrier is assumed to have constant angular speed and linear speed in a short time in the scanning process of the laser radar, the linear interpolation posture conversion can be carried out on the points received at different times in the scanning process,

wherein, t_k+1Is the time at which the (k + 1) th scan starts, t_k+2To be the time at which the scanning is finished,

is [ t ]_k+1,t_k+2]The laser radar posture is changed between the two modes,

including six-degree-of-freedom rigid movement of the laser radar,

wherein t is_x,t_yAnd t_zIs a translation along the x-, y-and z-axes of { L }, and θ_x,θ_yAnd theta_zIs the angle of rotation, t, following the right hand rule_iIs the time stamp of the point i,

is [ t ]_k+1,t_i]And the attitude of the laser radar is changed.

In this embodiment, the specific method for establishing a geometric relationship between the related point clouds in step S3 and performing motion compensation is as follows:

from the transformation in equation (4), it can be derived,

wherein,

is epsilon_k+1Or H_k+1The coordinates of the middle point i are,

is that

And

the corresponding point of (i) in (1),

is that

R is a rotation matrix defined by the Rodrigues equation, facilitating derivation:

in the above formula, θ is the rotation amplitude

ω is a unit vector representing the direction of rotation

Omega is

Is determined by the skew-symmetric matrix of (a),

by combining equation (2) and equations (4) - (8), ε can be obtained_k+1The edge points in (b) and the corresponding edge lines,

similarly, in combination with formula (3) and formulas (4) - (8), can be at H_k+1Another geometrical relationship is established between the plane points in (a) and the corresponding planes,

for epsilon_k+1Or H_k+1The iteration of equations (8) and (9) for each feature point in (a) can obtain a non-linear function,

where each row of f corresponds to a feature point and d contains the corresponding distance. In this case, equation (11) has only one unknown quantity

Then, a Levenberg-Marquardt method is used for iterative optimization, and the aim is to find an optimal pose transformation

The distance d is minimized in such a way that,

first, calculate the relative

The Jacobian matrix of f, denoted J, where

Then, d is minimized to zero by iteration through equation (12),

where λ is a penalty factor determined by Levenberg-Marquardt.

In the step S4 in this embodiment, a Levenberg-Marquardt algorithm is used at the back end to perform nonlinear optimization state estimation, and the specific steps are as follows:

1): assuming that a nonlinear least square problem is researched, an optimization function is set, and an iteration step size of a Levenberg-Marquardt algorithm is defined:

where f (x) is an optimization function, the Levenberg-Marquardt algorithm defines an iteration step size of,

h_lm＝-(J^TJ+μI)^-1g,g＝J^Tf and mu is more than or equal to 0 (14)

Where, f ═ f (x), J ═ J (x), is the jacobian matrix of f, μ is a penalty factor, and it has the following effects:

(1) when μ > 0, all coefficient matrices are positive, which ensures that h_lmRepresents the descending direction of f (x), i.e. each step h_lmWill result in a decrease in the value of f (x);

(2) when the value of μ is large, it is preferable that,

h_lmdegenerating to an iteration step size of a steepest descent (steepest gradient) algorithm;

(3) if the value of mu is small or equal to zero,

h_lm＝-(J^TJ)^-1g (16)

h_lmthe method has the advantages that the method is degraded into the iteration step length of the Gauss-Newton algorithm, is suitable for the final stage of iteration, is very close to the optimal solution, and avoids the oscillation of the steepest descent;

it follows that μ affects both the descending direction of the function and the size of the iteration step, and that μ should be set to a small value at the start of the iteration and the size of μ and the matrix a in order to minimize the optimization function₀＝J^T(x₀)J(x₀) The size of the middle element is related to that:

where tau is selected by the user on a case-by-case basis,

is a matrix A₀The elements of (1);

2): in an iterative process, the confidence domain method is a common method for updating μ, and defines a gain ratio Q to control the update, as follows:

here, F (x) -F (x + h)_lm) Is F (x) the actual reduction value of iteration at step k, L (h)_lm) Is F (x + h)_lm) Is a function of the linear approximation of (c),

denominator L (0) -L (h)_lm) A decreasing value for the linear model prediction, which is constantly zero;

if Q is larger, indicate L (h)_lm) Is F (x + h)_lm) Better approximation can reduce mu, so that the algorithm is closer to the gauss-newton algorithm in the next iteration, if Q is very small or negative, the approximation effect is poor, mu needs to be increased to reduce the iteration step length, and at the moment, the algorithm is closer to the steepest descent method, specifically:

(1) when Q > 0, this iteration is valid

(2) When Q is less than or equal to 0, the iteration is invalid, and a parameter v is introduced to update:

μ＝μ×v，v＝2×v (21)

and finally, judging whether to finish iteration according to an iteration termination condition, wherein the method has two specific forms:

(1) one is that the gradient g satisfies

||g||_∞≤ε₁ (22)

Where epsilon₁A defined threshold, positive, meaning a number of times when the iteration is downWhen zero is reached, the convergence to the vicinity of the extreme point is considered;

(2) alternatively, when the amount of change in the variable x is too small, the iteration is considered to converge,

||x_new-x||＜ε₂(||x||+ε₂) (23)

here,. epsilon₂A positive number greater than zero, defined by the user;

3): defining a maximum number of iterations k_maxWhen the iteration number k is more than or equal to k_maxThe iteration is stopped to avoid the situation of infinite loop.

The specific calculation process of the Levenberg-Marquardt algorithm in this embodiment is as follows:

in this embodiment, when a certain number of point clouds are accumulated in step S5, a map may be built, and the point cloud data is merged into a world map, which includes the following specific steps:

at the end of the (k + 1) th frame of point cloud scanning, a frame of undistorted point cloud can be obtained through a laser radar odometer,

and a pose change

It is contained in [ t_k+1,t_k+2]Will be paired under the world coordinate system { W }, the mapping algorithm

And (6) carrying out registration.

Herein is defined as Q_kFor all point clouds accumulated to the k frame under { W }The projection of (a) is performed,

is the last time t of the kth scanning of the laser radar on the map_k+1The mapping algorithm will follow the output of the laser radar odometer

From t_k+1Extension to t_k+2Thereby obtaining

Then pass through

Handle

Projected into the world coordinate System { W }, denoted

Then through registration

And Q_kTo further optimize radar pose

The embodiment has the advantages of optimizing the radar pose

The method comprises the following steps:

as shown in figure 6 of the drawings,

representing the pose of the radar on a map

It is generated by the mapping algorithm at the kth frame;

the first curve segment in FIG. 6 represents the radar motion between the (k + 1) th frame

It is calculated by a mileage calculation method;

through

And

the distorted point cloud issued by the mileage calculation method can be mapped on a map and defined as

(second line segment in fig. 6) and matched to the existing map Q_k(broken line in FIG. 6);

the definition and use of the characteristic points by the mapping node are the same as those of a mileage calculation method, but the quantity of the mapping node is more than ten times that of the mileage calculation method, so that a covariance matrix is introduced to find the corresponding relation of the characteristic points:

firstly, Q is put in_kStored in a cube with a side length of 10m, will be combined with

Extracting points in the intersected cube and storing the points in the 3D KD-tree;

then, using a 3D KD-tree to index, and finding a group of point cloud data of the adjacent characteristic points, and recording the point cloud data as S'; according to the characteristic of the covariance matrix, the coordinate distribution of the three-dimensional points of one area and the eigenvalue and the eigenvector of the covariance matrix formed by the coordinate distribution can be known to have a certain relation;

then, calculating a covariance matrix of S', and recording the covariance matrix as M, wherein the eigenvalue and the eigenvector of M are respectively recorded as V and V; for a line approximated by V, which contains a feature value significantly larger than the other two, the corresponding feature vector is the line vector of the midpoint of S';

for S 'on an approximate plane, V contains two larger eigenvalues, the third eigenvalue is significantly smaller, and the eigenvector associated with the smallest eigenvalue in E is the normal vector of the plane on which the midpoint of S' lies. Determining the position of the edge line or plane through the geometric center of S';

thus, an edge line corresponding to a lower edge point of the world coordinate system is found, and a plane corresponding to the plane point is found; next, selecting two points on the edge line, selecting three points on the plane, calculating the distance by using the formula same as the formula (2) and the formula (3), and then performing iterative optimization, wherein the method used in the method is the same as the method used in the odometer node;

finally, the obtained optimal transformation is utilized

Will be provided with

Superposing the map again to the map;

to evenly distribute the points, the size of the map cloud will be reduced by a voxel grid filter, voxel size being a cube of 5cm, as shown in fig. 7.

The foregoing is only a preferred embodiment of the present invention, and it should be noted that modifications can be made by those skilled in the art without departing from the principle of the present invention, and these modifications should also be construed as the protection scope of the present invention.

Claims (10)

1. A laser autonomous positioning mapping method for a quadruped robot in a cross-country environment is characterized by comprising the following steps: the method comprises the following steps: the real-time positioning and mapping are decomposed into two threads, wherein the first thread is a high-frequency thread, the patrol device motion estimation is mainly carried out at 10Hz, IMU mileage information is fused, and the second thread is a low-frequency thread and is mainly responsible for local mapping;

the specific mapping method comprises the following steps:

s1: acquiring laser radar original point cloud under a field environment, extracting less than 100 three-dimensional feature points for registration on the basis of the original point cloud, focusing the alternative three-dimensional feature points into edge points and plane points, and extracting the edge points and the plane points according to the value c;

s2: after the three-dimensional characteristic points are obtained, registering the two frames of point clouds;

s3: establishing a geometric relation between the related point clouds, positioning the inspection device, and performing motion compensation on a distortion problem generated by the motion of the laser radar;

s4: the redundancy parameters are effectively processed through a Levenberg-Marquardt algorithm, and nonlinear optimization state estimation is carried out at the rear end through the Levenberg-Marquardt algorithm;

s5: when a certain number of point clouds are accumulated, mapping can be carried out, the pose of the laser radar in a world coordinate system is accurately estimated, the point cloud data are integrated into a world map, and the operation frequency of an algorithm adopted for mapping is lower than that of a mileage calculation method.

2. The cross-country environment quadruped robot laser autonomous positioning mapping method according to claim 1, characterized in that: the specific method for extracting the edge points and the plane points according to the value of c in step S1 is as follows:

wherein i is p_kA point of (1), p_kFor the point cloud obtained by one scan of the laser scanner, i ∈ p_kS is a set of continuous points near i in the same line scanning of the laser scanner, the S comprises half of points on two sides of i, and | S | is the number of the points;

modulo the normalized vector sum of points in i and S;

the modulus of the edge point vector sum is large, while the modulus of the plane point vector sum is small;

the points in the scan are sorted based on the c-value, and then the largest c-value and the smallest c-value are selected as feature points, wherein the largest c-value is an edge point, and the smallest c-value is a plane point.

3. The cross-country environment quadruped robot laser autonomous positioning mapping method according to claim 2, characterized in that: in the process of extracting the edge point and the plane point by the value of c in S1, when the point i is selected as the edge point or the plane point, the following condition needs to be satisfied:

in order to uniformly distribute the characteristic points, the data obtained by each scanning needs to be uniformly divided into four regions, each region contains 2 edge points and 4 plane points at most, the number of the selected edge points or plane points cannot exceed the maximum value of the sub-region, the points in 10 points in the neighborhood around the point i cannot be selected again, and the point i cannot be on a plane which is approximately parallel to the laser beam and cannot be on the boundary of the shielded region;

assuming that point a is on a curved surface at an angle to the laser beam and point B is on a curved surface substantially parallel to the laser beam, point B is an unreliable laser echo and is not selected as a feature point, a real line segment is an object observable by the laser, and point a is on the boundary of the occlusion region and may be detected as an edge point, but if viewed from a different angle, the occlusion region may change and become observable, point a is not a significant edge point and may not be selected as a feature point.

4. The cross-country environment quadruped robot laser autonomous positioning mapping method according to claim 2, characterized in that: the specific method for registering the two frames of point clouds in the step S2 is as follows:

1)：t_kdenotes the time at which the k-th scan starts, t_k+1The point cloud p obtained by the scanning is used as the scanning end time_kProjection to time stamp t after distortion correction_k+1The above step (1);

2): recording the point cloud after projection

3): at the k-thWill be used for +1 scan

And newly acquired point cloud p_k+1Performing registration to estimate the movement of the lidar;

4): at the end of the kth scan, p is divided_kReprojection of time stamp t_k+1To obtain

Wherein p is_kIs the point cloud perceived during the kth scan;

5): during the (k + 1) th scan, p_kAnd a newly perceived point cloud p_k+1Together for estimating the movement of the lidar;

6): for p_k+1Extracting characteristic points from the laser radar point cloud, and recording epsilon_k+1And H_k+1Respectively, the edge points and the plane points of the collection;

7): then find out from

Has an edge line of as ∈_k+1Finding a plane as H according to the corresponding relation of the midpoints_k+1The corresponding relation of the midpoints;

8): transform using current estimate will be epsilon_k+1And H_k+1Projecting the coordinate system of the scanning starting point again to record

And

the point set is re-projected, so that pose transformation information of all points in the point cloud of the (k + 1) th frame can be obtained;

9): for the

And

will be at each point in

The corresponding relation is found out in the step (A),

the characteristic points of (2) are stored in a 3D KD-tree, which is commonly used as a range search and a nearest neighbor search.

5. The cross-country environment quadruped robot laser autonomous positioning mapping method according to claim 4, characterized in that: in step S2, the two frames of point clouds are registered, and the specific method of the three-dimensional edge feature point registration is as follows:

let i be

At the point of one of (a) and (b),

two points need to be found to represent the edge line, first at

Finding the point nearest to i, marking as j, then finding the point nearest to i in two continuous scans of the adjacent line of j, marking as l, then (j, l) forming the corresponding relation of i, wherein the nearest neighbor point is selected and used as the nearest neighbor index of KD-tree, in order to avoid the situation that the three points of i, j and l are collinear to cause no solution, j and l must come from the scans of different lines, and for the point

If (j, l) is the corresponding edge line,

the distance of the point to the edge line can be calculated as:

wherein

And

the coordinates of the points i, j and L in { L } respectively, and finding an edge line as

J is the point closest to the feature point, at

Finding a first line representing the same scan as j, a second line representing two consecutive scans, finding another point l on the second line for finding the correspondence of the edge line, and representing the correspondence as (j, l); geometric constraint relation constructed by three points i, j and l, d_εIs the distance from the point to the edge line.

6. The cross-country environment quadruped robot laser autonomous positioning mapping method according to claim 4, characterized in that: in step S2, registering the two frames of point clouds, where the specific method of registering the three-dimensional plane feature points is as follows:

let i be

At the point of one of (a) and (b),

find three points to represent a plane, first at

Finding the point nearest to i, marking as j, then marking the other two points as l and m, which are the points nearest to i, i is in the same line scanning with j, m is the point in two continuous scans of the adjacent line of j, thus ensuring that the three points are non-collinear, and calculating the distance between the plane point and the corresponding plane for the point

If (j, l, m) is the corresponding plane,

the point-to-plane distance is then:

wherein

Is the coordinate of the mid-point m of { L }, find a plane as

J is the point closest to the feature point and can be in

Finding out the points l and m, and respectively finding out other two points l and m on different lines, wherein the corresponding relation is expressed as (j, l, m); the geometric constraint relation constructed by four points of i, j, l and m in the process of matching the characteristics of the plane points, d_HIs the point-to-plane distance;

after the corresponding relation of the characteristic points is obtained, the positioning work of the patrol instrument is carried out, the carrier is assumed to have constant angular speed and linear speed in a short time in the scanning process of the laser radar, the linear interpolation posture conversion can be carried out on the points received at different times in the scanning process,

wherein, t_k+1Is the time at which the (k + 1) th scan starts, t_k+2To be the time at which the scanning is finished,

is [ t ]_k+1,t_k+2]The laser radar posture is changed between the two modes,

including six-degree-of-freedom rigid movement of the laser radar,

wherein t is_x,t_yAnd t_zIs a translation along the x-, y-and z-axes of { L }, and θ_x,θ_yAnd theta_zIs the angle of rotation, t, following the right hand rule_iIs the time stamp of the point i,

is [ t ]_k+1,t_i]And the attitude of the laser radar is changed.

7. The cross-country environment quadruped robot laser autonomous positioning mapping method according to claim 6, characterized in that: in step S3, a geometric relationship is established between the related point clouds, and a specific method for performing motion compensation is as follows:

from the transformation in equation (4), it can be derived,

wherein,

is epsilon_k+1Or H_k+1The coordinates of the middle point i are,

is that

And

the corresponding point of (i) in (1),

is that

R is a rotation matrix defined by the Rodrigues equation, facilitating derivation:

in the above formula, θ is the rotation amplitude

ω is a unit vector representing the direction of rotation

Omega is

Of the diagonal-symmetrical matrix of (a),

by combining equation (2) and equations (4) - (8), ε can be obtained_k+1The edge points in (b) and the corresponding edge lines,

similarly, in combination with formula (3) and formulas (4) - (8), can be at H_k+1Another geometrical relationship is established between the plane points in (a) and the corresponding planes,

for epsilon_k+1Or H_k+1The iteration of equations (8) and (9) for each feature point in (a) can obtain a non-linear function,

where each row of f corresponds to a feature point and d contains the corresponding distance. In this case, equation (11) has only one unknown quantity

Then, a Levenberg-Marquardt method is used for iterative optimization, and the aim is to find an optimal pose transformation

The distance d is minimized in such a way that,

first, calculate the relative

The Jacobian matrix of f, denoted J, where

Then, d is minimized to zero by iteration through equation (12),

where λ is a penalty factor determined by Levenberg-Marquardt.

8. The cross-country environment quadruped robot laser autonomous positioning mapping method according to claim 7, characterized in that: in the step S4, the Levenberg-Marquardt algorithm is used at the back end to perform nonlinear optimization state estimation, and the specific steps are as follows:

1): assuming that a nonlinear least square problem is researched, an optimization function is set, and an iteration step size of a Levenberg-Marquardt algorithm is defined:

where f (x) is an optimization function, the Levenberg-Marquardt algorithm defines an iteration step size of,

h_lm＝-(J^TJ+μI)^-1g,g＝J^Tf and mu is more than or equal to 0 (14)

Where, f ═ f (x), J ═ J (x), is the jacobian matrix of f, μ is a penalty factor, and it has the following effects:

(1) when μ > 0, all coefficient matrices are positive, which ensures that h_lmRepresents the descending direction of f (x), i.e. each step h_lmWill result in a decrease in the value of f (x);

(2) when the value of μ is large, it is preferable that,

h_lmdegenerating to an iteration step size of a steepest descent (steepest gradient) algorithm;

(3) if the value of mu is small or equal to zero,

h_lm＝-(J^TJ)^-1g (16)

h_lmdegradation of stool into gauss newton's calculationThe iteration step length of the method is suitable for the final stage of iteration, is very close to the optimal solution, and avoids the oscillation of the steepest descent;

it follows that μ affects both the descending direction of the function and the size of the iteration step, and that μ should be set to a small value at the start of the iteration and the size of μ and the matrix a in order to minimize the optimization function₀＝J^T(x₀)J(x₀) The size of the middle element is related to that:

where tau is selected by the user on a case-by-case basis,

is a matrix A₀The element (1) in (1);

2): in an iterative process, the confidence domain method is a common method for updating μ, and defines a gain ratio Q to control the update, as follows:

here, F (x) -F (x + h)_lm) Is F (x) the actual reduction value of iteration at step k, L (h)_lm) Is F (x + h)_lm) Is a function of the linear approximation of (c),

denominator L (0) -L (h)_lm) A decreasing value for the linear model prediction, which is constantly zero;

if Q is larger, indicate L (h)_lm) Is F (x + h)_lm) Better approximation can reduce mu, so that the algorithm is closer to the Gauss-Newton algorithm in the next iteration, if Q is very small or negative, the approximation effect is poor, and mu needs to be increased to reduce the iteration stepLong, when the algorithm is closer to the steepest descent method, specifically:

(1) when Q > 0, this iteration is valid

(2) When Q is less than or equal to 0, the iteration is invalid, and a parameter v is introduced to update:

μ＝μ×v，v＝2×v (21)

and finally, judging whether to finish iteration according to an iteration termination condition, wherein the method has two specific forms:

(1) one is that the gradient g satisfies

||g||_∞≤ε₁ (22)

Where epsilon₁A defined threshold, positive, meaning that when the rate of iterative descent is almost zero, it is considered to have converged to near the extreme point;

(2) alternatively, when the amount of change in the variable x is too small, the iteration is considered to converge,

||x_new-x||＜ε₂(||x||+ε₂) (23)

here,. epsilon₂A positive number greater than zero, defined by the user;

3): defining a maximum number of iterations k_maxWhen the iteration number k is more than or equal to k_maxThe iteration is stopped to avoid the situation of infinite loop.

9. The cross-country environment quadruped robot laser autonomous positioning mapping method according to claim 8, characterized in that: in step S5, when a certain number of point clouds are accumulated, a map may be created, and the point cloud data is merged into a world map, which includes the following specific steps: at the end of the (k + 1) th frame of point cloud scanning, a frame of undistorted point cloud can be obtained through a laser radar odometer,

and a pose change

It is contained in [ t_k+1,t_k+2]Will be paired under the world coordinate system { W }, the mapping algorithm

And (6) carrying out registration.

Herein is defined as Q_kFor the projection of all point clouds accumulated to the k-th frame under W,

is the last time t of the kth scanning of the laser radar on the map_k+1Along with the output of the laser radar odometer, the mapping algorithm will be

From t_k+1Extension to t_k+2Thereby obtaining

Then pass through

Handle

Projected into the world coordinate System { W }, denoted

Then through registration

And Q_kTo further optimize radar pose

10. The cross-country environment quadruped robot laser autonomous positioning mapping method according to claim 9, characterized in that: with optimized radar pose

The method comprises the following steps:

representing the pose of the radar on a map

It is generated by the mapping algorithm at the kth frame;

radar motion between k +1 th frames

It is calculated by a mileage calculation method;

through

And

the distorted point cloud issued by the mileage calculation method can be mapped on a map and defined as

And matched to an existing map Q_kThe above step (1);

the definition and use of the characteristic points by the mapping node are the same as those of a mileage calculation method, but the quantity of the mapping node is more than ten times that of the mileage calculation method, so that a covariance matrix is introduced to find the corresponding relation of the characteristic points:

firstly, Q is put in_kStored in a cube with a side length of 10m, will be combined with

Extracting points in the intersected cube and storing the points in the 3D KD-tree;

then, using a 3D KD-tree to index, and finding a group of point cloud data of the adjacent characteristic points, and recording the point cloud data as S'; according to the characteristic of the covariance matrix, the coordinate distribution of the three-dimensional points of one area and the eigenvalue and the eigenvector of the covariance matrix formed by the coordinate distribution can be known to have a certain relation;

then, calculating a covariance matrix of S', and recording the covariance matrix as M, wherein the eigenvalue and the eigenvector of M are respectively recorded as V and V; for a line approximated by V, which contains a feature value significantly larger than the other two, the corresponding feature vector is the line vector of the midpoint of S';

for S ' on an approximate plane, V contains two larger characteristic values, the third characteristic value is obviously smaller, the characteristic vector related to the minimum characteristic value in E is the normal vector of the plane where the midpoint of S ' is located, and the position of the edge line or the plane is determined through the geometric center of S ';

thus, an edge line corresponding to a lower edge point of the world coordinate system is found, and a plane corresponding to the plane point is found; next, selecting two points on the edge line, selecting three points on the plane, calculating the distance by using the formula same as the formula (2) and the formula (3), and then performing iterative optimization, wherein the method used in the method is the same as the method used in the odometer node;

finally, the obtained optimal transformation is utilized

Will be provided with

Superposing the map again to the map;

to evenly distribute the points, the size of the map cloud will be reduced by a voxel grid filter, the voxel size being a 5cm cube.

Images