Kinematics positive solution method, system and equipment of six-degree-of-freedom motion platform and medium

Abstract

The invention provides a kinematics positive solution method of a six-degree-of-freedom motion platform, equipment and a storage medium, aiming at the problem of large calculated amount of a traditional method. The method belongs to the technical field of robot control, and comprises the steps that six motion states of the six-degree-of-freedom motion platform include transverse movement, longitudinal movement, lifting, pitching, transverse rolling and yawing, each motion state has three motion directions, and the motion direction of the platform can be obtained. And calculating the motion state of the electric cylinder through 729 platform motion directions, comparing the motion state of the electric cylinder with the known motion state of the electric cylinder, taking the platform motion direction of the motion state of the electric cylinder closest to the to-be-solved state as a solved positive solution value, and carrying out multiple times of loop iteration until an accurate solution in a tolerance range is solved. According to the method, a Newton iteration method does not need to be used for solving, the calculation amount can be reduced by setting the iteration step length, even by setting the stage iteration step length, and the positive solution precision in any tolerance range can be obtained.

Description

Technical Field

[0001] The present invention belongs to the field of robot control technology, and is mainly applied to a six-degree-of-freedom motion platform, and specifically is a kinematics correct solution method, system, equipment and medium for the six-degree-of-freedom motion platform. Background Art

[0002] The six-degree-of-freedom motion platform is a structure composed of six electric cylinders, six universal hinges on the upper and lower platforms, and two platforms. On the basis of fixing the lower platform, the six electric cylinders can be used to complete the movement of the upper platform in six degrees of freedom (X, Y, Z, α, β, γ) in space, including lateral movement, longitudinal movement, lifting, pitch, roll and yaw, so as to simulate various spatial motion postures.

[0003] The correct solution of the six-degree-of-freedom motion platform refers to solving the motion state of the six-degree-of-freedom motion platform when the motion state of the electric cylinder is known. The kinematic correct solution of the traditional six-degree-of-freedom motion platform can only establish a nonlinear equation group and solve it through the Newton iteration method. This method has a large amount of calculation. If the position and posture of the motion platform need to be monitored in real time through the elongation of the electric cylinder, the computer hardware requirements are high, and it may even cause operation jams and freezes. Summary of the invention

[0004] In view of the deficiencies in the prior art, the present invention provides a kinematic correct solution method, system, device and medium for a six-degree-of-freedom motion platform. The present invention does not need to be solved by the Newton iteration method, and the amount of calculation can be reduced by setting the iteration step size, or even by setting a staged iteration step size, to obtain the correct solution accuracy within any tolerance range.

[0005] To achieve the above object, the present invention adopts the following technical solutions:

[0006] A kinematics forward solution method for a six-degree-of-freedom motion platform calculates the motion state of the six-degree-of-freedom motion platform in real time by monitoring the length of an electric cylinder; each time the length of the electric cylinder changes, the following steps are performed:

[0007] Step 1. Input the lengths of the six electric cylinders to be monitored;

[0008] Step 2, calculate the length of the electric cylinder corresponding to the movement direction of the six-degree-of-freedom motion platform by position inverse solution; calculate the difference between the length of the electric cylinder corresponding to the movement direction of the six-degree-of-freedom motion platform and the monitored length of the electric cylinder as the first difference △Ln_i; calculate the difference between the length of the electric cylinder in the initial state and the monitored length of the electric cylinder as the second difference △Ln; calculate the difference between the length of the electric cylinder in the initial state and the length of the electric cylinder corresponding to the movement direction of the six-degree-of-freedom motion platform as the third difference △Ln_0i;

[0009] Step 3, determine whether there is and only one motion direction that satisfies the first condition, wherein the first condition is: the first difference and the second difference have the same positive and negative signs, and the absolute value of the first difference is less than the absolute value of the second difference; if so, output the motion direction of the six-degree-of-freedom motion platform, and use the motion direction as the initial state of the six-degree-of-freedom motion platform in the next calculation round triggered by the change in the length of the electric cylinder, and the electric cylinder length corresponding to the motion direction is used as the initial state electric cylinder length in the next calculation round; if not, proceed to step 4;

[0010] Step 4, determine whether the first difference and the second difference of all electric cylinders have the same positive or negative sign. If so, add up the absolute values ​​of the first differences of the six electric cylinders as the reference value Sum_△Li for determination; add up the absolute values ​​of the second differences of the six electric cylinders as the total electric cylinder length change Sum_△Ln; proceed to step 5; if not, add up the absolute values ​​of the first differences of the electric cylinders whose first difference and the second difference have the same positive or negative sign, and add up the absolute values ​​of the third differences of the electric cylinders whose first difference and the second difference have different positive or negative signs, as the reference value Sum_△Li for determination; add up the absolute values ​​of the second differences of the six electric cylinders as the total electric cylinder length change Sum_△Ln; proceed to step 5;

[0011] Step 5. Select the minimum judgment reference value from all the judgment reference values ​​of the motion directions. Is the minimum judgment reference value less than the total change in the length of the electric cylinder? If so, output the motion direction of the six-degree-of-freedom motion platform corresponding to the minimum judgment reference value, and use the motion direction as the initial state of the six-degree-of-freedom motion platform in the next calculation round triggered by the change in the length of the electric cylinder. The electric cylinder length corresponding to the motion direction is used as the initial state electric cylinder length in the next calculation round; if not, reduce the iteration step size and return to step 2.

[0012] To optimize the above technical solutions, the specific measures taken also include:

[0013] Further, the initial state of the six-degree-of-freedom motion platform is [X0, Y0, Z0, α0, β0, γ0], where X0 represents the initial lateral displacement, Y0 represents the initial longitudinal displacement, Z0 represents the initial lifting, α0 represents the initial pitch, β0 represents the initial roll, and γ0 represents the initial yaw. The length of the electric cylinder corresponding to the motion direction of the six-degree-of-freedom motion platform calculated by position inverse solution is expressed by the formula:

[0014] The lengths of the electric cylinders Ln_i (i=1, 2, ..., 729; n=1, 2, ..., 6) corresponding to 729 motion directions [X0+△X_i, Y0+△Y_i, Z0+△Z_i, α0+△α_i, β0+△β_i, γ0+△γ_i] are calculated respectively by position inversion; i represents the sequence number of the motion direction, and n represents the sequence number of the electric cylinder. In the formula, △X_i represents the offset of the lateral motion state in the i-th motion direction, △X_i=-a or 0 or a; △Y_i represents the sequence number of the i-th motion direction. △Y_i represents the offset of the longitudinal motion state in the i-th motion direction, △Y_i=-a or 0 or a; △Z_i represents the offset of the lifting motion state in the i-th motion direction, △Z_i=-a or 0 or a; △α_i represents the offset of the pitch motion state in the i-th motion direction, △α_i=-a or 0 or a; △β_i represents the offset of the rolling motion state in the i-th motion direction, △β_i=-a or 0 or a; △γ_i represents the offset of the yaw motion state in the i-th motion direction, △γ_i=-a or 0 or a; a represents the iteration step.

[0015] Further, in step 4, the absolute value of the accumulated first difference of the six electric cylinders is expressed by the formula:

[0016]

[0017] In the formula, Sum_△Li represents the judgment reference value, and △Ln_i represents the first difference value of the nth electric cylinder in the i-th motion direction of the six-degree-of-freedom motion platform.

[0018] Further, in step 4, the absolute value of the accumulated second difference of the six electric cylinders is expressed by the formula:

[0019]

[0020] Where Sum_△Ln represents the total electric cylinder length change, and △Ln represents the second difference of the nth electric cylinder.

[0021] The present invention also proposes an electronic device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the kinematics forward solution method of the six-degree-of-freedom motion platform as described above is implemented.

[0022] The present invention further provides a computer-readable storage medium storing a computer program, wherein the computer program enables a computer to execute the kinematics forward solution method of the six-degree-of-freedom motion platform as described above.

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

[0024] The present invention does not need to be solved by the Newton iteration method, and the amount of calculation can be reduced by setting the iteration step size, or even by setting the phased iteration step size, to obtain the correct solution accuracy within any tolerance range. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] Figure 1 Schematic diagram of the six-degree-of-freedom motion platform.

[0026] Figure 2 The following is a flow chart of the correct solution method. DETAILED DESCRIPTION

[0027] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0028] Embodiment 1

[0029] The present invention proposes a kinematic solution method for a six-degree-of-freedom motion platform, which calculates the motion state of the six-degree-of-freedom motion platform in real time by monitoring the length of the electric cylinder; the schematic diagram of the six-degree-of-freedom motion platform is shown in Figure 1 As shown in the figure, the six telescopic structures on the side are electric cylinders. The six motion states of the six-degree-of-freedom motion platform are: lateral displacement △X, longitudinal displacement △Y, lifting △Z, pitch △α, roll △β, yaw △γ. The six motion states are written in vector form forward = [△X, △Y, △Z, △α, △β, △γ]. Each motion state has three motion directions [-a, 0, a] (a is the iteration step length, which is a constant. The iteration accuracy can be changed according to its value setting). 729 (3 6 ) Platform movement direction.

[0030] Each time the length of the electric cylinder changes, perform the following steps. The process is as follows: Figure 2 As shown:

[0031] Step 1. Input the lengths of the six electric cylinders to be monitored;

[0032] Step 2. The initial state of the six-degree-of-freedom motion platform is [X0, Y0, Z0, α0, β0, γ0], where X0 represents the initial lateral displacement, Y0 represents the initial longitudinal displacement, Z0 represents the initial lift, α0 represents the initial pitch, β0 represents the initial roll, and γ0 represents the initial yaw. The length of the electric cylinder corresponding to the motion direction of the six-degree-of-freedom motion platform is calculated by position inverse solution; it can be expressed as:

[0033] The lengths of the electric cylinders Ln_i (i=1, 2, ..., 729; n=1, 2, ..., 6) corresponding to 729 motion directions [X0+△X_i, Y0+△Y_i, Z0+△Z_i, α0+△α_i, β0+△β_i, γ0+△γ_i] are calculated respectively by position inversion; i represents the sequence number of the motion direction, and n represents the sequence number of the electric cylinder. In the formula, △X_i represents the offset of the lateral motion state in the i-th motion direction, △X_i=-a or 0 or a; △Y_i represents the sequence number of the i-th motion direction. △Y_i represents the offset of the longitudinal motion state in the i-th motion direction, △Y_i=-a or 0 or a; △Z_i represents the offset of the lifting motion state in the i-th motion direction, △Z_i=-a or 0 or a; △α_i represents the offset of the pitch motion state in the i-th motion direction, △α_i=-a or 0 or a; △β_i represents the offset of the rolling motion state in the i-th motion direction, △β_i=-a or 0 or a; △γ_i represents the offset of the yaw motion state in the i-th motion direction, △γ_i=-a or 0 or a; a represents the iteration step.

[0034] The difference between the length of the electric cylinder corresponding to the motion direction of the six-degree-of-freedom motion platform and the length of the monitored electric cylinder is calculated as the first difference △Ln_i; specifically:

[0035] Calculate the length difference between the electric cylinder length Ln_i corresponding to the 729 motion directions of the six-degree-of-freedom motion platform and the electric cylinder length Ln in the state to be solved (n = 1, 2, ..., 6): (a positive value requires contraction, and a negative value requires extension)

[0036] △Ln_i=Ln_i-Ln;

[0037] The difference between the length of the electric cylinder in the initial state and the length of the electric cylinder under monitoring is calculated as the second difference △Ln; specifically:

[0038] Calculate the length difference between the initial state electric cylinder length Ln_0 and the electric cylinder length Ln of the state to be solved (n=1, 2, ..., 6); (a positive value requires contraction, and a negative value requires extension)

[0039] △Ln=Ln_0-Ln;

[0040] The difference between the length of the electric cylinder in the initial state and the length of the electric cylinder corresponding to the motion direction of the six-degree-of-freedom motion platform is calculated as the third difference △Ln_0i; specifically:

[0041] Calculate the length difference between the initial state electric cylinder length Ln_0 and the electric cylinder length Ln_i of the 729 motion directions of the six-degree-of-freedom motion platform (n=1, 2, ..., 6);

[0042] △Ln_0i=Ln_0-Ln_i;

[0043] Step 3, determine whether there is and only one motion direction that satisfies the first condition, the first condition being: the first difference and the second difference have the same positive and negative signs, and the absolute value of the first difference is less than the absolute value of the second difference, that is, |△Ln_i|<|△Ln|; if so, output this motion direction of the six-degree-of-freedom motion platform, and use this motion direction as the initial state of the six-degree-of-freedom motion platform in the next calculation round triggered by the change in the length of the electric cylinder, and the electric cylinder length corresponding to this motion direction is used as the initial state electric cylinder length in the next calculation round; if not, proceed to step 4;

[0044] Step 4: Determine whether the first difference and the second difference of all electric cylinders have the same positive and negative signs. If so, add up the absolute values ​​of the first differences of the six electric cylinders as the judgment reference value Sum_△Li; expressed as:

[0045]

[0046] In the formula, Sum_△Li represents the judgment reference value, and △Ln_i represents the first difference value of the nth electric cylinder in the i-th motion direction of the six-degree-of-freedom motion platform.

[0047] The absolute values ​​of the second differences of the six electric cylinders are accumulated as the total electric cylinder length change Sum_△Ln; go to step 5; the absolute values ​​of the second differences of the six electric cylinders are accumulated and expressed by the formula:

[0048]

[0049] Where Sum_△Ln represents the total electric cylinder length change, and △Ln represents the second difference of the nth electric cylinder.

[0050] If not, the absolute value of the first difference of the electric cylinders whose first difference and second difference have the same positive and negative signs is accumulated, and the absolute value of the third difference of the electric cylinders whose first difference and second difference have different positive and negative signs is added to serve as the judgment reference value Sum_△Li; for example, if the △Ln_i and △Ln of the electric cylinders numbered 4, 5, and 6 have different positive and negative signs, then the calculation method of Sum_△Li is:

[0051]

[0052] The absolute values ​​of the second differences of the six electric cylinders are accumulated as the total electric cylinder length change Sum_△Ln; go to step 5;

[0053] Step 5. Select the minimum judgment reference value from all the judgment reference values ​​Sum_△Li in the motion direction. The smaller Sum_△Li is, the closer the motion direction is to the correct solution to be found. Assuming that the minimum value of Sum_△Li is Min_Sum, then Min_Sum should also be less than the cumulative sum of |△Ln| Sum_△Ln. Is the minimum judgment reference value Min_Sum less than the total electric cylinder length change Sum_△Ln? If so, output the motion direction of the six-degree-of-freedom motion platform corresponding to the minimum judgment reference value, and use the motion direction as the initial state of the six-degree-of-freedom motion platform in the next calculation round triggered by the change in the electric cylinder length. The electric cylinder length corresponding to the motion direction is used as the initial state electric cylinder length in the next calculation round; the formula is expressed as:

[0054] X 0＝ X0+△X_i,

[0055] Y 0＝ Y0+△Y_i,

[0056] Z 0＝ Z0+△Z_i,

[0057] α 0＝ α0+△α_i,

[0058] β 0＝ β0+△β_i,

[0059] γ0=γ0+△γ_i,

[0060] Ln_0=Ln_i(n=1, 2,...,6);

[0061] If not, reduce the iteration step size and return to step 2.

[0062] Embodiment 2

[0063] The present invention provides an electronic device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the kinematics correct solution method for a six-degree-of-freedom motion platform as described in the first embodiment is implemented.

[0064] Embodiment 3

[0065] The present invention provides a computer-readable storage medium storing a computer program, wherein the computer program enables a computer to execute the kinematics forward solution method of a six-degree-of-freedom motion platform as described in the first embodiment.

[0066] In the embodiments disclosed in the present application, the computer storage medium can be a tangible medium that can contain or store a program for use by or in conjunction with an instruction execution system, device or equipment. The computer storage medium can include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices or equipment, or any suitable combination of the above. More specific examples of computer storage media can include electrical connections based on one or more lines, portable computer disks, hard disks, random access memories (RAM), read-only memories (ROM), erasable programmable read-only memories (EPROM or flash memory), optical fibers, portable compact disk read-only memories (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the above.

[0067] Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed in this application can be implemented in electronic hardware or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of this application.

[0068] The above are only preferred embodiments of the present invention, and the protection scope of the present invention is not limited to the above embodiments. All technical solutions under the concept of the present invention belong to the protection scope of the present invention. It should be pointed out that for ordinary technicians in this technical field, some improvements and modifications without departing from the principle of the present invention should be regarded as the protection scope of the present invention.

Claims

1. A kinematic solution method for a six-degree-of-freedom motion platform, characterized in that: The motion state of the six-degree-of-freedom motion platform is calculated in real time by monitoring the length of the electric cylinder; each time the length of the electric cylinder changes, the following steps are performed: Step 1. Input the lengths of the six electric cylinders to be monitored; Step 2: Calculate the length of the electric cylinder corresponding to the motion direction of the six-degree-of-freedom motion platform by position inverse solution; calculate the difference between the length of the electric cylinder corresponding to the motion direction of the six-degree-of-freedom motion platform and the monitored length of the electric cylinder as the first difference; Calculating the difference between the length of the electric cylinder in the initial state and the length of the monitored electric cylinder as a second difference; Calculate the difference between the length of the electric cylinder in the initial state and the length of the electric cylinder corresponding to the motion direction of the six-degree-of-freedom motion platform as the third difference; Step 3, determine whether there is and only one motion direction that satisfies the first condition, wherein the first condition is: the first difference and the second difference have the same positive and negative signs, and the absolute value of the first difference is less than the absolute value of the second difference; if so, output the motion direction of the six-degree-of-freedom motion platform, and use the motion direction as the initial state of the six-degree-of-freedom motion platform in the next calculation round triggered by the change in the length of the electric cylinder, and the electric cylinder length corresponding to the motion direction is used as the initial state electric cylinder length in the next calculation round; if not, proceed to step 4; Step 4, determine whether the first difference and the second difference of all electric cylinders have the same positive and negative signs. If so, add up the absolute values ​​of the first differences of the six electric cylinders as a reference value for determination; The absolute values ​​of the second differences of the six electric cylinders are accumulated as the total electric cylinder length change; Go to step 5; if not, add the absolute value of the first difference of the electric cylinder whose first difference and second difference have the same positive and negative signs, and add the absolute value of the third difference of the electric cylinder whose first difference and second difference have different positive and negative signs, as a reference value for judgment; The absolute values ​​of the second differences of the six electric cylinders are accumulated as the total electric cylinder length change; Go to step 5; Step 5. Select the minimum judgment reference value from all the judgment reference values ​​of the motion directions. Is the minimum judgment reference value less than the total change in the length of the electric cylinder? If so, output the motion direction of the six-degree-of-freedom motion platform corresponding to the minimum judgment reference value, and use the motion direction as the initial state of the six-degree-of-freedom motion platform in the next calculation round triggered by the change in the length of the electric cylinder. The electric cylinder length corresponding to the motion direction is used as the initial state electric cylinder length in the next calculation round; if not, reduce the iteration step size and return to step 2.

2. The kinematics correct solution method of the six-degree-of-freedom motion platform as claimed in claim 1, characterized in that: The initial state of the six-degree-of-freedom motion platform is [X0, Y0, Z0, α0, β0, γ0], where X0 represents the initial lateral displacement, Y0 represents the initial longitudinal displacement, Z0 represents the initial lift, α0 represents the initial pitch, β0 represents the initial roll, and γ0 represents the initial yaw. The length of the electric cylinder corresponding to the motion direction of the six-degree-of-freedom motion platform calculated by position inverse solution is expressed by the formula: The lengths of the electric cylinders Ln_i (i=1, 2, ..., 729; n=1, 2, ..., 6) corresponding to 729 motion directions [X0+△X_i, Y0+△Y_i, Z0+△Z_i, α0+△α_i, β0+△β_i, γ0+△γ_i] are calculated respectively by position inversion; i represents the sequence number of the motion direction, and n represents the sequence number of the electric cylinder. In the formula, △X_i represents the offset of the lateral motion state in the i-th motion direction, △X_i=-a or 0 or a; △Y_i represents the sequence number of the i-th motion direction. △Y_i represents the offset of the longitudinal motion state in the i-th motion direction, △Y_i=-a or 0 or a; △Z_i represents the offset of the lifting motion state in the i-th motion direction, △Z_i=-a or 0 or a; △α_i represents the offset of the pitch motion state in the i-th motion direction, △α_i=-a or 0 or a; △β_i represents the offset of the rolling motion state in the i-th motion direction, △β_i=-a or 0 or a; △γ_i represents the offset of the yaw motion state in the i-th motion direction, △γ_i=-a or 0 or a; a represents the iteration step.

3. The kinematics correct solution method of the six-degree-of-freedom motion platform as claimed in claim 1, characterized in that: In step 4, the absolute value of the first difference of the accumulated six electric cylinders is expressed by the formula: In the formula, Sum_△Li represents the judgment reference value, and △Ln_i represents the first difference value of the nth electric cylinder in the i-th motion direction of the six-degree-of-freedom motion platform.

4. The kinematics correct solution method of the six-degree-of-freedom motion platform as claimed in claim 1, characterized in that: In step 4, the absolute value of the accumulated second difference of the six electric cylinders is expressed by the formula: Where Sum_△Ln represents the total electric cylinder length change, and △Ln represents the second difference of the nth electric cylinder.

5. An electronic device, characterized in that: include: A memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the kinematics correct solution method for the six-degree-of-freedom motion platform as described in any one of claims 1 to 4 is implemented.

6. A computer-readable storage medium storing a computer program, characterized in that: The computer program enables a computer to execute the kinematics forward solution method for a six-degree-of-freedom motion platform as described in any one of claims 1-4.

Images