A task trajectory planning method based on high-order motion information

Abstract

The application discloses a task trajectory planning method based on high-order motion information, and belongs to the technical field of robots.The method comprises the following steps: positioning three-dimensional coordinate information of a target feature point; taking the target feature point as input, designing a high-order motion observer based on a detection model 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 an obstacle; and designing a virtual impedance model which outputs a task trajectory of a robot end effector in real time under the action of the virtual controller.The application integrates target high-order motion information observation, trajectory planning and obstacle avoidance functions, and improves the autonomous decision-making ability and safe operation ability of the robot in a complex and dynamic environment.

Description

Technical Field

[0001] This invention belongs to the field of robotics technology, specifically relating to a task trajectory planning method based on high-order motion information. Background Technology

[0002] Task trajectory planning is a common and key technology in dynamic maneuvering tasks, such as human-robot collaboration, dynamic assembly, and space satellite capture. These tasks are considered a class of vision-guided navigation technologies. To date, vision-guided manipulation of stationary targets has been extensively studied, while manipulation of dynamic targets remains a challenging issue. Among the key common technologies are the observation of target motion information, noise filtering in the observed trajectory, and the robot's environmental adaptation capabilities (such as obstacle avoidance) during task manipulation.

[0003] The performance of target motion observation directly affects the robot's grasping accuracy and effectiveness, providing crucial decision-making reference information for the robotic arm. Existing motion observation algorithms primarily establish filtering estimation models based on noisy target poses acquired by visual sensors (or angular velocities obtained by IMU). Essentially, these are multi-sensor fusion-based filtering estimation algorithms that output the target's pose and velocity. For example, CN111696155A relates to a multi-sensor fusion robot localization method based on monocular vision, which essentially still fuses monocular vision, encoder data, and IMU data, using extended Kalman filtering to estimate the robot's velocity. Therefore, how to observe higher-order motion information such as velocity and acceleration using only a single visual sensor remains an unsolved problem.

[0004] The robotic arm plans the movement path of its end effector based on the observed target motion information, thereby achieving the grasping action. However, visual noise inevitably causes significant jitter in the observed target motion trajectory, especially in terms of speed and acceleration, affecting the smoothness and accuracy of the robot's motion. For example, CN115619828A discloses an on-orbit capture method for a space robot based on simulated binocular vision measurement, which uses filters for motion estimation, estimates the velocity, and uses reinforcement learning to achieve trajectory planning. Furthermore, obstacles in the environment may affect the safety of robot operation. Summary of the Invention

[0005] To overcome the shortcomings of existing technologies, this 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 capability in complex and dynamic environments.

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

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

[0008] S2. Using the target feature points as input, design a high-order motion observer based on the detection model to obtain the high-order motion information observation values ​​of the target;

[0009] S3. Design a virtual controller based on the high-order motion information observations of the target and the position of obstacles;

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

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

[0012] S21. Using the N pairs of feature points obtained in step S1 as input, connecting a pair of feature points forms a line segment. Therefore, N pairs of feature points can form N line segments. The description of the i-th line segment in the target coordinate system and the robot base coordinate system are respectively A... i and B i , i = 1, ..., N; A i and B i The geometric and motion features of the target are described respectively. Then, the set of feature elements of the target obtained from N pairs of feature points is {(A1, B1), ..., (A... i B i ),…,(A N B N )};

[0013] S22. Establish detection error in, φ e Let Euler angles be the values ​​of the target to be observed. Represents the rotation matrix;

[0014] Simultaneously, a hybrid error function is established using the cumulative and instantaneous changes in error as performance evaluation indicators. In the formula, λ j Let (j) be the parameter to be designed, and (j) represent the j-th derivative, where j takes the values ​​0, 1, 2, or 3.

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

[0016]

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

[0018] S24. Design a rotational dynamics detection model, whose state-space equations are as follows:

[0019]

[0020] In the formula, Y1=φ e , X1=ω e , It describes the rate of change of Euler angles. to absolute angular velocity ω e The mapping matrix.

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

[0022]

[0023] Where S is the soft start function, which achieves a smooth transition from 0 to 1 within the soft start time t0. c Indicates the location of the end effector to be planned. φ represents the speed of the end effector to be planned. c This represents the Euler angles of the end effector to be planned. p represents the rate of change of the Euler angle of the end effector to be planned. e Indicates the location of the target feature point. Indicates the velocity of the target feature point. φ represents the velocity of the target feature point. e Representing the Euler angles of the target, Indicates the rate of change of the target Euler angle. This represents the acceleration due to the change in the target's Euler angles. k1, k2, and k3 are all positive definite diagonal matrices. Set f o =0, When the end effector position p e Distance d between the object and the obstacle e When the distance d0 is greater than the distance affected by the obstacle, 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 position of the end effector, and 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] In the formula, U represents the control quantity output by the virtual controller, and L, D, and K are the inertia, damping, and stiffness matrices in the virtual impedance model, respectively, all of which are parameters to be designed; p c Indicates the position of the end effector to be planned, φ c This represents the Euler angles of the end effector to be planned. This indicates the speed of the end effector to be planned. This represents the rate of change of the Euler angles of the end effector to be planned. This represents the acceleration of the end effector to be planned. This represents the Euler angle change acceleration of the end effector to be planned.

[0027] The advantages of this invention compared to the prior art are:

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

[0029] Second, compared with traditional filtering-based motion observation algorithms, the high-order motion observer in this application can achieve synchronous observation of target high-order motion information (pose, velocity and acceleration) by relying on only a single visual sensor and combining it with a rotational dynamics detection model. It has the advantages of high observation accuracy, smooth trajectory and deep observation dimension.

[0030] Third, this application realizes noise filtering and obstacle avoidance trajectory planning in the observed motion trajectory through a virtual impedance model. Together with the high-order motion observer, it constitutes a real-time task trajectory planning system. This system only needs the target feature point as input to output a smooth, accurate and safe end effector operation trajectory in real time, and has good operational safety and noise robustness.

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

[0032] The proposed solution will be further described below with reference to the accompanying drawings and embodiments: Attached Figure Description

[0033] Figure 1 This is a flowchart illustrating the implementation of a task trajectory planning method based on high-order motion information.

[0034] Figure 2This is a flowchart illustrating the implementation of the high-order motion observer in this application;

[0035] Figure 3 This is a high-order motion information map of the target observed using the scheme of this application in the embodiment;

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

[0037] Figure 5 This is a 3D trajectory diagram of a robot tracking a dynamic target using the method of this application in an obstacle-free situation, as shown in the embodiment; in the diagram, x, y, and z represent the three coordinate axes of the robot's base coordinate system, respectively;

[0038] Figure 6 This is a diagram showing the trajectory information of the end effector planned using the method of this application in the case of obstacles.

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

[0040] The symbols in the diagram are defined as follows:

[0041] e represents the motion information of the target to be observed, and c represents the end effector trajectory command to be planned; Indicates the location of the target feature point; Indicates the velocity of the target feature point; This represents the acceleration of the target feature point; Denotes the target Euler angles, where and These represent the target's yaw angle, pitch angle, and roll angle, respectively. Indicates the rate of change of the target Euler angles; This represents the acceleration due to the change in the target's Euler angles; Indicates the location of the end effector to be planned; This indicates the speed of the end effector to be planned; This represents the Euler angles of the end effector to be planned; This represents the rate of change of the Euler angles of the end effector to be planned, where and These represent the yaw angle, pitch angle, and roll angle of the end effector, respectively. Detailed Implementation

[0042] The embodiments of the technical solution of the present invention will now be described in detail with reference to the accompanying drawings. Unless otherwise stated, the technical or scientific terms used in this application have the ordinary meaning as understood by those skilled in the art.

[0043] In dynamic operating environments with obstacles, a proper task trajectory generation mechanism to guide robot movements is essential. Therefore, a robot task trajectory planning method based on high-order motion information is proposed, addressing key common technologies in dynamic target manipulation, such as motion information observation, noise filtering in the observed trajectory, and end-effector obstacle avoidance. This method not only observes target motion information with high dimensionality, high precision, and good smoothness, but also plans safe and robust task operation trajectories for the end effector, enhancing the robot's autonomous decision-making ability and environmental adaptability.

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

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

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

[0047] S2. Using the target feature points as input, design a high-order motion observer based on the detection model to obtain the high-order motion information observation values ​​of the target;

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

[0049] S21. Using the N pairs of feature points obtained in step S1 as input, connecting a pair of feature points forms a line segment. Therefore, N pairs of feature points can form N line segments. The description of the i-th line segment in the target coordinate system and the robot base coordinate system are respectively A... i and B i , i = 1, ..., N; A i and B i The geometric and motion characteristics of the target are described respectively, therefore (A i B i Let A be the "feature element" of the moving target. 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 )};

[0050] S22. Establish detection error in, φ e Let Euler angles be the values ​​of the target to be observed. Represents the rotation matrix;

[0051] Simultaneously, a hybrid error function is established using the cumulative and instantaneous changes in error as performance evaluation indicators. In the formula, λ j Let (j) be the parameter to be designed, and (j) represent the j-th derivative, where j takes the values ​​0, 1, 2, or 3.

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

[0053]

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

[0055] S24. Design a rotational dynamics detection model, whose state-space equations are as follows:

[0056]

[0057] In the formula, Y1=φ e , X1=ω e , It describes the rate of change of Euler angles. to absolute angular velocity ω e The mapping matrix.

[0058] In this step, the high-order motion observer can output attitude motion information in real time with fast convergence speed, high observation accuracy, and smooth trajectory, including Euler angles φ. e Euler angle change rate and Euler angle change acceleration Furthermore, the translational motion information of the target is defined as the higher-order derivative of the target feature point position (including 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 position of obstacles;

[0060] Based on the target higher-order motion information observations output in step S2 Design a virtual controller for obstacle location.

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

[0062]

[0063] Where S is the soft start function, which achieves a smooth transition from 0 to 1 within the soft start time t0. c Indicates the location of the end effector to be planned. φ represents the speed of the end effector to be planned. c This represents the Euler angles of the end effector to be planned. This represents the rate of change of Euler angles of the end effector to be planned. k1, k2, and k3 are all positive definite diagonal matrices. Set f o =0, When the end effector position p e Distance d between the object and the obstacle e When the distance d0 is greater than the distance affected by the obstacle, 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 position of the end effector, t0, k1, k2, k3, d0, α, β are all parameters to be designed.

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

[0065] In step S4, to further reduce and eliminate observation trajectory fluctuations caused by visual noise, a virtual impedance model with inertial, damping, and stiffness characteristics is introduced at the end effector of the robotic arm. This model optimizes the system's dynamic response and enhances its 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 Indicates the position of the end effector to be planned, φ c This represents the Euler angles of the end effector to be planned. This indicates the speed of the end effector to be planned. This represents the rate of change of the Euler angles of the end effector to be planned. This represents the acceleration of the end effector to be planned. The value represents the Euler angle variation acceleration of the end effector to be planned, and 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, as the controlled object, can reconstruct the target motion trajectory observed in step S2 under the action of the virtual controller U, while effectively avoiding obstacles, and thus output a robust and safe end effector operation trajectory command, including: position command p c Speed ​​command Euler angle command φ c Euler angle change rate command

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

[0070] The following examples further illustrate the application of this implementation scheme:

[0071] This embodiment demonstrates trajectory planning for a robot tracking a dynamic target.

[0072] Step S1: To simulate the motion of the target in three-dimensional space, the translational trajectory p(t) and rotational trajectory φ(t) of the target in the robot base coordinate system are given. Two (N=2) pairs of feature points on the target are selected for simulation. The three-dimensional coordinates of the two pairs of feature points in the robot base coordinate system and the target coordinate system can then be calculated based on p(t) and φ(t). To simulate the effect 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 trajectory setting process described above is only for illustrating this embodiment. In practical applications, the spatial coordinates of feature points are identified in real time using a measurement system for locating target feature points, such as depth cameras, binocular cameras, and motion capture systems.

[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 observations of the target. The design process of the high-order motion observer is as follows:

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

[0076] S22. Establish the system's detection error in, φ e Let R(φ) be the Euler angle to be observed. e Let represent the rotation matrix. Furthermore, using the cumulative and instantaneous changes in error as performance evaluation indicators, a hybrid error function was established as follows: In this step, the selected parameters are λ0 = 3375, λ1 = 675, λ2 = 45, and λ3 = 1.

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

[0078]

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

[0080] S24. Design a rotational dynamics detection model, whose state-space equations are as follows:

[0081]

[0082] In the formula, Y1=φ e , X1=ω e , It describes the rate of change of Euler angles. to absolute angular velocity ω e The mapping matrix.

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

[0084] Step S3: Based on the target higher-order motion information observations output in S2 The obstacle location is designed with a virtual controller, and the control output of the virtual controller is:

[0085]

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

[0087] To demonstrate the obstacle avoidance capability of this embodiment, two operation scenarios are set up: one with no obstacles and one with static obstacles. In the obstacle scenario, the obstacle is placed between the robot's end effector and the target.

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

[0089] The formula for the virtual impedance model is as follows:

[0090]

[0091] In the formula, L, D, and K are the 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 Euler angle change rate command

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

[0094] from Figure 3 As can be seen, this embodiment can simultaneously observe the target's position p. e ,speed acceleration Euler angle φ e Euler angle change rate and Euler angle change acceleration Specifically, the position observation error is within 0.3 mm, the velocity observation error is within 0.5 mm / s, and the acceleration observation error is within 0.2 mm / s. 2 Within this range, the observation error of Euler angles reaches within 0.1°, the observation error of the rate of change of Euler angles reaches within 0.1° / s, and the observation error of the acceleration of the change of Euler angles reaches within 0.2° / s. 2 Within this range. The above results demonstrate 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 As can be seen, this scheme can plan safe and robust end-effector commands, including: position command p c Speed ​​command Euler angle command φ c Euler angle change rate command Specifically, the position planning error is within 0.8 mm, the velocity planning error is within 0.2 mm / s, the Euler angle planning error is within 0.2°, and the Euler angle change rate planning error is within 0.1° / s. This demonstrates that even with noise in the trajectory of the target feature points, the planned end effector commands remain smooth and accurate. Figure 5 and Figure 7 The results of the robot tracking a dynamic target using this method are visually demonstrated. The green solid line represents the actual value of the target's motion, and the purple solid line represents the end effector trajectory calculated using this method. It can be seen that before the end effector reaches the target position, in an obstacle-free scenario, the end effector position directly approaches the target position; in an obstacle-containing scenario, the end effector position effectively avoids obstacles while approaching the target position. After the end effector reaches the target position, the planned end effector trajectory almost completely overlaps with the target trajectory.

[0096] The present invention has been disclosed above with reference to preferred embodiments, but it is not intended to limit the present invention. Any person skilled in the art can make some modifications or alterations to the above-disclosed structure and technical content to create equivalent embodiments without departing from the scope of the present invention, and all such modifications or alterations shall still fall within the scope of the present invention.

Claims

1. A task trajectory planning method based on high-order motion information, characterized in that: It includes the following steps: S1, the three-dimensional coordinate information of the target feature points; S2. Using the target feature points as input, design a high-order motion observer based on the detection model to obtain the high-order motion information observation values ​​of the target; The design process of the higher-order motion observer in step S2 is as follows: S21. Using the N pairs of feature points obtained in step S1 as input, connecting a pair of feature points forms a line segment. Therefore, N pairs of feature points can form N line segments. The description of the i-th line segment in the target coordinate system and the robot base coordinate system are respectively A... i and B i , i = 1, ..., N; A i and B i The geometric and motion features of the target are described respectively. Then, the set of feature elements of the target obtained from N pairs of feature points is {(A1, B1), ..., (A... i B i ),…,(A N B N )}; S22. Establish detection error in, φ e Let Euler angles be the values ​​of the target to be observed. Represents the rotation matrix; Simultaneously, a hybrid error function is established using the cumulative and instantaneous changes in error as performance evaluation indicators. In the formula, λ j Let (j) be the parameter to be designed, and (j) represent the j-th derivative, where j takes the values ​​0, 1, 2, or 3. S23. Design a policy generator based on the hybrid error function Z, whose state-space equation is as follows: In the formula, X1=ω e and The strategy to be solved express antisymmetric matrix, × represents the cross product. The parameters to be designed; S24. Design a rotational dynamics detection model, whose state-space equations are as follows: In the formula, Y1=φ e , X1=ω e , It describes the rate of change of Euler angles. to absolute angular velocity ω e The mapping matrix; S3. Design a virtual controller based on the high-order motion information observations of the target and the position of obstacles; S4. Design a virtual impedance model that outputs the task trajectory of the robot's end effector in real time under the action of a 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 virtual controller in step S3 is as follows: Record the control quantity output by the virtual controller Where S is the soft start function, which achieves a smooth transition from 0 to 1 within the soft start time t0, and p c Indicates the location of the end effector to be planned. φ represents the speed of the end effector to be planned. c This represents the Euler angles of the end effector to be planned. p represents the rate of change of the Euler angle of the end effector to be planned. e Indicates the location of the target feature point. Indicates the velocity of the target feature point. φ represents the acceleration of the target feature point. e Representing the Euler angles of the target, Indicates the rate of change of the target Euler angle. The acceleration representing the change in the target's Euler angles is represented by k1, k2, and k3, which are all positive definite diagonal matrices. Set f o =0, When the end effector position p e Distance d between the object and the obstacle e When the distance d0 is greater than the distance affected by the obstacle, 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 position of the end effector, t0, k1, k2, k3, d0, α, β are all parameters to be designed.

3. 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: In the formula, U represents the control quantity output by the virtual controller, and L, D, and K are the inertia, damping, and stiffness matrices in the virtual impedance model, respectively, all of which are parameters to be designed; p c Indicates the position of the end effector to be planned, φ c This represents the Euler angles of the end effector to be planned. This indicates the speed of the end effector to be planned. This represents the rate of change of the Euler angles of the end effector to be planned. This represents the acceleration of the end effector to be planned. This represents the Euler angle change acceleration of the end effector to be planned.

4. 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 in the robot's base coordinate system are obtained, where N≥2.

5. The task trajectory planning method based on high-order motion information according to claim 1, characterized in that: In step S1, a stereo vision or motion capture system capable of locating the three-dimensional spatial coordinates of the target feature points is used to obtain the three-dimensional coordinates of N pairs of feature points of the target in the robot base coordinate system, where N≥2.

Images