CN116893627A - Motion platform pose control method and device based on iteration step length and storage medium - Google Patents
Motion platform pose control method and device based on iteration step length and storage medium Download PDFInfo
- Publication number
- CN116893627A CN116893627A CN202311158793.5A CN202311158793A CN116893627A CN 116893627 A CN116893627 A CN 116893627A CN 202311158793 A CN202311158793 A CN 202311158793A CN 116893627 A CN116893627 A CN 116893627A
- Authority
- CN
- China
- Prior art keywords
- hinge point
- upper hinge
- electric cylinder
- height
- initial
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 39
- 230000003247 decreasing effect Effects 0.000 claims description 8
- 230000008569 process Effects 0.000 claims description 8
- 238000010276 construction Methods 0.000 claims description 6
- 238000005096 rolling process Methods 0.000 claims description 3
- 238000004088 simulation Methods 0.000 description 7
- 230000001133 acceleration Effects 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 230000006978 adaptation Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B17/00—Systems involving the use of models or simulators of said systems
- G05B17/02—Systems involving the use of models or simulators of said systems electric
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Length Measuring Devices With Unspecified Measuring Means (AREA)
Abstract
The invention provides a motion platform pose control method, a motion platform pose control device and a storage medium based on iteration step length.
Description
Technical Field
The invention relates to the technical field of multi-degree-of-freedom motion platforms, in particular to a motion platform pose control method, device and storage medium based on iteration step length.
Background
The multi-freedom-degree motion platform is widely cited in the fields of industry and simulation, including fields of scientific simulation, driving simulation, aircraft simulation, entertainment simulation and the like, and the pose change of the multi-freedom-degree platform is applied to simulate actual actions and processes in various scenes so as to achieve the effect of physical simulation or semi-physical simulation in the scene.
The multi-degree-of-freedom motion platform is also called a kinematic robot, and the upper platform is driven to move in a plurality of degrees of freedom through motion mechanisms such as an electric cylinder, a hydraulic cylinder and the like, so that the pose under various conditions is simulated. The process of solving parameters such as the position, the speed, the acceleration, the thrust, the torque and the like of the electric cylinder is called parallel robot dynamics inverse solution in the state of knowing the position posture, (angular) speed, (angular) acceleration and the like of the moving platform; the process of solving the states of the position, the speed, the acceleration and the like of the moving platform is called as the dynamics forward solution of the parallel robot by knowing the states of the position, the speed, the thrust and the like of the moving mechanism such as the electric cylinder. Taking a three-degree-of-freedom motion platform or a six-degree-of-freedom motion platform as an example, in the existing pose control method, a plurality of control modes are included, and taking the kinematic positive solution of the three-degree-of-freedom motion platform as an example, the method is based on the establishment of a nonlinear equation set, and the information such as the pose, the speed, the acceleration and the like of the motion platform is obtained through solving by a Newton iteration method, but the calculated amount of the method is large, if the position pose of the upper platform needs to be monitored in real time through the elongation of an electric cylinder, the requirement on computer hardware is high, and even the operation is blocked and halted.
Disclosure of Invention
In view of the defects and shortcomings of the prior art, the invention aims to provide a method for controlling the correct solution of a multi-degree-of-freedom motion platform, which does not use a traditional Newton iteration method, reduces calculated quantity by setting iteration step length, approximately solves pitch angle and roll angle of an upper platform based on the elongation of an electric cylinder, solves the expansion quantity of the electric cylinder according to the approximate solution, compares the expansion quantity with the expansion quantity of the known electric cylinder, corrects errors caused by the inclination of the electric cylinder according to a comparison result, and iterates repeatedly until an accurate solution within a tolerance range is obtained, and the position and the gesture of the upper platform are obtained, so that the pose control of the platform is realized.
According to a first aspect of the present invention, there is provided a motion platform pose control method based on iterative step length, the motion platform is a three-degree-of-freedom motion platform, three lower hinge points and three upper hinge points thereof respectively form regular triangle distribution, the pose control method includes:
step 1, establishing a global coordinate system CoordOXYZ, wherein the origin O of the coordinate system is positioned in a triangle plane formed by three lower hinge points and coincides with the high midpoint of a triangle passing through the lower hinge point 1; the Y axis points to the lower hinge point 1, the X axis points to one side of the lower hinge point 2, and the Z axis points vertically upwards;
step 2, establishing a motion reference coordinate system CoordO 1 X 1 Y 1 Z 1 Origin of coordinate system O 1 In the triangle plane formed by the three upper hinge points, the triangle plane coincides with the high middle point of the triangle passing through the upper hinge point 1; y is Y 1 Axis-directed upper hinge point 1, X 1 The axis is directed to the side of the upper hinge point 2, Z 1 The shaft is vertically directed upwards;
step 3, acquiring an initial inclination angle theta_i of the ith electric cylinder according to the initial Z coordinate value of the lower hinge point corresponding to the ith electric cylinder and the initial Z coordinate value of the upper hinge point, wherein i represents the serial number of the electric cylinder;
step 4, acquiring the height H_i of an upper hinge point of the ith electric cylinder according to the initial length, the stroke and the initial inclination angle theta_i of the ith electric cylinder;
step 5, acquiring the height H_23 of the midpoint of the connecting line of the upper hinge point 2 and the upper hinge point 3, and accordingly acquiring the height difference delta H_1 between the height of the upper hinge point 1 and the height of the upper hinge point H_23;
step 6: acquiring a Y coordinate distance difference delta Hor1 from an upper hinge point 1 to the midpoint of a connecting line of an upper hinge point 2 and an upper hinge point 3;
step 7: acquiring a height difference delta H_2 between the upper hinge point 2 and the upper hinge point 3 and a distance difference delta Hor2 between the upper hinge point 2 and the X coordinate of the upper hinge point 3;
step 8: calculating the approximate solution of the pitch angle alpha, the roll angle beta and the lifting distance H of the motion platform;
step 9, calculating the elongation Len_i of the electric cylinder according to the approximate solution of the pitch angle alpha and the roll angle beta
Step 10, obtaining a difference delta L_i between the electric cylinder elongation Len_i obtained in the step 9 and the actual electric cylinder elongation L_i;
step 11, obtaining the Sum Sum delta L of the elongation differences delta L_i of all the electric cylinders obtained in the step 10;
and step 12, performing accuracy correction of the approximate solution of the pitch angle alpha, the roll angle beta and the lifting distance H according to the delta L_i and the Sum delta L.
As an optional embodiment, in the step 8, calculating an approximate solution of the pitch angle α, the roll angle β, and the lifting distance H of the motion platform includes:
α = arctan(△H_1/△Hor1)
β = arctan(△H_2/△Hor2)
H = (H_1 + H_2 + H_3)/3 – (L + L_stroke/2)。
as an optional embodiment, in the step 12, the accuracy correction of the approximate solution of the pitch angle α, the roll angle β, and the lifting distance H is performed according to Δl_i and sum_Δl, including:
logic 1: because the pitch angle α is positive, the electric cylinders No. 1 are extended, the electric cylinders No. 2 and 3 are contracted, the pitch angle α is reduced if Δl_1 is greater than 0, or Δl_2 and Δl_3 are simultaneously less than 0, otherwise the pitch angle α is increased;
logic 2: because the roll angle β is positive, the electric cylinder No. 3 is extended, the electric cylinder No. 2 is contracted, so if Δl_2 is less than 0 and Δl_3 is greater than 0, the roll angle β is decreased, otherwise the roll angle β is increased;
logic 3: if the platform only has pitching motion and rolling motion, the sum of the lengths of the three electric cylinders is equal to the sum of the lengths of the three electric cylinders in the middle position; thus, if Sum Δl is greater than 0, the lifting distance H is decreased, otherwise the lifting distance H is increased.
In a preferred embodiment, the calculation amount can be reduced by setting the iteration step, or setting the iteration step stepwise by the order of magnitude of the error, so as to obtain the positive solution precision within any tolerance range.
According to a second aspect of the present invention, there is also provided a motion platform pose control device based on iteration step, including:
the first coordinate system construction module is used for establishing a global coordinate system CoordOXYZ, and a coordinate system origin O of the global coordinate system CoordOXYZ is positioned in a triangle plane formed by three lower hinge points and coincides with the high midpoint of a triangle passing through the lower hinge point 1; the Y axis points to the lower hinge point 1, the X axis points to one side of the lower hinge point 2, and the Z axis points vertically upwards;
for establishing a motion reference frame CoordO 1 X 1 Y 1 Z 1 A second coordinate system construction module of the motion reference coordinate system CoordO 1 X 1 Y 1 Z 1 Origin of coordinate system O of (2) 1 In the triangle plane formed by the three upper hinge points, the triangle plane coincides with the high middle point of the triangle passing through the upper hinge point 1; y is Y 1 Axis-directed upper hinge point 1, X 1 The axis is directed to the side of the upper hinge point 2, Z 1 The shaft is vertically directed upwards;
the electric cylinder initial inclination angle estimation module is used for acquiring an initial inclination angle theta_i of the ith electric cylinder according to the initial Z coordinate value of the lower hinge point corresponding to the ith electric cylinder and the initial Z coordinate value of the upper hinge point, wherein i represents the serial number of the electric cylinder;
a first height estimation module for acquiring the height H_i of the upper hinge point of the ith electric cylinder according to the initial length of the ith electric cylinder, the electric cylinder stroke and the electric cylinder initial inclination angle theta_i;
a second height estimation module for acquiring the height H_23 of the midpoint of the connecting line of the upper hinge point 2 and the upper hinge point 3 and accordingly acquiring the height difference delta H_1 between the height of the upper hinge point 1 and the height of the upper hinge point H_23;
the first difference value estimation module is used for acquiring a Y-coordinate distance difference delta Hor1 between the upper hinge point 1 and the midpoint of the connecting line of the upper hinge point 2 and the upper hinge point 3;
the second difference value estimation module is used for acquiring the height difference delta H_2 between the upper hinge point 2 and the upper hinge point 3 and the distance difference delta Hor2 between the upper hinge point 2 and the X coordinate of the upper hinge point 3;
the motion platform estimation module is used for calculating an approximate solution of a pitch angle alpha, a roll angle beta and a lifting distance H of the motion platform;
the electric cylinder elongation estimation module is used for calculating the electric cylinder elongation Len_i according to the approximate solution of the pitch angle alpha and the roll angle beta;
the electric cylinder extension difference value acquisition module is used for acquiring a difference DeltaL_i between the electric cylinder extension Len_i and the actual electric cylinder extension L_i;
the electric cylinder elongation difference total module is used for obtaining the Sum Sum_ delta L of all electric cylinder elongation differences delta L_i; and
and the motion platform pose correction module is used for correcting the precision of the approximate solutions of the pitch angle alpha, the roll angle beta and the lifting distance H according to the delta L_i and the Sum delta L.
According to a third aspect of the object of the present invention, there is also provided a computer readable medium storing software comprising instructions executable by one or more computers, which instructions, when executed by the one or more computers, perform the process of the motion platform pose control method as described above.
According to the technical scheme, the motion platform pose control method based on the iteration step is not required to be solved through a Newton iteration method, the approximate solution of the pitch angle and the roll angle of the upper platform is firstly solved through the extension of the electric cylinder, then the extension of the electric cylinder is solved according to the approximate solution, the extension of the electric cylinder is compared with the extension of the known electric cylinder, finally the error caused by the inclination of the electric cylinder is corrected according to the comparison result, and repeated loop iteration is performed until the accurate solution within the tolerance range is obtained, so that the pitch angle alpha, the roll angle beta and the lifting distance H of the motion platform are determined, and the position and the pose of the motion platform are determined.
It should be understood that all combinations of the foregoing concepts, as well as additional concepts described in more detail below, may be considered a part of the inventive subject matter of the present disclosure as long as such concepts are not mutually inconsistent. In addition, all combinations of claimed subject matter are considered part of the disclosed inventive subject matter.
The foregoing and other aspects, embodiments, and features of the present teachings will be more fully understood from the following description, taken together with the accompanying drawings. Other additional aspects of the invention, such as features and/or advantages of the exemplary embodiments, will be apparent from the description which follows, or may be learned by practice of the embodiments according to the teachings of the invention.
Drawings
The drawings are not intended to be drawn to scale. In the drawings, each identical or nearly identical component that is illustrated in various figures may be represented by a like numeral. For purposes of clarity, not every component may be labeled in every drawing. Embodiments of various aspects of the invention will now be described, by way of example, with reference to the accompanying drawings.
Fig. 1 is a schematic diagram of a three-degree-of-freedom motion platform according to an embodiment of the present invention.
Fig. 2 is a schematic view of a hinge point of a three-degree-of-freedom motion platform according to an embodiment of the present invention.
Fig. 3 is a schematic diagram of defining a coordinate system of a three-degree-of-freedom motion platform according to an embodiment of the present invention.
Fig. 4A and 4B are side views of a three-degree-of-freedom motion platform in a lowest pose and a highest pose, respectively, according to an embodiment of the invention.
Fig. 5 is a side view of a three degree of freedom motion platform of an embodiment of the present invention with the pitch angle α angle positive.
Detailed Description
For a better understanding of the technical content of the present invention, specific examples are set forth below, along with the accompanying drawings.
Aspects of the invention are described in this disclosure with reference to the drawings, in which are shown a number of illustrative embodiments. The embodiments of the present disclosure are not necessarily intended to include all aspects of the invention. It should be understood that the various concepts and embodiments described above, as well as those described in more detail below, may be implemented in any of a number of ways, as the disclosed concepts and embodiments are not limited to any implementation. Additionally, some aspects of the disclosure may be used alone or in any suitable combination with other aspects of the disclosure.
The invention provides a three-degree-of-freedom platform forward solution control method with small calculated amount, quick estimation and high efficiency, which can solve the output of an executing piece, namely the elongation of a known electric cylinder and solve the position and the posture of an upper platform under the condition of the input of a known driving piece.
In combination with the examples shown in fig. 1, 2 and 3, the motion platform in the embodiment of the invention takes a three-degree-of-freedom motion platform as an example, three electric cylinders are adopted for hinging and driving between the upper platform and the lower platform, and three lower hinging points and three upper hinging points respectively form regular triangle distribution.
For convenience of description, the lower hinge point refers to an intersection point of a bearing seat rotating shaft at the bottom of an electric cylinder of the three-degree-of-freedom motion platform and an axis of a guide rod of the electric cylinder, and the upper hinge point refers to an intersection point of two rotating shafts of a Hooke hinge at the top of the electric cylinder of the three-degree-of-freedom motion platform, which are shown in the combination of figures 1 to 3.
In connection with the examples shown in fig. 1-3, the respective hinge points and their definitions are as follows:
XB_i (1.ltoreq.i.ltoreq.3): an initial X coordinate value of a hinging point under the three-degree-of-freedom motion platform;
YB_i (1.ltoreq.i.ltoreq.3): an initial Y-coordinate value of a lower hinge point of the three-degree-of-freedom motion platform;
zb_i (1.ltoreq.i.ltoreq.3): an initial Z coordinate value of a hinge point under the three-degree-of-freedom motion platform;
XP_i (1.ltoreq.i.ltoreq.3): an initial X coordinate value of a hinge point on the three-degree-of-freedom motion platform;
yp_i (1.ltoreq.i.ltoreq.3): an initial Y-coordinate value of a hinge point on the three-degree-of-freedom motion platform;
zp_i (1.ltoreq.i.ltoreq.3): an initial Z coordinate value of a hinge point on the three-degree-of-freedom motion platform.
1-3, the pose control method of the embodiment of the invention comprises the following steps:
step 1, establishing a global coordinate system CoordOXYZ, wherein the origin O of the coordinate system is positioned in a triangle plane formed by three lower hinge points and coincides with the high midpoint of a triangle passing through the lower hinge point 1; the Y axis points to the lower hinge point 1, the X axis points to one side of the lower hinge point 2, and the Z axis points vertically upwards;
step 2, establishing a motion reference coordinate system CoordO 1 X 1 Y 1 Z 1 Origin of coordinate system O 1 Triangular plane formed by three upper hinge pointsIn, coincides with the high midpoint of the triangle passing the upper hinge point 1; y is Y 1 Axis-directed upper hinge point 1, X 1 The axis is directed to the side of the upper hinge point 2, Z 1 The shaft is vertically directed upwards;
step 3, acquiring an initial inclination angle theta_i of the ith electric cylinder according to the initial Z coordinate value of the lower hinge point corresponding to the ith electric cylinder and the initial Z coordinate value of the upper hinge point, wherein i represents the serial number of the electric cylinder;
step 4, acquiring the height H_i of an upper hinge point of the ith electric cylinder according to the initial length, the stroke and the initial inclination angle theta_i of the ith electric cylinder;
step 5, acquiring the height H_23 of the midpoint of the connecting line of the upper hinge point 2 and the upper hinge point 3, and accordingly acquiring the height difference delta H_1 between the height of the upper hinge point 1 and the height of the upper hinge point H_23;
step 6: acquiring a Y coordinate distance difference delta Hor1 from an upper hinge point 1 to the midpoint of a connecting line of an upper hinge point 2 and an upper hinge point 3;
step 7: acquiring a height difference delta H_2 between the upper hinge point 2 and the upper hinge point 3 and a distance difference delta Hor2 between the upper hinge point 2 and the X coordinate of the upper hinge point 3;
step 8: calculating the approximate solution of the pitch angle alpha, the roll angle beta and the lifting distance H of the motion platform;
step 9, calculating the elongation Len_i of the electric cylinder according to the approximate solution of the pitch angle alpha and the roll angle beta
Step 10, obtaining a difference delta L_i between the electric cylinder elongation Len_i obtained in the step 9 and the actual electric cylinder elongation L_i;
step 11, obtaining the Sum Sum delta L of the elongation differences delta L_i of all the electric cylinders obtained in the step 10;
and step 12, performing accuracy correction of the approximate solution of the pitch angle alpha, the roll angle beta and the lifting distance H according to the delta L_i and the Sum delta L.
In the step 3, according to the initial Z coordinate value of the lower hinge point and the initial Z coordinate value of the upper hinge point corresponding to the ith electric cylinder, an initial tilt angle θ_i of the ith electric cylinder is obtained, including:
θ_i = arcsin{(ZP_i - ZB_i)/L}
wherein ZB_i represents an initial Z coordinate value of a lower hinge point corresponding to the ith electric cylinder; ZP_i represents the initial Z coordinate value of the upper hinge point, and i is more than or equal to 1 and less than or equal to 3.
In the step 4, the step of obtaining the height h_i of the upper hinge point of the ith electric cylinder according to the initial length of the ith electric cylinder, the electric cylinder stroke and the electric cylinder initial tilt angle θ_i includes:
H_i = (L_i+L+L_stroke/2)/cos(θ_i)
where L represents the initial length of the i-th electric cylinder, l_stroke represents the stroke of the i-th electric cylinder, and l_i represents the actual extension amount of the i-th electric cylinder.
In the step 5, the step of obtaining the height h_23 of the midpoint of the connection line between the upper hinge point 2 and the upper hinge point 3 includes:
H_23 = (H_2 + H_3)/2；
it should be understood that, here, h_2 and h_3 refer to the heights of the upper hinge point 2 and the upper hinge point 3, respectively, and their values are the same as the Z coordinate values of the upper hinge point 2 and the upper hinge point 3;
the step of obtaining the difference Δh_1 between the height of the upper hinge point 1 and the height of the h_23 includes:
△H_1 = (H_1 - H_23)；
it should be understood that h_1 herein refers to the height of the upper hinge point 1, which takes the same value as the Z coordinate value of the upper hinge point 1.
In the step 6, a Y coordinate distance difference Δhor1 between the upper hinge point 1 and a midpoint of a connecting line between the upper hinge point 2 and the upper hinge point 3 is obtained, including:
△Hor1= YP_1 – (YP_2 - YP_3)/2；
wherein yp_1, yp_2, yp_3 refer to the corresponding initial Y coordinate values of the upper hinge point 1, the upper hinge point 2, the upper hinge point 3, respectively.
In the step 7, the step of obtaining the difference Δh_2 between the upper hinge point 2 and the upper hinge point 3 includes:
△H_2 = (H_2 - H_3)；
acquiring the distance difference Δhor2 of the X coordinates of the upper hinge point 2 and the upper hinge point 3 includes:
△Hor2 = XP_2 - XP_3 ；
wherein XP_2-XP_3 refer to the initial X coordinate values corresponding to the upper hinge point 2 and the upper hinge point 3, respectively.
In the step 8, an approximate solution of the pitch angle α, the roll angle β, and the lifting distance H of the motion platform is calculated, including:
α = arctan(△H_1/△Hor1)
β = arctan(△H_2/△Hor2)
H = (H_1 + H_2 + H_3)/3 – (L + L_stroke/2)。
in step 12, the accuracy correction of the approximate solution of the pitch angle α, the roll angle β, and the elevation distance H is performed based on Δl_i and sum_Δl, and includes:
logic 1: because the pitch angle α is positive, the electric cylinders No. 1 are extended, the electric cylinders No. 2 and 3 are contracted, the pitch angle α is reduced if Δl_1 is greater than 0, or Δl_2 and Δl_3 are simultaneously less than 0, otherwise the pitch angle α is increased;
logic 2: because the roll angle β is positive, the electric cylinder No. 3 is extended, the electric cylinder No. 2 is contracted, so if Δl_2 is less than 0 and Δl_3 is greater than 0, the roll angle β is decreased, otherwise the roll angle β is increased;
logic 3: if the platform only has pitching motion and rolling motion, the sum of the lengths of the three electric cylinders is equal to the sum of the lengths of the three electric cylinders in the middle position; thus, if Sum Δl is greater than 0, the lifting distance H is decreased, otherwise the lifting distance H is increased.
Therefore, the pitch angle alpha, the roll angle beta and the lifting distance H of the moving platform are estimated, so that the position and the gesture of the moving platform are determined, and the kinematic positive solution process of the three-degree-of-freedom moving platform is realized. For example, taking logic 1 as an example in connection with fig. 3 and 5, when it is determined in step 12 that the pitch angle α needs to be increased or decreased, the pitch angle α is controlled to be increased or decreased in 1 ° as an iteration step, so that based on a new set of pitch angle α, roll angle β and lifting distance H, further calculation and comparison in steps 9, 10 and 11 are further performed, and finally, accuracy correction is performed according to step 12 to determine whether the error is within a preset allowable range, for example, if the pitch angle α obtained by the first estimation has an error of 10 °, then error correction within 1 ° can be implemented by performing correction 10 times according to the method of the present invention.
Of course, if the error accuracy is required to be within the range of 0.5 °, the iteration step may be set to be 0.5 ° for iterative error correction until the error range is within the range of 0.5 °. Alternatively, the iteration step of the ladder is set, for example, the first n times are set to perform error correction according to the iteration step of 1 ° and then error correction is performed according to the iteration step of 0.5 ° until the error range is within the range of 0.5 °.
Similarly, according to the above-described method, the iterative step error correction of the contralateral inclination angle β (with the angle as the iterative step) and the lifting distance H (with the distance as the iterative step, for example, 0.5mm,0.2mm, etc.) can be realized.
In a preferred embodiment, the calculation amount can be reduced by setting the iteration step, or setting the iteration step stepwise by the order of magnitude of the error, so as to obtain the positive solution precision within any tolerance range.
In combination with the structures shown in fig. 1-3 and the platform movement process shown in fig. 4-6, according to the disclosed embodiment of the invention, a movement platform pose control device based on iteration step length is also provided, including:
the first coordinate system construction module is used for establishing a global coordinate system CoordOXYZ, and a coordinate system origin O of the global coordinate system CoordOXYZ is positioned in a triangle plane formed by three lower hinge points and coincides with the high midpoint of a triangle passing through the lower hinge point 1; the Y axis points to the lower hinge point 1, the X axis points to one side of the lower hinge point 2, and the Z axis points vertically upwards;
for establishing a motion reference frame CoordO 1 X 1 Y 1 Z 1 A second coordinate system construction module of the motion reference coordinate system CoordO 1 X 1 Y 1 Z 1 Origin of coordinate system O of (2) 1 In the triangle plane formed by the three upper hinge points, the triangle plane coincides with the high middle point of the triangle passing through the upper hinge point 1; y is Y 1 Axis-directed upper hinge point 1, X 1 The axis is directed to the side of the upper hinge point 2, Z 1 The shaft is vertically directed upwards;
the electric cylinder initial inclination angle estimation module is used for acquiring an initial inclination angle theta_i of the ith electric cylinder according to the initial Z coordinate value of the lower hinge point corresponding to the ith electric cylinder and the initial Z coordinate value of the upper hinge point, wherein i represents the serial number of the electric cylinder;
a first height estimation module for acquiring the height H_i of the upper hinge point of the ith electric cylinder according to the initial length of the ith electric cylinder, the electric cylinder stroke and the electric cylinder initial inclination angle theta_i;
a second height estimation module for acquiring the height H_23 of the midpoint of the connecting line of the upper hinge point 2 and the upper hinge point 3 and accordingly acquiring the height difference delta H_1 between the height of the upper hinge point 1 and the height of the upper hinge point H_23;
the first difference value estimation module is used for acquiring a Y-coordinate distance difference delta Hor1 between the upper hinge point 1 and the midpoint of the connecting line of the upper hinge point 2 and the upper hinge point 3;
the second difference value estimation module is used for acquiring the height difference delta H_2 between the upper hinge point 2 and the upper hinge point 3 and the distance difference delta Hor2 between the upper hinge point 2 and the X coordinate of the upper hinge point 3;
the motion platform estimation module is used for calculating an approximate solution of a pitch angle alpha, a roll angle beta and a lifting distance H of the motion platform;
the electric cylinder elongation estimation module is used for calculating the electric cylinder elongation Len_i according to the approximate solution of the pitch angle alpha and the roll angle beta;
the electric cylinder extension difference value acquisition module is used for acquiring a difference DeltaL_i between the electric cylinder extension Len_i and the actual electric cylinder extension L_i;
the electric cylinder elongation difference total module is used for obtaining the Sum Sum_ delta L of all electric cylinder elongation differences delta L_i; and
and the motion platform pose correction module is used for correcting the precision of the approximate solutions of the pitch angle alpha, the roll angle beta and the lifting distance H according to the delta L_i and the Sum delta L.
According to an embodiment of the present disclosure, there is also provided a computer-readable medium storing software including instructions executable by one or more computers, which when executed by the one or more computers, are capable of performing the process of the motion platform pose control method as in the previous embodiment.
While the invention has been described with reference to preferred embodiments, it is not intended to be limiting. Those skilled in the art will appreciate that various modifications and adaptations can be made without departing from the spirit and scope of the present invention. Accordingly, the scope of the invention is defined by the appended claims.
Claims (10)
1. The utility model provides a motion platform pose control method based on iteration step length, its characterized in that, motion platform is three degree of freedom motion platform, and its three lower pin joint and three last pin joint constitute regular triangle distribution respectively, the pose control method includes:
step 1, establishing a global coordinate system CoordOXYZ, wherein the origin O of the coordinate system is positioned in a triangle plane formed by three lower hinge points and coincides with the high midpoint of a triangle passing through the lower hinge point 1; the Y axis points to the lower hinge point 1, the X axis points to one side of the lower hinge point 2, and the Z axis points vertically upwards;
step 2, establishing a motion reference coordinate system CoordO 1 X 1 Y 1 Z 1 Origin of coordinate system O 1 In the triangle plane formed by the three upper hinge points, the triangle plane coincides with the high middle point of the triangle passing through the upper hinge point 1; y is Y 1 Axis-directed upper hinge point 1, X 1 The axis is directed to the side of the upper hinge point 2, Z 1 The shaft is vertically directed upwards;
step 3, acquiring an initial inclination angle theta_i of the ith electric cylinder according to the initial Z coordinate value of the lower hinge point corresponding to the ith electric cylinder and the initial Z coordinate value of the upper hinge point, wherein i represents the serial number of the electric cylinder;
step 4, acquiring the height H_i of an upper hinge point of the ith electric cylinder according to the initial length, the stroke and the initial inclination angle theta_i of the ith electric cylinder;
step 5, acquiring the height H_23 of the midpoint of the connecting line of the upper hinge point 2 and the upper hinge point 3, and accordingly acquiring the height difference delta H_1 between the height of the upper hinge point 1 and the height of the upper hinge point H_23;
step 6: acquiring a Y coordinate distance difference delta Hor1 from an upper hinge point 1 to the midpoint of a connecting line of an upper hinge point 2 and an upper hinge point 3;
step 7: acquiring a height difference delta H_2 between the upper hinge point 2 and the upper hinge point 3 and a distance difference delta Hor2 between the upper hinge point 2 and the X coordinate of the upper hinge point 3;
step 8: calculating the approximate solution of the pitch angle alpha, the roll angle beta and the lifting distance H of the motion platform;
step 9, calculating the elongation Len_i of the electric cylinder according to the approximate solution of the pitch angle alpha and the roll angle beta
Step 10, obtaining a difference delta L_i between the electric cylinder elongation Len_i obtained in the step 9 and the actual electric cylinder elongation L_i;
step 11, obtaining the Sum Sum delta L of the elongation differences delta L_i of all the electric cylinders obtained in the step 10;
and step 12, performing accuracy correction of the approximate solution of the pitch angle alpha, the roll angle beta and the lifting distance H according to the delta L_i and the Sum delta L.
2. The method for controlling the pose of the motion platform based on the iterative step according to claim 1, wherein in the step 3, the initial inclination angle θ_i of the i-th electric cylinder is obtained according to the initial Z coordinate value of the lower hinge point and the initial Z coordinate value of the upper hinge point corresponding to the i-th electric cylinder, comprising:
θ_i = arcsin{(ZP_i - ZB_i)/L}
wherein ZB_i represents an initial Z coordinate value of a lower hinge point corresponding to the ith electric cylinder; ZP_i represents the initial Z coordinate value of the upper hinge point, and i is more than or equal to 1 and less than or equal to 3.
3. The method for controlling the pose of the motion platform based on the iterative step according to claim 2, wherein in the step 4, the step of obtaining the height h_i of the upper hinge point of the i-th electric cylinder according to the initial length of the i-th electric cylinder, the stroke of the electric cylinder, and the initial tilt angle θ_i of the electric cylinder includes:
H_i = (L_i+L+L_stroke/2)/cos(θ_i)
where L represents the initial length of the i-th electric cylinder, l_stroke represents the stroke of the i-th electric cylinder, and l_i represents the actual extension amount of the i-th electric cylinder.
4. The method for controlling the pose of the motion platform based on the iterative step according to claim 3, wherein in the step 5, the step of obtaining the height h_23 of the midpoint of the connection line between the upper hinge point 2 and the upper hinge point 3 includes:
H_23 = (H_2 + H_3)/2；
the step of obtaining the difference Δh_1 between the height of the upper hinge point 1 and the height of the h_23 includes:
△H_1 = (H_1 - H_23)。
5. the method for controlling the pose of a motion platform based on iterative step length according to claim 4, wherein in step 6, the step of obtaining the Y coordinate distance difference Δhor1 from the upper hinge point 1 to the midpoint of the upper hinge point 2 and the upper hinge point 3 includes:
△Hor1= YP_1 – (YP_2 - YP_3)/2 。
6. the method for controlling the pose of a motion platform based on iterative step according to claim 5, wherein in step 7, the step of obtaining the difference Δh_2 between the upper hinge point 2 and the upper hinge point 3 includes:
△H_2 = (H_2 - H_3)；
acquiring the distance difference Δhor2 of the X coordinates of the upper hinge point 2 and the upper hinge point 3 includes:
△Hor2 = XP_2 - XP_3 。
7. the method for controlling the pose of the moving platform based on the iterative step according to claim 6, wherein in the step 8, the approximate solution of the pitch angle α, the roll angle β and the lifting distance H of the moving platform is calculated, comprising:
α = arctan(△H_1/△Hor1)
β = arctan(△H_2/△Hor2)
H = (H_1 + H_2 + H_3)/3 – (L + L_stroke/2)。
8. the method for controlling the pose of a moving platform based on iterative step sizes according to claim 7, wherein in the step 12, the accuracy correction of the approximate solution of the pitch angle α, the roll angle β, and the lifting distance H is performed according to Δl_i and sum_Δl, and the method comprises:
logic 1: because the pitch angle α is positive, the electric cylinders No. 1 are extended, the electric cylinders No. 2 and 3 are contracted, the pitch angle α is reduced if Δl_1 is greater than 0, or Δl_2 and Δl_3 are simultaneously less than 0, otherwise the pitch angle α is increased;
logic 2: because the roll angle β is positive, the electric cylinder No. 3 is extended, the electric cylinder No. 2 is contracted, so if Δl_2 is less than 0 and Δl_3 is greater than 0, the roll angle β is decreased, otherwise the roll angle β is increased;
logic 3: if the platform only has pitching motion and rolling motion, the sum of the lengths of the three electric cylinders is equal to the sum of the lengths of the three electric cylinders in the middle position; thus, if Sum Δl is greater than 0, the lifting distance H is decreased, otherwise the lifting distance H is increased.
9. The utility model provides a motion platform position appearance controlling means based on iteration step, its characterized in that includes:
the first coordinate system construction module is used for establishing a global coordinate system CoordOXYZ, and a coordinate system origin O of the global coordinate system CoordOXYZ is positioned in a triangle plane formed by three lower hinge points and coincides with the high midpoint of a triangle passing through the lower hinge point 1; the Y axis points to the lower hinge point 1, the X axis points to one side of the lower hinge point 2, and the Z axis points vertically upwards;
for establishing a motion reference frame CoordO 1 X 1 Y 1 Z 1 A second coordinate system construction module of the motion reference coordinate system CoordO 1 X 1 Y 1 Z 1 Origin of coordinate system O of (2) 1 In the triangle plane formed by the three upper hinge points, the triangle plane coincides with the high middle point of the triangle passing through the upper hinge point 1; y is Y 1 Axis-directed upper hinge point 1, X 1 The axis is directed to the side of the upper hinge point 2, Z 1 The shaft is vertically directed upwards;
the electric cylinder initial inclination angle estimation module is used for acquiring an initial inclination angle theta_i of the ith electric cylinder according to the initial Z coordinate value of the lower hinge point corresponding to the ith electric cylinder and the initial Z coordinate value of the upper hinge point, wherein i represents the serial number of the electric cylinder;
a first height estimation module for acquiring the height H_i of the upper hinge point of the ith electric cylinder according to the initial length of the ith electric cylinder, the electric cylinder stroke and the electric cylinder initial inclination angle theta_i;
a second height estimation module for acquiring the height H_23 of the midpoint of the connecting line of the upper hinge point 2 and the upper hinge point 3 and accordingly acquiring the height difference delta H_1 between the height of the upper hinge point 1 and the height of the upper hinge point H_23;
the first difference value estimation module is used for acquiring a Y-coordinate distance difference delta Hor1 between the upper hinge point 1 and the midpoint of the connecting line of the upper hinge point 2 and the upper hinge point 3;
the second difference value estimation module is used for acquiring the height difference delta H_2 between the upper hinge point 2 and the upper hinge point 3 and the distance difference delta Hor2 between the upper hinge point 2 and the X coordinate of the upper hinge point 3;
the motion platform estimation module is used for calculating an approximate solution of a pitch angle alpha, a roll angle beta and a lifting distance H of the motion platform;
the electric cylinder elongation estimation module is used for calculating the electric cylinder elongation Len_i according to the approximate solution of the pitch angle alpha and the roll angle beta;
the electric cylinder extension difference value acquisition module is used for acquiring a difference DeltaL_i between the electric cylinder extension Len_i and the actual electric cylinder extension L_i;
the electric cylinder elongation difference total module is used for obtaining the Sum Sum_ delta L of all electric cylinder elongation differences delta L_i; and
and the motion platform pose correction module is used for correcting the precision of the approximate solutions of the pitch angle alpha, the roll angle beta and the lifting distance H according to the delta L_i and the Sum delta L.
10. A computer readable medium storing software, wherein the software comprises instructions executable by one or more computers which when executed by the one or more computers perform the process of the motion platform pose control method according to any of claims 1-8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311158793.5A CN116893627B (en) | 2023-09-08 | 2023-09-08 | Motion platform pose control method and device based on iteration step length and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311158793.5A CN116893627B (en) | 2023-09-08 | 2023-09-08 | Motion platform pose control method and device based on iteration step length and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116893627A true CN116893627A (en) | 2023-10-17 |
| CN116893627B CN116893627B (en) | 2023-12-08 |
Family
ID=88313744
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311158793.5A Active CN116893627B (en) | 2023-09-08 | 2023-09-08 | Motion platform pose control method and device based on iteration step length and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116893627B (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117601103A (en) * | 2023-12-06 | 2024-02-27 | 江苏普旭科技股份有限公司 | Swing pose inverse solution control method and system for three-degree-of-freedom parallel motion platform based on error correction and computer storage medium |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2000293094A (en) * | 1999-04-05 | 2000-10-20 | Honda Motor Co Ltd | Drive simulation device |
| US20030197482A1 (en) * | 2002-04-19 | 2003-10-23 | Murata Kikai Kabushiki Kaisha | Parallel link manipulator and its control device |
| EP1501065A1 (en) * | 2003-07-22 | 2005-01-26 | William Lai | Parallel kinematic hexapodal dynamic simulator |
| US20120209427A1 (en) * | 2011-02-16 | 2012-08-16 | Chang Jung Christian University | Manipulator with three degrees of freedom and control method for the same |
| CN105180965A (en) * | 2015-08-05 | 2015-12-23 | 陕西科瑞迪机电设备有限公司 | Triaxial ship attitude angle analogue simulation test platform |
| CN107478448A (en) * | 2017-09-13 | 2017-12-15 | 中国铁道科学研究院 | Circuit gauge detects dynamic simulation test system and method |
| CN108668545A (en) * | 2018-04-28 | 2018-10-19 | 江苏大学 | A kind of control device of tractor Three Degree Of Freedom farm implements suspension mechanism |
| CN108825941A (en) * | 2018-05-03 | 2018-11-16 | 长春工业大学 | A kind of Airborne Camera ground motion test device of multiaxis cooperative motion |
| WO2019006994A1 (en) * | 2017-07-07 | 2019-01-10 | 江苏科技大学 | Compensation method for two-degrees-of-freedom wave compensation system based on sea-sky line detection |
| CN116353875A (en) * | 2023-03-15 | 2023-06-30 | 中国人民解放军第六九O五工厂 | Unmanned aerial vehicle take-off and landing platform based on three-degree-of-freedom electric control leveling and leveling method |
-
2023
- 2023-09-08 CN CN202311158793.5A patent/CN116893627B/en active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2000293094A (en) * | 1999-04-05 | 2000-10-20 | Honda Motor Co Ltd | Drive simulation device |
| US20030197482A1 (en) * | 2002-04-19 | 2003-10-23 | Murata Kikai Kabushiki Kaisha | Parallel link manipulator and its control device |
| EP1501065A1 (en) * | 2003-07-22 | 2005-01-26 | William Lai | Parallel kinematic hexapodal dynamic simulator |
| US20120209427A1 (en) * | 2011-02-16 | 2012-08-16 | Chang Jung Christian University | Manipulator with three degrees of freedom and control method for the same |
| CN105180965A (en) * | 2015-08-05 | 2015-12-23 | 陕西科瑞迪机电设备有限公司 | Triaxial ship attitude angle analogue simulation test platform |
| WO2019006994A1 (en) * | 2017-07-07 | 2019-01-10 | 江苏科技大学 | Compensation method for two-degrees-of-freedom wave compensation system based on sea-sky line detection |
| CN107478448A (en) * | 2017-09-13 | 2017-12-15 | 中国铁道科学研究院 | Circuit gauge detects dynamic simulation test system and method |
| CN108668545A (en) * | 2018-04-28 | 2018-10-19 | 江苏大学 | A kind of control device of tractor Three Degree Of Freedom farm implements suspension mechanism |
| CN108825941A (en) * | 2018-05-03 | 2018-11-16 | 长春工业大学 | A kind of Airborne Camera ground motion test device of multiaxis cooperative motion |
| CN116353875A (en) * | 2023-03-15 | 2023-06-30 | 中国人民解放军第六九O五工厂 | Unmanned aerial vehicle take-off and landing platform based on three-degree-of-freedom electric control leveling and leveling method |
Non-Patent Citations (1)
| Title |
|---|
| 曹晓宁: "高速列车转向架测试台3-六自由度平台运动学及工作空间研究" * |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117601103A (en) * | 2023-12-06 | 2024-02-27 | 江苏普旭科技股份有限公司 | Swing pose inverse solution control method and system for three-degree-of-freedom parallel motion platform based on error correction and computer storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116893627B (en) | 2023-12-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN116893627B (en) | Motion platform pose control method and device based on iteration step length and storage medium | |
| CN109297666B (en) | Interstage separation wind tunnel test device and test method based on two sets of motion mechanisms | |
| US20220227195A1 (en) | Leveling control method for multi-point support platform | |
| US20130317646A1 (en) | Robot program changing device | |
| Wang et al. | Visual servoing of robots with uncalibrated robot and camera parameters | |
| CN109746915A (en) | A kind of kinematic method promoting industrial robot absolute fix precision | |
| CN105382843B (en) | Coordination control method for mechanical arm and operation platform in final stage of grabbing | |
| CN106094880B (en) | Based on 12 point-supported Vertical Launch platform stance leveling control methods | |
| CN110039548B (en) | Control method, device and equipment for assembling machine | |
| JP2022551218A (en) | dynamic planning controller | |
| CN113738390B (en) | Time-sharing migration space attitude fine-adjustment deviation-rectifying method of rectangular pipe jacking machine | |
| CN105183009A (en) | Trajectory control method for redundant mechanical arm | |
| CN103950552B (en) | Based on the digitalisation calibrating method of the aircraft target ship assembly deflections of six Shaft and NC Machining Test steady arms | |
| CN103921954B (en) | Based on the digitalisation calibrating method of the aircraft target ship assembly deflections of three-axis numerical control steady arm | |
| CN114523477A (en) | Joint pose calibration method, system and storage medium | |
| CN111553028B (en) | Rocket engine nozzle swing simulation analysis method and system | |
| CN107121930B (en) | Six Degree-of-Freedom Parallel Platform motion control method based on dummy model | |
| CN114237076B (en) | Method and controller for motion simulation of movable cable based on position dynamics | |
| CN113485459B (en) | Vehicle-mounted platform rapid leveling control method based on mechanical deformation interference compensation | |
| CN113263498B (en) | Mechanical arm movement control method and device, terminal equipment and storage medium | |
| CN112699506B (en) | Dynamics simulation verification method of photoelectric two-dimensional reflector | |
| CN113500584A (en) | Tail end error correction system and method of three-degree-of-freedom parallel robot | |
| CN116147881B (en) | Reset method of reset mechanism in wind tunnel six-component Tian-bang correction system | |
| CN116276907A (en) | Parallel six-degree-of-freedom motion platform with variable rigidity and stroke and motion control method thereof | |
| CN116652939A (en) | Calibration-free visual servo compliant control method for parallel robot |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |