Rapid dynamic collision detection method for movement track of positioner

Abstract

The invention discloses a rapid dynamic collision detection method for the motion trail of a positioner, and the method comprises the steps: firstly constructing a forward kinematics equation of the positioner through forward kinematics analysis, representing the position of a point on a vertical column of the positioner as a polynomial function of a joint parameter, and providing an accurate mathematical model for collision detection; thirdly, defining a tangent configuration space of the positioner, re-parameterizing the forward kinematics through self-defined parameters, and constructing a mathematical space for describing all possible postures of the positioner; on the basis, the positioner and the obstacle are abstracted as convex bodies in a topological linear space, a series of hyperplanes for segmenting the positioner and the obstacle are searched by using a polynomial convex optimization method, and the hyperplanes are used as collision-free proof, so that potential collision risks can be detected in the planned movement track of the positioner. According to the technical scheme provided by the invention, the problems of low collision detection efficiency and insufficient accuracy in the prior art are solved.

Description

Technical Field

[0001] The present invention relates to the field of, but is not limited to, collision detection technology in large aircraft component assembly, and in particular to a method for rapid dynamic collision detection of a positioner motion trajectory. Background Art

[0002] In the development of industrial automation and positioner technology, positioners are able to perform various complex tasks, such as assembly, handling, welding and processing. This type of positioner is composed of multiple columns connected by joints, each of which usually has a revolute joint or a prismatic joint. The revolute joint allows rotation around the axis, while the prismatic joint allows translation along the axis. These joints usually have only one degree of freedom.

[0003] Dynamic collision detection is a key technology in the motion planning of the locator, which aims to predict and avoid collisions with obstacles in the surrounding environment during the locator's mission. Traditional collision detection methods are mainly based on geometric models and rigid body collision detection algorithms. The computational complexity of the above traditional collision detection methods is usually high, especially when dealing with complex locator motion paths and multiple obstacles, and usually cannot meet the requirements of real-time and accuracy. Summary of the invention

[0004] Purpose of the present invention: In order to solve the above-mentioned technical problems, an embodiment of the present invention provides a method for fast dynamic collision detection of a locator motion trajectory to solve the problem that the computational complexity of traditional collision detection methods is usually high, especially when dealing with complex locator motion paths and multiple obstacles, and usually cannot meet the requirements of real-time and accuracy.

[0005] Technical solution of the present invention: In a first aspect, an embodiment of the present invention provides a method for fast dynamic collision detection of a locator motion trajectory, comprising:

[0006] Step 1, performing forward kinematics analysis on the positioner, including: performing forward kinematics analysis on the positioner based on its structure, and establishing a forward kinematics equation of the positioner;

[0007] Step 2, based on the forward kinematics equation, constructing a tangent configuration space of the locator; wherein the tangent configuration space describes all possible postures that the locator may achieve under different joint parameters;

[0008] Step 3, using the constructed tangent configuration space and the locator motion trajectory represented by the forward kinematics equation, dynamically detect the collision of the locator, including: based on the fact that both the locator and the obstacle are convex bodies, a polynomial convex optimization method is used to search for a series of hyperplanes that separate the locator and the surrounding obstacles at each moment. If a hyperplane is found, it is detected that there is no collision between the locator and the obstacle.

[0009] Optionally, in the method for rapid dynamic collision detection of the locator motion trajectory as described above,

[0010] The structure of the locator in step 1 is as follows: the locator is composed of multiple columns and joints, each column has a joint, the joint type is a prismatic joint (P) or a rotational joint (R), each joint has a single degree of freedom, which is the degree of freedom of translation along the axis or rotation around the axis, and the degrees of freedom of each joint are different.

[0011] Optionally, in the method for rapid dynamic collision detection of locator motion trajectory as described above, step 1 comprises:

[0012] According to the type of rotation or prismatic and motion freedom parameters of each joint, the forward kinematics equations of the position and posture of the end effectors of each column of the positioner are established, and the forward kinematics equations are expressed as polynomial functions of time for subsequent motion trajectory description and dynamic collision detection in the time domain;

[0013] The dynamic collision detection refers to: performing rapid collision detection at each moment during the movement of the locator, and the movement process is represented as a polynomial function of time.

[0014] Optionally, in the method for fast dynamic collision detection of the locator motion trajectory as described above, the forward kinematic analysis of the locator in step 1 comprises the following steps:

[0015] Step 11: Based on the joint types and joint parameters, obtain the joint parameters q i Polynomial functions with respect to time;

[0016] Step 12: Based on the joint parameters q of each joint i , construct the forward kinematics equation of the positioner, express the forward kinematics equation as a multilinear trigonometric polynomial; express the ωth component of the position of the end point A on the positioner column relative to the F base coordinate system as:

[0017]

[0018] Where j represents the jth column, i represents the ith joint, c jω is a coefficient determined based on the column parameters, determined by the length of the jth column of the locator, ξ ij,ω (q i ) represents the forward kinematics parameters, which are determined according to the joints of the locator.

[0019] Optionally, in the method for rapid dynamic collision detection of the locator motion trajectory as described above, in step 11,

[0020] According to the type of each joint, determine the joint parameters and forward kinematics parameters of forward kinematics;

[0021] If the i-th joint is a revolute joint, then the joint parameter q i Denoted as θ i , the forward kinematics parameters are expressed as:

[0022] ξ ij,ω (q i )∈{cos(θ i ), sin(θ i )};

[0023] If the i-th joint is a prismatic joint, then the joint parameter q i Represented as d i , the forward kinematics parameter is expressed as ξ ij,ω (q i )∈d i .

[0024] Optionally, in the method for rapid dynamic collision detection of the motion trajectory of the positioner as described above, in step 2, based on the forward kinematics equation obtained by forward kinematics analysis, the tangent configuration space of the positioner is constructed in the following manner:

[0025] The custom parameter S is formed by reparameterization. The motion state of the locator is described by the polynomial of the custom parameter S, and the joint parameters q1, q2, q3 are regarded as variables of the polynomial. The custom parameter S is a linear combination of the joint parameters:

[0026] S = a1q1 + a2q2 + a3q3;

[0027] Among them, a1, a2, a3 are the reparameterized coefficients;

[0028] Substitute the linear expression of the custom parameter S into the forward kinematics equation to solve the joint parameter q i , and the joint parameters q i It is expressed as a function of the custom parameter S, so that the forward kinematics equation is expressed as a function of the custom parameter S, forming the tangent configuration space of the positioner.

[0029] Optionally, in the method for rapid dynamic collision detection of locator motion trajectory as described above, step 2 comprises:

[0030] Step 21, Customize parameters

[0031] Step 22, use custom parameter S i Represents ξ ij,ω (q i ),but The forward kinematics parameter is ξ ij,ω (q i)∈{cos(θ i ), sin(θ i )};

[0032] Step 23, re-parameterize the forward kinematics equation of the positioner to construct the tangent configuration space as follows:

[0033]

[0034] in,

[0035] Optionally, in the method for fast dynamic collision detection of locator motion trajectory as described above, the series of hyperplanes in step 3 refers to: a set of separating hyperplanes of the locator and the obstacle corresponding to each moment t of the locator motion trajectory. According to the separating hyperplane theorem, a hyperplane a is a set of separating hyperplanes of the locator and the obstacle if and only if there exists a hyperplane a separating the two objects. T When x+b=0, the two closed convex bodies A and B will not intersect.

[0036] Optionally, in the method for fast dynamic collision detection of the locator motion trajectory as described above, the method of using a polynomial convex optimization method to search for a series of hyperplanes that separate the locator and surrounding obstacles in step 3 includes:

[0037] Step 31, using the motion trajectory, the configuration of the positioner in the tangent configuration space at each moment is represented as A(s), and based on the fact that the forward kinematics parameter S is a polynomial function of time t, A(s) can be represented as A(t);

[0038] Step 32, using polynomial convex optimization method, find a series of hyperplanes a that meet the following conditions A,B (t),b A,B (t), if it can be found, it means there is no collision; if it does not exist, it means that the locator has collided with the obstacle:

[0039]

[0040] Among them, a A,B (t) and b A,B (t) represents the parameters of the hyperplane, A and B represent the configuration space formed by the locator and the configuration space formed by the obstacle, respectively, x(t) represents the points on the two convex sets, and the two inequalities indicate that the points on the two convex bodies are on both sides of the hyperplane.

[0041] In a second aspect, an embodiment of the present invention further provides a computer-readable storage medium, including: a memory and a processor;

[0042] The memory is configured to store executable instructions;

[0043] The processor is specifically configured to implement the method for rapid dynamic collision detection of the locator motion trajectory as described in any one of the above when executing the executable instructions stored in the memory.

[0044] Beneficial effects of the invention: The embodiment of the invention provides a method for fast dynamic collision detection of the motion trajectory of a locator. The method uses a polynomial to describe the motion trajectory of the locator. Through forward kinematics analysis and the construction of a tangent configuration space, the configuration of the locator is represented as a polynomial function of time. Then, a convex optimization method is used to search for a series of hyperplanes that separate the locator and the obstacle, which is used to prove the safe interval between the locator and the obstacle at each moment, thereby achieving fast dynamic collision detection. The technical solution provided by the embodiment of the invention has the following beneficial effects:

[0045] First, high efficiency and real-time performance: through polynomial representation and convex optimization methods, the complexity of dynamic collision detection is significantly simplified, and the detection speed and real-time response capability are improved.

[0046] Second, accuracy and safety: Convex body and convex optimization methods ensure the accuracy and safety of the algorithm in complex environments, effectively avoid misjudgment and misoperation, and ensure the safe operation of the locator.

[0047] Third, wide applicability: It is suitable for locators in various complex environments and task scenarios, including industrial production lines, collaborative locators and autonomous mobile locators.

[0048] The technical solution provided by the present invention not only fills the gap in the existing technology in dynamic collision detection, but also provides important technical support for the safe operation and efficient operation of the positioner, and has significant technical innovation and market application potential. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] The accompanying drawings are used to provide a further understanding of the technical solution of the present invention and constitute a part of the specification. Together with the embodiments of the present application, they are used to explain the technical solution of the present invention and do not constitute a limitation on the technical solution of the present invention.

[0050] Figure 1 It is a structural schematic diagram of a locator;

[0051] Figure 2 A flowchart of a method for rapid dynamic collision detection of a locator motion trajectory provided by an embodiment of the present invention;

[0052] Figure 3 Schematic diagram of the tangent configuration space in an embodiment of the present invention;

[0053] Figure 4 Schematic diagram of searching for a segmentation hyperplane in an embodiment of the present invention. DETAILED DESCRIPTION

[0054] In order to make the purpose, technical solution and advantages of the present invention more clear, the embodiments of the present invention will be described in detail with reference to the accompanying drawings. It should be noted that the embodiments and features in the embodiments of the present application can be combined with each other arbitrarily without conflict.

[0055] As described in the above background technology, the positioner (such as Figure 1 The conventional composition of a locator, its wide application, and the dynamic collision detection method of the locator in motion planning are shown in Figure 1. However, the computational complexity of the traditional collision detection method is usually high, especially when dealing with complex locator motion paths and multiple obstacles, and it usually cannot meet the requirements of real-time and accuracy.

[0056] In addition, the dynamic collision detection of the locator faces the following challenges:

[0057] First, multiple degrees of freedom and nonlinear dynamics: The locator is connected by multiple joints, and its kinematic and dynamic models are usually nonlinear, which increases the complexity of the collision detection algorithm.

[0058] Second, real-time requirements: In many application scenarios, such as automated production lines or collaborative positioner operations, the positioner is required to be able to perform actions under real-time monitoring, so an efficient collision detection algorithm is needed to quickly respond to environmental changes.

[0059] Third, complex environments and dynamic obstacles: Positioners often need to operate in complex environments, which include dynamic obstacles (such as moving people or other positioners), which increases the difficulty of collision detection.

[0060] Fourth, safety and accuracy: Dynamic collision detection not only needs to accurately predict the collision between the locator and the obstacle, but also needs to ensure the safety of the detection algorithm to avoid misjudgment and misoperation.

[0061] In response to the above-mentioned problems and challenges in the dynamic collision detection of the locator, an embodiment of the present invention proposes a fast dynamic collision detection method based on the motion trajectory of the locator. This method is crucial to improving the motion planning and operation efficiency of the locator. Through forward kinematics analysis and polynomial convex optimization methods, it effectively solves the bottleneck problems in traditional methods and has significant technical advantages and application potential.

[0062] The present invention provides the following specific embodiments which can be combined with each other. The same or similar concepts or processes may not be described in detail in some embodiments.

[0063] An embodiment of the present invention provides a method for fast dynamic collision detection of a locator motion trajectory. The method uses a polynomial to describe the motion trajectory of the locator. Through forward kinematics analysis and the construction of a tangent configuration space, the configuration of the locator is represented as a polynomial function of time. Then, a convex optimization method is used to search for a series of hyperplanes that separate the locator and the obstacle, which is used to prove the safe interval between the locator and the obstacle at each moment, thereby achieving fast dynamic collision detection.

[0064] Figure 2 A flowchart of a method for fast dynamic collision detection of a locator motion trajectory provided by an embodiment of the present invention. The method for fast dynamic collision detection of a locator motion trajectory provided by an embodiment of the present invention aims to solve the problems of high computational complexity and poor real-time performance of traditional collision detection methods in complex locator paths and dynamic environments. The method provided by an embodiment of the present invention comprises the following steps:

[0065] Step 1, perform forward kinematic analysis on the positioner;

[0066] In this step, first, a forward kinematic analysis is performed on the locator based on its structure; the locator is composed of multiple columns and joints, each column has a joint, each joint can be a revolute joint or a prismatic joint, each joint has a single degree of freedom of motion, and the degrees of freedom of motion of each joint are different. The purpose of forward kinematic analysis is to calculate the configuration of the locator at a specified moment, that is, the position and posture of the end effector of each column, according to the motion law of each joint. Secondly, according to the type of each joint (revolute or prismatic) and the parameters of the degree of freedom of motion, the forward kinematic equations of the position and posture of the end effector of each column of the locator are established, and the forward kinematic equations are expressed as polynomial functions of time, so as to facilitate the subsequent description of motion trajectory and collision detection in the time domain.

[0067] It should be noted that dynamic collision detection refers to: performing rapid collision detection at each moment during the movement of the locator, and the movement process is represented as a polynomial function of time.

[0068] Step 2, based on the forward kinematics equation, construct the tangent configuration space of the positioner;

[0069] In this step, the tangent configuration space of the locator is constructed based on the forward kinematics equations obtained by forward kinematics analysis. The tangent configuration space is used for all possible postures that the locator can achieve under different joint parameters. The core idea is to convert the configuration parameters of the locator into a time-dependent polynomial function. The forward kinematics equations of the locator are reparameterized through a custom parameterization method, so that the tangent configuration of the locator can be flexibly expressed in the time domain and can be used in the subsequent collision detection.

[0070] Step 3, dynamically detect collision of the locator;

[0071] In this step, the locator is quickly and dynamically detected for collision using the locator motion trajectory represented by the constructed tangent configuration space and the forward kinematics equation. In the specific implementation, based on the fact that both the locator and the obstacle are convex bodies, a series of hyperplanes that separate the locator and the surrounding obstacles are searched at each moment by the polynomial convex optimization method. These hyperplanes mathematically prove the safe interval between the locator and the obstacle, and can ensure that the locator will not collide with the obstacle even in complex motion paths and dynamic environments.

[0072] It should be noted that if a hyperplane is found, it is detected that there is no collision between the locator and the obstacle. The use of the convex optimization method in step 3 is one of the key technologies in the embodiment of the present invention; the convex optimization method can efficiently search for a segmentation hyperplane in a convex body (such as the geometric body of the locator and the obstacle), ensuring that it not only effectively but also accurately divides the collision area between the locator and the obstacle. This method not only improves the speed of collision detection, but also ensures the reliability and accuracy of the detection results.

[0073] The embodiment of the present invention provides a method for fast dynamic collision detection of the motion trajectory of a locator. The method uses a polynomial to describe the motion trajectory of the locator. Through forward kinematics analysis and the construction of a tangent configuration space, the configuration of the locator is represented as a polynomial function of time. Then, a convex optimization method is used to search for a series of hyperplanes that separate the locator and the obstacle, which is used to prove the safe interval between the locator and the obstacle at each moment, thereby achieving fast dynamic collision detection. The technical solution provided by the embodiment of the present invention has the following beneficial effects:

[0074] First, high efficiency and real-time performance: through polynomial representation and convex optimization methods, the complexity of dynamic collision detection is significantly simplified, and the detection speed and real-time response capability are improved.

[0075] Second, accuracy and safety: Convex body and convex optimization methods ensure the accuracy and safety of the algorithm in complex environments, effectively avoid misjudgment and misoperation, and ensure the safe operation of the locator.

[0076] Third, wide applicability: It is suitable for locators in various complex environments and task scenarios, including industrial production lines, collaborative locators and autonomous mobile locators.

[0077] The technical solution provided by the present invention not only fills the gap in the existing technology in dynamic collision detection, but also provides important technical support for the safe operation and efficient operation of the positioner, and has significant technical innovation and market application potential.

[0078] The following describes in detail the implementation method and specific implementation steps of the locator motion trajectory fast dynamic collision detection method provided by the embodiment of the present invention:

[0079] Step 1, perform forward kinematic analysis on the positioner;

[0080] First, a detailed forward kinematic analysis is performed on the locator based on its structure. The locator consists of three columns, each of which has a joint, and the joint type is either a prismatic joint or a revolute joint. i Translation along its axis, the corresponding joint parameter is distance d i ; Rotary joint P i Rotate along its axis, the corresponding joint parameter is the rotation angle θ i According to the type and motion parameters (such as length or rotation angle) of each joint, the forward kinematics equation of the position and posture of the end effector of the positioner is established. The method of establishing the forward kinematics equation includes the following implementation process:

[0081] Step 11: Based on the joint types and joint parameters, obtain the joint parameters q i Polynomial functions with respect to time;

[0082] In this step, the joint parameters and forward kinematics parameters of forward kinematics are determined according to the type of each joint.

[0083] In one possible implementation, if the i-th joint is a revolute joint, the joint parameter q i Denoted as θ i , the forward kinematics parameters are expressed as:

[0084] ξ ij,ω (q i )∈{cos(θ i ), sin(θ i )};

[0085] In another possible implementation, if the i-th joint is a prismatic joint, the joint parameter q i Represented as d i , the forward kinematics parameter is expressed as ξ ij,ω (q i )∈d i .

[0086] For example, suppose that all joints are prismatic joints, and their joint parameters d1, d2, and d3 are polynomial functions of time:

[0087]

[0088] For example, suppose that all joints are rotational joints, and their joint parameters θ1, θ2, θ3 are polynomial functions of time:

[0089]

[0090] Step 12: Based on the joint parameters q of each joint i , construct the forward kinematics equation of the positioner, express the forward kinematics equation as a multilinear trigonometric polynomial; express the ωth component of the position of the end point A on the positioner column relative to the F base coordinate system as:

[0091]

[0092] Where j represents the jth column, i represents the ith joint, c jω is a coefficient determined based on the column parameters, determined by the length of the jth column of the locator, ξ ij,ω (q i ) represents the forward kinematics parameters, which are determined according to the joints of the locator.

[0093] Step 2, construct the tangent configuration space of the locator;

[0094] In this step, based on the forward kinematics equation obtained by forward kinematics analysis, the present invention further constructs the tangent configuration space of the positioner. This tangent configuration space describes all possible postures that the positioner can achieve under different joint parameters, such as Figure 3 As shown, it is a schematic diagram of the tangent configuration space in an embodiment of the present invention. The custom parameter S is formed by reparameterization, and the motion state of the positioner is described by the polynomial of the custom parameter S. The joint parameters q1, q2, q3 are regarded as variables of the polynomial, and the custom parameter S can be a linear combination of the joint parameters:

[0095] S = a1q1 + a2q2 + a3q3;

[0096] Among them, a1, a2, a3 are the reparameterized coefficients;

[0097] Substitute the linear expression of the custom parameter S into the forward kinematics equation to solve the joint parameter q i , and the joint parameters q i It is expressed as a function of the custom parameter S, so that the forward kinematics equation is expressed as a function of the custom parameter S, forming the tangent configuration space of the positioner.

[0098] In one implementation, the implementation process of step 2 may include:

[0099] Step 21, Customize parameters

[0100] Step 22, use custom parameter S i Represents ξ ij,ω (q i ),

[0101] but The forward kinematics parameter is ξ ij,ω (q i )∈{cos(θ i ), sin(θ i )};

[0102] Step 23, re-parameterize the forward kinematics equation of the positioner to construct the tangent configuration space as follows:

[0103]

[0104] in,

[0105] Step 3, dynamically detect collision of the locator;

[0106] In the embodiment of the present invention, the locator and the obstacle are defined as convex bodies, so that the collision detection problem can be handled using convex optimization theory; the convex body refers to a closed set in a topological linear space that is not only convex (i.e., the line segment between any two points is still within the set) but also has an interior point.

[0107] In this step, a polynomial convex optimization method is used to search for a series of hyperplanes that separate the two convex bodies. By detecting the series of hyperplanes, it can be determined whether the locator will collide with surrounding obstacles during movement. These hyperplanes mathematically prove the safe interval between the locator and obstacles, and can ensure that the locator will not collide with obstacles even in complex motion paths and dynamic environments; Figure 4 As shown in FIG. 1 , it is a schematic diagram of searching for a segmentation hyperplane in an embodiment of the present invention. It should be noted that the series of hyperplanes in this step refers to the set of separation hyperplanes of the locator and the obstacle corresponding to each moment t of the locator's motion trajectory. According to the separation hyperplane theorem, a hyperplane a is obtained if and only if there exists a hyperplane a separating the two objects. T When x+b=0, the two closed convex bodies A and B will not intersect.

[0108] In one implementation, the implementation process of step 3 may include:

[0109] Step 31, using the motion trajectory, the configuration of the positioner in the tangent configuration space at each moment is represented as A(s), and based on the fact that the forward kinematics parameter S is a polynomial function of time t, A(s) can be represented as A(t);

[0110] Step 32, using polynomial convex optimization method, find a series of hyperplanes a that meet the following conditions A,B (t),b A,B (t), if it can be found, it means there is no collision; if it does not exist, it means that the locator has collided with the obstacle:

[0111]

[0112] Among them, a A,B (t) and b A,B (t) represents the parameters of the hyperplane, A and B represent the configuration space formed by the locator and the configuration space formed by the obstacle, respectively, x(t) represents the points on the two convex sets, and the two inequalities indicate that the points on the two convex bodies are on both sides of the hyperplane.

[0113] The following is an explanation using a specific implementation example:

[0114] Assume the obstacle is a sphere, its equation is: (x-5) 2 +(y-5) 2 +(z-5) 2 ≤2 2 ; The coordinates of the sphere center are (5, 5, 5) and the radius is 2. The trajectory of the end effector of the positioner is: x EE =d1(t),y EE =d2(t),z EE =d3(t).

[0115] In step 3 above, the polynomial convex optimization method is used.

[0116] This implementation example uses a polynomial convex optimization method to search for a series of hyperplanes that separate the locator and the surrounding obstacles. This method can effectively handle optimization problems in high-dimensional space and has good computational efficiency. In this way, a series of hyperplanes that separate the locator and obstacles can be quickly found to complete collision detection. For spherical obstacles, a hyperplane H is found whose normal vector is n = (A, B, C) and whose intercept is D, such that: Ax + By + Cz + D = 0. This hyperplane must meet the following conditions to ensure that the locator does not intersect with the obstacle at any time t. The conditions that the hyperplane is required to meet are:

[0117] Ax EE (t)+By EE (t)+Cz EE (t)+D>r.

[0118] Through the polynomial convex optimization method, the values ​​of the hyperplane parameters A, B, and C at each moment can be calculated, proving that under this trajectory, the locator will not collide with the circular obstacle.

[0119] Combined with the above steps, the present invention provides a complete set of collision detection algorithms, which can receive the joint parameters of the locator in real time, quickly calculate the motion trajectory of the locator, and use polynomial convex optimization method to detect whether the locator has the risk of colliding with obstacles at each moment. If a potential collision is detected, the algorithm will issue a warning in time to avoid accidents.

[0120] The fast dynamic collision detection method for the motion trajectory of the locator proposed in the embodiment of the present invention effectively solves the problems of high computational complexity and poor real-time performance in the traditional dynamic collision detection method through innovative forward kinematics analysis, construction of tangent configuration space and application of convex optimization method, and has the following significant technical advantages and application value: efficient and real-time collision detection capability; accurate and reliable collision detection results; applicable to a variety of complex environments and task scenarios; technical innovation and market application potential. The present invention fills the gap in the field of dynamic collision detection of the prior art and has significant technical innovation and market application potential. In the fields of industrial automation, intelligent manufacturing and locator applications, it can provide important technical support for improving production efficiency and reducing accident risks.

[0121] In summary, the technical solution provided by the present invention not only achieves an innovative breakthrough in dynamic collision detection in theory, but also has significant technical advantages and practical value in practical applications. Further research and development in the future will further promote the widespread application and promotion of this technology in the fields of industrial intelligence and locator technology.

[0122] Based on the method for fast dynamic collision detection of a locator motion trajectory provided by the above-mentioned embodiment of the present invention, an embodiment of the present invention further provides a computer-readable storage medium, including: a memory and a processor;

[0123] The memory is configured to store executable instructions;

[0124] The processor is specifically configured to implement the method for rapid dynamic collision detection of the locator motion trajectory as described in any one of the above items when executing the executable instructions stored in the memory.

[0125] Although the embodiments disclosed in the present invention are as above, the contents are only embodiments adopted to facilitate understanding of the present invention and are not intended to limit the present invention. Any technician in the field to which the present invention belongs can make any modifications and changes in the form and details of implementation without departing from the spirit and scope disclosed in the present invention, but the patent protection scope of the present invention shall still be subject to the scope defined in the attached claims.

Claims

1. A method for rapid dynamic collision detection of a locator motion trajectory, characterized in that: include: Step 1, performing forward kinematics analysis on the positioner, including: performing forward kinematics analysis on the positioner based on its structure, and establishing a forward kinematics equation of the positioner; Step 2, based on the forward kinematics equation, constructing a tangent configuration space of the locator; wherein the tangent configuration space describes all possible postures that the locator may achieve under different joint parameters; Step 3, using the constructed tangent configuration space and the locator motion trajectory represented by the forward kinematics equation, dynamically detect the collision of the locator, including: based on the fact that both the locator and the obstacle are convex bodies, a polynomial convex optimization method is used to search for a series of hyperplanes that separate the locator and the surrounding obstacles at each moment. If a hyperplane is found, it is detected that there is no collision between the locator and the obstacle.

2. The method for rapid dynamic collision detection of a locator motion trajectory according to claim 1, characterized in that: The structure of the locator in step 1 is as follows: the locator is composed of multiple columns and joints, each column has a joint, the joint type is a prismatic joint (P) or a rotational joint (R), each joint has a single degree of freedom, which is the degree of freedom of translation along the axis or rotation around the axis, and the degrees of freedom of each joint are different.

3. The method for rapid dynamic collision detection of a locator motion trajectory according to claim 2, characterized in that: The step 1 comprises: According to the type of rotation or prismatic and motion freedom parameters of each joint, the forward kinematics equations of the position and posture of the end effectors of each column of the positioner are established, and the forward kinematics equations are expressed as polynomial functions of time for subsequent motion trajectory description and dynamic collision detection in the time domain; The dynamic collision detection refers to: performing rapid collision detection at each moment during the movement of the locator, and the movement process is represented as a polynomial function of time.

4. The method for rapid dynamic collision detection of a locator motion trajectory according to claim 3, characterized in that: The forward kinematics analysis of the positioner in step 1 includes the following steps: Step 11: Based on the joint types and joint parameters, obtain the joint parameters q i Polynomial functions with respect to time; Step 12: Based on the joint parameters q of each joint i , construct the forward kinematics equation of the positioner, express the forward kinematics equation as a multilinear trigonometric polynomial; express the ωth component of the position of the end point A on the positioner column relative to the F base coordinate system as: Where j represents the jth column, i represents the ith joint, c jω is a coefficient determined based on the column parameters, determined by the length of the jth column of the locator, ξ ij,ω (q i ) represents the forward kinematics parameters, which are determined according to the joints of the locator.

5. The method for rapid dynamic collision detection of a locator motion trajectory according to claim 4, characterized in that: In the step 11, According to the type of each joint, determine the joint parameters and forward kinematics parameters of forward kinematics; If the i-th joint is a revolute joint, then the joint parameter q i Denoted as θ i , the forward kinematics parameters are expressed as: x ij,ω (q i )∈{cos(θ i ),sin(θ i )}; If the i-th joint is a prismatic joint, then the joint parameter q i Represented as d i , the forward kinematics parameter is expressed as ξ ij,ω (q i )∈d i .

6. The method for rapid dynamic collision detection of a locator motion trajectory according to claim 1, characterized in that: In step 2, based on the forward kinematics equation obtained by forward kinematics analysis, the tangent configuration space of the positioner is constructed as follows: The custom parameter S is formed by reparameterization. The motion state of the locator is described by the polynomial of the custom parameter S, and the joint parameters q1, q2, q3 are regarded as variables of the polynomial. The custom parameter S is a linear combination of the joint parameters: S = a1q1 + a2q2 + a3q3; Among them, a1, a2, a3 are the reparameterized coefficients; Substitute the linear expression of the custom parameter S into the forward kinematics equation to solve the joint parameter q i , and the joint parameters q i It is expressed as a function of the custom parameter S, so that the forward kinematics equation is expressed as a function of the custom parameter S, forming the tangent configuration space of the positioner.

7. The method for rapid dynamic collision detection of a locator motion trajectory according to claim 6, characterized in that: The step 2 comprises: Step 21, Customize parameters Step 22, use custom parameter S i Represents ξ ij,ω (q i ),but The forward kinematics parameter is ξ ij,ω (q i )∈{cos(θ i ), sin(θ i )}; Step 23, re-parameterize the forward kinematics equation of the positioner to construct the tangent configuration space as follows: in, 8. The method for rapid dynamic collision detection of a locator motion trajectory according to any one of claims 1 to 7, characterized in that: The series of hyperplanes in step 3 refers to the set of separating hyperplanes of the locator and the obstacle corresponding to each moment t of the locator's motion trajectory. According to the separating hyperplane theorem, a is a set of separating hyperplanes if and only if there exists a hyperplane a that separates the two objects. T When x+b=0, the two closed convex bodies A and B will not intersect.

9. The method for rapid dynamic collision detection of a locator motion trajectory according to claim 8, characterized in that: The method of using a polynomial convex optimization method in step 3 to search for a series of hyperplanes for segmenting the locator and surrounding obstacles includes: Step 31, using the motion trajectory, the configuration of the positioner in the tangent configuration space at each moment is represented as A(s), and based on the fact that the forward kinematics parameter S is a polynomial function of time t, A(s) can be represented as A(t); Step 32, using the polynomial convex optimization method, find a series of hyperplanes a that satisfy the following conditions A,B (t),b A,B (t), if it can be found, it means there is no collision; if it does not exist, it means that the locator has collided with the obstacle: Among them, a A,B (t) and b A,B (t) represents the parameters of the hyperplane, A and B represent the configuration space formed by the locator and the configuration space formed by the obstacle, respectively, x(t) represents the points on the two convex sets, and the two inequalities indicate that the points on the two convex bodies are on both sides of the hyperplane.

10. A computer-readable storage medium, characterized in that: include: Memory and processor; The memory is configured to store executable instructions; The processor is specifically configured to implement the method for rapid dynamic collision detection of a locator motion trajectory according to any one of claims 1 to 9 when executing the executable instructions stored in the memory.

Images