A method and system for robot motion planning based on dmps

Abstract

The application discloses a kind of robot motion planning method and system based on DMPs, the method includes: according to action setting DMPs model, and learning the preset trajectory of action;According to actual environment, determine the new starting and ending point of action;Based on DMPs model, the new starting and ending point of action is coupled with the original starting and ending point in the preset trajectory and analyzed, to obtain new action trajectory;According to the new action trajectory, control robot motion.The system includes: learning module, target determination module, trajectory calculation module and control module.Through using the application, the change of the starting and ending point of new trajectory can be analyzed to make up for the lack of coupling information, complete the task of action planning combining real-time environment and teaching trajectory.The application can be widely applied in mechanical control field.

Description

Technical Field

[0001] This invention relates to the field of mechanical control, and in particular to a robot motion planning method and system based on DMPs. Background Technology

[0002] In recent years, with the advancement of science and technology and the development of society, robot system technology has emerged. Robot systems such as robotic arms, mobile robots and collaborative robots have been widely used in various technical fields, greatly improving people's work efficiency with their characteristics of efficiency, precision and all-weather operation.

[0003] Taking inspection tasks as an example, task-oriented programming of the robot system for inspection tasks is necessary for the robot system to complete the specified work. However, this requires programmers to have certain professional knowledge and is not suitable for users without professional knowledge to make secondary adjustments. For example, opening and closing a door is a complex combination of actions during inspection. Manual programming alone requires a lot of time and professional knowledge and is subject to many limitations. In robot systems, teaching-based learning (imitation learning) is used to solve the above situation. Teaching-based learning refers to the process of a human teacher remotely or directly controlling and moving the robot system to complete corresponding actions and skills. This process is called teaching. During the teaching process, the robot system records information such as trajectory, impedance, and force, and learns from it, so that it can accurately replicate or generalize the actions and skills after the demonstration by the human teacher is no longer required. Complete information replication cannot meet the robot system's requirement for generalization in completing tasks because the real-time environment is not completely consistent during repetition. Currently, there is no planning method that can adaptively adjust the robot's teaching content according to the real-time environment. Summary of the Invention

[0004] In view of this, in order to solve the technical problem that motion planning methods cannot be adjusted according to the real-time environment, thus resulting in the limited practicality of teach-and-learn applications in robot systems, this invention proposes a robot motion planning method and system based on DMPs. The method includes the following steps:

[0005] DMPs models are set based on actions, and preset trajectories of actions are learned;

[0006] Determine the new start and end points of the action based on the actual environment;

[0007] Based on the DMPs model, a new motion trajectory is obtained by combining the new start and end points of the action with the original start and end points in the preset trajectory for coupled information analysis.

[0008] The robot's movement is controlled based on the new motion trajectory.

[0009] In some embodiments, the step of setting a DMPs model based on the action and learning a preset trajectory of the action specifically includes:

[0010] Set the corresponding number of multidimensional DMPs models based on the number of actions;

[0011] The robot is controlled to perform preset actions in sequence, learn the preset trajectories of each action, and adjust the hyperparameters of the multidimensional DMPs model according to the learning structure.

[0012] In some embodiments, the step of determining the new start and end points of the action based on the actual environment specifically includes:

[0013] Acquire current environmental information and perform target identification to obtain the identification results;

[0014] Based on the recognition results, the new start and end points of each action are calculated.

[0015] In some embodiments, the step of performing coupled information analysis based on the DMPs model, combining the new start and end points of the action with the original start and end points in the preset trajectory, to obtain a new action trajectory specifically includes:

[0016] Read the model parameters of the DMPs model;

[0017] Using the new start and end points of each action and the original start and end points in the preset trajectory as input, calculate the vector from the start point to the end point respectively to obtain the new vector and the original vector;

[0018] Based on the cross product of vectors, solve for the rotation variables of the new vector and the original vector;

[0019] The three-dimensional spatial difference is calculated based on the new starting point and the original starting point to obtain the trajectory initial deviation;

[0020] Set the adaptive start and end points for each action based on the original starting point;

[0021] The adaptive start point and adaptive end point of each action are input into the DMPs model, and combined with the rotation variable and the trajectory start deviation, a new action trajectory is obtained.

[0022] In some embodiments, the step of inputting the adaptive start point and adaptive end point of each action into the DMPs model, and combining the rotation variable and the trajectory start deviation to obtain a new action trajectory specifically includes:

[0023] The adaptive start point and adaptive end point of each action are input into the DMPs model to obtain similar trajectories;

[0024] The rotational variable is applied to the similar trajectory, and the trajectory initial deviation is added to obtain the adaptive trajectory for each action;

[0025] Based on the adaptive trajectories of each action, the actions are arranged sequentially and transition information is generated to obtain a complete new action trajectory.

[0026] In some embodiments, the step of setting the adaptive start and end points of each action based on the original starting point specifically includes:

[0027] The adaptive starting point of each action remains unchanged from the original starting point;

[0028] The adaptive endpoint of each action is obtained by first adding the original vector to the original starting point, then multiplying it by the magnitude of the new vector, and finally dividing it by the magnitude of the original vector.

[0029] This invention also proposes a robot motion planning system based on DMPs, the system comprising:

[0030] The learning module is used to set up DMPs models based on actions and learn preset trajectories of actions;

[0031] The target determination module is used to determine the new start and end points of the action based on the actual environment.

[0032] The trajectory calculation module, based on the DMPs model, performs coupled information analysis by combining the new start and end points of the action with the original start and end points in the preset trajectory to obtain a new action trajectory.

[0033] The control module is used to control the robot's movement based on the new motion trajectory.

[0034] Based on the above scheme, this invention provides a robot motion planning method based on DMPs. By introducing DMPs for motion planning, the characteristics of the original trajectory can be preserved while adaptively generating a new trajectory. Furthermore, by combining mathematical analysis, the changes in three-dimensional information between the new trajectory and the original learned trajectory are analyzed, and the coupling information is learned and compensated, which is then applied to the new trajectory. This enables the generated adaptive trajectory to cope with changes in high-dimensional information, such as rotation, thus increasing the adaptability of the DMPs algorithm. Attached Figure Description

[0035] Figure 1 This is a flowchart of the steps of a robot motion planning method based on DMPs according to the present invention;

[0036] Figure 2 This is a structural block diagram of a robot motion planning system based on DMPs according to the present invention. Detailed Implementation

[0037] Inspection is an indispensable daily task in power systems, especially for indoor relay protection rooms. The various parameters of the indoor protection cabinets are key indicators for monitoring the normal operation of the power system. However, most protection cabinets were not designed for network connectivity when put into operation, thus requiring daily manual inspection and monitoring, significantly increasing the workload. Utilizing a comprehensive mobile robot system to replace manual inspection is a reasonable solution.

[0038] DMPs can quickly acquire features of a given trajectory and adaptively regenerate it. However, DMPs are one-dimensional algorithms; their learning of multi-dimensional information is achieved by multiple DMPs learning each dimension separately. This makes DMPs unable to handle coupling information between multi-dimensional elements, such as rotation in three-dimensional space. This limits the effectiveness of DMPs to similar environments; once the environment undergoes high-dimensional changes, the trajectories generated by DMPs will fail to meet requirements, such as rotation, thus restricting their application in real-world scenarios. Therefore, this invention, based on the DMP model and combined with mathematical calculations, addresses the inherent problem of missing coupling information in DMP algorithms, achieving real-time performance, and constructs a robot motion planning method.

[0039] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0040] It should be noted that, for ease of description, only the parts relevant to the invention are shown in the accompanying drawings. Unless otherwise specified, the embodiments and features described in this application can be combined with each other.

[0041] It should be understood that the terms "system," "apparatus," "unit," and / or "module" used in this application are a method of distinguishing different components, elements, parts, sections, or assemblies at different levels. However, if other terms can achieve the same purpose, they may be replaced by other expressions.

[0042] As indicated in this application and claims, unless the context clearly indicates otherwise, the words "a," "an," "a," and / or "the" are not specifically singular and may include the plural. Generally, the terms "comprising" and "including" only indicate the inclusion of expressly identified steps and elements, which do not constitute an exclusive list, and the method or apparatus may also include other steps or elements. An element defined by the phrase "comprising an..." does not exclude the presence of other identical elements in the process, method, product, or apparatus that includes the element.

[0043] In the description of the embodiments of this application, "a plurality of" refers to two or more. The terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature.

[0044] Furthermore, flowcharts are used in this application to illustrate the operations performed by the system according to embodiments of this application. It should be understood that the preceding or following operations are not necessarily performed precisely in sequence. Instead, the steps can be processed in reverse order or simultaneously. Additionally, other operations can be added to these processes, or one or more steps can be removed from them.

[0045] Reference Figure 1 This is a flowchart illustrating an optional example of the robot motion planning method proposed in this invention. This method can be applied to computer devices. Taking a door-opening task as an example, the robot motion planning method proposed in this embodiment may include, but is not limited to, the following steps:

[0046] Step S1: Set up a DMPs model based on the action and learn the preset trajectory of the action;

[0047] Step S1.1: Set the corresponding number of multidimensional DMPs models according to the number of actions;

[0048] Step S1.2: Control the robot to complete the preset actions in sequence, learn the preset trajectory of each action, and adjust the hyperparameters of the multidimensional DMPs model according to the learning structure.

[0049] Specifically, this also includes presetting the hyperparameters of DMPs before learning; finally, storing the hyperparameters and Gaussian kernel weights of each DMP as model parameters for the DMPs.

[0050] Among them, the small deviation of the target point and the small overall deviation are used as the judgment criteria to ensure that the DMPs are ultimately reproduced well.

[0051] Step S2: Determine the new start and end points of the action based on the actual environment;

[0052] Specifically, the door lock image information is acquired, and the target recognition is performed using the visual information processing module to obtain the door lock recognition information (three-dimensional spatial information). Based on the center of the keyhole, the new start and end points of each action are calculated.

[0053] Step S3: Based on the DMPs model, perform coupling information analysis by combining the new start and end points of the action with the original start and end points in the preset trajectory to obtain a new action trajectory;

[0054] Step S3.1: Read the model parameters of the DMPs model;

[0055] Step S3.2: Using the new start and end points of each action and the original start and end points in the preset trajectory as input, calculate the vector from the start point to the end point respectively to obtain the new vector and the original vector;

[0056] Step S3.3: Based on the cross product of vectors, solve for the rotation variables of the new vector and the original vector;

[0057] Step S3.4: Calculate the three-dimensional spatial difference based on the new starting point and the original starting point to obtain the trajectory initial deviation;

[0058] Step S3.5: Set the adaptive start and end points of each action based on the original starting point;

[0059] Specifically, the starting point of the adaptability of each action remains unchanged from the original starting point; the ending point of the adaptability of each action is obtained by adding the original vector to the original starting point, multiplying it by the magnitude of the new vector, and finally dividing it by the magnitude of the original vector.

[0060] Step S3.6: Input the adaptive start point and adaptive end point of each action into the DMPs model, and combine the rotation variable and the trajectory start deviation to obtain a new action trajectory.

[0061] Step S3.6.1: Input the adaptive start point and adaptive end point of each action into the DMPs model to obtain similar trajectories;

[0062] Step S3.6.2: Apply the rotation variable to the similar trajectory and add the trajectory initial deviation to obtain the adaptive trajectory of each action;

[0063] Step S3.6.3: Based on the adaptive trajectory of each action, arrange the actions sequentially and generate transition information to obtain a complete new action trajectory.

[0064] Step S4: Control the robot's movement according to the new motion trajectory.

[0065] Before step S4, the generated sequence can be a unified action sequence composed of various actions connected by intermediate transition information. Step S4 parses this action sequence, generates robotic arm control commands, and sends them to the robotic arm to complete the new door opening action.

[0066] Based on actual needs, new start and end points for each action are determined as prior information for generating new trajectories. This information, along with the original start and end point information, is input into the post-processing module for coupled information analysis. The analysis results are then applied to the reproduction and post-processing processes of the DMPs (Distributed Dynamic Management Platforms) to generate new adaptive trajectories for each action. Intermediate transition information is generated, combining the various actions into a unified action sequence, which is then applied to the robotic arm, enabling it to complete the door-opening task even when the environment changes.

[0067] Based on the steps above, an example is provided. Assume: the trajectory to be learned is y_d, with one thousand points, and the starting and ending points are s_d and e_d respectively; the new starting and ending points are s_n and e_n.

[0068] s_d=(x1,y1,z1,rx1,ry1,rz1)

[0069] e_d=(x2,y2,z2,rx2,ry2,rz2)

[0070] s_n=(x3,y3,z3,rx3,ry3,rz3)

[0071] e_n = (x4, y4, z4, rx4, ry4, rz4)

[0072] The information of each point is six-dimensional = (x, y, z, rx, ry, rz), where rx, ry, and rz are a way of representing rotation, namely Euler angles. It is equivalent to the rotation represented by a 3x3 matrix, and the two can be converted into each other.

[0073] Decentralization yields their respective vectors: n1 = e_d – s_d = (x2 – x1, y2 – y1, z2 – z1); n2 = e_n – s_n = (x4 – x3, y4 – y3, z4 – z3).

[0074] The new adaptive origin and end points are calculated and input into the model to obtain the y_xin trajectory: s_xin=s_d;e_xin=||n2|| / ||n1|*nl+s_d;y_xin=DMPs(s_xin,e_xin).

[0075] Find the rotation vector and Euler angle of the vectors corresponding to the two starting and ending points (spatial vectors), and rotate n1 to n2:

[0076] 1. Calculate the cross product n, where n = n1 × n2;

[0077] 2. Calculate the rotation angle θ for , θ=n1·n2 / (||n1||||n2||);

[0078] 3. Calculate the unit rotation vector k, k = n / ||n||;

[0079] 4. Calculate the antisymmetric matrix K of the rotation vector k.

[0080] K = [

[0081] [0,-k[2],k[1]],

[0082] [k[2],0,-k[0]],

[0083] [-k[1],k[0],0] ]

[0085] 5. Calculate the rotation matrix R, R = E + sin(θ) * K + (1 – cos(θ)) * (K·K);

[0086] 6. Convert R to Euler angles: rx_t, ry_t, rz_t ← R

[0087] Applying to the new trajectory: For each trajectory point y_xin_i of y_xin, let the starting point be s_xin:

[0088] s_xin=(x1,y1,z1,rx1,ry1,rz1);

[0089] y_xin_i=(xi,yi,zi,rxi,ryi,rzi);

[0090] Let the final trajectory be y_fin, the corresponding points be y_fin_i, and the starting point be s_n.

[0091] Process each point sequentially:

[0092] Three-dimensional information: ni=(xi–x1,yi–y1,zi–z1); y_fin_i=R·ni+s_n;

[0093] Spatial angle (range -180 to 180): y_fin_i=((rxi,ryi,rzi)+(rx_t,ry_t,rz_t)+180)%360–180.

[0094] At this point, y_fin is the adaptive trajectory of each action—the trajectory of six-dimensional information (x,y,z,rx,ry,rz).

[0095] As some possible embodiments, before step S1, the method further includes:

[0096] S0. Arrange the robot system appropriately so that the working space of the robotic arm can completely cover the entire area to be operated. In the zero-force drag mode of the robotic arm, a human teacher breaks down the entire door-opening process into multiple actions and teaches them step by step to obtain the preset trajectory of the actions.

[0097] Specifically, in step S0.1, based on the position and movement characteristics of the door, the robot system is placed on the optical platform and positioned in a reasonable location so that the work of the robotic arm can completely cover the entire area to be operated.

[0098] Step S0.2: Start the robotic arm and gripper and other equipment, and set the robotic arm to work in zero-force drag mode;

[0099] Step S0.3: Break down the door-opening skills according to requirements: rotation, pull, push, flick, move, etc.

[0100] Step S0.4: Under the guidance of a human teacher, drag the robotic arm to smoothly complete the above actions in sequence, and record each action.

[0101] like Figure 2 As shown, a robot motion planning system based on DMPs includes:

[0102] The learning module is used to set up DMPs models based on actions and learn preset trajectories of actions;

[0103] The target determination module is used to determine the new start and end points of the action based on the actual environment.

[0104] The trajectory calculation module, based on the DMPs model, performs coupled information analysis by combining the new start and end points of the action with the original start and end points in the preset trajectory to obtain a new action trajectory.

[0105] The control module is used to control the robot's movement based on the new motion trajectory.

[0106] The content of the above method embodiments is applicable to this system embodiment. The specific functions implemented in this system embodiment are the same as those in the above method embodiments, and the beneficial effects achieved are also the same as those achieved in the above method embodiments.

[0107] A robot motion planning device based on DMPs:

[0108] The robotic arm can be accurately controlled within a given radius, at any given angle, and at any given position to achieve high-precision combined operations. The integrated robotic arm module includes modeling, forward and inverse kinematics, and zero-force dragging functions. The module provides a terminal program to control the robotic arm, enabling parameter modification and robotic arm model simulation. Parameter modification includes communication and safety protection parameter modification. Robotic arm model simulation includes robotic arm model display and movement control. The terminal program includes an APP or PC software. A programming interface is provided, including API interface files, dynamic link libraries, static link libraries, and code usage references, allowing control of the robotic arm to achieve desired movements via a programming language. The robotic arm module is a six-axis robotic arm, preferably based on the Jaka Zu5.

[0109] For the gripper, a two-finger gripper was chosen as the standard choice for the algorithm. It is connected to the robotic arm module through a linkage structure and interacts directly with external objects. The Dahuan gripper is preferred.

[0110] The visual information processing unit uses a high-performance embedded computer as the standard for the algorithm, runs a deep network for target recognition, acquires image information through a wired connection to the camera module, performs recognition, and obtains recognition results containing three-dimensional spatial information. Jetson Xavier NX is preferred.

[0111] The communication unit enables interaction between modules through wireless communication and multi-threading. The communication methods include socket and request.

[0112] The central control unit receives status information from the gripper and robotic arm and issues relevant control commands through the communication unit, and performs comprehensive control scheduling; the central control unit uses a host computer or a laptop computer;

[0113] The functional unit carries modular functional module algorithms for quickly processing intermediate information, including Bézier curve algorithms and B-spline curve algorithms.

[0114] At least one processor;

[0115] At least one memory for storing at least one program;

[0116] When the at least one program is executed by the at least one processor, the at least one processor implements a robot motion planning method based on DMPs as described above.

[0117] The robotic arm module is fixed to the optical platform to ensure its stability. The six-axis center of the robotic arm is connected to the gripper module via a linkage mechanism, and the camera is fixed to the six axes of the robotic arm module via a flange structure. The robotic arm module and gripper module are connected to their respective control boxes via wired connections, providing power and communication. The camera is connected to the vision information processing module via a wired connection, transmitting acquired images to the latter. A WiFi module connects the robotic arm module, gripper module, vision information processing module, and central control module to the same local area network via wired or wireless connections, enabling wireless communication between the modules. The central control module reads and records the status of the robotic arm module and gripper module, obtains the recognition results from the vision information processing module, and uses the DMPs algorithm in conjunction with functional modules to generate control commands, which are then sent to the robotic arm or gripper, completing the functions of recording, learning, and control.

[0118] The content of the above method embodiments is applicable to the device embodiments. The specific functions implemented by the device embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0119] The above is a detailed description of the preferred embodiments of the present invention. However, the present invention is not limited to the embodiments described. Those skilled in the art can make various equivalent modifications or substitutions without departing from the spirit of the present invention. All such equivalent modifications or substitutions are included within the scope defined by the claims of this application.

Claims

1. A method for robot motion planning based on DMPs, characterized in that, Includes the following steps: DMPs models are set based on actions, and preset trajectories of actions are learned; Determine the new start and end points of the action based on the actual environment; Based on the DMPs model, a new motion trajectory is obtained by combining the new start and end points of the action with the original start and end points in the preset trajectory for coupled information analysis. Control the robot's movement based on the new motion trajectory; The step of setting up a DMPs model based on the action and learning the preset trajectory of the action specifically includes: Set the corresponding number of multidimensional DMPs models based on the number of actions; The robot is controlled to perform preset actions in sequence, learn the preset trajectories of each action, and adjust the hyperparameters of the multidimensional DMPs model according to the learning structure. The step of obtaining a new motion trajectory by performing coupled information analysis based on the DMPs model, combining the new start and end points of the action with the original start and end points in the preset trajectory, specifically includes: Read the model parameters of the DMPs model; Using the new start and end points of each action and the original start and end points in the preset trajectory as input, calculate the vector from the start point to the end point respectively to obtain the new vector and the original vector; Based on the cross product of vectors, solve for the rotation variables of the new vector and the original vector; The three-dimensional spatial difference is calculated based on the new starting point and the original starting point to obtain the trajectory initial deviation; Set the adaptive start and end points for each action based on the original starting point; The starting point of the adaptiveness of each action remains unchanged from the original starting point; the ending point of the adaptiveness of each action is obtained by adding the original vector to the original starting point, multiplying it by the magnitude of the new vector, and finally dividing it by the magnitude of the original vector. The adaptive start point and adaptive end point of each action are input into the DMPs model, and combined with the rotation variable and the trajectory start deviation, a new action trajectory is obtained.

2. The method of robot motion planning based on DMPs according to claim 1, wherein, The step of determining the new start and end points of the action based on the actual environment specifically includes: Acquire current environmental information and perform target identification to obtain the identification results; Based on the recognition results, the new start and end points of each action are calculated.

3. The robot motion planning method based on DMPs according to claim 2, characterized in that, The step of inputting the adaptive start and adaptive end points of each action into the DMPs model, and combining the rotation variables and the trajectory start deviation to obtain a new action trajectory, specifically includes: The adaptive start point and adaptive end point of each action are input into the DMPs model to obtain similar trajectories; The rotational variable is applied to the similar trajectory, and the trajectory initial deviation is added to obtain the adaptive trajectory for each action; Based on the adaptive trajectories of each action, the actions are arranged sequentially and transition information is generated to obtain a complete new action trajectory.

4. A robot motion planning system based on DMPs, characterized in that, A robot motion planning method based on DMPs as described in claim 1, comprising: The learning module is used to set up DMPs models based on actions and learn preset trajectories of actions; The target determination module is used to determine the new start and end points of the action based on the actual environment. The trajectory calculation module, based on the DMPs model, performs coupled information analysis by combining the new start and end points of the action with the original start and end points in the preset trajectory to obtain a new action trajectory. The control module is used to control the robot's movement based on the new motion trajectory.

Images