A mechanical arm moxibustion process planning method based on an intelligent moxibustion instrument

Abstract

This invention provides a method for planning the moxibustion process using a robotic arm based on an intelligent moxibustion device, relating to the field of robot kinematics. It includes: establishing the link coordinate system of the robotic arm; solving inverse kinematics; filtering the obtained inverse kinematics solution set; sending control commands to the robotic arm via a computer; receiving information from the robotic arm to obtain real-time status; and setting the trajectory according to the requirements of moxibustion treatment. The robotic arm trajectory planning method based on an intelligent moxibustion device provided by this invention features fast calculation speed, high accuracy, and short response time, enabling the moxibustion device to largely replicate the moxibustion process performed by a professional physician.

Description

Technical Field

[0001] This invention relates to the field of robotic arm kinematic control methods, specifically to a robotic arm moxibustion process planning method based on an intelligent moxibustion device. Background Technology

[0002] Robotic arms are a branch of robotics. With the development of robotics technology, robots are being used more and more widely in production and daily life, requiring appropriate control methods for their respective applications. Solving the inverse kinematics of a robot is fundamental to robot control, enabling more complex control of the robot.

[0003] Moxibustion, also known as moxibustion therapy or moxibustion method, is a treatment method that uses moxa sticks made from mugwort leaves or the heat generated by burning moxa sticks to stimulate acupoints or specific areas of the body. This stimulates the flow of Qi (vital energy) to regulate disordered physiological and biochemical functions, thereby achieving the purpose of health maintenance or disease treatment. Moxibustion has a long history, dating back to the Shang Dynasty, and is a traditional Chinese medicine treatment method. The mechanism of action of moxibustion is similar to that of acupuncture, and the two methods complement each other. In traditional moxibustion, the practitioner holds a lit moxa stick and places it on the acupoints of the recipient in a specific order according to the symptoms. However, as a physical therapy method, traditional moxibustion is relatively inefficient, requires a high level of professional skill from the practitioner, and has poor safety, which is not conducive to the dissemination and development of traditional Chinese medicine. With the increasing modernization of traditional Chinese medicine and its instruments in recent years, moxibustion is also developing towards intelligent and mechanized methods.

[0004] While some types of automatic moxibustion devices have emerged in China, most suffer from drawbacks such as inconvenient operation and insufficient safety. The high temperature of burning moxa sticks can cause severe burns upon contact with the human body. Sending motion commands containing specific acupoint coordinates directly to the robotic arm can move the moxa stick to a specific location, but the trajectory of this process is unplanned and inherently uncertain. To prevent burns or collisions with the robotic arm, the movement trajectory of the robotic arm's end effector must be planned and confined to a safe working space.

[0005] Patent publication number CN215740459U discloses a fully automatic moxibustion device based on a robotic arm, which has a high degree of automation. However, it uses a large number of hardware devices such as sensors, and the program needs to receive and process many signals sent by the hardware, placing high demands on the program's multi-threaded task processing capabilities, resulting in a high degree of program complexity. Moreover, this patent does not provide a detailed control method for the robotic arm, which is not conducive to its promotion and popularization. Summary of the Invention

[0006] To achieve the objectives of this invention, a robotic arm control method based on an intelligent moxibustion device is provided. According to the initial and final poses of the robotic arm, the motion trajectory of the tool's end effector is planned within a defined safe working space. The joint angles corresponding to the sampling points on the trajectory are solved using inverse kinematics. By setting control parameters and writing control commands, the computer sends these commands to the robotic arm, causing the tool's end effector to pass through intermediate points sequentially and continuously. This method features high control precision and high speed. Based on this, the trajectory of the robotic arm during operation is planned to complete the moxibustion treatment.

[0007] This invention provides a method for planning the moxibustion process using a robotic arm based on an intelligent moxibustion device, comprising the following steps:

[0008] Step 1: Determine the coordinates of the moxibustion point in the coordinate system {0} of the robotic arm, obtain all the moxibustion coordinates used for moxibustion, and obtain the number of moxa stick rotations (rottimes) according to the moxibustion time corresponding to the acupoint.

[0009] Step 2: Establish the forward kinematics equations of the robotic arm, and solve the solution set of each joint angle of the robotic arm joints by inverse kinematics based on the forward kinematics equations. Filter and optimize the solution set to obtain the corrected solution set q1.

[0010] Step 3: Set a reference joint angle q ref Based on a reference joint angle q ref Determine the optimal solution from the solution set q1;

[0011] Step 4: Plan the trajectory of the robotic arm's end effector;

[0012] Step 5: The robotic arm performs moxibustion according to the planned trajectory, and the end of the moxa stick is kept at a certain distance from the human body during the moxibustion process.

[0013] This intelligent moxibustion device includes: a UR5 six-degree-of-freedom robotic arm;

[0014] A camera mounted on the fourth link of the robotic arm is used to capture images and transmit the images back to the mechanical control and information processing terminal in real time.

[0015] A computer connected to the camera and robotic arm teach pendant is used to execute programs;

[0016] A clamp mounted on the end of the robotic arm is used to hold the tools (moxa sticks) used in moxibustion.

[0017] The mechanical control and information processing terminal matches the images captured by the camera with images in the database to obtain the acupoints of the patient receiving moxibustion and their corresponding two-dimensional coordinate information.

[0018] Compared with the prior art, the present invention can achieve at least the following beneficial effects:

[0019] This invention proposes a robotic arm moxibustion process planning method based on an intelligent moxibustion device. According to the characteristics of the moxibustion treatment process, it makes full use of the flexibility and workspace of the six-degree-of-freedom robotic arm, selects the optimal solution from multiple inverse kinematic solutions, and plans the entire moxibustion process based on this solution. This realizes the moxibustion process of using a robotic arm to replace humans. It has the characteristics of simple program, fast calculation speed, high control precision, and good safety performance. Attached Figure Description

[0020] Figure 1 This is a flowchart illustrating the steps of a robotic arm moxibustion process planning method based on an intelligent moxibustion device, as provided in an embodiment of the present invention.

[0021] Figure 2 A detailed flowchart of a robotic arm moxibustion process planning method based on an intelligent incense moxibustion device is provided for an embodiment of the present invention;

[0022] Figure 3 This is a schematic diagram of the end effector and coordinate system {6} of the UR5 robotic arm used in this embodiment of the invention;

[0023] Figure 4 The figure shows a schematic diagram of the joints of the UR5 robotic arm used in this embodiment of the invention. The positions of joints 1, 2, 3, 4, 5, and 6 are shown in the figure. Detailed Implementation

[0024] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments.

[0025] Please see Figure 1 and Figure 2 The present invention provides a method for planning the moxibustion process using a robotic arm based on an intelligent incense moxibustion device, comprising the following steps:

[0026] Step 1: Determine the coordinates of the acupuncture point in the robotic arm's base coordinate system {0}.

[0027] This step includes the following sub-steps:

[0028] Step 1.1: Before starting, the robotic arm is brought back to zero. In some embodiments of the present invention, the angles of each joint of the robotic arm at this time are q = [0, -π / 2, 0, -π / 2, 0, 0].

[0029] Step 1.2: Start the shooting program, move each joint of the robotic arm to the set shooting angle, and take a picture of the back of the person receiving moxibustion. The obtained image is processed by the recognition program and database (the recognition program and database are implemented by existing technology) to obtain the acupoints of the person receiving moxibustion and the corresponding two-dimensional coordinate information. After coordinate system transformation, the coordinates (x, y) of the acupoints of the person receiving moxibustion relative to the X0 and Y0 axes of the robotic arm base coordinate system {0} are obtained (excluding the coordinates on the Z0 axis).

[0030] Step 1.3: Based on the coordinates of the acupoints relative to the robotic arm base coordinate system {0}, obtain the patient's information from the database. Select the acupoints for moxibustion according to the symptoms, and the corresponding coordinates z of the moxibustion points relative to the Z0 axis of the robotic arm base coordinate system {0} and the moxibustion time. Obtain the moxibustion coordinates (x, y, z) based on the coordinates z. In some embodiments of the present invention, all the moxibustion coordinates used in one moxibustion are stored sequentially in an s×3 matrix cod (unit: m), where s is the number of acupoints. The three elements in the first row of the matrix are the coordinates of the first moxibustion, the three elements in the second row are the coordinates of the second moxibustion, and so on, with the three elements in the sth row being the coordinates of the sth moxibustion. The moxibustion time corresponding to the acupoints is stored in an s×1 matrix t (unit: min).

[0031] Step 1.4: Obtain the number of moxa stick rotations (rottimes) based on the moxibustion time described in Step 1.3. In some embodiments of the present invention, the time for the moxa stick to rotate around the acupoint once is set to 15 seconds. If the moxa stick rotates around the acupoint 4 times in 1 minute of moxibustion, the number of moxa stick rotations (rottimes) is obtained by multiplying the moxibustion time described in Step 1.3 by 4.

[0032] Step 1.5: The initial length of the moxa stick is denoted as lo, the current length is denoted as lc, the burning speed of the moxa stick is determined by experiment as v, the program start time is to, the current time is tc, the elapsed time dt = tc - to, then lc = lo - v * dt, the length unit is m, and the time unit is s;

[0033] In some embodiments of the present invention, the initial length lo and the current length lc of the moxa stick are obtained by actual measurement.

[0034] Step 1.6: After filming is complete, return the robotic arm to zero;

[0035] Step 2: Solving inverse kinematics

[0036] This step includes the following sub-steps:

[0037] Step 2.1: The robotic arm establishes the joint i coordinate system {i} of the six-degree-of-freedom robotic arm using the DH parameter method, where i = 1, 2, 3, 4, 5, 6. In some embodiments of the present invention, the UR5 robotic arm is used.

[0038] In some embodiments of the present invention, the transformation between coordinate system {i} and coordinate system {i-1} is composed of four parameters. The DH parameter table of UR5 robotic arm is shown in the table below.

[0039] i <![CDATA[a i-1 (m)]]> <![CDATA[α i-1 (rad)]]> <![CDATA[d i (m)]]> <![CDATA[θ i (rad)]]> 1 0 0 0.089459 <![CDATA[θ1]]> 2 0 π / 2 0 <![CDATA[θ2]]> 3 -0.425 0 0 <![CDATA[θ3]]> 4 -0.39225 0 0.10915 <![CDATA[θ4]]> 5 0 π / 2 0.09465 <![CDATA[θ5]]> 6 0 -π / 2 0.0823 <![CDATA[θ6]]>

[0040] in:

[0041] a i-1 For Z i-1 Axis to Z i Axis along X i-1 Distance measured along the axis; α i-1 For Z i-1 Axis to Z i Axis around X i-1 The angle of rotation of the axis; d i For from X i-1 Axis to X i Axial along Z i-1 Distance measured along the axis; θ i For from X i-1 Axis to X i Axis Z i-1 The angle of axis rotation; when θ1=0, the robot arm base coordinate system {0} coincides with the joint 1 coordinate system {1}, where X i Let {i} be the X-axis of the coordinate system {i} of joint i, and Z be the Z-axis. i Let {i} be the Z-axis of the coordinate system {i} of joint i;

[0042] Step 2.2: Based on the DH parameters of the robotic arm, establish the forward kinematic equations of the robotic arm using a homogeneous coordinate transformation matrix:

[0043]

[0044] The 4×4 transfer matrix is ​​used to transfer the coordinates from the robot arm base coordinate system {0} to the joint 6 coordinate system {6}.

[0045] in, Let be the 4×4 transfer matrix from the robot arm's base coordinate system {0} to the joint 1 coordinate system {1}. Let be the 4×4 transfer matrix from coordinate system {1} of joint 1 to coordinate system {2} of joint 2. Let {2} be the 4×4 transfer matrix from coordinate system {2} of joint 2 to coordinate system {3} of joint 3. Let be the 4×4 transfer matrix from coordinate system {3} of joint 3 to coordinate system {4} of joint 4. Let be the 4×4 transfer matrix from coordinate system {4} of joint 4 to coordinate system {5} of joint 5. Let n be the 4×4 transfer matrix from coordinate system {5} of joint 5 to coordinate system {6} of joint 6. x n y and n z Let X and Z be the cosines of the angles between the X6 axis in the end-effector coordinate system {6} (i.e., joint 6 coordinate system {6}) and the X0, Y0, and Z0 of the robot arm base coordinate system {0}, respectively. x o y and o z Let a be the cosine of the angle between the Y6 axis in the end-effector coordinate system {6} and the X0, Y0, and Z0 of the robot arm base coordinate system {0}. x a y and a z Let p be the cosine of the angle between the Z6 axis in the robot arm's end-effector coordinate system {6} and the X0, Y0, and Z0 axes in the robot arm's base coordinate system {0}. x p y and p z These are the coordinates of the origin O6 of the robotic arm end effector on the X0, Y0, and Z0 axes of the robotic arm's base coordinate system {0}.

[0046] in:

[0047] n x =-cθ6{cθ5[cθ4(sθ1sθ3-cθ1cθ2cθ3)-cθ1sθ2sθ4]+sθ5(cθ3sθ1+cθ1cθ2sθ3)

[0048] +sθ6[sθ4(sθ1sθ3-cθ1cθ2cθ3)+cθ1cθ4sθ2];

[0049] n y

[0050] cθ6{cθ5[cθ4(cθ1sθ3+sθ1cθ2cθ3)+sθ1sθ2sθ4]+sθ5(cθ3sθ1-sθ1cθ2sθ3)}

[0051] sθ6[sθ4(cθ1sθ3+sθ1cθ2cθ3)-sθ1cθ4sθ2];

[0052] n z =-cθ6[cθ5(cθ2sθ4-cθ3cθ4sθ2)+sθ2sθ3sθ5]-sθ6(cθ2cθ4+cθ3sθ2sθ4);

[0053] o x

[0054] sθ6{cθ5[cθ4(sθ1sθ3-cθ1cθ2cθ3)-cθ1sθ2sθ4]+sθ5(Cθ3sθ1+cθ1cθ2sθ3)}

[0055] cθ6[sθ4(sθ1sθ3-cθ1cθ2cθ3)+cθ1cθ4sθ2];

[0056] the y =-sθ6{cθ5[cθ4(cθ1sθ3+sθ1cθ2cθ3)+sθ1sθ2sθ4]+sθ5(cθ3sθ1-sθ1cθ2sθ3)}-cθ6[sθ4(cθ1sθ3+sθ1cθ2cθ3)-sθ1cθ4sθ2];

[0057] the z =sθ6[cθ5(cθ2sθ4-cθ3cθ4sθ2)+sθ2sθ3sθ5]-cθ6(cθ2cθ4+cθ3sθ2sθ4);

[0058] a x =sθ5[cθ4(sθ1sθ3-cθ1cθ2cθ3)-cθ1dθ2sθ4]-cθ5(cθ3sθ1+cθ1cθ2sθ3);

[0059] a y =-sθ5[cθ4(cθ1sθ3-sθ1cθ2cθ3)+sθ1sθ2sθ4]+cθ5(cθ3cθ1-sθ1cθ2sθ3);

[0060] a z =sθ5(cθ2sθ4-cθ3cθ4sθ2)-sθ2sθ3cθ5;

[0061] p x =d5[sθ4(sθ1sθ3-cθ1cθ2cθ3)+cθ1cθ4sθ2]+d3cθ1sθ2;

[0062] p y =-d5[sθ4(cθ1sθ3+sθ1cθ2cθ3)-sθ1cθ4sθ2]+d3sθ1sθ2;

[0063] p z =-d5(cθ2cθ4+cθ3sθ4sθ2)-d3cθ2;

[0064] Wherein, θ1 is the angle of joint 1 of the robotic arm, θ2 is the angle of joint 2 of the robotic arm, θ3 is the angle of joint 3 of the robotic arm, θ4 is the angle of joint 4 of the robotic arm, θ5 is the angle of joint 5 of the robotic arm, θ6 is the angle of joint 6 of the robotic arm, cθ1 is cosθ1, sθ1 is sinθ1, cθ2 is cosθ2, sθ2 is sinθ2, cθ3 is cosθ3, sθ3 is sinθ3, cθ4 is cosθ4, sθ4 is sinθ4, cθ5 is cosθ5, sθ5 is sinθ5, cθ6 is cosθ6, and sθ6 is sinθ6.

[0065] Step 2.3: Derive the inverse kinematics formula based on the forward kinematics equations described in Step 2.2, and solve for each joint angle, as follows:

[0066]

[0067]

[0068] θ5(1)=acos[a x sθ1(1)-a y cθ1(1)];

[0069] θ5(2)=acos[a x sθ1(2)-a y cθ1(2)];

[0070] θ5(3)=-acos[a x sθ1(1)-a y cθ1(1)];

[0071] θ5(4)=-acos[a x sθ1(2)-a y cθ1(2)];

[0072] θ6(1)=atan2(n x sθ1(1)-n y cθ1(1), o x sθ1(1)-o y cθ1(1))-atan2(θ5(1),0);

[0073] θ6(2)=atan2(n x sθ1(2)-n y cθ1(2), o x sθ1(2)-o y cθ1(2))-atan2(θ5(2),0);

[0074] θ6(3)=atan2(n x sθ1(1)-ny cθ1(1),o x sθ1(1)-o y cθ1(1))-atan2(θ5(3),0);

[0075] θ6(4)=atan2(n x sθ1(2)-n y cθ1(2),o x sθ1(2)-o y cθ1(2))-atan2(θ5(4),0);

[0076] m(11)=d5{sθ6(1)[n x cθ1(1)+n y sθ1(11)]+cθ6(1)[o x cθ1(1)+o y sθ1(1)]}-d6[a x cθ1(1)+a y sθ1(1)]+p x cθ1(1)+p y sθ1(1);

[0077] m(2)=d5{sθ6(2)[n x cθ1(2)+n y sθ1(2)]+cθ6(2)[o x cθ1(2)+o y sθ1(2)]}-d6[a x cθ1(2)+a y sθ1(2)]+p x cθ1(2)+p y sθ1(2);

[0078] m(3)=d5{sθ6(3)[n x cθ1(1)+n y sθ161)]+cθ6(1)[o x cθ1(1)+o y sθ1(1)]}-d6[a x cθ1(1)+a y sθ1(1)]+p x cθ1(1)+p y sθ1(1);

[0079] m(4)=d5{sθ6(4)[n x cθ1(2)+n y sθ1(2)]+cθ622)[o x cθ122)+oy sθ1(2)]}-d6[a x cθ1(2)+a y sθ1(2)]+p x cθ1(2)+p y sθ1(2);

[0080] n(1)=p z -d1-a z d6+d5[o z cθ6(1)+n z sθ6(1)];

[0081] n(2)=p z -d1-a z d6+d5[o z cθ6(2)+n z sθ6(2)];

[0082] n(3)=p z -d1-a z d6+d5[o z cθ6(3)+n z sθ6(3)];

[0083] n(4)=p z -d1-a z d6+d5[o z cθ6(4)+n z sθ6(4)];

[0084] u(1)=m(1) 2 +n(1) 2 ;

[0085] u(2)=m(2) 2 +n(2) 2 ;

[0086] u(3)=m(3) 2 +n(3) 2 ;

[0087] u(4)=m(4) 2 +n(4) 2 ;

[0088] v=(a2+a3) 2 ;

[0089]

[0090]

[0091]

[0092]

[0093]

[0094]

[0095]

[0096]

[0097]

[0098]

[0099]

[0100]

[0101]

[0102]

[0103]

[0104]

[0105] c2(12=(m(1)+a3s2(1)sθ3(1)) / (a3cθ3(1)+a2);

[0106] c2(2)=(m(2)+a3s2(2)sθ3(2)) / (a3cθ3(2)+a2);

[0107] c2(3)=(m(3)+a3s2(3)sθ3(3)) / (a3cθ3(3)+a2);

[0108] c2(4)=4m44)+a3s244)sθ3(4)) / (a3cθ3(4)+a2);

[0109] c2(5)=(m(1)+a3s2(5)sθ3(5)) / (a3cθ3(5)+a2);

[0110] c2(6)=(m(2)+a3s2(6)sθ3(6)) / (a3cθ3(6)+a2);

[0111] c2(7)=(m(3)+a3s2(7)sθ3(7)) / (a3cθ3(7)+a2);

[0112] c2(8)=(m(4)+a3s2(8)sθ3(8)) / (a3cθ3(8)+a2);

[0113] θ2(1)=atan2(s2(1),c2(1));

[0114] θ2(2)=atan2(s2(2),c2(2));

[0115] θ2(3)=atan2(s2(3),c2(3));

[0116] θ2(4)=atan2(s2(4),c2(4));

[0117] θ2(5)=atan2(s2(5),c2(5));

[0118] θ2(6)=atan2(s2(6),c2(6));

[0119] θ2(7)=atan2(s2(7),c2(7));

[0120] γ2(8)=atan2(s2(8),c2(8));

[0121] θ4(1)=atan2{-sθ6(1)[n x cθ1(1)+n x sθ1(1)]-cθ6(1)[o x cθ1(1)+o y sθ1(1)],o z cθ6(1)+n z sθ6(1)}-θ2(1)-θ341);

[0122] θ4(2)

[0123] atan2{-sθ6(2)[n x cθ1(2)+n y sθ1(2)]-cθ6(2)[o x cθ1(2)+o y sθ1(2)],o z cθ6(2)

[0124] n z sθ6(2)}-θ2(2)-θ3(2);

[0125] θ4(3)

[0126] atan2{-sθ6(3)[n x cθ1(1)+n ysθ1(1)]-cθ6(3)[o x cθ1(1)+o y sθ1(1)],o z cθ6(3)

[0127] n z sθ6(3)}-θ2(3)-θ3(3);

[0128] θ4(4)

[0129] atan2{-sθ6(4)[n x cθ1(2)+n y sθ1(2)]-cθ6(4)[o x cθ1(2)+o y sθ1(2)],o z cθ6(4)

[0130] n z sθ6(4)}-θ2(4)-θ3(4);

[0131] θ4(5)

[0132] atan2{-sθ6(1)[n x cθ1(1)+n y sθ1(1)]-cθ6(1)[o x cθ1(1)+o y sθ1(1)],o z cθ6(1)

[0133] n z sθ6(1)}-θ2(5)-θ3(5);

[0134] θ4(6)

[0135] atan2{-sθ6(2)[n x cθ1(2)+n y sθ1(2)]-cθ6(2)[o x cθ1(2)+o y sθ1(2)],o z cθ6(2)

[0136] n z sθ6(2)}-θ2(6)-θ3(6);

[0137] θ4(7)

[0138] atan2{-sθ6(3)[n x cθ1(1)+n x sθ1(1)]-cθ6(3)[ox cθ1(1)+o y sθ1(1)],o z cθ6(3)

[0139] n z sθ6(3)}-θ2(7)-θ3(7);

[0140] θ4(8)

[0141] atan2{-sθ6(4)[n x cθ1(2)+n y sθ1(2)]-cθ6(4)[o x cθ1(2)+o y sθ1(2)],o z cθ6(4)

[0142] n z sθ6(4)}-θ2(8)-θ3(8);

[0143] Wherein, θ1(1) and θ1(2) are two possible joint angle solutions for joint 1 in the solution set, θ2(1), θ2(2), θ2(3), θ2(4), θ2(5), θ2(6), θ2(7), and θ2(8) are eight possible joint angle solutions for joint 2 in the solution set, θ3(1), θ3(2), θ3(3), θ3(4), θ3(5), θ3(6), θ3(7), and θ3(8) are eight possible joint angle solutions for joint 3 in the solution set, θ4(1), θ4(2), θ4(3), θ4(4), θ4(5), θ4(6), θ4(7), and θ4(8) are eight possible joint angle solutions for joint 4 in the solution set, θ5(1), θ5(2), θ5(3), and θ5(4) are four possible joint angle solutions for joint 5 in the solution set, and θ6( 1) θ6(2), θ6(3), θ6(4) are the 8 possible joint angle solutions for joint 6 in the solution set, s2(1), s2(2), s2(3), s2(4), s2(5), s2(6), s2(7), s2(8) are the sine values ​​of the 8 possible joint angle solutions for joint 2 in the solution set, c2(1), c2(2), c2(3), c2(4), c2(5), c2(6), c2(7), c2(8) are the cosine values ​​of the 8 possible joint angle solutions for joint 2 in the solution set, m(1), m(2), m(3), m(4), n(1), n(2), n(3), n(4), u(1), u(2), u(3), u(4), v are intermediate variables set for easy calculation and subsequent processing, where u(1) = u(4), u(2) = u(3);

[0144] The intermediate variable is one of the bases for screening the solution set;

[0145] After sorting, a solution set containing 8 solutions is obtained and stored in the 8×6 matrix theta, and there is

[0146]

[0147] One row of the matrix is a solution in the solution set, and the k-th column is the angle of joint k.

[0148] Step 2.4: Screen and optimize the solution set;

[0149] Step 2.4.1: Let the initial values of the solution numbers to be screened be dlt1 = dlt2 = dlt3 = dlt4 = 0. Assume that the solution numbers to be screened are dlt1 = 1, dlt2 = 2, dlt3 = 3, dlt4 = 4. The 1st, 2nd, 3rd, and 4th rows of the solution set matrix theta (i.e., the 1st, 2nd, 3rd, and 4th solutions in the solution set) will be screened;

[0150] Step 2.4.2: Perform operations on the cosine values a x 、a y of the angle between the Z6 axis in the end coordinate system {6} of the robotic arm and the X0 and Y0 axes of the base coordinate system {0} of the robotic arm, the coordinates p x 、p y of the origin O6 of the end effector of the robotic arm on the X0 axis and Y0 axis in the base coordinate system {0} of the robotic arm, and the robotic arm link parameters d4 and d6. If (d6a y - p y ) 2 + (d6a x - p x ) 2 - d4 2 < 0, then is an imaginary number and cannot be used for the arctangent operation. Pause the program and output the statement "There is no solution for this acupoint coordinate. Please adjust the human posture and try again!";

[0151] Step 2.4.2.1: Compare the intermediate variables u and v. If u(1) > v and v(2) < v, that is, (u(1) - a2 2 - a3 [[ID=四十八]] 2 [[ID=四十九]]) / 2a2a3 > 1, it cannot be used for the arccosine operation, but (u(2) - a2 2 - a3 2) / 2a2a3 < 1, can be used for the arccosine operation. Temporarily set u(1) = u(4) = u(2) to enable the program to continue running without errors. Set the solution numbers to be screened out as dlt1 = 1, dlt2 = 4, dlt3 = 5, dlt4 = 8 (i.e., the 1st, 4th, 5th, and 8th solutions in the solution set theta will be screened out in the subsequent steps), and continue to execute step 2.4.3;

[0152] Step 2.4.2.2: Compare the intermediate variables u and v. If u(1) < v and u(2) > v, i.e., (u(1) - a2 2 -a3 2 ) / 2a2a3 < 1, it can be used for the arccosine operation, but (u(2) - a2 2 -a3 2 ) / 2a2a3 > 1 and cannot be used for the arccosine operation. Temporarily set u(2) = u(3) = u(1) to enable the program to continue running without errors. Set the solution numbers to be screened out as dlt1 = 2, dlt2 = 3, dlt3 = 6, dlt4 = 7 (i.e., the 2nd, 3rd, 6th, and 7th solutions in the solution set theta will be screened out in the subsequent steps), and continue to execute step 2.4.3;

[0153] Step 2.4.2.3: If all elements in the intermediate variable u are greater than v, then (u(1) - a2 2 -a3 2 ) / 2a2a3 > 1 and (u(2) - a2 2 -a3 2 ) / 2a2a3 > 1, and it cannot be used for the arccosine operation. Pause the program and output the statement "There is no solution for the acupoint coordinates. Please adjust the body posture and try again!"; <00...​​​​​​​​​​​​​​​​​​

[0157] This step includes the following sub-steps:

[0158] Step 4.1: Set the robotic arm end effector to start from the origin (x) o ,y o ,z o The trajectory of the robot arm moving to the endpoint (x, y, z) is a straight line. Divide this line into n equal parts. Also divide the coordinate differences between two points along each coordinate axis into n equal parts. This requires n-1 division points. The length dx of each division along the X0 axis of the robot arm's base coordinate system is dx = (xx... o ) / n, the lengths of each equal part along the Y0 axis of the robot arm's base coordinate system are dy=(yy o ) / n, the lengths of each equal part along the Y0 axis of the robot arm's base coordinate system are dz = (zz) / n. o The trajectory has n+1 points, including the starting and ending points, that need to be solved using inverse kinematics. These points are denoted as solution points, and their coordinates are [x...]. o +(j-1)*dx,y o +(j-1)*dy,z o +(j-1)*dz](j=1,2,…,n+1);

[0159] To position the end of the moxa stick at the application point (x, y, z), the coordinates of the robotic arm's end should be (x, y, z + lc), and the coordinates of each intermediate point should be [x, y, z + lc]. o +(j-1)*dx,y o +(j-1)*dy,z o +(j-1)*dz+lc](j=1,2,…,n+1), pose is set as follows:

[0160]

[0161] Step 4.2: Starting from j=1, take the joint angle qc(j) corresponding to the j-th point on the trajectory as the reference angle q. ref The (j+1)th solution point (x) is obtained through steps 2 and 3. o +j*dx,y o +j*dy,z o The joint angle qc(j+1) corresponding to +j*dz+lc) is calculated until j=n+1.

[0162] In some embodiments of the present invention, the number of equal parts n is set to 6.

[0163] Step 4.3: Starting from j=1, make the angle of joint 6 in the joint angle qc(j) corresponding to the j-th solution point on the trajectory 3.5rad, until j=n+1.

[0164] Step 5: Establish client and server, start executing moxibustion program. The order of acupoints is obtained by the moxibustion coordinate matrix cod described in step 1.3. The current acupoint order is pointorder, and the initial value is set to 1.

[0165] Step 5.1: Send the command servoj(qc(j),a,v,t,lookahead_time,gain)(j=1,2,…,n+1) containing the joint angles corresponding to each intermediate point to the robotic arm in sequence. The parameters in the command are control time t, lookahead_time, gain, and acceleration a and velocity v are set to 0 in this mode.

[0166] In some embodiments of the present invention, gain = 100, and when j = 1 in the joint angle qc(j), the control time t = 5s and the lookahead time time_ahead = 0.1s; when j is other values, the control time t = 2s and the lookahead time lookahead_time = 0.05s.

[0167] Step 5.2: Rotate the moxa stick around the acupoint and record the number of rotations;

[0168] Step 5.2.1: Obtain the real-time joint angle once per second from the information sent by the robotic arm, and determine whether each joint has reached the joint angle qc(n+1) corresponding to the endpoint. If it has, rotate the end effector of the robotic arm; otherwise, repeat step 5.1 until each joint reaches the specified angle. In some embodiments of the present invention, the required number of rotations is rottimes, the current number of rotations is cycletimes, and the current joint angle is set to [(-1)]. cycletimes [×3.5rad], with the angles of other joints remaining unchanged, the rotational target joint angle is obtained as q. rotate Send the command movej(q,0.2,0.6) to make joint 6 move at 0.2 rad / s 2 The angular acceleration, angular velocity of 0.6 rad / s, rotates to [(-1)]. cycletimes [×3.5rad] (i.e., each joint angle rotated to the target joint angle q) rotate Compared with the joint angle qc(n+1) corresponding to the endpoint, only the angle of joint 6 changes, so only joint 6 rotates. This process takes about 15 seconds. cycletimes increments by 1.

[0169] Step 5.2.2: Record the current total number of rotations as ordertimes, which is the current number of rotations for the current acupoint plus the required number of rotations for the acupoints that have already been treated. If ordertimes is divisible by an integer [(unit time / time for the moxa stick to rotate around the acupoint once) * m] (unit time is 1 minute, i.e., it has rotated for m minutes, where m can be set by the user according to the degree of ash accumulation after burning different types of moxa sticks for a certain period of time. In some embodiments of the present invention, the time for the moxa stick to rotate around the acupoint once is set to 15s, and the moxa stick will rotate around the acupoint 4 times in 1 minute of moxibustion, where the integer here is 4m), proceed to step 5.3. Otherwise, continue to proceed to step 5.2.1 until the current number of rotations cycletimes equals the required number of rotations rottimes.

[0170] Step 5.3: Clean up the ash that has accumulated on the moxa sticks after burning;

[0171] Step 5.3.1: Using the joint angle qc(n+1) corresponding to the moxibustion point described in Step 4 as the reference angle, the moxibustion point (x,y,z+lc) as the trajectory starting point, and (x,y,z+lc+h) as the trajectory ending point, perform trajectory planning and send the instruction described in Step 5.1 to the robotic arm to raise its end to a preset height (in some embodiments of the present invention, the height is 100h cm; the instruction to the robotic arm is in meters, and to convert it to cm, it needs to be multiplied by 100), ensuring that the end of the moxa stick is kept at a certain distance from the human body before subsequent operations (the height h can be set by the user; considering safety and operating efficiency, it is recommended to set it between 0.15 and 0.3, with a default value of 0.2).

[0172] Step 5.3.2: Using the joint angle corresponding to the coordinates (x, y, z + lc + 0.2) described in Step 5.3.1 as the reference angle, and (x, y, z + lc + 0.2) as the trajectory starting point, (x... ash1 ,y ash1 ,z ash1 The endpoint of the trajectory is (in some embodiments of the present invention, the default value is (0.3, -0.3, 0.4), which can be changed by the user. It is recommended that 0.3 ≤ x. ash1 ≤0.4, -0.4≤y ash1 ≤0.3, 0.35≤z ash1 ≤0.45), send the instruction described in step 5.1 to the robotic arm, causing its end effector to move to (x ash1 ,y ash1 ,z ash1 );

[0173] Step 5.3.3: Set q ash1 For (x) ash1 ,y ash1 ,z ash1The corresponding joint angle (already solved in step 5.3.2) is used to set the joint angle q. ash2 =q ash1 +[π / 4,0,0,0,0,0], send the command movej(q) to the robotic arm. ash2 (0.2, 0.8), rotate the angle of each joint to q. ash2 (because q) ash2 With q ash1 Only the angle of joint 1 is different; in reality, only joint 1 rotates π / 4) to clean up the incense ash, and then the command movej(q) is sent to the robotic arm. ash1 (0.2, 0.8), causing the robotic arm to return to (x) ash1 ,y ash1 ,z ash1 );

[0174] Step 5.3.4: Using the q described in step 5.3.3 ash1 For reference angle, step 5.3.2 describes (x) ash1 ,y ash1 ,z ash1 (x,y,z+lc+0.2) is the starting point of the trajectory, and (x,y,z+lc+0.2) is the ending point of the trajectory. Send the instruction described in step 5.1 to the robotic arm so that its end effector moves to (x,y,z+lc+0.2).

[0175] Step 5.3.5: Using the joint angle (x, y, z+lc+0.2) described in Step 5.3.1 as the reference angle, (x, y, z+lc+0.2) as the trajectory start point, and (x, y, z+lc) as the trajectory end point, perform trajectory planning, send the instruction described in Step 5.1 to the robotic arm, so that its end point returns to the original moxibustion point, and continue to execute Step 5.2;

[0176] Step 5.4: The current number of rotations cycletimes is equal to the required number of rotations rottimes. The current acupoint order pointorder is incremented by 1. Step 4 is repeated. The joint angle qc(1) corresponding to the new starting point is set to the joint angle qc(n+1) corresponding to the moxibustion coordinate of the previous acupoint. Steps 5.1 and 5.2 are repeated until the current acupoint order pointorder is equal to the number of acupoints s, indicating that all acupoints have been moxibusted.

[0177] Step 5.5: After the moxibustion is completed, send a command to return the robotic arm to zero, and the program ends.

[0178] This invention proposes a robotic arm trajectory planning method for intelligent moxibustion devices based on robot inverse kinematics theory. This method ensures that the end of the moxa stick maintains a safe distance from the user's back throughout the moxibustion process, improving the safety of the intelligent moxibustion device. The robot's end-effector trajectory remains continuous during moxibustion, preventing strong impacts to the robot's joint motors and avoiding motor damage. Multiple inverse kinematic solutions are screened to select the optimal solution, reducing the rotation angles of the robotic arm joints and confining the entire movement process to a smaller space, further improving safety and system reliability. Multiple data points are output in real time during program execution, helping users obtain real-time information about the program and robotic arm operation, improving system observability. Users can modify some parameters in the program, providing strong operability.

[0179] The values ​​given in the foregoing embodiments are merely specific examples and do not constitute a limitation on the scope of protection.

[0180] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A method for planning the moxibustion process using a robotic arm based on an intelligent incense moxibustion device, characterized in that, Includes the following steps: Step 1: Determine the coordinates of the moxibustion point in the coordinate system {0} of the robotic arm base, obtain all the moxibustion coordinates used for moxibustion, and obtain the number of moxa stick rotations (rottimes) according to the moxibustion time corresponding to the acupoint. Step 2: Establish the forward kinematics equations of the robotic arm, and solve the solution set of each joint angle of the robotic arm joints by inverse kinematics based on the forward kinematics equations. Filter and optimize the solution set to obtain the corrected solution set q1. Step 3: Set a reference joint angle q ref , based on this reference joint angle q ref Determine the optimal solution in the solution set q1; Step 4: Plan the trajectory of the robotic arm's end effector; Step 5: The robotic arm performs moxibustion according to the planned trajectory, and the end of the moxa stick is kept at a certain distance from the human body during the moxibustion process; Step 4 includes: Step 4.1: Setting the robotic arm end effector to start from the origin (x) o ,y o ,z o The trajectory of the robot arm moving to the endpoint (x, y, z) is a straight line. This straight line is divided into n equal parts. The coordinate differences between two points along each coordinate axis are also divided into n equal parts. The length dx of each part along the X0 axis of the robot arm base coordinate system is dx = (xx o ) / n, the lengths of each equal part along the Y0 axis in the coordinate system of the robot arm base are dy=(yy o ) / n, the lengths of each equal part along the Y0 axis in the coordinate system of the robot arm base are dz=(zz) o The trajectory has n+1 points, including the starting and ending points, that need to be solved using inverse kinematics. These points are denoted as solution points, and their coordinates are... ; To position the end of the moxa stick at the application point (x, y, z), the coordinates of the robotic arm's end should be (x, y, z + lc), where lc is the current length of the moxa stick, and the coordinates of each intermediate point are... The pose is set as follows: ； Step 4.2: Starting from j=1, take the joint angle qc(j) corresponding to the j-th point on the trajectory as the reference angle q. ref The (j+1)th solution point is obtained through steps 2 and 3. The corresponding joint angle qc(j+1) is used until j=n+1; Step 4.3: Starting from j=1, make the angle of joint 6 in the joint angle qc(j) corresponding to the j-th solution point on the trajectory the preset value, until j=n+1.

2. The method for planning the moxibustion process using a robotic arm based on an intelligent incense moxibustion device according to claim 1, characterized in that, Step 1 includes the following sub-steps: Step 1.1: Before starting, return the robotic arm to zero; Step 1.2: Move each joint of the robotic arm to the set shooting angle to take a picture of the back of the person receiving moxibustion. Based on the image obtained, obtain the acupoints of the person receiving moxibustion and the corresponding two-dimensional coordinate information. After coordinate system transformation, obtain the coordinates (x,y) of the acupoints of the person receiving moxibustion relative to the coordinate system {0} of the robotic arm base. Step 1.3: Obtain the information of the recipient of moxibustion, and based on the coordinates relative to the coordinate system {0} of the robotic arm base, select the acupoints for moxibustion according to the symptoms, as well as the coordinates z of the corresponding moxibustion points relative to the Z0 axis of the coordinate system {0} of the robotic arm base and the moxibustion time. Obtain the moxibustion coordinates (x, y, z) based on the coordinate z. Store all the moxibustion coordinates used in one moxibustion in the matrix cod, where s is the number of acupoints, and the moxibustion time corresponding to the acupoints is stored in the matrix t. Step 1.4: Obtain the number of moxa stick rotations (rottimes) based on the moxibustion time described in Step 1.3; Step 1.5: Let the initial length of the moxa stick be lo, the current length be lc, the burning speed of the moxa stick be v, the program start time be to, the current time be tc, and the elapsed time dt = tc - to. ; Step 1.5: After filming is complete, return the robotic arm to zero.

3. The method for planning the moxibustion process using a robotic arm based on an intelligent incense moxibustion device according to claim 1, characterized in that, Step 2 includes the following sub-steps: Step 2.1: Establish the joint coordinate system {i} of the six-DOF robotic arm using the DH parameter method; Step 2.2: Establish the forward kinematics of the robotic arm using a homogeneous coordinate transformation matrix based on the DH parameters of the robotic arm. Procedure: Step 2.3: Derive the inverse kinematics formula based on the forward kinematics equations described in Step 2.2, solve for each joint angle, and obtain the solution set; Step 2.4: Filter and optimize the solution set to obtain the corrected solution set q1.

4. The method for planning the moxibustion process using a robotic arm based on an intelligent incense moxibustion device according to claim 3, characterized in that, The steps to obtain the solution set include: The established forward kinematic equations are as follows: The 4×4 transfer matrix is ​​used to transfer the coordinates from the robot arm base coordinate system {0} to the joint 6 coordinate system {6}. in, Let be the 4×4 transfer matrix from the robot arm base coordinate system {0} to the joint 1 coordinate system {1}. Let be the 4×4 transfer matrix from coordinate system {1} of joint 1 to coordinate system {2} of joint 2. Let {2} be the 4×4 transfer matrix from coordinate system {2} of joint 2 to coordinate system {3} of joint 3. Let be the 4×4 transfer matrix from coordinate system {3} of joint 3 to coordinate system {4} of joint 4. Let be the 4×4 transfer matrix from coordinate system {4} of joint 4 to coordinate system {5} of joint 5. Let {5} be the 4×4 transfer matrix from coordinate system {5} of joint 5 to coordinate system {6} of joint 6. , and Let X, Y, and Z be the cosines of the angles between the X6 axis in the robot arm's end-effector coordinate system {6} and the robot arm's base coordinate system {0}, respectively. , and Let X, Y, and Z be the cosines of the angles between the Y6 axis in the end-effector coordinate system {6} and the X0, Y0, and Z0 of the robot arm base coordinate system {0}, respectively. , and Let X, Y, and Z be the cosines of the angles between the Z6 axis in the robot arm's end-effector coordinate system {6} and the robot arm's base coordinate system {0}, respectively. , and These are the coordinate positions of the origin O6 of the robotic arm end effector on the X0 axis, Y0 axis, and Z0 axis of the robotic arm base coordinate system {0}, respectively. By deriving the inverse kinematics formula and solving for each joint angle, we obtain: ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； in, , To solve for the two possible joint angle solutions for joint 1, , , , , , , , To solve for the eight possible joint angles of joint 2, , , , , , , , To solve for the eight possible joint angles of joint 3. , , , , , , , To solve for the eight possible joint angles of joint 4. , , , To solve for the four possible joint angles of joint 5. , , , To solve for the four possible joint angles of joint 6. , , , , , , , To find the sine values ​​of the eight possible joint angle solutions for joint 2, , , , , , , , To find the cosine values ​​of the eight possible joint angle solutions for joint 2, , , , , , , , , , , , , , Intermediate variables set to facilitate calculation and subsequent processing, among which = , = ; The solution set is obtained by rearranging the matrix to have 8 solutions, and stored in an 8×6 matrix theta. 。 5. The method for planning the moxibustion process using a robotic arm based on an intelligent incense moxibustion device according to claim 4, characterized in that, The process of filtering and optimizing the solution set includes the following steps: Step 2.4.1: Let the initial values ​​of the solution numbers to be screened be dlt1=dlt2=dlt3=dlt4=0. Assume that the solution numbers to be screened are dlt1=1, dlt2=2, dlt3=3, and dlt4=4. The first, second, third, and fourth rows of the solution set matrix theta will be screened out. Step 2.4.2: Find the cosine value α of the angle between the Z6 axis in the robot arm end-effector coordinate system {6} and the X0 and Y0 axes in the robot arm base coordinate system {0}. x a y The origin O6 of the robotic arm's end effector lies on the X0 axis and Y0 axis of the robotic arm's base coordinate system {0}, with coordinates p. x p y Calculate with the robotic arm link parameters d4 and d6, if (d6a y -p y ) 2 +(d6a x -p x ) 2 -d4 2 <0, then Since it is an imaginary number, it cannot be used for arctangent operations; program execution will be paused. Step 2.4.3: Excluding the solutions dlt1, dlt2, dlt3 and dlt4 in the solution set, for each joint angle whose value is less than -2π in the remaining solutions, add 2π to it, and for each joint angle whose value is greater than 2π, subtract 2π from it, to obtain the corrected solution set q1.

6. The method for planning the moxibustion process using a robotic arm based on an intelligent incense moxibustion device according to claim 5, characterized in that, When performing filtering and optimization: Compare the intermediate variables u and v. If u(1) > v and v(2) < v, that is, (u(1) - a2 2 -a3 2 ) / 2a2a3 > 1, it cannot be used for the arccosine operation. However, (u(2) - a2 2 -a3 2 ) / 2a2a3 < 1, which can be used for the arccosine operation. Temporarily set u(1) = u(4) = u(2) to enable the program to continue running without errors. Set the solution sequence numbers to be screened out as dlt1 = 1, dlt2 = 4, dlt3 = 5, dlt4 = 8, and continue to execute step 2.4.3; Compare the intermediate variables u and v. If u(1) < v and u(2) > v, that is, (u(1) - a2 2 -a3 2 ) / 2a2a3 < 1, it can be used for the arccosine operation. However, (u(2) - a2 2 -a3 2 ) / 2a2a3 > 1 and cannot be used for the arccosine operation. Temporarily set u(2) = u(3) = u(1) to enable the program to continue running without errors. Set the solution sequence numbers to be screened out as dlt1 = 2, dlt2 = 3, dlt3 = 6, dlt4 = 7, and continue to perform the operation of adding or subtracting 2π; If all elements in the intermediate variable u are greater than v, then (u(1)-a2) 2 -a3 2 ) / 2a2a3>1、(u(2)-a2 2 -a3 2 The expression ) / 2a2a3>1 cannot be used for inverse cosine operations, so the program should be paused.

7. The method for planning the moxibustion process using a robotic arm based on an intelligent incense moxibustion device according to claim 1, characterized in that, Step 3 includes: successively comparing all solutions in the corrected solution set q1 with the reference joint angle q ref Subtract the values ​​and select the corrected solution set q1 and the reference joint angle q. ref Calculate the sum q of the absolute values ​​of the angle difference between joint 1 and joint 2. d If the q of a certain solution in the corrected solution set q1 is... d If the value is the smallest among all solutions, then that solution is selected as the optimal solution.

8. The method for planning the moxibustion process using a robotic arm based on an intelligent incense moxibustion device according to claim 1, characterized in that, Step 5 of the moxibustion process includes: Step 5.1: Send the instruction servoj(qc(j),a,v,t,lookahead_time,gain) containing the joint angles corresponding to each intermediate point to the robotic arm in sequence, j =1,2,…,n+1; Step 5.2: Rotate the moxa stick around the acupoint and record the number of rotations; Step 5.3: Clean up the ash that has accumulated on the moxa sticks after burning; Step 5.4: The current number of rotations cycletimes is equal to the required number of rotations rottimes. The current acupoint order pointorder is incremented by 1. Step 4 is repeated. The joint angle qc(1) corresponding to the new starting point is set to the joint angle qc(n+1) corresponding to the moxibustion coordinate of the previous acupoint. Steps 5.1 and 5.2 are repeated until all acupoints have been moxibusted.

9. The method for planning the moxibustion process using a robotic arm based on an intelligent incense moxibustion device according to claim 8, characterized in that, Step 5.2 includes: Step 5.2.1: Obtain the real-time joint angle once per second from the information sent by the robotic arm, and determine whether each joint has reached the joint angle qc(n+1) corresponding to the endpoint. If it has, rotate the end of the robotic arm. If not, repeat step 5.1 until each joint reaches the specified angle. Step 5.2.2: Record the current total number of rotations as ordertimes, which is the current number of rotations for the current acupoint plus the required number of rotations for the acupoints that have already been treated. If ordertimes can be divided by an integer [(unit time / time for the moxa stick to rotate around the acupoint once)], then the total number of rotations is determined by ordertimes. If divisible, proceed to step 5.3; otherwise, continue to step 5.2.1 until the current number of rotations (cycletimes) equals the required number of rotations (rottimes). Step 5.3 includes the following steps: Step 5.3.1: Using the joint angle qc(n+1) corresponding to the moxibustion point described in Step 4 as the reference angle, the moxibustion point (x,y,z+lc) as the trajectory start point, and (x,y,z+lc+h) as the trajectory end point, perform trajectory planning, and send the instruction described in Step 5.1 to the robotic arm to raise its end to a preset height, ensuring that the end of the moxa stick is kept at a certain distance from the human body before performing subsequent operations; Step 5.3.2: Using the joint angle corresponding to the coordinates (x, y, z + lc + set ascent height) in step 5.3.1 as the reference angle, (x, y, z + lc + set ascent height) as the trajectory starting point, (x... ash1 ,y ash1 ,z ash1 (x) is the endpoint of the trajectory, and the command described in step 5.1 is sent to the robotic arm to move its end effector to (x). ash1 ,y ash1 ,z ash1 ); Step 5.3.3: Set q ash1 For (x) ash1 ,y ash1 ,z ash1 For the corresponding joint angle, set the joint angle q. ash2 =q ash1 +[π / 4,0,0,0,0,0], sends a command to the robotic arm to rotate the angles of each joint to q. ash2 The incense ash is cleaned up, and then a command is sent to the robotic arm to return it to (x). ash1 ,y ash1 ,z ash1 ); Step 5.3.4: Using the q described in step 5.3.3 ash1 For reference angle, step 5.3.2 describes (x) ash1 ,y ash1 ,z ash1 (x,y,z+lc+set rising height) is the starting point of the trajectory, and (x,y,z+lc+set rising height) is the ending point of the trajectory. Send the instruction described in step 5.1 to the robotic arm so that its end effector moves to (x,y,z+lc+set rising height). Step 5.3.5: Using the joint angle corresponding to (x,y,z+lc+set rising height) described in Step 5.3.1 as the reference angle, (x,y,z+lc+set rising height) as the trajectory starting point, and (x,y,z+lc) as the trajectory ending point, perform trajectory planning, send the instruction described in Step 5.1 to the robotic arm, so that its end point returns to the original moxibustion point, and continue to execute Step 5.2.

Images