An online acquisition method for the terminal pose information of a dexterous hand based on inertial navigation network

Abstract

A method for online acquisition of the terminal position and posture information of a dexterous hand based on an inertial navigation network is used to obtain high-precision position and posture information of the terminal of a dexterous hand online. First, inertial navigation networking is adopted to distribute five inertial sensors at the ends of five fingers, and the position and posture information of the five fingers is measured simultaneously; then, the strapdown solution method is used to calculate the output position and posture information of the inertial sensors; and then, the angle information of the servo system of each finger joint is fully utilized to obtain a set of terminal position and posture information through positive kinematic solution; finally, the Kalman filter method is used to realize the data fusion of the inertial sensor information and the forward kinematic solution position and posture information. The present invention realizes the data fusion of the inertial sensor solution position and posture information and the forward kinematic solution position and posture information through the Kalman filter method, thereby obtaining high-precision finger terminal position and posture information.

Description

Technical Field

[0001] The present invention relates to an online acquisition method for the terminal position and posture information of a dexterous hand based on an inertial navigation network, which is used to online acquire the terminal position / posture information of an intelligent mechanical device such as a dexterous hand, and provide closed-loop feedback data for high-precision position / posture control. Background Art

[0002] To meet the demands of complex, dexterous, and precise tasks, dexterous hands require high-precision, real-time feedback on the position and posture of the fingertips. This allows for contact position and grasping posture planning between the multi-fingered hand and the object, ensuring the fingertips are in the optimal position and posture for the grasping operation, and ensuring the dexterous hand's configuration has the optimal grasping plane. Commonly used posture detection methods include optical measurement, which is highly accurate but requires the assistance of peripheral optical equipment, is costly, has limited usage scenarios, and is easily obstructed, affecting measurement results. Binocular camera measurement is also costly and easily obstructed in multi-fingered situations.

[0003] The angle information of each joint of the dexterous hand can be used to obtain the end-point posture information through the forward kinematics solution method. However, due to the complexity of the transmission mechanism, there is a certain degree of flexibility and dead zone. The posture information obtained through the forward kinematics solution is difficult to accurately describe the actual state of the end of the finger of the dexterous hand. Summary of the Invention

[0004] The technical problem solved by the present invention is: in order to realize the online acquisition of finger end position and posture information of dexterous hands with high precision and real time feedback, thereby realizing the contact position planning and grasping posture planning between the multi-fingered hand and the object, a method for online acquisition of dexterous hand end position and posture information based on an inertial navigation network is proposed. This method integrates the advantages of inertial sensors such as independence, resistance to external interference, high precision in a short time and stable joint end angle position information through sensor information fusion, laying the foundation for realizing multi-finger operation motion control of dexterous hands.

[0005] The technical solution of the present invention is:

[0006] A method for online acquisition of position and posture information of a dexterous hand end based on an inertial navigation network comprises:

[0007] An inertial sensor is placed at the end of each finger of the dexterous hand;

[0008] Online collection and storage of dexterous hand inertial sensor output data and angle information of each joint servo system;

[0009] According to the output data of the inertial sensor, the position and posture information of each finger end are calculated using the strapdown solution method;

[0010] According to the angle information of each joint servo system, the position and posture information of each finger end are solved using forward kinematics;

[0011] The Kalman filter method is used to integrate the position and posture information solved by the inertial sensor and the position and posture information solved by forward kinematics to obtain the final position and posture information of each finger end.

[0012] Preferably, when an inertial sensor is arranged at the end of each finger of the dexterous hand, the sensor reference coordinate system is parallel to the end posture coordinate system of the finger of the dexterous hand where it is located, and is installed at the center of the finger end.

[0013] Preferably, the method for online collecting and storing the output data of the dexterous hand inertial sensor and the angle information of each joint servo system is:

[0014] The output data of the position sensors of the servo systems of each joint of the dexterous hand and the gyroscope and accelerometer data output by each inertial sensor are collected and stored respectively.

[0015] Preferably, the position and posture information of each finger tip is calculated using a strapdown solution method based on the inertial sensor output data. The specific method is:

[0016] ① Update the inertial sensor attitude matrix using the quaternion method

[0017] Initial quaternion The calculation formula is:

[0018]

[0019] ψ, θ, and γ are the heading angle, pitch angle, and roll angle of the inertial sensor, respectively;

[0020] Using the angle increment method, the quaternion q(t+1) at time t is calculated based on the quaternion q(t) at time t. The discretized fourth-order calculation formula is:

[0021]

[0022] Where: Δθ x , Δθ y , Δθ z is the attitude rate The three components of the load system; represents the projection of the rotational angular velocity of the carrier system relative to the navigation system on the carrier system, the carrier system is the b system, and the navigation system is the n system;

[0023] After obtaining the quaternion update value q(t+1), the attitude matrix renew;

[0024] ②Calculate the inertial sensor attitude

[0025] The attitude measurement method and effective range of the inertial sensor are defined as follows:

[0026] The heading angle ψ is the angle between the projection of the carrier system's y-axis on the navigation system and the y-axis of the navigation system. It is positive counterclockwise from the y-axis of the navigation system and has a valid range of [0°, 360°]. The pitch angle θ is the angle between the carrier system's y-axis and its projection on the navigation system. It is positive when the inertial sensor tilts up. The valid range is [-90°, 90°]. The roll angle γ is the angle between the carrier system's x-axis and its projection on the navigation system. It is positive when the inertial sensor tilts right and negative when it tilts left. The valid range is [-180°, 180°].

[0027] remember T, after the attitude matrix is ​​updated, the updated attitude angle is calculated by the following formula:

[0028]

[0029] T 23 The second row and third column data of matrix T, T 13 The first row and third column of the matrix T, T 33 The third row and third column of the matrix T, T 21 The data of the second row and first column of matrix T, T 22 The data of the second row and second column of matrix T;

[0030] ③Calculate the inertial sensor speed

[0031] The inertial sensor velocity is updated by the following velocity equation:

[0032]

[0033] Indicates the motion acceleration of the inertial sensor in the x, y, and z axes in the n system, Indicates the specific acceleration information of the x, y, and z axes output by the accelerometer of the inertial sensor in the n-frame;

[0034] express The components on the x, y, and z axes, express Components on the x, y, and z axes; represents the Earth's rotation angular rate ω ie The projection in the n-system, represents the angular velocity of the n system relative to the Earth coordinate system; Indicates the speed of the inertial sensor in the x, y, and z axes in the n system;

[0035] Calculating inertial sensor acceleration but T′ is the acquisition period;

[0036] g is the gravitational acceleration value at the location of the inertial sensor.

[0037] ④Calculate the position of the inertial sensor

[0038] Inertial sensor position (λ b ,L b ,H b ) is obtained by integrating the velocity through the initial position information (λ0, L0, H0), λ b is longitude, L b is longitude, H b is the height, λ0 is the initial longitude, L0 is the initial longitude, H0 is the initial height;

[0039]

[0040] t k The kth acquisition moment, t k+1 k+1th collection moment;

[0041] Preferably, The calculation formula is:

[0042]

[0043] Where: Output angular rate for the inertial sensor gyroscope; It represents the projection of the rotational angular velocity of the n system relative to the inertial system onto the b system, and the inertial system is the i system; represents the Earth's rotation angular rate ω ie The projection in the n-system, It represents the angular velocity of the n system relative to the earth coordinate system. The calculation formulas of the two are:

[0044]

[0045] Where: V E 、V N 、V U To calculate the eastward, northward and celestial components of velocity V in the n system; R M and R N are the principal curvature radii along the meridian and the meridian, respectively.

[0046] Preferably,

[0047] In the WGS-84 global geodetic coordinate system, the gravitational acceleration g at the position of the carrier is:

[0048]

[0049] Preferably, the position and posture information of each finger end is calculated using forward kinematics based on the angle information of each joint servo system, and the implementation method is as follows:

[0050] The DH method is used to establish the finger coordinate system, where x0y0z0 is the hand-specified reference coordinate system and the others are follower coordinate systems. The lengths of each finger joint are a1, a2, a3, and a4, and the rotation angles of each joint are θ1, θ2, θ3, and θ4. The lateral swing axis is perpendicular to the flexion axis, and the angle is represented by α. The position coordinates of the fingertip in the hand-specified reference coordinate system are (x, y, z).

[0051] To reduce coordinate transformation, the coordinate system of joint 1 is made to coincide with the reference coordinate system specified by the corresponding hand;

[0052] Forward kinematic transformation matrix of dexterous hand fingers 0 T H for:

[0053]

[0054] Where s i′ = sinθ i′ , c i′ =cosθ i′ , s i′j′ = sin(θ i′ +θ j′ ), c i′j′ =cos(θ i′ +θ j′ ), s i′j′k′ = sin(θ i′ +θ j′ +θ k′ ), c i′j′k′ =cos(θ i′ +θ j′ +θ k′ ), i′, j′, k′=1,2,3,4;

[0055] 0 T H The first three elements of the fourth column are the position coordinates (x, y, z) of the fingertip in the hand's specified reference coordinate system, so:

[0056] x=(a4c 234 +a3c 23 +a2c2+a1)c1

[0057] y=(a4c 234 +a3c 23 +a2c2+a1)s1

[0058] z=-(a4s 234 +a3s23 +a2s2)

[0059] The palm base coordinate system is fixed, so the transformation matrix from the n system to the palm base coordinate system is Fixed and known; the dexterous hand configuration is determined, and the transformation matrix from the palm base coordinate system to the fixed reference coordinate system of each finger is Fixed and known; so the attitude rotation matrix obtained by the positive kinematics solution is is the posture transformation matrix of the finger end relative to the heel joint;

[0060] The finger end posture information θ obtained by forward kinematics calculation b1 ,γ b1 , Obtained according to the following formula:

[0061]

[0062] T″ 23 is a matrix The second row and third column data, T″ 13 is a matrix The first row and third column data, T″ 33 is a matrix The third row and third column data, T″ 21 is a matrix The second row and first column data, T″ 22 is a matrix The second row and second column data;

[0063] Forward kinematics uses the following formula to calculate the finger end position information λ b1 is longitude, L b1 is the longitude, h b1 is the height, λ Base is the initial longitude, L Base is the initial longitude, h Base is the initial height.

[0064] Preferably, the posture transformation matrix of the finger end relative to the heel joint is for:

[0065]

[0066] Preferably, the Kalman filter method is used to achieve the fusion of the position and posture information solved by the inertial sensor and the position and posture information solved by the forward kinematics method to obtain the final position and posture information of each finger end. The specific implementation method is as follows:

[0067] Assume the state equation of Kalman filter

[0068] The state quantity x selects the error parameter of the inertial sensor at the end of the finger

[0069]

[0070] Where: ε x ,ε y ,ε z is the constant drift of the gyroscope on the three coordinate axes of the carrier system, is the constant bias of the accelerometer on the three coordinate axes of the load system; x ,φ y ,φ z is the misalignment angle error of the three coordinate axes of the carrier system, δV x ,δV y ,δV z is the velocity error of the carrier system on the three coordinate axes, δL, δλ, δH are the position errors of the carrier system on the three coordinate axes; F is the state transfer matrix, Q is the random driving vector;

[0071] The error between the position information obtained by the forward kinematics solution and the position information obtained by the inertial sensor solution, and the error between the attitude information obtained by the forward kinematics solution and the attitude information obtained by the inertial sensor solution are selected as the measurement Z of the combined system. The measurement equation of the combined system is:

[0072]

[0073] Among them, θ b1 ,γ b1 , is the posture information obtained by forward kinematics solution, θ,γ, L is the posture information obtained by the inertial sensor at the end of the corresponding finger; b1 ,λ b1 ,h b1 is the position information obtained by forward kinematics solution, L b ,λ b ,H b is the position information calculated by the inertial navigation sensor at the end of the corresponding finger, G represents the measurement noise of the forward kinematics calculation, and H′ is the measurement equation;

[0074] According to the state equation and measurement equation, the Kalman filter algorithm is used to obtain the final position and posture information of each finger end.

[0075] Preferably, I 3×3 is a 3×3 unit matrix.

[0076] The advantages of the present invention compared with the prior art are:

[0077] The present invention adopts the form of inertial navigation networking and utilizes the Kalman filtering method to realize data fusion of posture information solved by inertial sensors and posture information solved by forward kinematics. Among them, the posture information solved by forward kinematics serves as the measurement information of the Kalman filter, and the inertial sensor information serves as the state information of the Kalman filter. This method combines the advantages of inertial sensors: independence, susceptibility to external interference, high accuracy in a short period of time, and stable angle position information of the joint end. It realizes high-precision real-time feedback of finger end position and posture information of dexterous hands, and online acquisition, thus laying the foundation for contact position planning and grasping posture planning between multi-fingered hands and objects. BRIEF DESCRIPTION OF THE DRAWINGS

[0078] Figure 1 This is a flow chart of a method for online acquisition of position and posture information of a dexterous hand terminal based on an inertial navigation network according to the present invention;

[0079] Figure 2 The present invention relates to a sensor layout diagram;

[0080] Figure 3 The present invention relates to a schematic diagram of a finger mechanism and a space coordinate system. DETAILED DESCRIPTION

[0081] The present invention adopts the form of inertial navigation networking and utilizes the Kalman filtering method to realize the data fusion of the posture information solved by the inertial sensor and the posture information solved by the forward kinematics. Among them, the posture information solved by the forward kinematics is used as the measurement information of the Kalman filter, and the inertial sensor information is used as the state information of the Kalman filter. This method combines the advantages of the inertial sensor: independence, susceptibility to external interference, high accuracy in a short time, and stable angle position information of the joint end.

[0082] The specific implementation steps of the present invention are as follows Figure 1 The specific implementation steps are as follows:

[0083] 1. Sensor layout: Place five inertial sensors at the ends of the five fingers of the dexterous hand. The sensor reference coordinate system is parallel to the pose coordinate system of the dexterous hand end, and the sensor is installed at the center of the finger end. Figure 2 shown.

[0084] 2. Collect and store the output data of the servo system position sensors of each finger joint and the gyroscope and accelerometer data output by each inertial sensor.

[0085] 3. The inertial sensor output data uses the strapdown solution method to calculate the end position and attitude information. This article takes an inertial sensor as an example:

[0086] ① Update the inertial sensor attitude matrix using the quaternion method

[0087] Initial quaternion The calculation formula is:

[0088]

[0089] ψ, θ, and γ are the heading angle, pitch angle, and roll angle of the inertial sensor, respectively;

[0090] Using the angle increment method, the quaternion q(t+1) at time t is calculated based on the quaternion q(t) at time t. The discretized fourth-order calculation formula is:

[0091]

[0092] Where: Δθ x , Δθ y , Δθ z is the attitude rate The three components of the load system; It represents the projection of the angular velocity of the carrier system (frame b) relative to the navigation system (frame n) on the carrier system;

[0093] After obtaining the quaternion update value q(t+1), the attitude matrix renew;

[0094] In formula (2) The calculation formula is:

[0095]

[0096] Where: Output angular rate for the gyroscope; represents the projection of the rotational angular velocity of the n system relative to the inertial system (i system) on the b system; represents the Earth's rotation angular rate ω ie The projection in the n-system, It represents the angular velocity of the n system relative to the earth coordinate system. The calculation formulas of the two are:

[0097]

[0098] Where: V E 、V N 、V U To calculate the eastward, northward and celestial components of velocity V in the n system; R M and R N R are the main curvature radii along the meridian and the zodiac circle respectively. M =R e (1-2e+3esin 2 L), R N =R e (1+esin 2L), where the ellipticity e = 1 / 298.257, and the major axis of the earth ellipsoid R e =6378137m;

[0099] ②Calculate the inertial sensor attitude

[0100] The carrier's attitude measurement method and effective range are defined as follows:

[0101] The heading angle ψ is the angle between the projection of the carrier system's y-axis on the navigation system and the y-axis of the navigation system. It is positive counterclockwise from the y-axis of the navigation system, and its valid range is [0°, 360°]. The pitch angle θ is the angle between the carrier system's y-axis and its projection on the navigation system. It is positive when the carrier is tilted to the right (looking from the rear of the carrier to the front), and negative when it is tilted to the left. Its valid range is [-180°, 180°].

[0102] remember T, after the attitude matrix is ​​updated, the updated attitude angle is calculated by the following formula:

[0103]

[0104] T 23 The second row and third column data of matrix T, T 13 The first row and third column of the matrix T, T 33 The third row and third column of the matrix T, T 21 The data of the second row and first column of matrix T, T 22 The data of the second row and second column of matrix T;

[0105] ③Calculate the inertial sensor speed

[0106] The inertial sensor velocity is updated by the following velocity equation:

[0107]

[0108] Indicates the acceleration of the inertial sensor on the x, y, and z axes in the n-frame. Indicates the specific acceleration information about the x, y, and z axes output by the inertial sensor in the n-frame.

[0109] express The components on the x, y, and z axes, express Components on the x, y, and z axes; Indicates the speed of the inertial sensor in the x, y, and z axes in the n system;

[0110] The inertial sensor acceleration is obtained from formula (6): but T′ is the acquisition period;

[0111] In the WGS-84 global geodetic coordinate system, the gravitational acceleration g at the position of the carrier is:

[0112]

[0113] ④Calculate the position of the inertial sensor

[0114] Inertial sensor position (λ b ,L b ,H b ) is obtained by integrating the velocity through the initial position information (λ0, L0, H0), λ b is longitude, L b is longitude, H b is the height, λ0 is the initial longitude, L0 is the initial longitude, H0 is the initial height;

[0115]

[0116] t k The kth acquisition moment, t k+1 k+1th collection moment;

[0117] The algorithms of the five inertial sensors are the same, where the palm base coordinate system is Base.

[0118] 4. The angle information of each joint servo system is solved using forward kinematics to calculate the end position and posture information. Taking a single finger as an example:

[0119] The DH method is used to establish the finger coordinate system, such as Figure 3 As shown in Figure 1. x0y0z0 is the hand-specified reference coordinate system, and the others are follower coordinate systems. The lengths of the finger joints are a1, a2, a3, and a4, respectively. The joint rotation angles are θ1, θ2, θ3, and θ4, respectively. The lateral swing axis is perpendicular to the flexion axis, and the angle is α. The position coordinates of the fingertip in the hand-specified reference coordinate system are (x, y, z).

[0120] To reduce coordinate transformation, the coordinate system of joint 1 is made to coincide with the reference coordinate system of the hand. The DH parameters corresponding to the finger mechanism of the dexterous hand are shown in Table 1.

[0121] Table 1 DH parameters of the single-finger mechanism of the dexterous hand

[0122]

[0123]

[0124] Forward kinematic transformation matrix of dexterous hand fingers0 T H for:

[0125]

[0126] Where s i′ = sinθ i′ , c i′ =cosθ i′ , s i′j′ = sin(θ i′ +θ j′ ), c i′j′ =cos(θ i′ +θ j′ ), s i′j′k′ = sin(θ i′ +θ j′ +θ k′ ), c i′j′k′ =cos(θ i′ +θ j′ +θ k′ ), i′, j′, k′=1,2,3,4.

[0127] The first three elements of the fourth column in formula (9) are the coordinates (x, y, z) of the fingertip in the reference coordinate system of the hand, so:

[0128] x=(a4c 234 +a3c 23 +a2c2+a1)c1 (10)

[0129] y=(a4c 234 +a3c 23 +a2c2+a1)s1 (11)

[0130] z=-(a4s 234 +a3s 23 +a2s2) (12)

[0131] The posture transformation matrix of the finger end relative to the heel joint for:

[0132]

[0133] The palm base coordinate system is fixed, so the transformation matrix from the n system to the palm base coordinate system is Fixed and known; the dexterous hand configuration is determined, and the transformation matrix from the palm base coordinate system to the fixed reference coordinate system of each finger is Fixed and known; so the attitude rotation matrix obtained by the positive kinematics solution is is the posture transformation matrix of the finger end relative to the heel joint; the finger end posture information θ calculated by forward kinematicsb1 ,γ b1 , Obtained according to the following formula.

[0134]

[0135] T″ 23 is a matrix The second row and third column data, T″ 13 is a matrix The first row and third column data, T″ 33 is a matrix The third row and third column data, T″ 21 is a matrix The second row and first column data, T″ 22 is a matrix The second row and second column data;

[0136] Finger end position information obtained by forward kinematics calculation λ b1 is longitude, L b1 is the longitude, h b1 is the height, λ Base is the initial longitude, L Base is the initial longitude, h Base is the initial height.

[0137] 5. Use the Kalman filter method to achieve data fusion of the posture information calculated by the inertial sensor and the posture information calculated by forward kinematics:

[0138] ① Determination of the equation of state

[0139] Assume the state equation of Kalman filter

[0140] The state quantity x selects the error parameter of the inertial sensor at the end of the finger

[0141]

[0142] Where: ε x ,ε y ,ε z is the constant drift of the gyroscope on the three coordinate axes of the carrier system, is the constant bias of the accelerometer on the three coordinate axes of the load system; x ,φ y ,φ z is the misalignment angle error of the three coordinate axes of the carrier system, δV x ,δV y ,δV zis the velocity error of the carrier system on the three coordinate axes, δL, δλ, δH are the position errors of the carrier system on the three coordinate axes; F is the state transfer matrix, Q is the random driving vector;

[0143] ②Determination of measurement equation

[0144] The error between the position information obtained by the forward kinematics solution and the position information obtained by the inertial sensor solution, and the error between the attitude information obtained by the forward kinematics solution and the attitude information obtained by the inertial sensor solution are selected as the measurement Z of the combined system. The measurement equation of the combined system is:

[0145]

[0146] Among them, θ b1 ,γ b1 , is the posture information obtained by forward kinematics calculation, θ,γ, L is the posture information calculated by the inertial sensor at the end of the corresponding finger; b1 ,λ b1 ,h b1 is the position information obtained by forward kinematics calculation, L b ,λ b ,H b is the position information calculated by the corresponding finger end inertial sensor, G represents the measurement noise of the positive kinematic calculation,

[0147] 6. Use Kalman filtering to obtain the final position and posture information of each finger end.

[0148] The principle of the present invention is as follows: first, inertial navigation networking is adopted to distribute five inertial sensors at the ends of five fingers, and the posture information of the five fingers is measured simultaneously; then, the strapdown solution method is used to calculate the position and posture information output by the inertial sensors; then, the angle information of the servo system of each finger joint is fully utilized to obtain a set of terminal posture information through positive kinematic solution; finally, the Kalman filter method is used to realize data fusion of the inertial sensor information and the posture information solved by positive kinematic solution, wherein the posture information solved by positive kinematic solution serves as the measurement information of the Kalman filter, and the inertial sensor information serves as the state information of the Kalman filter. The present invention is used to obtain high-precision posture and posture information of the finger ends of the dexterous hand online. The method combines the advantages of inertial sensors being independent, not susceptible to external interference, high accuracy in a short time, and stable angle position information of the joint ends. Through the Kalman filter method, the data fusion of the posture information solved by the inertial sensor and the posture information solved by positive kinematic solution is realized to obtain high-precision finger end position and posture information.

[0149] The contents not described in detail in the specification of the present invention belong to the prior art known to those skilled in the art.

Claims

1. A method for online acquisition of position information of a dexterous hand terminal based on an inertial navigation network, characterized in that: include: An inertial sensor is placed at the end of each finger of the dexterous hand; Online collection and storage of dexterous hand inertial sensor output data and angle information of each joint servo system; According to the output data of the inertial sensor, the position and posture information of each finger end are calculated using the strapdown solution method; According to the angle information of each joint servo system, the position and posture information of each finger end are solved using forward kinematics; The Kalman filter method is used to integrate the position and posture information calculated by the inertial sensor and the position and posture information calculated by forward kinematics to obtain the final position and posture information of each finger end. Based on the inertial sensor output data, the strapdown solution method is used to calculate the position and posture information of each finger end. The specific method is as follows: ① Update the inertial sensor attitude matrix using the quaternion method Initial quaternion The calculation formula is: ψ, θ, and γ are the heading angle, pitch angle, and roll angle of the inertial sensor, respectively; Using the angle increment method, the quaternion q(t+1) at time t is calculated based on the quaternion q(t) at time t. The discretized fourth-order calculation formula is: Where: Δθ x , Δθ y , Δθ z is the attitude rate The three components of the load system; represents the projection of the rotational angular velocity of the carrier system relative to the navigation system on the carrier system, the carrier system is the b system, and the navigation system is the n system; After obtaining the quaternion update value q(t+1), the attitude matrix renew; ②Calculate the inertial sensor attitude The attitude measurement method and effective range of the inertial sensor are defined as follows: The heading angle ψ is the angle between the projection of the carrier system's y-axis on the navigation system and the y-axis of the navigation system. It is positive counterclockwise from the y-axis of the navigation system and has a valid range of [0°, 360°]. The pitch angle θ is the angle between the carrier system's y-axis and its projection on the navigation system. It is positive when the inertial sensor tilts up. The valid range is [-90°, 90°]. The roll angle γ is the angle between the carrier system's x-axis and its projection on the navigation system. It is positive when the inertial sensor tilts right and negative when it tilts left. The valid range is [-180°, 180°]. remember T, after the attitude matrix is ​​updated, the updated attitude angle is calculated by the following formula: T 23 The second row and third column data of matrix T, T 13 The first row and third column of the matrix T, T 33 The third row and third column of the matrix T, T 21 The second row and first column data of matrix T, T 22 The data of the second row and second column of matrix T; ③Calculate the inertial sensor speed The inertial sensor velocity is updated by the following velocity equation: Indicates the motion acceleration of the inertial sensor in the x, y, and z axes in the n system, Indicates the specific acceleration information of the x, y, and z axes output by the accelerometer of the inertial sensor in the n-frame; express The components on the x, y, and z axes, express Components on the x, y, and z axes; represents the Earth's rotation angular rate ω ie The projection in the n-system, represents the angular velocity of the n system relative to the Earth coordinate system; Indicates the speed of the inertial sensor in the x, y, and z axes in the n system; Calculating inertial sensor acceleration but T′ is the acquisition period; g is the gravitational acceleration value at the location of the inertial sensor; ④Calculate the position of the inertial sensor Inertial sensor position (λ b ,L b ,H b ) is obtained by integrating the velocity through the initial position information (λ0, L0, H0), λ b is longitude, L b is longitude, H b is the height, λ0 is the initial longitude, L0 is the initial longitude, H0 is the initial height; t k The kth acquisition moment, t k+1 At the k+1th acquisition moment, V E 、V N 、V U To calculate the eastward, northward and celestial components of the velocity V in the n system.

2. The method for online acquisition of position information of a dexterous hand terminal based on an inertial navigation network according to claim 1 is characterized in that: When an inertial sensor is arranged at the end of each finger of the dexterous hand, the sensor reference coordinate system is parallel to the end pose coordinate system of the finger of the dexterous hand, and is installed at the center of the finger end.

3. The method for online acquisition of position information of a dexterous hand terminal based on an inertial navigation network according to claim 1 is characterized in that: The method for online acquisition and storage of the dexterous hand inertial sensor output data and the angle information of each joint servo system is as follows: The output data of the position sensors of the servo systems of each joint of the dexterous hand and the gyroscope and accelerometer data output by each inertial sensor are collected and stored respectively.

4. The method for online acquisition of position information of a dexterous hand terminal based on an inertial navigation network according to claim 1 is characterized in that: The calculation formula is: Where: Output angular rate for the inertial sensor gyroscope; It represents the projection of the rotational angular velocity of the n system relative to the inertial system onto the b system, and the inertial system is the i system; represents the Earth's rotation angular rate ω ie The projection in the n-system, It represents the angular velocity of the n system relative to the earth coordinate system. The calculation formulas of the two are: Where: R M and R N are the principal curvature radii along the meridian and the meridian, respectively.

5. The method for online acquisition of position and posture information of a dexterous hand terminal based on an inertial navigation network according to claim 1 is characterized in that: In the WGS-84 global geodetic coordinate system, the gravitational acceleration g at the position of the carrier is:

6. The method for online acquisition of position information of a dexterous hand terminal based on an inertial navigation network according to claim 1 is characterized in that: Based on the angle information of each joint servo system, the position and posture information of each finger end are calculated using forward kinematics. The implementation method is as follows: The DH method is used to establish the finger coordinate system, where x0y0z0 is the hand-specified reference coordinate system and the others are follower coordinate systems. The lengths of each finger joint are a1, a2, a3, and a4, and the rotation angles of each joint are θ1, θ2, θ3, and θ4. The lateral swing axis is perpendicular to the flexion axis, and the angle is represented by α. The position coordinates of the fingertip in the hand-specified reference coordinate system are (x, y, z). To reduce coordinate transformation, the coordinate system of joint 1 is made to coincide with the reference coordinate system specified by the corresponding hand; Forward kinematic transformation matrix of dexterous hand fingers 0 T H for: where s i′ = sinθ i′ , c i′ = cosθ i′ , s i′j′ = sin(θ i′ + θ j′ ), c i′j′ = cos(θ i′ + θ j′ ), s i′j′k′ = sin(θ i′ + θ j′ + θ k′ ), c i′j′k′ = cos(θ i′ + θ j′ + θ k′ ), i′, j′, k′ = 1, 2, 3, 4; 0 T H The first three elements of the fourth column are the position coordinates (x, y, z) of the fingertip in the hand's specified reference coordinate system, so: x=(a4c 234 +a3c 23 +a2c2+a1)c1 <h2 style=";text-align:left;direction:ltr">y=(a4c<h2 style=";text-align:left;direction:ltr"> 234 <h2 style=";text-align:left;direction:ltr"> +a3c<h2 style=";text-align:left;direction:ltr"> 23 <h2 style=";text-align:left;direction:ltr"> +a2c2+a1)s1 <h2 style=";text-align:left;direction:ltr">z=-(a4s<h2 style=";text-align:left;direction:ltr"> 234 <h2 style=";text-align:left;direction:ltr"> +a3s<h2 style=";text-align:left;direction:ltr"> 23 <h2 style=";text-align:left;direction:ltr"> +a2s2) The palm base coordinate system is fixed, so the transformation matrix from the n system to the palm base coordinate system is Fixed and known; the dexterous hand configuration is determined, and the transformation matrix from the palm base coordinate system to the fixed reference coordinate system of each finger is Fixed and known; so the attitude rotation matrix obtained by the positive kinematics solution is is the posture transformation matrix of the finger end relative to the heel joint; The finger end posture information θ obtained by forward kinematics calculation b1 ,γ b1 , Obtained according to the following formula: T″ 23 is a matrix The second row and third column data, T″ 13 is a matrix The first row and third column data, T″ 33 is a matrix The third row and third column data, T″ 21 is a matrix The second row and first column data, T″ 22 is a matrix The second row and second column data; Forward kinematics uses the following formula to calculate the finger end position information λ b1 is longitude, L b1 is the longitude, h b1 is the height, λ Base is the initial longitude, L Base is the initial longitude, h Base is the initial height.

7. The method for online acquisition of position information of a dexterous hand terminal based on an inertial navigation network according to claim 1 is characterized in that: The posture transformation matrix of the finger end relative to the heel joint for:

8. The method for online acquisition of position information of a dexterous hand terminal based on an inertial navigation network according to claim 1 is characterized in that: The Kalman filter method is used to integrate the position and attitude information calculated by the inertial sensor with the position and attitude information calculated by forward kinematics to obtain the final position and attitude information of each finger end. The specific implementation method is as follows: Assume the state equation of Kalman filter The state quantity x selects the error parameter of the inertial sensor at the end of the finger Where: ε x ,ε y ,ε z is the constant drift of the gyroscope on the three coordinate axes of the carrier system, is the constant bias of the accelerometer on the three coordinate axes of the load system; x ,φ y ,φ z is the misalignment angle error of the three coordinate axes of the carrier system, δV x ,δV y ,δV z is the velocity error of the carrier system on the three coordinate axes, δL, δλ, δH are the position errors of the carrier system on the three coordinate axes; F is the state transfer matrix, Q is the random driving vector; The error between the position information obtained by the forward kinematics solution and the position information obtained by the inertial sensor solution, and the error between the attitude information obtained by the forward kinematics solution and the attitude information obtained by the inertial sensor solution are selected as the measurement Z of the combined system. The measurement equation of the combined system is: Among them, θ b1 ,γ b1 , is the posture information obtained by forward kinematics solution, θ,γ, L is the posture information obtained by the inertial sensor at the end of the corresponding finger; b1 ,λ b1 ,h b1 is the position information obtained by forward kinematics solution, L b ,λ b ,H b is the position information calculated by the inertial navigation sensor at the end of the corresponding finger, G represents the measurement noise of the forward kinematics calculation, and H′ is the measurement equation; According to the state equation and measurement equation, the Kalman filter algorithm is used to obtain the final position and posture information of each finger end.

9. The method for online acquisition of position and posture information of a dexterous hand terminal based on an inertial navigation network according to claim 8, characterized in that: I 3×3 is a 3×3 unit matrix.

Images