CN104809276A

CN104809276A - Multi-finger robot dynamic analytical model and modeling method thereof

Info

Publication number: CN104809276A
Application number: CN201510175658.0A
Authority: CN
Inventors: 焦生杰; 赵睿英; 王欣
Original assignee: Changan University
Current assignee: Changan University
Priority date: 2015-04-14
Filing date: 2015-04-14
Publication date: 2015-07-29
Anticipated expiration: 2035-04-14
Also published as: CN104809276B

Abstract

The invention provides a multi-finger robot dynamic analytical model and a modeling method thereof. According to the method, a system restriction expression is built, and the relationship between the system virtual displacement and the restriction conditions is obtained; then, the virtual displacement vector general solution is solved, and is substituted into a virtual work principle expression to obtain a relational expression capable of simplifying the restriction force in a dynamic model; then, the simplification relational expression is adopted for processing the moving equation at the system non-restriction time, the processing result and the restriction expression are combined for replacing a mass supplementing matrix of a special mass matrix; finally, the mass supplementing matrix is used for solving the finger robot dynamic analytical model at the system mass matrix special time, and the model still having the solution at the mass matrix special time is obtained.

Description

Multi-finger robot dynamics analysis model and modeling method thereof

Technical Field

The invention belongs to the field of robots, relates to a multi-finger robot, and particularly relates to a multi-finger robot dynamics analysis model and a modeling method thereof.

Background

The multi-finger robot has a bionic finger design, and the multi-joint fingers can realize various clamping modes and can accurately complete various tasks in a complex environment. The mathematical model of the multi-finger robot is the basis for control and application research, and as the theories of multi-body modeling, contact kinematics and the like are involved, dynamic modeling is one of the difficulties of the control problem of the multi-finger robot and is also a hot research problem in the field in recent years.

At present, a modeling method commonly adopted by a multi-finger robot is mainly based on a Lagrange multiplier method, and the Lagrange multiplier is utilized to introduce the constraint of fingers and a grabbed object into a system and establish a dynamic model under the constraint condition. When the modeling method is adopted, the system needs to meet the following assumed conditions: (1) the grabbing force is closed; (2) the jacobian matrix of the finger is reversible; (3) the contact force is within the friction cone. In addition, when the contact between a finger and a grabbed object is researched, the position of a contact point needs to be described by using local coordinate parameters (for example, the spherical parameters can be used for expressing the position of any point on a finger curved surface).

Disclosure of Invention

Aiming at the problems in the prior art, the invention aims to provide a multi-finger robot dynamics analysis model, which solves the dynamics problem of pure rolling between a multi-finger robot and a captured object.

The invention also aims to provide a modeling method of the multi-finger robot dynamics analytic model, which solves the problem that the motion equation is not solved when the mass matrix is singular in the multi-finger robot modeling process.

In order to realize the technical task, the invention adopts the following technical scheme to realize:

a multi-finger robot dynamics analysis model is a dynamics model under the condition of pure rolling constraint between fingers and a captured object, and comprises the following specific models:

\overset{. .}{q} = {[\begin{matrix} (I - A^{+} (\dot{q}, q) A (\dot{q}, q) M (q)) \\ A (\dot{q}, q) \end{matrix}]}^{+} [\begin{matrix} \hat{Q} (\dot{q}, q) \\ b (\dot{q}, q) \end{matrix}]

in the formula: i is an identity matrix;

q＝[q_f1 q_f2 … q_fl q_o]^Tis a system generalized coordinate vector;

is a system generalized velocity vector;

is a system generalized acceleration vector;

M(q)：＝diag[M_f1，M_f2，…，M_fl，M_o]is a system quality matrix;

constraining a second order expression coefficient matrix for the system;

a second order expression constant term matrix is constrained for the system;

A (\dot{q}, q) \overset{. .}{q} = b (\dot{q}, q)

is a constraint expression of the system;

is the system external force vector;

q_ficoordinate parameter vector of ith finger subsystem, (i ═ 1, 2, …, l);

M_fian ith finger subsystem quality matrix, (i ═ 1, 2, …, l);

Q_fiis the ith finger subsystem external force vector, (i ═ 1, 2, …, l);

q_ois a coordinate parameter of a grabber system;

M_ois a grabber system quality matrix;

Q_othe external force vector of the grabber system is obtained;

[·]⁺is a matrix [. C]The generalized inverse matrix of (2);

f denotes a finger subsystem, o denotes a grasping object subsystem, l is 2, 3, …, n, l denotes the number of fingers of the robot.

A modeling method of the multi-finger robot dynamics analytic model as described above, the method comprising the steps of:

the method comprises the following steps that firstly, a multi-finger robot and an object grabbing system are decomposed, and modeling setting conditions are provided;

defining a modeling basic coordinate system and a reference coordinate system of each subsystem;

step three, establishing an ith finger subsystem dynamic model according to a Lagrange equation;

fourthly, establishing a dynamic model of the captured object subsystem according to a Lagrange equation;

step five, establishing a dynamic model of the finger and the captured object non-constraint system according to a Lagrange equation;

step six, establishing a constraint equation under the pure rolling constraint condition between the fingers and the grabbed object;

step seven, firstly, establishing a system constraint expression to obtain the relationship between the system virtual displacement and the constraint condition; secondly, solving a common solution of the virtual displacement vector, and substituting the common solution into a virtual work principle expression to obtain a relational expression capable of simplifying the constraint force in the dynamic model; then, a motion equation of the simplified relational processing system without constraint is adopted, and a quality supplementary matrix replacing a singular quality matrix is constructed by combining a processing result and a constraint expression; and finally, solving the finger robot dynamics analysis model when the system quality matrix is different by using the quality supplement matrix, and obtaining the model which still has a solution when the quality matrix is different as shown above.

Due to the adoption of the technical scheme, the invention has the following advantages:

the problem that the motion equation is not solved when the mass matrix is singular in the multi-finger robot modeling process is solved, and a multi-finger robot dynamics analytic model is obtained. Compared with a numerical model obtained by a Lagrange multiplier method, the model is in an analytic form and is easier to be applied by a control system. Compared with a Gibbs-Appell method and a Kane (Kane) method, the modeling method does not need to help auxiliary variables, so that the modeling process is simpler and more efficient.

Pure roll constraint is a form of constraint that is difficult to achieve, requiring contact between the fingertip and the grasped object without relative slippage. Compared with sliding constraint and clamping constraint, the mathematical expression of pure rolling constraint has more numbers and relatively complex form. The finger robot model established under the pure rolling constraint condition can provide reference value for the modeling process under other form constraints.

At present, in the force and position hybrid control of a robot, a force sensor needs to be installed on the robot to sample the interaction force of the end effector contacting with the outside in real time. The dynamics analytic model that this patent provided can obtain each joint moment of robot and the analytic formula of grabbing power for robot force control can only rely on system state feedback to accomplish, need not to install additional force sense sensor.

Note: the analysis model is a mathematical expression model obtained under a certain condition, the solution of the model can be obtained by giving any independent variable, and the analysis model does not depend on the initial value of the independent variable. The numerical model is an approximate model obtained by adopting a numerical method, the model precision depends on the numerical calculation result, the solution of any independent variable cannot be given at will, and the numerical model result depends on the setting of the initial value of the independent variable. Compared with a numerical model, the analytical model is more accurate and is more convenient to realize automatic control.

The system is a system formed by a multi-finger robot and a captured object, and the system is divided into a finger subsystem and a captured object subsystem.

Drawings

Fig. 1 is a schematic diagram of the kinematic relationship between a three-finger robot and a grabbed object.

Fig. 2 is a schematic diagram of the pure rolling contact of the ith finger with the grabbed object.

FIG. 3 is a diagram showing the results of numerical simulation of the three joint moments of the 1 st finger.

Fig. 4 is a diagram showing the result of numerical simulation of the 2 nd finger three-joint moment.

Fig. 5 is a diagram showing the result of numerical simulation of the 3 rd finger three-joint moment.

FIG. 6 is a simulation diagram showing the results of numerical simulation of the 1 st finger tip gripping force.

FIG. 7 is a simulation diagram showing the result of numerical simulation of the 2 nd fingertip gripping force.

Fig. 8 is a simulation diagram of the result of numerical simulation of the 3 rd fingertip gripping force.

The details of the present invention will be described in further detail below with reference to the drawings and examples.

Detailed Description

The invention provides a multi-finger robot dynamics analytic model and a modeling method thereof, which comprises the following specific implementation steps:

step one, decomposing a multi-finger robot and object grabbing system and providing modeling setting conditions:

in the modeling process, the robot and the grabbing object are regarded as a complex multi-rigid-body system, and the complex multi-body system is divided into (l +1) subsystems: the robot comprises l finger subsystems and a grabbed object subsystem, wherein l is 2, 3, …, n and l represent the number of fingers of the robot, such as a three-finger robot, and l is 3.

The modeling method is based on the following settings:

the number of finger subsystems of the multi-finger robot is not less than 2, namely l is not less than 2;

the number of finger joints of the multi-finger robot is 3;

(3) the finger subsystem model can be simplified as a linkage: the knuckles are regarded as a single rod, and the joints between the knuckles are regarded as rotary hinges;

(4) in the finger subsystem model, setting the position coordinates of a finger root to be fixed relative to a base coordinate system;

(5) the finger tip and the grabbing object are in point contact, and the finger tip and the grabbing object at the contact point are not deformed relatively.

Step two, defining a modeling basic coordinate system and a reference coordinate system of each subsystem:

(1) defining a modeling basic coordinate system { B }, namely a ground coordinate system, and selecting any point in space as an origin O_BTransverse axis X_BParallel to the ground, longitudinal axis Y_BIs vertical to the ground;

(2) define the reference coordinate system { B of the ith finger_i1, 2, …, l, selecting the root junction as the originTransverse axisWith the transverse axis X_BParallel, longitudinal axisAnd the longitudinal axis Y_BParallel connection;

(3) define the fingertip coordinate system { f) of the ith finger_i1, 2, …, l, the coordinate system is fixed on the finger end, and any point on the axis of the knuckle at the end of the finger except the surface of the finger tip is selected as the originTransverse axisCoincident with the central axis, the longitudinal axisTo the cross axisAnd is vertical.

(4) Defining a reference coordinate system { O } of the grabbed object, fixing the coordinate system on the grabbed object, and selecting the centroid of the grabbed object as an origin O_OWith any direction being transverse axisTo the cross axisThe vertical direction being the longitudinal axis

Step three, establishing an ith finger subsystem dynamic model:

selecting q_fi＝[q_i1，q_i2，q_i3，v_i]^TCoordinate parameter vector for the ith finger subsystem, as shown in FIG. 1, q_ijIs the rotational displacement of the j-th joint of the ith finger, j is 1, 2, 3, i.e. q_i1，q_i2，q_i3，v_iWhen the ith finger is in contact with the grasped object, the fingertip coordinate system { f of the ith finger_iThe local coordinates below.

The ith finger subsystem dynamics model obtained according to the Lagrange equation is as follows:

M_{fi} (q_{fi}) {\overset{. .}{q}}_{fi} = Q_{fi} ({\dot{q}}_{fi}, q_{fi}) - - - (1)

M_{fi} (q_{fi}) : = [\begin{matrix} M_{fi 11} & M_{fi 12} & M_{fi 13} & M_{fi 14} \\ M_{fi 21} & M_{fi 22} & M_{fi 23} & M_{fi 24} \\ M_{fi 31} & M_{fi 32} & M_{fi 33} & M_{fi 34} \\ M_{fi 41} & M_{fi 42} & M_{fi 43} & M_{fi 44} \end{matrix}] - - - (2)

Q_{fi} ({\dot{q}}_{fi}, q_{fi}) : = {[\begin{matrix} Q_{fi 1} & Q_{fi 2} & Q_{fi 3} & Q_{fi 4} \end{matrix}]}^{T} - - - (3)

M_fi(q_fi) Is the inertia matrix of the ith finger subsystem,is the external force matrix of the ith finger subsystem.

Wherein,

M_{fi 11} = I_{I 1} + \frac{1}{2} l_{i 1}^{2} m_{i 1} + m_{i 2} + m_{i 3} (l_{i 1}^{2} + l_{i 2}^{2} + 2 l_{i 1} l_{i 2} \cos q_{i 2}) - - - (4)

M_{fi 12} = m_{i 3} l_{i 2}^{2} + m_{i 3} l_{i 1} l_{i 2} \cos q_{i 2} M_{fi 22} = I_{i 2} + m_{i 3} l_{i 1}^{2} M_{fi 33} = I_{i 3} - - - (5)

M_Fi13＝M_fi14＝M_fi23＝M_fi24＝M_fi31＝M_fi32＝M_fi34＝0 (6)

M_fi41＝M_fi42＝M_fi43＝M_fi44＝0 (7)

\begin{matrix} Q_{fi 1} = 2 m_{i 3} l_{i 1} l_{i 2} (2 {\dot{q}}_{i 1} {\dot{q}}_{i 2} + {\dot{q}}_{i 2}^{2}) \cos q_{i 1} - (\frac{1}{2} m_{i 2} l_{i 2} g + m_{i 3} l_{i 2} g) \cos q_{i 2} \\ - \frac{1}{2} m_{i 3} l_{i 3} g \cos (q_{i 1} + q_{i 2} + q_{i 3}) \end{matrix} - - - (8)

\begin{matrix} Q_{fi 2} = - m_{i 3} l_{i 2}^{2} + m_{i 3} l_{i 1} l_{i 2} \cos q_{i 2} - (\frac{1}{2} m_{i 2} l_{i 2} g + m_{i 3} l_{i 2} g) \cos (q_{i 1} + q_{i 2}) \\ - \frac{1}{2} m_{i 3} l_{i 3} \cos (q_{i 1} + q_{i 2} + q_{i 3}) \end{matrix} - - - (9)

Q_{fi 3} = - \frac{1}{2} m_{i 3} l_{i 3} \cos (q_{i 1} + q_{i 2} + q_{i 3}), q_{fi 4} = 0 - - - (10)

the parameters referred to in the above formula are as follows:

m_ijrepresenting the mass of the jth joint of the ith finger;

l_ijrepresents the length of the jth knuckle of the ith finger;

I_ijrepresenting the moment of inertia of the jth knuckle of the ith finger with respect to the knuckle's centroid;

g represents the gravitational acceleration.

Step four, establishing a subsystem dynamic model of the captured object:

selecting a coordinate parameter vector of a subsystem for grabbing the object as q_o＝[x_o，y_o，φ，u₁，u₂，…，u_l]^TWherein (x)_o，y_o) Is the position parameter of the barycenter of the object in the base coordinate system { B }, phi is the rotational displacement of the coordinate system { O } of the object to be grabbed relative to the base coordinate system { B }, u_iAnd the local coordinate parameter of the contact point of the ith finger and the grabbed object is within { O }.

The dynamic model of the subsystem for capturing the object is obtained according to the Lagrange equation and is as follows:

M_{O} (q_{o}) {\overset{. .}{q}}_{o} = Q_{o} ({\dot{q}}_{o}, q_{o}) - - - (11)

wherein M is_o(q_o) Is the inertia matrix of the ith finger subsystem,is the external force matrix of the ith finger subsystem. m is_oFor grasping object mass, I_oTo capture the moment of inertia of an object about its center of mass,

<math> <mrow> <msub> <mi>M</mi> <mi>o</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mi>o</mi> </msub> <mo>)</mo> </mrow> <mo>&Element;</mo> <msup> <mi>R</mi> <mrow> <mrow> <mo>(</mo> <mi>l</mi> <mo>+</mo> <mn>3</mn> <mo>)</mo> </mrow> <mo>×</mo> <mrow> <mo>(</mo> <mi>l</mi> <mo>+</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow> </msup> <mo>,</mo> <msub> <mi>Q</mi> <mi>o</mi> </msub> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mi>o</mi> </msub> <mo>,</mo> <msub> <mi>q</mi> <mi>o</mi> </msub> <mo>)</mo> </mrow> <mo>&Element;</mo> <msup> <mi>R</mi> <mrow> <mo>(</mo> <mi>l</mi> <mo>+</mo> <mn>3</mn> <mo>)</mo> </mrow> </msup> <mo>.</mo> </mrow> </math>

step five, establishing a dynamic model of the finger and the capture object 'unconstrained' system:

the finger subsystem and the grabbed object subsystem are used as 'unconstrained' systems, and a system model is established according to a Lagrange equation as follows:

M (q) \overset{. .}{q} = \hat{Q} (\dot{q}, q) - - - (14)

q＝[q_f1 q_f2 … q_f1 q_o]^T (15)

M(q)：＝diag[M_f1，M_f2，…，M_fl，M_o] (16)

wherein M (q) is an inertia matrix of an 'unconstrained' system of fingers and a grabbed object,the method comprises the steps of taking an external force matrix of an 'unconstrained' system of fingers and a grabbed object, taking q as a coordinate parameter vector of the 'unconstrained' system of the fingers and the grabbed object, and taking a diag function as a new matrix constructed by taking independent variables as diagonal elements.

Q_f1And when i is 1Equal, M_f1And M when i is 1_fi(q_fi) Equal, are abbreviated as such for convenienceThe other parameters are expressed in the same manner.

Step six, establishing a constraint equation under the pure rolling constraint condition between the fingers and the grabbed object:

when pure rolling motion is maintained between the finger robot and the grabbed object, the fingers and the grabbed object need to meet three constraint conditions, namely the position, the speed and the normal vector constraint of the fingers and the grabbed object at a contact point.

When the fingers keep in contact with the grabbed object, the ith finger and the grabbed object meet the position constraint, and the constraint equation can be expressed as:

wherein (x)_bi，y_bi) Is the base B of the ith finger_iThe position parameter in the base coordinate system { B }, r is the radius of the curved surface of the object to be grasped, and ρ is the radius of the sharp curved surface.

The finger and the grabber do not slide relatively at the contact point, namely the finger and the contact point meet the speed constraint, and the speed constraint of the ith finger and the grabber can be expressed as follows:

wherein,

\begin{matrix} {\dot{x}}_{fi} = l_{i 1} {\dot{q}}_{i 1} \cos q_{i 1} + l_{i 2} ({\dot{q}}_{i 1} + {\dot{q}}_{i 2}) \cos (q_{i 1} + q_{i 2}) \\ + l_{i 3} ({\dot{q}}_{i 1} + {\dot{q}}_{i 2} + {\dot{q}}_{i 3}) \cos (q_{i 1} + q_{i 2} + q_{i 3}) \end{matrix} - - - (20)

\begin{matrix} {\dot{y}}_{fi} = l_{i 1} {\dot{q}}_{i 1} \cos q_{i 1} + l_{i 2} ({\dot{q}}_{i 1} + {\dot{q}}_{i 2}) \sin (q_{i 1} + q_{i 2}) \\ + l_{i 3} ({\dot{q}}_{i 1} + {\dot{q}}_{i 2} + {\dot{q}}_{i 3}) \sin (q_{i 1} + q_{i 2} + q_{i 3}) \end{matrix} - - - (21)

the unit normal vectors of the fingertip plane and the grabber plane at the contact point are equal in size and are positioned in the opposite direction of the common tangent plane. The normal vector constraint of the ith finger and the grabbed object at the contact point can be expressed as:

the second order form of the constraint is obtained by taking the derivative with respect to time t by the above equations (18), (19), (22), and the second order constraint is expressed in the form of a matrix as:

<math> <mrow> <msub> <mi>A</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <msub> <mover> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mo>·</mo> </mover> <mi>i</mi> </msub> <mo>,</mo> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mi>i</mi> </msub> <mo>)</mo> </mrow> <msub> <mover> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mrow> <mo>.</mo> <mo>.</mo> </mrow> </mover> <mn>1</mn> </msub> <mo>=</mo> <msub> <mi>b</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <msub> <mover> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mo>.</mo> </mover> <mi>i</mi> </msub> <mo>,</mo> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>23</mn> <mo>)</mo> </mrow> </mrow> </math>

wherein,

<math> <mrow> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mi>i</mi> </msub> <mo>=</mo> <msup> <mrow> <mo>[</mo> <msubsup> <mi>q</mi> <mi>fi</mi> <mi>T</mi> </msubsup> <mo>,</mo> <msubsup> <mi>q</mi> <mi>oi</mi> <mi>T</mi> </msubsup> <mo>]</mo> </mrow> <mi>T</mi> </msup> <mo>,</mo> <msub> <mi>q</mi> <mi>oi</mi> </msub> <mo>=</mo> <msup> <mrow> <mo>[</mo> <msub> <mi>x</mi> <mi>o</mi> </msub> <mo>,</mo> <msub> <mi>y</mi> <mi>o</mi> </msub> <mo>,</mo> <mi>φ</mi> <mo>,</mo> <msub> <mi>u</mi> <mi>i</mi> </msub> <mo>]</mo> </mrow> <mi>T</mi> </msup> <mo>,</mo> <mi>i</mi> <mo>=</mo> <mn>1,2</mn> <mo>,</mo> <mo>.</mo> <mo>.</mo> <mo>.</mo> <mo>,</mo> <mi>l</mi> <mo>.</mo> </mrow> </math>

equation (23) can be decomposed into:

<math> <mrow> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>A</mi> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mi>fi</mi> </msub> <mo>,</mo> <msub> <mi>q</mi> <mi>fi</mi> </msub> <mo>)</mo> </mrow> </mtd> <mtd> <msub> <mi>A</mi> <mrow> <mi>i</mi> <mn>2</mn> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mi>oi</mi> </msub> <mo>,</mo> <msub> <mi>q</mi> <mi>oi</mi> </msub> <mo>)</mo> </mrow> </mtd> <mtd> <msub> <mi>A</mi> <mrow> <mi>i</mi> <mn>3</mn> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mi>oi</mi> </msub> <mo>,</mo> <msub> <mi>q</mi> <mi>oi</mi> </msub> <mo>)</mo> </mrow> </mtd> </mtr> </mtable> </mfenced> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mover> <mi>q</mi> <mrow> <mo>.</mo> <mo>.</mo> </mrow> </mover> <mi>fi</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mover> <mi>q</mi> <mrow> <mo>.</mo> <mo>.</mo> </mrow> </mover> <mi>oi</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>b</mi> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mover> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mo>·</mo> </mover> <mi>i</mi> </msub> <mo>,</mo> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mi>i</mi> </msub> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>b</mi> <mrow> <mi>i</mi> <mn>2</mn> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mover> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mo>·</mo> </mover> <mi>i</mi> </msub> <mo>,</mo> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mi>i</mi> </msub> <mo>)</mo> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>24</mn> <mo>)</mo> </mrow> </mrow> </math>

in formula (24):

<math> <mrow> <msub> <mi>A</mi> <mrow> <mi>i</mi> <mn>3</mn> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mi>oi</mi> </msub> <mo>,</mo> <msub> <mi>q</mi> <mi>oi</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mo>-</mo> <msub> <mi>rs</mi> <mrow> <mi>φ</mi> <msub> <mi>u</mi> <mi>i</mi> </msub> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <mi>r</mi> <msub> <mi>c</mi> <mrow> <mi>φ</mi> <msub> <mi>u</mi> <mi>i</mi> </msub> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <msub> <mi>s</mi> <mrow> <mi>φ</mi> <msub> <mi>u</mi> <mi>i</mi> </msub> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <mo>-</mo> <msub> <mi>c</mi> <mrow> <mi>φ</mi> <msub> <mi>u</mi> <mi>i</mi> </msub> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> <msub> <mi>b</mi> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mover> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mo>.</mo> </mover> <mi>i</mi> </msub> <mo>,</mo> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mover> <mi>b</mi> <mo>&OverBar;</mo> </mover> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mover> <mi>b</mi> <mo>&OverBar;</mo> </mover> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mover> <mi>b</mi> <mo>&OverBar;</mo> </mover> <mn>3</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mover> <mi>b</mi> <mo>&OverBar;</mo> </mover> <mn>4</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mover> <mi>b</mi> <mo>&OverBar;</mo> </mover> <mn>5</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mover> <mi>b</mi> <mo>&OverBar;</mo> </mover> <mn>5</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>27</mn> <mo>)</mo> </mrow> </mrow> </math>

<math> <mrow> <msub> <mi>n</mi> <mrow> <mi>i</mi> <mn>2</mn> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mover> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mo>.</mo> </mover> <mi>i</mi> </msub> <mo>,</mo> <msub> <mover> <mi>q</mi> <mo>&OverBar;</mo> </mover> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mo>-</mo> <msub> <mi>c</mi> <mrow> <mi>φ</mi> <msub> <mi>u</mi> <mi>i</mi> </msub> </mrow> </msub> <msup> <mrow> <mo>(</mo> <mover> <mi>φ</mi> <mo>.</mo> </mover> <mo>+</mo> <msub> <mover> <mi>u</mi> <mo>.</mo> </mover> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>-</mo> <msub> <mi>c</mi> <mrow> <mn>123</mn> <msub> <mi>v</mi> <mi>i</mi> </msub> </mrow> </msub> <msup> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>2</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>3</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>v</mi> <mo>.</mo> </mover> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mtd> </mtr> <mtr> <mtd> <mo>-</mo> <msub> <mi>s</mi> <mrow> <mi>φ</mi> <msub> <mi>u</mi> <mi>i</mi> </msub> </mrow> </msub> <msup> <mrow> <mo>(</mo> <mover> <mi>φ</mi> <mo>.</mo> </mover> <mo>+</mo> <msub> <mover> <mi>u</mi> <mo>.</mo> </mover> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>-</mo> <msub> <mi>s</mi> <mrow> <mn>123</mn> <msub> <mi>v</mi> <mi>i</mi> </msub> </mrow> </msub> <msup> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>2</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>3</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>v</mi> <mo>.</mo> </mover> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>28</mn> <mo>)</mo> </mrow> </mrow> </math>

in the above formulas (25), (26), (27) and (28), for the convenience of writing into a canonical matrix, some intermediate variables are used for writing, and the meaning of the specific intermediate variables is as follows:

<math> <mfenced open='' close=''> <mtable> <mtr> <mtd> <msub> <mover> <mi>b</mi> <mo>&OverBar;</mo> </mover> <mn>1</mn> </msub> <mo>=</mo> <mi>r</mi> <msub> <mi>c</mi> <mrow> <mi>φ</mi> <msub> <mi>u</mi> <mi>i</mi> </msub> </mrow> </msub> <msup> <mrow> <mo>(</mo> <mover> <mi>φ</mi> <mo>.</mo> </mover> <mo>+</mo> <msub> <mover> <mi>u</mi> <mo>.</mo> </mover> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>-</mo> <msub> <mi>l</mi> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <msub> <mi>c</mi> <mn>12</mn> </msub> <msubsup> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> <mo>-</mo> <msub> <mi>l</mi> <mrow> <mi>i</mi> <mn>2</mn> </mrow> </msub> <msub> <mi>c</mi> <mn>12</mn> </msub> <msup> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>2</mn> </mrow> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>-</mo> <msub> <mi>l</mi> <mrow> <mi>i</mi> <mn>3</mn> </mrow> </msub> <msub> <mi>c</mi> <mn>123</mn> </msub> <msup> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>2</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>3</mn> </mrow> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mtd> </mtr> <mtr> <mtd> <mo>-</mo> <mi>ρ</mi> <msub> <mi>c</mi> <mrow> <mn>123</mn> <msub> <mi>v</mi> <mi>i</mi> </msub> </mrow> </msub> <msup> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>2</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>3</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>v</mi> <mo>.</mo> </mover> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>,</mo> </mtd> </mtr> </mtable> </mfenced> </math>

<math> <mfenced open='' close=''> <mtable> <mtr> <mtd> <msub> <mover> <mi>b</mi> <mo>&OverBar;</mo> </mover> <mn>2</mn> </msub> <mo>=</mo> <mi>r</mi> <msub> <mi>s</mi> <mrow> <mi>φ</mi> <msub> <mi>u</mi> <mi>i</mi> </msub> </mrow> </msub> <msup> <mrow> <mo>(</mo> <mover> <mi>φ</mi> <mo>.</mo> </mover> <mo>+</mo> <msub> <mover> <mi>u</mi> <mo>.</mo> </mover> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>-</mo> <msub> <mi>l</mi> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <msub> <mi>s</mi> <mn>12</mn> </msub> <msubsup> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> <mo>-</mo> <msub> <mi>l</mi> <mrow> <mi>i</mi> <mn>2</mn> </mrow> </msub> <msub> <mi>s</mi> <mn>12</mn> </msub> <msup> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>2</mn> </mrow> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>-</mo> <msub> <mi>l</mi> <mrow> <mi>i</mi> <mn>3</mn> </mrow> </msub> <msub> <mi>s</mi> <mn>123</mn> </msub> <msup> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>2</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>3</mn> </mrow> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mtd> </mtr> <mtr> <mtd> <mo>-</mo> <mi>ρ</mi> <msub> <mi>s</mi> <mrow> <mn>123</mn> <msub> <mi>v</mi> <mi>i</mi> </msub> </mrow> </msub> <msup> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>2</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>3</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>v</mi> <mo>.</mo> </mover> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>,</mo> </mtd> </mtr> </mtable> </mfenced> </math>

<math> <mfenced open='' close=''> <mtable> <mtr> <mtd> <msub> <mover> <mi>b</mi> <mo>&OverBar;</mo> </mover> <mn>3</mn> </msub> <mo>=</mo> <mi>r</mi> <msub> <mi>c</mi> <mrow> <mi>φ</mi> <msub> <mi>u</mi> <mi>i</mi> </msub> </mrow> </msub> <msup> <mrow> <mo>(</mo> <mover> <mi>φ</mi> <mo>.</mo> </mover> <mo>+</mo> <msub> <mover> <mi>u</mi> <mo>.</mo> </mover> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>-</mo> <msub> <mi>l</mi> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <msub> <mi>c</mi> <mn>12</mn> </msub> <msubsup> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> <mo>-</mo> <msub> <mi>l</mi> <mrow> <mi>i</mi> <mn>2</mn> </mrow> </msub> <msub> <mi>c</mi> <mn>12</mn> </msub> <msup> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>2</mn> </mrow> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>-</mo> <msub> <mi>l</mi> <mrow> <mi>i</mi> <mn>3</mn> </mrow> </msub> <msub> <mi>c</mi> <mn>123</mn> </msub> <msup> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>2</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>3</mn> </mrow> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mtd> </mtr> <mtr> <mtd> <mo>-</mo> <mi>ρ</mi> <msub> <mi>c</mi> <mrow> <mn>123</mn> <msub> <mi>v</mi> <mi>i</mi> </msub> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mn>12</mn> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mn>13</mn> </msub> <mo>+</mo> <msub> <mover> <mi>v</mi> <mo>.</mo> </mover> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>2</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>3</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>,</mo> </mtd> </mtr> </mtable> </mfenced> </math>

<math> <mfenced open='' close=''> <mtable> <mtr> <mtd> <msub> <mover> <mi>b</mi> <mo>&OverBar;</mo> </mover> <mn>4</mn> </msub> <mo>=</mo> <mi>r</mi> <msub> <mi>s</mi> <mrow> <mi>φ</mi> <msub> <mi>u</mi> <mi>i</mi> </msub> </mrow> </msub> <msup> <mrow> <mo>(</mo> <mover> <mi>φ</mi> <mo>.</mo> </mover> <mo>+</mo> <msub> <mover> <mi>u</mi> <mo>.</mo> </mover> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>-</mo> <msub> <mi>l</mi> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <msub> <mi>s</mi> <mn>12</mn> </msub> <msubsup> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> <mo>-</mo> <msub> <mi>l</mi> <mrow> <mi>i</mi> <mn>2</mn> </mrow> </msub> <msub> <mi>s</mi> <mn>12</mn> </msub> <msup> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>2</mn> </mrow> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>-</mo> <msub> <mi>l</mi> <mrow> <mi>i</mi> <mn>3</mn> </mrow> </msub> <msub> <mi>s</mi> <mn>123</mn> </msub> <msup> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>2</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>3</mn> </mrow> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mtd> </mtr> <mtr> <mtd> <mo>-</mo> <mi>ρ</mi> <msub> <mi>s</mi> <mrow> <mn>123</mn> <msub> <mi>v</mi> <mi>i</mi> </msub> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mn>12</mn> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mn>13</mn> </msub> <mo>+</mo> <msub> <mover> <mi>v</mi> <mo>.</mo> </mover> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>2</mn> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>.</mo> </mover> <mrow> <mi>i</mi> <mn>3</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>.</mo> </mtd> </mtr> </mtable> </mfenced> </math>

in the formula s₁＝sinq_i1，s₁₂＝sin(q_i1+q_i2)，s₁₂₃＝sin(q_i1+q_i2+q_i3)，

s_{123 v_{i}} = \sin (q_{i 1} + q_{i 2} + q_{i 3} + v_{i}), c_{i} = \cos q_{i 1}, c_{12} = \cos (q_{i 1} + q_{i 2}),

c_{123} = \cos (q_{i 1} + q_{i 2} + q_{i 3}), c_{123 v_{i}} = \cos (q_{i 1} + q_{i 2} + q_{i 3} + v_{i}),

Step seven, establishing a dynamic analysis model of a finger and captured object 'constraint' system:

establishing a motion equation of the finger and the object grabbing system, wherein the expression is as follows:

M (q) \overset{. .}{q} = \hat{Q} (\dot{q}, q) + Q^{c} (\dot{q}, q) - - - (29)

wherein,

is the system constraint.

From equation (24), the constraint expression of the system can be rewritten as:

A (\dot{q}, q) \overset{. .}{q} = b (\dot{q}, q) - - - (30),

wherein,

from the virtual displacement definition, the following holds:wherein:

q is the system virtual displacement.

According to the principle of imaginary work, the following holds:

wherein,is the system constraint.

The solution of equation (33) is: q ═ I (I-A)⁺A) And gamma (35), wherein gamma is an arbitrary vector.

Substituting (34) with equation (33) to obtain:

since γ is an arbitrary vector, then

{(I - A^{+} A)}^{T} Q^{c} (\dot{q}, q) = 0 - - - (37)

Multiplying both sides of formula (29) by (I-A)⁺A) Obtaining:

(I - A^{+} A) M (q) \overset{. .}{q} = (I - A^{+} A) (\hat{Q} (\dot{q}, q) + Q^{c} (\dot{q}, q)) - - - (38)

due to equation (37), the above equation can be rewritten as:

(I - A^{+} A) M (q) \overset{. .}{q} = (I - A^{+} A) \hat{Q} (\dot{q}, q) - - - (39)

combining formulas (24) and (38) to obtain:

[\begin{matrix} (I - A^{+} (\dot{q}, q) A (\dot{q}, q) M (q)) \\ A (\dot{q}, q) \end{matrix}] \overset{. .}{q} = [\begin{matrix} (I - A^{+} A) \hat{Q} (\dot{q}, q) \\ b (\dot{q}, q) \end{matrix}] - - - (40)

defining:

<math> <mrow> <mover> <mi>M</mi> <mo>&OverBar;</mo> </mover> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msup> <mi>A</mi> <mo>+</mo> </msup> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>.</mo> </mover> <mo>,</mo> <mi>q</mi> <mo>)</mo> </mrow> <mi>A</mi> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>.</mo> </mover> <mo>,</mo> <mi>q</mi> <mo>)</mo> </mrow> <mi>M</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>)</mo> </mrow> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mi>A</mi> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>.</mo> </mover> <mo>,</mo> <mi>q</mi> <mo>)</mo> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>41</mn> <mo>)</mo> </mrow> </mrow> </math>

by solving (40), we get:

<math> <mrow> <mover> <mi>q</mi> <mrow> <mo>.</mo> <mo>.</mo> </mrow> </mover> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msup> <mi>A</mi> <mo>+</mo> </msup> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>.</mo> </mover> <mo>,</mo> <mi>q</mi> <mo>)</mo> </mrow> <mi>A</mi> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>.</mo> </mover> <mo>,</mo> <mi>q</mi> <mo>)</mo> </mrow> <mo>)</mo> </mrow> <mi>M</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mi>A</mi> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>.</mo> </mover> <mo>,</mo> <mi>q</mi> <mo>)</mo> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>+</mo> </msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mover> <mi>Q</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>.</mo> </mover> <mo>,</mo> <mi>q</mi> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mi>b</mi> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>.</mo> </mover> <mo>,</mo> <mi>q</mi> <mo>)</mo> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>+</mo> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msup> <mover> <mi>M</mi> <mo>&OverBar;</mo> </mover> <mo>+</mo> </msup> <mover> <mi>M</mi> <mo>&OverBar;</mo> </mover> <mo>)</mo> </mrow> <mi>η</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>42</mn> <mo>)</mo> </mrow> </mrow> </math>

where η is an arbitrary vector. If it is notAnd if the system is full, the dynamic model of the system is as follows:

\overset{. .}{q} = {[\begin{matrix} (I - A^{+} (\dot{q}, q) A (\dot{q}, q)) M (q) \\ A (\dot{q}, q) \end{matrix}]}^{+} [\begin{matrix} \hat{Q} (\dot{q}, q) \\ b (\dot{q}, q) \end{matrix}] - - - (43)

formula (43) is that the multi-finger robot dynamics analysis model is a multi-finger robot dynamics analysis model under the pure rolling constraint condition between the fingers and the grabbed object, wherein:

q is a system generalized coordinate vector;is a system generalized velocity vector;

is a system generalized acceleration vector; i is an identity matrix;

m (q) is a system quality matrix;

constraining a second order expression coefficient matrix for the system;

a second order expression constant term matrix is constrained for the system;

is the system external force vector.

On the basis of the dynamic model, further, an analytic model for obtaining the constraint force of the system is as follows:

Q^{c} (\dot{q}, q) = Q (\dot{q}, q) - \hat{Q} (\dot{q}, q) - - - (44)

wherein,

{Q (\dot{q}, q) = M (q) [\begin{matrix} (I - A^{+} (\dot{q}, q) A (\dot{q}, q)) M (q) \\ A (\dot{q}, q) \end{matrix}]}^{+} [\begin{matrix} \hat{Q} (\dot{q}, q) \\ b (\dot{q}, q) \end{matrix}] - - - (45)

the analytic model of the grasping force of the ith contact point is as follows:

F_{i}^{c} (\dot{q}, q) = {(J_{i} (q) J_{i}^{T} (q))}^{- 1} J_{i} (q) Q^{c} (\dot{q}, q) - - - (46)

wherein,

<math> <mrow> <msubsup> <mi>F</mi> <mi>i</mi> <mi>c</mi> </msubsup> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>.</mo> </mover> <mo>,</mo> <mi>q</mi> <mo>)</mo> </mrow> <mo>&Element;</mo> <msup> <mi>R</mi> <mn>2</mn> </msup> <mo>,</mo> <msubsup> <mi>F</mi> <mi>i</mi> <mi>c</mi> </msubsup> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>.</mo> </mover> <mo>,</mo> <mi>q</mi> <mo>)</mo> </mrow> <mo>=</mo> <msup> <mrow> <mo>[</mo> <msubsup> <mi>F</mi> <mi>ix</mi> <mi>c</mi> </msubsup> <mo>,</mo> <msubsup> <mi>F</mi> <mi>iy</mi> <mi>c</mi> </msubsup> <mo>]</mo> </mrow> <mi>T</mi> </msup> <mo>,</mo> <msub> <mi>J</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>q</mi> <mo>)</mo> </mrow> <mo>&Element;</mo> <msup> <mi>R</mi> <mrow> <mn>2</mn> <mo>×</mo> <mrow> <mo>(</mo> <mn>5</mn> <mi>l</mi> <mo>+</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow> </msup> </mrow> </math>

and the jacobian matrix is the contact point of the ith finger and the grabbed object.

Simulation application example 1:

according to the multi-finger robot dynamics analytic model provided by the invention, the specific structure is shown in fig. 1 and fig. 2, a three-finger robot dynamics analytic model is established, numerical simulation is carried out on the mathematical model in Matlab software, and specific parameters used in the three-finger robot simulation are as follows:

table 1 simulation parameter settings

In table, m_ijIs the mass of the jth knuckle of the ith finger,/_ijIs the length of the jth knuckle of the ith finger, I_ijFor the ith finger, the moment of inertia, ρ, of the jth knuckle about the centroid_iFor the ith finger tip surface with respect to coordinate system { f_iRadius of curvature of }, m_oTo capture the mass of the object, I_oFor grasping moment of inertia of object about center of mass, r_oTo obtain the radius of curvature of the surface of the object with respect to the coordinate system { O }, i is 1, 2, 3, and j is 1, 2, 3.

In the simulation process, a set of initial states is given as:

1) the initial displacement vector q ═ pi/6, pi/3, — pi/6, pi/3, pi/6, 0, 0, 0, pi/5, 7 pi/10, 2 pi/5]^T；

2) Initial velocity vector

\dot{q} = {[0.1,0.1,0.1,0.1,0,0,0,0,0,0,0,0,0,0,0,0.1,0,0.1,0,0]}^{T};

3) Initial acceleration vector

\overset{. .}{q} = {[0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0]}^{T};

4) The position parameters of the finger base in the base coordinate system { B } are: b is₁＝(0.75，-0.29)，

B₂＝(-1.5，-0.3)，B₃＝(0.75，-0.1)。

Fig. 3-8 are numerical simulation results of a three-finger robot dynamics analytic model. FIGS. 3-5 show the simulation results of the torque values of each joint of three fingers under the condition of satisfying the pure rolling constraintMoment of j knuckle of ith finger; FIGS. 6-8 show the results of numerical simulation of the grasping forces of three fingers under the constraint of pure scrolling, in whichThe fingertip removal force of the ith finger is X_BComponent of direction, in the figureThe finger tip grasping force of the ith finger is Y_BThe component of the direction.

As can be seen from fig. 3 to 8, the three-finger robot dynamics analysis model proposed by the present invention has continuous and convergent numerical simulation results, and is effective and feasible.

Claims

1. A multi-finger robot dynamics analysis model is characterized in that: the multi-finger robot dynamics analysis model is a dynamics model under the pure rolling constraint condition between fingers and a captured object, and the specific model is as follows:

in the formula:

is a system generalized acceleration vector; i is an identity matrix;

m (q) is a system quality matrix;

constraining a second order expression coefficient matrix for the system;

a second order expression constant term matrix is constrained for the system;

is the system external force vector.

2. A modeling method of a multi-finger robot dynamics analysis model is characterized by comprising the following steps: the method comprises the following steps:

step seven, firstly, establishing a system constraint expression to obtain the relationship between the system virtual displacement and the constraint condition;

secondly, solving a common solution of the virtual displacement vector, and substituting the common solution into a virtual work principle expression to obtain a relational expression capable of simplifying the constraint force in the dynamic model;

then, a motion equation of the simplified relational processing system without constraint is adopted, and a quality supplementary matrix replacing a singular quality matrix is constructed by combining a processing result and a constraint expression;

finally, the finger robot dynamics analysis model when the quality matrix of the system is different is solved by using the quality supplement matrix, and the model with the solution when the quality matrix is different is obtained as follows:

in the formula:

is a system generalized acceleration vector; i is an identity matrix;

m (q) is a system quality matrix;

constraining a second order expression coefficient matrix for the system;

a second order expression constant term matrix is constrained for the system;

is the system external force vector.