Kinematic forward solving methods, systems, equipment, and media for six-degree-of-freedom motion platforms

Abstract

This invention addresses the problem of excessive computational load in traditional methods by proposing a kinematic forward solution method, device, and storage medium for a six-degree-of-freedom motion platform. Belonging to the field of robot control technology, the method includes: six motion states of the six-degree-of-freedom motion platform: lateral, longitudinal, heave, pitch, roll, and yaw. Each motion state has three motion directions, from which the platform's motion directions can be obtained. The motion states of electric cylinders are calculated using 729 platform motion directions. These are compared with known electric cylinder motion states, and the platform motion direction of the electric cylinder motion state closest to the desired forward solution state is taken as the solved forward solution value. This process is iterated multiple times until an accurate solution within the tolerance range is obtained. This invention does not require Newton's iteration method; the computational load can be reduced by setting the iteration step size, or even by setting staged iteration step sizes, achieving forward solution accuracy within any tolerance range.

Description

Technical Field

[0001] This invention belongs to the field of robot control technology and is mainly applied to six-degree-of-freedom motion platforms. Specifically, it is a kinematic forward kinematics solution method, system, device, and medium for a six-degree-of-freedom motion platform. Background Technology

[0002] The six-degree-of-freedom motion platform is a structure consisting of six electric cylinders, six universal hinges on the upper and lower sides, and two platforms. With the lower platform fixed, the upper platform can move in six degrees of freedom (X, Y, Z, α, β, γ) in space by means of the telescopic movement of the six electric cylinders. These movements include lateral movement, longitudinal movement, lifting, pitching, rolling, and yaw, thus simulating various spatial motion postures.

[0003] Forward kinematics of a six-DOF motion platform refers to solving for the motion state of the six-DOF motion platform given the motion state of the electric cylinder. Traditional forward kinematics solutions for six-DOF motion platforms can only establish a system of nonlinear equations and solve them using Newton's iteration method. This method is computationally intensive. If real-time monitoring of the motion platform's position and attitude via the electric cylinder's extension is required, it places high demands on computer hardware and may even cause system crashes or freezes. Summary of the Invention

[0004] To address the shortcomings of existing technologies, this invention provides a method, system, device, and medium for forward kinematics of a six-degree-of-freedom motion platform. This invention does not require solving the problem using Newton's iteration method; instead, it reduces the computational load by setting iteration step sizes, or even by setting staged iteration step sizes, thus achieving forward kinematics accuracy within any tolerance range.

[0005] To achieve the above objectives, the present invention adopts the following technical solution:

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

[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 motion direction of the six-degree-of-freedom motion platform using inverse kinematics; 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 △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 motion direction of the six-degree-of-freedom motion platform, as the third difference △Ln_0i.

[0009] Step 3: Determine if there is one and only one motion direction that satisfies the first condition. The first condition is: the first difference and the second difference have the same sign, and the absolute value of the first difference is less than the absolute value of the second difference. If yes, 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. The length of the electric cylinder corresponding to this motion direction is used as the initial state electric cylinder length in the next calculation round. If no, proceed to step 4.

[0010] Step 4: Determine if the first and second differences of all electric cylinders have the same sign. If yes, sum the absolute values ​​of the first differences of the six electric cylinders as the reference value Sum_△Li; sum the absolute values ​​of the second differences of the six electric cylinders as the total change in electric cylinder length Sum_△Ln; proceed to Step 5. If no, sum the absolute values ​​of the first differences of the electric cylinders whose first and second differences have the same sign, and add the absolute values ​​of the third differences of the electric cylinders whose first and second differences have different signs as the reference value Sum_△Li; sum the absolute values ​​of the second differences of the six electric cylinders as the total change in electric cylinder length Sum_△Ln; proceed to Step 5.

[0011] Step 5: Select the minimum judgment reference value from all the judgment reference values ​​for motion directions. Check if the minimum judgment reference value is 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 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. The length of the electric cylinder corresponding to this motion direction is used as the initial state length of the electric cylinder in the next calculation round. If not, reduce the iteration step size and return to step 2.

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

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

[0014] The electric cylinder length Ln_i (i = 1, 2, ..., 729; n = 1, 2, ..., 6) corresponding to each of the 729 motion directions [X0 + △X_i, Y0 + △Y_i, Z0 + △Z_i, α0 + △α_i, β0 + △β_i, γ0 + △γ_i] is calculated using inverse kinematics. 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 movement state in the i-th motion direction, and △X_i = -a, 0, or a; △Y_i represents the offset of the lateral movement state in the i-th motion direction. The offset of the longitudinal motion state in the direction of time, △Y_i = -a or 0 or a; △Z_i represents the offset of the heave motion state in the i-th direction of motion, △Z_i = -a or 0 or a; △α_i represents the offset of the pitch motion state in the i-th direction of motion, △α_i = -a or 0 or a; △β_i represents the offset of the roll motion state in the i-th direction of motion, △β_i = -a or 0 or a; △γ_i represents the offset of the yaw motion state in the i-th direction of motion, △γ_i = -a or 0 or a; a represents the iteration step size.

[0015] Furthermore, in step 4, the absolute value of the first difference between 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] Furthermore, in step 4, the absolute value of the second difference between the six accumulated electric cylinders is expressed by the formula:

[0019]

[0020] In the formula, Sum_△Ln represents the total change in the length of the electric cylinder, 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, it implements the kinematic forward solution method for a six-degree-of-freedom motion platform as described above.

[0022] The present invention also proposes a computer-readable storage medium storing a computer program that enables a computer to execute the kinematic forward solution method for a six-degree-of-freedom motion platform as described above.

[0023] The beneficial effects of this invention are:

[0024] This invention does not require solving the problem using Newton's iteration method. Instead, it reduces the computational load by setting the iteration step size, or even by setting phased iteration step sizes, and obtains the correct solution accuracy within any tolerance range. Attached Figure Description

[0025] Figure 1 This is a schematic diagram of a six-degree-of-freedom motion platform.

[0026] Figure 2 This is a flowchart of the correct solution method. Detailed Implementation

[0027] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of this application.

[0028] Example 1

[0029] This invention proposes a forward kinematics 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; a schematic diagram of the six-degree-of-freedom motion platform is shown below. Figure 1 As shown, the six telescopic structures on the side are electric cylinders. The six motion states of the six-degree-of-freedom motion platform are: lateral movement △X, longitudinal movement △Y, heave △Z, pitch △α, roll △β, and yaw △γ. These six motion states can be written as a vector form: forward = [△X, △Y, △Z, △α, △β, △γ]. Each motion state has three motion directions [-a, 0, a] (where a is the iteration step size, a constant, and the iteration precision can be changed according to its value). This yields 729 (3...)... 6 The direction of platform movement.

[0030] Each time the length of the electric cylinder changes, the following steps are executed, 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 movement, Y0 represents the initial longitudinal movement, Z0 represents the initial heave, α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 through inverse kinematics; expressed by the formula:

[0033] The electric cylinder length Ln_i (i = 1, 2, ..., 729; n = 1, 2, ..., 6) corresponding to each of the 729 motion directions [X0 + △X_i, Y0 + △Y_i, Z0 + △Z_i, α0 + △α_i, β0 + △β_i, γ0 + △γ_i] is calculated using inverse kinematics. 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 movement state in the i-th motion direction, and △X_i = -a, 0, or a; △Y_i represents the offset of the lateral movement state in the i-th motion direction. The offset of the longitudinal motion state in the direction of time, △Y_i = -a or 0 or a; △Z_i represents the offset of the heave motion state in the i-th direction of motion, △Z_i = -a or 0 or a; △α_i represents the offset of the pitch motion state in the i-th direction of motion, △α_i = -a or 0 or a; △β_i represents the offset of the roll motion state in the i-th direction of motion, △β_i = -a or 0 or a; △γ_i represents the offset of the yaw motion state in the i-th direction of motion, △γ_i = -a or 0 or a; a represents the iteration step size.

[0034] 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, and use this as the first difference ΔLn_i; specifically:

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

[0036] △Ln_i=Ln_i-Ln;

[0037] Calculate the difference between the initial length of the electric cylinder and the monitored length of the electric cylinder, and use this difference 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 in the state to be solved (n = 1, 2, ..., 6); (a positive value indicates contraction, a negative value indicates extension).

[0039] △Ln=Ln_0-Ln;

[0040] Calculate the difference between the initial state length of the electric cylinder and the length of the electric cylinder corresponding to the motion direction of the six-degree-of-freedom motion platform, and use this 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 in 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 if there is one and only one motion direction that satisfies the first condition. The first condition is: the first difference and the second difference have the same sign, and the absolute value of the first difference is less than the absolute value of the second difference, i.e., |△Ln_i| < |△Ln|. If yes, 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. The length of the electric cylinder corresponding to this motion direction is used as the initial state electric cylinder length in the next calculation round. If no, proceed to step 4.

[0044] Step 4: Determine if the first and second differences of all electric cylinders have the same sign. If so, sum the absolute values ​​of the first differences of the six electric cylinders as the reference value Sum_△Li; expressed by the formula:

[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 value of the second difference between the six electric cylinders is accumulated and used as the total change in electric cylinder length, Sum_△Ln; proceed to step 5; the absolute value of the second difference between the six electric cylinders is expressed by the formula:

[0048]

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

[0050] If not, sum the absolute values ​​of the first differences of the electric cylinders whose first and second differences have the same sign, and add the absolute values ​​of the third differences of the electric cylinders whose first and second differences have different signs, as the reference value Sum_△Li; for example, for electric cylinders numbered 4, 5, and 6, △Ln_i and △Ln have different signs, then the calculation method for Sum_△Li is as follows:

[0051]

[0052] Accumulate the absolute value of the second difference of the six electric cylinders, and use it as the total change in electric cylinder length, Sum_△Ln; proceed to step 5;

[0053] Step 5: Select the minimum reference value from all motion direction judgment reference values ​​Sum_△Li. The smaller Sum_△Li is, the closer the motion direction is to the desired positive solution state. Assuming the minimum value of Sum_△Li is Min_Sum, then Min_Sum should be less than the sum of |△Ln| Sum_△Ln. Check if the minimum judgment reference value Min_Sum is less than the total change in electric cylinder length 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 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 electric cylinder length. The electric cylinder length corresponding to this 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] Example 2

[0063] This invention proposes 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, it implements the kinematic forward kinematics method for a six-degree-of-freedom motion platform as described in Embodiment 1.

[0064] Example 3

[0065] This invention proposes a computer-readable storage medium storing a computer program that causes a computer to execute the forward kinematics method for a six-degree-of-freedom motion platform as described in Embodiment 1.

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

[0067] Those skilled in the art will recognize that the units and algorithm steps of the various examples 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 implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0068] The above are merely preferred embodiments of the present invention. The scope of protection of the present invention is not limited to the above embodiments. All technical solutions falling within the scope of the present invention's concept are within the scope of protection of the present invention. It should be noted that for those skilled in the art, any improvements and modifications made without departing from the principles of the present invention should be considered within the scope of protection of the present invention.

Claims

1. A forward kinematics 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 executed: 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 through inverse kinematics; 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, and use it as the first difference. Calculate the difference between the initial state electric cylinder length and the monitored electric cylinder length, and use it as the second difference; Calculate the difference between the initial state electric cylinder length and the electric cylinder length corresponding to the motion direction of the six-degree-of-freedom motion platform, and use it as the third difference; Step 3: Determine if there is one and only one motion direction that satisfies the first condition. The first condition is: the first difference and the second difference have the same sign, and the absolute value of the first difference is less than the absolute value of the second difference. If yes, 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. The length of the electric cylinder corresponding to this motion direction is used as the initial state electric cylinder length in the next calculation round. If no, proceed to step 4. Step 4: Determine whether the first difference and the second difference of all electric cylinders have the same sign. If so, sum the absolute values ​​of the first differences of the six electric cylinders as the reference value for judgment. The absolute value of the second difference between the six electric cylinders is added together to obtain the total change in the length of the electric cylinder. Proceed to step 5; if not, accumulate the absolute value of the first difference of the electric cylinders whose first and second differences have the same sign, and add the absolute value of the third difference of the electric cylinders whose first and second differences have different signs, as a reference value for judgment. The absolute value of the second difference between the six electric cylinders is added together to obtain the total change in the length of the electric cylinder. Proceed to step 5; Step 5: Select the minimum judgment reference value from all the judgment reference values ​​for motion directions. Check if the minimum judgment reference value is 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 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. The length of the electric cylinder corresponding to this motion direction is used as the initial state length of the electric cylinder in the next calculation round. If not, reduce the iteration step size and return to step 2.

2. The forward kinematics solution method for a six-degree-of-freedom motion platform as described 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 initial lateral movement, Y0 represents initial longitudinal movement, Z0 represents initial heave, α0 represents initial pitch, β0 represents initial roll, and γ0 represents initial yaw. The electric cylinder length corresponding to the motion direction of the six-degree-of-freedom motion platform calculated by inverse kinematics is expressed by the following formula: The electric cylinder length Ln_i (i = 1, 2, ..., 729; n = 1, 2, ..., 6) corresponding to each of the 729 motion directions [X0 + △X_i, Y0 + △Y_i, Z0 + △Z_i, α0 + △α_i, β0 + △β_i, γ0 + △γ_i] is calculated using inverse kinematics. 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 movement state in the i-th motion direction, and △X_i = -a, 0, or a; △Y_i represents the offset of the lateral movement state in the i-th motion direction. The offset of the longitudinal motion state in the direction of time, △Y_i = -a or 0 or a; △Z_i represents the offset of the heave motion state in the i-th direction of motion, △Z_i = -a or 0 or a; △α_i represents the offset of the pitch motion state in the i-th direction of motion, △α_i = -a or 0 or a; △β_i represents the offset of the roll motion state in the i-th direction of motion, △β_i = -a or 0 or a; △γ_i represents the offset of the yaw motion state in the i-th direction of motion, △γ_i = -a or 0 or a; a represents the iteration step size.

3. The forward kinematics solution method for a six-degree-of-freedom motion platform as described in claim 1, characterized in that, In step 4, the absolute value of the first difference between the 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 forward kinematics solution method for a six-degree-of-freedom motion platform as described in claim 1, characterized in that, In step 4, the absolute value of the second difference between the six electric cylinders is expressed by the formula: In the formula, Sum_△Ln represents the total change in the length of the electric cylinder, and △Ln represents the second difference of the nth electric cylinder.

5. An electronic device, characterized in that, include: The memory, the processor, and the computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, it implements the kinematic forward kinematics method for a six-degree-of-freedom motion platform as described in any one of claims 1-4.

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

Images