A robot servo motor rotor position estimation method based on circle fitting

Abstract

This invention discloses a method for estimating the rotor position of a robot servo motor based on circle fitting. The method includes: in each control cycle of the robot servo motor, using a Hall sensor to detect the absolute position of the servo motor rotor in real time and outputting two Hall signals; using a fitted Taubin circle and its parameters to perform debiasing and normalization processing on the two Hall signals output in each control cycle, and calculating the corrected rotor position angle for each control cycle using an arctangent function; wherein the Taubin circle is constructed based on data digitization and robust fitting using the RANSAC algorithm. This invention improves the accuracy of robot joint motor position detection and control performance by fitting a Taubin circle based on data digitization and the RANSAC algorithm, correcting the Hall signals in real time, and calculating the rotor position angle.

Description

Technical Field

[0001] This invention relates to the field of robot servo control technology, and more specifically to a method for estimating the rotor position of a robot servo motor based on circle fitting. Background Technology

[0002] In the servo drive systems of modern industrial robots, collaborative robots, and humanoid robots, permanent magnet synchronous motors (PMSMs) and brushless DC motors (BLDCs) are core actuators due to their high power density and excellent dynamic performance. The vector control (FOC) of these motors relies heavily on real-time and precise feedback of the rotor flux position.

[0003] Hall effect sensors, as a method of absolute position detection, have physical advantages such as low cost, small size, and resistance to oil and dust. However, the analog sine and cosine signals output by Hall sensors are often extremely coarse. In actual robot joint modules, due to limitations in installation accuracy, the uniformity of magnetization of the magnets, and interference from the strong electromagnetic field of the motor stator, the trajectory of the original signal on the phase plane is not an ideal standard circle, but rather exhibits an eccentric, elliptical, non-orthogonal, or even Lissajous figure with a large amount of noise.

[0004] If these distorted signals are processed directly using the arctangent (Atan2) algorithm, the calculated electrical angles will contain significant periodic errors and random noise. These angle errors will be directly amplified as torque ripples in the FOC control loop, leading to robot end effector jitter, increased heat generation, and decreased control accuracy.

[0005] Existing signal correction methods mainly include:

[0006] Least squares circle fitting (Kåsa method): minimizes the sum of squares of algebraic distance, but is extremely sensitive to outliers. Voltage spikes that occur during motor commutation or high current switching can severely disturb the fitting results. At the same time, this method has a "shrinkage bias" when processing short circular arc data, tending to estimate circles with too small a radius.

[0007] Look-up table (LUT) calibration: Signal values ​​at multiple angular positions are pre-calibrated, and interpolation is performed by looking up the table during runtime. However, the calibration workload is large, and it cannot adapt to sensor aging or changes in operating conditions.

[0008] Kalman filtering requires an accurate noise statistical model, has high computational complexity, and is difficult to run in real time on low-cost MCUs.

[0009] Existing methods generally lack the ability to comprehensively handle multi-source errors (deterministic distortion + random noise + impulse interference) and also ignore the unbiasedness problem of the circle fitting algorithm itself. Especially in application scenarios where robot joints do not rotate in a full circle but only swing within a limited angular range, the performance of traditional methods degrades significantly.

[0010] Therefore, how to achieve robust, high-precision, and low-deviation correction of Hall sensor signals on embedded platforms with limited computing resources is a problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0011] In view of the above problems, the present invention is proposed to provide a method for estimating the rotor position of a robot servo motor based on circle fitting to overcome or at least partially solve the above problems. The method is based on data digitization and RANSAC algorithm to fit the Taubin circle, correct the Hall signal in real time and calculate the rotor position angle.

[0012] To achieve the above objectives, the present invention adopts the following technical solution:

[0013] In a first aspect, embodiments of the present invention provide a method for estimating the rotor position of a robot servo motor based on circle fitting, comprising:

[0014] S1. In each control cycle of the robot servo motor, a Hall sensor is used to detect the absolute position of the servo motor rotor in real time and output two Hall signals.

[0015] S2. The fitted Taubin circle and its parameters are used to debias and normalize the two Hall signals output in each control cycle, and the rotor position angle after correction in each control cycle is calculated by the arctangent function; wherein, the Taubin circle is constructed based on data digitization and robust fitting of the RANSAC algorithm.

[0016] Furthermore, in step S2, the fitting process of the Taubin circle is as follows:

[0017] S21. For the two Hall signals acquired within any period, perform centering preprocessing within a sliding window; the centering preprocessing is: calculating the data centroid and translating the modulus coordinate system to the centroid;

[0018] S22. Based on the RANSAC algorithm, robust fitting is performed on the preprocessed data after centering to solve the corresponding circle model and obtain the corresponding set of interior points.

[0019] S23. Based on the set of interior points, construct a data scatter matrix M and a Taubin constraint matrix N, transform the problem of solving the circle parameters into the problem of solving the generalized eigenvalues, solve the minimum positive eigenvalue and its corresponding eigenvector, and obtain the center coordinates and radius of the circle.

[0020] S24. Transform the fitted center coordinates from the centered coordinate system back to the modulo coordinate system to obtain the fitted Taubin circle parameters.

[0021] Furthermore, in step S1, the two Hall signals satisfy the distortion model:

[0022]

[0023]

[0024]

[0025] in, h This is a two-channel Hall signal model. and To measure the vectors of the two Hall signals, u For an ideal unit circle signal, o Zero-point offset, G This is the magnitude gain matrix. k x , k y These are the amplitude gains of the two channels, Let be a non-orthogonal rotation matrix, and η be random noise.

[0026] Furthermore, in step S21, the sliding window is expressed by the formula:

[0027]

[0028]

[0029]

[0030]

[0031] Where W is determined by the number of pole pairs p and angular velocity of the servo motor. The calculated window length, Electric angular velocity, For half an electrical cycle time, The sampling frequency of the analog-to-digital converter. W min Minimum window length ，W max The maximum window length, This is the window length coefficient.

[0032] Furthermore, in step S21, the centralized preprocessing specifically includes:

[0033] Data within the sliding window The centroid coordinates of the data are calculated using the following formula:

[0034]

[0035] The centralization transformation can be expressed by the following formula:

[0036]

[0037] in, i The number of data points within the sliding window, ( ) are the coordinates of the centroid, ( x,y i ) represents the data within the sliding window, x i ’,y i ’ () represents the data after the centralization transformation.

[0038] Furthermore, step S22 specifically includes:

[0039] S221. Based on the RANSAC algorithm, three data points are randomly sampled from the data after the centering transformation, and a candidate circle model is fitted.

[0040] Three randomly sampled data points are used to construct a set of undetermined coefficients. D , E , F If the circle is a circle, then the coordinates of its center are ( x c ,y c ) and radius R They are represented as follows:

[0041]

[0042]

[0043]

[0044]

[0045] S222. Calculate the geometric distance from the data after the centering transformation to the circumference of the candidate circle, and classify the data with a geometric distance less than a preset threshold into an interior point set;

[0046] S223. Repeat steps S221-S222 multiple times, and select the circle model with the most data in the set of interior points as the optimal model.

[0047] Furthermore, step S23 specifically includes:

[0048] S231. Construct a data scatter matrix M and a Taubin constraint matrix N based on the set of interior points, expressed by the following formula:

[0049]

[0050]

[0051]

[0052]

[0053] in, The number of interior points in the set of interior points. For the first j The feature vectors of each interior point This is the transpose of the eigenvector. For the first j The sum of the squares of the x and y coordinates of each interior point For the first j The x-coordinates of the interior points For the first j The y-coordinates of the interior points, where N is the Taubin constraint matrix. For all interior points The average of the values;

[0054] S232, Constructing the Characteristic Polynomial And solve for it to obtain information about The fourth-degree polynomial:

[0055]

[0056] The minimum positive eigenvalue is solved using the Newton-Raphson iterative method. Substituting into the polynomial, we get:

[0057]

[0058] Solve for the parameter vector In the equation, A, B, C, and D are the coefficients of the equations in the corresponding plane geometric circles. c 0、 c 1. c 2. c 3. c 4 represents the corresponding polynomial coefficients. These are generalized eigenvalues;

[0059] S233. Based on the solved values ​​of B, C, and D, calculate the coordinates of the center and the radius of the circle:

[0060]

[0061]

[0062]

[0063] in, The fitted center coordinates of the circle in the centered coordinate system. Rc,rel The radius of the fitted circle is denoted as .

[0064] Furthermore, in step S24, the fitted center coordinates are transformed from the centralized coordinate system back to the modular coordinate system, and the transformation formula is:

[0065]

[0066] in, The coordinates of the absolute center of the circle in the modular coordinate system;

[0067] The obtained fitted Taubin circle parameters include: absolute center coordinates, circle radius, timestamp, and valid flag; wherein, the circle radius is equal to the fitted circle radius; the timestamp is the time sequence number of the control cycle; and the valid flag is determined based on the reasonableness of the circle center position, the reasonableness of the radius, the inlier rate, and the fitting residual.

[0068] Furthermore, in step S2, the process of debiasing and normalizing the two Hall signals output in each control cycle using the fitted Taubin circle and its parameters, and calculating the corrected rotor position angle for each control cycle using the arctangent function, specifically includes:

[0069] In each servo motor control cycle, acquire the raw analog values ​​of the two Hall signals. ;

[0070] Using the absolute center coordinates The original modulus value is debiased to obtain the debiased signal. The formula is expressed as follows:

[0071]

[0072] Using the fitted circle radius R c,rel Normalization is performed, expressed by the formula:

[0073]

[0074] The rotor position electrical angle is calculated using the four-quadrant arctangent function. and mechanical angle It can be expressed by the formula:

[0075]

[0076]

[0077] in, This is the standard unit circle signal component after center debiasing and radius normalization.

[0078] As can be seen from the above technical solution, compared with the prior art, the present invention discloses a method for estimating the rotor position of a robot servo motor based on circle fitting, which has the following beneficial effects:

[0079] This invention designs a sliding window plus annular buffer structure to continuously collect the latest Hall data; at the same time, it constructs a Taubin circle based on data centralization and RANSAC algorithm to estimate the true circular trajectory parameters (center and radius) of the Hall sensor signal with high robustness and high accuracy, thus laying the foundation for the accurate calculation of the rotor position angle in the future.

[0080] This invention removes impulse noise and outliers using the RANSAC algorithm and combines it with the Taubin unbiased estimation algorithm, effectively solving the problems of noise sensitivity and short-circuit data deviation in the traditional least squares method, and significantly improving the accuracy of robot joint motor position detection and control performance.

[0081] This invention also supports multiple self-calibration strategies such as periodic triggering, operating condition change triggering, and temperature drift triggering; it achieves maintenance-free adaptive angle compensation throughout the entire life cycle, reducing manufacturing and maintenance costs. Attached Figure Description

[0082] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0083] Figure 1 This is a flowchart of a robot servo motor rotor position estimation method based on Taubin circle fitting provided in an embodiment of the present invention. Detailed Implementation

[0084] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0085] This invention discloses a method for estimating the rotor position of a robot servo motor based on circle fitting, referring to... Figure 1 As shown, it includes:

[0086] S1. In each control cycle of the robot servo motor, a Hall sensor is used to detect the absolute position of the servo motor rotor in real time and output two Hall signals.

[0087] S2. The fitted Taubin circle and its parameters are used to debias and normalize the two Hall signals output in each control cycle, and the rotor position angle after correction in each control cycle is calculated by the arctangent function; wherein, the Taubin circle is constructed based on data digitization and robust fitting of the RANSAC algorithm.

[0088] This embodiment employs the method of the present invention in a robot joint controller (such as a servo drive board based on STM32 or TI C2000). When a temperature change / load change is detected, the background task automatically triggers a Taubin circle fitting calibration process. After calibration, the Taubin circle parameters are stored in a double buffer. In each FOC control cycle (e.g., 10kHz, i.e., 100μs), the original Hall signal is read in real time, and the latest effective parameters are used to perform debiasing and normalization. The high-precision rotor electrical angle is calculated through atan2. Even when the motor is at low speed, stopped, or subjected to external disturbances, it can continuously output a stable angle value without jumps, avoiding angle errors caused by bias due to traditional lookup table methods or simple arctangent methods.

[0089] This embodiment uses a TLE5012B magnetic encoder Hall sensor to output two analog sine and cosine signals. and The sensor is connected to the 12-bit ADC input port of the controller (such as an STM32F405 microcontroller). The ADC is configured for high-frequency sampling (e.g., The dual-channel synchronous sampling is triggered by a timer interrupt, and the conversion result is automatically moved to memory using DMA.

[0090] The specific implementation steps of this embodiment are as follows:

[0091] 1) Acquire the two sine and cosine signals output by the Hall sensor and store the sampling points in a sliding window buffer of fixed length;

[0092] 2) Perform centering preprocessing on the data within the sliding window, calculate the data centroid, and translate the modular coordinate system to the centroid;

[0093] 3) Use the RANSAC algorithm to robustly fit the centered data, randomly sample three points to solve the candidate circle model, calculate the geometric distance from all points to the candidate circle, determine the interior points according to the adaptive threshold, iteratively update and obtain the optimal model containing the most interior points.

[0094] 4) Construct a data scatter matrix M and a Taubin constraint matrix N for the set of interior points after RANSAC filtering, solve the generalized eigenvalue problem to obtain the minimum positive eigenvalue and its corresponding eigenvector, and extract the center coordinates and radius of the circle;

[0095] 5) Transform the fitted circle center coordinates from the centered coordinate system back to the original coordinate system, update and store the circle parameters;

[0096] 6) Read the real-time Hall signal in each control cycle, use the stored circular parameters for debiasing and normalization, and calculate the corrected rotor position angle using the arctangent function.

[0097] The steps of this embodiment are described in detail below.

[0098] First, follow step 1) to acquire the two sine and cosine signals output by the Hall sensor.

[0099] Two signals output by the Hall sensor and Ideally, the following conditions should be met:

[0100] in, For rotor electrical angle, This represents the signal amplitude.

[0101] The actual signal is affected by various errors, and the two Hall signals satisfy the distortion model:

[0102]

[0103]

[0104]

[0105]

[0106]

[0107]

[0108] in, h This is a two-channel Hall signal model; and Here are the measured vectors of the two Hall signals; G is the amplitude gain matrix. These represent the amplitude gains of the two signals, respectively. It is a non-orthogonal rotation matrix; Let be the phase non-orthogonality error angle between the two signals; u is the ideal unit circle signal vector; o is the zero-point offset vector. These are the zero-point offsets of the two signals, respectively; It is a random noise vector. These are the random noises of the two signals, respectively.

[0109] Specifically, it can be elaborated as follows:

[0110]

[0111] To efficiently store continuously acquired data points, this embodiment employs a circular buffer data structure. This structure updates the head pointer cyclically, ensuring that new data overwrites the oldest data. It offers advantages such as no memory relocation and fixed memory usage, making it suitable for the real-time requirements of embedded systems. The buffer stores the W most recent data points. and Sampling points (converted to floating-point numbers)

[0112] The choice of window length W needs to balance data coverage, computational real-time performance, and memory constraints. W should cover at least half an electrical cycle of data to ensure that the fitted circular arc has a sufficient arc length.

[0113] This embodiment is based on the number of motor pole pairs p and the rotational speed. (Mechanical angular velocity), dynamically adjust window length:

[0114] Calculate the electric angular velocity:

[0115] Calculate the time of half an electrical cycle:

[0116] Calculate the number of sampling points required for half a cycle:

[0117] in, The sampling frequency of the analog-to-digital converter (in Hz).

[0118] The actual setting is W. half a certain multiple And limit it to a preset minimum and maximum value (such as [500, 2000]) to ensure stability at low and high speeds.

[0119] The formula is expressed as follows:

[0120] In this embodiment, the sliding window length W is set to 500~2000 sampling points, and a circular buffer structure is used for storage. The window update frequency is the ADC sampling frequency.

[0121] Secondly, perform data preprocessing according to step 2).

[0122] Centralization involves shifting the data so that the centroid (mean) of the dataset is located at the origin.

[0123] For the raw data within the sliding window: , where W is the length of the sliding window.

[0124] Calculate the centroid coordinates:

[0125] Centralization transformation: After centralization, the mean of the new dataset is zero. .

[0126] To improve efficiency, the centroid in this embodiment is calculated using an incremental update method. When new data... Enter window, old data When sliding out:

[0127]

[0128] However, in this embodiment, since RANSAC needs to traverse all points, it is simpler to traverse the circular buffer once to calculate the centroid when calibration is triggered, and to use double-precision floating-point numbers (double) for summation to avoid accumulated errors.

[0129] Iterate through all valid data points in the circular buffer and subtract the centroid calculated above. This generates a new set of centralized data arrays, which serve as input for the subsequent RANSAC algorithm.

[0130] Next, perform RANSAC robust fitting based on 3).

[0131] The RANSAC (Random Sample Consensus) algorithm used in this embodiment is an iterative robust parameter estimation method for estimating model parameters from datasets containing a large number of outliers. Its core idea is:

[0132] Hypothesis: Randomly sample the smallest subset from the dataset (fitting a circle, the smallest subset is 3 points) to fit a temporary model (circle).

[0133] Verification: Test the model with all data and count the number of data points (inliers) that match the model, i.e., those whose distance to the circumference is less than a certain threshold. point.

[0134] Selection: Repeat the first two steps k times and select the model with the most interior points as the optimal model.

[0135] In this embodiment, the proportion of interior points in the dataset is set to be... The probability that all three randomly sampled points are interior points is: .

[0136] The probability of success in at least one of k independent samplings is p (usually taken as 0.99):

[0137]

[0138] The inverse solution yields the required number of iterations, k:

[0139]

[0140] Based on internal point rate Conservative estimates (such as) A fixed number of iterations k (e.g., k=50) can be determined to ensure high confidence.

[0141] This embodiment provides three non-collinear points. The general equation of a circle is:

[0142]

[0143] Substituting the three points, we obtain a system of linear equations about D, E, and F:

[0144]

[0145] Written in matrix form:

[0146]

[0147] Solve for D, E, and F using Cramer's Rule. If the determinant of the coefficient matrix... If the value is close to zero, then the three points are collinear, and the fitting fails.

[0148] After solving for D, E, and F, the center and radius of the circle are:

[0149]

[0150] Verification required Otherwise, it does not constitute a valid circle.

[0151] In addition, the threshold for determining interior points This is crucial for RANSAC performance. This embodiment employs an adaptive thresholding strategy based on the chi-square distribution. It is assumed that the noise follows a normal distribution. Then the sum of the squared distances from the point to the real circle follows a chi-square distribution with 1 degree of freedom. .

[0152] This embodiment calculates the average distance from the data point to the centroid (as a rough estimate of the radius R) and the noise standard deviation. (Estimated via MAD), thresholds are set dynamically. The circle model with the largest number of data points in the interior point set is selected as the optimal model, and the corresponding interior point set is obtained.

[0153] Next, follow step 4) to perform Taubin circle fitting.

[0154] 4.1) Construct the data scatter matrix M and Taubin constraint matrix N based on the set of interior points.

[0155] RANSAC-selected A set of internal points (already centralized) is used to construct a data scatter matrix M.

[0156] Define vector ,in .

[0157] The data scatter matrix M is a normalized sum of the outer products of all interior point vectors:

[0158]

[0159] Because the data has been centralized ( The data scatter matrix M simplifies to a diagonally dominant form:

[0160]

[0161] in, These are the central moments of the interior point set of each order.

[0162] The Taubin constraint matrix N is used to normalize the algebraic distance, making its first order approximate the geometric distance, thereby eliminating the shrinkage bias of the traditional least squares method; then

[0163]

[0164] 4.2) Constructing the characteristic polynomial transforms the problem of solving for circle parameters into solving for generalized eigenvalues:

[0165]

[0166] This is equivalent to solving the characteristic equation:

[0167]

[0168] Expanding the determinant, we get about The fourth-degree polynomial:

[0169]

[0170] polynomial coefficients The elements of matrices M and N are calculated using defined algebraic relationships.

[0171] This embodiment uses Newton's iteration method to solve for the minimum positive eigenvalue. The optimal solution fitted by Taubin corresponds to... The smallest positive feature The root is solved using the Newton-Raphson iterative method:

[0172] Calculate the first derivative of the polynomial: Set initial values ​​for iteration (e.g., 0.01); Execute iteration: .when or The iteration stops when the absolute value is less than the convergence threshold.

[0173] The obtained minimum positive eigenvalue Substitute into the system of linear equations:

[0174]

[0175] This is a system of singular linear equations. In this embodiment, the normalization fixed method is used to solve for the parameter vector. If A = 1, then the system of equations degenerates into a single equation. The non-homogeneous linear system of equations can be used to solve for the values ​​of B, C, and D.

[0176] 4.3) Calculate the coordinates of the center and the radius based on the obtained B, C and D values.

[0177] From parameter vector Extracting geometric parameters (note that A=1 at this point):

[0178] Center of the circle (relative to the centralized coordinate system):

[0179]

[0180] radius:

[0181]

[0182] The discriminant needs to be verified. and for radius Perform a rationality check; if it fails, the fit is deemed unsuccessful.

[0183] Next, follow step 5) to perform coordinate transformation and parameter storage.

[0184] Step 4) Obtain the center coordinates of the circle It is relative to the centralized coordinate system, and needs to be transformed back to the original ADC coordinate system to obtain the absolute center coordinates. The transformation formula is:

[0185]

[0186] in The original data centroid coordinates calculated in step 2). Radius It remains unchanged under coordinate translation.

[0187] Before storing the parameters, this embodiment performs multiple validity checks:

[0188] Reasonableness of the center position: Check Whether it is within or near the ADC sampling range.

[0189] Radius rationality: Check Does it match the nominal amplitude of the Hall signal?

[0190] Interior ratio: Check whether the proportion of interior points screened by RANSAC is greater than the set threshold (e.g., 50%).

[0191] Fitting residuals: Check if the average distance from all interior points to the fitted circle is small enough.

[0192] If any check item fails, retain the last valid parameters and trigger the alarm flag.

[0193] This embodiment employs a double-buffering mechanism to store circle parameters: center coordinates, radius, timestamp, and valid flags. The calibration task writes new parameters to the backup buffer, while the FOC real-time interrupt reads parameters from the main buffer. The main buffer is updated via pointer switching or atomic operations to avoid data races during FOC interrupt reads.

[0194] This embodiment employs multiple strategies to trigger calibration updates:

[0195] Low-frequency periodic updates: such as triggering a complete RANSAC-Taubin calibration every 100ms.

[0196] Condition-triggered update: Triggered when a sudden change in Hall signal or a significant change in motor operating conditions (speed, load) is detected.

[0197] Temperature drift update: Triggered when a sensor temperature change exceeding a threshold is detected. A moving average is applied to multiple consecutive calibration results to suppress parameter jitter.

[0198] Finally, perform real-time position correction and angle calculation according to step 6).

[0199] In each FOC control cycle (e.g., 100μs), the interrupt service routine reads the raw ADC values ​​of the two Hall signals. .

[0200] Using the absolute center coordinates stored in step 5) The real-time sampled data is de-biased.

[0201]

[0202] The signal after bias removal The centroid has been aligned with the origin, eliminating zero-point drift error.

[0203] This embodiment eliminates amplitude inconsistencies and non-orthogonality errors by utilizing the fitted circle radius. R c,rel Normalization is performed, expressed by the formula:

[0204]

[0205] in, This is the standard unit circle signal component after center debiasing and radius normalization.

[0206] The rotor position electrical angle is calculated using the four-quadrant arctangent function (atan2). :

[0207]

[0208] For a motor with p pole pairs, the Hall sensor detects the electrical angle. , and mechanical angle The relationship is:

[0209] Based on this, the electrical angle is converted into the mechanical angle.

[0210] This invention effectively solves the problems of noise sensitivity and short-circuit data deviation in the traditional least squares method by eliminating impulse noise and outliers through RANSAC and combining it with the Taubin unbiased estimation algorithm, thus significantly improving the accuracy of robot joint motor position detection and control performance.

[0211] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. The same or similar parts between the various embodiments can be referred to each other.

[0212] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A method for estimating the rotor position of a robot servo motor based on circle fitting, characterized in that, include: S1. In each control cycle of the robot servo motor, a Hall sensor is used to detect the absolute position of the servo motor rotor in real time and output two Hall signals. S2. The fitted Taubin circle and its parameters are used to debias and normalize the two Hall signals output in each control cycle, and the corrected rotor position angle in each control cycle is calculated by the arctangent function; wherein, the Taubin circle is constructed based on data digitization and robust fitting of the RANSAC algorithm. In step S2, the fitting process of the Taubin circle is as follows: S21. For the two Hall signals acquired within any period, perform centering preprocessing within a sliding window; the centering preprocessing is: calculating the data centroid and translating the modulus coordinate system to the centroid; S22. Based on the RANSAC algorithm, robust fitting is performed on the preprocessed data after centering to solve the corresponding circle model and obtain the corresponding set of interior points. S23. Based on the set of interior points, construct the data scatter matrix M and the Taubin constraint matrix N, solve for the minimum positive eigenvalue and its corresponding eigenvector, and obtain the center coordinates and radius of the circle. S24. Transform the center coordinates of the circle from the centralized coordinate system back to the modular coordinate system to obtain the fitted Taubin circle parameters.

2. The method as described in claim 1, characterized in that, In step S1, the two Hall signals satisfy the distortion model: in, h This is a two-channel Hall signal model. and To measure the vectors of the two Hall signals, u For an ideal unit circle signal, o Zero-point offset, G This is the magnitude gain matrix. k x , k y These are the amplitude gains of the two channels, Let be a non-orthogonal rotation matrix, and η be random noise.

3. The method as described in claim 1, characterized in that, In step S21, the sliding window is expressed by the formula: Where W is determined by the number of pole pairs p and angular velocity of the servo motor. The calculated window length, Electric angular velocity, For half an electrical cycle time, The sampling frequency of the analog-to-digital converter. Wmin Minimum window length, Wmax The maximum window length, This is the window length coefficient.

4. The method as described in claim 3, characterized in that, In step S21, the centralized preprocessing specifically includes: Data within the sliding window The centroid coordinates of the data are calculated using the following formula: The centralization transformation can be expressed by the following formula: in, i The number of data points within the sliding window, ( ) are the coordinates of the centroid, ( x i ,y i ) represents the data within the sliding window, x i ’,y i ’ () represents the data after the centralization transformation.

5. The method as described in claim 4, characterized in that, Step S22 specifically includes: S221. Based on the RANSAC algorithm, three data points are randomly sampled from the data after the centering transformation, and a candidate circle model is fitted. Three randomly sampled data points are used to construct a set of undetermined coefficients. D , E , F If the circle is a circle, then the coordinates of its center are ( x c ,y c ) and radius R They are represented as follows: S222. Calculate the geometric distance from the data after the centering transformation to the circumference of the candidate circle, and classify the data with a geometric distance less than a preset threshold into an interior point set; S223. Repeat steps S221-S222 for a preset number of times, and select the circle model with the most data in the set of interior points as the optimal model.

6. The method as described in claim 5, characterized in that, Step S23 specifically includes: S231. Construct a data scatter matrix M and a Taubin constraint matrix N based on the set of interior points, expressed by the following formula: in, The number of interior points in the set of interior points. For the first j The feature vectors of each interior point This is the transpose of the eigenvector. For the first j The sum of the squares of the x and y coordinates of each interior point For the first j The x-coordinates of the interior points For the first j The y-coordinates of the interior points, where N is the Taubin constraint matrix. For all interior points The average of the values; S232, Constructing the Characteristic Polynomial And solve for it to obtain information about The fourth-degree polynomial: The minimum positive eigenvalue is solved using the Newton-Raphson iterative method. Substituting into the polynomial, we get: Solve for the parameter vector In the equation, A, B, C, and D are the coefficients of the equations in the corresponding plane geometric circles. c 0、 c 1. c 2. c 3. c 4 are the corresponding polynomial coefficients, These are generalized eigenvalues; S233. Based on the solved values ​​of B, C, and D, calculate the coordinates of the center and the radius of the circle: in, The fitted center coordinates of the circle in the centered coordinate system. Rc,rel The radius of the fitted circle is denoted as .

7. The method as described in claim 6, characterized in that, In step S24, the fitted center coordinates are transformed from the centered coordinate system back to the modulo coordinate system. The transformation formula is as follows: in, The coordinates of the absolute center of the circle in the modular coordinate system; The obtained fitted Taubin circle parameters include: absolute center coordinates, circle radius, timestamp, and valid flag; wherein, the circle radius is equal to the fitted circle radius; the timestamp is the time sequence number of the control cycle; and the valid flag is determined based on the reasonableness of the circle center position, the reasonableness of the radius, the inlier rate, and the fitting residual.

8. The method as described in claim 7, characterized in that, In step S2, the fitted Taubin circle and its parameters are used to debias and normalize the two Hall signals output in each control cycle, and the corrected rotor position angle for each control cycle is calculated using the arctangent function; specifically, this includes: In each servo motor control cycle, acquire the raw analog values ​​of the two Hall signals. ; Using the absolute center coordinates The original modulus value is debiased to obtain the debiased signal. The formula is expressed as follows: Using the fitted circle radius R c,rel Normalization is performed, expressed by the formula: The rotor position electrical angle is calculated using the four-quadrant arctangent function. and mechanical angle It can be expressed by the formula: in, This is the standard unit circle signal component after center debiasing and radius normalization.

Images