Robot pose determination method and device, electronic equipment and storage medium

Abstract

The invention provides a pose determination method and device of a robot, electronic equipment and a storage medium. The method comprises the steps that first point cloud data collected by the robot in a first scanning period and second point cloud data collected by the robot in a second scanning period are acquired; according to the curvature of each first sub-point cloud data in the second point cloud data, constructing an edge feature point set and a plane feature point set corresponding to the second point cloud data; for each second sub-point cloud data in the first point cloud data, determining a first feature distance according to the second sub-point cloud data and the edge feature point set, and determining a second feature distance according to the second sub-point cloud data and the plane feature point set; according to all the first feature distances and all the second feature distances, a least square optimization function is constructed, the current pose of the robot is determined according to the optimal solution of the least square optimization function, and the optimal solution is used for representing the pose transformation relation between the first point cloud data and the second point cloud data. And the positioning result of the robot with relatively high accuracy can be determined.

Description

Technical Field

[0001] The present invention relates to the field of robot positioning technology, and in particular to a method, device, electronic equipment and storage medium for determining the position and posture of a robot. Background Art

[0002] The existing robot positioning method is often implemented by using ultra-wideband (UWB) positioning technology, which specifically determines the robot's position and posture to determine the robot's positioning result. Among them, ultra-wideband positioning technology can achieve high-precision positioning of 10 centimeters (cm).

[0003] However, the ultra-wideband signal involved in the above-mentioned ultra-wideband positioning technology is prone to interfere with other equipment, and a lot of preliminary preparation work is required during the positioning process, such as establishing a location fingerprint database, which takes a lot of time and effort. In addition, if a sudden fire is involved, it is impossible to give time for advance preparation and layout, which leads to the final determined posture result is not accurate enough, and thus the positioning result of the robot cannot be accurately determined. Summary of the invention

[0004] The present invention provides a method, device, electronic device and storage medium for determining the posture of a robot, which are used to solve the defect that the posture determination method of the robot in the related art leads to the posture result being not accurate enough, and thus the positioning result of the robot cannot be accurately determined. Based on the point cloud data corresponding to each of the multiple scanning cycles, the registration of the point cloud key frames is realized, and the current posture of the robot is determined in combination with the optimal solution of the least squares optimization function. The accuracy of the current posture is high, and the positioning result of the robot with high accuracy can be determined later.

[0005] The present invention provides a method for determining the position and posture of a robot, comprising:

[0006] Acquire first point cloud data collected by the robot in a first scanning cycle, and second point cloud data collected in a second scanning cycle, wherein the second scanning cycle is later than the first scanning cycle;

[0007] Constructing an edge feature point set and a plane feature point set corresponding to the second point cloud data according to the curvature of each first sub-point cloud data in the second point cloud data;

[0008] For each second sub-point cloud data in the first point cloud data, determining a first characteristic distance according to the second sub-point cloud data and the edge feature point set, and determining a second characteristic distance according to the second sub-point cloud data and the plane feature point set;

[0009] A least squares optimization function is constructed based on all first feature distances and all second feature distances, and the current posture of the robot is determined based on the optimal solution of the least squares optimization function, wherein the optimal solution is used to characterize the posture transformation relationship between the first point cloud data and the second point cloud data.

[0010] According to a method for determining the posture of a robot provided by the present invention, the edge feature point set and the plane feature point set corresponding to the second point cloud data are constructed according to the curvature of each first sub-point cloud data in the second point cloud data, including: determining the curvature of each first sub-point cloud data in the second point cloud data; constructing the edge feature point set according to the first sub-point cloud data corresponding to the curvature greater than a preset curvature threshold; constructing the plane feature point set according to the first sub-point cloud data corresponding to the curvature less than or equal to the preset curvature threshold.

[0011] According to a method for determining the posture of a robot provided by the present invention, determining the first feature distance based on the second sub-point cloud data and the edge feature point set includes: obtaining a first coordinate corresponding to the second sub-point cloud data, a second coordinate corresponding to the first feature point in the edge feature point set, and a third coordinate corresponding to the second feature point; determining a first difference based on the first coordinate and the second coordinate, and determining a second difference based on the first coordinate and the third coordinate; determining a third difference based on the second coordinate and the third coordinate; and determining the first feature distance based on the first difference, the second difference, and the third difference.

[0012] According to a robot posture determination method provided by the present invention, the second feature distance is determined based on the second sub-point cloud data and the plane feature point set, including: obtaining a first coordinate corresponding to the second sub-point cloud data, and a fourth coordinate corresponding to a third feature point in the plane feature point set; determining a fourth difference based on the first coordinate and the fourth coordinate; determining a normal vector corresponding to the plane formed by the plane feature point set; and determining the second feature distance based on the fourth difference and the normal vector.

[0013] According to a method for determining a position and posture of a robot provided by the present invention, the determining of the curvature of each first sub-point cloud data in the second point cloud data comprises: performing the following operations for each first sub-point cloud data: acquiring adjacent sub-point cloud data of the first sub-point cloud data and the number of the adjacent sub-point cloud data in the second point cloud data;

[0014] The curvature of the first sub-point cloud data is determined according to the quantity, the coordinates corresponding to the first sub-point cloud data, and the coordinates corresponding to the adjacent sub-point cloud data.

[0015] According to a method for determining a robot's posture provided by the present invention, the method further includes: acquiring environmental information perceived by the robot in a current posture; and constructing global information of the robot's environment based on the environmental information.

[0016] According to a method for determining the position and posture of a robot provided by the present invention, the method of obtaining first point cloud data collected by the robot in a first scanning cycle and second point cloud data collected in a second scanning cycle includes: obtaining first initial point cloud data collected by the robot in the first scanning cycle and second initial point cloud data collected in the second scanning cycle; correcting and filtering the first initial point cloud data to obtain the first point cloud data, and correcting and filtering the second initial point cloud data to obtain the second point cloud data.

[0017] The present invention also provides a robot posture determination device, comprising:

[0018] A point cloud acquisition module, used to acquire first point cloud data collected by the robot in a first scanning cycle, and second point cloud data collected in a second scanning cycle, wherein the second scanning cycle is later than the first scanning cycle;

[0019] A point set construction module, used to construct an edge feature point set and a plane feature point set corresponding to the second point cloud data according to the curvature of each first sub-point cloud data in the second point cloud data;

[0020] A data processing module is used to determine, for each second sub-point cloud data in the first point cloud data, a first feature distance based on the second sub-point cloud data and the edge feature point set, and to determine a second feature distance based on the second sub-point cloud data and the plane feature point set; construct a least squares optimization function based on all first feature distances and all second feature distances, and determine the current posture of the robot based on the optimal solution of the least squares optimization function, wherein the optimal solution is used to characterize the posture transformation relationship between the first point cloud data and the second point cloud data.

[0021] The present invention also provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, a method for determining the posture of a robot as described above is implemented.

[0022] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, and when the computer program is executed by a processor, the method for determining the posture of the robot as described in any one of the above is implemented.

[0023] The present invention also provides a computer program product, comprising a computer program, wherein when the computer program is executed by a processor, the method for determining the position and posture of the robot as described in any one of the above is implemented.

[0024] The robot posture determination method, device, electronic device and storage medium provided by the present invention use the point cloud data corresponding to each of the multiple scanning cycles as the basis for the entire process, realize the registration of the point cloud key frames, and combine with the optimal solution of the least squares optimization function to determine the current posture of the robot with higher accuracy, so as to subsequently determine the positioning result of the robot with higher accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0026] Figure 1a It is a structural schematic diagram of the robot provided by the present invention;

[0027] Figure 1b It is a structural schematic diagram of the radar rectangular coordinate system provided by the present invention;

[0028] Figure 2 It is a schematic flow chart of a method for determining the position and posture of a robot provided by the present invention;

[0029] Figure 3 It is a schematic diagram of the solution process of the least squares optimization function provided by the present invention;

[0030] Figure 4 It is a plan schematic diagram of the global information of the fire scene environment provided by the present invention;

[0031] Figure 5 It is a structural schematic diagram of a robot position and posture determination device provided by the present invention;

[0032] Figure 6 It is a structural schematic diagram of the electronic device provided by the present invention. DETAILED DESCRIPTION

[0033] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the drawings of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0034] In order to better understand the embodiments of the present invention, the robot involved in the embodiments of the present invention is first described in detail:

[0035] A robot is a machine system that can automatically perform tasks. For example, if a fire-fighting task needs to be performed, the robot is a fire-fighting robot.

[0036] like Figure 1a , which is a schematic diagram of the structure of the robot provided by the present invention. Figure 1a In the present invention, a lidar sensor and an adapter box are installed on the robot, and the adapter box is connected to the electronic equipment.

[0037] The laser radar sensor is a multi-line laser radar sensor, which is used to obtain point cloud data of the environment in real time. The point cloud data is a set of vectors in a three-dimensional coordinate system, which reflects the geometric structure of the environment where the robot is located. Optionally, the point cloud data may include: three-dimensional coordinate information, color information, and reflection intensity information. Optionally, the above laser radar sensor may be a 16-line laser radar sensor, a 32-line laser radar sensor, or a laser radar sensor with more than 32 lines, which is not specifically limited here.

[0038] The adapter box is equipped with a network cable interface, a power interface and a global positioning system (GPS) interface, which enables the robot to have the ability to perceive the environment. Specifically, the adapter box can convert the information transmitted by the lidar sensor into the user datagram protocol (UDP) and connect it to the electronic device through the network cable. At the same time, the adapter box can also realize voltage conversion, converting the 9V-32V DC input into a 12V DC input to power the lidar sensor.

[0039] The electronic device is a computing platform that can be installed on a robot or can be a device independent of the robot, which is not specifically limited here.

[0040] It should be noted that when the robot, lidar sensor, adapter box and electronic device are connected and the robot is in normal operation, the electronic device can drive specific software to parse the data packets transmitted by the lidar sensor to obtain perception information such as rotation angle, ranging information and reflectivity.

[0041] Optional, such as Figure 1b FIG. 1 is a schematic diagram of the structure of the radar rectangular coordinate system provided by the present invention. Figure 1b, the electronic device can use the formulas x=R*cosω*sinα, y=R*cosω*cosα and z=R*sinω to convert the angle information (such as ω, α) of the point cloud data collected in the polar coordinate system into the coordinates (x, y, z) corresponding to the point cloud data in the radar rectangular coordinate system.

[0042] Among them, the lidar sensor is located at the origin of the radar rectangular coordinate system.

[0043] x represents the coordinate of the point cloud data on the X-axis in the radar rectangular coordinate system; y represents the coordinate of the point cloud data on the Y-axis in the radar rectangular coordinate system; z represents the coordinate of the point cloud data on the Z-axis in the radar rectangular coordinate system; R represents the distance between the point cloud data and the origin measured by the time of flight (ToF) method; ω represents the angle formed between the current laser beam generated by the lidar sensor and the horizontal plane; α represents the rotation angle of the current laser beam of the lidar sensor.

[0044] Optionally, the robot and the electronic device can be connected via wireless communication technology, which may include but is not limited to one of the following: the 4th generation mobile communication technology (4G) and the 5th generation mobile communication technology (5G), etc.

[0045] It should be noted that the execution subject involved in the embodiment of the present invention can be a robot's posture determination device or an electronic device. The embodiment of the present invention is further explained below by taking the electronic device as an example.

[0046] like Figure 2 FIG. 1 is a flow chart of a method for determining the position and posture of a robot provided by the present invention, which may include:

[0047] 201. Acquire first point cloud data collected by the robot in a first scanning cycle and second point cloud data collected in a second scanning cycle, where the second scanning cycle is later than the first scanning cycle.

[0048] The scanning cycle refers to the time required for the robot to complete a comprehensive scan of the environment it is in. The first scanning cycle is adjacent to the second scanning cycle.

[0049] The first point cloud data is a frame of point cloud information collected by the robot in the first scanning cycle; the second point cloud data is a frame of point cloud information collected by the robot in the second scanning cycle.

[0050] When the robot is scanning in its environment, the lidar sensor is rotated. At this time, the first point cloud data can be collected in the first scanning cycle, and the second point cloud data can be collected in the second scanning cycle. The first point cloud data and the second point cloud data are sent to an electronic device through the 4G / 5G network, and the electronic device then receives the first point cloud data and the second point cloud data to prepare for the subsequent determination of the robot's posture transformation relationship.

[0051] In some embodiments, the electronic device obtains first point cloud data collected by the robot in a first scanning cycle and second point cloud data collected in a second scanning cycle, which may include: the electronic device obtains first initial point cloud data collected by the robot in the first scanning cycle and second initial point cloud data collected in the second scanning cycle; the electronic device corrects and filters the first initial point cloud data to obtain first point cloud data, and corrects and filters the second initial point cloud data to obtain second point cloud data.

[0052] In the process of determining the first point cloud data and the second point cloud data, the electronic device may first receive the first initial point cloud data collected by the robot in the first scanning cycle and the second initial point cloud data collected in the second scanning cycle; since the first initial point cloud data and the second initial point cloud data have error information and redundant information, resulting in inaccuracy, the electronic device may correct the first initial point cloud data, which can effectively eliminate or reduce the deviation information introduced by factors such as the robot's own posture and sensor errors, and then filter the corrected first initial point cloud data to remove noise and redundant information in the corrected first initial point cloud data, improve data quality and accuracy, and obtain first point cloud data with higher accuracy. Similarly, the electronic device may also correct and filter the second initial point cloud data to obtain second point cloud data with higher accuracy. At this time, the first point cloud data and the second point cloud data can more accurately reflect the environment of the above-mentioned robot, and provide reliable input for subsequent tasks such as global information generation and scene reconstruction.

[0053] Optionally, the filtering algorithm may include: Kalman filtering, extended Kalman filtering, etc.

[0054] Among them, Kalman filtering is a method that uses linear system state equations to update and process the target initial point cloud data collected on-site in real time.

[0055] The extended Kalman filter is an extended form of the Kalman filter, which is mainly used to deal with nonlinear system problems.

[0056] For example, in a fire scene environment, the filtering algorithm can effectively remove interference frames (such as some particulate matter) involved in the process of the fire-fighting robot collecting the target initial point cloud data.

[0057] Optionally, the electronic device may use a de-distortion algorithm to correct the target initial point cloud data to obtain corrected target point cloud data.

[0058] Wherein, when the target initial point cloud data is the first initial point cloud data, the target point cloud data is the first point cloud data; when the target initial point cloud data is the second initial point cloud data, the target point cloud data is the second point cloud data.

[0059] It should be noted that the laser radar sensor in the robot usually adopts a mechanical rotating structure. Generally, the scanning frequency of the laser radar sensor is 10 Hz. The generation of a frame of point cloud information (such as the target initial point cloud data) requires the internal clockwise rotation of the laser radar sensor, which takes 100 milliseconds (ms). The translation and rotation movement generated by the laser radar sensor during this scanning cycle will cause the collected target initial point cloud data to be distorted.

[0060] As the lidar sensor translates forward, it gets closer to the obstacle in front, and the distance of the first point cloud collected also gets closer; as it gets farther away from the obstacle in the opposite direction of movement, the distance of the second point cloud collected also gets farther away. When the lidar sensor collects one circle, errors will occur in the point cloud information formed in one frame.

[0061] When dedistorting the target initial point cloud data, since each sub-target initial point cloud data will be affected by motion, the coordinates of each target initial point cloud data need to be motion compensated. The compensation amount is the change of the instantaneous LiDAR coordinates relative to the LiDAR coordinates at the beginning of this frame.

[0062] Assume that after the LiDAR sensor collects the initial point cloud data of the target within a certain scanning cycle, the LiDAR pose at the beginning is: Among them, R0 represents the rotation angle at the starting time; t0 represents the translation distance at the starting time.

[0063] When the LiDAR sensor rotates a certain angle and collects the initial point cloud data of the i-th sub-target, the instant LiDAR pose is: Among them, R i Indicates the instantaneous rotation angle, indicating the direction of the LiDAR sensor; t i Indicates the instantaneous translation distance, indicating the position of the lidar sensor.

[0064] At this time, the coordinates corresponding to the initial point cloud data of the i-th sub-target (without distortion correction compensation) are: P i =[P ix P iy P iz ]T Among them, P ix represents the coordinate of the initial point cloud data of the i-th sub-target on the X-axis; P iy represents the coordinate of the initial point cloud data of the i-th sub-target on the Y axis; P iz Represents the coordinate of the initial point cloud data of the i-th sub-target on the Z axis.

[0065] At this time, the actual coordinates corresponding to the initial point cloud data of the i-th sub-target after compensation for distortion are: in, Represents a preset matrix.

[0066] At this point, the actual coordinates can be obtained by calculating the relative translation and rotation changes of the position and posture of the lidar sensor from the time of collecting the 0th sampling point to the time of collecting the ith sampling point.

[0067] In practical applications, the initial point cloud data of the above target is expressed in the radar rectangular coordinate system. When the initial 0 moment is used as the reference coordinate system, is the unit matrix, which represents the relative translation and rotation changes from time 0 to time i. At this time, the relative rotation can be expressed as: R′ i =a*Time(i-0); the rotation change can be expressed as: t′ i =v*Time(i-0)

[0068] In the above formula, Time(i-0) represents the time interval between the 0th sampling point and the i-th sampling point; a represents the average angular velocity in the time interval; and v represents the average velocity in the time interval.

[0069] To further improve the accuracy, these two values ​​(i.e., relative translation R′) can be i and rotation change t′ i ) is obtained through IMU.

[0070] 202. Construct an edge feature point set and a plane feature point set corresponding to the second point cloud data according to the curvature of each first sub-point cloud data in the second point cloud data.

[0071] The second point cloud data includes multiple first sub-point cloud data. In order to facilitate the determination of the subsequent posture transformation relationship, the curvature of each first sub-point cloud data can be determined first, and based on all the curvatures, all the first sub-point cloud data are classified to obtain the first sub-point cloud data that can represent edge features (also called edge feature points) and the first sub-point cloud data that can represent plane features (also called patch feature points), and then construct edge feature point sets and plane feature point sets. Among them, the edge feature point set includes all the first sub-point cloud data that represent edge features; the plane feature point set includes all the first sub-point cloud data that represent plane features.

[0072] In some embodiments, the electronic device constructs an edge feature point set and a plane feature point set corresponding to the second point cloud data based on the curvature of each first sub-point cloud data in the second point cloud data, which may include: the electronic device determines the curvature of each first sub-point cloud data in the second point cloud data; the electronic device constructs the edge feature point set based on the first sub-point cloud data corresponding to the curvature greater than a preset curvature threshold; the electronic device constructs the plane feature point set based on the first sub-point cloud data corresponding to the curvature less than or equal to the preset curvature threshold.

[0073] The preset curvature threshold may be set before the electronic device leaves the factory, or may be set after the electronic device leaves the factory, and is not specifically limited here.

[0074] When all the first sub-point cloud data are sufficiently dense, since the first sub-point cloud data with larger curvature are taken from the sharper edges in the space, and the first sub-point cloud data with smaller curvature are taken from the flatter surfaces in the space, after determining the curvature of each first sub-point cloud data in the second point cloud data, the electronic device uses the preset curvature threshold as the dividing line to classify the first sub-point cloud data corresponding to the curvature greater than the preset curvature threshold into the edge feature point set; and classifies the first sub-point cloud data corresponding to the curvature less than or equal to the preset curvature threshold into the plane feature point set.

[0075] The above-mentioned edge feature point set can be represented by a point set M, and the above-mentioned plane feature point set can be represented by a point set N.

[0076] In some embodiments, the electronic device determines the curvature of each first sub-point cloud data in the second point cloud data, which may include: performing the following operations for each first sub-point cloud data: the electronic device obtains the adjacent sub-point cloud data in space of the first sub-point cloud data and the number of adjacent sub-point cloud data in the second point cloud data; the electronic device determines the curvature of the first sub-point cloud data based on the number, the coordinates corresponding to the first sub-point cloud data, and the coordinates corresponding to the adjacent sub-point cloud data.

[0077] The adjacent sub-point cloud data are other first sub-point cloud data adjacent to the first sub-point cloud data in the same line bundle, and the number of the adjacent sub-point cloud data is multiple.

[0078] Since the process of determining the curvature of each first sub-point cloud data is the same, the following operations are performed for each first sub-point cloud data: the electronic device first obtains the adjacent sub-point cloud data of the first sub-point cloud data in the second point cloud data, and these adjacent sub-point cloud data jointly describe the local geometric features around the first sub-point cloud data, and then determines the number of the adjacent sub-point cloud data, which provides information about the point cloud density around the first sub-point cloud data; then, the electronic device performs an approximate calculation on the number, the coordinates corresponding to the first sub-point cloud data, and the coordinates corresponding to the adjacent sub-point cloud data to obtain the curvature of the first sub-point cloud data.

[0079] Optionally, the electronic device uses the formula Get the curvature of the λth first sub-point cloud data.

[0080] Wherein, c(λ) represents the curvature of the λth first sub-point cloud data; S represents the adjacent sub-point cloud data set of the λth first sub-point cloud data, and |S| represents the number of adjacent sub-point cloud data; represents the coordinates of the λth first sub-point cloud data in the kth frame scanning period in the radar rectangular coordinate system L; ‖·‖ represents the norm; It represents the coordinates of the j-th first sub-point cloud data in the radar rectangular coordinate system L in the k-th frame scanning period. The j-th first sub-point cloud data is the adjacent sub-point cloud data of the λ-th first sub-point cloud data and belongs to the adjacent sub-point cloud data set S.

[0081] 203. For each second sub-point cloud data in the first point cloud data, determine a first characteristic distance according to the second sub-point cloud data and an edge feature point set, and determine a second characteristic distance according to the second sub-point cloud data and a plane feature point set.

[0082] Since the process of determining the characteristic distance of each second sub-point cloud data is the same, the following operations are performed for each second sub-point cloud data: the electronic device can calculate the second sub-point cloud data and the edge feature point set to obtain the first characteristic distance, and calculate the second sub-point cloud data and the plane feature point set to obtain the second characteristic distance.

[0083] Based on this, the electronic device will eventually obtain as many first characteristic distances and second characteristic distances as the number of second sub-point cloud data in the first point cloud data. In other words, the number of second sub-point cloud data, first characteristic distances and second characteristic distances are the same.

[0084] In some embodiments, the electronic device determines the first feature distance based on the second sub-point cloud data and the edge feature point set, which may include: the electronic device obtains the first coordinate corresponding to the second sub-point cloud data, the second coordinate corresponding to the first feature point in the edge feature point set, and the third coordinate corresponding to the second feature point; the electronic device determines the first difference based on the first coordinate and the second coordinate, and determines the second difference based on the first coordinate and the third coordinate; the electronic device determines the third difference based on the second coordinate and the third coordinate; the electronic device determines the first feature distance based on the first difference, the second difference, and the third difference.

[0085] In the process of determining the first characteristic distance, the electronic device may first randomly select two characteristic points from the edge feature point set, namely the first characteristic point and the second characteristic point; then determine the first coordinate corresponding to the second sub-point cloud data, the second coordinate corresponding to the first characteristic point, and the third coordinate corresponding to the second characteristic point; then, the electronic device calculates these three coordinates to obtain the first difference between the first coordinate and the second coordinate, the second difference between the first coordinate and the third coordinate, and the third difference between the second coordinate and the third coordinate, and then calculates these three differences, namely the first difference, the second difference and the third difference, to obtain the first characteristic distance between the second sub-point cloud data and the edge feature point set.

[0086] It should be noted that the electronic device has no limitation on the timing of determining the first difference, the second difference and the third difference.

[0087] Optionally, the electronic device uses the point-line distance formula Get the first characteristic distance.

[0088] Among them, d ε represents the first characteristic distance; |·| represents the modulus; represents the second coordinate of the first feature point, that is, the coordinate of the θth second sub-point cloud data in the radar rectangular coordinate system L in the k+1th frame scanning period, Represents the third coordinate of the second feature point, that is, the coordinate of the μth second sub-point cloud data in the radar rectangular coordinate system L in the k+1th frame scanning period, μ∈M, μ≠θ.

[0089] In some embodiments, the electronic device determines the second feature distance based on the second sub-point cloud data and the plane feature point set, which may include: the electronic device obtains a first coordinate corresponding to the second sub-point cloud data, and a fourth coordinate corresponding to the third feature point in the plane feature point set; the electronic device determines a fourth difference based on the first coordinate and the fourth coordinate; the electronic device determines a normal vector corresponding to the plane formed by the plane feature point set; the electronic device determines the second feature distance based on the fourth difference and the normal vector.

[0090] In the process of determining the second characteristic distance, the electronic device may first randomly select a third feature point in the plane feature point set; then determine the first coordinate corresponding to the second sub-point cloud data, and the fourth coordinate corresponding to the third feature point; then, the electronic device calculates the two coordinates to obtain a fourth difference between the first coordinate and the fourth coordinate, and determines the plane formed by the above-mentioned plane feature point set, and the normal vector corresponding to the platform; then, the electronic device calculates the normal vector and the fourth difference again to obtain the second characteristic distance between the second sub-point cloud data and the patch feature point set.

[0091] It should be noted that there is no limitation on the timing of the electronic device determining the normal vector and the fourth difference.

[0092] Optionally, the electronic device uses the point-to-surface distance formula Get the second characteristic distance.

[0093] Among them, d h represents the second characteristic distance; |·| represents the modulus; represents the normal vector; Represents the fourth coordinate of the third feature point, that is, the coordinate of the τth second sub-point cloud data in the k+1th frame scanning period in the radar rectangular coordinate system L, τ∈N.

[0094] 204. Construct a least squares optimization function based on all first feature distances and all second feature distances, and determine the current posture of the robot based on the optimal solution of the least squares optimization function, where the optimal solution is used to characterize the posture transformation relationship between the first point cloud data and the second point cloud data.

[0095] Among them, the pose transformation relationship describes the change in relative position and posture from one point cloud data to another. It should be noted that each point in the point cloud data represents a specific location in the environment, and the pose describes the direction and position of the robot at that location. Therefore, when the robot moves from one location to another, the point cloud data it perceives will also change, and this change is reflected in the pose transformation.

[0096] Since the first characteristic distance and the second characteristic distance corresponding to each of the plurality of second sub-point cloud data in the first point cloud data have been determined in step 203, after acquiring all the first characteristic distances and all the second characteristic distances, the electronic device can establish the inter-frame transfer parameters between the kth frame and the k+1th frame based on all the first characteristic distances and all the second characteristic distances. The function of establishing the least squares optimization function between the first point cloud data of the first scanning cycle and the second point cloud data of the second scanning cycle is specifically: According to the principle of overdetermined problems, the electronic device can construct a least squares optimization problem based on the distance from the point to the line surface for solution. By solving the above least squares optimization function, the posture transformation relationship between the first point cloud data and the second point cloud data can be obtained; then, the electronic device determines the current posture of the robot based on the posture transformation relationship.

[0097] For example, Figure 3 As shown, it is a schematic diagram of the solution process of the least squares optimization function provided by the present invention. In combination with the above steps 202-204, based on the first point cloud data and the second point cloud data, an optimization problem is constructed to generate a least squares optimization function, and then the least squares optimization function is linearized and iteratively solved until the iteration stop condition (such as the maximum number of iterations) is met to obtain the optimal solution.

[0098] In some embodiments, the method may further include: the electronic device obtains environmental information perceived by the robot in the current position; and the electronic device constructs global information of the environment in which the robot is located based on the environmental information.

[0099] The robot obtains the current point cloud data in the current position, and the current point cloud data represents the environment information, and sends the environment information to the electronic device; after receiving the environment information, the electronic device can construct global information based on the environment information. The global information is usually expressed as a two-dimensional or three-dimensional map, which includes the overall structure and characteristics of the robot's environment, such as Figure 4 , which is a plan schematic diagram of the global information of the fire scene environment provided by the present invention.

[0100] Optionally, the electronic device constructs global information of the robot's environment according to the environmental information, which may include: the electronic device constructs global information of the robot's environment according to the environmental information based on graph factor optimization.

[0101] Graph factor optimization estimates the optimal map and robot trajectory by minimizing the energy function of the entire graph (usually expressed as the sum of squared residuals of all factors). This means that the electronic device uses the factor graph to integrate all available environmental information and finds the map representation that best represents the actual environment through an optimization algorithm.

[0102] During the optimization process, the electronics consider various factors, such as the measurement errors of sensors, the uncertainty of robot motion, etc. These factors are incorporated into the construction and optimization of the factor graph to ensure the accuracy and robustness of the global information.

[0103] In addition, as the robot continues to move and perceive new environmental information, the factor graph can be incrementally updated and maintained. This means that the electronic device can incorporate new perception data into the global information without recalculating the entire graph, thus achieving real-time updates and dynamic adjustments.

[0104] Ultimately, through graph factor optimization, electronic devices can construct accurate, consistent and complete global information. This global information not only includes the geometric structure of the robot's environment, but may also include semantic information, obstacle distribution, etc., providing strong support for the robot's autonomous navigation, path planning, task execution, etc.

[0105] For example, in a fire scene, the fire-fighting robot can transmit its current posture and global information to the command vehicle / command platform in real time, so that the command vehicle / command platform can receive fire scene information and make scientific decisions. Specifically, the fire-fighting robot and the command vehicle / command platform can interact and coordinate control, thereby realizing multi-party collaborative operations, effective deployment and scientific rescue, improving search and rescue efficiency and maximizing the safety of rescue personnel.

[0106] In an embodiment of the present invention, the first point cloud data collected by the robot in the first scanning cycle and the second point cloud data collected in the second scanning cycle are obtained, and the second scanning cycle is later than the first scanning cycle; according to the curvature of each first sub-point cloud data in the second point cloud data, the edge feature point set and the plane feature point set corresponding to the second point cloud data are constructed; for each second sub-point cloud data in the first point cloud data, the first feature distance is determined according to the second sub-point cloud data and the edge feature point set, and the second feature distance is determined according to the second sub-point cloud data and the plane feature point set; according to all the first feature distances and all the second feature distances, a least squares optimization function is constructed, and according to the optimal solution of the least squares optimization function, the current posture of the robot is determined, and the optimal solution is used to characterize the posture transformation relationship between the first point cloud data and the second point cloud data. The method is based on the point cloud data corresponding to each of the multiple scanning cycles, realizes the registration of the point cloud key frames, and combines the optimal solution of the least squares optimization function to determine the current posture of the robot with higher accuracy, so that the positioning result of the robot with higher accuracy can be determined later.

[0107] The robot posture determination device provided by the present invention is described below. The robot posture determination device described below and the robot posture determination method described above can be referred to each other.

[0108] like Figure 5 FIG. 1 is a schematic diagram of the structure of the device for determining the position and posture of a robot provided by the present invention, which may include:

[0109] The point cloud acquisition module 501 is used to acquire first point cloud data collected by the robot in a first scanning cycle and second point cloud data collected in a second scanning cycle, where the second scanning cycle is later than the first scanning cycle;

[0110] A point set construction module 502, configured to construct an edge feature point set and a plane feature point set corresponding to the second point cloud data according to the curvature of each first sub-point cloud data in the second point cloud data;

[0111] The data processing module 503 is used to determine, for each second sub-point cloud data in the first point cloud data, the first feature distance according to the second sub-point cloud data and the edge feature point set, and to determine the second feature distance according to the second sub-point cloud data and the plane feature point set; construct a least squares optimization function according to all the first feature distances and all the second feature distances, and determine the current posture of the robot according to the optimal solution of the least squares optimization function, wherein the optimal solution is used to characterize the posture transformation relationship between the first point cloud data and the second point cloud data.

[0112] Optionally, the point set construction module 502 is specifically used to determine the curvature of each first sub-point cloud data in the second point cloud data; construct the edge feature point set based on the first sub-point cloud data corresponding to the curvature greater than the preset curvature threshold; construct the plane feature point set based on the first sub-point cloud data corresponding to the curvature less than or equal to the preset curvature threshold.

[0113] Optionally, the data processing module 503 is specifically used to obtain the first coordinate corresponding to the second sub-point cloud data, the second coordinate corresponding to the first feature point in the edge feature point set, and the third coordinate corresponding to the second feature point; determine the first difference based on the first coordinate and the second coordinate, and determine the second difference based on the first coordinate and the third coordinate; determine the third difference based on the second coordinate and the third coordinate; determine the first feature distance based on the first difference, the second difference and the third difference.

[0114] Optionally, the data processing module 503 is specifically used to obtain the first coordinate corresponding to the second sub-point cloud data, and the fourth coordinate corresponding to the third feature point in the plane feature point set; determine the fourth difference based on the first coordinate and the fourth coordinate; determine the normal vector corresponding to the plane formed by the plane feature point set; and determine the second feature distance based on the fourth difference and the normal vector.

[0115] Optionally, the point set construction module 502 is specifically used to perform the following operations for each first sub-point cloud data: in the second point cloud data, obtain the adjacent sub-point cloud data of the first sub-point cloud data and the number of the adjacent sub-point cloud data; determine the curvature of the first sub-point cloud data based on the number, the coordinates corresponding to the first sub-point cloud data and the coordinates corresponding to the adjacent sub-point cloud data.

[0116] Optionally, the data processing module 503 is further used to obtain environmental information perceived by the robot in the current position; and construct global information of the environment where the robot is located based on the environmental information.

[0117] Optionally, the point cloud acquisition module 501 is specifically used to obtain the first initial point cloud data collected by the robot in the first scanning cycle, and the second initial point cloud data collected in the second scanning cycle; correct and filter the first initial point cloud data to obtain the first point cloud data, and correct and filter the second initial point cloud data to obtain the second point cloud data.

[0118] like Figure 6 As shown, it is a schematic diagram of the structure of the electronic device provided by the present invention, and the electronic device may include: a processor (processor) 610, a communication interface (Communications Interface) 620, a memory (memory) 630 and a communication bus 640, wherein the processor 610, the communication interface 620, and the memory 630 communicate with each other through the communication bus 640. The processor 610 can call the logic instructions in the memory 630 to execute the robot's posture determination method, which includes: obtaining first point cloud data collected by the robot in a first scanning cycle, and second point cloud data collected in a second scanning cycle, the second scanning cycle being later than the first scanning cycle; constructing an edge feature point set and a plane feature point set corresponding to the second point cloud data according to the curvature of each first sub-point cloud data in the second point cloud data; for each second sub-point cloud data in the first point cloud data, determining a first feature distance according to the second sub-point cloud data and the edge feature point set, and determining a second feature distance according to the second sub-point cloud data and the plane feature point set; constructing a least squares optimization function according to all first feature distances and all second feature distances, and determining the current posture of the robot according to the optimal solution of the least squares optimization function, the optimal solution being used to characterize the posture transformation relationship between the first point cloud data and the second point cloud data.

[0119] In addition, the logic instructions in the above-mentioned memory 630 can be implemented in the form of a software functional unit and can be stored in a computer-readable storage medium when it is sold or used as an independent product. Based on such an understanding, the technical solution of the present invention is essentially or the part that contributes to the prior art or the part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions to enable a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (Read-Only Memory, ROM), random access memory (Random Access Memory, RAM), disk or optical disk and other media that can store program codes.

[0120] On the other hand, the present invention also provides a computer program product, which includes a computer program. The computer program can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer can execute the robot posture determination method provided by the above methods, which method includes: obtaining first point cloud data collected by the robot in a first scanning cycle, and second point cloud data collected in a second scanning cycle, the second scanning cycle being later than the first scanning cycle; constructing an edge feature point set and a plane feature point set corresponding to the second point cloud data according to the curvature of each first sub-point cloud data in the second point cloud data; for each second sub-point cloud data in the first point cloud data, determining a first feature distance according to the second sub-point cloud data and the edge feature point set, and determining a second feature distance according to the second sub-point cloud data and the plane feature point set; constructing a least squares optimization function according to all first feature distances and all second feature distances, and determining the current posture of the robot according to the optimal solution of the least squares optimization function, the optimal solution being used to characterize the posture transformation relationship between the first point cloud data and the second point cloud data.

[0121] On the other hand, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, is implemented to execute the robot posture determination method provided by the above-mentioned methods, the method comprising: obtaining first point cloud data collected by the robot in a first scanning cycle, and second point cloud data collected in a second scanning cycle, the second scanning cycle being later than the first scanning cycle; constructing an edge feature point set and a plane feature point set corresponding to the second point cloud data according to the curvature of each first sub-point cloud data in the second point cloud data; for each second sub-point cloud data in the first point cloud data, determining a first feature distance according to the second sub-point cloud data and the edge feature point set, and determining a second feature distance according to the second sub-point cloud data and the plane feature point set; constructing a least squares optimization function according to all first feature distances and all second feature distances, and determining the current posture of the robot according to the optimal solution of the least squares optimization function, the optimal solution being used to characterize the posture transformation relationship between the first point cloud data and the second point cloud data.

[0122] The device embodiments described above are merely illustrative, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Ordinary technicians in this field can understand and implement it without paying creative labor.

[0123] Through the description of the above implementation methods, those skilled in the art can clearly understand that each implementation method can be implemented by means of software plus a necessary general hardware platform, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solution is essentially or the part that contributes to the prior art can be embodied in the form of a software product, and the computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a disk, an optical disk, etc., including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or some parts of the embodiments.

[0124] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for determining the position and posture of a robot, characterized in that: include: Acquire first point cloud data collected by the robot in a first scanning cycle, and second point cloud data collected in a second scanning cycle, wherein the second scanning cycle is later than the first scanning cycle; Constructing an edge feature point set and a plane feature point set corresponding to the second point cloud data according to the curvature of each first sub-point cloud data in the second point cloud data; For each second sub-point cloud data in the first point cloud data, determining a first characteristic distance according to the second sub-point cloud data and the edge feature point set, and determining a second characteristic distance according to the second sub-point cloud data and the plane feature point set; A least squares optimization function is constructed based on all first feature distances and all second feature distances, and the current posture of the robot is determined based on the optimal solution of the least squares optimization function, wherein the optimal solution is used to characterize the posture transformation relationship between the first point cloud data and the second point cloud data.

2. The method according to claim 1, characterized in that The step of constructing an edge feature point set and a plane feature point set corresponding to the second point cloud data according to the curvature of each first sub-point cloud data in the second point cloud data comprises: Determining the curvature of each of the first sub-point cloud data in the second point cloud data; Constructing the edge feature point set according to the first sub-point cloud data corresponding to the curvature greater than a preset curvature threshold; The plane feature point set is constructed according to the first sub-point cloud data corresponding to the curvature that is less than or equal to the preset curvature threshold.

3. The method according to claim 1, characterized in that The determining of the first feature distance according to the second sub-point cloud data and the edge feature point set includes: Acquire a first coordinate corresponding to the second sub-point cloud data, a second coordinate corresponding to the first feature point in the edge feature point set, and a third coordinate corresponding to the second feature point; Determine a first difference value according to the first coordinate and the second coordinate, and determine a second difference value according to the first coordinate and the third coordinate; Determine a third difference value according to the second coordinate and the third coordinate; The first characteristic distance is determined according to the first difference, the second difference and the third difference.

4. The method according to claim 1, characterized in that: The determining a second characteristic distance according to the second sub-point cloud data and the plane characteristic point set comprises: Acquire a first coordinate corresponding to the second sub-point cloud data and a fourth coordinate corresponding to the third feature point in the plane feature point set; Determine a fourth difference value according to the first coordinate and the fourth coordinate; Determine a normal vector corresponding to the plane formed by the plane feature point set; The second characteristic distance is determined according to the fourth difference and the normal vector.

5. The method according to claim 2, characterized in that: The determining the curvature of each first sub-point cloud data in the second point cloud data includes: The following operations are performed for each of the first sub-point cloud data: In the second point cloud data, obtaining adjacent sub-point cloud data of the first sub-point cloud data and the number of the adjacent sub-point cloud data; The curvature of the first sub-point cloud data is determined according to the quantity, the coordinates corresponding to the first sub-point cloud data, and the coordinates corresponding to the adjacent sub-point cloud data.

6. The method according to any one of claims 1 to 5, characterized in that: The method further comprises: Obtaining environmental information perceived by the robot at its current position; Based on the environmental information, global information of the environment where the robot is located is constructed.

7. The method according to any one of claims 1 to 5, characterized in that: The step of acquiring first point cloud data collected by the robot in a first scanning cycle and second point cloud data collected in a second scanning cycle comprises: Acquire first initial point cloud data collected by the robot in the first scanning cycle and second initial point cloud data collected in the second scanning cycle; The first initial point cloud data is corrected and filtered to obtain the first point cloud data, and the second initial point cloud data is corrected and filtered to obtain the second point cloud data.

8. A robot posture determination device, characterized in that: include: A point cloud acquisition module, used to acquire first point cloud data collected by the robot in a first scanning cycle, and second point cloud data collected in a second scanning cycle, wherein the second scanning cycle is later than the first scanning cycle; A point set construction module, used to construct an edge feature point set and a plane feature point set corresponding to the second point cloud data according to the curvature of each first sub-point cloud data in the second point cloud data; A data processing module is used to determine, for each second sub-point cloud data in the first point cloud data, a first feature distance based on the second sub-point cloud data and the edge feature point set, and to determine a second feature distance based on the second sub-point cloud data and the plane feature point set; construct a least squares optimization function based on all first feature distances and all second feature distances, and determine the current posture of the robot based on the optimal solution of the least squares optimization function, wherein the optimal solution is used to characterize the posture transformation relationship between the first point cloud data and the second point cloud data.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the program, the method for determining the position and posture of the robot as described in any one of claims 1 to 7 is implemented.

10. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method for determining the position and posture of the robot as claimed in any one of claims 1 to 7 is implemented.

Images