Large-angle and high-precision control method for 3-RPS parallel servo mechanism

Abstract

The invention relates to a large-angle and high-precision control method for a 3-RPS parallel servo mechanism. The method comprises the following steps: step 1) establishing a kinematic model of the 3-RPS parallel servo mechanism and performing motion constraint analysis; 2) considering speed feed-forward compensation of the parallel mechanism, and performing attitude closed-loop cooperative control on the basis of the speed feed-forward compensation; and step 3) planning a motion trail of wide-angle attitude change, considering multiple physical constraints, and realizing accurate and rapid attitude control. According to the large-angle and high-precision control method for the 3-RPS parallel servo mechanism, the kinematics of the 3-RPS parallel servo mechanism is analyzed to obtain an analytic equation of a 3-RPS off-axis angle, an azimuth angle and a parallel three-connecting-rod; the control rate based on speed feed-forward and position compensation gives consideration to motion rapidity and accuracy; multi-physical constraint trajectory planning is considered, and the platform performance is brought into full play; and large-angle and high-precision servo control of the 3-RPS parallel mechanism is realized.

Description

Technical Field

[0001] The invention belongs to the technical field of special components and electromechanical assemblies, and in particular relates to a large-angle and high-precision control method for a 3-RPS parallel servo mechanism. Background Art

[0002] Servo platforms for stabilizing attitude or tracking attitude are widely used in radar tracking, disturbance isolation, helicopter attitude simulation and other fields. Such mechanisms are generally composed of upper and lower platforms and an intermediate connecting rod, and the intermediate branches are connected in series or parallel. When the lower platform is in motion, the intermediate branches can be controlled to stabilize the upper platform; when the lower platform is stationary, the branches can be controlled to track the attitude of the upper platform.

[0003] Among them, the parallel mechanism is generally composed of an upper and lower platform and an intermediate connecting rod, and the intermediate branches are connected in parallel. When the posture of the lower platform is known, the branches can be controlled to achieve the desired posture angle control of the upper platform. The parallel servo mechanism has the following advantages over the serial servo mechanism:

[0004] 1) The upper platform is supported by multiple branches, with high rigidity and good structural stability;

[0005] 2) High load-bearing capacity at the same dead weight and volume;

[0006] 3) The parallel mechanism has no cumulative error and can achieve high-precision control;

[0007] 4) Low power demand and good dynamic response.

[0008] 5) The driving device can be placed on the lower platform to facilitate circuit design.

[0009] Large-angle, high-precision control algorithms are the technical core of the parallel servo mechanism. The upper platform posture of the serial mechanism is decoupled, and each degree of freedom can be independently controlled. However, each joint of the parallel mechanism will affect the upper platform posture, and the synergy of the parallel joints determines the upper platform posture. 3-RPS is a mechanical structure consisting of three drive rods and upper and lower platforms. Each drive rod contains a rotary joint (Rotate), a prismatic joint (Prismatic) and a spherical joint (Spherical). The 3-RPS parallel servo mechanism has few actuators, large motion angles, and severe angle coupling, making high-precision posture collaborative control difficult.

[0010] In general, the precise tracking of the attitude of parallel servo mechanisms in existing research is mainly concentrated within a small angle (20°). When the angle changes greatly, the coupling between the three-axis angles of the mechanism is very intense, and the nonlinear characteristics of the rod length and the three-axis angle are obvious, making it difficult to accurately and real-timely coordinate tracking control. This invention explores the kinematics, precise control and planning methods of the 3-RPS parallel mechanism to solve the key difficulties. Summary of the invention

[0011] The purpose of the present invention is to overcome the shortcomings of the prior art and provide a large-angle, high-precision control method for a 3-RPS parallel servo mechanism. The large-angle and high-precision control target is achieved by analyzing the kinematics of the 3-RPS parallel mechanism, designing a precise posture control method, and planning a compliant trajectory considering physical constraints.

[0012] The present invention solves the technical problem by the following technical solutions:

[0013] A large-angle, high-precision control method for a 3-RPS parallel servo mechanism, the control method comprises the following steps:

[0014] Step 1) Establishing the kinematic model and motion constraint analysis of the 3-RPS parallel mechanism;

[0015] Step 2) consider the velocity feedforward compensation of the parallel mechanism, and perform attitude closed-loop coordinated control based on this;

[0016] Step 3) Plan the motion trajectory of large-angle posture changes, consider multiple physical constraints, and achieve accurate and fast posture control.

[0017] Moreover, the specific process of establishing the kinematic model of the 3-RPS parallel mechanism and the motion constraint analysis in step 1) is as follows:

[0018] 1.1. Define the translation and rotation transformation of the 3-RPS parallel mechanism;

[0019] 1.2. Perform constraint analysis on the 3-RPS parallel mechanism;

[0020] 1.3. Derivation of azimuth and off-axis angle;

[0021] 1.4. Inverse kinematics is used to solve joint variables for known end position.

[0022] Moreover, the process of attitude closed-loop collaborative control in step 2) is:

[0023] According to the three desired rod length trajectories calculated in real time, real-time coordinated rod length control is performed to ensure that the upper platform can still accurately track the desired trajectory under large angle conditions. The control law is as follows:

[0024]

[0025] in, is the speed command function, which consists of two parts. One is The planning speed trajectory function ensures the dynamic rapidity of the movement. The second is the position trajectory function feedback, l(t) is the real-time rod length feedback, l d (t) is the planned rod length trajectory, and k is the controller gain, which ensures the steady-state accuracy of the motion.

[0026] Moreover, the specific process of planning the motion trajectory of large-angle posture changes in step 3) and considering multiple physical constraints to achieve precise and rapid posture control is: designing the speed curve so that the planned curve meets the physical constraints of the motor, such as the motion limit of the rod length, the speed limit, and the acceleration limit, to obtain the posture control curve.

[0027] The beneficial effects of the present invention are:

[0028] 1. The large-angle and high-precision control method of the 3-RPS parallel servo mechanism of the present invention obtains the analytical equations of the 3-RPS off-axis angle, azimuth angle and parallel three-link by analyzing the kinematics of the 3-RPS parallel mechanism; the control rate based on velocity feedforward and position compensation takes into account both the rapidity and accuracy of movement; the trajectory planning considering multiple physical constraints gives full play to the platform performance; and the large-angle and high-precision servo control of the 3-RPS parallel mechanism is realized. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 It is a schematic diagram of the composition of the 3-RPS parallel mechanism;

[0030] Figure 2 It is the coordinate diagram of 3-RPS parallel mechanism;

[0031] Figure 3 It is a schematic diagram of off-axis angle and azimuth angle;

[0032] Figure 4 It is a smooth trajectory planning map;

[0033] Figure 5 It is a flow chart of the present invention.

[0034] Description of reference numerals:

[0035] 1- spherical joint; 2- translation joint; 3- rotation joint, 4- upper plane, 5- lower plane. DETAILED DESCRIPTION

[0036] The present invention is further described in detail below through specific examples. The following examples are only illustrative and not restrictive, and the protection scope of the present invention cannot be limited thereto.

[0037] A large-angle, high-precision control method for a 3-RPS parallel servo mechanism, the control method comprises the following steps:

[0038] Step 1) Establishing the kinematic model and motion constraint analysis of the 3-RPS parallel mechanism;

[0039] 1.1、Define the translation and rotation transformation of the 3-RPS parallel mechanism:

[0040] 3-RPS parallel mechanism schematic diagram Figure 1 As shown, it includes a lower plane 5, an upper plane 4, a spherical joint 1, a translation joint 2 and a rotation joint 3. The spherical joint 1, the translation joint 2 and the rotation joint 3 are arranged between the upper plane 4 and the lower plane 5. The lower plane 5 is A, and the upper plane 4 is B. A1, A2, and A3 are the intersection points of the rotation joint 3 and the translation joint 2 of the lower plane. A is the center of the lower plane and the radius is r A Points B1, B2, and B3 are the intersection points of the spherical joint 1 and the translation joint 2 on the upper plane. B is the center of the circle on the upper plane, and its radius is r B A1B1 is the length of the translation joint rod l1, A2B2 is the length of the translation joint rod l2, and A3B3 is the length of the translation joint rod l3. A X A is the coordinate system X-axis of the lower plane and O A A1 overlaps, such as Figure 1 As shown, the coordinate system is established using the right-hand rule. B X B The X-axis of the coordinate system of the upper plane and O B B1 coincides, and the coordinate system is established using the right-hand rule.

[0041] The lower plane coordinate system A and the upper plane coordinate system B have been defined. The poses of the two are given by the joint space rod length vector q = [l1l2 l3] Τ The pose vector of coordinate system B from the perspective of coordinate system A is defined as p = [xy zαβγ] Τ , which can be regarded as a set of parameters of the coordinate system A after one translation and three rotation transformations.

[0042] Define the intermediate coordinate system A 1 , A 2 , A 3 , the coordinate system transformation process can be expressed as

[0043]

[0044] in

[0045]

[0046] A coordinate system is transformed by translation Become A 1 ; A 1 The coordinate system rotates around its own X axis by α and becomes A 2 Coordinate system, the transformation matrix is A 2 The coordinate system rotates around its own Y axis by β to become A 3 Coordinate system, the transformation matrix is A 3 The coordinate system rotates γ around its own Z axis to become the B coordinate system, and the transformation matrix is From A 1 The three rotation transformations to B are equivalent to one rotation transformation:

[0047]

[0048] 1.2. Constraint analysis of 3-RPS parallel mechanism:

[0049] As shown in the parallel mechanism Figure 1 As shown, since A1, A2, A3 are the centers of the rotation axis, there is only one degree of freedom and it is subject to rotation constraints. The rod length l1 is constrained to the plane O A A1Z A Similarly, the rod length l2 is constrained to the plane O A A2Z A , the rod length l3 is constrained to the plane O A A3Z A .

[0050] In the A coordinate system, the coordinate of A1 is r A (1,0,0), the coordinates of A2 are A3 coordinates are Written in matrix form, the coordinates of points A1, A2, and A3 in the A coordinate system are

[0051]

[0052] Similarly, the coordinates of points B1, B2, and B3 in the B coordinate system are

[0053]

[0054] In the A coordinate system, the coordinates of B1, B2, and B3 are

[0055]

[0056] The three rotation constraints can be specifically expressed as a set of constraint equations:

[0057]

[0058] Expands to be equivalent to

[0059]

[0060] Simplify equation (8) by adding equations 2 and 3 together to obtain

[0061] 2y+3R 12 r B -R 21 r B =0 (9)

[0062] Combining the first formula of formula (8), we can get

[0063] R 12 =R 21 (10)

[0064] That is

[0065] -cosβsinγ=cosαsinγ+cosγsinαsinβ (11)

[0066] It can be seen that due to the multiple constraints and coupling of the parallel servo platform, there are constraint equations for α, β, and γ.

[0067] We can solve γ based on α and β.

[0068]

[0069] is α, β, γ, and then we get matrix.

[0070] According to the first equation of constraint equation (8), we can get y:

[0071] y=-R 21 r B (13)

[0072] The third equation in constraint equation (8) is decomposed into

[0073]

[0074] Solve for x,

[0075]

[0076] In summary, there is a coupling relationship between the five variables α, β, γ, x, and y, and γ, x, and y can be analytically inferred by specifying α and β, that is, (α, β) → (γ, x, y).

[0077] 1.3. Derivation of azimuth and off-axis angle:

[0078] There are only two degrees of freedom among the five variables α, β, γ, x, and y. By specifying α and β, γ, x, and y can be obtained analytically. However, due to the special rotation order, α, β, and γ are not the generally defined roll angle, pitch angle, and yaw angle, and their physical meanings are relatively abstract. In the use scenario of radar, its azimuth angle and off-axis angle have clear physical meanings and are also two degrees of freedom. The schematic diagram is as follows: Figure 2 As shown. In coordinate system A 1 middle, coincides with the radar normal. The azimuth angle θ is the normal at Projection on a plane Angle, off-axis angle is the normal and Angle.

[0079] Define the vertex coordinates of the radar unit normal, that is, B or A 3 The coordinate system's Z-axis basis vector vertex coordinates, in coordinate system A 1 is (x z ,y z ,z z ). When it is known There are the following constraints

[0080]

[0081] Define θ∈[-π,π), Then x z ∈(-1,1),y z ∈(-1,1), z z ∈(0,1], classification discussion has

[0082] when

[0083]

[0084] when

[0085]

[0086] In A 1 In the coordinate system, B or A 3 The Z-axis basis vector of the coordinate system is The third column has

[0087]

[0088] Can be solved

[0089]

[0090] This can be done according to the specified Derived (xz ,y z ,z z ), and then deduce (α, β), and then deduce (γ, x, y).

[0091]

[0092] Right now

[0093] 1.4. Inverse kinematics is used to solve joint variables for known end position.

[0094] Inverse kinematics is how to solve the joint variables when the end position is known, that is, according to (α, β, γ, x, y, z), solve (l1, l2, l3). We get (α, β, γ, x, y), and the z in the translation vector is not constrained by the other variables, so we reassign it. We get 6 transformation parameters (α, β, γ, x, y, z).

[0095] In the previous article, we obtained the coordinates of the six points A1, A2, A3, B1, B2, and B3 in the lower platform coordinate system A. A P A , A P B , and we get the inverse kinematics

[0096]

[0097] in

[0098] l 1x =xr A +r B cosβcosγ

[0099] l 1y =y+r B (cosαsinγ+cosγsinαsinβ)

[0100] l 1z =z+r B (sinαsinγ-cosαcosγsinβ)

[0101]

[0102] Step 2) Consider the velocity feedforward compensation of the parallel mechanism and perform attitude closed-loop coordinated control based on this:

[0103] According to the kinematic model, the three rod lengths of the mechanism determine the relative position of the upper and lower platforms. Therefore, the relative position of the upper and lower platforms can be adjusted by adjusting the rod lengths in real time. On the basis of precise kinematics, the three connecting rod lengths are coordinated and controlled in real time through inverse kinematics solution, so that the mechanism can achieve the desired off-axis and azimuth angles. According to the desired three rod length trajectories calculated in real time, the rod length control is carried out in real time to achieve the upper platform can still accurately track the desired trajectory under large angle conditions. The control law is as follows:

[0104]

[0105] in, is the speed command function, which consists of two parts. One is The planning speed trajectory function ensures the dynamic rapidity of the movement. The second is the position trajectory function feedback, l(t) is the real-time rod length feedback, l d (t) is the planned rod length trajectory, and k is the controller gain, which ensures the steady-state accuracy of the motion.

[0106] Step 3) Plan the motion trajectory of large-angle posture changes, consider multiple physical constraints, and achieve accurate and fast posture control.

[0107] Smooth trajectory planning: At a large angle of the desired posture, the rod length will move a long distance in a short time. The control of the rod length needs to plan a smooth trajectory first, otherwise it is very easy to have a non-differentiable speed, jitter, or even overshoot. The rod length velocity curve is designed according to the first-order differentiable basic function, as shown in equation (25), and then the position curve is integrated to satisfy the second-order differentiability. By considering the physical constraints of the motor, such as the motion limit, speed limit, acceleration limit, etc. of the rod length, a smooth velocity reference trajectory is obtained, as shown in Figure 4 shown.

[0108]

[0109] Although the embodiments and drawings of the present invention are disclosed for illustrative purposes, those skilled in the art will appreciate that various substitutions, changes and modifications are possible without departing from the spirit and scope of the present invention and the appended claims. Therefore, the scope of the present invention is not limited to the contents disclosed in the embodiments and drawings.

Claims

1. A large-angle, high-precision control method for a 3-RPS parallel servo mechanism, characterized in that: The steps of the control method are: Step 1) Establishing the kinematic model and motion constraint analysis of the 3-RPS parallel mechanism; Step 2) consider the velocity feedforward compensation of the parallel mechanism, and perform attitude closed-loop coordinated control based on this; Step 3) Plan the motion trajectory of large-angle posture changes, consider multiple physical constraints, and achieve accurate and fast posture control.

2. The large-angle, high-precision control method of the 3-RPS parallel servo mechanism according to claim 1, characterized in that: The specific process of establishing the kinematic model of the 3-RPS parallel mechanism and the motion constraint analysis in step 1) is as follows: 1.

1. Define the translation and rotation transformation of the 3-RPS parallel mechanism; 1.

2. Perform constraint analysis on the 3-RPS parallel mechanism; 1.

3. Derivation of azimuth and off-axis angle; 1.

4. Inverse kinematics is used to solve joint variables for known end position.

3. The large-angle, high-precision control method of the 3-RPS parallel servo mechanism according to claim 1, characterized in that: The process of attitude closed-loop collaborative control in step 2) is as follows: According to the three desired rod length trajectories calculated in real time, real-time coordinated rod length control is performed to ensure that the upper platform can still accurately track the desired trajectory under large angle conditions. The control law is as follows: in, is the speed command function, which consists of two parts. One is The planning velocity trajectory function ensures the dynamic rapidity of the movement. The second is the position trajectory function feedback, l(t) is the real-time rod length feedback, l d (t) is the planned rod length trajectory, and k is the controller gain, which ensures the steady-state accuracy of the motion.

4. The large-angle, high-precision control method of the 3-RPS parallel servo mechanism according to claim 1, characterized in that: The specific process of planning the motion trajectory of large-angle posture change in step 3) and considering multiple physical constraints to achieve precise and rapid posture control is as follows: designing the speed curve so that the planned curve meets the physical constraints of the motor, such as the motion limit of the rod length, the speed limit, and the acceleration limit, to obtain the posture control curve.

Images