Robot positioning method, speedometer without accumulated drift, robot and storage medium

Abstract

The invention discloses a robot positioning method, an accumulated drift-free odometer, a robot and a computer readable storage medium, the method comprises the following steps: obtaining a UWB range finding value and an odometer pose, the UWB range finding value being a distance measurement value of a UWB base station and the robot; according to the UWB distance measurement value and the speedometer pose, calculating an estimated pose meeting a constraint condition; determining transformation parameters according to the pre-estimated pose and a speedometer pose corresponding to the pre-estimated pose; and determining the current pose of the robot according to the transformation parameters and the current odometer pose. According to the method, discrete UWB ranging values are used, local speedometer pose increment is combined, the pose of the robot is pre-estimated and optimized, transformation parameters are determined according to the pre-estimated pose and the speedometer pose, the original accumulated drift error of the speedometer pose can be corrected according to the transformation parameters, the requirement for global consistency of UWB ranging is lowered, and the precision of the robot is improved. Therefore, the robot can be accurately positioned.

Description

Technical Field

[0001] The present invention relates to the technical field of robot positioning, and particularly to a robot positioning method, a drift-free odometer, a robot and a storage medium. Background Art

[0002] With the development of robot technology, robots are increasingly applied to various scenarios. When a robot performs tasks in a scenario, it first needs to determine its position in the scenario through positioning technology. Traditional positioning includes two categories. One is to perform positioning through laser or vision algorithms. Such positioning methods are difficult to accurately position the robot in the presence of obstacle occlusion. The other is to perform positioning through an odometer combined with other sensors. However, due to the cumulative drift of the odometer, only local positioning accuracy can be guaranteed, and global positioning accuracy cannot be maintained for a long time. Summary of the Invention

[0003] The present application provides a robot positioning method, a robot and a storage medium to solve the problem of cumulative drift error of the odometer in the prior art for robots.

[0004] In a first aspect, the present application provides a robot positioning method, including:

[0005] Obtain UWB ranging values and odometer poses, where the UWB ranging values are distance measurement values between a UWB base station and the robot;

[0006] Calculate a predicted pose that satisfies the constraint conditions according to the UWB ranging values and the odometer poses;

[0007] Determine transformation parameters according to the predicted pose and the odometer pose corresponding to the predicted pose;

[0008] Determine the current pose of the robot according to the transformation parameters and the current odometer pose.

[0009] Optionally, the calculating a predicted pose that satisfies the distance constraint condition and the displacement constraint condition according to the UWB ranging values and the odometer poses includes:

[0010] Determine a sliding window;

[0011] Calculate the predicted pose that satisfies the constraint conditions corresponding to the sliding window;

[0012] Then, the determining the transformation parameters includes:

[0013] Redetermine the transformation parameters according to the predicted pose of the sliding window and the odometer pose corresponding to the predicted pose.

[0014] Optionally, calculating the estimated pose that satisfies the constraint condition within the sliding window includes:

[0015] Constructing a distance constraint equation set based on the UWB ranging values included in the sliding window;

[0016] Constructing a displacement constraint equation set based on the UWB ranging values included in the sliding window and the odometry pose;

[0017] Calculating the estimated pose according to the distance constraint equation set and the displacement constraint equation set.

[0018] Optionally, the sliding window includes multiple sets of UWB ranging values, and constructing a distance constraint equation set based on the UWB ranging values included in the sliding window and the odometry pose includes:

[0019] Constructing a corresponding distance constraint equation according to the UWB ranging value;

[0020] Constructing a distance constraint equation set according to all the distance constraint equations corresponding to the sliding window.

[0021] Optionally, constructing a corresponding distance constraint equation according to each set of UWB ranging values includes:

[0022] Generating an estimated translation component;

[0023] Generating an estimated distance error according to the estimated translation component, the UWB ranging value, and the UWB base station position;

[0024] Constructing the distance constraint equation according to the estimated distance error and a preset distance constraint weight.

[0025] Optionally, the sliding window includes multiple sets of UWB ranging values, and constructing a displacement constraint equation set based on the UWB ranging values included in the sliding window and the odometry pose includes:

[0026] Constructing a corresponding rotation constraint equation and translation constraint equation according to the UWB ranging value and the odometry pose;

[0027] Constructing the displacement constraint equation set according to all the translation constraint equations and the rotation constraint equations corresponding to the sliding window.

[0028] Optionally, the odometry pose includes an odometry rotation component, and constructing a corresponding rotation constraint equation according to the UWB ranging value and the odometry pose includes:

[0029] Generating an estimated rotation component;

[0030] Constructing the rotation constraint equation according to the estimated rotation component and the odometry rotation component.

[0031] Optionally, constructing the corresponding translation constraint equation according to the UWB ranging value and the odometer pose includes:

[0032] Generating a predicted rotation component and a predicted translation component;

[0033] Constructing the translation constraint equation according to the predicted rotation component, the predicted translation component and the odometer pose.

[0034] Optionally, constructing the translation constraint equation according to the predicted rotation component, the predicted translation component and the odometer pose includes:

[0035] Determining the relative displacement between different odometer poses according to the odometer pose;

[0036] Constructing the translation constraint equation according to the predicted rotation component, the predicted translation component and the relative displacement.

[0037] Optionally, calculating the predicted pose according to the distance constraint equation set and the displacement constraint equation set.

[0038] Constructing a cost function of the predicted pose according to the distance constraint equation set and the displacement constraint equation set;

[0039] Calculating the result that satisfies the minimum condition of the cost function as the predicted pose.

[0040] Optionally, before re-determining the transformation parameter according to the predicted pose of the sliding window and the odometer pose corresponding to the predicted pose, further includes:

[0041] Judging whether the predicted pose corresponding to the sliding window meets the error condition;

[0042] If it meets, re-determining the transformation parameter according to the predicted pose of the sliding window and the odometer pose corresponding to the predicted pose;

[0043] If it does not meet, giving up determining the transformation parameter.

[0044] Optionally, judging whether the predicted pose corresponding to the sliding window meets the error condition includes:

[0045] Calculating the error mean value according to the UWB ranging value, the position of the UWB base station and the predicted pose;

[0046] Judging whether the error mean value is less than a preset threshold;

[0047] If it is less, determining that the predicted pose meets the error condition;

[0048] If it is greater than or equal to, it is determined that the estimated pose does not meet the error condition.

[0049] Optionally, the odometry pose, the estimated pose, and the transformation parameter all include a rotation matrix and a translation matrix. Determining the transformation parameter according to the odometry pose and the estimated pose includes:

[0050] Determine the rotation matrix and translation matrix of the estimated pose and the rotation matrix and translation matrix of the odometry pose;

[0051] Determine the rotation matrix of the transformation parameter according to the rotation matrix of the estimated pose and the rotation matrix of the odometry pose;

[0052] Determine the translation matrix of the transformation parameter according to the translation matrix of the estimated pose and the translation matrix of the odometry pose;

[0053] Determine the transformation parameter according to the rotation matrix and translation matrix of the transformation parameter.

[0054] Optionally, determining the current pose of the robot according to the transformation parameter and the current odometry pose includes:

[0055] Determine the rotation matrix and translation matrix of the current odometry pose;

[0056] Determine the target rotation matrix according to the rotation matrix of the current odometry and the rotation matrix of the transformation parameter;

[0057] Determine the target translation matrix according to the translation matrix of the current odometry and the translation matrix of the transformation parameter;

[0058] Determine the current pose of the robot according to the target rotation matrix and the target translation matrix.

[0059] In a second aspect, the present application also provides an odometer without cumulative drift, including a sensor and a controller. The sensor is communicatively connected to the controller. The sensor is used to collect the movement information of the robot and the UWB ranging value. The controller outputs the pose of the robot based on the method described in the first aspect according to the movement information and the UWB ranging value.

[0060] In a third aspect, the present application also provides a robot, including a memory and a processor. The memory is connected to the processor. The processor is configured to execute one or more computer programs stored in the memory. When the processor executes the one or more computer programs, the computer device implements the method described in the first aspect.

[0061] Fourthly, the present application further provides a computer-readable storage medium storing a computer program, the computer program including program instructions which, when executed by a processor, cause the processor to execute the method as described in the first aspect.

[0062] In the technical solution provided by the present application, by using discrete UWB ranging values and combining the increment of the local odometer pose, the pose of the robot is estimated and optimized. Then, according to the estimated pose and the odometer pose, the transformation parameters are determined, and the cumulative drift error originally existing in the odometer pose can be corrected based on the transformation parameters, and the global consistency requirement for UWB ranging is reduced, so as to accurately position the robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following will briefly introduce the drawings required for the description of the embodiments of the present invention. Obviously, the drawings in the following description are only some embodiments of the present invention, and those of ordinary skill in the art can obtain other drawings without creative efforts based on these drawings.

[0064] Figure 1 Schematic diagram of the application environment of the robot positioning method provided in an embodiment of the present application;

[0065] Figure 2 Schematic diagram of the robot architecture provided in an embodiment of the present application;

[0066] Figure 3 Schematic diagram of the principle of using UWB positioning technology to position a robot in the prior art;

[0067] Figure 4 Schematic diagram of the flow of the robot positioning method provided in an embodiment of the present application;

[0068] Figure 5 Schematic diagram of the method flow for calculating the estimated pose that meets the constraint conditions provided in an embodiment of the present application;

[0069] Figure 6 Schematic diagram of calculating the estimated pose according to the sliding window provided in an embodiment of the present application;

[0070] Figure 7 Schematic diagram of determining the distance constraint equations according to the UWB ranging values provided in an embodiment of the present application;

[0071] Figure 8 Schematic diagram of the method flow for determining the transformation parameters provided in an embodiment of the present application;

[0072] Figure 9Schematic diagram for generating transformation parameters provided by an embodiment of the present application;

[0073] Figure 10 Schematic diagram of the architecture of a computer device provided by an embodiment of the present application. Detailed implementation manners

[0074] In order to make the objectives, technical solutions and advantages of the present application clearer and more understandable, the present application will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application. All other embodiments obtained by those of ordinary skill in the art based on the embodiments in the present application without making creative efforts fall within the protection scope of the present application.

[0075] It should be noted that if there is no conflict, the various features in the embodiments of the present application can be combined with each other and all fall within the protection scope of the present application. In addition, although the functional modules are divided in the device schematic diagram and the logical order is shown in the flowchart, in some cases, the steps shown or described can be executed in a different order from the module division in the device or the flowchart. Furthermore, the terms "first", "second", "third", etc. used in the present application do not limit the data and execution order, but only distinguish the same items or similar items with basically the same functions and effects.

[0076] First, to facilitate the description of the method for determining similar point clouds based on data compression provided by the embodiments of the present application, the application environment of the method provided by the embodiments of the present application will be introduced.

[0077] Please refer to Figure 1 , Figure 1 which is a schematic diagram of the application environment of the robot positioning method provided by an embodiment of the present application. The application scenario is provided with a target space 10, which includes a robot 20 and a UWB base station 30. The robot 20 can move to different positions within the target space 10 and obtain the position information of the target object within the target space 10, so as to generate a global map of the target space 10 based on the movement position information of the robot 20 itself and the position information of the target object, for subsequent robot 20 positioning or functional interaction between the robot 20 and the target space 10.

[0078] The target space 10 is a space that provides functions for any area. For example, the target space 10 includes indoor spaces, shopping malls, living rooms, kitchens, offices and other spaces. The robot 20 can be deployed in any of the above types of target spaces 10 to perform tasks within the target space 10.

[0079] Please refer to Figure 2 , Figure 2Schematic diagram of a robot architecture provided by an embodiment of the present application. The robot 20 includes a mobile component 21, a sensing component 22, and a controller 23. The mobile component 21 is used to drive the robot 20 to move between different positions within the target space 10. The sensing component 22 is used to obtain environmental information near the robot 20. The controller 23 is communicatively connected to the mobile component 21 and the sensing component 22, and determines the position and posture of the robot 20 based on the environmental information obtained at different positions of the robot. For example, the mobile component 21 can be wheels, tracks, or mechanical feet. The sensing component 22 can be a binocular camera, a radar, a motion sensor, etc. The controller 23 is a general-purpose processor, a digital signal processor, an application-specific integrated circuit, a field-programmable gate array, a single-chip microcomputer, an ARM, or other programmable logic devices, discrete gate or transistor logic, discrete hardware components, or any combination of these components. It can also be a combination of computing devices. For example, a combination of a DSP and a microprocessor, multiple microprocessors, one or more microprocessors combined with a DSP and / or any other such configuration, which is not limited herein. In some embodiments, the robot 20 further includes a functional component 24. Depending on the tasks performed by the robot 20, the functional component 24 can be specifically different configurations. For example, when the robot 20 is a cleaning robot performing a cleaning task, the functional component 24 can be a suction pump, a cleaning cloth, and a roller brush; when the robot 20 is a sorting robot performing a sorting task, the functional component 24 can be a manipulator and a robotic arm, which are not listed one by one here.

[0080] It should be specifically noted that, in the embodiment of the present application, the sensing component 22 includes an odometer 221 and a UWB sensor 222. Specifically, the odometer 221 can be a simple wheel odometer, an inertial odometer, a LiDAR odometer, or a visual odometer, etc. The specific structure thereof will not be elaborated here. However, no matter what kind of odometer 221 is, it is used to determine the position and posture of the chassis of the robot 20. In this process, due to noises such as accidental slipping and skidding of the mobile component 21, certain errors will be generated, and this error will gradually increase with the distance the robot moves until it cannot be ignored, resulting in an incorrect estimation of the position and posture of the robot 20. This is the source and definition of the cumulative drift error of the odometer. And the present application will also design a brand-new odometer system to correct the cumulative drift error of the traditional odometer 221, which will be described in detail later.

[0081] The UWB sensor 222 is used to cooperate with the UWB base station 30 to measure the distance between the UWB base station 30 and the robot 20 equipped with the UWB sensor 222. Please refer to Figure 3 , Figure 3 , which is a schematic diagram of the principle of positioning a robot using UWB positioning technology in the prior art. In the prior art, the UWB (Ultra Wide Band) positioning technology uses UWB electromagnetic waves to locate a target. Multiple UWB base stations 30 send UWB electromagnetic waves to the target equipped with the UWB sensor, and then locate the target according to the flight time or time difference of arrival of the UWB electromagnetic waves returned by the target. In this process, multiple UWB base stations need to synchronously transmit UWB electromagnetic waves to ensure time synchronization, otherwise the positioning error will increase. In the embodiment of the present application, the UWB sensor 222 is only used to obtain the UWB ranging value between the UWB base station 30 and the robot 20, and it is not necessary to calculate the position of the robot 20 according to the flight time of the UWB electromagnetic wave. Therefore, there is no requirement for the time synchronization of the UWB base stations. Specifically, in the present application, it is not necessary for all UWB base stations 30 to measure the distance to the robot 20 at the same time. It is only necessary to intermittently receive the UWB ranging value of the UWB base station 30. The position of the UWB base station 30 does not need to consider the penetration of UWB too much, nor is it necessary to ensure that every position in the target space 10 is covered by a sufficient number of UWB base stations 30. It is only necessary to ensure that the UWB electromagnetic wave can be received.

[0082] Please refer to again Figure 1 , the initial position of the robot 20 in the target space 10 is point A. When the robot 20 moves from position point A to position point B, due to the drift error of the odometer, the recorded position of the robot 20 is B'. It can be understood that the error between point B and point B' is relatively small and has no actual impact on the position of the robot 20 at a certain resolution. Then, when the robot moves from point B to point C, the drift error further increases. At this time, there is already a certain error between the recorded position point C' of the odometer and the real position point C. Further, when the robot 20 moves from position point C to position point D, there is already a large error between the real position point D and the position point D' of the odometer that cannot be ignored. In the present application, by using the UWB ranging value obtained by the UWB base station 30 and combining the local odometer pose data, when the robot 20 moves to point B, the odometer position B' is corrected and re-estimated as the correct position point B. Then, when the robot 20 moves from point B to point C, C' is further corrected to position point C, and the same applies to position point D. Thus, through the locally relatively accurate odometer data and the discrete UWB ranging data, the accurate position of the robot 20 can finally be obtained. The attitude and position of the robot 20 are the same and will not be elaborated.

[0083] Based on the above application scenario description, the following introduces the method for determining similar point clouds based on data compression provided by the embodiments of the present application.

[0084] Please refer to Figure 4 , Figure 4 which is a schematic flowchart of a robot positioning method provided by an embodiment of the present application. The method includes:

[0085] S41. Obtain the UWB ranging value and the odometer pose.

[0086] In this step, the UWB ranging value is defined as the distance measurement value between the UWB base station and the robot. Specifically, there are multiple UWB base stations, and the position of the robot also changes. Therefore, the UWB ranging value changes according to the position of the robot and the position of the UWB base station. For example, the position of the UWB base station is defined as A j =(x_a j , y_a j , z_a j ), T where j = 1, 2... n, and n is the number of UWB base stations. The UWB base station sends UWB electromagnetic waves at time i to obtain the UWB ranging value. The UWB ranging value can be used to reflect the real distance between the UWB base station and the robot, and can be specifically calculated according to the following exemplary formula:

[0087]

[0088] where j is the number of the corresponding UWB base station, that is, d_m 1,i represents the ranging value between the robot and the UWB base station A1 at time i. d_r j,i represents the real distance value between the UWB base station and the robot, and ε r represents the ranging error. T_r i is the translation component of the assumed real pose of the robot, that is, T_r i =(x_r i , y_r i , z_r i ), ||||2 represents the two-norm, It represents the three-dimensional spatial distance from the position of the robot at time i to the position of the base station j. Those skilled in the art can reasonably control the ranging error according to the adjustment of the parameters of UWB electromagnetic waves and the adjustment of the base station position. In some embodiments, when the robot is moving, multiple UWB base stations take turns to intermittently send UWB electromagnetic waves to the robot to determine the UWB ranging value. The specific number of UWB base stations and the ranging frequency can be set according to the actual situation. It can be understood that the UWB ranging value is a parameter used to determine the relative position of the robot to the base station. Therefore, it is not limited to only using the UWB technology. Other means such as infrared and ultrasonic waves that can be used to determine the relative distance between the robot and the base station can be correspondingly replaced by those skilled in the art according to the common general knowledge in the industry.

[0089] In this step, the odometry pose is defined as the position and attitude of the robot determined by the odometry. Specifically, the robot pose P_o i represents the robot pose recorded by the odometry at time i, P_o i can be decomposed into a translation component T_o i and a rotation component R_o i , where the translation component T_o i represents the position of the robot determined by the odometry, and the rotation component R_o i represents the attitude of the robot determined by the odometry. Among them, the translation component T_o i can be expressed as T_o i =(x_o i ,y_o i ,z_o i ) T , and the rotation component R_o i can be expressed as R_o i ∈SO(3), that is, the rotation component R_o i belongs to the special orthogonal group SO(3). More specifically, the rotation component R_o i is a 3x3 real matrix and satisfies R_o i T *R_o i =I and det(R_o i ) = 1. For example belongs to the special orthogonal group SO(3).

[0090] In this step, the UWB ranging value can be directly obtained based on the UWB base station and the UWB sensor, and the odometry pose can be directly obtained based on the odometer. In some embodiments, the robot controller further processes the data according to the UWB ranging value and the odometry pose to facilitate subsequent robot positioning operations. The data processing can use methods such as signal attenuation, ranging rate of change, and Ransac algorithm consistency discrimination to judge the outliers in the data and perform data filtering. It can be understood that data processing is not the main part of this application, and those skilled in the art can freely select data processing means according to the common knowledge in the relevant field, so no more details will be provided here.

[0091] S42. Calculate the estimated pose that satisfies the constraint conditions according to the UWB ranging value and the odometry pose.

[0092] In this step, the constraint conditions are the constraint conditions for correcting the cumulative drift error of the odometer according to the UWB ranging value and the odometry pose. Specifically, the constraint conditions are artificially set conditions based on physical laws. When the constraint conditions are set correctly, the true pose of the robot should also conform to the constraint conditions. It should be noted that in practical applications, although the constraint conditions cannot fully reflect the constraints of the real physical world, they can also be equivalent to the real physical constraints within a certain error range, that is, the estimated pose deduced and calculated according to the constraint conditions can be considered equivalent to the true pose of the robot within a certain error range. More specifically, in some embodiments, the constraint conditions are specifically expressed as a distance constraint equation set and a displacement constraint equation set. In some other embodiments, the distance constraint equation set and the displacement constraint equation set also change accordingly according to the set sliding window, which will not be introduced in detail here and will be elaborated in the following text.

[0093] In this step, the estimated pose is defined as the calculated robot pose that satisfies the constraint conditions. Compared with the odometry pose, since the cumulative drift error is corrected, it is closer to the true pose of the robot. Therefore, the estimated pose can also be understood as the optimized odometry pose. The estimated pose can also be decomposed into an estimated translation component and an estimated rotation component. For example, for the estimated translation component and the estimated rotation component The estimated pose can be represented as a 3x4 matrix Specifically, the estimated translation component and the estimated rotation component need to be calculated respectively according to the constraint conditions, and then the estimated translation component and the estimated rotation component are spliced to obtain the estimated pose The more specific calculation method will be described in detail below and will not be introduced in detail here.

[0094] In this step, the estimated pose can be determined by interpolation based on the sampling time of the UWB ranging value to obtain the odometry pose of the robot at that moment, and then the key frame of the estimated pose is determined according to the time threshold and / or span threshold of the odometry pose, that is, the position and time corresponding to the estimated pose are determined. In some embodiments, the key frame is directly determined according to the time threshold and / or span threshold for the estimated pose, and then multiple UWB ranging values with close times are searched near the key frame, and then the corresponding odometry pose is determined by interpolation for the multiple UWB ranging values. The relationship between the key frame and the UWB ranging value is established through the relative pose between the pose of the key frame and the odometry pose interpolated at the UWB sampling moment, so that the determination of the estimated pose does not completely depend on the sampling moment of the UWB ranging value.

[0095] S43. Determine the transformation parameter according to the estimated pose and the odometry pose corresponding to the estimated pose.

[0096] In this step, the estimated pose is specifically the estimated pose of the robot at time i. Obviously, there is also an odometry pose Po corresponding to time i. i , and the transformation parameter T is calculated according to the estimated pose at the same time i and the odometry pose Po i .

[0097] In this step, the transformation parameter is defined as the parameter for correcting the odometry pose drift error. Specifically, the transformation parameter T can be calculated according to the following exemplary formula:

[0098]

[0099] where Po i -1 is the inverse of the odometry pose corresponding to time i. T is also represented by a rotation matrix R and a translation vector R, that is, T is a 3x4 matrix (R|T). At the initial moment, due to the absence of cumulative drift, the initial value of the transformation parameter T is As time changes and the moving distance of the robot increases, the transformation parameter T also changes accordingly to correct the odometry pose. The update frequency of the transformation parameter is determined according to the acquisition frequency of the UWB ranging value. For example, if the acquisition frequency of the UWB ranging value is 1 Hz, the update frequency of the corresponding transformation parameter T is also set to about 1 Hz. As new UWB ranging values and odometry poses are continuously received, the transformation parameter T also changes continuously. If T is not updated within a certain period of time or a certain distance, it means that the odometry does not need to be corrected during this time period or this distance segment.

[0100] S44. Determine the current pose of the robot according to the transformation parameters and the current odometry pose.

[0101] In this step, the current pose of the robot is the pose calculated and determined by the robot at the current moment according to the odometry pose and the transformation parameters. The current odometry pose is the odometry pose at the current moment. Obviously, the odometry pose at the current moment includes the accumulated drift error, and the transformation parameter is the transformation parameter determined according to the latest estimated pose, that is, this transformation parameter can be used to correct this accumulated drift error. Specifically, the current pose can be calculated according to the following exemplary formula:

[0102] P′ u = T * P′ o

[0103] where P′ u is the current pose of the robot, T is the transformation parameter, and P′ o is the current odometry pose. The current pose P′ u determined thereby is the pose that has been corrected and does not include the accumulated drift error.

[0104] In summary, the robot positioning method provided by the embodiments of the present application uses discrete UWB ranging values, combines the increment of the local odometry pose, estimates and optimizes the pose of the robot, then determines the transformation parameter according to the estimated pose and the odometry pose, and can correct the original accumulated drift error of the odometry pose according to the transformation parameter to obtain the current pose of the robot that does not include the accumulated drift error, thereby accurately positioning the robot.

[0105] Next, a method for calculating the estimated pose that satisfies the constraint conditions in the present application is introduced.

[0106] Please refer to Figure 5 , Figure 5 which is a schematic flowchart of a method for calculating the estimated pose that satisfies the constraint conditions provided by an embodiment of the present application, specifically including:

[0107] S510. Determine the sliding window.

[0108] S520. Construct a distance constraint equation set according to the UWB ranging values and the odometry poses included in the sliding window.

[0109] S530. Construct a displacement constraint equation set according to the UWB ranging values and the odometry poses included in the sliding window.

[0110] S540. Calculate the estimated pose according to the distance constraint equation set and the displacement constraint equation set.

[0111] In S510, the sliding window is a window area used to limit the amount of data. Specifically, for a series of sampled data points in the robot's movement trajectory, as the amount of UWB ranging values and odometry pose data continuously increases, the constraint conditions constructed based on these data become more massive and complex, imposing a relatively large computational burden on the processor, making it difficult for the robot to ensure real-time operation when calculating the estimated pose. For example, please refer to Figure 6 , Figure 6 which is a schematic diagram for calculating the estimated pose according to the sliding window provided by an embodiment of the present application. The movement trajectory of the robot consists of the poses at a set of discrete sampling time points, that is, P = {P0, P1, P2, … P n}, obviously, calculating based on the data volume of such an array takes a great deal of time. Therefore, the entire movement trajectory is divided according to the sliding window, that is, SW1, SW2, SW3, …. For each sliding window, the trajectory points it contains are P = {P k-N+1 , P k-N+2 , P k-N+3 , … P k}, where N is the number of pose points within the corresponding sliding window, and k is the number of the trajectory point at the end. For example, for the SW1 window, it only contains three pose points P0, P1, and P2, and the computational amount is relatively small. When SW1 is calculated, the sliding window can be moved to SW2, and SW2 only contains three discrete pose points P1, P2, and P3. Thus, the estimated pose within each sliding window is easy to calculate. Then, after moving the sliding window, the estimated pose of the new sliding window is re-determined, and the transformation parameters are updated based on the new estimated pose to maintain the overall computational efficiency.

[0112] In S520, the distance constraint equation set is an equation set generated by performing distance constraints on each pose point based on the odometry pose and UWB ranging values. Specifically, please refer to Figure 7 , Figure 7 which is a schematic diagram for determining the distance constraint equation set according to the UWB ranging value provided by an embodiment of the present application. A sliding window includes multiple pose points, and each pose point is generated based on the UWB ranging value. Therefore, a distance constraint equation is constructed for each UWB ranging value within the sliding window, and then according to all the corresponding distance constraint equations within the sliding window, a distance constraint equation set is formed by aggregation. In some embodiments, due to the unavailability of UWB data within the sliding window, it may be necessary to expand to obtain more data. When the UWB data in the sliding window is sufficient, the construction of the distance constraint equation set will start. For example, for the three pose points P0, P1, and P2 included in the sliding window SW1, the distance constraint equation is constructed according to P0, the distance constraint equation is constructed according to P1, and the distance constraint equation In some embodiments, the distance constraint equation can be calculated according to the following exemplary formula:

[0113]

[0114] where i = 1, 2... m, and m is the number of distance constraint equations for the corresponding sliding window. α i is the distance constraint weight coefficient for the corresponding distance constraint equation, is the estimated distance error for the corresponding distance constraint equation. Where, can also be calculated according to the following exemplary formula:

[0115]

[0116] where d_m j,i is the UWB ranging value measured by UWB base station j at time i, is the estimated pose translation component at time i, A j is the position of UWB base station j. Thus, three distance constraint equations for the sliding window SW1 can be obtained and The three distance constraint equations are relatively independent of each other, and the distance constraint equations they form are constraints composed of several discrete sparse points, that is, the estimated pose points calculated according to the distance constraint equations cannot form a smooth trajectory.

[0117] In S530, in order to make the movement trajectory of the estimated pose of the robot in the sliding window smoother to conform to the trajectory of the odometry pose, a displacement constraint equation set also needs to be constructed. The displacement constraint equation set is an equation set generated by performing translation constraint and rotation constraint on adjacent pose points based on the odometry pose and the UWB ranging value, that is, the displacement constraint equation also includes a translation constraint equation and a rotation constraint equation. Specifically, similar to the distance constraint equation set, the number of displacement constraint equations is equal to the number of UWB ranging values obtained in the sliding window. For example, for the three pose points P0, P1, and P2 included in the sliding window SW1, the displacement constraint equation is constructed according to P0 respectively The displacement constraint equation is constructed according to P1 The displacement constraint equation is constructed according to P2 In some embodiments, the displacement constraint equation can be calculated according to the following exemplary formula:

[0118]

[0119] where β i is the translation constraint weight coefficient for the corresponding displacement constraint equation, γ i is the rotation constraint weight coefficient for the corresponding displacement constraint equation, is the estimated translational error corresponding to the displacement constraint equation, is the estimated rotational error corresponding to the displacement constraint equation. Among them, and can be calculated according to the following exemplary formulas respectively:

[0120]

[0121]

[0122] Among them, is the estimated rotational component at time i within the sliding window, is the estimated translational component at time i within the sliding window, T_o i is the translational component of the odometry pose at time i within the sliding window, R_o i is the rotational component of the odometry pose at time i within the sliding window. The superscript T represents the transpose of the matrix, the superscript -1 represents the inverse of the matrix, and ||||2 represents the 2-norm.

[0123] In some embodiments, S530 includes the following specific steps:

[0124] S531. Construct a corresponding translational constraint equation according to the UWB ranging value and the odometry pose.

[0125] S532. Construct a corresponding rotational constraint equation according to the UWB ranging value and the odometry pose.

[0126] S533. Construct a displacement constraint equation set according to all the translational constraint equations and rotational constraint equations corresponding to the sliding window.

[0127] In S531, the translational constraint equation is defined for the calculation of the estimated translational error, and both belong to the decomposition parameters of the estimated pose, T_o i and T_o i-1 are the translational components of the odometry pose. For the trajectory P_o = {P_o k-N+1 , P_o k-N+2 , P_o k-N+3 , … P_o k} composed of a set of odometry pose points within the sliding window, where the trajectory T_o = {T_o k-N+1 , T_o k-N+2 , T_o k-N+3 , … T_o k} composed of the translational components. Due to the cumulative drift error of the odometry, the translational components T_o k and T_o k-1It is not accurate. However, the local increment of the odometry pose is relatively accurate. Therefore, it is necessary to introduce the relative displacement between the translational components of adjacent odometry poses. The relationship between the relative displacement and the translational components of adjacent odometry poses can be calculated according to the following exemplary formula:

[0128] T_o k = T_o k-1 + r k

[0129] ||r k ||² = ||T_o k - T_o k-1 ||²

[0130] where r k is the relative displacement between adjacent pose points, and r k can be directly determined according to the odometry pose. Thus, based on the relative displacement r k the relationship between the estimated translation error and the estimated pose can be determined, and the translation constraint equation can be determined.

[0131] In S532, the rotation constraint equation is defined for the calculation of the estimated rotation error. Specifically, and are the rotation components of the estimated pose, R_o i and R_o i-1 are the rotation components of the odometry pose. As mentioned above, the rotation component R belongs to the Lie group SO(3), and there is no good addition definition in the SO(3) space. Therefore, through the logarithmic mapping log SO(3) (R) = ω, the rotation component R is converted from the Lie group SO(3) to the Lie algebra so(3), where ω = (ω1, ω2, ω3) T , that is, ω is in the form of a 3x1 three-dimensional vector, which enables ω to have good addition operations, so that the estimated rotation error can be calculated by constructing the rotation constraint equation.

[0132] In S540, based on the already constructed distance constraint equations and displacement constraint equations the estimated poses of each position point within the sliding window can be calculated. Specifically, first, a cost function is constructed, and the cost function can be calculated according to the following exemplary formula:

[0133]

[0134] where is the estimated pose, is the distance constraint equation, is the displacement constraint equation, is the cost function. After constructing the cost function for the estimated pose, it is also necessary to calculate the result that satisfies the minimum condition of the cost function as the estimated pose. Specifically, the composition of the cost function is obtained by configuring relevant weight coefficients for the estimated distance error, estimated translation error, and estimated rotation error. Therefore, based on the idea of optimization, the estimated pose that minimizes the cost function (i.e., minimizes the error) is solved, and it can be calculated according to the following exemplary formula:

[0135]

[0136] wherein, is the estimated pose of the robot at any position point within the sliding window, and argmin is the variable function for finding the variable that makes the objective function the minimum value. The specific solution algorithm can use the Gauss-Newton method, the L-M method, can be solved by hand calculation, or can be calculated through, for example, the ceres non-linear optimization library. The solution algorithm is not the focus of the inventive concept of this application, so it will not be elaborated here.

[0137] In some embodiments, after determining the estimated pose of the sliding window and before re-determining the transformation parameters, it is also necessary to evaluate the estimated pose. The method includes the steps of: determining whether the estimated position corresponding to the sliding window meets the error condition. If it meets, the transformation parameters are re-determined according to the estimated pose of the sliding window and the odometer pose corresponding to the estimated pose; if it does not meet, the determination of the transformation parameters is abandoned. Specifically, determining whether it meets the error condition is to perform reprojection on the estimated pose within each sliding window to calculate the reprojection error, and the reprojection error can be calculated according to the following exemplary formula:

[0138]

[0139] wherein, d_m j,i is the UWB ranging value measured by the UWB base station j at time i, is the translation component of the estimated pose at time i, and A j is the position of the UWB base station j, and abs is the absolute value function. After determining the reprojection error of each position point, it is also necessary to calculate the error mean, and the error mean can be calculated according to the following exemplary formula:

[0140] error_mean = ∑error i / n

[0141] wherein, n is the total number of reprojection errors. Determine whether the error mean error_mean is less than a preset threshold. If it is less, the estimated pose meets the error condition and the estimated pose is valid; if it is greater than or equal, the estimated pose does not meet the error condition and this estimate is invalid, that is, the subsequent calculation does not use this estimated pose.

[0142] The method for determining transformation parameters in this application is introduced below.

[0143] Please refer to Figure 8 , Figure 8 which is a schematic flowchart of the method for determining transformation parameters provided by an embodiment of this application, including the following steps:

[0144] S810. Determine the rotation component, translation component of the estimated pose, and the rotation component, translation component of the odometry pose.

[0145] S820. Determine the rotation component of the transformation parameter according to the rotation component of the estimated pose and the rotation component of the odometry pose.

[0146] S830. Determine the translation component of the transformation parameter according to the translation component of the estimated pose and the translation component of the odometry pose.

[0147] S840. Determine the transformation parameter according to the rotation component and translation component of the transformation parameter.

[0148] In S810, the estimated pose is the optimized estimated pose of the latest frame. As described above, the estimated pose can be represented by a rotation component and a translation component, that is where and can be directly obtained when determining the estimated pose, and the odometry pose is the pose at time i corresponding to this estimated pose, which can also be represented by a rotation component and a translation component, that is P_o i =(R_o i |T_o i ).

[0149] In S820, the transformation parameter can also be represented by a rotation component and a translation component. Therefore, calculate the rotation component and translation component of the transformation parameter respectively. The calculation can refer to the following formula:

[0150]

[0151] Substitute the rotation component of the estimated pose and the rotation component of the odometry pose into the calculation respectively, that is, perform a multiplication operation on the 3x3 rotation matrix R to obtain the rotation component of the transformation parameter.

[0152] In S830, the calculation method of the translation component is similar to that of the rotation component. Perform an addition operation on the 3x1 translation vector T to obtain the translation component of the transformation parameter, which will not be elaborated here.

[0153] In S840, please refer to Figure 9 , Figure 9Schematic diagram for generating transformation parameters provided by an embodiment of the present application. For each UWB ranging value, an estimated pose is calculated accordingly, and each estimated pose can determine the odometry pose at the same moment. Thus, the transformation parameter T at this moment can be determined. For example, when the robot calculates the transformation parameter T at the initial moment k-5 . As the robot moves continuously to obtain new estimated poses and odometry poses, the transformation parameter also changes from T k-5 and is continuously re-determined as T k-4 , T k-3 , T k-2 ……T k , where T k is the latest transformation parameter. Thus, by maintaining the transformation parameter T during the movement of the robot, the robot can always determine the latest current pose without cumulative drift.

[0154] In some embodiments, after determining the transformation parameter T, the current pose P' of the robot can be calculated according to the current odometry pose P' o and the latest transformation parameter T. u . Since the transformation parameter T is generated based on the estimated pose with the cumulative drift error corrected most recently, the current pose P' u calculated according to the transformation parameter T also does not contain cumulative drift error, thus achieving precise positioning of the robot.

[0155] In summary, the robot positioning method provided by the embodiment of the present application optimizes the robot pose within the sliding window based on the accuracy of the local odometry pose increment and combines discrete UWB ranging values to obtain an estimated pose. Since the estimated pose is within the local sliding window, it does not contain cumulative drift error. Then, the transformation parameter is obtained by comparing the estimated pose with the odometry pose, that is, the change parameter can be used to help correct the odometry pose to obtain a pose without cumulative drift error. As the sliding window continues to move or new UWB ranging values are continuously received, the transformation parameter is updated accordingly. The transformation parameter has been corrected for the cumulative drift of the previous odometry pose. Thus, according to the current odometry pose and the transformation parameter, the current robot pose without cumulative drift can be calculated, overcoming the problem of cumulative drift error of the odometry and improving the positioning accuracy of the robot pose. At the same time, this method does not need to consider time synchronization for the use of UWB base stations, reducing the cost of UWB base station deployment and improving the flexibility of UWB base station deployment.

[0156] The present application also provides an odometer without cumulative drift, which includes a sensor and a controller. The sensor is communicatively connected to the controller. The sensor is configured to collect the movement information of the robot and the UWB ranging value. The controller outputs the pose of the robot based on the movement information and the UWB ranging value according to the method described in the foregoing embodiments.

[0157] An embodiment of the present application also provides a computer-readable storage medium. The computer-readable storage medium stores a computer program, and the computer program includes program instructions. When the program instructions are executed by a computer, the computer is caused to execute the method according to the foregoing embodiments.

[0158] An embodiment of the present application also provides a robot, which includes a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the computer program, the method according to the foregoing embodiments is implemented.

[0159] An embodiment of the present application also provides a computer device, which may be a server, and its internal structure diagram may be as Figure 10 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 database of the computer device is used to store the data that needs to be saved in the method of the foregoing embodiments. 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, the method provided in the foregoing embodiments is implemented.

[0160] Those of ordinary skill in the art can understand that all or part of the processes in the methods of the above embodiments can be completed by instructing relevant hardware through a computer program. 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 methods. Among them, any reference to a memory, storage, database, or other medium used in the embodiments provided in the present application can include non-volatile and / or volatile memories. Non-volatile memories can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memories can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), Rambus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and Rambus dynamic RAM (RDRAM), etc.

[0161] Those skilled in the art can clearly understand that for the convenience and brevity of description, only the above division of each functional unit and module is used as an example. In actual applications, the above functions can be allocated to different functional units and modules according to needs, 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.

[0162] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements for some of the technical features; 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 of the embodiments of the present invention, and should all be included in the protection scope of the present invention.

Claims

1. A robot positioning method, characterized in that, Including: Obtain the UWB ranging value and the odometer pose, where the UWB ranging value is the distance measurement value between the UWB base station and the robot; Calculate the predicted pose that satisfies the constraint conditions according to the UWB ranging value and the odometer pose; Determine the transformation parameters according to the predicted pose and the odometer pose corresponding to the predicted pose; Determine the current pose of the robot according to the transformation parameters and the current odometer pose.

2. The method according to claim 1, wherein The calculating the predicted pose that satisfies the constraint conditions according to the UWB ranging value and the odometer pose includes: Determine the sliding window; Calculate the predicted pose that satisfies the constraint conditions corresponding to the sliding window; Then, the determining the transformation parameters includes: Redetermine the transformation parameters according to the predicted pose of the sliding window and the odometer pose corresponding to the predicted pose.

3. The method according to claim 2, wherein The calculating the predicted pose that satisfies the constraint conditions within the sliding window includes: Construct a distance constraint equation set according to the UWB ranging values included in the sliding window; Construct a displacement constraint equation set according to the UWB ranging values and the odometer pose included in the sliding window; Calculate the predicted pose according to the distance constraint equation set and the displacement constraint equation set.

4. The method according to claim 3, wherein The sliding window includes multiple groups of UWB ranging values, and the constructing a distance constraint equation set according to the UWB ranging values and the odometer pose included in the sliding window includes: Construct a corresponding distance constraint equation according to the UWB ranging value; Construct a distance constraint equation set according to all the distance constraint equations corresponding to the sliding window.

5. The method according to claim 4, characterized in that, The constructing a corresponding distance constraint equation according to each group of the UWB ranging values includes: Generate a predicted translation component; Generate a predicted distance error according to the predicted translation component, the UWB ranging value, and the UWB base station position; Construct the distance constraint equation according to the predicted distance error and the preset distance constraint weight.

6. The method according to claim 3, characterized in that The sliding window includes multiple groups of UWB ranging values, and the constructing a displacement constraint equation set according to the UWB ranging values and the odometer pose included in the sliding window includes: Construct a corresponding rotation constraint equation and translation constraint equation according to the UWB ranging value and the odometer pose; Construct the displacement constraint equation set according to all the translation constraint equations and the rotation constraint equations corresponding to the sliding window.

7. The method according to claim 6, wherein The odometer pose includes an odometer rotation component, and the constructing a corresponding rotation constraint equation according to the UWB ranging value and the odometer pose includes: Generate a predicted rotation component; Construct the rotation constraint equation according to the predicted rotation component and the odometer rotation component.

8. The method according to claim 6, wherein The constructing a corresponding translation constraint equation according to the UWB ranging value and the odometer pose includes: Generate a predicted rotation component and a predicted translation component; Construct the translation constraint equation according to the predicted rotation component, the predicted translation component, and the odometer pose.

9. The method according to claim 8, wherein The constructing the translation constraint equation according to the predicted rotation component, the predicted translation component, and the odometer pose includes: Determine the relative displacement between different odometer poses according to the odometer pose. Construct the translation constraint equation according to the estimated rotation component, the estimated translation component, and the relative displacement.

10. The method according to claim 3, characterized in that The calculating the estimated pose according to the distance constraint equation set and the displacement constraint equation set includes: Construct a cost function of the estimated pose according to the distance constraint equation set and the displacement constraint equation set; Calculate the result that satisfies the minimum condition of the cost function as the estimated pose.

11. The method according to claim 2, wherein Before re-determining the transformation parameter according to the estimated pose of the sliding window and the odometry pose corresponding to the estimated pose, it further includes: Judge whether the estimated pose corresponding to the sliding window satisfies the error condition; If it is satisfied, re-determine the transformation parameter according to the estimated pose of the sliding window and the odometry pose corresponding to the estimated pose; If it is not satisfied, give up determining the transformation parameter.

12. The method according to claim 11, wherein The judging whether the estimated pose corresponding to the sliding window satisfies the error condition includes: Calculate the error mean value according to the UWB ranging value, the position of the UWB base station, and the estimated pose; Judge whether the error mean value is less than a preset threshold; If it is less, determine that the estimated pose satisfies the error condition; If it is greater than or equal to, determine that the estimated pose does not satisfy the error condition.

13. The method according to any one of claims 1 to 10, characterized in that, The odometry pose, the estimated pose, and the transformation parameter all include a rotation matrix and a translation matrix. The determining the transformation parameter according to the odometry pose and the estimated pose includes: Determine the rotation component and translation component of the estimated pose, and the rotation component and translation component of the odometry pose; Determine the rotation component of the transformation parameter according to the rotation component of the estimated pose and the rotation component of the odometry pose; Determine the translation component of the transformation parameter according to the translation component of the estimated pose and the translation component of the odometry pose; Determine the transformation parameter according to the rotation component and translation component of the transformation parameter.

14. The method according to claim 13, characterized in that, The determining the current pose of the robot according to the transformation parameter and the current odometry pose includes: Determine the rotation matrix and translation matrix of the current odometry pose; Determine the target rotation matrix according to the rotation matrix of the current odometry and the rotation matrix of the transformation parameter; Determine the target translation matrix according to the translation matrix of the current odometry and the translation matrix of the transformation parameter; Determine the current pose of the robot according to the target rotation matrix and the target translation matrix.

15. A non-cumulative drift odometer, characterized in that, It includes a sensor and a controller. The sensor is communicatively connected to the controller. The sensor is used to collect the movement information of the robot and the UWB ranging value. The controller outputs the pose of the robot based on the method according to any one of claims 1 to 14 according to the movement information and the UWB ranging value.

16. A robot, characterized in that, It includes a memory and a processor. The memory is connected to the processor. The processor is used to execute one or more computer programs stored in the memory. When the processor executes the one or more computer programs, the robot realizes the method according to any one of claims 1 to 14.

17. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program, and the computer program includes program instructions which, when executed by a processor, cause the processor to execute the method according to any one of claims 1 to 14.

Images