Motion track optimization method and device, mechanical arm and storage medium

Abstract

The invention relates to the technical field of intelligent control, and discloses a motion trail optimization method and device, a mechanical arm and a storage medium, and the motion trail optimization method comprises the steps that when an instruction for executing an operation task is received, all teaching trails for the operation task are obtained; feature extraction processing is conducted according to the teaching tracks, and track features of the tail end pose of the mechanical arm at all moments are obtained; and performing continuous iterative optimization processing on the trajectory features to obtain an optimal motion trajectory for executing the job task. By means of the motion trail planning method, the process of automatically finding the optimal motion trail based on the teaching trail can be achieved, and the intelligent degree of the mechanical arm is improved.

Description

Technical Field

[0001] The present application relates to the field of intelligent control technology, and in particular to a motion trajectory optimization method, device, robotic arm and storage medium. Background Art

[0002] A robotic arm, also known as a robot arm, is a multi-jointed mechanical device used to perform a variety of tasks, mimicking the structure and movement of a human arm. It consists of multiple connected joints and actuators, and usually has a structure similar to the shoulder, elbow, wrist, and fingers. The robotic arm can be programmed to perform a variety of complex movements and tasks, from simple object handling to highly precise operations, and is therefore widely used in industry, medicine, military, space exploration and other fields.

[0003] In scenarios where robotic arms are used, due to the complexity and diversity of actual scenarios, there will inevitably be various task constraints when the robotic arm performs tasks. For example, when holding a cup of water, the end of the robotic arm needs to maintain a certain posture at all times; in spray welding scenarios, the end of the robotic arm needs to strictly follow the specified path. According to the complexity of the task constraint description, it can be divided into two types: mathematically describable and difficult to describe. For mathematically describable task constraint problems, there are currently many studies, which are relatively mature in simple scenarios, but for difficult to describe task constraint problems (such as asking the robotic arm to unscrew the door lock and then push the door open), how to enable the robotic arm to automatically find the optimal motion trajectory route is still a hot research direction. Summary of the invention

[0004] The embodiments of the present application aim to provide a motion trajectory optimization method, device, robotic arm and storage medium to solve the technical problem in the prior art of how to enable the robotic arm to automatically find the optimal motion trajectory route when performing a difficult-to-describe task.

[0005] In a first aspect, an embodiment of the present application provides a motion trajectory planning method, which is applied to a robotic arm. The method includes: when receiving an instruction to perform a work task, obtaining each teaching trajectory for the work task; performing feature extraction processing based on the each teaching trajectory to obtain the trajectory characteristics of the end position of the robotic arm at each moment; and performing continuous iterative optimization processing on the trajectory characteristics to obtain the optimal motion trajectory for performing the work task.

[0006] Optionally, the feature extraction processing is performed according to each teaching trajectory to obtain the trajectory characteristics of the end posture of the robot arm at each moment, including: performing coordinate transformation processing and time alignment processing on each teaching trajectory to obtain the teaching trajectory after time alignment; performing feature extraction processing on the teaching trajectory after time alignment to obtain the trajectory characteristics of the end posture of the robot arm at each moment.

[0007] Optionally, the coordinate transformation and time alignment processing are performed on the various teaching trajectories to obtain the time-aligned teaching trajectories, including: extracting the relative continuous posture trajectory of the various teaching trajectories at the end of the robot arm relative to the base of the robot arm, and converting the relative continuous posture trajectory into a Cartesian space coordinate system to obtain the absolute continuous posture trajectory of the end of the robot arm in the world coordinate system; performing time alignment processing on the absolute continuous posture trajectory of the end of the robot arm in the world coordinate system to obtain the time-aligned teaching trajectory.

[0008] Optionally, the absolute continuous posture trajectory of the end of the robot arm in the world coordinate system is time-aligned to obtain the teaching trajectory after time alignment, including: based on a dynamic time warping algorithm, the absolute continuous posture trajectory of the end of the robot arm in the world coordinate system is time-aligned to obtain each teaching data after time alignment.

[0009] Optionally, the extracting of the relative continuous pose trajectory of each teaching trajectory at the end of the robot arm relative to the robot arm base, and performing a Cartesian space coordinate system transformation on the relative continuous pose trajectory to obtain the absolute continuous pose trajectory of the end of the robot arm in the world coordinate system includes: obtaining the relative continuous pose trajectory of each teaching trajectory at the end of the robot arm relative to the robot arm base according to forward kinematics; performing a Cartesian space coordinate system transformation according to the positional relationship of the work task object perceived by the camera relative to the robot arm base to obtain the absolute continuous pose trajectory of the end of the robot arm in the world coordinate system.

[0010] Optionally, the teaching trajectory after time alignment is subjected to feature extraction processing to obtain the trajectory characteristics of the end posture of the robot arm at each moment, including: performing feature description on each continuous posture trajectory after time alignment based on a Gaussian mixture model algorithm to obtain the trajectory characteristics of the end posture of the robot arm at each moment.

[0011] Optionally, the feature description of each continuous posture trajectory after the time alignment is performed based on the Gaussian mixture model algorithm to obtain the trajectory characteristics of the end posture of the robot arm at each moment, including: based on the Gaussian mixture model algorithm, each continuous posture trajectory after the time alignment is modeled using a joint probability distribution to obtain a feature description of each continuous posture trajectory after the time alignment; based on Gaussian mixture regression, the feature description is subjected to task constraint regression processing to obtain the trajectory characteristics of the end posture of the robot arm at each moment.

[0012] Optionally, the trajectory features include: the mean and covariance of the teaching trajectory at any time, and the mean and covariance of the teaching trajectory are obtained from the position and posture information of the end of the robot arm.

[0013] Optionally, the continuous iterative optimization processing of the trajectory characteristics to obtain the optimal motion trajectory for executing the operation task includes: taking the trajectory characteristics as task constraints, and calculating obstacle avoidance constraints and smoothness constraints; and continuously iterative optimization processing based on the task constraints, the obstacle avoidance constraints and the smoothness constraints to obtain the optimal motion trajectory for executing the operation task.

[0014] Optionally, the continuous iterative optimization processing is performed according to the task constraints, the obstacle avoidance constraints and the smoothness constraints to obtain the optimal motion trajectory for executing the operation task, including: according to the task constraints, the obstacle avoidance constraints and the smoothness constraints, the optimal motion trajectory for executing the operation task is obtained in a staged iterative optimization manner; wherein the staged iterative optimization manner refers to: using the gradients of the obstacle avoidance constraints and the smoothness constraints for every odd-numbered iterations, and using the gradient of the task constraints for every even-numbered iterations, and stopping the iteration when a preset iteration termination condition is met.

[0015] In a second aspect, a motion trajectory planning device is provided, which includes: an acquisition module, which is used to acquire each teaching trajectory for the work task when receiving an instruction to perform the work task; a processing module, which is used to perform feature extraction processing based on the each teaching trajectory to obtain the trajectory characteristics of the end position of the robot arm at each moment; the processing module is also used to perform continuous iterative optimization processing on the trajectory characteristics to obtain the optimal motion trajectory for performing the work task.

[0016] In a third aspect, a robotic arm is provided, comprising a memory and a processor, wherein the memory is connected to the processor, and the processor is used to execute one or more computer programs stored in the memory, and when the processor executes the one or more computer programs, the robotic arm implements the method described in the first aspect.

[0017] In a fourth aspect, a computer-readable storage medium is provided, wherein the computer-readable storage medium stores a computer program, wherein the computer program includes program instructions, and when the program instructions are executed by a processor, the processor executes the method as described in the first aspect.

[0018] In the motion trajectory planning method provided in the embodiment of the present application, when the robot arm receives an instruction to perform a work task, each teaching trajectory for the work task is obtained, and feature extraction processing is performed based on each teaching trajectory to obtain the trajectory characteristics of the end position of the robot arm at each moment, and iterative optimization processing is performed based on the trajectory characteristics to obtain the optimal motion trajectory for performing the work task. The above method extracts the features of the teaching trajectory at each moment, and constructs an optimization problem based on the trajectory features, thereby performing continuous iterative optimization to obtain the optimal motion trajectory, thereby realizing the process of automatically finding the optimal motion trajectory based on the teaching trajectory, and improving the intelligence level of the robot arm. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] One or more embodiments are exemplarily described by pictures in the corresponding drawings, and these exemplified descriptions do not constitute limitations on the embodiments. Elements with the same reference numerals in the drawings represent similar elements, and unless otherwise stated, the figures in the drawings do not constitute proportional limitations.

[0020] Figure 1 A schematic diagram of the structure of a mechanical arm provided in an embodiment of the present application;

[0021] Figure 2 A schematic diagram of a motion trajectory planning method provided in an embodiment of the present application;

[0022] Figure 3 A schematic diagram of a flow chart of another motion trajectory planning method provided in an embodiment of the present application;

[0023] Figure 4 A schematic diagram of the effect of a teaching trajectory before time alignment provided in an embodiment of the present application;

[0024] Figure 5 A schematic diagram of the structure of a motion trajectory planning device provided in an embodiment of the present application;

[0025] Figure 6 A schematic diagram of the structure of another robotic arm provided in an embodiment of the present application. DETAILED DESCRIPTION

[0026] In order to make the purpose, technical solutions and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not intended to limit the present application. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in the field without making creative work are within the scope of protection of the present application.

[0027] It should be noted that, if there is no conflict, the various features in the embodiments of the present application can be combined with each other, all within the scope of protection of the present application. In addition, although the functional modules are divided in the device schematic diagram and the logical order is shown in the flow chart, in some cases, the steps shown or described can be performed in a different order from the module division in the device or the flow chart. Furthermore, the words "first", "second", "third", etc. used in this application do not limit the data and execution order, but only distinguish the same items or similar items with basically the same functions and effects.

[0028] To describe the present application in more detail, a robotic arm involved in the present application is first introduced. The robotic arm involved in the present application is a mechanical system with multiple joints and actuators, which can imitate the movements and functions of human arms. Figure 1 , is a schematic diagram of the structure of a robotic arm provided in an embodiment of the present application. Figure 1 The robot arm 10 shown may include at least: a base 101, a joint 102, a connecting rod 103, an end 104, and a control system.

[0029] The base 101 is the installation foundation of the robot arm 10 , and is usually fixed on a workbench, the ground or other supporting structures. The base 101 provides overall stability and support for the robot arm 10 .

[0030] There may be multiple joints 102, and the robot 102 is composed of multiple joints 102, each of which allows the robot 10 to move in a specific direction. The joints 102 can be divided into rotary joints, sliding joints, universal joints, etc. according to their rotation direction. In some embodiments, the joints can be connected by connecting rods, which can determine the length and shape of the robot 10.

[0031] The connecting rod 103 is a rigid structure connecting each joint 102, which determines the length and shape of the robot arm. As a feasible implementation, the material of the connecting rod 103 is preferably light but strong to ensure the stability and load capacity of the robot arm 10.

[0032] The end 104 is located at the end of the robot arm and can be integrated with an end effector, which can be used as the "hand" of the robot arm 10 to perform specific tasks. The end effector can be a gripper, a suction cup, a welding gun, a pliers, a sensor, etc. ( Figure 1 The design of the end effector determines the function of the robot arm, enabling it to adapt to different working environments and tasks. It is understandable that in addition to the above-mentioned end effector, the robot arm 10 can also be equipped with other actuators according to the requirements of the work task.

[0033] The robot 10 may also be integrated with a control system, which is equivalent to the brain of the robot 10, and realizes precise control of the posture and position of the robot by controlling the movement of each joint 102 and the end 104. The control system may be a combination of hardware and software algorithms for realizing automatic, semi-automatic or remote control, and may include a processor and a memory at the hardware level.

[0034] In some feasible implementations, the robot arm 10 may also be equipped with a camera to record the trajectory data of the task for analysis by the control system. It is understandable that the structure of the above-mentioned robot arm 10 is only an example, and different types of robot arms may have different structures. In actual application scenarios, components of the robot arm can be added as needed.

[0035] The motion trajectory planning method shown in this application can be implemented by the above-mentioned robot arm 10. Figure 2 , is a flow chart of a motion trajectory planning method involved in this application. Figure 2 The motion trajectory planning method shown mainly includes two stages: teaching learning and trajectory optimization. In the teaching learning stage, the obtained teaching trajectory can be time-aligned, so as to obtain the trajectory characteristics of each moment based on the time-aligned teaching trajectory. In the trajectory optimization stage, task constraints can be constructed based on the trajectory characteristics obtained above, and then the optimization problem can be constructed, and the optimal motion trajectory can be obtained through continuous iterative optimization.

[0036] The following first introduces the teaching learning stage: the teaching learning stage mainly includes three steps: time alignment 201, task modeling 202, and Gaussian regression 203. In the time alignment 201 step, when performing a task, the robot arm can first obtain the teaching trajectory for the task, and then use the dynamic time warping algorithm to time align each teaching trajectory. In the task modeling 202 stage, the robot arm can use the Gaussian mixture model algorithm to construct a joint probability distribution to model the task, that is, to characterize the teaching trajectory after time alignment. In the Gaussian regression 203 stage, the robot arm can obtain the trajectory characteristics of the teaching trajectory at each moment based on Gaussian mixture regression, such as the mean and covariance of joint angles, joint velocities, and end postures, where the mean represents the desired task constraints, and the covariance represents the degree of constraint on the task.

[0037] Next, the trajectory optimization stage is introduced: the trajectory optimization stage mainly includes two steps: constrain construction 211 and iterative optimization 212. In the constraint construction 211 stage, the robot can first use the trajectory features obtained above to form task constraint items, and then combine trajectory smoothing and trajectory obstacle avoidance to add smoothing constraint items and obstacle avoidance constraint items to construct the task constraint problem. In the iterative optimization 201 stage, the robot can continuously iterate and optimize the task constraint problem constructed above to obtain the optimal motion trajectory.

[0038] It can be seen that the motion trajectory planning method shown in the embodiment of the present application, by combining teaching learning with trajectory optimization, quickly obtains the optimal motion trajectory that meets the task requirements through iterative optimization of the task constraint items learned based on the teaching trajectory, which is easy to operate, has strong engineering practicality, and a high degree of automation. In addition, the embodiment of the present application utilizes the Gaussian mixture model algorithm and Gaussian mixture regression to extract trajectory features from multiple teaching trajectories, effectively solving the problem that complex task constraints are difficult to describe mathematically, so that the robot arm can better find the optimal motion trajectory when performing complex task tasks, further improving the automation and intelligence of the robot arm.

[0039] See below Figure 3 , which is another motion trajectory planning method provided in an embodiment of the present application. Figure 3 The motion trajectory planning method shown may include:

[0040] S301: When receiving an instruction to execute a work task, obtaining each teaching trajectory for the work task.

[0041] As a feasible implementation, the operation task may be a mathematically describable operation task or a difficult to describe and complex operation task. For example, the operation task may be to let the robot arm unscrew the door lock and then push the door open.

[0042] It should be noted that the teaching trajectory is a motion trajectory for manually controlling the robot arm to operate the robot arm to complete a specific task. The teaching trajectory is a key component of the programming and control of the robot arm. It provides a description of the robot arm's movement and behavior, allowing the robot arm to learn and imitate human operations, or perform predetermined tasks based on specific inputs.

[0043] As a feasible implementation, the teaching trajectory can be obtained by dragging or teleoperating the robot arm. The teaching trajectory can include the joint angle or end position information of the robot arm operation trajectory, and then the robot arm can play back these data to perform the same or similar tasks.

[0044] As a feasible implementation, there can be multiple teaching trajectories for the operation task, and all of them correspond to the operation task. The robot arm can obtain the teaching trajectory according to the type of the operation task. For example, the type of the operation task is to let the robot arm pick up a container. Then, whether the container is a coffee cup or a tea cup, it is classified as the same type. The teaching trajectory for picking up the coffee cup or the tea cup is also considered to be a teaching trajectory of the same type.

[0045] S302: Perform feature extraction processing according to each teaching trajectory to obtain the trajectory features of the end position of the robot arm at each moment.

[0046] For example, multiple robot arm joint trajectories can be obtained by dragging or remote operation. The robot arm joint trajectories can be the various teaching trajectories for the work task. The robot arm obtains the trajectory characteristics of the work task corresponding to the robot arm end posture at each moment through feature extraction processing based on the various teaching trajectories. The trajectory characteristics can be described in the form of mean and covariance.

[0047] As a feasible implementation, the feature extraction process may include time alignment, task modeling, and Gaussian regression steps.

[0048] In one embodiment, the step S302 may include:

[0049] S3021: performing coordinate transformation processing and time alignment processing on each teaching trajectory to obtain a teaching trajectory after time alignment.

[0050] It should be noted that, since the end effector is usually used at the end of the robot to perform actual operations (such as the end gripper performs the clamping task, and the end spray gun performs the spraying task), its trajectory characteristics are mainly reflected in the posture transformation of the end, and are relative to the task object. The obtained teaching trajectory is usually the relative trajectory of the end of the robot relative to the robot base. In order to facilitate the automation of the robot, the teaching trajectory can be processed by coordinate conversion.

[0051] This article stipulates that the task object refers to the specific object that this task is directed to, such as water cups, door handles, tables and chairs, etc.

[0052] Furthermore, the teaching trajectory obtained by manual dragging is usually not synchronized in time. In order to enable the robot arm to obtain more accurate trajectory characteristics, the teaching trajectory can be time-aligned.

[0053] As a feasible implementation, the S3021 step may include: extracting the relative continuous posture trajectory of each teaching trajectory at the end of the robot arm relative to the robot arm base, and converting the relative continuous posture trajectory into a Cartesian space coordinate system to obtain an absolute continuous posture trajectory of the end of the robot arm in a world coordinate system; performing time alignment processing on the absolute continuous posture trajectory of the end of the robot arm in the world coordinate system to obtain a teaching trajectory after time alignment.

[0054] It should be noted that the coordinate conversion processing for the teaching trajectory can be performed based on the Cartesian space coordinate system to convert the relative continuous posture trajectory of each teaching trajectory at the end of the robot arm relative to the robot arm base into an absolutely continuous posture trajectory of the end of the robot arm in the world coordinate system.

[0055] In one embodiment, the extracting of the relative continuous pose trajectory of each teaching trajectory at the end of the robot arm relative to the robot arm base, and performing a Cartesian space coordinate system transformation on the relative continuous pose trajectory to obtain the absolute continuous pose trajectory of the end of the robot arm in the world coordinate system includes: obtaining the relative continuous pose trajectory of each teaching trajectory at the end of the robot arm relative to the robot arm base according to forward kinematics; performing a Cartesian space coordinate system transformation according to the positional relationship of the work task object perceived by the camera relative to the robot arm base to obtain the absolute continuous pose trajectory of the end of the robot arm in the world coordinate system.

[0056] As a feasible implementation, the camera may be configured on the robotic arm to record trajectory data of the robotic arm performing a task.

[0057] For example, the robot arm can obtain M teaching trajectories and record the teaching trajectories as Among them, t represents time, θ represents angle, n represents the discretized time length, and m represents the number of teaching trajectories. Then, the relative continuous posture trajectory of the tool end relative to the robot arm base can be obtained through forward kinematics. The relative continuous posture trajectory can be recorded as Combined with the position relationship of the work object relative to the base perceived by the camera b T wp , the absolute continuous posture trajectory of the end of the robot arm relative to the world coordinate system can be converted, and the absolute continuous posture trajectory can be recorded as D p,ob Specifically, it can be expressed by the following formula 1

[0058]

[0059] Among them, p n,m ,q n,m They respectively represent the position of the end of the robotic arm and the posture of the end of the robotic arm described by quaternion.

[0060] In one embodiment, the absolute continuous posture trajectory of the end of the robot arm in the world coordinate system is time-aligned to obtain the teaching trajectory after time alignment, including: based on a dynamic time warping algorithm, the absolute continuous posture trajectory of the end of the robot arm in the world coordinate system is time-aligned to obtain each teaching data after time alignment.

[0061] It should be noted that no matter whether manual dragging or remote operation is used, the final recorded teaching trajectory may have speed inconsistency problems, such as Figure 4 In order to accurately obtain the trajectory characteristics of the currently acquired teaching trajectory at each moment, the robot arm can perform time alignment on multiple teaching trajectories after coordinate transformation processing.

[0062] As a feasible implementation method, the time alignment process shown in this application can be implemented based on the dynamic time warping algorithm (Dynamic Time Warping, DTW). The dynamic time warping algorithm is a dynamic programming algorithm that calculates the similarity of two time series, especially sequences of different lengths. It calculates the similarity between two time series by extending and shortening the time series. Its essence is a dynamic programming algorithm. By extending and shortening the time series, the dynamic time warping algorithm finally outputs multiple teaching trajectories Dp with a time length of n after alignment, which can be expressed by the following formula 2:

[0063]

[0064] Among them, t represents time, p represents the position of the end of the robot arm, q represents the posture of the end of the robot arm, which is described by quaternion, n represents the discretized time length, and m represents the number of teaching trajectories.

[0065] As a feasible implementation method, using quaternions to describe postures can solve the universal lock problem well. However, there is a certain coupling between its various elements, and it is difficult to generalize the postures in all directions. Therefore, the embodiment of the present application adopts the quaternion q n,m Projected into Euclidean space for processing.

[0066] Specifically, take the auxiliary quaternion Using the logarithmic function log(·) Switch to Euclidean space:

[0067]

[0068] Thus, the converted time-aligned teaching trajectory can be obtained:

[0069]

[0070] S3022: Perform feature extraction processing on the time-aligned teaching trajectory to obtain the trajectory features of the end position of the robot arm at each moment.

[0071] As a feasible implementation, the feature extraction process may include two parts: task modeling and Gaussian regression. In the task modeling part, a Gaussian mixture model may be used to model the aligned teaching trajectory so as to better handle complex tasks that are difficult to describe. In the Gaussian regression part, the trajectory features of the teaching data may be obtained by using a Gaussian mixture regression algorithm.

[0072] In one embodiment, the step S3022 may include: performing feature description on each continuous posture trajectory after the time alignment based on a Gaussian mixture model algorithm to obtain the trajectory features of the end posture of the robotic arm at each moment.

[0073] It should be noted that the Gaussian Mixed Model (GMM) algorithm refers to a linear combination of multiple Gaussian distribution functions. In theory, the Gaussian Mixed Model algorithm can fit any type of distribution and is usually used to solve the situation where the data in the same set contains multiple different distributions.

[0074] In one embodiment, the feature description of each continuous posture trajectory after the time alignment is performed based on the Gaussian mixture model algorithm to obtain the trajectory characteristics of the end posture of the robot arm at each moment, including: based on the Gaussian mixture model algorithm, each continuous posture trajectory after the time alignment is modeled using a joint probability distribution to obtain a feature description of each continuous posture trajectory after the time alignment; based on Gaussian mixture regression, the feature description is subjected to task constraint regression processing to obtain the trajectory characteristics of the end posture of the robot arm at each moment.

[0075] In one embodiment, the trajectory features include: the mean and covariance of the teaching trajectory at any time, and the mean and covariance of the teaching trajectory are obtained from the position and posture information of the end of the robot arm.

[0076] It should be noted that the mean can represent the expected terminal trajectory, and the covariance can represent the degree of constraint of the trajectory at each moment.

[0077] For example, the robot arm can use the Gaussian mixture model algorithm to model each teaching trajectory after time alignment using the joint probability distribution P(t,ζ), that is,

[0078]

[0079] Where t and ζ represent the time and trajectory characteristics respectively (here, the end position relative to the task object), C is the number of Gaussian components in the Gaussian mixture model algorithm, and π c , μ c and Σ c Represent the prior probability, mean and covariance of the cth Gaussian component, satisfying 0≤π c ≤1.

[0080] In a feasible implementation, the parameters of the Gaussian mixture model algorithm can be iteratively optimized by an expectation-maximization (EM) algorithm. The specific steps are as follows:

[0081] Define the number of Gaussian components C and set π for each component c c , μ c and Σ c The initial value of is:

[0082]

[0083] p(η k |π,μ,Σ) represents the k=n*mth observation data η under the current parameters kThe probability of existence is the observed value of the mth teaching trajectory at the nth time. The total number of observed data after time alignment is K = N*M.

[0084] η k =η n,m ={t n,m ,ζ n,m} (Formula 7)

[0085] The embodiment of the present application considers both position and posture, so each n k It can be represented by a 7-dimensional vector. According to the current parameter π c , μ c and Σ c , calculate the posterior probability γ nc , represents the kth observation data η k The probability of belonging to sub-model c can be calculated using the following formula 8:

[0086]

[0087] The posterior probability γ calculated according to the above formula 8 nc Update parameter π c , μ c and Σ c :

[0088]

[0089]

[0090]

[0091] Calculate the log-likelihood function for Equation 6:

[0092]

[0093] Next, it can be checked whether the parameters or the log-likelihood function converge. If not, return to the step shown in formula 8 and iterate until convergence.

[0094] For another example, in the processing of Gaussian regression, it can be achieved by the following calculation method: According to formula 7, the observation data η contains two parts: time t and trajectory feature ζ. Therefore, the mean and covariance of each component obtained after EM iteration can also be written as a combination of these two parts:

[0095]

[0096] For any time t * , Gaussian mixture regression (GMR) can be used to predict the corresponding trajectory feature ζ * The conditional probability distribution of :

[0097]

[0098] in,

[0099]

[0100]

[0101]

[0102] Furthermore, Formula 14 can be transformed into:

[0103]

[0104] in

[0105]

[0106]

[0107] So far, the initial teaching trajectory After a series of coordinate transformation and feature extraction, the trajectory characteristics of the teaching data can be finally obtained, that is, the mean value at any time n∈[0,N] and covariance in Represents the desired task constraints, which are the position p(n) of the end of the manipulator and the posture of the end of the manipulator described in Euclidean space composition; It reflects the degree of trajectory constraints on the work tasks in various dimensions.

[0108] S303: Continuously iterate and optimize the trajectory features to obtain an optimal motion trajectory for executing the task.

[0109] As a feasible implementation method, the robotic arm can acquire trajectory features, i.e., the expected terminal trajectory (mean) and the degree of trajectory constraints (covariance) of the work task in various dimensions, and can add new environmental factors. By using trajectory optimization methods, the learned task features can be used as task constraint optimization items, and different weights can be given according to the degree of trajectory constraints at each moment. At the same time, obstacle avoidance constraints and smoothness constraints can be added to iterate the optimal solution.

[0110] In one embodiment, the trajectory features are continuously iteratively optimized to obtain the optimal motion trajectory for executing the operation task, including: taking the trajectory features as task constraints, and calculating obstacle avoidance constraints and smoothness constraints; and continuously iteratively optimizing the task constraints, the obstacle avoidance constraints, and the smoothness constraints to obtain the optimal motion trajectory for executing the operation task.

[0111] For example, the embodiment of the present application can use the trajectory feature as a task constraint item, improve it on the basis of the CHOMP (CHOM-Patterns) algorithm, obtain a smooth obstacle avoidance operation trajectory that meets the task constraints through iterative optimization, and drive the robot to effectively perform the task. The optimization-based algorithm first needs to construct an objective function. The embodiment of the present application can integrate three attributes: smoothness constraint item, obstacle avoidance constraint item, and task constraint item, which can be specifically described by the following formula 21:

[0112] u(ξ)=f task (ξ)+λ1f obs (ξ)+λ2f smooth (ξ) (Formula 21)

[0113] Among them, ξ is the optimization term, which represents the joint state of the continuous trajectory; λ1 and λ2 are constants, which represent the obstacle avoidance term f obs (ξ) and the smoothing term f smooth (ξ) right of occupation; f task (ξ) represents the task constraint, i.e., the initial teaching trajectory to the desired end pose of error.

[0114] For another example, the construction of task constraints can be described based on the following formula: Medium Described by Euclidean space, in order to facilitate the calculation of errors, the algorithm first performs an exponential transformation to convert it to a quaternion description:

[0115]

[0116] Therefore, the expected terminal position at time n∈[0,N] is recorded as x dn =[p dn q dn ]. According to the motion trajectory ξ(θ1,θ2…θ N ), the actual position x of the tool end relative to the work object at time n∈[0,N] can be obtained through forward kinematics n :

[0117]

[0118] Calculate pose error Covariance It directly represents the degree of constraint of the current task in each dimension. Therefore, the algorithm first converts the posture term in Δx to the Euclidean space, and combines the covariance to calculate the task constraint term f task (ξ):

[0119]

[0120] For another example, the construction of the smooth constraint term can be described based on the following formula 25: Let the smooth constraint term be f smooth (ξ), then:

[0121]

[0122] For another example, the obstacle avoidance constraint is used to avoid interference between the planned trajectory and the environment and itself. In order to quickly calculate the distance information, the embodiment of the present application can first construct a distance length for the working environment, and use a spherical bounding box to construct an envelope for each motion joint of the robot arm. The construction of the obstacle avoidance constraint can be described based on the following formula 26:

[0123]

[0124] Among them, x(θ n ,p) is the partial forward kinematics, that is, the focus point (center of the bounding ball) p∈P in the joint link at the joint state θ n c(·) represents the distance cost corresponding to each focus point, which can be directly obtained by looking up the pre-calculated environmental distance field.

[0125] Based on the above method, the task constraint item, the obstacle avoidance constraint item and the smoothness constraint item can be calculated.

[0126] In one embodiment, the task constraints, the obstacle avoidance constraints and the smoothness constraints are continuously iterated and optimized to obtain the optimal motion trajectory for executing the operation task, including: obtaining the optimal motion trajectory for executing the operation task in a staged iterative optimization manner according to the task constraints, the obstacle avoidance constraints and the smoothness constraints.

[0127] It should be noted that the embodiment of the present application obtains the task constraint item, the obstacle avoidance constraint item and the smoothness constraint item (specifically, it can be the cost function and its gradient of the above three constraint items). Since the gradients of the above three optimization items used at the same time may conflict with the calculated motion trajectory, resulting in poor final convergence. Therefore, the embodiment of the present application can adopt a staged optimization method to obtain the optimal motion trajectory for executing the task, thereby improving the accuracy of the final result.

[0128] Among them, the staged iterative optimization method means: using the gradients of the obstacle avoidance constraint item and the smoothness constraint item in every odd-numbered iteration, using the gradient of the task constraint item in every even-numbered iteration, and stopping the iteration when the preset iteration termination condition is met.

[0129] For example, the phased iterative optimization method can be expressed by the following formula 27:

[0130]

[0131] Use obstacle avoidance f at every odd iteration obs (ξ) and the smoothing term f smooth The gradient of (ξ) is used for even-numbered iterations. The task constraint f is used for even-numbered iterations. tsak The gradient of (ξ).

[0132] As a feasible implementation, the iteration termination condition may be set as:

[0133] (1) Reach the maximum number of iterations iter max , i>iter max ;

[0134] (2) In continuous iterations, the total target cost u(ξ) decreases to less than the threshold diff max ,u(ξ i )-u(ξ i+1 )<diff max .

[0135] It can be seen that the motion trajectory planning method provided in the embodiment of the present application obtains each teaching trajectory for the work task when the robot arm receives an instruction to perform a work task, and performs feature extraction processing based on each teaching trajectory to obtain the trajectory characteristics of the end position of the robot arm at each moment, and performs iterative optimization processing based on the trajectory characteristics to obtain the optimal motion trajectory for performing the work task. The above method extracts the features of the teaching trajectory at each moment, and constructs an optimization problem based on the trajectory features, thereby performing continuous iterative optimization to obtain the optimal motion trajectory, thereby realizing the process of automatically finding the optimal motion trajectory based on the teaching trajectory, and improving the intelligence level of the robot arm.

[0136] The following is an introduction to a motion trajectory planning device involved in this application. Figure 5 , is a motion trajectory planning device 50 provided in an embodiment of the present application, and the motion trajectory planning device 50 may include:

[0137] The acquisition module 501 is used to acquire each teaching trajectory for the operation task when receiving an instruction to execute the operation task;

[0138] A processing module 502 is used to perform feature extraction processing according to each teaching trajectory to obtain the trajectory features of the end position of the robot arm at each moment;

[0139] The processing module 502 is further used to perform continuous iterative optimization processing on the trajectory features to obtain an optimal motion trajectory for executing the task.

[0140] As a possible design, the processing module 502 is used to perform feature extraction processing according to the various teaching trajectories to obtain the trajectory characteristics of the end position of the robot arm at each moment, and is specifically used to: perform coordinate transformation processing and time alignment processing on the various teaching trajectories to obtain the teaching trajectories after time alignment; perform feature extraction processing on the teaching trajectories after time alignment to obtain the trajectory characteristics of the end position of the robot arm at each moment.

[0141] As a possible design, the processing module 502 is used to perform coordinate transformation and time alignment processing on each of the teaching trajectories. When the teaching trajectory after time alignment is obtained, it is specifically used to: extract the relative continuous posture trajectory of each teaching trajectory at the end of the robot arm relative to the base of the robot arm, and transform the relative continuous posture trajectory into a Cartesian space coordinate system to obtain an absolute continuous posture trajectory of the end of the robot arm in the world coordinate system; perform time alignment processing on the absolute continuous posture trajectory of the end of the robot arm in the world coordinate system to obtain the teaching trajectory after time alignment.

[0142] As a possible design, the processing module 502 is used to extract the relative continuous pose trajectory of each teaching trajectory at the end of the robot arm relative to the robot arm base, and perform Cartesian space coordinate system transformation on the relative continuous pose trajectory to obtain the absolute continuous pose trajectory of the end of the robot arm in the world coordinate system. It is specifically used for: obtaining the relative continuous pose trajectory of each teaching trajectory at the end of the robot arm relative to the robot arm base according to forward kinematics; performing Cartesian space coordinate system transformation according to the positional relationship of the work task object perceived by the camera relative to the robot arm base to obtain the absolute continuous pose trajectory of the end of the robot arm in the world coordinate system.

[0143] As a possible design, the processing module 502 is used to perform feature extraction processing on the teaching trajectory after time alignment to obtain the trajectory characteristics of the end posture of the robot arm at each moment, and is specifically used to: perform feature description on each continuous posture trajectory after time alignment based on a Gaussian mixture model algorithm to obtain the trajectory characteristics of the end posture of the robot arm at each moment.

[0144] As a possible design, the processing module 502 is used to perform a feature description on each continuous posture trajectory after the time alignment based on a Gaussian mixture model algorithm to obtain the trajectory characteristics of the end posture of the robot arm at each moment. It is specifically used to: based on the Gaussian mixture model algorithm, model each continuous posture trajectory after the time alignment using a joint probability distribution to obtain a feature description of each continuous posture trajectory after the time alignment; based on Gaussian mixture regression, perform task constraint regression processing on the feature description to obtain the trajectory characteristics of the end posture of the robot arm at each moment.

[0145] As a possible design, the trajectory features include: the mean and covariance of the teaching trajectory at any time, and the mean and covariance of the teaching trajectory are obtained from the position and posture information of the end of the robot arm.

[0146] As a possible design, the processing module 502 is used to perform time alignment processing on the absolute continuous posture trajectory of the end of the robot arm in the world coordinate system, and when obtaining the teaching trajectory after time alignment, it is specifically used to: perform time alignment processing on the absolute continuous posture trajectory of the end of the robot arm in the world coordinate system based on the dynamic time warping algorithm, and obtain various teaching data after time alignment.

[0147] As a possible design, the processing module 502 is used to perform continuous iterative optimization processing on the trajectory features to obtain the optimal motion trajectory for performing the operation task, specifically for: using the trajectory features as task constraints, and calculating obstacle avoidance constraints and smoothness constraints; performing continuous iterative optimization processing based on the task constraints, the obstacle avoidance constraints and the smoothness constraints to obtain the optimal motion trajectory for performing the operation task.

[0148] As a possible design, the processing module 502 is used to perform continuous iterative optimization processing according to the task constraints, the obstacle avoidance constraints and the smoothness constraints to obtain the optimal motion trajectory for executing the operation task. It is specifically used to: obtain the optimal motion trajectory for executing the operation task in a staged iterative optimization manner according to the task constraints, the obstacle avoidance constraints and the smoothness constraints; wherein the staged iterative optimization manner refers to: using the gradients of the obstacle avoidance constraints and the smoothness constraints for every odd-numbered iterations, and using the gradient of the task constraints for every even-numbered iterations, and stopping the iteration when a preset iteration termination condition is met.

[0149] It can be seen that through the motion trajectory planning device provided in the embodiment of the present application, when the robot arm receives an instruction to perform a work task, it can obtain each teaching trajectory for the work task, and perform feature extraction processing based on each teaching trajectory to obtain the trajectory characteristics of the end position of the robot arm at each moment, and perform iterative optimization processing based on the trajectory characteristics to obtain the optimal motion trajectory for performing the work task. The above-mentioned device extracts the features of the teaching trajectory at each moment and constructs an optimization problem based on the trajectory characteristics to perform continuous iterative optimization to obtain the optimal motion trajectory, thereby realizing the process of automatically finding the optimal motion trajectory based on the teaching trajectory, thereby improving the intelligence level of the robot arm.

[0150] The following is an introduction to a robotic arm involved in this application. Figure 6, is a schematic diagram of the structure of a robotic arm 60 provided in an embodiment of the present application. The robotic arm 60 may include: a processor 601 and a memory 602.

[0151] As a feasible implementation, the processor 601 and the memory 602 may be integrated in Figure 1 In the control system described in the corresponding embodiment.

[0152] The processor 601 is the control center of the robot arm. It uses various interfaces and lines to connect the various parts of the entire robot arm. By running or executing the software programs and / or modules stored in the memory 602, and calling the data stored in the memory 602, it performs various functions of the robot arm and processes data, thereby monitoring the robot arm as a whole. Optionally, the processor 601 may include one or more processing units; preferably, the processor 601 may integrate an application processor and a modem processor, wherein the application processor mainly processes the operating system, user interface, and application programs, and the modem processor mainly processes wireless communication and wired communication. It is understandable that the above-mentioned modem processor may not be integrated into the processor 601.

[0153] The memory 602 can be used to store software programs and modules. The processor 601 executes various functional applications and data processing of the robot arm by running the software programs and modules stored in the memory 602. The memory 602 can mainly include a program storage area and a data storage area, wherein the program storage area can store an operating system, an application required for at least one function, etc.; the data storage area can store data created according to the use of the robot arm, etc. In addition, the memory 602 can include a high-speed random access memory, and can also include a non-volatile memory, such as at least one disk storage device, a flash memory device, or other volatile solid-state storage devices.

[0154] The processor 601 may call the program code to perform the following operations:

[0155] When receiving an instruction to execute a task, obtaining each teaching trajectory for the task;

[0156] Performing feature extraction processing according to each teaching trajectory to obtain trajectory features of the end position of the robotic arm at each moment;

[0157] The trajectory features are continuously iterated and optimized to obtain the optimal motion trajectory for executing the task.

[0158] An embodiment of the present application further provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, wherein the computer program includes program instructions, and when the program instructions are executed by a computer, the computer executes the method described in the above embodiment.

[0159] It should be noted that, in each of the above-mentioned embodiments, there is not necessarily a certain order between the above-mentioned steps. A person skilled in the art can understand, based on the description of the embodiments of the present application, that in different embodiments, the above-mentioned steps may have different execution orders, that is, they may be executed in parallel, may be executed interchangeably, and so on.

[0160] The above described device or equipment embodiments are merely illustrative, wherein the unit modules described as separate components may or may not be physically separated, and the components displayed as module units may or may not be physical units, that is, they may be located in one place, or may be distributed on multiple network module units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the present embodiment.

[0161] Through the description of the above embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus a general hardware platform, or of course by hardware. Based on this understanding, the above technical solution, in essence or in other words, the part that contributes to the relevant technology, can be embodied in the form of a computer software product, which can be stored in a computer-readable storage medium, such as ROM / RAM, a magnetic disk, an optical disk, etc., and includes a number of instructions for a robotic arm to execute the methods described in each embodiment or some parts of the embodiment.

[0162] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit them. Under the concept of the present application, the technical features in the above embodiments or different embodiments may also be combined, the steps may be implemented in any order, and there are many other changes in different aspects of the present application as described above, which are not provided in detail for the sake of simplicity. Although the present application has been described in detail with reference to the aforementioned embodiments, a person of ordinary skill in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. These modifications or replacements do not deviate the essence of the corresponding technical solutions from the scope of the technical solutions of the embodiments of the present application.

Claims

1. A motion trajectory planning method, characterized in that: Applied to a robotic arm, the method comprises: When receiving an instruction to perform a task, obtaining each teaching trajectory for the task; Performing feature extraction processing according to each teaching trajectory to obtain trajectory features of the end position of the robotic arm at each moment; The trajectory features are continuously iterated and optimized to obtain the optimal motion trajectory for executing the task.

2. The method according to claim 1, characterized in that The feature extraction process is performed according to each teaching trajectory to obtain the trajectory features of the end position of the robot arm at each moment, including: Performing coordinate transformation processing and time alignment processing on each teaching trajectory to obtain a teaching trajectory after time alignment; The teaching trajectory after time alignment is subjected to feature extraction processing to obtain the trajectory features of the end position of the robot arm at each moment.

3. The method according to claim 2, characterized in that The performing coordinate transformation processing and time alignment processing on each teaching trajectory to obtain the teaching trajectory after time alignment includes: Extracting the relative continuous posture trajectory of each teaching trajectory at the end of the robot arm relative to the robot arm base, and converting the relative continuous posture trajectory into a Cartesian space coordinate system to obtain an absolute continuous posture trajectory of the end of the robot arm in a world coordinate system; The absolute continuous posture trajectory of the end of the robot arm in the world coordinate system is time-aligned to obtain a teaching trajectory after time alignment.

4. The method according to claim 3, characterized in that The step of performing time alignment processing on the absolute continuous posture trajectory of the end of the robot arm in the world coordinate system to obtain a teaching trajectory after time alignment includes: Based on the dynamic time warping algorithm, the absolute continuous posture trajectory of the end of the robot arm in the world coordinate system is time-aligned to obtain various teaching data after time alignment.

5. The method according to claim 3, characterized in that The step of extracting the relative continuous posture trajectory of each teaching trajectory at the end of the robot arm relative to the robot arm base, and performing Cartesian space coordinate system conversion on the relative continuous posture trajectory to obtain the absolute continuous posture trajectory of the end of the robot arm in the world coordinate system includes: Obtaining the relative continuous posture trajectory of each teaching trajectory at the end of the robot arm relative to the robot arm base according to forward kinematics; A Cartesian space coordinate system transformation is performed according to the positional relationship of the work task object perceived by the camera relative to the base of the robotic arm, so as to obtain an absolutely continuous posture trajectory of the end of the robotic arm in the world coordinate system.

6. The method according to claim 2, characterized in that The feature extraction process is performed on the teaching trajectory after time alignment to obtain the trajectory features of the end position of the robot arm at each moment, including: Based on the Gaussian mixture model algorithm, the characteristics of each continuous posture trajectory after the time alignment are described to obtain the trajectory characteristics of the end posture of the robot arm at each moment.

7. The method according to claim 6, characterized in that The Gaussian mixture model algorithm is used to characterize each continuous posture trajectory after the time alignment to obtain the trajectory characteristics of the end posture of the manipulator at each moment, including: Based on the Gaussian mixture model algorithm, each continuous posture trajectory after the time alignment is modeled using a joint probability distribution to obtain a feature description of each continuous posture trajectory after the time alignment; The feature description is subjected to task constraint regression processing based on Gaussian mixture regression to obtain the trajectory characteristics of the end position of the robotic arm at each moment.

8. The method according to claim 1, characterized in that The trajectory features include: the mean and covariance of the teaching trajectory at any time, and the mean and covariance of the teaching trajectory are obtained from the position and posture information of the end of the robot arm.

9. The method according to any one of claims 1 to 8, characterized in that The continuously iterative optimization process of the trajectory features to obtain the optimal motion trajectory for executing the task includes: The trajectory features are used as task constraints, and obstacle avoidance constraints and smoothness constraints are calculated; Continuous iterative optimization processing is performed according to the task constraint item, the obstacle avoidance constraint item and the smoothness constraint item to obtain an optimal motion trajectory for executing the operation task.

10. The method according to claim 9, characterized in that The continuously iterative optimization process is performed according to the task constraint item, the obstacle avoidance constraint item and the smoothness constraint item to obtain the optimal motion trajectory for executing the operation task, including: According to the task constraints, obstacle avoidance constraints and smoothness constraints, an optimal motion trajectory for executing the task is obtained in a staged iterative optimization manner; Among them, the staged iterative optimization method means: using the gradients of the obstacle avoidance constraint item and the smoothness constraint item in every odd-numbered iteration, using the gradient of the task constraint item in every even-numbered iteration, and stopping the iteration when the preset iteration termination condition is met.

11. A motion trajectory planning device, characterized in that: The device comprises: An acquisition module, used for acquiring each teaching trajectory for the operation task when receiving an instruction to execute the operation task; A processing module, used for performing feature extraction processing according to each teaching trajectory to obtain the trajectory features of the end position of the robot arm at each moment; The processing module is also used to perform continuous iterative optimization processing on the trajectory features to obtain an optimal motion trajectory for executing the task.

12. A robotic arm, characterized in that: The method comprises a memory and a processor, wherein the memory is connected to the processor, and the processor is used to execute one or more computer programs stored in the memory. When the processor executes the one or more computer programs, the robotic arm implements the method according to any one of claims 1 to 10.

13. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, wherein the computer program includes program instructions, and when the program instructions are executed by a processor, the processor is caused to perform the method according to any one of claims 1 to 10.

Images