Disclosure of Invention
The invention aims to provide a special mechanism collision prediction method based on a virtual prototype in an interstage separation test, which overcomes the defects of the prior art.
The invention discloses a special mechanism collision prediction method based on a virtual prototype in an interstage separation test, which comprises the following steps of:
s10, establishing a virtual prototype three-dimensional model according to a mechanism to be detected;
the virtual prototype three-dimensional model comprises a mechanism model to be detected and a spray pipe model; the mechanism model to be detected comprises a yaw mechanism model, a pitch mechanism model and a roll mechanism model;
s20, setting each driving joint in the mechanism model to be detected as a position input variable;
s30, acquiring a position input variable, and establishing a mapping relation between a mechanism model to be detected and the positions of all driving joints in the mechanism to be detected;
s40, calibrating the three-dimensional model of the virtual prototype according to a calibration standard, namely calibrating the virtual machine system;
s50, carrying out an interstage separation test on the mechanism to be detected, and synchronizing the motions of the mechanism to be detected and the virtual prototype three-dimensional model in real time;
s60, carrying out frame segmentation on the motion of the mechanism to be detected and the virtual prototype three-dimensional model, and predicting the pose and the speed of the nth frame of the virtual prototype three-dimensional model according to the motion position, the speed and the acceleration of the current frame of the virtual prototype three-dimensional model;
s70, predicting collision conditions of a mechanism model to be detected and a spray pipe model in an nth frame in the future by using a primitive intersection method;
s80, returning the collision condition of the nth frame in the future to a motion control system of the mechanism to be detected, and making a motion decision of the mechanism to be detected.
Further, the mechanism model to be detected and the spray pipe model are wrapped by triangular patches.
Further, the calibration standard of the three-dimensional model of the virtual prototype is as follows: the initial pose of the mechanism to be detected and the initial pose of the virtual prototype three-dimensional model are completely consistent, and the relative pose of each driving joint of the mechanism to be detected and the virtual prototype three-dimensional model are completely consistent.
Further, the standard for performing frame segmentation on the motion of the mechanism to be detected and the virtual prototype three-dimensional model is as follows: the three-dimensional model of the virtual prototype predicts the pose and the speed of the nth frame of the three-dimensional model of the virtual prototype according to the motion position, the speed and the acceleration of the current frame, and the time required for obtaining the pose and the speed of the nth frame of the three-dimensional model of the virtual prototype is less than the time for one frame of motion of a mechanism to be detected.
Further, the motion acceleration of the virtual prototype three-dimensional model is set to be a constant value.
Further, the virtual prototype three-dimensional model is displayed through an open graphics library OpenGL or DirectX based technology.
Further, the motion decision includes: if collision is predicted, gradually reducing the speed of the mechanism to zero; if no collision is predicted, the mechanism continues to move according to the planned track.
Further, the step of predicting the collision condition of the mechanism model to be detected and the spray pipe model in the nth frame in the future by using the primitive intersection method includes:
s71, recording two triangular patches to be detected asT 1 AndT 2 the method comprises the steps of carrying out a first treatment on the surface of the Which is a kind ofMiddle triangular surface patchT 1 Belongs to a mechanism model to be detected, triangular surface patchT 2 Belongs to a spray pipe model;
triangular face plate
T 1 Is defined by three vertexes
、
、
The method comprises the steps of carrying out a first treatment on the surface of the Triangular face plate
T 2 Is +.>
、
、
; wherein ,
Is vertex->
Coordinates of (c);
Is vertex->
Coordinates of (c);
is vertex->
Coordinates of (c);
Is vertex->
Coordinates of (c);
Is vertex->
Coordinates of (c);
Is vertex->
Coordinates of (c);
s72, calculating parameters
Parameter->
Parameters and parameters
The method comprises the following steps: />
S73, judging parameters
Parameter->
Parameters and parameters
Whether the same number is not 0, if so, the triangular surface patch
T 1 And triangular dough piece
T 2 No intersection occurs in the n-th frame in the future, otherwise, step S74 is entered;
s74, judging parameters
Parameter->
Parameters and parameters
If the numbers are the same and are 0, the step S75 is carried out, otherwise, the step S76 is carried out;
s75, triangular dough piece
T 1 And triangular dough piece
T 2 The vertex coordinates of (2) are converted into two-dimensional coordinates to obtain
、
、
、
、
、
;
By triangular patches
T 1 Any one of the edges of (a)
Establish the linear parameter equation ax+by+c=0 and fit the triangular patches
T 1 And triangular dough piece
T 2 Substituting the two-dimensional vertex coordinates of the two-dimensional vertex into a linear parameter equation to obtain:
if the parameters are
Parameter->
Parameters are the same as the number and are the same as the parameters->
Different numbers, triangular dough piece
T 1 And triangular dough piece
T 2 No intersection occurs in the future nth frame, otherwise, the triangular patch
T 1 And triangular dough piece
T 2 Crossing in the future nth frame;
s76, in the triangular dough sheet
T 1 On which a coordinate system O is established
1 X
1 Y
1 And putting triangular dough sheet
T 1 And triangular dough piece
T 2 Conversion of vertex coordinates to coordinate System O
1 X
1 Y
1 In (a) and (b); judging straight line
Straight line->
Straight line->
With O
1 X
1 Y
1 Intersection point P of planes
1 Intersection point P
2 Intersection point P
3 In (c) whether there is a triangle>
Inside and at triangle->
Intersection on one side, if yes, triangular surface patch
T 1 And triangular dough piece
T 2 Crossing in the future nth frame, otherwise, proceeding to step S77; />
S77, in triangular dough piece
T 2 On which a coordinate system O is established
2 X
2 Y
2 And putting triangular dough sheet
T 1 And triangular dough piece
T 2 Conversion of vertex coordinates to coordinate System O
2 X
2 Y
2 In (a) and (b); judging straight line
Straight line->
Straight line->
With O
2 X
2 Y
2 Intersection point P of planes
4 Intersection point P
5 Intersection point P
6 In (c) whether there is a triangle>
Inside and at triangle->
Intersection on one side, if yes, triangular surface patch
T 1 And triangular dough piece
T 2 Intersecting in the future nth frame, if not, then triangular patches
T 1 And triangular dough piece
T 2 No intersection occurs in the future nth frame;
s78, updating the triangular dough piece to be detectedT 1 And triangular dough pieceT 2 And returning to the step 1) until determining whether each triangular patch to be detected in the mechanism model to be detected and all triangular patches to be detected of the spray pipe model intersect in the future nth frame, and further judging whether the mechanism model to be detected and the spray pipe model collide in the future nth frame.
Further, the collision prediction includes: if a certain triangular patch to be detected exists in the mechanism model to be detected and the nozzle model, the mechanism model to be detected and the nozzle model collide in the future nth frame;
if all triangular patches in the mechanism model to be detected and the spray pipe model are not intersected in the future nth frame, the mechanism model to be detected and the spray pipe model are not collided in the future nth frame.
Further, the triangular patches to be detected comprise triangular patches with the same speed direction as the normal direction.
The special mechanism collision prediction method based on the virtual prototype in the interstage separation test has the following characteristics: establishing a high-precision three-dimensional model in the virtual machine according to requirements, and wrapping by using triangular patches; establishing a mapping relation between each driving joint position in the virtual prototype and each driving joint position of the actual mechanism; calibrating a virtual machine system; an interstage separation test was performed. Cutting the mechanism motion into a plurality of frame motions in 1 second by taking a frame as a unit, and synchronizing each frame motion of the actual model and the virtual prototype model in real time; in the virtual prototype, according to the motion position and the speed of the previous frame, predicting acceleration data to obtain pose and speed data of the next frame; obtaining the pose condition of the predicted frame according to the intersecting calculation of the graphic elements, and obtaining the collision condition of a mechanism and a model under the predicted frame; and the virtual prototype returns collision information of the predicted frame to the motion control system to carry out motion decision.
In short, the special mechanism collision prediction method based on the virtual prototype in the interstage separation test establishes a high-precision three-dimensional digital model of the whole test section, mechanism and separator, and is used for predicting the collision prediction in the test process; a virtual machine system which is independent of a control system and better in graphic calculation is established, the virtual machine system communicates through a high-speed communication protocol, and PLC resources can be occupied less when collision prediction is carried out; the method is characterized in that the method is displayed in real time through a computer OpenGL or DirectX technology, so that a user can monitor the whole motion process in real time and even perform human intervention collision prediction; when the triangular patch primitives are intersected and calculated, only triangular patches with the triangular patch speed direction being the same as the normal direction of the triangular patches are detected, so that the collision prediction accuracy is ensured, and meanwhile, the efficiency is improved. The special mechanism collision prediction method based on the virtual prototype in the interstage separation test can perform real-time collision prediction with high precision, and improves the safety of the whole interstage separation.
Description of the embodiments
The present invention is further described below with reference to examples, but it should not be construed that the scope of the above subject matter of the present invention is limited to the following examples. Various substitutions and alterations are made according to the ordinary skill and familiar means of the art without departing from the technical spirit of the invention, and all such substitutions and alterations are intended to be included in the scope of the invention.
Example 1
As shown in fig. 1, the method for predicting the collision of the special mechanism based on the virtual prototype in the interstage separation test of the embodiment comprises the following steps:
s10, establishing a virtual prototype three-dimensional model according to a mechanism to be detected; the virtual prototype three-dimensional model comprises a mechanism model 4 to be detected and a spray pipe model 5; the mechanism model 4 to be detected comprises a yaw mechanism model, a pitch mechanism model and a roll mechanism model;
the mechanism model 4 to be detected and the spray pipe model 5 are wrapped by triangular patches.
The virtual prototype three-dimensional model is displayed through an open graphics library OpenGL or DirectX based technology.
S20, setting each driving joint in the mechanism model 4 to be detected as a position input variable;
s30, acquiring a position input variable, and establishing a mapping relation between a mechanism to be detected model 4 and the positions of all driving joints in the mechanism to be detected;
s40, calibrating the three-dimensional model of the virtual prototype, namely calibrating a virtual machine system;
the standard for completing the calibration of the three-dimensional model of the virtual prototype comprises the following steps: the initial pose of the mechanism to be detected is completely consistent with that of the virtual prototype three-dimensional model, and the relative pose of each driving joint of the mechanism to be detected is completely consistent with that of the virtual prototype three-dimensional model;
s50, carrying out an interstage separation test on the mechanism to be detected, and synchronizing the motions of the mechanism to be detected and the virtual prototype three-dimensional model in real time;
s60, carrying out frame segmentation on the motion of the mechanism to be detected and the virtual prototype three-dimensional model, and predicting the pose and the speed of the nth frame of the virtual prototype three-dimensional model in the future through C++ software according to the motion position, the speed and the acceleration of the current frame of the virtual prototype three-dimensional model; the C++ software stores a program for predicting the pose and the speed in advance;
the frame segmentation criteria for the motion of the mechanism to be detected and the virtual prototype three-dimensional model include: the three-dimensional model of the virtual prototype predicts that the time required for obtaining the pose and the speed of the future nth frame of the three-dimensional model of the virtual prototype is less than the time required for one frame of motion of a mechanism to be detected according to the motion position, the speed and the acceleration of the current frame;
setting the motion acceleration of the three-dimensional model of the virtual prototype as a constant value;
s70, predicting the collision condition of the mechanism model 4 to be detected and the spray pipe model 5 in the nth frame in the future by using a primitive intersection method;
the step of predicting the collision condition of the mechanism model 4 to be detected and the spray pipe model 5 in the nth frame in the future by using the primitive intersection method comprises the following steps:
s71, recording two triangular patches to be detected asT 1 AndT 2 the method comprises the steps of carrying out a first treatment on the surface of the Wherein, triangular dough pieceT 1 Belonging to waiting forDetecting mechanism model 4, triangular dough pieceT 2 Belonging to the nozzle model 5;
triangular face plate
T 1 Is defined by three vertexes
、
、
The method comprises the steps of carrying out a first treatment on the surface of the Triangular face plate
T 2 Is +.>
、
、
; wherein ,
Is vertex->
Coordinates of (c);
Is vertex->
Coordinates of (c);
is vertex->
Coordinates of (c);
Is vertex->
Coordinates of (c);
Is vertex->
Coordinates of (c);
Is vertex->
Coordinates of (c);
s72, calculating parameters
Parameter->
Parameters and parameters
The method comprises the following steps:
s73, judging parameters
Parameter->
Parameters and parameters
Whether the same number is not 0, if so, the triangular surface patch
T 1 And triangular dough piece
T 2 No intersection occurs in the n-th frame in the future, otherwise, step S74 is entered;
s74, judging parameters
Parameter->
Parameters and parameters
If the numbers are the same and are 0, the step S75 is carried out, otherwise, the step S76 is carried out;
s75, triangular dough piece
T 1 And triangular dough piece
T 2 The vertex coordinates of (2) are converted into two-dimensional coordinates to obtain
、
、
、
、
、
;/>
By triangular patches
T 1 Any one of the edges of (a)
Establish the linear parameter equation ax+by+c=0 and fit the triangular patches
T 1 And triangular dough piece
T 2 Substituting the two-dimensional vertex coordinates of the two-dimensional vertex into a linear parameter equation to obtain:
if the parameters are
Parameter->
Parameters are the same as the number and are the same as the parameters->
Different numbers, triangular dough piece
T 1 And triangular dough piece
T 2 No intersection occurs in the future nth frame, otherwise, the triangular patch
T 1 And triangular dough piece
T 2 Crossing in the future nth frame;
s76, in the triangular dough sheet
T 1 On which a coordinate system O is established
1 X
1 Y
1 And putting triangular dough sheet
T 1 And triangular dough piece
T 2 Conversion of vertex coordinates to coordinate System O
1 X
1 Y
1 In (a) and (b); judging straight line
Straight line->
Straight line->
With O
1 X
1 Y
1 Intersection point P of planes
1 Intersection point P
2 Intersection point P
3 In (c) whether there is a triangle>
Inside and at triangle->
Intersection on one side, if yes, triangular surface patch
T 1 And triangular dough piece
T 2 Crossing in the future nth frame, otherwise, proceeding to step S77;
s77, in triangular dough piece
T 2 On which a coordinate system O is established
2 X
2 Y
2 And putting triangular dough sheet
T 1 And triangular dough piece
T 2 Conversion of vertex coordinates to coordinate System O
2 X
2 Y
2 In (a) and (b); judging straight line
Straight line->
Straight line->
With O
2 X
2 Y
2 Intersection point P of planes
4 Intersection point P
5 Intersection point P
6 In (c) whether there is a triangle>
Inside and at triangle->
Intersection on one side, if yes, triangular surface patch
T 1 And triangular dough piece
T 2 Intersecting in the future nth frame, if not, then triangular patches
T 1 And triangular dough piece
T 2 No intersection occurs in the future nth frame;
s78, updating the triangular dough piece to be detectedT 1 And triangular dough pieceT 2 Returning to the step 1) until determining whether each triangular patch to be detected in the mechanism model 4 to be detected and all triangular patches to be detected of the spray pipe model 5 intersect in the future nth frame, and further judging whether the mechanism model 4 to be detected and the spray pipe model 5 collide in the future nth frame;
s79, if a certain triangular patch to be detected exists in the mechanism model 4 to be detected and a certain triangular patch to be detected of the spray pipe model 5 are intersected in the future nth frame, the mechanism model 4 to be detected and the spray pipe model 5 collide in the future nth frame;
if each triangular patch in the mechanism model 4 to be detected and all triangular patches of the spray pipe model 5 do not intersect in the future nth frame, the mechanism model 4 to be detected and the spray pipe model 5 do not collide in the future nth frame.
The triangular patches to be detected comprise triangular patches with the same speed direction as the normal direction.
S80, returning the collision condition of the nth frame in the future to a motion control system of the mechanism to be detected, and making a motion decision of the mechanism to be detected.
The motion decision comprises: if collision is predicted, gradually reducing the speed of the mechanism to zero; if no collision is predicted, the mechanism continues to move according to the planned track.
Example 2
The special mechanism collision prediction method based on the virtual prototype in the interstage separation test of the embodiment is a simplified method, and specifically comprises the following steps:
s10, accurately establishing a wind tunnel test section, a mechanism and a three-dimensional digital model of a test model in the virtual prototype, and setting each driving joint in the mechanism as a position input variable.
In step S10, the critical model outline may be enlarged by a certain threshold to perform three-dimensional accurate modeling, and the final three-dimensional digital model is wrapped with triangular patches, taking into account the threshold for collision prediction between different components.
S20, establishing mapping relations between the positions of all driving joints in the virtual prototype and the positions of all driving joints of the actual mechanism.
S30, calibrating the virtual machine system.
In step S30, the purpose of the calibration is to ensure that the relative pose between the models in the real model and in the virtual prototype model at the initial moment is completely consistent.
S40, performing an interstage separation test. And cutting the mechanism motion into a plurality of frame motions in 1 second by taking the frame as a unit, and synchronizing each frame motion of the actual model and the virtual prototype model in real time.
In step S40, when frame slicing is performed, the communication capability of the field device and the calculation speed of the virtual prototype should be considered, so as to ensure that the total time for the virtual prototype to process a frame and predict the next frame is less than the time for the mechanism to actually operate a frame.
S50, in the virtual prototype, the pose and speed data of the last n frames are obtained according to the motion position, the speed and the acceleration of the last frame.
In step S50, a constant acceleration assumption is used for motion prediction at the next time in the entire frame.
S60, according to the position and pose conditions of the predicted frame obtained through the intersecting calculation of the graphic elements, the collision conditions of the mechanism and the model under the predicted frame are obtained.
In step S60, in order to improve the detection efficiency, only triangular patches having the same triangular patch speed direction as the normal direction thereof are detected in the triangular patch primitive intersection calculation.
S70, the virtual prototype returns collision information of the predicted frame to the motion control system to carry out motion decision.
Example 3
The embodiment is used for describing in detail the primitive intersection method in the special mechanism collision prediction method based on the virtual prototype in the interstage separation test, and specifically comprises the following steps:
fig. 2a is a phantom in a wind tunnel. In fig. 2a, 1 is a nozzle in a wind tunnel test, in this example an obstacle to the operation of the mechanism; and 2, a physical model in a wind tunnel test is mounted at the tail end of the executing mechanism 3. The tail end of the actuator 3 has six degrees of freedom, and three rotations of yaw, pitch and roll are realized in the three directions of X, Y and Z in an aviation coordinate system. Fig. 2b is a virtual prototype model based on the physical model of fig. 2a, taking into account the threshold requirements of collision prediction, and in fig. 2b, the yaw, pitch, roll mechanism and mechanism to be detected model 4 of a possible collision are wrapped in a triangular patch, and also the nozzle model 5 is wrapped in a triangular patch. The display technology of the virtual prototype adopts an open graphics library OpenGL or DirectX technology.
Before wind tunnel test, a one-to-one mapping relationship between the joint information (X movement, axis 1, axis 2, pitch, yaw and roll) of the physical model and the corresponding joint in the virtual prototype is established. And then calibrating the system, namely ensuring that the initial pose of the physical model and the initial pose of the virtual prototype are completely consistent, and ensuring that the relative pose of each part is completely consistent. When wind tunnel test is carried out, the pose and the speed of the physical model and the virtual prototype model are synchronized in real time according to a certain frame rate, and the time for synchronizing one frame is smaller than the time for predicting one frame by collision. Assuming that the time of a frame is deltat, assuming uniform motion when the position and the speed of the frame are obtained, obtaining the pose of the nth frame (such as the 5 th frame) later, then calculating the positions and the speeds of all triangular patch vertexes on the model to be detected, and then carrying out collision detection according to the predicted pose. In order to reduce the detection effort, before collision detection, all triangular patches to be detected are classified into three types as shown in fig. 3a, 3b and 3c, and the triangular patches in fig. 3b have an obtuse angle between the moving direction and the normal direction, so that collision risk does not occur, and therefore, such triangular patches are omitted in the detection of this frame, and only the triangular patches shown in fig. 3a and 3c are detected. The detection mode is to detect all triangular patches which are consistent with fig. 3a and 3c on the yaw, pitch and roll mechanisms and the mechanism to be detected model 4 and all triangular patches of the spray pipe model 5 one by one. The detection algorithm for each pair of triangular patches is as follows:
assume that the two triangles to be detected are
T 1 And
T 2, wherein
T 1 Is defined by three vertexes
、
、
;
T 2 Is +.>
、
、
;
The following determinant operation is defined:
first by
,
,
Classifying the problem by the value of (2):
if the three determinant are of the same sign and are not 0, then this is illustratedT 1 AndT 2 no intersection occurs;
if the three determinant is 0, the description and coplanarity are changed to the intersection judgment of the two-dimensional triangle, and the following can be realized
T 1 And
T 2 the point coordinates of (2) are converted into two-dimensional coordinates, and can be arbitrarily found in the two-dimensional coordinate system, and the point coordinates are assumed to be at the moment
T 1 And
T 2 two-dimensional coordinate values of (a) are respectively
、
、
、
、
、
. To be used for
T 1 Is->
A linear parameter equation ax+by+c=0 is established. Will be
T 1 Is the third point of (2)
T 2 Is brought into this equation:
if for
T 1 Is provided with three sides
The same number and->
In the case of different numbers, then
T 1 And
T 2 no intersection occurs, whereas there is an intersection.
If not, then the description is thatT 1 AndT 2 different surfaces, wherein a certain triangle intersects with a plane determined by another triangle, a coordinate system is established on the certain triangle, and coordinate values of the two triangles are converted into the coordinate system.
As shown in fig. 4, a judgment is made
,
,
Determined straight line and O
1 X
1 Y
1 Three intersection points P of planes
1 、P
2 、P
3 (if there is no intersection point, it is defined to intersect at infinity) whether there is a certain intersection point P
i Belongs to triangle->
Is a closure of triangle ++>
Is provided. If present, the two triangles intersect. If not, a local coordinate system will be established on the other triangle, and the above is repeated. If neither calculation exists, then the two triangles do not intersect.
After the detection of all the triangular surface patches is finished, if the condition that two triangles intersect exists, the feedback is that collision occurs; if the condition that two triangles intersect does not exist for all the triangular patch pairs, no collision occurs in feedback. And then waiting for data of the next frame to perform collision prediction.