Layered motion planning method and device for robot arm in dynamic environment

Abstract

The application discloses a layered motion planning method and equipment for a mechanical arm in a dynamic environment and belongs to the technical field of mechanical arm motion planning. A target position of the mechanical arm is set, a collision-free initial global path of each joint of the mechanical arm from a current position to the target position is planned, a continuous and smooth planned motion trajectory of the mechanical arm is generated, and joint angular velocity control amounts of the mechanical arm are generated. When it is detected that a dynamic obstacle is contained in a moving range of the mechanical arm on the basis of avoiding a static obstacle, a dynamic obstacle avoidance framework is constructed in combination with a dynamic control barrier function DCBF, signal timing logic STL is introduced into the dynamic obstacle avoidance framework for task constraint, and the mechanical arm is controlled to dynamically avoid the obstacle. The method is simple in steps, high in execution efficiency, and capable of guiding the mechanical arm to avoid the obstacle and complete a task in real time.

Description

Technical Field

[0001] This invention belongs to the field of robotic arm motion planning technology, specifically relating to a hierarchical motion planning method and device for robotic arms in dynamic environments. Background Technology

[0002] Traditional robotic arm path planning methods, including graph search-based algorithms and sampling-based RRT algorithms, both have significant limitations. Graph search-based algorithms require discretization of the environment, which can lead to the curse of dimensionality in the robotic arm's joint space, resulting in an exponential increase in computational cost. Furthermore, the planned paths often ignore the robotic arm's kinematic constraints, leading to poor path feasibility. While sampling-based algorithms can handle high-dimensional spaces, their random sampling characteristics result in low path quality, numerous redundant nodes, and a lack of real-time response mechanisms for dynamic obstacles.

[0003] For dynamic obstacle avoidance, traditional methods such as the artificial potential field method suffer from local minima. While the control obstacle function can adjust the control input in real time through safety constraints, constructing an effective obstacle function and efficiently solving optimization problems in high-dimensional nonlinear systems such as robotic arms remains a significant challenge. Furthermore, robotic arm motion planning must meet the temporal task requirements of industrial scenarios. Traditional methods rely on heuristic rules to describe tasks, lacking formal modeling capabilities and making it difficult to guarantee the accuracy and verifiability of task execution.

[0004] The prior art disclosed in CN113580146A describes a real-time obstacle avoidance method for a robotic arm that integrates dynamic systems and model predictive control. This method combines dynamic systems and model predictive control to handle real-time obstacle avoidance problems in dynamic environments. It can plan the obstacle avoidance trajectory in real time based on the position information of moving obstacles and control the robotic arm using model predictive control, considering multiple linear or nonlinear constraints. However, it relies on neural networks for learning, resulting in long training times, high costs, and uncertain obstacle avoidance performance. Summary of the Invention

[0005] To address the shortcomings of existing technologies, a method and device for hierarchical motion planning of robotic arms in dynamic environments are provided. This method has low computational requirements and simple steps, and can ensure that the obstacle avoidance process meets the time-series task specifications. At the same time, it controls the robotic arm to avoid both static and dynamic obstacles during movement.

[0006] To achieve the above objectives, the present invention provides a layered motion planning method for a robotic arm in a dynamic environment, in which a robotic arm with m joints avoids both static and dynamic obstacles during its movement.

[0007] The specific steps are as follows:

[0008] Step 1: Construct the forward and inverse kinematics model and Jacobian matrix of the robotic arm using the improved DH parameter method, set the target position of the robotic arm, and consider the position of static obstacles. Based on the fast walking tree algorithm, plan the collision-free initial global path from the current position of each joint of the robotic arm to the target position.

[0009] Step 2: Use cubic spline interpolation to smooth the initial global path generated by the fast moving tree algorithm, generating a continuous and smooth planned robotic arm motion trajectory;

[0010] Step 3: Based on the planned robotic arm trajectory, use a linear quadratic regulator to generate the angular velocity control values ​​for each joint of the robotic arm;

[0011] Step 4: When a dynamic obstacle is detected within the robotic arm's movement range, based on the avoidance of static obstacles, a dynamic obstacle avoidance framework is constructed by combining the dynamic control obstacle function. Signal timing logic is introduced into the dynamic obstacle avoidance framework for task constraints, controlling the robotic arm to perform dynamic obstacle avoidance. Specifically:

[0012] When there are no dynamic obstacles and no signal timing logic tasks during the tracking process of the linear quadratic regulator, the linear quadratic regulator controls the robotic arm to execute the robotic arm's motion trajectory.

[0013] When dynamic obstacles exist during the linear quadratic controller's tracking process, a dynamic control obstacle function is calculated based on these obstacles. When signal-sequential logic tasks also exist during the tracking process, the signal-sequential logic paradigm is used to describe the timing tasks. The atomic predicates are obtained by parsing the signal-sequential logic paradigm, and a time-varying control obstacle function is designed based on these atomic predicates. The robotic arm's motion trajectory is then replanned by combining the signal-sequential logic timing constraints and the time-varying control obstacle function. This ensures that the robotic arm can not only safely avoid dynamic obstacles in a dynamic environment but also strictly follow the logical execution path planning of the signal-sequential task. Using the dynamic control obstacle function and the time-varying control obstacle function as constraints, a quadratic programming problem is solved to obtain the angular velocity control quantities of each joint of the robotic arm that allow it to avoid dynamic obstacles while fulfilling the timing constraints, enabling the robotic arm to reach the target position without collision.

[0014] Furthermore, in step 1, a global path is generated using the Fast Moving Tree Algorithm (FMT*), taking into account both the robot arm's own constraints and safe distance constraints from static obstacles:

[0015] In the joint space of the robotic arm, the FMT* algorithm needs to connect sampling points to form a search tree and find the optimal path from the starting position to the target position. When performing single-step expansion, the FMT* algorithm uses the geometric envelope method to perform collision detection after finding the optimal connection for the single step, ensuring that the path does not collide with any obstacle before adding the path to the path tree. An envelope model is constructed, using cylinders of different radii to enclose the joints and links of the robotic arm, and using spheres to enclose static and dynamic obstacles, and using the envelope model to perform collision detection.

[0016] Furthermore, the initial global path generated by the FMT* algorithm is smoothed using cubic spline interpolation as follows:

[0017] The initial global path corresponding to the i-th joint of the robotic arm is represented as a sequence of n path nodes. Let q represent the angle of the robotic arm joint. A piecewise cubic spline interpolation method is used to fit the entire path, generating a smooth and continuous trajectory to connect these discrete path nodes. For each pair of adjacent path nodes of the i-th joint... and The interval between Construct a piecewise cubic polynomial :

[0018] ,

[0019] In the formula, Indicates the current time. These are polynomial coefficients, obtained through path nodes. Solving for position constraints, continuity constraints, and boundary conditions, where j represents the j-th path node;

[0020] By integrating all path nodes By applying positional constraints, continuity constraints, and boundary conditions, a system of equations is obtained to solve for the coefficients of piecewise cubic polynomials between all two adjacent path nodes. Solving this system of equations determines the coefficients of each piecewise polynomial. ;

[0021] Connecting all the piecewise cubic polynomials forms a curve that not only passes precisely through all path nodes. Moreover, the smooth trajectory with continuous changes in both speed and acceleration meets the requirements for angular velocity control of each joint of the robotic arm.

[0022] Furthermore, the process of generating the angular velocity control quantities for each joint of the robotic arm using the linear quadratic regulator (LQR) is as follows:

[0023] Let the state variables of the robotic arm's motion in three-dimensional space be represented by the angles of each joint of the robotic arm. ,in Let the angular velocity of the i-th robotic arm joint be denoted as ; let the angular velocity control value of the robotic arm joint be denoted as . The state-space equation of the robotic arm is expressed as follows:

[0024] ,

[0025] In the formula, the zero matrix With the identity matrix The angular velocity control quantity u of the robotic arm joints and the state quantity The following equation applies between them:

[0026] ,

[0027] The objective function J of LQR is minimized as follows:

[0028] ,

[0029] In the formula, Q is a positive semi-definite matrix used to penalize state errors; R is a positive definite matrix used to penalize the energy consumption of the control quantities of the angular velocity control quantities of each joint of the robotic arm. By adjusting the weights of Q and R, the state tracking accuracy of the robotic arm and the energy consumption of the angular velocity control quantities of each joint of the robotic arm can be balanced.

[0030] Using LQR to solve for the joint angular velocity control of the robotic arm: The solution of the algebraic Riccati equation ARE is used as the control quantity of LQR. Assume that there exists a positive definite matrix P that satisfies the ARE equation:

[0031] ,

[0032] The optimal feedback gain matrix K is represented as follows:

[0033] ,

[0034] Therefore, the optimal control values ​​for the angular velocities of each joint of the robotic arm are obtained as shown in the following formula:

[0035] ,

[0036] In the formula, K is the LQR gain matrix, q init With q goal Let q represent the current pose of the robotic arm and the target pose, respectively. When the robotic arm is performing trajectory tracking, if the current time point is less than the maximum time point of the piecewise cubic spline interpolation, q... goal Let q be the joint angle value corresponding to the next moment; if the current moment is greater than the maximum moment of the piecewise cubic spline interpolation, then q goal This represents the piecewise cubic spline interpolation pose corresponding to the maximum time.

[0037] Furthermore, the workflow for constructing the Dynamic Obstacle Control Function (DCBF) within the dynamic obstacle avoidance framework, which avoids both static and dynamic obstacles, is as follows:

[0038] The joints and links of the robotic arm are simplified into cylinders of different radii, and the dynamic obstacle is simplified into a sphere. A DCBF (Distributed Distance Boundary Function) is constructed for each link. By constraining the safe distance, the DCBF ensures that the links of the robotic arm maintain a safe distance from the dynamic obstacle during movement. The expression is as follows:

[0039] ,

[0040] In the formula, D is the distance from the center of the dynamic obstacle sphere to the axis of the connecting rod envelope cylinder. The radius of the dynamic obstacle sphere Inner diameter of the cylinder enveloped by the connecting rod The sum, h represents the dynamic control barrier function;

[0041] As long as the robotic arm's DCBF initial state This ensures the forward invariance of the robotic arm, guaranteeing that the cylinder representing the link will not contact the sphere representing the dynamic obstacle, thus preventing the robotic arm from entering a dangerous collision state. The first derivative of DCBF... The following conditions must be met:

[0042] ,

[0043] In the formula, The coefficients for the convergence rate are given; further expansion yields the constraints of the robotic arm:

[0044] ,

[0045] In the formula, The current velocity of the dynamic obstacle. The first derivative of the distance D;

[0046] Let the center of the dynamic obstacle sphere be... The cylindrical link of the robotic arm has joints at both ends. and Calculate using the following formula exist Projection vectors and vectors on the axis The ratio of its own length z:

[0047] ,

[0048] In the formula, Representative from point to ; Representative from point to ; Representative from point to The magnitude of the vector;

[0049] The position of the foot O of the perpendicular representing the center of the sphere, which is a dynamic obstacle, is determined by the ratio z. Then the foot O of the perpendicular falls on the axis of the cylinder representing the link of the robotic arm. Above; otherwise, the foot of the perpendicular O lies on the axis of the cylinder representing the robotic arm link. On the extension line, if This represents the center of the sphere from the dynamic obstacle. To the joint end of the cylinder representing the robotic arm link The closest point is at [location name], and the distance is calculated using the following formula: ;like This represents the center of the dynamic obstacle sphere. To the cylindrical joint end representing the robotic arm link The closest point is at [location name], and the distance is calculated using the following formula: ;

[0050] Based on the ratio z, the distance D under different conditions is represented as follows:

[0051] ,

[0052] Based on the vector representation rules and combined with the forward kinematics of the robotic arm, the first derivatives of each vector in distance D can be obtained through the expression of distance D:

[0053] ,

[0054] ,

[0055] ,

[0056] In the formula, J i V represents the Jacobian matrix representing the velocity of the i-th joint of the robotic arm. o The instantaneous moving speed of the dynamic obstacle is represented by u, which represents the angular velocity control value of the robotic arm joint.

[0057] According to the vector differentiation rule, we can calculate... Expressions for different ratios z:

[0058] when hour:

[0059] ,

[0060] when hour:

[0061] ,

[0062] when hour:

[0063] ,

[0064] The relative position of the dynamic obstacle and the link is determined by the range of values ​​for the ratio z, and the constraints of the robotic arm are defined as follows: This is transformed into a linear inequality constraint on the angular velocity control quantity u of the robotic arm joints, as shown in the following equation:

[0065] ,

[0066] By rearranging the linear inequality constraint of the angular velocity control quantity u of the robotic arm joint, we can obtain... Solve the quadratic programming QP problem in the form of [formula missing]. and Let represent the coefficient matrix and the right-hand side vector of the inequality constraint, respectively. and The expressions are shown in the following formulas:

[0067] ,

[0068] .

[0069] When there are dynamic obstacles and timing tasks during the tracking process of the linear quadratic regulator (LQR), the timing tasks are described using the Signal-Time Logic (STL) paradigm. STL accurately expresses the behavioral constraints of the robotic arm in the time dimension through the combination of timing operators and predicates. The timing operators of STL include the always operator, the end operator, and the until operator.

[0070] By combining STL constraints and TVCBF during path planning, the robotic arm can not only avoid obstacles in dynamic environments but also strictly execute path planning according to task logic. The specific construction steps of TVCBF are as follows:

[0071] Task decomposition and predicate extraction: Syntax tree parsing is performed on the temporal tasks described using the STL paradigm to extract the atomic predicates from the STL-described temporal tasks. :

[0072] ,

[0073] In the formula, Here, x is the predicate function of STL, and u is the angular velocity control quantity of each joint of the robotic arm.

[0074] Design modulation functions within the time window for different timing operators in STL For the existence operator in STL normal form, construct a decrement function:

[0075] ,

[0076] In the formula, a and b are time points, and c is the scaling factor. To ensure the stable offset of the robotic arm, where t is time, an incrementing function is constructed for the start-stop operator in the STL paradigm:

[0077] ,

[0078] In the formula, k and To ensure Adjustment parameters that satisfy the inequality conditions. The offset to ensure the stability of the robotic arm; e is a constant, with a value of 2.71828;

[0079] Design modulation function outside the time window The goal is to design a piecewise modulation function outside the time window to ensure continuous control of the angular velocity of each joint of the robotic arm. This function aims to gradually weaken the control effect of the TVCBF (Transmission-to-Conversion-Frequency) on the robotic arm to zero outside the time window. Specifically, the modulation function value should decay over time and remain consistently greater than zero. The modulation function outside the time window is then constructed. It is expressed as follows:

[0080] ,

[0081] In the formula and To adjust the parameters to meet the requirements To ensure ;

[0082] By merging modulation functions within the time window Modulation function outside the time window Obtain the modulation function in the entire time domain :

[0083] ,

[0084] In the formula, For time window The modulation function value within, Time window The modulation function value;

[0085] For the existence operator and the always operator, TVCBF is represented as:

[0086] ,

[0087] In the formula, This is the Euclidean distance from the current position to the target position of the robotic arm link;

[0088] For the until operator in the STL paradigm, the TVCBF is constructed by smoothing the minimum function combination for l cases as follows:

[0089] ,

[0090] In the formula, l represents the number of timing constraints. For the time-varying control barrier function corresponding to the signal-sequential logic paradigm up to the operator; This represents a smooth approximation of the minimum value operation;

[0091] Constructing a time-varying control barrier function Time-varying safety set:

[0092] ,

[0093] In the formula, Define the domain of joint angles for an m-DOF robotic arm. For safety conditions, i.e., satisfying STL constraints;

[0094] The time-varying control barrier function (TVCBF) must satisfy the following equation to ensure a safe set. Within a specified time range, the robotic arm exhibits forward invariance:

[0095] ,

[0096] To construct the QP problem, we rearrange the terms in the above inequality and rewrite it as follows: Format:

[0097] ,

[0098] In the formula, and Let the coefficient matrix and the right-hand side vector of the inequality constraint be represented respectively:

[0099] ,

[0100] ,

[0101] The constraints of DCBF and TVCBF are combined with the optimal control values ​​of the angular velocities of each joint of the robotic arm generated by the LQR controller. By combining these methods, the robotic arm can simultaneously satisfy STL timing constraints and dynamic obstacle avoidance requirements in a dynamic environment, thus constructing the QP problem as shown in the following equation:

[0102] ,

[0103] In the formula, H represents the weight matrix and f represents the cost vector. By solving the QP problem through the above formula, the final angular velocity control quantity u of the robotic arm joints that simultaneously satisfies the STL timing constraints and dynamic obstacle avoidance requirements is obtained, so that the robotic arm can complete the specified timing task while avoiding collisions with dynamic obstacles during trajectory tracking.

[0104] A computer device includes a processor and a memory, the processor being electrically connected to the memory, the memory being used to store instructions and data, and the processor being used to execute a hierarchical motion planning method for a robotic arm in a dynamic environment.

[0105] A computer-readable storage medium storing a computer program configured to implement a hierarchical motion planning method for a robotic arm in a dynamic environment when invoked by a processor.

[0106] Beneficial Effects: This invention constructs a global path planning framework based on a fast walking tree algorithm, combines axis-aligned bounding boxes and bounding spheres for collision detection, and employs cubic spline interpolation to smooth the path, significantly improving search efficiency and path quality. It exhibits faster path convergence and better smoothness compared to traditional algorithms. During the robotic arm's trajectory tracking, a dynamic control obstacle function and a linear quadratic regulator are used to generate control inputs, transforming dynamic obstacle avoidance into a safety constraint optimization problem. The control quantity is adjusted by solving a quadratic programming problem in real time, ensuring the robotic arm maintains a safe distance from dynamic obstacles and avoiding oscillations or stagnation. Combining signal timing logic and time-varying control obstacle functions transforms complex temporal tasks into computable constraint optimization problems, ensuring the obstacle avoidance process meets temporal task specifications and improving practical applicability. Hierarchical motion planning decouples global planning, dynamic obstacle avoidance, and temporal constraints, supporting independent optimization and flexible combinations. It is applicable to robotic arms with different degrees of freedom and complex environmental scenarios, significantly improving generalization capabilities in industrial applications. Attached Figure Description

[0107] Figure 1 This is a flowchart of a layered motion planning method for a robotic arm in a dynamic environment, according to an embodiment of the present invention.

[0108] Figure 2 This is a schematic diagram of the envelope model in the hierarchical motion planning method for a robotic arm in a dynamic environment according to an embodiment of the present invention; wherein (a) is a schematic diagram of the robotic arm after the envelope method is processed, and (b) is a schematic diagram of the sphere composed of dynamic obstacles after the envelope method is processed.

[0109] Figure 3 This is a schematic diagram of the simulation environment in the hierarchical motion planning method for a robotic arm under dynamic conditions according to an embodiment of the present invention; wherein (a) is a schematic diagram of the robotic arm simulation environment; and (b) is a schematic diagram of the robotic arm obstacle avoidance process.

[0110] Figure 4 This is a schematic diagram of the calculation of collision distance D in the layered motion planning method for a robotic arm in a dynamic environment according to an embodiment of the present invention; wherein (a) is a schematic diagram in which the foot of the center of the sphere representing the dynamic obstacle falls on the axis of the cylinder representing the robotic arm link, and (b) is a schematic diagram in which the foot of the center of the sphere representing the dynamic obstacle falls on the extension line of the axis of the cylinder representing the robotic arm link.

[0111] Figure 5 This is a simulation diagram of the hierarchical motion planning method for a robotic arm in a dynamic environment in a time-series task according to Embodiment 1 of the present invention; wherein (a) represents a schematic diagram of the safe distance D between the robotic arm and the dynamic obstacle throughout the motion, and (b) represents a schematic diagram of the angular velocity of each joint of the robotic arm as input to the control of the robotic arm throughout the motion.

[0112] Figure 6 This is a simulation diagram of the layered motion planning method for a robotic arm in a dynamic environment under a time-series task according to Embodiment 2 of the present invention; wherein (a) represents a schematic diagram of the safe distance D between the robotic arm and the dynamic obstacle throughout the motion, and (b) represents a schematic diagram of the angular velocity of each joint of the robotic arm controlled by the robotic arm throughout the motion. Detailed Implementation

[0113] The embodiments of the present invention will now be explained in more detail with reference to the accompanying drawings.

[0114] like Figure 1 As shown, this invention discloses a layered motion planning method for a robotic arm in a dynamic environment, in which a robotic arm with m joints avoids both static and dynamic obstacles during its movement.

[0115] The specific steps are as follows:

[0116] Step 1: Construct the forward and inverse kinematics models and Jacobian matrix of the robotic arm using the improved DH parameter method, set the target position of the robotic arm, and consider the positions of static obstacles. Based on the Fast Moving Tree Algorithm (FMT*), plan the collision-free initial global path from the current position of each joint of the robotic arm to the target position; simultaneously considering the constraints of the robotic arm itself and the safe distance constraints with dynamic obstacles.

[0117] In the joint space of the robotic arm, the FMT* algorithm needs to connect sampling points to form a search tree and find the optimal path from the starting position to the target position. When performing single-step expansion, the FMT* algorithm uses the geometric envelope method to perform collision detection after finding the optimal single-step connection, ensuring that the path does not collide with any obstacle before adding the path to the path tree. The robotic arm is enveloped with cylinders, and static obstacles and dynamic obstacles are enveloped with spheres, and collision detection is performed with the help of the envelope model.

[0118] like Figure 2As shown, an envelope model is created for the robotic arm, and a spherical envelope model is created for dynamic obstacles to achieve collision detection; the schematic diagram of the robotic arm envelope method is shown below. Figure 2 As shown in (a) above, the schematic diagram of the sphere formed by the obstacle Paul is as follows. Figure 2 As shown in (b);

[0119] Step 2: The initial global path generated by the fast moving tree algorithm is smoothed using cubic spline interpolation to generate a continuous and smooth planned robotic arm motion trajectory.

[0120] The initial global path corresponding to the i-th joint of the robotic arm is represented as a sequence of n path nodes. Let q represent the angle of the robotic arm joint. A piecewise cubic spline interpolation method is used to fit the entire path, generating a smooth and continuous trajectory to connect these discrete path nodes. For each pair of adjacent path nodes of the i-th joint... and The interval between Construct a piecewise cubic polynomial for:

[0121] ,

[0122] In the formula, Indicates the current time. These are polynomial coefficients, obtained through path nodes. The solution is obtained by considering position constraints, continuity constraints, and boundary conditions, where j represents the j-th path node.

[0123] By integrating all path nodes By applying positional constraints, continuity constraints, and boundary conditions, a system of equations is obtained to solve for the coefficients of piecewise cubic polynomials between all two adjacent path nodes. Solving this system of equations determines the coefficients of each piecewise polynomial. ;

[0124] Connecting all the piecewise cubic polynomials forms a curve that not only passes precisely through all path nodes. Moreover, the smooth trajectory with continuous changes in both speed and acceleration meets the requirements for angular velocity control of each joint of the robotic arm.

[0125] Step 3: Based on the planned robotic arm trajectory, use a linear quadratic regulator (LQR) to generate the angular velocity control values ​​for each joint of the robotic arm:

[0126] Let the state variables of the robotic arm's motion in three-dimensional space be represented by the angles of each joint of the robotic arm. ,in Let the angular velocity of the i-th robotic arm joint be denoted as ; let the angular velocity control value of the robotic arm joint be denoted as . The state-space equation of the robotic arm is expressed as follows:

[0127] ,

[0128] In the formula, the zero matrix With the identity matrix The angular velocity control quantity u of the robotic arm joints and the state quantity The relationship between them is as shown in the following formula:

[0129] ,

[0130] The objective function J of LQR is minimized as follows:

[0131] ,

[0132] In the formula, Q is a positive semi-definite matrix used to penalize state errors; R is a positive definite matrix used to penalize the energy consumption of the control quantities of the angular velocity control quantities of each joint of the robotic arm. By adjusting the weights of Q and R, the state tracking accuracy of the robotic arm and the energy consumption of the angular velocity control quantities of each joint of the robotic arm can be balanced.

[0133] Using LQR to solve for the joint angular velocity control of the robotic arm: The solution of the algebraic Riccati equation ARE is used as the control quantity of LQR. Suppose there exists a positive definite matrix P that satisfies the ARE equation as shown below:

[0134] ,

[0135] The optimal feedback gain matrix K is represented as follows:

[0136] ,

[0137] Therefore, the optimal control values ​​for the angular velocities of each joint of the robotic arm are obtained as shown in the following formula:

[0138] ,

[0139] In the formula, K is the LQR gain matrix, q init With q goal These represent the current pose and the target pose of the robotic arm, respectively.

[0140] Step 4: When a dynamic obstacle is detected within the robotic arm's movement range, based on avoiding static obstacles, a dynamic obstacle avoidance framework is constructed using the Dynamic Obstacle Avoidance Function (DCBF). The dynamic obstacle avoidance framework incorporates Signal-Temporal Logic (STL) for task constraints, controlling the robotic arm to perform dynamic obstacle avoidance. The specific details are as follows:

[0141] When there are no dynamic obstacles and no signal timing logic tasks during LQR tracking, the LQR controls the robotic arm to execute the robotic arm motion trajectory generated in step 2.

[0142] When dynamic obstacles exist during LQR tracking, the dynamic control obstacle function DCBF is calculated based on the dynamic obstacles.

[0143] like Figure 3 As shown, specifically, Figure 3 (a) is a schematic diagram of the robotic arm simulation environment, and (b) is a schematic diagram of the robotic arm obstacle avoidance process, which includes the robotic arm and a dynamic obstacle. The initial posture and target posture of the robotic arm are set as follows: and There is a sphere with a radius of 0.2m between the initial attitude and the target attitude, and the initial center coordinates are... The speed of movement is Dynamic obstacles;

[0144] The joints and links of the robotic arm are simplified into cylinders of different radii, and the dynamic obstacles are simplified into spheres. A dynamic control obstacle function (DCBF) is constructed for each link. By constraining the safe distance, the DCBF ensures that the links of the robotic arm maintain a safe distance from the dynamic obstacles during movement. The expression is as follows:

[0145] ,

[0146] In the formula, D is the distance from the center of the dynamic obstacle sphere to the axis of the connecting rod envelope cylinder. The radius of the dynamic obstacle sphere Inner diameter of the cylinder enveloped by the connecting rod The sum; h represents the dynamic control barrier function;

[0147] To ensure the safety of the robotic arm, the minimum distance between the link and the ball is set to be greater than or equal to the sum of their inner diameters, denoted as: To ensure the forward invariance of the robotic arm, that is, as long as the initial state of the robotic arm's dynamic control barrier function DCBF is maintained... The robotic arm will not enter a dangerous collision state, and the first derivative of DCBF... The following conditions must be met:

[0148] ,

[0149] In the formula, Let be the coefficient of the convergence rate; further expansion yields the following equation:

[0150] ,

[0151] In the formula, v o The current velocity of the dynamic obstacle. The first derivative of the distance D;

[0152] Let the center of the dynamic obstacle sphere be... The two joints at both ends of one link of the robotic arm are and Calculate using the following formula exist Projection vectors and vectors on the axis The ratio of its own length z:

[0153] ,

[0154] In the formula, Representative from point to ; Representative from point to ; Representative from point to The magnitude of the vector;

[0155] The position of the foot O of the perpendicular representing the center of the sphere, which is a dynamic obstacle, is determined by the value of the ratio z. Then the foot O of the perpendicular falls on the axis of the cylinder representing the link of the robotic arm. Above; otherwise, the foot of the perpendicular O lies on the axis of the cylinder representing the robotic arm link. On the extension line, if This represents the center of the sphere from the dynamic obstacle. To the joint end of the cylinder representing the robotic arm link The closest point is at [location name], and the distance is calculated using the following formula: ;like This represents the center of the obstacle's dynamic obstacle sphere. To the cylindrical joint end representing the robotic arm link The closest point is at [location name], and the distance is calculated using the following formula: ;like Figure 4 As shown in (a) and (b) in the figure.

[0156] Based on the ratio z, the expressions for distance D under different conditions are as follows:

[0157] ,

[0158] Based on the vector representation rules and combined with the forward kinematics of the robotic arm, the first derivatives of each vector in distance D are obtained through the expression of distance D, as shown in the following equation:

[0159] ,

[0160] ,

[0161] ,

[0162] In the formula, J i The Jacobian matrix representing the velocity of the i-th joint of the robotic arm, v o Represents the instantaneous speed of a dynamic obstacle;

[0163] According to the vector differentiation rule, we can calculate... Expressions for different ratios z:

[0164] when hour:

[0165] ,

[0166] when hour:

[0167] ,

[0168] when hour:

[0169] ,

[0170] The relative position of the dynamic obstacle and the link is determined by the range of values ​​for the ratio z, and the constraints of the robotic arm are defined as follows: This is transformed into a linear inequality constraint on the angular velocity control quantity u of the robotic arm joints, as shown in the following equation:

[0171] ,

[0172] To solve the quadratic programming problem, the linear inequality constraint on the angular velocity control variable u of the robotic arm joints is rearranged into... In form, and Let represent the coefficient matrix and the right-hand side vector of the inequality constraint, respectively. and The expressions are shown in the following formulas:

[0173] ,

[0174] .

[0175] When there are signal timing logic tasks during LQR tracking, the STL paradigm is used to describe the timing tasks. The STL paradigm is parsed to obtain atomic predicates. Based on the atomic predicates, a time-varying control obstacle function (TVCBF) is designed. Combining the STL timing constraints and TVCBF, the robot arm's motion trajectory is re-planned. Using DCBF and TVCBF as constraints, a quadratic programming QP problem is solved to obtain the angular velocity control quantities of each joint of the robot arm that allow it to avoid dynamic obstacles while fulfilling timing constraints, enabling the robot arm to reach the target position without collision.

[0176] The specific construction steps of the time-varying control barrier function TVCBF are as follows:

[0177] Task decomposition and predicate extraction: Syntax tree parsing is performed on the temporal tasks described using the STL paradigm to extract the atomic predicates from the STL-described temporal tasks. :

[0178] ,

[0179] In the formula, Here, x is the STL predicate function, and u is the control input angular velocity control quantity of each joint of the robotic arm.

[0180] Design modulation functions within the time window for different timing operators in STL For the existence operator in STL normal form, construct a decrement function:

[0181] ,

[0182] In the formula, a and b are time points, and c is the scaling factor. To ensure the stable offset of the robotic arm, where t is time, an incrementing function is constructed for the start-stop operator in the STL paradigm:

[0183] ,

[0184] In the formula, k and To ensure Adjustment parameters that satisfy the inequality conditions. To ensure the stable offset of the robotic arm;

[0185] Design modulation function outside the time window Design a piecewise modulation function outside the time window to ensure continuous control of the angular velocity of each joint of the robotic arm. The control effect of the CBF on the robotic arm should gradually weaken to zero outside the time window; that is, the modulation function value should decay over time and remain always greater than zero. Construct the modulation function outside the time window according to the design requirements. As shown in the following formula:

[0186] ,

[0187] In the formula and To adjust the parameters to meet the requirements To ensure ;

[0188] The full-time domain modulation function is obtained by merging the modulation functions within and outside the time window. :

[0189] ,

[0190] In the formula, For time window The modulation function value within, Time window The modulation function value;

[0191] For the existence operator and the always operator, TVCBF is represented as:

[0192] ,

[0193] In the formula, This is the Euclidean distance from the current position to the target position of the robotic arm link;

[0194] For the until operator in STL paradigm The TVCBF is constructed by considering l cases of smooth minimum function combinations as follows:

[0195] ,

[0196] In the formula, l represents the number of timing constraints. For the time-varying control barrier function corresponding to the signal-sequential logic paradigm up to operator U; This represents a smooth approximation of the minimum value operation.

[0197] like Figure 5 As shown in Example 1: The signal timing logic task STL for a six-degree-of-freedom robotic arm is as follows:

[0198] ,

[0199] In the formula, It is a logical AND operator that represents the logical AND relationship between all subconditions in the formula. It requires that both conditions i=1 and i=2 must be satisfied at the same time. As a constant operator, it is specified that the time interval [5,8] must be maintained continuously. ; Let be the distance from the axis of the cylinder corresponding to the i-th link of the robotic arm to the center of the sphere representing the dynamic obstacle. It is the sum of the inner diameter of the cylinder corresponding to the i-th link of the robotic arm and the radius of the sphere corresponding to the obstacle.

[0200] Considering only one robotic arm link, parsing the task using a syntax tree yields the atomic predicates as shown in the following equation:

[0201] ,

[0202] Based on atomic predicates, construct As shown in the following formula:

[0203] ,

[0204] according to Build Substitute The expression yields... The expression is shown in the following formula:

[0205] .

[0206] like Figure 6 As shown in Example 2: The signal timing logic task STL for a six-DOF robotic arm is as follows:

[0207] ,

[0208] In the formula, The existence operator is defined as the condition that an entity exists within the time interval [2,4]. ; For the first robotic arm Each link corresponds to the distance from the axis of the cylinder to the center of the sphere of the corresponding dynamic obstacle. For the first robotic arm The sum of the inner diameter of the cylinder corresponding to each link and the radius of the sphere corresponding to the dynamic obstacle.

[0209] Considering only one robotic arm link, parsing the task using a syntax tree yields the atomic predicates as shown in the following equation:

[0210] ,

[0211] Based on atomic predicates, construct As shown in the following formula:

[0212] ,

[0213] according to Build ,get :

[0214] ,

[0215] To ensure The forward invariance will and Substitute into the following formula and solve for... and , and Let the coefficient matrix and the right-hand side vector of the inequality constraint be represented respectively:

[0216] ,

[0217] For task G and As shown in the following formula:

[0218] ,

[0219] ,

[0220] For task F, and As shown in the following formula:

[0221] ,

[0222] ,

[0223] By combining the coefficient matrices and right-hand vectors of tasks F and G, we can obtain... and :

[0224] ,

[0225] The constraints of DCBF and TVCBF are combined with the optimal control values ​​of the angular velocities of each joint of the robotic arm generated by the LQR controller. By combining these methods, a quadratic programming QP problem is constructed to enable the robotic arm to simultaneously satisfy STL timing constraints and dynamic obstacle avoidance requirements in a dynamic environment:

[0226] ,

[0227] In the formula, H represents the weight matrix and f represents the cost vector; by solving the quadratic programming problem using the above formula, the final angular velocity control quantity of the robotic arm joints that simultaneously satisfies the STL timing constraints and dynamic obstacle avoidance requirements is obtained. This allows the robotic arm to complete the specified time sequence task while avoiding collisions with dynamic obstacles during trajectory tracking.

[0228] Figure 5 and Figure 6 These are simulation diagrams of the robotic arm under two different time-series tasks, Example 1 and Example 2. Figure 5 and Figure 6 (a) The safe distance D between the robotic arm and dynamic obstacles throughout the entire movement. Figure 5 and Figure 6 (b) in the figure represents the angular velocity of each joint of the robotic arm that is input to the robotic arm control throughout the entire motion. .

[0229] Simulation experiments were conducted in MATLAB software to construct a quadratic programming problem by combining the constraints of the time-varying control barrier function with the control input generated by a linear quadratic regulator. Figure 5 Figure (a) presents the motion simulation results of the robotic arm controlled under the constraints of the constructed DCBF and TVCBF. Figure 5 As shown in (a), the horizontal dashed line represents The unit is meters, and the two vertical dotted lines represent... and The unit is seconds. As can be seen from the graph, in... Within the range, safe distance The distance is always greater than 0.3m, satisfying the signal timing and logic timing constraints, and the safe distance of the robotic arm throughout its movement is always greater than 0, which verifies the effective execution of the STL task. Figure 5 As shown in (b), the angular velocity control quantity u of the six joints of the robotic arm remains continuous and smooth throughout the entire motion process, without abrupt changes or high-frequency oscillations, indicating that the robotic arm control commands generated by this method are stable.

[0230] Figure 6 Figure (a) presents the motion simulation results of the robotic arm controlled under the constraints of the constructed DCBF and TVCBF. Figure 6 As shown in (a), the horizontal dashed line represents The unit is meters, and the two vertical dotted lines represent... and The unit is seconds, from Figure 6 As can be seen from (a), there exists a time. , making Timely satisfaction The signal timing and logic timing constraints are met, and the safe distance of the robotic arm's movement is always greater than 0 throughout the entire process, which verifies the effective execution of the STL task; such as Figure 6 As shown in (b), the angular velocity control quantity u of the six joints of the robotic arm remains continuous and smooth throughout the entire motion process, without abrupt changes or high-frequency oscillations, indicating that the control commands generated by this method are stable.

[0231] In summary, this method constructs a global path planning framework based on the fast walking tree algorithm, achieves collision detection through axis-aligned bounding boxes and bounding spheres, and smooths the path through cubic spline interpolation. It has high search efficiency and better path quality in high-dimensional space. Compared with traditional sampling-based path planning algorithms, it has faster path convergence speed and better smoothness.

[0232] Furthermore, a dynamic obstacle control function is introduced during trajectory tracking. Combined with the control input generated by a linear quadratic regulator, dynamic obstacle avoidance is transformed into a safety constraint optimization problem. By adjusting the control quantity through real-time solution of the quadratic programming problem, the safe distance between the robotic arm and dynamic obstacles is always maintained above the threshold, and the control input is smooth and continuous, avoiding the oscillation or stagnation problems of traditional methods.

[0233] Specifically, by using signal-sequential logic and time-varying control obstacle functions, complex temporal tasks are transformed into computable temporal constraint optimization problems. By analyzing the signal-sequential logic paradigm to design time-dependent modulation functions, and constructing time-varying control obstacle functions to dynamically adjust safety boundaries, the robotic arm can simultaneously meet temporal task specifications during obstacle avoidance, thus improving the applicability of motion planning in real-world scenarios.

[0234] This method decouples global planning, dynamic obstacle avoidance, and temporal constraints through a hierarchical structure, allowing each layer of technology to be independently optimized and flexibly combined. The fast walking tree algorithm provides the globally optimal path, while the dynamic and time-varying obstacle control functions enable responses to dynamic environments. Signal timing logic ensures the formalization of task logic, making the method applicable to robotic arms with different degrees of freedom and multi-obstacle, highly constrained scenarios, significantly improving its generalization ability in industrial applications.

Claims

1. A hierarchical motion planning method for robotic arms in dynamic environments, characterized in that, The specific steps are as follows: Step 1: Set the target position of the robotic arm, while taking into account the position of static obstacles, and plan the collision-free initial global path from the current position of each joint of the robotic arm to the target position; Step 2: Smooth the initial global path and plan the robotic arm's motion trajectory; Step 3: Generate the angular velocity control values ​​for each joint of the robotic arm based on the planned robotic arm trajectory; Step 4: When a dynamic obstacle is detected within the robotic arm's movement range, based on avoiding static obstacles, a dynamic obstacle avoidance framework is constructed by combining the dynamic control obstacle function. Signal timing logic is introduced into the dynamic obstacle avoidance framework to constrain the task and control the robotic arm to perform dynamic obstacle avoidance. In step 1, a global path is generated using the Fast Moving Tree Algorithm (FMT*), taking into account both the robot arm's own constraints and safe distance constraints from static obstacles: In the joint space of the robotic arm, the FMT* algorithm needs to connect sampling points to form a search tree and find the optimal path from the starting position to the target position. When performing single-step expansion, the FMT* algorithm uses the geometric envelope method to perform collision detection after finding the optimal single-step connection, ensuring that the path does not collide with any obstacles before adding the path to the path tree. An envelope model is constructed, using cylinders of different radii to enclose the joints and links of the robotic arm, and using spheres to enclose static and dynamic obstacles, and using the envelope model to perform collision detection. The process of smoothing the initial global path generated by the FMT* algorithm using cubic spline interpolation is as follows: The initial global path corresponding to the i-th joint of the robotic arm is represented as a sequence of n path nodes. Let q represent the angle of the robotic arm joint. A piecewise cubic spline interpolation method is used to fit the entire path, generating a smooth and continuous trajectory to connect these discrete path nodes. For each pair of adjacent path nodes of the i-th joint... and The interval between Construct a piecewise cubic polynomial : ， In the formula, Indicates the current time. These are polynomial coefficients, obtained through path nodes. Solving for position constraints, continuity constraints, and boundary conditions, where j represents the j-th path node; By integrating all path nodes By applying positional constraints, continuity constraints, and boundary conditions, a system of equations is obtained to solve for the coefficients of piecewise cubic polynomials between all two adjacent path nodes. Solving this system of equations determines the coefficients of each piecewise polynomial. ; Connecting all the piecewise cubic polynomials forms a curve that not only passes precisely through all path nodes. Moreover, the smooth trajectory with continuous changes in both speed and acceleration meets the requirements for angular velocity control of each joint of the robotic arm.

2. The hierarchical motion planning method for a robotic arm in a dynamic environment according to claim 1, characterized in that, The process of generating the angular velocity control values ​​for each joint of the robotic arm using the linear quadratic regulator (LQR) is as follows: Let the state variables of the robotic arm's motion in three-dimensional space be represented by the angles of each joint of the robotic arm. ,in Let the angular velocity of the i-th robotic arm joint be denoted as ; let the angular velocity control value of the robotic arm joint be denoted as . The state-space equation of the robotic arm is expressed as follows: ， In the formula, the zero matrix With the identity matrix The angular velocity control quantity u of the robotic arm joints and the state quantity The following equation applies between them: ， The objective function J of LQR is minimized as follows: ， In the formula, Q is a positive semi-definite matrix used to penalize state errors; R is a positive definite matrix used to penalize the energy consumption of the control quantities of the angular velocity control quantities of each joint of the robotic arm. By adjusting the weights of Q and R, the state tracking accuracy of the robotic arm and the energy consumption of the angular velocity control quantities of each joint of the robotic arm can be balanced. Using LQR to solve for the joint angular velocity control of the robotic arm: The solution of the algebraic Riccati equation ARE is used as the control quantity of LQR. Assume that there exists a positive definite matrix P that satisfies the ARE equation: ， The optimal feedback gain matrix K is represented as follows: ， Therefore, the optimal control values ​​for the angular velocities of each joint of the robotic arm are obtained as shown in the following formula: ， In the formula, K is the LQR gain matrix, q init With q goal Let q represent the current pose of the robotic arm and the target pose, respectively. When the robotic arm is performing trajectory tracking, if the current time point is less than the maximum time point of the piecewise cubic spline interpolation, q... goal Let q be the joint angle value corresponding to the next moment; if the current moment is greater than the maximum moment of the piecewise cubic spline interpolation, then q goal This represents the piecewise cubic spline interpolation pose corresponding to the maximum time.

3. The hierarchical motion planning method for a robotic arm in a dynamic environment according to claim 2, characterized in that, The specific steps for controlling a robotic arm to perform dynamic obstacle avoidance include: When there are no dynamic obstacles and no signal timing logic tasks during the tracking process of the linear quadratic regulator, the linear quadratic regulator controls the robotic arm to execute the robotic arm's motion trajectory. When dynamic obstacles exist during the linear quadratic controller's tracking process, a dynamic control obstacle function is calculated based on these obstacles. When signal-sequential logic tasks also exist during the tracking process, the signal-sequential logic paradigm is used to describe the timing tasks. The atomic predicates are obtained by parsing the signal-sequential logic paradigm, and a time-varying control obstacle function is designed based on these atomic predicates. The robotic arm's motion trajectory is then replanned by combining the signal-sequential logic timing constraints and the time-varying control obstacle function. This ensures that the robotic arm can not only safely avoid dynamic obstacles in a dynamic environment but also strictly follow the logical execution path planning of the signal-sequential task. Using the dynamic control obstacle function and the time-varying control obstacle function as constraints, a quadratic programming problem is solved to obtain the angular velocity control quantities of each joint of the robotic arm that allow it to avoid dynamic obstacles while fulfilling the timing constraints, enabling the robotic arm to reach the target position without collision.

4. The hierarchical motion planning method for a robotic arm in a dynamic environment according to claim 3, characterized in that, The workflow for constructing the Dynamic Obstacle Control Function (DCBF) within a dynamic obstacle avoidance framework that avoids both static and dynamic obstacles is as follows: The joints and links of the robotic arm are simplified into cylinders of different radii, and the dynamic obstacle is simplified into a sphere. A DCBF (Distributed Distance Boundary Function) is constructed for each link. By constraining the safe distance, the DCBF ensures that the links of the robotic arm maintain a safe distance from the dynamic obstacle during movement. The expression is as follows: ， In the formula, D is the distance from the center of the dynamic obstacle sphere to the axis of the connecting rod envelope cylinder. The radius of the dynamic obstacle sphere Inner diameter of the cylinder enveloped by the connecting rod The sum; h represents the dynamic control barrier function; As long as the robotic arm's DCBF initial state This ensures the forward invariance of the robotic arm, guaranteeing that the cylinder representing the link will not contact the sphere representing the dynamic obstacle, thus preventing the robotic arm from entering a dangerous collision state. The first derivative of DCBF... The following conditions must be met: ， In the formula, The coefficients for the convergence rate are given; further expansion yields the constraints of the robotic arm: ， In the formula, The current velocity of the dynamic obstacle. The first derivative of the distance D; Let the center of the dynamic obstacle sphere be... The cylindrical link of the robotic arm has joints at both ends. and Calculate using the following formula exist Projection vectors and vectors on the axis The ratio of its own length z: ， In the formula, Representative from point to ; Representative from point to ; Representative from point to The magnitude of the vector; The position of the foot O of the perpendicular representing the center of the sphere, which is a dynamic obstacle, is determined by the ratio z. Then the foot O of the perpendicular falls on the axis of the cylinder representing the link of the robotic arm. Above; otherwise, the foot of the perpendicular O lies on the axis of the cylinder representing the robotic arm link. On the extension line, if This represents the center of the sphere from the dynamic obstacle. To the joint end of the cylinder representing the robotic arm link The closest point is at [location name], and the distance is calculated using the following formula: ;like This represents the center of the dynamic obstacle sphere. To the cylindrical joint end representing the robotic arm link The closest point is at [location name], and the distance is calculated using the following formula: ; Based on the ratio z, the distance D under different conditions is represented as follows: ， Based on the vector representation rules and combined with the forward kinematics of the robotic arm, the first derivatives of each vector in distance D can be obtained through the expression of distance D: ， ， ， In the formula, J i Let v be the Jacobian matrix representing the velocity of the i-th joint of the robotic arm. o The instantaneous moving speed of the dynamic obstacle is represented by u, and the angular velocity control value of the robotic arm joint is represented by u. According to the vector differentiation rule, we can calculate... Expressions for different ratios z: when hour: ， when hour: ， when hour: ， The relative position of the dynamic obstacle and the link is determined by the range of values ​​for the ratio z, and the constraints of the robotic arm are defined as follows: This is transformed into a linear inequality constraint on the angular velocity control quantity u of the robotic arm joints, as shown in the following equation: ， By rearranging the linear inequality constraint of the angular velocity control quantity u of the robotic arm joint, we can obtain... Solve the quadratic programming QP problem in the form of [formula missing]. and Let represent the coefficient matrix and the right-hand side vector of the inequality constraint, respectively. and The expressions are shown in the following formulas: ， 。 5. The hierarchical motion planning method for a robotic arm in a dynamic environment according to claim 4, characterized in that: When there are dynamic obstacles and timing tasks during the tracking process of the linear quadratic regulator (LQR), the timing tasks are described using the Signal-Time Logic (STL) paradigm. STL accurately expresses the behavioral constraints of the robotic arm in the time dimension through the combination of timing operators and predicates. The timing operators of STL include the always operator, the end operator, and the until operator. In the path planning process, STL constraints and the time-varying obstacle control function (TVCBF) are combined to ensure that the robotic arm can not only avoid obstacles in a dynamic environment, but also strictly execute path planning according to the task logic. The specific construction steps of TVCBF are as follows: Task decomposition and predicate extraction: Syntax tree parsing is performed on the temporal tasks described using the STL paradigm to extract the atomic predicates from the STL-described temporal tasks. : ， In the formula, Here, x is the predicate function of STL, and u is the angular velocity control quantity of each joint of the robotic arm. Design modulation functions within the time window for different timing operators of STL logic. For the existence operator in STL normal form, construct a decrement function: ， In the formula, a and b are time points, and c is the scaling factor. To ensure the stable offset of the robotic arm, where t is time, an incrementing function is constructed for the start-stop operator in the STL paradigm: ， In the formula, k and To ensure Adjustment parameters that satisfy the inequality conditions. The offset to ensure the stability of the robotic arm; e is a constant, with a value of 2.71828; Design modulation functions outside the time window. The goal is to design a piecewise modulation function outside the time window to ensure continuous control of the angular velocity of each joint of the robotic arm. This function aims to gradually weaken the control effect of the TVCBF (Transmission-to-Convection-to-Frequency) on the robotic arm to zero outside the time window. Specifically, the modulation function value should decay over time and remain consistently greater than zero. The modulation function outside the time window is then constructed. It is expressed as follows: ， In the formula and To adjust the parameters to meet the requirements To ensure ; By merging modulation functions within the time window Modulation function outside the time window Obtain the modulation function in the entire time domain : ， In the formula, For time window The modulation function value within, Time window The modulation function value; For the existence operator and the always operator, TVCBF is represented as: ， In the formula, This is the Euclidean distance from the current position to the target position of the robotic arm link; For the until operator in the STL paradigm, the TVCBF is constructed by smoothing the minimum function combination for l cases as follows: ， In the formula, l represents the number of timing constraints. For the time-varying control barrier function corresponding to the signal-sequential logic paradigm up to the operator; This represents a smooth approximation of the minimum value operation; Constructing a time-varying control barrier function Time-varying safety set: ， In the formula, Define the domain of joint angles for an m-DOF robotic arm. For safety conditions, i.e., satisfying STL constraints; The design of TVCBF must satisfy the following formula to ensure safe collection. Within a specified time range, the robotic arm exhibits forward invariance: ， To construct the QP problem, we rearrange the terms in the above inequality and rewrite it as follows: Format: ， In the formula, and Let the coefficient matrix and the right-hand side vector of the inequality constraint be represented respectively: ， ， The constraints of DCBF and TVCBF are combined with the optimal control values ​​of the angular velocities of each joint of the robotic arm generated by the LQR controller. By combining these methods, the robotic arm can simultaneously satisfy STL timing constraints and dynamic obstacle avoidance requirements in a dynamic environment, thus constructing the QP problem as shown in the following equation: ， In the formula, H represents the weight matrix and f represents the cost vector. By solving the QP problem through the above formula, the final angular velocity control quantity u of the robotic arm joints that simultaneously satisfies the STL timing constraints and dynamic obstacle avoidance requirements is obtained, so that the robotic arm can complete the specified timing task while avoiding collisions with dynamic obstacles during trajectory tracking.

6. A computer device, characterized in that, It includes a processor and a memory, the processor being electrically connected to the memory, the memory being used to store instructions and data, and the processor being used to execute the hierarchical motion planning method for a robotic arm in a dynamic environment as described in any one of claims 1-5.

7. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program configured to implement, when invoked by a processor, the hierarchical motion planning method for a robotic arm in a dynamic environment as described in any one of claims 1-5.

Images