Design method for mechanical arm of household auxiliary robot

Abstract

The invention discloses a design method of a household auxiliary robot mechanical arm, which comprises the following steps: step 1, establishing a mathematical model: step 2, establishing a connecting rod coordinate system of a three-degree-of-freedom mechanical arm by adopting a standard DH parameter method, solving a homogeneous coordinate transformation matrix of a tail end relative to a base coordinate system through coordinate transformation, and finally solving a speed Jacobian matrix of the mechanical arm; and 3, simulating the process that the three-degree-of-freedom mechanical arm grabs the medicines in the storage grids by using MATLAB, and drawing a kinematics parameter curve of linear trajectory planning and joint space planning. According to the method, a connecting rod coordinate system of the three-degree-of-freedom mechanical arm is established by adopting a standard DH parameter method, a homogeneous coordinate transformation matrix of a tail end relative to a base coordinate system is solved through coordinate transformation, a speed Jacobian matrix of the mechanical arm is finally solved, and kinematics analysis of the three-degree-of-freedom mechanical arm is completed. And finally, the robot automatically delivers the medicine to the old.

Description

Technical Field

[0001] The present invention relates to the field of robots, and in particular to a design method for a mechanical arm of a household auxiliary robot. Background Art

[0002] The household care robots that are widely circulated in the current market can be roughly divided into two categories: mobile nursing robots and intelligent care robots.

[0003] Robear robot developed by Toshiharu Mukai is a representative of transfer nursing robots. It is a humanoid robot with a maximum load of 80kg at the end of the four-degree-of-freedom mechanical arm. There are dense capacitive touch sensors distributed along the mechanical arm, which can timely transmit the pressure information felt at the end to the brake to make the contact between the robot and the human body more gentle. At present, the main purpose of this robot is to help the elderly with limited mobility to transfer. However, the disadvantage of this type of robot is also obvious. According to the current literature, the actions and work of Robear robot must be completed in cooperation with the caregiver.

[0004] As a representative of intelligent accompanying robots, the "CZ-SRobot1s" developed by Chuangze in China integrates robot vision system, deep voice interaction, SLAM walking technology, face recognition, intelligent dialogue communication and remote video monitoring, etc. It is a very mature and complete system in the field of human-computer interaction. However, this type of robot focuses too much on interaction with users, which makes it impossible to carry related hardware for accompanying. So it is better to say that it is a companion robot than a companion robot.

[0005] The above two robots have their own shortcomings in caring for the elderly: the Robear robot cannot complete the task of delivering medicine alone; the CZ-SRobot1s robot cannot perform the task of accompanying the elderly in delivering medicine. Summary of the invention

[0006] In order to overcome the deficiencies in the prior art, the present invention proposes a design method for a household assistant robot mechanical arm, which is mainly intended to solve the technical problem of how to independently complete the task of delivering medicine to the elderly.

[0007] In order to achieve the above object, the present invention adopts the following scheme: The design method of the household assistive robot arm includes the following steps: Step 1: Establish a mathematical model: The structure of the household assistant robot includes a mechanical arm, a two-stage scissor lift platform, a cleaning mechanism and four storage compartments; wherein the mechanical arm includes three servos, three-section connecting rods and an end-effector gripper assembly; therefore, the mechanical arm is simplified into a three-degree-of-freedom connecting rod mechanism, and a mathematical model is established; Step 2: Perform kinematic analysis on the three-degree-of-freedom robotic arm: The standard DH parameter method is used to establish the link coordinate system of the three-degree-of-freedom manipulator, and the homogeneous coordinate transformation matrix of the end relative to the base coordinate system is solved through coordinate transformation, and finally the velocity Jacobian matrix of the manipulator is obtained; Step 3: Simulation verification: MATLAB will be used to simulate the process of a three-freedom robot arm grabbing medicines in a storage compartment, and to draw the kinematic parameter curves of linear trajectory planning and joint space planning.

[0008] Further optimization, in the step 1, the mechanical arm includes a first steering gear; the housing of the first steering gear is fixed on the bottom plate, the output shaft of the first steering gear is arranged in the vertical direction, and a first connecting rod is fixed on the output shaft of the first steering gear; a second steering gear is arranged on the first connecting rod, the housing of the second steering gear is fixed on the first connecting rod, and the output shaft of the second steering gear is arranged in the horizontal direction; a second connecting rod is fixed on the output shaft of the second steering gear, a third steering gear is arranged on the second connecting rod, the output shaft of the third steering gear is arranged in the horizontal direction, the output shaft of the third steering gear is fixedly connected to the second connecting rod, and the housing of the third steering gear is not connected to the mounting frame; The gripper assembly includes a stand; the stand is fixed on the outer shell of the third servo; a casing is fixed on the stand, a clamping servo is fixed on the casing, and the output shaft of the clamping servo is along the up and down direction; two clamping rods arranged in parallel are fixed on the casing, and one end of the clamping rod away from the casing is rotatably connected to a clamping claw through a hinge shaft; each clamping claw is fixed with a gear, and the two gears are meshed with each other.

[0009] Further optimization, in step 2, the rotation angle of the first steering gear is set to , the rotation angle of the second servo is set to , the rotation angle of the third servo is set to , then the DH parameter table of the three-DOF manipulator is shown in Table 1; Table 1: DH parameter table of three-DOF robotic arm

[0010] The homogeneous coordinate transformation matrix of the end of the robot arm relative to the base coordinate system can be obtained by the following matrix transformation: (1) Representing coordinate system With coordinate system The homogeneous transformation matrix between ; According to the DH connecting rod parameters given in Table 1, the transformation matrices between the four coordinate systems can be obtained by using the calculation sequence of the standard DH parameter method: for (2) (3) (4) Substituting formula (2)-(4) into formula (1), we can get the homogeneous transformation matrix from the base coordinate system to the end point: for (5) The three displacement components of the center point of the robotic arm wrist are the homogeneous transformation matrix The fourth column of (6) The symbol Respectively x,y,z The displacement component in the direction has completed the forward kinematic analysis of the robot arm.

[0011] Further optimization, in step 2, the ultimate goal of kinematic analysis is to perform trajectory planning for the three-freedom robot arm to achieve accurate medicine collection. Trajectory planning not only involves planning the posture of the path points, but also planning the speed and acceleration of the path points. Therefore, it is necessary to solve the Jacobian matrix. The process is as follows: (7) Substituting formula (6) into formula (7) and solving the total differential, we get (8) In the formula, ; The velocity Jacobian matrix reflects the mapping of small transformations in the joint space in the base coordinate system. The velocity Jacobian matrix is ​​used to solve the velocity and acceleration of the end of the robot, and it can also plan the velocity and acceleration of the joint space in reverse. Therefore, solving the velocity Jacobian matrix is ​​the core of the kinematic analysis of the robot.

[0012] The velocity Jacobian matrix is ​​defined as: (9) Rewriting formula (8) into a matrix form that satisfies the relationship in formula (9) yields the velocity Jacobian matrix of the three-degree-of-freedom manipulator: (10) In order to achieve drug grabbing, the end trajectory needs to be planned. The robotic arm designed and studied in this application has no obstructions within the working range, and the initial position of the end of the robotic arm is z The directional height is much higher than the lifting height of the scissor mechanism, so only the linear motion trajectory needs to be designed; The starting point of the end motion of the robot arm is , the end point is The starting point and the end point are both the positions of the end relative to the base system. If the number of trajectory interpolation steps is N, then the coordinates of each interpolation point are It can be expressed as: (11) At the same time, the pose of each interpolation point is determined to list the transformation matrix of the end point relative to the base coordinate system.

[0013] Further optimization, considering that the robot arm cannot fall over during the process of clamping and moving the medicine, the coordinate system of the end of the robot arm x The axis should be perpendicular to the x-axis of the base coordinate system, so the rotation matrix between the end coordinate system and the base coordinate system is T for: (12) in, q Indicates the rotation angle around the x-axis during the movement; The translation vector determined by formula (12) and the rotation matrix obtained by formula (11) are T The homogeneous transformation matrix of each interpolation point relative to the base coordinate system is: (13) Combining the results obtained by formula (11) with those obtained by formula (1), we can immediately solve the joint angle corresponding to each interpolation point: , writing the three angles into the servo driver and performing smooth interpolation can complete the motion control of the three-degree-of-freedom robotic arm.

[0014] Further optimization, in step 3, the center point of the wrist of the gripper assembly is used as the kinematic simulation point, and the medicine in the storage compartment on the left side of the direction facing the robotic arm is taken as an example: First, the linear trajectory of the terminal is planned; the output shaft of the first servo is set vertically and the output shaft of the second servo is set horizontally as the starting point, and the end point is set directly above the storage compartment and at the same height as the storage compartment. The homogeneous transformation matrix of the starting point and the end point is T 0 and T n Written as: (14) (15) Set the number of motion interpolation points to 20, and then use the ctraj function provided by MATLAB's Robotics Toolbox to obtain the homogeneous transformation matrix corresponding to the interpolation points, thereby obtaining the displacement, velocity, and acceleration components of the robot end relative to the base coordinate system x1y1z1. Then use the kinematic inverse function ikine to convert the homogeneous transformation matrix of the interpolation points solved by the ctraj function to the joint coordinate system to draw the motion trajectory of the corresponding end. Beneficial Effects

[0015] The present invention uses the standard DH parameter method to establish the connecting rod coordinate system of the three-degree-of-freedom manipulator and solves the homogeneous coordinate transformation matrix of the terminal relative to the base coordinate system through coordinate transformation, and finally obtains the velocity Jacobian matrix of the manipulator, completing the kinematic analysis of the three-degree-of-freedom manipulator. The main working process of the drug delivery robot studied by the present invention is to grab the medicine in the storage compartment through the designed three-freedom manipulator. MATLAB is used to simulate the process and draw the kinematic parameter curves of linear trajectory planning and joint space planning. Compared with the Robear robot and CZ-SRobot1s robot, this robot can automatically deliver medicine to the elderly without the need for a caregiver. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 This is a schematic diagram of the overall structure of the household assistant robot of the present invention; Figure 2 This is a schematic diagram of the structure of the mechanical arm of the present invention; Figure 3 It is a simplified schematic diagram of the connecting rod mechanism of the mechanical arm of the present invention; Figure 4 is the relationship curve between displacement and time; Figure 5 is the relationship curve between speed and time; Figure 6 is the relationship curve between acceleration component and time; Figure 7 is the linear motion trajectory of the end of the robotic arm; Figure 8 is the relationship curve between displacement and time after interpolation; Fig. 9 is the relationship curve between the interpolated speed and time; Fig.10 is the relationship curve between the acceleration component and time after interpolation; Fig.11 is the curved motion trajectory of the end of the robotic arm after interpolation. DETAILED DESCRIPTION

[0017] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the accompanying drawings. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0018] The design method of the household assistive robot arm includes the following steps: Step 1: Establish a mathematical model: like Figure 1 As shown in the figure, the structure of the household assistant robot includes a mechanical arm 1, a two-stage scissor lift platform 2, a cleaning mechanism 3 and four storage compartments 4; wherein the mechanical arm includes three steering gears, three-section connecting rods and an end-actuating gripper assembly; therefore, the mechanical arm is simplified into a three-degree-of-freedom connecting rod mechanism, and a mathematical model is established. Step 2: Perform kinematic analysis on the three-degree-of-freedom robotic arm: The standard DH parameter method is used to establish the link coordinate system of the three-degree-of-freedom manipulator, and the homogeneous coordinate transformation matrix of the end relative to the base coordinate system is solved through coordinate transformation, and finally the velocity Jacobian matrix of the manipulator is obtained; Step 3: Simulation verification: MATLAB will be used to simulate the process of a three-freedom robot arm grabbing medicines in a storage compartment, and to draw the kinematic parameter curves of linear trajectory planning and joint space planning.

[0019] In this embodiment, in step 1, the robotic arm includes a first steering gear 25; the housing of the first steering gear is fixed on the base plate 11, the output shaft of the first steering gear is arranged in the vertical direction, and a first connecting rod 26 is fixed on the output shaft of the first steering gear; a second steering gear 27 is arranged on the first connecting rod, the housing of the second steering gear is fixed on the first connecting rod, and the output shaft of the second steering gear 27 is arranged in the horizontal direction; a second connecting rod 28 is fixed on the output shaft of the second steering gear, and a third steering gear 29 is arranged on the second connecting rod, the output shaft of the third steering gear is arranged in the horizontal direction, and the output shaft of the third steering gear is fixedly connected to the second connecting rod.

[0020] The gripper assembly includes a stand; the stand 30 is fixed on the outer shell of the third steering gear; a housing is fixed on the stand, a clamping steering gear 32 is fixed on the housing, and the output shaft of the clamping steering gear is in the up-down direction; two clamping rods arranged in parallel are fixed on the housing, and one end of the clamping rod away from the housing is rotatably connected to a clamping claw 33 through a hinge shaft; each clamping claw is fixed with a gear 37, and the two gears are meshed with each other; A first driving rod is fixed on the output shaft of the clamping servo; one end of the first driving rod is rotatably connected to the second driving rod via shaft eight; the end of the second driving rod away from the first driving rod is rotatably connected to one of the clamping claws via shaft nine; the hinge shaft, shaft eight and shaft nine are all arranged in the same direction as the output shaft of the clamping servo.

[0021] In this embodiment, in step 2, the rotation angle of the first steering gear is set to , the rotation angle of the second servo is set to , the rotation angle of the third servo is set to . Figure 3 The mechanism model of the three-degree-of-freedom robot is shown, and a three-dimensional rectangular coordinate system is established. The lengths of the three connecting rods from the base to the end are 37.22mm, 90.00mm, and 96.54mm respectively. The DH parameter table of the three-degree-of-freedom robot is shown in Table 1; Table 1: DH parameter table of three-DOF robotic arm

[0022] The homogeneous coordinate transformation matrix of the end of the robot arm relative to the base coordinate system can be obtained by the following matrix transformation: (1) Representing coordinate system With coordinate system The homogeneous transformation matrix between ; According to the DH connecting rod parameters given in Table 1, the transformation matrices between the four coordinate systems can be obtained by using the calculation sequence of the standard DH parameter method: for: (2) (3) (4) Substituting formula (2)-(4) into formula (1), we can get the homogeneous transformation matrix from the base coordinate system to the end point: for (5) The three displacement components of the center point of the robotic arm wrist are the homogeneous transformation matrix The fourth column of (6) The symbol Respectively x,y,z The displacement component in the direction has completed the forward kinematic analysis of the robot arm.

[0023] In this embodiment, in step 2, the ultimate goal of kinematic analysis is to perform trajectory planning on the three-freedom manipulator to achieve accurate medicine collection. Trajectory planning not only involves planning the posture of the path points, but also planning the speed and acceleration of the path points. Therefore, it is necessary to solve the Jacobian matrix. The process is as follows: (7) Substituting formula (6) into formula (7) and solving the total differential, we get (8) In the formula, ; The velocity Jacobian matrix reflects the mapping of small transformations in the joint space in the base coordinate system. The velocity Jacobian matrix is ​​used to solve the velocity and acceleration of the end of the robot, and it can also plan the velocity and acceleration of the joint space in reverse. Therefore, solving the velocity Jacobian matrix is ​​the core of the kinematic analysis of the robot.

[0024] The velocity Jacobian matrix is ​​defined as: (9) Rewriting formula (8) into a matrix form that satisfies the relationship in formula (9) yields the velocity Jacobian matrix of the three-degree-of-freedom manipulator: (10) In order to achieve drug grabbing, the end trajectory needs to be planned. The robotic arm designed and studied in this application has no obstructions within the working range, and the initial position of the end of the robotic arm is z The directional height is much higher than the lifting height of the scissor mechanism, so only the linear motion trajectory needs to be designed; The starting point of the end motion of the robot arm is , the end point is The starting point and the end point are both the positions of the end relative to the base system. If the number of trajectory interpolation steps is N, then the coordinates of each interpolation point are It can be expressed as: (11) At the same time, the pose of each interpolation point is determined to list the transformation matrix of the end point relative to the base coordinate system.

[0025] In this embodiment, considering that the robot arm cannot fall over during the process of clamping and moving the medicine, the coordinate system of the end of the robot arm is x The axis should be perpendicular to the x-axis of the base coordinate system, so the rotation matrix between the end coordinate system and the base coordinate system is T for: (12) in, q Indicates the rotation angle around the x-axis during the movement; The translation vector determined by formula (12) and the rotation matrix obtained by formula (11) are T The homogeneous transformation matrix of each interpolation point relative to the base coordinate system is: (13) Combining the results obtained by formula (11) with those obtained by formula (1), we can immediately solve the joint angle corresponding to each interpolation point: , writing the three angles into the servo driver and performing smooth interpolation can complete the motion control of the three-degree-of-freedom robotic arm.

[0026] In this embodiment, the center point of the wrist of the gripper assembly is used as the kinematic simulation point, and the medicine in the storage compartment on the left side of the direction facing the robotic arm is taken as an example: First, the linear trajectory of the terminal is planned; the output shaft of the first servo is set vertically and the output shaft of the second servo is set horizontally as the starting point, and the end point is set directly above the storage compartment and at the same height as the storage compartment. The homogeneous transformation matrix of the starting point and the end point is T 0 and T n Written as: (14) (15) Set the number of motion interpolation points to 20, and then use the function ctraj provided by MATLAB's Robotics Toolbox to obtain the homogeneous transformation matrix corresponding to the interpolation points, thereby obtaining the displacement, velocity, and acceleration components of the end of the robot relative to the base coordinate system x1y1z1. The relationship curve between them and time is shown in the following figure: Figure 4 , Figure 5 , Figure 6 As shown in the figure, the kinematic inverse function ikine is then used to transform the homogeneous transformation matrix of the interpolation points solved by the ctraj function into the joint coordinate system to draw the motion trajectory of the corresponding robot end. The drawn linear motion trajectory is shown in Figure 7 shown.

[0027] Above Figure 4-7 If the three-DOF robot arm requires smooth motion of each joint, it is necessary to perform a fifth-order polynomial interpolation on the path points in the joint space. The joint angles corresponding to the starting point and the end point are They are , set the number of interpolation points to 20. Then use the jtraj function to perform quintic polynomial interpolation on the joint motion and use the fkine function to complete the kinematic solution of the interpolation result. The generated displacement, velocity, and acceleration component curves of the end of the robot are as follows: Figure 8 , Fig. 9 , Fig.10 As shown, the motion trajectory of the end curve of the robot arm is drawn by jtraj solution. Fig.11 shown.

[0028] Based on the above ideal embodiments of the present invention, the relevant staff can make various changes and modifications without departing from the technical concept of the present invention through the above description. The technical scope of the present invention is not limited to the contents of the specification, and its technical scope must be determined according to the scope of the claims.

Claims

1. A method for designing a mechanical arm of a household assistive robot, characterized in that: The steps include: Step 1: Establish a mathematical model: The structure of the household assistant robot includes a mechanical arm, a two-stage scissor lift platform, a cleaning mechanism and four storage compartments; wherein the mechanical arm includes three servos, three-section connecting rods and an end-effector gripper assembly; therefore, the mechanical arm is simplified into a three-degree-of-freedom connecting rod mechanism, and a mathematical model is established; Step 2: Perform kinematic analysis on the three-degree-of-freedom robotic arm: The standard DH parameter method is used to establish the link coordinate system of the three-degree-of-freedom manipulator, and the homogeneous coordinate transformation matrix of the end relative to the base coordinate system is solved through coordinate transformation, and finally the velocity Jacobian matrix of the manipulator is obtained; Step 3: Simulation verification: MATLAB will be used to simulate the process of a three-freedom robot arm grabbing medicines in a storage compartment, and to draw the kinematic parameter curves of linear trajectory planning and joint space planning.

2. The design method of the household assistant robot mechanical arm according to claim 1, characterized in that: In the step 1, the mechanical arm includes a first steering gear; the housing of the first steering gear is fixed on the bottom plate, the output shaft of the first steering gear is arranged in the vertical direction, and a first connecting rod is fixed on the output shaft of the first steering gear; a second steering gear is arranged on the first connecting rod, the housing of the second steering gear is fixed on the first connecting rod, and the output shaft of the second steering gear is arranged in the horizontal direction; a second connecting rod is fixed on the output shaft of the second steering gear, a third steering gear is arranged on the second connecting rod, the output shaft of the third steering gear is arranged in the horizontal direction, the output shaft of the third steering gear is fixedly connected to the second connecting rod, and the housing of the third steering gear is not connected to the mounting frame; The gripper assembly includes a stand; the stand is fixed on the outer shell of the third servo; a casing is fixed on the stand, a clamping servo is fixed on the casing, and the output shaft of the clamping servo is along the up and down direction; two clamping rods arranged in parallel are fixed on the casing, and one end of the clamping rod away from the casing is rotatably connected to a clamping claw through a hinge shaft; each clamping claw is fixed with a gear, and the two gears are meshed with each other.

3. The design method of the household assistant robot mechanical arm according to claim 2, characterized in that: In step 2, the rotation angle of the first steering gear is set to , the rotation angle of the second servo is set to , the rotation angle of the third servo is set to , then the DH parameter table of the three-DOF manipulator is shown in Table 1; Table 1: DH parameter table of three-DOF robotic arm ； The homogeneous coordinate transformation matrix of the end of the robot arm relative to the base coordinate system can be obtained by the following matrix transformation: (1) Representing coordinate system With coordinate system The homogeneous transformation matrix between ; According to the DH connecting rod parameters given in Table 1, the transformation matrices between the four coordinate systems can be obtained by using the calculation sequence of the standard DH parameter method: for: (2) (3) (4) Substituting formula (2)-(4) into formula (1), we can get the homogeneous transformation matrix from the base coordinate system to the end point: for: (5) The three displacement components of the center point of the robotic arm wrist are the homogeneous transformation matrix The fourth column of (6) The symbol Respectively x,y,z The displacement component in the direction has completed the forward kinematic analysis of the robot arm.

4. The design method of the household assistant robot mechanical arm according to claim 3, characterized in that: In step 2, the ultimate goal of kinematic analysis is to perform trajectory planning on the three-freedom robot arm to achieve accurate medicine collection. Trajectory planning not only involves planning the posture of the path points, but also planning the speed and acceleration of the path points. Therefore, the Jacobian matrix needs to be solved. The process is as follows: (7) Substituting formula (6) into formula (7) and solving the total differential, we get (8) Where: The velocity Jacobian matrix reflects the mapping of small transformations in the joint space in the base coordinate system. The velocity Jacobian matrix is ​​used to solve the velocity and acceleration of the end of the manipulator, and it can also plan the velocity and acceleration of the joint space in reverse. The velocity Jacobian matrix is ​​defined as: (9) Rewriting formula (8) into a matrix form that satisfies the relationship in formula (9) yields the velocity Jacobian matrix of the three-degree-of-freedom manipulator: (10) In order to achieve drug grabbing, the end trajectory needs to be planned. The robotic arm designed and studied in this application has no obstructions within the working range, and the initial position of the end of the robotic arm is z The directional height is much higher than the lifting height of the scissor mechanism, so only the linear motion trajectory needs to be designed; The starting point of the end motion of the robot arm is, the ending point is, and the starting point is and end point are the positions of the end relative to the base system. If the number of trajectory interpolation steps is N, then the coordinates of each interpolation point are It can be expressed as: (11) At the same time, the pose of each interpolation point is determined to list the transformation matrix of the end point relative to the base coordinate system.

5. The design method of the household assistant robot mechanical arm according to claim 4, characterized in that: Considering that the robot arm cannot fall over when it is moving the medicine, the coordinate system of the end of the robot arm is x The axis should be perpendicular to the x-axis of the base coordinate system, so the rotation matrix between the end coordinate system and the base coordinate system is T for: (12) in, q Indicates the rotation angle around the x-axis during the movement; The translation vector determined by formula (12) and the rotation matrix obtained by formula (11) are T The homogeneous transformation matrix of each interpolation point relative to the base coordinate system is: (13) By combining the results of formula (11) with those of formula (1), the joint angle corresponding to each interpolation point can be solved. The motion control of the three-degree-of-freedom robotic arm can be completed by writing the three angles into the servo driver and performing smooth interpolation.

6. The design method of the household assistant robot mechanical arm according to claim 5, characterized in that: In step 3, the center point of the wrist of the gripper assembly is used as the kinematic simulation point, and the medicine in the storage compartment on the left side of the direction facing the robotic arm is taken as an example: First, the linear trajectory of the terminal is planned; the output shaft of the first servo is set vertically and the output shaft of the second servo is set horizontally as the starting point, and the end point is set directly above the storage compartment and the height is flush with the storage compartment. The homogeneous transformation matrix of the starting point and the end point is T 0 and T n Written as: (14) (15) Set the number of motion interpolation points to 20, and then use the ctraj function provided by MATLAB's Robotics Toolbox to obtain the homogeneous transformation matrix corresponding to the interpolation points, thereby obtaining the displacement, velocity, and acceleration components of the robot end relative to the base coordinate system x1y1z1. Then use the kinematic inverse function ikine to convert the homogeneous transformation matrix of the interpolation points solved by the ctraj function to the joint coordinate system to draw the motion trajectory of the corresponding end.

Images