Method and device for dynamic collision detection of mobile manipulator based on time-varying volume model

Abstract

The application discloses a kind of mobile manipulator dynamic collision detection method and device based on time-varying volume model.The mobile manipulator dynamic collision detection method based on time-varying volume model includes the following steps: based on the pose of mobile platform and the joint angle data of manipulator obtained in real time, construct the digital twin joint body model that is completely consistent with the current physical state of real mobile manipulator;Based on the planned path, the swept volume of the digital twin joint body model in the process of simulating the execution of the planned path is calculated;Volume intersection operation is carried out on the swept volume and the three-dimensional environment map in three-dimensional space, and whether the planned path is safe is determined according to the operation result.The application solves the technical problems of the collision detection method in the prior art, such as being too conservative, low efficiency, unable to cover extreme pose risks and having tunnel effect security vulnerabilities.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of robot and industrial automation, and in particular to a mobile manipulator dynamic collision detection method and device based on a time-varying volume model. BACKGROUND

[0002] With the development of intelligent manufacturing and automated warehousing technology, the traditional fixed base manipulator is installed on an autonomous mobile platform (such as an AGV) to form a mobile manipulator system, which has been widely used in industrial scenarios. This combination greatly expands the working range and flexibility of the manipulator. However, the complex structure of the manipulator itself with multiple axes and joints can form multiple poses that greatly extend its actual physical size during movement, and in combination with the continuous movement of the mobile platform, the collision risk faced by the entire system is much higher than before, and a highly efficient and absolutely reliable collision detection method is urgently needed to eliminate safety hazards.

[0003] Currently, the collision detection methods in the prior art mainly include the following two types:

[0004] 1. Collision detection method based on abstract safety zone: the core of this type of collision detection method is to define a safety boundary by calculating a dynamic safety zone radius centered on the robot. In order to ensure safety in all worst-case scenarios, the radius value set by this method must be large enough to envelope all possible poses. This method can be described as follows: let the reference point of the mobile manipulator in the world coordinate system be , and define a dynamic dangerous radius . The safety condition is simplified as follows: for any obstacle point in the environment, it must satisfy: This results in a very conservative solution, as the system will be falsely triggered due to this excessively large safety zone in many real-world scenarios that can be safely passed through, resulting in unnecessary emergency stops or task interruptions, greatly limiting the working efficiency and space utilization of the mobile manipulator in real-world environments. In addition, this method simplifies the complex multi-joint body to a spherical model, ignoring the fact that the actual space occupied by the mobile manipulator in some extreme poses may exceed the safety zone radius, thus creating a collision risk that cannot be detected in extreme poses, posing a serious safety hazard.

[0005] 2. Collision detection method based on discrete time points: the working mode of this type of collision detection method is essentially a "motion-stop-detection" cycle, that is, a series of discrete key frames are selected on the planned path, and collision checking is only performed at these static configurations. This method can be described as follows: a continuous motion trajectory is discretized into a series of time points Let the configuration of the robot at this moment be , and the environment be . This method only checks whether the robot entity interferes with the environment at these discrete moments, that is, whether holds. Its security vulnerability lies in that even if it is safe at all sampling points , there may be a time point such that . Although its detection at each static point is accurate, this method is not continuous in the time dimension. When the moving arm moves at a high speed or the gap with the obstacle is small, it is entirely possible that a collision occurs between two discrete detection snapshots, that is, the so-called tunnel effect. This defect makes it unable to guarantee the continuity of the entire motion process.

[0006] In summary, the collision detection methods in the prior art have the technical problems of being overly conservative, low efficiency, unable to cover risks in extreme poses, and having a tunnel effect security vulnerability, and cannot simultaneously meet the requirements of high efficiency, high precision, and high reliability when the moving arm is working in a complex dynamic environment. SUMMARY

[0007] The technical problem to be solved by the present application is to provide a moving arm dynamic collision detection method and device based on a time-varying volume model, aiming at solving the technical problems of being overly conservative, low efficiency, unable to cover risks in extreme poses, and having a tunnel effect security vulnerability in the collision detection methods in the prior art.

[0008] To solve the above technical problems, the present application adopts the technical solutions as follows:

[0009] The present application provides a moving arm dynamic collision detection method based on a time-varying volume model in a first aspect, comprising the following steps: based on the real-time acquired pose of the moving platform and the joint angle data of the arm, a digital twin joint body model completely consistent with the current physical state of the real moving arm is constructed; based on the planned path, the swept volume of the digital twin joint body model in the process of simulating the execution of the planned path is calculated; volume intersection operation is performed on the swept volume and the three-dimensional environment map in the three-dimensional space, and whether the planned path is safe is determined according to the operation result.

[0010] The application provides a mobile manipulator dynamic collision detection device based on a time-varying volume model.

[0011] The application has the beneficial technical effects that the mobile manipulator dynamic collision detection method and device based on the time-varying volume model can construct a digital twin joint body model that is completely synchronized with a real mobile manipulator and has a real-time variable shape, and perform collision detection based on a swept volume of the digital twin joint body model on a planned path, and whether the path is safe depends on whether the swept volume and a three-dimensional environment map have an overlap. The digital twin joint body model can accurately and timely represent a complete three-dimensional shape of the mobile manipulator, and is used to replace a conservative safety radius simplified model to perform collision detection, and can maximize work efficiency and spatial adaptability under the premise of absolute safety, and solves the technical problems of the existing collision detection methods, such as being too conservative and low in efficiency. In addition, the swept volume representing a space occupied by the mobile manipulator in a whole continuous motion process is used to perform collision detection, and fundamentally solves the tunnel effect problem inherent in the collision detection method based on discrete time points. Finally, the swept volume of the digital twin joint body model can accurately reflect a real space occupation of the mobile manipulator in any complex and extreme pose, eliminates the defect that the safety radius simplified model cannot cover collision risks in extreme poses, and ensures safety in all working conditions. BRIEF DESCRIPTION OF DRAWINGS

[0012] Figure 1 FIG. 1 is a flowchart of a mobile manipulator dynamic collision detection method based on a time-varying volume model in an embodiment of the application;

[0013] Figure 2 FIG. 2 is a flowchart of a mobile manipulator dynamic collision detection method based on a time-varying volume model in another embodiment of the application;

[0014] Figure 3 FIG. 3 is a structural schematic diagram of a mobile manipulator dynamic collision detection device based on a time-varying volume model in an embodiment of the application;

[0015] Figure 4 FIG. 4 is a structural schematic diagram of a mobile manipulator dynamic collision detection device based on a time-varying volume model in an embodiment of the application. Detailed Implementation

[0016] 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.

[0017] 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.

[0018] 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:

[0019] 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.

[0020] 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.

[0021] Step S10 specifically includes the following steps:

[0022] 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.

[0023] 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.

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

[0025] Specifically, the accurate pose (position and attitude) of the mobile platform and the real-time angle data of all joints of the robot arm are acquired in real time by the sensor.

[0026] S13, based on the real-time acquired pose of the mobile platform and the joint angle data of the robot arm, each three-dimensional geometric unit of the geometric unit library is assembled to construct a digital twin joint body model completely consistent with the current physical state of the real mobile robot arm.

[0027] Step S13 is mathematically achieved by forward kinematics, and the specific process is as follows:

[0028] S131, let the world coordinate system be , the pose of the mobile platform at time t is described by a homogeneous transformation matrix , which contains a rotation matrix and a translation vector :

[0029] ;

[0030] Wherein, represents the mobile platform.

[0031] S132, based on the joint angle vector of the robot arm at time t, the pose of the connecting rod of the robot arm at time t relative to the mobile platform is calculated by the standard D-H parameter method or screw theory.

[0032] Specifically, let the robot arm have joints, and the joint angle vector of the robot arm at time t is composed of the angle data of each joint at time t, , represents the angle of the i joint at time t, i =1,2,3,…,N. And the pose of any connecting rod of the robot arm at time t relative to the mobile platform is a function of , which can be calculated by the standard D-H parameter method or screw theory.

[0033] S133, calculate the real-time pose of the connecting rod of the robot arm in the world coordinate system:

[0034] ;

[0035] Wherein, represents the ia three-dimensional geometric unit corresponding to each link.

[0036] S134, according to the calculation Adjust the instantaneous position and attitude of each link of the mechanical arm, and construct the digital twin joint body model.

[0037] Specifically, the three-dimensional geometric units representing each link in the geometric unit library are assembled, and the instantaneous position and attitude of the three-dimensional geometric units representing each link are adjusted accurately, thereby forming a complete digital twin joint body model.

[0038] The digital twin joint body model not only reflects the ideal kinematic relationship, but also can be calibrated and corrected for kinematic parameters (such as link length, joint offset, etc.) based on closed-loop data (such as torque sensor, encoder bias, etc.) through offline calibration to compensate for physical wear or manufacturing tolerance, ensuring high fidelity.

[0039] S20, based on the planned path, calculate the swept volume of the digital twin joint body model in the process of simulating the execution of the planned path.

[0040] The swept volume is the space volume occupied by the object when moving along any trajectory in space. The step S20 specifically includes the following steps:

[0041] S21, receive the planned path of the mobile manipulator from the upper motion planning module.

[0042] The planned path is the target motion trajectory of the mobile manipulator in the future period of time, which defines the continuous change process of the state quantity of the mobile platform and each joint of the manipulator with time.

[0043] S22, use the digital twin joint body model to accurately simulate the execution of the planned path.

[0044] S23, calculate and aggregate all three-dimensional space point sets passed through by the digital twin joint body model in the process of simulating the execution of the planned path, to obtain the swept volume occupied by the digital twin joint body model in the entire motion process.

[0045] The step S23 further includes:

[0046] S231, calculate the three-dimensional space point set occupied by the digital twin joint body model at

[0047] ;

[0048] wherein,​​​ represents the real-time pose of the link in the world coordinate system, represents the third i dimensional geometric element corresponding to the link.

[0049] S232, calculate the swept volume of the digital twin joint body model in the planning time period . :

[0050] .

[0051] The planning time period is the time period in which the digital twin joint body model executes the planned path. From the above formula, the swept volume is the union set of the three-dimensional space point sets occupied by the digital twin joint body model at each time in the planning time period .

[0052] In practice, this continuous union operation cannot be directly calculated. Usually, a discrete approximation method is used for calculation: the planning time period is divided into small enough time steps, and the time step length is . For each time step , calculate the three-dimensional space point sets and occupied by the model at two times, and perform union operation on and to obtain the local swept volume of the time step , and then combine all the local swept volumes to form a high-precision approximation to the real .

[0053] S30, perform volume intersection operation on the swept volume and the three-dimensional environment map in the three-dimensional space, and determine whether the planned path is safe according to the operation result.

[0054] The step S30 further includes:

[0055] S31, obtain a three-dimensional environment grid map corresponding to the environment around the planned path.

[0056] The three-dimensional environment grid map (such as OctoMap) is generated by a SLAM (Simultaneous Localization and Mapping) system, and includes information such as obstacles in the environment around the planned path.

[0057] S32, in the shared three-dimensional space coordinate system, a volume intersection operation is performed on the swept volume and the three-dimensional environment grid map, and whether the planned path is safe is determined according to the operation result.

[0058] Suppose the space occupied by the three-dimensional environment grid map is In the three-dimensional environment grid map, It is expressed as the set of all voxels marked as occupied. The core task of collision detection is to determine whether the intersection of the swept volume and is an empty set; if the intersection is empty, it is determined that the planned path is absolutely safe; if the intersection is non-empty, it is determined that there is a collision risk in the planned path.

[0059] Specifically, the intersection operation can be implemented using an efficient algorithm. For example, the swept volume is voxelized, and then compared one by one with to determine whether the intersection of the two is empty. Alternatively, the swept volume is expressed as the union of a series of convex polyhedrons, and the Separating Axis Theorem (SAT) or the GJK (Gilbert-Johnson-Keerthi) algorithm is used for fast interference query.

[0060] The mobile manipulator dynamic collision detection method based on the time-varying volume model provided by the embodiments of the present application has the following significant beneficial effects:

[0061] 1. Extremely high accuracy and space utilization: By accurately modeling and sweeping the complete three-dimensional form of the mobile manipulator, the overly conservative safety radius simplified model is completely abandoned. The embodiments of the present application can accurately identify the limit narrow space through which the mobile manipulator can pass, significantly reducing invalid shutdowns caused by false positives, greatly improving the work efficiency and space utilization of the mobile manipulator in complex and crowded environments, and solving the technical problems of the collision detection methods in the prior art, such as being overly conservative and low in efficiency;

[0062] 2. Absolute continuous safety guarantee: By generating a swept volume representing the space occupied by the entire continuous motion of the mobile manipulator for collision detection, the tunnel effect problem inherent in collision detection methods based on discrete time points is fundamentally solved. No matter how fast the mobile manipulator moves, it can be guaranteed that there will be no undetected penetration or collision, providing a truly continuous trajectory safety guarantee;

[0063] 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.

[0064] 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.

[0065] like Figure 2 As 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:

[0066] 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.

[0067] 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.

[0068] Step S10 specifically includes the following steps:

[0069] 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.

[0070] 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.

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

[0072] Specifically, the accurate pose (position and attitude) of the mobile platform and the real-time angle data of all joints of the robot arm are acquired in real time by the sensor.

[0073] S13, based on the real-time acquired pose of the mobile platform and the joint angle data of the robot arm, each three-dimensional geometric unit of the geometric unit library is assembled to construct a digital twin joint body model completely consistent with the current physical state of the real mobile robot arm.

[0074] Step S13 is mathematically achieved by forward kinematics, and the specific process is as follows:

[0075] S131, let the world coordinate system be , the pose of the mobile platform at time t is described by a homogeneous transformation matrix , which contains a rotation matrix and a translation vector :

[0076] ;

[0077] wherein represents the mobile platform.

[0078] S132, based on the joint angle vector of the robot arm at time t, the pose of the connecting rod of the robot arm at time t relative to the mobile platform is calculated by the standard D-H parameter method or screw theory.

[0079] Specifically, let the robot arm have joints, and the joint angle vector of the robot arm at time t is composed of the angle data of each joint at time t, , represents the angle of the i joint at time t, i =1,2,3,…,N. And the pose of any connecting rod of the robot arm at time t relative to the mobile platform is a function of , which can be calculated by the standard D-H parameter method or screw theory.

[0080] S133, calculate the real-time pose of the connecting rod of the robot arm in the world coordinate system:

[0081] ;

[0082] wherein represents the three-dimensional geometric unit corresponding to the i connecting rod.

[0083] S134、According to the calculated Adjust the instantaneous position and pose of each link of the mechanical arm, and construct the digital twin joint body model.

[0084] Specifically, the three-dimensional geometric body units representing each link in the geometric body unit library are assembled, and the instantaneous position and pose of the three-dimensional geometric body units representing each link are adjusted accurately during the assembly process according to the formula calculated in step S133

[0085] The digital twin joint body model not only reflects the ideal kinematic relationship, but also can be calibrated and corrected for kinematic parameters (such as link length, joint offset, etc.) based on closed-loop data (such as torque sensor, encoder bias, etc.) through offline calibration to compensate for physical wear or manufacturing tolerance, ensuring high fidelity.

[0086] S20, based on the planned path, calculate the swept volume of the digital twin joint body model in the process of simulating the execution of the planned path.

[0087] The swept volume is the space volume occupied by an object when it moves along an arbitrary trajectory in space. The step S20 specifically includes the following steps:

[0088] S21, receive the planned path of the moving mechanical arm from the upper motion planning module.

[0089] The planned path is the target motion trajectory of the moving mechanical arm in the future period of time, which defines the continuous change process of the state quantity of the moving platform and each joint of the mechanical arm with time.

[0090] S22, accurately simulate the execution of the planned path using the digital twin joint body model.

[0091] S23, calculate and aggregate all three-dimensional space point sets passed through by the digital twin joint body model in the process of simulating the execution of the planned path, to obtain the swept volume occupied by the digital twin joint body model in the entire motion process.

[0092] The step S23 further includes:

[0093] S231, calculate the three-dimensional space point set occupied by the digital twin joint body model at

[0094] ;

[0095] wherein, represents the real-time pose of the link in the world coordinate system,​​​ represents the third i dimensional geometric element corresponding to the first

[0096] S232, calculate the swept volume of the digital twin joint body model in the planning time period :

[0097] .

[0098] The planning time period is the time period in which the digital twin joint body model executes the planned path. From the above formula, the swept volume is the union set of the three-dimensional space point sets occupied by the digital twin joint body model at each time in the planning time period .

[0099] In practice, this continuous union operation cannot be directly calculated. Usually, a discrete approximation method is used for calculation: the planning time period is divided into small enough time steps, and the time step length is . For each time step , calculate the three-dimensional space point sets and occupied by the model at two times, and perform union operation on and to obtain the local swept volume of the time step , and then combine all the local swept volumes to form a high-precision approximation to the real .

[0100] S30, perform volume intersection operation on the swept volume and the three-dimensional environment map in the three-dimensional space, and determine whether the planned path is safe according to the operation result.

[0101] The step S30 further includes:

[0102] S31, obtain a three-dimensional environment grid map corresponding to the environment around the planned path.

[0103] The three-dimensional environment grid map (such as OctoMap) is generated by a SLAM (Simultaneous Localization and Mapping) system, and includes information such as obstacles in the environment around the planned path.

[0104] S32, perform volume intersection operation on the swept volume and the three-dimensional environment grid map in the shared three-dimensional space coordinate system, and determine whether the planned path is safe according to the operation result. ​

[0105] Let the space occupied by the three-dimensional environment raster map be In the three-dimensional environment raster map, is represented as the set of all voxels marked as occupied. The core task of collision detection is to determine whether the intersection of the swept volume and is an empty set; if the intersection is empty, it is determined that the planned path is absolutely safe; if the intersection is non-empty, it is determined that the planned path has a collision risk.

[0106] Specifically, the intersection operation can be implemented using an efficient algorithm. For example, the swept volume is voxelized, and then compared one by one with to determine whether the intersection of the two is empty. Alternatively, the swept volume is represented as the union of a series of convex polyhedrons, and a fast interference query is performed using the Separating Axis Theorem (SAT) or the GJK (Gilbert-Johnson-Keerthi) algorithm.

[0107] S40, based on the path safety determination result, performing a corresponding safety decision.

[0108] The path safety determination result obtained through step S30 is specifically represented as the planned path being absolutely safe or the planned path having a collision risk.

[0109] Step S40 performs a corresponding safety decision based on the path safety determination result, and the specific process is as follows:

[0110] S41, if the planned path is absolutely safe, then controlling the mobile manipulator to execute the planned path;

[0111] S42, if the planned path has a collision risk, then controlling the mobile manipulator to refuse to execute the planned path, and locating the first collision point, the first collision time, and the specific components and environmental positions of the mobile manipulator that have a collision through binary search.

[0112] a) Locating the first collision time:

[0113] When a collision is detected (i.e., the intersection is non-empty), the first collision time can be efficiently located through binary search on the planning time period .

[0114] b) Locating the collision position and components:

[0115] Once the first collision time is determined,The precise collision information can be obtained by analyzing the intersection The point in the intersection is the first collision point in the world coordinate system. Then, through reverse query, the specific components on the mobile manipulator corresponding to the points can be determined, and the specific positions of the points in the three-dimensional environment grid map can be determined, so that the specific components and the environment positions of the mobile manipulator where the collision occurs can be accurately located.

[0116] The mobile manipulator dynamic collision detection method based on the time-varying volume model provided by the embodiment of the application has the following remarkable beneficial effects:

[0117] 1. Extremely high accuracy and space utilization: By accurately modeling and sweeping the complete three-dimensional form of the mobile manipulator, the overly conservative safety radius simplified model is completely abandoned. The embodiment of the application can accurately identify the limit narrow space through which the mobile manipulator can pass, significantly reduces the invalid shutdown caused by false positives, greatly improves the operation efficiency and space utilization of the mobile manipulator in a complex and crowded environment, and solves the technical problems of the collision detection method in the prior art, such as being overly conservative and low in efficiency;

[0118] 2. Absolute continuous safety guarantee: The collision detection is performed by generating a swept volume representing the space occupation of the entire continuous motion process of the mobile manipulator, which fundamentally solves the tunnel effect problem inherent in the collision detection method based on discrete time points. No matter how fast the mobile manipulator moves, it can be guaranteed that no undetected penetration or collision will occur, providing a truly continuous trajectory safety guarantee;

[0119] 3. Comprehensive risk coverage: The swept volume of the digital twin joint body model can accurately reflect the real space occupation of the robot in any complex and extreme pose, eliminating the defect that the safety radius simplified model cannot cover the collision risk in the extreme pose, and ensuring the safety in all working conditions;

[0120] 4. High-fidelity real-world decision-making: The collision detection calculation is performed based on the digital twin joint body model that is real-time synchronized with the real mobile manipulator, and the model on which the safety decision is based reflects the real physical state of the mobile manipulator (rather than an idealized CAD model), making the safety evaluation more reliable and providing higher confidence for deploying the mobile manipulator in the demanding real world.

[0121] Figure 3 The structure of the mobile manipulator dynamic collision detection device based on the time-varying volume model in the embodiment of the application is shown in the structural schematic diagram, and each module of the mobile manipulator dynamic collision detection device based on the time-varying volume model is the same as Figure 1The steps of the mobile manipulator dynamic collision detection method based on the time-varying volume model in the illustrated embodiment correspond one by one. As shown in the figure Figure 3 As shown, the mobile manipulator dynamic collision detection device based on the time-varying volume model includes a model construction module 10, a swept volume calculation module 20, and a path safety detection module 30. Each functional module is described in detail as follows:

[0122] The model construction module 10 is configured to construct a digital twin joint body model that is completely consistent with the current physical state of the real mobile manipulator based on the real-time acquired pose of the mobile platform and the joint angle data of the manipulator;

[0123] The swept volume calculation module 20 is configured to calculate the swept volume of the digital twin joint body model in the process of simulating the execution of the planned path based on the planned path;

[0124] The path safety detection module 30 is configured to perform volume intersection operation on the swept volume and the three-dimensional environment map in the three-dimensional space, and determine whether the planned path is safe according to the operation result.

[0125] For specific limitations of the mobile manipulator dynamic collision detection device based on the time-varying volume model of the present embodiment, please refer to Figure 1 For the limitations of the mobile manipulator dynamic collision detection method based on the time-varying volume model in the illustrated embodiment, they are not described here. Each module in the above mobile manipulator dynamic collision detection device based on the time-varying volume model can be realized by software, hardware, and combinations thereof, in whole or in part. The above modules can be embedded in or independent of the processor in the computer device in hardware form, or can be stored in the memory in the computer device in software form, so as to be called and executed by the processor to perform the operations corresponding to each module.

[0126] The mobile manipulator dynamic collision detection device based on the time-varying volume model provided by the embodiment of the present application has the following remarkable beneficial effects:

[0127] 1. Extremely high accuracy and space utilization: By accurately modeling and sweeping the complete three-dimensional form of the mobile manipulator, the overly conservative safety radius simplified model is completely abandoned. The embodiment of the present application can accurately identify the limit narrow space through which the mobile manipulator can pass, significantly reducing the invalid shutdown caused by false positives, greatly improving the operation efficiency and space utilization of the mobile manipulator in complex and crowded environments, and solving the technical problems of the collision detection method in the prior art, such as being overly conservative and low efficiency;

[0128] 2. Absolute continuity safety guarantee: By generating a swept volume representing the space occupancy of the entire continuous motion of the mobile manipulator, collision detection is performed, fundamentally solving the tunnel effect problem inherent in collision detection methods based on discrete time points. Regardless of the speed of the mobile manipulator motion, it can be ensured that no undetected penetration or collision occurs, providing a true sense of continuous trajectory safety guarantee;

[0129] 3. Comprehensive risk coverage: The swept volume of the digital twin joint body model can accurately reflect the real space occupancy of the robot in any complex, extreme pose, eliminating the defect that the safety radius simplified model cannot cover the collision risk in extreme pose, ensuring safety in all working conditions;

[0130] 4. High-fidelity real ground decision: Based on the digital twin joint body model that is real-time synchronized with the real mobile manipulator, collision detection calculation is performed, and the model on which the safety decision is based reflects the real physical state of the mobile manipulator (rather than the idealized CAD model), making the safety assessment more reliable and providing higher confidence for deploying the mobile manipulator in the demanding real world.

[0131] Figure 4 A structure diagram of a mobile manipulator dynamic collision detection device based on a time-varying volume model in another embodiment of the application is shown, and each module of the mobile manipulator dynamic collision detection device based on the time-varying volume model corresponds to each step of the mobile manipulator dynamic collision detection method based on the time-varying volume model in the embodiment shown in Figure 2 As shown in Figure 4 The mobile manipulator dynamic collision detection device based on the time-varying volume model includes a model construction module 10, a swept volume calculation module 20, a path safety detection module 30, and a safety decision module 40. The functions of each module are described in detail as follows:

[0132] The model construction module 10 is used to construct a digital twin joint body model consistent with the current physical state of the real mobile manipulator based on the real-time acquired pose of the mobile platform and the joint angle data of the manipulator;

[0133] The swept volume calculation module 20 is used to calculate the swept volume of the digital twin joint body model in the process of simulating the execution of the planned path based on the planned path;

[0134] The path safety detection module 30 is used to perform volume intersection operation on the swept volume and the three-dimensional environment map in three-dimensional space, and determine whether the planned path is safe according to the operation result;

[0135] The safety decision module 40 is used to execute corresponding safety decisions based on the path safety determination result.

[0136] For specific definitions of the time-varying volume model-based mobile manipulator dynamic collision detection device of the present embodiment, please refer to Figure 2 For the definitions of the time-varying volume model-based mobile manipulator dynamic collision detection method in the illustrated embodiment, no further elaboration is given here. The various modules in the above-described time-varying volume model-based mobile manipulator dynamic collision detection device can be realized in whole or in part by software, hardware, and combinations thereof. The above-mentioned modules can be embedded in or independent of the processor in the computer device in hardware form, or can be stored in the memory in the computer device in software form, so as to be called and executed by the processor to perform the operations corresponding to the above-mentioned modules.

[0137] The time-varying volume model-based mobile manipulator dynamic collision detection device provided by the embodiments of the present application has the following significant beneficial effects:

[0138] 1. Extremely high accuracy and space utilization: By accurately modeling and sweeping the complete three-dimensional form of the mobile manipulator, the overly conservative safety radius simplified model is completely abandoned. The embodiments of the present application can accurately identify the limit narrow space through which the mobile manipulator can pass, significantly reducing invalid shutdowns caused by false positives, greatly improving the operation efficiency and space utilization of the mobile manipulator in complex and crowded environments, and solving the technical problems of the collision detection methods in the prior art, such as being overly conservative and low in efficiency;

[0139] 2. Absolute continuous safety guarantee: By generating a swept volume representing the spatial occupancy of the entire continuous motion process of the mobile manipulator for collision detection, the inherent tunnel effect problem of the collision detection method based on discrete time points is fundamentally solved. No matter how fast the mobile manipulator moves, it can be guaranteed that there will be no undetected penetration or collision, providing a truly continuous trajectory safety guarantee;

[0140] 3. Comprehensive risk coverage: The swept volume of the digital twin joint body model can accurately reflect the true spatial occupancy of the robot in any complex and extreme pose, eliminating the defect that the safety radius simplified model cannot cover the collision risk in extreme poses, ensuring safety in all working conditions;

[0141] 4. High-fidelity real-world grounding decision: Collision detection calculation is based on the digital twin joint body model that is synchronized in real time with the real mobile manipulator, and the model on which the safety decision is based reflects the real physical state of the mobile manipulator (rather than an idealized CAD model), making safety assessment more reliable and providing higher confidence for deploying mobile manipulators in demanding real-world environments.

[0142] The above merely illustrates the preferred embodiments of the present application, but is not intended to limit the present application in any form. Based on the above embodiments, those skilled in the art can make various equivalent changes and improvements, and any equivalent changes or modifications made within the scope of claims should fall into the protection scope of the present application.

Claims

1. A dynamic collision detection method for a mobile robotic arm based on a time-varying volume model, characterized in that, Comprising the following steps: S10, based on the real-time acquired pose of the mobile platform and the joint angle data of the robot arm, a digital twin joint body model completely consistent with the current physical state of the real mobile robot arm is constructed; S20, based on the planned path, the swept volume of the digital twin joint body model in the process of simulating the execution of the planned path is calculated; S30, volume intersection operation is performed on the swept volume and the three-dimensional environment map in the three-dimensional space, and whether the planned path is safe is determined according to the operation result; The step S20 further comprises: S21, receiving the planned path of the mobile robot arm from the upper motion planning module; S22, using the digital twin joint body model to accurately simulate the execution of the planned path; S23, calculating and aggregating all three-dimensional space point sets passed through by the digital twin joint body model in the process of simulating the execution of the planned path, to obtain the swept volume occupied by the digital twin joint body model in the whole motion process; The step S23 further comprises: S231、calculating a set of three-dimensional space points occupied by the digital twin joint body model at a moment in time : ； wherein, represents the real-time pose of the link in the world coordinate system, represents the three-dimensional geometric body unit corresponding to the i-th link; S232, computing a swept volume of the digital twin joint body model within a planning time period :​ 。 2. The time-varying volume model based dynamic collision detection method for a mobile manipulator as claimed in claim 1, wherein, The step S10 further comprises: S11, each physical component of the mobile platform and the robot arm is pre-modeled as a corresponding three-dimensional geometric body unit, forming a geometric body unit library; S12, real-time acquisition of the pose of the mobile platform and the angle data of all joints of the robot arm; S13, based on the real-time acquired pose of the mobile platform and the joint angle data of the robot arm, assembling each three-dimensional geometric body unit of the geometric body unit library to construct a digital twin joint body model completely consistent with the current physical state of the real mobile robot arm.

3. The time-varying volume model based dynamic collision detection method for a mobile manipulator as claimed in claim 2, wherein, The step S13 further comprises: S131、Set the world coordinate system as , the pose of the mobile platform at time t is described by a homogeneous transformation matrix : ； wherein denotes a mobile platform, , denotes a rotation matrix and , denotes a translation vector and ; S132, based on the joint angle vector of the mechanical arm at time t The pose of the link of the mechanical arm relative to the mobile platform at time t is calculated by the standard D-H parameter method or screw theory wherein, , denotes the angle of the j i th joint at time t, i =1,2,3,…,N; N represents the number of joints of the mechanical arm; S133, calculate real-time pose of the connecting rod of the mechanical arm in the world coordinate system : ； wherein, represents the three-dimensional geometric element corresponding to the i-th link; S134、according to the calculation Adjust the instantaneous position and posture of each link of the mechanical arm, and construct the digital twin joint body model.

4. The time-varying volume model based dynamic collision detection method for a mobile manipulator as claimed in claim 3, wherein, The step S10 further comprises: correcting the kinematic parameters of the digital twin joint body model through offline calibration.

5. The time-varying volume model based dynamic collision detection method for a mobile manipulator as claimed in claim 1, wherein, The step S30 further comprises: S31, acquiring a three-dimensional environment grid map corresponding to the environment around the planned path; S32, performing volume intersection operation on the swept volume and the three-dimensional environment grid map in the shared three-dimensional space coordinate system, and determining whether the planned path is safe according to the operation result; The step S32 further comprises: sweep volume voxelization; the swept volume after voxelization with all the set of voxels in the three-dimensional environment raster map that are marked as occupied performing an intersection operation; If is empty, then the planned path is determined to be absolutely safe; if is non-empty, then the planned path is determined to be at risk of collision.

6. The time-varying volume model based dynamic collision detection method for a mobile manipulator as claimed in claim 1, wherein, Comprising the following steps: S40, based on the path safety determination result, performing corresponding safety decision.

7. The time-varying volume model based dynamic collision detection method for a mobile manipulator as claimed in claim 6, wherein, The step S40 further comprises: S41, if the planned path is absolutely safe, controlling the mobile robot arm to execute the planned path; S42, if the planned path has collision risk, controlling the mobile robot arm to refuse to execute the planned path, and positioning the first collision point, the first collision time, and the specific component and environment position of the mobile robot arm that collides through bisection method.

8. A dynamic collision detection device for a mobile robotic arm based on a time-varying volume model, characterized in that, Comprise: A model construction module, configured to construct a digital twin joint body model completely consistent with the current physical state of the real mobile robot arm based on the real-time acquired pose of the mobile platform and the joint angle data of the robot arm; A swept volume calculation module, configured to calculate the swept volume of the digital twin joint body model in the process of simulating the execution of the planned path based on the planned path; A path safety detection module, configured to perform volume intersection operation on the swept volume and the three-dimensional environment map in the three-dimensional space, and determine whether the planned path is safe according to the operation result; The sweep volume calculation module is specifically configured to: receive a planned path of the mobile manipulator from an upper motion planning module; simulate execution of the planned path accurately by using the digital twin joint body model; calculate and aggregate all three-dimensional space point sets passed through by the digital twin joint body model in the process of simulating execution of the planned path to obtain a swept volume occupied by the digital twin joint body model in a whole motion process in space-time; The calculation and aggregation of all three-dimensional space point sets passed through by the digital twin joint body model in the process of simulating execution of the planned path to obtain a swept volume occupied by the digital twin joint body model in a whole motion process in space-time further includes: a three-dimensional point set occupied by the digital twin joint body model at a time instant :​ ； wherein, represents the real-time pose of the link in the world coordinate system, represents the three-dimensional geometric body unit corresponding to the i-th link; computing a swept volume within the digital twin joint body model at a planning time period :​ 。

Images