CN113390408A - Robot positioning method and device, robot and storage medium - Google Patents

Abstract

The present application is applicable to the field of robotics, and provides a robot positioning method, device, robot and storage medium, wherein the method includes: obtaining the first pose of the robot according to image data collected by two monocular cameras with different fields of view, respectively. According to the odometer data collected by the wheeled odometer, the second pose data of the robot is obtained; according to the fusion of the first pose data and the second pose data, the real pose data of the robot is obtained. In this application, the visual range of the robot can be increased by using two monocular cameras with different fields of view, and the first pose data obtained based on the two monocular cameras and the second pose data obtained based on the wheel odometer are fused. , the precise pose data of the robot can be obtained, thereby improving the positioning accuracy of the robot.

Description

A robot positioning method, device, robot and storage medium

technical field

The present application belongs to the field of robot technology, and in particular relates to a robot positioning method, device, robot and storage medium.

Background technique

In the positioning and mapping of the robot, it is necessary to obtain an accurate pose estimation of the robot. Vision sensors have the advantages of low cost and rich information, and are often used for visual positioning and mapping of robots. At present, most of the visual positioning is based on a monocular camera, lacking the real world scale, and some use the positioning method based on the camera and the Inertial Measurement Unit (IMU), but in actual use, it is found that the IMU is stationary when the robot is stationary When , zero-speed drift is likely to occur, resulting in the robot's pose error, and the IMU will vibrate when the robot moves, resulting in an inaccurate IMU pre-integration value and affecting the positioning accuracy.

SUMMARY OF THE INVENTION

The embodiments of the present application provide a robot positioning method, device, robot, and storage medium to solve the problem of visual positioning based on a monocular camera, lack of real world scale, and a positioning method based on a camera and an IMU. When the robot is stationary, the IMU can easily Zero-speed drift occurs, resulting in the robot's pose error, and the IMU will vibrate when the robot moves, resulting in inaccurate IMU pre-integration values and affecting positioning accuracy.

A first aspect of the embodiments of the present application provides a method for positioning a robot, including:

Obtain the first pose data of the robot according to the image data collected by the two monocular cameras with different fields of view respectively;

Obtain the second pose data of the robot according to the odometer data collected by the wheel odometer;

According to the fusion of the first pose data and the second pose data, the real pose data of the robot is obtained.

A second aspect of the embodiments of the present application provides a robot positioning device, including:

a first pose obtaining unit, configured to obtain the first pose data of the robot according to the image data collected by the two monocular cameras with different fields of view;

A second pose obtaining unit, configured to obtain the second pose data of the robot according to the odometer data collected by the wheeled odometer;

A pose fusion unit, configured to fuse the first pose data and the second pose data to obtain the real pose data of the robot.

A third aspect of the embodiments of the present application provides a robot, including a processor and a computer program stored in the memory and executable on the processor, when the processor executes the computer program, the present invention is implemented The steps of the robot positioning method described in the first aspect of the application embodiments.

A fourth aspect of the embodiments of the present application provides a computer-readable storage medium, where the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the first aspect of the embodiments of the present application is implemented Describe the steps of the robot positioning method.

In the robot positioning method provided by the first aspect of the embodiments of the present application, the first pose data of the robot is obtained according to the image data collected by two monocular cameras with different fields of view respectively; according to the odometer data collected by the wheel odometer, Obtain the second pose data of the robot; fuse the first pose data and the second pose data to obtain the real pose data of the robot, and increase the visual range of the robot by using two monocular cameras with different fields of view , after fusing the first pose data obtained based on the two monocular cameras and the second pose data obtained based on the wheeled odometer, the precise pose data of the robot can be obtained, thereby improving the positioning accuracy of the robot.

It can be understood that, for the beneficial effects of the foregoing second aspect to the fourth aspect, reference may be made to the relevant descriptions in the foregoing first aspect, and details are not described herein again.

Description of drawings

In order to illustrate the technical solutions in the embodiments of the present application 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 application. 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.

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

2 is a first schematic flow chart of a robot positioning method provided by an embodiment of the present application;

3 is a second schematic flowchart of the robot positioning method provided by the embodiment of the present application;

4 is a third schematic flowchart of the robot positioning method provided by the embodiment of the present application;

5 is a sequence diagram of image data and odometer data provided by an embodiment of the present application;

6 is a fourth schematic flowchart of the robot positioning method provided by the embodiment of the present application;

7 is a schematic structural diagram of a robot positioning device provided by an embodiment of the present application;

FIG. 8 is a schematic diagram of a second structure of the robot provided by the embodiment of the present application.

Detailed ways

In the following description, for the purpose of illustration rather than limitation, specific details such as a specific system structure and technology are set forth in order to provide a thorough understanding of the embodiments of the present application. However, it will be apparent to those skilled in the art that the present application 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 application with unnecessary detail.

It is to be understood that, when used in this specification and the appended claims, the term "comprising" indicates the presence of the described feature, integer, step, operation, element and/or component, but does not exclude one or more other The presence or addition of features, integers, steps, operations, elements, components and/or sets thereof.

It will also be understood that, as used in this specification and the appended claims, the term "and/or" refers to and including any and all possible combinations of one or more of the associated listed items.

As used in the specification of this application and the appended claims, the term "if" may be contextually interpreted as "when" or "once" or "in response to determining" or "in response to detecting ". Similarly, the phrases "if it is determined" or "if the [described condition or event] is detected" may be interpreted, depending on the context, to mean "once it is determined" or "in response to the determination" or "once the [described condition or event] is detected. ]" or "in response to detection of the [described condition or event]".

In addition, in the description of the specification of the present application and the appended claims, the terms "first", "second", "third", etc. are only used to distinguish the description, and should not be construed as indicating or implying relative importance.

References in this specification to "one embodiment" or "some embodiments" and the like mean that a particular feature, structure or characteristic described in connection with the embodiment is included in one or more embodiments of the present application. Thus, appearances of the phrases "in one embodiment," "in some embodiments," "in other embodiments," "in other embodiments," etc. in various places in this specification are not necessarily All refer to the same embodiment, but mean "one or more but not all embodiments" unless specifically emphasized otherwise. The terms "including", "including", "having" and their variants mean "including but not limited to" unless specifically emphasized otherwise.

The embodiment of the present application provides a method for positioning a robot, which can be executed by a processor of a robot when a corresponding computer program is run. The second pose data is obtained by the meter for fusion, and the precise pose data of the robot can be obtained, thereby improving the positioning accuracy of the robot.

In the application, the robot can be any type of robot with moving rollers. There are robot types with moving rollers in various types of robots such as service robots, entertainment robots, production robots, and agricultural robots. For example, bionic educational robots, bionic robots Welcome robot, bionic dancing robot, bionic nanny robot, etc.

In the application, the robot is provided with two monocular cameras with different fields of view and a wheeled odometer. The two monocular cameras are set at any position where the image data in the moving direction of the robot can be collected. The odometer can be integrated with the robot and be a part of the robot, or it can be an external device additionally installed in the robot and connected to the robot. The wheeled odometer is arranged on the driving roller. Usually, the robot includes a driving roller for driving the robot to move, and it can also include a driven roller. When the robot includes multiple driving rollers, the wheeled odometer should be arranged at the center of the plurality of driving rollers. , the center of the plurality of driving rollers is the geometric center of a polygon with the geometric center of each driving roller as the corner point, and the polygon is parallel to the motion plane of the robot.

As shown in FIG. 1 , a schematic diagram of the structure of the robot 100 is exemplarily shown; wherein, a monocular camera 1 is arranged on the upper front side of the robot 100 , and another monocular camera 2 is arranged on the upper rear side of the robot 100 , the wheel odometer 3 is arranged on the driving roller of the chassis of the robot 100, the direction of the dashed arrow indicates the movement direction of the robot 100, and it is indicated that the coordinate system of the monocular camera 1 is the camera coordinate system, and the X-axis direction of the camera coordinate system points to the robot 100 The right side, the Y-axis direction is the gravity direction, and the Z-axis direction is the same as the movement direction of the robot 100. It is indicated that the coordinate system of the wheel odometer 3 is the wheel odometer coordinate system, and the X-axis direction of the wheel odometer coordinate system It is the same as the moving direction of the robot 100 , the Y-axis direction points to the left side of the robot 100 , and the Z-axis direction is opposite to the direction of gravity.

As shown in FIG. 2 , the robot positioning method provided by the embodiment of the present application includes the following steps S201 to S203:

Step S201: Obtain the first pose data of the robot according to the image data collected by the two monocular cameras with different fields of view respectively.

In the application, the image data is collected by two monocular cameras synchronously, that is, the time stamps of the image data collected by the two monocular cameras are synchronized. At the same time, the two monocular cameras each collect one frame of image data. After synchronously collecting image data through two monocular cameras, one first pose data of the robot is obtained according to the image data collected by one of the monocular cameras, and another first pose data of the robot is obtained according to the image data collected by the other monocular camera. One pose data, that is, according to the image data collected by the two monocular cameras, respectively, can obtain two first pose data of the robot.

Step S202, obtaining the second pose data of the robot according to the odometer data collected by the wheeled odometer;

Step S203: Fusion is performed according to the first pose data and the second pose data to obtain the real pose data of the robot.

In the application, since the visual positioning based on the monocular camera lacks the real world scale, it is also necessary to collect the odometer data during the movement of the robot through the wheel odometer, and obtain the real world scale according to the odometer data. Then the two first pose data obtained based on the two monocular cameras and the second pose data obtained based on the wheel odometer are fused, and finally the real pose data of the robot is obtained.

As shown in FIG. 3, in one embodiment, step S201 includes the following steps S301 to S304:

Step S301 , for each monocular camera, perform feature point detection on the first frame of image data collected by the monocular camera, and extract a preset number of feature points in the first frame of image data.

In the application, for each monocular camera, it is necessary to perform feature point detection on the first frame of image data collected by the monocular camera to extract the preset number of the first frame of image data collected by each monocular camera. feature points. Any corner detection method can be used to extract feature points, for example, corner detection based on grayscale images, corner detection based on binary images, corner detection based on contour curves, etc. Specifically, FAST (Features fromAccelerated Segment Test) feature point detection method to extract feature points. Before the feature point extraction is performed, image processing may be performed on the first frame of image data, for example, histogram equalization (Histogram Equalization) processing is performed on the first frame of image data to adjust the contrast of the first frame of image data. The preset number can be set according to actual needs, for example, any value between 70 and 150, specifically 100.

Step S302: Perform feature point tracking on the k+1 th frame of image data collected by the monocular camera, where k=1, 2, . . . n, n is any positive integer.

In the application, after the feature point detection is performed on the first frame of image data collected by each monocular camera, it is necessary to continue to perform feature point tracking on each frame of image data subsequently collected by each monocular camera. Any corner tracking algorithm can be used to track a preset number of feature points in each subsequent frame of image data, for example, an optical flow method.

In one embodiment, after step S302, it further includes:

If the number of feature points in the k+1 th frame of image data tracked is less than the preset number, perform feature point detection on the k+1 th frame of image data, and extract the k+1 th frame of image data. The feature points in the frame image data, so that the sum of the tracked feature points in the k+1 th frame image data and the extracted feature points in the k+1 th frame image data is equal to the Preset quantity.

In the application, for each monocular camera, if the number of feature points tracked in any frame of image data after the first frame of image data collected by the monocular camera does not reach the preset number, the The feature point detection method in which the feature points in the frame image data are the same, extract the feature points in the image data of any frame to supplement the missing feature points, so that the tracked feature points in the image data of any frame are the same as those in the frame image data. The sum of the number of the extracted feature points in any frame of image data is equal to the preset number.

Step S303, constructing a reprojection error cost function according to the common view feature points between the multi-frame image data collected by the monocular camera;

Step S304: Minimize the reprojection error solution for the reprojection error cost function, and obtain the three-dimensional coordinates of a preset number of feature points in the world coordinate system in each frame of image data collected by the monocular camera, as the The first pose data of the robot.

In the application, for each monocular camera, the least squares optimization problem is constructed according to the common-view feature points between the multi-frame image data collected by the monocular camera, that is, the reprojection error cost function that minimizes the feature points is constructed. The reprojection error cost function is solved to obtain the solution that minimizes the reprojection error, that is, the three-dimensional coordinates of the preset number of feature points in each frame of image data in the world coordinate system are obtained, and the three-dimensional coordinates are the first coordinates of the robot. pose data.

As shown in FIG. 4, in one embodiment, step S202 includes the following steps S401 and S402:

Step S401: Align the time stamp of the image data collected by the monocular camera with the time stamp of the odometer data collected by the wheel odometer to obtain odometer data synchronized with the time stamp of the image data.

In the application, the frame rate of the two monocular cameras is the same and the image data is collected synchronously. Therefore, the time stamps of the image data collected by the two monocular cameras are the same, because the frame rate of the monocular camera and the collection frequency of the wheel odometer are the same. Usually different, therefore, it is necessary to align the time stamp of the image data collected by the monocular camera with the time stamp of the odometer data collected by the wheel odometer to obtain the odometer data synchronized with the image data in the collection time.

As shown in FIG. 5 , a time sequence diagram of image data and odometer data is exemplarily shown; wherein, the dotted line is the odometer data synchronized with the time stamp of the image data.

In one embodiment, step S401 includes:

If the frame rate of the monocular camera is different from the collection frequency of the wheel odometer, the time stamp of the image data collected by the monocular camera is aligned with the time stamp of the odometer data collected by the wheel odometer, Odometry data is obtained that is synchronized with the time stamp of the image data.

In the application, the frame rate of the monocular camera and the collection frequency of the wheel odometer may also be the same. Only when the two are different, the timestamp alignment will be performed. If the two are the same, the timestamp alignment will not be performed. Therefore, it can be determined first. Whether the frame rate of the monocular camera is the same as the acquisition frequency of the wheel odometer.

In one embodiment, step S401 includes:

Linear interpolation is performed on the odometer data collected by the wheel odometer according to the time stamp of the image data collected by the monocular camera, so as to align the time stamp of the image data with the time stamp of the odometer data, and obtain a The odometer data is synchronized with the timestamp of the image data.

In applications, linear interpolation can be used to align the timestamps of the image data with the timestamps of the odometry data.

Step S402: Pre-integrate the odometer data synchronized with the time stamp of the image data to obtain the integral value of the wheel odometer as the second pose data of the robot.

In the application, after aligning the time stamp of the image data and the time stamp of the odometer data, pre-integrate the odometer data after the time stamps are synchronized to obtain the integral value of the odometer data collected by the wheel odometer, and the integral value of the odometer data collected by the wheel odometer is obtained. The value is used as the second pose data that can reflect the real world scale.

In one embodiment, the expression of the second pose data is:

表示与第k+1帧图像数据的时间戳同步的所述轮式里程计的旋转角度的积分值，

表示与第k帧图像数据的时间戳同步的所述轮式里程计的旋转角度的积分值，ΔR表示所述第k+1帧图像数据的时间戳与所述第k帧图像数据的时间戳之间的所述轮式里程计的旋转角度的积分值的变化量，

表示与第k+1帧图像数据的时间戳同步的所述轮式里程计的位移的积分值，

表示与第k帧图像数据的时间戳同步的所述轮式里程计的位移的积分值，Δp表示所述第k+1帧图像数据的时间戳与所述第k帧图像数据的时间戳之间的所述轮式里程计的位移的积分值的变化量，G表示世界坐标系，O表示轮式里程计坐标系。in,

represents the integral value of the rotation angle of the wheel odometer synchronized with the time stamp of the k+1th frame of image data,

represents the integral value of the rotation angle of the wheel odometer synchronized with the time stamp of the kth frame of image data, ΔR represents the time stamp of the k+1th frame of image data and the time stamp of the kth frame of image data The amount of change between the integral value of the rotation angle of the wheel odometer,

represents the integral value of the displacement of the wheel odometer synchronized with the time stamp of the k+1th frame of image data,

represents the integral value of the displacement of the wheel odometer synchronized with the time stamp of the kth frame of image data, Δp represents the sum of the time stamp of the k+1th frame of image data and the time stamp of the kth frame of image data The amount of change in the integral value of the displacement of the wheel odometer between, G represents the world coordinate system, and O represents the wheel odometer coordinate system.

As shown in FIG. 6, in one embodiment, step S203 includes the following steps S601 to S603:

Step S601: Project the three-dimensional coordinates of the preset number of feature points in the world coordinate system back to the image coordinate system, and obtain the two-dimensional coordinates of the preset number of feature points in the image coordinate system.

In the application, for each monocular camera, the three-dimensional coordinates of a preset number of feature points in each frame of image data obtained by the monocular camera in the world coordinate system, that is, the first pose data, are projected back to the image coordinate system , to obtain the two-dimensional coordinates of a preset number of feature points in each frame of image data in the image coordinate system.

Step S602: Obtain the difference between the two-dimensional coordinates of the preset number of feature points under the image coordinate system and the extracted two-dimensional coordinates of the preset number of feature points in the first frame of image data under the image coordinate system. difference between.

In the application, after obtaining the two-dimensional coordinates of a preset number of feature points in the image coordinate system in each frame of image data in step S601, these two-dimensional coordinates and the first frame of image data extracted in step S301 are further obtained. The difference between the two-dimensional coordinates of the preset number of feature points in the image coordinate system, the specific method for obtaining the difference is: for each feature point, obtain the image where the feature point is located in the k+1th frame of image data The difference between the two-dimensional coordinates in the coordinate system and the two-dimensional coordinates of the feature point in the image coordinate system where the first frame of image data is located.

Step S603 , using the second pose data as an estimated value and the difference as an observation variable, perform extended Kalman filter fusion to obtain the real pose data of the robot in the wheeled odometer coordinate system.

In the application, after obtaining the difference between the feature points in the image data collected by each monocular camera according to step S602, the second pose data and the difference are used as the Extended Kalman Filter (EKF) ) fused estimated values and observed variables, and after data fusion, the real pose data of the robot in the wheeled odometer coordinate system is finally obtained.

In one embodiment, step S603 includes:

Taking the second pose data as an estimated value and the difference as an observation variable, performing extended Kalman filter fusion under constraints to obtain the real pose data of the robot in the wheeled odometer coordinate system, The constraint condition is that the X-axis component and the Y-axis component of the rotation angle of the robot in the wheeled odometer coordinate system are 0, and the Z-axis component of the real pose data is fixed.

In applications, since robots generally move on a plane, in order to further improve the positioning accuracy, plane constraints can be introduced as constraints for extended Kalman filter fusion. Since the pose data estimated based on the image data is three-dimensional, a high degree of drift will be introduced when tightly coupled with the odometer-based data. Therefore, plane constraints need to be added to eliminate this drift, that is, the robot solved under the constraints is in the The three-dimensional rotation angle in the wheel odometer coordinate system can only be rotated around the Z axis, the components of the X axis and the Y axis are 0, and the Z axis component of the pose data is fixed.

In one embodiment, step S603 includes:

The second pose data is used as the estimated value, the difference is used as the observation variable, and the relative rotation angle and relative displacement between the wheel odometer coordinate system and the camera coordinate system are used as external parameters, and extended Kalman filtering is performed. Fusion to obtain the real pose data of the robot in the wheeled odometer coordinate system.

In one embodiment, since the wheel odometer coordinate system and the camera coordinate system are different, coordinate transformation needs to be performed between the two to align the coordinates to realize coordinate calibration, and the wheel odometer coordinate system can be obtained by rough manual measurement. The relative rotation angle and relative displacement between the camera and the camera coordinate system are used as external parameters, which will be added to the state variables of the EKF for online calibration update to improve the accuracy of the final real pose data.

In one embodiment, after step S603:

Output the real pose data.

In the application, the robot can perform offline positioning and navigation according to its own real pose data, and can also output the real pose data to other devices, so that users of other devices can know the real pose data of the robot in real time, and based on the real pose data The orientation data is used to position and navigate the robot to control the robot to move according to the specified path to move to the specified position. Other devices may be any terminal devices capable of wireless communication with the robot, such as mobile phones, tablet computers, personal computers, smart bracelets, personal digital assistants, (cloud) servers, and the like.

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

The embodiments of the present application further provide a robot positioning device, which is used for performing the method steps in the above method embodiments. The appliance may be a virtual appliance in the robot, run by the robot's processor, or the robot itself.

As shown in FIG. 7 , the robot positioning device 200 provided by the embodiment of the present application includes:

The first pose obtaining unit 201 is configured to obtain the first pose data of the robot according to the image data collected by two monocular cameras with different fields of view respectively;

The second pose obtaining unit 202 is configured to obtain the second pose data of the robot according to the odometer data collected by the wheeled odometer;

The pose fusion unit 203 is configured to fuse the first pose data and the second pose data to obtain the real pose data of the robot.

In one embodiment, the robot positioning apparatus further includes an output unit for outputting the real pose data.

In application, each module in the above apparatus may be a software program module, or may be implemented by different logic circuits integrated in the processor or independent physical components connected to the processor, or may be implemented by multiple distributed processors.

As shown in FIG. 8, an embodiment of the present application further provides a robot 300, including: at least one processor 301 (only one processor is shown in FIG. 8), a memory 302, and a robot 300 that is stored in the memory 302 and can be processed in at least one The computer program 303 running on the processor 301 is executed. When the processor 301 executes the computer program 303, the steps in each of the above embodiments of the robot positioning method are implemented.

In application, the robot may include, but is not limited to, a processor and a memory. FIG. 8 is only an example of a robot, and does not constitute a limitation to the robot. It may include more or less components than those shown in the figure, or combine some Components, or different components, for example, may also include two monocular cameras, a wheel odometer, moving components, input and output devices, network access devices, and the like. The moving parts may include moving rollers, steering gears, motors, drivers and other devices for driving the movement of the robot. The input and output device may include the aforementioned human-computer interaction device, and may also include a display screen for displaying the working parameters of the robot. The network access device may include a communication module for the robot to communicate with the user terminal.

In an application, the processor may be a central processing unit (Central Processing Unit, CPU), and the processor may also be other general-purpose processors, digital signal processors (Digital Signal Processors, DSP), application specific integrated circuits (Application Specific Integrated Circuits) , ASIC), off-the-shelf programmable gate array (Field-Programmable Gate Array, FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like. The processor may specifically be a PID controller.

In application, the memory may in some embodiments be the robot's internal storage unit, such as the robot's hard disk or memory. In other embodiments, the memory can also be an external storage device of the robot, for example, a plug-in hard disk equipped on the robot, a Smart Media Card (SMC), a Secure Digital (SD) card, and a flash memory card. (Flash Card) etc. The memory may also include both an internal storage unit of the robot and an external storage device. The memory is used to store an operating system, an application program, a boot loader (Boot Loader), data, and other programs, such as program codes of computer programs, and the like. The memory can also be used to temporarily store data that has been or will be output.

In application, the display screen can be Thin Film Transistor LiquidCrystal Display (TFT-LCD), Liquid Crystal Display (LCD), Organic Light-Emitting Diode (OLED) , Quantum Dot Light Emitting Diodes (QLED) display screen, seven-segment or eight-segment digital tube, etc.

In the application, the communication module can be set as any device that can directly or indirectly perform long-distance wired or wireless communication with the client according to actual needs. For example, the communication module can provide wireless local area networks (Wireless Localarea Networks, WLAN) (such as Wi-Fi network), Bluetooth, Zigbee, mobile communication network, Global Navigation Satellite System (GNSS), Frequency Modulation (FM), Near Field Communication (NFC) , Infrared technology (Infrared, IR) and other communication solutions. The communication module may include an antenna, and the antenna may have only one array element, or may be an antenna array including multiple array elements. The communication module can receive electromagnetic waves through the antenna, frequency modulate and filter the electromagnetic wave signals, and send the processed signals to the processor. The communication module can also receive the signal to be sent from the processor, perform frequency modulation and amplification on it, and convert it into electromagnetic waves for radiation through the antenna.

It should be noted that the information exchange, execution process and other contents between the above-mentioned devices/modules are based on the same concept as the method embodiments of the present application. For specific functions and technical effects, please refer to the method embodiments section. It is not repeated here.

Those skilled in the art can clearly understand that, for the convenience and conciseness of the description, only the division of the above-mentioned functional modules is used as an example for illustration. The internal structure of the device is divided into different functional modules to complete all or part of the functions described above. Each functional module in the embodiment may be integrated in one processing module, or each module may exist physically alone, or two or more modules may be integrated in one module, and the above-mentioned integrated modules may be implemented in the form of hardware. , can also be implemented in the form of software function modules. In addition, the specific names of the functional 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 process of the modules in the above system, reference may be made to the corresponding process in the foregoing method embodiments, which will not be repeated here.

Embodiments of the present application further provide a computer-readable storage medium, where a computer program is stored in the computer-readable storage medium, and when the computer program is executed by a processor, the steps in the foregoing method embodiments can be implemented.

The embodiments of the present application provide a computer program product, which enables the robot to implement the steps in each of the foregoing method embodiments when the computer program product runs on a robot.

If the integrated modules are implemented in the form of software functional modules and sold or used as independent products, they may be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments can be implemented by a computer program to instruct the relevant hardware. The computer program can be stored in a computer-readable storage medium, and the computer program can be processed When the device is executed, the steps of the foregoing method embodiments may be implemented. Wherein, the computer program includes computer program code, and the computer program code may be in the form of source code, object code, executable file or some intermediate form, and the like. The computer-readable medium may include at least: any entity or device capable of carrying the computer program code to the robot, a recording medium, a computer memory, a read-only memory (ROM, Read-Only Memory), a random access memory (RAM, Random Access Memory), electrical carrier signals, telecommunication signals, and software distribution media. For example, U disk, mobile hard disk, disk or CD, etc.

In the foregoing embodiments, the description of each embodiment has its own emphasis. For parts that are not described or described in detail in a certain embodiment, reference may be made to the relevant descriptions of other embodiments.

Those of ordinary skill in the art can realize that the modules 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 this application.

In the embodiments provided in this application, it should be understood that the disclosed apparatus and method may be implemented in other manners. For example, the apparatus embodiments described above are only illustrative. For example, the division of the modules is only a logical function division. In actual implementation, there may be other division methods. For example, multiple modules or components may be combined or Can be integrated into another system, or some features can be ignored, 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 modules, and may be in electrical, mechanical or other forms.

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

The above-mentioned embodiments are only used to illustrate the technical solutions of the present application, but not to limit them; although the present application has been described in detail with reference to the above-mentioned embodiments, those of ordinary skill in the art should understand that: it can still be used for the above-mentioned 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 application, and should be included in the within the scope of protection of this application.

Claims (12)

1. a robot positioning method, is characterized in that, comprises: Obtain the first pose data of the robot according to the image data collected by the two monocular cameras with different fields of view respectively; Obtain the second pose data of the robot according to the odometer data collected by the wheel odometer; According to the fusion of the first pose data and the second pose data, the real pose data of the robot is obtained. 2. The method for positioning a robot according to claim 1, wherein the first pose data of the robot is obtained according to the image data collected by two monocular cameras with different fields of view, comprising: For each monocular camera, feature point detection is performed on the first frame of image data collected by the monocular camera, and a preset number of feature points in the first frame of image data are extracted; Perform feature point tracking on the k+1th frame of image data collected by the monocular camera, where k=1, 2, ..., n, n is any positive integer; Constructing a reprojection error cost function according to the common view feature points between the multiple frames of image data collected by the monocular camera; The reprojection error cost function is minimized to solve the reprojection error, and the three-dimensional coordinates of the preset number of feature points in each frame of image data collected by the monocular camera in the world coordinate system are obtained as the robot's three-dimensional coordinates. First pose data. 3. The robot positioning method according to claim 2, wherein after the feature point tracking is performed on the k+1 th frame of image data collected by the monocular camera, the method comprises: If the number of feature points in the k+1 th frame of image data tracked is less than the preset number, perform feature point detection on the k+1 th frame of image data, and extract the k+1 th frame of image data. The feature points in the frame image data, so that the sum of the tracked feature points in the k+1 th frame image data and the extracted feature points in the k+1 th frame image data is equal to the Preset quantity. 4. The method for positioning a robot according to claim 2, wherein the fusion according to the first pose data and the second pose data to obtain the real pose data of the robot, comprising: Projecting the three-dimensional coordinates of the preset number of feature points under the world coordinate system back to the image coordinate system to obtain the two-dimensional coordinates of the preset number of feature points under the image coordinate system; Obtain the difference between the two-dimensional coordinates of the preset number of feature points under the image coordinate system and the extracted two-dimensional coordinates of the preset number of feature points in the first frame of image data under the image coordinate system value; Using the second pose data as an estimated value and the difference as an observation variable, extended Kalman filter fusion is performed to obtain the real pose data of the robot in the wheeled odometer coordinate system. 5. The robot positioning method according to claim 4, wherein the second pose data is used as an estimated value, and the difference is used as an observation variable, and extended Kalman filter fusion is performed to obtain the The real pose data of the robot in the wheeled odometer coordinate system, including: Taking the second pose data as an estimated value and the difference as an observation variable, performing extended Kalman filter fusion under constraints to obtain the real pose data of the robot in the wheeled odometer coordinate system, The constraint condition is that the X-axis component and the Y-axis component of the rotation angle of the robot in the wheeled odometer coordinate system are 0, and the Z-axis component of the real pose data is fixed. 6. The robot positioning method according to claim 4, wherein the second pose data is used as an estimated value, and the difference is used as an observation variable, and extended Kalman filter fusion is performed to obtain the The real pose data of the robot in the wheeled odometer coordinate system, including: The second pose data is used as the estimated value, the difference is used as the observation variable, and the relative rotation angle and relative displacement between the wheel odometer coordinate system and the camera coordinate system are used as external parameters, and extended Kalman filtering is performed. Fusion to obtain the real pose data of the robot in the wheeled odometer coordinate system. 7. The robot positioning method according to any one of claims 1 to 6, wherein, obtaining the second pose data of the robot according to the odometer data collected by the wheeled odometer, comprising: Aligning the time stamp of the image data collected by the monocular camera with the time stamp of the odometer data collected by the wheel odometer to obtain the odometer data synchronized with the time stamp of the image data; Pre-integration is performed on the odometer data synchronized with the time stamp of the image data, and the integral value of the wheel odometer is obtained as the second pose data of the robot. 8 . The robot positioning method according to claim 7 , wherein the time stamp of the image data collected by the monocular camera is aligned with the time stamp of the odometer data collected by the wheel odometer, and the obtained data is obtained by aligning the time stamp of the image data collected by the monocular camera with the time stamp of the odometer data collected by the wheel odometer. odometry data synchronized with the timestamp of the image data, including: Linear interpolation is performed on the odometer data collected by the wheel odometer according to the time stamp of the image data collected by the monocular camera, so as to align the time stamp of the image data with the time stamp of the odometer data, and obtain a The odometer data is synchronized with the timestamp of the image data. 9. The robot positioning method according to claim 7, wherein the expression of the second pose data is:

in,

represents the integral value of the rotation angle of the wheel odometer synchronized with the time stamp of the k+1th frame of image data,

represents the integral value of the rotation angle of the wheel odometer synchronized with the time stamp of the kth frame of image data, ΔR represents the time stamp of the k+1th frame of image data and the time stamp of the kth frame of image data The amount of change between the integral value of the rotation angle of the wheel odometer,

represents the integral value of the displacement of the wheel odometer synchronized with the time stamp of the k+1th frame of image data,

represents the integral value of the displacement of the wheel odometer synchronized with the time stamp of the kth frame of image data, Δp represents the sum of the time stamp of the k+1th frame of image data and the time stamp of the kth frame of image data The amount of change in the integral value of the displacement of the wheel odometer between, G represents the world coordinate system, and O represents the wheel odometer coordinate system. 10. A robot positioning device, comprising: a first pose obtaining unit, configured to obtain the first pose data of the robot according to the image data collected by the two monocular cameras with different fields of view; A second pose obtaining unit, configured to obtain the second pose data of the robot according to the odometer data collected by the wheeled odometer; A pose fusion unit, configured to fuse the first pose data and the second pose data to obtain the real pose data of the robot. 11. A robot, characterized by comprising a processor and a computer program stored in the memory and executable on the processor, the processor implementing the computer program as claimed in claims 1 to 9 when the processor executes the computer program any one of the steps of the robot positioning method. 12. A computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the robot positioning method according to any one of claims 1 to 9 is implemented A step of.

Images