Dynamic collision detection method and device for mobile mechanical arm based on time-varying volume model

Abstract

The invention discloses a mobile mechanical arm dynamic collision detection method and device based on a time-varying volume model. The mobile mechanical arm dynamic collision detection method based on the time-varying volume model comprises the following steps: constructing a digital twin joint body model which is completely consistent with the current physical state of a real mobile mechanical arm based on the pose of a mobile platform and the joint angle data of the mechanical arm, which are acquired in real time; based on the planned path, calculating the swept volume of the digital twin joint body model in the process of simulating and executing the planned path; and carrying out volume intersection operation on the swept volume and the three-dimensional environment map in the three-dimensional space, and judging whether a planned path is safe or not according to an operation result. The technical problems that a collision detection method in the prior art is too conservative, low in efficiency and incapable of covering risks under extreme poses and has tunnel effect security vulnerabilities are solved.

Description

Technical Field

[0001] This invention relates to the field of robotics and industrial automation technology, and in particular to a method and apparatus for dynamic collision detection of a mobile robotic arm based on a time-varying volume model. Background Technology

[0002] With the development of intelligent manufacturing and automated warehousing technologies, mobile robotic arm systems, which consist of traditional fixed-base robotic arms mounted on autonomous mobile platforms (such as AGVs), have been widely used in industrial settings. This combination greatly expands the working range and flexibility of the robotic arm. However, the complex multi-axis, multi-joint structure of the robotic arm itself can result in various poses that significantly extend its actual physical dimensions during movement. Combined with the continuous movement of the mobile platform, this makes the collision risk of the entire system far greater than before, necessitating an efficient and absolutely reliable collision detection method to eliminate safety hazards.

[0003] Currently, existing collision detection methods mainly fall into the following two categories: 1. Collision Detection Method Based on Abstract Safe Zone: This type of collision detection method defines the safety boundary by calculating a dynamically changing safe zone radius centered on the robot. To ensure safety in all worst-case scenarios, the radius must be large enough to encompass all possible poses. This method can be described as follows: Assume the moving robotic arm is in a world coordinate system... A certain reference point in is And define a dynamically changing danger radius. The safety conditions are simplified to any obstacle point in the environment. All of them must satisfy: This approach leads to an overly conservative solution. In many narrow or complex scenarios where passage is actually safe, the system may be mistakenly triggered by this excessively large safety zone, resulting in unnecessary emergency stops or task interruptions. This significantly limits the efficiency and space utilization of the mobile robotic arm in real-world environments. Furthermore, this method simplifies complex multi-joint bodies into a sphere model, ignoring the fact that in certain extreme poses of extension or twisting, the mobile robotic arm may actually occupy space exceeding the radius of the safety zone. This creates collision risks that cannot be detected in extreme poses, posing a serious safety hazard.

[0004] 2. Collision Detection Based on Discrete Time Points: This type of collision detection method essentially operates on a "motion-stop-detection" loop, selecting a series of discrete keyframes along the planned path and performing collision checks only under these static configurations. This method can be described as: taking a continuous motion trajectory (… Discretized into a series of time points Let the robot's configuration at this moment be... The environment is This method only checks whether the robot entity interferes with the environment at these discrete moments, i.e., it checks... Whether this is true or not. Its security vulnerability lies in the fact that even at all sampling points... It is safe in all areas, but there may be a point in time. , making While its detection at each static point is accurate, this method is inconsistent over time. When the mobile robotic arm moves at high speeds or has small gaps between obstacles, collisions can easily occur between two discrete detection snapshots, a phenomenon known as the tunneling effect. This deficiency prevents it from guaranteeing the continuous safety of the entire movement process.

[0005] In summary, existing collision detection methods suffer from technical problems such as being overly conservative, inefficient, unable to cover risks under extreme poses, and having tunnel effect safety vulnerabilities. They cannot simultaneously meet the requirements of high efficiency, high precision, and high reliability for mobile robotic arms operating in complex dynamic environments. Summary of the Invention

[0006] The technical problem to be solved by the present invention is to provide a dynamic collision detection method and device for a mobile robotic arm based on a time-varying volume model, which aims to solve the technical problems of existing collision detection methods, such as being too conservative, inefficient, unable to cover risks under extreme poses, and having tunnel effect safety vulnerabilities.

[0007] To solve the above-mentioned technical problems, the present invention adopts the following technical solution: The first aspect of this invention provides a dynamic collision detection method for a mobile robotic arm based on a time-varying volume model, comprising the following steps: constructing a digital twin joint model that is completely consistent with the current physical state of the real mobile robotic arm based on the pose of the mobile platform and the joint angle data of the robotic arm acquired in real time; calculating the sweep volume of the digital twin joint model in the process of simulating the execution of the planned path based on the planned path; performing a volume intersection operation between the sweep volume and the three-dimensional environment map in three-dimensional space, and determining whether the planned path is safe based on the operation result.

[0008] The first aspect of this invention provides a dynamic collision detection device for a mobile robotic arm based on a time-varying volume model, comprising: a model building module for constructing a digital twin joint model that is completely consistent with the current physical state of a real mobile robotic arm based on the pose of the mobile platform and the joint angle data of the robotic arm acquired in real time; a sweep volume calculation module for calculating the sweep volume of the digital twin joint model during the simulation of executing the planned path based on the planned path; and a path safety detection module for performing volume intersection calculation between the sweep volume and the three-dimensional environment map in three-dimensional space, and determining whether the planned path is safe based on the calculation result.

[0009] The beneficial technical effects of this invention are as follows: The above-mentioned dynamic collision detection method and device for a mobile robotic arm based on a time-varying volume model constructs a digital twin joint model with a real-time variable shape that is completely synchronized with the actual mobile robotic arm. Collision detection is performed based on the swept volume occupied by the digital twin joint model on the planned path. Whether the path is safe depends on whether the swept volume overlaps with the volume of the three-dimensional environment map. Specifically, by using a digital twin joint model that can accurately and in real-time represent the complete three-dimensional shape of the mobile robotic arm instead of the overly conservative simplified safety radius model for collision detection, the operating efficiency and spatial adaptability can be maximized while ensuring absolute safety, solving the technical problems of overly conservative and inefficient collision detection methods in the prior art. Secondly, by using the swept volume representing the space occupied by the mobile robotic arm throughout its continuous movement, the tunneling effect problem inherent in collision detection methods based on discrete time points is fundamentally solved. Finally, the swept volume of the digital twin joint model can accurately reflect the real space occupied by the mobile robotic arm in any complex and extreme pose, eliminating the defect that the simplified safety radius model cannot cover the collision risk under extreme poses, ensuring safety under all working conditions. Attached Figure Description

[0010] Figure 1 This is a flowchart illustrating a dynamic collision detection method for a mobile robotic arm based on a time-varying volume model according to one embodiment of the present invention. Figure 2 This is a flowchart illustrating a dynamic collision detection method for a mobile robotic arm based on a time-varying volume model, according to another embodiment of the present invention. Figure 3 This is a schematic diagram of the structure of a dynamic collision detection device for a mobile robotic arm based on a time-varying volume model according to one embodiment of the present invention. Figure 4 This is a schematic diagram of the structure of a dynamic collision detection device for a mobile robotic arm based on a time-varying volume model according to one embodiment of the present invention. Detailed Implementation

[0011] To enable those skilled in the art to more clearly understand the purpose, technical solution, and advantages of the present invention, the present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0012] This invention provides a dynamic collision detection method and device for a mobile robotic arm based on a time-varying volume model. It constructs a digital twin joint model that is completely synchronized with the real mobile robotic arm and whose shape can be changed in real time. Collision detection is performed based on the sweep volume occupied by the digital twin joint model on the planned path. Whether the path is safe depends on whether the sweep volume overlaps with the volume of the three-dimensional environment map.

[0013] like Figure 1 As shown, in one embodiment of the present invention, the dynamic collision detection method for a mobile robotic arm based on a time-varying volume model includes steps S10 to S30: S10. Based on the pose of the mobile platform and the joint angle data of the robotic arm acquired in real time, construct a digital twin joint model that is completely consistent with the current physical state of the real mobile robotic arm.

[0014] The digital twin joint model here refers to a model that represents the structure and state of a mobile robotic arm. Each component of the digital twin joint model is completely identical to each component of the real mobile robotic arm, and the physical state of the digital twin joint model at each moment is completely identical to the physical state of the real mobile robotic arm at the corresponding moment. This digital twin joint model can accurately and in real time represent the complete three-dimensional form of the mobile robotic arm.

[0015] Step S10 specifically includes the following steps: S11. Model each physical component of the mobile platform and robotic arm as a corresponding three-dimensional geometric unit in advance to form a geometric unit library.

[0016] A mobile robotic arm comprises a mobile platform and a robotic arm mounted on the platform. By pre-modeling each physical component of the mobile platform and robotic arm (including the chassis, links, joints, end effectors, etc.) with precise 3D geometry, corresponding 3D geometric units are obtained, forming an independent library of geometric units. Each physical component of the mobile platform and robotic arm corresponds to one 3D geometric unit. Each 3D geometric unit is typically represented as a triangular mesh model or a composite of basic geometric primitives (such as spheres, capsules, and convex hulls) to facilitate efficient subsequent geometric calculations.

[0017] S12. Real-time acquisition of the pose of the mobile platform and the angle data of all joints of the robotic arm.

[0018] Specifically, the precise pose (position and attitude) of the mobile platform and the real-time angle data of all joints of the robotic arm are acquired in real time through sensors.

[0019] S13. Based on the pose of the mobile platform and the joint angle data of the robotic arm acquired in real time, the various three-dimensional geometric units in the geometric unit library are assembled to construct a digital twin joint model that is completely consistent with the current physical state of the real mobile robotic arm.

[0020] Step S13 is mathematically achieved through forward kinematics, and the specific process is as follows: S131. Let the world coordinate system be... The pose of the mobile platform at time t is obtained through a homogeneous transformation matrix. To describe, the homogeneous transformation matrix contains the rotation matrix. Translation vector : ; in, Indicates mobile platform.

[0021] S132, Based on the joint angle vector of the robotic arm at time t The pose of the robotic arm's link relative to the moving platform at time t can be calculated using the standard DH parameter method or the screw theory. .

[0022] Specifically, let the robotic arm have The joint angle vector of the robotic arm at time t. It consists of the angle data of each joint at time t. , Indicates the first i The angle of each joint at time t i =1,2,3,…,N. The pose of any link of the robotic arm relative to the moving platform at time t. yes The function can be calculated using the standard DH parameter method or spiral theory.

[0023] S133. Calculate the real-time pose of the links of the robotic arm in the world coordinate system. : ; in, Indicates the first i Each link corresponds to a three-dimensional geometric element.

[0024] S134, based on the calculation Adjust the instantaneous position and attitude of each link of the robotic arm to construct the digital twin joint model.

[0025] Specifically, the three-dimensional geometric elements representing each link in the geometric element library are assembled, and the assembly process is based on the formula calculated in step S133. The instantaneous position and orientation of the three-dimensional geometric units representing each link are precisely adjusted to form a complete digital twin joint model.

[0026] The digital twin joint model not only reflects the ideal kinematic relationship, but also can calibrate and correct kinematic parameters (such as link length, joint offset, etc.) through offline calibration based on closed-loop data (such as torque sensor, encoder deviation, etc.) to compensate for physical wear or manufacturing tolerances and ensure its high fidelity.

[0027] S20. Based on the planned path, calculate the sweep volume of the digital twin joint model during the simulation of executing the planned path.

[0028] The swept volume is the volume of space occupied by an object moving along an arbitrary trajectory in space. Step S20 specifically includes the following steps: S21. Receive the planned path of the mobile robotic arm from the upper-level motion planning module.

[0029] The planned path is the target motion trajectory of the mobile robotic arm over a period of time in the future. This trajectory defines the process of the continuous change of the state variables of each joint of the mobile platform and the robotic arm over time.

[0030] S22. Using the digital twin joint model, accurately simulate and execute the planned path.

[0031] S23. Calculate and aggregate all three-dimensional spatial point sets traversed by the digital twin joint model during the simulation of the planned path, to obtain the sweep volume occupied by the digital twin joint model in space and time during the entire motion.

[0032] Step S23 further includes: S231. Calculate the digital twin joint model in... The set of three-dimensional points occupied by a time point : ; in, This indicates the real-time pose of the link in the world coordinate system. Indicates the first i Each link corresponds to a three-dimensional geometric element.

[0033] S232. Calculate the digital twin joint model within the planning time period. The sweep volume inside : .

[0034] The planning time period This refers to the time period during which the digital twin joint model executes the planned path. The sweep volume can be determined using the formula above. For the digital twin joint model during the planned time period The set of three-dimensional spatial points occupied at each moment within the time frame The union of .

[0035] In practice, this continuous union operation cannot be calculated directly. A discrete approximation method is typically used: the planning time period is... Divide into sufficiently small time steps, with a time step size of For each time step Calculate the set of three-dimensional points occupied by the model at two time points. and ,Will and Perform a union operation to obtain the time step. The local sweep volumes are then merged to form a representation of the real data. A high-precision approximation.

[0036] S30. Perform volume intersection calculation between the swept volume and the three-dimensional environment map in three-dimensional space, and determine whether the planned path is safe based on the calculation result.

[0037] Step S30 further includes: S31. Obtain a 3D environment raster map corresponding to the surrounding environment of the planned path.

[0038] The three-dimensional environment raster map (such as OctoMap) is generated by systems such as SLAM (Simultaneous Localization and Mapping) and includes information such as obstacles in the environment surrounding the planned path.

[0039] S32. In the shared three-dimensional spatial coordinate system, perform volume intersection operation on the swept volume and the three-dimensional environment grid map, and determine whether the planned path is safe based on the operation result.

[0040] Let the space occupied by the three-dimensional environment raster map be... In a 3D environment raster map, It is represented as the set of all voxels marked as occupied. The core task of collision detection is to determine the sweep volume. and The intersection of the two paths is determined to be empty. If the intersection is empty, the planned path is considered absolutely safe. If the intersection is not empty, the planned path is considered to have a collision risk.

[0041] Specifically, the intersection operation This can be achieved using efficient algorithms. For example, the swept volume... Voxelization, and then with Perform a step-by-step comparison to determine if the intersection of the two is empty. Alternatively, measure the swept volume. It is represented as the union of a series of convex polyhedra, and fast interference lookup is performed using the Separating Axis Theorem (SAT) or the GJK (Gilbert-Johnson-Keerthi) algorithm.

[0042] The dynamic collision detection method for a mobile robotic arm based on a time-varying volume model provided in this invention has the following significant advantages: 1. Extremely high accuracy and space utilization: By accurately modeling and sweeping the complete three-dimensional shape of the mobile robotic arm, the overly conservative simplified model of the safety radius is completely abandoned. This invention can accurately identify the extremely narrow spaces through which the mobile robotic arm can pass, significantly reducing unnecessary downtime caused by false alarms. It greatly improves the operating efficiency and space utilization of the mobile robotic arm in complex and congested environments, solving the technical problems of overly conservative and inefficient collision detection methods in existing technologies. 2. Absolute Continuity Safety Guarantee: Collision detection is performed by generating a swept volume representing the space occupied by the entire continuous movement of the mobile robotic arm, fundamentally solving the tunneling effect problem inherent in collision detection methods based on discrete time points. No matter how fast the mobile robotic arm moves, it guarantees that no undetected penetration or collision will occur, providing a true guarantee of continuous trajectory safety. 3. Comprehensive risk coverage: The swept volume of the digital twin joint model can accurately reflect the robot's real space occupation in any complex and extreme pose, eliminating the shortcomings of the simplified safety radius model that cannot cover the collision risk in extreme poses, and ensuring safety under all working conditions. 4. High-fidelity real-world grounding decision-making: Collision detection calculations are performed based on a digital twin joint model that is synchronized in real time with the real mobile robotic arm. The model on which the safety decision is based reflects the real physical state of the mobile robotic arm (rather than an idealized CAD model), making the safety assessment more reliable and providing a higher degree of confidence for deploying mobile robotic arms in demanding real-world environments.

[0043] like Figure 2As shown, in another embodiment of the present invention, the dynamic collision detection method for a mobile robotic arm based on a time-varying volume model includes steps S10 to S40: S10. Based on the pose of the mobile platform and the joint angle data of the robotic arm acquired in real time, construct a digital twin joint model that is completely consistent with the current physical state of the real mobile robotic arm.

[0044] The digital twin joint model here refers to a model that represents the structure and state of a mobile robotic arm. Each component of the digital twin joint model is completely identical to each component of the real mobile robotic arm, and the physical state of the digital twin joint model at each moment is completely identical to the physical state of the real mobile robotic arm at the corresponding moment. This digital twin joint model can accurately and in real time represent the complete three-dimensional form of the mobile robotic arm.

[0045] Step S10 specifically includes the following steps: S11. Model each physical component of the mobile platform and robotic arm as a corresponding three-dimensional geometric unit in advance to form a geometric unit library.

[0046] A mobile robotic arm comprises a mobile platform and a robotic arm mounted on the platform. By pre-modeling each physical component of the mobile platform and robotic arm (including the chassis, links, joints, end effectors, etc.) with precise 3D geometry, corresponding 3D geometric units are obtained, forming an independent library of geometric units. Each physical component of the mobile platform and robotic arm corresponds to one 3D geometric unit. Each 3D geometric unit is typically represented as a triangular mesh model or a composite of basic geometric primitives (such as spheres, capsules, and convex hulls) to facilitate efficient subsequent geometric calculations.

[0047] S12. Real-time acquisition of the pose of the mobile platform and the angle data of all joints of the robotic arm.

[0048] Specifically, the precise pose (position and attitude) of the mobile platform and the real-time angle data of all joints of the robotic arm are acquired in real time through sensors.

[0049] S13. Based on the pose of the mobile platform and the joint angle data of the robotic arm acquired in real time, the various three-dimensional geometric units in the geometric unit library are assembled to construct a digital twin joint model that is completely consistent with the current physical state of the real mobile robotic arm.

[0050] Step S13 is mathematically achieved through forward kinematics, and the specific process is as follows: S131. Let the world coordinate system be... The pose of the mobile platform at time t is obtained through a homogeneous transformation matrix. To describe, the homogeneous transformation matrix contains the rotation matrix. Translation vector : ; in, Indicates mobile platform.

[0051] S132, Based on the joint angle vector of the robotic arm at time t The pose of the robotic arm's link relative to the moving platform at time t can be calculated using the standard DH parameter method or the screw theory. .

[0052] Specifically, let the robotic arm have The joint angle vector of the robotic arm at time t. It consists of the angle data of each joint at time t. , Indicates the first i The angle of each joint at time t i =1,2,3,…,N. The pose of any link of the robotic arm relative to the moving platform at time t. yes The function can be calculated using the standard DH parameter method or spiral theory.

[0053] S133. Calculate the real-time pose of the links of the robotic arm in the world coordinate system. : ;

[0054] in, Indicates the first i Each link corresponds to a three-dimensional geometric element.

[0055] S134, based on the calculation Adjust the instantaneous position and attitude of each link of the robotic arm to construct the digital twin joint model.

[0056] Specifically, the three-dimensional geometric elements representing each link in the geometric element library are assembled, and the assembly process is based on the formula calculated in step S133. The instantaneous position and orientation of the three-dimensional geometric units representing each link are precisely adjusted to form a complete digital twin joint model.

[0057] The digital twin joint model not only reflects the ideal kinematic relationship, but also can calibrate and correct kinematic parameters (such as link length, joint offset, etc.) through offline calibration based on closed-loop data (such as torque sensor, encoder deviation, etc.) to compensate for physical wear or manufacturing tolerances and ensure its high fidelity.

[0058] S20. Based on the planned path, calculate the sweep volume of the digital twin joint model during the simulation of executing the planned path.

[0059] The swept volume is the volume of space occupied by an object moving along an arbitrary trajectory in space. Step S20 specifically includes the following steps: S21. Receive the planned path of the mobile robotic arm from the upper-level motion planning module.

[0060] The planned path is the target motion trajectory of the mobile robotic arm over a period of time in the future. This trajectory defines the process of the continuous change of the state variables of each joint of the mobile platform and the robotic arm over time.

[0061] S22. Using the digital twin joint model, accurately simulate and execute the planned path.

[0062] S23. Calculate and aggregate all three-dimensional spatial point sets traversed by the digital twin joint model during the simulation of the planned path, to obtain the sweep volume occupied by the digital twin joint model in space and time during the entire motion.

[0063] Step S23 further includes: S231. Calculate the digital twin joint model in... The set of three-dimensional points occupied by a time point : ; in, This indicates the real-time pose of the link in the world coordinate system. Indicates the first i Each link corresponds to a three-dimensional geometric element.

[0064] S232. Calculate the digital twin joint model within the planning time period. The sweep volume inside : .

[0065] The planning time period This refers to the time period during which the digital twin joint model executes the planned path. The sweep volume can be determined using the formula above. For the digital twin joint model during the planned time period The set of three-dimensional spatial points occupied at each moment within the time frame The union of .

[0066] In practice, this continuous union operation cannot be calculated directly. A discrete approximation method is typically used: the planning time period is... Divide into sufficiently small time steps, with a time step size of For each time step Calculate the set of three-dimensional points occupied by the model at two time points. and ,Will and Perform a union operation to obtain the time step. The local sweep volumes are then merged to form a representation of the real data. A high-precision approximation.

[0067] S30. Perform volume intersection calculation between the swept volume and the three-dimensional environment map in three-dimensional space, and determine whether the planned path is safe based on the calculation result.

[0068] Step S30 further includes: S31. Obtain a 3D environment raster map corresponding to the surrounding environment of the planned path.

[0069] The three-dimensional environment raster map (such as OctoMap) is generated by systems such as SLAM (Simultaneous Localization and Mapping) and includes information such as obstacles in the environment surrounding the planned path.

[0070] S32. In the shared three-dimensional spatial coordinate system, perform volume intersection operation on the swept volume and the three-dimensional environment grid map, and determine whether the planned path is safe based on the operation result.

[0071] Let the space occupied by the three-dimensional environment raster map be... In a 3D environment raster map, It is represented as the set of all voxels marked as occupied. The core task of collision detection is to determine the sweep volume. and The intersection of the two paths is determined to be empty. If the intersection is empty, the planned path is considered absolutely safe. If the intersection is not empty, the planned path is considered to have a collision risk.

[0072] Specifically, the intersection operation This can be achieved using efficient algorithms. For example, the swept volume... Voxelization, and then with Perform a step-by-step comparison to determine if the intersection of the two is empty. Alternatively, measure the swept volume. It is represented as the union of a series of convex polyhedra, and fast interference lookup is performed using the Separating Axis Theorem (SAT) or the GJK (Gilbert-Johnson-Keerthi) algorithm.

[0073] S40. Based on the path security determination result, execute the corresponding security decision.

[0074] Step S30 can obtain the path safety determination result, which specifically indicates that the planned path is absolutely safe or that the planned path has a collision risk.

[0075] Step S40 executes the corresponding security decision based on the path security determination result. The specific process is as follows: S41. If the planned path is absolutely safe, then control the mobile robotic arm to execute the planned path; S42. If there is a collision risk in the planned path, the mobile robotic arm is controlled to refuse to execute the planned path, and the first collision point, the first collision time, and the specific parts and environmental locations of the mobile robotic arm that collided are located by the dichotomy method.

[0076] a) Determine the time of the first collision: When a collision is detected (i.e., the intersection is not empty), it can be addressed by planning the time period. A binary search is used to efficiently locate the first collision time. .

[0077] b) Locating the collision location and components: Once the time of the first collision is determined This can be achieved by analyzing the intersection. To obtain accurate collision information. This intersection... The points in the map represent the points where the first collision occurred in the world coordinate system (i.e., the first collision point). Then, by performing a reverse lookup, the specific components on the mobile robotic arm corresponding to these points can be identified, as well as the specific locations of these points in the 3D environment grid map, thereby accurately locating the specific components and environmental locations where the mobile robotic arm collided.

[0078] The dynamic collision detection method for a mobile robotic arm based on a time-varying volume model provided in this invention has the following significant advantages: 1. Extremely high accuracy and space utilization: By accurately modeling and sweeping the complete three-dimensional shape of the mobile robotic arm, the overly conservative simplified model of the safety radius is completely abandoned. This invention can accurately identify the extremely narrow spaces through which the mobile robotic arm can pass, significantly reducing unnecessary downtime caused by false alarms. It greatly improves the operating efficiency and space utilization of the mobile robotic arm in complex and congested environments, solving the technical problems of overly conservative and inefficient collision detection methods in existing technologies. 2. Absolute Continuity Safety Guarantee: Collision detection is performed by generating a swept volume representing the space occupied by the entire continuous movement of the mobile robotic arm, fundamentally solving the tunneling effect problem inherent in collision detection methods based on discrete time points. No matter how fast the mobile robotic arm moves, it guarantees that no undetected penetration or collision will occur, providing a true guarantee of continuous trajectory safety. 3. Comprehensive risk coverage: The swept volume of the digital twin joint model can accurately reflect the robot's real space occupation in any complex and extreme pose, eliminating the shortcomings of the simplified safety radius model that cannot cover the collision risk in extreme poses, and ensuring safety under all working conditions. 4. High-fidelity real-world grounding decision-making: Collision detection calculations are performed based on a digital twin joint model that is synchronized in real time with the real mobile robotic arm. The model on which the safety decision is based reflects the real physical state of the mobile robotic arm (rather than an idealized CAD model), making the safety assessment more reliable and providing a higher degree of confidence for deploying mobile robotic arms in demanding real-world environments.

[0079] Figure 3 This diagram illustrates the structure of a dynamic collision detection device for a mobile robotic arm based on a time-varying volume model, according to one embodiment of the present invention. The various modules of this dynamic collision detection device for a mobile robotic arm based on a time-varying volume model are... Figure 1 The steps of the dynamic collision detection method for a mobile robotic arm based on a time-varying volume model in the illustrated embodiment correspond one-to-one. For example... Figure 3 As shown, the dynamic collision detection device for the mobile robotic arm based on the time-varying volume model includes a model building module 10, a sweep volume calculation module 20, and a path safety detection module 30. Detailed descriptions of each functional module are as follows: Model building module 10 is used to build a digital twin joint model that is completely consistent with the current physical state of the real mobile robotic arm based on the pose of the mobile platform and the joint angle data of the robotic arm acquired in real time. The sweep volume calculation module 20 is used to calculate the sweep volume of the digital twin joint model during the simulation of executing the planned path, based on the planned path. The path safety detection module 30 is used to perform volume intersection calculation between the swept volume and the three-dimensional environment map in three-dimensional space, and determine whether the planned path is safe based on the calculation result.

[0080] For specific limitations regarding the dynamic collision detection device for the mobile robotic arm based on the time-varying volume model in this embodiment, please refer to [link / reference needed]. Figure 1The limitations of the dynamic collision detection method for the mobile robotic arm based on the time-varying volume model in the illustrated embodiment will not be repeated here. Each module in the aforementioned dynamic collision detection device for the mobile robotic arm based on the time-varying volume model can be implemented entirely or partially through software, hardware, or a combination thereof. These modules can be embedded in or independent of the processor in a computer device in hardware form, or stored in the memory of a computer device in software form, so that the processor can call and execute the operations corresponding to each module.

[0081] The dynamic collision detection device for a mobile robotic arm based on a time-varying volume model provided in this invention has the following significant advantages: 1. Extremely high accuracy and space utilization: By accurately modeling and sweeping the complete three-dimensional shape of the mobile robotic arm, the overly conservative simplified model of the safety radius is completely abandoned. This invention can accurately identify the extremely narrow spaces through which the mobile robotic arm can pass, significantly reducing unnecessary downtime caused by false alarms. It greatly improves the operating efficiency and space utilization of the mobile robotic arm in complex and congested environments, solving the technical problems of overly conservative and inefficient collision detection methods in existing technologies. 2. Absolute Continuity Safety Guarantee: Collision detection is performed by generating a swept volume representing the space occupied by the entire continuous movement of the mobile robotic arm, fundamentally solving the tunneling effect problem inherent in collision detection methods based on discrete time points. No matter how fast the mobile robotic arm moves, it guarantees that no undetected penetration or collision will occur, providing a true guarantee of continuous trajectory safety. 3. Comprehensive risk coverage: The swept volume of the digital twin joint model can accurately reflect the robot's real space occupation in any complex and extreme pose, eliminating the shortcomings of the simplified safety radius model that cannot cover the collision risk in extreme poses, and ensuring safety under all working conditions. 4. High-fidelity real-world grounding decision-making: Collision detection calculations are performed based on a digital twin joint model that is synchronized in real time with the real mobile robotic arm. The model on which the safety decision is based reflects the real physical state of the mobile robotic arm (rather than an idealized CAD model), making the safety assessment more reliable and providing a higher degree of confidence for deploying mobile robotic arms in demanding real-world environments.

[0082] Figure 4 This diagram illustrates a structural schematic of a dynamic collision detection device for a mobile robotic arm based on a time-varying volume model, according to another embodiment of the present invention. The various modules of this dynamic collision detection device for a mobile robotic arm based on a time-varying volume model are... Figure 2 The steps of the dynamic collision detection method for a mobile robotic arm based on a time-varying volume model in the illustrated embodiment correspond one-to-one. For example... Figure 4As shown, the dynamic collision detection device for the mobile robotic arm based on the time-varying volume model includes a model building module 10, a sweep volume calculation module 20, a path safety detection module 30, and a safety decision module 40. Detailed descriptions of each functional module are as follows: Model building module 10 is used to build a digital twin joint model that is completely consistent with the current physical state of the real mobile robotic arm based on the pose of the mobile platform and the joint angle data of the robotic arm acquired in real time. The sweep volume calculation module 20 is used to calculate the sweep volume of the digital twin joint model during the simulation of executing the planned path, based on the planned path. The path safety detection module 30 is used to perform volume intersection calculation between the swept volume and the three-dimensional environment map in three-dimensional space, and determine whether the planned path is safe based on the calculation result. The security decision module 40 is used to execute corresponding security decisions based on the path security determination results.

[0083] For specific limitations regarding the dynamic collision detection device for the mobile robotic arm based on the time-varying volume model in this embodiment, please refer to [link / reference needed]. Figure 2 The limitations of the dynamic collision detection method for the mobile robotic arm based on the time-varying volume model in the illustrated embodiment will not be repeated here. Each module in the aforementioned dynamic collision detection device for the mobile robotic arm based on the time-varying volume model can be implemented entirely or partially through software, hardware, or a combination thereof. These modules can be embedded in or independent of the processor in a computer device in hardware form, or stored in the memory of a computer device in software form, so that the processor can call and execute the operations corresponding to each module.

[0084] The dynamic collision detection device for a mobile robotic arm based on a time-varying volume model provided in this invention has the following significant advantages: 1. Extremely high accuracy and space utilization: By accurately modeling and sweeping the complete three-dimensional shape of the mobile robotic arm, the overly conservative simplified model of the safety radius is completely abandoned. This invention can accurately identify the extremely narrow spaces through which the mobile robotic arm can pass, significantly reducing unnecessary downtime caused by false alarms. It greatly improves the operating efficiency and space utilization of the mobile robotic arm in complex and congested environments, solving the technical problems of overly conservative and inefficient collision detection methods in existing technologies. 2. Absolute Continuity Safety Guarantee: Collision detection is performed by generating a swept volume representing the space occupied by the entire continuous movement of the mobile robotic arm, fundamentally solving the tunneling effect problem inherent in collision detection methods based on discrete time points. No matter how fast the mobile robotic arm moves, it guarantees that no undetected penetration or collision will occur, providing a true guarantee of continuous trajectory safety. 3. Comprehensive risk coverage: The swept volume of the digital twin joint model can accurately reflect the robot's real space occupation in any complex and extreme pose, eliminating the shortcomings of the simplified safety radius model that cannot cover the collision risk in extreme poses, and ensuring safety under all working conditions. 4. High-fidelity real-world grounding decision-making: Collision detection calculations are performed based on a digital twin joint model that is synchronized in real time with the real mobile robotic arm. The model on which the safety decision is based reflects the real physical state of the mobile robotic arm (rather than an idealized CAD model), making the safety assessment more reliable and providing a higher degree of confidence for deploying mobile robotic arms in demanding real-world environments.

[0085] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention in any way. Those skilled in the art can make various equivalent changes and improvements based on the above embodiments, and all equivalent variations or modifications made within the scope of the claims should fall within the protection scope of the present invention.

Claims

1. A dynamic collision detection method for a mobile robotic arm based on a time-varying volume model, characterized in that, Includes the following steps: S10. Based on the pose of the mobile platform and the joint angle data of the robotic arm acquired in real time, construct a digital twin joint model that is completely consistent with the current physical state of the real mobile robotic arm. S20. Based on the planned path, calculate the sweep volume of the digital twin joint model during the simulation of executing the planned path; S30. Perform volume intersection calculation between the swept volume and the three-dimensional environment map in three-dimensional space, and determine whether the planned path is safe based on the calculation result.

2. The dynamic collision detection method for a mobile robotic arm based on a time-varying volume model as described in claim 1, characterized in that, Step S10 further includes: S11. Pre-model each physical component of the mobile platform and robotic arm as a corresponding three-dimensional geometric unit to form a geometric unit library; S12. Real-time acquisition of the pose of the mobile platform and the angle data of all joints of the robotic arm; S13. Based on the pose of the mobile platform and the joint angle data of the robotic arm acquired in real time, the various three-dimensional geometric units in the geometric unit library are assembled to construct a digital twin joint model that is completely consistent with the current physical state of the real mobile robotic arm.

3. The dynamic collision detection method for a mobile robotic arm based on a time-varying volume model as described in claim 2, characterized in that, Step S13 further includes: S131. Let the world coordinate system be... Using a homogeneous transformation matrix Describe the pose of the mobile platform at time t: ， in, Indicates mobile platform, , Represents a rotation matrix and , Describes the translation vector and ; S132, Based on the joint angle vector of the robotic arm at time t The pose of the robotic arm's link relative to the moving platform at time t can be calculated using the standard DH parameter method or the screw theory. ,in, , Indicates the first i The angle of each joint at time t i =1,2,3,…,N; N represents the number of joints in the robotic arm; S133. Calculate the real-time pose of the links of the robotic arm in the world coordinate system. : ， in, This represents the three-dimensional geometric element corresponding to the i-th link; S134, based on the calculation Adjust the instantaneous position and attitude of each link of the robotic arm to construct the digital twin joint model.

4. The dynamic collision detection method for a mobile robotic arm based on a time-varying volume model as described in claim 3, characterized in that, Step S10 further includes: correcting the kinematic parameters of the digital twin joint model through offline calibration.

5. The dynamic collision detection method for a mobile robotic arm based on a time-varying volume model as described in claim 1, characterized in that, Step S20 further includes: S21. Receive the planned path of the mobile robotic arm from the upper-level motion planning module; S22. Using the digital twin joint model, accurately simulate and execute the planned path; S23. Calculate and aggregate all three-dimensional spatial point sets traversed by the digital twin joint model during the simulation of the planned path, to obtain the sweep volume occupied by the digital twin joint model in space and time during the entire motion.

6. The dynamic collision detection method for a mobile robotic arm based on a time-varying volume model as described in claim 5, characterized in that, Step S23 further includes: S231. Calculate the digital twin joint model in... The set of three-dimensional points occupied by a time point : ， in, This indicates the real-time pose of the link in the world coordinate system. This represents the three-dimensional geometric element corresponding to the i-th link; S232. Calculate the digital twin joint model within the planning time period. The sweep volume inside : 。 7. The dynamic collision detection method for a mobile robotic arm based on a time-varying volume model as described in claim 1, characterized in that, Step S30 further includes: S31. Obtain a 3D environment raster map corresponding to the surrounding environment of the planned path; S32. In the shared three-dimensional spatial coordinate system, perform volume intersection operation on the swept volume and the three-dimensional environment grid map, and determine whether the planned path is safe based on the operation result.

8. The dynamic collision detection method for a mobile robotic arm based on a time-varying volume model as described in claim 1, characterized in that, Includes the following steps: S40. Based on the path security determination result, execute the corresponding security decision.

9. The dynamic collision detection method for a mobile robotic arm based on a time-varying volume model as described in claim 8, characterized in that, Step S40 further includes: S41. If the planned path is absolutely safe, then control the mobile robotic arm to execute the planned path; S42. If there is a collision risk in the planned path, the mobile robotic arm is controlled to refuse to execute the planned path, and the first collision point, the first collision time, and the specific parts and environmental locations of the mobile robotic arm that collided are located by the dichotomy method.

10. A dynamic collision detection device for a mobile robotic arm based on a time-varying volume model, characterized in that, include: The model building module is used to build a digital twin joint model that is completely consistent with the current physical state of the real mobile robotic arm based on the pose of the mobile platform and the joint angle data of the robotic arm acquired in real time. The sweep volume calculation module is used to calculate the sweep volume of the digital twin joint model during the simulation of executing the planned path, based on the planned path. The path safety detection module is used to perform volume intersection calculations between the swept volume and the 3D environment map in 3D space, and determine whether the planned path is safe based on the calculation results.

Images