Seven-degree-of-freedom loading and unloading robot inverse solution optimization algorithm

Abstract

The application provides a seven-freedom-degree loading and unloading robot inverse solution optimization algorithm, which sets a pose matrix of the end of the robot in an initial state and initial joint angles θ of each joint of the robot 0 , sets a pose matrix of the end of the robot in a target state, calculates an angular displacement difference between the joint angles of each joint of the robot in the target state and the joint angles of each joint of the robot in the initial state, increases or decreases the joint angles of the redundant joints by a first adjustment value, and adjusts the first adjustment value based on a redundant angle variance algorithm until the angular displacement difference of the joint angles of each joint of the robot reaches a minimum. The application discloses a seven-freedom-degree loading and unloading robot inverse solution optimization algorithm, solves the kinematics inverse solution of the robot, has no numerical calculation error, has a simple calculation process, fully meets the real-time control requirements of the robot, can conveniently avoid obstacles, and can well eliminate the singularity of a wrist and a shoulder.

Description

Technical Field

[0001] This invention relates to the field of inverse kinematics of loading and unloading robots, and in particular to an inverse kinematics optimization algorithm for a seven-degree-of-freedom loading and unloading robot. Background Technology

[0002] Traditional six-DOF loading and unloading robots have a one-to-one correspondence between their end-effector pose and joint angles. This means that once the end-effector pose is determined, only a limited number of joint positions correspond to it. The drawback of this type of traditional robot is its lack of flexibility. If there are obstacles or singular configurations at the joint positions corresponding to a specific pose, the robot's joint angles have no room for adjustment, thus making it impossible to achieve the specified pose. Summary of the Invention

[0003] The purpose of this invention is to provide an inverse kinematics optimization algorithm for a seven-degree-of-freedom loading and unloading robot, which optimizes the robot's inverse kinematics and improves the robot's operational efficiency.

[0004] To achieve the above objectives, the present invention provides the following technical solution: an inverse kinematics optimization algorithm for a seven-DOF loading and unloading robot, wherein the robot comprises seven joints and has seven degrees of freedom, and the seven joints are sequentially connected, comprising the following steps: setting the pose matrix of the robot's end effector and the initial joint angles of each joint of the robot in the initial state. ; Set the pose matrix of the robot's end effector in the target state; Select one joint of the robot as a redundant joint, and calculate the target joint angles of each joint of the robot in the target state based on the joint angles of the redundant joint in the initial state and the pose matrix of the robot's end effector in the target state. ; Calculate the angular displacement difference between the joint angles of each joint of the robot in the target state and the joint angles of each joint of the robot in the initial state; Maintain the pose matrix of the end effector of the robot in the target state, and increase or decrease the joint angles of the redundant joints by a first adjustment value; Adjust the first adjustment value based on the redundancy angle variance algorithm until the angular displacement difference between the joint angles of each joint of the robot reaches the minimum, and take the first adjustment value at this time as the optimal adjustment value.

[0005] Furthermore, the seven sequentially connected joints are the first joint, the second joint, the third joint, the fourth joint, the fifth joint, the sixth joint, and the seventh joint, wherein the seventh joint is the end joint of the robot, and the redundant joint is the seventh joint.

[0006] Furthermore, the joint angle of the first joint The expression is: (8)

[0007] in, This represents the link offset of the fourth joint of the robot. Let be the link offset of the sixth joint of the robot, where , , 'a' is the approach axis of the robot's end effector. x a y a z These are the components of the approach axis along the x-axis, y-axis, and z-axis in the Cartesian coordinate system, respectively, and o is the direction axis of the robot's end effector. x o y o z These are the components of the direction axis along the x-axis, y-axis, and z-axis in the Cartesian coordinate system, respectively, where n is the vertical axis of the robot's end effector. x n y n z These are the components of the vertical axis along the x, y, and z axes in the Cartesian coordinate system. n, o, and a represent the pose of the robot's end effector joint 7 coordinate system mapped to the joint 6 coordinate system relative to the base coordinate system. The coordinate system of the seventh joint is mapped to the position of the coordinate system of the sixth joint relative to the base coordinate system of the robot.

[0008] Furthermore, the joint angle of the fifth joint The expression is:

[0009] (9)

[0010] in, , .

[0011] Furthermore, the sixth joint angle The expression is as follows:

[0012] (10)

[0013] in: .

[0014] Furthermore, the second joint angle Third joint angle Fourth joint angle The expressions are as follows:

[0015] (11)

[0016] (12)

[0017] (13)

[0018] In the formula: , , , , This represents the link offset of the robot's first joint. This represents the link offset of robot joint 5. , This is the link length of the robot's fourth joint. , .

[0019] Furthermore, the step of adjusting the first adjustment value based on the redundant angle variance algorithm until the angular displacement difference of the joint angles of the robot's joints reaches its minimum, and using the first adjustment value at this point as the optimal inverse solution, includes:

[0020] S1: Based on the redundant angle variance algorithm, obtain the linear relationship between the angular displacement of the seventh joint and the angular displacement of the remaining 6 robot joints, and express the linear relationship in the form of a linear absolute value function piecewise line in a two-dimensional coordinate system;

[0021] S2: Find the linear absolute value function polyline with the largest y-coordinate of its intersection point with the Y-axis of the two-dimensional coordinate system, and use the found linear absolute value function polyline as the reference polyline;

[0022] S3: Along the descent direction of the y-coordinate of the reference broken line, identify the first linear absolute value function broken line that intersects the reference broken line, and take the identified linear absolute value function broken line as the target broken line;

[0023] S4: Starting from the intersection of the baseline polyline and the target polyline, along the descending direction of the y-coordinate of the baseline polyline, determine whether the y-coordinate of the target polyline increases. If the y-coordinate increases, take the current x-coordinate as the optimal adjustment value. If the y-coordinate decreases, take the target polyline as the baseline polyline and repeat step S3.

[0024] Preferably, step S1 includes:

[0025] The adjusted joint angles of each joint of the robot are determined after the joint angles of the redundant joints are increased or decreased by the first adjustment value. ;

[0026] Calculate and adjust joint angles Relative to target joint angle angular displacement , ,in, , These are the joint angles of the robot's first joint, second joint, third joint, fourth joint, fifth joint, sixth joint, and seventh joint, respectively, under the target state. , The joint angles of the robot's first joint, second joint, third joint, fourth joint, fifth joint, sixth joint, and seventh joint are respectively adjusted by increasing or decreasing the joint angles of the redundant joints by a first adjustment value. ; , , , , , , Joint angles Relative to joint angle The angular displacements of the first joint, the second joint, the third joint, the fourth joint, the fifth joint, the sixth joint, and the seventh joint.

[0027] Confirm angular displacement Angular displacement relative to the seventh joint The expression:

[0028] (14)

[0029] in, .

[0030] Analysis shows that the present invention discloses an inverse kinematics optimization algorithm for a seven-degree-of-freedom loading and unloading robot. Under the condition of robot decoupling, the inverse kinematics of the robot is solved. It not only has no numerical calculation error, but also has a simple calculation process, which fully meets the real-time control requirements of the robot. Furthermore, the robot with this configuration can perform self-motion around the straight line connecting the shoulder joint and the wrist joint, which can easily avoid obstacles and can effectively eliminate the singularity of the wrist and shoulder. Attached Figure Description

[0031] The accompanying drawings, which form part of this application, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an undue limitation of the invention. Wherein:

[0032] Figure 1 A schematic diagram of the structure of a robot according to an embodiment of the present invention.

[0033] Figure 2 A schematic diagram of the coordinates of a robot according to an embodiment of the present invention.

[0034] Figure 3 A schematic diagram of a linear absolute value broken line according to an embodiment of the present invention.

[0035] Figure 4 A flowchart of an embodiment of the present invention.

[0036] Figure 5 This is a schematic diagram of the redundancy angle variance algorithm according to an embodiment of the present invention;

[0037] Explanation of reference numerals in the attached diagram: 1. Base; 2. First joint; 3. Second joint; 4. Upper arm; 5. Third joint; 6. Forearm; 7. Fourth joint; 8. Fifth joint; 9. Sixth joint; 10. Seventh joint. Detailed Implementation

[0038] The present invention will now be described in detail with reference to the accompanying drawings and embodiments. Various examples are provided by way of explanation and not by way of limitation. Indeed, those skilled in the art will recognize that modifications and variations can be made to the invention without departing from its scope or spirit. For example, a feature shown or described as part of one embodiment may be used in another embodiment to produce yet another embodiment. Therefore, it is desirable that the invention encompass such modifications and variations falling within the scope of the appended claims and their equivalents.

[0039] In the description of this invention, the terms "longitudinal," "lateral," "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "top," and "bottom," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are used only for the convenience of describing the invention and do not require the invention to be constructed and operated in a specific orientation; therefore, they should not be construed as limitations on the invention. The terms "connected," "linked," and "set up" used in this invention should be interpreted broadly. For example, they can refer to a fixed connection or a detachable connection; a direct connection or an indirect connection through intermediate components; a wired connection, a radio connection, or a wireless communication signal connection. Those skilled in the art can understand the specific meaning of the above terms according to the specific circumstances.

[0040] The accompanying drawings illustrate one or more examples of the invention. The detailed description uses numerals and letters to refer to features in the drawings. Similar or analogous reference numerals in the drawings and description have been used to refer to similar or analogous parts of the invention. As used herein, the terms “first,” “second,” “third,” and “fourth,” etc., are used interchangeably to distinguish one component from another and are not intended to indicate the location or importance of a single component.

[0041] like Figures 1-5As shown, according to an embodiment of the present invention, a seven-DOF loading and unloading robot inverse kinematics optimization algorithm is provided. The robot includes seven joints and has seven degrees of freedom. The seven joints are connected sequentially. The present invention uses a seven-DOF robot with a pitch-rotor joint. All seven joints of this seven-DOF robot are rotary joints. The first joint 2 is located on the base 1. The second joint 3 is connected to the third joint 5 through the upper arm 4. The third joint 5 is connected to the fourth joint 7 through the forearm 6. The rotation axes of the second joint 3, the third joint 5, and the fourth joint 7 of the robot are parallel to each other, and the rotation axes of the fifth joint 8, the sixth joint 9, and the seventh joint 10 are perpendicular to each other. This robot has seven degrees of freedom. This structural design is formed by adding a pitch-rotor joint after the sixth joint 9 of a six-DOF collaborative robot. The robot with this configuration can perform self-motion around the straight line connecting the shoulder joint and the wrist joint, which can easily avoid obstacles and can effectively eliminate the singularity of the wrist and shoulder. The algorithm includes the following steps: setting the pose matrix of the robot's end effector and the initial joint angles of each joint of the robot in the initial state. Select one joint of the robot as a redundant joint, and calculate the target joint angles of each joint of the robot in the target state based on the joint angles of the redundant joint in the initial state and the pose matrix of the robot's end effector in the target state. The joint angles of redundant joints are called redundant angles. Calculate the angular displacement difference between the joint angles of each joint of the robot in the target state and the joint angles of each joint of the robot in the initial state. Maintain the pose matrix of the robot's end effector in the target state, and increase or decrease the joint angles of redundant joints by a first adjustment value. Adjust the first adjustment value based on the redundancy angle variance algorithm until the angular displacement difference of the joint angles of each joint of the robot reaches its minimum, and take the first adjustment value at this time as the optimal adjustment. The optimal adjustment value is the change value of the joint angles of redundant joints when the robot is in the target state, at which time the angular displacement of the largest joint angle among the robot's joints is the minimum.

[0042] Preferably, the seven joints connected in sequence are the first joint 2, the second joint 3, the third joint 5, the fourth joint 7, the fifth joint 8, the sixth joint 9, and the seventh joint 10, wherein the seventh joint 10 is the end joint of the robot and the seventh joint 10 is a redundant joint.

[0043] Preferably, the joint angle of the first joint 2 The expression is: (15)

[0044] in, This refers to the link offset of the fourth joint 7 of the robot. This refers to the link offset of the sixth joint 9 of the robot. , , n, o, and a are used to represent the three axes of the motion coordinate system relative to the center point of the end effector in the base coordinate system. To avoid collisions with the manipulated object during robot movement, the robot's end effector must approach the object along the z-axis of the operating end. In this case, a is the approach axis of the robot's end effector. x a y a z These are the components of the approach axis along the x-axis, y-axis, and z-axis in the Cartesian coordinate system. o represents the direction axis of the robot's end effector. x o y o z These are the components of the direction axis along the x-axis, y-axis, and z-axis in the Cartesian coordinate system. n is the vertical axis of the robot's end effector. x n y n z These are the components of the vertical axis along the x-axis, y-axis, and z-axis in the Cartesian coordinate system. n, o, and a represent the pose of the robot's seventh joint 10 relative to the base coordinate system, mapped from the coordinate system of the sixth joint 9. To map the coordinate system of the seventh joint 10 to the coordinate system of the sixth joint 9 relative to the robot's base coordinate system, p x p y and p z The coordinate system of the seventh joint 10 is mapped to the coordinate system of the sixth joint 9 relative to the x-axis, y-axis, and z-axis positions of the robot's base coordinate system.

[0045] Preferably, the joint angle of the fifth joint 8 The expression is:

[0046] (16)

[0047] in, , .

[0048] Joint angle of joint 9 (sixth joint) The expression is as follows:

[0049] (17)

[0050] in: .

[0051] The joint angle of the second joint 3 The joint angle of the third joint 5 The joint angle of the fourth joint 7 The expressions are as follows:

[0052] (18)

[0053] (19)

[0054] (20)

[0055] In the formula: , , , , This represents the link offset of the robot's first joint 2. This represents the link offset of robot joint 5. , This refers to the link length of the robot's fourth joint 7. , .

[0056] Preferably, the first adjustment value is adjusted based on the redundancy angle variance algorithm until the angular displacement difference of the joint angles of the robot's joints reaches its minimum, and the first adjustment value at this point is taken as the optimal inverse solution, including:

[0057] S1: Based on the redundant angle variance algorithm, obtain the linear relationship between the angular displacement of the seventh joint 10 and the angular displacements of the remaining 6 robot joints, and express the linear relationship in the form of a linear absolute value function piecewise line in a two-dimensional coordinate system.

[0058] Step S1 includes: calculating the adjusted joint angles of each joint of the robot after increasing or decreasing the joint angle of the redundant joint by the first adjustment value. ; Calculate and adjust joint angles Relative to target joint angle angular displacement , ,in, , These are the joint angles of the robot's first joint 2, second joint 3, third joint 5, fourth joint 7, fifth joint 8, sixth joint 9, and seventh joint 10, respectively, under the target state. , These are the joint angles of the robot's first joint 2, second joint 3, third joint 5, fourth joint 7, fifth joint 8, sixth joint 9, and seventh joint 10 after the joint angles of the redundant joints are increased or decreased by the first adjustment value. ; , , , , , , Joint angles Relative to joint angle The angular displacements of the first joint 2, the second joint 3, the third joint 5, the fourth joint 7, the fifth joint 8, the sixth joint 9, and the seventh joint 10 are calculated. Due to the presence of redundant joints, the magnitude of the redundant joint angles needs to be specified according to certain principles before inverse kinematics can be performed to calculate the interpolated displacements. A commonly used principle is the efficiency optimization principle. To improve the robot's operational efficiency, without the need for obstacle avoidance, the largest displacement among the seven joints should be minimized as much as possible to reduce the maximum joint speed, or to reduce the motion time and the robot's range of motion at the same speed.

[0059] In the robot of this invention, a redundancy angle variance algorithm is adopted to adjust the displacement magnitude of redundant joints. , , , , , The magnitude of the joint angular displacement, and what causes the joint , , , , , The maximum angular displacement in the middle reaches its minimum. That is, to find... A suitable value for such that:

[0060] (twenty one)

[0061] This means that the angular displacement of the robot's seven joints is minimized. In actual calculations, weights can be added based on the specific characteristics of each joint.

[0062] When a robot performs linear or circular interpolation motion, the pose change is very small with each interpolation, and the interpolation displacement of each joint is also very small, allowing for linearization; that is, when When the value is relatively small, in practical engineering applications, it can be approximately considered that... , , , , , Follow Linear variation. Therefore, using the redundant angle variance algorithm, we can calculate... , , , , , and The approximate linear relationship is then found by searching for the optimal one. Substituting the inverse solution, we can obtain the optimized solution. , , , , , .

[0063] The steps of the redundancy angle variance optimization algorithm are as follows:

[0064] Let the robot's current pose be the initial state, and record the initial joint angles at this point.

[0065] (twenty two)

[0066] And determine the joint point of the seventh joint 10 at this time. position , where vector Indicates wrist joint point The initial state position vector, Matrix representing wrist joint points The initial state attitude matrix.

[0067] wrist joint points in target state The position is Let's assume first equal After inverse solution, the vector Indicates wrist joint point The target state position vector Matrix representing wrist joint points From the target state attitude matrix, the target joint angles are obtained as follows:

[0068] (twenty three)

[0069] And the maximum displacement is calculated as follows:

[0070] (twenty four)

[0071] Keep the current pose unchanged, set After inverse kinematics, the adjusted joint angle is obtained as follows:

[0072] (25)

[0073] Reconfirm angular displacement Angular displacement relative to the seventh joint 10 The expression:

[0074] (26)

[0075] in, .

[0076] Equation (26) is transformed into equation (27) using the linear programming algorithm, where: , , , , , Corresponding to , correspond , , , , , Corresponding to ( ), ( ), ( ), ( ), ( ), ( ).

[0077] (27)

[0078] The constraints of formula (27) are: ,beg Why is it worth it? The graph of a 6-line graph with a minimum value is shown below. Figure 3 As shown.

[0079] S2: Find the linear absolute value function polyline with the largest y-coordinate intersection point with the y-axis of the 2D coordinate system among all linear absolute value function polylines, and use the found linear absolute value function polyline as the baseline polyline; S3: Along the descending direction of the y-coordinate of the baseline polyline, identify the first linear absolute value function polyline that intersects the baseline polyline, and use the identified linear absolute value function polyline as the target polyline; S4: Starting from the intersection point of the baseline polyline and the target polyline, along the descending direction of the y-coordinate of the baseline polyline, determine whether the y-coordinate of the target polyline increases. If the y-coordinate increases, use the current x-coordinate as the optimal adjustment value; if the y-coordinate decreases, use the target polyline as the baseline polyline and repeat step S3. According to the constraints, the optimal solution must exist. This algorithm can be generalized to n polylines, with a time complexity of O[2n(n-1)]. For 7 polylines, an average of 84 baseline points need to be searched. Under current hardware conditions, the time consumed is negligible.

[0080] From the above description, it can be seen that the embodiments of the present invention achieve the following technical effects: Based on the decoupling characteristic of loading and unloading robots, the present invention solves the inverse kinematics of the robot, which not only eliminates numerical calculation errors but also simplifies the calculation process, fully meeting the real-time control requirements of the robot. The key to controlling a redundant seven-free loading and unloading robot is selecting suitable joints as redundant joints and determining the size of the redundant joint angles. The present invention provides the process for selecting redundant joints and proposes a redundancy angle variance algorithm with the optimization objective of reducing the maximum joint displacement. This algorithm is simple to calculate but has good optimization effects, reducing the robot's motion amplitude and greatly improving the robot's operating efficiency, achieving excellent results in experiments. Different optimization strategies can be proposed for different optimization objectives, making the robot's motion control more flexible and enabling the robot to adapt to more complex application environments.

[0081] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A reverse inverse kinematics optimization algorithm for a seven-DOF loading and unloading robot, wherein the robot comprises seven joints and has seven degrees of freedom, and the seven joints are sequentially connected, characterized in that... Includes the following steps: The pose matrix of the robot's end effector and the initial joint angles of each joint of the robot are set in the initial state. ; Set the pose matrix of the robot's end effector in the target state; Select one joint of the robot as a redundant joint, and calculate the target joint angles of each joint of the robot in the target state based on the joint angles of the redundant joint in the initial state and the pose matrix of the robot's end effector in the target state. ; Calculate the angular displacement difference between the joint angles of each joint of the robot in the target state and the joint angles of each joint of the robot in the initial state; Maintain the pose matrix of the robot's end effector in the target state, and increase or decrease the joint angle of the redundant joint by a first adjustment value; The first adjustment value is adjusted based on the redundancy angle variance algorithm until the angular displacement difference of the joint angles of each joint of the robot reaches the minimum, and the first adjustment value at this time is taken as the optimal adjustment value.

2. The inverse kinematics optimization algorithm for a seven-degree-of-freedom loading and unloading robot according to claim 1, characterized in that, The seven sequentially connected joints are the first joint, the second joint, the third joint, the fourth joint, the fifth joint, the sixth joint, and the seventh joint, wherein the seventh joint is the end joint of the robot, and the redundant joint is the seventh joint.

3. The inverse kinematics optimization algorithm for a seven-degree-of-freedom loading and unloading robot according to claim 2, characterized in that, The joint angle of the first joint The expression is: (1) in, This represents the link offset of the fourth joint of the robot. Let be the link offset of the sixth joint of the robot, where , , 'a' is the approach axis of the robot's end effector. x a y a z These are the components of the approach axis along the x-axis, y-axis, and z-axis in the Cartesian coordinate system, respectively, and o is the direction axis of the robot's end effector. x o y o z These are the components of the direction axis along the x-axis, y-axis, and z-axis in the Cartesian coordinate system, respectively, where n is the vertical axis of the robot's end effector. x n y n z These are the components of the vertical axis along the x, y, and z axes in the Cartesian coordinate system, respectively. n, o, and a represent the pose of the robot's end effector joint 7 coordinate system mapped to the joint 6 coordinate system relative to the base coordinate system. The coordinate system of the seventh joint is mapped to the position of the coordinate system of the sixth joint relative to the base coordinate system of the robot.

4. The inverse kinematics optimization algorithm for a seven-degree-of-freedom loading and unloading robot according to claim 3, characterized in that, The joint angle of the fifth joint The expression is: （2） in, , .

5. The inverse kinematics optimization algorithm for a seven-degree-of-freedom loading and unloading robot according to claim 4, characterized in that, Sixth joint angle The expression is as follows: （3） in: .

6. The inverse kinematics optimization algorithm for a seven-degree-of-freedom loading and unloading robot according to claim 5, characterized in that, Second joint angle Third joint angle Fourth joint angle The expressions are as follows: （4） （5） （6） In the formula: , , , , This represents the link offset of the robot's first joint. This represents the link offset of robot joint 5. , This is the link length of the robot's fourth joint. , .

7. The inverse kinematics optimization algorithm for a seven-degree-of-freedom loading and unloading robot according to claim 2, characterized in that, The process of adjusting the first adjustment value based on the redundant angle variance algorithm until the angular displacement difference of the joint angles of the robot reaches its minimum, and taking the first adjustment value at this point as the optimal inverse solution, includes: S1: Based on the redundant angle variance algorithm, obtain the linear relationship between the angular displacement of the seventh joint and the angular displacement of the remaining 6 robot joints, and express the linear relationship in the form of a linear absolute value function piecewise line in a two-dimensional coordinate system; S2: Find the linear absolute value function polyline with the largest y-coordinate of its intersection point with the Y-axis of the two-dimensional coordinate system, and use the found linear absolute value function polyline as the reference polyline; S3: Along the descent direction of the y-coordinate of the reference broken line, identify the first linear absolute value function broken line that intersects the reference broken line, and take the identified linear absolute value function broken line as the target broken line; S4: Starting from the intersection of the baseline polyline and the target polyline, along the descending direction of the y-coordinate of the baseline polyline, determine whether the y-coordinate of the target polyline increases. If the y-coordinate increases, take the current x-coordinate as the optimal adjustment value. If the y-coordinate decreases, take the target polyline as the baseline polyline and repeat step S3.

8. The inverse kinematics optimization algorithm for a seven-degree-of-freedom loading and unloading robot according to claim 7, characterized in that, Step S1 includes: The adjusted joint angles of each joint of the robot are determined after the joint angles of the redundant joints are increased or decreased by the first adjustment value. ; Calculate and adjust joint angles Relative to target joint angle angular displacement , ,in, , These are the joint angles of the robot's first joint, second joint, third joint, fourth joint, fifth joint, sixth joint, and seventh joint, respectively, under the target state. , The joint angles of the robot's first joint, second joint, third joint, fourth joint, fifth joint, sixth joint, and seventh joint are respectively adjusted by increasing or decreasing the joint angles of the redundant joints by a first adjustment value. ; , , , , , , Joint angles Relative to joint angle The angular displacements of the first joint, the second joint, the third joint, the fourth joint, the fifth joint, the sixth joint, and the seventh joint. Confirm angular displacement Angular displacement relative to the seventh joint The expression: （7） in, .

Images