Multi-degree-of-freedom parallel motion platform motion trail collision risk early warning method and system for simulator

Abstract

The invention provides a multi-degree-of-freedom parallel motion platform motion trail collision risk early warning method and a multi-degree-of-freedom parallel motion platform motion trail collision risk early warning system for a simulator. The position change of the electric cylinder in a period of time in the future is predicted in advance, predicted position point information can be continuously updated in the movement process through prediction of a dynamic movement track, the movement state of the electric cylinder can be tracked in real time, accurate position information is provided for collision pre-judgment, and the position change of the electric cylinder in the period of time in the future is predicted. Whether the collision risk is generated due to speed and position changes in the movement process of the electric cylinder is judged in advance, the accuracy and foresight of collision pre-judgment are improved, and the reliability and stability of platform control and movement are guaranteed.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-degree-of-freedom motion platforms, and in particular to platform motion interference detection and early warning, and specifically to a motion trajectory collision risk early warning method and system for a multi-degree-of-freedom parallel motion platform for a simulation simulator. Background Art

[0002] The six-degree-of-freedom motion platform used in the simulation simulator is the basis for completing the simulation motion in multiple postures. It is usually composed of an upper platform, a lower platform, and multiple linear motion mechanisms located between the two platforms, such as electric cylinders, hydraulic cylinders, motor drive mechanisms, etc. During the simulation motion, the spatial position and posture of the upper platform are controlled by controlling the telescopic coordination between the electric cylinders. Affected by the control accuracy (positive and negative solutions) and the length and displacement of the motion mechanism, the movement of the upper platform and the connecting rod of the electric cylinder will produce abnormal solution singular points and interference between the connecting rod, the connecting rod and the hinge position, resulting in rigid mechanical collision, resulting in damage to the electric cylinder, joints, connecting rods and platform components, and setting serious dangers and safety construction.

[0003] In addition, due to the influence of electrical and mechanical aging of electric cylinders, motors, joints, hinges, connecting rods, etc., the motion trajectory and accuracy of the platform will be affected, resulting in deviations in the posture of the platform, causing vibration and instability of the platform. This unstable factor will continue to accumulate in the subsequent movement process, not only affecting the dynamic performance of the platform, but also causing the risk of collision. For example, the piston rod of the electric cylinder is bent and deformed due to a collision, which not only affects the motion accuracy of the platform, but also causes damage to the sealing structure inside the electric cylinder, thereby causing the leakage of hydraulic oil or grease, further affecting the normal operation of the equipment.

[0004] At present, the collision detection and protection in the motion control process of multi-degree-of-freedom motion platforms mainly adopts the method based on physical limit detection and distance monitoring. For example, by installing physical limit switches at key parts of the electric cylinder or motion platform, such as installing mechanical limit switches at both ends of the stroke of the electric cylinder, when the piston rod of the electric cylinder moves to the limit position, it will touch the limit switch, and the limit switch will feed back the signal to the control system, so that the electric cylinder stops moving. At the same time, limit devices can also be installed at the edge of the platform's frame structure where a collision may occur to provide position warnings. The use of physical limit monitoring is a passive monitoring method, which is mainly used to prevent simple collision situations such as overtravel of the electric cylinder. It can only be detected when the collision is about to occur (reaching the physical limit position).

[0005] The collision detection effect between the electric cylinders inside the platform, the hinge position and the connecting rod is limited. It can only detect the collision that has reached the physical limit position, and cannot provide early warning for the collision caused by the relative position change between the electric cylinders during the movement. For example, when the platform performs complex posture adjustment movements, the electric cylinders may collide before reaching the travel limit. In this case, the physical limit device is difficult to play a role. Summary of the invention

[0006] In view of the defects and shortcomings of the prior art, the present invention aims to provide a method for early warning of motion trajectory collision risk of a multi-degree-of-freedom parallel motion platform for a simulation simulator, comprising:

[0007] Get the current position coordinates of the i-th electric cylinder among the n electric cylinders of the multi-degree-of-freedom parallel motion platform, denoted as (x i ,y i ,z i ); x i ,y i ,z i Respectively represent the x, y, and z coordinates of the current position of the i-th electric cylinder;

[0008] Get the speed and acceleration of the i-th electric cylinder, denoted as (v xi ,v yi ,v zi ) and (α xi ,α yi ,α zi ) and, v xi ,v yi ,v zi With α xi ,α yi ,α zi The speed and acceleration of the i-th electric cylinder in the x, y, and z directions respectively;

[0009] Get the platform target pose information, recorded as (x d ,y d ,z d ,α,β,γ), where x d ,y d ,z d They represent the target position coordinates in the Cartesian coordinate system, and α, β, and γ represent the rotation angles around the x, y, and z directions respectively;

[0010] Establish the equivalent motion equation of the six-degree-of-freedom parallel motion platform;

[0011] Configure the time step Δt and detection period T of platform collision detection;

[0012] Based on the platform equivalent motion equation, starting from the current time t=0, the prediction is performed cyclically at intervals of time step Δt until the detection period T; in each cycle, the electric cylinder position of the next time step is calculated based on the selected motion model and the position, velocity, and acceleration predicted last time;

[0013] According to all the position prediction values ​​of each detection period T, a position trajectory sequence is formed;

[0014] Combined with the geometric model of the cylindrical cylinder body and the connecting rod of the electric cylinder and the position trajectory sequence, the distance d between the axes of any i-th electric cylinder and the j-th electric cylinder is calculated. c (t) and the angle θ between the connecting rod direction vector ij (t);

[0015] Based on the distance d between the axes of the i-th electric cylinder and the j-th electric cylinder c (t) and the angle θ between the connecting rod direction vector ij (t) to provide early warning of collision and interference risks between electric cylinders.

[0016] As an optional embodiment, the speed and acceleration of the i-th electric cylinder are configured to be obtained by calculating the position difference and the speed difference, respectively, or obtained from feedback data of the platform motion control system.

[0017] As an optional embodiment, the establishment of an equivalent motion equation of the six-degree-of-freedom parallel motion platform includes:

[0018] Establish the motion equation of the i-th electric cylinder in the x direction i (t):

[0019]

[0020] Among them, at t = 0, x i (0), v xi (0), a xi (0) are the initial position, initial velocity and initial acceleration of the i-th electric cylinder in the x direction;

[0021] Establish the motion equation y of the i-th electric cylinder in the y direction i (t):

[0022]

[0023] Among them, at t = 0, y i (0), v yi (0)t、a yi (0) are the initial position, initial velocity and initial acceleration of the i-th electric cylinder in the y direction;

[0024] Establish the motion equation of the i-th electric cylinder in the z direction i (t):

[0025]

[0026] Among them, at t = 0, z i (0), v zi (0), a zi (0) are the initial position, initial velocity and initial acceleration of the i-th electric cylinder in the z direction.

[0027] As an optional embodiment, the platform equivalent motion equation is based on the current time t=0, and the prediction is performed cyclically at intervals of time step Δt until the detection period T; in each cycle, the electric cylinder position of the next time step is calculated according to the selected motion model and the position, velocity, and acceleration predicted last time, including:

[0028] For the position prediction of the i-th electric cylinder in the x-direction, at the k-th cycle, t = k*Δt, the position prediction value in the x-direction is:

[0029]

[0030] Among them, x i ((k-1)Δt),v xi ((k-1)Δt), a xi ((k-1Δt) are the position, velocity and acceleration in the x direction obtained from the last prediction;

[0031] For the position prediction of the i-th electric cylinder in the y direction, at the k-th cycle, t = k*Δt, the position prediction value in the y direction is:

[0032]

[0033] Among them, y i ((k-1)Δt),v yi ((k-1)Δt), a yi ((k-1)Δt) are the position, velocity and acceleration in the y direction obtained from the last prediction;

[0034] For the position prediction of the i-th electric cylinder in the z direction, at the k-th cycle, t = k*Δt, the position prediction value in the z direction is:

[0035]

[0036] Among them, z i (k-1)Δt)、v zi ((k-1)Δt), azi ((k-1)Δt) are the position, velocity and acceleration in the z direction obtained from the last prediction.

[0037] As an optional embodiment, the geometric model of the cylindrical cylinder body and the connecting rod of the electric cylinder and the position trajectory sequence are combined to calculate the angle θ between any i-th electric cylinder and the j-th electric cylinder axis connecting rod direction vector ij (t), including:

[0038] First, an equivalent model of the electric cylinder consisting of a cylindrical cylinder body and a connecting rod is established. Assume that the radius of the i-th electric cylinder is r i , the connecting rod length is L i , the initial position vector is The position vector at a certain time t is Similarly, for the jth electric cylinder, the radius is r j , the connecting rod length is L j , the initial position vector is The position vector at a certain time t is Among them, r i =r j ;

[0039] Then, the connecting rod direction vectors of the i-th electric cylinder and the j-th electric cylinder at a certain time t are calculated respectively.

[0040] Its mold length represents the movement length of the connecting rod of the i-th electric cylinder at time t;

[0041] Its mold length represents the movement length of the jth electric cylinder connecting rod at time t;

[0042] Then, based on the connecting rod direction vector Use vector dot product operation to calculate the angle θ between the connecting rod direction vectors ij (t):

[0043]

[0044] but:

[0045] As an optional embodiment, the geometric model of the cylindrical cylinder body and the connecting rod of the electric cylinder and the position trajectory sequence are combined to calculate the distance d between the axes of any i-th electric cylinder and the j-th electric cylinder. c (t), including:

[0046]

[0047] As an optional embodiment, based on the distance d between the axes of the i-th electric cylinder and the j-th electric cylinder c (t) and the angle θ between the connecting rod direction vector ij (t) To provide early warning of collision and interference risks between electric cylinders, including:

[0048] In each detection cycle T, according to d c (t) is compared with the safety distance threshold to determine whether there is a risk of collision and interference between the electric cylinders:

[0049] If d c (t)<L i sinθ ij (t)+L j sinθ ij (t)+r i +r j , it is determined that there is a risk of collision and interference between the i-th electric cylinder and the j-th electric cylinder, and an early warning is issued;

[0050] In each detection cycle T, according to θ ij (t) and the safety angle threshold θ safe Compare the two to determine whether there is a risk of collision and interference between the electric cylinders:

[0051] If θ ij (t) is less than θ safe , it is determined that there is a risk of collision and interference between the i-th electric cylinder and the j-th electric cylinder, and an early warning is issued.

[0052] As an optional embodiment, the method further includes:

[0053] The motion state and position prediction results of the electric cylinder are represented through a three-dimensional visual graphic interface, and different colors or marks are used to indicate electric cylinders with interference risks.

[0054] As an optional embodiment, the method further includes:

[0055] Based on the judgment of the existence of collision and interference risks, the RRT algorithm or A* algorithm is used to find the optimal motion trajectory of the electric cylinder without collision according to the platform target posture information, and the movement of the electric cylinder is replanned accordingly.

[0056] According to a second aspect of the present invention, a computer system is also provided, comprising:

[0057] one or more processors;

[0058] A memory stores operable instructions, which, when executed by the one or more processors, cause the one or more processors to perform operations, wherein the operations include the process of the aforementioned method for warning the motion trajectory collision risk of a multi-degree-of-freedom parallel motion platform for a simulation simulator.

[0059] Compared with the prior art, the motion trajectory collision risk warning method of a multi-degree-of-freedom parallel motion platform for a simulation simulator according to the above embodiment of the present invention has the following significant advantages:

[0060] In view of the traditional passive detection method based on physical limit devices, it can only be detected when the collision is about to occur (reaching the physical limit position). Therefore, in the multi-degree-of-freedom parallel motion platform motion trajectory collision risk warning method proposed in the present invention, the position change of the electric cylinder in the future period of time can be predicted in advance according to the current motion state and target posture of the platform, so as to discover the potential collision risk in advance. For example, when the platform performs a complex acceleration-deceleration-steering motion task, the prediction of the dynamic motion trajectory can continuously update the predicted position point information during the motion process, and judge in advance whether the electric cylinder will have a collision risk due to speed and position changes during the motion process.

[0061] The multi-degree-of-freedom parallel motion platform motion trajectory collision risk warning method of the present invention can determine the time step by considering the motion speed of the platform and the response time of the control system, ensure that the algorithm can timely capture the changes in the motion trajectory, and determine the prediction duration according to the working environment and safety requirements of the platform, which can cover the time period from the current state of the platform to the possible collision, and make early warnings. Through comprehensive calculations of the equivalent motion equations of the platform and the electric cylinder, the position of the electric cylinder is cyclically predicted. By continuously updating the position, speed and acceleration, the motion state of the electric cylinder can be tracked in real time, providing accurate position information for collision prediction, accurately predicting the position change of the electric cylinder in the future, and whether there will be collision and interference risks between electric cylinders, the accuracy and foresight of collision prediction are improved, and the reliability and stability of platform control and motion are improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] Figure 1 The present invention is a flow chart of a method for early warning of motion trajectory collision risk of a multi-degree-of-freedom parallel motion platform for a simulation simulator according to an embodiment of the present invention.

[0063] Figure 2 is a flowchart of constructing a position trajectory sequence based on cyclic prediction according to an embodiment of the present invention.

[0064] Figure 3 It is a flow chart for calculating the distance between two electric cylinder axes and the angle between connecting rod direction vectors according to an embodiment of the present invention. DETAILED DESCRIPTION

[0065] In order to better understand the technical content of the present invention, specific embodiments are given and described as follows in conjunction with the accompanying drawings.

[0066] Various aspects of the present invention are described in this disclosure with reference to the accompanying drawings, in which many illustrative embodiments are shown. The embodiments of the present disclosure are not necessarily intended to include all aspects of the present invention. It should be understood that the various concepts and embodiments introduced above, as well as those described in more detail below, can be implemented in any of many ways, because the concepts and embodiments disclosed by the present invention are not limited to any implementation. In addition, some aspects disclosed by the present invention can be used alone or in any appropriate combination with other aspects disclosed by the present invention.

[0067] {Example 1}

[0068] Combination Figure 1 , Figure 2 As shown, the motion trajectory collision risk warning method of a multi-degree-of-freedom parallel motion platform for a simulation simulator according to an embodiment of the present invention includes the following processes:

[0069] Get the current position coordinates of the i-th electric cylinder among the n electric cylinders of the multi-degree-of-freedom parallel motion platform, denoted as (x i ,y i ,z i ); x i ,y i ,z i Respectively represent the x, y, and z coordinates of the current position of the i-th electric cylinder;

[0070] Get the speed and acceleration of the i-th electric cylinder, denoted as (v xi ,v yi ,v zi ) and (α xi ,α yi ,α zi ) and, v xi ,v yi ,v zi With α xi ,α yi ,α zi The speed and acceleration of the i-th electric cylinder in the x, y, and z directions respectively;

[0071] Get the platform target pose information, recorded as (x d ,y d ,z d ,α,β,γ), where x d ,y d ,z dThey represent the target position coordinates in the Cartesian coordinate system, and α, β, and γ represent the rotation angles around the x, y, and z directions respectively;

[0072] Establish the equivalent motion equation of the six-degree-of-freedom parallel motion platform;

[0073] Configure the time step Δt and detection period T of platform collision detection;

[0074] Based on the platform equivalent motion equation, starting from the current time t=0, the prediction is performed cyclically at intervals of time step Δt until the detection period T; in each cycle, the electric cylinder position of the next time step is calculated based on the selected motion model and the position, velocity, and acceleration predicted last time;

[0075] According to all the position prediction values ​​of each detection period T, a position trajectory sequence is formed;

[0076] Combined with the geometric model of the cylindrical cylinder body and the connecting rod of the electric cylinder and the position trajectory sequence, the distance d between the axes of any i-th electric cylinder and the j-th electric cylinder is calculated. c (t) and the angle θ between the connecting rod direction vector ij (t);

[0077] Based on the distance d between the axes of the i-th electric cylinder and the j-th electric cylinder c (t) and the angle θ between the connecting rod direction vector ij (t) to provide early warning of collision and interference risks between electric cylinders.

[0078] As an optional embodiment, the speed and acceleration of the i-th electric cylinder are configured to be obtained by calculating the position difference and the speed difference, respectively, or obtained from feedback data of the platform motion control system.

[0079] As an optional embodiment, the establishment of an equivalent motion equation of the six-degree-of-freedom parallel motion platform includes:

[0080] Establish the motion equation of the i-th electric cylinder in the x direction i (t):

[0081]

[0082] Among them, at t = 0, x i (0), v xi (0), a xi (0) are the initial position, initial velocity and initial acceleration of the i-th electric cylinder in the x direction;

[0083] Establish the motion equation y of the i-th electric cylinder in the y direction i (t):

[0084]

[0085] Among them, at t = 0, y i (0), v yi (0)t、a yi (0) are the initial position, initial velocity and initial acceleration of the i-th electric cylinder in the y direction;

[0086] Establish the motion equation of the i-th electric cylinder in the z direction i (t):

[0087]

[0088] Among them, at t = 0, z i (0), v zi (0), a zi (0) are the initial position, initial velocity and initial acceleration of the i-th electric cylinder in the z direction.

[0089] As an optional embodiment, combining Figure 2 As shown, based on the platform equivalent motion equation, starting from the current time t=0, the prediction is performed cyclically at intervals of time step Δt until the detection period T; in each cycle, the electric cylinder position of the next time step is calculated according to the selected motion model and the position, velocity, and acceleration predicted last time, including:

[0090] For the position prediction of the i-th electric cylinder in the x-direction, at the k-th cycle, t = k*Δt, the position prediction value in the x-direction is:

[0091]

[0092] Among them, x i ((k-1)Δt),v xi ((k-1)Δt), a xi ((k-1)Δt) are the position, velocity and acceleration in the x direction obtained from the last prediction;

[0093] For the position prediction of the i-th electric cylinder in the y direction, at the k-th cycle, t = k*Δt, the position prediction value in the y direction is:

[0094]

[0095] Among them, y i ((k-1)Δt),v yi ((k-1)Δt), a yi ((k-1)Δt) are the position, velocity and acceleration in the y direction obtained from the last prediction;

[0096] For the position prediction of the i-th electric cylinder in the z direction, at the k-th cycle, t = k*Δt, the position prediction value in the z direction is:

[0097]

[0098] Among them, z i ((k-1)Δt),v zi ((k-1)Δt), a zi ((k-1)Δt) are the position, velocity and acceleration in the z direction obtained from the last prediction.

[0099] As an optional embodiment, the geometric model of the cylindrical cylinder body and the connecting rod of the electric cylinder and the position trajectory sequence are combined to calculate the angle θ between any i-th electric cylinder and the j-th electric cylinder axis connecting rod direction vector ij (t), including:

[0100] First, an equivalent model of the electric cylinder consisting of a cylindrical cylinder body and a connecting rod is established. Assume that the radius of the i-th electric cylinder is r i , the connecting rod length is L i , the initial position vector is The position vector at a certain time t is Similarly, for the jth electric cylinder, the radius is r j , the connecting rod length is L j , the initial position vector is The position vector at a certain time t is Among them, r i =r j ;

[0101] Then, the connecting rod direction vectors of the i-th electric cylinder and the j-th electric cylinder at a certain time t are calculated respectively.

[0102] Its mold length represents the movement length of the connecting rod of the i-th electric cylinder at time t; Its mold length represents the movement length of the jth electric cylinder connecting rod at time t;

[0103] Then, based on the connecting rod direction vector Use vector dot product operation to calculate the angle θ between the connecting rod direction vectors ij (t):

[0104]

[0105] but:

[0106] As an optional embodiment, the geometric model of the cylindrical cylinder body and the connecting rod of the electric cylinder and the position trajectory sequence are combined to calculate the distance d between the axes of any i-th electric cylinder and the j-th electric cylinder. c (t), including:

[0107]

[0108] As an optional embodiment, based on the distance d between the axes of the i-th electric cylinder and the j-th electric cylinder c (t) and the angle θ between the connecting rod direction vector ij (t) To provide early warning of collision and interference risks between electric cylinders, including:

[0109] In each detection cycle T, according to d c (t) is compared with the safety distance threshold to determine whether there is a risk of collision and interference between the electric cylinders:

[0110] If d c (t)<L i sinθ ij (t)+L j sinθ ij (t)+r i +r j , it is determined that there is a risk of collision and interference between the i-th electric cylinder and the j-th electric cylinder, and an early warning is issued;

[0111] In each detection cycle T, according to θ ij (t) and the safety angle threshold θ safe Compare the two to determine whether there is a risk of collision and interference between the electric cylinders:

[0112] If θ ij (t) is less than θ safe , it is determined that there is a risk of collision and interference between the i-th electric cylinder and the j-th electric cylinder, and an early warning is issued.

[0113] As an optional embodiment, the method of the present invention further includes:

[0114] The motion state and position prediction results of the electric cylinder are represented through a three-dimensional visual graphic interface, and different colors or marks are used to indicate electric cylinders with interference risks.

[0115] As an optional embodiment, the method further includes:

[0116] Based on the judgment of the existence of collision and interference risks, the RRT algorithm or A* algorithm is used to find the optimal motion trajectory of the electric cylinder without collision according to the platform target posture information, and the movement of the electric cylinder is replanned accordingly.

[0117] {Example 2}

[0118] In this embodiment, we will describe the exemplary implementation process of the method of the above embodiment of the present invention in more detail with reference to examples.

[0119] (1) Based on sensors or platform motion control systems, obtain platform-related motion process data.

[0120] 1.1、Real-time location information acquisition

[0121] In this embodiment, the current position coordinates can be read from a high-precision position sensor (such as a linear encoder or a magnetostrictive sensor) installed on each electric cylinder. Taking the six-degree-of-freedom parallel motion platform as an example, including 6 electric cylinders, the current position coordinates of the i-th electric cylinder are recorded as (x i ,y i ,z i ); x i ,y i ,z i They respectively represent the x, y, and z coordinates of the current position of the ith electric cylinder; the units are millimeters, and the accuracy is preferably at the micron level.

[0122] 1.2. Speed ​​information acquisition

[0123] In this embodiment, the speed information of the electric cylinder can be obtained through position difference calculation or from feedback data of the platform motion control system.

[0124] For example, the speed of the electric cylinder in the x, y, and z directions, that is, the rotation speed around the x, y, and z directions, is calculated based on the position data and sampling time interval of two adjacent samples, in millimeters per second (mm / s), with an accuracy of ±0.1mm / s.

[0125] 1.3 Acceleration information acquisition

[0126] In this embodiment, the speed information of the electric cylinder can be obtained through speed difference calculation or from feedback data of the platform motion control system.

[0127] As mentioned above, similar to the speed calculation method, the acceleration is estimated by taking the difference between two adjacent speed sampling data, and the unit is millimeter per square second (mm / s 2 ), the accuracy reaches ±0.01mm / s 2 .

[0128] 1.4 Target pose information

[0129] In this embodiment, the target position and posture information of the moving platform can be obtained from the motion planning module of the platform motion control system. As an optional example, the target posture information is represented as (x d,y d ,z d ,α,β,γ), including position information and attitude information, where x d ,y d ,z d They represent the target position coordinates in the Cartesian coordinate system, α, β, and γ represent the rotation angles around the x, y, and z directions, respectively, expressed in the form of Euler angles.

[0130] (2) Constructing an equivalent motion model of a six-degree-of-freedom motion platform

[0131] In this embodiment, according to the motion characteristics of the electric cylinder and the working requirements of the platform, a suitable motion model is selected for trajectory prediction. In the six-degree-of-freedom parallel motion platform, the motion of the electric cylinder is a complex spatial motion, but in a short time interval it can be approximately equivalent to uniformly accelerated linear motion or segmented uniformly accelerated linear motion.

[0132] To this end, in an embodiment of the present invention, the six-degree-of-freedom motion platform is equivalent to a uniformly accelerated linear motion platform within the set time step Δt and detection period T of the platform collision detection, and the platform equivalent motion equation is established accordingly.

[0133] In the embodiment of the present invention, the time step Δt can be determined according to the movement speed of the platform and the response time of the control system, and is selected between 0.005s and 0.01s. The detection period T is usually set according to the working environment and safety requirements of the platform, and usually needs to cover the time period from the current state of the platform to the possible collision, and is selected between 0.1s and 0.5s.

[0134] In the uniformly accelerated linear motion model, assuming that the motion of the electric cylinder in the x, y, and z directions is independent of each other and is approximately equivalent to uniformly accelerated linear motion in a short time interval, the equivalent motion equation of the six-degree-of-freedom parallel motion platform is established as follows:

[0135] Establish the motion equation of the i-th electric cylinder in the x direction i (t):

[0136]

[0137] Among them, at t = 0, x i (0), v xi (0), a xi (0) are the initial position, initial velocity and initial acceleration of the i-th electric cylinder in the x direction;

[0138] Establish the motion equation y of the i-th electric cylinder in the y direction i (t):

[0139]

[0140] Among them, at t = 0, y i (0), v yi (0)t、a yi (0) are the initial position, initial velocity and initial acceleration of the i-th electric cylinder in the y direction;

[0141] Establish the motion equation of the i-th electric cylinder in the z direction i (t):

[0142]

[0143] Among them, at t = 0, z i (0), v zi (0), a zi (0) are the initial position, initial velocity and initial acceleration of the i-th electric cylinder in the z direction.

[0144] (3) Starting from the current time t=0, the electric cylinder position is predicted cyclically at intervals of time step Δt until the detection period T, thereby constructing a position trajectory sequence.

[0145] Combination Figure 2 As shown, in each cycle, the electric cylinder position of the next time step is calculated based on the selected motion model and the last predicted position, velocity, and acceleration, including:

[0146] For the position prediction of the i-th electric cylinder in the x-direction, at the k-th cycle, t = k*Δt, the position prediction value in the x-direction is:

[0147]

[0148] Among them, x i ((k-1)Δt),v xi ((k-1)Δt), a xi ((k-1)Δt) are the position, velocity and acceleration in the x direction obtained from the last prediction;

[0149] For the position prediction of the i-th electric cylinder in the y direction, at the k-th cycle, t = k*Δt, the position prediction value in the y direction is:

[0150]

[0151] Among them, y i ((k-1)Δt),v yi ((k-1)Δt、a yi ((k-1)Δt) are the position, velocity and acceleration in the y direction obtained from the last prediction;

[0152] For the position prediction of the i-th electric cylinder in the z direction, at the k-th cycle, t = k*Δt, the position prediction value in the z direction is:

[0153]

[0154] Among them, z i ((k-1)Δt),v zi ((k-1)Δt), a zi ((k-1)Δt) are the position, velocity and acceleration in the z direction obtained from the last prediction.

[0155] As an alternative, the speed and acceleration of the electric cylinder can be updated in each cycle. The speed update can be calculated based on the acceleration and time step, that is:

[0156] v xi (kΔt)=v xi ((k-1)Δt+a xi ((k-1)Δt)Δt.

[0157] The acceleration can be updated based on control inputs to the system or actual measurements.

[0158] On this basis, a position trajectory sequence is formed according to all position prediction values ​​of each detection period T for subsequent collision prediction operations.

[0159] (4) Calculate the distance d between the axis of any i-th electric cylinder and the j-th electric cylinder c (t) and the angle θ between the connecting rod direction vector ij (t).

[0160] In this embodiment, the electric cylinder is equivalent to a structure consisting of a cylindrical cylinder body and a connecting rod.

[0161] Combination Figure 3 As shown, the angle θ between the direction vector of the connecting rod of any i-th electric cylinder and the j-th electric cylinder axis is calculated by combining the geometric model of the cylindrical cylinder body and the connecting rod of the electric cylinder and the position trajectory sequence. ij (t), including:

[0162] First, an equivalent model of the electric cylinder consisting of a cylindrical cylinder body and a connecting rod is established. Assume that the radius of the i-th electric cylinder is r i , the connecting rod length is L i , the initial position vector is The position vector at a certain time t is Similarly, for the jth electric cylinder, the radius is r j , the connecting rod length is L j , the initial position vector is The position vector at a certain time t is Among them, r i =r j ;

[0163] Then, the connecting rod direction vectors of the i-th electric cylinder and the j-th electric cylinder at a certain time t are calculated respectively.

[0164] Its mold length represents the movement length of the connecting rod of the i-th electric cylinder at time t; Its mold length represents the movement length of the jth electric cylinder connecting rod at time t;

[0165] Then, based on the connecting rod direction vector Use vector dot product operation to calculate the angle θ between the connecting rod direction vectors ij (t):

[0166]

[0167] but:

[0168] Furthermore, the distance d between the axis of any i-th electric cylinder and the j-th electric cylinder is calculated by combining the geometric model of the cylindrical cylinder body and the connecting rod of the electric cylinder and the position trajectory sequence. c (t), including:

[0169]

[0170] (5) Based on the distance d between the axes of the i-th electric cylinder and the j-th electric cylinder c (t) and the angle θ between the connecting rod direction vector ij (t) to provide early warning of collision and interference risks between electric cylinders.

[0171] 5.1. In each detection period T, according to d c (t) is compared with the safety distance threshold to determine whether there is a risk of collision and interference between the electric cylinders:

[0172] If d c (t) <L i sinθ ij (t)+L j sinθ ij (t)+r i +r j , it is determined that there is a risk of collision and interference between the i-th electric cylinder and the j-th electric cylinder, and an early warning is issued;

[0173] 5.2. In each detection cycle T, according to θ ij (t) and the safety angle threshold θ safe Compare the two to determine whether there is a risk of collision and interference between the electric cylinders:

[0174] If θ ij (t) is less than θ safe , it is determined that there is a risk of collision and interference between the i-th electric cylinder and the j-th electric cylinder, and an early warning is issued.

[0175] Furthermore, the motion state and position prediction results of the electric cylinder are represented through a three-dimensional visual graphic interface, and different colors or marks are used to indicate the electric cylinders with interference risks.

[0176] When receiving a collision warning signal, the platform motion control system or operator can take corresponding adjustment measures according to the warning level. For example, for a low-level warning, the control system can reduce the movement speed of the electric cylinder by adjusting the speed command of the motor. For example, the current speed command is reduced by a certain proportion (such as 0.8-0.9 times) while continuing to monitor the collision risk. If the collision risk still exists or increases further after reducing the speed, consider taking more stringent measures.

[0177] For advanced warning, the control system immediately stops the movement of all electric cylinders and enters safety mode.

[0178] As an optional embodiment, based on the judgment that there is a risk of collision and interference, the optimal motion trajectory of the electric cylinder without collision is found through the RRT algorithm or the A* algorithm according to the platform target posture information, and the movement of the electric cylinder is replanned accordingly.

[0179] {Example 3}

[0180] In combination with the multi-degree-of-freedom parallel motion platform motion trajectory collision risk warning method of the above embodiment, according to an embodiment of the present invention, a computer system is also proposed, including: one or more processors and a memory.

[0181] The aforementioned memory is used to store operable instructions, which, when executed by the one or more processors, enable the one or more processors to perform operations, and the operations include the process of the multi-degree-of-freedom parallel motion platform motion trajectory collision risk warning method for a simulation simulator of the aforementioned embodiment.

[0182] Although the present invention has been disclosed as above with preferred embodiments, it is not intended to limit the present invention. A person with ordinary knowledge in the technical field to which the present invention belongs may make various changes and modifications without departing from the spirit and scope of the present invention. Therefore, the protection scope of the present invention shall be determined by the definition of the claims.

Claims

1. A method for early warning of motion trajectory collision risk of a multi-degree-of-freedom parallel motion platform for a simulation simulator, characterized in that: include: Get the current position coordinates of the i-th electric cylinder among the n electric cylinders of the multi-degree-of-freedom parallel motion platform, denoted as (x i ,y i ,z i ); x i ,y i ,z i Respectively represent the x, y, and z coordinates of the current position of the i-th electric cylinder; Get the speed and acceleration of the i-th electric cylinder, denoted as (v xi ,v yi ,v zi ) and (α xi ,α yi ,α zi ) and, v xi ,v yi ,v zi With α xi ,α yi ,α zi The speed and acceleration of the i-th electric cylinder in the x, y, and z directions respectively; Get the platform target pose information, recorded as (x d ,y d ,z d ,α,β,γ), where x d ,y d ,z d They represent the target position coordinates in the Cartesian coordinate system, and α, β, and γ represent the rotation angles around the x, y, and z directions respectively; Establish the equivalent motion equation of the six-degree-of-freedom parallel motion platform; Configure the time step Δt and detection period T of platform collision detection; Based on the platform equivalent motion equation, starting from the current time t=0, the prediction is performed cyclically at intervals of time step Δt until the detection period T; in each cycle, the electric cylinder position of the next time step is calculated based on the selected motion model and the position, velocity, and acceleration predicted last time; According to all the position prediction values ​​of each detection period T, a position trajectory sequence is formed; Combined with the geometric model of the cylindrical cylinder body and the connecting rod of the electric cylinder and the position trajectory sequence, the distance d between the axes of any i-th electric cylinder and the j-th electric cylinder is calculated. c (t) and the angle θ between the connecting rod direction vector ij (t); Based on the distance d between the axes of the i-th electric cylinder and the j-th electric cylinder c (t) and the angle θ between the connecting rod direction vector ij (t) to provide early warning of collision and interference risks between electric cylinders.

2. The method for early warning of motion trajectory collision risk of a multi-degree-of-freedom parallel motion platform for a simulation simulator according to claim 1, characterized in that: The speed and acceleration of the i-th electric cylinder are respectively obtained by calculating the position difference and the speed difference, or obtained from the feedback data of the platform motion control system.

3. The method for early warning of motion trajectory collision risk of a multi-degree-of-freedom parallel motion platform for a simulation simulator according to claim 1, characterized in that: The equivalent motion equation of the six-degree-of-freedom parallel motion platform is established, including: Establish the motion equation of the i-th electric cylinder in the x direction i (t): Among them, at t = 0, x i (0), v xi (0), a xi (0) are the initial position, initial velocity and initial acceleration of the i-th electric cylinder in the x direction; Establish the motion equation y of the i-th electric cylinder in the y direction i (t): Among them, at t = 0, y i (0), v yi (0)t、a yi (0) are the initial position, initial velocity and initial acceleration of the i-th electric cylinder in the y direction; Establish the motion equation of the i-th electric cylinder in the z direction i (t): Among them, at t = 0, z i (0), v xi (0), a zi (0) are the initial position, initial velocity and initial acceleration of the i-th electric cylinder in the z direction.

4. The method for early warning of motion trajectory collision risk of a multi-degree-of-freedom parallel motion platform for a simulation simulator according to claim 3 is characterized in that: Based on the platform equivalent motion equation, starting from the current time t=0, the cyclic prediction is performed at intervals of time step Δt until the detection period T; In each cycle, the electric cylinder position for the next time step is calculated based on the selected motion model and the last predicted position, velocity, and acceleration, including: For the position prediction of the i-th electric cylinder in the x-direction, at the k-th cycle, t = k*Δt, the position prediction value in the x-direction is: Among them, x i ((k-1)Δt),v xi ((k-1)Δt), a xi ((k-1)Δt) are the position, velocity and acceleration in the x direction obtained from the last prediction; For the position prediction of the i-th electric cylinder in the y direction, at the k-th cycle, t = k*Δt, the position prediction value in the y direction is: Among them, y i ((k-1)Δt),v yi ((k-1)Δt), a yi ((k-1)Δt) are the position, velocity and acceleration in the y direction obtained from the last prediction; For the position prediction of the i-th electric cylinder in the z direction, at the k-th cycle, t = k*Δt, the position prediction value in the z direction is: Among them, z i ((k-1)Δt),v zi ((k-1)Δt), a zi ((k-1)Δt) are the position, velocity and acceleration in the z direction obtained from the last prediction.

5. The method for early warning of motion trajectory collision risk of a multi-degree-of-freedom parallel motion platform for a simulation simulator according to claim 4, characterized in that: The geometric model of the cylindrical cylinder body and the connecting rod of the electric cylinder and the position trajectory sequence are combined to calculate the angle θ between any i-th electric cylinder and the connecting rod direction vector of the j-th electric cylinder axis. ij (t), including: First, an equivalent model of the electric cylinder consisting of a cylindrical cylinder body and a connecting rod is established. Assume that the radius of the i-th electric cylinder is r i , the connecting rod length is L i , the initial position vector is The position vector at a certain time t is Similarly, for the jth electric cylinder, the radius is r j , the connecting rod length is L j , the initial position vector is The position vector at a certain time t is Among them, r i =r j ; Then, the connecting rod direction vectors of the i-th electric cylinder and the j-th electric cylinder at a certain time t are calculated respectively. Its mold length represents the movement length of the connecting rod of the i-th electric cylinder at time t; Its mold length represents the movement length of the jth electric cylinder connecting rod at time t; Then, based on the connecting rod direction vector Use vector dot product operation to calculate the angle θ between the connecting rod direction vectors ij (t): but:

6. The method for early warning of motion trajectory collision risk of a multi-degree-of-freedom parallel motion platform for a simulation simulator according to claim 5, characterized in that: The geometric model of the cylindrical cylinder body and the connecting rod of the electric cylinder and the position trajectory sequence are combined to calculate the distance d between the axes of any i-th electric cylinder and the j-th electric cylinder. c (t), including:

7. The method for early warning of motion trajectory collision risk of a multi-degree-of-freedom parallel motion platform for a simulation simulator according to claim 1, characterized in that: Based on the distance d between the axes of the i-th electric cylinder and the j-th electric cylinder c (t) and the angle θ between the connecting rod direction vector ij (t) To provide early warning of collision and interference risks between electric cylinders, including: In each detection cycle T, according to d c (t) is compared with the safety distance threshold to determine whether there is a risk of collision and interference between the electric cylinders: If d c (t)<L i sinθ ij (t)+L j sinθ ij (t)+r i +r j , it is determined that there is a risk of collision and interference between the i-th electric cylinder and the j-th electric cylinder, and an early warning is issued; In each detection cycle T, according to θ ij (t) and the safety angle threshold θ safe Compare the two to determine whether there is a risk of collision and interference between the electric cylinders: If θ ij (t) is less than θ safe , it is determined that there is a risk of collision and interference between the i-th electric cylinder and the j-th electric cylinder, and an early warning is issued.

8. The method for early warning of motion trajectory collision risk of a multi-degree-of-freedom parallel motion platform for a simulation simulator according to claim 1, characterized in that: The method further comprises: The motion state and position prediction results of the electric cylinder are represented through a three-dimensional visual graphic interface, and different colors or marks are used to indicate electric cylinders with interference risks.

9. The method for early warning of motion trajectory collision risk of a multi-degree-of-freedom parallel motion platform for a simulation simulator according to claim 1, characterized in that: The method further comprises: Based on the judgment of the existence of collision and interference risks, the RRT algorithm or A* algorithm is used to find the optimal motion trajectory of the electric cylinder without collision according to the platform target posture information, and the movement of the electric cylinder is replanned accordingly.

10. A computer system, characterized in that: include: one or more processors; A memory storing operable instructions, wherein when the instructions are executed by the one or more processors, the one or more processors perform operations, wherein the operations include the process of the motion trajectory collision risk warning method for a multi-degree-of-freedom parallel motion platform for a simulation simulator as described in any one of claims 1-9.

Images