Monocular camera and UWB fused mobile robot positioning method

Through the fusion positioning method of monocular camera and UWB, combined with the Levenberg-Marquardt algorithm and damping matrix, the dynamic learning rate is solved, and the problem of uncertainty in monocular visual odometer scale is realized, high-precision positioning of indoor mobile robots is achieved, and hardware costs are reduced. and layout complexity.

CN119022930APending Publication Date: 2024-11-26CHANGCHUN UNIV OF SCI & TECH
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202411124108.1
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-16
Publication Date
2024-11-26

AI Technical Summary

Technical Problem

In indoor environments, monocular visual odometers cannot determine the true scale of distance, and the existing monocular camera and UWB fusion positioning methods require the layout of multiple ultra-wideband base stations, which are costly and strictly required by hardware, making it difficult to achieve cost-effectiveness and layout Simple and real-time positioning of mobile robots.

Method used

Using the positioning method of fusion of monocular cameras and UWB, the Levenberg-Marquardt algorithm combines the damping matrix and dynamic learning rate, and uses ultra-wideband point-to-point distance measurement and visual odometer data to perform timestamp alignment, estimate the scale factor and base station coordinates to realize each axes Accurate estimation of location.

Benefits of technology

It provides high-precision position information with low cost, simple layout and good real-time performance, solves the problem of uncertainty in monocular visual odometer scale, and is suitable for various indoor autonomous mobile robots, realizing three-dimensional reconstruction and scene construction of real-scale scales.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119022930A_ABST
    Figure CN119022930A_ABST
Patent Text Reader

Abstract

The invention belongs to the field of mobile robot positioning, and relates to a positioning method based on monocular camera and ultra wide band fusion. According to the method, under the condition that a group of ultra-wideband base stations are used, the problem of scale uncertainty in a monocular vision odometer is solved by utilizing point-to-point ultra-wideband distance measurement values. A Levenberg-Marquardt nonlinear least square regression method is used, a damping matrix is added in a Levenberg-Marquardt algorithm and corresponds to each to-be-estimated parameter, and therefore the more accurate scale of the monocular visual odometer and the more accurate position of the ultra-wideband base station can be estimated at the same time. In addition, as a loose coupling method, the two input modules can be flexibly replaced by other monocular vision mileage calculation methods or distance measurement algorithms. According to the invention, the problem that a monocular vision odometer cannot acquire real scale information is solved, and meanwhile, the problem that hardware facilities have harsh requirements on placement positions in an environment is avoided. In addition, the fusion positioning method is suitable for various mobile robot platforms with low computing power.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of mobile robot positioning, and in particular to a mobile robot positioning method integrating a monocular camera and UWB. Background Art

[0002] In modern society, with the continuous development of technology, mobile robots play an increasingly important role in many fields such as industrial production, daily life, exploration and rescue. Especially in indoor environments, such as factory workshops, warehouse storage areas, etc., mobile robots need accurate positioning systems to perform tasks. However, there are some challenges in achieving accurate positioning of mobile robots in indoor environments. Since GPS signals are blocked by buildings and the signal attenuation is serious, it is impossible to provide accurate location information for mobile robots.

[0003] The currently used monocular visual odometry (VO) system has the advantages of low cost and simple implementation. It does not rely on external signals and is very suitable for indoor environments where GPS signals are blocked. However, a significant limitation of monocular visual odometry is that it cannot determine the true scale of the distance. Therefore, introducing ultra-wideband (UWB) with true scale information is an ideal choice. The positioning method of monocular camera fusion ultra-wideband can effectively solve such problems in monocular visual odometry.

[0004] However, the current fusion method of these two sensors requires the deployment of multiple ultra-wideband base stations in the environment, and the use of ultra-wideband positioning to estimate the scale of the monocular visual odometer. This has strict requirements on the placement of the base stations and has high hardware costs.

[0005] Therefore, developing a cost-effective, simple to deploy, real-time and reliable positioning method is crucial for mobile robot applications in indoor environments. Summary of the invention

[0006] 1. Technical issues to be resolved

[0007] In view of the above problems, the present invention provides a multi-sensor fusion positioning method, which adopts a positioning method of a monocular camera and ultra-wideband fusion to solve the scale uncertainty problem of the monocular visual odometer; and by adding a damping matrix and a dynamic learning rate method in the Levenberg-Marquardt algorithm, the position estimation of the monocular visual odometer on each axis is made more accurate; it can continuously provide reliable and high-precision position information for the autonomous service robot without being restricted by the hardware layout scene, has the advantages of low cost, simple layout, good real-time performance, high reliability, etc., and can be widely used in various indoor autonomous mobile robots.

[0008] (II) Technical solution

[0009] In order to achieve the above-mentioned purpose, the present invention specifically adopts the following technical solutions:

[0010] A mobile robot positioning method based on the fusion of a monocular camera and UWB comprises the following steps:

[0011] S1. Use any monocular visual odometry method to obtain scale-free position data, use a set of ultra-wideband sensors to obtain ultra-wideband point-to-point distance measurement data, and align the timestamps of the two sets of data;

[0012] S2, using a Levenberg-Marquardt nonlinear least squares regression method, combining the scale-free position data with the UWB distance measurement data input into a sliding window to simultaneously estimate the scale factor and the coordinates of the UWB base station;

[0013] S3. Add a damping matrix and a dynamic learning factor to the Levenberg-Marquardt algorithm so that it can dynamically adjust the damping factors of each parameter and improve the position estimation accuracy of the algorithm in the world coordinate system.

[0014] Furthermore, the specific steps of S1 are as follows:

[0015] S11. Install a monocular camera and an ultra-wideband sensor on the mobile robot platform. The ultra-wideband sensor is used as an ultra-wideband tag, and ensure that the fixed position between the two remains unchanged.

[0016] S12, using a monocular camera to acquire a continuous image sequence;

[0017] S12, inputting the acquired image sequence into a visual mileage calculation method (such as ORB-SLAM2) for processing;

[0018] S13, extracting the relative position change information of the camera through a visual odometer calculation method to obtain scale-free position information;

[0019] S14, the real coordinates of the monocular camera in the world coordinate system are expressed as A fixed ultra-wideband base station is placed in the environment, and the coordinates are expressed as

[0020] S15, the ultra-wideband base station sends a signal to the ultra-wideband tag on the mobile robot platform, and records the timestamps of sending and receiving the signal;

[0021] S16. Calculate the point-to-point distance between the tag and the base station using the following formula based on the timestamps of signal transmission and reception:

[0022]

[0023] in is the receiving time of the UWB tag, is the transmission time of the UWB base station, c is the speed of light;

[0024] S17. Synchronize the ultra-wideband distance measurement data with the position information of the monocular visual odometer, and align the timestamps of the two sets of data.

[0025] Furthermore, the specific steps of S2 are as follows:

[0026] S21. The coordinates of the ultra-wideband base station are introduced into the cost function as one of the optimization variables and optimized together with the scale factor.

[0027]

[0028] Where k is the time index of the latest data added to the sliding window, s k is the scale factor, It is the scale-free position data of the monocular visual odometer;

[0029] S22, setting relevant parameters, including the estimated starting scale, starting base station coordinates, sliding window size, and various thresholds and learning rates, where the scale and base station coordinates are defined as target vectors as follows:

[0030]

[0031] in is the scale factor on the x-axis in the visual odometry, is the scale factor on the y-axis, is the scale factor on the z-axis;

[0032] S23, when receiving new visual odometer data and ultra-wideband distance data, determine whether to add the new data to the sliding window according to the preset threshold. If the change between the new data and the last data in the window exceeds the set displacement threshold, the new data will be added to the sliding window, otherwise it will be discarded;

[0033] S24. Use the data in the sliding window to perform an optimization algorithm and calculate the residual under the current estimated value:

[0034]

[0035] The cost function is defined as follows:

[0036]

[0037] Using the cost function, p is obtained by minimizing the objective function ak and k The optimal value of , that is, the objective function is:

[0038]

[0039] Furthermore, the specific steps of S3 are as follows:

[0040] S31, according to the data calculated in S24, appropriately adjusting the initial estimated values ​​of the scale and the base station coordinates, and dynamically adjusting the damping factor of each parameter to ensure stable convergence of the algorithm, and updating the dynamic learning rate to improve the accuracy of parameter estimation;

[0041] The damping factors corresponding to the various parameters described in S32 and S31 are shown in the following formula:

[0042]

[0043] in is the damping factor of the x-axis in the visual odometry, is the damping factor of the y-axis, is the damping factor of the z-axis, is the damping factor of the x-axis of the UWB tag, is the damping factor of the UWB tag y-axis, is the damping factor of the UWB tag in the z-axis.

[0044] The dynamic learning rate described in S33 and S31 is set to v>1. For each parameter vector, if it meets a certain threshold, it means that it is close enough to the optimal solution, and the corresponding damping matrix is ​​updated by v;

[0045] S34, evaluating whether the result of the current iteration has reached convergence, or whether the maximum number of iterations has been reached. The convergence criterion may be based on the rate of change of the residual, and if the rate of change is lower than a preset convergence threshold or the maximum number of iterations has been reached, the iteration is terminated;

[0046] S35. After the termination condition is met, the scale of the latest received visual odometer data is corrected using the current estimation parameters to obtain the final position estimate. This position estimate combines the pose information of the visual odometer and the distance information measured by the ultra-wideband to provide a more accurate and reliable positioning result;

[0047] S36. Output the final determined position information as a reference for the robot's autonomous positioning system to guide its subsequent autonomous movement and task execution.

[0048] (III) Beneficial effects

[0049] Compared with the prior art, the present invention provides a mobile robot positioning method integrating a monocular camera and UWB, which has the following beneficial effects:

[0050] The present invention realizes a fusion positioning method based on monocular vision and ultra-wideband. As a loosely coupled fusion method, it can flexibly replace the visual algorithm and the ranging algorithm. The method of fusing monocular vision and ultra-wideband can make full use of the advantages of the two sensors, restore the scale of monocular vision, and provide accurate positioning information while being lightweight in hardware. It solves the problem that in practical applications, when data arrives one by one in the form of streams, the data received up to the current time may not provide enough information to optimize the parameters of all axes. The present invention provides real scale information for realizing three-dimensional reconstruction of the real scale of a mobile robot and restoring the scene construction with maximum authenticity. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0052] Figure 1 is a flow chart of the method of the present invention;

[0053] Figure 2 A schematic flow chart of a monocular camera scale recovery method based on the Levenberg-Marquardt algorithm and UWB point-to-point ranging provided by the present invention;

[0054] Figure 3 Flow chart of the Levenberg-Marquardt algorithm used in the present invention;

[0055] Figure 4 This is an algorithm framework diagram of the present invention applied in the ORB-SLAM2 system. DETAILED DESCRIPTION

[0056] 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 only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments in the invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0057] The present invention will be further described below in conjunction with the embodiments and the accompanying drawings.

[0058] Example

[0059] like Figure 1 The present invention provides a fusion positioning method based on monocular vision and ultra-wideband, and the method is as follows:

[0060] S1. Use any monocular visual odometry method to obtain scale-free position data, use a set of ultra-wideband sensors to obtain ultra-wideband point-to-point distance measurement data, and align the timestamps of the two sets of data. The implementation method is as follows:

[0061] S11. Install a monocular camera and an ultra-wideband sensor on the mobile robot platform. The ultra-wideband sensor is used as an ultra-wideband tag, and ensure that the fixed position between the two remains unchanged.

[0062] S12, using a monocular camera to acquire a continuous image sequence;

[0063] S12, inputting the acquired image sequence into a visual mileage calculation method (such as ORB-SLAM2) for processing;

[0064] S13, extracting the relative position change information of the camera through a visual odometer calculation method to obtain scale-free position information;

[0065] S14, the real coordinates of the monocular camera in the world coordinate system are expressed as A fixed ultra-wideband base station is placed in the environment, and the coordinates are expressed as

[0066] S15, the ultra-wideband base station sends a signal to the ultra-wideband tag on the mobile robot platform, and records the timestamps of sending and receiving the signal;

[0067] S16. Calculate the point-to-point distance between the tag and the base station using the following formula based on the timestamps of signal transmission and reception:

[0068]

[0069] in is the receiving time of the UWB tag, is the transmission time of the UWB base station, c is the speed of light;

[0070] S17. Synchronize the ultra-wideband distance measurement data with the position information of the monocular visual odometer, and align the timestamps of the two sets of data.

[0071] S2. Use a Levenberg-Marquardt nonlinear least squares regression method to combine the scale-free position data with the ultra-wideband distance measurement data input into a sliding window, and simultaneously estimate the scale factor and the coordinates of the ultra-wideband base station. The implementation method is as follows:

[0072] S21. The coordinates of the ultra-wideband base station are introduced into the cost function as one of the optimization variables and optimized together with the scale factor.

[0073]

[0074] Where k is the time index of the latest data added to the sliding window, s k is the scale factor, It is the scale-free position data of the monocular visual odometer;

[0075] S22, setting relevant parameters, including the estimated starting scale, starting base station coordinates, sliding window size, and various thresholds and learning rates, where the scale and base station coordinates are defined as target vectors as follows:

[0076]

[0077] in is the scale factor on the x-axis in the visual odometry, is the scale factor on the y-axis, is the scale factor on the z-axis;

[0078] S23, when receiving new visual odometer data and ultra-wideband distance data, determine whether to add the new data to the sliding window according to the preset threshold. If the change between the new data and the last data in the window exceeds the set displacement threshold, the new data will be added to the sliding window, otherwise it will be discarded;

[0079] S24. Use the data in the sliding window to perform an optimization algorithm and calculate the residual under the current estimated value:

[0080]

[0081] The cost function is defined as follows:

[0082]

[0083] Using the cost function, p is obtained by minimizing the objective function ak and k The optimal value of , that is, the objective function is:

[0084]

[0085] S3. Add a damping matrix and a dynamic learning factor to the Levenberg-Marquardt algorithm to dynamically adjust the damping factors of each parameter and improve the position estimation accuracy of the algorithm in the world coordinate system. The implementation method is as follows:

[0086] S31, according to the data calculated in S24, appropriately adjusting the initial estimated values ​​of the scale and the base station coordinates, and dynamically adjusting the damping factor of each parameter to ensure stable convergence of the algorithm, and updating the dynamic learning rate to improve the accuracy of parameter estimation;

[0087] The damping factors corresponding to the various parameters described in S32 and S31 are shown in the following formula:

[0088]

[0089] in is the damping factor of the x-axis in the visual odometry, is the damping factor of the y-axis, is the damping factor of the z-axis, is the damping factor of the x-axis of the UWB tag, is the damping factor of the UWB tag y-axis, is the damping factor of the UWB tag in the z-axis.

[0090] The dynamic learning rate described in S33 and S31 is set to v>1. For each parameter vector, if it meets a certain threshold, it means that it is close enough to the optimal solution, and the corresponding damping matrix is ​​updated by v;

[0091] S34, evaluating whether the result of the current iteration has reached convergence, or whether the maximum number of iterations has been reached. The convergence criterion may be based on the rate of change of the residual, and if the rate of change is lower than a preset convergence threshold or the maximum number of iterations has been reached, the iteration is terminated;

[0092] S35. After the termination condition is met, the scale of the latest received visual odometer data is corrected using the current estimation parameters to obtain the final position estimate. This position estimate combines the pose information of the visual odometer and the distance information measured by the ultra-wideband to provide a more accurate and reliable positioning result;

[0093] S36. Output the final determined position information as a reference for the robot's autonomous positioning system to guide its subsequent autonomous movement and task execution.

[0094] In this embodiment, Figure 2 As shown, the robot is equipped with a monocular camera and an ultra-wideband to acquire data; the acquired data is added to a sliding window, and a Levenberg-Marquardt nonlinear least squares regression method is used to estimate the scale factor of the monocular camera and the position of the ultra-wideband base station in the environment; in addition, the monocular vision and ultra-wideband fusion positioning method provided by the present invention is a loosely coupled method, which can replace the algorithm of the monocular visual odometer or the selection of the ultra-wideband sensor to varying degrees.

[0095] In this embodiment, Figure 3 As shown, the process of the Levenberg-Marquardt algorithm is shown, which is specifically:

[0096] Set the initial scale factor, base station location, and damping factor. Next, construct the initial residual equation, which is calculated based on the current parameter estimates. When calculating the residual equation and the Jacobian matrix, the Jacobian matrix contains the partial derivative information of the residual with respect to each parameter, which is used to calculate the target parameter update amount. After updating the parameters, the algorithm checks whether the parameter change is less than the preset threshold. If the value is less than the threshold, it is considered that the current parameter estimate is relatively accurate and can proceed to the next step. Otherwise, it is necessary to dynamically adjust the damping factor based on the change. If the change is large, increase the damping factor (damping matrix multiplied by the dynamic learning rate) to make the algorithm closer to the gradient descent method; if the change is small, reduce the damping factor (damping matrix divided by the dynamic learning rate) to make the algorithm closer to the Gauss-Newton method. Through this adjustment, the algorithm can find a balance between stability and convergence speed.

[0097] In this embodiment, Figure 4 As shown, the application framework of the Levenberg-Marquardt algorithm in the ORB-SLAM2 system is shown. The process of constructing key frames in the ORB-SLAM2 system can be regarded as the visual mileage calculation method in the present invention, and all continuous key frames are regarded as image sequences and input into the algorithm. For ultra-wideband ranging, some common digital filters can be used for processing to obtain more accurate ranging values.

[0098] Finally, it should be noted that the above is only a preferred embodiment of the present invention and is not intended to limit the present invention. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art can still modify the technical solutions described in the aforementioned embodiments or replace some of the technical features therein by equivalents. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A mobile robot positioning method integrating a monocular camera and UWB, characterized in that: The following steps are involved: S1. Use any monocular visual odometry method to obtain scale-free position data, use a set of ultra-wideband sensors to obtain ultra-wideband point-to-point distance measurement data, and align the timestamps of the two sets of data; S2, using a Levenberg-Marquardt nonlinear least squares regression method, combining the scale-free position data with the UWB distance measurement data input into a sliding window to simultaneously estimate the scale factor and the coordinates of the UWB base station; S3. Add a damping matrix and a dynamic learning factor to the Levenberg-Marquardt algorithm so that it can dynamically adjust the damping factors of each parameter and improve the position estimation accuracy of the algorithm in the world coordinate system.

2. The method for positioning a mobile robot by integrating a monocular camera and UWB according to claim 1, characterized in that: The specific steps of S1 are as follows: S11. Install a monocular camera and an ultra-wideband sensor on the mobile robot platform. The ultra-wideband sensor is used as an ultra-wideband tag, and ensure that the fixed position between the two remains unchanged. S12, using a monocular camera to acquire a continuous image sequence; S12, inputting the acquired image sequence into a visual mileage calculation method (such as ORB-SLAM2) for processing; S13, extracting the relative position change information of the camera through a visual odometer calculation method to obtain scale-free position information; S14, the real coordinates of the monocular camera in the world coordinate system are expressed as A fixed ultra-wideband base station is placed in the environment, and the coordinates are expressed as S15, the ultra-wideband base station sends a signal to the ultra-wideband tag on the mobile robot platform, and records the timestamps of sending and receiving the signal; S16. Calculate the point-to-point distance between the tag and the base station using the following formula based on the timestamps of signal transmission and reception: in is the receiving time of the UWB tag, is the transmission time of the UWB base station, c is the speed of light; S17. Synchronize the ultra-wideband distance measurement data with the position information of the monocular visual odometer, and align the timestamps of the two sets of data.

3. The method for positioning a mobile robot by integrating a monocular camera and UWB according to claim 1, characterized in that: The specific steps of S2 are as follows: S21. The coordinates of the ultra-wideband base station are introduced into the cost function as one of the optimization variables and optimized together with the scale factor. Where k is the time index of the latest data added to the sliding window, s k is the scale factor, It is the scale-free position data of the monocular visual odometer; S22, setting relevant parameters, including the estimated starting scale, starting base station coordinates, sliding window size, various thresholds and learning rates, where the scale and base station coordinates are defined as target vectors as follows: in is the scale factor on the x-axis in the visual odometry, is the scale factor on the y-axis, is the scale factor on the z-axis; S23, when receiving new visual odometer data and ultra-wideband distance data, determine whether to add the new data to the sliding window according to the preset threshold. If the change between the new data and the last data in the window exceeds the set displacement threshold, the new data will be added to the sliding window, otherwise it will be discarded; S24. Use the data in the sliding window to perform an optimization algorithm and calculate the residual under the current estimated value: The cost function is defined as follows: Using the cost function, p is obtained by minimizing the objective function ak and k The optimal value of , that is, the objective function is:

4. The method for positioning a mobile robot by integrating a monocular camera and UWB according to claim 1, characterized in that: The specific steps of S3 are as follows: S31, according to the data calculated in S24, appropriately adjusting the initial estimated values ​​of the scale and the base station coordinates, and dynamically adjusting the damping factor of each parameter to ensure stable convergence of the algorithm, and updating the dynamic learning rate to improve the accuracy of parameter estimation; The damping factors corresponding to the various parameters described in S32 and S31 are shown in the following formula: in is the damping factor of the x-axis in the visual odometry, is the damping factor of the y-axis, is the damping factor of the z-axis, is the damping factor of the x-axis of the UWB tag, is the damping factor of the UWB tag y-axis, is the damping factor of the UWB tag in the z-axis. The dynamic learning rate described in S33 and S31 is set to v>1. For each parameter vector, if it meets a certain threshold, it means that it is close enough to the optimal solution, and the corresponding damping matrix is ​​updated by v; S34, evaluating whether the result of the current iteration has reached convergence, or whether the maximum number of iterations has been reached. The convergence criterion may be based on the rate of change of the residual, and if the rate of change is lower than a preset convergence threshold or the maximum number of iterations has been reached, the iteration is terminated; S35. After the termination condition is met, the scale of the latest received visual odometer data is corrected using the current estimation parameters to obtain the final position estimate. This position estimate combines the pose information of the visual odometer and the distance information measured by the ultra-wideband to provide a more accurate and reliable positioning result; S36. Output the final determined position information as a reference for the robot's autonomous positioning system to guide its subsequent autonomous movement and task execution.