A method for fast dynamic collision detection of locator motion trajectory

Abstract

This invention discloses a fast dynamic collision detection method for a locator's motion trajectory. First, through forward kinematics analysis, the method constructs the forward kinematic equations of the locator, representing the positions of points on the locator's pillars as polynomial functions of joint parameters, providing an accurate mathematical model for collision detection. Next, the tangent configuration space of the locator is defined, and the forward kinematics is re-parameterized by custom parameters to construct a mathematical space describing all possible poses of the locator. Based on this, the locator and obstacles are abstracted as convex bodies in a topological linear space. A series of hyperplanes separating the locator and obstacles are searched using a polynomial convex optimization method. These hyperplanes serve as collision-free proofs, enabling the detection of potential collision risks in the planned locator motion trajectory. The technical solution provided by this invention solves the problems of low efficiency and insufficient accuracy in collision detection in existing technologies.

Description

Technical Field

[0001] This invention relates to the field of collision detection technology in the assembly of large aircraft components, and particularly to a method for rapid dynamic collision detection of the movement trajectory of a locator. Background Technology

[0002] In the development of industrial automation and positioner technology, positioners are able to perform a variety of complex tasks, such as assembly, handling, welding, and machining. These positioners consist of multiple columns connected by joints. Each joint typically has a rotary joint or a prismatic joint. Rotary joints allow rotation about an axis, while prismatic joints allow translation along an axis. These joints usually have only one degree of freedom.

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

[0004] The purpose of this invention is to solve the above-mentioned technical problems. This invention provides a fast dynamic collision detection method for locator motion trajectories, which solves the problem that traditional collision detection methods usually have high computational complexity, especially when dealing with complex locator motion paths and multiple obstacles, and usually cannot meet the requirements of real-time performance and accuracy.

[0005] The technical solution of the present invention: In a first aspect, embodiments of the present invention provide a method for rapid dynamic collision detection of a locator's motion trajectory, comprising:

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

[0007] Step 2: Based on the forward kinematics equations, construct the tangent configuration space of the positioner; wherein, the tangent configuration space describes all possible postures that the positioner may achieve under different joint parameters;

[0008] Step 3: Using the constructed tangent configuration space and the locator's motion trajectory represented by the forward kinematic equation, the locator is dynamically detected for collisions. This includes: 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 divide the locator and the surrounding obstacles at each time step. If a hyperplane is found, it is detected that there is no collision between the locator and the obstacle.

[0009] Optionally, in the fast dynamic collision detection method for the locator's motion trajectory described above,

[0010] The structure of the positioner in step 1 is as follows: the positioner consists of multiple columns and joints, each column has one joint, the joint type is a prism joint (P) or a rotary joint (R), each joint has a single degree of freedom of motion, which is the degree of freedom of translation along the axis or rotation around the axis, and the degree of freedom of motion of each joint is different.

[0011] Optionally, in the fast dynamic collision detection method for the locator's motion trajectory as described above, step 1 includes:

[0012] Based on the rotational or prismatic and motion degree-of-freedom parameters of each joint, the forward kinematic equations of the position and orientation of the actuators at the ends of each column of the locator are established, and the forward kinematic 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 every moment during the movement of the locator, and the movement process is represented as a polynomial function of time.

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

[0015] Step 11: Based on each joint type and joint parameters, obtain the joint parameter q. i A polynomial function of time;

[0016] Step 12, based on the joint parameters q of each joint i The forward kinematics equations of the locator are constructed and expressed as multilinear trigonometric polynomials. The ω-th component of the position of the end point A on the locator column relative to the F base coordinate system is expressed as:

[0017]

[0018] Where j represents the j-th column, i represents the i-th joint, and c jω ξ is a coefficient determined based on the column parameters, and is determined by the length of the j-th column of the locator. ij,ω (q i ) represents the positive kinematic parameters, which are determined based on the joints of the positioner.

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

[0020] Based on the type of each joint, determine the joint parameters and forward kinematic parameters of the forward kinematics;

[0021] If the i-th joint is a rotational joint, then the joint parameter q i Represented as θ i The positive kinematic 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 positive kinematic parameters are expressed as ξ ij,ω (q i )∈d i .

[0024] Optionally, in the fast dynamic collision detection method for the locator's motion trajectory as described above, in step 2, the method for constructing the tangent configuration space of the locator based on the forward kinematic equations obtained from the forward kinematic analysis is as follows:

[0025] A custom parameter S is generated through reparameterization. The motion state of the positioner is described by a polynomial of the custom parameter S, and the joint parameters q1, q2, and q3 are treated as variables of the polynomial. The custom parameter S is a linear combination of the joint parameters.

[0026] S = a1q1 + a2q2 + a3q3;

[0027] Where a1, a2, and a3 are the reparameterized coefficients;

[0028] The joint parameter q is solved by substituting a linear expression for the custom parameter S into the forward kinematic equation. i and the joint parameter q i It is expressed as a function of the user-defined parameter S, thus representing the forward kinematic equations as a function of the user-defined parameter S, forming the tangent configuration space of the positioner.

[0029] Optionally, in the fast dynamic collision detection method for the locator's motion trajectory as described above, step 2 includes:

[0030] Step 21, Customize parameters

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

[0032] Step 23: Reparameterize the forward kinematics equations of the positioner to construct the tangent configuration space as follows:

[0033]

[0034] in,

[0035] Optionally, in the fast dynamic collision detection method for locator motion trajectory described above, the series of hyperplanes in step 3 refers to the set of separating hyperplanes between the locator and the obstacle corresponding to each time t of the locator's motion trajectory. According to the separating hyperplane theorem, a hyperplane a separates the two objects 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.

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

[0037] Step 31: Using the motion trajectory, the configuration of the locator in the tangent configuration space at each moment is represented as A(s). Based on the fact that the positive kinematic parameter S is a polynomial function of time t, A(s) can be represented as A(t).

[0038] 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 If (t) can be found, it means there is no collision; if it does not exist, it means 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 represent the points on the two convex bodies on opposite sides of the hyperplane.

[0041] In a second aspect, embodiments of the present invention also provide 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 fast dynamic collision detection method for the locator motion trajectory as described in any one of the above description when executing the executable instructions stored in the memory.

[0044] The beneficial effects of this invention: This invention provides a method for rapid dynamic collision detection of a locator's motion trajectory. This method uses a polynomial to describe the locator's motion trajectory. Through forward kinematic analysis and the construction of the tangent configuration space, the locator's configuration is represented as a polynomial function of time. Then, a series of hyperplanes segmenting the locator and obstacles are searched using a convex optimization method to prove the safe interval between the locator and obstacles at each time step, thereby achieving rapid dynamic collision detection. The technical solution provided by this invention has the following beneficial effects:

[0045] First, high efficiency and real-time performance: By using 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: convexity and convex optimization methods ensure the accuracy and safety of the algorithm in complex environments, effectively avoiding misjudgments and misoperations, and guaranteeing the safe operation of the locator.

[0047] Third, it has 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 this invention not only fills the gap in the existing technology for dynamic collision detection, but also provides important technical support for the safe operation and efficient operation of the locator, and has significant technological innovation and market application potential. Attached Figure Description

[0049] The accompanying drawings are provided to further understand the technical solutions of the present invention and constitute a part of the specification. They are used together with the embodiments of this application to explain the technical solutions of the present invention and do not constitute a limitation on the technical solutions of the present invention.

[0050] Figure 1 This is a schematic diagram of the structure of a positioner;

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

[0052] Figure 3 This is a schematic diagram of the tangent configuration space in an embodiment of the present invention;

[0053] Figure 4 This is a schematic diagram of searching for the segmentation hyperplane in an embodiment of the present invention. Detailed Implementation

[0054] To make the objectives, technical solutions, and advantages of the present invention clearer, the embodiments of the present invention will be described in detail below with reference to the accompanying drawings. It should be noted that, unless otherwise specified, the embodiments and features described in this application can be arbitrarily combined with each other.

[0055] As explained in the background section above, the positioner (such as...) Figure 1 The diagram shows the conventional composition of a locator, its wide applications, and its dynamic collision detection methods in motion planning. However, traditional collision detection methods typically have high computational complexity, especially when dealing with complex locator motion paths and multiple obstacles, often failing to meet the requirements of real-time performance and accuracy.

[0056] In addition, the dynamic collision detection of the locator also 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 collision detection algorithms.

[0058] Second, real-time requirements: In many application scenarios, such as automated production lines or collaborative locator operations, the locator is required to perform actions under real-time monitoring. Therefore, efficient collision detection algorithms are needed to quickly respond to environmental changes.

[0059] Third, complex environments and dynamic obstacles: locators often need to operate in complex environments, including dynamic obstacles (such as moving people or other locators), 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] To address the aforementioned problems and challenges in dynamic collision detection of locators, this invention proposes a fast dynamic collision detection method based on the locator's motion trajectory. This method is crucial for improving the motion planning and operational efficiency of locators. By employing forward kinematics analysis and polynomial convex optimization, it effectively solves the bottleneck problems of traditional methods, demonstrating significant technical advantages and application potential.

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

[0063] This invention provides a method for rapid dynamic collision detection of a locator's motion trajectory. This method uses a polynomial to describe the locator's motion trajectory. Through forward kinematic analysis and the construction of the tangent configuration space, the locator's configuration is represented as a polynomial function of time. Then, a series of hyperplanes dividing the locator and obstacles are searched using convex optimization methods to prove the safe interval between the locator and obstacles at each time step, thereby achieving rapid dynamic collision detection.

[0064] Figure 2 This is a flowchart illustrating a fast dynamic collision detection method for a locator's motion trajectory provided in an embodiment of the present invention. The fast dynamic collision detection method for a locator's motion trajectory provided in this embodiment 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 in this embodiment includes the following steps:

[0065] Step 1: Perform forward kinematic analysis on the positioner;

[0066] In this step, firstly, a forward kinematics analysis is performed on the locator based on its structure. The locator consists of multiple columns and joints, each column containing one joint. Each joint can be a rotary joint or a prismatic joint, and each joint has a single degree of freedom, which varies among the joints. The purpose of the forward kinematics analysis is to calculate the configuration of the locator at a specified time, i.e., the position and orientation of the end effectors of each column, based on the motion laws of each joint. Secondly, based on the type of joint (rotational or prismatic) and the degree of freedom parameters, forward kinematic equations for the position and orientation of the end effectors of each column are established, and these equations are expressed as polynomial functions of time for subsequent motion trajectory description and collision detection in the time domain.

[0067] It should be noted that dynamic collision detection refers to performing rapid collision detection at every 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 kinematic equations, 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 from the forward kinematics analysis. The tangent configuration space represents all possible poses that the locator can achieve under different joint parameters. Its core idea is to transform the locator's configuration parameters into time-dependent polynomial functions. By customizing the parameterization method, the locator's forward kinematics equations are re-parameterized, allowing the tangent configuration of the locator to be flexibly expressed in the time domain and used in subsequent collision detection.

[0070] Step 3: Dynamically detect collisions for the locator;

[0071] In this step, the locator's trajectory, represented by the constructed tangent configuration space and the forward kinematic equations, is used to perform rapid dynamic collision detection. Specifically, since both the locator and obstacles are convex bodies, at each time step, a series of hyperplanes are searched using a polynomial convex optimization method to separate the locator from surrounding obstacles. These hyperplanes mathematically prove the safe distance between the locator and obstacles, ensuring that the locator will not collide with obstacles even in complex motion paths and dynamic environments.

[0072] It should be noted that if a hyperplane is found, it indicates that there is no collision between the locator and the obstacle. The use of convex optimization in step 3 is one of the key technologies in this embodiment of the invention; convex optimization can efficiently search for and segment hyperplanes within convex bodies (such as the geometry 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 guarantees the reliability and accuracy of the detection results.

[0073] This invention provides a method for fast dynamic collision detection of a locator's motion trajectory. This method uses a polynomial to describe the locator's motion trajectory. Through forward kinematic analysis and the construction of the tangent configuration space, the locator's configuration is represented as a polynomial function of time. Then, a series of hyperplanes segmenting the locator and obstacles are searched using convex optimization methods to prove the safe interval between the locator and obstacles at each time step, thereby achieving fast dynamic collision detection. The technical solution provided by this invention has the following beneficial effects:

[0074] First, high efficiency and real-time performance: By using 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: convexity and convex optimization methods ensure the accuracy and safety of the algorithm in complex environments, effectively avoiding misjudgments and misoperations, and guaranteeing the safe operation of the locator.

[0076] Third, it has 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 this invention not only fills the gap in the existing technology for dynamic collision detection, but also provides important technical support for the safe operation and efficient operation of the locator, and has significant technological innovation and market application potential.

[0078] The following describes in detail the implementation method and specific steps of the fast dynamic collision detection method for the locator motion trajectory provided in the embodiments 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 positioner based on its structure. The positioner consists of three columns, each with a joint, which can be either a prismatic joint or a rotary joint. Specifically, the prismatic joint P... i The joint parameter for translating along its axis is a distance d. i Rotational joint P i Rotating along its axis, the corresponding joint parameter is the rotation angle θ. i Based on the type and motion parameters (such as length or rotation angle) of each joint, the forward kinematic equations for the position and orientation of the positioner's end effector are established. The establishment of these forward kinematic equations involves the following steps:

[0081] Step 11: Based on each joint type and joint parameters, obtain the joint parameter q. i A polynomial function of time;

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

[0083] In one possible implementation, if the i-th joint is a rotary joint, then the joint parameter q i Represented as θ i The positive kinematic 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, then the joint parameter q i Represented as d i The positive kinematic parameters are expressed as ξ ij,ω (q i )∈d i .

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

[0087]

[0088] For example, suppose 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 The forward kinematics equations of the locator are constructed and expressed as multilinear trigonometric polynomials. The ω-th component of the position of the end point A on the locator column relative to the F base coordinate system is expressed as:

[0091]

[0092] Where j represents the j-th column, i represents the i-th joint, and c jω ξ is a coefficient determined based on the column parameters, and is determined by the length of the j-th column of the locator. ij,ω (q i ) represents the positive kinematic parameters, which are determined based on the joints of the positioner.

[0093] Step 2: Construct the tangent configuration space of the locator;

[0094] In this step, based on the forward kinematics equations obtained from forward kinematics analysis, this 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 The diagram shown is a schematic of the tangent configuration space in an embodiment of the present invention. A custom parameter S is formed through reparameterization, and the motion state of the positioner is described by a polynomial of the custom parameter S. The joint parameters q1, q2, and q3 are considered as variables of a polynomial, and the custom parameter S can be a linear combination of the joint parameters:

[0095] S = a1q1 + a2q2 + a3q3;

[0096] Where a1, a2, and a3 are the reparameterized coefficients;

[0097] The joint parameter q is solved by substituting a linear expression for the custom parameter S into the forward kinematic equation. i and the joint parameter q i It is expressed as a function of the user-defined parameter S, thus representing the forward kinematic equations as a function of the user-defined parameter S, forming the tangent configuration space of the positioner.

[0098] In one implementation, step 2 may include the following steps:

[0099] Step 21, Customize parameters

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

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

[0102] Step 23: Reparameterize the forward kinematics equations of the positioner to construct the tangent configuration space as follows:

[0103]

[0104] in,

[0105] Step 3: Dynamically detect collisions for the locator;

[0106] In this embodiment of the invention, the locator and the obstacle are defined as convex bodies, so that the collision detection problem can be handled by 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 interior points.

[0107] In this step, a polynomial convex optimization method is used to search for a series of hyperplanes that divide the two convex bodies. By detecting these hyperplanes, it can be determined whether the locator will collide with surrounding obstacles during its movement. These hyperplanes mathematically prove the safe distance between the locator and obstacles, ensuring that the locator will not collide with obstacles even in complex motion paths and dynamic environments; for example... Figure 4 The diagram shown illustrates the search for a separating 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 separating hyperplanes between the locator and the obstacle corresponding to each time t of the locator's trajectory. According to the separating hyperplane theorem, a separating hyperplane is defined 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.

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

[0109] Step 31: Using the motion trajectory, the configuration of the locator in the tangent configuration space at each moment is represented as A(s). Based on the fact that the positive kinematic parameter S is a polynomial function of time t, A(s) can be represented as A(t).

[0110] 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 If (t) can be found, it means there is no collision; if it does not exist, it means 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 represent the points on the two convex bodies on opposite sides of the hyperplane.

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

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

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

[0116] This implementation example employs a polynomial convex optimization method to search for a series of hyperplanes between the segmentation locator and surrounding obstacles. This approach effectively handles optimization problems in high-dimensional spaces and has good computational efficiency. Using this method, a series of hyperplanes between the segmentation locator and obstacles can be quickly found, thus completing collision detection. For a spherical obstacle, a hyperplane H is found with normal vector n = (A, B, C) and intercept D, such that Ax + By + Cz + D = 0. This hyperplane must satisfy the following condition to ensure that the locator does not intersect with the obstacle at any time t: The hyperplane must satisfy the following condition:

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

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

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

[0120] The fast dynamic collision detection method for locator motion trajectories proposed in this invention effectively solves the problems of high computational complexity and poor real-time performance in traditional dynamic collision detection methods through innovative forward kinematics analysis, tangent configuration space construction, and the application of convex optimization methods. It possesses the following significant technical advantages and application value: efficient and real-time collision detection capability; accurate and reliable collision detection results; applicability to various complex environments and task scenarios; and technological innovation and market application potential. This invention fills a gap in the existing technology in the field of dynamic collision detection, possessing significant technological 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 this invention not only achieves a theoretical breakthrough in dynamic collision detection, but also possesses significant technical advantages and practical value in real-world applications. Further research and development will further promote the widespread application and promotion of this technology in the fields of industrial intelligence and positioner technology.

[0122] Based on the fast dynamic collision detection method for locator motion trajectory provided in the above embodiments of the present invention, the present invention also 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 fast dynamic collision detection method for locator motion trajectory as described above when executing the executable instructions stored in the memory.

[0125] While the embodiments disclosed in this invention are as described above, they are merely illustrative of the embodiments to facilitate understanding of the invention and are not intended to limit the invention. Any person skilled in the art to which this invention pertains may make any modifications and variations in the form and details of the implementation without departing from the spirit and scope disclosed herein; however, the scope of patent protection for this invention shall still be determined by the scope defined in the appended claims.

Claims

1. A method for rapid dynamic collision detection of a locator's motion trajectory, characterized in that, include: Step 1: Perform forward kinematics analysis on the positioner, including: performing forward kinematics analysis on the positioner based on its structure and establishing the forward kinematics equations of the positioner; Step 2: Based on the forward kinematics equations, construct the tangent configuration space of the positioner; wherein, the tangent configuration space describes all possible postures that the positioner may achieve under different joint parameters; Step 3: Using the constructed tangent configuration space and the locator's motion trajectory represented by the forward kinematic equation, the locator is dynamically detected for collisions. This includes: 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 divide the locator and the surrounding obstacles at each time step. If a hyperplane is found, it is detected that there is no collision between the locator and the obstacle. The structure of the locator in step 1 is as follows: the locator consists of multiple columns and joints, each column has one joint, the joint type is a prism joint or a rotary joint, each joint has a single degree of freedom of motion, which is the degree of freedom of translation along the axis or rotation around the axis, and the degree of freedom of motion of each joint is different. Step 1 includes: Based on the rotational or prismatic type and motion degree-of-freedom parameters of each joint, the forward kinematic equations of the position and orientation of the actuators at the ends of each column of the locator are established, and the forward kinematic equations are expressed as polynomial functions of time for subsequent motion trajectory description and dynamic collision detection in the time domain; wherein, the dynamic collision detection refers to: performing rapid collision detection at each moment during the movement of the locator, and its motion process is expressed as a polynomial function of time. The forward kinematic analysis of the positioner in step 1 includes the following steps: Step 11: Based on each joint type and joint parameters, obtain the joint parameters. q i A polynomial function of time; Step 12, based on the joint parameters of each joint q i Construct the forward kinematic equations of the locator, and express the forward kinematic equations as multilinear trigonometric polynomials; determine the position of the end point A on the locator column relative to the F base coordinate system. The components are represented as: ； in, The position of the end point A on the locator column relative to the coordinate system of base F is the first... Quantity, Indicates the first One pillar Indicates the first One joint, The coefficients are determined based on the column parameters, and are derived from the first... The length of each column is determined. These represent the positive kinematic parameters, which are determined based on the joints of the positioner. In step 11, Based on the type of each joint, determine the joint parameters and forward kinematic parameters of the forward kinematics; If the If the joint is a rotational joint, then the joint parameters... Represented as The positive kinematic parameters are expressed as: ； If the If the joint is a prismatic joint, then the joint parameters... Represented as d i The positive kinematic parameters are expressed as d i .

2. The method for rapid dynamic collision detection of the locator's motion trajectory according to claim 1, characterized in that, In step 2, based on the forward kinematic equations obtained from the forward kinematic analysis, the tangent configuration space of the positioner is constructed as follows: A custom parameter S is generated through reparameterization. The motion state of the positioner is described by a polynomial of the custom parameter S, and the joint parameters are then... q 1, q 2, q 3 is considered a variable of a polynomial, and the user-defined parameter S is a linear combination of joint parameters: ； in, a 1, a 2, a 3 represents the reparameterized coefficients; The joint parameters are solved by substituting a linear expression for a custom parameter S into the forward kinematic equations. q i and joint parameters q i It is expressed as a function of the user-defined parameter S, thus representing the forward kinematic equations as a function of the user-defined parameter S, forming the tangent configuration space of the positioner.

3. The method for rapid dynamic collision detection of the locator's motion trajectory according to claim 2, characterized in that, Step 2 includes: Step 21, Customize parameters ; Step 22, using custom parameters express ,but The forward kinematic parameters are ; Step 23: Reparameterize the forward kinematics equations of the positioner to construct the tangent configuration space as follows: ； in, .

4. The method for rapid dynamic collision detection of the locator's motion trajectory according to any one of claims 1 to 3, characterized in that, The series of hyperplanes in step 3 refers to the set of separating hyperplanes between the locator and the obstacle corresponding to each time t of the locator's trajectory. According to the separating hyperplane theorem, a hyperplane is considered to separate two objects if and only if there exists a hyperplane that separates the two objects. At this time, two closed convex bodies A and B will not intersect.

5. The method for rapid dynamic collision detection of the locator's motion trajectory according to claim 4, characterized in that, Step 3 employs a polynomial convex optimization method to search for a series of hyperplanes between the segmentation locator and surrounding obstacles, including: Step 31: Using the motion trajectory, the configuration of the locator in the tangent configuration space at each moment is represented as A(s). Based on the fact that the positive kinematic 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 that satisfy the following conditions. If a collision can be found, it means there was no collision; if it does not exist, it means the locator collided with an obstacle. ； in, and The parameters represent the hyperplane, where A and B represent the configuration space formed by the locator and the configuration space formed by the obstacle, respectively. Let represent points on two convex sets, and let represent points on two convex bodies on opposite sides of the hyperplane.

6. 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 fast dynamic collision detection method for the locator motion trajectory as described in any one of claims 1 to 5 when executing the executable instructions stored in the memory.

Images