A real-time calibration method, device, robot and medium for the error of a wheel-type displacement gauge

Abstract

The present invention provides a method for real-time calibration of the error of a wheel displacement meter. S1, obtaining a first displacement positioning result of laser positioning when the robot moves forward normally and / or obtaining a second displacement positioning result of laser positioning when the robot moves backward normally; S2, optimizing the forward optimization cost function and / or the backward optimization cost function by using the Gaussian-Newton method according to the first displacement positioning result and / or the second displacement positioning result, and obtaining the error coefficient in the measurement model of the wheel displacement meter with error parameters; S3, correcting the positioning result of the wheel displacement meter according to the error coefficient to obtain the final positioning result given by the wheel displacement meter. The present invention uses the positioning result of laser positioning to estimate two error parameters in the measurement model of the wheel displacement meter with error parameters, thereby improving the accuracy of the wheel displacement meter of the patrol robot.

Description

Technical Field

[0001] The present invention relates to the technical field of robots, and more specifically, to a method and device for real-time calibration of errors of a wheel displacement gauge, a robot, and a medium. Background Art

[0002] A patrol robot belongs to the category of wheeled mobile robots. It has a differential drive structure or a similar Ackerman drive structure of an automobile (the front wheels are responsible for steering and the rear wheels are responsible for driving). The wheel displacement gauge calculates the displacement of the robot by indicating how many circles the wheels of the wheeled robot have advanced. The wheel displacement gauge is not affected by external signals and is an independent and relatively reliable means of robot positioning.

[0003] The wheel displacement gauge is used to calculate how far the patrol robot has moved. The wheel displacement calculation formula is as follows

[0004] L = α * r

[0005] Where α represents the angle measured cumulatively by the code disk, in radians. The code disk is installed on the wheel or the rotating shaft to measure the rotation angle of the wheel or the rotating shaft; r represents the radius of the wheel, in meters. The calculation formula of the wheel displacement gauge is simple and convenient. It can be seen from the above formula that its main error sources are the accuracy of the wheel radius r and the accuracy of the angle α measured by the code disk. The size of the wheel radius r is greatly affected by the tire pressure and wear of the wheel. Low tire pressure and wear will both cause the actual radius r of the wheel to become smaller. The installation accuracy of the code disk on the rotating shaft or the hub, the wheel angle problem caused by the starting and stopping inertia of the robot, the accuracy of the sensor itself, and other interferences will all affect the accuracy of the code disk. The error parameter measurement model for wheel displacement calculation is:

[0006] L1 = (α + δα) * (r + δr) α>0 when the wheel moves forward

[0007] L2 = (α - δα) * (r + δr) α<0 when the wheel moves backward

[0008] The background description provided in this article is for the purpose of presenting the context of the present disclosure generally. Unless otherwise indicated herein, the materials described in this section are not prior art to the claims of this application and should not be admitted to be prior art by including them in this section. Summary of the Invention

[0009] In view of the above technical problems in the related art, the present invention proposes a method for real-time calibration of errors of a wheel displacement gauge, which includes the following steps:

[0010] S1. Obtain the first displacement positioning result Lidar_S of the laser positioning when the robot moves forward normally i, where the subscript i represents the i-th moment and / or obtains the second displacement positioning result Lidar_S of the laser positioning for the robot to move backward normally j , where the subscript j represents the j-th moment;

[0011] S2. According to the first displacement positioning result and / or the second displacement positioning result, the Gauss-Newton method is used to optimize the forward optimization cost function and / or the backward optimization cost function, and the error coefficient in the measurement model of the wheel displacement meter with error parameters is obtained;

[0012] S3. According to the error coefficient, the positioning result of the wheel displacement meter is corrected to obtain the final positioning result given by the wheel displacement meter.

[0013] Specifically, the measurement model with error parameters is:

[0014] L1 = (α + δα) * (r + δr) α > 0, the wheel moves forward

[0015] L2 = (α - δα) * (r + δr) α < 0, the wheel moves backward

[0016] Specifically, the laser odometer is used to give the first displacement positioning result and / or the second displacement positioning result.

[0017] Specifically, the forward optimization cost function is:

[0018]

[0019] Or / and

[0020] The backward optimization cost function is:

[0021]

[0022] In a second aspect, another embodiment of the present invention discloses a device for real-time calibration of the error of a wheel displacement meter, which includes the following units:

[0023] A laser positioning result acquisition unit for acquiring the first displacement positioning result Lidar_S of the laser positioning for the robot to move forward normally i , where the subscript i represents the i-th moment and / or obtains the second displacement positioning result Lidar_S of the laser positioning for the robot to move backward normally j , where the subscript j represents the j-th moment;

[0024] An error coefficient estimation unit for optimizing the forward optimization cost function and / or the backward optimization cost function according to the first displacement positioning result and / or the second displacement positioning result by using the Gauss-Newton method, and obtaining the error coefficient in the measurement model of the wheel displacement meter with error parameters;

[0025] The wheel displacement meter positioning result correction unit is used to correct the positioning result of the wheel displacement meter according to the error coefficient to obtain the final positioning result given by the wheel displacement meter.

[0026] Specifically, the error parameter measurement model is as follows:

[0027] L1 = (α + δα) * (r + δr), where α > 0 and the wheel is moving forward

[0028] L2 = (α - δα) * (r + δr), where α < 0 and the wheel is moving backward

[0029] Specifically, the laser odometer is used to give the first displacement positioning result or / and the second displacement positioning result.

[0030] Specifically, the forward optimization cost function is:

[0031]

[0032] Or / and

[0033] The backward optimization cost function is:

[0034]

[0035] In a third aspect, another embodiment of the present invention discloses a robot, which includes a central processor, a memory, a laser odometer, and a wheel displacement meter. Instructions are stored on the memory, and when the processor executes the instructions, it is used to implement the above-mentioned real-time error calibration method for the wheel displacement meter.

[0036] In a fourth aspect, another embodiment of the present invention discloses a non-volatile memory. Instructions are stored on the memory, and when the processor executes the instructions, it is used to implement the above-mentioned real-time error calibration method for the wheel displacement meter.

[0037] The present invention uses the positioning result of laser positioning to estimate the two error parameters δα and δr in the error parameter measurement model of the wheel displacement meter, thereby improving the accuracy of the wheel displacement meter of the patrol robot. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0039] Figure 1It is a flowchart of a method for real-time calibration of the error of a wheel-type displacement meter provided by an embodiment of the present invention;

[0040] Figure 2 It is a flowchart of another method for real-time calibration of the error of a wheel-type displacement meter provided by an embodiment of the present invention;

[0041] Figure 3 It is a schematic diagram of a device for real-time calibration of the error of a wheel-type displacement meter provided by an embodiment of the present invention;

[0042] Figure 4 It is a schematic diagram of another device for real-time calibration of the error of a wheel-type displacement meter provided by an embodiment of the present invention;

[0043] Figure 5 It is a schematic diagram of a device for real-time calibration of the error of a wheel-type displacement meter provided by an embodiment of the present invention. Specific embodiments

[0044] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art belong to the scope of protection of the present invention.

[0045] Embodiment 1

[0046] Refer to Figure 1 , this embodiment discloses a method for real-time calibration of the error of a wheel-type displacement meter, which includes the following steps:

[0047] S1, set the measurement model of the wheel-type displacement meter with error parameters;

[0048] The robot for patrol operation in this embodiment has laser positioning and a wheel-type displacement meter. Among them, laser positioning can be used to confirm the position of the robot. The laser positioning function can be realized by existing lidar or laser odometer. The above laser positioning methods all belong to the prior art and will not be elaborated in this embodiment. The initial r of the wheel-type displacement meter can be directly given according to the tire parameters or measured with a caliper; the α angle is measured and output by the encoder device; the error coefficients δr and δα at the initial moment are set to 0.

[0049] The measurement model of the wheel-type displacement meter is as follows:

[0050] L1 = (α + δα) * (r + δr) when α > 0 and the wheel moves forward

[0051] L2 = (α - δα) * (r + δr) when α < 0 and the wheel moves backward

[0052] S2, obtain the first displacement positioning result Lidar_S of the laser positioning when the robot moves forward normallyi , where the subscript i represents the i-th moment;

[0053] The robot in this embodiment has a laser odometer, and the laser odometer can give the positioning information of the robot at fixed time intervals. For example, the laser odometer can give the positioning information of the robot at an interval of 1 s.

[0054] S3. Construct the forward optimization cost function

[0055]

[0056] S4. Obtain the second displacement positioning result Lidar_S of the laser positioning when the robot moves backward normally j , where the subscript j represents the j-th moment;

[0057] The obtaining method in this step is similar to that in step S2 and will not be elaborated here.

[0058] S5. Construct the backward optimization cost function

[0059]

[0060] S6. Optimize the forward optimization cost function and the backward optimization cost function according to the first displacement positioning result and the second displacement positioning result by using the Gauss-Newton method, and obtain the error coefficient in the measurement model of the wheel displacement meter with error parameters;

[0061] Call the Gauss-Newton method in the Ceres library to iteratively optimize the cost function in the above steps. Essentially, it is to continuously adjust the variables δr and δα until the whole reaches the minimum value, and complete the estimation of the error coefficient of the wheel displacement meter.

[0062] S7. Correct the positioning result of the wheel displacement meter according to the error coefficient to obtain the final positioning result given by the wheel displacement meter.

[0063] In this embodiment, according to the obtained error coefficient, when the wheel displacement meter outputs the result subsequently, the obtained error coefficient is used to calculate the output result of the wheel displacement meter.

[0064] This embodiment uses the positioning result of the laser positioning to estimate the two error parameters δα and δr in the measurement model of the wheel displacement meter with error parameters, thereby improving the accuracy of the wheel displacement meter of the patrol robot.

[0065] Embodiment 2

[0066] Reference Figure 2 , this embodiment discloses a method for real-time calibration of the error of a wheel displacement meter, which includes the following steps:

[0067] S1. Obtain the first displacement positioning result Lidar_S of the laser positioning for the robot to move forward normally i , where the subscript i represents the i-th moment and / or obtain the second displacement positioning result Lidar_S of the laser positioning for the robot to move backward normally j , where the subscript j represents the j-th moment;

[0068] S2. According to the first displacement positioning result and / or the second displacement positioning result, use the Gauss-Newton method to optimize the forward optimization cost function and / or the backward optimization cost function, and obtain the error coefficients in the measurement model of the wheel displacement meter with error parameters;

[0069] Among them,

[0070] The measurement model of the wheel displacement meter with error parameters is as follows:

[0071] L1 = (α + δα) * (r + δr) α > 0, the wheel moves forward

[0072] L2 = (α - δα) * (r + δr) α < 0, the wheel moves backward

[0073] The forward optimization cost function is:

[0074]

[0075] The backward optimization cost function is:

[0076]

[0077] S3. Calibrate the positioning result of the wheel displacement meter according to the error coefficients to obtain the final positioning result given by the wheel displacement meter.

[0078] In this embodiment, the positioning result of the laser positioning is used to estimate the two error parameters δα and δr in the measurement model of the wheel displacement meter with error parameters, thereby improving the accuracy of the wheel displacement meter of the patrol robot.

[0079] Embodiment III

[0080] Reference Figure 3 , this embodiment discloses a device for real-time calibration of the error of a wheel displacement meter, which includes the following units:

[0081] The measurement model setting unit with error parameters is used to set the measurement model of the wheel displacement meter with error parameters;

[0082] The robot for patrol operation in this embodiment is equipped with laser positioning and a wheel displacement meter. The laser positioning can be used to confirm the position of the robot. The laser positioning function can be implemented by existing lidar or laser odometer. The above laser positioning methods all belong to the prior art and will not be elaborated in this embodiment. The initial r of the wheel displacement meter can be directly given according to the tire parameters or measured with a caliper; the α angle is measured and output by a code disc device; the initial moment error coefficients δr and δα are set to 0.

[0083] The measurement model of the wheel displacement meter is as follows:

[0084] L1 = (α + δα) * (r + δr) when α > 0, the wheel moves forward

[0085] L2 = (α - δα) * (r + δr) when α < 0, the wheel moves backward

[0086] The first displacement positioning result acquisition unit is used to acquire the first displacement positioning result Lidar_S of the laser positioning when the robot moves forward normally i , where the subscript i represents the i-th moment;

[0087] The robot in this embodiment is equipped with a laser odometer, which can give the positioning information of the robot at fixed time intervals. For example, the laser odometer can give the positioning information of the robot every 1 s.

[0088] The forward optimization cost function construction unit is used to construct the forward optimization cost function

[0089]

[0090] The second displacement positioning result acquisition unit is used to acquire the second displacement positioning result Lidar_S of the laser positioning when the robot moves backward normally j , where the subscript j represents the j-th moment;

[0091] The backward optimization cost function construction unit is used to construct the backward optimization cost function

[0092]

[0093] The error coefficient estimation unit is used to optimize the forward optimization cost function and the backward optimization cost function by using the Gauss-Newton method according to the first displacement positioning result and the second displacement positioning result, and obtain the error coefficients in the measurement model of the wheel displacement meter with error parameters;

[0094] Calling the Gauss-Newton method in the Ceres library to iteratively optimize the cost function in the above steps is essentially to continuously adjust the variables δr and δα until the whole reaches the minimum value, and complete the estimation of the error coefficients of the wheel displacement meter.

[0095] The wheel displacement meter positioning result correction unit is used to correct the positioning result of the wheel displacement meter according to the error coefficient to obtain the final positioning result given by the wheel displacement meter.

[0096] In this embodiment, the positioning result of laser positioning is used to estimate the two error parameters δα and δr in the measurement model of the wheel displacement meter with error parameters, thereby improving the accuracy of the wheel displacement meter of the patrol robot.

[0097] Embodiment 4

[0098] Reference Figure 4 , this embodiment discloses a real-time calibration device for wheel displacement meter errors, which includes the following units:

[0099] The laser positioning result acquisition unit is used to acquire the first displacement positioning result Lidar_S of the laser positioning when the robot moves forward normally i , where the subscript i represents the i-th moment or / and acquire the second displacement positioning result Lidar_S of the laser positioning when the robot moves backward normally j , where the subscript j represents the j-th moment;

[0100] The error coefficient estimation unit is used to optimize the forward optimization cost function or / and the backward optimization cost function according to the first displacement positioning result or / and the second displacement positioning result by using the Gauss-Newton method, and obtain the error coefficient in the measurement model of the wheel displacement meter with error parameters;

[0101] Among them,

[0102] The measurement model of the wheel displacement meter with error parameters is as follows:

[0103] L1 = (α + δα) * (r + δr) α>0 when the wheel moves forward

[0104] L2 = (α - δα) * (r + δr) α<0 when the wheel moves backward

[0105] The forward optimization cost function is:

[0106]

[0107] The backward optimization cost function is:

[0108]

[0109] The wheel displacement meter positioning result correction unit is used to correct the positioning result of the wheel displacement meter according to the error coefficient to obtain the final positioning result given by the wheel displacement meter.

[0110] In this embodiment, the positioning result of laser positioning is used to estimate the two error parameters δα and δr in the measurement model of the wheel displacement meter with error parameters, so as to improve the accuracy of the wheel displacement meter of the patrol robot.

[0111] Embodiment 5

[0112] This embodiment discloses a robot, which includes a central processor, a memory, a laser odometer, and a wheel displacement meter. Instructions are stored on the memory, and when the processor executes the instructions, it is used to implement the above-mentioned real-time calibration method for the error of the wheel displacement meter.

[0113] Embodiment 6

[0114] Reference Figure 5 , Figure 5 FIG. is a schematic structural diagram of a device for real-time calibration of the error of a wheel displacement meter according to this embodiment. The device 20 for real-time calibration of the error of the wheel displacement meter in this embodiment includes a processor 21, a memory 22, and a computer program stored in the memory 22 and executable on the processor 21. When the processor 21 executes the computer program, it implements the steps in the above method embodiment. Alternatively, when the processor 21 executes the computer program, it implements the functions of each module / unit in the above device embodiments.

[0115] Exemplarily, the computer program can be divided into one or more modules / units. The one or more modules / units are stored in the memory 22 and executed by the processor 21 to complete the present invention. The one or more modules / units can be a series of computer program instruction segments capable of performing specific functions, and this instruction segment is used to describe the execution process of the computer program in the device 20 for real-time calibration of the error of the wheel displacement meter. For example, the computer program can be divided into the respective modules in Embodiment 2. For the specific functions of each module, please refer to the working process of the device described in the above embodiments, and details are not described herein again.

[0116] The device 20 for real-time calibration of the error of the wheel displacement meter may include, but is not limited to, a processor 21 and a memory 22. Those skilled in the art can understand that the schematic diagram is only an example of the device 20 for real-time calibration of the error of the wheel displacement meter, and does not constitute a limitation on the device 20 for real-time calibration of the error of the wheel displacement meter. It may include more or fewer components than shown in the figure, or combine certain components, or different components. For example, the device 20 for real-time calibration of the error of the wheel displacement meter may further include input / output devices, network access devices, buses, etc.

[0117] The processor 21 may be a Central Processing Unit (CPU), or may also be other general-purpose processors, Digital Signal Processors (DSPs), Application Specific Integrated Circuits (ASICs), Field-Programmable Gate Arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc. The processor 21 is the control center of the wheel displacement meter error real-time calibration device 20, and connects various parts of the entire wheel displacement meter error real-time calibration device 20 through various interfaces and lines.

[0118] The memory 22 can be used to store the computer programs and / or modules. The processor 21 realizes various functions of the wheel displacement meter error real-time calibration device 20 by running or executing the computer programs and / or modules stored in the memory 22, and by calling the data stored in the memory 22. The memory 22 may mainly include a program storage area and a data storage area. Among them, the program storage area can store an operating system, application programs required for at least one function (such as a sound playback function, an image playback function, etc.); the data storage area can store data created according to the use of the mobile phone (such as audio data, phone book, etc.). In addition, the memory 22 may include high-speed random access memory, and may also include non-volatile memory, such as a hard disk, a memory, a plug-in hard disk, a Smart Media Card (SMC), a Secure Digital (SD) card, a Flash Card, at least one magnetic disk storage device, a flash memory device, or other volatile solid-state storage devices.

[0119] Among them, if the modules / units integrated in the wheel displacement error real-time calibration device 20 are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on such an understanding, to implement all or part of the processes in the above-described embodiment methods of the present invention, it can also be completed by a computer program instructing relevant hardware. The computer program can be stored in a computer-readable storage medium. When the computer program is executed by the processor 21, the steps of the above-described various method embodiments can be implemented. Among them, the computer program includes computer program code, and the computer program code can be in the form of source code, object code, executable file, or some intermediate form, etc. The computer-readable medium can include: any entity or device capable of carrying the computer program code, recording medium, USB flash drive, mobile hard disk, magnetic disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signal, telecommunication signal, and software distribution medium, etc. It should be noted that the content included in the computer-readable medium can be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, the computer-readable medium does not include electrical carrier signals and telecommunication signals.

[0120] It should be noted that the device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separated. The components shown as units may or may not be physical units, that is, they may be located in one place or distributed to multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this embodiment. In addition, in the attached drawings of the device embodiments provided by the present invention, the connection relationship between the modules indicates that they have a communication connection, which can be specifically implemented as one or more communication buses or signal lines. Those of ordinary skill in the art can understand and implement it without creative efforts.

[0121] The above is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent replacements, improvements, 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 real-time calibration method for the error of a wheel displacement meter, which comprises the following steps: S1, obtain the first displacement positioning result Lidar_S of the laser positioning when the robot moves forward normally i , where the subscript i represents the i-th moment and / or obtain the second displacement positioning result Lidar_S of the laser positioning when the robot moves backward normally j , where the subscript j represents the j-th moment; S2, according to the first displacement positioning result Lidar_S i or / and the second displacement positioning result Lidar_S j adopt the Gauss-Newton method to optimize the forward optimization cost function or / and the backward optimization cost function, and obtain the error coefficient in the error parameter measurement model of the wheel displacement meter; The measurement model with error parameters is as follows: L1 = (α + δα) * (r + δr) when α > 0 and the wheel moves forward; L2 = (α - δα) * (r + δr) when α < 0 and the wheel moves backward; Among them, α is the cumulative measurement angle of the code disk, r is the radius of the wheel, δα is the code disk angle error coefficient, and δr is the wheel diameter error coefficient; The forward optimization cost function is: Or / and The backward optimization cost function is: S3. Correct the positioning result of the wheel displacement meter according to the error coefficient to obtain the final positioning result given by the wheel displacement meter.

2. The method according to claim 1, wherein: Use the laser odometer to give the first displacement positioning result or / and the second displacement positioning result.

3. A real-time calibration device for the error of a wheel displacement meter, which comprises the following units: The laser positioning result acquisition unit is used to obtain the first displacement positioning result Lidar_S of the laser positioning for the robot to move forward normally i , where the subscript i represents the i-th moment and / or the second displacement positioning result Lidar_S of the laser positioning for the robot to move backward normally j , where the subscript j represents the j-th moment; An error coefficient estimation unit, which is used to optimize the forward optimization cost function or / and the backward optimization cost function by using the Gauss-Newton method according to the first displacement positioning result or / and the second displacement positioning result, and obtain the error coefficients in the measurement model with error parameters of the wheel displacement meter; The measurement model with error parameters is as follows: L1 = (α + δα) * (r + δr) when α > 0 and the wheel moves forward; L2 = (α - δα) * (r + δr) when α < 0 and the wheel moves backward; Among them, α is the cumulative measurement angle of the code disk, r is the radius of the wheel, δα is the code disk angle error coefficient, and δr is the wheel diameter error coefficient; The forward optimization cost function is: Or / and The backward optimization cost function is: A wheel displacement meter positioning result correction unit, which is used to correct the positioning result of the wheel displacement meter according to the error coefficient to obtain the final positioning result given by the wheel displacement meter.

4. The device according to claim 3, characterized in that: Use the laser odometer to give the first displacement positioning result or / and the second displacement positioning result.

5. A robot, which includes a central processor, a memory, a laser odometer, and a wheel displacement meter. Instructions are stored on the memory, and when the processor executes the instructions, it is used to implement the method described in any one of claims 1-2.

6. A non-volatile computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the central processor of the robot, it implements the real-time calibration method for the error of the wheel displacement meter described in any one of claims 1-2.

Images