CN114986513A - Speed calculation method and device based on magnetic encoder and robot - Google Patents
Speed calculation method and device based on magnetic encoder and robot Download PDFInfo
- Publication number
- CN114986513A CN114986513A CN202210778560.4A CN202210778560A CN114986513A CN 114986513 A CN114986513 A CN 114986513A CN 202210778560 A CN202210778560 A CN 202210778560A CN 114986513 A CN114986513 A CN 114986513A
- Authority
- CN
- China
- Prior art keywords
- speed
- value
- original
- filter
- acceleration value
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000004364 calculation method Methods 0.000 title claims abstract description 45
- 230000001133 acceleration Effects 0.000 claims abstract description 162
- 238000005070 sampling Methods 0.000 claims abstract description 157
- 238000001914 filtration Methods 0.000 claims abstract description 49
- 238000000034 method Methods 0.000 claims description 16
- 230000004044 response Effects 0.000 abstract description 16
- 238000005259 measurement Methods 0.000 abstract description 10
- 238000010586 diagram Methods 0.000 description 5
- 230000008859 change Effects 0.000 description 3
- 230000000737 periodic effect Effects 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 230000003068 static effect Effects 0.000 description 2
- 125000004122 cyclic group Chemical group 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 230000003111 delayed effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000001228 spectrum Methods 0.000 description 1
- 230000001629 suppression Effects 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/1651—Programme controls characterised by the control loop acceleration, rate control
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Transmission And Conversion Of Sensor Element Output (AREA)
Abstract
The invention discloses a speed calculation method and device based on a magnetic encoder and a robot, wherein the speed calculation method comprises the following steps: calculating an original speed value at each sampling moment according to the position information detected by the magnetic encoder; sequentially storing the original speed values in a first speed filtering window; calculating the original acceleration value of each sampling moment according to the original velocity value of each sampling moment; filtering the original acceleration value to obtain a filtered acceleration value at each sampling moment; determining the length of a second speed filter window according to the filtered acceleration value; and determining the filtered speed value of the current sampling moment according to the original speed value of each sampling moment in the length of the second speed filtering window. Therefore, the speed measurement precision in the constant speed state can be improved, the speed response capability in the variable speed state can be improved, and the speed feedback requirements in various scenes can be met.
Description
Technical Field
The invention relates to the technical field of speed detection, in particular to a speed calculation method and device based on a magnetic encoder and a robot.
Background
In a robot motion control system, absolute magnetic encoders are typically employed as position sensors for each joint of the robot to provide velocity feedback to achieve closed loop control of each joint.
At present, the speed calculation based on the absolute magnetic encoder adopts the following two methods:
first, the current velocity is calculated according to the formula V ═ P ')/T, where V is the current velocity, P is the current sampling position, P' is the last sampling position, and T is the position sampling period (i.e., the time difference between two sampling positions);
however, since noise generally exists at the position fed back by the magnetic encoder, the first method has poor interference resistance, which results in a large deviation between the calculated speed and the actual speed;
second, a filter window (FIFO) as shown in fig. 1 is introduced, and sample positions P1, P2,. and PN are stored in the filter window in time sequence, and the sample positions in the filter window are updated in a manner of shifting data to the left, and the current speed is calculated according to the formula V ═ P1)/(N × T), where V is the current speed, PN is the nth sample position, P1 is the 1 st sample position, N is the length of the filter window, and T is the position sample period.
Although the position noise is smoothed in the second mode, because the filter window is long, sudden and rapid changes of the speed cannot be accurately identified when the filter window is long, the requirement of rapid speed following during speed change cannot be met, and the situation that the calculated speed is seriously delayed from the real speed exists, so that the requirement of the periodic motion of the robot on rapid speed response cannot be met; when the filtering window is short, the speed noise suppression capability is poor, and the requirement of high speed precision cannot be met, so that the requirement of low speed noise when the robot is at rest or at a low speed cannot be met.
Disclosure of Invention
The present invention is directed to solving, at least to some extent, one of the technical problems in the related art. To this end, a first object of the present invention is to provide a velocity calculation method based on a magnetic encoder, which determines a second velocity filter window length according to a filtered acceleration value at each sampling time, and determines a filtered velocity at a current sampling time according to the second velocity filter window length and a raw velocity value at each sampling time. Because the length of the filter window is determined according to the acceleration value, when the acceleration value is small, more speed values can be obtained by adopting a longer filter window to calculate the speed at the current moment, the speed measurement precision in a constant speed state is ensured, when the acceleration value is large, less speed values can be obtained by adopting a shorter filter window to calculate the speed at the current moment, the speed response capability in a variable speed state is ensured, and the speed feedback requirements under various scenes are met.
A second object of the present invention is to provide a speed calculation device based on a magnetic encoder.
A third object of the invention is to propose a robot.
In order to achieve the above object, an embodiment of a first aspect of the present invention provides a speed calculation method based on a magnetic encoder, including: calculating an original speed value at each sampling moment according to the position information detected by the magnetic encoder; sequentially storing the original speed values in a first speed filtering window; calculating the original acceleration value of each sampling moment according to the original velocity value of each sampling moment; filtering the original acceleration value to obtain a filtered acceleration value at each sampling moment; determining the length of a second speed filter window according to the filtered acceleration value; and determining the filtered speed value of the current sampling moment according to the original speed value of each sampling moment in the length of the second speed filtering window.
According to the speed calculation method based on the magnetic encoder, the original speed value of each sampling moment is determined according to the position information detected by the magnetic encoder, the filtered acceleration value of each sampling moment is calculated according to the original speed value of each sampling moment, the length of the second speed filter window is determined according to the filtered acceleration value of each sampling moment, and the filtered speed value of the current sampling moment is determined according to the length of the second speed filter window and the original speed value of each sampling moment. Because the length of the filter window is determined according to the acceleration value, when the acceleration value is small, more speed values can be obtained by adopting a longer filter window to calculate the speed at the current moment, the speed measurement precision in a constant speed state is ensured, when the acceleration value is large, less speed values can be obtained by adopting a shorter filter window to calculate the speed at the current moment, the speed response capability in a variable speed state is ensured, and the speed feedback requirements under various scenes are met.
According to one embodiment of the present invention, sequentially storing raw velocity values within a first velocity filter window comprises: and sequentially storing the original speed values in a first speed filtering window according to the speed updating indexes, wherein when the index value of the speed updating index is greater than the length of the first speed filtering window, the index value of the speed updating index is reset to 1.
According to an embodiment of the present invention, the filtering the original acceleration value to obtain the filtered acceleration value at each sampling time includes: determining a filter type; determining a filter formula according to the type of the filter, and carrying out filter processing on the original acceleration value according to the filter formula to obtain a filtered acceleration value at each sampling moment, wherein when the type of the filter is a first-order low-pass filter, the filtered acceleration value at the current sampling moment is calculated according to the following formula:
wherein,
is the filtered acceleration value for the current sample time,
is the original acceleration value at the last sampling instant,
is the original acceleration value at the current sampling instant i a Update index, ε, for acceleration 1 、ε 2 N is an integer of 2 or more, which is a first-order low-pass filter coefficient.
According to one embodiment of the invention, the second velocity filter window length is calculated according to the following formula:
wherein,
for the second speed filter window length, ε 3 For the coefficients of the filter window, the filter window coefficients,
is the filtered acceleration value of the current sampling moment i a For the acceleration update index, N is an integer greater than or equal to 2.
According to one embodiment of the invention, the filtered velocity value at the current sampling instant is calculated according to the following formula:
wherein V is the filtered velocity value at the current sampling moment, V k For the value of the k-th raw velocity,
for the second speed filter window length, i a For the acceleration update index, N is an integer greater than or equal to 2.
In order to achieve the above object, a second aspect of the present invention provides a speed calculation apparatus based on a magnetic encoder, including: the first processing module is used for calculating an original speed value at each sampling moment according to the position information detected by the magnetic encoder and sequentially storing the original speed values in a first speed filtering window; the second processing module is used for calculating the original acceleration value of each sampling moment according to the original velocity value of each sampling moment and filtering the original acceleration value to obtain the filtered acceleration value of each sampling moment; and the third processing module is used for determining the length of a second speed filter window according to the filtered acceleration value and determining the filtered speed value of the current sampling moment according to the original speed value of each sampling moment in the length of the second speed filter window.
According to the speed calculation device based on the magnetic encoder, the original speed value of each sampling moment is calculated through the first processing module according to the position information detected by the magnetic encoder, the filtered acceleration value of each sampling moment is obtained through the second processing module according to the original speed value of each sampling moment after calculation, the length of the second speed filtering window is determined through the third processing module according to the filtered acceleration value, and the filtered speed of the current sampling moment is determined according to the length of the second speed filtering window and the original speed value of each sampling moment. Because the length of the filter window is determined according to the acceleration value, when the acceleration value is small, more speed values can be obtained by adopting a longer filter window to calculate the speed at the current moment, the speed measurement precision in a constant speed state is ensured, and when the acceleration value is large, less speed values can be obtained by adopting a shorter filter window to calculate the speed at the current moment, the speed response capability in a variable speed state is ensured, so that the speed feedback requirements in various scenes are met.
According to an embodiment of the present invention, the first processing module is specifically configured to: and sequentially storing the original speed values in a first speed filtering window according to the speed updating index, wherein when the index value of the speed updating index is greater than the length of the first speed filtering window, the index value of the speed updating index is reset to 1.
According to an embodiment of the present invention, the second processing module is specifically configured to: determining a filter type; determining a filter formula according to the type of the filter, and carrying out filter processing on the original acceleration value according to the filter formula to obtain a filtered acceleration value at each sampling moment, wherein when the type of the filter is a first-order low-pass filter, the filtered acceleration value at the current sampling moment is calculated according to the following formula:
wherein,
is the filtered acceleration value for the current sample time,
for the original acceleration value at the last sampling instant，
Is the original acceleration value at the current sampling instant i a Updating the index, ε, for acceleration 1 、ε 2 N is an integer of 2 or more, which is a first-order low-pass filter coefficient.
According to an embodiment of the present invention, the third processing module is specifically configured to: calculating a second velocity filter window length according to the following equation:
wherein,
for the second speed filter window length, ε 3 For the coefficients of the filter window, the filter window coefficients,
is the filtered acceleration value of the current sampling moment i a For the acceleration update index, N is an integer greater than or equal to 2.
According to an embodiment of the present invention, the third processing module is specifically configured to: calculating the filtered speed value at the current sampling moment according to the following formula:
wherein V is the filtered velocity value at the current sampling moment, V k For the value of the k-th raw velocity,
for the second speed filter window length, i a For the acceleration update index, N is an integer greater than or equal to 2.
In order to achieve the above object, a robot according to a third aspect of the present invention includes a memory, a processor, and a magnetic encoder-based speed calculation program stored in the memory and executable on the processor, wherein the processor implements the magnetic encoder-based speed calculation method when executing the magnetic encoder-based speed calculation program.
According to the robot provided by the embodiment of the invention, by the speed calculation method based on the magnetic encoder, the speed measurement precision in a constant speed state can be ensured, and the speed response capability in a variable speed state can be ensured, so that the requirements of the robot on speed and low noise when the robot is static or at a low speed can be met, the requirement of the robot on the quick response of the periodic motion to the speed can be met, the closed-loop control performance of the robot is effectively improved, and the running stability of the robot is further improved.
Additional aspects and advantages of the invention will be set forth in part in the description which follows and, in part, will be obvious from the description, or may be learned by practice of the invention.
Drawings
FIG. 1 is a diagram illustrating a filter window of a velocity calculation method in the related art;
FIG. 2 is a flow chart of a method of velocity calculation based on a magnetic encoder according to one embodiment of the present invention;
FIG. 3 is a diagram illustrating a first velocity filter window according to one embodiment of the present invention;
FIG. 4 is a schematic diagram of an acceleration filter window according to an embodiment of the present invention;
FIG. 5 is a schematic diagram of a magnetic encoder based speed calculation apparatus according to one embodiment of the present invention;
fig. 6 is a schematic structural diagram of a robot according to an embodiment of the present invention.
Detailed Description
It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
In the following description, suffixes such as "module", "component", or "unit" used to indicate components are used only for facilitating the description of the present invention, and have no particular meaning in themselves. Thus, "module", "component" or "unit" may be used mixedly.
Referring to fig. 2, a flow chart of a method for calculating a speed based on a magnetic encoder according to an embodiment of the invention is shown, wherein the method for calculating a speed based on a magnetic encoder comprises the following steps:
s1, a raw velocity value is calculated for each sampling time based on the position information detected by the magnetic encoder.
Specifically, the magnetic encoder may be an absolute magnetic encoder, and in use, the magnetic encoder is mounted on an object to be measured (e.g., a robot joint) to detect position information of the object to be measured, and then a raw velocity value at each sampling time is calculated based on the detected position information. For example, the position difference between the position information of the current sampling time and the position information of the previous sampling time is calculated, the time difference between the two sampling times is calculated, and then the ratio of the position difference to the time difference is calculated to obtain the original speed value of the current sampling time, wherein the original speed value contains noise interference.
S2, storing the original speed values in the first speed filter window;
specifically, a filter window with a length of N (N is a fixed value, specifically an integer greater than or equal to 2, and a larger value is selected) is introduced as the first speed filter window, and the structure of the first speed filter window is shown in fig. 3, and is used for storing the original speed value, such as V, at each sampling time 1 、V 2 、...、V N 。
In some embodiments, sequentially storing the raw velocity values within the first velocity filter window comprises: and sequentially storing the original speed values in a first speed filtering window according to the speed updating indexes, wherein when the index value of the speed updating index is greater than the length of the first speed filtering window, the index value of the speed updating index is reset to 1.
That is, a speed update index i may be introduced v Updating index i according to speed v Sequentially storing the original speed value of each sampling moment calculated in the steps into a first speed filtering window, and updating an index i when the speed v Having an index value greater than the first speed filter windowAt length, the speed is updated to index i v The index value of the index is reset to 1 (for the sake of clarity and simplicity of description, the index is in a proper position by default in the application, and no overflow exists after the addition and subtraction of the index), and the index i is updated by introducing the speed v The cyclic shift operation can be avoided when the first speed filter window is updated, so that the operation amount of filter window updating is reduced.
And S3, calculating the original acceleration value at each sampling moment according to the original velocity value at each sampling moment.
Specifically, there are various ways to calculate the original acceleration value at each sampling time from the original velocity value at each sampling time. For example, the original acceleration at the current sampling time may be calculated according to the original velocity value at the previous sampling time and the original velocity value at the current sampling time, that is, the velocity difference between the original velocity value at the current sampling time and the original velocity value at the previous sampling time is calculated, the time difference between the two sampling times is calculated, and then the ratio of the velocity difference to the time difference is calculated to obtain the original acceleration at the current sampling time.
S4, filtering the original acceleration value to obtain a filtered acceleration value at each sampling moment;
specifically, since the original acceleration includes noise interference, in order to further improve the accuracy of the acceleration value at each sampling time, the original acceleration may be filtered to obtain a filtered acceleration value at each sampling time.
In some embodiments, the filtering the original acceleration value to obtain a filtered acceleration value at each sampling time includes: determining a filter type; and determining a filter formula according to the type of the filter, and filtering the original acceleration value according to the filter formula to obtain the filtered acceleration value at each sampling moment.
That is, the original acceleration at each sampling time can be filtered through a filter and in a software manner, so as to obtain a more accurate acceleration value at each sampling time. Optionally, the filter may be a first-order low-pass filter, a second-order low-pass filter, and the like, and may be specifically selected based on actual requirements, for example, a low-pass filter with a smaller order may be selected under the condition that accuracy requirements are met, so as to reduce the amount of computation, for example, an order of low-pass filter may be selected.
As an example, when the filter type is a first-order low-pass filter, the filtered acceleration value at the current sampling time is calculated according to the following equation (1):
wherein,
is the filtered acceleration value for the current sample time,
is the original acceleration value at the last sampling instant,
is the original acceleration value at the current sampling instant i a Updating the index, ε, for acceleration 1 、ε 2 The first-order low-pass filter coefficient can be calculated according to the acceleration noise frequency spectrum. N is an integer of 2 or more.
It should be noted that, in this example, the calculation of the filtered acceleration value participating in the current sampling moment includes the original acceleration value at the previous sampling moment and the original acceleration value at the current sampling moment, so an acceleration filter window with a length of N may be introduced, and the structure of the acceleration filter window is shown in fig. 4, and is used for storing the original acceleration value at each sampling moment, such as a 1 、a 2 、...、a N And the acceleration filtering windows are in one-to-one correspondence with the first speed filtering windows so as to ensure that the original speed value at each sampling moment is in one-to-one correspondence with the original acceleration value. Meanwhile, an acceleration updating index i can be introduced a For indicating the storage location of the original acceleration value at the current sampling instant in the corresponding filter window, numerically i v ＝i a In addition, the acceleration update index i a The working mode of the acceleration updating index is similar to that of the speed updating index, and the repeated description is omitted, and the acceleration updating index i is introduced a The calculation amount of the acceleration filter window update can be reduced.
In the above embodiment, the original acceleration is filtered to eliminate noise interference, so that a more accurate acceleration value at each sampling time can be obtained, and the length of the second speed filter window determined subsequently is more accurate.
And S5, determining the length of the second speed filter window according to the filtered acceleration value.
Specifically, the second speed filter window is a corresponding filter window when the original speed value needs to be selected from the fixed first speed filter window when calculating the filtered speed value at the current sampling time. The length of the second speed filter window is variable, the specific length is determined by the filtered acceleration value at each sampling moment, for example, when the filtered acceleration value is large, the length of the second speed filter window can be set to be short; the second speed filter window length may be set longer as the post-filter acceleration value decreases. That is, the post-filter acceleration value is inversely proportional to the second speed filter window length.
In some embodiments, the second velocity filter window length may be calculated according to the following equation (2):
wherein,
for the second speed filter window length, ε 3 For the coefficients of the filter window, the filter window coefficients,
is the post-filter acceleration value, i, of the current sampling instant a The index is updated for the acceleration and,
ε 4 the minimum filter window length is determined in particular by the magnetic encoder characteristics.
And S6, determining the filtered speed value of the current sampling time according to the original speed value of each sampling time in the length of the second speed filtering window.
Specifically, the speed values of all sampling moments in the length of the second speed filter window can be averaged to obtain the filtered speed value of the current sampling moment, because the length of the second speed filter window is determined by the filtered acceleration value, when the filtered acceleration value is large, the length of the second speed filter window is short, so that the original speed values participating in the calculation of the filtered speed value of the current sampling moment are less, namely the calculation of the filtered speed value of the current sampling moment is less influenced by the original speed values, and the quick speed response can be realized, and the speed response capability during speed change is met; when the filtered acceleration value is smaller, the length of the second speed filtering window is longer, so that more original speed values participating in calculation of the filtered speed value at the current sampling moment are obtained, most noise interference can be well counteracted by more original speed values, the filtered speed value at the current sampling moment is more accurate, and the speed measurement precision at the constant speed is met.
In some embodiments, the filtered velocity value for the current sampling instant may be calculated according to equation (3) below:
wherein, V is the filtered velocity value at the current sampling moment, V k For the value of the k-th original velocity,
for the second speed filter window length, i a The index is updated for acceleration.
As can be seen from the above equation (3), the filtered velocity value V at the current sampling time is obtained by filtering the second velocity window length
Velocity values V at all sampling instants k Averaged over the current filter window length
Determines the value of the original velocity V involved in the calculation k And as can be seen from the above equation (2), the second speed filter window length
Filtered acceleration value of current sampling time relative to current sampling time
Is inversely proportional, so that the post-filter acceleration value
The length of the second speed filter window is reduced, i.e. when the speed changes slowly
Become larger, the original velocity value V participating in the calculation k The number of the sampling points is increased, and more speed values can enable the calculated filtered speed value V at the current sampling moment to be more accurate and less interfered by noise; acceleration value after filtering
The length of the second speed filter window is increased, i.e. when the speed changes faster
Smaller, original velocity values V involved in the calculation k The number of the sampling points is reduced, so that the calculated filtered speed value V at the current sampling moment is more sensitive to speed value change, and high dynamic response is realized. Therefore, the calculation accuracy of the speed at the current moment in the constant speed state can be improved, the response speed of the speed at the current moment in the variable speed state can be improved, and different speed feedback requirements are met.
In the above embodiment, since the length of the filter window is determined according to the acceleration value, when the acceleration value is small, more speed values can be obtained by adopting a longer filter window to calculate the speed at the current moment, so that the speed measurement precision in the constant speed state is ensured, and when the acceleration value is large, less speed values can be obtained by adopting a shorter filter window to calculate the speed at the current moment, so that the speed response capability in the variable speed state is ensured, and thus the speed feedback requirements in various scenes are met.
In summary, according to the speed calculation method based on the magnetic encoder in the embodiment of the present invention, the length of the second speed filter window is determined according to the filtered acceleration value at each sampling time, and the filtered speed at the current sampling time is determined according to the length of the second speed filter window and the original speed value at each sampling time, because the length of the filter window is determined according to the acceleration value, when the acceleration value is smaller, a longer filter window can be used to obtain more speed values to calculate the speed at the current time, so as to ensure the speed measurement accuracy in the constant speed state, and when the acceleration value is larger, a shorter filter window can be used to obtain less speed values to calculate the speed at the current time, so as to ensure the speed response capability in the variable speed state, thereby satisfying the speed feedback requirements in various scenes; meanwhile, the filtered acceleration value at each sampling moment is obtained by filtering the original acceleration, so that the accuracy of the acceleration value can be improved, the accuracy of the length of a second speed filter window is improved, and the accuracy of the speed at the current sampling moment is improved.
In addition, an embodiment of the present invention further provides a speed calculation apparatus based on a magnetic encoder, and referring to fig. 5, the speed calculation apparatus 100 based on a magnetic encoder includes: a first processing module 110, a second processing module 120, and a third processing module 130.
The first processing module 110 is configured to calculate an original speed value at each sampling time according to position information detected by the magnetic encoder, and sequentially store the original speed values in a first speed filtering window; the second processing module 120 is configured to calculate an original acceleration value at each sampling time according to the original velocity value at each sampling time, and perform filtering processing on the original acceleration value to obtain a filtered acceleration value at each sampling time; and the third processing module 130 is configured to determine a length of a second speed filter window according to the filtered acceleration value, and determine a filtered velocity value at the current sampling time according to an original velocity value at each sampling time within the length of the second speed filter window. .
According to an embodiment of the present invention, the first processing module is specifically configured to: and sequentially storing the original speed values in a first speed filtering window according to the speed updating indexes, wherein when the index value of the speed updating index is greater than the length of the first speed filtering window, the index value of the speed updating index is reset to 1.
According to an embodiment of the present invention, the second processing module is specifically configured to: determining a filter type; determining a filter formula according to the type of the filter, and carrying out filter processing on the original acceleration value according to the filter formula to obtain a filtered acceleration value at each sampling moment, wherein when the type of the filter is a first-order low-pass filter, the filtered acceleration value at the current sampling moment is calculated according to the following formula:
wherein,
is the filtered acceleration value at the current sample time,
is the original acceleration value at the last sampling instant,
is the original acceleration value at the current sampling instant i a Updating the index, ε, for acceleration 1 、ε 2 N is an integer of 2 or more, which is a first-order low-pass filter coefficient.
According to an embodiment of the present invention, the third processing module is specifically configured to: calculating a second velocity filter window length according to the following equation:
wherein,
for the second speed filter window length, ε 3 For the coefficients of the filter window, the filter window coefficients,
is the filtered acceleration value of the current sampling moment i a For the acceleration update index, N is an integer equal to or greater than 2.
According to an embodiment of the present invention, the third processing module is specifically configured to: calculating the filtered speed value at the current sampling moment according to the following formula:
wherein V is the filtered velocity value at the current sampling moment, V k For the value of the k-th raw velocity,
for the second speed filter window length, i a For the acceleration update index, N is an integer greater than or equal to 2.
The specific implementation of the speed calculation apparatus 100 based on a magnetic encoder according to the embodiment of the present invention is basically the same as that of the above embodiments of the speed calculation method based on a magnetic encoder, and is not described herein again.
According to the speed calculation device based on the magnetic encoder, the length of the second speed filter window is determined according to the filtered acceleration value at each sampling moment, and the filtered speed at the current sampling moment is determined according to the length of the second speed filter window and the original speed value at each sampling moment. Because the length of the filter window is determined according to the acceleration value, when the acceleration value is smaller, more speed values can be obtained by adopting a longer filter window to calculate the speed at the current moment, the speed measurement precision in a constant speed state is ensured, and when the acceleration value is larger, less speed values can be obtained by adopting a shorter filter window to calculate the speed at the current moment, the speed response capability in a variable speed state is ensured, so that the speed feedback requirements in various scenes are met; meanwhile, the acceleration value at each sampling moment is obtained by filtering the original acceleration, so that the accuracy of the acceleration value can be improved, the accuracy of the length of the second speed filtering window is improved, and the accuracy of the speed at the current moment is improved.
In addition, referring to fig. 6, the robot 200 includes a memory 210, a processor 220, and a speed calculation program based on a magnetic encoder, which is stored in the memory 210 and is executable on the processor 220, and when the processor 220 executes the speed calculation program based on the magnetic encoder, the speed calculation method based on the magnetic encoder is implemented.
The extension of the specific implementation of the robot 200 according to the embodiment of the present invention is substantially the same as that of each embodiment of the speed calculation method based on the magnetic encoder, and is not described herein again.
According to the robot provided by the embodiment of the invention, by the speed calculation method based on the magnetic encoder, the speed measurement precision in a constant speed state can be ensured, and the speed response capability in a variable speed state can be ensured, so that the requirements of the robot on speed and low noise when the robot is static or at a low speed can be met, the requirement of the robot on the quick response of the periodic motion to the speed can be met, the closed-loop control performance of the robot is effectively improved, and the running stability of the robot is further improved.
It should be noted that, in this document, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising an … …" does not exclude the presence of other like elements in a process, method, article, or apparatus that comprises the element.
The above-mentioned serial numbers of the embodiments of the present invention are merely for description and do not represent the merits of the embodiments.
Through the above description of the embodiments, those skilled in the art will clearly understand that the method of the above embodiments can be implemented by software plus a necessary general hardware platform, and certainly can also be implemented by hardware, but in many cases, the former is a better implementation manner. Based on such understanding, the technical solutions of the present invention may be embodied in the form of a software product, which is stored in a storage medium (such as ROM/RAM, magnetic disk, optical disk) and includes instructions for enabling a terminal (such as a mobile phone, a computer, a server, an air conditioner, or a network device) to execute the method according to the embodiments of the present invention.
While the present invention has been described with reference to the embodiments shown in the drawings, the present invention is not limited to the embodiments, which are illustrative and not restrictive, and it will be apparent to those skilled in the art that various changes and modifications can be made therein without departing from the spirit and scope of the invention as defined in the appended claims.
Claims (11)
1. A method of velocity calculation based on a magnetic encoder, comprising:
calculating an original speed value at each sampling moment according to the position information detected by the magnetic encoder;
sequentially storing the original speed values in a first speed filtering window;
calculating the original acceleration value of each sampling moment according to the original velocity value of each sampling moment;
filtering the original acceleration value to obtain a filtered acceleration value at each sampling moment;
determining the length of a second speed filter window according to the filtered acceleration value;
and determining the filtered speed value of the current sampling moment according to the original speed value of each sampling moment in the length of the second speed filtering window.
2. The method of claim 1, wherein storing the raw speed values sequentially within a first speed filter window comprises:
and sequentially storing the original speed values in the first speed filtering window according to a speed updating index, wherein when the index value of the speed updating index is greater than the length of the first speed filtering window, the index value of the speed updating index is reset to 1.
3. The method according to claim 1, wherein the filtering the original acceleration value to obtain a filtered acceleration value at each sampling time comprises:
determining a filter type;
determining a filter formula according to the filter type, and performing filter processing on the original acceleration value according to the filter formula to obtain a filtered acceleration value at each sampling moment, wherein when the filter type is a first-order low-pass filter, the filtered acceleration value at the current sampling moment is calculated according to the following formula:
wherein,
is the filtered acceleration value at the current sample time,
is the original acceleration value at the last sampling instant,
for the original acceleration value at the current sampling instant, i a Updating the index, ε, for acceleration 1 、ε 2 N is an integer of 2 or more, which is a first-order low-pass filter coefficient.
4. A velocity calculation method according to any one of claims 1-3, wherein the second velocity filter window length is calculated according to the following formula:
wherein,
filtering window length, ε, for said second velocity 3 For the coefficients of the filter window, the filter window coefficients,
is the filtered acceleration value of the current sampling moment i a For the acceleration update index, N is an integer greater than or equal to 2.
5. The speed calculation method of claim 4, wherein the filtered speed value for the current sampling instant is calculated according to the following formula:
wherein V is the filtered velocity value of the current sampling moment, V k For the value of the k-th raw velocity,
for said second speed filtering window length, i a For the acceleration update index, N is an integer greater than or equal to 2.
6. A magnetic encoder based speed calculation apparatus, comprising:
the first processing module is used for calculating an original speed value at each sampling moment according to the position information detected by the magnetic encoder and sequentially storing the original speed values in a first speed filtering window;
the second processing module is used for calculating the original acceleration value of each sampling moment according to the original velocity value of each sampling moment and filtering the original acceleration value to obtain a filtered acceleration value of each sampling moment;
and the third processing module is used for determining the length of a second speed filter window according to the filtered acceleration value and determining the filtered speed value at the current sampling moment according to the original speed value of each sampling moment in the length of the second speed filter window.
7. The speed calculation device of claim 6, wherein the first processing module is specifically configured to:
and sequentially storing the original speed values in the first speed filtering window according to a speed updating index, wherein when the index value of the speed updating index is greater than the length of the first speed filtering window, the index value of the speed updating index is reset to 1.
8. The speed calculation device of claim 6, wherein the second processing module is specifically configured to:
determining a filter type;
determining a filter formula according to the filter type, and performing filter processing on the original acceleration value according to the filter formula to obtain a filtered acceleration value at each sampling moment, wherein when the filter type is a first-order low-pass filter, the filtered acceleration value at the current sampling moment is calculated according to the following formula:
wherein,
is the filtered acceleration value for the current sample time,
is the original acceleration value at the last sampling instant,
is the original acceleration value at the current sampling instant i a Updating the index, ε, for acceleration 1 、ε 2 N is an integer of 2 or more, which is a first-order low-pass filter coefficient.
9. The speed calculation device according to any one of claims 6-8, wherein the third processing module is specifically configured to: calculating the second velocity filter window length according to the following equation:
wherein,
filtering window length, ε, for said second velocity 3 For the coefficients of the filter window, the filter window coefficients,
is the filtered acceleration value of the current sampling moment i a For the acceleration update index, N is an integer greater than or equal to 2.
10. The speed calculation device of claim 9, wherein the third processing module is specifically configured to: calculating the filtered speed value of the current sampling moment according to the following formula:
wherein V is the filtered velocity value of the current sampling moment, V k For the value of the k-th raw velocity,
for said second speed filtering window length, i a For the acceleration update index, N is an integer greater than or equal to 2.
11. A robot comprising a memory, a processor, and a magnetic encoder based speed calculation program stored on the memory and executable on the processor, the processor implementing the magnetic encoder based speed calculation method according to any one of claims 1-5 when executing the magnetic encoder based speed calculation program.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210778560.4A CN114986513A (en) | 2022-06-30 | 2022-06-30 | Speed calculation method and device based on magnetic encoder and robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210778560.4A CN114986513A (en) | 2022-06-30 | 2022-06-30 | Speed calculation method and device based on magnetic encoder and robot |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114986513A true CN114986513A (en) | 2022-09-02 |
Family
ID=83020512
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210778560.4A Pending CN114986513A (en) | 2022-06-30 | 2022-06-30 | Speed calculation method and device based on magnetic encoder and robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114986513A (en) |
-
2022
- 2022-06-30 CN CN202210778560.4A patent/CN114986513A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US9341499B2 (en) | Sensor system | |
| Narasimhappa et al. | A modified Sage-Husa adaptive Kalman filter for denoising Fiber Optic Gyroscope signal | |
| US20120215477A1 (en) | Accelerometer and Automatic Calibration of Same | |
| RU2011152911A (en) | METHOD FOR CALIBRATING AN INERTIAL SENSOR INSTALLED IN AN ARBITRARY POSITION ON THE BOARD OF THE VEHICLE, AND THE SYSTEM OF DYNAMIC PARAMETERS OF THE VEHICLE DONE | |
| CN110375772B (en) | Ring laser random error modeling and compensating method for adaptive Kalman filtering | |
| CN109586645B (en) | Permanent magnet synchronous motor inertia identification method and equipment | |
| CN111546346B (en) | Flexible joint disturbance observation method, moment control method and equipment | |
| CN108413986B (en) | Gyroscope filtering method based on Sage-Husa Kalman filtering | |
| CN112762962A (en) | Zero offset compensation method for micro-electro-mechanical system accelerometer based on temperature hysteresis model | |
| CN105912013A (en) | Model-free self-adaptive control method for attitude of assembled spacecraft | |
| CN112304337B (en) | Motion angle estimation method and system based on gyroscope and accelerometer | |
| CN113791240B (en) | Acceleration estimation method, system, equipment and medium based on high-order synovial membrane tracking differentiator | |
| WO2018081945A1 (en) | Method for detecting background noise of sensor, and device thereof | |
| CN107356786B (en) | Method and device for calibrating accelerometer and computer-readable storage medium | |
| CN114986513A (en) | Speed calculation method and device based on magnetic encoder and robot | |
| CN112067844B (en) | MEMS sensor array type high-precision output control method | |
| CN114665780A (en) | Control method, device and equipment of permanent magnet synchronous motor and storage medium | |
| CN112461258A (en) | Parameter correction method and device | |
| CN111427262A (en) | Intelligent control method for unknown disturbance of MEMS sensor in extreme environment | |
| CN110350886A (en) | A kind of resonance frequency detection method, system and the device of permanent magnetic servo system | |
| CN115313931A (en) | Sensor-free vector control method of permanent magnet synchronous motor based on AEKF | |
| CN113076826B (en) | Filtering method and device of sensor | |
| US11333678B2 (en) | Method for offset calibration of a yaw rate sensor signal of a yaw rate sensor, system and computer program | |
| CN113758503A (en) | Process parameter estimation method and device, electronic equipment and storage medium | |
| Amalkar et al. | Design and implementation of sensorless speed control for induction motor drive using an optimized extended kalman filter |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |