CN105509748B - Robot navigation method and device - Google Patents

Abstract

The invention is applicable to the field of robotics technology, and provides a navigation method and device for a robot, comprising: inputting the point cloud data set P _k and the point cloud data set X of the previous frame collected by the somatosensory sensor into a KD tree matcher, Find the corresponding data point closest to P _k in X, and generate a set of point pairs Y _k =C(P _k ,X), where k is the number of iterations, initialization k=0, and P ₀ is collected by the somatosensory sensor The point cloud data set of the current frame; the point cloud data set P _k is registered with the point cloud data set X; before the preset termination condition is not reached, set k=k+1, return to execute Describe the operation of inputting the point cloud data set P _k and the point cloud data set X of the previous frame collected by the somatosensory sensor into the KD tree matcher; if the preset termination condition has been reached, according to the current point cloud data The set _Pk corrects the state change of the robot. The embodiment of the present invention greatly reduces the manufacturing cost of the robot.

Description

Robot navigation method and device

technical field

The invention belongs to the technical field of robots, and in particular relates to a navigation method and device of a robot.

Background technique

Since the advent of mobile machines at Stanford University in the 1960s, with the development of science and technology, the application fields of mobile robots have also become more and more extensive, extending from industry to home, service, entertainment, military, etc. field. The operation of mobile robots is based on navigation, which requires the robot to understand the map of its environment and locate itself. Traditional indoor mobile robots usually use laser equipment for navigation. Although laser equipment has high navigation accuracy, its The price of tens of thousands brings the high manufacturing cost of mobile robots.

SUMMARY OF THE INVENTION

In view of this, embodiments of the present invention provide a navigation method and device for a robot, so as to solve the problem that the existing robot navigation technology will lead to high manufacturing cost of the robot.

In a first aspect, a navigation method for a robot is provided, wherein the robot has a built-in somatosensory sensor, and the method includes:

Input the point cloud data set P _k and the point cloud data set X of the previous frame collected by the somatosensory sensor into the KD tree matcher, find the corresponding data point closest to P _k in X, and generate a point pair set Y _k = C(P _k , X), wherein the k is the number of iterations, the initialization k=0, and P ₀ is the point cloud data set of the current frame collected by the somatosensory sensor;

registering the point cloud dataset P _k with the point cloud dataset X;

Before the preset termination condition is reached, set k=k+1, and return to the input KD tree matcher to perform the inputting of the point cloud data set P _k and the point cloud data set X of the previous frame collected by the somatosensory sensor operation;

If the preset termination condition has been reached, the state change amount of the robot is corrected according to the current point cloud data set P _k .

In a second aspect, a navigation device for a robot is provided, the robot has a built-in somatosensory sensor, and the device includes:

The input unit is used to input the point cloud data set P _k and the point cloud data set X of the previous frame collected by the somatosensory sensor into the KD tree matcher, find the corresponding data point closest to P _k in X, and generate a point For the set Y _k =C(P _k ,X), wherein the k is the number of iterations, the initialization k=0, and P ₀ is the point cloud data set of the current frame collected by the somatosensory sensor;

a registration unit, configured to register the point cloud data set P _k with the point cloud data set X;

a first return unit, configured to set k=k+1 before the preset termination condition is reached, and return to execute the operation of the input unit;

The first correction unit is configured to correct the state change of the robot according to the current point cloud data set P _k if the preset termination condition has been reached.

In the embodiment of the present invention, the somatosensory sensor is used to replace the laser device to realize the pose correction during the robot navigation process, which greatly reduces the manufacturing cost of the robot.

Description of drawings

In order to illustrate the technical solutions in the embodiments of the present invention more clearly, the following briefly introduces the accompanying drawings that need to be used in the description of the embodiments or the prior art. Obviously, the drawings in the following description are only for the present invention. In some embodiments, for those of ordinary skill in the art, other drawings can also be obtained according to these drawings without any creative effort.

1 is a structural block diagram of a robot hardware system provided by an embodiment of the present invention;

2 is a structural block diagram of a robot software system provided by an embodiment of the present invention;

Fig. 3 is the realization flow chart of the navigation method of the robot provided by the embodiment of the present invention;

4 is a schematic diagram of a processing module of a navigation method for a robot provided by an embodiment of the present invention;

Fig. 5 is the realization flow chart of the navigation method of the robot provided by another embodiment of the present invention;

6 is a schematic diagram of point set alignment of a navigation method for a robot provided by an embodiment of the present invention;

FIG. 7 is a structural block diagram of a navigation device for a robot provided by an embodiment of the present invention.

Detailed ways

In the following description, for the purpose of illustration rather than limitation, specific details such as specific system structures and technologies are set forth in order to provide a thorough understanding of the embodiments of the present invention. However, it will be apparent to those skilled in the art that the present invention may be practiced in other embodiments without these specific details. In other instances, detailed descriptions of well-known systems, devices, circuits, and methods are omitted so as not to obscure the description of the present invention with unnecessary detail.

FIG. 1 shows a structural block diagram of a robot hardware system provided by an embodiment of the present invention. For convenience of description, only parts related to this embodiment are shown.

As shown in Figure 1, the hardware system of the robot is mainly composed of the following parts:

1. Industrial control computer: It is equipped with an operating system for data processing and storage, and provides services for other modules in the hardware system;

2. Somatosensory sensor: used to collect the color and depth information of the environment where the robot is located;

3. Motion sensor: used to obtain the motion information of the robot, including sensors such as code disc and gyroscope;

4. Drive control unit: It drives the robot to walk by inputting the drive signal into the underlying controller.

5. Other modules: including the display module, the communication module and the communication interface between the modules.

FIG. 2 shows a structural block diagram of a robot software system provided by an embodiment of the present invention. For convenience of description, only parts related to this embodiment are shown.

As shown in Figure 2, the software system of the robot is mainly composed of the following parts from the bottom to the top:

1. Operating system: it is mounted on the industrial control computer;

2. Robot driver;

3. Robot Operating System (ROS);

4. Navigation function package: it is installed in the robot operating system.

In the embodiment of the present invention, the industrial control computer is used as the main calculation processing unit, the somatosensory sensor and the code disc send the collected data to the industrial control computer, and the map is constructed by calculation and the positioning of the mobile robot is realized. In the process of navigation, the mobile robot controls the robot to drive and walk through the underlying embedded system.

Based on the software and hardware structures of the robot shown in FIG. 1 and FIG. 2 , the following describes the navigation method of the robot provided by the embodiment of the present invention:

FIG. 3 shows the implementation process of the navigation method of the robot provided by the embodiment of the present invention, and the details are as follows:

In S301, input the point cloud data set P _k and the point cloud data set X of the previous frame collected by the somatosensory sensor into a KD (K-dimensional, K-dimensional) tree matcher, and find the closest to P _k in X The corresponding data points of , generate a point pair set Y _k =C(P _k ,X), where the k is the number of iterations, the initialization k=0, and P ₀ is the point cloud data of the current frame collected by the somatosensory sensor set.

The rate of image acquisition by the somatosensory sensor is 30 frames per second. Through the iterative closest point method, the point cloud data of the current frame collected by the somatosensory sensor is continuously matched with the point cloud data of the previous frame to achieve further pose correction. As shown in Figure 4, the data point filter takes point cloud data as input, and outputs another point cloud data after processing. The processing methods include adding descriptors, reducing the number of data points by sampling, etc., and can also use multiple data at the same time. Point filter, the user can choose as needed.

Further, after S301 and before S302, the method further includes:

The point pair set Y _{k is detected by the outer filter, and the point pairs in the point pair set Y k that} do not meet the preset rules are removed.

For example, it is possible to judge whether the distance between two points exceeds a certain threshold, and if so, remove the two points.

In S302, the point cloud dataset P _k is registered with the point cloud dataset X.

Specifically, the eigenvector q _R =[q ₀ q ₁ q ₂ q ₃ ] of the matrix Q(∑ _py ) can be calculated first, where, The ∑ _py is the co-covariance matrix of the point sets P _k and Y _k , I ₃ is the third-order identity matrix, Δ=[A ₂₃ A ₃₁ A ₁₂ ] ^T ,

Secondly, according to the feature vector q _R , the rotation matrix R and the translation vector q _T are calculated, and the registration vector q=[q _R | q _T ] ^T is obtained, so as to realize the combination of the point cloud dataset P _k with the point cloud Data set X for registration. in: q _T = μ _y −R (q _R ) μ _p , where μ _y and μ _p are the centroids of point sets Y _k and P _k , respectively.

In S303, before the preset termination condition is reached, set k=k+1, and return to execute the input of the point cloud data set P _k and the point cloud data set X of the previous frame collected by the somatosensory sensor Operation of the KD tree matcher.

In S304, if the preset termination condition has been reached, the state change amount of the robot is corrected according to the current point cloud data set _Pk .

In this embodiment of the present invention, the preset termination conditions include:

The value of k exceeds the preset number of iterations; or,

The error from the point cloud data set P _k to the point cloud data set X is less than a preset error value.

Further, while the robot is moving, its motion sensor will acquire the state change of the robot in unit time. Generally speaking, due to the influence of factors such as sliding and offset between the wheels of the robot and the smooth ground, the reliability of the state change obtained by the motion sensor is not high. Therefore, in the embodiment of the present invention, Figure 5 is used. The method shown to increase the confidence of the state change obtained by the motion sensor:

In S501, a correction parameter of the robot motion sensor is obtained, where the correction parameter includes a linear correction parameter and a rotation correction parameter.

For example, using the correction program that comes with the built-in gyroscope of the robot, after multiple verifications and verifications, the linear correction parameters and rotational correction parameters of the gyroscope are determined.

In S502, according to the state of each movement of the robot, the possible error increment direction and value of the robot during this movement are calculated, and combined with the state change obtained by the motion sensor, and the corrected state change is obtained by calculation .

In S503, the correction parameter is combined with the corrected state change, and the state change of the robot is estimated.

The final state change obtained is a value with high reliability.

Further, it is assumed that the robot scans S _ref once at the initial pose P _ref , and then reaches a new pose P _new after a small movement, and scans S _new again on this pose, usually, in the above During the process, _Pref and _Pnew can be obtained from the motion sensor, however, in order to further improve the accuracy of _Pref and _Pnew , _Pref and Pnew can be obtained by aligning the points in the two scans _Sref and _Snew A more precise relationship between _new to correct for the amount of state change obtained by the motion sensor, as follows:

Step 1: Initialize the preset threshold e, and initialize parameters l=0, d _l =d ₀ =0.

Exemplarily, the preset threshold e may be set to 0.0001.

Step 2: For each point P _n on the new pose S _new of the robot, find the point P _r with the closest distance to the robot initial pose S _ref and the distance is less than the preset threshold , forming a set of point pairs

Step 3: Calculate the rotation matrix R _w and translation matrix T of the set of point pairs, and calculate

Step 4: Calculate Wherein, the num is the set of point pairs The number of pairs in .

Step 5: If |d _l -d _l+1 |<e, output the R _w and the T, and correct the state change of the robot according to the R _w and the T.

Step 6: If |d _l -d _l+1 |≥e, set k=l+1, and return to step 2.

Through the above algorithm, the change matrix between two adjacent state changes can be obtained according to the scanning of two adjacent frames, so as to obtain the corrected state change of the robot. As shown in Figure 6, with each point on S _new , find the closest point corresponding to it on S _ref , if the distance between them is less than the set threshold, they are considered to be paired points, and then Calculate the rotation matrix R _w and the translation matrix T between the paired points, and iteratively calculate, so that these paired points can be better aligned, and the distance function between them has been reduced.

In the embodiment of the present invention, the somatosensory sensor is used to replace the laser device to realize the pose correction during the robot navigation process, which greatly reduces the manufacturing cost of the robot. At the same time, the correction strategy of the measurement value of the motion sensor and the fusion strategy of various matching algorithms It also helps to map the environment more accurately and determine the state of the robot.

It should be understood that the size of the sequence numbers of the steps in the above embodiments does not mean the sequence of execution, and the execution sequence of each process should be determined by its function and internal logic, and should not constitute any limitation to the implementation process of the embodiments of the present invention.

Corresponding to the robot navigation method described in the above embodiments, FIG. 7 shows a structural block diagram of a robot navigation device provided by an embodiment of the present invention. For convenience of explanation, only the parts related to this embodiment are shown.

Referring to Figure 7, the device includes:

The input unit 71 inputs the point cloud data set P _k and the point cloud data set X of the previous frame collected by the somatosensory sensor into the KD tree matcher, finds the corresponding data point closest to P _k in X, and generates a point pair Set Y _k =C(P _k ,X), wherein k is the number of iterations, initialization k=0, and P ₀ is the point cloud data set of the current frame collected by the somatosensory sensor;

A registration unit 72, for registering the point cloud data set P _k with the point cloud data set X;

The first return unit 73, before the preset termination condition is not reached, sets k=k+1, and returns to execute the operation of the input unit;

The first correction unit 74, if the preset termination condition has been reached, corrects the state change amount of the robot according to the current point cloud data set P _k .

Optionally, the device further includes:

an acquisition unit to acquire correction parameters of the motion sensor of the robot, where the correction parameters include linear correction parameters and rotational correction parameters;

The first calculation unit, according to the state of each movement of the robot, calculates the possible error increment direction and value of the robot in this movement process, and combines it with the state change obtained by the motion sensor, and the calculation is obtained. Corrected state change amount;

The estimation unit combines the correction parameter and the corrected state change to obtain the state change of the robot by estimation.

Optionally, the device further includes:

an initialization unit, initializing a preset threshold e, and initialization parameters l=0, d _l =d ₀ =0;

A search unit, for each point P _n on the new pose S _new of the robot, find the point P _r with the closest distance to it and the distance is less than the preset threshold on the initial pose S _ref of the robot , forming a set of point pairs

The second calculation unit calculates the rotation matrix R _w and the translation matrix T of the point pair set, and calculates

The third computing unit, computing Wherein, the num is the set of point pairs The number of pairs in;

The output unit, if |d _l -d _l+1 |<e, output the R _w and the T, and correct the state change of the robot according to the R _w and the T;

The second returning unit, if |d _l -d _l+1 |≥e, let l=l+1, and return to execute the operation of the searching unit.

Optionally, the preset termination conditions include:

The value of k exceeds the preset number of iterations; or,

The error from the point cloud data set P _k to the point cloud data set X is less than a preset error value.

Optionally, the device further includes:

The removing unit detects the point pair set Y _k , and removes point pairs that do not meet the preset rules in the point pair set Y _k .

Those skilled in the art can clearly understand that, for the convenience and simplicity of description, only the division of the above-mentioned functional units and modules is used as an example for illustration. In practical applications, the above-mentioned functions can be allocated to different functional units, Module completion, that is, dividing the internal structure of the device into different functional units or modules to complete all or part of the functions described above. Each functional unit and module in the embodiment may be integrated in one processing unit, or each unit may exist physically alone, or two or more units may be integrated into one unit, and the above-mentioned integrated units may adopt hardware. It can also be realized in the form of software functional units. In addition, the specific names of the functional units and modules are only for the convenience of distinguishing from each other, and are not used to limit the protection scope of the present application. For the specific working processes of the units and modules in the above-mentioned system, reference may be made to the corresponding processes in the foregoing method embodiments, which will not be repeated here.

Those of ordinary skill in the art can realize that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Skilled artisans may implement the described functionality using different methods for each particular application, but such implementations should not be considered beyond the scope of the present invention.

In the embodiments provided by the present invention, it should be understood that the disclosed apparatus and method may be implemented in other manners. For example, the system embodiments described above are only illustrative. For example, the division of the modules or units is only a logical function division. In actual implementation, there may be other division methods. For example, multiple units or components may be Incorporation may either be integrated into another system, or some features may be omitted, or not implemented. On the other hand, the shown or discussed mutual coupling or direct coupling or communication connection may be through some interfaces, indirect coupling or communication connection of devices or units, and may be in electrical, mechanical or other forms.

The units described as separate components may or may not be physically separated, and components displayed as units may or may not be physical units, that is, may be located in one place, or may be distributed to multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution in this embodiment.

In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically alone, or two or more units may be integrated into one unit. The above-mentioned integrated units may be implemented in the form of hardware, or may be implemented in the form of software functional units.

The integrated unit, if implemented in the form of a software functional unit and sold or used as an independent product, may be stored in a computer-readable storage medium. Based on such understanding, the technical solutions of the embodiments of the present invention are essentially or contribute to the prior art, or all or part of the technical solutions may be embodied in the form of software products, and the computer software products are stored in a storage The medium includes several instructions to cause a computer device (which may be a personal computer, a server, or a network device, etc.) or a processor (processor) to execute all or part of the steps of the methods described in the embodiments of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, Read-Only Memory (ROM, Read-Only Memory), Random Access Memory (RAM, Random Access Memory), magnetic disk or optical disk and other media that can store program codes .

The above-mentioned embodiments are only used to illustrate the technical solutions of the present invention, but not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: it is still possible to implement the foregoing implementations. The technical solutions described in the examples are modified, or some technical features thereof are equivalently replaced; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions in the embodiments of the present invention.

The above descriptions are only preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements and improvements made within the spirit and principles of the present invention shall be included in the protection of the present invention. within the range.

Claims (8)

1. the navigation method of a robot, is characterized in that, built-in somatosensory sensor in described robot, described method comprises: Input the point cloud data set P _k and the point cloud data set X of the previous frame collected by the somatosensory sensor into the KD tree matcher, find the corresponding data point closest to P _k in X, and generate a point pair set Y _k = C(P _k , X), wherein the k is the number of iterations, the initialization k=0, and P ₀ is the point cloud data set of the current frame collected by the somatosensory sensor; registering the point cloud dataset P _k with the point cloud dataset X; Before the preset termination condition is reached, set k=k+1, and return to the input KD tree matcher to perform the inputting of the point cloud data set P _k and the point cloud data set X of the previous frame collected by the somatosensory sensor operation; If the preset termination condition has been reached, correct the state change of the robot according to the current point cloud data set P _k ; Initialize the preset threshold e, and the initialization parameters l=0, d _l =d ₀ =0; For each point P _n on the new pose S _new of the robot, find the point P _r that is closest to it and the distance is less than the preset threshold on the initial pose S _ref of the robot to form a point pair collection compute the set of point pairs the rotation matrix R _w and translation matrix T, and calculate calculate Wherein, the num is the set of point pairs the number of pairs in ; the Represents the point cloud dataset obtained after the l+1th iteration the ith point in , Represents a point cloud dataset the i-th point in ; If |d _l -d _l+1 |<e, output the R _w and the T, and correct the state change of the robot according to the R _w and the T; If |d _l -d _l+1 |≥e, let l=l+1, return to execute the new pose S _new for each point P _n of the robot, at the initial pose S of the robot Find the point P _r on the _ref that is the closest to it and the distance is less than the preset threshold to form a point pair set operation. 2. The method according to claim 1, wherein, before the point cloud data set P _k and the point cloud data set X of the previous frame collected by the somatosensory sensor are input into the KD tree matcher, the The method also includes: Acquiring correction parameters of the motion sensor of the robot, where the correction parameters include linear correction parameters and rotational correction parameters; According to the state of each movement of the robot, the possible error increment direction and value of the robot during this movement are calculated, and combined with the state change obtained by the motion sensor to obtain the corrected state change. ; Combining the correction parameter and the corrected state change, the state change of the robot is estimated. 3. The method of claim 1, wherein the preset termination condition comprises: The value of k exceeds the preset number of iterations; or, The error from the point cloud data set P _k to the point cloud data set X is less than a preset error value. 4. The method according to claim 1, wherein, in the point cloud data set P _k and the point cloud data set X of the previous frame collected by the somatosensory sensor, the KD tree matcher is input, and in X After searching for the corresponding data point closest to P _k , and generating a point pair set Y _k =C(P _k , X), before the point cloud dataset P _k is registered with the point cloud dataset X , the method also includes: The point pair set Y _{k is detected, and the point pairs in the point pair set Y k that} do not meet the preset rules are removed. 5. A navigation device for a robot, wherein a somatosensory sensor is built in the robot, and the device comprises: The input unit is used to input the point cloud data set P _k and the point cloud data set X of the previous frame collected by the somatosensory sensor into the KD tree matcher, find the corresponding data point closest to P _k in X, and generate a point For the set Y _k =C(P _k ,X), wherein the k is the number of iterations, the initialization k=0, and P ₀ is the point cloud data set of the current frame collected by the somatosensory sensor; a registration unit, configured to register the point cloud data set P _k with the point cloud data set X; a first return unit, configured to set k=k+1 before the preset termination condition is reached, and return to execute the operation of the input unit; a first correction unit, used for correcting the state change of the robot according to the current point cloud data set P _k if the preset termination condition has been reached; an initialization unit, used to initialize a preset threshold e, and initialization parameters l=0, d _l =d ₀ =0; A searching unit for searching for each point P _n on the new pose S _new of the robot on the initial pose S _ref of the robot to find the point with the closest distance to it and the distance is less than the preset threshold P _r , which constitutes a set of point pairs a second computing unit for computing the set of point pairs the rotation matrix R _w and translation matrix T, and calculate i represents the ith point, l is k, a third computing unit for computing Wherein, the num is the set of point pairs the number of pairs in ; the Represents the point cloud dataset obtained after the l+1th iteration the ith point in , Represents a point cloud dataset the ith point in ; an output unit, configured to output the R _w and the T if |d _l -d _l+1 |<e, and correct the state change of the robot according to the R _w and the T; The second returning unit is used for returning to execute the operation of the searching unit if |d _l -d _l+1 |≥e, let l=l+1. 6. The apparatus of claim 5, wherein the apparatus further comprises: an acquisition unit, configured to acquire correction parameters of the motion sensor of the robot, where the correction parameters include linear correction parameters and rotational correction parameters; The first calculation unit is used to calculate the possible error increment direction and value of the robot during this movement according to the state of each movement of the robot, and combine them with the state change obtained by the motion sensor, Calculate the corrected state change; An estimation unit, configured to combine the correction parameter and the corrected state change to obtain the state change of the robot by estimation. 7. The apparatus of claim 5, wherein the preset termination condition comprises: The value of k exceeds the preset number of iterations; or, The error from the point cloud data set P _k to the point cloud data set X is less than a preset error value. 8. The apparatus of claim 5, wherein the apparatus further comprises: A removal unit, configured to detect the point pair set Y _k , and remove the point pairs that do not meet the preset rules in the point pair set Y _k .