Mechanical arm dynamic obstacle avoidance trajectory planning method based on dynamic motion primitive and steering force field

Abstract

The invention provides a mechanical arm dynamic obstacle avoidance trajectory planning method based on a dynamic motion element and a steering force field, and belongs to the technical field of robots. Secondly, learning the teaching track preprocessed in the step 1 by using an improved dynamic motion primitive (DMP) algorithm, so that the DMP can generate a motion mode similar to the teaching track; through a depth camera installed around a mechanical arm or at a fixed position, obstacle information, shape and motion state are obtained in real time, obstacle avoidance repulsive force is calculated, steering force is quantified, and obstacle avoidance of free motion obstacles is achieved; a Lyapunov equation is constructed, based on the Lyapunov stability theory, the stability of the improved DMP algorithm is proved, and it is ensured that the motion trail of the mechanical arm in the dynamic environment is converged to a target point; and finally, carrying out an obstacle avoidance experiment to verify the dynamic obstacle avoidance capability of the mechanical arm. According to the invention, the trajectory generalization ability of the mechanical arm in a narrow working space is obviously improved.

Description

Technical Field

[0001] The present invention belongs to the field of robotics technology, and in particular, relates to a dynamic obstacle avoidance trajectory planning method for a robotic arm based on dynamic motion primitives and a steering force field. Background Art

[0002] When the robot arm completes the on-orbit operation task in space, the working space is small and the environment changes frequently, which makes the trajectory planning of the robot arm face many challenges. For example, the robot arm needs to operate accurately in a limited space while avoiding dynamic obstacles to ensure the smooth progress of the task. However, most of the current trajectory planning adopts manual programming, which is not only inefficient, but also cumbersome to change the program, and it is difficult to adapt to the task requirements in complex dynamic environments. In order to improve the adaptability and operation efficiency of the robot arm in complex environments, learning from demonstration (LfD) as an emerging technology can transfer the skills of human operators to the robot arm and realize the generalization of skills, thus providing a new idea for the trajectory planning of the robot arm. Among them, dynamic movement primitives (DMP) is a commonly used teaching learning method, which is widely used in trajectory planning and task execution of the robot arm. DMP decomposes complex motion trajectories into a series of simple motion patterns, enabling the robot arm to perform complex humanoid movements in a flexible and creative way. However, traditional DMP has obvious limitations in complex dynamic environments. For example, it is difficult to effectively avoid dynamic obstacles, especially when the obstacles are rotating, the obstacle avoidance capability of DMP is insufficient. In addition, although the existing improvement methods have improved the adaptability of DMP to a certain extent, there are still many deficiencies in obstacle avoidance capability, which are mainly manifested in the following aspects:

[0003] Excessive free space loss: Existing methods often sacrifice too much free space during obstacle avoidance, which limits the range of motion of the robot arm and reduces operational efficiency.

[0004] Falling into local minima: In complex environments, the combination of DMP and traditional potential fields is prone to falling into local minima, resulting in trajectory planning failure or trajectory mutation, affecting the motion stability of the robot arm.

[0005] Not applicable to rotational motion of obstacles: Most existing methods can only handle the translational motion of obstacles, but lack effective response strategies for the rotational motion of obstacles, which limits the application of robotic arms in complex dynamic environments.

[0006] In view of the above problems, it is particularly important to propose a new trajectory planning method to meet the needs of on-orbit operation tasks of the robotic arm, improve the adaptability and dynamic obstacle avoidance capability of DMP, and reduce the free space loss. Summary of the invention

[0007] In order to solve the problem that the existing DMP method has insufficient obstacle avoidance ability in complex dynamic environments, the present invention proposes a robot arm dynamic obstacle avoidance trajectory planning method based on dynamic motion primitives (DMP) and steering force fields. By introducing steering force field constraints, the obstacle avoidance performance of DMP is enhanced and the free space loss is reduced. It is suitable for robot arm operations in complex dynamic environments.

[0008] A dynamic obstacle avoidance trajectory planning method for a robotic arm based on dynamic motion primitives and steering force field:

[0009] The method specifically comprises the following steps:

[0010] Step 1, enabling the robot arm to obtain the teaching trajectory and perform preprocessing;

[0011] Step 2, using an improved dynamic motion primitive (DMP) algorithm to learn the teaching trajectory preprocessed in step 1, so that the DMP can generate a motion pattern similar to the teaching trajectory;

[0012] Step 3: Obstacle information, shape, and motion state are acquired in real time through a depth camera installed around the robotic arm or at a fixed position, and the obstacle avoidance repulsion force is calculated and the steering force is quantified;

[0013] Step 4: construct the Lyapunov equation and prove the stability of the improved DMP algorithm based on the Lyapunov stability theory to ensure that the motion trajectory of the robot in a dynamic environment converges to the target point;

[0014] Step 5: Conduct an obstacle avoidance experiment to verify the dynamic obstacle avoidance capability of the robotic arm.

[0015] Furthermore, in step 1,

[0016] The teaching trajectory is the motion path of the robot arm when completing a specific task, which is manually demonstrated or given in advance by a human operator;

[0017] The acquired teaching trajectory is preprocessed, and the preprocessing includes noise removal, smoothing and normalization; the preprocessed trajectory data will be used as the basic data for DMP learning.

[0018] Furthermore, in step 2,

[0019] The specific implementation process of the improved dynamic motion primitive algorithm is as follows: on the basis of the traditional DMP, a steering force field coupling term is introduced, including the obstacle avoidance repulsion force of different obstacles;

[0020] The traditional dynamic motion primitive (DMP) equation is:

[0021]

[0022]

[0023] In the formula, τ is the time scale factor, the system duration can be adjusted by τ, x is the current position, x init is the initial position, g is the target position, v and are velocity and acceleration respectively, and the diagonal d×d matrix A=diag[α1,α2,…,α d ],B=diag[β1,β2,…,β d ] are elastic term and damping term respectively, λ is a predefined positive constant, and f(s) is a forcing term composed of N weighted radial basis functions, which can be expressed as:

[0024]

[0025] in is the radial basis function, ω i Represents the weight of the radial basis function. When the initial state or target state of the trajectory changes, ω i It has the ability to generalize and can generate similar trajectories;

[0026] The method of adding obstacle avoidance coupling terms on the basis of DMP, the improved DMP control equation is:

[0027]

[0028]

[0029] Among them, the additional items is the obstacle avoidance coupling term.

[0030] Furthermore, in step 3,

[0031] The specific implementation process of the quantified steering force is: by analyzing the shape and motion state of the obstacle, the obstacle avoidance repulsion force is quantified, and the direction and magnitude of the repulsion force are dynamically adjusted according to the generalized distance and generalized angle of the obstacle;

[0032] The steering force is defined as follows:

[0033]

[0034] Where k, n, μ1, μ2 are constant coefficients, v is the velocity of the current position, d is the generalized distance, θ is the generalized angle, and R is the rotation matrix of π / 2 around the axis generated by XO×v;

[0035] For ellipses, arbitrary quadrilaterals, and triangles, they are mapped to circles, regular rectangles, and triangles by left-multiplying the affine transformation matrix R1. At this time, the quantized steering force expression is:

[0036] X a =R1X

[0037] v a =R1v

[0038]

[0039] Among them, R1 is a two-dimensional affine transformation matrix, and its expression is determined by the shape of the obstacle. is the angle between the major axis and the x-axis, X a is the virtual position after affine transformation, v a is the virtual velocity after affine transformation, d a ,θ a are the generalized distance and generalized angle determined by the virtual position and velocity of the affine transformation.

[0040] Furthermore, in step 3, the specific process of the generalized distance and the generalized angle is:

[0041] Define the generalized distance to obtain smooth equidistance lines. The generalized distances of circular, rectangular, and triangular obstacles are defined as follows:

[0042]

[0043]

[0044] Wherein, d1 is the generalized distance of circular obstacles, d2 is the generalized distance of triangular obstacles, d3 is the generalized distance of rectangular obstacles, r is the radius of circular obstacles, a2 is 1 / 2 of the side length of rectangular obstacles, a3 is 1 / 3 of the side length of triangular obstacles, and l is the Euclidean distance between the current position and the nearest geometric structure of the contour;

[0045] Determine the angle between the current velocity direction and the obstacle. For a circular obstacle, O is the center of the obstacle, X is the current position, and o is the tangent point between the current position and the obstacle. The generalized angle is defined as follows:

[0046]

[0047] Among them, θ1 is the angle between v and XO, θ2 is the angle between Oo and OX;

[0048] For rectangular, triangular and free-form obstacles, the relative angle is defined in a simple way. For rectangular and triangular obstacles, only the corresponding virtual circular obstacles are effective at different positions. When the current position changes, the virtual circular obstacle moves with it to ensure that the outer envelope of the virtual circular trajectory can cover the entire obstacle and that the trajectory will not intersect with the obstacle.

[0049] Furthermore, in step 4,

[0050] When t→∞, the phase variable x approaches 0; therefore, the simplified DMP equation It can be expressed as:

[0051]

[0052]

[0053] point is the stationary point of the equation; construct a Lyapunov function V and prove Therefore, it is shown that all points will converge to this stationary point; using the energy function of the damped spring mass system

[0054]

[0055]

[0056] Since R is a matrix rotated by π / 2, we can get v T Rv=0, It is proved that DMP converges to

[0057] A dynamic obstacle avoidance trajectory planning system for a robotic arm based on dynamic motion primitives and steering force fields:

[0058] The trajectory planning system includes: a teaching trajectory module, a DMP learning module, a steering force quantification module and a verification module;

[0059] The teaching trajectory module enables the robot arm to obtain the teaching trajectory and perform preprocessing;

[0060] The DMP learning module uses an improved dynamic motion primitive (DMP) algorithm to learn the teaching trajectory preprocessed by the teaching trajectory module, so that the DMP can generate a motion pattern similar to the teaching trajectory;

[0061] The steering force quantification module obtains obstacle information, shape and motion state in real time through a depth camera installed around the robot arm or at a fixed position, calculates the obstacle avoidance repulsion force, and quantifies the steering force;

[0062] The verification module constructs the Lyapunov equation and proves the stability of the improved DMP algorithm based on the Lyapunov stability theory to ensure that the motion trajectory of the robot in a dynamic environment converges to the target point; finally, an obstacle avoidance experiment is performed to verify the dynamic obstacle avoidance capability of the robot.

[0063] An electronic device comprises a memory and a processor, wherein the memory stores a computer program, and the processor implements the steps of the above method when executing the computer program.

[0064] A computer-readable storage medium is used to store computer instructions, and when the computer instructions are executed by a processor, the steps of the above method are implemented.

[0065] Beneficial effects of the present invention

[0066] Compared with the prior art, the present invention has the following effects:

[0067] Different from the traditional DMP and potential field-based methods and steering angle-based solutions, the present invention introduces a steering force field to enable the robot arm to effectively avoid three-dimensional dynamic obstacles of different shapes and sizes, and the obstacles can move arbitrarily with six degrees of freedom. This method avoids trajectory mutations and local minima problems, while achieving minimal free space loss.

[0068] In addition, the analysis based on Lyapunov stability theory ensures that the motion trajectory of the robot arm in a dynamic environment is stable and reliable. The robot arm trajectory planned by this method can provide reliable path planning support for operations in complex dynamic environments, significantly improve the trajectory generalization ability of the robot arm in a small working space, and provide a new solution for the application of the robot arm in aerospace, industrial manufacturing and other fields, with strong practical application value. BRIEF DESCRIPTION OF THE DRAWINGS

[0069] Figure 1 This is a schematic diagram of the equidistant line distribution of basic obstacles.

[0070] Figure 2 is the angle between the current speed and the obstacle.

[0071] Figure 3 Schematic diagram of affine transformation.

[0072] Figure 4 This is an example diagram for setting up the experimental system of the present invention.

[0073] Figure 5 This is a snapshot of the experiment of the present invention. DETAILED DESCRIPTION

[0074] The technical solutions in the embodiments of the present invention will be described clearly and completely below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0075] The experimental methods used in the following examples are conventional methods unless otherwise specified. The materials, reagents, methods and instruments used are conventional materials, reagents, methods and instruments in the art unless otherwise specified, and can be obtained through commercial channels by those skilled in the art.

[0076] Combination Figures 1 to 5 The present invention proposes a dynamic obstacle avoidance trajectory planning method for a robotic arm based on dynamic motion primitives (DMP) and steering force fields. The method includes improving the dynamic motion primitives (DMP) algorithm, quantifying steering forces, Lyapunov stability analysis, and obstacle avoidance experimental verification.

[0077] In an embodiment, the system is set up as Figure 4 Shown is a 7-DOF Franka EmikaPanda robotic arm, a table with obstacles and a Realsense D435 RGB depth camera, with the obstacle movement driven by servo motors. The camera is used to detect the location of the obstacle, and the robotic arm is controlled using the ROS framework on Ubuntu 20.04 to move across the table while avoiding obstacles.

[0078] Step 1, trajectory learning:

[0079] Acquisition and preprocessing of teaching trajectory: The teaching trajectory is the motion path of the robot arm when completing a specific task, which is usually demonstrated manually or given in advance by a human operator. For example, the drag-and-drop teaching method can be used to allow the operator to manually guide the robot arm to complete a complete task operation, record its position, speed and time information, and preprocess the acquired teaching trajectory, including noise removal, smoothing and normalization. The preprocessed trajectory data will serve as the basic data for DMP learning.

[0080] The teaching trajectory of this example is given in advance, and the teaching trajectory equation is: x=0.5sin(-t), y=0.5cos(t), z=0.3, where t∈(0,π), x, y, z are the Cartesian coordinates of the end of the robot arm in the base coordinate system.

[0081] Step 2, DMP parameter learning: Apply the improved DMP algorithm to learn the preprocessed teaching trajectory. The radial basis function weights in the DMP are adjusted through optimization algorithms (such as local weighted regression, LWR) so that the DMP can generate motion patterns similar to the teaching trajectory. After learning is completed, the DMP will have the ability to generate similar trajectories when the target state changes.

[0082] The specific implementation process of the improved dynamic motion primitive (DMP) algorithm is as follows: on the basis of the traditional DMP, a steering force field coupling term is introduced, including the obstacle avoidance repulsion force of different obstacles, to achieve obstacle avoidance of dynamic obstacles. The traditional dynamic motion primitive (DMP) equation is:

[0083]

[0084]

[0085] In the formula, τ is the time scale factor, the system duration can be adjusted by τ, x is the current position, x init is the initial position, g is the target position, v and are velocity and acceleration respectively, and the diagonal d×d matrix A=diag[α1,α2,…,α d ],B=diag[β1,β2,…,β d ] are elastic term and damping term respectively, λ is a predefined positive constant, and f(s) is a forcing term composed of N weighted radial basis functions, which can be expressed as:

[0086]

[0087] where ω i represents the weights of the radial basis functions. An optimization algorithm such as Locally Weighted Regression (LWR) can be used to select appropriate model parameters for these basis functions so that the forcing function matches the desired trajectory. When the initial state or target state of the trajectory changes, ω i It has the ability to generalize and can generate similar trajectories. In addition, f depends on the phase variable s, which is a reparameterization of time t and is controlled by the so-called canonical system. The transient behavior of s is defined as:

[0088]

[0089] Where λ is a predefined positive constant. Radial basis function It can be defined as follows:

[0090]

[0091] where exp(·) is an exponential function. The width of the Gaussian function h is i It can be calculated by the following formula:

[0092] h i =(c i+1 -c i ) -2 ,h N =h N-1

[0093] The center c of the Gaussian function i It can be calculated as follows:

[0094]

[0095] The method of adding obstacle avoidance coupling terms on the basis of DMP, the improved DMP control equation is:

[0096]

[0097]

[0098] Among them, the additional items is the obstacle avoidance coupling term.

[0099] Step 3: Obtain obstacle information and calculate steering force:

[0100] Obstacle detection and identification: Use a depth camera installed around the robot arm or in a fixed position to obtain obstacle information in the working environment in real time. Use image processing or point cloud analysis algorithms to identify the shape, position, and motion state of obstacles. For example, video analysis algorithms such as optical flow can be used to extract the movement speed and direction of obstacles. In this example, the obstacle is elliptical, with a major axis a of 0.4m, a minor axis b of 0.2m, a linear velocity of 4mm / s, and an angular velocity of 0.04rad / s.

[0101] Data import and processing: Import the detected obstacle information (including shape, position, speed, etc.) into the computer system. Further process the obstacle information, such as coordinate conversion, data filtering, etc., to ensure the accuracy and real-time nature of the data, and use the data as input for calculating the steering force.

[0102] Determine the generalized distance and generalized angle, and calculate the generalized distance and generalized angle between the current position of the robot and the obstacle according to the shape and position of the obstacle.

[0103] The specific implementation process of the quantified steering force is: by analyzing the shape and motion state of the obstacle, the quantified obstacle avoidance repulsion force is realized to avoid the freely moving obstacle. The direction and magnitude of the repulsion force are dynamically adjusted according to the generalized distance and generalized angle of the obstacle to ensure that the robot arm will not collide with the obstacle during the obstacle avoidance process.

[0104] Use Figure 1 The generalized distance is defined in this way to obtain a smooth equidistance line. Area 2 is the boundary straight line area, and area 1 is the arc transition area. The generalized distances of circular, rectangular, and triangular obstacles are defined as follows:

[0105]

[0106]

[0107]

[0108] Wherein, d1 is the generalized distance of circular obstacles, d2 is the generalized distance of triangular obstacles, d3 is the generalized distance of rectangular obstacles, r is the radius of circular obstacles, a2 is 1 / 2 of the side length of rectangular obstacles, a3 is 1 / 3 of the side length of triangular obstacles, in area 1, l is the Euclidean distance between the current position and the nearest endpoint of the contour, and in area 2, l is the Euclidean distance between the current position and the nearest straight line of the contour.

[0109] The steering force is defined as follows:

[0110]

[0111] Among them, k, n, μ1, μ2 are constant coefficients, v is the velocity of the current position, d is the generalized distance, θ is the generalized angle, and R is the rotation matrix of π / 2 around the axis generated by XO×v. Figure 3 The process shown

[0112] For an ellipse, any quadrilateral, or a triangle, it can be mapped to a circle, a regular rectangle, or a triangle simply by left-multiplying the affine transformation matrix R1, such as Figure 3 As shown, at this time:

[0113] X a =R1X

[0114] v a =R1v

[0115]

[0116] Among them, R1 is a two-dimensional affine transformation matrix, and its expression is determined by the shape of the obstacle, such as the affine transformation matrix of an ellipse mapped to a circle: is the angle between the major axis and the x-axis, X a is the virtual position after affine transformation, v a is the virtual velocity after affine transformation, d a ,θ a are the generalized distance and generalized angle determined by the virtual position and velocity of the affine transformation.

[0117] Note that the generalized distance and angle definitions of the steering force field solution are not limited to the above forms, and the analytical equations can be independently designed as needed, and graphics of arbitrary shapes can be obtained by combining basic graphics.

[0118] Step 4: Update the steering force field coupling term in real time based on Lyapunov stability analysis: During the motion of the robot arm, the steering force field coupling term is calculated in real time and added to the control equation of the DMP. The steering force field coupling term is adjusted in real time according to the dynamic changes of the obstacle to ensure that the robot arm can effectively avoid the obstacle.

[0119] The specific implementation process of the Lyapunov stability analysis is as follows: constructing the Lyapunov equation, and based on the Lyapunov stability theory, proving the stability of the improved DMP algorithm to ensure that the motion trajectory of the robot in a dynamic environment converges to the target point.

[0120] When t→∞, the phase variable x approaches 0. Therefore, the simplified DMP equation It can be expressed as:

[0121]

[0122]

[0123] point is the stationary point of the equation. Construct a Lyapunov function V and prove that Therefore, it is shown that all points will converge to this stationary point. Using the energy function of the damped spring mass system

[0124]

[0125]

[0126] Since R is a matrix rotated by π / 2, we can get v T Rv=0, It is proved that DMP converges to

[0127] Step 5, experimental verification: The effectiveness of the method of the present invention is verified through a series of numerical simulations and Franka robot arm experiments. The experimental results show that this method can provide reliable trajectory planning for the robot arm in a complex dynamic environment and significantly improve the generalization operation capability of the robot arm.

[0128] The center of the Franka robot base is taken as the coordinate origin. Figure 5As shown in the figure, the snapshots of the experimental process show the trajectory of the Franka robot end effector. Each row of figures represents an experimental process consisting of 3 snapshots. The initial position and the target position are: x init =[-0.5, 0.0, 0.3] and g = [0.0, 0.5, 0.3]. To verify the effectiveness of the method, the obstacle avoidance performance was tested in the presence of elliptical, rectangular, and triangular obstacles, respectively. The obstacle was placed at [-0.375, 0.3, 0.31]. Since the free space loss of this scheme is extremely small, a 5mm safety margin was left between the end of the robot arm and the obstacle for the safety of the experiment.

[0129] Performance evaluation: The experimental results are evaluated, including indicators such as obstacle avoidance success rate, trajectory smoothness, free space loss, and computational efficiency. The experimental results show that the proposed method can effectively avoid obstacles, has good trajectory smoothness, minimal free space loss, and computational efficiency that can meet real-time requirements.

[0130] A dynamic obstacle avoidance trajectory planning system for a robotic arm based on dynamic motion primitives and steering force fields is proposed:

[0131] The trajectory planning system includes: a teaching trajectory module, a DMP learning module, a steering force quantification module and a verification module;

[0132] The teaching trajectory module enables the robot arm to obtain the teaching trajectory and perform preprocessing;

[0133] The DMP learning module uses an improved dynamic motion primitive (DMP) algorithm to learn the teaching trajectory preprocessed by the teaching trajectory module, so that the DMP can generate a motion pattern similar to the teaching trajectory;

[0134] The steering force quantification module obtains obstacle information, shape and motion state in real time through a depth camera installed around the robot arm or at a fixed position, calculates the obstacle avoidance repulsion force, and quantifies the steering force to achieve obstacle avoidance of freely moving obstacles;

[0135] The verification module constructs the Lyapunov equation and proves the stability of the improved DMP algorithm based on the Lyapunov stability theory to ensure that the motion trajectory of the robot in a dynamic environment converges to the target point; finally, an obstacle avoidance experiment is performed to verify the dynamic obstacle avoidance capability of the robot.

[0136] An electronic device comprises a memory and a processor, wherein the memory stores a computer program, and the processor implements the steps of the above method when executing the computer program.

[0137] A computer-readable storage medium is used to store computer instructions, and when the computer instructions are executed by a processor, the steps of the above method are implemented.

[0138] The memory in the embodiments of the present application may be a volatile memory or a non-volatile memory, or may include both volatile and non-volatile memories. Among them, the non-volatile memory may be a read-only memory, ROM, a programmable read-only memory, PROM, an erasable programmable read-only memory, EPROM, an electrically erasable programmable read-only memory, EEPROM, or a flash memory. The volatile memory may be a random access memory, RAM, which is used as an external cache. By way of example but not limitation, many forms of RAM are available, such as static random access memory static RAM, SRAM, dynamic random access memory dynamic RAM, DRAM, synchronous dynamic random access memory synchronous DRAM, SDRAM, double data rate synchronous dynamic random access memory double data rate SDRAM, DDR SDRAM, enhanced synchronous dynamic random access memory enhanced SDRAM, ESDRAM, synchronous link dynamic random access memory synchlink DRAM, SLDRAM, and direct memory bus random access memory direct rambus RAM, DR RAM. It should be noted that memory of the methods described herein is intended to include, but is not limited to, these and any other suitable types of memory.

[0139] In the above embodiments, it can be implemented in whole or in part by software, hardware, firmware or any combination thereof. When implemented by software, it can be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions. When the computer instructions are loaded and executed on a computer, the process or function described in the embodiment of the present application is generated in whole or in part. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer-readable storage medium, or transmitted from one computer-readable storage medium to another computer-readable storage medium. For example, the computer instructions can be transmitted from a website site, computer, server or data center through a wired method such as coaxial cable, optical fiber, digital subscriber line digital subscriber line, DSL or wireless such as infrared, wireless, microwave, etc. to another website site, computer, server or data center. The computer-readable storage medium can be any available medium that can be accessed by a computer or a data storage device such as a server or data center that includes one or more available media integrated. The available medium can be a magnetic medium such as a floppy disk, a hard disk, a tape, an optical medium such as a high-density digital video disc digital video disc, DVD, or a semiconductor medium such as a solid state hard disk solid state disc, SSD, etc.

[0140] In the implementation process, each step of the above method can be completed by an integrated logic circuit of hardware in a processor or an instruction in the form of software. The steps of the method disclosed in conjunction with the embodiment of the present application can be directly embodied as a hardware processor for execution, or a combination of hardware and software modules in a processor for execution. The software module can be located in a storage medium mature in the art such as a random access memory, a flash memory, a read-only memory, a programmable read-only memory or an electrically erasable programmable memory, a register, etc. The storage medium is located in a memory, and the processor reads the information in the memory and completes the steps of the above method in conjunction with its hardware. To avoid repetition, it is not described in detail here.

[0141] It should be noted that the processor in the embodiment of the present application can be an integrated circuit chip with signal processing capabilities. In the implementation process, each step of the above method embodiment can be completed by an integrated logic circuit of hardware in the processor or an instruction in the form of software. The above processor can be a general-purpose processor, a digital signal processor DSP, an application-specific integrated circuit ASIC, a field programmable gate array FPGA or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components. The methods, steps and logic block diagrams disclosed in the embodiments of the present application can be implemented or executed. The general-purpose processor can be a microprocessor or the processor can also be any conventional processor, etc. The steps of the method disclosed in the embodiment of the present application can be directly embodied as a hardware decoding processor to perform, or the hardware and software modules in the decoding processor can be combined to perform. The software module can be located in a mature storage medium in the field such as a random access memory, a flash memory, a read-only memory, a programmable read-only memory or an electrically erasable programmable memory, a register, etc. The storage medium is located in a memory, and the processor reads the information in the memory and completes the steps of the above method in combination with its hardware.

[0142] The above is a detailed introduction to the dynamic obstacle avoidance trajectory planning method for a robotic arm based on dynamic motion primitives and steering force fields proposed in the present invention, and the principles and implementation methods of the present invention are explained. The description of the above embodiments is only used to help understand the method of the present invention and its core idea; at the same time, for general technical personnel in this field, according to the idea of ​​the present invention, there will be changes in the specific implementation method and application scope. In summary, the content of this specification should not be understood as a limitation on the present invention.

Claims

1. A dynamic obstacle avoidance trajectory planning method for a robotic arm based on dynamic motion primitives and steering force field, characterized in that: The method specifically comprises the following steps: Step 1, enabling the robot arm to obtain the teaching trajectory and perform preprocessing; Step 2, using an improved dynamic motion primitive (DMP) algorithm to learn the teaching trajectory preprocessed in step 1, so that the DMP can generate a motion pattern similar to the teaching trajectory; Step 3: Obstacle information, shape, and motion state are acquired in real time through a depth camera installed around the robotic arm or at a fixed position, and the obstacle avoidance repulsion force is calculated and the steering force is quantified; Step 4: construct the Lyapunov equation and prove the stability of the improved DMP algorithm based on the Lyapunov stability theory to ensure that the motion trajectory of the robot in a dynamic environment converges to the target point; Step 5: Conduct an obstacle avoidance experiment to verify the dynamic obstacle avoidance capability of the robotic arm.

2. The planning method according to claim 1, characterized in that: In step 1, The teaching trajectory is the motion path of the robot arm when completing a specific task, which is manually demonstrated or given in advance by a human operator; The acquired teaching trajectory is preprocessed, and the preprocessing includes noise removal, smoothing and normalization; the preprocessed trajectory data will be used as the basic data for DMP learning.

3. The planning method according to claim 2, characterized in that: In step 2, The specific implementation process of the improved dynamic motion primitive algorithm is as follows: on the basis of the traditional DMP, a steering force field coupling term is introduced, including the obstacle avoidance repulsion force of different obstacles; The traditional dynamic motion primitive (DMP) equation is: In the formula, τ is the time scale factor, the system duration can be adjusted by τ, x is the current position, x init is the initial position, g is the target position, v and are velocity and acceleration respectively, and the diagonal d×d matrix A=diag[α1,α2,…,α d ],B=diag[β1,β2,…,β d ] are elastic term and damping term respectively, λ is a predefined positive constant, and f(s) is a forcing term composed of N weighted radial basis functions, which can be expressed as: in is the radial basis function, ω i Represents the weight of the radial basis function. When the initial state or target state of the trajectory changes, ω i It has the ability to generalize and can generate similar trajectories; The method of adding obstacle avoidance coupling terms on the basis of DMP, the improved DMP control equation is: Among them, the additional items is the obstacle avoidance coupling term.

4. The planning method according to claim 3, characterized in that: In step 3, The specific implementation process of the quantified steering force is: by analyzing the shape and motion state of the obstacle, the obstacle avoidance repulsion force is quantified, and the direction and magnitude of the repulsion force are dynamically adjusted according to the generalized distance and generalized angle of the obstacle; The steering force is defined as follows: Where k, n, μ1, μ2 are constant coefficients, v is the velocity of the current position, d is the generalized distance, θ is the generalized angle, and R is the rotation matrix of π / 2 around the axis generated by XO×v; For ellipses, arbitrary quadrilaterals, and triangles, they are mapped to circles, regular rectangles, and triangles by left-multiplying the affine transformation matrix R1. At this time, the quantized steering force expression is: X a =R1X in a =R1v Among them, R1 is a two-dimensional affine transformation matrix, and its expression is determined by the shape of the obstacle. is the angle between the major axis and the x-axis, X a is the virtual position after affine transformation, v a is the virtual velocity after affine transformation, d a ,θ a are the generalized distance and generalized angle determined by the virtual position and velocity of the affine transformation.

5. The planning method according to claim 4, characterized in that: In step 3, the specific process of generalized distance and generalized angle is: Define the generalized distance to obtain smooth equidistance lines. The generalized distances of circular, rectangular, and triangular obstacles are defined as follows: Wherein, d1 is the generalized distance of circular obstacles, d2 is the generalized distance of triangular obstacles, d3 is the generalized distance of rectangular obstacles, r is the radius of circular obstacles, a2 is 1 / 2 of the side length of rectangular obstacles, a3 is 1 / 3 of the side length of triangular obstacles, and l is the Euclidean distance between the current position and the nearest geometric structure of the contour; Determine the angle between the current velocity direction and the obstacle. For a circular obstacle, O is the center of the obstacle, X is the current position, and o is the tangent point between the current position and the obstacle. The generalized angle is defined as follows: Among them, θ1 is the angle between v and XO, θ2 is the angle between Oo and OX; For rectangular, triangular and free-form obstacles, the relative angle is defined in a simple way. For rectangular and triangular obstacles, only the corresponding virtual circular obstacles are effective at different positions. When the current position changes, the virtual circular obstacle moves with it to ensure that the outer envelope of the virtual circular trajectory can cover the entire obstacle and that the trajectory will not intersect with the obstacle.

6. The planning method according to claim 4, characterized in that: In step 4, When t→∞, the phase variable x approaches 0; therefore, the simplified DMP equation It can be expressed as: point is the stationary point of the equation; construct a Lyapunov function V and prove Therefore, it is shown that all points will converge to this stationary point; using the energy function of the damped spring mass system Since R is a matrix rotated by π / 2, we can get v T Rv=0, It is proved that DMP converges to 7. A trajectory planning system for executing the robot arm dynamic obstacle avoidance trajectory planning method based on dynamic motion primitives and steering force field as described in any one of claims 1 to 6, characterized in that: The trajectory planning system includes: a teaching trajectory module, a DMP learning module, a steering force quantification module and a verification module; The teaching trajectory module enables the robot arm to obtain the teaching trajectory and perform preprocessing; The DMP learning module uses an improved dynamic motion primitive (DMP) algorithm to learn the teaching trajectory preprocessed by the teaching trajectory module, so that the DMP can generate a motion pattern similar to the teaching trajectory; The steering force quantification module obtains obstacle information, shape and motion state in real time through a depth camera installed around the robot arm or at a fixed position, calculates the obstacle avoidance repulsion force, and quantifies the steering force; The verification module constructs the Lyapunov equation and proves the stability of the improved DMP algorithm based on the Lyapunov stability theory to ensure that the motion trajectory of the robot in a dynamic environment converges to the target point; finally, an obstacle avoidance experiment is performed to verify the dynamic obstacle avoidance capability of the robot.

8. An electronic device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 6 are implemented.

9. A computer-readable storage medium for storing computer instructions, characterized in that: When the computer instructions are executed by a processor, the steps of the method according to any one of claims 1 to 6 are implemented.

Images