Robot motion trail smoothing method and system and storage medium

Abstract

The invention relates to a robot motion trail smoothing method and system. The method comprises the specific steps that S1, discrete path points of robot motion are obtained, and a B-spline position path curve meeting the third-order derivative continuity is constructed; s2, representing the posture of an end effector of the robot in the form of an Euler angle, carrying out smooth interpolation, and constraining a posture error by utilizing a Jacobian matrix; s3, iteratively optimizing the curvature of the position path curve under the condition that the tolerance and continuity requirements are met; s4, determining a position and attitude synchronization condition, and further calculating an attitude path curve transition length and a control point parameter; s5, optimizing a speed curve and realizing non-sudden-change transition; and S6, the discrete command is converted into a joint angle instruction through inverse kinematics, and the robot is controlled to move along the designed track. The method is suitable for robot motion track smoothing of a complex manufacturing task, can solve the problems that the generated path curve does not pass through an initial path point and the speed suddenly changes, and remarkably improves the running precision and efficiency of the robot.

Description

Technical Field

[0001] The present invention belongs to the field of robot control, and in particular relates to a robot motion trajectory smoothing method, system and storage medium. Background Art

[0002] Traditionally, robots are mainly used to perform point-to-point object transportation tasks. These tasks have no special requirements for the robot's motion trajectory. Usually, the robot's joint angles are directly controlled in the joint space. The planned objects are joint variables, joint angular velocities, joint angular accelerations and other motion quantities. However, the resulting robot trajectory is not intuitive enough and has errors. With the improvement of robot stiffness and accuracy, robots have begun to be widely used in more complex manufacturing tasks, such as milling, welding and deburring. These tasks require the robot to move continuously and accurately along the curved contour defined in Cartesian space, while ensuring that the robot's motion trajectory has continuity in speed and acceleration.

[0003] Trajectory planning in Cartesian space is more complex than trajectory planning in joint space. It usually uses a continuous linear path generated by discrete reference commands to approximate the original curved contour. However, due to the tangential discontinuity at the corners of the linear path, the robot actually passes through the corner at a speed much lower than the command feed rate to ensure that the joint does not exceed the kinematic limits, thereby avoiding shock and vibration. To solve this problem, the original linear path must be smoothed.

[0004] According to the application type, linear path smoothing methods can be divided into two categories: global smoothing methods and local smoothing methods. The global smoothing method achieves overall smoothing of the linear path by fitting all discrete path points. This method can provide high global continuity, but it is difficult to accurately evaluate and constrain the end-effector position and posture path errors. In contrast, the local smoothing method connects adjacent linear path segments by replacing local angles while retaining part of the initial linear path. This method not only achieves high-order continuity between the inserted transition segment and the original linear segment, but also can accurately control the path error introduced by the smoothing process. However, the path curve generated by the local smoothing method usually does not pass through the discrete path points, which may lead to information loss, and the robot runs at a low speed and has mutations at the transition curve. Summary of the invention

[0005] The main purpose of the present invention is to overcome the deficiencies of the above-mentioned prior art and to provide a robot motion trajectory smoothing method, system and storage medium, which respectively construct position and posture path curves at local corners that can accurately pass through discrete path points, and realize synchronous control, so as to solve the problem that the path curve generated by the prior art does not pass through discrete path points, resulting in loss of feature information, and there is a sudden change in speed at the transition curve, so as to ensure that the robot runs accurately along the designed trajectory, so as to meet the needs of high-precision and high-efficiency industrial applications, and provide an effective solution for efficient robot applications in complex manufacturing tasks.

[0006] To achieve the above purpose, the technical solution provided by the present invention is:

[0007] A robot motion trajectory smoothing method, characterized by comprising:

[0008] Step S1: Obtain the discrete path points of the robot motion, determine the transition length of the position path curve, and generate a path that satisfies C by setting specific node vectors and control point construction rules. 3 B-spline position path curve that is continuous and accurate through discrete path points; adjust control points to constrain position path errors;

[0009] Step S2. The robot end effector posture is expressed in the form of Euler angles, and smooth interpolation is performed in the rotation space composed of Euler angles; the transition length of the posture path curve is determined, and a B-spline posture path curve is generated using the same structure as the position path curve; the posture path error is estimated by the Jacobian matrix and the rotation angle, and the constraint is imposed by adjusting the control points;

[0010] Step S3: Calculate the curvature of the position path curve, iteratively optimize the control point position and transition length to reduce the curvature while meeting the tolerance and continuity requirements, and use least squares regression to fit the relationship between the position transition length and the control point at different angles;

[0011] Step S4: Determine the necessary and sufficient conditions for synchronization of position and attitude by deriving relevant differential equations, and then calculate the transition length and control points of the attitude path curve;

[0012] Step S5: Calculate the allowable speed of each transition curve according to the maximum speed, tangential acceleration, tangential jerk and chord tolerance constraints; define the area where the allowable speed is less than the maximum speed as the speed sensitive area, and set the speed of the speed sensitive area to the minimum value of the area; The speeds of different areas are transitioned without sudden changes through the S-shaped acceleration and deceleration algorithm;

[0013] Step S6: Use parameter interpolation based on Taylor expansion and arc length compensation to obtain the smooth motion trajectory of the robot, and obtain the joint angle drive command through robot inverse kinematics calculation, and finally control the robot to run along the designed trajectory.

[0014] Preferably, the step S1 adopts: in the workpiece coordinate system, obtaining the discrete path points of the robot movement, Q i (i=0, 1, ..., n) is the position of each discrete path point. Taking the discrete path point positions Q1, Q2 and Q3 of two adjacent position paths as an example, the position transition length l is determined. p satisfy

[0015]

[0016] Part of the linear path is reserved for synchronous adjustment of position and posture; the B-spline position path curve C is constructed by setting specific node vectors and control points p (u), so that it passes through each discrete path point and satisfies C 3 Continuity requirement, control point P i (i=0, 1, ..., 10) satisfies

[0017] 2||P0P1||=2||P9P 10 ||=||P1P2||=||P2P3||=||P7P8||=||P8P9||.

[0018] Analytically calculate the maximum error ε of the position path p By adjusting the control points to ensure that they are within the user-defined tolerance range, the maximum error of the position path is calculated as follows:

[0019]

[0020] Where, d p represents the length of vectors P5P4 and P5P6, and α is the angle between the initial position path segments.

[0021] Preferably, the step S2 adopts: the rotation matrix R corresponding to each position path point is expressed as the Euler angle B=[α, β, γ] of the three-axis rotation around the fixed workpiece coordinate system T Form, each Euler angle coordinate B i (i=0, 1, ..., n) constitutes the initial posture path segment, performs smooth interpolation in the rotation space composed of Euler angles, and generates the posture path curve C using the same structure as the position path curve 。 (u); use the Jacobian matrix J and the rotation space coordinates to estimate the end effector posture error, and constrain the end effector posture error range by adjusting the control points. The calculation formula of the Jacobian matrix J is:

[0022]

[0023] In the formula, S α , S β , S γ, C ɑ , C β and C γ They represent sinα, sinβ, sinγ, cosα, cosβ and cosγ respectively.

[0024] Preferably, the step S3 adopts: calculating the first and second order derivatives of the position path curve C(u) to obtain the curvature

[0025]

[0026] Iteratively optimize the position path curve control point P i With transition length l p To reduce curvature and meet tolerance and C 3 Continuity requirement, fitting the curve control points P at different angles by least squares regression i With transition length l p The relationship between

[0027]

[0028] Preferably, the step S4 adopts: deriving the first-order, second-order and third-order differential equations of the end effector posture relative to the displacement, determining the necessary and sufficient conditions for the synchronization of the position and posture path; combining the position path related parameters, the transition length of the posture path curve satisfies

[0029]

[0030] In addition, to comply with the attitude tolerance constraint, the attitude path curve should also satisfy

[0031]

[0032] Where, d o represents the length of vectors O5O4 and O5O6, and β is the angle between the initial posture path segments.

[0033] Preferably, the step S5 adopts: based on kinematic and geometric constraints, the maximum speed, tangential acceleration, tangential jerk and chord tolerance constraints of each spline curve are comprehensively considered to calculate the allowable speed v lim (u), and define the area where the speed is allowed to be lower than the set threshold as the speed sensitive area, and set the speed of this area as the minimum value of the area. The S-shaped acceleration and deceleration algorithm is used to achieve a non-abrupt speed transition between different areas.

[0034] Preferably, step S6 adopts: obtaining a smooth motion trajectory of the robot based on parameter interpolation of Taylor expansion and arc length compensation to reduce the speed error caused by trajectory error; calculating discrete joint angle drive commands in combination with inverse kinematics, and finally controlling the robot to run along the planned trajectory.

[0035] A robot motion trajectory smoothing system, characterized by comprising:

[0036] The path smoothing module is configured to fit discrete path points using B-spline curves, adjust control point constraint position errors, and perform smooth interpolation in rotation space to achieve synchronization of position and attitude paths;

[0037] A speed planning module, configured to calculate the allowed speed and smoothly transition it through an S-shaped acceleration and deceleration algorithm;

[0038] The parameter interpolation module is configured to obtain the smooth motion trajectory of the robot by using parameter interpolation of Taylor expansion and arc length compensation, and calculate the joint angle drive command in combination with inverse kinematics.

[0039] A computer-readable storage medium having a computer program stored thereon, characterized in that when the computer program is executed by a processor, it can implement any of the above method steps and call the functional modules of any of the above systems.

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

[0041] 1. The robot motion trajectory smoothing method designed by the present invention can achieve global C 3 For the synchronous control of continuous and end-effector position and attitude, the inserted local B-spline curves can be calculated analytically without any iterative calculation;

[0042] 2. The present invention can realize a smooth curve passing through initial discrete path points based on tolerance constraints and curvature optimization;

[0043] 3. The present invention can effectively reduce the curvature of the designed path curve, increase the running speed of the robot and reduce speed fluctuations. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] In order to more clearly illustrate the technical solution of the embodiment of the present invention, the drawings of the embodiment are briefly introduced below.

[0045] Figure 1 A schematic diagram of a robot motion trajectory smoothing method provided by an embodiment of the present invention;

[0046] Figure 2 A position path curve structure diagram provided by an embodiment of the present invention;

[0047] Figure 3 A structural diagram of a posture path curve provided by an embodiment of the present invention;

[0048] Figure 4 A robot experimental platform shown in an embodiment of the present invention;

[0049] Figure 5 The initial path diagram of the robot shown in the embodiment of the present invention;

[0050] Figure 6 A schematic diagram of a position path curve shown in an embodiment of the present invention;

[0051] Figure 7 A schematic diagram of a posture path curve shown in an embodiment of the present invention;

[0052] Figure 8 This is a schematic diagram showing a comparison of tracking errors of various joints of a robot according to an embodiment of the present invention. Specific implementation methods

[0053] The following describes the implementation of the present invention through specific embodiments and in conjunction with the accompanying drawings.

[0054] In view of the problem that the path curve in the prior art does not pass through discrete path points, resulting in loss of feature information, the present invention provides a robot motion trajectory smoothing method, which constructs a position and posture path curve that can accurately pass through discrete path points and achieve synchronization, so that the robot can run accurately along the designed trajectory.

[0055] Figure 1 A schematic diagram of a robot motion trajectory smoothing method provided by an embodiment of the present invention is described in detail below.

[0056] The robot motion trajectory smoothing method provided by the present invention comprises:

[0057] In the workpiece coordinate system, obtain the discrete path points of the robot motion, Q i (i=0, 1, ..., n) is the position of each discrete path point. Taking the discrete path point positions Q1, Q2 and Q3 of two adjacent position paths as an example, the position transition length l is determined. p satisfy

[0058]

[0059] Quintic B-spline curve C for smoothing position path p (u) can be expressed as follows

[0060]

[0061] Where k = 5 is the B-spline order; P i (i=0,1,...,n) represents the control point; the knot vector of the B-spline is set to U=[0,0,0,0,0,0,0.25,0.5,0.5,0.5,0.75,1,1,1,1,1,1,1], and the repetition at u=0 and u=1 is set to 6 to ensure that the curve passes through P0 and P 10; N i，k is the B-spline basis function, which can be evaluated using the following recursive equation

[0062]

[0063] To ensure that the intersection of the linear path and the spline curve is C 3 Continuity, that is, at u = 0 and u = 1, the following equation should hold

[0064]

[0065] In the formula, ω=2,3 represents the second and third order derivatives, C 1 Continuity is already achieved by the position of the control points, and the equation is further calculated as

[0066]

[0067] Set vector P0P1, P9P 10 The lengths are 0.5l pa , 0.5l pb Due to the symmetry of the initial generated transition curve, in order to ensure that the C 3 Continuity, the relationship between control points can be expressed as follows

[0068] 2||P0P1||=2||P9P 10 ||=||P1P2||=||P2P3||=||P7P8||=||P8P9||=l p

[0069] Figure 2 The position path curve structure diagram provided by the embodiment of the present invention, P0~P 10 is the control point of the B-spline curve in the figure, and the calculation formula of the control point is

[0070]

[0071] In the formula, l pa = l pb = l p ; c is a preset smoothing length ratio, which is set to c=5; e1~e4 are the unit vectors of vectors P5P4, P5P6, P5P3 and P5P7 respectively; d p Represents the length of vectors P5P4 and P5P6; vectors P5P4 and P5P6 are in opposite directions and have equal lengths to ensure that the curve passes through the discrete path point Q2.

[0072] Due to the symmetry of the position path curve, only the maximum position path error ε in the interval (0.5, 1] ​​is calculated p . Let the B-spline function Cp (u) The derivative component perpendicular to the direction of Q2Q3 is zero, and the equation is established, that is, C p,y ′(u)=0, and u=0.6327 is obtained, and the maximum error ε of the position path is further calculated p for

[0073]

[0074] The rotation matrix R corresponding to each position path point is expressed as the Euler angle B = [α, β, γ] rotating around the three axes of the fixed workpiece coordinate system XYZ in sequence T Form, each Euler angle coordinate B i (i=0,1,...,n) form the initial posture path segment, and perform smooth interpolation in the rotation space composed of Euler angles. Determine the transition length of the posture path curve, and use the same structure as the position path. The spline curve is inserted into the angle between each two initial path points to achieve the C position of the end effector posture. 3 Continuous motion. Figure 3 The structure diagram of the posture path curve provided in the embodiment of the present invention, the B-spline curve C used o (u) is expressed as

[0075]

[0076] In the formula, O i (i=0, 1...n) represents a control point; similarly, the degree of the posture path curve is set to k=5.

[0077] When the posture path error is small, it can be obtained by the Jacobian matrix of the end effector posture and the rotation space coordinates O = [α, β, γ] T To approximate

[0078] ΔPO≈JΔO

[0079] Where J is the Jacobian matrix of the end effector posture, and the calculation formula is:

[0080]

[0081] The posture path error ΔPO is defined as the shortest deviation between the initial posture path and the constructed smooth spline curve, so the following relationship exists

[0082] ΔPO≤min{||JΔO||| u＝0.3673 ,||JΔO||| u＝0.6327}

[0083] Since the structure of the position path curve and the attitude path curve are similar, the formula for the maximum error of the position path can also be used for the attitude path curve. The above formula can be further organized as

[0084] ΔPO≤min{0.518cos(β / 2)d o J| u＝0.3673 e9, 0.518cos(β / 2)d o J| u＝0.6327 e 10}

[0085] In the formula, e9, e 10 is a unit vector perpendicular to B1B2, B2B3.

[0086] Since the attitude path error should satisfy ΔPO≤2sin(ε ow / 2), so the sufficient condition to satisfy the attitude tolerance constraint is

[0087]

[0088] In d p ≤d pw , under the premise of known transition length l p In the case of , an iterative optimization method is used to reduce the curvature of the position path curve. p The initial value is set to 0, and then it is increased with the program loop iteration until the curvature no longer decreases. The curvature calculation formula is as follows

[0089]

[0090] The final calculated d p With l p The corresponding relationship can be regarded as the independent variable as a function of the angle α, and the least squares regression method is used for fitting

[0091]

[0092] In order to achieve synchronization between the position and attitude of the end effector, the first, second and third order differentials of the end effector attitude relative to the displacement s should satisfy

[0093]

[0094] Combined with the control polygon structure of the position path and attitude path mentioned above, the necessary and sufficient condition for the third-order continuity of the end effector attitude relative to the displacement s is:

[0095]

[0096] Furthermore, the attitude path curve under attitude tolerance constraint satisfies

[0097]

[0098] To ensure that the robot has a continuous speed without sudden changes after interpolation planning, the maximum speed, tangential acceleration, tangential jerk and chord tolerance limit of each spline curve are comprehensively considered based on kinematic and geometric constraints, and the allowable speed is calculated as

[0099]

[0100] In the formula, F, ε, a max and j max They are defined as maximum velocity, position path tolerance, maximum tangential acceleration and maximum tangential jerk respectively, and T is the interpolation period.

[0101] The area where the speed is allowed to be lower than the set maximum speed is defined as a sensitive area, and the speed of the area is set to the minimum value in each area. By adopting the S-shaped acceleration and deceleration algorithm, the speed transition between different areas can be achieved without sudden changes.

[0102] The smooth motion trajectory of the robot is obtained by parameter interpolation based on Taylor expansion and arc length compensation, and the joint angle drive command is obtained by inverse kinematics calculation of the robot, and finally the robot is controlled to run along the designed trajectory. The interpolation parameter calculation formula is:

[0103]

[0104] In the formula, s k Indicates that at t k The total arc length at time, u k Indicates t k Interpolation parameters at time instant.

[0105] In order to verify the effectiveness of the robot motion trajectory smoothing method provided by the present invention, a UR10 robot with 6 revolute joints (6R) is taken as an example. Figure 4 The robot experimental platform shown in the embodiment of the present invention is shown. The host computer receives motion data in real time at a maximum frequency of 500Hz to collect the joint position and speed of the robot. The sampling period and interpolation period are set to 0.002s, and the position and posture path tolerances are set to 0.3mm and 0.005rad respectively. During the speed scheduling, the maximum speed, tangential acceleration and tangential jerk are set to 25mm / s and 2000mm / s respectively. 2 and 20000mm / s 3 .

[0106] Figure 5 The initial path diagram of the robot shown in the embodiment of the present invention is a typical original path including multiple angles and end effector postures that is smoothed.

[0107] Figure 6 is a schematic diagram of a position path curve shown in an embodiment of the present invention, Figure 7The figure is a schematic diagram of a posture path curve shown in an embodiment of the present invention. The method proposed in the present invention and the inscribed smoothing method are respectively used to smooth the initial path. By comparison, it can be verified that the method of the present invention can achieve the advantage of passing the initial discrete path points based on tolerance constraints and curvature optimization.

[0108] Figure 8 The maximum tracking error (difference between target value and actual value) of each joint of the method of the present invention and the existing inscribed smoothing method is compared. The method of the present invention is superior to the existing inscribed smoothing method in terms of robot operation accuracy and can significantly reduce the maximum tracking error of the robot operation.

[0109] The present invention has been described above in conjunction with preferred embodiments, but these embodiments are only exemplary and serve only as an illustration. On this basis, the present invention may be subjected to a variety of substitutions and improvements, all of which fall within the scope of protection of the present invention.

Claims

1. A robot motion trajectory smoothing method, characterized in that: include: Step S1: Obtain the discrete path points of the robot motion, determine the transition length of the position path curve, and generate a path that satisfies C by setting specific node vectors and control point construction rules. 3 B-spline position path curve that is continuous and accurate through discrete path points; adjust control points to constrain position path errors; Step S2: The robot end effector posture is expressed in the form of Euler angles, and smooth interpolation is performed in the rotation space composed of Euler angles; the transition length of the posture path curve is determined, and a B-spline posture path curve is generated using the same structure as the position path curve; the posture path error is estimated through the Jacobian matrix and the rotation angle, and the control points are adjusted to constrain: Step S3: Calculate the curvature of the position path curve, iteratively optimize the control point position and transition length to reduce the curvature while meeting the tolerance and continuity requirements, and use least squares regression to fit the relationship between the position transition length and the control point at different angles; Step S4: Determine the necessary and sufficient conditions for synchronization of position and attitude by deriving relevant differential equations, and then calculate the transition length and control points of the attitude path curve; Step S5: Calculate the allowable speed of each transition curve according to the maximum speed, tangential acceleration, tangential jerk and chord tolerance constraint; define the area where the allowable speed is less than the set maximum speed as the speed sensitive area, and set the speed of the speed sensitive area as the minimum value of the area; The speeds of different areas are transitioned without sudden changes through the S-shaped acceleration and deceleration algorithm; Step S6: Use parameter interpolation based on Taylor expansion and arc length compensation to obtain the smooth motion trajectory of the robot, and obtain the joint angle drive command through robot inverse kinematics calculation, and finally control the robot to run along the designed trajectory.

2. The robot motion trajectory smoothing method according to claim 1, characterized in that: The step S1 adopts: in the workpiece coordinate system, obtaining the discrete path points of the robot movement, Q i (i=0, 1, ..., n) is the position of each discrete path point. Taking the discrete path point positions Q1, Q2 and Q3 of two adjacent position paths as an example, the position transition length l is determined. p satisfy Part of the linear path is reserved for synchronous adjustment of the end effector position and posture; the B-spline position path curve C is constructed by setting specific node vectors and control points p (u), so that it passes through each discrete path point and satisfies C 3 Continuity requirement, control point P i (i=0, 1, ..., 10) satisfies 2||P0P1||=2||P9P 10 ||=||P1P2||=||P2P3||=||P7P8||=||P8P9|| The maximum error of the position path εp is calculated analytically and the control points are adjusted to ensure that it is within the user-defined tolerance range. The calculation formula for the maximum error of the position path is: Where, d p represents the length of vectors P5P4 and P5P6, and α is the angle between the initial position path segments.

3. The robot motion trajectory smoothing method according to claim 1, characterized in that: The step S2 adopts: the rotation matrix R corresponding to each position path point is expressed as the Euler angle B=[α, β, γ] of the three-axis rotation around the fixed workpiece coordinate system T Form, each Euler angle coordinate B i (i=0, 1, ..., n) constitutes the initial posture path segment, performs smooth interpolation in the rotation space composed of Euler angles, and generates the posture path curve C using the same structure as the position path curve o (u); use the Jacobian matrix J and the rotation space coordinates to estimate the attitude path error, and constrain it by adjusting the control points. The calculation formula of the Jacobian matrix J is: In the formula, S ɑ , S β , S γ , C α , C β and C γ They represent sinα, sinβ, sinγ, cosα, cosβ and cosγ respectively.

4. The robot motion trajectory smoothing method according to claim 1, characterized in that: The step S3 adopts: calculating the first and second order derivatives of the position path curve C(u) to obtain the curvature Iteratively optimize the position path curve control point P i With transition length l p To reduce curvature and meet tolerance and C 3 Continuity requirement, fitting the curve control points P at different angles by least squares regression i With transition length l p The relationship between 5. The robot motion trajectory smoothing method according to claim 1, characterized in that: The step S4 adopts: deriving the first-order, second-order and third-order differential equations of the end effector posture relative to the displacement, determining the necessary and sufficient conditions for the synchronization of the position and posture path; combining the position path related parameters, the transition length of the posture path curve satisfies In addition, to comply with the attitude tolerance constraint, the attitude path curve should also satisfy Where, d o represents the length of vectors O5O4 and O5O6, and β is the angle between the initial posture path segments.

6. The robot motion trajectory smoothing method according to claim 1, characterized in that: The step S5 adopts: based on kinematics and geometric constraints, the maximum speed, tangential acceleration, tangential jerk and chord tolerance constraints of each spline curve are comprehensively considered to calculate the allowable speed v lim (u), and define the area where the speed is allowed to be lower than the set threshold as the speed sensitive area, and set the speed of this area as the minimum value of the area; the S-shaped acceleration and deceleration algorithm is used to achieve a non-sudden speed transition between different areas.

7. The robot motion trajectory smoothing method according to claim 1, characterized in that: The step S6 adopts: parameter interpolation based on Taylor expansion and arc length compensation to obtain the smooth motion trajectory of the robot to reduce the speed error caused by the trajectory error; discrete joint angle drive commands are calculated in combination with inverse kinematics, and finally the robot is controlled to run along the planned trajectory.

8. A robot motion trajectory smoothing system, characterized in that: include: The path smoothing module is configured to fit discrete path points using B-spline curves, represent the robot end effector posture in the form of Euler angles, and perform smooth interpolation in the rotation space to achieve synchronization of position and posture paths; A speed planning module, configured to calculate the allowed speed and smoothly transition it through an S-shaped acceleration and deceleration algorithm; The parameter interpolation module is configured to obtain the smooth motion trajectory of the robot by using parameter interpolation of Taylor expansion and arc length compensation, and calculate the joint angle drive command in combination with inverse kinematics.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, it can implement any method step described in claims 1 to 7 and call any functional module of the system described in claim 8.

Images