A force sensing method for touch gloves based on FOC and MPC algorithms

Abstract

The present invention discloses a force sensing method for a touch glove based on the Field Operational Control (FOC) and Multi-Purpose Control (MPC) algorithms. The method comprises a current MPC calculation module, an inverse Park transform module, a Clark transform module, a Park transform module, an SVPWM generation module, and an extended Kalman filter, all of which are part of the FOC algorithm. Force sensing data from different objects is received via the algorithm module's SPI interface and parameter configuration is performed to achieve force sensing at the finger of a single-finger touch glove. The algorithm is integrated into a FOC force sensing control chip, which is installed on the touch glove, providing precise motor control for force sensing at the finger of the touch glove. Compared with existing technologies, the present invention boasts a simpler structure, more precise control, and improved dynamic response and parameter robustness of the FOC algorithm, making it an ideal force sensing method for VR touch gloves.

Description

Technical Field

[0001] The present invention relates to the technical field of virtual reality technology applications, and in particular to a touch glove force sensing method based on FOC and MPC algorithms. Background Art

[0002] The core user experience of VR technology consists of three components: immersion, smoothness, and interactivity. To enhance this user experience, numerous interactive devices have emerged. Touch gloves, among them, enable users to connect the virtual and real worlds, allowing them to experience the feel of holding virtual objects in the real world. When a user touches a virtual object in VR, the touch gloves track their movements and provide feedback in the opposite direction, as if touching a real physical object and being bounced back. Therefore, it is crucial that the force sensing methods used in touch gloves can quickly and accurately provide force sensing feedback. Existing force sensing methods for touch gloves mostly employ traditional field-of-control (FOC) algorithms. These control algorithms employ a PID induction motor current regulator for current loop control and an extended Kalman filter for angle and velocity loop control. Furthermore, the Verilog implementation of the algorithms employs a lookup table or calls a ROM IP core to calculate sine and cosine.

[0003] The existing force sensing methods are deficient in terms of control accuracy and speed. They are not only unstable and easily affected by the environment, but also have high production costs and are not conducive to integration into FOC algorithm chips. Summary of the Invention

[0004] The purpose of the present invention is to provide a force sensing method for touch gloves based on FOC and MPC algorithms to address the deficiencies of the existing technology. An FPGA chip with FOC and MPC control algorithms burned in it is installed on the touch glove and together with the MCU single-chip computer, motor, and the host computer in the virtual world, constitutes a touch glove system based on FOC and MPC algorithms. When testing the algorithm, the motor is placed at the finger on the back of the hand, and force sensing feedback is performed on the finger through the transmission rod and the connecting line. The algorithm module receives different force sensing feedback data of different objects in the virtual world obtained by the MCU from the host computer through the SPI interface and performs parameter configuration. The FOC and MPC algorithms control the motor by outputting three-phase PWM waves to achieve precise control of force sensing feedback at the finger. The algorithm receives the force sensing feedback data sent by the MCU through the MPC induction motor current regulator, inverse Park transform, SVPWM, Clark transform and Park transform The generated current vector is used as the current loop feedback input of the FOC and MPC algorithms into the MPC module to complete the closed-loop calculation of the current loop. The extended Kalman filter module is used to receive the current angle and speed of the motor. After calculation, the output result is input into the MPC algorithm module to complete the closed-loop calculation of the angle loop and the speed loop. This method has accurate, fast and stable control, improves the dynamic response and parameter robustness of the FOC algorithm, is easy to burn into FPGA chips or integrate into FOC and MPC control chips, and is installed on touch gloves to make it light in weight, low in production cost and simple in structure. Users can feel the touch of objects in the virtual world more quickly, accurately and realistically in the real world, and the algorithm can provide feedback information more timely. Users can gain an immersive experience in the virtual world, effectively reducing the perception gap between the virtual world and the real world. It is an ideal force sensing method for VR virtual devices with good application prospects and development market.

[0005] The specific technical solution for achieving the objectives of the present invention is: a touch glove force sensing method based on FOC and MPC algorithms, which is characterized by first constructing a single-finger touch glove model; then constructing a FOC and MPC control algorithm model, including: a current MPC calculation module, an inverse Park transform module, a Clark transform module, a Park transform module, an SVPWM generation module, and an extended Kalman filter module; finally, the FOC and MPC control algorithm model is applied to the force sensing module of the single-finger touch glove to verify the accuracy of the algorithm, ensuring that this algorithm can be extended to a complete touch glove.

[0006] The touch single-finger glove model consists of a transmission rod, a rotating shaft, a connecting rope, a motor, an MCU single-chip computer, and an FPGA chip with FOC and MPC algorithms burned in it (or an FPGA chip with integrated FOC and MPC algorithms). Force feedback data collected by a host computer is input into the MCU single-chip computer. The FOC and MPC algorithm modules in the FPGA chip receive parameters transmitted by the MCU single-chip computer via an SPI interface and perform parameter configuration. The FPGA chip outputs a PWM wave based on the input motor parameters through the FOC and MPC algorithms, and controls the operation of the motor through a three-phase full-bridge circuit and signal lines. The transmission rod and the rope achieve precise control of force sensing feedback at the finger of the touch single-finger glove, allowing users of the single-finger touch glove to clearly feel the force sensing from virtual to real.

[0007] The host computer is equipped with VR software, which is used to correspond the force sensing data of objects in the virtual world collected in reality with the force sensing data of objects in the virtual world, and transmit the force sensing parameters of objects in the virtual world to the MCU microcontroller.

[0008] The FOC and MPC algorithms in the FPGA chip that is burned or integrated with the FOC and MPC algorithms receive data from the MCU through the SPI interface. The FOC and MPC algorithms need to be configured with parameters such as the motor angle, resistance and inductance, and the magnetic flux of the motor. Different parameters are input when different motors are used. At the same time, the MCU needs to send the parameters of the objects in the virtual world transmitted by the host computer to the FPGA chip as the control quantities required by the FOC and MPC algorithms.

[0009] The FOC and MPC algorithms in the FPGA chip with burned or integrated FOC and MPC algorithms can perform faster and more accurate calculations. Compared with the common traditional FOC algorithms, the FOC and MPC algorithms have the advantages of higher dynamic response and parameter robustness. In addition, compared with the traditional FOC algorithms, the FOC and MPC algorithms use the MPC algorithm instead of the traditional PID algorithm to realize current loop control, use the extended Kalman filter module to replace the traditional sensor, and use the CORDIC algorithm to replace the table lookup method or the method of calling ROM IP used in the Verilog implementation of the algorithm. After the calculation is completed, a three-phase PWM wave is output to control the motor.

[0010] The FOC and MPC algorithms specifically include an MPC algorithm module, a Park transform module, an inverse Park transform module, a Clark transform module, an SVPWM generation module, and an extended Kalman filter module. The FOC and MPC algorithms are primarily used for pixel-level control of the motor, enabling more accurate and rapid force feedback when later applied to touch gloves. The FOC and MPC algorithms enable smoother motor rotation, resulting in a more realistic feel. Compared to traditional motor control methods or FOC and PID algorithms, they offer faster system dynamic response and parameter robustness, reducing motor rotation stuttering. Force sensing also accurately changes as virtual world objects change, creating a more realistic experience.

[0011] Each module in the FOC and MPC algorithms is used to complete the closed-loop calculation of the sensorless FOC and MPC algorithms, wherein the three MPC algorithm modules are applied to the closed-loop control of the FOC speed loop, position loop and current loop. The FOC and MPC algorithm modules receive data from the MCU through the SPI interface, and iteratively correct the motor state by training the current state of the motor with the input target state through the MPC algorithm; the inverse Park transformation module is used to transform the control voltage vector based on the DQ coordinate system output by the MPC algorithm module into the αβ coordinate axis direction; the SVPWM generation module is used to transform the voltage vector generated by the inverse Park transformation module in the αβ coordinate axis direction into the SVPWM wave for controlling the motor; the Clark transformation The module is used to convert the three-phase current sampled during motor operation into a two-phase current; the Park transformation module is used to convert the two-phase current obtained by the Clark transformation module from the αβ coordinate axis direction to the DQ coordinate system, and the generated current vector will be used as the current loop feedback input MPC module of the FOC and MPC algorithms to complete the closed-loop calculation of the current loop; the extended Kalman filter module is used to detect the current angle and speed of the motor, use the current vector output by the Clark transformation module as measurement, use the voltage control vector generated by the MPC algorithm module used to calculate the current loop as the control quantity input, output the motor's operating speed and the motor's current angle, and input the output result into the MPC algorithm module for closed-loop calculation of the angle loop and speed loop.

[0012] The specific implementation steps of the MPC algorithm are as follows:

[0013] Step 1: In the synchronous rotating coordinate system (dq coordinate system) with oriented rotor magnetic field, the stator voltage equation of the asynchronous motor can be written as the following equation system defined by equation (a):

[0014]

[0015] Where: R s is the stator resistance; Lm is the excitation inductance; L s , L r is the stator and rotor inductance; L σ =σL s is the total leakage inductance; ω δ is the synchronous angular velocity; is the rotor flux amplitude; i sq 、i sd is the stator current of d-axis and q-axis; u sd 、u sq is the stator voltage of the d-axis and q-axis; p is the differential factor

[0016] Step 2: To reduce the complexity of the model predictive control algorithm, the two-dimensional current equations can be simplified into two single-input single-output (SISO) systems. Treating the cross-coupling terms between the d-axis and q-axis as disturbances, the following equation (b) is obtained to represent the object equations for current control:

[0017]

[0018] Step 3: Consider the cross-coupling term as the feedforward compensation for current control expressed by the following equation (c):

[0019]

[0020] Step 4: Taking the d-axis as an example, its state equation can be expressed as follows (d):

[0021]

[0022] in, For i sd Perform differential operations.

[0023] Step 5: Discretize the above formula (d) according to the first-order Euler method, T s is the sampling time, and the same applies to the d and q axes, resulting in the following equations (e) to (i):

[0024]

[0025]

[0026] i sd (k+1)=Ai sd (k)+ Bu sd (g);

[0027]

[0028] i sq (k+1)=Ai sq(k)+Bu sq (i).

[0029] Step 6: Use the aforementioned MPC algorithm to establish control models for the d-axis and q-axis currents. Because the d-axis and q-axis current state equations are similar, a modular programming approach can be used, with shared code, to implement model predictive control for both d-axis and q-axis currents. The MPC algorithm can be used to determine the optimal control variables for the d-axis and q-axis. To improve control performance, feedforward compensation can be applied to the MPC-derived control variables.

[0030] When the FOC and MPC algorithms need to use sine-cosine transformation to convert into Verilog code, the traditional table lookup method or calling ROM IP is replaced by the CORDIC algorithm to calculate the sin function and cos function. This is extremely important for integrating into FOC and MPC algorithm chips, because during the integrated wiring, the table lookup method is integrated and the wiring is interspersed with each other, similar to the routing structure, which is more difficult. This is very troublesome for the back-end tools. The back-end tools are not so smart for winding. If it cannot be bypassed, it will be given a larger area, and it will be continuously pushed away (ripple algorithm) until it is large enough to bypass the position. If the ROM IP development cost is called, it will become higher, and the CORDIC algorithm is an algorithm that simplifies the complex, converting many complex operations into an iterative operation that only requires shifting and addition. The CORDIC algorithm has two modes: rotation and vector, which can be used in circular coordinate systems, linear coordinate systems, and hyperbolic coordinate systems, respectively. The CORDIC algorithm involves three coordinate systems, two modes, and a total of six combinations. Among them, the rotation mode is used to implement sin and cos in the circular coordinate system, making the wiring more reasonable when integrated into FOC and MPC algorithm chips.

[0031] The specific steps of the rotation mode of the CORDIC algorithm are as follows:

[0032] Step 1: Set the number of iterations to 16, then x0 = 0.607253, y0 = 0, and enter the angle θ to be calculated, which is in the range of [-99.7°, 99.7°].

[0033] Step 2: Iterate according to the following three iteration formulas (j) to (l): i ranges from 0 to 15;

[0034] x i =(x i-1 -y i-1 × d i × 2 -(i-1) ) (j);

[0035] y i =(y i-1 + x i-1 × d i × 2-(i-1) ) (k);

[0036] z i =(z i-1 -θ i-1 )(l).

[0037] Where i iterates from 0 to 15; z 0 =θ,d i With z i Same symbol.

[0038] Step 3: After 16 iterations, we get x 16 and y 16 It can be seen that for each iteration, x and y only need 2 i-bit shifters, and the accumulation of x and y coordinate changes and angles only requires 3 adders in total, among which the accumulation of angles also requires the addition of tan -1 (2 -i ) to find out.

[0039] The FOC and MPC algorithms have two working modes. One is the normal working mode, that is, all modules are turned on to complete the complete closed-loop calculation of the FOC and MPC algorithms; the second mode is the sampling working mode, which is used when the force feedback control glove is sampling. During sampling, the three MPC algorithm modules, the inverse Park transform module, the SVPWM generation module and the Park transform module are all in the off state, and only the Clark transform module and the extended Kalman filter module are in the on state to sample the motion state of the motor.

[0040] The specific implementation steps of the force sensing of the touch gloves based on the FOC and MPC algorithms are as follows:

[0041] Step 1: The MCU inputs the parameters into the MPC algorithm module in the FOC system. The current state of the motor and the input target state are iteratively corrected through MPC algorithm training. The MPC current control model equations are shown in the following equations (n) to (p):

[0042]

[0043] i sd (k+1)=Ai sd (k)+ Bu sd (m);

[0044]

[0045] i sq (k+1)=Ai sq (k)+Bu sq (p).

[0046] Step 2: The inverse Park transform module converts the control voltage vector based on the DQ coordinate system output by the MPC algorithm module into the αβ coordinate axis direction by the following (q) to (r) formulas:

[0047] Vα =Vdcos(θ) - Vqsin(θ) (q);

[0048] Vβ=Vqcos(θ)+Vdsin(θ)(r).

[0049] Step 3: The SVPWM generation module converts the voltage vector control quantity in the αβ coordinate axis direction generated by the inverse Park transformation module into the SVPWM wave for controlling the motor.

[0050] Step 4: The Clark transformation module converts the three-phase currents Ia, Ib, and Ic into two-phase currents Iα and Iβ using the following equations (s) to (t):

[0051] Iα=Ia-cos(2π / 3)Ib-cos(2π / 3)Ic (s);

[0052] Iβ=sin(2π / 3)Ib-sin(2π / 3)Ic(t).

[0053] Step 5: The Park transform module converts the two-phase currents obtained by the Clark transform module from the αβ coordinate axis to the DQ coordinate system. The generated current vector will be used as the current loop feedback input of the FOC and MPC algorithms to the MPC module, and the closed-loop calculation of the current loop is completed by the following equations (u) to (v):

[0054] Id=Iαcos(θ)+Iβsin(θ) (u);

[0055] Iq=-Iαsin(θ)+Iβcos(θ)(v).

[0056] Step 6: The extended Kalman filter module is used to detect the current angle and speed of the motor, and the current vector output by the Clark transform module is used as the measurement; the voltage control vector generated by the MPC algorithm module used to calculate the current loop is used as the control input, and the motor's operating speed and current angle are output. The output results are input into the MPC algorithm module for closed-loop calculation of the angle loop and speed loop.

[0057] Step 7: Algorithm Testing: The FPGA chip with the FOC and MPC algorithms programmed or integrated receives force feedback data from the MCU via the SPI interface for configuration. This force feedback data includes angle and velocity, which serve as external inputs to the angle and velocity loops. Wearing a single-finger touchscreen cuff, grasping a real-world object, the FOC and MPC algorithms' sampling modes collect force sensor feedback from the object, transmit this information to the MCU's storage module, and match the data with objects in the virtual world in the active mode. The FOC and MPC algorithms programmed or integrated into the FPGA chip continuously execute in a closed loop. Changes in the user's hand posture, such as loosening or wrapping, alter the motor's current operating state based on the force feedback data. Ultimately, a three-phase PWM wave is output, which is fed into a three-phase full-bridge circuit to control the motor's operation.

[0058] The FOC and MPC algorithms still need to be resampled for the same object depending on the motor used, and different motors have different force sensor feedback data.

[0059] When the FOC and MPC algorithms are sampling, only the extended Kalman filter module and the Clark transform module are running in the FOC algorithm, where the control quantity input of the extended Kalman filter module is 0, that is, the input Vα and Vβ are 0. The extended Kalman filter module calculates and outputs the running speed and angle of the motor to the MCU to complete the sampling.

[0060] The FOC and MPC algorithm modules in step 1 will receive the next frame of force feedback data transmitted by the MCU through the SPI interface based on the current speed and angle of the motor. The user can then release or tighten the object at any time and still receive force feedback.

[0061] In order to prevent wiring difficulties and synthesis difficulties when the algorithm is converted into Verilog and integrated into FOC and MPC algorithm chips, the present invention optimizes the traditional table lookup method or ROMIP calling method used when sine-cosine transform is required in the FOC algorithm into a CORDIC algorithm for calculating the sin function and cos function.

[0062] Compared with the prior art, the present invention has the following beneficial technical effects and significant technical progress:

[0063] 1) Sampling the force sensing data of objects in the real world before executing the algorithm can better achieve the feedback effect of virtual objects, and users will feel less difference from the real world when using it.

[0064] 2) The FOC and MPC algorithm modules use the SPI interface to receive parameters sent by the MCU. Their fixed parameters can be adjusted at any time according to different motor models or actual conditions, making the algorithm adaptable to more types of touch gloves.

[0065] 3) Compared with the closed-loop calculation of traditional angle loop and velocity loop, the extended Kalman filter module can cleverly integrate observation data and estimated data, perform closed-loop management of errors, and limit the errors to a certain range. Since the information source relied on when making decisions has an additional observation data, the error can still remain stable over a long period of time.

[0066] 4) Using an MPC algorithm instead of a PID algorithm can reduce sensor limitations on accuracy and improve the system's dynamic response and parameter robustness. MPC algorithms are better at handling delays than PID. Through rolling optimization and feedback correction, they repeatedly optimize a performance indicator online to determine the optimal control action. The actual output of the controlled object is used to correct the predicted results, preventing model mismatch or environmental interference. This is the greatest advantage of model predictive control over other algorithms.

[0067] 5) Using the CORDIC algorithm instead of the traditional table lookup method or calling ROM IP method improves synthesizability and reduces costs. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] Figure 1 A schematic diagram of a touch glove force sensing system according to the present invention;

[0069] Figure 2 This is a schematic diagram of the touch single finger sleeve structure;

[0070] Figure 3 Schematic diagram of the modules of the two modes in FOC and MPC algorithms;

[0071] Figure 4 Schematic diagram of the top-level modules of FOC and MPC algorithms;

[0072] Figure 5 Schematic diagram of the overall structure of FOC and MPC algorithms;

[0073] Figure 6 Flowchart of the present invention;

[0074] Figure 7 Schematic diagram of the three-phase PWM wave finally output by the FOC and MPC algorithms. DETAILED DESCRIPTION

[0075] See Figure 1The present invention adopts FOC and MPC algorithm modules to receive real-time parameters transmitted from the outside through the SPI interface, and uses three MPC algorithm modules, an inverse Park transform module, an SVPWM generation module, a Clark transform module, a Park transform module and an extended Kalman filter module to form closed-loop calculations of the current loop, the angle loop and the speed loop. The FOC and MPC algorithms are burned into the FPGA chip or integrated into the FOC algorithm chip. The correctness of the algorithm is verified by externally connecting the MCU microcontroller, the three-phase full-bridge circuit, the motor and the touch single finger sleeve. The MCU microcontroller obtains the force sensing feedback of the object from the virtual world of the host computer. The FOC and MPC algorithm parts enter the corresponding state according to the sampling or working mode through the SPI interface, and output PWM waves to control the rotation of the motor through the three-phase full-bridge circuit, thereby transmitting the force sensing feedback to the finger through the touch single finger sleeve.

[0076] See Figure 2 The touch single finger sleeve consists of an MCU microcontroller 1, an FPGA chip 2, a motor 3, a connecting rope 4, a transmission rod 5, and a rotating shaft 6, which is used to test the FOC and MPC algorithms. The transmission rod 5 includes two rotating shafts 6 and three hollow rods of different lengths connected thereto; the rotating shaft 6 is a connecting shaft set at the joint of the touch single finger sleeve, so that when the motor 3 drives the transmission rod 5 to move, the finger can bend and feel force sensing feedback; the motor 3 is connected to the FPGA chip 2 burned with the FOC and MPC algorithms through a signal line. One end of the connecting rope 4 is wrapped around the rotating shaft of the motor 3, and the other end is connected to the transmission rod 5, which is used to transmit the rotation of the motor 3 to drive the movement of the transmission rod 5.

[0077] The present invention achieves precise force sensing control through FOC closed-loop control. First, the algorithm's sampling mode collects force feedback data from objects in the real world. When an object is touched, motor 3 rotates at a certain speed and angle. This data is collected via the SPI interface of the FOC algorithm module embedded in FPGA chip 2 and transmitted to the MCU 1 for storage in the MCU's memory module. After the sampling mode ends, the system enters operating mode. When an object in the virtual world of the host computer changes, the FOC and MPC algorithm modules embedded in FPGA chip 2 obtain configuration parameters and control variables from MCU 1 via the SPI interface. The control variables include angle, speed, and current. The angle control variable controls the rotation angle of motor 3 and can be used to simulate the size of an object. The speed control variable controls the rotation speed of motor 3 and can be used to simulate the hardness or softness of an object. After receiving control signals from MCU 1 via the SPI interface, the FOC and MPC algorithm modules begin generating SVPWM waves to control motor 3. First, the position loop is used to calculate the position control variable and the current rotational position of motor 3 through the MPC algorithm module; then, the input speed control variable and the current speed of the motor are used to complete the speed loop calculation through the MPC algorithm module; then, the calculated current control variable and the current current value of the click are calculated to complete the current loop calculation; finally, the SVPWM wave for controlling motor 3 is obtained through the inverse Park transformation module and the SVPWM generation module. Subsequently, the FOC and MPC algorithm modules output the generated SVPWM wave to motor 3. Motor 3 is connected to the touch-control single finger sleeve, allowing users to feel the force sensing of objects in the virtual world, that is, the touch of objects.

[0078] The present invention will be further described in detail below with reference to specific embodiments and accompanying drawings. The processes, conditions, experimental methods, etc. for implementing the present invention, except for those specifically mentioned below, are common knowledge and common common sense in the art and are not particularly limited by the present invention.

[0079] Example 1

[0080] See Figure 3 Different from the traditional single FOC algorithm mode, the present invention proposes two states: sampling mode and working mode. When in sampling mode, only the Clark transform module and the extended Kalman filter module are turned on. The FOC algorithm module receives the force sensor feedback data of the real object sent by the MCU microcontroller 1 through the SPI interface. Through the Clark transform module and the extended Kalman filter module, the control quantity input of the extended Kalman filter module is 0, that is, the input Vα and Vβ are 0. The operating speed and angle of the motor 3 are output to the storage module of the MCU microcontroller 1 through calculation to complete the sampling.

[0081] When in working mode, all submodules of the FOC and MPC algorithm modules are turned on, and the MCU 1 matches the object force sensing information transmitted from the virtual world of the host computer with the information stored in the sampling mode. Then, the FOC and MPC algorithm modules receive real-time parameter configuration information through the SPI interface, and after calculation of the current loop, speed loop and angle loop of each submodule, output PWM waves to control the rotation of the motor 3.

[0082] The sampling mode of the present invention requires multiple measurements to input force sensor data, ensuring that the sampling mode for a single object is maintained for at least 5 seconds. The sampling process includes the entire process of touching the object with a single fingertip, wrapping the finger around the object, and then dragging the object. After recording the object's force feedback data three times, the FOC algorithm module calculates and transmits the force sensor feedback to the storage module of the MCU 1 through the Clark transform module and the extended Kalman filter module, allowing it to be matched with objects in the host computer's virtual world when in working mode.

[0083] See Figure 4 The top level of the FOC and MPC algorithm modules is the control module. To accelerate calculations, the FOC and MPC algorithms designed in this invention utilize a four-stage pipeline to calculate input data. The first stage is the SPI interface module, which receives parameters transmitted by MCU1. The second stage comprises three MPC algorithm modules, which perform control calculations for the velocity loop, angle loop, and current loop. The third stage comprises the inverse Park transform module and the SVPWM generation module, which generate SVPWM waves. The fourth stage comprises the Clark transform module, the Park transform module, and the extended Kalman filter module, which calculate feedback signals and complete the feedback loop calculations of the FOC and MPC algorithm modules, thereby forming a closed control loop. The inverse Park transform module, the Clark transform module, and the Park transform module all utilize sine-cosine transforms. When the FOC and MPC algorithm modules are converted into Verilog code, the traditional table lookup method or ROM IP-based calculation method is replaced with the CORDIC algorithm for sine and cosine calculations, reducing costs while improving synthesizability.

[0084] See Figure 5 The specific calculation process of the FOC and MPC algorithm modules designed by the present invention is as follows:

[0085] 1) The first two MPC algorithm modules (speed and angel) in the FOC and MPC algorithm modules receive the speed and angle parameters sent by MCU1 through the SPI interface, and compare the current state of the motor with the input target state through the next-level MPC algorithm module (current). The current id, iq and the expected id_aim, iq_aim are trained and iteratively corrected to the motor state. The MPC current control model equations are shown in the following (n) to (p):

[0086]

[0087] i sd (k+1)=Ai sd (k)+ Bu sd (m);

[0088]

[0089] i sq (k+1)=Ai sq (k)+Bu sq (p).

[0090] 2) The inverse Park transform module converts the control voltage vector based on the DQ coordinate system output by the MPC algorithm module into the αβ coordinate axis direction. The specific conversion is performed by the following (q) to (r):

[0091] Vα =Vdcos(θ) - Vqsin(θ) (q);

[0092] Vβ=Vqcos(θ)+Vdsin(θ)(r).

[0093] 3) The SVPWM generation module converts the voltage vector control quantity in the αβ coordinate axis direction generated by the inverse Park transformation module into the SVPWM wave for controlling the motor.

[0094] 4) Clark converts the three-phase currents Ia, Ib, and Ic into two-phase currents Iα and Iβ using the following equations (s) to (t):

[0095] Iα=Ia-cos(2π / 3)Ib-cos(2π / 3)Ic (s);

[0096] Iβ=sin(2π / 3)Ib-sin(2π / 3)Ic(t).

[0097] 5) The Park transformation module converts the two-phase currents obtained by the Clark transformation module from the αβ coordinate axis direction to the DQ coordinate system. The generated current vector will be used as the current loop feedback input of the FOC and MPC algorithms. The MPC module (current) is compared with Iq_aim and Id_aim, and the current loop is closed by the following (u) to (v):

[0098] Id=Iαcos(θ)+Iβsin(θ) (u);

[0099] Iq=-Iαsin(θ)+Iβcos(θ)(v).

[0100] 6) The extended Kalman filter module is used to detect the current angle and speed of the motor. It uses the current vector output by the Clark transform module as the measurement, and the voltage control vector generated by the MPC algorithm module (speed) used to calculate the current loop as the control input. It outputs the motor's operating speed and current angle. The output results are input into the MPC algorithm module to form a closed-loop calculation of the angle loop and speed loop;

[0101] See Figure 6 The present invention works as follows: it determines whether it is in sampling mode or working mode. If it is in sampling mode, the FOC and MPC algorithm modules enter the sampling mode, shut down other submodules and leave only Clark transform and extended Kalman filter. Use a single touch finger sleeve for testing to surround the object to be sampled, and the finger link rope 4 rotates at a speed of 5 rad / s to maintain the posture of surrounding the object. When the FOC and MPC algorithm modules detect that the motor 3 no longer rotates and the motor 4 no longer rotates, the FOC and MPC algorithm modules start reading and calculate the force sensing feedback data of the object through the Clark transform module and the extended Kalman filter module, and transmit it back to the storage module in the MCU 1 for storage so as to correspond to the force sensing feedback data sent from the host computer virtual world in the working mode.

[0102] If in working mode, the FOC and MPC algorithm modules enter working mode, and the SPI interface receives the configuration parameters sent by MCU 1. Through the calculation of the three MPC algorithm modules and the inverse Park transform module, SVPWM generation module, three-phase full-bridge circuit, Clark transform module, extended Kalman filter module, and Park transform module, it returns to the MPC algorithm module to form a closed-loop calculation of the current loop, angle loop, and speed loop, and outputs the PWM wave generated by the SVPWM module through the three-phase full-bridge circuit, thereby driving the motor to rotate and completing the force sensing feedback from the virtual world to the real world.

[0103] See Figure 7The FOC and MPC algorithm modules simulate and output three-phase PWM waves. The SVPWM generation module is used to generate SVPWM waves. It generates three-phase SVPWM waves (PWMA, PWMB, PWMC) based on the voltage control vector (Vsρ, Vsθ) in the stator polar coordinate system (i.e., the αβ coordinate system) generated by the inverse Park transform module. The three-phase SVPWM waves control the switching of the MOSFETs in the three-phase full-bridge circuit. When the voltage is high, the MOSFETs are turned on, corresponding to the forward vector. When the voltage is low, the MOSFETs are turned off, corresponding to the reverse vector. Therefore, the six basic vectors of the stator in motor 3 are composed of the forward vectors A, B, and C and the reverse vectors A, B, and C. The PWM wave provides the motor with a pulsed current with a certain frequency and adjustable pulse width. A larger pulse width, or greater duty cycle, increases the average voltage supplied to the motor, resulting in higher motor speed. Conversely, a smaller pulse width, or smaller duty cycle, decreases the average voltage supplied to the motor, resulting in lower motor speed.

[0104] The protection content of the present invention is not limited to the above embodiments. Without departing from the spirit and scope of the present invention, changes and advantages that can be thought of by those skilled in the art are included in the present invention and are protected by the appended claims.

Claims

1. A touch glove force sensing method based on FOC and MPC algorithms, characterized in that: The method specifically comprises the following steps: Step 1: Construction of algorithm model Construct a FOC and MPC control algorithm model consisting of a current MPC calculation module, an inverse Park transform module, a Clark transform module, a Park transform module, an SVPWM generation module, and an extended Kalman filter module; Step 2: Construction of the touch single finger sleeve A single-finger touch cuff was constructed to test FOC and MPC algorithms, consisting of an MCU, FPGA chip, motor, connecting rope, and transmission rod. The transmission rod consisted of three hollow rods of varying lengths connected by two rotating shafts. The rotating shaft was located at the joint of the single-finger touch cuff, allowing the finger to bend and feel force sensing feedback when the motor drove the transmission rod. Step 3: Construction of force sensing module The FPGA chip with burned or integrated FOC and MPC algorithm modules is connected to the motor and MCU microcontroller respectively to form a force sensing module of the touch single finger sleeve. One end of the connecting rope is wrapped around the rotating shaft of the motor and the other end is connected to the transmission rod. The MCU microcontroller receives the force sensing feedback data of the object from the virtual world; the FPGA chip inputs the current data of the DQ coordinate system into the first two-level MPC calculation modules of the FOC and MPC algorithm modules through the SPI interface for controlling the speed and angle; the inverse Park transformation module transforms the control voltage vector of the DQ coordinate system output by the MPC algorithm into the αβ coordinate axis direction; the SVPWM generation module transforms the control voltage vector in the αβ coordinate axis direction generated by the inverse Park transformation module into the SVPWM wave for controlling the motor; the Clark transformation module transforms the three-phase current 、 and Converted into two-phase current and The Park transformation module converts the two-phase current obtained by the Clark transformation module from the αβ coordinate axis to the DQ coordinate system, and the generated current vector is used for the current loop feedback of the FOC and MPC algorithm modules, and is input into the MPC algorithm module and and Compare and perform iterative calculation of the MPC algorithm module to complete the closed loop of the current loop; The extended Kalman filter module uses the current vector output by the Clark transform module to detect the current angle and speed of the motor; the voltage control vector generated by the MPC algorithm module is used as the operating speed and angle of the motor, and is input into the MPC control algorithm module to form a closed-loop calculation of the angle loop and the speed loop; the FOC algorithm module optimizes the current loop, angle loop and speed loop, and drives the motor to rotate through the output of the three-phase PWM wave through the three-phase full-bridge circuit, and the motor drives the transmission rod through the connecting rope, so that the wearer's fingers bend and feel force sensing feedback, thereby expanding the touch single finger sleeve to a complete touch glove, realizing touch glove force sensing based on FOC and MPC algorithms.

2. The touch glove force sensing method based on FOC and MPC algorithms according to claim 1, characterized in that: The touch-controlled single-finger cuff receives parameters transmitted by the MCU through the SPI interface by the FOC and MPC algorithm modules. Based on the output of the three-phase PWM wave by the FOC and MPC algorithm modules, the motor is controlled to rotate, thereby driving the extension and retraction of the connecting rope, causing the transmission rod to bend through the rotating shaft, and feeding force sensing feedback to the wearer's finger. This achieves precise control of the force sensing feedback at the finger of the touch-controlled single-finger cuff, allowing the user to experience the force sensing of virtual reality.

3. The touch glove force sensing method based on FOC and MPC algorithms according to claim 1 or claim 2, characterized in that: The steps of implementing motor control by the FOC and MPC algorithm modules are as follows: (1) FOC and MPC algorithm modules are in sampling mode Step 1: Use the constructed single-finger touch control to grasp a real-world object and collect the motor's speed and angle data as force sensing feedback data for the virtual object. Step 2: The FOC algorithm module transfers the data to the storage module of the MCU for matching the object data in the virtual world when in working mode; (2) FOC and MPC algorithm modules are in working mode Step 3: The FOC and MPC algorithm modules receive the force sensor feedback data obtained by MCU1 from the host computer through the SPI interface; Step 4: The FOC and MPC algorithm modules pass the obtained force sensor feedback data through the MPC induction motor current regulator, inverse Park transform module, SVPWM generation module, Clark transform module and Park transform module. The generated current vector is used for current loop feedback of the FOC and MPC algorithm modules and input into the MPC module to complete the closed-loop calculation of the current loop; Step 5: The extended Kalman filter module is used to detect the current angle and speed of the force sensor motor, and the output results are input into the MPC algorithm module for closed-loop calculation of the angle loop and speed loop; Step 6: Input the motor's SVPWM wave into the three-phase full-bridge circuit to control the motor's operation.

4. The touch glove force sensing method based on FOC and MPC algorithms according to claim 3 is characterized in that When the FOC and MPC algorithm modules are in sampling mode, the touch single finger sleeve grasps the collected object, and then records the rotation angle and speed of the motor as the force feedback data of the object. The collected force feedback data is input into the storage module of the MCU microcontroller, and matched with the object in the virtual world in the working mode. When the user grasps the object in the virtual world, the FOC and MPC algorithm modules in the FPGA chip obtain the force feedback data of the real object corresponding to the virtual object from the MCU microcontroller through the SPI interface and perform precise control.

5. The touch glove force sensing method based on FOC and MPC algorithms according to claim 1 or claim 2, characterized in that: The FOC and MPC algorithm modules in the FPGA chip input parameters from the MCU through the SPI interface and perform parameter control, configure the parameters of the FOC and MPC algorithm modules, so that the FOC and MPC algorithm modules can have different force sensing when grasping different objects. The FOC and MPC algorithm modules are implemented in a pipeline manner, so that the FOC and MPC algorithm modules can perform more control on the motor within one clock cycle, thereby reducing the operation delay of the motor.

6. The touch glove force sensing method based on FOC and MPC algorithms according to claim 1, characterized in that: The FPGA chip that burns or integrates the FOC and MPC algorithm modules has six FOC and MPC algorithm modules, and the top module outputs PWM waves to control the rotation of the motor.

Images