Task trajectory planning method based on high-order motion information

Abstract

A task trajectory planning method based on high-order motion information belongs to the technical field of robots, and comprises the following steps: positioning three-dimensional coordinate information of a target feature point; designing a high-order motion observer based on a detection model by taking the target feature point as input to obtain a high-order motion information observation value of the target; designing a virtual controller based on the high-order motion information observation value of the target and the position of the obstacle; and designing a virtual impedance model, and outputting the task trajectory of the robot end effector in real time under the action of the virtual controller. According to the invention, the functions of target high-order motion information observation, trajectory planning and obstacle avoidance are integrated, and the autonomous decision-making ability and safe operation ability of the robot in a complex and dynamic environment are improved.

Description

Technical Field

[0001] The present invention belongs to the field of robotics technology, and in particular relates to a task trajectory planning method based on high-order motion information. Background Art

[0002] Mission trajectory planning is a common and key technology in dynamic manipulation tasks, such as human-machine collaboration, dynamic assembly, and space satellite capture, which are considered a type of navigation technology under vision guidance. So far, the manipulation of stationary targets under vision guidance has been widely studied, while the manipulation of dynamic targets remains a challenging topic. Among them, the observation of the target's motion information, the filtering of noise in the observation trajectory, and the robot's environmental adaptation ability during the task operation process (such as obstacle avoidance) are key common technologies.

[0003] The performance of target motion observation directly affects the robot's grasping accuracy and grasping effect, which provides key decision-making reference information for the robot arm. The existing motion observation algorithm mainly establishes a filter estimation model based on the noisy target posture obtained by the visual sensor (or the angular velocity obtained in combination with the IMU). It is essentially a filter estimation algorithm that relies on multi-sensor fusion to output the posture and speed of the target. For example, CN111696155A relates to a multi-sensor fusion robot positioning method based on monocular vision, which is essentially still through the fusion of monocular vision, encoder data, and IMU data, and the use of extended Kalman filtering to estimate the robot speed. Therefore, how to use only a single visual sensor to observe high-order motion information such as speed and acceleration is still a problem to be solved.

[0004] The robot arm formulates the movement path of the end effector according to the observed target motion information, thereby realizing the capture action. However, visual noise will inevitably cause significant jitter in the observed target motion trajectory, especially the speed and acceleration, affecting the smoothness and accuracy of the robot's motion. For example, CN115619828A relates to a space robot on-orbit capture method based on simulated binocular vision measurement, using filters for motion estimation, estimating the speed, and using reinforcement learning to achieve trajectory planning. In addition, obstacles that may exist in the environment affect the safety of robot operation. Summary of the invention

[0005] In order to overcome the shortcomings of the prior art, the present invention proposes a task trajectory planning method based on high-order motion information, which integrates target high-order motion information observation, trajectory planning and obstacle avoidance functions, thereby improving the robot's autonomous decision-making ability and safe operation ability in complex and dynamic environments.

[0006] A task trajectory planning method based on high-order motion information includes the following steps:

[0007] S1, three-dimensional coordinate information of the positioning target feature point;

[0008] S2. Taking the target feature points as input, a high-order motion observer based on the detection model is designed to obtain the high-order motion information observation value of the target;

[0009] S3, design a virtual controller based on the high-order motion information observations of the target and the obstacle position;

[0010] S4. Design a virtual impedance model, which outputs the task trajectory of the robot end effector in real time under the action of the virtual controller.

[0011] Furthermore, the design process of the high-order motion observer in step S2 is:

[0012] S21, taking the N pairs of feature points obtained in step S1 as input, a pair of feature points is connected to form a line segment, then N pairs of feature points can form N line segments, where the descriptions of the i-th line segment in the target coordinate system and the robot base coordinate system are A i and B i , i=1,…,N;A i and B i Describes the geometric features and motion features of the target respectively, then the feature element set of the target obtained by N pairs of feature points is {(A1, B1),…,(A i ,B i ),…,(A N ,B N )};

[0013] S22. Establish detection error in, φ e is the Euler angle of the target to be observed, represents the rotation matrix;

[0014] At the same time, the cumulative change and instantaneous change of the error are used as performance evaluation indicators to establish a mixed error function In the formula, λ j is the parameter to be designed, (j) represents the j-order derivative, j is 0, 1, 2, 3;

[0015] S23. Design a strategy generator based on the mixed error function Z, whose state space equation is as follows:

[0016]

[0017] Where X1 = ω e and For the strategy to be solved, express The antisymmetric matrix of × represents the cross product, ∈>0 is the parameter to be designed;

[0018] S24. Design a rotational dynamics detection model, whose state space equation is as follows:

[0019]

[0020] Where, Y1 = φ e , X1=ω e , It describes the speed of change of Euler angles To the absolute angular velocity ω e The mapping matrix.

[0021] Furthermore, the design process of the virtual controller in step S3 is:

[0022]

[0023] Among them, S is the soft start function, and within the soft start time t0, a smooth transition from 0 to 1 is achieved. c represents the end effector position to be planned, represents the end effector speed to be planned, φ c represents the Euler angle of the end effector to be planned, Indicates the change speed of the Euler angle of the end effector to be planned, p e represents the location of the target feature point, represents the velocity of the target feature point, represents the velocity of the target feature point, φ e represents the Euler angle of the target, Indicates the change rate of the target Euler angle, Represents the acceleration of the target Euler angle change. k1, k2 and k3 are all positive definite diagonal matrices. Set f o is 0, When the end effector position p e The distance to the obstacle d e When it is greater than the obstacle impact distance d0, f p =0, otherwise f p =αexp(β(d0-d e ))n1, α>0, β>0, n1 is the unit vector pointing from the obstacle to the current end-effector position, t0, k1, k2, k3, d0, α, β are all parameters to be designed.

[0024] Furthermore, the virtual impedance model in step S4 is expressed as follows:

[0025]

[0026] Where U represents the control quantity output by the virtual controller, L, D, and K are the inertia, damping, and stiffness matrices in the virtual impedance model, respectively, and are all parameters to be designed; c represents the end effector position to be planned, φ c represents the Euler angle of the end effector to be planned, represents the end effector speed to be planned, Indicates the change speed of the Euler angle of the end effector to be planned, represents the acceleration of the end effector to be planned, Indicates the acceleration of the Euler angle change of the end effector to be planned.

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

[0028] 1. This application is a task trajectory planning method for dynamic target operations, which solves key common problems such as motion information observation, noise filtering in observation trajectories, and obstacle avoidance in dynamic target operations.

[0029] 2. Compared with the traditional filtering-based motion observation algorithm, the high-order motion observer in this application relies only on a single visual sensor and combines it with a rotational dynamics detection model to achieve synchronous observation of the target's high-order motion information (position, velocity and acceleration), which has the advantages of high observation accuracy, smooth trajectory and deep observation dimension.

[0030] 3. This application realizes noise filtering and obstacle avoidance trajectory planning in the observed motion trajectory through a virtual impedance model, and together with a high-order motion observer, constitutes a real-time task trajectory planning system. The system only requires target feature points as input, and can output a smooth, accurate and safe end-effector operation trajectory in real time, with good operational safety and noise robustness.

[0031] 4. This application is based on the dynamic principles of general moving targets and is therefore universal and applicable to any measurement system that can locate target feature points, such as depth cameras, binocular cameras, motion capture systems, etc.

[0032] The following is a further description of the scheme of the application in conjunction with the accompanying drawings and embodiments: BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 It is a flowchart for implementing the task trajectory planning method based on high-order motion information;

[0034] Figure 2This is a flowchart for implementing the high-order motion observer in this application;

[0035] Figure 3 This is a high-order motion information diagram of the target observed using the solution of the present application in the embodiment;

[0036] Figure 4 This is a diagram of the trajectory information of the end effector planned by the method of the present application in the case of no obstacles in the embodiment;

[0037] Figure 5 This is a three-dimensional trajectory display diagram of the robot tracking a dynamic target using the method of the present application in the absence of obstacles in the embodiment; in the figure, x, y, and z respectively represent the three coordinate axes of the robot's base coordinate system;

[0038] Figure 6 This is a diagram of the trajectory information of the end effector planned by the method of the present application in the case of obstacles in the embodiment;

[0039] Figure 7 This is a three-dimensional trajectory display diagram of the robot tracking a dynamic target using the method of the present application in the presence of obstacles in the embodiment; in the figure, x, y, and z respectively represent the three coordinate axes of the robot's base coordinate system.

[0040] The symbols in the figure are defined as:

[0041] e represents the motion information of the target to be observed, and c represents the trajectory command of the end effector to be planned; Indicates the location of the target feature point; Indicates the speed of the target feature point; Indicates the acceleration of the target feature point; represents the target Euler angle, where and Represent the yaw angle, pitch angle and roll angle of the target respectively; Indicates the changing speed of the target Euler angle; Indicates the acceleration of the target Euler angle change; represents the end effector position to be planned; represents the end effector speed to be planned; represents the Euler angle of the end effector to be planned; represents the change speed of the Euler angle of the end effector to be planned, where and They represent the yaw angle, pitch angle and roll angle of the end effector respectively. DETAILED DESCRIPTION

[0042] The embodiments of the technical solution of the present invention will be described in detail below in conjunction with the accompanying drawings. Unless otherwise specified, the technical terms or scientific terms used in this application are generally understood by those skilled in the art.

[0043] In a dynamic operation environment with obstacles, a task trajectory generation mechanism that correctly guides the robot's actions is necessary. Therefore, a robot task trajectory planning method based on high-order motion information is proposed, which solves key common technologies such as motion information observation in dynamic target operation, noise filtering in the observed trajectory, and terminal obstacle avoidance. It can not only observe the target motion information with deep dimensions, high precision, and good smoothness, but also plan a safe and robust task operation trajectory for the end effector, improving the robot's conscious decision-making ability and environmental adaptability.

[0044] In view of this, refer to Figure 1 and Figure 2 , a task trajectory planning method based on high-order motion information provided in this embodiment includes the following steps:

[0045] S1, three-dimensional coordinate information of the positioning target feature point;

[0046] In step S1, the three-dimensional coordinates of the target N>2 pairs of feature points in the robot base coordinate system (absolute coordinate system) are obtained through a measurement system that can locate the spatial coordinate information of the target feature points, such as stereo vision (such as a depth camera, a binocular camera) or a motion capture system.

[0047] S2. Taking the target feature points as input, a high-order motion observer based on the detection model is designed to obtain the high-order motion information observation value of the target;

[0048] The design process of the high-order motion observer in step S2 is:

[0049] S21, taking the N pairs of feature points obtained in step S1 as input, a pair of feature points is connected to form a line segment, then N pairs of feature points can form N line segments, where the descriptions of the i-th line segment in the target coordinate system and the robot base coordinate system are A i and B i , i=1,…,N;A i and B i They describe the geometric and motion characteristics of the target respectively, so (A i ,B i is the "characteristic element" of the moving target, then the characteristic element set of the target obtained by N pairs of feature points is {(A1, B1),…, (A i ,B i ),…,(A N ,B N )};

[0050] S22. Establish detection error in, φ e is the Euler angle of the target to be observed, represents the rotation matrix;

[0051] At the same time, the cumulative change and instantaneous change of the error are used as performance evaluation indicators to establish a mixed error function In the formula, λ j is the parameter to be designed, (j) represents the j-order derivative, j is 0, 1, 2, 3;

[0052] S23. Design a strategy generator based on the mixed error function Z, whose state space equation is as follows:

[0053]

[0054] Where X1 = ω e and For the strategy to be solved, express The antisymmetric matrix of × represents the cross product, ∈>0 is the parameter to be designed;

[0055] S24. Design a rotational dynamics detection model, whose state space equation is as follows:

[0056]

[0057] Where, Y1 = φ e , X1=ω e , It describes the speed of change of Euler angles To the absolute angular velocity ω e The mapping matrix.

[0058] In this step, the high-order motion observer can output the posture motion information with fast convergence speed, high observation accuracy and smooth trajectory in real time, including the Euler angle φ e , Euler angle change speed and Euler angle change acceleration In addition, the translational motion information of the target is defined as the high-order derivatives of the target feature point positions (including the position p e ,speed and acceleration ).

[0059] S3, design a virtual controller based on the high-order motion information observations of the target and the obstacle position;

[0060] According to the target high-order motion information observation value output in step S2 Design a virtual controller for obstacle locations.

[0061] The design process of the virtual controller in step S3 is:

[0062]

[0063] Among them, S is the soft start function, and within the soft start time t0, a smooth transition from 0 to 1 is achieved. c represents the end effector position to be planned, represents the end effector speed to be planned, φ c represents the Euler angle of the end effector to be planned, Indicates the change speed of the Euler angle of the end effector to be planned. k1, k2 and k3 are all positive definite diagonal matrices. Set f o is 0, When the end effector position p e The distance to the obstacle d e When it is greater than the obstacle impact distance d0, f p =0, otherwise f p =αexp(β(d0-d e ))n1, α>0, β>0, n1 is the unit vector pointing from the obstacle to the current end-effector position, t0, k1, k2, k3, d0, α, β are all parameters to be designed.

[0064] S4. Design a virtual impedance model, which outputs the task trajectory of the robot end effector in real time under the action of the virtual controller.

[0065] In step S4, in order to reduce and further eliminate the observed trajectory fluctuations caused by visual noise, a virtual impedance model with inertia, damping and stiffness characteristics is introduced at the end of the robot arm, which can optimize the dynamic response of the system and enhance the robustness to noise. The virtual impedance model is expressed as follows:

[0066]

[0067] In the formula, U represents the control quantity output by the virtual controller, p c represents the end effector position to be planned, φ c represents the Euler angle of the end effector to be planned, represents the end effector speed to be planned, Indicates the change speed of the Euler angle of the end effector to be planned, represents the acceleration of the end effector to be planned, It represents the acceleration of the Euler angle change of the end effector to be planned. L, D, and K are the inertia, damping, and stiffness matrices, respectively, which are the parameters to be designed.

[0068] In this step, the virtual impedance model is used as the controlled object. Under the action of the virtual controller U, it can reconstruct the target motion trajectory observed in step S2, while effectively avoiding obstacles, and then output a robust and safe end-effector operation trajectory command, including: position command p c , speed command Euler angle command φ c and Euler angle change speed command

[0069] This implementation scheme realizes noise filtering and obstacle avoidance trajectory planning in the observed motion trajectory by designing a virtual impedance model, and together with the high-order motion observer, it constitutes a real-time task trajectory planning system. The system only requires target feature points as input and can output a smooth, accurate and safe end-effector operation trajectory in real time, with good operational safety and noise robustness.

[0070] The following is a further description of the application of this embodiment with examples:

[0071] This embodiment implements trajectory planning for a robot's tracking task of a dynamic target.

[0072] Step S1: In order to simulate the motion state of the target in three-dimensional space, the translation motion trajectory p(t) and rotation motion trajectory φ(t) described by the target in the robot base coordinate system are given here. Simulate 2 (N=2) pairs of feature points on the selected target, and then calculate the three-dimensional coordinates of the two pairs of feature points in the robot base coordinate system and the target coordinate system according to p(t) and φ(t). In order to simulate the influence of visual noise, Gaussian white noise with a signal-to-noise ratio of 35 is added to the motion trajectory of the feature points.

[0073] The above trajectory setting process is only for illustrating this embodiment. In practical applications, the recognition of the spatial coordinates of the feature points is obtained in real time based on a measurement system for locating the target feature points, such as a depth camera, a binocular camera, a motion capture system, and the like.

[0074] Step S2: Using the two pairs of feature points obtained in step S1 as input, design a high-order motion observer based on the detection model to obtain the high-order motion information observation value of the target. The design process of the high-order motion observer is as follows:

[0075] S21. Take the two pairs of feature points obtained in step S1 as input. A pair of feature points can form a line segment, and the two pairs of feature points can form two line segments. The target feature element set is obtained as {(A1, B1), (A2, B2)}.

[0076] S22. Establishing the detection error of the system in, φ e is the Euler angle to be observed, R(φ e ) represents the rotation matrix. Furthermore, the cumulative change and instantaneous change of the error are used as performance evaluation indicators, and a hybrid error function is established: In this step, the selected parameters are λ0=3375, λ1=675, λ2=45, and λ3=1.

[0077] S23. Design a strategy generator based on the mixed error function Z, whose state space equation is as follows:

[0078]

[0079] Where X1 = ω e and For the strategy to be solved, express The antisymmetric matrix of In this step, the selected parameter is ∈=1.5.

[0080] S24. Design a rotational dynamics detection model, whose state space equation is as follows:

[0081]

[0082] Where, Y1 = φ e , X1=ω e , It describes the speed of change of Euler angles To the absolute angular velocity ω e The mapping matrix.

[0083] In this step, the high-order motion observer can output the posture motion information with fast convergence speed, high observation accuracy and smooth trajectory in real time, including the Euler angle φ e , Euler angle change speed and Euler angle change acceleration In addition, the translational motion information of the target is defined as the high-order derivatives of the target feature point position (including the position p e ,speed and acceleration ).

[0084] Step S3: Based on the target high-order motion information observation value output in S2 A virtual controller is designed at the obstacle position, and the control quantity output by the virtual controller is:

[0085]

[0086] Where S is the soft start function, and a smooth transition from 0 to 1 is achieved within the soft start time t0. The introduction of can improve the dynamic response and tracking accuracy of the system. Set f o is 0, When the end effector position p e The distance to the obstacle d e When it is greater than the obstacle impact distance d0, f p =0, otherwise f p =αexp(β(d0-d e ))n1, n1 is the unit vector pointing from the obstacle to the current end effector position. In this step, the selected parameters are t0 = 10s, k1 = 500I6, k2 = 5I6, k3 = I6, I6 is the unit matrix with a dimension of 6×6, d0 = 200mm, α = 30000, β = 0.015.

[0087] In order to demonstrate the obstacle avoidance capability of this embodiment, this embodiment sets an operation scenario without obstacles and an operation scenario with static obstacles. In the obstacle scenario, the obstacle is set between the robot end effector and the target.

[0088] Step S4: In order to reduce and further eliminate the trajectory fluctuation caused by visual noise, a virtual impedance model with inertia, damping and stiffness characteristics is introduced at the end of the robot arm, which can optimize the dynamic response of the system and enhance the robustness to noise.

[0089] The virtual impedance model formula is as follows

[0090]

[0091] Where L, D, and K are inertia, damping, and stiffness matrices, respectively. In this step, the selected parameters are L = I6, D = 25I6, and K = 0.1I6.

[0092] In this step, the virtual impedance model can output robust and safe end-effector operation trajectory commands, including: position command p c , speed command Euler angle command φ c and Euler angle change speed command

[0093] After the above steps, the observation results of the high-order motion observer in this embodiment are as follows: Figure 3 The trajectory planning result of the end effector in the obstacle-free scene is shown in Figure 4-Figure 5The trajectory planning result of the end effector in the obstacle scene is shown in Figure 6-Figure 7 shown.

[0094] from Figure 3 It can be seen that this embodiment can synchronously observe the position p of the target e ,speed Acceleration Euler angle φ e , Euler angle change speed and Euler angle change acceleration Among them, the position observation error is within 0.3mm, the speed observation error is within 0.5mm / s, and the acceleration observation error is within 0.2mm / s 2 The observation error of Euler angle is within 0.1°, the observation error of Euler angle change speed is within 0.1° / s, and the observation error of Euler angle change acceleration is within 0.2° / s. 2 The above results prove that the high-order motion observer in this scheme has the advantages of high observation accuracy, smooth trajectory and deep observation dimension.

[0095] from Figure 4 and Figure 6 It can be seen that this scheme can plan safe and robust end-effector commands, including: position command p c , speed command Euler angle command φ c and Euler angle change speed command Among them, the position planning error is within 0.8mm, the speed planning error is within 0.2mm / s, the Euler angle planning error is within 0.2°, and the Euler angle change speed planning error is within 0.1° / s. It can be seen that even if there is noise in the trajectory of the target feature point, the planned end effector command is still smooth and accurate. Figure 5 and Figure 7 The result of the robot tracking a dynamic target using this solution is intuitively shown, where the green solid line is the true value of the set target motion, and the purple solid line is the end effector trajectory calculated according to this solution. It can be seen that before the end effector reaches the target position, in a scene without obstacles, the end effector position directly approaches the target position; in a scene with obstacles, the end effector position can effectively avoid obstacles while approaching the target position. After the end effector reaches the target position, the planned end effector trajectory and the target trajectory are basically completely overlapped.

[0096] The present invention has been disclosed as above with preferred implementation cases, but it is not used to limit the present invention. Any technician familiar with the profession can make slight changes or modifications to equivalent implementation cases with equivalent changes by using the above-disclosed structures and technical contents without departing from the scope of the technical solution of the present invention, which still fall within the scope of the technical solution of the present invention.

Claims

1. A task trajectory planning method based on high-order motion information, characterized in that: The following steps are included: S1, three-dimensional coordinate information of the positioning target feature point; S2. Taking the target feature points as input, a high-order motion observer based on the detection model is designed to obtain the high-order motion information observation value of the target; S3, design a virtual controller based on the high-order motion information observations of the target and the obstacle position; S4. Design a virtual impedance model, which outputs the task trajectory of the robot end effector in real time under the action of the virtual controller.

2. The task trajectory planning method based on high-order motion information according to claim 1, characterized in that: The design process of the high-order motion observer in step S2 is: S21, taking the N pairs of feature points obtained in step S1 as input, a pair of feature points is connected to form a line segment, then N pairs of feature points can form N line segments, where the descriptions of the i-th line segment in the target coordinate system and the robot base coordinate system are A i and B i , i=1,...,N;A i and B i Describes the geometric features and motion features of the target respectively, then the set of feature elements of the target obtained by N pairs of feature points is {(A1, B1),…, (A i , B i ),…,(A N , B N )}; S22. Establish detection error in, φ e is the Euler angle of the target to be observed, represents the rotation matrix; At the same time, the cumulative change and instantaneous change of the error are used as performance evaluation indicators to establish a mixed error function In the formula, λ j is the parameter to be designed, (I) represents the j-order derivative, j is 0, 1, 2, 3; S23. Design a strategy generator based on the mixed error function Z, whose state space equation is as follows: Where X1 = ω e and For the strategy to be solved, express The antisymmetric matrix of × represents the cross product, ∈>0 is the parameter to be designed; S24. Design a rotational dynamics detection model, whose state space equation is as follows: Where, Y1 = φ e , X1=ω e , It describes the speed of change of Euler angles To the absolute angular velocity ω e The mapping matrix.

3. The task trajectory planning method based on high-order motion information according to claim 1, characterized in that: The design process of the virtual controller in step S3 is: Record the control quantity output by the virtual controller Among them, S is the soft start function, and within the soft start time t0, a smooth transition from 0 to 1 is achieved. c represents the end effector position to be planned, represents the end effector speed to be planned, φ c represents the Euler angle of the end effector to be planned, Indicates the change speed of the Euler angle of the end effector to be planned, p e represents the location of the target feature point, represents the velocity of the target feature point, represents the velocity of the target feature point, φ e represents the Euler angle of the target, Indicates the change rate of the target Euler angle, Represents the change acceleration of the target Euler angle, k1, k2 and k3 are all positive definite diagonal matrices, Set f o is 0, When the end effector position p e The distance to the obstacle d e When it is greater than the obstacle impact distance d0, f p =0, otherwise f p =αexp(β(d0-d e ))n1, α>0, β>0, n1 is the unit vector pointing from the obstacle to the current end-effector position, k0, k1, k2, k3, d0, α, β are all parameters to be designed.

4. The task trajectory planning method based on high-order motion information according to claim 1, characterized in that: The virtual impedance model in step S4 is expressed as follows: Where U represents the control quantity output by the virtual controller, L, D, and K are the inertia, damping, and stiffness matrices in the virtual impedance model, respectively, and are all parameters to be designed; c represents the end effector position to be planned, φ c represents the Euler angle of the end effector to be planned, represents the end effector speed to be planned, Indicates the change speed of the Euler angle of the end effector to be planned, represents the acceleration of the end effector to be planned, Indicates the acceleration of the Euler angle change of the end effector to be planned.

5. The task trajectory planning method based on high-order motion information according to claim 1, characterized in that: In the step S1, three-dimensional coordinates of N pairs of feature points in the robot base coordinate system are obtained, where N≥2.

6. The task trajectory planning method based on high-order motion information according to claim 1, characterized in that: In step S1, the three-dimensional coordinates of N pairs of feature points of the target in the robot base coordinate system are obtained through a stereoscopic vision or motion capture system that can locate the three-dimensional spatial coordinate information of the target feature points, where N≥2.

Images