CN114111772B - Underwater robot soft operation hand position tracking method based on data glove - Google Patents

Underwater robot soft operation hand position tracking method based on data glove Download PDF

Info

Publication number
CN114111772B
CN114111772B CN202111429539.5A CN202111429539A CN114111772B CN 114111772 B CN114111772 B CN 114111772B CN 202111429539 A CN202111429539 A CN 202111429539A CN 114111772 B CN114111772 B CN 114111772B
Authority
CN
China
Prior art keywords
time
state
underwater
value
data
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202111429539.5A
Other languages
Chinese (zh)
Other versions
CN114111772A (en
Inventor
曾庆军
杨淦华
邱海洋
张永林
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jiangsu University of Science and Technology
Original Assignee
Jiangsu University of Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jiangsu University of Science and Technology filed Critical Jiangsu University of Science and Technology
Priority to CN202111429539.5A priority Critical patent/CN114111772B/en
Publication of CN114111772A publication Critical patent/CN114111772A/en
Application granted granted Critical
Publication of CN114111772B publication Critical patent/CN114111772B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/30—Assessment of water resources

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Feedback Control In General (AREA)
  • Manipulator (AREA)

Abstract

The invention discloses a method for tracking the position of an underwater robot soft operation hand based on data gloves aiming at the dynamic capturing accuracy of the data gloves and the inaccuracy of the position tracking of the underwater soft operation hand. Aiming at the problem that the angle is inaccurate when the data glove captures the data of the hand joint, the gesture fusion algorithm is applied to the gesture acquisition of the data glove, and the gesture angle is solved by fusing the data of the three-axis magnetometer, the three-axis accelerometer and the three-axis gyroscope. Aiming at the problem of tracking the position of the underwater soft operator on the hand, the invention provides the method which adopts dynamic matrix prediction control, establishes the tracking error constraint condition by designing the track tracking error performance optimization index, converts the performance optimization problem meeting the constraint condition into a quadratic programming problem of solving control increment, obtains the prediction control meeting the error constraint condition in the time domain, and improves the accuracy of dynamic capturing of the data glove and the position tracking of the underwater soft operator.

Description

Underwater robot soft operation hand position tracking method based on data glove
Technical Field
The invention relates to the technical field of dynamic capturing of data gloves and underwater soft operation hand position tracking, in particular to an underwater robot soft operation hand position tracking method based on the data gloves.
Background
In recent years, the development of the operation technology of a cabled underwater Robot (ROV), more and more complex operation tasks are borne by the cabled underwater Robot (ROV), and the man-machine interaction of the underwater robot also becomes a research hot spot and a future direction of the operation technology of the underwater robot. The data glove developed based on the motion capture technology can collect finger gestures of a human hand, convert collected finger bending angle and arm gesture data into rotation angles of a mechanical hand driving motor, and further realize remote control of the mechanical hand gestures. The motion capture technology is a technology for converting motion gestures of a human body in a three-dimensional space into digital information, and is widely used in the fields of man-machine interaction such as computer animation, sports and education, interactive games, virtual reality, medical research and the like at present, and can be classified from the capture principle level, and the motion capture technology can be divided into an optical sensor, a mechanical sensor, an inertial sensor and the like. Among the commonly used motion capture techniques are optical, mechanical, and inertial-based sensors. The optical type motion capture system is high in overall cost, and has strict requirements on illumination and reflection conditions of the environment, so the optical type motion capture system is often suitable for scenes such as 3D film shooting and the like;
the method and system for estimating the hand gesture based on fusion of visual and inertial information is Jin Jie, which is closest to the algorithm. It proposes to construct hand pose data first. Then, extracting features, including extracting visual information features of the color images acquired by the AR glasses through a Resnet50 residual network to finally obtain image feature vectors; performing inertial information feature extraction by constructing a convolutional neural network to obtain an inertial information feature vector; and connecting the image feature vector and the inertia information feature vector to obtain a fused feature vector. And then carrying out 2D and 3D gesture estimation on the hand. And then, deploying the trained hand gesture estimation network model to the AR glasses through network training and testing, and carrying out real-time hand gesture estimation by calling the color camera and the data glove.
The technical scheme closest to the method has the advantages that although the method has strong adaptability to uncertain parameters and external environments, the accelerometer is easy to generate high-frequency noise during movement, the dynamic characteristics of the accelerometer are poor, and meanwhile, the integration error of the gyroscope is gradually increased along with time due to the poor low-stage dynamic frequency characteristics of the gyroscope. And only the triaxial accelerometer and the triaxial gyroscope are arranged in the inertial measurement unit, so that the transverse movement of the finger cannot be measured, the accurate position of the hand cannot be accurately captured by the algorithm, and the method is not easy to apply to actual engineering.
Meanwhile, the targets grabbed by the soft manipulator in the grabbing process of the underwater soft manipulator have high value, and the moving position of the underwater soft manipulator has important significance for guaranteeing the safety and integrity of the targets. The common sliding mode control or PID control position can exceed the target bearing capacity, so that the target is injured again due to uncoordinated movement of the underwater soft operator; the position state exceeds the limit of the space environment, and the underwater soft operator can collide with surrounding objects.
Disclosure of Invention
The invention aims to provide a data glove-based underwater robot soft operation hand position tracking method, which aims to improve the capability of an underwater soft operation hand to grasp high-value targets and protect the integrity of the underwater soft operation hand, and in the aspect of dynamic capturing of data gloves, the invention provides an improved gesture fusion algorithm for fusing nine-axis inertial sensor data, solving the accurate hand joint position and solving the problem that the hand joint position of a person is easy to error when the data glove only depends on an angle sensor to measure; in the aspect of tracking control of the position of the underwater soft operation hand to the human hand, the invention provides an improved dynamic matrix prediction control algorithm to improve the robustness and the anti-interference capability of the system, and the problem of the position error of the underwater soft operation hand and the human hand is solved by adding a limiting condition and rolling optimization; the data glove dynamic capturing and underwater soft operation hand position tracking method designed by the invention can enable the cabled underwater robot to carry out underwater high-value object detection and acquisition work by the underwater soft operation hand.
The aim of the invention is realized by the following technical scheme:
a method for tracking the position of a soft working hand of an underwater robot based on data gloves comprises the following steps:
step 1, building a complementary filter, eliminating high-frequency noise of an accelerometer and a magnetometer and low-frequency noise of a gyroscope, designing a discrete Kalman filter, solving a four-element differential equation by a four-order Dragon-Gregorian tower method, solving a posture angle represented by Euler angles at the current moment by a conversion relation between four elements and Euler angles, and constructing a state equation of the process;
and 2, reading current gyroscope data in a state equation and an observation equation. Calculating pre-estimation of state quantity, attitude angle data calculated by acceleration and magnetism, and calculating residual in the measuring process;
step 3, calculating Kalman gain, updating state estimation and error covariance of the system, waiting for sampling time delta t, and returning to execute the first step to estimate the angle at the next moment;
and 4, building an underwater soft operator manual kinematics control increment prediction model. The dynamic matrix predictive controller was designed to be [0t ]]At time n in time, the hand joint position captured by the data gloveAs the coordinate value of the current moment, the position psi of the hand joint of the underwater soft operation is used as the coordinate value in the previous period;
step 5, capturing the positions of the joints of the human hand by using the data gloveSubtracting the position psi of the underwater soft operation hand joint to obtain an increment, and correcting the underwater soft operation hand kinematics model through model parameters;
step 6, inputting the value of psi into the kinematic model of the underwater soft manipulator to predict the predicted value of the underwater soft manipulator coordinates at the next momentAnd then repeating the step in the next period, setting a limiting condition, and performing rolling optimization to realize the position tracking control of the underwater soft operator.
The object of the invention can be further achieved by the following technical measures:
further, the step (1) specifically includes:
step (1.1): according to the angular velocity data output by the gyroscope, a four-element differential equation is established, and the calculation formula is as follows:
namely:
wherein: omega represents angular velocity, omega x 、ω y 、ω z Represents an angular velocity component, Q represents an attitude angle,representing a four element component;
step (1.2): solving the above differential equation by a fourth-order Dragon-Gregory tower method to obtain four elements of the gesture at the current moment;
step (1.3): the attitude angle represented by the Euler angle at the current moment can be obtained through the conversion relation between the four elements and the Euler angle.
Further, the step (2) specifically includes:
step (2.1): reading current gyroscope data in a state equation and an observation equation;
kalman filtering state equation for cabled underwater Robot (ROV) data glove:
θ k+1 ＝θ k +[ω k -ω err_k ]·Δt+v k (3)
cabled underwater Robot (ROV) data glove kalman filter observation equation:
wherein: θ k For the attitude angle omega of the kth moment target k For angular velocity, ω, of the gyroscope output at time k err_k Outputting an error of angular velocity for the gyroscope at the kth moment, v k For input noise, Δt is the sampling period of the system, θ k+1 Is of known theta k Angle value, ω at time k+1 estimated from gyroscope data in case of time angle k For angular velocity, ε, of gyroscope output at time k k Is a random signal, y k+1 A variable value at time k+1;
step (2.2): the error generated by the attitude calculation of the gyroscope mainly comes from integration accumulation, and the angular velocity of the gyroscope at the current moment of measurement is stable, so that the angular velocity measurement error of the gyroscope can be regarded as constant, namely
ω err_k+1 ＝ω err_k (5)
Step (2.3): θ for the system k And omega err_k Omega is the state observed by the system k For the system input variables, the system state matrix equation established by the gyroscope is:
wherein: a and B are coefficient matrix, x k+1 System state vector at time k+1, v k Is a random signal, belongs to normally distributed white noise, v k ～N(0,Q)；
Step (2.4): the state matrix of the system available according to the state equations (1) and (4) of the system is as follows:
step (2.5): from the state matrix (7) and the observation equation (4) of the system, a gain matrix from the state quantity to the observed quantity can be obtained:
H＝(1 0) (8)
wherein: h is the observed gain;
step (2.6): attitude angle data y of accelerometer and magnetometer k The measurement equation is:
y k ＝Hx k +ε k (9)
wherein: x, x k The system state vector is the k moment;
step (2.7): residue during measurement:
wherein s is k As a result of the fact that the residual,for predicting the difference +.>Is a priori estimated;
step (2.8): error covariance of a priori estimates:
x k ＝Ax k-1 +BU k +v k (11)
wherein: a and B are coefficient matrixes, U k Input quantity for the system;
the state x of the system at time k-1 can be calculated from the state equation k-1 The current state is estimated a priori, namely a prediction part of the Kalman filter, and the system state value obtained at the moment is an estimated a priori value with a certain error;
step (2.9): at this time, from (9), the measurement equation is:
y k ＝Hx k +ε k (12)
wherein: h is coefficient matrix, x k For the system state vector at time k, ε k Is a random signal, belongs to normally distributed white noise, epsilon k ～N(0,R)；
Step (2.10): the prior estimation of the k time of the process based on the k-1 time of the system is thatPosterior estimation of its corrected state using the measurement equation is +.>Then there are:
in which the variable y is measured k And the difference between their predictionsIs an innovation of the measurement process and reflects the deviation degree between the predicted value and the true value. K (K) k Is the kalman gain, which acts to minimize the posterior estimation error covariance of the process;
P k∣k-1 ＝AP k-1 A T +Q (14)
wherein: p (P) k∣k-1 Is the error covariance of the a priori estimate.
Further, the step (3) specifically includes:
step (3.1): kalman gain K k ：
K k ＝P k∣k-1 H T [HP k∣k-1 H T +R] -1 (15)
Wherein: r is the radius of the self-adaptive receiving circle;
step (3.2): updating state, i.e. current stateAnd the error covariance in this state is:
step (3.3): error covariance P k ：
P k ＝(1-K k H)P k∣k-1 (17)
Step (3.4): waiting for sampling time, and returning to the first step for estimating the angle of the next time.
Further, the step (4) specifically includes:
establishing a finger unidirectional bending kinematic model:
wherein: j P ij is the central point of each joint hinge, namely the connecting point;representing the homogeneous coordinate transformation matrix when the connection point on the ith finger is transformed from the j coordinate system to the j-1 coordinate system. θ ij The rotation angle of the unidirectional bending joint; l (L) ij For each joint length, s and c are sin and cos. X is x i And y i Respectively represent the coordinate values of the origins of the coordinate systems of the fingers in the palm coordinate system.
Further, the step (5) specifically includes:
step (5.1): for a finger unidirectional bending kinematic model at a desired path pointPerforming first-order Taylor expansion linearization processing on the position to obtain the following prediction model:
wherein: A. b is a Jacobian matrix;
step (5.2): linearizing and discretizing the model to obtain a state space model in a control increment form:
wherein: gamma represents output.
Further, the step (6) specifically includes:
step (6.1): inputting the value of psi into the kinematic model of the underwater soft manipulator to predict the predicted value of the underwater soft manipulator coordinates at the next momentThis step is then repeated in the next cycle;
step (6.2): setting constraint conditions, and limiting control increment, control quantity and output quantity in the current time and prediction time domain as follows:
γ min ≤γ(k+i)≤γ max ,i＝0,1,2,…,N p (27)
wherein: n (N) C Representing the control time domain, N p Representing the prediction time domain,indicating the control increment at time k + i,represents the control quantity at time k+i, and γ (k+i) represents the output quantity at time k+i,/->Representing a control increment minimum,/->Represents the maximum value of the control increment,/, and%>Representing the control quantity minimum,/-, for example>The maximum value of the control amount is represented and selected according to the bending performance of the finger. Gamma ray min Representing minimum output, gamma max Representing the maximum output;
step (6.3): performing rolling optimization to minimize deviation of the controlled variable from the expected value in a future period of time;
wherein, gamma represents the output value at the current time, gamma ref Indicating the expected value after the adaptive line-of-sight processing,and (3) representing control increment, Q and R representing weight matrix, selecting the diagonal matrix with the value of the main diagonal as an integer and less than 100, and J representing performance index.
Compared with the prior art, the invention has the beneficial effects that:
1. the underwater robot data glove system has better anti-interference capability in motion capture, the first-order low-pass filter in the complementary filter can effectively inhibit high-frequency noise generated by the accelerometer during motion, and the first-order high-pass filter can effectively inhibit low-frequency noise of the gyroscope and eliminate the defect that integral errors become larger gradually along with time.
2. The underwater robot data glove system has more accurate acquisition capability in motion capture, a three-axis magnetometer is introduced, a nine-axis inertial sensor is formed by the three-axis magnetometer and the three-axis gyroscope, and the noise is eliminated more effectively while the hand traversing acquisition capability is increased, so that the dynamic capture is more accurate.
3. The underwater robot data glove system has better resolving power in dynamic capture, introduces a gesture fusion algorithm, combines the advantages of respective sensors, and combines a Kalman filtering algorithm to ensure that the data settlement of the sensors is more accurate, thereby obtaining accurate hand action positions.
4. The underwater soft operation hand has more accurate position tracking capability when grabbing the target, dynamic matrix prediction control is adopted, and the position of the underwater soft operation hand joint is continuously improved and limited by making the difference between the position of the hand joint and the position of the underwater soft operation hand joint, so that the position of the underwater soft operation hand joint is close to the position of the hand, the grabbing accuracy is ensured, and the occurrence of misoperation is reduced.
5. The underwater soft operator adopts a controller designed by dynamic matrix predictive control, the method adopts a rolling optimization strategy, the dynamic control performance is better, and the closed-loop control system designed by the method has strong anti-interference capability.
6. The invention combines the data glove, the gesture fusion algorithm and the dynamic matrix control algorithm to carry out position tracking control on the underwater soft operator, can efficiently grasp the underwater high-value target and protect the integrity of the underwater high-value target.
Drawings
FIG. 1 is a block diagram of a method for tracking the position of a soft manipulator of an underwater robot based on data gloves;
FIG. 2 is a flow chart of a gesture fusion algorithm;
FIG. 3 is a diagram of a Kalman filtering process;
FIG. 4 is a flow chart of dynamic matrix predictive control of the position of a soft underwater manipulator.
Specific implementation measures
The invention will be further described with reference to the drawings and the specific examples.
According to the method, a complementary filter is built, high-frequency noise of an accelerometer and a magnetometer and low-frequency noise of a gyroscope are eliminated, a discrete Kalman filter is designed, a four-element differential equation is solved by a four-order Dragon-Gerdostat method, an attitude angle represented by Euler angles at the current moment is obtained through a conversion relation between four elements and Euler angles, and a state equation of the process is constructed;
according to the angular velocity data output by the gyroscope, a four-element differential equation is established, and the calculation formula is as follows:
namely:
wherein: ω represents angular velocity, Q represents attitude angle;
the four-order Longku tower method is used for solving the above differential equation, so that the four elements of the gesture at the current moment can be solved, and the gesture angle represented by the Euler angle at the current moment can be obtained through the conversion relation between the four elements and the Euler angle:
wherein: h is the solution step length, k i Is a coefficient.
According to the illustration of fig. 2, the current gyroscope data is read in the state equation and the observation equation. And calculating pre-estimation of state quantity, attitude angle data calculated by acceleration and magnetic force, and calculating residual in the measuring process:
1) Kalman filtering state equation for cabled underwater Robot (ROV) data glove:
θ k+1 ＝θ k +[ω k -ω err_k ]·Δt+v k (32)
2) Cabled underwater Robot (ROV) data glove kalman filter observation equation:
wherein: θ k For the attitude angle omega of the kth moment target k For angular velocity, ω, of the gyroscope output at time k err_k Outputting an error of angular velocity for the gyroscope at the kth moment, v t For input noise, Δt is the sampling period of the system, θ k+1 Is of known theta k An angle value at time k+1 estimated from the gyroscope data in the case of the angle at time; omega k For the angular velocity output by the gyroscope at the kth moment, the error generated by the attitude calculation of the gyroscope mainly comes from integration accumulation, and the angular velocity of the gyroscope at the current moment is measured more stably, so the angular velocity measurement error of the gyroscope can be regarded as constant, namely
ω err_k+1 ＝ω err_k (34)
θ for the system k And omega err_k Omega is the state observed by the system k For the system input variables, the system state matrix equation established by the gyroscope is:
the state matrix of the system available from the state equations (29) and (32) of the system is as follows:
from the state matrix (35) and the observation equation (32) of the system, a gain matrix from state quantity to observed quantity can be obtained:
H＝(1 0) (37)
attitude angle data y of accelerometer and magnetometer k The measurement equation is:
y k ＝Hx k +ε k (38)
residue during measurement:
error covariance of a priori estimates:
x k ＝Ax k-1 +BU k +v k (40)
wherein: a and B are coefficient matrix, x k For the system state vector at time k, U k V is the input to the system k Is a random signal, belongs to normally distributed white noise, v k ～N(0,Q)。
According to FIG. 3, the state x of the system at time k-1 can be calculated from the state equation k-1 The current state is estimated a priori, namely a prediction part of the Kalman filter, and the system state value obtained at the moment is an estimated a priori value with a certain error;
at this time, from (37), the measurement equation is:
y k ＝Hx k +ε k (41)
wherein: h is coefficient matrix, x k For the system state vector at time k, ε k Is a random signal, belongs to normally distributed white noise,ε k ～N(0,R)；
the prior estimation of the k time of the process based on the k-1 time of the system is thatPosterior estimation of its corrected state using the measurement equation is +.>Then there are:
in which the variable y is measured k And the difference between their predictionsIs an innovation of the measurement process and reflects the deviation degree between the predicted value and the true value. K (K) k Is the kalman gain, which acts to minimize the posterior estimation error covariance of the process;
wherein: p (P) k∣k-1 Is the error covariance of the a priori estimate.
P k∣k-1 ＝AP k-1 A T +Q (43)
Calculating Kalman gain, updating state estimation and error covariance of the system, waiting for sampling time delta t, and returning to execute the first step to estimate the angle at the next time;
kalman gain K k ：
K k ＝P k∣k-1 H T [HP k∣k-1 H T +R] -1 (44)
Updating state, i.e. current stateAnd the error covariance in this state is:
error covariance P k ：
P k ＝(1-K k H)P k∣k-1 (46)
Waiting for sampling time, and returning to the first step for estimating the angle of the next time.
According to the figure 4, a model for predicting the increment of the kinematic control of the underwater soft manipulator is built; the dynamic matrix predictive controller was designed to be [0t ]]At time n in time, the hand joint position captured by the data gloveAs the coordinate value of the current moment, the position psi of the hand joint of the underwater soft operation is used as the coordinate value in the previous period;
establishing a finger unidirectional bending kinematic model:
wherein: j P ij is the central point of each joint hinge, namely the connecting point;representing a homogeneous coordinate transformation array when the connection point on the ith finger is transformed from a j coordinate system to a j-1 coordinate system; θ ij The rotation angle of the unidirectional bending joint; l (L) ij For the length of each joint rod, s and c are sin and cos; x is x i And y i Respectively represent the coordinate values of the origins of the coordinate systems of the fingers in the palm coordinate system.
Hand joint position captured with data gloveSubtracting the position psi of the underwater soft operation hand joint to obtain an increment, and correcting the underwater soft operation hand kinematics model through model parameters;
for a finger unidirectional bending kinematic model at a desired path pointPerforming first-order Taylor expansion linearization processing on the position to obtain the following prediction model:
wherein: A. b is a Jacobian matrix;
linearizing and discretizing the model to obtain a state space model in a control increment form:
wherein: gamma represents output.
Inputting the value of psi into the kinematic model of the underwater soft manipulator to predict the predicted value of the underwater soft manipulator coordinates at the next momentThen repeating the step in the next period to realize the position tracking control of the underwater soft operator;
meanwhile, constraint conditions are set, and the control increment, the control quantity and the output quantity are limited as follows in the current moment and the prediction time domain:
wherein: n (N) C Representing the control time domain, N p Representing the prediction time domain,indicating the control increment at time k + i,represents the control quantity at time k+i, and γ (k+i) represents the output quantity at time k+i,/->Representing a control increment minimum,/->Represents the maximum value of the control increment,/, and%>Representing the control quantity minimum,/-, for example>The maximum value of the control amount is represented and selected according to the bending performance of the finger. Gamma ray min Representing minimum output, gamma max Representing the maximum output.
Performing rolling optimization to minimize deviation of the controlled variable from the expected value in a future period of time;
wherein, gamma represents the output value at the current time, gamma ref Indicating the expected value after the adaptive line-of-sight processing,and (3) representing control increment, Q and R representing weight matrix, selecting the diagonal matrix with the value of the main diagonal as an integer and less than 100, and J representing performance index. />

Claims (7)

1. The underwater robot soft operation hand position tracking method based on the data glove is characterized by comprising the following steps:
step 1, building a complementary filter, eliminating high-frequency noise of an accelerometer and a magnetometer and low-frequency noise of a gyroscope, designing a discrete Kalman filter, solving a four-element differential equation by a four-order Dragon-Gregorian tower method, solving a posture angle represented by Euler angles at the current moment by a conversion relation between four elements and Euler angles, and constructing a state equation of the process;
step 2, reading current gyroscope data in a state equation and an observation equation, calculating pre-estimation of state quantity, calculating attitude angle data calculated by acceleration and magnetism, and calculating residual in a measurement process;
step 3, calculating Kalman gain, updating state estimation and error covariance of the system, waiting for sampling time delta t, and returning to execute the first step to estimate the angle at the next moment;
step 4, building an underwater soft operation manual kinematics control increment prediction model, designing a dynamic matrix prediction controller, and setting the dynamic matrix prediction controller as [0t ]]At time n in time, the hand joint position captured by the data gloveAs the coordinate value of the current moment, the position psi of the hand joint of the underwater soft operation is used as the coordinate value in the previous period;
step 5, capturing the positions of the joints of the human hand by using the data gloveSubtracting the position psi of the underwater soft operation hand joint to obtain an increment, and correcting the underwater soft operation hand kinematics model through model parameters;
step 6, inputting the value of psi into the kinematic model of the underwater soft manipulator to predict the predicted value of the underwater soft manipulator coordinates at the next momentAnd then repeating the step in the next period, setting a limiting condition, and performing rolling optimization to realize the position tracking control of the underwater soft operator.
2. The method for tracking the position of the soft working hand of the underwater robot based on the data glove according to claim 1, wherein the specific steps of the step 1 for constructing the state equation of the process are as follows:
step 1.1: according to the angular velocity data output by the gyroscope, a four-element differential equation is established, and the calculation formula is as follows:
namely:
wherein: omega represents angular velocity, omega x 、ω y 、ω z Represents an angular velocity component, Q represents an attitude angle,representing a four element component;
step 1.2: solving the above differential equation by a fourth-order Dragon-Gregory tower method to obtain four elements of the gesture at the current moment;
step 1.3: the attitude angle represented by the Euler angle at the current moment can be obtained through the conversion relation between the four elements and the Euler angle.
3. The method for tracking the position of the soft working hand of the underwater robot based on the data glove according to claim 1, wherein the specific steps of calculating the pre-estimation of the state quantity and calculating the residual in the measurement process in the step 2 are as follows:
step 2.1: reading current gyroscope data in a state equation and an observation equation;
kalman filtering state equation for cabled underwater Robot (ROV) data glove:
θ k+1 ＝θ k +[ω k -ω err_k ]·Δt+v k (3)
cabled underwater Robot (ROV) data glove kalman filter observation equation:
wherein: θ k For the attitude angle omega of the kth moment target k For angular velocity, ω, of the gyroscope output at time k err_k Outputting an error of angular velocity for the gyroscope at the kth moment, v k For input noise, Δt is the sampling period of the system, θ k+1 Is of known theta k Angle value, ω at time k+1 estimated from gyroscope data in case of time angle k For angular velocity, ε, of gyroscope output at time k k Is a random signal, y k+1 A variable value at time k+1;
step 2.2: the error generated by the attitude calculation of the gyroscope mainly comes from integration accumulation, and the angular velocity of the gyroscope at the current moment of measurement is stable, so that the angular velocity measurement error of the gyroscope can be regarded as constant, namely
ω err_k+1 ＝ω err_k (5)
Step 2.3: θ for the system k And omega err_k Omega is the state observed by the system k For the system input variables, the system state matrix equation established by the gyroscope is:
wherein: a and B are coefficient matrix, x k+1 System state vector at time k+1, v k Is a random signal, belongs to normally distributed white noise, v k ～N(0,Q)；
Step 2.4: the state matrix of the system available according to the state equations (1) and (4) of the system is as follows:
step 2.5: from the state matrix (7) and the observation equation (4) of the system, a gain matrix from the state quantity to the observed quantity can be obtained:
H＝(1 0) (8)
wherein: h is the observed gain;
step 2.6: attitude angle data y of accelerometer and magnetometer k The measurement equation is:
y k ＝Hx k +ε k (9)
wherein: x is x k The system state vector is the k moment;
step 2.7: residue during measurement:
wherein s is k As a result of the fact that the residual,for predicting the difference +.>Is a priori estimated;
step 2.8: error covariance of a priori estimates:
x k ＝Ax k-1 +BU k +v k (11)
wherein: a and B are coefficient matrixes, U k Input quantity for the system;
the state x of the system at time k-1 can be calculated from the state equation k-1 The current state is estimated a priori, namely a prediction part of the Kalman filter, and the system state value obtained at the moment is an estimated a priori value with a certain error;
step 2.9: at this time, from (9), the measurement equation is:
y k ＝Hx k +ε k (12)
wherein: h is coefficient matrix, x k For the system state vector at time k, ε k Is a random signal, belongs to normally distributed white noise, epsilon k ～N(0,R)；
Step 2.10: the prior estimation of the k time of the process based on the k-1 time of the system is thatPosterior estimation of its corrected state using the measurement equation is +.>Then there are:
in which the variable y is measured k And the difference between their predictionsIs an innovation of the measurement process, and reflects the deviation degree between the predicted value and the true value; k (K) k Is the kalman gain, which acts to minimize the posterior estimation error covariance of the process;
P k∣k-1 ＝AP k-1 A T +Q (14)
wherein: p (P) k∣k-1 Is the error covariance of the a priori estimate.
4. The method for tracking the position of the working hand of the underwater robot software based on the data glove according to claim 1, wherein the step 3 calculates the kalman gain, updates the state estimation and the error covariance of the system, waits for the sampling time Δt, and returns to the first step to estimate the angle at the next time, which comprises the following specific steps:
step 3.1: kalman gain K k ：
K k ＝P k∣k-1 H T [HP k∣k-1 H T +R] -1 (15)
Wherein: r is the radius of the self-adaptive receiving circle;
step 3.2: updating state, i.e. current stateAnd the error covariance in this state is:
step 3.3: error covariance P k ：
P k ＝(1-K k H)P k∣k-1 (17)
Step 3.4: waiting for sampling time, and returning to the first step for estimating the angle of the next time.
5. The method for tracking the position of the soft manipulator of the underwater robot based on the data glove according to claim 1, wherein the step 4 is to build a model for predicting the kinematic control increment of the soft manipulator of the underwater robot; the dynamic matrix predictive controller was designed to be [0t ]]At time n in time, the hand joint position captured by the data gloveAs the coordinate value of the current moment, the specific steps of taking the position psi of the hand joint of the underwater soft operation as the coordinate value in the previous period are as follows:
establishing a finger unidirectional bending kinematic model:
wherein: j P ij is the central point of each joint hinge, namely the connecting point;representing a homogeneous coordinate transformation array when the connection point on the ith finger is transformed from a j coordinate system to a j-1 coordinate system; θ ij The rotation angle of the unidirectional bending joint; l (L) ij For the length of each joint rod, s and c are sin and cos; x is x i And y i Respectively represent the coordinate values of the origins of the coordinate systems of the fingers in the palm coordinate system.
6. The method for tracking the position of the soft working hand of the underwater robot based on the data glove according to claim 1, wherein the step 5 is to capture the position of the joints of the hand by the data gloveSubtracting the position psi of the underwater soft operation hand joint to obtain an increment, and correcting the underwater soft operation hand kinematics model through model parameters, wherein the method comprises the following specific steps of:
step 5.1: for a finger unidirectional bending kinematic model at a desired path point (ψ R ,) Performing first-order Taylor expansion linearization processing on the position to obtain the following prediction model:
wherein:A. b is a Jacobian matrix;
step 5.2: linearizing and discretizing the model to obtain a state space model in a control increment form:
wherein:gamma represents output.
7. The method for tracking the position of the soft manipulator of the underwater robot based on the data glove according to claim 1, wherein the step 6 predicts the coordinate value of the next moment; the specific steps of limiting the control increment, the control quantity and the output quantity and performing rolling optimization are as follows:
step 6.1: inputting the value of psi into the kinematic model of the underwater soft manipulator to predict the predicted value of the underwater soft manipulator coordinates at the next momentThis step is then repeated in the next cycle;
step 6.2: setting constraint conditions, and limiting control increment, control quantity and output quantity in the current time and prediction time domain as follows:
γ min ≤γ(k+i)≤γ max ,i＝0,1,2,…,N p (27)
wherein: n (N) C Representing the control time domain, N p Representing the prediction time domain,represents the control increment at time k+i, +.>Represents the control quantity at time k+i, and γ (k+i) represents the output quantity at time k+i,/->Representing the minimum value of the control increment,represents the maximum value of the control increment,/, and%>Representing the control quantity minimum,/-, for example>Representing the maximum value of the control quantity, and selecting according to the bending performance of the finger; gamma ray min Representing minimum output, gamma max Representing the maximum output;
step 6.3: performing rolling optimization to minimize deviation of the controlled variable from the expected value in a future period of time;
wherein, gamma represents the output value at the current time, gamma ref Indicating the expected value after the adaptive line-of-sight processing,and (3) representing control increment, Q and R representing weight matrix, selecting the diagonal matrix with the value of the main diagonal as an integer and less than 100, and J representing performance index.
CN202111429539.5A 2021-11-29 2021-11-29 Underwater robot soft operation hand position tracking method based on data glove Active CN114111772B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111429539.5A CN114111772B (en) 2021-11-29 2021-11-29 Underwater robot soft operation hand position tracking method based on data glove

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111429539.5A CN114111772B (en) 2021-11-29 2021-11-29 Underwater robot soft operation hand position tracking method based on data glove

Publications (2)

Publication Number Publication Date
CN114111772A CN114111772A (en) 2022-03-01
CN114111772B true CN114111772B (en) 2023-10-03

Family

ID=80370970

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111429539.5A Active CN114111772B (en) 2021-11-29 2021-11-29 Underwater robot soft operation hand position tracking method based on data glove

Country Status (1)

Country Link
CN (1) CN114111772B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114454174B (en) * 2022-03-08 2022-10-04 江南大学 Mechanical arm motion capturing method, medium, electronic device and system

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
TW201118662A (en) * 2009-11-30 2011-06-01 Yin-Chen Chang Trace-generating systems and methods thereof
CN104764452A (en) * 2015-04-23 2015-07-08 北京理工大学 Hybrid position-posture tracking method based on inertia and optical tracking systems
CN106679649A (en) * 2016-12-12 2017-05-17 浙江大学 Hand movement tracking system and tracking method
CN109481226A (en) * 2018-09-27 2019-03-19 南昌大学 A kind of both hands tracking mode multiple degrees of freedom software finger gymnastic robot and application method
WO2020253854A1 (en) * 2019-06-21 2020-12-24 台州知通科技有限公司 Mobile robot posture angle calculation method
CN113332104A (en) * 2021-07-08 2021-09-03 中国科学技术大学 Recovered robot gloves of articulated type software

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11106273B2 (en) * 2015-10-30 2021-08-31 Ostendo Technologies, Inc. System and methods for on-body gestural interfaces and projection displays

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
TW201118662A (en) * 2009-11-30 2011-06-01 Yin-Chen Chang Trace-generating systems and methods thereof
CN104764452A (en) * 2015-04-23 2015-07-08 北京理工大学 Hybrid position-posture tracking method based on inertia and optical tracking systems
CN106679649A (en) * 2016-12-12 2017-05-17 浙江大学 Hand movement tracking system and tracking method
CN109481226A (en) * 2018-09-27 2019-03-19 南昌大学 A kind of both hands tracking mode multiple degrees of freedom software finger gymnastic robot and application method
WO2020253854A1 (en) * 2019-06-21 2020-12-24 台州知通科技有限公司 Mobile robot posture angle calculation method
CN113332104A (en) * 2021-07-08 2021-09-03 中国科学技术大学 Recovered robot gloves of articulated type software

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Research on optimal grasping planning based on flexible wrist-hand;ZHANG X et al.;Chinese Journal of Engineering Design;第27卷(第3期);全文 *
一种面向机器人分拣的杂乱工件视觉检测识别方法;谢先武;熊禾根;陶永;刘辉;许曦;孙柏树;;高技术通讯(04);全文 *

Also Published As

Publication number Publication date
CN114111772A (en) 2022-03-01

Similar Documents

Publication Publication Date Title
CN110076772B (en) Grabbing method and device for mechanical arm
Du et al. Online serial manipulator calibration based on multisensory process via extended Kalman and particle filters
CN107627303B (en) PD-SMC control method of visual servo system based on eye-on-hand structure
CN110815258B (en) Robot teleoperation system and method based on electromagnetic force feedback and augmented reality
CN110253574B (en) Multi-task mechanical arm pose detection and error compensation method
CN113175929B (en) UPF-based spatial non-cooperative target relative pose estimation method
Choi et al. Enhanced SLAM for a mobile robot using extended Kalman filter and neural networks
CN114454174B (en) Mechanical arm motion capturing method, medium, electronic device and system
Yousuf et al. Information fusion of GPS, INS and odometer sensors for improving localization accuracy of mobile robots in indoor and outdoor applications
CN114111772B (en) Underwater robot soft operation hand position tracking method based on data glove
Du et al. A novel human–manipulators interface using hybrid sensors with Kalman filter and particle filter
Qu et al. Dynamic visual tracking for robot manipulator using adaptive fading Kalman filter
Kopniak et al. Natural interface for robotic arm controlling based on inertial motion capture
Tan et al. New varying-parameter recursive neural networks for model-free kinematic control of redundant manipulators with limited measurements
CN109885073B (en) Prediction method for free floating motion state of space non-cooperative target
CN114986498A (en) Mobile operation arm cooperative control method
CN110967017A (en) Cooperative positioning method for rigid body cooperative transportation of double mobile robots
Luo et al. End-Effector Pose Estimation in Complex Environments Using Complementary Enhancement and Adaptive Fusion of Multisensor
CN114046800B (en) High-precision mileage estimation method based on double-layer filtering frame
Pankert et al. Learning Contact-Based State Estimation for Assembly Tasks
US11662742B2 (en) Self-position estimation method
Anderle et al. Sensor fusion for simple walking robot using low-level implementation of Extended Kalman Filter
Olsson et al. Flexible force-vision control for surface following using multiple cameras
Du et al. Human-manipulator interface using particle filter
CN114764830A (en) Object pose estimation method based on quaternion EKF and uncalibrated hand-eye system

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant