A pose smoothing method and system, and electronic device

Abstract

The application provides a pose smoothing method and system and an electronic device, comprising: acquiring a pose quaternion of an original pose at a current time, updating a pose cache area according to the acquired pose quaternion, and determining a first weight coefficient of the cache area according to a preset cache area weight coefficient calculation method; calculating a first weighted quaternion cumulative sum matrix of the original pose quaternion through a preset pose weighted smoothing method, performing normalization processing on the first weighted quaternion cumulative sum matrix, and calculating a smoothed pose quaternion; performing smoothing pose post-processing on the smoothed pose quaternion through a preset smoothing pose post-processing method, and outputting the processed pose quaternion as the smoothed pose quaternion at the current time, so that the pose smoother in the application has a linear phase, ensures that the pose data is not distorted and does not jump, reduces the pose data noise, reduces the time lag, and meets the demand for real-time smoothing and denoising of the pose.

Description

Technical Field

[0001] This invention relates to the field of attitude smoothing methods, specifically to an attitude smoothing method and system, and electronic equipment. Background Technology

[0002] In orthopedic surgical robot products, NDI cameras are typically used to acquire the patient's pose in real time to control the robotic arm to track the lesion. However, the output data from NDI cameras is subject to noise interference. Before using the measured pose data for real-time tracking, it needs to be smoothed. This improves the stability of the robotic arm's tracking and reduces vibration, resulting in smoother movement. Using unsmoothed pose data directly can increase robotic arm vibration and potentially lead to poor osteotomy accuracy, reducing the product's system-level precision. Furthermore, the real-time display of the patient and osteotomy tools on the software interface of orthopedic surgical robot products also requires the system to smooth the pose data after acquisition before display to improve display continuity and ensure the displayed image is realistic and noise-free.

[0003] Attitude data can be represented in various ways, generally through Euler angles, axis angles, rotation matrices, or quaternions. Euler angles, however, represent attitudes in a non-unique manner, as different Euler angles can represent the same attitude. This can easily lead to numerical jumps during smoothing, and attitudes represented by Euler angles may also suffer from gimbal lock, affecting conversions with other attitude representations. The axis angle method uses a spatial rotation axis and the angle of rotation around that axis to represent attitude. When the angle of rotation around the axis is near 0° (360°), data jumps can easily occur, affecting the numerical solution for attitude smoothing. Rotation matrices require nine variables to store attitude data, and there is an orthogonality between the data in each column. Directly smoothing the data often affects its unity and orthogonality. In comparison, quaternions are more suitable as the original data form for attitude data smoothing. However, conventional direct processing of single-channel quaternion data can also affect the unity of the quaternion representation of attitude, and general quaternion processing methods can also have problems with the continuity (jumps) of attitude data.

[0004] Patent application number US9492237B2 discloses a method for smoothing position data, which uses a method of filtering the three position components separately through low-pass filters. For smoothing attitude data, the method is to first convert the attitude data into quaternions, and then filter the four components of the quaternions separately through low-pass filters. This method is prone to smoothing attitude jumps, and filtering the four components separately will, in principle, cause phase synchronization problems, resulting in distortion of the smoothed attitude data. In addition, the quaternion synthesized from the four filtered components is not a unit quaternion and needs to be normalized again before it can be used to represent attitude.

[0005] Patent application CN116476073A discloses a posture smoothing method based on the Kalman filter principle. It uses robotic arm feedback data to fuse camera pose data to estimate the actual pose of the patient, aiming to reduce the noise level of the patient tracker. The Kalman filter principle assumes that the noise of the data is a composite Gaussian distribution. However, in the field of medical robotics, the vibration of the patient tracker is often not a random vibration that conforms to a normal distribution, but a passive vibration caused by oscillating saw bone cutting or grinding, which has certain vibration noise characteristics. Therefore, this method cannot achieve effective smoothing and accuracy improvement of posture data, and will bring a large amount of unnecessary computation.

[0006] Therefore, the existing technology still needs further development. Summary of the Invention

[0007] The purpose of this invention is to overcome the above-mentioned technical deficiencies and provide an attitude smoothing method, system, and electronic device to solve the problems existing in the prior art.

[0008] To achieve the above-mentioned technical objectives, according to a first aspect of the present invention, the present invention provides an attitude smoothing method, comprising: S100. Obtain the attitude quaternion of the original attitude at the current moment, update the attitude buffer according to the obtained attitude quaternion, and determine the first weight coefficient of the buffer according to the preset buffer weight coefficient calculation method. S200. Calculate the first weighted quaternion sum matrix of the original attitude quaternion using a preset attitude weighted smoothing method. Normalize the first weighted quaternion sum matrix to obtain the second weighted quaternion sum matrix, and calculate the smoothed attitude quaternion. S300. The smoothed attitude quaternion is processed by a preset smoothed attitude post-processing method to obtain the smoothed attitude quaternion, and the processed attitude quaternion is output as the smoothed attitude quaternion at the current time.

[0009] Specifically, updating the pose buffer based on the obtained pose quaternions includes any of the following: First-in-first-out queue storage update, moving array index storage update.

[0010] Specifically, the steps for determining the first weight coefficient of the cache based on the preset cache weight coefficient calculation method include: The first weighting coefficient of the cache is denoted as... The specific calculation process is as follows: First, determine the number of first weight coefficients in the cache. The specific calculation formula is as follows: ; ; ; in, It represents the percentage of fluctuations within the smoothed frequency band. It is the minimum attenuation within the filtering frequency band. It is the sampling frequency. It is the maximum value of the smooth frequency band. It is the minimum value of the filter frequency band, and A and B are process variables; Secondly, the calculation process design coefficients The calculation formula is as follows: ; The distribution coefficient of the first weight coefficient of the buffer is determined by the following formula. , ; in, For process variables, , If it is a zero-order modified Bessel function of the first kind, then it can be obtained from the distribution coefficient. The first weighting coefficient of the cache is calculated using the following formula. : ; in, The median of the transition frequency band after normalization to half the sampling frequency; The lag time caused by smoothing can be expressed as: ; in, The lag time caused by smoothing, This represents the sampling time interval.

[0011] Specifically, the step of calculating the first weighted quaternion sum matrix of the original attitude quaternions using a preset attitude weighted smoothing method includes: Suppose that the pose buffer contains n poses represented by quaternions. ,Will The first weighting coefficient for each pose is set as follows: Then, the first weighted quaternion sum matrix of the original pose is calculated according to the following formula. ; in, It is the first weighted quaternion sum matrix of the original pose.

[0012] Specifically, the normalization of the first weighted quaternion sum matrix to obtain the second weighted quaternion sum matrix, and the calculation of the smoothed attitude quaternions, includes: Calculate the first weighting coefficient The sum of The calculation formula is as follows: ; For the first weighted quaternion cumulative sum matrix After normalization, the second weighted quaternion sum matrix is ​​obtained, calculated as follows: ; in, This is the second weighted quaternion sum matrix.

[0013] Specifically, the step of normalizing the first weighted quaternion sum matrix to obtain the second weighted quaternion sum matrix and calculating the smoothed attitude quaternions further includes: Calculate the eigenvalues ​​and eigenvectors of the second weighted quaternion sum matrix, obtain the eigenvector corresponding to the largest real eigenvalue, and use the eigenvector corresponding to the largest real eigenvalue as the smoothed attitude quaternion. The calculation formula is as follows: ; in, The smoothed attitude quaternion. , representing the unit quaternion to be solved.

[0014] Specifically, the step of performing smooth attitude quaternion smoothing post-processing on the smoothed attitude quaternion using a preset smooth attitude post-processing method to obtain a smooth attitude quaternion, and then outputting the processed attitude quaternion as the smoothed attitude quaternion at the current time, includes: The current reference attitude is represented as Based on the smoothed pose quaternion The smoothed attitude quaternions are then subjected to post-smoothing to obtain the processed attitude quaternions. The specific calculation formula is as follows: ; in, .

[0015] Specifically, the step of performing post-processing on the smoothed attitude quaternions to obtain post-processed attitude quaternions, and outputting the processed attitude quaternions as the smoothed attitude quaternions at the current time, further includes: The pose quaternion after smooth pose post-processing is output as the smooth pose quaternion at the current time step to update the reference pose quaternion, and the pose quaternion after smooth pose post-processing is used as the reference pose quaternion at the next time step.

[0016] According to a second aspect of the present invention, an attitude smoothing system is provided, comprising: Acquisition module: used to acquire the pose quaternion of the original pose at the current moment, update the pose buffer based on the acquired pose quaternion, and determine the first weight coefficient of the buffer according to the buffer weight coefficient calculation method; The control module is used to calculate the first weighted quaternion sum matrix of the original attitude quaternion using a preset attitude weighted smoothing method, normalize the first weighted quaternion sum matrix to obtain the second weighted quaternion sum matrix, and calculate the smoothed attitude quaternion; or it is used to perform smoothed attitude post-processing on the smoothed attitude quaternion using a preset smoothed attitude post-processing method to obtain the smoothed attitude quaternion, and output the processed attitude quaternion as the smoothed attitude quaternion at the current time.

[0017] According to a third aspect of the present invention, an electronic device is provided, comprising: a memory; and a processor, wherein the memory stores computer-readable instructions that, when executed by the processor, implement the attitude smoothing method described above.

[0018] Beneficial effects: This invention proposes an attitude smoother that performs real-time smoothing of noisy quaternion attitudes using a preset attitude weighted smoothing method. This ensures that the output attitude data is a unit quaternion, and the weight of each input attitude in the buffer can be preset. The weight coefficients of the buffer are optimized by a preset buffer weight coefficient calculation method, and the lag time caused by smoothing can be accurately evaluated, greatly improving the effectiveness of attitude smoothing. The smoothed attitude quaternion is post-processed using a preset smoothed attitude post-processing method, so that the attitude smoother proposed in this invention has a linear phase. This ensures that the attitude data is distortion-free and has no jumps, while reducing attitude data noise and time lag, thus meeting the functional requirements of real-time attitude smoothing and denoising. This greatly improves the stability of tracking with robotic arms, making the motion smoother, and significantly enhances the intelligence, usability, and reliability of this invention, greatly expanding its application scenarios. Attached Figure Description

[0019] Figure 1 This is a flowchart of the attitude smoothing method provided in a specific embodiment of the present invention; Figure 2 This is a schematic diagram of the system composition of the attitude smoothing system provided in a specific embodiment of the present invention; Figure 3 This is a flowchart illustrating the design of the attitude smoother provided in a specific embodiment of the present invention; Figure 4 This is a schematic diagram of the smoothing posture post-processing provided in a specific embodiment of the present invention; Figure 5 The horizontal axis provided in the specific embodiment of the present invention is... The vertical axis corresponds to A schematic diagram of the image; Figure 6 This is a schematic diagram of the amplitude-frequency response characteristic curve corresponding to the weighting coefficients provided in a specific embodiment of the present invention; Figure 7 The horizontal axis provided in the specific embodiment of the present invention is... The vertical axis corresponds to A schematic diagram of the image; Figure 8 This is a schematic diagram of the amplitude-frequency response characteristic curve corresponding to the weighting coefficients provided in a specific embodiment of the present invention. Detailed Implementation

[0020] To enable those skilled in the art to better understand the technical solutions of the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings. Based on the embodiments in this application, other similar embodiments obtained by those skilled in the art without creative effort should all fall within the scope of protection of this application. Furthermore, directional terms mentioned in the following embodiments, such as "up," "down," "left," and "right," are only for reference to the directions in the accompanying drawings; therefore, the directional terms used are for illustrative purposes and not for limiting the invention.

[0021] The present invention will be further described below with reference to the accompanying drawings and preferred embodiments.

[0022] Example 1 Please see Figure 1 This embodiment provides an attitude smoothing method, including: S100. Obtain the attitude quaternion of the original attitude at the current moment, update the attitude buffer according to the obtained attitude quaternion, and determine the first weight coefficient of the buffer according to the preset buffer weight coefficient calculation method. Specifically, updating the pose buffer based on the obtained pose quaternions includes any of the following: First-in-first-out queue storage update, moving array index storage update.

[0023] It should be noted that the attitude cache can be updated using a first-in-first-out (FIFO) queue or dynamically using a moving array index. FIFO queue updates ensure the order of data updates, meaning the first data written is read first. When implementing a cache replacement strategy, FIFO queues provide the simplest basic strategy: when the cache is full, the oldest data is replaced. Moving array index updates, by dynamically adjusting the index, can quickly locate and update data while avoiding unnecessary data movement, thus improving update efficiency. When rapid random access to any data in the cache is required, or when data access patterns do not strictly follow chronological order, moving array index updates can be chosen. For example, if the data access frequency in the cache is uneven, moving array indexes can be used to manage data more efficiently. The attitude cache update method of this invention is not specifically limited and can be selected according to the actual application scenario. The above updates to the attitude cache can optimize data storage efficiency, reduce access latency, and significantly improve space utilization.

[0024] Specifically, the steps for determining the first weight coefficient of the cache based on the preset cache weight coefficient calculation method include: The first weighting coefficient of the cache is denoted as... The specific calculation process is as follows: First, determine the number of first weight coefficients in the cache. The specific calculation formula is as follows: ; ; ; in, It represents the percentage of fluctuations within the smoothed frequency band. It is the minimum attenuation within the filtering frequency band. It is the sampling frequency. It is the maximum value of the smooth frequency band. It is the minimum value of the filter frequency band, and A and B are process variables; Understandably, this refers to the percentage of fluctuation within the smoothed frequency band. Typically 0.01~0.05, the minimum attenuation within the filtering frequency band is usually 25~60dB, and the sampling frequency... Time interval between sampling They are in reciprocal relationship. , The units for all values ​​are Hz. The other coefficients A and B in the above formula are process variables, and the constant values ​​are empirical values ​​that can be obtained by trial and error and can be fine-tuned.

[0025] Secondly, the calculation process design coefficients The calculation formula is as follows: ; The distribution coefficient of the first weight coefficient of the buffer is determined by the following formula. , ; in, For process variables, , If it is a zero-order modified Bessel function of the first kind, then it can be obtained from the distribution coefficient. The first weighting coefficient of the cache is calculated using the following formula. : ; in, The median of the transition frequency band after normalization to half the sampling frequency; It should be noted here that the weighting coefficients are actually... It doesn't change in every cycle; it generally applies to a specific use case. Once set, the weighting coefficients for each period None of these parameters will change; otherwise, the length of n would also change dynamically, causing the buffer length to change dynamically as well. Therefore, in actual operation, the weight coefficients are calculated based on the required parameters. After that, the only data input in each cycle is the original attitude quaternion at the current moment, and then weighted smoothing is performed to output an attitude quaternion.

[0026] The lag time caused by smoothing can be expressed as: ; in, The lag time caused by smoothing, This represents the sampling time interval.

[0027] It is understandable that, as can be seen from the above calculation formula, by Assuming central even-symmetry, the calculation of the cache weight coefficient is illustrated below with a specific example: (1) Let , ; Substituting the above parameter values ​​into the formula, we get n=20. Therefore, as Figure 5 As shown, we obtain An image that is evenly symmetric about 9.5, such as Figure 6 As shown, the amplitude-frequency response characteristic curves corresponding to the weight coefficients of this group of buffers are obtained, where the horizontal axis is in Hz and the vertical axis is the amplitude ratio. Figure 6 This indicates that the set of buffer weight coefficients enables the attitude smoother to... Signals within the range are retained proportionally. ,Right now Signals within the range are filtered out.

[0028] (2) Let , ; Substituting the above parameter values ​​into the formula, we get n=57. Therefore, as Figure 7 As shown, we obtain A graph that is evenly symmetric around 28, such as Figure 8 As shown, the amplitude-frequency response characteristic curves corresponding to the weight coefficients of this group of buffers are obtained, where the horizontal axis is in Hz and the vertical axis is the amplitude ratio. Figure 8 This indicates that the set of buffer weight coefficients enables the attitude smoother to... Signals within the range are retained proportionally. ,Right now Signals within the range are filtered out.

[0029] S200. Calculate the first weighted quaternion sum matrix of the original attitude quaternion using a preset attitude weighted smoothing method. Normalize the first weighted quaternion sum matrix to obtain the second weighted quaternion sum matrix, and calculate the smoothed attitude quaternion. Specifically, the step of calculating the first weighted quaternion sum matrix of the original attitude quaternions using a preset attitude weighted smoothing method includes: Suppose that the pose buffer contains n poses represented by quaternions. ,Will The first weighting coefficient for each pose is set as follows: Then, the first weighted quaternion sum matrix of the original pose is calculated according to the following formula. ; in, It is the first weighted quaternion sum matrix of the original pose.

[0030] It should be noted here that... for Matrix, because the pose represented by quaternions is symmetric on a four-dimensional sphere, and In quaternion space, they represent the same pose, while in the above formula... Can guarantee input of any symbol Do not change the matrix The direction.

[0031] Specifically, the normalization of the first weighted quaternion sum matrix to obtain the second weighted quaternion sum matrix, and the calculation of the smoothed attitude quaternions, includes: Calculate the first weighting coefficient The sum of The calculation formula is as follows: ; For the first weighted quaternion cumulative sum matrix After normalization, the second weighted quaternion sum matrix is ​​obtained, calculated as follows: ; in, This is the second weighted quaternion sum matrix.

[0032] Understandably, to ensure that the introduction of the first weight coefficient in the cache has no impact on the data volume, it is necessary to normalize the weighted quaternion sum matrix based on the sum of the first weight coefficients, thereby obtaining the normalized weighted quaternion sum matrix, which greatly improves the accuracy and reliability of the data.

[0033] Specifically, the step of normalizing the first weighted quaternion sum matrix to obtain the second weighted quaternion sum matrix and calculating the smoothed attitude quaternions further includes: Calculate the eigenvalues ​​and eigenvectors of the second weighted quaternion sum matrix, obtain the eigenvector corresponding to the largest real eigenvalue, and use the eigenvector corresponding to the largest real eigenvalue as the smoothed attitude quaternion. The calculation formula is as follows: ; in, The smoothed attitude quaternion. , representing the unit quaternion to be solved.

[0034] It should be further explained that the `arg max` function is used to find the eigenvector corresponding to the largest real eigenvalue. In addition, the eigenvector corresponding to the largest real eigenvalue can also be used as the smoothed pose quaternion, which can be solved using common open-source mathematical libraries, such as the mathematical algorithm library Eigen and the mathematical package Octave. This invention does not restrict the specific solution method, and since the solution eigenvector obtained according to the above formula is a unit vector, the output smoothed pose is a unit quaternion. Therefore, there is no need to perform normalization again, which greatly simplifies the calculation process.

[0035] S300. The smoothed attitude quaternion is processed by a preset smoothed attitude post-processing method to obtain the smoothed attitude quaternion, and the processed attitude quaternion is output as the smoothed attitude quaternion at the current time.

[0036] Specifically, the step of performing smooth attitude quaternion smoothing post-processing on the smoothed attitude quaternion using a preset smooth attitude post-processing method to obtain a smooth attitude quaternion, and then outputting the processed attitude quaternion as the smoothed attitude quaternion at the current time, includes: The current reference attitude is represented as Based on the smoothed pose quaternion The smoothed attitude quaternions are then subjected to post-smoothing to obtain the processed attitude quaternions. The specific calculation formula is as follows: ; in, .

[0037] Understandably, because the attitude represented by quaternions is symmetric on a four-dimensional sphere, the eigenvector solution obtained by the attitude weighted smoothing method may exhibit a jump in sign compared to the smoothed attitude quaternion from the previous time step. Therefore, it is necessary to adjust the solution based on the reference attitude at the current time step. and smoothed posture Post-processing to smooth the attitude is performed to prevent the smoothed attitude from circling the optimal arc on the four-dimensional sphere, which would cause attitude jumps and affect the continuity of attitude applications such as navigation tracking or motion status display. It needs to be further explained that, Figure 4 This is a schematic diagram after smoothing the attitude post-processing, i.e., if the reference attitude is at the current time. and smoothed pose The dot product is less than 0, indicating that the reference attitude is at the current moment. Smoothed pose If it will travel around the dominant arc on a four-dimensional sphere, then it will Invert assignment This makes the reference attitude at the current moment... Post-processing attitude Travel along the minor arc on the four-dimensional sphere to eliminate the jump phenomenon; If the current reference attitude and smoothed pose The dot product being greater than or equal to 0 indicates the current reference attitude. Smoothed pose Since it is already traveling along a minor arc on a four-dimensional sphere, no processing is needed; the smoothed posture can be directly applied. As the processed posture Output the results.

[0038] Specifically, the step of performing post-processing on the smoothed attitude quaternions to obtain post-processed attitude quaternions, and outputting the processed attitude quaternions as the smoothed attitude quaternions at the current time, further includes: The pose quaternion after smooth pose post-processing is output as the smooth pose quaternion at the current time step to update the reference pose quaternion, and the pose quaternion after smooth pose post-processing is used as the reference pose quaternion at the next time step.

[0039] The working principle of this invention will be further illustrated by specific examples below: The core idea of ​​this invention includes: First, updating the attitude buffer using the original attitude quaternions at the current moment; then, calculating the weighted quaternion sum matrix based on the set buffer weight coefficients; normalizing the weighted quaternion sum matrix based on the accumulated sum of buffer weight coefficients to obtain a normalized weighted quaternion sum matrix, thus eliminating the impact of buffer weight coefficients on the data volume of the weighted quaternion sum matrix; calculating the eigenvalues ​​and eigenvectors of the normalized weighted quaternion sum matrix, and taking the eigenvector corresponding to the largest real eigenvalue as the smoothed attitude quaternion; then, post-processing the smoothed attitude quaternion based on the reference attitude quaternion at the current moment to obtain the processed non-jump attitude quaternion, and recording the processed attitude quaternion as the reference attitude quaternion for the next moment, thus completing the update of the reference attitude quaternion; finally, outputting the processed attitude quaternion as the smoothed attitude quaternion at the current moment.

[0040] When the next motion control cycle (or image interface refresh cycle) arrives, the above steps continue, repeating in a loop to complete the real-time smoothing of the attitude data. In the above steps, the original attitude quaternion at the current moment is the data input, and the buffer weight coefficient is a settable parameter. In practice, for a typical use case, the weight coefficient will not change every cycle. The smoothed attitude quaternion at the current moment is the data output. In addition, in practical applications, the initial reference attitude quaternion can be set as the current attitude quaternion, and the entire attitude buffer can be assigned the current attitude quaternion at the initial moment to avoid the influence of unknown initial conditions on the attitude smoothing process.

[0041] like Figure 3 As shown, the specific implementation steps of this invention include: Step 1: Using the original pose quaternion at the current time Update the attitude cache The attitude buffer can be stored and updated using a first-in-first-out queue or dynamically stored and updated using a moving array index to optimize data access efficiency. No restrictions are placed on the specific method of buffer storage and updating here. Step 2: Based on the set cache weight coefficient Calculate the weighted quaternion sum matrix; Step 3: Calculate the cumulative sum of cache weight coefficients The weighted quaternion sum matrix is ​​normalized to obtain the normalized weighted quaternion sum matrix. ; Step 4: Calculate the eigenvalues ​​and eigenvectors of the normalized weighted quaternion sum matrix, and take the eigenvector corresponding to the largest real eigenvalue as the smoothed attitude quaternion. ; Step 5: Based on the current reference attitude quaternion For the smoothed pose quaternion Post-processing is performed to obtain the processed non-jump pose quaternion. ; Step 6: Process the attitude quaternions Record the reference attitude quaternion for the next time step. This completes the update of the reference attitude quaternion; Step 7: Use the processed attitude quaternion as the smoothed attitude quaternion at the current time step. Output; The aforementioned attitude smoother features linear phase lag, ensuring that the smoothed attitude data is free from phase distortion.

[0042] It should be noted that this invention proposes an attitude smoother that performs real-time smoothing of noisy quaternion attitudes using a preset attitude weighted smoothing method. This ensures that the output attitude data is a unit quaternion, and the proportion of each input attitude in the buffer can be preset. The weight coefficients of the buffer are optimized by a preset buffer weight coefficient calculation method, and the lag time caused by smoothing can be accurately evaluated, greatly improving the effectiveness of attitude smoothing. The smoothed attitude quaternion is post-processed using a preset smoothed attitude post-processing method, so that the attitude smoother proposed in this invention has a linear phase. This ensures that the attitude data is distortion-free and has no jumps, while reducing attitude data noise and time lag, thus meeting the functional requirements of real-time attitude smoothing and denoising. This greatly improves the stability of tracking with a robotic arm, making the motion smoother, and significantly improves the intelligence, usability, and reliability of this invention, greatly expanding its application scenarios.

[0043] Example 2 In this embodiment, You can also press The numerical value is designed as an average coefficient; for example, n is generally a positive integer between 3 and 10. hour, Can be equal to To simplify the use of the attitude smoother, fine-tuning some empirical coefficients in the above buffer weight coefficient calculation formula for optimization or simplification is an obvious method to those skilled in the art and will not be elaborated here. Regarding the specific implementation scheme of the cache area, there are various optional data storage types, such as arrays and linked lists, which have different storage update efficiencies. No specific type restrictions are imposed here. There are multiple solutions for the eigenvector corresponding to the largest real eigenvalue of a matrix, with different calculation speeds and accuracies. No specific method is restricted here.

[0044] It should be noted that this invention uses a preset attitude weighted smoothing method to smooth noisy quaternion attitudes in real time, ensuring that the output attitude data is a unit quaternion. Furthermore, the proportion of each input attitude in the buffer can be preset, greatly improving the effectiveness of attitude smoothing. A preset smoothed attitude post-processing method is used to post-process the smoothed attitude quaternions, giving the proposed attitude smoother a linear phase. This ensures that the attitude data is distortion-free and jump-free, reducing attitude data noise and time lag to meet the functional requirements of real-time attitude smoothing and denoising. This significantly improves the stability of tracking with robotic arms, making the motion smoother and greatly enhancing the intelligence, usability, and reliability of this invention, thus greatly expanding its application scenarios.

[0045] Example 3 Please see Figure 2 This embodiment provides an attitude smoothing system, the system comprising: Acquisition Module 100: Used to acquire the attitude quaternion of the original attitude at the current moment, update the attitude buffer based on the acquired attitude quaternion, and determine the first weight coefficient of the buffer according to the buffer weight coefficient calculation method. Control module 200: is used to calculate the first weighted quaternion sum matrix of the original attitude quaternion using a preset attitude weighted smoothing method, normalize the first weighted quaternion sum matrix to obtain the second weighted quaternion sum matrix, and calculate the smoothed attitude quaternion; or is used to perform smoothed attitude post-processing on the smoothed attitude quaternion using a preset smoothed attitude post-processing method to obtain the smoothed attitude post-processed attitude quaternion, and output the processed attitude quaternion as the smoothed attitude quaternion at the current time.

[0046] It should be noted that this invention proposes an attitude smoother that performs real-time smoothing of noisy quaternion attitudes using a preset attitude weighted smoothing method. This ensures that the output attitude data is a unit quaternion, and the proportion of each input attitude in the buffer can be preset. The weight coefficients of the buffer are optimized by a preset buffer weight coefficient calculation method, and the lag time caused by smoothing can be accurately evaluated, greatly improving the effectiveness of attitude smoothing. The smoothed attitude quaternion is post-processed using a preset smoothed attitude post-processing method, so that the attitude smoother proposed in this invention has a linear phase. This ensures that the attitude data is distortion-free and has no jumps, while reducing attitude data noise and time lag, thus meeting the functional requirements of real-time attitude smoothing and denoising. This greatly improves the stability of tracking with a robotic arm, making the motion smoother, and significantly improves the intelligence, usability, and reliability of this invention, greatly expanding its application scenarios.

[0047] Example 4 In a preferred embodiment, this embodiment also provides an electronic device, the electronic device comprising: The computer device includes a memory and a processor, wherein the memory stores computer-readable instructions that, when executed by the processor, implement the attitude smoothing method described herein. The computer device can be broadly categorized as a server, terminal, or any other electronic device with the necessary computing and / or processing capabilities. In one embodiment, the computer device may include a processor, memory, network interface, communication interface, etc., connected via a system bus. The processor of the computer device can be used to provide the necessary computing, processing, and / or control capabilities. The memory of the computer device may include a non-volatile storage medium and internal memory. The non-volatile storage medium may store an operating system, computer programs, etc. The internal memory can provide an environment for the operation of the operating system and computer programs in the non-volatile storage medium. The network interface and communication interface of the computer device can be used to connect and communicate with external devices via a network. When the computer program is executed by the processor, it performs the steps of the method of the present invention.

[0048] This invention can be implemented as a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, causes the steps of the methods of embodiments of the invention to be performed. In one embodiment, the computer program is distributed across multiple network-coupled computer devices or processors, such that the computer program is stored, accessed, and executed in a distributed manner by one or more computer devices or processors. A single method step / operation, or two or more method steps / operations, may be executed by a single computer device or processor or by two or more computer devices or processors. One or more method steps / operations may be executed by one or more computer devices or processors, and one or more other method steps / operations may be executed by one or more other computer devices or processors. One or more computer devices or processors may execute a single method step / operation, or execute two or more method steps / operations.

[0049] Those skilled in the art will understand that the method steps of this invention can be performed by a computer program instructing related hardware, such as a computer device or processor, to perform the steps of this invention when executed. Depending on the context, any references herein to memory, storage, databases, or other media may include non-volatile and / or volatile memory. Examples of non-volatile memory include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), flash memory, magnetic tape, floppy disk, magneto-optical data storage device, optical data storage device, hard disk, solid-state drive, etc. Examples of volatile memory include random access memory (RAM), external cache memory, etc.

[0050] It should be noted that this invention proposes an attitude smoother that performs real-time smoothing of noisy quaternion attitudes using a preset attitude weighted smoothing method. This ensures that the output attitude data is a unit quaternion, and the proportion of each input attitude in the buffer can be preset. The weight coefficients of the buffer are optimized by a preset buffer weight coefficient calculation method, and the lag time caused by smoothing can be accurately evaluated, greatly improving the effectiveness of attitude smoothing. The smoothed attitude quaternion is post-processed using a preset smoothed attitude post-processing method, so that the attitude smoother proposed in this invention has a linear phase. This ensures that the attitude data is distortion-free and has no jumps, while reducing attitude data noise and time lag, thus meeting the functional requirements of real-time attitude smoothing and denoising. This greatly improves the stability of tracking with a robotic arm, making the motion smoother, and significantly improves the intelligence, usability, and reliability of this invention, greatly expanding its application scenarios.

[0051] The technical features described above can be combined arbitrarily. Although not all possible combinations of these technical features are described, any combination of these technical features should be considered to be covered by this specification, provided that such combination does not contain contradictions.

[0052] The specific embodiments of the present invention described above do not constitute a limitation on the scope of protection of the present invention. Any other corresponding changes and modifications made in accordance with the technical concept of the present invention should be included within the scope of protection of the claims of the present invention.

Claims

1. A posture smoothing method, characterized in that, include: S100. Obtain the attitude quaternion of the original attitude at the current moment, update the attitude buffer according to the obtained attitude quaternion, and determine the first weight coefficient of the buffer according to the preset buffer weight coefficient calculation method. S200. Calculate the first weighted quaternion sum matrix of the original attitude quaternion using a preset attitude weighted smoothing method. Normalize the first weighted quaternion sum matrix to obtain the second weighted quaternion sum matrix, and calculate the smoothed attitude quaternion. S300. The smoothed attitude quaternion is processed by a preset smoothed attitude post-processing method to obtain the smoothed attitude quaternion, and the processed attitude quaternion is output as the smoothed attitude quaternion at the current time. The specific steps for determining the first weight coefficient of the cache based on the preset cache weight coefficient calculation method include: The first weighting coefficient of the cache is denoted as... The specific calculation process is as follows: First, determine the number of first weight coefficients in the cache. The specific calculation formula is as follows: ； ； ； in, It represents the percentage of fluctuations within the smoothed frequency band. It is the minimum attenuation within the filtering frequency band. It is the sampling frequency. It is the maximum value of the smooth frequency band. It is the minimum value of the filter frequency band, and A and B are process variables; Secondly, the calculation process design coefficients The calculation formula is as follows: ； The distribution coefficient of the first weight coefficient of the buffer is determined by the following formula. , ； in, For process variables, , For a zero-order modified Bessel function of the first kind, then the distribution coefficients... The first weighting coefficient of the cache is calculated using the following formula. : ； in, The median of the transition frequency band after normalization to half the sampling frequency; The lag time caused by smoothing is expressed as: ； in, The lag time caused by smoothing, This represents the sampling time interval.

2. The attitude smoothing method according to claim 1, characterized in that, The step of updating the pose buffer based on the obtained pose quaternion includes any of the following: First-in-first-out queue storage update, moving array index storage update.

3. The attitude smoothing method according to claim 1, characterized in that, The calculation of the first weighted quaternion sum matrix of the original attitude quaternions using a preset attitude weighted smoothing method includes: Suppose that the pose buffer contains n poses represented by quaternions. ,Will The first weighting coefficient for each pose is set as follows: Then, the first weighted quaternion sum matrix of the original pose is calculated according to the following formula. ； in, It is the first weighted quaternion sum matrix of the original pose.

4. The attitude smoothing method according to claim 3, characterized in that, The process of normalizing the first weighted quaternion sum matrix to obtain the second weighted quaternion sum matrix, and calculating the smoothed attitude quaternions, includes: Calculate the first weighting coefficient The sum of The calculation formula is as follows: ； For the first weighted quaternion cumulative sum matrix After normalization, the second weighted quaternion sum matrix is ​​obtained, calculated as follows: ； in, This is the second weighted quaternion sum matrix.

5. The attitude smoothing method according to claim 4, characterized in that, The process of normalizing the first weighted quaternion sum matrix to obtain the second weighted quaternion sum matrix and calculating the smoothed attitude quaternions also includes: Calculate the eigenvalues ​​and eigenvectors of the second weighted quaternion sum matrix, obtain the eigenvector corresponding to the largest real eigenvalue, and use the eigenvector corresponding to the largest real eigenvalue as the smoothed attitude quaternion. The calculation formula is as follows: ； in, The smoothed attitude quaternion. , representing the unit quaternion to be solved.

6. The attitude smoothing method according to claim 5, characterized in that, The process involves performing smooth attitude post-processing on the smoothed attitude quaternions using a preset smooth attitude post-processing method to obtain a smoothed attitude quaternion, and then outputting the processed attitude quaternion as the smoothed attitude quaternion at the current time. This includes: The current reference attitude is represented as Based on the smoothed pose quaternion The smoothed attitude quaternions are then subjected to post-smoothing to obtain the processed attitude quaternions. The specific calculation formula is as follows: ； in, .

7. The attitude smoothing method according to claim 6, characterized in that, The step of performing post-processing on the smoothed attitude quaternions to obtain post-processed attitude quaternions, and outputting the processed attitude quaternions as the smoothed attitude quaternions at the current time, also includes: The pose quaternion after smooth pose post-processing is output as the smooth pose quaternion at the current time step to update the reference pose quaternion, and the pose quaternion after smooth pose post-processing is used as the reference pose quaternion at the next time step.

8. An attitude smoothing system, characterized in that, The system employs the attitude smoothing method according to any one of claims 1 to 7, wherein the system comprises: Acquisition module: used to acquire the pose quaternion of the original pose at the current moment, update the pose buffer based on the acquired pose quaternion, and determine the first weight coefficient of the buffer according to the buffer weight coefficient calculation method; The control module is used to calculate the first weighted quaternion sum matrix of the original attitude quaternions using a preset attitude weighted smoothing method, normalize the first weighted quaternion sum matrix to obtain the second weighted quaternion sum matrix, and calculate the smoothed attitude quaternions; it is also used to perform smoothed attitude post-processing on the smoothed attitude quaternions using a preset smoothed attitude post-processing method to obtain the smoothed attitude quaternions, and output the processed attitude quaternions as the smoothed attitude quaternions at the current time.

9. An electronic device, characterized in that, include: Memory; The processor, wherein computer-readable instructions are stored in the memory, and when executed by the processor, the computer-readable instructions implement the attitude smoothing method according to any one of claims 1 to 7.

Images