Positioning method, device, medium and equipment based on the fusion of vision and wheel odometer

Abstract

The present invention discloses a positioning method based on the fusion of vision and wheel odometer, including: calibrating the coordinate system of the binocular camera, wheel odometer and IMU installed on the mobile robot, wherein the wheel odometer coincides with the IMU; obtaining three-dimensional odometer information according to the wheel odometer and IMU, and saving the three-dimensional odometer information in a first cache; obtaining image information collected by the binocular camera, and saving the image information in a second cache; obtaining a number of image information in a corresponding timestamp of the three-dimensional odometer information when the three-dimensional odometer information meets a preset condition; for a number of image information in the timestamp, calculating the posture information between the two image information; constructing a cost function according to the posture information, and obtaining the global posture according to the cost function. The present invention improves the positioning accuracy and stability of the mobile robot.

Description

Technical Field

[0001] The present invention relates to the field of information technology, and in particular to a positioning method, device, medium and equipment based on the fusion of vision and wheel odometer. Background Art

[0002] The positioning technology of mobile robots has become one of the hot research technologies in recent years. This is because the mobile robot needs to know its location before it can realize the point-to-point autonomous path-finding walking capability.

[0003] Existing mobile robot positioning technology mainly adopts a fusion positioning method based on vision and wheel odometer. Patent document CN 110864685 provides a vehicle monocular vision wheel odometer positioning method based on loose coupling, and patent CN108253963 provides a robot self-anti-disturbance positioning method and positioning system based on multi-sensor fusion, both of which use the Kalman filter fusion method. However, Kalman filtering is a method for optimally estimating the state of a system. It targets the state of the system, inputs the measurement information of the wheel odometer and the visual measurement information at the current moment, and then obtains the optimal estimate of the current state of the system through appropriate weights. The block diagram of the Kalman filter idea is as follows: Figure 1 As shown, KF-k is the weight value of the output of each module calculated by Kalman filtering.

[0004] Although the above Kalman filter method has the advantages of simple algorithm and good real-time performance, the Kalman filter algorithm only considers the posture information of the kth frame and the k+1th frame, and the posture information of the k+1th frame and the k+2th frame, and loses the advantage of the image having multiple loops, resulting in low positioning accuracy; and the Kalman filter algorithm performs weighted addition on the direct positioning results of the image and the wheel odometer and outputs a fusion result. If the wheel slips, it is impossible to adaptively adjust the weight value because the algorithm does not know whether the mobile robot has slipped, resulting in low positioning stability. Summary of the invention

[0005] The embodiments of the present invention provide a positioning method, device, medium and equipment based on the fusion of vision and wheel odometer to solve the problems of low positioning accuracy and stability when the existing technology uses Kalman filter algorithm to realize the fusion positioning of vision and wheel odometer.

[0006] A positioning method based on the fusion of vision and wheel odometer, comprising:

[0007] Calibrate the coordinate system of the binocular camera, wheel odometer and inertial measurement unit (IMU) installed on the mobile robot, wherein the wheel odometer coincides with the IMU;

[0008] Acquire three-dimensional odometer information according to the wheel odometer and the IMU, and save the three-dimensional odometer information to a first cache;

[0009] Acquire image information collected by the binocular camera, and save the image information to a second cache;

[0010] Acquire a plurality of image information within a corresponding time stamp of the three-dimensional odometer information when the three-dimensional odometer information meets a preset condition;

[0011] For the plurality of image information within the corresponding timestamp, calculating the position and posture information between each two image information;

[0012] A cost function is constructed according to the pose information, and a global pose is obtained according to the cost function.

[0013] Optionally, when the three-dimensional odometer information is obtained to meet a preset condition, the plurality of image information in a corresponding timestamp of the three-dimensional odometer information includes:

[0014] Loop through the first cache to calculate the difference information of the three-dimensional odometer information of the previous and next two frames;

[0015] Determining whether the difference information meets the preset condition;

[0016] If yes, obtaining from the second cache a number of image information within corresponding timestamps of the two frames of three-dimensional odometer information;

[0017] If not, the image information before the corresponding timestamp of the previous frame of three-dimensional odometer information is removed from the second cache.

[0018] Optionally, the difference information between the two frames of three-dimensional odometer information includes a distance deviation and an angle deviation;

[0019] The preset conditions include that the distance deviation is greater than 0.2 meters or the angle deviation is greater than 5 degrees.

[0020] Optionally, the calculating the pose information between two pieces of image information within the corresponding timestamp includes:

[0021] Using the ORB feature point detection class in the OPENCV library, feature point detection is performed on the plurality of image information;

[0022] Use the Match feature point matching class in the OPENCV library to perform feature matching on pairwise image information;

[0023] The SOLVEPNP function in the OPENCV library is used to calculate the pose of each pair of image information and obtain the pose information between each pair of image information.

[0024] Optionally, constructing a cost function according to the pose information, and acquiring a global pose according to the cost function includes:

[0025] According to the posture information, construct a cost function based on the second norm of the vector;

[0026] The cost function is iteratively optimized using the Gauss-Newton method in the Ceres library so that the cost function reaches a minimum value;

[0027] Obtain the pose difference information to be estimated when the cost function reaches a minimum value, and obtain the global pose according to the pose difference information to be estimated.

[0028] Optionally, the cost function constructed based on the second norm of the vector is:

[0029]

[0030] In the above formula represents the cost function, |||| represents the second norm of the vector, camera() represents the pose information, Delat_odom() represents the difference information of the three-dimensional odometer information of the previous and next two frames, True() represents the pose difference information to be estimated, Represents the transformation matrix of the binocular camera and IMU, where the subscript c represents the binocular camera coordinate system and the subscript i represents the IMU coordinate system.

[0031] Optionally, the coordinate system calibration of the binocular camera, wheel odometer and IMU installed on the mobile robot includes:

[0032] Calibrate the intrinsic parameters and distortion parameters of the binocular camera by using a preset camera checkerboard method;

[0033] The external parameters of the binocular camera and IMU are calibrated according to a preset open source algorithm.

[0034] A positioning device based on the fusion of vision and wheel odometer, the device comprising:

[0035] A calibration module, used to calibrate the coordinate system of a binocular camera, a wheel odometer and an inertial measurement unit IMU installed on the mobile robot, wherein the wheel odometer coincides with the IMU;

[0036] A first acquisition module, configured to acquire three-dimensional odometer information according to the wheel odometer and the IMU, and save the three-dimensional odometer information into a first cache;

[0037] A second acquisition module, used to acquire image information collected by the binocular camera and save the image information to a second cache;

[0038] A third acquisition module, used to acquire a plurality of image information in a corresponding timestamp of the three-dimensional odometer information when the three-dimensional odometer information meets a preset condition;

[0039] A calculation module, used for calculating the position information between two pieces of image information within the corresponding timestamp;

[0040] The fourth acquisition module is used to construct a cost function according to the posture information, and acquire the global posture according to the cost function.

[0041] A computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the positioning method based on the fusion of vision and wheel odometer as described above is implemented.

[0042] A computer device comprises a memory, a processor and a computer program stored in the memory and executable on the processor, wherein the processor implements the positioning method based on the fusion of vision and wheel odometer as described above when executing the computer program.

[0043] The embodiment of the present invention calibrates the coordinate system of a binocular camera, a wheel odometer and an IMU installed on a mobile robot, wherein the wheel odometer coincides with the IMU; obtains three-dimensional odometer information according to the wheel odometer and the IMU, and saves the three-dimensional odometer information in a first cache; obtains image information collected by the binocular camera, and saves the image information in a second cache; obtains a plurality of image information within a corresponding timestamp of the three-dimensional odometer information when the three-dimensional odometer information meets a preset condition; calculates the pose information between the pair of image information for the plurality of image information within the corresponding timestamp; constructs a cost function according to the pose information, and obtains the global pose according to the cost function; thereby optimizing the fusion positioning method of vision and wheel odometer, and effectively improving the positioning accuracy and positioning stability of the mobile robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings required for use in the description of the embodiments of the present invention will be briefly introduced below. Obviously, the accompanying drawings in the following description are only some embodiments of the present invention. For ordinary technicians in this field, other accompanying drawings can be obtained based on these accompanying drawings without paying creative labor.

[0045] Figure 1 This is the block diagram of the Kalman filter idea;

[0046] Figure 2 is a flow chart of a positioning method based on the fusion of vision and wheel odometer in one embodiment of the present invention;

[0047] Figure 3 is a flowchart of step S104 in a positioning method based on fusion of vision and wheel odometer in one embodiment of the present invention;

[0048] Figure 4 is a flowchart of step S105 in a positioning method based on fusion of vision and wheel odometer in one embodiment of the present invention;

[0049] Figure 5 is a flowchart of step S106 in a positioning method based on fusion of vision and wheel odometer in one embodiment of the present invention;

[0050] Figure 6 is a schematic diagram of a positioning device based on the fusion of vision and wheel odometer in one embodiment of the present invention;

[0051] Figure 7 is a schematic diagram of a computer device in one embodiment of the present invention. DETAILED DESCRIPTION

[0052] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments 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.

[0053] The present embodiment provides a positioning method based on the fusion of vision and wheel odometer, by calibrating the coordinate system of the binocular camera, wheel odometer and IMU installed on the mobile robot, wherein the wheel odometer coincides with the IMU; obtaining three-dimensional odometer information according to the wheel odometer and the IMU, and saving the three-dimensional odometer information in a first cache; obtaining image information collected by the binocular camera, and saving the image information in a second cache; obtaining a number of image information within a corresponding timestamp of the three-dimensional odometer information when the three-dimensional odometer information meets a preset condition; calculating the pose information between the two image information within the timestamp; constructing a cost function according to the pose information, and obtaining the global pose according to the cost function; thereby optimizing the fusion positioning method of vision and wheel odometer, and effectively improving the positioning accuracy and positioning stability of the mobile robot.

[0054] The following is a detailed description of the positioning method based on the fusion of vision and wheel odometer provided in this embodiment. Figure 2 As shown, the positioning method based on the fusion of vision and wheel odometer includes:

[0055] In step S101, coordinate system calibration is performed on a binocular camera, a wheel odometer, and an inertial measurement unit IMU installed on the mobile robot, wherein the wheel odometer coincides with the IMU.

[0056] The mobile robot described in the embodiment of the present invention is a wheeled mobile robot, which is equipped with a wheel odometer, a binocular camera, and an inertial measurement unit (IMU), and the IMU is fixedly connected to the wheel odometer to ensure that the wheel odometer coincides with the IMU, and the wheel odometer and the IMU together constitute an inertial wheel odometer. As a preferred embodiment of the present invention, the coordinate system calibration of the binocular camera, the wheel odometer and the IMU installed on the mobile robot described in step S101 includes:

[0057] In step S1011, the intrinsic parameters and distortion parameters of the binocular camera are calibrated by a preset camera checkerboard method.

[0058] Optionally, the embodiment of the present invention adopts the Zhang Zhengyou-Camera Chessboard Method to complete the calibration of the intrinsic parameters and distortion parameters of the binocular camera.

[0059] In step S1012, the external parameters of the binocular camera and the IMU are calibrated according to a preset open source algorithm.

[0060] Optionally, the embodiment of the present invention calibrates the external parameters of the binocular camera and IMU according to the open source algorithm Kalibr, which is recorded as Represents the transformation matrix of the binocular camera and IMU; R represents a 3×3 rotation matrix, which represents the rotation relationship of one orthogonal coordinate system relative to another orthogonal coordinate system; T represents a 3×1 displacement transformation, which represents the relationship between the origin of one orthogonal coordinate system and the origin of another orthogonal coordinate system; the subscript c represents the binocular camera coordinate system, and the subscript i represents the IMU coordinate system.

[0061] In step S102, three-dimensional odometer information is acquired according to the wheel odometer and the IMU, and the three-dimensional odometer information is saved in a first cache.

[0062] The embodiment of the present invention pre-allocates a buffer zone for the first cache and the second cache. Optionally, the embodiment of the present invention obtains three-dimensional odometer information based on the projection of the IMU and the wheel odometer, which is recorded as (R w , T w ), the three-dimensional odometer information and its timestamp (R w ,T w,t) is saved in the first cache, where R represents a 3×3 rotation matrix, representing the rotation relationship of an orthogonal coordinate system relative to another orthogonal coordinate system; T represents a 3×1 displacement transformation, representing the relationship between the origin of an orthogonal coordinate system and the origin of another orthogonal coordinate system; t represents the system timestamp corresponding to the three-dimensional odometer information, which is a collection data of the odometer, and the subscript w represents the world coordinate, which is the starting position of an origin and a fixed coordinate system parallel to the IMU coordinate system. It should be understood that the IMU coordinate system moves with the mobile robot.

[0063] In step S103, image information collected by the binocular camera is obtained, and the image information is saved in a second cache.

[0064] Optionally, the embodiment of the present invention may further perform distortion correction on the image information collected by the binocular camera, and then save the distortion-corrected image information into the second cache.

[0065] In step S104, a plurality of image information in a corresponding time stamp of the three-dimensional odometer information is obtained when the three-dimensional odometer information meets a preset condition.

[0066] Alternatively, if Figure 3 As shown, the step S104 further includes:

[0067] In step S1041, the first cache is traversed in a loop to calculate difference information between the previous and next two frames of three-dimensional odometer information.

[0068] Here, the first cache stores a number of three-dimensional odometer information. The embodiment of the present invention loops through the first cache to calculate the difference between the three-dimensional odometer information of the previous and next two frames, which is recorded as in Said Indicates the kth three-dimensional odometer information stored in the first cache.

[0069] Here, the difference information between the two frames of three-dimensional odometer information includes a distance deviation and an angle deviation.

[0070] In step S1042, it is determined whether the difference information meets the preset condition.

[0071] Optionally, the preset condition includes but is not limited to the distance deviation being greater than 0.2 meters or the angle deviation being greater than 5 degrees. or Whether the degree is established.

[0072] In step S1043, if yes, a plurality of image information within corresponding timestamps of the two frames of three-dimensional odometer information is obtained from the second cache.

[0073] If the or degree is established, then read from the second cache in the odom k+1 and odom k The image information between the corresponding timestamps of the image and the corresponding time stamps form a processing window. Then, step S105 is executed.

[0074] In step S1044 , if not, the image information before the corresponding timestamp of the previous frame of three-dimensional odometer information is removed from the second cache.

[0075] If the difference information does not meet the preset condition, that is, and When the 3D odometer information of the previous frame is odom k The image information before the corresponding timestamp is removed from the second cache, because the accuracy of the short-time three-dimensional odometer is relatively high, and there is no need to process the image information whose accuracy has reached the standard. Then return to step S1041 to continue calculating the difference information of the three-dimensional odometer information of the previous and next two frames.

[0076] In step S105, for a plurality of image information within the corresponding timestamp, pose information between each two image information is calculated.

[0077] After obtaining a plurality of image information through step S104, the pose calculation is performed on the image information. Figure 4 As shown, the step S105 further includes:

[0078] In step S1051, the ORB feature point detection class in the OPENCV library is used to perform feature point detection on the plurality of image information.

[0079] In step S1052, the Match feature point matching class in the OPENCV library is used to perform feature matching on pairwise image information.

[0080] In step S1053, the SOLVEPNP function in the OPENCV library is used to calculate the pose of each pair of image information to obtain the pose information between each pair of image information.

[0081] Here, according to the sampling frequency of the binocular camera and the movement speed of the wheel odometer, usually n (n≥3) image information will be obtained in a processing window. For ease of understanding, it is assumed here that there are 3 image information in a processing window, namely image k, image Tmp and image k+1. The embodiment of the present invention first traverses each image information, and uses the ORB feature point detection class in the OPENCV library to perform feature point detection on the image k, image Tmp and image k+1 respectively; then feature matching is performed on the image information pairwise, thereby completing the pose calculation between the image information pairwise. The pose information between image k and image Tmp can be obtained by calculation as follows: The pose information between image k and image k+1 is The pose information between image Tmp and image k+1 is

[0082] It can be seen that, unlike the existing Kalman filter algorithm that only considers the posture information of the previous and next two frames, the embodiment of the present invention also considers the posture information between any two image information, constructs a posture error model between any two image information, fully considers the advantage of the image having multiple loops, realizes more constraints on the quantity to be calculated, and optimizes the fusion positioning method of vision and wheel odometer according to the posture information between any two image information, thereby effectively improving the positioning accuracy of the mobile robot.

[0083] In step S106, a cost function is constructed according to the pose information, and a global pose is acquired according to the cost function.

[0084] Optionally, as a preferred embodiment of the present invention, Figure 5 As shown, the step S106 also includes:

[0085] In step S1061, a cost function is constructed based on the binary norm of the vector according to the posture information.

[0086] Optionally, the cost function constructed based on the second norm of the vector is:

[0087] In the above formula represents the cost function, |||| represents the second norm of the vector, camera() represents the pose information calculated by the camera, Delat_odom() represents the difference information of the three-dimensional odometer information of the previous and next two frames, True() represents the pose difference information to be estimated, Represents the transformation matrix of the binocular camera and IMU, where the subscript c represents the binocular camera coordinate system and the subscript i represents the IMU coordinate system.

[0088] In step S1062, the cost function is iteratively optimized using the Gauss-Newton method in the Ceres library so that the cost function reaches a minimum value.

[0089] Here, the embodiment of the present invention adopts the Gauss-Newton method in the Ceres library, and continuously adjusts the variables in the True() brackets of the estimated pose difference information, so that the entire cost function Reach a minimum value.

[0090] In step S1063, the pose difference information to be estimated when the cost function reaches a minimum value is obtained, and the global pose is obtained according to the pose difference information to be estimated.

[0091] The present invention is implemented by finding a cost function The estimated pose difference information when reaching the minimum value That is the optimal value, according to the optimal estimator Pass-through Get the global pose, where Represents the global pose of the robot.

[0092] The embodiment of the present invention constructs an integrated cost function by combining the measurement information of the binocular camera and the measurement information of the inertial wheel odometer, which has a mutual constraint effect and can adaptively adjust to situations such as wheel slippage. As long as the visual and inertial wheel odometers do not fail to position at the same time, the positioning system will not fail, thereby effectively improving the positioning stability of the mobile robot.

[0093] In addition, the optimization of the cost function in the embodiment of the present invention is an iterative process, which consumes less resources than the Kalman filter algorithm; and the embodiment of the present invention optimizes by processing windows, selects image information within a short period of time for optimization and fusion, and can achieve real-time positioning effects.

[0094] In summary, the embodiment of the present invention calibrates the coordinate system of the binocular camera, wheel odometer and IMU installed on the mobile robot, wherein the wheel odometer coincides with the IMU; obtains three-dimensional odometer information according to the wheel odometer and the IMU, and saves the three-dimensional odometer information in a first cache; obtains image information collected by the binocular camera, and saves the image information in a second cache; obtains a number of image information within a corresponding timestamp of the three-dimensional odometer information when the three-dimensional odometer information meets a preset condition; calculates the pose information between the two image information within the corresponding timestamp; constructs a cost function according to the pose information, and obtains the global pose according to the cost function; thereby optimizing the fusion positioning method of the vision and wheel odometer, and effectively improving the positioning accuracy and positioning stability of the mobile robot.

[0095] It should be understood that the order of execution of the steps in the above embodiment does not necessarily mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiment of the present invention.

[0096] In one embodiment, a positioning device based on the fusion of vision and wheel odometer is provided, and the positioning device based on the fusion of vision and wheel odometer corresponds to the positioning method based on the fusion of vision and wheel odometer in the above embodiment. Figure 6 As shown, the positioning device based on the fusion of vision and wheel odometer includes a calibration module 61, a first acquisition module 62, a second acquisition module 63, a third acquisition module 64, a calculation module 65, and a fourth acquisition module 66. The functional modules are described in detail as follows:

[0097] A calibration module 61 is used to calibrate the coordinate system of a binocular camera, a wheel odometer and an inertial measurement unit IMU installed on the mobile robot, wherein the wheel odometer coincides with the IMU;

[0098] A first acquisition module 62, configured to acquire three-dimensional odometer information according to the wheel odometer and the IMU, and save the three-dimensional odometer information into a first cache;

[0099] A second acquisition module 63 is used to acquire image information collected by the binocular camera and save the image information to a second cache;

[0100] A third acquisition module 64 is used to acquire a plurality of image information in a corresponding time stamp of the three-dimensional odometer information when the three-dimensional odometer information meets a preset condition;

[0101] A calculation module 65, for calculating the position information between two pieces of image information within the corresponding timestamp;

[0102] The fourth acquisition module 66 is used to construct a cost function according to the posture information, and acquire the global posture according to the cost function.

[0103] Optionally, the third acquisition module 64 includes:

[0104] A difference calculation unit, used for looping through the first cache to calculate difference information between two frames of three-dimensional odometer information;

[0105] A judging unit, used to judge whether the difference information satisfies the preset condition;

[0106] an image acquisition unit, configured to acquire, from the second cache, a plurality of image information within corresponding timestamps of the two frames of three-dimensional odometer information when the determination result of the determination unit is yes;

[0107] The elimination unit is used to eliminate the image information before the corresponding timestamp of the previous frame of three-dimensional odometer information from the second cache when the judgment result of the judgment unit is negative.

[0108] Optionally, the difference information between the two frames of three-dimensional odometer information includes a distance deviation and an angle deviation;

[0109] The preset conditions include that the distance deviation is greater than 0.2 meters or the angle deviation is greater than 5 degrees.

[0110] Optionally, the calculation module 65 includes:

[0111] A detection unit, used to perform feature point detection on the plurality of image information by using an ORB feature point detection class in an OPENCV library;

[0112] The matching unit is used to use the Match feature point matching class in the OPENCV library to perform feature matching on pairwise image information;

[0113] The pose calculation unit is used to use the SOLVEPNP function in the OPENCV library to perform pose calculation on the pairwise image information to obtain the pose information between the pairwise image information.

[0114] Optionally, the fourth acquisition module 66 includes:

[0115] A construction unit, used for constructing a cost function based on the second norm of the vector according to the posture information;

[0116] An optimization unit, used for iteratively optimizing the cost function by using the Gauss-Newton method in the Ceres library, so that the cost function reaches a minimum value;

[0117] The acquisition unit is used to obtain the pose difference information to be estimated when the cost function reaches a minimum value, and obtain the global pose according to the pose difference information to be estimated.

[0118] Optionally, the cost function constructed based on the second norm of the vector is:

[0119]

[0120] In the above formula represents the cost function, |||| represents the second norm of the vector, camera() represents the pose information, Delat_odom() represents the difference information of the three-dimensional odometer information of the previous and next two frames, True() represents the pose difference information to be estimated, Represents the transformation matrix of the binocular camera and IMU, where the subscript c represents the binocular camera coordinate system and the subscript i represents the IMU coordinate system.

[0121] Optionally, the calibration module 61 includes:

[0122] A first calibration unit, used to calibrate the intrinsic parameters and distortion parameters of the binocular camera by using a preset camera checkerboard method;

[0123] The second calibration unit is used to calibrate the external parameters of the binocular camera and the IMU according to a preset open source algorithm.

[0124] For the specific limitations of the positioning device based on the fusion of vision and wheel odometer, please refer to the limitations of the positioning method based on the fusion of vision and wheel odometer mentioned above, which will not be repeated here. Each module in the above-mentioned positioning device based on the fusion of vision and wheel odometer can be implemented in whole or in part through software, hardware and a combination thereof. The above-mentioned modules can be embedded in or independent of the processor in the computer device in the form of hardware, or can be stored in the memory of the computer device in the form of software, so that the processor can call and execute the operations corresponding to the above modules.

[0125] In one embodiment, a computer device is provided. The computer device may be a server, and its internal structure diagram may be as follows: Figure 7 As shown. The computer device includes a processor, a memory, a network interface and a database connected through a system bus. Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The network interface of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, a positioning method based on the fusion of vision and wheel odometer is implemented.

[0126] In one embodiment, a computer device is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the following steps when executing the computer program:

[0127] Calibrate the coordinate system of the binocular camera, wheel odometer and inertial measurement unit (IMU) installed on the mobile robot, wherein the wheel odometer coincides with the IMU;

[0128] Acquire three-dimensional odometer information according to the wheel odometer and the IMU, and save the three-dimensional odometer information to a first cache;

[0129] Acquire image information collected by the binocular camera, and save the image information to a second cache;

[0130] Acquire a plurality of image information within a corresponding time stamp of the three-dimensional odometer information when the three-dimensional odometer information meets a preset condition;

[0131] For the plurality of image information within the corresponding timestamp, calculating the position and posture information between each two image information;

[0132] A cost function is constructed according to the pose information, and a global pose is obtained according to the cost function.

[0133] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided by the present invention can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. As an illustration and not limitation, RAM is available in many forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).

[0134] 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. In actual applications, the above-mentioned functions can be distributed and completed by different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above.

[0135] The embodiments described above are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. Such 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, and should all be included in the protection scope of the present invention.

Claims

1. A positioning method based on the fusion of vision and wheel odometer, characterized in that: include: Calibrate the coordinate system of the binocular camera, wheel odometer and inertial measurement unit (IMU) installed on the mobile robot, wherein the wheel odometer coincides with the IMU; Acquire three-dimensional odometer information according to the wheel odometer and the IMU, and save the three-dimensional odometer information to a first cache; Acquire image information collected by the binocular camera, and save the image information to a second cache; Acquire a plurality of image information within a corresponding time stamp of the three-dimensional odometer information when the three-dimensional odometer information meets a preset condition; For the plurality of image information within the corresponding timestamp, calculating the position and posture information between each two image information; Constructing a cost function according to the posture information, and obtaining a global posture according to the cost function; When the three-dimensional odometer information is obtained to meet the preset condition, the plurality of image information in the corresponding timestamp of the three-dimensional odometer information includes: Loop through the first cache to calculate the difference information of the three-dimensional odometer information of the previous and next two frames; Determining whether the difference information meets the preset condition; If yes, obtaining from the second cache a number of image information within corresponding timestamps of the two frames of three-dimensional odometer information; If not, remove the image information before the corresponding timestamp of the previous frame of three-dimensional odometer information from the second cache; The difference information of the two frames of three-dimensional odometer information includes a distance deviation and an angle deviation; The preset conditions include that the distance deviation is greater than 0.2 meters or the angle deviation is greater than 5 degrees.

2. The positioning method based on the fusion of vision and wheel odometer as claimed in claim 1, characterized in that: The step of calculating the position information between two pieces of image information in the corresponding timestamp includes: Using the ORB feature point detection class in the OPENCV library, feature point detection is performed on the plurality of image information; Use the Match feature point matching class in the OPENCV library to perform feature matching on pairwise image information; The SOLVEPNP function in the OPENCV library is used to calculate the pose of each pair of image information and obtain the pose information between each pair of image information.

3. The positioning method based on the fusion of vision and wheel odometer as claimed in claim 2, characterized in that: The constructing a cost function according to the pose information and obtaining the global pose according to the cost function comprises: According to the posture information, construct a cost function based on the second norm of the vector; The cost function is iteratively optimized using the Gauss-Newton method in the Ceres library so that the cost function reaches a minimum value; Obtain the pose difference information to be estimated when the cost function reaches a minimum value, and obtain the global pose according to the pose difference information to be estimated.

4. The positioning method based on the fusion of vision and wheel odometer as claimed in claim 3, characterized in that: The cost function constructed based on the vector's two norm is: In the above formula represents the cost function, || || represents the vector binorm, camera() represents the pose information, Delat_odom() represents the difference information of the three-dimensional odometer information of the previous and next two frames, True() represents the pose difference information to be estimated, Represents the transformation matrix of the binocular camera and IMU, where the subscript c represents the binocular camera coordinate system and the subscript i represents the IMU coordinate system.

5. The positioning method based on the fusion of vision and wheel odometer according to any one of claims 1 to 4, characterized in that: The coordinate system calibration of the binocular camera, wheel odometer and IMU installed on the mobile robot includes: Calibrate the intrinsic parameters and distortion parameters of the binocular camera by using a preset camera checkerboard method; The external parameters of the binocular camera and IMU are calibrated according to a preset open source algorithm.

6. A positioning device based on the fusion of vision and wheel odometer, characterized in that: The device comprises: A calibration module, used to calibrate the coordinate system of a binocular camera, a wheel odometer and an inertial measurement unit IMU installed on the mobile robot, wherein the wheel odometer coincides with the IMU; A first acquisition module, configured to acquire three-dimensional odometer information according to the wheel odometer and the IMU, and save the three-dimensional odometer information into a first cache; A second acquisition module, used to acquire image information collected by the binocular camera and save the image information to a second cache; A third acquisition module, used to acquire a plurality of image information in a corresponding timestamp of the three-dimensional odometer information when the three-dimensional odometer information meets a preset condition; A calculation module, used for calculating the position information between two pieces of image information within the corresponding timestamp; A fourth acquisition module, used to construct a cost function according to the posture information, and acquire a global posture according to the cost function; When the three-dimensional odometer information is obtained to meet the preset condition, the plurality of image information in the corresponding timestamp of the three-dimensional odometer information includes: Loop through the first cache to calculate the difference information of the three-dimensional odometer information of the previous and next two frames; Determining whether the difference information meets the preset condition; If yes, obtaining from the second cache a number of image information within corresponding timestamps of the two frames of three-dimensional odometer information; If not, remove the image information before the corresponding timestamp of the previous frame of three-dimensional odometer information from the second cache; The difference information of the two frames of three-dimensional odometer information includes a distance deviation and an angle deviation; The preset conditions include that the distance deviation is greater than 0.2 meters or the angle deviation is greater than 5 degrees.

7. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the positioning method based on the fusion of vision and wheel odometer is implemented as described in any one of claims 1 to 5.

8. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the positioning method based on the fusion of vision and wheel odometer is implemented as described in any one of claims 1 to 5.

Images