Operation arm motion control method, device and system and storage medium

Abstract

The invention relates to the technical field of motion planning of an operating arm, and discloses a motion control method, device and system of the operating arm and a storage medium, and the method comprises the following steps: determining an initial track of the operating arm based on a current state of the operating arm, task constraint, a force vector at an end effector and a target pose, and controlling the operating arm to execute the initial track; detecting a risk of collision between the operating arm and the obstacle during the movement of the operating arm; when it is determined that the collision risk exists between the operation arm and the obstacle, obstacle avoidance motion parameters of the operation arm away from the obstacle are calculated, and the operation arm is controlled to execute the obstacle avoidance motion parameters away from the obstacle; repeatedly executing the new trajectory planning process to obtain a new trajectory during execution of the obstacle avoidance motion parameters away from the obstacle; and when it is determined that the collision risk of the operation arm and the obstacle disappears, planning of the new trajectory of the operation arm is stopped, and the operation arm is controlled to execute the newly planned new trajectory. According to the method, the reaction speed and the path smoothness of the operation arm in a dynamic environment are improved, and the production efficiency is improved.

Description

Technical Field

[0001] The present disclosure relates to the technical field of manipulator motion planning, for example, to a manipulator motion control method, device, system, and storage medium. Background Art

[0002] With the continuous expansion of the application of human-robot collaboration in fields such as manufacturing and aerospace, the online motion generation of manipulators in restricted and dynamic working environments has become particularly crucial. To ensure the safety of human-robot collaboration, standards such as ISO / TS15066:2016 stipulate methods for real-time monitoring of the human-robot distance and dynamically adjusting the robot speed according to the separation distance to reduce the collision risk. However, this method may cause the manipulator to move slowly or discontinuously, affecting production efficiency.

[0003] To solve the above problems, some reactive motion planning methods have been proposed in related technologies. These methods allow the manipulator to quickly respond to the state changes of dynamic obstacles and avoid them. However, since the local controller is prone to falling into local extrema, this may cause the manipulator to be unable to complete the predetermined task smoothly, or even exhibit discontinuous motion. In addition, the motion generation of manipulators in related technologies usually relies on a combination of offline path planning and online obstacle avoidance. Although this method can ensure safety to a certain extent, in the face of real-time environmental changes, the path lacks the ability of adaptive adjustment. Especially when encountering obstacles that need to be avoided, the manipulator often needs to return to the state before obstacle avoidance and then continue to execute the task, which not only causes the problem of discontinuous motion but also further reduces the task execution efficiency.

[0004] In summary, the solutions of related technologies can ensure the safety of human-robot collaboration within a certain range, but there are still obvious deficiencies in improving the reaction speed of the manipulator to dynamic obstacles and ensuring the smoothness of the motion path. Especially in aspects such as accurate distance calculation, effective collision state inspection, and optimized motion planning strategies, there are still many challenges, and new solutions are urgently needed to improve the quality and efficiency of human-robot collaboration. Summary of the Invention

[0005] To provide a basic understanding of some aspects of the disclosed embodiments, a simple summary is given below. This summary is not a comprehensive review, nor is it intended to identify key / important elements or delineate the protection scope of these embodiments. Instead, it serves as a preface to the following detailed description.

[0006] The embodiments of the present disclosure provide a manipulator motion control method, device, system, and storage medium, which can effectively improve the reaction speed and path smoothness of the manipulator in a dynamic environment while ensuring human-robot safety, thereby optimizing the production efficiency of human-robot collaboration.

[0007] According to a first aspect of the present disclosure, there is provided a method for controlling the movement of a robotic arm, including:

[0008] Determining an initial trajectory of the robotic arm based on the current state of the robotic arm, task constraints, the force vector at the end effector, and the target pose, and controlling the robotic arm to execute the initial trajectory;

[0009] During the movement of the robotic arm, detecting the collision risk between the robotic arm and an obstacle;

[0010] When it is determined that there is a collision risk between the robotic arm and an obstacle, calculating the obstacle avoidance movement parameters for the robotic arm to move away from the obstacle, and controlling the robotic arm to execute the obstacle avoidance movement parameters for moving away from the obstacle;

[0011] Repeating the new trajectory planning process during the execution of the obstacle avoidance movement parameters for moving away from the obstacle to obtain a new trajectory;

[0012] When it is determined that the collision risk between the robotic arm and the obstacle disappears, stopping the planning of the new trajectory of the robotic arm and controlling the robotic arm to execute the latest planned new trajectory.

[0013] In some embodiments, determining an initial trajectory of the robotic arm based on the current state of the robotic arm, task constraints, the force vector at the end effector, and the target pose includes:

[0014] Determining an initial pose of the end effector based on the current state of the robotic arm and the force vector at the end effector;

[0015] Determining an initial trajectory of the robotic arm based on the task constraints of the robotic arm, and the force vector, initial pose, and target pose at the end effector.

[0016] In some embodiments, during the movement of the robotic arm, detecting the collision risk between the robotic arm and an obstacle includes:

[0017] Constructing an equivalent model of the robotic arm and the obstacle, where the equivalent model is represented by a superquadric surface;

[0018] During the movement of the robotic arm, calculating the minimum distance between the equivalent models of the robotic arm and the obstacle, and this minimum distance is used to characterize the collision risk between the robotic arm and the obstacle.

[0019] In some embodiments, calculating the minimum distance between the equivalent models of the robotic arm and the obstacle includes:

[0020] For the equivalent models of the robotic arm and the obstacle, expanding one equivalent model into a Minkowski sum envelope surface and degenerating the other equivalent model into an equivalent point;

[0021] By calculating the Euclidean distance between the Minkowski sum envelope surface and the equivalent point, obtaining the Euclidean distance as the minimum distance between the equivalent models of the robotic arm and the obstacle.

[0022] In some embodiments, when it is determined that there is a risk of collision between the robotic arm and an obstacle, the obstacle avoidance motion parameters for the robotic arm to move away from the obstacle are calculated, and the robotic arm is controlled to execute the obstacle avoidance motion parameters for moving away from the obstacle, including: when it is determined that there is a risk of collision between the robotic arm and an obstacle, the joint angular velocity of the robotic arm moving away from the obstacle is calculated, and the robotic arm is controlled to execute the joint angular velocity of moving away from the obstacle.

[0023] In some embodiments, the collision risk between the robotic arm and the obstacle is characterized by the minimum distance between the equivalent models of the robotic arm and the obstacle; when it is determined that there is a risk of collision between the robotic arm and the obstacle, the joint angular velocity of the robotic arm moving away from the obstacle is calculated, and the robotic arm is controlled to execute the joint angular velocity of moving away from the obstacle, including: when the minimum distance is less than the safety distance threshold, the joint angular velocity of the robotic arm moving away from the obstacle is calculated, and the robotic arm is controlled to execute the joint angular velocity of moving away from the obstacle.

[0024] In some embodiments, calculating the joint angular velocity of the robotic arm moving away from the obstacle includes:

[0025] Calculating a first joint angular velocity corresponding to the gravitational vector of the joint, where the gravitational vector is used to guide the end effector to move towards the target position;

[0026] Calculating a second joint angular velocity corresponding to the repulsive vector of the joint, where the repulsive vector is used to guide the robotic arm to avoid obstacles;

[0027] Based on the first joint angular velocity and the second joint angular velocity, calculating the joint angular velocity of the robotic arm moving away from the obstacle.

[0028] In some embodiments, calculating the first joint angular velocity corresponding to the gravitational vector of the joint includes:

[0029] Determining the current pose of the end effector, and calculating the gravitational vector of the joint based on the current pose and the target pose of the end effector;

[0030] Calculating the first joint angular velocity corresponding to the gravitational vector.

[0031] In some embodiments, calculating the gravitational vector of the joint based on the current pose and the target pose of the end effector includes:

[0032] Determining a proportional gain coefficient and a positive definite coefficient matrix related to the gravitational vector;

[0033] Calculating the pose difference value between the current pose and the target pose of the end effector;

[0034] Based on the proportional gain coefficient, the positive definite coefficient matrix, and the pose difference value between the current pose and the target pose of the end effector, calculating the gravitational vector of the joint.

[0035] In some embodiments, calculating a first joint angular velocity corresponding to a gravitational vector includes:

[0036] Determining a damping factor associated with the gravitational vector and a Jacobian matrix associated with the end effector;

[0037] Calculating a first joint angular velocity corresponding to the gravitational vector based on the gravitational vector, the damping factor associated with the gravitational vector, and the Jacobian matrix associated with the end effector.

[0038] In some embodiments, calculating a second joint angular velocity corresponding to a repulsive force vector of a joint includes: calculating a repulsive force vector of the joint based on a minimum distance between an equivalent model of the robotic arm and an obstacle; calculating a second joint angular velocity corresponding to the repulsive force vector based on the repulsive force vector.

[0039] In some embodiments, calculating a repulsive force vector of a joint based on a minimum distance between an equivalent model of the robotic arm and an obstacle includes:

[0040] Comparing the minimum distance between the equivalent model of the robotic arm and the obstacle with an activation distance;

[0041] Calculating a repulsive force vector of the joint based on a comparison result between the minimum distance between the equivalent model of the robotic arm and the obstacle and the activation distance.

[0042] In some embodiments, calculating a second joint angular velocity corresponding to a repulsive force vector based on the repulsive force vector includes: determining a damping factor associated with the repulsive force vector and a Jacobian matrix associated with an equivalent model of the robotic arm; calculating a second joint angular velocity corresponding to the repulsive force vector based on the repulsive force vector, the damping factor associated with the repulsive force vector, and the Jacobian matrix associated with an equivalent model of the robotic arm.

[0043] In some embodiments, a new trajectory planning process includes: determining a current pose of the end effector; determining a new trajectory of the robotic arm based on a target pose of the robotic arm, task constraints, a force vector at the end effector, and the current pose of the end effector.

[0044] In some embodiments, determining a new trajectory of the robotic arm based on a target pose of the robotic arm, task constraints, a force vector at the end effector, and the current pose of the end effector includes:

[0045] Calculating a gravitational vector of a joint based on the current pose and the target pose of the end effector;

[0046] Determining a predicted pose of the end effector based on the current pose of the end effector and the gravitational vector of the joint;

[0047] Determining a new trajectory of the robotic arm based on a target pose of the robotic arm, task constraints, a force vector at the end effector, and the predicted pose of the end effector.

[0048] According to a second aspect of the present disclosure, there is provided a robotic arm motion control device, including a processor and a memory storing program instructions, and the processor executes the robotic arm motion control method provided by the first aspect of the present disclosure.

[0049] According to a third aspect of the present disclosure, there is provided a robotic arm motion control system, including a robotic arm and the robotic arm motion control device provided by the second aspect of the present disclosure, and the robotic arm motion control device is communicatively connected to the robotic arm.

[0050] According to a fourth aspect of the present disclosure, there is provided a storage medium storing computer program instructions, and when the computer program instructions are run by a processor, the robotic arm motion control method provided by the first aspect of the present disclosure is executed.

[0051] The robotic arm motion control method, device, system, and storage medium provided by the embodiments of the present disclosure can achieve the following technical effects:

[0052] The robotic arm motion control method provided by the embodiments of the present disclosure introduces a real-time trajectory adjustment mechanism, enabling the robotic arm to dynamically calculate and execute obstacle avoidance motion parameters to move away from obstacles when detecting a collision risk, rather than simply stopping or returning to a safe distance. This fast response mechanism ensures that the robotic arm can still work efficiently in the face of unforeseen environmental changes and reduces the delay caused by obstacle avoidance. At the same time, by continuously re-planning a new trajectory while the robotic arm performs the obstacle avoidance action, once it is confirmed that the collision risk is lifted, the robotic arm can continue to execute the task according to the latest optimal path without having to return to a previous state and start again. This continuous re-planning method minimizes the additional time cost and improves the overall task completion efficiency. It can be seen that the above obstacle avoidance process integrates a global motion planning and a local reactive obstacle avoidance strategy, ensuring that the robotic arm can maintain smooth motion and quickly resume on the re-planned new trajectory, avoiding intermittent motion caused by local extreme value problems, and can effectively improve the reaction speed and path smoothness of the robotic arm in a dynamic environment while ensuring human-machine safety, thereby optimizing the production efficiency of human-machine collaboration.

[0053] The above general description and the following description are only exemplary and explanatory and are not used to limit the present disclosure. Description of the Drawings

[0054] One or more embodiments are exemplarily illustrated by corresponding drawings. These exemplary illustrations and the drawings do not constitute limitations on the embodiments. Elements with the same reference numerals in the drawings are shown as similar elements. The drawings do not constitute a scale limitation, and among them:

[0055] Figure 1It is a schematic diagram of a manipulator motion control system provided by an embodiment of the present disclosure;

[0056] Figure 2 It is a schematic flow diagram of a manipulator motion control method provided by an embodiment of the present disclosure;

[0057] Figure 3 It is a schematic flow diagram of another manipulator motion control method provided by an embodiment of the present disclosure;

[0058] Figure 4 It is a distance calculation and collision state model between superquadrics provided by an embodiment of the present disclosure;

[0059] Figure 5 It is a kinematic model based on local exponential product provided by an embodiment of the present disclosure;

[0060] Figure 6 It is a schematic diagram of pose calculation of a human equivalent superquadric provided by an embodiment of the present disclosure;

[0061] Figure 7 It is a global planner incorporating an operability force ellipsoid provided by an embodiment of the present disclosure;

[0062] Figure 8 It is a schematic diagram of the integration of global and local planning in configuration space provided by an embodiment of the present disclosure;

[0063] Figure 9 It is a schematic diagram of the integration of global and local planning in Euclidean space provided by an embodiment of the present disclosure;

[0064] Figure 10 It is a schematic diagram of a manipulator motion control device provided by an embodiment of the present disclosure. Detailed implementation manners

[0065] In order to be able to understand the features and technical content of the embodiments of the present disclosure in more detail, the implementation of the embodiments of the present disclosure will be described in detail below with reference to the accompanying drawings. The accompanying drawings are for reference and illustration purposes only and are not used to limit the embodiments of the present disclosure. In the following technical description, for the sake of explanation, numerous details are provided to give a thorough understanding of the disclosed embodiments. However, one or more embodiments may still be implemented without these details. In other cases, well-known structures and devices may be shown in a simplified manner to simplify the drawings.

[0066] In the description, claims and the above drawings of the embodiments of the present disclosure, terms such as "first" and "second" are used to distinguish similar objects, and do not necessarily describe a specific order or sequence. It should be understood that the data used in this way can be interchanged under appropriate circumstances, so as to implement the embodiments of the present disclosure described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusion.

[0067] Unless otherwise specified, the term "plurality" means two or more.

[0068] In the embodiments of the present disclosure, the character " / " indicates that the objects before and after are in an "or" relationship. For example, A / B means: A or B.

[0069] The term "and / or" is an associative relationship describing an object, indicating that three relationships can exist. For example, A and / or B means: A or B, or, the three relationships of A and B.

[0070] The term "corresponding" may refer to an associative relationship or a binding relationship. A corresponding to B means that there is an associative relationship or a binding relationship between A and B.

[0071] The embodiments of the present disclosure provide a manipulator motion control system, as Figure 1 shown, the manipulator motion control system includes a manipulator and a manipulator motion control device. The manipulator motion control device can control the manipulator to move along a corresponding trajectory, and an operator can cooperate with the manipulator.

[0072] Combined with the manipulator motion control device (hereinafter referred to as the control device) provided by the embodiments of the present disclosure, the embodiments of the present disclosure provide a manipulator motion control method. Combined Figure 2 shown, the manipulator motion control method includes the following steps:

[0073] S201, the control device determines an initial trajectory of the manipulator based on the current state of the manipulator, task constraints, the force vector at the end effector, and the target pose, and controls the manipulator to execute the initial trajectory.

[0074] S202, during the movement of the manipulator, the control device detects the collision risk between the manipulator and an obstacle.

[0075] S203, when the control device determines that there is a collision risk between the manipulator and an obstacle, it calculates the obstacle avoidance motion parameters for the manipulator to move away from the obstacle, and controls the manipulator to execute the obstacle avoidance motion parameters for moving away from the obstacle.

[0076] S204, during the execution of the obstacle avoidance motion parameters for moving away from the obstacle, the control device repeatedly executes a new trajectory planning process to obtain a new trajectory.

[0077] In S205, when the control device determines that the collision risk between the robotic arm and the obstacle has disappeared, it stops planning a new trajectory for the robotic arm and controls the robotic arm to execute the latest planned new trajectory.

[0078] In the embodiments of the present disclosure, S202 to S205 can be executed cyclically until the end effector of the robotic arm reaches the target position.

[0079] The robotic arm motion control method provided by the embodiments of the present disclosure introduces a real-time trajectory adjustment mechanism, enabling the robotic arm to dynamically calculate and execute obstacle avoidance motion parameters away from the obstacle when detecting a collision risk, rather than simply stopping or returning to a safe distance. This fast response mechanism ensures that the robotic arm can still work efficiently in the face of unforeseen environmental changes and reduces the delay caused by obstacle avoidance. At the same time, by continuously re-planning a new trajectory while the robotic arm executes the obstacle avoidance action, once it is confirmed that the collision risk is lifted, the robotic arm can continue to execute the task according to the latest optimal path without having to return to a previous state and start again. This method of continuous re-planning minimizes the additional time cost and improves the overall task completion efficiency. It can be seen that the above obstacle avoidance process integrates the global motion planning and the local reactive obstacle avoidance strategy, ensuring that the robotic arm can maintain smooth motion and quickly resume on the re-planned new trajectory, avoiding intermittent motion caused by local extreme value problems, and can effectively improve the reaction speed and path smoothness of the robotic arm in a dynamic environment while ensuring human-robot safety, thereby optimizing the production efficiency of human-robot collaboration.

[0080] In some embodiments, the control device is deployed with a distance calculation and collision detection module and a collision-free motion generator module. Among them, the distance calculation and collision detection module is a module based on IMUs (Inertial Measurement Units MotionCapture System).

[0081] The distance calculation and collision detection module can detect the collision risk between the robotic arm and the obstacle during the motion of the robotic arm. The collision-free motion generator module can determine the initial trajectory of the robotic arm based on the current state of the robotic arm, task constraints, the force vector at the end effector, and the target pose, control the robotic arm to execute the initial trajectory; when determining that there is a collision risk between the robotic arm and the obstacle, calculate the obstacle avoidance motion parameters for the robotic arm to move away from the obstacle, and control the robotic arm to execute the obstacle avoidance motion parameters for moving away from the obstacle; repeat the new trajectory planning process during the execution of the obstacle avoidance motion parameters for moving away from the obstacle to obtain a new trajectory; when determining that the collision risk between the robotic arm and the obstacle has disappeared, stop planning a new trajectory for the robotic arm and control the robotic arm to execute the latest planned new trajectory.

[0082] In some embodiments, determining the initial trajectory of the robotic arm based on the current state of the robotic arm, task constraints, the force vector at the end effector, and the target pose includes: determining the initial pose of the end effector based on the current state of the robotic arm and the force vector at the end effector; determining the initial trajectory of the robotic arm based on the task constraints of the robotic arm, as well as the force vector, the initial pose, and the target pose at the end effector.

[0083] By determining the initial pose of the end effector based on the current state of the robotic arm and the force vector at the end effector, and combining the task constraints, the force vector, the initial pose, and the target pose to determine the initial trajectory of the robotic arm. This method ensures the accuracy and adaptability of the initial trajectory planning and can better respond to different task requirements and environmental conditions.

[0084] In some embodiments, during the movement of the robotic arm, detecting the collision risk between the robotic arm and an obstacle includes: constructing an equivalent model of the robotic arm and the obstacle, where the equivalent model is represented by a superquadric surface; during the movement of the robotic arm, calculating the minimum distance between the equivalent models of the robotic arm and the obstacle, and this minimum distance is used to characterize the collision risk between the robotic arm and the obstacle.

[0085] Introducing the superquadric surface as a geometric representation method for the equivalent models of the robotic arm and the obstacle. This representation method is applicable not only to static obstacles but also covers dynamic obstacles (such as operators), thus simplifying the design and implementation of collision detection, path planning, and obstacle avoidance algorithms. It provides a more accurate and consistent modeling of the interaction between the robotic arm and the environment.

[0086] In some embodiments, when it is determined that there is a collision risk between the robotic arm and an obstacle, calculating the obstacle avoidance motion parameters for the robotic arm to move away from the obstacle and controlling the robotic arm to execute the obstacle avoidance motion parameters includes: when it is determined that there is a collision risk between the robotic arm and an obstacle, calculating the joint angular velocity of the robotic arm to move away from the obstacle and controlling the robotic arm to execute the joint angular velocity to move away from the obstacle.

[0087] In some embodiments, the collision risk between the robotic arm and an obstacle is characterized by the minimum distance between the equivalent models of the robotic arm and the obstacle. When it is determined that there is a collision risk between the robotic arm and an obstacle, calculating the joint angular velocity of the robotic arm to move away from the obstacle and controlling the robotic arm to execute the joint angular velocity to move away from the obstacle includes: when the minimum distance is less than the safety distance threshold, calculating the joint angular velocity of the robotic arm to move away from the obstacle and controlling the robotic arm to execute the joint angular velocity to move away from the obstacle.

[0088] In the embodiments of the present disclosure, the collision risk between the robotic arm and the obstacle is characterized by the minimum distance, and a safety distance threshold is set to more accurately determine the collision risk. When the minimum distance is less than the safety distance threshold, the safety strategy is activated to adjust the motion trajectory of the robot to reduce the occurrence of collisions. This effectively improves the safety and reliability of human-robot collaboration and reduces the possibility of accidents.

[0089] In some embodiments, calculating the joint angular velocity of the robotic arm away from the obstacle includes: calculating a first joint angular velocity corresponding to the gravitational vector of the joint, where the gravitational vector is used to guide the end effector towards the target position; calculating a second joint angular velocity corresponding to the repulsive vector of the joint, where the repulsive vector is used to guide the robotic arm to avoid the obstacle; and calculating the joint angular velocity of the robotic arm away from the obstacle based on the first joint angular velocity and the second joint angular velocity.

[0090] By calculating the first joint angular velocity corresponding to the gravitational vector and the second joint angular velocity corresponding to the repulsive vector, and calculating the joint angular velocity of the robotic arm away from the obstacle based on the two, such a design enables the robotic arm to avoid the obstacle while attracting the end effector towards the target position, achieving smooth and efficient path adjustment.

[0091] Combined with the robotic arm motion control device provided in the embodiments of the present disclosure (hereinafter referred to as the device), the embodiments of the present disclosure provide another robotic arm motion control method. As shown in Figure 3 The robotic arm motion control method includes the following steps:

[0092] S301, the control device determines the initial pose of the end effector based on the current state of the robotic arm and the force vector at the end effector.

[0093] S302, the control device determines the initial trajectory of the robotic arm based on the task constraints of the robotic arm, as well as the force vector, the initial pose, and the target pose at the end effector.

[0094] S303, the control device controls the robotic arm to execute the initial trajectory.

[0095] S304, the control device constructs an equivalent model of the robotic arm and the obstacle.

[0096] In the embodiments of the present disclosure, the equivalent model is represented by a superquadric surface.

[0097] S305, the control device calculates the minimum distance between the equivalent model of the robotic arm and the obstacle during the motion of the robotic arm.

[0098] In the embodiments of the present disclosure, the minimum distance is used to characterize the collision risk between the robotic arm and the obstacle.

[0099] S306. When the minimum distance is less than the safety distance threshold, the control device calculates a first joint angular velocity corresponding to the gravitational vector of the joint and a second joint angular velocity corresponding to the repulsive vector of the joint.

[0100] In the embodiments of the present disclosure, the gravitational vector is used to guide the end effector to move towards the target position, and the repulsive vector is used to guide the robotic arm to avoid obstacles.

[0101] S307. The control device calculates a joint angular velocity for the robotic arm to move away from the obstacle based on the first joint angular velocity and the second joint angular velocity.

[0102] S308. The control device controls the robotic arm to execute the joint angular velocity for moving away from the obstacle.

[0103] S309. During the execution of the joint angular velocity for moving away from the obstacle, the control device repeatedly executes the new trajectory planning process to obtain a new trajectory.

[0104] S310. When the control device determines that the collision risk between the robotic arm and the obstacle has disappeared, it stops planning the new trajectory of the robotic arm and controls the robotic arm to execute the latest planned new trajectory.

[0105] In the embodiments of the present disclosure, S305 to S310 can be executed cyclically until the end effector of the robotic arm reaches the target position.

[0106] The embodiments of the present disclosure introduce a method based on the geometric representation of superquadrics for creating equivalent models of the robotic arm and obstacles. By using superquadrics to represent these equivalent models, the geometric characteristics and spatial states of the robotic arm and obstacles are described in a unified manner. This method is applicable not only to static obstacles but also to dynamic obstacles, including the activities of operators. This method provides an integrated framework that enables the interaction between the robotic arm and all obstacles in the environment to be modeled more precisely and consistently. Under this framework, both fixed static obstacles and dynamic obstacles whose positions change over time (such as operators) can be characterized by the same mathematical form, thus simplifying the design and implementation of collision detection, path planning, and obstacle avoidance algorithms.

[0107] In Figure 1 shows the equivalent models of the robotic arm and the operator, which are enveloped by superquadrics. The equivalent models of the robotic arm and the operator both include multiple superquadrics. Figure 1 in to are the superquadrics of the respective links of the robotic arm. Figure 1 in to are the superquadrics of the respective body parts of the operator.

[0108] In the embodiments of the present disclosure, while maintaining the symmetric characteristics of the regular quadratic surface, the superquadric surface allows a more flexible set of shapes, and its display expression is as shown in Formula 1 below:

[0109]

[0110] In Formula 1, τ = (a1, a2, a3, ε1, ε2) is the parameter vector of the superquadric surface. The parameters a1, a2, and a3 are the scaling ratios of the superquadric surface on the x, y, and z coordinate axes respectively, and ε1, ε2 are the deformation parameters. Let ε ∈ (0, 2), which represents a convex equivalent model. As the magnitudes of ε1 and ε2 change, the superquadric surface can represent 3D graphics such as cubes, spheres, cylinders, etc. The above parameters are stored in the form of a data structure.

[0111] η is the polar angle, η ∈ [-π / 2, π / 2], that is, the angle between the line connecting the origin to a point on the spherical surface and the positive direction of the z-axis. ω is the azimuth angle, ω ∈ [-π, π], that is, the projection line of the line connecting the origin to a point on the spherical surface on the x - y plane.

[0112] The above superquadric surface SQ τ is described in the local coordinate system. To determine the distance between a pair of objects, the quadratic surface needs to be transformed to be described in a global coordinate system. As shown in Figure 1 , the superquadric surface of the robotic arm is described in the local coordinate system {0}, and the superquadric surface of the operator is described in the local coordinate system {H}. To determine the distance between the robotic arm and the operator, the quadratic surface needs to be transformed to be described in a global coordinate system {W}. The quadratic surface SQ τ is expressed in the global coordinate system {W} as shown in Formula 2: ---Formula 2.

[0113] In the above Formula 2, is the superquadric surface in the global coordinate system, and T(R, P) is the rotation matrix and the translation vector define the homogeneous transformation matrix.

[0114] Using the above modeling method, each superquadric surface is completely defined by the parameters {τ, R, P}, which fully represents its geometric shape and spatial pose. Among them, τ is stored offline and is known. The equivalent model of the manipulator consists of 7 superquadric surfaces, and the equivalent model of the operator consists of 16 superquadric surfaces. In theory, we need to detect the distances between 112 pairs of superquadric surfaces. However, excluding the cases that never occur, we only need to calculate the distances between 50 pairs of superquadric surfaces. Therefore, this representation method can reduce the number of distance calculations performed while meeting the modeling accuracy, balance the complexity and accuracy of collision detection to a certain extent, and is conducive to improving the efficiency of collision detection. Based on the above modeling, during the movement of the manipulator, the control device calculates the minimum distance between the equivalent models of the manipulator and the obstacle.

[0115] In the embodiment of the present disclosure, calculating the minimum distance between the equivalent models of the manipulator and the obstacle includes: for the equivalent models of the manipulator and the obstacle, expanding one equivalent model into a Minkowski sum envelope surface and degenerating the other equivalent model into an equivalent point; obtaining the Euclidean distance as the minimum distance between the equivalent models of the manipulator and the obstacle by calculating the Euclidean distance between the Minkowski sum envelope surface and the equivalent point.

[0116] When calculating the minimum distance between the equivalent models of the manipulator and the obstacle, using the method of the Minkowski sum envelope surface, expanding one equivalent model into an envelope surface and degenerating the other into a point, and then calculating the Euclidean distance. This method improves the speed and efficiency of calculating the minimum distance, and at the same time ensures a high degree of accuracy, which helps to monitor in real time and respond quickly to potential collision risks.

[0117] In the embodiment of the present disclosure, the minimum distance method based on the closed-form Minkowski sum can be used to calculate the minimum distance between each superquadric surface of the equivalent models of the manipulator and the obstacle. The core idea is to solve the closed-form Minkowski sum between each pair of superquadric surfaces. As shown in Figure 4 SQ τ1 represents a superquadric surface of the manipulator, and SQ τ2 represents a superquadric surface of the operator. Expand the superquadric surface SQ τ2 of the operator into a Minkowski sum envelope surface P1 is the equivalent point of the superquadric surface SQ τ2 of the operator. Degenerate the superquadric surface SQ τ1 of the manipulator into an equivalent point P2. is a point on the Minkowski sum envelope surface Solve the distance from the equivalent point P2 to the Minkowski sum envelope surface The Euclidean distance is used as the true distance between the manipulator and the equivalent model of the obstacle. It can be understood that when the minimum Euclidean distance is obtained, the minimum distance between the manipulator and the equivalent model of the obstacle is obtained.

[0118] In the embodiments of the present disclosure, in order to solve the Euclidean distance between two superquadric surfaces, the minimum distance point vector parameters are obtained by solving the objective function shown in Formula 3 below

[0119]

[0120] respectively represent the homogeneous transformation matrices of the superquadric surfaces where η ∈ (-π / 2, π / 2), ω ∈ (-π, π), is the non-normalized gradient at the closest distance point from the equivalent point P2 to the Minkowski sum envelope surface where

[0121] The solution of the above formula is a non-linear optimization problem. Therefore, the present disclosure adopts the L-BFGS algorithm to construct the inverse of the approximate Hessian matrix, which overcomes the problems of storing and calculating matrices in high-dimensional cases and can quickly converge to the local optimal solution. The superiority of this method is significantly better than the convex optimization and common normal concept based on implicit expression. This is attributed to the fact that the closed-form expression of the Minkowski sum reduces both the variables and dimensions of the optimization objective function by half, accelerating the convergence process of the L-BFGS algorithm.

[0122] After the minimum distance point vector parameters are solved, the minimum distance between the two superquadric surfaces is calculated according to the minimum distance point vector parameters through the following Formula 4:

[0123]

[0124] Introducing the minimum distance point vector parameters into Formula 5, the collision state can be obtained:

[0125]

[0126] When , there is no collision between the objects. On the contrary, a collision occurs. If a collision occurs between the objects, d min represents the minimum distance to separate from the collision; if no collision occurs between the objects, d min represents the minimum distance to approach the collision.

[0127] In order to quickly obtain the collision distance and collision state, based on the above method, next, the methods for updating the spatial states of the manipulator and the operator model will be introduced separately. First, the local exponential product formula is introduced to establish the kinematic model of the equivalent model, describing the positions and velocities of the respective superquadrics as the manipulator configuration changes.

[0128] In Figure 5 , Link1 to Link4 represent 4 links, {0} is the base coordinate system, usually fixed on the ground or the workbench. {1}, {2}, {3} and {T} are the local coordinate systems of the respective links, corresponding to different parts of the manipulator. q1, q2, q3, q4 represent the four joints respectively. L1, L2, L3, L4 represent the lengths of the respective links. is a specific reference coordinate system used to describe the position of a specific point or object. Combining Figure 5 shown, in the local POE (Partial Order Expansion) formula, all joint axes are represented in their respective local coordinate systems. Using this property, each superquadric is fixed in a local coordinate system.

[0129] First, assume that the local coordinate system {0} is stationary relative to the global coordinate system {W}, and the tool coordinate system {T} is attached to the end effector of the manipulator. Let link i and link i - 1 be connected by joint i with an angle of q i , and the coordinate system {i - 1} represents the coordinate system of link {i - 1}. The pose of the coordinate system {i} relative to the coordinate system {i - 1} is described by the homogeneous matrix T i-1,i (q i ) ∈ SE(3), satisfying the following formula 6:

[0130]

[0131] In the above formula 6, T i-1,i (0) ∈ SE(3) represents the initial pose of link i relative to link i - 1,, T i-1,i (0) satisfies the following formula 7:

[0132]

[0133] In formula 7, R i-1,i (0) ∈ SO(3), are respectively the initial attitude and position of link i relative to link i - 1.

[0134] Here, [S i ∈ se(3) is the screw axis S i = (ω i , v i ) ∈ R6 The kinematic screw of [S i satisfies the following formula 8:

[0135]

[0136] In formula 8, where ω i , v i respectively represent the angular velocity and the linear velocity.

[0137] Based on the above definitions, the expression of the forward kinematic model of the manipulator based on the local exponential product is shown in the following formula 9:

[0138]

[0139] In formula 9, Q = [q1, q2,..., q n T , representing the joint angle variables.

[0140] In the embodiments of the present disclosure, let the center point of the equivalent model be fixedly connected to the link i - 1, and use the coordinate system to describe its kinematic parameters such as position and velocity. We define the homogeneous transformation matrix of the coordinate system relative to the link coordinate system {i - 1} The kinematic expression of the equivalent model is shown in formula 10:

[0141]

[0142] Wherein,

[0143] Therefore, only by tracking the joint state of the manipulator feedback in real time by the encoder, the state parameters of any superquadric surface can be calculated. This method can uniformly handle serial manipulators with different joint types (rotational and translational joints) and degrees of freedom.

[0144] In the embodiments of the present disclosure, a wearable motion capture system based on IMUs can be used to obtain the positions of each joint, and the pose of the superquadric surface is updated according to this position information. The signals of the IMU sensors are continuously updated and integrated into the biomechanical model of the human body. To clearly describe the human body space state, combined with Figure 6 as shown, two types of coordinate systems can be defined: 1) The central coordinate system of the equivalent model of the operator 2) The segment coordinate system of the equivalent model of the operator {B j}(j = 0, 1,..., 14).

[0145] In the embodiments of the present disclosure, in order to estimate the changes in the direction and position of the equivalent model, the pose of the equivalent model and the segment pose B of the human body model​j T is associated through a homogeneous transformation matrix, and the expression for their association is shown in Equation 10:

[0146]

[0147] In Equation 10, is the homogeneous transformation matrix of the equivalent geometric body relative to the segment coordinates, and this matrix is known. B j T represents the pose of the segment coordinate system {B j}(j = 1, 2, …, 14) relative to the human body coordinate system {H} (local coordinate system). Since the initial transformation between the IMU sensor and the body part is unknown, the expression of B j T in the coordinate system {H} must perform a calibration process.

[0148] The embodiments of the present disclosure use these states to update the poses of each superquadric convex body, update the Minkowski sum between each model in real time, and dynamically monitor the distances between the manipulator and dynamic obstacles (such as operators) and between the manipulator and static obstacles (environment). When the minimum distance between them is less than the safety distance threshold, a safety strategy is activated to adjust the robot's motion trajectory to reduce the occurrence of collisions. The effective numerical calculation of the above method allows a fast enough cycle time to update the distance.

[0149] In the embodiments of the present disclosure, the collision-free motion generator module integrates global planning and local reactive control. The collision-free motion generator module can avoid obstacles locally in real time while reaching a given target position. When the pose of the dynamic obstacle changes, this module generates an obstacle avoidance trajectory in real time. Since the target position of the manipulator is dynamically measured by the hand-eye camera, this module considers the current state of the manipulator and quickly replans the trajectory to the target in the background.

[0150] The collision-free motion generator module includes a global planner, which can plan a trajectory that meets certain conditions according to the current environmental information and state. Due to the manipulator carrying a load, the global planner needs to consider the force characteristics of the manipulator to enhance its stability. To improve the force characteristics of the planned trajectory, we introduce the manipulability force ellipsoid into the global planner. As Figure 7 shown, for the same end pose, different joint states are shown in the inverse kinematics of the manipulator, and each joint state corresponds to a different manipulability force ellipsoid. In addition, for the same manipulability force ellipsoid, the closer the external force F in a given direction is to the main axis, the easier it is to apply an external force to the outside world. Therefore, based on the above properties, the joint state with the optimal force characteristics can be determined. In Figure 7 , q1, q2, …, qn represent the joints of the manipulator.

[0151] In the embodiments of the present disclosure, a global motion planner is used to find a trajectory that satisfies the constraints and has approximately optimal force characteristics. In Figure 8 , Subfigure a represents the global planning in the initial stage and the reactive obstacle avoidance stage, and Subfigure b represents the reactive obstacle avoidance of the planned initial trajectory according to the information of dynamic obstacles and dynamic measurement targets. As shown in Subfigure a of Figure 8 , this algorithm is a global planner based on bidirectional sampling. As described above, the control device determines the initial trajectory of the manipulator based on the current state of the manipulator, task constraints, the force vector at the end effector, and the target pose. Here, the global planner inputs the current state of the manipulator, task constraints, the force vector at the end effector, and the target pose to determine the manipulator, and determines the best route for the optimal pose of the manipulated object. This best route is the initial trajectory. From the starting point Q Start and the end point Q Tartget-I or Q Tartget-II simultaneously generate two trees T a and T b , until these two trees are connected, so as to find a path (initial trajectory) from the starting point to the end point. Q rand represents a random sampling point, respectively represent the nearest neighbor points of the random sampling points of the trees T a and T b , respectively represent the newly generated points of the trees T a and T b , C obs represents a static obstacle, and Q e represents the current state of the joint. Based on the above method, this planner is used for the global planning in the initial stage and the reactive obstacle avoidance stage. As shown in Subfigure b of Figure 8 , the initial trajectory adjusts the current trajectory in real time according to the dynamic obstacles and dynamically measured targets described in the configuration space to obtain an obstacle avoidance trajectory, and re-plans to the termination configuration. The obstacle avoidance trajectory includes the trajectory during obstacle avoidance and the new trajectory after obstacle avoidance.

[0152] Combined with Figure 9 shown, the embodiments of the present disclosure are based on the concept of artificial potential field, and use the assumed repulsive force vector and attractive-repulsive force vector to calculate the joint velocity of the manipulator. In Figure 9 , represents the minimum distance between the i-th superquadric surface in the manipulator and the superquadric surface of the static obstacle, represents the minimum distance between the n-th superquadric surface in the manipulator and the superquadric surface of the dynamic obstacle. Target-I and Target-II are two different target positions.

[0153] ​The embodiments of the present disclosure consider an obstacle avoidance strategy for multiple obstacles simultaneously and obtain the joint velocities for continuous motion. During obstacle avoidance, an attractive force vector acts on the end effector and attracts the end effector to move towards the task target; the repulsive force vector is determined by the distance between the equivalent geometric primitive of the robotic arm and other obstacles. Under the action of the attractive and repulsive force vectors, the robotic arm avoids dynamic obstacles in real time, and its end effector moves towards the target as much as possible. As described above, when the minimum distance is less than the safety distance threshold, the control device calculates the first joint angular velocity corresponding to the attractive force vector of the joint and calculates the second joint angular velocity corresponding to the repulsive force vector of the joint. Based on the first joint angular velocity and the second joint angular velocity, the joint angular velocity for the manipulator to move away from the obstacle is calculated.

[0154] In some embodiments, calculating the first joint angular velocity corresponding to the attractive force vector of the joint includes: determining the current pose of the end effector, calculating the attractive force vector of the joint based on the current pose and the target pose of the end effector; calculating the first joint angular velocity corresponding to the attractive force vector.

[0155] In some embodiments, calculating the attractive force vector of the joint based on the current pose and the target pose of the end effector includes: determining the proportional gain coefficient and the positive definite coefficient matrix related to the attractive force vector; calculating the pose difference value between the current pose and the target pose of the end effector; calculating the attractive force vector of the joint based on the proportional gain coefficient, the positive definite coefficient matrix, and the pose difference value between the current pose and the target pose of the end effector.

[0156] In the embodiments of the present disclosure, in the Cartesian space, the attractive force vector can be expressed by Equation 11 as:

[0157]

[0158] In Equation 11, is the attractive force vector. is the attractive force vector of the proportional gain coefficient. is the positive definite coefficient matrix. is the pose or position of any task target. is the pose of the end effector of the manipulator during obstacle avoidance.

[0159] In some embodiments, calculating the first joint angular velocity corresponding to the attractive force vector includes: determining the damping factor associated with the attractive force vector and the Jacobian matrix associated with the end effector; calculating the first joint angular velocity corresponding to the attractive force vector based on the attractive force vector, the damping factor associated with the attractive force vector, and the Jacobian matrix associated with the end effector.

[0160] Appropriately selecting the damping factor can avoid the system from being overly sensitive or sluggish, while the Jacobian matrix is used to map the velocity in Cartesian space to the velocity in joint space, which helps to achieve precise motion control.

[0161] In the embodiments of the present disclosure, the damping least squares technique is adopted to obtain the corresponding joint velocity. Specifically, the first joint angular velocity corresponding to the gravitational vector can be calculated using the following formula 12:

[0162]

[0163] In formula 12, represents the first joint angular velocity corresponding to the gravitational vector. is the damping factor associated with the attractive force vector. is the Jacobian matrix associated with the end effector.

[0164] In some embodiments, calculating the second joint angular velocity corresponding to the repulsive force vector of the joint includes: calculating the repulsive force vector of the joint based on the minimum distance between the equivalent models of the robotic arm and the obstacle; calculating the second joint angular velocity corresponding to the repulsive force vector based on the repulsive force vector.

[0165] In some embodiments, calculating the repulsive force vector of the joint based on the minimum distance between the equivalent models of the robotic arm and the obstacle includes: comparing the minimum distance between the equivalent models of the robotic arm and the obstacle with the activation distance; calculating the repulsive force vector of the joint based on the comparison result of the minimum distance between the equivalent models of the robotic arm and the obstacle and the activation distance.

[0166] By comparing the minimum distance with the activation distance, it is determined whether it is necessary to calculate the repulsive force vector. This step increases the intelligence and flexibility of the obstacle avoidance mechanism. The obstacle avoidance action is only initiated when approaching the obstacle, which not only saves computing resources but also improves the reaction speed.

[0167] In the embodiments of the present disclosure, the obstacle avoidance motion of each link of the robotic arm can be described by the equation as formula 13:

[0168] In formula 13, represents the repulsive force vector acting on the k-th joint of the robotic arm, is the second joint angular velocity for avoiding multiple obstacles simultaneously, is the Jacobian matrix.

[0169] In the embodiments of the present disclosure, in Cartesian space, the repulsive force vector acting on the k-th joint of the robotic arm can be calculated using the following formula 14:

[0170]

[0171] In Equation 14, d cz is the activation distance, representing the minimum distance between the n-th superquadric surface in the manipulator and the superquadric surfaces of all obstacles (i.e., calculating the minimum distance between the equivalent models of the manipulator and the obstacles), and n k is the unit vector of the minimum distance between the n-th superquadric surface of the manipulator and the superquadric surfaces of all obstacles, and n k can be expressed by Equation 15:

[0172]

[0173] In some embodiments, calculating the second joint angular velocity corresponding to the repulsive force vector based on the repulsive force vector includes: determining the damping factor associated with the repulsive force vector and the Jacobian matrix associated with the equivalent model of the manipulator; calculating the second joint angular velocity corresponding to the repulsive force vector based on the repulsive force vector, the damping factor associated with the repulsive force vector, and the Jacobian matrix associated with the equivalent model of the manipulator.

[0174] Appropriately selecting the damping factor can avoid the system from being overly sensitive or sluggish, while the Jacobian matrix is used to map the velocity in Cartesian space to the velocity in joint space, which helps to achieve precise motion control.

[0175] When the minimum distance between the equivalent models of the manipulator and the obstacles is less than the safety distance threshold, the motion of the manipulator will be subjected to the corresponding repulsive force. Since the obstacle avoidance strategy only needs to move along the direction of the line connecting the critical point and the nearest point on the obstacle, the repulsive force vector and the second joint angular velocity corresponding to the repulsive force vector have the relationship shown in Equation 16 below:

[0176]

[0177] Wherein,

[0178]

[0179] In the above Equation 16, is the repulsive force vector defined by multiple obstacles, is the Jacobian matrix associated with the equivalent model of the manipulator. The above formula can be applied to both the case of obstacle avoidance between a single link of the manipulator and a single obstacle and the case of simultaneous obstacle avoidance between multiple links of the manipulator and multiple obstacles.

[0180] In the embodiments of the present disclosure, the damping least squares technique is used to obtain the corresponding joint velocity, and specifically, the second joint angular velocity corresponding to the repulsive force vector can be calculated by the following Equation 17:

[0181]

[0182] In Equation 17, represents the second joint angular velocity corresponding to the repulsive force vector. λ rep represents the damping factor associated with the repulsive force vector. is the Jacobian matrix associated with the equivalent model of the robotic arm.

[0183] As shown above, the control device calculates the joint angular velocity for the robotic arm to move away from the obstacle based on the first joint angular velocity and the second joint angular velocity. Specifically, the joint angular velocity for the robotic arm to move away from the obstacle can be calculated by the following Equation 18:

[0184]

[0185] In Equation 18, represents the joint angular velocity, represents the first joint angular velocity, represents the second joint angular velocity.

[0186] In some embodiments, a new trajectory planning process includes: determining the current pose of the end effector; determining a new trajectory for the robotic arm based on the target pose of the robotic arm, task constraints, the force vector at the end effector, and the current pose of the end effector.

[0187] The above new trajectory planning process allows the robotic arm to dynamically adjust its motion path in the face of environmental changes, improving the efficiency and accuracy of task completion.

[0188] In some embodiments, determining a new trajectory for the robotic arm based on the target pose of the robotic arm, task constraints, the force vector at the end effector, and the current pose of the end effector includes: calculating the gravitational vector of the joint based on the current pose and the target pose of the end effector; determining the predicted pose of the end effector based on the current pose of the end effector and the gravitational vector of the joint; determining a new trajectory for the robotic arm based on the target pose of the robotic arm, task constraints, the force vector at the end effector, and the predicted pose of the end effector.

[0189] The above process enhances the forward-looking and adaptability of trajectory planning, enabling the robotic arm to continuously and efficiently work in a changing environment, ultimately achieving the goal of optimizing production efficiency.

[0190] Combined with Figure 10As shown in the figure, an embodiment of the present disclosure provides another robotic arm motion control device 1000. The robotic arm motion control device 1000 includes a processor 1001 and a memory 1002. Optionally, the device 1000 may further include a communication interface 1003 and a bus 1004. Among them, the processor 1001, the communication interface 1003, and the memory 1002 can communicate with each other through the bus 1004. The communication interface 1003 can be used for information transmission. The processor 1001 can call the logical instructions in the memory 1002 to execute the robotic arm motion control method in the above embodiment.

[0191] In addition, when the logical instructions in the above-mentioned memory 1002 are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium.

[0192] As a computer-readable storage medium, the memory 1002 can be used to store software programs and computer-executable programs, such as the program instructions / modules corresponding to the methods in the embodiments of the present disclosure. The processor 1001 executes functional applications and data processing by running the program instructions / modules stored in the memory 1002, that is, implements the robotic arm motion control method in the above embodiment.

[0193] The memory 1002 may include a program storage area and a data storage area. Among them, the program storage area can store an operating system and application programs required for at least one function; the data storage area can store data created according to the use of the terminal device, etc. In addition, the memory 1002 may include high-speed random access memory and may also include non-volatile memory.

[0194] An embodiment of the present disclosure provides a computer-readable storage medium storing computer-executable instructions, and the computer-executable instructions are set to execute the above-mentioned robotic arm motion control method.

[0195] The technical solution of the embodiment of the present disclosure can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes one or more instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the method described in the embodiment of the present disclosure. The foregoing storage medium may be a non-transitory storage medium, such as: a USB flash drive, a mobile hard disk, a read-only memory (ROM, Read-Only Memory), a random access memory (RAM, Random Access Memory), a magnetic disk, or an optical disc, etc., which are various media that can store program codes.

[0196] The above description and the accompanying drawings fully illustrate the embodiments of the present disclosure, enabling those skilled in the art to practice them. Other embodiments may include structural, logical, electrical, process, and other changes. The embodiments only represent possible variations. Unless explicitly required, the individual components and functions are optional, and the order of operations may vary. Parts and features of some embodiments may be included in or replace parts and features of other embodiments. Moreover, the terms used in this application are only for describing the embodiments and do not limit the claims. As used in the description of the embodiments and the claims, unless the context clearly indicates otherwise, the singular forms "a", "an", and "the" are intended to also include the plural forms. Similarly, as used in this application, the term "and / or" refers to any and all possible combinations of one or more of the associated listed items. Additionally, when used in this application, the term "comprise" and its variants "comprises" and / or "comprising" etc. mean the presence of the stated features, wholes, steps, operations, elements, and / or components, but do not exclude the presence or addition of one or more other features, wholes, steps, operations, elements, components, and / or groups of these. Without further limitation, an element defined by the statement "comprising an..." does not exclude the presence of additional identical elements in the process, method, or device comprising the element. Herein, each embodiment may focus on the differences from other embodiments, and the same or similar parts among the embodiments may be referred to each other. For the methods, products, etc. disclosed in the embodiments, if they correspond to the method parts disclosed in the embodiments, the relevant parts may refer to the description of the method parts.

[0197] Those skilled in the art can realize that the units and algorithm steps of each example described in combination with the embodiments disclosed herein can be implemented by electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are executed in a hardware or software manner may depend on the specific application and design constraints of the technical solution. The skilled person may use different methods for each specific application to implement the described functions, but such implementation should not be considered to exceed the scope of the embodiments of the present disclosure. The skilled person can clearly understand that for the convenience and brevity of description, the specific working processes of the systems, devices, and units described above can refer to the corresponding processes in the foregoing method embodiments, and will not be elaborated herein.

[0198] In the embodiments disclosed in this document, the disclosed methods, products (including but not limited to devices, equipment, etc.) can be implemented in other ways. For example, the device embodiments described above are merely illustrative. For example, the division of the units can be merely a logical function division. In actual implementation, there can be other division methods. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Additionally, the displayed or discussed couplings or direct couplings or communication connections to each other can be through some interfaces. The indirect couplings or communication connections of devices or units can be in electrical, mechanical, or other forms. The units described as separate components may or may not be physically separated. The components displayed as units may or may not be physical units, that is, they can be located in one place or distributed to multiple network units. Some or all of the units can be selected according to actual needs to implement this embodiment. Additionally, in the embodiments of this disclosure, the various functional units can be integrated in one processing unit, or each unit can exist physically separately, or two or more units can be integrated in one unit.

[0199] The flowcharts and block diagrams in the accompanying drawings illustrate the possible architectures, functions, and operations of systems, methods, and computer program products according to the embodiments of this disclosure. In this regard, each block in the flowchart or block diagram can represent a module, a program segment, or a part of code that contains one or more executable instructions for implementing the specified logical function. In some alternative implementations, the functions marked in the blocks can also occur in a different order than that marked in the accompanying drawings. For example, two consecutive blocks can actually be executed substantially in parallel, and they can sometimes be executed in the reverse order, which can depend on the functions involved. In the descriptions corresponding to the flowcharts and block diagrams in the accompanying drawings, the operations or steps corresponding to different blocks can also occur in a different order than that disclosed in the description. Sometimes, there is no specific order between different operations or steps. For example, two consecutive operations or steps can actually be executed substantially in parallel, and they can sometimes be executed in the reverse order, which can depend on the functions involved. Each block in the block diagram and / or flowchart, as well as the combination of blocks in the block diagram and / or flowchart, can be implemented by a dedicated hardware-based system for performing the specified functions or actions, or can be implemented by a combination of dedicated hardware and computer instructions.

Claims

1. A method for controlling motion of an operating arm, characterized in that: include: Determine the initial trajectory of the manipulator based on the current state of the manipulator, task constraints, the force vector at the end effector, and the target posture, and control the manipulator to execute the initial trajectory; Detecting the risk of collision between the manipulator arm and obstacles during the movement of the manipulator arm; When it is determined that there is a risk of collision between the operating arm and the obstacle, obstacle avoidance motion parameters of the operating arm moving away from the obstacle are calculated, and the operating arm is controlled to execute the obstacle avoidance motion parameters moving away from the obstacle; Repeating the new trajectory planning process to obtain a new trajectory during the execution of obstacle avoidance motion parameters away from the obstacle; When it is determined that the collision risk between the operating arm and the obstacle disappears, the planning of the new trajectory of the operating arm is stopped, and the operating arm is controlled to execute the latest planned new trajectory.

2. The method for controlling the motion of an operating arm according to claim 1, characterized in that: Determine the initial trajectory of the manipulator based on the current state of the manipulator, the task constraints, the force vector at the end effector, and the target pose, including: Determine the initial pose of the end effector based on the current state of the manipulator and the force vector at the end effector; The initial trajectory of the manipulator is determined based on the task constraints of the manipulator, as well as the force vector at the end effector, the initial pose, and the target pose.

3. The method for controlling the motion of an operating arm according to claim 1, characterized in that: During the movement of the manipulator arm, the risk of collision between the manipulator arm and obstacles is detected, including: Construct an equivalent model of the manipulator and the obstacle, and the equivalent model is represented by a super quadratic surface; During the movement of the manipulator arm, the minimum distance between the equivalent model of the manipulator arm and the obstacle is calculated, and the minimum distance is used to characterize the collision risk between the manipulator arm and the obstacle.

4. The method for controlling the motion of an operating arm according to claim 3, characterized in that: Calculate the minimum distance between the manipulator and the equivalent model of the obstacle, including: For the equivalent models of the manipulator and the obstacle, one equivalent model is expanded into a Minkowski and envelope surface, and the other equivalent model is degenerated into an equivalent point; By calculating the Euclidean distances of the Minkowski and envelope surfaces to the equivalent points, the Euclidean distance is obtained as the minimum distance between the equivalent models of the manipulator and the obstacle.

5. The method for controlling the motion of an operating arm according to claim 1, characterized in that: When it is determined that there is a risk of collision between the operating arm and the obstacle, obstacle avoidance motion parameters of the operating arm moving away from the obstacle are calculated, and the operating arm is controlled to execute the obstacle avoidance motion parameters to move away from the obstacle, including: when it is determined that there is a risk of collision between the operating arm and the obstacle, the joint angular velocity of the operating arm moving away from the obstacle is calculated, and the joint angular velocity of the operating arm moving away from the obstacle is controlled.

6. The method for controlling the motion of an operating arm according to claim 1, characterized in that: The collision risk between the manipulator and the obstacle is characterized by the minimum distance between the equivalent models of the manipulator and the obstacle; When it is determined that there is a risk of collision between the operating arm and the obstacle, the joint angular velocity of the operating arm moving away from the obstacle is calculated, and the joint angular velocity of the operating arm moving away from the obstacle is controlled, including: when the minimum distance is less than a safety distance threshold, the joint angular velocity of the operating arm moving away from the obstacle is calculated, and the joint angular velocity of the operating arm moving away from the obstacle is controlled.

7. The method for controlling the motion of an operating arm according to claim 1, characterized in that: Calculate the joint angular velocity of the manipulator away from the obstacle, including: Calculate the first joint angular velocity corresponding to the gravity vector of the joint; Calculate the second joint angular velocity corresponding to the repulsive force vector of the joint; The joint angular velocity of the operating arm moving away from the obstacle is calculated based on the first joint angular velocity and the second joint angular velocity.

8. The method for controlling the motion of an operating arm according to claim 7, characterized in that: Calculate the first joint angular velocity corresponding to the gravity vector of the joint, including: Determine the current pose of the end effector, and calculate the gravity vector of the joint based on the current pose of the end effector and the target pose; Calculate the angular velocity of the first joint corresponding to the gravity vector.

9. The method for controlling the motion of an operating arm according to claim 8, characterized in that: Calculate the gravity vector of the joint based on the current pose and target pose of the end effector, including: Determine the proportional gain coefficients and positive definite coefficient matrix associated with the gravity vector; Calculate the posture difference between the current posture of the end effector and the target posture; The gravity vector of the joint is calculated based on the proportional gain coefficient, the positive definite coefficient matrix, and the posture difference value between the current posture and the target posture of the end effector.

10. The method for controlling the motion of an operating arm according to claim 8, characterized in that: Calculate the first joint angular velocity corresponding to the gravity vector, including: Determine the damping factor associated with the gravity vector and the Jacobian matrix associated with the end effector; The first joint angular velocity corresponding to the gravity vector is calculated based on the gravity vector, the damping factor associated with the gravity vector, and the Jacobian matrix associated with the end effector.

11. The method for controlling the motion of an operating arm according to claim 7, characterized in that: Calculate the second joint angular velocity corresponding to the repulsive force vector of the joint, including: Calculate the repulsive force vector of the joint based on the minimum distance between the equivalent model of the manipulator and the obstacle; A second joint angular velocity corresponding to the repulsive force vector is calculated based on the repulsive force vector.

12. The method for controlling the motion of an operating arm according to claim 11, characterized in that: Based on the minimum distance between the equivalent model of the manipulator and the obstacle, the repulsive force vector of the joint is calculated, including: The minimum distance between the equivalent model of the manipulator and the obstacle is compared with the activation distance; Based on the comparison of the minimum distance between the equivalent model of the manipulator and the obstacle and the activation distance, the repulsive force vector of the joint is calculated.

13. The method for controlling the motion of an operating arm according to claim 11, characterized in that: Calculating the second joint angular velocity corresponding to the repulsive force vector based on the repulsive force vector includes: Determine the damping factor associated with the repulsive force vector and the Jacobian matrix associated with the equivalent model of the manipulator; The second joint angular velocity corresponding to the repulsive force vector is calculated based on the repulsive force vector, the damping factor associated with the repulsive force vector, and the Jacobian matrix associated with the equivalent model of the manipulator.

14. The method for controlling the motion of an operating arm according to claim 1, characterized in that: A new trajectory planning process includes: Determine the current pose of the end effector; The new trajectory of the manipulator is determined based on the target pose of the manipulator, the task constraints, the force vector at the end effector, and the current pose of the end effector.

15. The method for controlling the motion of an operating arm according to claim 14, characterized in that: Determine the new trajectory of the manipulator based on the target pose of the manipulator, the task constraints, the force vector at the end effector, and the current pose of the end effector, including: Calculate the gravity vector of the joint based on the current pose and target pose of the end effector; Determine a predicted pose of the end effector based on the current pose of the end effector and the gravity vector of the joint; The new trajectory of the manipulator is determined based on the target pose of the manipulator, the task constraints, the force vector at the end effector, and the predicted pose of the end effector.

16. A motion control device for an operating arm, comprising a processor and a memory storing program instructions, characterized in that: The processor executes the operating arm motion control method according to any one of claims 1 to 14.

17. A motion control system for an operating arm, characterized in that: It comprises an operating arm and the operating arm motion control device as claimed in claim 16, wherein the operating arm motion control device is communicatively connected with the operating arm.

18. A storage medium, characterized in that: The storage medium stores computer program instructions, and when the computer program instructions are executed by the processor, the operating arm motion control method according to any one of claims 1 to 15 is executed.

Images