OBVP active motion control trajectory optimization method, control system and device

Abstract

The present invention discloses an OBVP active motion control trajectory optimization method, including: obtaining the current position of the cross slide, calculating the final position according to the patient's motion force; setting the objective function to minimize the integral of the square of the acceleration, and constructing a loss function; based on the Hamiltonian function, using the boundary conditions of fixed position and fixed speed, obtaining the co-state input matrix; solving the co-state input matrix, obtaining the optimal state input, calculating the cross slide final position force; and obtaining the optimal trajectory of the cross slide. The OBVP active motion control trajectory optimization method provided by the present invention can enable desktop rehabilitation equipment to obtain mechanical control input signals at a relatively low hardware price, and have control quality that is not inferior to high-performance force sensors.

Description

Technical Field

[0001] The present invention belongs to the technical field of rehabilitation control, and in particular relates to an OBVP (Optimal Boundary Value Problem) active motion control trajectory optimization method, control system and device. Background Art

[0002] The exercise therapy developed in rehabilitation medicine can be divided into many types according to the exercise method, muscle contraction type and therapeutic effect. The movement caused by external force acting on a part of the human body is called passive movement. It is generally used to maintain normal or increase the limited range of motion of joints, prevent muscle atrophy and joint contracture; the method of exercising by relying on the patient's own muscle strength is called active exercise. Patients with muscle strength above level 3 can perform active exercise. Pure active exercise generally does not provide assistance or apply resistance. It is mainly used to maintain the range of motion of joints, conduct training to enhance muscle strength and endurance, and enhance the coordination between muscles.

[0003] In desktop rehabilitation systems, many companies and research institutions use a cross slide as a mechanical linkage mechanism and a high-precision force sensor as a mechanical data input mechanism. When a patient uses the platform for rehabilitation exercises, he or she places the upper arm on the hand support mechanism of the rehabilitation platform and uses the joystick to exert force. The motor drive mechanism evaluates the patient's force output and then measures the range of motion to achieve active rehabilitation exercises for the patient. Figure 1 The U-ReSmart desktop upper limb rehabilitation robot developed by the applicant provides a common style of this type of mechanism.

[0004] For example, patent CN115047823A discloses a control method for a robot based on trajectory planning. The control method for the robot based on trajectory planning includes: obtaining the current trajectory equation from the starting position to the ending position; wherein, the determination of the current trajectory equation includes: obtaining a preset trajectory equation; obtaining the theoretical muscle output corresponding to each position in the preset trajectory equation; optimizing the parameters in the preset trajectory equation according to the optimization target related to the sum of all theoretical muscle outputs to obtain the current trajectory equation; and controlling the robot according to the current trajectory equation. The use of this control method based on trajectory planning can reduce the requirements for the skills or knowledge of rehabilitation trainers or coaches in the training process. In this patent, when the patient uses the platform for rehabilitation exercises, he needs to place his upper arm on the hand support mechanism of the rehabilitation platform and hold the joystick to exert force. During the entire rehabilitation cycle, the two-dimensional force sensing acquisition system undertakes the mechanical input signal acquisition work of the entire rehabilitation system. The price of high-precision industrial force sensors ranges from several hundred to tens of thousands of yuan depending on their range, accuracy and other parameters. In order to meet the requirements of high real-time response performance and measurement accuracy, the ideal situation is that the system should be able to monitor the patient's output force with high precision over the entire range, so most manufacturers usually select high-performance force sensors for them. The acquisition and hardware costs of such sensors are high, which limits the large-scale mass production of such products. In the actual rehabilitation process, it was found that many patients in the early stages of rehabilitation have weak muscle strength and cannot cover the full range of input. Summary of the invention

[0005] Technical problem: Design an OBVP active motion control trajectory optimization method, control system and device to solve the problems in the above background technology.

[0006] Technical solution: The present invention discloses an OBVP active motion control trajectory optimization method, comprising the following steps:

[0007] Get the current position of the cross slide and calculate the final position based on the patient's movement force;

[0008] Set the objective function to minimize the integral of the square of acceleration and construct the loss function;

[0009] Based on the Hamiltonian function, the co-state input matrix is ​​obtained by using the boundary conditions of fixed position and fixed velocity;

[0010] Solve the co-state input matrix, obtain the optimal state input, calculate the end position force of the cross slide, and obtain the optimal trajectory of the cross slide.

[0011] Furthermore, the specific steps of obtaining the current position of the cross slide and calculating the final position according to the patient's movement force are:

[0012] The fifth-order polynomial at any moment represents the spatial information and kinematic information equation X(t) of the machine at that moment as follows:

[0013] X(t)=c5t 5 +c4t 4 +c3t 3 +c2t 2 +c1t+c0 Formula 1

[0014] Among them, the boundary conditions are set: when t is the first moment t1 and the second moment t2, that is, the system state conditions of t1=0 and t2=T, the position, velocity, and acceleration system states are as follows:

[0015] t1=0, position is a, velocity is 0, acceleration is 0;

[0016] t2=T, position is b, velocity is 0, acceleration is 0;

[0017] Substituting the above conditions of the first time t1 and the second time t2 into Formula 1 can be described as the following matrix equation:

[0018]

[0019] The first line is the initial position P start , the second line is the final position P end , the third line is the initial velocity V start , the fourth line is the final velocity V end , V end YesP end Derivative; the fifth line is the initial acceleration a start , the sixth line is the final acceleration a end , a end Yes V end Derivatives;

[0020] Get the system state x(t) and control quantity u(t):

[0021]

[0022] Among them, X(t) represents the current state of the system, p x is the position of the system on the x-axis at time t, p y is the position of the system on the y-axis at time t, p z is the position of the system on the z axis at time t, v x is the linear velocity of the system on the x-axis at time t, v y is the linear velocity of the system on the y-axis at time t, v z is the position of the system on the z-axis at time t; U(t) is the input quantity, a x 、a y 、az are the linear accelerations on the x, y, and z axes at time t;

[0023] The derivatives of the system state and control quantity are obtained by formula 3 as follows:

[0024]

[0025] Furthermore, the objective function is set to minimize the integral of the square of acceleration, and the specific steps of constructing the loss function are:

[0026] Define the loss function J as:

[0027]

[0028] where α x , α y , α z are the linear accelerations on the x, y, and z axes respectively, x and u are the matrices in formula 3 respectively, and formula 5 constrains the operation process by using the Pondriagin minimum principle to solve the optimal control problem.

[0029] Furthermore, the specific steps of “obtaining the co-state input matrix based on the Hamiltonian function and using the boundary conditions of fixed position and fixed speed” are:

[0030] Construct the Hamiltonian function:

[0031]

[0032] Where H(x,u,λ) is the Hamiltonian function, which is designed as the loss function J and the state equation λ T The sum of f(x,u), λ T is the co-state coefficient matrix, f(x,u) is the state equation, is the derivative of the costate λ of the Hamiltonian function, (0,0,0,-λ1,-λ2,-λ3) T is the final result of the derivation. x 、a y 、a z are the linear accelerations on the x, y, and z axes respectively, and -λ1, -λ2, and -λ3 represent the covariance coefficients on the x, y, and z directions respectively.

[0033] Using the boundary conditions of fixed position, fixed velocity, and no acceleration restrictions, solve the co-state input matrix:

[0034]

[0035] The optimal input solution is u*:

[0036]

[0037] Among them, the specific meanings of the parameters in Formula 7 and Formula 8 are as follows:.

[0038] 2α1 represents the velocity polynomial of the x-axis, 2α2 represents the velocity polynomial of the y-axis, 2α3 represents the velocity polynomial of the z-axis, -2α1t-2β1 represents the acceleration polynomial of the x-axis, -2α2t-2β2 represents the acceleration polynomial of the y-axis, and -2α3t-2β3 represents the acceleration polynomial of the z-axis. α1 represents the coefficient of the first-order term of the x-axis polynomial, and =1 represents the coefficient of the constant term of the x-axis polynomial. α2 represents the coefficient of the first-order term of the y-axis polynomial, and β2 represents the coefficient of the constant term of the y-axis polynomial. α3 represents the coefficient of the first-order term of the z-axis polynomial, and β3 represents the coefficient of the constant term of the z-axis polynomial.

[0039] Furthermore, the specific steps of solving the co-state input matrix, obtaining the optimal state input, calculating the cross slide end position force, and obtaining the optimal trajectory of the cross slide are:

[0040] The optimal state input solution is x*:

[0041]

[0042] Among them, the subscript f represents the end point, the subscript 0 represents the starting point, x, y, z represent the x, y, z axis respectively, p represents the position, and v represents the linear velocity, such as v y0 represents the linear velocity on the y-axis of the starting point, v z0 Represents the linear velocity on the z-axis of the starting point, p z0 Represents the position of the starting point on the z-axis, p x0 Represents the position of the starting point on the x-axis.

[0043] The three co-states are resolved into the state matrix of formula 10:

[0044]

[0045] Among them, Δp x Represents the change in x-axis position within time T, Δp y Represents the change in the y-axis position within time T, Δp z Represents the change in z-axis position within time T, Δv x Represents the change in x-axis velocity within time T, Δx y Represents the change in y-axis velocity within time T, Δv zRepresents the change in the z-axis velocity at time T, α1 represents the undetermined coefficient of the first-order polynomial of the x-axis, and β1 represents the undetermined coefficient of the constant term of the x-axis polynomial. α2 represents the undetermined coefficient of the first-order polynomial of the y-axis, and β2 represents the undetermined coefficient of the constant term of the y-axis polynomial. α3 represents the undetermined coefficient of the first-order polynomial of the z-axis, and β3 represents the undetermined coefficient of the constant term of the z-axis polynomial.

[0046] The loss function J is expressed in the co-state of formula 10 as:

[0047]

[0048] Among them, α1 represents the coefficient of the first-order term of the x-axis polynomial, and β1 represents the coefficient of the constant term of the y-axis polynomial. α2 represents the coefficient of the first-order term of the y-axis polynomial, and β2 represents the coefficient of the constant term of the y-axis polynomial. α3 represents the coefficient of the first-order term of the z-axis polynomial, and β3 represents the coefficient of the constant term of the z-axis polynomial.

[0049] Express the two co-states with known starting speed, final speed, starting position, and final position

[0050]

[0051] Among them, Δp x Represents the change in x-axis position within time T, Δp y Represents the change in the y-axis position within time T, Δp z Represents the change in z-axis position within time T, Δv x Represents the change in x-axis velocity within time T, Δv y Represents the change in y-axis velocity within time T, Δv z It represents the change of the z-axis velocity within time T, α1 represents the undetermined coefficient of the first-order term of the x-axis polynomial, and β1 represents the undetermined coefficient of the constant term of the x-axis polynomial.

[0052] Get the final expression of the cost function, find the minimum value of the entire function of formula 13 and formula 14, and get the current optimal input, with continuous position and speed and minimal energy loss;

[0053]

[0054] Among them, subscript f represents the end point, subscript 0 represents the starting point, x, y, z represent the x, y, z axis respectively, p represents the position, and v represents the linear velocity, such as V yf Represents the linear velocity on the y-axis of the end point, V zf Represents the linear velocity on the z-axis of the end point, V z0 Represents the linear velocity on the z-axis of the starting point, p xf Represents the position of the end point on the X axis;

[0055] The formula for the minimum cost function is:

[0056]

[0057] When the speed and position changes are determined, it is only related to time T. The T with the smallest function in formula 14 is substituted into the co-state and converted into a form with position and speed changes to obtain the optimal trajectory.

[0058] The present invention also discloses a control system based on the OBVP active motion control trajectory optimization method, comprising a position module and a mechanical acquisition module connected to the position module, wherein the control program of the control system comprises the following steps:

[0059] Obtain the current position from the position module, obtain the patient's movement force from the mechanical acquisition module, and calculate the final position;

[0060] Calculate the minimum motion cost matrix and generate a trajectory library;

[0061] Does the cost matrix contain imaginary values ​​and unreasonable values ​​(maximum value or 0, Inf or 0), if so, discard the cost matrix corresponding to the imaginary values ​​and unreasonable values; if not, enter trajectory generation;

[0062] Trajectory generation starts: through the cost matrix, the minimum cost is selected as the most effective trajectory, all generated trajectories are calculated and added to the trajectory library;

[0063] Select the trajectory with the minimum cost from the trajectory library.

[0064] The present invention also discloses an electronic device, comprising a processor and a storage medium, wherein the storage medium is used to store a computer program;

[0065] The processor is configured to operate according to the computer program to execute the steps of the above method.

[0066] The present invention also discloses a storage medium on which a computer program is stored, characterized in that the steps of the above method are implemented when the computer program is executed by a processor.

[0067] In the U-ReSmart desktop upper limb rehabilitation platform developed by the applicant, the present invention defines active movement as the patient applies a certain force on the rehabilitation plane, and the device driving mechanism can move the corresponding distance accordingly, assisting the patient's upper limb to complete a corresponding active movement. In view of the above rehabilitation scenario, the present invention aims to provide a simple mechanical signal acquisition device based on a rocker potentiometer and an active motion control algorithm based on OBVP, so that this type of desktop rehabilitation equipment can obtain mechanical control input signals at a relatively low hardware price, and the control quality is not worse than that of a high-performance force sensor. Specifically, if the patient's movement force is small, there is not much difference between the movement of a small-range sensor and a large-range sensor. The compression type and other force sensors measure force based on the deformation variable, and the range is usually large, while the potentiometer actually estimates the force based on the displacement, and the range is usually small. When the patient's output is greater, it can actually be considered as a full-scale output of a small range. Adjusting the gain ratio of this full-scale can achieve an output close to that of a large-range sensor, that is, the potentiometer scheme adopted by this method under the small range adopts an output that is basically consistent with the force sensor scheme, and the full-scale gain output of the small range is adopted under the large range.

[0068] The present invention has the advantages of one order of magnitude in cost over large-range force sensors (more than a dozen yuan vs. more than a few hundred yuan). Second, it combines a relatively high-redundancy optimization method (usually used for drones or autonomous vehicles) to limit the speed and position at each moment, and can achieve the effect that the original high-specification device (0-200N) can achieve with a cheap and small-range (0-30N) input device, and the steering is more sensitive. Third, the proposed OBVP optimization implementation method has a fast calculation speed, basically at the level of ten milliseconds, and the generated trajectory is smoother. It also takes into account kinematic constraints and suppresses some mutations on the final trajectory.

[0069] Beneficial effects: Compared with the prior art, the present invention has the following substantial features and improvements:

[0070] The active motion control algorithm of OBVP provided by the present invention can enable desktop rehabilitation equipment to obtain mechanical control input signals at a relatively low hardware price. The generated trajectory is smoother, the response is more sensitive, the calculation speed is fast, the single acquisition time is short, the output waveform is smoother, and the control quality is not inferior to that of high-performance force sensors. BRIEF DESCRIPTION OF THE DRAWINGS

[0071] Figure 1 A schematic diagram of a cross slide of a desktop upper limb desktop rehabilitation robot provided by an embodiment of the present invention;

[0072] Figure 2 A schematic diagram of the structure of a remote control potentiometer provided in an embodiment of the present invention;

[0073] Figure 3 A schematic diagram of the flight rocker structure provided by an embodiment of the present invention;

[0074] Figure 4 A cost matrix and trajectory generation flow chart provided for an embodiment of the present invention;

[0075] Figure 5 Generate trajectory plots for force sensors.

[0076] Figure 6 A trajectory map is generated for the present invention.

[0077] Figure 7 Comparison chart of the time required to generate single-shot data for force sensors and potentiometers plus OBVP.

[0078] Figure 8 Generates a graph of the raw data for the force sensor.

[0079] Fig. 9 Graph of generated data for potentiometer plus OBVP optimization. DETAILED DESCRIPTION

[0080] This embodiment is further described in conjunction with the accompanying drawings.

[0081] In this embodiment, Figure 1 The cross slide of the desktop rehabilitation robot shown in the figure defines an active rehabilitation movement in detail as: in a static state, the system obtains the patient's current operating position point P start , the patient applies a force, the acquisition device will obtain a mechanical signal input force and convert it into a node motion speed V start , and calculate the target point P after the force is applied based on the force end After the calculation is completed, the system can automatically output the active motion trajectory under the input. We try to turn a single active rehabilitation process into an OBVP process, that is, an optimization process of sampling, calculation, calculating the loss function, finding the function with the minimum cost, and outputting the trajectory corresponding to the minimum loss function.

[0082] In order to make the entire system run continuously and smoothly, and to clearly describe the position, velocity, acceleration, jerk and other information at that moment, we use multi-order polynomials to describe the state of the system at a certain moment.

[0083] In this way, a fifth-order polynomial at a certain moment is defined to represent the spatial information and kinematic information equation X(t) of the machine cross slide at a certain moment.

[0084] X(t)=c5t 5 +c4t 4 +c3t 3 +c2t 2 +c1t+c0

[0085] Formula 1 Spatial information and kinematic information equation

[0086] For example, given the boundary conditions: when t is the first moment t1 and the second moment t2, that is, t1 = 0 and t2 = T, the position, velocity, and acceleration system states are as follows:

[0087] t1=0, position is a, velocity is 0, acceleration is 0;

[0088] t2=T, position is b, velocity is 0, acceleration is 0;

[0089] Substituting the above two conditions into formula 1 can be described as the following matrix equation:

[0090]

[0091] The first line is the initial position P start , the second line is the final position P end , the third line is the initial velocity V start , the fourth line is the final velocity V end , V end YesP end Derivative; the fifth line is the initial acceleration a start , the sixth line is the final acceleration a end , a end Yes V end Derivatives;

[0092] Get the system state x(t) and control quantity u(t)

[0093]

[0094] Among them, the X matrix represents the current state of the system, p x is the position of the system on the x-axis at time t, p y is the position of the system on the y-axis at time t, p z is the position of the system on the z axis at time t, v x is the linear velocity of the system on the x-axis at time t, v y is the linear velocity of the system on the y-axis at time t, v z is the position of the system on the z-axis at time t; the U matrix is ​​the input, a x 、a y 、a z are the linear accelerations on the x, y, and z axes at time t;

[0095] The derivatives of the system state and control quantity are obtained by formula 3 as follows:

[0096]

[0097] Define the loss function J as

[0098]

[0099] where a x 、a y 、a z are the linear accelerations on the x, y, and z axes respectively, x and u are the matrices in formula 3 respectively, and formula 5 constrains the operation process by using the Pondriagin minimum principle to solve the optimal control problem.

[0100] Constructing the Hamiltonian function

[0101]

[0102]

[0103] Where H(x,u,λ) is the Hamiltonian function, which is designed as the loss function J and the state equation λ T The sum of f(x,u), λ T is the co-state coefficient matrix, f(x,u) is the state equation, is the derivative of the costate λ of the Hamiltonian function, (0,0,0,-λ1,-λ2,-λ3) T is the final result of the derivation, where a x 、a y 、a z are the linear accelerations on the x, y, and z axes respectively, and -λ1, -λ2, and -λ3 represent the covariance coefficients on the x, y, and z directions respectively.

[0104] Using the boundary conditions of fixed position, fixed velocity, and no acceleration restrictions, solve the co-state input matrix:

[0105]

[0106] The optimal input solution is u*

[0107]

[0108] In Formula 7 and Formula 8, 2α1 represents the velocity polynomial of the x-axis, 2α2 represents the velocity polynomial of the y-axis, 2α3 represents the velocity polynomial of the z-axis, -2α1t-2β1 represents the acceleration polynomial of the x-axis, -2α2t-2β2 represents the acceleration polynomial of the y-axis, and -2α3t-2β3 represents the acceleration polynomial of the z-axis. α1 represents the coefficient of the first-order term of the x-axis polynomial, and β1 represents the coefficient of the constant term of the x-axis polynomial. α2 represents the coefficient of the first-order term of the y-axis polynomial, and β2 represents the coefficient of the constant term of the y-axis polynomial. α3 represents the coefficient of the first-order term of the z-axis polynomial, and β3 represents the coefficient of the constant term of the z-axis polynomial.

[0109] The optimal state trajectory is solved as x*

[0110]

[0111] Among them, the subscript f represents the end point, the subscript 0 represents the starting point, x, y, z represent the x, y, z axis respectively, p represents the position, and v represents the linear velocity, such as v y0 represents the linear velocity on the y-axis of the starting point, v z0 Represents the linear velocity on the z-axis of the starting point, p z0 Represents the position of the starting point on the z-axis, p x0 Represents the position of the starting point on the x-axis.

[0112] The two costates are resolved into the following state matrices

[0113]

[0114] Among them, Δp x Represents the change in x-axis position within time T, Δp y Represents the change in the y-axis position within time T, Δp z Represents the change in z-axis position within time T, Δv x Represents the change in x-axis velocity within time T, Δv y Represents the change in y-axis velocity within time T, Δv z Represents the change in the z-axis velocity at time T, α1 represents the undetermined coefficient of the first-order polynomial of the x-axis, and β1 represents the undetermined coefficient of the constant term of the x-axis polynomial. α2 represents the undetermined coefficient of the first-order polynomial of the y-axis, and β2 represents the undetermined coefficient of the constant term of the y-axis polynomial. α3 represents the undetermined coefficient of the first-order polynomial of the z-axis, and β3 represents the undetermined coefficient of the constant term of the z-axis polynomial.

[0115] The loss function J is expressed using the above co-state as

[0116]

[0117] Among them, α1 represents the coefficient of the first-order term of the x-axis polynomial, and β1 represents the coefficient of the constant term of the y-axis polynomial. α2 represents the coefficient of the first-order term of the y-axis polynomial, and β2 represents the coefficient of the constant term of the y-axis polynomial. α3 represents the coefficient of the first-order term of the z-axis polynomial, and β3 represents the coefficient of the constant term of the z-axis polynomial.

[0118] Express the two co-morphic polynomial coefficients in terms of velocity and position changes

[0119]

[0120] Among them, Δp x Represents the change in x-axis position within time T, Δp y Represents the change in the y-axis position within time T, Δp z Represents the change in z-axis position within time T, Δv x Represents the change in x-axis velocity within time T, Δv y Represents the change in y-axis velocity within time T, Δv z represents the change in the z-axis velocity within time T, α1 represents the undetermined coefficient of the first-order term of the x-axis polynomial, and β1 represents the undetermined coefficient of the constant term of the x-axis polynomial

[0121] Get the final polynomial expression of the cost function. The minimum value of the entire function below is the current optimal input, with continuous position and speed and minimal energy loss.

[0122]

[0123] Among them, subscript f represents the end point, subscript 0 represents the starting point, x, y, z represent the x, y, z axis respectively, p represents the position, and v represents the linear velocity, such as V yf Represents the linear velocity on the y-axis of the end point, V zf Represents the linear velocity on the z-axis of the end point, V z0 Represents the linear velocity on the z-axis of the starting point, p xf Represents the position of the end point on the X axis;

[0124] Formula for the minimum value of the cost function

[0125]

[0126] When the speed and position changes are determined, it is only related to time T. The T with the smallest function in formula 14 is substituted into formula 10 to obtain the optimal trajectory.

[0127] like Figure 4As shown, get the current position, get the motion force, calculate the final position, calculate the cost matrix, discard the cost matrix containing imaginary values ​​and unreasonable values, and after the trajectory generation starts, traverse all the cost matrices calculated above, select the smallest cost as the optimal trajectory, calculate all the generated trajectories, add them to the trajectory library, and select the one with the smallest cost as the optimal trajectory.

[0128] From a cost perspective, the sensorless mechanical stress acquisition device is the most suitable for large-scale promotion, but its user experience is relatively simple, and it is impossible to analyze process data and obtain good feedback. In the scenario of pursuing a larger range and accuracy, the mechanical sensor acquisition solution is better. However, after weighing factors such as accuracy, range, average price and integration convenience, the rocker potentiometer solution proposed in the present invention is undoubtedly a choice with more advantages.

[0129] In this embodiment, Figure 2 As shown, the rocker potentiometer is a two-dimensional two-directional potential sensor, commonly found in various game controllers, and can provide a displacement change within 20N. Figure 3 It is called a flight stick because it is designed similar to an airplane joystick and is used in various flight simulation games. When this type of sensor device is working, when the user tilts and moves in the positive direction of the x-axis of the operating plane, a positive value change will be generated on the x-axis data channel. When the user tilts and moves in the positive direction of the y-axis of the operating plane, a positive value change will also be generated on the y-axis channel. The negative direction of the two axes is the same. In the example of the flight stick shown in the figure, if the user tilts in the positive x and y directions of the operating plane, a resultant force will be generated in the first quadrant. This simple mechanical acquisition device saves costs and obtains expanded usage scenarios. It meets the low-precision, small-scale rehabilitation scenarios and improves the cost-effectiveness of single rehabilitation. Combined with the minimum cost function designed by us, the energy loss is reduced as much as possible in the trajectory generation stage, so that we can use cheap small-scale input components to achieve the motion effect similar to that of high-priced large-scale devices. In addition to the cost advantage, compared with the traditional method, we also have the following advantages: first, the generated trajectory is smooth and the feedback is sensitive; second, the single calculation speed is fast and the acquisition time is short. Third, kinematic optimization is considered to make the sensor data smoother.

[0130] This embodiment is tested in the same trajectory display environment, and is analogous to the trajectory generated by the force sensor, such as Figure 5 and 6 As shown in the figure, the test requires the subjects to draw as many circles as possible. Figure 5 Force sensor data generates trajectories and Figure 6 The trajectory generated by the potentiometer plus OBVP optimization sensor is roughly similar in terms of smoothness, but Figure 6The turning radius of the optimized trajectory with the middle potentiometer plus OBVP is smaller, more turns can be made in a shorter time, and the response is faster (small torque range, more sensitive feedback). In the macroscopic view, the trajectory generation of this embodiment is smoother and the response is more sensitive.

[0131] like Figure 7 At a sampling rate of 100 Hz, the time consumption for generating single-shot data of the force sensor and the potentiometer plus OBVP was compared. Figure 7 In the comparison of the 5 rounds, the potentiometer plus OBVP optimization solution is 2 times ahead of the force sensor solution, but the single-time time of the two solutions is close to 12 milliseconds in total, which proves that the solution of this embodiment has fast microscopic calculation speed and short single-time acquisition time.

[0132] like Figure 8 and 9 As shown in the figure, the original output waveforms of the force sensor and potentiometer plus OBVP are collected. Since the optimization process of OBVP consumes more computing power, the smoothness of the potentiometer data sequence is optimized on the basis of considering kinematic information. In the output data waveform of the potentiometer, the overall waveform will be smoother than that of the unfiltered force sensor, and the sensor data considering kinematics will be smoother.

[0133] The specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner.

Claims

1. An OBVP active motion control trajectory optimization method, comprising a cross slide, characterized in that: The following steps are involved: Get the current position of the cross slide and calculate the final position based on the patient's movement force; Set the objective function to minimize the integral of the square of acceleration and construct the loss function; The objective function is set to minimize the integral of the square of acceleration, and the specific steps of constructing the loss function are: Define the loss function J as: where a x 、a y 、a z are the linear accelerations on the x, y, and z axes respectively, and x and u define the system state and control quantities respectively; Based on the Hamiltonian function, the co-state input matrix is ​​obtained by using the boundary conditions of fixed position and fixed velocity; Solve the co-state input matrix, obtain the optimal state input, calculate the end position force of the cross slide, and obtain the optimal trajectory of the cross slide.

2. The OBVP active motion control trajectory optimization method according to claim 1, characterized in that: The specific steps of obtaining the current position of the cross slide and calculating the final position according to the patient's movement force are: The current position of the patient's hand on the table is determined by reading the dual-motor encoder readings, the patient's initial force is determined by the force sensor, and the target position is estimated using the initial force readings.

3. The OBVP active motion control trajectory optimization method according to claim 1, characterized in that: The specific steps of obtaining the co-state input matrix based on the Hamiltonian function and using the boundary conditions of fixed position and fixed speed are as follows: Construct the Hamiltonian function: H(x,u,λ)=g(x,u)+λ T f(x,u) Where H(x,u,λ) is the Hamiltonian function, which is designed as the loss function J and the state equation λ T The sum of f(x,u), λ T is the co-state coefficient matrix, f(x,u) is the state equation, is the derivative of the costate λ of the Hamiltonian function, (0,0,0,-λ1,-λ2,-λ3) T is the final result of the derivation, where a x 、a y 、a z are the linear accelerations on the x, y, and z axes respectively, and -λ1, -λ2, and -λ3 represent the covariance coefficients on the x, y, and z directions respectively.

4. The OBVP active motion control trajectory optimization method according to claim 1, characterized in that: The specific steps of solving the co-state input matrix, obtaining the optimal state input, calculating the cross slide end position force, and obtaining the optimal trajectory of the cross slide are: Expand the three co-state quantities into a state matrix in polynomial form; solve for the optimal input; express the polynomial coefficients of the three co-states using the known starting speed, final speed, starting position, and final position Among them, Δp x Represents the change in x-axis position within time T, Δp y Represents the change in the y-axis position within time T, Δp z Represents the change in z-axis position within time T, Δv x Represents the change in x-axis velocity within time T, Δv y Represents the change in y-axis velocity within time T, Δv z represents the change in the z-axis velocity within time T, α1 represents the undetermined coefficient of the first-order term of the x-axis polynomial, and β1 represents the undetermined coefficient of the constant term of the x-axis polynomial; The formula for the minimum cost function is: When the speed and position changes are determined, it is only related to time T. The T with the smallest cost function is substituted into the state matrix of the resolved costate to obtain the optimal trajectory.

5. A control system based on the OBVP active motion control trajectory optimization method according to any one of claims 1 to 4, characterized in that: The control system comprises a position module and a mechanical acquisition module connected to the position module, and the control program of the control system comprises the following steps; Obtain the current position from the position module, obtain the patient's movement force from the mechanical acquisition module, and calculate the final position based on the patient's movement force; Calculate the minimum motion cost matrix and generate a trajectory library; Check whether the cost matrix contains imaginary values ​​and unreasonable values. If so, discard the cost matrix corresponding to the imaginary values ​​and unreasonable values; if not, enter trajectory generation; After the trajectory generation starts, the cost matrix is ​​used to select the one with the smallest cost as the optimal trajectory, and all generated trajectories are calculated and added to the trajectory library; Select the trajectory with the minimum cost from the trajectory library.

6. An electronic device, comprising a processor and a storage medium, characterized in that: The storage medium is used to store a computer program; The processor is configured to operate according to the computer program to perform the steps of the method according to any one of claims 1-4.

7. A storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 4 are implemented.

Images