CN116108701A - FAST novel feed cabin mechanism kinematics positive solution solving and control method - Google Patents
FAST novel feed cabin mechanism kinematics positive solution solving and control method Download PDFInfo
- Publication number
- CN116108701A CN116108701A CN202310375061.5A CN202310375061A CN116108701A CN 116108701 A CN116108701 A CN 116108701A CN 202310375061 A CN202310375061 A CN 202310375061A CN 116108701 A CN116108701 A CN 116108701A
- Authority
- CN
- China
- Prior art keywords
- lower platform
- cable
- pose
- rope
- parallel flexible
- 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
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F2111/00—Details relating to CAD techniques
- G06F2111/04—Constraint-based CAD
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F2111/00—Details relating to CAD techniques
- G06F2111/06—Multi-objective optimisation, e.g. Pareto optimisation using simulated annealing [SA], ant colony algorithms or genetic algorithms [GA]
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F2119/00—Details relating to the type or aim of the analysis or the optimisation
- G06F2119/14—Force analysis or force optimisation, e.g. static or dynamic forces
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Mathematical Physics (AREA)
- Mathematical Optimization (AREA)
- Data Mining & Analysis (AREA)
- General Engineering & Computer Science (AREA)
- Computational Mathematics (AREA)
- Mathematical Analysis (AREA)
- Pure & Applied Mathematics (AREA)
- Computer Hardware Design (AREA)
- Operations Research (AREA)
- Evolutionary Computation (AREA)
- Algebra (AREA)
- Geometry (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- Manipulator (AREA)
- Feedback Control In General (AREA)
Abstract
The invention discloses a method for solving and controlling kinematic positive solutions of a FAST novel feed cabin mechanism, and belongs to the field of astronomical equipment control. The solving method comprises the following steps: step S1, establishing a local coordinate system to describe the pose of a lower platform; s2, determining geometric parameters and structural parameters of the parallel flexible cable mechanism; step S3, defining a rope length constraint equation; step S4, establishing a kinematic model of the parallel flexible cable mechanism based on increment; and S5, setting a kinematic forward solution optimization objective function and solving. The control method comprises the following steps: calculating expected cable force and expected rope length through the expected pose of the lower platform by using an offline planning module based on a kinematic positive solution solving equation and a dynamic model; the rope length of the 6-set upper inhaul cable mechanism and the rope force of the 3-set lower inhaul cable mechanism are respectively controlled. The invention can realize the control of the FAST feed cabin mechanism under the maximum zenith angle of 50 degrees, meet the control precision of the pose of the lower platform, effectively adjust the internal force of the parallel flexible cable mechanism, improve the cable force performance and improve the anti-interference capability.
Description
Technical Field
The invention relates to the field of astronomical equipment control, in particular to a FAST novel feed cabin mechanism kinematics positive solution solving method and a lower platform pose force-position hybrid control method.
Background
500 m caliber spherical radio telescope (Five-handred-meter Aperture Spherical radioTelescope, FAST) is currently the single caliber radio telescope with the largest caliber and highest sensitivity in the world. The FAST system mainly comprises a deformable active reflecting surface, a six-cable parallel driving mechanism, a central feed cabin and the like. The feed cabin is internally provided with a star-shaped frame, an AB rotating shaft mechanism, a Stewart platform and other auxiliary mechanisms. The Stewart platform consists of an upper platform, a lower platform and 6 telescopic rods connected with each other, wherein the upper platform is connected with an AB rotating shaft, and the lower platform is used for installing a feed source receiver; the pose change of the lower platform relative to the upper platform is realized by controlling the lengths of 6 telescopic rods. The multi-beam feed source receiver is arranged on the lower platform, and the high-precision directional tracking observation of the feed source receiver on the celestial body is realized through fine adjustment control on the pose and the angle of the lower platform.
In order to effectively reduce the weight of the feed cabin, the observation angle of the telescope is improved to 50 degrees, and a flexible cable driving mechanism is used for replacing a Stewart rigid parallel platform and an AB rotary shaft mechanism. The method for the kinematic forward solution of the Stewart platform mainly comprises an analysis method, a numerical method and the like, but the kinematic forward solution solving method using the Stewart parallel platform in the prior art is not applicable to a parallel flexible cable mechanism because of the unique working mode and redundancy characteristics of the flexible cable driving mechanism, and the instantaneity and the solving precision of the kinematic forward solution cannot be ensured.
For the lower platform pose control of the novel feed cabin mechanism, only a kinematic control scheme for controlling the length of the rope is considered, and under the conditions that the dynamic model and the actual model are uncertain and the system is subject to external disturbance, the tension on the rope can not be ensured to be kept within the rope force limiting range required by the system at any time, and the feedback and control on the internal force of the system are required in the complete control scheme. Therefore, how to accurately model and solve the kinematic positive solution of the flexible cable-driven FAST novel feed cabin mechanism and a control scheme meeting the pose control precision requirement are problems to be solved in FAST engineering.
In view of this, the present invention has been made.
Disclosure of Invention
In order to overcome the defects of the prior art, the invention aims to provide a method for solving and controlling the kinematic positive solution of a FAST novel feed cabin mechanism, which realizes the solution of the kinematic positive solution of the FAST novel feed cabin mechanism through an incremental kinematic positive solution modeling and an optimization objective function based on complete rope length constraint; on the other hand, the control precision of the pose of the lower platform of the FAST novel feed cabin mechanism is met by performing kinematic control on each upper inhaul cable mechanism and performing force and position hybrid control on each lower inhaul cable mechanism, so that the problems in the prior art are solved.
The invention aims at realizing the following technical scheme:
a method for solving a novel FAST feed cabin mechanism kinematics positive solution comprises the following steps:
step S1, establishing a local coordinate system C system, a P system and an M system for describing the pose of a lower platform according to the connection relation between an outer cable mechanism of a FAST novel feed cabin mechanism and a feed cabin;
s2, determining geometric parameters and structural parameters of a parallel flexible cable mechanism serving as an inner cable mechanism of the FAST novel feed cabin mechanism according to the local coordinate system C system, the local coordinate system P system and the local coordinate system M system established in the step S1;
step S3, determining a rope length constraint equation met by the current pose of the lower platform according to the local coordinate systems C, P and M established in the step S1 and the geometric parameters and the structural parameters of the parallel flexible cable mechanism determined in the step S2;
step S4, establishing an incremental parallel flexible cable mechanism kinematics forward solution model according to the rope length constraint equation met by the current pose of the lower platform determined in the step S3, establishing a kinematics forward solution optimization objective function according to the incremental parallel flexible cable mechanism kinematics forward solution model, and establishing an incremental parallel flexible cable mechanism kinematics forward solution equation according to the kinematics forward solution optimization objective function;
and S5, solving an incremental parallel flexible cable mechanism kinematics positive solution solving equation constructed in the step S4 to obtain the current pose of the lower platform of the FAST novel feed cabin mechanism.
A FAST novel feed cabin mechanism control method comprises the following steps:
a1, before a FAST novel feed cabin mechanism operates, receiving a lower platform expected pose of the FAST novel feed cabin mechanism planned out of line;
a2, when the system runs, the lower platform of the FAST novel feed cabin mechanism is controlled in a force and position mixed control mode according to the received expected pose of the lower platform, and in the control process, the current pose of the lower platform is monitored through a visual tracking system until the control process is finished;
step A3, judging whether the visual tracking system fails or not in the process of monitoring the current pose of the lower platform through the visual tracking system, if so, executing the step A4, and if not, repeatedly executing the step A2;
and step A4, solving the current pose of the lower platform of the FAST novel feed cabin mechanism by adopting the method for solving the kinematic positive solution of the FAST novel feed cabin mechanism, re-planning the expected pose of the lower platform according to the current pose of the lower platform, and re-executing the step A2 according to the re-planned expected pose of the lower platform to perform force-position hybrid control on the lower platform.
Compared with the prior art, the FAST novel feed cabin mechanism kinematics positive solution solving and controlling method provided by the invention has the beneficial effects that:
Through solving and force position hybrid control based on incremental parallel flexible cable mechanism kinematics positive solution, the control under the maximum 50 zenith angle of the novel FAST feed cabin mechanism can be realized, the control precision of the platform pose under the novel FAST feed cabin mechanism is met, the internal force of the parallel flexible cable mechanism of the novel FAST feed cabin mechanism can be effectively regulated, the cable force performance of the parallel flexible cable mechanism is improved, and the anti-interference capability is improved.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings that are needed in the description of the embodiments will be briefly described below, it being obvious that the drawings in the following description are only some embodiments of the present invention, and that other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
Fig. 1 is a flow chart of a FAST novel feed cabin mechanism kinematic forward solution solving method.
Fig. 2 is a schematic coordinate diagram of a FAST novel feed cabin mechanism provided by the invention.
Fig. 3 is a top view of the FAST novel feed cabin mechanism and a representation diagram of the anchor point coordinates.
Fig. 4 is a side view of the FAST novel feed cabin mechanism and a representation diagram of geometric parameters.
Fig. 5 is an overall schematic diagram of the FAST novel feed cabin mechanism provided by the invention.
Fig. 6 is a flowchart of a FAST novel feed cabin mechanism control method provided by the invention.
Fig. 7 is a block diagram of a control system corresponding to the FAST novel feed cabin mechanism control method provided by the invention.
The component names corresponding to the marks in the figures are as follows: 1-a lower platform; a 2-PAF beam receiver; a 3-19 beam receiver; 4-a pull-up rope mechanism; a 5-connecting ring; 6-a pull-down cable mechanism; 7-a winding mechanism of a lower inhaul cable mechanism; 8-a winding mechanism of a pull-up rope mechanism; 9-an anchor point of the stay rope mechanism; 10-a stay rope mechanism anchoring point; 11-star frame.
Detailed Description
The following description of the embodiments of the present invention will be made in detail, but clearly understood to mean a portion of the embodiments, not all of the embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to fall within the scope of the invention. What is not described in detail in the embodiments of the present invention belongs to the prior art known to those skilled in the art.
The terms that may be used herein will first be described as follows:
the term "and/or" is intended to mean that either or both may be implemented, e.g., X and/or Y are intended to include both the cases of "X" or "Y" and the cases of "X and Y".
The terms "comprises," "comprising," "includes," "including," "has," "having" or other similar referents are to be construed to cover a non-exclusive inclusion. For example: including a particular feature (e.g., a starting material, component, ingredient, carrier, formulation, material, dimension, part, means, mechanism, apparatus, step, procedure, method, reaction condition, processing condition, parameter, algorithm, signal, data, product or article of manufacture, etc.), should be construed as including not only a particular feature but also other features known in the art that are not explicitly recited.
The term "consisting of … …" is meant to exclude any technical feature element not explicitly listed. If such term is used in a claim, the term will cause the claim to be closed, such that it does not include technical features other than those specifically listed, except for conventional impurities associated therewith. If the term is intended to appear in only a clause of a claim, it is intended to limit only the elements explicitly recited in that clause, and the elements recited in other clauses are not excluded from the overall claim.
Unless specifically stated or limited otherwise, the terms "mounted," "connected," "secured," and the like should be construed broadly to include, for example: the connecting device can be fixedly connected, detachably connected or integrally connected; can be mechanically or electrically connected; can be directly connected or indirectly connected through an intermediate medium, and can be communication between two elements. The specific meaning of the terms herein above will be understood by those of ordinary skill in the art as the case may be.
The terms "center," "longitudinal," "transverse," "length," "width," "thickness," "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," "outer," "clockwise," "counterclockwise," etc. refer to an orientation or positional relationship based on that shown in the drawings, merely for ease of description and to simplify the description, and do not explicitly or implicitly indicate that the apparatus or element in question must have a particular orientation, be constructed and operated in a particular orientation, and therefore should not be construed as limiting the present disclosure.
The method for solving and controlling the kinematic positive solution of the FAST novel feed cabin mechanism provided by the invention is described in detail below. What is not described in detail in the embodiments of the present invention belongs to the prior art known to those skilled in the art. The specific conditions are not noted in the examples of the present invention and are carried out according to the conditions conventional in the art or suggested by the manufacturer. The reagents or apparatus used in the examples of the present invention were conventional products commercially available without the manufacturer's knowledge.
The embodiment of the invention provides a method for solving a kinematic positive solution of a FAST novel feed cabin mechanism, which comprises the following steps:
step S1, establishing a local coordinate system C system, a P system and an M system for describing the pose of a lower platform according to the connection relation between an outer cable mechanism of a FAST novel feed cabin mechanism and a feed cabin;
s2, determining geometric parameters and structural parameters of a parallel flexible cable mechanism serving as an inner cable mechanism of the FAST novel feed cabin mechanism according to the local coordinate system C system, the local coordinate system P system and the local coordinate system M system established in the step S1;
step S3, determining a rope length constraint equation met by the current pose of the lower platform according to the local coordinate systems C, P and M established in the step S1 and the geometric parameters and the structural parameters of the parallel flexible cable mechanism determined in the step S2;
Step S4, establishing an incremental parallel flexible cable mechanism kinematics forward solution model according to the rope length constraint equation met by the current pose of the lower platform determined in the step S3, establishing a kinematics forward solution optimization objective function according to the incremental parallel flexible cable mechanism kinematics forward solution model, and establishing an incremental parallel flexible cable mechanism kinematics forward solution equation according to the kinematics forward solution optimization objective function;
and S5, solving an incremental parallel flexible cable mechanism kinematics positive solution solving equation constructed in the step S4 to obtain the current pose of the lower platform of the FAST novel feed cabin mechanism.
Referring to fig. 5, preferably, in the above method, the parallel flexible cable mechanism serving as the inner cable mechanism of the FAST new feed cabin mechanism comprises 9 sets of cable mechanisms, wherein the split cable arrangement is formed by 6 sets of upper cable mechanisms and 3 sets of lower cable mechanisms, the 6 sets of upper cable mechanisms are divided into 3 pairs, each pair of upper cable mechanisms are uniformly arranged at intervals of 120 degrees, and the 3 sets of lower cable mechanisms are uniformly arranged at intervals of 120 degrees and are staggered with each pair of upper cable mechanisms at intervals of 60 degrees; 6 sets of upper inhaul cable mechanisms are used for controlling the kinematics of the lower platform pose of the feed cabin, and 3 sets of lower inhaul cable mechanisms are used for controlling the kinematics of the lower platform pose;
The upper inhaul cable mechanisms and the lower inhaul cable mechanisms are identical in composition and comprise a hoisting mechanism, a steel wire rope, a servo motor, an encoder and rope anchoring points.
Preferably, in step S1 of the above method, a local coordinate system C, P and M describing the pose of the lower platform is established according to the connection relationship between the outer cable mechanism of the FAST new feed cabin mechanism and the feed cabin, including:
anchoring point S of external cable mechanism on feed cabin i Establishing a local coordinate system C of the feed cabin by taking the plane center as an origin C;
the upper surface of the following platform is anchored with point A i The method comprises the steps of establishing a local coordinate system P system of a lower platform by taking the plane center as an origin P;
establishing a centroid coordinate system M of the lower platform by taking a centroid position M of the lower platform as an origin, wherein the directions of coordinate axes of the M system are parallel to the P system;
when the lower platform is kept horizontal relative to the feed deck, the P, M and C systems are completely parallel.
Preferably, in the above method, in the step S2, the geometric parameters and structural parameters of the parallel flexible cable mechanism serving as the inner cable mechanism of the FAST new feed cabin mechanism are determined according to the local coordinate system C-system, P-system and M-system established in the step S1 in the following manner, including:
in the local coordinate system C, the anchor point B on the feed cabin i For the corresponding anchoring point of the ith stay rope on the star-shaped frame, D j Anchoring point B for j-th stay rope on star-shaped frame i And D j The distribution radius of (2) is r B Stay cable anchorage point D j Is of a fixed distribution height H D Stay cable anchor point B i Distribution height of (2)Is H Bi ;
Anchor point A of upper stay rope on lower platform i Is of distribution radius r A ;
In the local coordinate system P, the anchor point C of the lower stay rope on the lower platform j Are all fixed on the connecting ring of the lower platform, and the distribution radius is r C The distribution height is H C The method comprises the steps of carrying out a first treatment on the surface of the Anchor point B i 、D j 、A i 、C j Position coordinates of (a) C B i 、 C D j 、 P A i 、 P C j The method comprises the following steps of:
in the above formulae (1) to (4), T represents a transposed matrix; i is the number of upper guy wires, i=1, …,6; j is the number of down-cables, j=1, …,3;
anchoring point A in local coordinate systems M and C i 、C j Coordinates of (c) M A i 、 M C j 、 C A i 、 C C j The method comprises the following steps of:
in the above formulas (5) and (6), P A i is the anchorage point A under the local coordinate system P system i Coordinates of (c); P C j is the anchoring point C under the local coordinate system P system j Coordinates of (c);an offset vector from the M origin of the lower M center to the P origin, i.e. +.>;/>Is the position vector of the origin of the M line under the C line, i.e. +.>;/>For the rotation matrix of M-series versus C-series, < >>,/>For being in charge of the rotation matrix>Corresponding Euler angle vectors represent the attitude angles of the lower platform; M A i is an anchor point A under a local coordinate system M system i Coordinates of (c); />The P-series and M-series rotation matrices are fixed on the lower platform and parallel to each otherIs a unit matrix; M C j is the anchoring point C under the local coordinate system M system j Is defined by the coordinates of (a).
Preferably, in step S3 of the above method, the determining the rope length constraint equation satisfied by the current pose of the lower platform according to the local coordinate systems C, P and M established in step S1 and the geometric parameters and structural parameters of the parallel flexible cable mechanism determined in step S2 includes:
the rope of the parallel flexible rope mechanism comprises an upper inhaul cable and a lower inhaul cable, and the length of the upper inhaul cableAnd rope length of the lower dragline->The method comprises the following steps of:
in the above formulas (7) and (8), the formula (I) is represented byThe length vector of the upper guy rope mechanism isThe method comprises the steps of carrying out a first treatment on the surface of the By->The length vector of the obtained down-cable mechanism is +.>;
In the above formulas (9) and (10), T represents a transposed matrix; M C j is the anchoring point C under the local coordinate system M system j Coordinates of (c);
lower platform current poseRepresented by the position and attitude angle of the M-line relative to the C-line, wherein>For Euler angle vector of current pose of lower platform in C system, determining current pose of lower platform according to Euler angle vector>The satisfied rope length constraint equation is:
in the above formulas (11) and (12),、/>the rope length of the upper inhaul cable and the rope length of the lower inhaul cable at the previous moment are respectively; p (P) 0 An initial position vector expressed as the origin of the M system under the C system; />For being in charge of the rotation matrix>Corresponding initial euler angle vectors; C B i is an anchor point B under a local coordinate system C system i Coordinates of (c); C D j is the anchorage point D under the local coordinate system C system j Is defined by the coordinates of (a).
Preferably, in the above method, in step S4, an incremental parallel flexible cable mechanism kinematic forward solution model is established according to a rope length constraint equation satisfied by the current pose of the lower platform determined in step S3, including:
the rope length constraint equations of (11), (12) are set in the previous pose of the lower platformPerforming Taylor expansion, and reserving until a second order term to obtain an incremental parallel flexible rope mechanism kinematic forward model of the following formula (13):
in the previous pose formula of the lower platform and (13),is the position vector of the previous moment; />Is the Euler angle vector of the previous moment; t is a transposed matrix; />、/>The rope length of the upper inhaul cable and the rope length of the lower inhaul cable at the current moment are respectively +.>、The rope length of the upper inhaul cable and the rope length of the lower inhaul cable at the previous moment are respectively; />For the increment of the lower platform pose->And->The position increment of the lower platform and the attitude angle increment of the lower platform are respectively; />Indicating the length of the upper guy cable at the current time >Before the lower platform, the position X P Partial derivative of lower platform position vector P, < >>Representing the partial derivative; />Indicating the length of the upper guy cable at the current time>In the position of the next previous platform X P Pair and rotation matrix->Partial derivatives of the corresponding euler angle vectors Φ; />Indicating the length of the upper guy cable at the current time>Before the lower platform, the position X P Position vector P and rotation matrix of the lower platform>The second partial derivative of the corresponding euler angle vector Φ; />Indicating the length of the downcable at the current time>Before the lower platform, the position X P Partial derivative of the pair with the lower platform position vector P; />Indicating the length of the downcable at the current time>Before the lower platform, the position X P Pair of parts and pair of partsRotation matrix->Partial derivatives of the corresponding euler angle vectors Φ; />Indicating the length of the downcable at the current time>Before the lower platform, the position X P Position vector P and rotation matrix of the lower platform>The second partial derivative of the corresponding euler angle vector Φ.
Preferably, in step S4 of the above method, the establishing a kinematic forward solution optimization objective function according to the kinematic forward solution model of the incremental parallel flexible cable mechanism, and the establishing an incremental kinematic forward solution equation of the parallel flexible cable mechanism according to the kinematic forward solution optimization objective function includes:
In the running process of the FAST novel feed cabin mechanism, the initial pose of the lower platform is accurately measured by an external sensor, and when the kinematic correct solution is solved each time, the previous pose of the lower platform is obtainedIs a known value, the rope length corresponding to the previous pose of the lower platform is +.>Calculated by the above formulas (11) and (12); solving the current pose of the lower platform through kinematic positive solution according to the following mode: measuring rope length +.>Calculating the pose increment of the lower platform by using the rope length at the current moment>The current pose X of the lower platform is obtained through calculation of a formula (14):
in the above-mentioned formula (14),and->The position increment of the lower platform and the attitude angle increment of the lower platform are respectively, and according to the rope length constraint equations of the formulas (11) and (12), the kinematic positive solution optimization objective function of the following formula (15) is obtained as follows:
establishing an incremental parallel flexible cable mechanism kinematic positive solution solving equation of the following formula (16) based on the kinematic positive solution optimizing objective function of the formula (15):
the parameters in the above formula (16) have the same meanings as the corresponding parameters in the above formula (13).
Preferably, in the above method, the method reduces the solution error of the solution equation of the kinematic positive solution of the incremental parallel flexible cable mechanism by the following method, including:
in the previous pose of the same lower platform When Taylor expansion is carried out on the position, the current pose of the lower platform obtained by solving each positive solution is used for +.>Carrying out Taylor expansion again, and carrying out next solving again in the updating mode (16);
in the method, the solution speed of the kinematic forward solution equation of the incremental parallel flexible cable mechanism is accelerated by the following steps:
solving to obtain the current pose of the lower platform in each positive solutionPerforming Taylor expansion again according to formula (16), and introducing the matrix of formula (16)>、/>Storing; when updating Taylor expansion each time, the current actual pose of the lower platform is +.>Carry to->、/>。
As shown in fig. 6, the embodiment of the present invention further provides a FAST novel feed cabin mechanism control method, including:
a1, before a FAST novel feed cabin mechanism operates, receiving a lower platform expected pose of the FAST novel feed cabin mechanism planned out of line;
a2, when the system runs, the lower platform of the FAST novel feed cabin mechanism is controlled in a force and position mixed control mode according to the received expected pose of the lower platform, and in the control process, the current pose of the lower platform is monitored through a visual tracking system until the control process is finished;
step A3, judging whether the visual tracking system fails or not in the process of monitoring the current pose of the lower platform through the visual tracking system, if so, executing the step A4, and if not, repeatedly executing the step A2;
And A4, solving the current pose of the lower platform of the FAST novel feed cabin mechanism by adopting the FAST novel feed cabin mechanism kinematics positive solution solving method, re-planning the expected pose of the lower platform according to the current pose of the lower platform, and re-executing the step A2 according to the re-planned expected pose of the lower platform to perform force-position hybrid control on the lower platform.
Preferably, in step A2 of the above control method, the control of the lower platform of the FAST new feed cabin mechanism by the hybrid control method of the received desired pose of the lower platform includes:
solving equations and dynamic models through kinematic positive solutions of incremental parallel flexible cable mechanisms, and utilizing received expected pose of lower platformRespectively calculating and obtaining expected rope length L of parallel flexible rope mechanisms of the platform under the drive of the FAST novel feed cabin mechanism d And the expected cable force T d ;
According to the obtained expected rope length L d Controlling the rope length of the 6-set upper stay rope mechanism of the parallel flexible rope mechanism and according to the obtained expected rope force T d And controlling the cable force of the 3 sets of lower cable mechanisms of the parallel flexible cable mechanism.
Preferably, in the control method, the received desired pose of the lower platform is utilized by solving an equation through kinematic positive solutions of incremental parallel-flexible-rope mechanisms in the following manner Calculating to obtain expected rope length L of parallel flexible rope mechanism of FAST novel feed cabin mechanism d Comprising:
the kinematic positive solution equation of the incremental parallel flexible cable mechanism is expressed as formula (16):
the meaning of each parameter in the formula (16) is the same as that of each corresponding parameter in the formula (16) in the FAST novel feed cabin mechanism kinematics positive solution method;
according to the length of the rope at the previous momentSolving the pose increment of the lower platform>Then combine the former pose of the lower platform>Solving the current pose of the lower platform>The expected rope length L is obtained according to the current pose of the lower platform d ;
Utilizing the received lower platform desired pose by parallel-flex mechanism dynamics model in the following mannerCalculating and obtaining expected cable force T of the parallel flexible cable mechanism d Comprising:
the parallel flexible rope mechanism dynamic model is (17):
determining expected cable force T of parallel flexible cable mechanism through dynamic model of parallel flexible cable mechanism of (17) d A cable force optimization function satisfying the following equation (18):
in the above formulae (17) and (18),the pose is expected for the lower platform; />A first order derivative of the expected pose of the lower platform is used for representing the speed variable of the lower platform; />A second derivative of the expected pose of the lower platform is used for representing the acceleration variable of the lower platform; / >;/>;/>;J d The matrix is a jacobian matrix corresponding to the parallel flexible cable mechanism; j (J) dT The device is a transpose of a jacobian matrix corresponding to the parallel flexible cable mechanism; m is m P For the lower platform quality, I 3×3 Is a unitary matrix, 0 3×3 Is an all-zero matrix, 0 3×1 Is an all-zero vector, g is a gravitational acceleration vector, < ->For a rotation matrix of the local coordinate system C system to the global coordinate system G system,/for>For an antisymmetric matrix corresponding to the angular velocity omega, C I n for the inertia matrix of the lower platform in C series, < >>, M I n For the inertia matrix of the lower platform in the M system, < >>For the rotation matrix of M-series versus C-series, < >>A transposed matrix of the rotation matrix of the M system relative to the C system; parameters corresponding to the subscript S and the subscript U are parameters corresponding to 6 sets of upper inhaul cable mechanisms and parameters corresponding to 3 sets of lower inhaul cable mechanisms respectively; />The matrix is a jacobian matrix corresponding to the upper inhaul cable mechanism;the matrix is a jacobian matrix corresponding to the lower guy cable mechanism; />
Based on the expected pose of the lower platform through the determined cable force optimization functionDesired cable force T for a given parallel flex cable mechanism d ;
Determining the expected cable force of the down cable mechanism according to the redundancy of the parallel flexible cable mechanismAfter that, the desired cable force of the cable-up mechanism +.>The unique determination is as follows:
in the above-mentioned formula (19),the cable-lifting mechanism corresponds to the transposition of the jacobian matrix; / >The device is a transposition of a jacobian matrix corresponding to the lower inhaul cable mechanism; the other parameters have the same meanings as those of the corresponding parameters in the above formulas (17) and (18).
In actual control, when the expected pose of the lower platform is given, the expected rope length is obtained by solving an equation through kinematic positive solution of an incremental parallel flexible rope mechanismActual rope length +.>Feeding back through an encoder;
tracking 6 the desired rope length of the pull-up rope mechanism 4 by control law in the position mode of the driveFor the current pose of the lower platform +.>Performing control;
the motor of the 6-set stay rope mechanism 4 works in a position mode, and controls the rope length by using a PID controller, and a position increment signal sent to a motor driver of the stay rope mechanism 4 is as follows:
wherein, the liquid crystal display device comprises a liquid crystal display device,、/>、/>diagonal coefficient matrixes of proportional term, integral term and differential term of the controller respectively;
real-time feedback of actual cable force through cable force sensorWhen the expected pose of the lower platform is given, the dynamics model and the cable force optimization function are combined, and the expected cable force of 3 sets of lower cable mechanisms 6 is calculated>And converts the torque signal into a motor which sends the torque signal to the 3 sets of lower guy rope mechanisms 6, and the cable force of the 3 sets of lower guy rope mechanisms 6 is +.>Control is carried out to realize the rope force of all the inhaul ropes of the parallel flexible rope mechanism >Is controlled by (a);
the motor of the 3 sets of down-cable mechanisms 6 is arranged to work in a position mode, the force-position hybrid controller is used for controlling the length and the force of the rope respectively, and the position increment signals sent to the motor driver of the down-cable mechanisms 6 are as follows:
wherein, the liquid crystal display device comprises a liquid crystal display device,、/>、/>diagonal coefficient matrixes of proportional term, integral term and differential term corresponding to the rope length error respectively; />And the diagonal coefficient matrix is a proportional term corresponding to the cable force error.
According to the invention, through incremental type parallel flexible cable mechanism kinematic forward solution and force position hybrid control, control of the FAST feed cabin mechanism under the maximum zenith angle of 50 degrees can be realized, the control precision of the lower platform pose of the FAST feed cabin mechanism is met, the internal force of the parallel flexible cable mechanism can be effectively regulated, the cable force performance of the parallel flexible cable mechanism is improved, and the anti-interference capability is improved.
In order to clearly show the technical scheme and the technical effects, the method for solving and controlling the positive kinematics of the novel FAST feed cabin mechanism provided by the embodiment of the invention is described in detail in the following by using a specific embodiment.
Example 1
The embodiment provides a method for solving kinematic positive solutions of a FAST novel feed cabin mechanism, which comprises the following steps:
Step S1, establishing a local coordinate system C, P and M to describe the pose of a lower platform of the FAST novel feed cabin mechanism, wherein as shown in FIG. 2, an external cable mechanism anchors a point S on the feed cabin i Establishing a local coordinate system C, y of a feed source cabin by taking the plane center as an origin C C The axis is S 5 Point to the direction of origin C, z C The axis is perpendicular to the feed cabin anchoring point S i Plane up, x C The axis being perpendicular to C-y C z C A plane; p-x of local coordinate P system of lower platform P y P Built under the platform1 upper surface, with anchor point A i The center of the plane is the origin P, x P 、y P The direction of the axis is shown in FIG. 2, z P Anchor point A of inhaul cable mechanism with shaft perpendicular to feed cabin i The plane is upward, a centroid coordinate system M system of the lower platform 1 is established, an origin M of the M system is positioned at the centroid position of the lower platform 1, directions of coordinate axes of the M system are parallel to the P system, and when the lower platform 1 keeps horizontal relative to the feed cabin, the P system, the M system and the C system are completely parallel. In the following, when the kinematic analysis in the feed cabin is performed, the coordinate system of the position and attitude angle references of the lower platform is selected as the coordinate system C.
And S2, determining geometric parameters and structural parameters of the parallel flexible cable mechanism. As shown in fig. 3, 4 and 5, the anchor point B on the feed deck is in the local coordinate system C i For the corresponding anchoring point of the ith strand upper stay rope mechanism on the star-shaped frame 11, D j Is the anchoring point of the j-th strand lower inhaul cable mechanism 6 on the star-shaped frame 11, and the distribution radius is r B The method comprises the steps of carrying out a first treatment on the surface of the When the structure reconstruction of the parallel flexible cable mechanism according to the task scene is considered, the anchoring point B i D because of the difference of the control distribution heights of the sliding rails j The distribution heights are fixed, and are respectively H Bi 、H D . Anchor point A of upper inhaul cable mechanism on lower platform i 9 has a distribution radius r A The method comprises the steps of carrying out a first treatment on the surface of the In the local coordinate system P, the anchor point C of the lower guy rope mechanism 6 on the lower platform i 10 are all fixed on the circumference of the upper part of the round table, and the distribution radius is r C The distribution height is H C . The position coordinates of these anchor points are expressed as:
in the above formulas, the upper left letter indicates a coordinate system to which the coordinates or vectors refer, for example: P A i is the anchoring point A of the lower platform 1 in the P system i Is used for the purpose of determining the coordinates of (a), C B i is the anchor point B of the upper stay rope mechanism in the C system i Coordinates of (c);
by using、/>The rotation matrices of the P-series relative to the M-series and the M-series relative to the C-series are respectively shown. Since the P-series and M-series are fixed on the lower platform 1 and parallel to each other, the two systems are +.>Is an identity matrix. Anchor point a in local coordinates M and C i 、C i The coordinates of (2) are:
in the above-mentioned description of the invention,an offset vector from the M origin of the lower M centroid to the P origin, i.e. +.>;/>Is the position vector of the origin of the M line under the C line, i.e. +. >;/>For being in charge of the rotation matrix>Corresponding Euler angle vector。/>
And S3, defining a rope length constraint equation, and analyzing rope length constraints satisfied by the rope. As shown in fig. 3, the parallel flexible cable mechanism is divided into an upper cable mechanism 4 and a lower cable mechanism 6, namely a in fig. 3 respectively i B i And C j D j Their rope lengths are respectively:
in the above formulas (7) and (8),
at this time, the length vector of the parallel flexible rope mechanism can be recorded as,/>、The length vectors of the upper inhaul cable mechanism and the lower inhaul cable mechanism are respectively.
The current pose of the lower platform is represented by the position and the attitude angle of the M system relative to the C system, and the pose is recorded asWherein->The Euler angle vector in the coordinate system C is the lower platform attitude. Anchor point B i Height H of (2) Bi Can be directly read by a sensor arranged on the sliding rail, and the anchoring point D j Height H of (2) D Is a fixed parameter. I.e. each time a kinematic forward solution is performed, C B i 、 C D j the coordinates are known, at this time the theoretical value of rope length +.>Given by the above formulas (5) - (10); rope length vector of stay rope mechanismAnd the rope length vector of the lower inhaul cable mechanism>And the actual value of the total length of the rope +.>Can initiate the pose through the lower platform>And (5) combining the motor encoder readings. At this time, the current pose of the lower platform +.>The satisfied rope length constraint equation is as follows:
And S4, establishing an incremental parallel flexible rope mechanism kinematic forward model. As the parallel flexible rope mechanism moves slowly in actual operation, the maximum traction speed of the feed cabin is 24mm/s, and the maximum rotation speed of the pitch angle is 1.2 multiplied by 10 -3 rad/s. Based on the hypothesized rope length constraint equation, the position of the lower platform in front of the previous positionPerforming Taylor expansion, and reserving until a second order term to obtain an incremental parallel flexible rope mechanism kinematic forward model of the following formula (13):
the meaning of each parameter in the above formula (13) is the same as that of each parameter in the above formula (13), and is not repeated here.
In the actual process, the initial pose of the lower platform 1 can be accurately measured through an external sensor. Therefore, each time the kinematic positive solution is solved, the previous pose of the lower platformThe rope length corresponding to this is known +.>Calculated by formulas (11) and (12) in the step S3; the current pose of the lower platform is obtained through kinematic positive solution according to the following mode: measuring rope length +.>Calculating the pose increment of the lower platform by using the rope length at the current momentThe current pose X of the lower platform is calculated according to the following formula (14):
step S5, establishing an optimization problem according to a rope length constraint equation to solve the kinematic positive solution, and obtaining the following kinematic positive solution optimization objective function according to constraint conditions of formulas (11) and (12):
The meaning of each parameter in the above formula (15) is the same as that of each corresponding parameter in the above formula (13), and is not repeated here;
based on the kinematic positive solution optimization objective function of the formula (15), constructing an incremental parallel flexible cable mechanism kinematic positive solution solving equation of the following formula (16):
the meaning of each parameter in the above formula (16) is the same as that of each corresponding parameter in the above formula (13), and is not repeated here.
The solving method can adopt the following mode (1) to reduce solving errors and adopts the following mode (2) to accelerate solving speed:
(1) In the previous pose of the same lower platformWhen Taylor expansion is carried out, along with +.>、/>The errors of the kinematic positive solution are obviously accumulated. Therefore, the current pose of the lower platform obtained by solving each time of positive solutionThe Taylor expansion is carried out again, and the next solving is carried out again by the updating (16).
(2) Lower platform current pose obtained by solving at each time of positive solutionThe Taylo is performed againr, expanding, so that the solving precision of an incremental kinematic algorithm can be effectively improved; however, the additional calculation of Taylor expansion causes the algorithm time to increase dramatically, so that the sign form of Taylor expansion can be stored, that is, the matrix +. >、/>Storing; when updating Taylor expansion each time, only the current pose of the lower platform is required to be +.>Carry to->、/>Thereby reducing computation time.
Example 2
The embodiment provides a control method of a FAST novel feed cabin mechanism, which is a force-position hybrid control method for kinematics-dynamics of parallel flexible cable mechanisms of the FAST novel feed cabin mechanism, wherein the lower platform pose is kinematically controlled by 6 sets of upper cable mechanisms, and the dynamics is controlled by 3 sets of lower cable mechanisms 6, so that the internal force of the parallel flexible cable mechanisms can be effectively regulated, the cable force performance is improved, and the anti-interference capability is improved.
As shown in fig. 5, the parallel flexible cable mechanism is divided into 9 sets of cable mechanisms, and comprises 6 sets of upper cable mechanisms 4 and 3 sets of lower cable mechanisms 6 which form a pair-pull type layout to drive the lower platform 1; the upper inhaul cable mechanisms 4 are divided into 3 pairs, each pair is evenly arranged at an interval of 120 degrees, and 3 sets of lower inhaul cable mechanisms 6 are equally spaced at an interval of 120 degrees and are staggered with each pair of upper inhaul cable mechanisms 4 at an interval of 60 degrees; the 6 sets of the cable-pulling mechanisms 4 consist of cable-pulling mechanism hoisting mechanisms 8, cable-pulling mechanism steel wires and cable-pulling mechanism anchoring points 9; the 3 sets of lower inhaul cable mechanisms 6 consist of lower inhaul cable mechanism hoisting mechanisms 7, lower inhaul cable mechanism steel wire ropes and lower inhaul cable mechanism anchoring points 10; the hoisting mechanisms of the upper inhaul cable mechanism 4 and the lower inhaul cable mechanism 6 are composed of a servo motor, an encoder, a winding drum and the like.
In the control system of the lower platform pose shown in fig. 7, the offline planning module is responsible for solving equations and dynamic models based on kinematic positive solutions, and calculating expected cable force through the lower platform expected poseDesired rope length +.>The method comprises the steps of carrying out a first treatment on the surface of the The kinematic control module and the dynamic control module respectively realize the rope length control of the 6-set upper inhaul cable mechanism 4 and the rope force control of the 3-set lower inhaul cable mechanism 6, and concretely realize the following:
solving an equation based on the kinematic positive solution of the formula (16), and according to the length of the rope at the current momentSolving the pose increment of the lower platform>Then combine the former pose of the lower platform>Solving the current pose of the lower platformSolving the expected rope length according to the current pose of the lower platform>;
The kinematic positive solution equation of formula (16) is identical to the kinematic positive solution equation of the incremental parallel-flexible-wire mechanism of formula (16) of embodiment 1, and the corresponding parameters have the same meaning and are not repeated here.
Determining the expected rope force of the rope based on the parallel flexible rope mechanism dynamics model of (17)Satisfy formula (18):
the meaning of each parameter in the above formulae (17) and (18) is the same as the meaning of each parameter corresponding to the above formulae (17) and (18), and is not repeated here;
by means of a given cable force optimization function, the expected cable force T of the cable is given based on the expected pose of the lower platform d The method comprises the steps of carrying out a first treatment on the surface of the Due to the redundancy of the parallel flexible cable mechanisms, a cable force is expected for a given down cable mechanism 6After that, the cable force is desired by the cable-up mechanism 4>The unique determination is as follows:
in the above-mentioned formula (19),the cable-lifting mechanism corresponds to the transposition of the jacobian matrix; />The device is a transposition of a jacobian matrix corresponding to the lower inhaul cable mechanism; the other parameters have the same meanings as those of the corresponding parameters in the above formulas (17) and (18), and are not repeated here.
The kinematics control of the upper inhaul cable mechanism can obtain the expected rope length and the actual rope length based on the incremental parallel flexible cable mechanism kinematics positive solution solving equation when the expected pose of the lower platform is givenFeedback can be provided by the encoder; tracking 6-packs by control law in position mode of driveDesired rope length of the guy rope mechanism>Thereby realizing the current pose of the lower platform>Is controlled by the rope length. The 6 sets of the motor of the cable-stayed mechanism is arranged to work in a position mode, a PID controller is used for controlling the length of the cable, and a position increment signal sent to a motor driver of the cable-stayed mechanism is as follows: />
Wherein, the liquid crystal display device comprises a liquid crystal display device,、/>、/>the diagonal coefficient matrix is the proportional term, integral term and differential term of the controller. On the basis of the kinematic control of the 6-set upper inhaul cable mechanism 4, the rope force of the 3-set lower inhaul cable mechanism 6 is controlled to be +. >Tracking desired cable force +.>Can effectively regulate and control 4 cable force of the upper cable mechanism of the parallel flexible cable mechanism>Improving the cable force of the parallel flexible cable mechanism>Thereby improving the anti-interference capability.
The dynamics control of the lower inhaul cable mechanism 6 and the actual cable forceBy means of real-time feedback of the cable force sensor, for the expected pose of a given lower platform, the expected cable force ++of 3 sets of lower cable mechanisms 4 can be calculated by combining the dynamic model and the cable force distribution function>And converts the torque signal into a torque signal to be sent to a motor, and controls the cable force of 3 sets of down-cable mechanisms 6 in real time>Thereby realizing the rope force of the parallel flexible rope mechanism>Is controlled by the control system. The motor of the 3 sets of lower inhaul cable mechanisms 6 is arranged to work in a position mode, the force and position mixed control controller is used for controlling the rope length and the rope force respectively, and the position increment signals sent to the motor driver of the lower inhaul cable mechanisms 6 are as follows:
wherein, the liquid crystal display device comprises a liquid crystal display device,、/>、/>the diagonal coefficient matrix of the proportional term, the integral term and the differential term corresponding to the rope length error; />And the diagonal coefficient matrix is a proportional term corresponding to the cable force error.
Those of ordinary skill in the art will appreciate that: all or part of the flow of the method implementing the above embodiments may be implemented by a program to instruct related hardware, where the program may be stored in a computer readable storage medium, and the program may include the flow of the embodiment of each method as described above when executed. The storage medium may be a magnetic disk, an optical disk, a Read-Only Memory (ROM), a random access Memory (Random Access Memory, RAM), or the like.
The above embodiments are only for illustrating the technical solution of the present invention, and are not limiting; although the invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical scheme described in the foregoing embodiments can be modified or some technical features thereof can be replaced by equivalents; such modifications and substitutions do not depart from the spirit and scope of the technical solutions of the embodiments of the present invention.
Claims (10)
1. A method for solving a novel FAST feed cabin mechanism kinematic positive solution is characterized by comprising the following steps:
step S1, establishing a local coordinate system C system, a P system and an M system for describing the pose of a lower platform according to the connection relation between an outer cable mechanism of a FAST novel feed cabin mechanism and a feed cabin;
s2, determining geometric parameters and structural parameters of a parallel flexible cable mechanism serving as an inner cable mechanism of the FAST novel feed cabin mechanism according to the local coordinate system C system, the local coordinate system P system and the local coordinate system M system established in the step S1;
step S3, determining a rope length constraint equation met by the current pose of the lower platform according to the local coordinate systems C, P and M established in the step S1 and the geometric parameters and the structural parameters of the parallel flexible cable mechanism determined in the step S2;
Step S4, establishing an incremental parallel flexible cable mechanism kinematics forward solution model according to the rope length constraint equation met by the current pose of the lower platform determined in the step S3, establishing a kinematics forward solution optimization objective function according to the incremental parallel flexible cable mechanism kinematics forward solution model, and establishing an incremental parallel flexible cable mechanism kinematics forward solution equation according to the kinematics forward solution optimization objective function;
and S5, solving an incremental parallel flexible cable mechanism kinematics positive solution solving equation constructed in the step S4 to obtain the current pose of the lower platform of the FAST novel feed cabin mechanism.
2. The method for solving the positive kinematics solution of the novel FAST feed cabin mechanism according to claim 1, wherein the parallel flexible cable mechanism serving as an inner cable mechanism of the novel FAST feed cabin mechanism comprises 9 sets of cable mechanisms, wherein the cable mechanisms are formed into a pair-pull type layout by 6 sets of upper cable mechanisms and 3 sets of lower cable mechanisms, the 6 sets of upper cable mechanisms are divided into 3 pairs, each pair of upper cable mechanisms is uniformly arranged at an interval of 120 degrees, and the 3 sets of lower cable mechanisms are uniformly arranged at an interval of 120 degrees and are staggered with each pair of upper cable mechanisms at an interval of 60 degrees; 6 sets of upper inhaul cable mechanisms are used for controlling the kinematics of the lower platform pose of the feed cabin, and 3 sets of lower inhaul cable mechanisms are used for controlling the kinematics of the lower platform pose;
The upper inhaul cable mechanisms and the lower inhaul cable mechanisms are identical in composition and comprise a hoisting mechanism, a steel wire rope, a servo motor, an encoder and rope anchoring points.
3. The method for solving the kinematic orthographic solution of the FAST new type feed cabin mechanism according to claim 1 or 2, wherein in the step S1, a local coordinate system C, P and M describing the pose of the lower platform is established according to the connection relationship between the outer cable mechanism of the FAST new type feed cabin mechanism and the feed cabin, and the method comprises the following steps:
anchoring point S of external cable mechanism on feed cabin i Establishing a local coordinate system C of the feed cabin by taking the plane center as an origin C;
the upper surface of the following platform is anchored with point A i The method comprises the steps of establishing a local coordinate system P system of a lower platform by taking the plane center as an origin P;
establishing a centroid coordinate system M of the lower platform by taking a centroid position M of the lower platform as an origin, wherein the directions of coordinate axes of the M system are parallel to the P system;
when the lower platform is kept horizontal relative to the feed deck, the P, M and C systems are completely parallel.
4. The method according to claim 1 or 2, wherein in the step S2, the geometric parameters and the structural parameters of the parallel flexible cable mechanism serving as the inner cable mechanism of the FAST new feed cabin mechanism are determined according to the local coordinate systems C-system, P-system and M-system established in the step S1 in the following manner, and the method comprises the following steps:
In the local coordinate system C, the anchor point B on the feed cabin i For the corresponding anchoring point of the ith stay rope on the star-shaped frame, D j Anchoring point B for j-th stay rope on star-shaped frame i And D j The distribution radius of (2) is r B Stay cable anchorage point D j Is of a fixed distribution height H D Stay cable anchor point B i Is distributed with the height of H Bi ;
Anchor point A of upper stay rope on lower platform i Is of distribution radius r A ;
In the local coordinate system P, the anchor point C of the lower stay rope on the lower platform j Are all fixed on the connecting ring of the lower platform, and the distribution radius is r C The distribution height is H C The method comprises the steps of carrying out a first treatment on the surface of the Anchor point B i 、D j 、A i 、C j Position coordinates of (a) C B i 、 C D j 、 P A i 、 P C j The method comprises the following steps of:
in the above formulae (1) to (4), T represents a transposed matrix; i is the number of upper guy wires, i=1, …,6; j is the number of down-cables, j=1, …,3;
anchoring point A in local coordinate systems M and C i 、C j Coordinates of (c) M A i 、 M C j 、 C A i 、 C C j The method comprises the following steps of:
in the above formulas (5) and (6), P A i is the anchorage point A under the local coordinate system P system i Coordinates of (c); P C j is the anchoring point C under the local coordinate system P system j Coordinates of (c);an offset vector from the M origin of the lower M center to the P origin, i.e. +.>;/>Is the position vector of the origin of the M line under the C line, i.e. +.>;/>For the rotation matrix of M-series versus C-series, < >>,/>For being in charge of the rotation matrix>Corresponding Euler angle vectors represent the attitude angles of the lower platform; M A i Is an anchor point A under a local coordinate system M system i Coordinates of (c); />The P-series and M-series rotation matrices are fixed on the lower platform and parallel to each other, so +.>Is a unit matrix; M C j is the anchoring point C under the local coordinate system M system j Coordinates of (c);
in the step S3, a rope length constraint equation satisfied by the current pose of the lower platform is determined according to the local coordinate systems C, P and M established in the step S1 and the geometric parameters and structural parameters of the parallel flexible cable mechanism determined in the step S2, and the method includes:
the rope of the parallel flexible rope mechanism comprises an upper inhaul cable and a lower inhaul cable, and the length of the upper inhaul cableAnd rope length of the lower dragline->The method comprises the following steps of:
in the above formulas (7) and (8), the formula (I) is represented byThe length vector of the upper guy rope mechanism is +.>The method comprises the steps of carrying out a first treatment on the surface of the By->The length vector of the obtained down-cable mechanism is +.>;/>
In the above formulas (9) and (10), T represents a transposed matrix; M C j is the anchoring point C under the local coordinate system M system j Coordinates of (c);
lower platform current poseRepresented by the position and attitude angle of the M-line relative to the C-line, wherein>For Euler angle vector of current pose of lower platform in C system, determining current pose of lower platform according to Euler angle vector>The satisfied rope length constraint equation is:
in the above formulas (11) and (12), 、/>The rope length of the upper inhaul cable and the rope length of the lower inhaul cable at the previous moment are respectively; p (P) 0 An initial position vector expressed as the origin of the M system under the C system; />For being in charge of the rotation matrix>Corresponding initial euler angle vectors; C B i is an anchor point B under a local coordinate system C system i Coordinates of (c); C D j is the anchorage point D under the local coordinate system C system j Is defined by the coordinates of (a).
5. The FAST new feed cabin mechanism kinematic forward solution method according to claim 4, wherein in the step S4, an incremental parallel flexible cable mechanism kinematic forward solution model is established according to a rope length constraint equation satisfied by the current pose of the lower platform determined in the step S3, and the method comprises the following steps:
the rope length constraint equations of (11), (12) are set in the previous pose of the lower platformPerforming Taylor expansion, and reserving until a second order term to obtain an incremental parallel flexible rope mechanism kinematic forward model of the following formula (13):
in the previous pose formula of the lower platform and (13),is the position vector of the previous moment; />Is the Euler angle vector of the previous moment; t is a transposed matrix; />、/>The rope length of the upper inhaul cable and the rope length of the lower inhaul cable at the current moment are respectively +.>、/>The rope length of the upper inhaul cable and the rope length of the lower inhaul cable at the previous moment are respectively; />For the increment of the lower platform pose- >And->The position increment of the lower platform and the attitude angle increment of the lower platform are respectively; />Indicating the length of the upper guy cable at the current time>Before the lower platform, the position X P Partial derivative of lower platform position vector P, < >>Representing the partial derivative;indicating the length of the upper guy cable at the current time>In the position of the next previous platform X P Pair and rotation matrix->Partial derivatives of the corresponding euler angle vectors Φ; />Indicating the length of the upper guy cable at the current time>Before the lower platform, the position X P Position vector P and rotation matrix of the lower platform>The second partial derivative of the corresponding euler angle vector Φ; />Indicating the length of the downcable at the current time>Before the lower platform, the position X P Partial derivative of the pair with the lower platform position vector P; />Indicating the length of the downcable at the current time>Before the lower platform, the position X P Pair and rotation matrix->Partial derivatives of the corresponding euler angle vectors Φ; />Indicating the length of the downcable at the current time>Before the lower platform, the position X P Position vector P and rotation matrix of position pair and lower platformThe second partial derivative of the corresponding euler angle vector Φ.
6. The FAST new type feed cabin mechanism kinematics forward solution solving method according to claim 5, wherein in the step S4, a kinematics forward solution optimizing objective function is constructed according to an incremental parallel flexible cable mechanism kinematics forward solution model, and an incremental parallel flexible cable mechanism kinematics forward solution solving equation is constructed according to the kinematics forward solution optimizing objective function, comprising:
In the running process of the FAST novel feed cabin mechanism, the initial pose of the lower platform is accurately measured by an external sensor, and when the kinematic correct solution is solved each time, the previous pose of the lower platform is obtainedIs a known value, the rope length corresponding to the previous pose of the lower platform is +.>Calculated by the above formulas (11) and (12); solving the current pose of the lower platform through kinematic positive solution according to the following mode: measuring rope length +.>Calculating the pose increment of the lower platform by using the rope length at the current moment>The current pose X of the lower platform is obtained through calculation of a formula (14):
in the above-mentioned formula (14),and->The position increment of the lower platform and the attitude angle increment of the lower platform are respectively, and according to the rope length constraint equations of the formulas (11) and (12), the kinematic positive solution optimization objective function of the following formula (15) is obtained as follows:
establishing an incremental parallel flexible cable mechanism kinematic positive solution solving equation of the following formula (16) based on the kinematic positive solution optimizing objective function of the formula (15):
each parameter in the above formula (16) has the same meaning as each corresponding parameter in the formula (13) in claim 5.
7. The method for solving the kinematic positive solution of the novel FAST feed cabin mechanism according to claim 6, wherein,
in the method, the solving error of the kinematic positive solution solving equation of the incremental parallel flexible cable mechanism is reduced by the following method, which comprises the following steps:
In the previous pose of the same lower platformWhen Taylor expansion is carried out on the position, the current pose of the lower platform obtained by solving each positive solution is used for +.>Carrying out Taylor expansion again, and carrying out next solving again in the updating mode (16);
in the method, the solution speed of the incremental parallel flexible cable mechanism kinematic forward solution equation is accelerated by the following steps:
solving to obtain the current pose of the lower platform in each positive solutionPerforming Taylor expansion again according to formula (16), and introducing the matrix of formula (16)>、/>Storing; when updating Taylor expansion each time, the current actual pose of the lower platform is +.>Carry to->、/>。
8. A FAST novel feed cabin mechanism control method is characterized by comprising the following steps:
a1, before a FAST novel feed cabin mechanism operates, receiving a lower platform expected pose of the FAST novel feed cabin mechanism planned out of line;
a2, when the system runs, the lower platform of the FAST novel feed cabin mechanism is controlled in a force and position mixed control mode according to the received expected pose of the lower platform, and in the control process, the current pose of the lower platform is monitored through a visual tracking system until the control process is finished;
step A3, judging whether the visual tracking system fails or not in the process of monitoring the current pose of the lower platform through the visual tracking system, if so, executing the step A4, and if not, repeatedly executing the step A2;
And step A4, solving the current pose of the lower platform of the FAST novel feed cabin mechanism by adopting the positive kinematics solution method of the FAST novel feed cabin mechanism according to any one of claims 1-7, re-planning the expected pose of the lower platform according to the current pose of the lower platform, and re-executing step A2 to perform force-position hybrid control on the lower platform according to the re-planned expected pose of the lower platform.
9. The method for controlling the FAST new feed cabin mechanism according to claim 8, wherein in the step A2, the lower platform of the FAST new feed cabin mechanism is controlled in a hybrid control manner according to the received desired pose of the lower platform, comprising:
solving equations and dynamic models through kinematic positive solutions of incremental parallel flexible cable mechanisms, and utilizing received expected pose of lower platformRespectively calculating and obtaining expected rope length L of parallel flexible rope mechanisms of the platform under the drive of the FAST novel feed cabin mechanism d And the expected cable force T d ;
According to the obtained expected rope length L d Controlling the rope length of the 6-set upper stay rope mechanism of the parallel flexible rope mechanism and according to the obtained expected rope force T d And controlling the cable force of the 3 sets of lower cable mechanisms of the parallel flexible cable mechanism.
10. The FAST new feed cabin mechanism control method according to claim 9, wherein in the method, the received lower platform expected pose is utilized by solving an equation through incremental parallel flexible cable mechanism kinematics positive solution in the following mannerCalculating to obtain expected rope length L of parallel flexible rope mechanism of FAST novel feed cabin mechanism d Comprising:
the kinematic positive solution equation of the incremental parallel flexible cable mechanism is expressed as formula (16):
the meaning of each parameter in the formula (16) is the same as that of each corresponding parameter in the formula (16) in the FAST novel feed cabin mechanism kinematics positive solution solving method of claim 6;
according to the length of the rope at the previous momentSolving the pose increment of the lower platform>Then combine the former pose of the lower platform>Solving the current pose of the lower platform>The expected rope length L is obtained according to the current pose of the lower platform d ;
Utilizing the received lower platform desired pose by parallel-flex mechanism dynamics model in the following mannerCalculating and obtaining expected cable force T of the parallel flexible cable mechanism d Comprising:
the parallel flexible rope mechanism dynamic model is (17):
determining expected cable force T of parallel flexible cable mechanism through dynamic model of parallel flexible cable mechanism of (17) d A cable force optimization function satisfying the following equation (18):
In the above formulae (17) and (18),the pose is expected for the lower platform; />A first order derivative of the expected pose of the lower platform is used for representing the speed variable of the lower platform; />A second derivative of the expected pose of the lower platform is used for representing the acceleration variable of the lower platform;;/>;/>;J d the matrix is a jacobian matrix corresponding to the parallel flexible cable mechanism; j (J) dT The device is a transpose of a jacobian matrix corresponding to the parallel flexible cable mechanism; m is m P For the lower platform quality, I 3×3 Is a unitary matrix, 0 3×3 Is an all-zero matrix, 0 3×1 Is an all-zero vector, g is a gravitational acceleration vector, < ->For a rotation matrix of the local coordinate system C system to the global coordinate system G system,/for>For an antisymmetric matrix corresponding to the angular velocity omega, C I n for the inertia matrix of the lower platform in C series, < >>, M I n For the inertia matrix of the lower platform in the M system, < >>For the rotation matrix of M-series versus C-series, < >>A transposed matrix of the rotation matrix of the M system relative to the C system; parameters corresponding to the subscript S and the subscript U are parameters corresponding to 6 sets of upper inhaul cable mechanisms and parameters corresponding to 3 sets of lower inhaul cable mechanisms respectively; />The matrix is a jacobian matrix corresponding to the upper inhaul cable mechanism; />The matrix is a jacobian matrix corresponding to the lower guy cable mechanism;
based on the expected pose of the lower platform through the determined cable force optimization functionDesired cable force T for a given parallel flex cable mechanism d ;
Determining the expected cable force of the down cable mechanism according to the redundancy of the parallel flexible cable mechanismAfter that, the desired cable force of the cable-up mechanism +.>The unique determination is as follows:
in the above-mentioned formula (19),the cable-lifting mechanism corresponds to the transposition of the jacobian matrix; />The device is a transposition of a jacobian matrix corresponding to the lower inhaul cable mechanism; the other parameters have the same meanings as those of the corresponding parameters in the above formulas (17) and (18). />
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310375061.5A CN116108701B (en) | 2023-04-10 | 2023-04-10 | FAST novel feed cabin mechanism kinematics positive solution solving and control method |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310375061.5A CN116108701B (en) | 2023-04-10 | 2023-04-10 | FAST novel feed cabin mechanism kinematics positive solution solving and control method |
Publications (2)
Publication Number | Publication Date |
---|---|
CN116108701A true CN116108701A (en) | 2023-05-12 |
CN116108701B CN116108701B (en) | 2023-06-23 |
Family
ID=86267614
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310375061.5A Active CN116108701B (en) | 2023-04-10 | 2023-04-10 | FAST novel feed cabin mechanism kinematics positive solution solving and control method |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116108701B (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117609673A (en) * | 2024-01-24 | 2024-02-27 | 中南大学 | Six-degree-of-freedom parallel mechanism forward solution method based on physical information neural network |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2015168799A1 (en) * | 2014-05-08 | 2015-11-12 | UNIVERSITé LAVAL | Parallel mechanism with kinematically redundant actuation |
CN110815180A (en) * | 2019-10-31 | 2020-02-21 | 陕西科技大学 | Six-degree-of-freedom parallel robot motion analysis modeling and fast solving method |
CN111400917A (en) * | 2020-03-18 | 2020-07-10 | 中国科学技术大学 | Kinematics optimization solving method of rope traction parallel robot with variable structure |
CN112487615A (en) * | 2020-11-17 | 2021-03-12 | 清华大学 | Method and device for calibrating main shaft head of five-shaft series-parallel machine tool |
CN112767493A (en) * | 2020-12-30 | 2021-05-07 | 浙江大学 | Machine vision calibration method for kinematic parameters of Stewart platform |
CN112975913A (en) * | 2021-03-10 | 2021-06-18 | 清华大学 | Self-calibration method and system for cable-driven parallel mechanism |
CN114643584A (en) * | 2022-05-17 | 2022-06-21 | 中国科学技术大学 | Rapid terminal sliding mode synchronous control method for rope traction parallel robot |
CN114706312A (en) * | 2022-06-06 | 2022-07-05 | 中国科学技术大学 | High-precision self-adaptive cooperative control method for rope traction parallel robot |
CN115648180A (en) * | 2022-10-28 | 2023-01-31 | 中国科学院国家天文台 | Novel FAST feed source cabin based on nine-cable driving parallel mechanism |
-
2023
- 2023-04-10 CN CN202310375061.5A patent/CN116108701B/en active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2015168799A1 (en) * | 2014-05-08 | 2015-11-12 | UNIVERSITé LAVAL | Parallel mechanism with kinematically redundant actuation |
CN110815180A (en) * | 2019-10-31 | 2020-02-21 | 陕西科技大学 | Six-degree-of-freedom parallel robot motion analysis modeling and fast solving method |
CN111400917A (en) * | 2020-03-18 | 2020-07-10 | 中国科学技术大学 | Kinematics optimization solving method of rope traction parallel robot with variable structure |
CN112487615A (en) * | 2020-11-17 | 2021-03-12 | 清华大学 | Method and device for calibrating main shaft head of five-shaft series-parallel machine tool |
CN112767493A (en) * | 2020-12-30 | 2021-05-07 | 浙江大学 | Machine vision calibration method for kinematic parameters of Stewart platform |
CN112975913A (en) * | 2021-03-10 | 2021-06-18 | 清华大学 | Self-calibration method and system for cable-driven parallel mechanism |
CN114643584A (en) * | 2022-05-17 | 2022-06-21 | 中国科学技术大学 | Rapid terminal sliding mode synchronous control method for rope traction parallel robot |
CN114706312A (en) * | 2022-06-06 | 2022-07-05 | 中国科学技术大学 | High-precision self-adaptive cooperative control method for rope traction parallel robot |
CN115648180A (en) * | 2022-10-28 | 2023-01-31 | 中国科学院国家天文台 | Novel FAST feed source cabin based on nine-cable driving parallel mechanism |
Non-Patent Citations (5)
Title |
---|
WEIWEI SHANG ET AL.: "Synchronization Control in the Cable Space for Cable-Driven Parallel Robots", 《IEEE TRANSACTIONS ON INDUSTRIAL ELECTRONICS》, vol. 66, no. 6, pages 4544 - 4554, XP011707460, DOI: 10.1109/TIE.2018.2864512 * |
任凭;朱聪斌;: "二自由度悬索机器人运动静力学正逆解分析", 机械设计与制造, no. 05, pages 241 - 244 * |
张飞: "绳索牵引并联机器人的结构优化与参数标定", 《中国博士学位论文全文数据库(电子期刊)信息科技辑》 * |
王晓光;马少宇;彭苗娇;林麒;: "绳牵引并联机器人弹性变形对动平台位姿精度的影响", 计算力学学报, no. 03, pages 306 - 312 * |
韩旺;段学超;仇原鹰;段宝岩;: "基于线驱动并联机构的FAST馈源平台位姿测量方法", 机械工程学报, no. 17, pages 43 - 49 * |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117609673A (en) * | 2024-01-24 | 2024-02-27 | 中南大学 | Six-degree-of-freedom parallel mechanism forward solution method based on physical information neural network |
CN117609673B (en) * | 2024-01-24 | 2024-04-09 | 中南大学 | Six-degree-of-freedom parallel mechanism forward solution method based on physical information neural network |
Also Published As
Publication number | Publication date |
---|---|
CN116108701B (en) | 2023-06-23 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN116108701B (en) | FAST novel feed cabin mechanism kinematics positive solution solving and control method | |
CN111400917B (en) | Kinematics optimization solving method of rope traction parallel robot with variable structure | |
EP2954986B1 (en) | Apparatus and method for managing and controlling motion of a multiple body system | |
CN102152307B (en) | Inclination-angle-constraint-based kinematic calibration method for Stewart parallel robot | |
DE102018001676A1 (en) | Robot for learning control with regard to an operation of a production line and method for controlling the same | |
DE102018005068A1 (en) | Crane and method for controlling such a crane | |
HUE026375T2 (en) | Wind system for converting energy through a vertical-axis turbine actuated by means of kites | |
CN110561425A (en) | Rope-driven flexible robot force and position hybrid control method and system | |
US20110010008A1 (en) | Method And Device For Controlling A Manipulator | |
DE102018115279A1 (en) | Robot that performs learning control in applications that require a constant speed, and control methods therefor | |
CN110361829A (en) | A kind of telescope Pointing Calibration method and telescope | |
DE102009007026A1 (en) | Control and control method for a manipulator | |
CN110597051A (en) | Stewart stable platform control method based on RBF neural network | |
CN116066123A (en) | Automatic tracking control method for shield tunneling track based on model predictive control | |
CN108303870B (en) | A kind of control method, the control system of multiple degrees of freedom change in displacement compensating platform | |
CN116141319A (en) | Control method, device, equipment and storage medium of three-rope parallel robot | |
JP2020187469A (en) | Control and adjustment device | |
US11977383B2 (en) | Intelligent course planning method and controller for unmanned surface vehicle | |
CN114185271B (en) | Annular truss three-dimensional follow-up system and control method thereof | |
CN113624399A (en) | Object rotational inertia measuring method applied to large-space rope driving system | |
CN114995137A (en) | Rope-driven parallel robot control method based on deep reinforcement learning | |
EP0653600A1 (en) | Method for determining the rotation speed of the aiming line with a strapped down seeker head | |
CN113119076A (en) | Attitude closed-loop feedback control method and system of three-degree-of-freedom parallel mechanism | |
CN104908030A (en) | SCARA robot accurate positioning method based on granularity model | |
US20240043246A1 (en) | Haptic feedback system for anti-sway control of a payload |
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 |