CN113305809B - Force sense simulation control method of fully-constrained space rope driving parallel mechanism - Google Patents

Abstract

The invention discloses a force sense simulation control method of a fully constrained space rope driven parallel mechanism, which comprises the following steps: step one, determining a motion calculation method; step two, solving a Jacobian matrix; step three, determining a tension distribution method; step four, establishing a dynamic model of the mechanism when the elasticity of the rope is considered; step five, building a closed-loop motion control frame; step six, determining a force sense simulation control scheme; and step seven, completing the construction of the overall control framework. The control frame of the invention is composed of three parts, which can ensure the good position tracking of the mobile platform, ensure the small and adjustable tension on the rope, and increase the man-machine interaction function of the rope parallel mechanism. In the specific control scheme, the dynamic performance of the mechanism is improved by introducing the internal force coordination ring, and the interaction with people is realized by introducing the force sense simulation ring. The force sensing simulation ring can be used or not according to the use requirement, and the rest two parts still have a good control effect.

Description

A force-sensing simulation control method for a fully constrained space rope-driven parallel mechanism

technical field

The invention belongs to the technical field of control, and relates to a force-sensing simulation control method of a fully constrained space rope-driven parallel mechanism.

Background technique

The rope-driven parallel mechanism has the advantages of small mechanism mass, large motion space, small motion inertia, adjustable stiffness, and strong load capacity. It is used in many fields, such as radio telescopes, wind tunnel tests, rehabilitation robots, industrial cameras, and handling and assembly. Due to the unidirectional force of the rope, the mechanism needs to realize flexible control of n degrees of freedom, and the number m of ropes should satisfy m ≥ n+1, that is, at least 7 ropes are required to realize the control of six degrees of freedom. In order to ensure the symmetry of the mechanism , most of which use 8 ropes to realize the control of the fully constrained space. The dynamic characteristics of the mechanism are involved in the control of the rope-driven parallel mechanism. At the same time, the mechanism is a redundant constraint, and it is also crucial to adjust the internal force of the rope. Secondly, it is also the development trend of the current era to enable the mechanism to interact with people, which puts forward higher requirements for the control framework of the rope-driven parallel mechanism. It is very important to study the control framework of the rope-driven parallel mechanism.

SUMMARY OF THE INVENTION

In order to solve the above problems, the present invention provides a force-sensing simulation control method of a fully constrained space rope-driven parallel mechanism. The invention has a simple structure, a large range of motion, and the rope has the characteristics of intrinsic safety. On the one hand, the mechanism can run stably and realize the movement of a given trajectory; on the other hand, it can interact with people and make corresponding responses to external forces It can realize the good control of the two controlled quantities of the position of the mechanism moving platform and the tension of the rope, and can be used to simulate the force feeling of the interaction between people and virtual objects.

The purpose of this invention is to realize through the following technical solutions:

A force-sensing simulation control method for a fully constrained space rope-driven parallel mechanism, comprising the following steps:

Step 1. Determine the motion solution method:

The motion calculation method considers the influence of the guide pulley on the one hand, and solves the relationship between the motor rotation angle and the change of the rope length when solving the hoisting structure, so the rope is divided into two parts for analysis: the part in the working space and the driving system In the inner part, for the inner part of the working space, solve the swing angle β _i , the wrapping angle α _i , the coordinates of the rope exit point ^B S _i , and finally get the rope length l _W,i of the working space part; for the inner part of the driving system, through two moments The rope length state is obtained by obtaining the identity relationship to solve the relationship between the motor rotation angle and the rope length variation, where:

(1) The swing angle β _i is expressed as:

In the formula, ^B a _i,x is the component of the moving hinge point vector on the x-axis of the global coordinate system; ^B a _i,y is the component of the moving hinge point vector on the y-axis of the global coordinate system; ^B b _i,x is the swing of the guide pulley The component of the tangent point vector between the axis and the edge of the guide pulley on the x-axis of the global coordinate system; ^B b _i,y is the component of the tangent point vector between the swing axis of the guide pulley and the edge of the guide pulley on the y-axis of the global coordinate system;

(2) The wrap angle α _i is expressed as:

α _i =π-(α _i,1 +α _i,2 );

In the formula, ^B m _i is the vector of the center of the guide pulley in the global coordinate system; ^B m _i,z is the component of the center vector of the guide pulley in the z-axis of the global coordinate system; ^B a _i is the dynamic hinge point in the global coordinate system. vector; ^B a _i,z is the component of the dynamic hinge point vector on the z-axis of the global coordinate system; ρ is the radius of the guide pulley.

(3) The coordinate ^B S _i of the rope exit point is expressed as:

In the formula, ^B m _i,x is the component of the center vector of the guide pulley on the x-axis of the global coordinate system; ^B m _i,y is the component of the center vector of the guide pulley on the y-axis of the global coordinate system;

(4) The rope length l _W,i of the working space part is expressed as:

l _W,i =q _P +l _AS =ρ·α _i +|| ^B a _i - ^B S _i || ₂ ;

In the formula, q _P is the length of the rope wrapped on the guide pulley; l _AS is the length of the rope between the rope exit point of the guide pulley and the moving hinge point;

(5) The relationship between motor rotation angle and rope length variation is expressed as:

为电机转角；v_PG为电机和卷扬之间的传动比；r为卷扬半径；In the formula, Δl _W is the effective variation of the rope; h is the rope winding pitch;

is the motor rotation angle; v _PG is the transmission ratio between the motor and the winch; r is the winch radius;

Step 2. Solve the Jacobian matrix:

In the formula, J is the Jacobian matrix of the mechanism; ^B R _P is the rotation matrix of the pose change between the global coordinate system and the body coordinate system; _ln is the unit rope length vector; ^P a _i is the dynamic hinge point in the body coordinate system. A vector in , ^B a _i = ^B R _P ^P a _i ;

Step 3. Determine the tension distribution method:

Tension distribution of the rope using the Closed-form theory:

f _is =-J ^+T w+f _m -J ^+T J ^T f _m ;

In the formula, f _is the rope tension; J ^{+ T} is the pseudo-inverse of the Jacobian matrix; w is the external force; f _m is the average value of the maximum and minimum values of the tension range;

Step 4. Establish the dynamic model of the mechanism when considering the elasticity of the rope:

Assuming that the connections between the rope and the mobile platform, the rope and the guide pulley, and the rope and the hoist are all ideal, based on the above assumptions, the dynamic modeling includes the mobile platform, the hoist and the guide pulley, and the rope, where:

(1) Using Newton's Euler method to model the mobile platform:

为动平台相对于定平台的转动角速度和角加速度；In the formula, L is the matrix formed by the unit direction vector of each rope; ^P A is the coordinate matrix of the hinge point of the moving platform; E is the 3×3 unit matrix; I is the inertia matrix of the moving platform relative to the fixed platform; Ω is the antisymmetric matrix; m is the mass of the moving platform; g is the gravitational acceleration vector; f _2i is the rope tension on the platform side; t is the displacement of the moving platform relative to the fixed platform; ω and

is the rotational angular velocity and angular acceleration of the moving platform relative to the fixed platform;

(2) Using the moment balance method to model the hoist and guide pulley:

为第i个电机的转角矢量；C_r为卷扬和滑轮模型的等效阻尼系数矩阵；λ_s为卷扬和滑轮模型的等效半径；f_1i为第i根绳索驱动侧的绳索拉力；T_act,i为驱动器的转矩；where J _r is the equivalent moment of inertia of the hoist and pulley models;

is the rotation angle vector of the ith motor; C _r is the equivalent damping coefficient matrix of the hoist and pulley models; λ _s is the equivalent radius of the hoist and pulley models; f _1i is the rope tension on the driving side of the ith rope; T _act,i is the torque of the driver;

(3) Model the rope as a spring damping model;

为绳索形变速度；In the formula, _{ci is the stiffness of the rope, d i is the damping of the rope; Δl i is the} deformation of the rope;

is the rope deformation speed;

(4) The dynamic mathematical model is:

In the formula, F is the external force acting on the mobile platform;

Step 5. Build a closed-loop motion control framework:

The closed-loop motion control framework includes a joint feedback-based motion control framework and a pose feedback-based motion control framework, wherein:

The joint feedback-based motion control framework and the pose feedback-based motion control framework both include desired pose, mobile platform pose feedback, hoisting angle feedback, internal force coordination, motor drive system, and a dynamic model with elastic ropes ;

In the motion control framework based on joint feedback, the expected change in rope length of each rope is obtained through the inverse kinematics solution from the desired pose, the result is converted into the expected motor rotation angle, and then the joint deviation is obtained by comparing with the actual rotation angle of the feedback. , input into the PID controller to realize position control;

In the motion control framework based on pose feedback, the pose error is obtained by comparing the desired pose and the model measured pose, and the rope length error can be obtained according to the difference between the rope length error and the pose error by a Jacobian matrix. The conversion of the task space error to the joint space error, and then the angle error of the motor is obtained and input into the PD controller to realize the position control;

In the inner force coordination loop, the expected tension is solved according to the tension distribution method, and the force inner loop feedback is formed with the rope tension measured by the model.

Step 6: Determine the force-sensing simulation control scheme:

The whole mechanism is equivalent to a mass-spring-damping system by using a force-sensing simulation ring, so as to obtain the transfer function between force and motion, so that the external force can be converted into the position correction value of the platform, and the physical characteristics of the mechanism can be changed by setting different parameter combinations. That is, the mobile platform of the mechanism produces different motion state outputs under the action of the external force F, among which:

The transfer function between force and motion is expressed as:

where M _d is the equivalent mass; D _d is the equivalent damping; K _d is the equivalent stiffness; F is the external force acting on the mobile platform; E is the displacement correction of the mobile platform;

Step 7. Complete the construction of the overall control framework:

The overall control framework includes at least two parts: a closed-loop position of the mobile platform (two kinds of frameworks) and an internal force coordination loop; if the parallel mechanism needs to realize force-sensing simulation, the overall control framework also includes a force-sensing simulation loop, and step 8 is performed;

Step 8: Simulate different force senses to verify the feasibility of human-computer interaction.

Compared with the prior art, the present invention has the following advantages:

The control frame of the present invention is composed of three parts, which can ensure the position tracking of the mobile platform is good, and can also ensure that the tension on the rope is small and adjustable, and at the same time, the human-computer interaction function of the rope parallel mechanism is increased. In the specific control scheme, the dynamic performance of the mechanism is improved by introducing an internal force coordination loop, and the interaction with humans is realized by introducing a force-sensing simulation loop. Among them, the force-sensing simulation ring can be selected according to the needs of use, and the other two parts still have a good control effect.

Description of drawings

Figure 1 is a schematic diagram of the overall mechanism;

Fig. 2 is a partial enlarged view of Fig. 1;

Figure 3 is a schematic diagram of a rope closure vector in the workspace;

Figure 4 is a schematic diagram of a rope drive;

Figure 5 is a schematic diagram of the installation of the hoisting mechanism;

Figure 6 is a block diagram of a dynamic model;

Figure 7 shows the overall control framework based on pose feedback;

Figure 8 shows the overall control framework based on joint feedback.

Detailed ways

The technical solutions of the present invention will be further described below in conjunction with the accompanying drawings, but are not limited thereto. Any modification or equivalent replacement of the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention shall be included in the present invention. within the scope of protection.

The invention is a parallel mechanism driven by eight ropes. The parallel mechanism is connected to the hoist through the motor, the rope is wound on the hoist, and the rope is guided by the guide pulley and finally connected to the end effector, that is, the mobile platform, and the end effector is completed by controlling the rotation angle of the motor. A handle is installed on the mobile platform of the parallel mechanism, which is convenient for people to interact with the mechanism. Figures 1 and 2 are schematic diagrams of the overall mechanism, and people can interact with the mechanism by holding the handle to move the platform. Figure 4 is a schematic diagram of a complete transmission of a rope, and Figure 5 is a schematic diagram of the installation of the hoisting mechanism. Specific steps are as follows:

Step 1: Determine the motion solution method.

The guide pulley of the parallel mechanism will affect the length of the rope, and the transmission structure of the parallel mechanism will cause the length of the rope loosened by the hoist and the length of the rope released at the end to be different. Therefore, the present invention will consider the influence of the guide pulley on the one hand, and the other The relationship between the motor rotation angle and the rope length variation when the structure is solved.

The schematic diagram of the rope transmission shown in Figure 4 is divided into two parts for analysis: the inner part of the working space (the hoisting rope point C - the tangent point B between the swing shaft of the guide pulley and the edge of the guide pulley) and the inner part of the drive system (the swing shaft of the guide pulley) Tangent point B to the edge of the guide pulley - moving hinge point A).

1. The inner part of the working space: As shown in Figure 3, the radius of the pulley in the known working space is ρ. Considering the influence of the pulley on the rope length, the length of the rope in the working space is decomposed into two parts to calculate: the first part is wrapped in the pulley. The length above is the arc length between the two tangent points of the rope and the pulley, and the second part is the length between the rope point and the connection point of the moving platform.

(1) Solve the swing angle β _i :

In the formula, ^B a _i,x is the component of the moving hinge point vector on the x-axis of the global coordinate system; ^B a _i,y is the component of the moving hinge point vector on the y-axis of the global coordinate system; ^B b _i,x is the swing of the guide pulley The component of the tangent point vector between the axis and the edge of the guide pulley on the x-axis of the global coordinate system; ^B b _i,y is the component of the tangent point vector between the swing axis of the guide pulley and the edge of the guide pulley on the y-axis of the global coordinate system.

(2) Solve the wrap angle α _i :

为导向滑轮关于摆动角的旋转矩阵；e_x为H坐标系中沿x轴正向的单位向量，e_x＝[1 0 0]^T。In the formula,

is the rotation matrix of the guide pulley about the swing angle; e _x is the unit vector along the positive x-axis in the H coordinate system, e _x =[1 0 0] ^T .

Then the wrap angle α _i can be expressed as α _i =π-(α _i,1 +α _i,2 ).

in,

(3) Solve the rope point coordinates ^B S _i :

(4) Solve the part of the rope length l _W,i in the workspace:

Adding the lengths of the two parts, l _W,i =q _P +l _AS =ρ·α _i +|| ^B a _i - ^B S _i || ₂ .

In the formula, q _P is the length of the rope wrapped on the guide pulley; l _AS is the length of the rope between the rope exit point of the guide pulley and the moving hinge point.

和绳索缠绕螺距h求得：2. Internal part of the drive system: As shown in Figure 4, s is the length of the straight rope between the pulley in the drive system and the guide pulley in the working space, which can be adjusted by the hoisting angle

And the rope winding pitch h is obtained:

The length of the rope loosened by the winch is:

加绳索有效变化量Δl_W等于松开前驱动系统内部长度加即将松开的长度，如下式：In this transmission form, the internal length of the rear drive system is released

The effective change Δl _W of the rope is equal to the internal length of the drive system before loosening plus the length to be loosened, as follows:

In the formula, c _A and c _B are shown in Figure 4 (c _A is the rope length from the point of hoisting the rope to the tangent point between the rope and the left side of the pulley γ, c _B is the length of the rope wrapped on the pulley γ); l _W0,i is the original length of the rope in the workspace.

Arranged:

Step 2: Solve the Jacobian matrix.

The Jacobian matrix can be used to describe the relationship between the speed of the joint variable of the mechanism and the speed of the moving platform, as well as the relationship between the joint force and the force of the end moving platform, so solve the Jacobian matrix to assign the rope tension and the joint variable to the end variable Conversion lays the foundation.

两边同时求导并化简得：

pair

Differentiate both sides and simplify to get:

Ignoring less influential terms, the above formula can be simplified to:

Step 3: Determine the tension distribution method.

The rope on the parallel mechanism has the characteristics of elasticity and one-way force. When the mobile platform moves, the rope should always be in a tensioned state. For a full restraint mechanism, the rope should be tensioned to ensure that the tension on the rope is within the desired range. Inside.

Tension distribution of the rope using the Closed-form theory:

f=-J ^{+ Tw} + _fm -J ⁺ _TJTfm .

In the formula, f is the rope tension; J ^{+ T} is the pseudo-inverse of the Jacobian matrix; w is the external force; f _m is the average value of the maximum and minimum values of the tension range.

Step 4: Build a dynamic model of the mechanism considering the elasticity of the rope.

The rope-driven parallel mechanism studied in the present invention does not need to bear a large load, and the mass of the mobile platform is relatively light. It is assumed that the connections between the rope and the moving platform, the rope and the guide pulley, and the rope and the hoist are all ideal, that is, there is no friction in the mechanism. Based on the above assumptions, the dynamic modeling includes the moving platform, hoisting and guiding pulleys, and ropes.

Dynamics of mobile platform: establish the dynamic model of the translation process of the end platform based on Newton's law, and establish the dynamic model of the rotation process of the mobile platform based on Euler's equation.

Arranged:

第i个电机的转角矢量，规定使绳索伸长的方向为正方向；C_r为卷扬和滑轮模型的等效阻尼系数矩阵；λ_s为卷扬和滑轮模型的等效半径；f_1i为第i根绳索驱动侧的绳索拉力；T_act,i为驱动器的转矩。则由力矩平衡得：Hoist and guide sheave dynamics: Assume J _r to be the equivalent moment of inertia of the hoist and sheave models;

The rotation angle vector of the i-th motor specifies that the direction of rope elongation is the positive direction; C _r is the equivalent damping coefficient matrix of the hoist and pulley models; λ _s is the equivalent radius of the hoist and pulley models; f _1i is the The rope tension on the drive side of the i-th rope; T _act,i is the torque of the drive. Then from the moment balance:

Rope dynamics: The rope is equivalent to a spring damping model, and the stiffness of the rope is set as _{ci and the damping of the rope as d i ,} there are:

Complete dynamics: When the mechanism interacts with the user, assuming that the force exerted by the user is F, the overall dynamic mathematical model at this time is,

The dynamic simulation framework built is shown in Figure 6.

Step 5: Build a closed-loop motion control framework.

The present invention provides two control frameworks: a motion control framework based on position and attitude feedback, and a motion control framework based on joint feedback. The two frameworks include desired pose, mobile platform pose feedback, hoisting angle feedback, internal force coordination, motor drive system, and dynamic models with elastic ropes, wherein:

The joint space control method obtains the expected change of each rope length from the expected pose and kinematic inverse solution, converts the result into the expected motor rotation angle, and then compares it with the feedback actual rotation angle to obtain the joint deviation, which is input into the PID controller. Realize position control;

The pose closed loop is a task space control method. The pose error is obtained by comparing the desired pose and the model measured pose. According to the difference between the rope length error and the pose error by a Jacobian matrix, the rope length error can be obtained. The conversion to the joint space error, and then the angle error of the motor is obtained and input into the PD controller to realize the position control;

The control framework also includes a rope tension control method, that is, an internal force coordination loop, which solves the expected tension according to the tension distribution method, and forms a force inner loop feedback with the rope tension measured by the model.

In the present invention, the motion control framework based on joint feedback is shown in Figure 7. The desired pose is obtained through the inverse kinematics solution to obtain the desired change Δl of each rope length, and is converted into the desired motor according to the result obtained in step 1. The rotation angle is compared with the actual rotation angle of the feedback to obtain the joint deviation Δθ, which is input into the PID controller, and the tension control adopts the tension distribution method described in step 3 to form a force inner loop feedback.

In the present invention, the motion control framework based on pose feedback is shown in Figure 8. The pose deviation Δq can be obtained by comparing the expected pose and the measured pose. According to the difference between the rope length error and the pose error, a Jacobian matrix can be The rope length error Δl=J·Δq is obtained, and the conversion from the task space error to the joint space error is completed, and then the angle error of the motor can be obtained and input into the PD controller to realize the position control, and the tension control adopts the step 3. The tension distribution method constitutes an inner loop feedback of force.

In the present invention, the above two kinds of control both control the movement of the mechanism by outputting the driving torque of the motor driving system.

Step 6: Determine the force-sensing simulation control scheme.

On the basis of realizing motion control, the present invention also provides a control scheme for human-computer interaction. The whole mechanism is equivalent to a mass-spring-damping system by using a force-sensing simulation ring, and the mechanism is changed by setting different parameter combinations. The physical characteristics of the mechanism, that is, the mobile platform of the mechanism can produce different motion state outputs under the action of the external force F. The transfer function between force and motion is:

The external force can be converted into the position correction amount of the platform.

In the present invention, a rope tension sensor is used. When the mobile platform is subjected to external force, the tension on the rope will inevitably change. Through Jacobian matrix and force analysis, the measured rope tension is converted into external force and input to the force sense. in the controller of the analog loop.

The resultant force w _is of the forces other than the rope tension can be expressed as:

w _is =-J ^T f _is .

where f _is the rope tension.

以及重力w_init，外力w_adm表示为：After force analysis, the inertial force is canceled

and the gravity w _init , the external force w _adm is expressed as:

Step 7: Complete the construction of the overall control framework.

The overall control framework includes three parts of the mobile platform position closed loop (two kinds of frameworks), the internal force coordination loop, and the force sense simulation loop, as shown in Figure 7 and Figure 8. If the mechanism does not need to achieve force-sensing simulation, it is only necessary to remove the force-sensing simulation ring in the control frame, and the remaining parts can still achieve good control of the mechanism.

Step 8: Simulate different force senses to verify the feasibility of human-computer interaction.

By simulating several parameter setting situations, it can be compared to the force feeling when a person interacts with a certain object in reality, for example, it can simulate the force feeling of pushing a wall, pushing a box, shooting a ball, etc. The specific simulation situation is as follows:

When the stiffness, damping and inertia all exist, the mobile platform can present a damped oscillation under the action of an instantaneous thrust and finally stabilize at the equilibrium position; when the damping is 0, the mobile platform can exhibit an elastic effect, and people can feel like a touch The force feeling of the spring, the platform can be used to simulate a ball; when the stiffness and inertia are both 0, the mobile platform can follow the external force, and when the external force is removed at a certain time, the mobile platform can stop at the point where the force disappears. position, people can feel the force feeling like pushing a very light object from one position to another, at this time the platform can be used to simulate the box; when the stiffness is 0, the moving platform acts on an instantaneous thrust The bottom will gradually go further until it stops, and people can feel the force feeling like pushing a boat on the water away. At this time, the platform can be used to simulate the boat on the water. When the infinite stiffness and damping are set at a certain position, the platform will not be moved under the action of people continuously increasing the external force, and people can feel the force when it hits the wall. At this time, the platform will not be moved. Can be used to simulate walls.

Claims (6)

1. A force sense simulation control method of a rope-driven parallel mechanism in a fully-constrained space is characterized by comprising the following steps of:

step one, determining a motion calculation method:

the rope was divided into two parts for analysis: the working space inner part and the driving system inner part solve the swing angle beta for the working space inner part_iAngle of wrap alpha_iCoordinates of rope outlet point^BS_iFinally, the length l of the rope of the working space part is obtained_W,i(ii) a For the part in the driving system, obtaining an identity relation through the rope length states at two moments to solve the relation between the motor rotation angle and the rope length variation;

step two, solving a Jacobian matrix:

the jacobian matrix is represented as:

J＝[l_n ^T (^BR_P ^Pa_i×l_n)^T]；

wherein J is a Jacobian matrix of the mechanism;^BR_Pa rotation matrix of pose change between a global coordinate system and a body coordinate system; l_nIs a unit rope length vector;^Pa_iis the vector of the movable hinge point in the body coordinate system,^Ba_i＝^BR_P ^Pa_i；

step three, determining a tension distribution method:

the ropes were tension distributed using Closed-form theory:

f_is＝-J^+Tw+f_m-J^+TJ^Tf_m；

in the formula (f)_isIs the rope tension; j. the design is a square^+TIs the pseudo-inverse of the Jacobian matrix; w is an external force; f. of_mThe average value of the maximum value and the minimum value of the tension range;

step four, establishing a dynamic model of the mechanism in consideration of the elasticity of the rope:

assuming that the connections between the ropes and the moving platform, between the ropes and the guide pulleys, and between the ropes and the winches are all in an ideal state, the mathematical model of the dynamics is expressed as:

wherein L is a matrix formed by unit direction vectors of each rope;^Pa is a movable platform hinge point coordinate matrix; c. C_iIs the stiffness of the rope; d_iDamping for the rope; Δ l_iIs the deformation amount of the rope;

the rope deformation speed; e is a 3 × 3 identity matrix; i is an inertia matrix of the movable platform relative to the fixed platform; omega is an antisymmetric matrix; m is the mobile platform mass; g is a gravity acceleration vector; t is the moving displacement of the movable platform relative to the fixed platform; ω and

the rotating angular velocity and the angular acceleration of the movable platform relative to the fixed platform are obtained; f is an external force acting on the mobile platform;

step five, building a closed-loop motion control framework:

the closed-loop motion control framework comprises a joint feedback-based motion control framework and a pose feedback-based motion control framework, wherein: the motion control frame based on joint feedback and the motion control frame based on pose feedback both comprise an expected pose, moving platform pose feedback, winch corner feedback, internal force coordination, a motor driving system and a dynamic model with an elastic rope;

step six, determining a force sense simulation control scheme:

the whole mechanism is equivalent to a mass-spring-damping system by adopting a force sense simulation ring, so that a transfer function between force and motion is obtained, external force is converted into position correction of a platform, and the physical characteristics of the mechanism are changed by setting different parameter combinations, namely the moving platform of the mechanism is subjected to external force F_extDifferent motion state outputs are generated under the action;

and seventhly, completing the construction of an integral control framework:

the overall control frame comprises at least two parts: a mobile platform position closed loop and an internal force coordination loop.

2. The method for simulating and controlling the force sense of a rope-driven parallel mechanism in a fully constrained space as claimed in claim 1, wherein in the first step, the swing angle β is_iAngle of wrap alpha_iCoordinates of rope outlet point^BS_iLength of rope in working space_W,iAnd the relation between the motor rotation angle and the rope length variation is expressed as follows:

(1) angle of oscillation beta_i：

In the formula,^Ba_i,xthe component of the dynamic hinge point vector on the x axis of the global coordinate system is shown;^Ba_i,ythe component of the dynamic hinge point vector on the y axis of the global coordinate system is shown;^Bb_i,xthe component of the vector of the tangent point of the swing axis of the guide pulley and the edge of the guide pulley on the x axis of the global coordinate system is shown;^Bb_i,ythe component of the vector of the oscillating axis of the guide pulley and the tangent point of the edge of the guide pulley on the y axis of the global coordinate system;

(2) wrap angle alpha_i：

α_i＝π-(α_i,1+α_i,2)；

In the formula,^Bm_ithe vector of the center of the guide pulley in the global coordinate system is taken as the vector;^Bm_i,zis the component of the guide pulley center vector on the z axis of the global coordinate system;^Ba_ithe vector of the movable hinge point in the global coordinate system is obtained;^Ba_i,zfor the vector of the pivot point in the z-axis of the global coordinate systemA component; rho is the radius of the guide pulley;

(3) coordinates of rope outlet point^BS_i：

In the formula,^Bm_i,xis the component of the guide pulley center vector on the x axis of the global coordinate system;^Bm_i,ythe component of the guide pulley center vector on the y axis of the global coordinate system is shown;

(4) rope length l of working space part_W,i：

l_W,i＝q_P+l_AS＝ρ·α_i+||^Ba_i-^BS_i||₂；

In the formula, q_PThe length of the rope wrapped on the guide pulley; l_ASThe length of the rope between the rope outlet point of the guide pulley and the movable hinge point is determined;

(5) the relationship between the motor rotation angle and the rope length variation is as follows:

in the formula,. DELTA.l_WIs the effective variation of the rope; h is the winding pitch of the rope;

is the motor corner; v. of_PGThe transmission ratio between the motor and the winch is set; r is the hoisting radius.

3. The force sense simulation control method of the fully constrained space rope driven parallel mechanism according to claim 1, wherein in the fourth step, the dynamic modeling comprises a moving platform, a hoisting and guiding pulley and a rope, wherein:

(1) adopting a Newton Euler method to model a mobile platform:

in the formula (f)_2iIs the platform side rope tension;

(2) a moment balance method is adopted to model a hoisting and guide pulley:

in the formula, J_rEquivalent moment of inertia of the winch model and the pulley model;

the rotation angle vector of the ith motor; c_rAn equivalent damping coefficient matrix of a winch model and a pulley model; lambda [ alpha ]_sThe equivalent radius of the winch model and the pulley model; f. of_1iThe rope tension of the ith rope driving side; t is_act,iIs the torque of the drive;

(3) modeling the rope as a spring damping model;

4. the force sense analog control method of the fully-constrained space rope-driven parallel mechanism according to claim 1, characterized in that in the fifth step, in a motion control frame based on joint feedback, the expected rope length variation of each rope is obtained by inverse kinematics solution from the expected pose, the result is converted into an expected motor rotation angle, and then the expected motor rotation angle is compared with the fed-back actual rotation angle to obtain joint deviation, and the joint deviation is input into a PID controller to realize position control; in a motion control frame based on pose feedback, comparing an expected pose with a model measurement pose to obtain a pose error, obtaining a rope length error according to a Jacobian matrix which is different from the rope length error and the pose error, realizing the conversion from a task space error to a joint space error, further solving a corner error of a motor and inputting the corner error into a PD controller to realize position control; in the internal force coordination loop, expected tension is solved according to a tension distribution method, and the expected tension and rope tension obtained by model measurement form force internal loop feedback.

5. The method for simulating and controlling the force sense of a rope-driven parallel mechanism in the fully constrained space according to claim 1, wherein in the sixth step, the transfer function between the force and the motion is expressed as:

in the formula, M_dIs equivalent mass; d_dEquivalent damping is adopted; k_dIs the equivalent stiffness; and E is the displacement correction quantity of the movable platform.

6. The force sense simulation control method of the fully-constrained space rope-driven parallel mechanism according to claim 1, wherein in the seventh step, if the parallel mechanism needs to realize force sense simulation, the overall control frame further comprises a force sense simulation ring, and the eighth step is executed;

and step eight, simulating different force senses and verifying the feasibility of human-computer interaction.

Images