Joint angular acceleration estimation method of mechanical arm and control method of mechanical arm

Abstract

The invention relates to the field of robot control, and discloses a joint angular acceleration estimation method of a mechanical arm, a control method of the mechanical arm, a robot system, computer equipment and a storage medium. The mechanical arm comprises a plurality of arm bodies and a plurality of joints connected with the arm bodies. The joint angular acceleration estimation method of the mechanical arm comprises the steps that at least one driving torque of the joints is obtained; obtaining joint angles of a plurality of joints; obtaining joint angular velocities of a plurality of joints; based on the at least one driving torque, the joint angles, the joint angular velocities and a kinetic model of the mechanical arm, first estimated values of joint angular accelerations of the multiple joints are calculated; wherein the dynamic model comprises at least one of the following items: a dynamic driving relation of a linkage mechanism in the mechanical arm, gravity offset of the mechanical arm or compensation of a gravity compensation mechanism of the mechanical arm.

Description

Technical Field

[0001] The present disclosure relates to the field of medical devices, and in particular, to a method for estimating joint angular acceleration of a robotic arm, a control method of a robotic arm, a robotic system, a computer device, and a storage medium. Background Art

[0002] For a robotic system, especially a surgical robotic system, it usually includes a master manipulator and a slave operating device. The master manipulator is the core input device of the robotic system, and the operator controls the movement of the slave operating device by operating the robotic arm of the master manipulator. When using a robotic system (such as a surgical robotic system) to complete a task (such as a surgical task), the operator needs to operate the master manipulator for a long time and be highly concentrated. At this time, the operating experience of the master manipulator becomes particularly important. The master manipulator is usually a robotic arm with multiple degrees of freedom. During the operation of the robotic arm, there is an operating resistance inside the robotic arm, which will greatly affect the operating experience of the operator.

[0003] In order to achieve a better dragging effect of the master manipulator, most current master manipulators adopt an impedance control method. Impedance control is based on the current position, speed, and acceleration information of each joint of the robotic arm of the master manipulator, and realizes the control of the robotic arm of the master manipulator with the help of an accurate dynamic model. An accurate, smooth, and low-delay acceleration estimation algorithm is particularly important. Summary of the Invention

[0004] In some embodiments, the present disclosure provides a method for estimating joint angular acceleration of a robotic arm. The robotic arm includes a plurality of arm bodies and a plurality of joints connecting the plurality of arm bodies. The method includes: obtaining at least one driving torque of the plurality of joints; obtaining the joint angles of the plurality of joints; obtaining the joint angular velocities of the plurality of joints; and calculating a first estimated value of the joint angular acceleration of the plurality of joints based on at least one driving torque, joint angle, joint angular velocity, and the dynamic model of the robotic arm; wherein the dynamic model includes at least one of the following: the dynamic driving relationship of the linkage mechanism in the robotic arm, the gravity offset of the robotic arm, or the compensation of the gravity compensation mechanism of the robotic arm.

[0005] In some embodiments, the present disclosure provides a control method of a robotic arm. The robotic arm includes a plurality of arm bodies and a plurality of joints of the plurality of arm bodies. The control method includes: obtaining joint parameters of the plurality of joints to form a joint parameter set; the joint parameters include joint angle, joint angular velocity, and joint angular acceleration; the joint angular acceleration is calculated based on the method for estimating joint angular acceleration of a robotic arm according to any one of some embodiments of the present disclosure; calculating a control signal for the plurality of joints based on the joint parameter set and the dynamic model of the robotic arm; and sending the control signal to the plurality of joints.

[0006] In some embodiments, the present disclosure provides a robot system, including: at least one robotic arm, where the at least one robotic arm includes a plurality of arm bodies and a plurality of joints connecting the plurality of arm bodies; and a control device, connected to the at least one robotic arm and configured to execute the joint angular acceleration estimation method of the robotic arm according to any one of some embodiments of the present disclosure or the control method of the robotic arm according to any one of some embodiments of the present disclosure.

[0007] In some embodiments, the present disclosure provides a computer device, including: a memory configured to store at least one instruction; and a processor, coupled to the memory and configured to execute the at least one instruction to execute the joint angular acceleration estimation method of the robotic arm according to any one of some embodiments of the present disclosure or the control method of the robotic arm according to any one of some embodiments of the present disclosure.

[0008] In some embodiments, the present disclosure provides a computer-readable storage medium for storing at least one instruction, where when the at least one instruction is executed by a computer, the computer is caused to implement the joint angular acceleration estimation method of the robotic arm according to any one of some embodiments of the present disclosure or the control method of the robotic arm according to any one of some embodiments of the present disclosure. BRIEF DESCRIPTION OF THE DRAWINGS

[0009] To more clearly illustrate the technical solutions in the embodiments of the present disclosure, the following briefly introduces the drawings required for description in the embodiments of the present disclosure. The drawings in the following description only show some embodiments of the present disclosure. For those of ordinary skill in the art, other embodiments can be obtained according to the content of the embodiments of the present disclosure and these drawings without creative efforts.

[0010] Figure 1 A flowchart showing the joint angular acceleration estimation method of a robotic arm according to some embodiments of the present disclosure;

[0011] Figure 2 A structural block diagram showing a robot system according to some embodiments of the present disclosure;

[0012] Figure 3 A schematic diagram showing a master manipulator according to some embodiments of the present disclosure;

[0013] Figure 4 A schematic diagram of the internal structure of an arm body including a gravity compensation mechanism according to some embodiments of the present disclosure;

[0014] Figure 5 Showing according to the present disclosure Figure 4 A schematic diagram of the gravity compensation principle of an arm body including a gravity compensation mechanism;

[0015] Figure 6Flowchart showing a method for calculating a first estimate of joint angular acceleration according to some embodiments of the present disclosure;

[0016] Figure 7A - Figure 7C Schematic diagram showing the estimated results of joint angular acceleration of a robotic arm according to some embodiments of the present disclosure;

[0017] Figure 8 Flowchart showing a control method for a robotic arm according to some embodiments of the present disclosure;

[0018] Figure 9 Schematic block diagram showing a computer device according to some embodiments of the present disclosure. Detailed implementation manners

[0019] To make the technical problems solved, the technical solutions adopted, and the technical effects achieved by the present disclosure clearer, the technical solutions of the embodiments of the present disclosure will be further described in detail below with reference to the drawings. Obviously, the described embodiments are only exemplary embodiments of the present disclosure, rather than all embodiments.

[0020] In the description of the present disclosure, it should be noted that the orientation or positional relationship indicated by the terms "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inner", "outer", etc. is based on the orientation or positional relationship shown in the drawings, and is only for the convenience of describing the present disclosure and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and thus should not be construed as a limitation to the present disclosure. In addition, the terms "first" and "second" are only used for descriptive purposes and cannot be construed as indicating or implying relative importance. In the description of the present disclosure, it should be noted that unless otherwise clearly specified and limited, the terms "installed", "connected", "coupled" should be understood in a broad sense. For example, it can be a fixed connection or a detachable connection; it can be a mechanical connection or an electrical connection; it can be directly connected or indirectly connected through an intermediate medium; it can be the communication inside two elements. For those of ordinary skill in the art, the specific meanings of the above terms in the present disclosure can be understood according to specific circumstances. In the present disclosure, the end close to the operator (such as a doctor) is defined as the proximal end, the proximal part, the rear end, or the rear part, and the end close to the surgical patient is defined as the distal end, the distal part, the front end, or the front part. Those skilled in the art can understand that the embodiments of the present disclosure can be used for medical devices or surgical robots, and can also be used for other non-medical devices.

[0021] Some embodiments of the present disclosure provide a method for estimating joint angular acceleration of a robotic arm. Figure 1The flowchart of a method 100 for estimating joint angular acceleration of a robotic arm according to some embodiments of the present disclosure (hereinafter also simply referred to as "method 100") is shown. Method 100 can be implemented or executed at least partially by hardware, software, or firmware. In some embodiments, method 100 can be executed at least partially by a robotic system (e.g., Figure 2 the robotic system 200 shown). In some embodiments, method 100 can be implemented as computer-readable instructions. These instructions can be read and executed by a general-purpose processor or a dedicated processor (e.g., Figure 2 the control device 230 shown). For example, the control device of the robotic system (e.g., Figure 2 the control device 230 shown) can include a processor configured to execute method 100. In some embodiments, these instructions can be stored on a computer-readable medium.

[0022] Some embodiments of the present disclosure provide a method for controlling a robotic arm. Figure 8 The flowchart of a method 800 for estimating joint acceleration of a robotic arm according to some embodiments of the present disclosure (hereinafter also simply referred to as "method 800") is shown. Method 800 can be implemented or executed at least partially by hardware, software, or firmware. In some embodiments, method 800 can be executed at least partially by a robotic system (e.g., Figure 2 the robotic system 200 shown). In some embodiments, method 800 can be implemented as computer-readable instructions. These instructions can be read and executed by a general-purpose processor or a dedicated processor (e.g., Figure 2 the control device 230 shown). For example, the control device of the robotic system (e.g., Figure 2 the control device 230 shown) can include a processor configured to execute method 800. In some embodiments, these instructions can be stored on a computer-readable medium.

[0023] Some embodiments of the present disclosure provide a robotic system. Figure 2 The structural block diagram of a robotic system 200 according to some embodiments of the present disclosure is shown. As Figure 2 shown, the robotic system 200 can include at least one robotic arm and a control device 230. In some embodiments, the at least one robotic arm includes a first robotic arm 210 and a second robotic arm 220. In some embodiments, the at least one robotic arm includes a multi-degree-of-freedom robotic arm. The multi-degree-of-freedom robotic arm includes a plurality of arm bodies and a plurality of joints connecting the plurality of arm bodies. In some embodiments, the control device 230 can be communicatively connected to the at least one robotic arm, for example, by a cable connection or a wireless connection. In some embodiments, the control device 230 is used to execute the method for estimating joint angular acceleration of the robotic arm in some embodiments of the present disclosure (e.g., Figure 1The method shown (100). In some embodiments, the control device 230 is further configured to execute the control method of the robotic arm in some embodiments of the present disclosure (for example, Figure 8 the method shown (800).

[0024] Those skilled in the art should understand that the robotic system provided by the present disclosure can be a surgical robotic system (such as a laparoscopic surgical robotic system). The robotic system can also be a dedicated or general robotic system in other fields (such as logistics, industrial manufacturing, etc.).

[0025] In some embodiments, at least one robotic arm can be the master manipulator of the robotic system. In some embodiments, the master manipulator generally includes a left master manipulator (for example, for controlling the first slave operating device) corresponding to the left hand operation of the operator (such as medical staff) and a right master manipulator (for example, for controlling the second slave operating device) corresponding to the right hand operation. In some embodiments, the master manipulator is used to collect the operation inputs of the operator, and the operator controls the movement of the slave operating device (such as a surgical tool or an imaging tool) by remotely operating the master manipulator. In some embodiments, the master manipulator includes a robotic arm with multiple degrees of freedom, and the robotic arm with multiple degrees of freedom includes multiple arm bodies and multiple joints connecting the multiple arm bodies. In some embodiments, the robotic arm with multiple degrees of freedom has six degrees of freedom. In some embodiments, the master manipulator further includes a handle, and the clamp on the handle can be used to control the opening and closing angle of the slave tool. In some embodiments, the master manipulator can specifically be Figure 3 the master manipulator 300 shown, which is described in detail below.

[0026] Figure 3 Schematic diagram showing the master manipulator 300 of some embodiments of the present disclosure. As Figure 3 shown, in some embodiments, the master manipulator 300 includes a robotic arm 310 with multiple degrees of freedom. The robotic arm 310 with multiple degrees of freedom includes multiple arm bodies (311 - 316) and multiple joints (3111 - 3117) connecting the multiple arm bodies. Adjacent two arm bodies are connected by joints. In some embodiments, the master manipulator 300 further includes a handle 330, and the handle 330 is arranged at the end of the arm body 316. During operation, the multiple arm bodies are driven to move through the multiple joints to adjust the configuration of the robotic arm 310 with multiple degrees of freedom, so that the arm body 316 of the robotic arm 310 with multiple degrees of freedom moves to an appropriate pose, and the handle 330 is driven to move to a suitable pose (such as a target pose) to control the slave operating device (such as a surgical tool or an imaging tool) to reach the corresponding pose.

[0027] In some embodiments, the joints of the multi-degree-of-freedom robotic arm 310 include coupled joints and uncoupled joints. A coupled joint may refer to a joint used to adjust the position and posture of the master manipulator. An uncoupled joint may refer to a joint that can only be used to adjust the position or posture of the master manipulator. In some embodiments, the joints of the multi-degree-of-freedom robotic arm 310 include position joints and posture joints. The posture joints serve as the orientation module of the master manipulator 300, and the posture of the handle 330 can be determined through one or more posture joints. The position joints serve as the positioning module of the master manipulator 300, and the position of the handle 330 can be determined through one or more position joints. For example, Figure 3 The illustrated master manipulator 300, in which the first joint 3111, the second joint 3112, and the third joint 3113 are position joints, the first joint 3111, the second joint 3112, the fifth joint 3115, the sixth joint 3116, and the seventh joint 3117 are posture joints, the first joint 3111 and the second joint 3112 are coupled joints that can both adjust the position and the posture of the master manipulator 300, and the fifth joint 3115, the sixth joint 3116, and the seventh joint 3117 are uncoupled posture joints that can only adjust the posture of the master manipulator 300.

[0028] In some embodiments, the first joint 3111, the second joint 3112, and the third joint 3113 in the multi-degree-of-freedom robotic arm 310 determine the three-degree-of-freedom position information of the end effector. The fifth joint 3115, the sixth joint 3116, and the seventh joint 3117 form a three-axis concurrent mechanism, which mainly provides the three-degree-of-freedom posture information of the end effector. When operating the multi-degree-of-freedom robotic arm 310, the operating resistance mainly comes from the first joint 3111, the second joint 3112, and the third joint 3113. The first three arm bodies (the first arm body 311, the second arm body 312, the third arm body 313) are respectively regarded as the first link, the second link, and the third link connected in sequence through joints. The overall three-axis concurrent mechanism of the last three arm bodies (the fourth arm body 314, the fifth arm body 315, the sixth arm body 316) and the last three joints (the fifth joint 3115, the sixth joint 3115, the seventh joint 3117) is regarded as the fourth link. The third link is connected to the fourth link through the fourth joint 3114. Thus, Figure 3 The illustrated multi-degree-of-freedom robotic arm 310 can be regarded as a four-link mechanism.

[0029] In some embodiments, a dynamic model of a robotic arm (e.g., Figure 3 the illustrated multi-degree-of-freedom robotic arm 310) can be constructed. In some embodiments, the robotic arm can be a multi-degree-of-freedom robotic arm (e.g., Figure 3The multi-degree-of-freedom robotic arm shown (310), the robotic arm includes multiple arm bodies and multiple joints connecting the multiple arm bodies. In some embodiments, the dynamic model includes at least one of the following: the dynamic drive relationship of the linkage mechanism in the robotic arm, the gravity offset of the robotic arm, or the compensation of the gravity compensation mechanism of the robotic arm. In some embodiments, the dynamic model of the robotic arm includes the balance of one or more of at least one driving torque and at least one resistance torque, at least one compensation torque, and at least one external force equivalent torque of multiple joints.

[0030] In some embodiments, a joint drive device is provided in multiple joints of the robotic arm, and the joint drive device is used to drive the joint to rotate around a joint rotation axis (for example, Figure 3 in axis, axis, axis, axis), and at least one driving torque includes the driving torque output by the joint drive device, which can be denoted as τ i,motor (i is the number of the joint).

[0031] In some embodiments, resistance will be generated during the movement of multiple joints of the robotic arm, and at least one resistance torque may include a first resistance torque. The first resistance torque may be the joint resistance torque of multiple joints caused by the gravity of the robotic arm, and the first resistance torque may be a function of the joint parameters (for example, joint angle) of multiple joints. In some embodiments, the first resistance torque of multiple joints can be calculated based on the joint parameters (for example, joint angle), which will be described in detail in some of the following embodiments.

[0032] In some embodiments, at least one resistance torque may include a second resistance torque. The second resistance torque may be the joint resistance torque of multiple joints caused by the gravity offset of the robotic arm. In some embodiments, the robotic arm is installed on a base, and the gravity offset of the robotic arm includes the gravity offset caused by the uneven installation of the base. The base uneven parameter includes the tilt angle vector of the base relative to the reference plane. In some embodiments, multiple joints include a first joint, and the robotic arm is connected to the base through the first joint. In some embodiments, the reference plane is a horizontal plane, and the tilt angle vector of the base relative to the reference plane is the angle between the plane perpendicular to the first rotation axis of the first joint and the reference plane. In the present disclosure, the reference plane may be a horizontal plane (for example, the ground, which will be used as an example hereinafter for description).

[0033] Taking Figure 3 the main manipulator 300 shown as an example, Figure 3 the coordinate system in

[0034] is defined as follows: The axis is upward along the rotation axis of the first joint 3111, The axis points to the second joint 3112, The axis is based on the axis and the axis is defined and determined in a right - hand coordinate system.

[0035] Second joint coordinate system {2}: The axis is along the rotation axis of the second joint 3112 upwards, the axis points to the third joint 3113, the axis is based on the axis and the axis is defined and determined in a right - hand coordinate system.

[0036] Third joint coordinate system {3}: The axis is along the rotation axis of the third joint 3113 outwards, the axis points to the fourth joint 3114, the axis is based on the axis and the axis is defined and determined in a right - hand coordinate system.

[0037] Fourth joint coordinate system {4}: The axis is along the rotation axis of the fourth joint 3114 outwards, the axis points to the fifth joint 3115, the axis is based on the axis and the axis is defined and determined in a right - hand coordinate system.

[0038] The origins of the first joint coordinate system {1}, the second joint coordinate system {2}, the third joint coordinate system {3} and the fourth joint coordinate system {4} are in the same plane.

[0039] Base coordinate system {0} of the master manipulator: The origin coincides with the origin of the first joint coordinate system {1}, the axis is along the rotation axis of the first joint 3111 upwards, the direction can be as Figure 3 shown in

[0040] Ground coordinate system {W}: The origin coincides with the origin of the first joint coordinate system {1}, the axis is opposite to the direction of gravity, the plane is parallel to the ground, the axis, the axis direction can be as Figure 3 shown in

[0041] Figure 3 In [reference], the first joint 3111 of the multi - degree - of - freedom robotic arm 310 is connected to the base 340, and the first rotation axis of the first joint 3111 ( Figure 3 shown in the axis) and the second rotation axis of the second joint 3112 (Figure 3 shown in axis) are parallel to each other, and the third rotation axis of the third joint 3113 ( Figure 3 shown in axis) is perpendicular to the first rotation axis and the second rotation axis. The fourth joint 3114 is a driven joint of the third joint 3113, and the fourth rotation axis of the fourth joint 3114 ( Figure 3 shown in axis) is parallel to the rotation axis of the third joint.

[0042] In some embodiments, the Figure 3 plane of the ground coordinate system {W} shown in can be used as a reference plane, and the tilt angle vector of the base relative to the reference plane includes a first tilt angle and a second tilt angle. The first tilt angle can be the plane perpendicular to the first rotation axis of the first joint (the plane of the base coordinate system {0} of the master manipulator) rotating around the axis by an angle θ x , and the second tilt angle can be the plane perpendicular to the first rotation axis of the first joint (the plane of the base coordinate system {0} of the master manipulator) rotating around the axis by an angle θ y . The tilt angle vector of the base relative to the reference plane is [θ x , θ y T .

[0043] In some embodiments, the base unevenness parameter can be the tilt angle vector of the base relative to the reference plane, which can be expressed as [θ x , θ y T . In some embodiments, the second resistance torque can be a function of the joint parameters (e.g., joint angles) of multiple joints. In some embodiments, the second resistance torque can be calculated based on the joint parameters (e.g., joint angles) and the base unevenness parameter, which will be described in detail in the following some embodiments.

[0044] In some embodiments, at least one resistance torque can include a third resistance torque, and the third resistance torque can be the joint resistance torque caused by the Coriolis force and centripetal force of the robotic arm. In some embodiments, the third resistance torque can be a function of the joint parameters (e.g., joint angles and joint angular velocities) of multiple joints. In some embodiments, the third resistance torque of multiple joints can be calculated based on the joint parameters (e.g., joint angles and joint angular velocities), which will be described in detail in the following some embodiments.

[0045] ​​In some embodiments, at least one resistance torque may include a fourth resistance torque, which may be a joint resistance torque caused by the inertia of the robotic arm, and the fourth resistance torque may be a function of joint parameters of multiple joints (e.g., joint angle and joint angular acceleration). In some embodiments, the joint angular accelerations of multiple joints (e.g., a first estimated value of the joint angular acceleration) may be calculated based on the joint angle and the fourth resistance torque, which will be described in detail in some of the following embodiments. In some embodiments, the fourth resistance torque of multiple joints may also be calculated based on the joint angle and the joint angular acceleration (e.g., a first estimated value of the joint angular acceleration or a second estimated value of the joint angular acceleration), which will be described in detail in some of the following embodiments.

[0046] In some embodiments, the robotic arm further includes a gravity compensation mechanism. In some embodiments, the gravity compensation mechanism is disposed within at least one of the multiple arm bodies (e.g., Figure 3 the third arm body 313 shown), and the gravity compensation mechanism is configured to balance the gravity of the arm bodies proximal to the robotic arm (e.g., the Figure 3 fourth arm body 314, the fifth arm body 315, and the sixth arm body 316 shown). Figure 4 A schematic internal structure diagram of an arm body 400 including a gravity compensation mechanism according to some embodiments of the present disclosure is shown. In some embodiments, as Figure 4 shown, the arm body 400 may include a parallelogram mechanism. The arm body 400 may include a first rod 410, a second rod 420, a third rod 430, and a base 440. The base 440 may be located at the distal end of the arm body 400. The first rod 410 and the second rod 420 are parallel, and the distal ends of the first rod 410 and the second rod 420 are hinged to the base 440. The hinge point of the first rod 410 and the base 440 is the Figure 4 point J1 shown, and the hinge point of the second rod 420 and the base 440 is the Figure 4 point J2 shown. The third rod 430 is respectively hinged to the proximal ends of the first rod 410 and the second rod 420. As Figure 4 shown, the connection line of the hinge points of the base 440 with the first rod 410 and the second rod 420 (see the Figure 4 connection line indicated by 401 shown), the first rod 410, the second rod 420, and the third rod 430 form a parallelogram mechanism.

[0047] In some embodiments, the line 401 connecting the hinge points of the base 440 with the first rod 410 and the second rod 420 is in the vertical direction. In some embodiments, the line 401 is in the vertical direction, for example, it can be perpendicular to the horizontal plane or perpendicular to the ground. In some embodiments, such as when the surgical robot system is located on a ground with a certain inclination, the line 401 is perpendicular to the ground. When the line 401, the first rod 410, the second rod 420, and the third rod 430 form a parallelogram structure, the axis of the third rod 430 is in the vertical direction and can be perpendicular to the horizontal plane or perpendicular to the ground, etc. In some embodiments, the rotation axis of the first rod 410 in the arm body 400 is a straight line perpendicular to the plane where the parallelogram structure formed by the line 401, the first rod 410, the second rod 420, and the third rod 430 is located and passing through the hinge point J1, and the rotation axis of the second rod 420 is a straight line perpendicular to the plane where the parallelogram structure is located and passing through the hinge point J2. Those skilled in the art can understand that rotating the arm body 400 can be understood as rotating the first rod 410 and the second rod 420 around the above rotation axes. When the first rod 410 and the second rod 420 rotate around the above rotation axes, they always maintain the same rotation angle, and the third rod 430 always remains in the vertical direction when the first rod 410 and the second rod 420 rotate.

[0048] In some embodiments, such as Figure 4 As shown, the gravity compensation mechanism includes an elastic balance mechanism, and the elastic balance mechanism includes a support rod 450, a guide block 460, a sliding rod 470, and an elastic member 480. In some embodiments, the two ends of the support rod 450 are respectively hinged to the first rod 410 and the second rod 420 to form hinge points B and D, and the support rod 450 is parallel to the third rod. In some embodiments, when the arm body 400 rotates, the support rod 450 always remains in the vertical direction. The guide block 460 is hinged to the support rod 450. In some embodiments, the hinge point of the guide block 460 and the support rod 450 can be the hinge point C as Figure 4 shown. One end of the sliding rod 470 can form a hinge end with the first rod 410 or the second rod 420. For example, as Figure 4 shown, one end of the sliding rod 470 is hinged to the first rod 410 to form a hinge end A. The other end of the sliding rod 470 forms a free end (for example, Figure 4 the free end N shown). The sliding rod 470 can be slidably connected to the guide block 460. One end of the elastic member 480 is connected to the guide block 460, and the other end is connected to the free end N of the sliding rod 470.

[0049] In some embodiments, among the arm bodies located at the proximal end of the arm body 400 (for example, as Figure 3Under the action of the gravity of the fourth arm body 314, the fifth arm body 315, and the sixth arm body 316 shown, the first rod 410 and the second rod 420 in the arm body 400 tend to rotate counterclockwise (the first rod 410 rotates in the direction indicated by the arrow 402 in Figure 4 and the second rod 420 rotates in the direction indicated by the arrow 403 in Figure 4 ). The included angle between the third rod 430 and the first rod 410 in the arm body 400 (such as the included angle α shown in Figure 4 ) tends to increase. Based on this, the sliding rod 470 slides obliquely upward to the right along the length direction, and the elastic member 480 whose two ends are respectively connected to the guide block 460 and the free end N of the sliding rod 470 is squeezed, generating a restoring force F along the length direction obliquely upward to the right. This restoring force F can balance the gravity of each arm body at the proximal end of the arm body 400, which is beneficial to reducing the inertial influence brought by the gravity of the arm body to the master manipulator. In some embodiments, the elastic member can be a spring, a compression spring, a tension spring, or other suitable elastic members. In some embodiments, parameters such as the elastic coefficient of the elastic member 480 can be determined according to the gravity of each arm body at the proximal end of the arm body 400, so that the restoring force F generated by the elastic member 480 can more accurately balance the gravity of each arm body at the proximal end of the balanceable arm 400, which is beneficial to achieving the effect of "arbitrary stop" when the user releases the master manipulator, and avoiding the master manipulator continuing to move under the influence of gravity and affecting the surgical operation.

[0050] In some embodiments, the gravity compensation mechanism in the robotic arm can balance part of the gravity of the arm body. Therefore, the compensation of the gravity compensation mechanism of the robotic arm can be included in the dynamic model of the robotic arm. In some embodiments, the dynamic model includes at least one compensation torque, and at least one compensation torque can include the gravity compensation torques of the gravity compensation mechanism of the robotic arm for multiple joints. In some embodiments, the gravity compensation torque has a functional relationship with the joint parameters (such as joint angles) of multiple joints. In some embodiments, the gravity compensation torque can be calculated based on the joint parameters (such as joint angles) and the elastic parameters of the elastic member, which will be described in detail in the following some embodiments.

[0051] In some embodiments, the robotic arm includes a linkage mechanism. The linkage mechanism includes at least one active joint and at least one passive joint. At least one active joint is driven by a joint driving device, and at least one active joint is linked with at least one passive joint. For example, as shown in Figure 3 the multi-degree-of-freedom robotic arm 310, the multi-degree-of-freedom robotic arm 310 includes a linkage mechanism. The linkage mechanism includes an active joint (the third joint 3113) and a passive joint (the fourth joint 3114). The third joint 3113 is driven by a joint driving device, and the third joint 3113 is linked with the fourth joint 3114. In some embodiments, the linkage mechanism includes a parallelogram mechanism. For example, in some embodiments, as shown inFigure 3 The shown multi-degree-of-freedom robotic arm 310, the third arm body 313 is a parallelogram structure. The distal end of the third arm body 313 is connected to the second arm body 312 through the third joint 3113, and the proximal end of the third arm body 313 is connected to the fourth arm body 314 through the fourth joint 3114. The third arm body 313, the third joint 3113 and the fourth joint 3114 form a parallelogram linkage mechanism. For example, it can be Figure 4 the arm body 400 containing a parallelogram mechanism in

[0052] In some embodiments, the dynamic drive relationship of the linkage mechanism includes the dynamic drive relationship between at least one active joint and at least one passive joint. For example, as Figure 3 shown in the multi-degree-of-freedom robotic arm 310, the dynamic drive relationship of the linkage mechanism includes the dynamic drive relationship between the third joint 3113 and the fourth joint 3114.

[0053] In some embodiments, the dynamic drive relationship between at least one active joint and at least one passive joint includes at least one of the following I-IV:

[0054] I. The joint angle of at least one active joint and the absolute value of the joint angle of at least one passive joint are the same and the directions are opposite. For example, in the multi-degree-of-freedom robotic arm 310 shown in Figure 3 , the joint angle θ3 of the third joint 3113 and the joint angle θ4 of the fourth joint 3114 have the same absolute value and opposite directions, which can be expressed as: θ4 = -θ3.

[0055] II. The joint angular velocity of at least one active joint and the absolute value of the joint angular velocity of at least one passive joint are the same and the directions are opposite. For example, in the multi-degree-of-freedom robotic arm 310 shown in Figure 3 , the joint angular velocity of the third joint 3113 and the joint angular velocity of the fourth joint 3114 have the same absolute value and opposite directions, which can be expressed as:

[0056] III. The joint angular acceleration of at least one active joint and the absolute value of the joint angular acceleration of at least one passive joint are the same and the directions are opposite. For example, in the multi-degree-of-freedom robotic arm 310 shown in Figure 3 , the joint angular acceleration of the third joint 3113 and the joint angular acceleration of the fourth joint 3114 have the same absolute value and opposite directions, which can be expressed as:

[0057] IV. The balance relationship of the torques of at least one active joint and at least one passive joint, including the balance of the driving torque of at least one active joint with one or more of the resistance torques, at least one compensation torque, and the equivalent external force torque of at least one active joint and at least one passive joint.

[0058] In some embodiments, the resultant torque of some or all of the at least one resistance torque, at least one compensation torque, and the equivalent external force torque of each joint can be regarded as the joint torque corresponding to that joint.

[0059] In some embodiments, a joint drive device is provided in at least one active joint. The balance relationship of the joint torques of at least one active joint and at least one passive joint can be the balance relationship between the driving torque of at least one active joint, the joint torque of at least one active joint, and the joint torque of at least one passive joint. For example, in the multi-degree-of-freedom robotic arm 310 shown as Figure 3 shown, the third joint 3113 is an active joint, the fourth joint 3114 is a passive joint. A joint drive device is provided in the third joint 3113. At least one driving torque of the third joint 3113 includes the driving torque output by the joint drive device, which can be denoted as τ 3，motor , the joint torque of the third joint 3113 can be denoted as τ3, the joint torque of the fourth joint 3114 can be denoted as τ4. The balance relationship of the joint torques of the third joint 3113 and the fourth joint 3114 can be expressed as shown in the following formula (1):

[0060] τ 3，motor = τ3 - τ4 (1)

[0061] Some embodiments of the present disclosure provide a method for constructing a dynamic model of a robotic arm. In some embodiments, the robotic arm includes multiple arm bodies and multiple joints connecting the multiple arm bodies. The method for constructing a dynamic model of the robotic arm may include: obtaining the DH parameters of the multiple joints of the robotic arm based on the mechanical configuration of the robotic arm; and establishing a dynamic model of the robotic arm based on the DH parameters of the multiple joints. In some embodiments, the arm bodies between the multiple joints can be regarded as linkages, and the Newton-Euler method can be used to establish the dynamic model of the arm bodies. The Newton-Euler method can be divided into two steps: First, iterate outward from the distal joint towards the proximal joint direction to calculate the angular velocity, angular acceleration, linear acceleration, centroidal linear acceleration, etc. of each linkage, and then calculate the inertial force and inertial torque of each linkage; then, iterate inward from the proximal joint towards the distal joint direction to calculate the internal force of each linkage, and then calculate the joint torque.

[0062] The following takes the Figure 3 multi-degree-of-freedom robotic arm 310 shown as an example to specifically illustrate the method for constructing a dynamic model of a robotic arm in some embodiments of the present disclosure.

[0063] As described in some of the above embodiments, Figure 3 the multi-degree-of-freedom robotic arm 310 shown can be regarded as a four-bar linkage mechanism, and its operating resistance mainly comes from the first joint 3111, the second joint 3112, and the third joint 3133. Therefore, based on Figure 3 the multi-degree-of-freedom robotic arm 310 shown, a dynamic model of the four-bar linkage mechanism is constructed.

[0064] In some embodiments, based on Figure 3 the mechanical configuration of the multi-degree-of-freedom robotic arm 310 shown, the DH parameters of multiple joints are obtained, as shown in Table 1. Among them, the fourth joint 3114 is the passive joint of the third joint 3113, and its joint angle satisfies θ4 = -θ3.

[0065] Table 1 DH parameters of multiple joints of the multi-degree-of-freedom robotic arm

[0066] i - 1 -> i <![CDATA[α i > <![CDATA[a i > <![CDATA[d i > <![CDATA[θ i > 0->1 0 0 0 <![CDATA[θ1 <!-- 8 -->]]> 1->2 0 <![CDATA[a2]]> 0 <![CDATA[θ2]]> 2->3 π / 2 <![CDATA[a3]]> 0 <![CDATA[θ3]]> 3->4 0 <![CDATA[a 3' > 0 <![CDATA[θ4 = -θ3]]>

[0067] In Table 1, α i is the rotation angle around the axis of the joint coordinate system, a i is the moving distance in the direction of the axis of the joint coordinate system, d i is the moving distance in the direction of the axis of the joint coordinate system, and θ i is the rotation angle around the axis of the joint coordinate system.

[0068] In some embodiments, the dynamic model of the robotic arm includes the gravity offset of the robotic arm. In some embodiments, the robotic arm is installed on a base, and the gravity offset of the robotic arm includes the gravity offset caused by the uneven installation of the base. The base unevenness parameters include the tilt angle vector of the base relative to the reference plane. In some embodiments, when the base of the multi-degree-of-freedom robotic arm 310 is unevenly installed, the base unevenness parameters can be the tilt angle vector of the base relative to the reference plane, which can be expressed as [θ x , θ y T , which has been described in detail in some of the above embodiments and will not be elaborated here.

[0069] In some embodiments, when the base of the multi-degree-of-freedom robotic arm 310 is unevenly installed, Figure 3 the ground coordinate system {W} shown in can be obtained by rotating the master manipulator base coordinate system {0} successively around the axis, and the transformation matrix between the master manipulator base coordinate system {0} and the ground coordinate system {W} can be expressed by the following formula (2):

[0070]

[0071] In some embodiments, as Figure 3 shown, the direction of gravity is direction. Therefore, the direction of gravity relative to the base coordinate system can be expressed by the following formula (3):

[0072] g = [g x g y g z = (Rot x (θ x )Rot y (θ y )) T [0 0 -9.81] T

[0073] = 9.81[cosθ x sinθ y -sinθ x -cosθ x cosθ y T (3)

[0074] In some embodiments, the dynamic model of the four-link mechanism of the multi-degree-of-freedom robotic arm 310 is established using the Newton-Euler method, which mainly includes two steps: outward iteration and inward iteration.

[0075] In some embodiments, starting from the distal link, using the kinematic formula, outward iteration can be performed to obtain the angular velocity i+1 ω i+1 , angular acceleration linear acceleration linear acceleration of the centroid inertial force i+1 F i+1 , and inertial moment i+1 N i+1 of each link in the four-link mechanism of the multi-degree-of-freedom robotic arm 310, which can be expressed by the following formulas (4)-(9) respectively:

[0076]

[0077]

[0078]

[0079]

[0080]

[0081] ​

[0082] Among them, is the rotation matrix, is the joint angular velocity, is the joint angular velocity, is the unit vector along the joint rotation axis, i P i+1 is the translation matrix, i+1 P C，i+1 is the position of the center of mass of the link, m i+1 is the mass of the link, C，i+1 I i+1 is the inertia matrix of the link.

[0083] The initial conditions for outward recursion based on the above formulas (4)-(9) can be expressed as: 0 ω0 = [0 0 0] T and where, [g x g y g z T = g T , and g can be obtained based on formula (3).

[0084] In some embodiments, based on the inertial force i+1 F i+1 and inertial moment i+1 N i+1 of the link obtained by outward iteration, starting from the proximal link, according to the moment and force balance equations, inward iteration can be performed to obtain the forces i f i moments i n i and joint torque τ i (the moment i n i in the axis direction component), which can be expressed by the following formulas (10)-(12) respectively:

[0085]

[0086]

[0087]

[0088] The initial conditions for inward recursion based on the above formulas (10)-(12) can be expressed as: 4 f4 = [0 0 0] T and 4 n4 = [00 0] T .

[0089] In some embodiments, the rotation matrix translation matrix i P i+1 can be obtained according to the DH parameters of multiple joints of the robotic arm. For example, Figure 3 for the multi-degree-of-freedom robotic arm 300 shown, based on the DH parameters of multiple joints of the multi-degree-of-freedom robotic arm shown in Table 1, the corresponding rotation matrix and translation matrix i P i+1 can be obtained through the following formulas (13)-(15):

[0090]

[0091]

[0092]

[0093] In some embodiments, the dynamic model of the robotic arm includes the dynamic drive relationship of the linkage mechanism in the robotic arm. In some embodiments, the robotic arm includes a linkage mechanism. The linkage mechanism includes at least one active joint and at least one passive joint. The at least one active joint is driven by a joint drive device, and the at least one active joint is linked with the at least one passive joint. The dynamic drive relationship of the linkage mechanism includes the dynamic drive relationship between the at least one active joint and the at least one passive joint. Specific details have been described in detail in the above-mentioned some embodiments and will not be elaborated here. In some embodiments, as Figure 3 shown in the multi-degree-of-freedom robotic arm 310, the third joint 3113 is an active joint, and the fourth joint 3114 is a passive joint. The third joint 3113 is linked with the fourth joint 3114. When constructing the dynamic model of the robotic arm, the dynamic drive relationship between the third joint 3113 and the fourth joint 3114 is considered. The dynamic drive relationship between the third joint 3113 and the fourth joint 3114 may include: the absolute values of the joint angle θ3 of the third joint 3113 and the joint angle θ4 of the fourth joint 3114 are the same and the directions are opposite, which can be expressed as: θ4 = -θ3. In some embodiments, the joint angle θ4 of the fourth joint 3114 can be obtained based on the joint angle θ3 of the third joint 3113 to obtain the DH parameter (θ i ) of the fourth joint 3114 of the multi-degree-of-freedom robotic arm 300. The dynamic drive relationship between the third joint 3113 and the fourth joint 3114 may also include: the absolute values of the joint angular velocity of the third joint 3113 and the joint angular velocity of the fourth joint 3114 are the same and the directions are opposite, which can be expressed as: and the joint angular acceleration of the third joint 3113 and the joint angular acceleration The absolute values are the same and the directions are opposite, which can be expressed as: In some embodiments, based on the joint angular velocity of the third joint 3113 Joint angular acceleration Obtain the joint angular velocity of the fourth joint 3114 Joint angular acceleration In some embodiments, based on the obtained joint angular velocity of the fourth joint 3114 Joint angular acceleration Can be used for the outward iteration of formulas (4)-(9) and the inward iteration of formulas (10)-(12) to obtain the dynamic model of the robotic arm.

[0094] In some embodiments, the robotic arm further includes a gravity compensation mechanism, and the dynamic model of the robotic arm includes the compensation of the gravity compensation mechanism of the robotic arm. In some embodiments, the gravity compensation mechanism includes, for example Figure 4 The elastic balance mechanism shown in, which has been described in detail in the above-mentioned some embodiments and will not be elaborated here. The gravity compensation mechanism in the robotic arm can balance part of the gravity of the arm body to achieve gravity compensation. Figure 5 Illustrate according to the present disclosure Figure 4 Schematic diagram of the gravity compensation principle of the arm body 400 including the gravity compensation mechanism in. When Figure 4 The arm body 400 in tends to rotate counterclockwise under the action of the gravity of each arm body located at the proximal end of the arm body 400, the sliding rod 470 slides obliquely upward to the right along the length direction, and the elastic member 480 whose two ends are respectively connected to the guide block 460 and the free end N of the sliding rod 470 is squeezed, and the elastic member pressure can be expressed as Figure 5 The f shown in s , f s Along the length direction of the sliding rod 470, obliquely downward to the left, due to the elastic action of the elastic member, the elastic member generates a restoring force F along the length direction of the sliding rod 470, obliquely upward to the right, F = f s .

[0095] In some embodiments, the elastic member can be a spring, and the elastic member pressure f s Can be obtained by the following formula (16):

[0096]

[0097] Among them, k s Is the spring stiffness coefficient, b vs Is the spring damping coefficient, l AC Is the distance between the hinge end A and the hinge point C.

[0098] In some embodiments, the elastic member pressure f obtained based on formula (16) s, the restoring force F generated by the elastic member can be obtained, and the restoring force F generated by the elastic member will generate a gravity compensation moment on the associated joint (for example, Figure 5 the joint at J2 in Figure 3 the third joint 3113 shown), and this gravity compensation moment is used to compensate for part of the gravity of the arm body. Exemplarily, as shown in Figure 5 , the elastic member generates a gravity compensation moment on the associated joint (for example, Figure 5 the joint at J2 in Figure 3 the third joint 3113 shown) can be obtained by the following formula (17):

[0099]

[0100] where l AB is the distance between the hinge end A and the hinge point B, and l BC is the distance between the hinge point B and the hinge point C. In some embodiments, l AB , l BC are mechanical design values.

[0101] In some embodiments, the dynamic model of the robotic arm can simultaneously consider the dynamic drive relationship of the linkage mechanism in the robotic arm, the gravity offset of the robotic arm, and the compensation of the gravity compensation mechanism of the robotic arm. Therefore, the dynamic model includes the balance of at least one driving torque, at least one resistance torque, at least one compensation torque, and at least one external force equivalent torque of multiple joints. In some embodiments, for the multi-degree-of-freedom robotic arm 300 as shown in Figure 3 , based on the above formulas (4)-(12) and formula (17), the dynamic model of the first three joints (the first joint 3111, the second joint 3112, and the third joint 3113) of the multi-degree-of-freedom robotic arm 300 can be expressed as shown in formula (18):

[0102]

[0103] where τ i，motor is the driving torque of the i-th joint, τ i is the joint torque of the i-th joint, is a 3×18 regression matrix, β r is the corresponding 18×1 regression coefficient vector, τ g is the first resistance torque of multiple joints caused by the gravity of the robotic arm, τ tilt is the second resistance torque of multiple joints caused by the gravity offset of the robotic arm, is a 3×3 Coriolis force and centripetal force matrix, is the third resistance torque of multiple joints caused by the Coriolis force and centripetal force of the robotic arm, and M(θ) is a 3×3 inertia matrix. The fourth resistance torque τ of multiple joints caused by the inertia of the robotic arm spring The gravity compensation torque τ of multiple joints by the gravity compensation mechanism of the robotic arm e The equivalent external force torque of the operating external force of the robotic arm at multiple joints, θ is the joint angle vector of multiple joints, The joint angular velocity vector of multiple joints, The joint angular acceleration vector of multiple joints.

[0104] In the above formula (18), the first resistance torque τ caused by the gravity of the robotic arm g , the second resistance torque τ caused by the gravity offset of the robotic arm tilt and the gravity compensation torque τ of multiple joints by the gravity compensation mechanism of the robotic arm spring are the torque parts that are only related to the joint angle and can be denoted as g(θ), g(θ)=τ g +τ tilt -τ spring . Therefore, the dynamic model of formula (18) can be sorted out as shown in formula (19):

[0105]

[0106] In the above formula (18), β r , τ g , τ tilt , τ e can be respectively expressed by the following formulas (20)-(27):

[0107]

[0108] τ g =[0 0 τ 3g T (21)

[0109] τ tilt =[τ 1，tilt τ 2，tilt τ 3，tilt T (22)

[0110] τ spring =[0 0 τ 3s T (23)

[0111] τ 3g =9.81cosθ3(l 3x +m4a4)-9.81l 3y sinθ3 (24)

[0112] ​​​

[0113]

[0114]

[0115] Among them, I iyy is the moment of inertia in the direction of the i-th joint, I is the moment of inertia in the direction of the i-th joint, I ixx is the moment of inertia in the direction of the i-th joint, I is the moment of inertia in the direction of the i-th joint, I izz is the moment of inertia in the direction of the i-th joint, I is the moment of inertia in the direction of the i-th joint, I ixy is the product of inertia in the direction of the i-th joint, I is the product of inertia in the direction of the i-th joint, I ixz is the product of inertia in the direction of the i-th joint, I is the product of inertia in the direction of the i-th joint, I iyz is the product of inertia in the direction of the i-th joint, I is the product of inertia in the direction of the i-th joint, f vi is the dynamic friction coefficient of the i-th joint, f ci is the static frictional force of the i-th joint, l ix is the angular momentum in the direction of the i-th joint, l iy is the angular momentum in the direction of the i-th joint, l iz is the angular momentum in the direction of the i-th joint, i = 1, 2, 3, 4.

[0116] Those skilled in the art can understand that the dynamic model of the robotic arm in the present disclosure can simultaneously include the dynamic drive relationship of the linkage mechanism in the robotic arm, the gravity offset of the robotic arm, and the compensation of the gravity compensation mechanism of the robotic arm. It can also, according to the specific structure of the robotic arm, include a part of the dynamic drive relationship of the linkage mechanism in the robotic arm, the gravity offset of the robotic arm, and the compensation of the gravity compensation mechanism of the robotic arm. For example, when there is only a linkage mechanism in the robotic arm, the dynamic model of the robotic arm can only include the dynamic drive relationship of the linkage mechanism. Another example is that when there are both gravity offset and the gravity compensation mechanism of the robotic arm, the dynamic model of the robotic arm can include the gravity offset of the robotic arm and the compensation of the gravity compensation mechanism of the robotic arm. Other situations are similar, and the dynamic model of the robotic arm can be obtained according to the specific structure of the robotic arm, and will not be specifically elaborated here.

[0117] The method for estimating the angular acceleration of the robotic arm provided by the present disclosure can accurately, effectively, and with low latency estimate the joint angular velocities of multiple joints of the robotic arm based on the dynamic model of the robotic arm.

[0118] Exemplarily, based on the kinetic model shown in Formula (19), the calculation formula for the joint angular acceleration of multiple joints of the robotic arm can be obtained, which can be expressed as shown in Formula (28):

[0119]

[0120] where M(θ) -1 is the inverse matrix of the inertia matrix.

[0121] The control method of the robotic arm provided by the present disclosure can be based on accurate, effective and low-latency joint acceleration information and the kinetic model to compensate in real time for the operating resistance during the operation (such as dragging) of the robotic arm (for example, Figure 3 the multi-degree-of-freedom robotic arm 310 shown), achieving an operation effect with approximately "zero force" at the end and improving the operator's operation experience.

[0122] In some embodiments, the kinetic model of the robotic arm can be calibrated to obtain an accurate kinetic model. The joint angular acceleration estimation method (for example, Figure 1 the method 100 shown) and / or the control method of the robotic arm (for example, Figure 8 the method 800 shown) provided by some embodiments of the present disclosure are executed based on an accurate kinetic model, which will be described in detail in the following some embodiments.

[0123] Those skilled in the art can understand that the robotic arm to which the acceleration estimation method and the control method of the robotic arm provided by the present disclosure can be applied is not limited to the main operator of the robot system, but also includes any applicable robotic arm. The main operator of the robot system is used for the master-slave teleoperation of the slave operating device of the robot system by the operator.

[0124] Figure 1 The flowchart of the joint angular acceleration estimation method 100 of the robotic arm according to some embodiments of the present disclosure is shown.

[0125] In some embodiments, the robotic arm can be a multi-degree-of-freedom robotic arm (for example, Figure 3 the multi-degree-of-freedom robotic arm 310 shown), and the robotic arm includes multiple arm bodies and multiple joints connecting the multiple arm bodies.

[0126] Referring to Figure 1 , in step 101, at least one driving torque of multiple joints is obtained.

[0127] In some embodiments, a joint driving device is provided in multiple joints of the robotic arm, and the joint driving device is used to drive the joint to rotate around the joint rotation axis (for example, Figure 3 the axis, axis, axis, rotates about the axis, and at least one driving torque includes the driving torque output by the joint driving device, which can be denoted as τ i,motor (where i is the number of the joint). In some embodiments, the joint driving device includes a motor, and the driving torque output by the motor can be measured by a sensor (e.g., an encoder).

[0128] Continue to refer to Figure 1 , in step 103, the joint angles of multiple joints are obtained.

[0129] In some embodiments, angle sensors for obtaining the joint angle θ i (where i is the number of the joint) are provided at multiple joints of the robotic arm. The joint angles of multiple joints can be obtained through the angle sensors. For example, the joint angle θ1 of the first joint, the joint angle θ2 of the second joint, the joint angle θ3 of the third joint, the joint angle θ4 of the fourth joint, the joint angle θ5 of the fifth joint, the joint angle θ6 of the sixth joint, and the joint angle θ7 of the seventh joint are obtained.

[0130] Continue to refer to Figure 1 , in step 105, the joint angular velocities of multiple joints are obtained.

[0131] In some embodiments, angular velocity sensors for obtaining the joint angular velocity (where i is the number of the joint) are provided at multiple joints of the robotic arm. The joint angular velocities of multiple joints can be obtained through the angular velocity sensors. For example, the joint angular velocity of the first joint The joint angular velocity of the second joint The joint angular velocity of the third joint The joint angular velocity of the fourth joint The joint angular velocity of the fifth joint The joint angular velocity of the sixth joint The joint angular velocity of the seventh joint

[0132] In some embodiments, obtaining the joint angular velocities of multiple joints may include calculating the joint angular velocities of multiple joints based on the joint angles of multiple joints. In some embodiments, the joint angle θ i，k sequence (k represents the sampling cycle number) can be obtained, and the joint angular velocity can be calculated by the following formula (29):

[0133]

[0134] where is the joint angular velocity.

[0135] In some embodiments, the robotic arm can be as Figure 3The shown multi-degree-of-freedom robotic arm 310, the operating resistance of the robotic arm mainly comes from the first joint 3111, the second joint 3112, and the third joint 3133. The first three arm bodies (the first arm body 311, the second arm body 312, the third arm body 313) are respectively regarded as the first link, the second link, and the third link connected in sequence through joints. The overall three-axis intersection mechanism of the last three arm bodies (the fourth arm body 314, the fifth arm body 315, the sixth arm body 316) and the last three joints (the fifth joint 3115, the sixth joint 3115, the seventh joint 3117) is regarded as the fourth link. The third link is connected to the fourth link through the fourth joint 3114. Thus, Figure 3 The shown multi-degree-of-freedom robotic arm 310 can be regarded as a four-link mechanism. In some embodiments, obtaining the joint parameters of multiple joints may include obtaining the joint parameters of the first joint, the second joint, the third joint, and the fourth joint. In some embodiments, the joint angle θ1 of the first joint 3111, the joint angle θ2 of the second joint 3112, and the joint angle θ3 of the third joint 3113 can be obtained. In some embodiments, based on the joint angle θ1 of the first joint 3111, the joint angle θ2 of the second joint 3112, the joint angle θ3 of the third joint 3113, and the above formula (29), the joint angular velocity of the first joint 3111 can be calculated The joint angular velocity of the second joint 3112 The joint angular velocity of the third joint 3113 In some embodiments, such as Figure 3 In the shown multi-degree-of-freedom robotic arm 310, the third joint 3113 is the active joint, and the fourth joint 3114 is the passive joint of the third joint 3113. The joint angle, joint angular velocity, and joint angular acceleration of the passive joint can be obtained based on the joint angle, joint angular velocity, and joint angular acceleration of the active joint. In some embodiments, the joint angle of the passive joint has the same absolute value as that of the active joint but in the opposite direction. The joint angle of the driven joint can be obtained based on the joint angle of the active joint. For example, as Figure 3 In [reference], the joint angle θ4 of the fourth joint 3114 = -θ3. In some embodiments, the joint angular velocity of the passive joint has the same absolute value as that of the active joint but in the opposite direction. The joint angular velocity of the driven joint can be obtained based on the joint angular velocity of the active joint. For example, as Figure 3 In [reference], the joint angular velocity of the fourth joint 3114

[0136] Continue to refer to Figure 1, in step 107, based on at least one driving torque, joint angle, joint angular velocity, and the dynamic model of the robotic arm, calculate a first estimate of the joint angular acceleration of multiple joints; wherein, the dynamic model includes at least one of the following: the dynamic driving relationship of the linkage mechanism in the robotic arm, the gravity offset of the robotic arm, or the compensation of the gravity compensation mechanism of the robotic arm. In some embodiments, the dynamic model of the robotic arm includes the balance of at least one driving torque of multiple joints with one or more of at least one resistance torque, at least one compensation torque, and at least one external force equivalent torque. The dynamic model of the robotic arm has been described in detail in the above-mentioned some embodiments and will not be elaborated here.

[0137] In some embodiments, the joint angular acceleration of multiple joints can be estimated based on the dynamic model of the robotic arm. In some embodiments, the dynamic model includes at least one external force equivalent torque. Therefore, based on obtaining at least one driving torque of multiple joints in step 101, at least one external force equivalent torque of multiple joints can be obtained, so that an estimated value of the joint angular acceleration of multiple joints can be calculated based on at least one driving torque, at least one external force equivalent torque, joint angle, joint angular velocity, and the dynamic model.

[0138] In the present disclosure, the momentum of multiple joints of the robotic arm can be expressed by the following formula (30):

[0139]

[0140] Taking the derivative of both sides of formula (30) and according to formula (28), the following formula (31) can be obtained:

[0141]

[0142] Formula (19) satisfies the property of passivity, that is, it satisfies the following equation (32):

[0143]

[0144] According to the passivity property, formula (31) is equivalent to formula (33):

[0145]

[0146] Let:

[0147]

[0148] Then there is the following formula (35):

[0149]

[0150] Performing Laplace transform on formula (35), the following formula (36) can be obtained:

[0151]

[0152] wherein, K GM is a diagonal matrix, and K GM is a pre-calibrated constant matrix.

[0153] Based on Equation (36), it can be known that r(t) is the first-order low-pass filtering result of the external force equivalent torque τ e (t). Therefore, the external force equivalent torque can be estimated by the following Equation (37):

[0154]

[0155] In some embodiments, method 100 may include: calculating at least one external force equivalent torque of multiple joints based on at least one driving torque, joint angles, and joint angular velocities.

[0156] In some embodiments, calculating at least one external force equivalent torque of multiple joints may include: calculating the inertia matrix of multiple joints based on joint angles, and the inertia matrix of multiple joints can be expressed as M(θ). Exemplarily, for the multi-degree-of-freedom robotic arm 300 as Figure 3 shown, the inertia matrix M(θ) of the first three joints is a 3×3 matrix.

[0157] In some embodiments, calculating at least one external force equivalent torque of multiple joints may include: calculating the momentum of multiple joints based on the inertia matrix and joint angular velocities. For example, in some embodiments, the momentum of multiple joints can be calculated based on Equation (30).

[0158] In some embodiments, calculating at least one external force equivalent torque of multiple joints may include: calculating at least one external force equivalent torque of multiple joints based on at least one driving torque, joint angles, joint angular velocities, and the momentum of multiple joints. In some embodiments, r(t) in the above Equation (34) can be calculated based on at least one driving torque, joint angles, joint angular velocities, and the momentum of multiple joints, and then at least one external force equivalent torque of multiple joints can be obtained based on Equation (37).

[0159] In some embodiments, method 100 may include: calculating a first estimated value of the joint angular acceleration of multiple joints based on at least one driving torque, at least one external force equivalent torque, joint angles, joint angular velocities, and a dynamic model.

[0160] Figure 6FIG. 600 (hereinafter also simply referred to as "Method 600") shows a flowchart of a method for calculating a first estimated value of joint angular acceleration according to some embodiments of the present disclosure. Method 600 can be implemented or executed at least in part by hardware, software, or firmware. In some embodiments, Method 600 can be executed at least in part by a robotic system (e.g., Figure 2 the robotic system 200 shown). In some embodiments, Method 600 can be implemented as computer-readable instructions. These instructions can be read and executed by a general-purpose processor or a special-purpose processor (e.g., Figure 2 the control device 230 shown). For example, the control device of the robotic system (e.g., Figure 2 the control device 230 shown) can include a processor configured to execute Method 600. In some embodiments, these instructions can be stored on a computer-readable medium.

[0161] Referring to Figure 6 , in step 601, based on the joint angles, calculate the first partial joint torques of a plurality of joints. The first partial joint torques include one or more of a first resistance torque, a second resistance torque, and at least one compensation torque.

[0162] In some embodiments, the first resistance torque is a function of the joint angles. In some embodiments, the first resistance torque can be a joint resistance torque caused by the gravity of the robotic arm. In some embodiments, Method 600 can include: calculating the first resistance torque of a plurality of joints based on the joint angles. Exemplarily, the first resistance torque of the multi-degree-of-freedom robotic arm 300 shown in Figure 3 can be expressed as τ in Equation (18) g , τ g can be calculated, for example, by Equations (21) and (24).

[0163] In some embodiments, the first partial joint torques can only include the first resistance torque, and the first partial joint torques can be obtained based on the first resistance torque. For example, the first partial joint torque of each joint is equal to the first resistance torque of the corresponding joint.

[0164] In some embodiments, the second resistance torque of a plurality of joints is caused by the gravity offset of the robotic arm. In some embodiments, the robotic arm is mounted on a base, and the gravity offset of the robotic arm includes the gravity offset caused by the uneven installation of the base, which has been described in detail in the above embodiments and will not be elaborated here. In some embodiments, Method 600 can include: obtaining a base unevenness parameter, where the base unevenness parameter includes an inclination angle vector of the base relative to a reference plane; and calculating the second resistance torque based on the joint angles and the base unevenness parameter.

[0165] In some embodiments, the base unevenness parameter includes the tilt angle vector of the base relative to a reference plane. In some embodiments, the plurality of joints includes a first joint, and the robotic arm is connected to the base through the first joint. In some embodiments, the reference plane is a horizontal plane, and the tilt angle vector is the angle between the plane perpendicular to the first rotation axis of the first joint and the reference plane, which can be expressed as [θ x , θ y T , which has been described in detail in some of the above embodiments and will not be elaborated here. Exemplarily, as Figure 3 shown, the second resistance torque of the multi-degree-of-freedom robotic arm 300 can be expressed as τ tilt in formula (18), and τ tilt can be calculated through, for example, formula (22) and formulas (25)-(27).

[0166] In some embodiments, the first partial joint torque may only include the second resistance torque, and the first partial joint torque can be obtained based on the second resistance torque. For example, the first partial joint torque of each joint is equal to the second resistance torque of the corresponding joint.

[0167] In some embodiments, the robotic arm includes a gravity compensation mechanism, and at least one compensation torque includes the gravity compensation torque of the gravity compensation mechanism of the robotic arm for the plurality of joints, which has been described in detail in some of the above embodiments and will not be elaborated here.

[0168] In some embodiments, the gravity compensation mechanism may include an elastic balance mechanism, the elastic balance mechanism includes an elastic member, and method 600 may include: obtaining the elastic parameters of the elastic member; and calculating the gravity compensation torque of the plurality of joints based on the joint angles and the elastic parameters. In some embodiments, the elastic member includes a spring, and the elastic parameters include at least one of a spring stiffness coefficient and a spring damping coefficient. Exemplarily, as Figure 3 shown, the gravity compensation torque of the multi-degree-of-freedom robotic arm 300 can be expressed as τ spring in formula (18), and τ spring can be obtained through, for example, formula (23) and formula (17).

[0169] In some embodiments, the first partial joint torque may only include the gravity compensation torque, and the first partial joint torque can be obtained based on the gravity compensation torque. For example, the first partial joint torque of each joint has the same absolute value as and the opposite direction to the gravity compensation torque of the corresponding joint.

[0170] ​In some embodiments, the first partial joint torque may include a plurality of the first resistance torque, the second resistance torque, and the gravity compensation torque. The first partial joint torque of each joint may be calculated based on a plurality of the first resistance torque, the second resistance torque, and the gravity compensation torque. For example, the first partial joint torque of each joint is equal to the sum of the first resistance torque and the second resistance torque of the corresponding joint, or the first partial joint torque of each joint is equal to the sum of the first resistance torque and the second resistance torque of the corresponding joint minus the gravity compensation torque, or the first partial joint torque of each joint is equal to the first resistance torque or the second resistance torque of the corresponding joint minus the gravity compensation torque.

[0171] Continuing to refer to Figure 6 , in step 603, based on the joint angle and the joint angular velocity, the second partial joint torque of multiple joints is calculated. The second partial joint torque includes the third resistance torque.

[0172] In some embodiments, the third resistance torque has a functional relationship with the joint angle and the joint angular velocity. In some embodiments, the third resistance torque may be the joint resistance torque caused by the Coriolis force and the centripetal force of the robotic arm. In some embodiments, method 600 may include: calculating the third resistance torque of multiple joints based on the joint angle and the joint angular velocity. Exemplarily, as Figure 3 shown, the third resistance torque of the multi-degree-of-freedom robotic arm 300 can be expressed as in

[0173] In some embodiments, the second partial joint torque may include the third resistance torque, and the second partial joint torque may be obtained based on the third resistance torque. For example, the second partial joint torque of each joint is equal to the third resistance torque of the corresponding joint.

[0174] Continuing to refer to Figure 6 , in step 605, based on at least one driving torque, at least one external force equivalent torque, the first partial joint torque, and the second partial joint torque, the third partial joint torque is calculated. The third partial joint torque includes the fourth resistance torque.

[0175] In some embodiments, the fourth resistance torque has a functional relationship with the joint angle and the joint angular acceleration. In some embodiments, the fourth resistance torque may be the joint resistance torque caused by the inertia of the robotic arm. In some embodiments, method 600 may include: calculating the fourth resistance torque of multiple joints based on at least one driving torque, at least one external force equivalent torque, the first partial joint torque, and the second partial joint torque. For example, the fourth resistance torque of each joint is equal to the sum of at least one driving torque and at least one external force equivalent torque of the corresponding joint minus the first partial joint torque and the second partial joint torque. Exemplarily, as Figure 3The fourth resistance torque of the multi-degree-of-freedom robotic arm 300 shown can be expressed as in wherein, is the joint angular acceleration to be estimated.

[0176] In some embodiments, the third partial joint torque may include the fourth resistance torque. For example, the third partial joint torque of each joint is equal to the fourth resistance torque of the corresponding joint.

[0177] Continuing to refer to Figure 6 , in step 607, based on the joint angles and the third partial joint torque, a first estimated value of the joint angular acceleration is calculated.

[0178] In some embodiments, method 600 includes: obtaining an inertia matrix based on the joint angles, and calculating a first estimated value of the joint angular acceleration based on the third partial joint torque and the joint matrix. For example, the first estimated value of the joint angular acceleration is equal to the product of the inverse matrix of the inertia matrix and the third partial joint torque.

[0179] Exemplarily, for the multi-degree-of-freedom robotic arm 300 shown as Figure 3 , based on the dynamic model of formula (19), a first estimated value of the joint angular acceleration of multiple joints can be calculated based on formula (28), and can be expressed as shown in formulas (38) and (39):

[0180]

[0181] g(θ) = τ g +τ tilt -τ spring (39)

[0182] wherein, f * is the first estimated value of the joint angular acceleration, τ motor is the driving torque vector composed of at least one driving torque of multiple joints, is the external force equivalent torque vector composed of the estimated values of at least one external force equivalent torque of multiple joints.

[0183] In some embodiments, due to the influence of joint friction, the accuracy of the dynamic parameters in the dynamic model of the robotic arm is limited, and there are errors in the estimated at least one external force equivalent torque. Therefore, an acceleration observer can be designed to compensate for the time-varying error of the first estimated value of the joint angular acceleration in real time. Therefore, in some embodiments, method 100 may further include: calculating a second estimated value of the joint angular acceleration based on the first estimated value of the joint angular acceleration and the acceleration observer.

[0184] In some embodiments, the acceleration observer may be as shown in the following formula (40):

[0185]

[0186] Among them, are three observed variable vectors, and K1, K2, K3, K4, K5, and K6 are six diagonal matrix coefficients.

[0187] In some embodiments, method 100 includes multiple estimation loops. Method 100 includes: in each estimation loop, calculating a first estimated value of the joint angular acceleration of multiple joints; and calculating a second estimated value of the joint angular acceleration based on the first estimated value of the joint angular acceleration and an acceleration observer.

[0188] In some embodiments, calculating the second estimated value of the joint angular acceleration based on the first estimated value of the joint angular acceleration and an acceleration observer includes: in each estimation loop, obtaining a previous observation error of the joint angular velocity based on the previous observed value of the joint angular velocity of multiple joints in the previous estimation loop and the previous actual value of the joint angular velocity of multiple joints in the previous estimation loop; calculating the current observed value of the joint angular velocity in the current estimation loop based on the previous observation error of the joint angular velocity, the first estimated value of the joint angular acceleration in the current estimation loop, and the acceleration observer; and calculating the second estimated value of the joint angular acceleration based on the current observed value of the joint angular velocity, the first estimated value of the joint angular acceleration in the current estimation loop, and the acceleration observer.

[0189] Exemplarily, taking the acceleration observer shown in formula (40) as an example, the specific process of calculating the second estimated value of the joint angular acceleration is described, including:

[0190] In the t-th estimation loop, steps 1101 to 1105 are executed;

[0191] In step 1101, based on the previous observed value of the joint angular velocity of multiple joints in the (t - 1)-th estimation loop the previous actual value of the joint angular velocity of multiple joints in the (t - 1)-th estimation loop obtain the previous observation error of the joint angular velocity in the (t - 1)-th estimation loop

[0192] In step 1103, based on the previous observation error e1(t - 1) of the joint angular velocity in the (t - 1)-th estimation loop, the first estimated value f * (t) of the joint angular acceleration in the t-th estimation loop, and the acceleration observer, calculate the current observed value of the joint angular velocity in the t-th estimation loop Specifically, step 1103 may include step 11031 and step 11033:

[0193] In step 11031, based on the previous observation error e1(t - 1) of the joint angular velocity in the (t - 1)-th estimation cycle and formula (40), calculate u1(t - 1), u2(t - 1), and u3(t - 1);

[0194] In step 11033, based on u1(t - 1), u2(t - 1), u3(t - 1), the first estimate f * (t) of the joint angular acceleration in the t-th estimation cycle and formula (40), recursively calculate Specifically:

[0195]

[0196]

[0197]

[0198] where T is the period of the estimation cycle;

[0199] In step 1105, based on the current observed value of the joint angular velocity in the t-th estimation cycle the first estimate f * (t) of the joint angular acceleration in the t-th estimation cycle and formula (40), calculate the second estimate of the joint angular acceleration in the t-th estimation cycle, which can be expressed as:

[0200] Iteratively execute the above estimation cycle to continuously update the second estimate of the joint angular acceleration. In the present disclosure, due to at least one external force equivalent moment estimation error and / or calibration error existing in the dynamic parameters in the dynamic model, there is a time-varying error in the first estimate of the joint angular acceleration estimated by the dynamic model. Therefore, the joint angular acceleration is observed and updated by an angular acceleration observer to obtain the second estimate of the joint angular acceleration, which greatly improves the accuracy of the joint angular acceleration estimation.

[0201] In some embodiments, the joint angular acceleration of the robotic arm can be estimated by three different methods. Among them, Method 1 is to use the method in some of the above embodiments to obtain the second estimate of the joint angular acceleration as the estimation result of the joint angular acceleration; Method 2 is to perform differentiation on the joint angular velocity to obtain the estimation result of the joint angular acceleration; Method 3 is to perform filtering on the differentiated joint angular velocity to obtain the estimation result of the joint angular jerk. Figure 7A - Figure 7CA schematic diagram showing the estimated results of the joint angular acceleration of a robotic arm in some embodiments of the present disclosure, where Method 1 to Method 3 correspond to the above. As can be seen from the figure: when the noise is basically the same, the joint angular acceleration estimated by Method 1 has a smaller delay than the joint angular acceleration estimated by Method 3 (the delay is reduced by about 60 ms), and the joint angular acceleration estimated by Method 1 is more real-time.

[0202] Figure 8 A flowchart showing a control method 800 for a robotic arm according to some embodiments of the present disclosure.

[0203] In some embodiments, the robotic arm can be a multi-degree-of-freedom robotic arm (e.g., Figure 3 the multi-degree-of-freedom robotic arm 310 shown), and the robotic arm includes a plurality of arm bodies and a plurality of joints connecting the plurality of arm bodies.

[0204] Referring to Figure 8 , in step 801, joint parameters of a plurality of joints are obtained to form a joint parameter set. In the present disclosure, the joint parameter set refers to a set of joint parameter vectors of a plurality of joints. The joint parameter vector of each joint can include a plurality of joint parameters, such as joint angle, joint angular velocity, or joint angular acceleration.

[0205] In some embodiments, the joint angle and / or the joint angular velocity can be obtained by a method similar to that in the above-mentioned some embodiments, which will not be elaborated here.

[0206] In some embodiments, the joint angular acceleration can be estimated by the method in the above-mentioned some embodiments, for example, the first estimated value of the joint angular acceleration is estimated by the method in the above-mentioned some embodiments or the second estimated value of the joint angular acceleration is estimated by the method in the above-mentioned some embodiments.

[0207] Continuing to refer to Figure 8 , in step 803, based on the joint parameter set and the dynamic model of the robotic arm, control signals for a plurality of joints are calculated. In some embodiments, the dynamic model includes at least one of the following: the dynamic drive relationship of the linkage mechanism in the robotic arm, the gravity offset of the robotic arm, or the compensation of the gravity compensation mechanism of the robotic arm. In some embodiments, the dynamic model of the robotic arm includes the balance of one or more of at least one driving torque and at least one resistance torque, at least one compensation torque, and at least one external force equivalent torque of a plurality of joints. It has been described in detail in the above-mentioned some embodiments and will not be elaborated here.

[0208] In some embodiments, calculating the control signals for a plurality of joints based on the joint parameter set and the dynamic model of the robotic arm includes calculating at least one resistance torque of a plurality of joints based on the joint parameter set and the dynamic model.

[0209] In some embodiments, at least one resistance torque may include one or more of a first resistance torque, a second resistance torque, a third resistance torque, and a fourth resistance torque.

[0210] In some embodiments, the first resistance torque may be the joint resistance torques of multiple joints caused by the gravity of the robotic arm, and the first resistance torque has a functional relationship with the joint angles. In some embodiments, the first resistance torque of multiple joints may be calculated based on the joint angles. Exemplarily, as Figure 3 the first resistance torque of the multi-degree-of-freedom robotic arm 300 shown can be expressed as τ in formula (18) g .

[0211] In some embodiments, the second resistance torque may be the joint resistance torques of multiple joints caused by the gravity offset of the robotic arm, and the second resistance torque has a functional relationship with the joint angles. In some embodiments, the first resistance torque of multiple joints may be calculated based on the joint angles and the base unevenness parameter. Exemplarily, as Figure 3 the second resistance torque of the multi-degree-of-freedom robotic arm 300 shown can be expressed as τ in formula (18) tilt .

[0212] In some embodiments, the third resistance torque may be the joint resistance torque caused by the Coriolis force and centripetal force of the robotic arm, and the third resistance torque has a functional relationship with the joint angles and joint angular velocities. In some embodiments, the third resistance torque of multiple joints may be calculated based on the joint angles and joint angular velocities. In some embodiments, the Coriolis force and centripetal force matrix of multiple joints may be calculated based on the joint angles and joint angular velocities, and the third resistance torque of multiple joints may be calculated based on the Coriolis force and centripetal force matrix of multiple joints and the joint angular velocities. Exemplarily, as Figure 3 the third resistance torque of the multi-degree-of-freedom robotic arm 300 shown can be expressed as

[0213] In some embodiments, the fourth resistance torque may be the joint resistance torque caused by the inertia of the robotic arm, and the fourth resistance torque may have a functional relationship with the joint angles and joint angular accelerations of multiple joints. In some embodiments, the third resistance torque of multiple joints may be calculated based on the joint angles and joint angular accelerations (e.g., the first estimated value of the joint angular acceleration or the second estimated value of the joint angular acceleration). In some embodiments, the inertia matrix of multiple joints may be calculated based on the joint angles, and the third resistance torque of multiple joints may be calculated based on the inertia matrix of multiple joints and the joint angular accelerations (e.g., the first estimated value of the joint angular acceleration or the second estimated value of the joint angular acceleration). Exemplarily, as Figure 3 the fourth resistance torque of the multi-degree-of-freedom robotic arm 300 shown can be expressed as

[0214] In some embodiments, calculating control signals for a plurality of joints based on a set of joint parameters and a dynamic model of the robotic arm includes calculating at least one driving torque for the plurality of joints based on at least one resistance torque. In some embodiments, the driving torque of each joint is equal to the sum of at least one resistance torque of the corresponding joint.

[0215] In some embodiments, calculating control signals for a plurality of joints based on a set of joint parameters and a dynamic model of the robotic arm may further include calculating at least one compensation torque for the plurality of joints based on the set of joint parameters and the dynamic model.

[0216] In some embodiments, at least one compensation torque may include a gravity compensation torque, and the gravity compensation torque has a functional relationship with the joint angles of the plurality of joints. In some embodiments, the gravity compensation torque may be calculated based on the joint angles and the elastic parameters of the elastic members in the gravity compensation mechanism. Exemplarily, as Figure 3 shown, the fourth resistance torque of the multi-degree-of-freedom robotic arm 300 can be expressed as τ in formula (18) spring .

[0217] In some embodiments, calculating control signals for a plurality of joints based on a set of joint parameters and a dynamic model of the robotic arm may further include calculating at least one driving torque for the plurality of joints based on at least one resistance torque and at least one compensation torque.

[0218] In some embodiments, at least one resistance torque may include one or more of the first resistance torque, the second resistance torque, the third resistance torque, and the fourth resistance torque in the above-mentioned some embodiments, and at least one compensation torque includes the gravity compensation torque in the above-mentioned some embodiments. The driving torque of each joint is equal to one resistance torque of the corresponding joint minus the gravity compensation torque, or the driving torque of each joint is equal to the sum of multiple resistance torques of the corresponding joint minus the gravity compensation torque.

[0219] Continuing to refer to Figure 8 In step 805, control signals are sent to the plurality of joints. In some embodiments, the plurality of joints of the robotic arm are provided with joint driving devices, and at least one driving torque corresponding to each joint can be used as the control signal of the joint, and the control signal is sent to the joint driving device to control the movement of the joint driving device.

[0220] In some embodiments, the multiple joints of the robotic arm include a first joint, a second joint, and a third joint. The first rotation axis of the first joint is parallel to the second rotation axis of the second joint, and the third rotation axis of the third joint is perpendicular to the first rotation axis and the second rotation axis. The first joint, the second joint, and the third joint are respectively driven by joint driving devices. In some embodiments, a method similar to that of the above - mentioned embodiments can be used to calculate the driving torque of the joint driving device, and based on the driving torque, control the movement of the joint driving device to drive the first joint, the second joint, and the third joint to move. In some embodiments, the joint driving device includes a motor, and the rotation angle of the motor can be obtained based on the driving torque to control the rotation of the motor, thereby driving the joint to move.

[0221] Some control methods of the robotic arms in the present disclosure are based on the dynamic model of the robotic arm, calculate the joint torques of multiple joints of the robotic arm (including some or all of at least one resistance torque, at least one compensation torque, and at least one external - force equivalent torque), and calculate the driving torque of the joint driving devices in multiple joints based on the joint torques to obtain control signals for the joints. Based on the control signals, control the movement of the joints, and output the driving torque through the joint driving devices to compensate for the joint torques of multiple joints, so that when an operator operates the robotic arm, an approximate "zero - force" operation (such as dragging) effect is achieved, reducing the fatigue of the operation and enhancing the operation experience.

[0222] In some embodiments of the present disclosure, the present disclosure also provides a computer device. The computer device includes a memory and a processor. The memory can be used to store at least one instruction. The processor is coupled to the memory and is used to execute at least one instruction to perform some or all of the steps in the method of the present disclosure, such as Figure 1 some or all of the steps in method 100 disclosed in Figure 6 some or all of the steps in method 600 disclosed in Figure 8 some or all of the steps in method 800 disclosed in

[0223] Figure 9 A schematic block diagram of a computer device 900 according to some embodiments of the present disclosure is shown. Refer to Figure 9 , the computer device 900 may include a central processing unit (CPU) 901, a system memory 904 including a random - access memory (RAM) 902 and a read - only memory (ROM) 903, and a system bus 905 connecting the components. The computer device 900 may also include an input / output system 906 and a mass - storage device 907 for storing an operating system 913, application programs 914, and other program modules 915. The input / output system mainly includes an input / output controller 906 composed of a display 908 and an input device 909.

[0224] The mass storage device 907 is connected to the central processing unit 901 through a mass storage controller (not shown) connected to the system bus 905. The mass storage device 907 or the computer-readable medium provides non-volatile storage for the computer device. The mass storage device 907 may include computer-readable media (not shown) such as a hard disk or a Compact Disc Read-Only Memory (CD-ROM) drive.

[0225] Without loss of generality, the computer-readable medium may include computer storage media and communication media. Computer storage media includes volatile and non-volatile, removable and non-removable media implemented by any method or technology for storing information such as computer-readable instructions, data structures, program modules, or other data. Computer storage media includes RAM, ROM, flash memory or other solid-state storage technologies, CD-ROM, or other optical storage, magnetic tape cartridges, tapes, magnetic disk storage or other magnetic storage devices. Of course, those skilled in the art will appreciate that computer storage media is not limited to the above several. The above system memory and mass storage device may be collectively referred to as memory.

[0226] The computer device 900 may be connected to the network 912 through a network interface unit 911 connected to the system bus 905. The system memory 904 or the mass storage device 907 is also used to store one or more instructions. The central processing unit 901 implements all or part of the steps of the methods in some embodiments of the present disclosure by executing the one or more instructions, such as Figure 1 some or all of the steps in the method 100 disclosed in Figure 6 some or all of the steps in the method 600 disclosed in Figure 8 some or all of the steps in the method 800 disclosed in

[0227] In some embodiments of the present disclosure, the present disclosure also provides a computer-readable storage medium storing at least one instruction, and the at least one instruction is executed by a processor to cause the computer to execute some or all of the steps of the methods in some embodiments of the present disclosure, such as Figure 1 some or all of the steps in the method 100 disclosed in Figure 6 some or all of the steps in the method 600 disclosed in Figure 8Some or all of the steps in the method 800 disclosed in [reference]. Examples of computer-readable storage media include memories for computer programs (instructions), such as read-only memory (ROM), random access memory (RAM), compact disc read-only memory (CD-ROM), magnetic tapes, floppy disks, and optical data storage devices, etc.

[0228] Note that the above are only exemplary embodiments of the present disclosure and the technical principles applied. Those skilled in the art should understand that the present disclosure is not limited to the specific embodiments here, and various obvious changes, re-adjustments, and substitutions can be made by those skilled in the art without departing from the protection scope of the present disclosure. Therefore, although the present disclosure has been described in more detail through the above embodiments, the present disclosure is not limited to the above embodiments only. Without departing from the concept of the present disclosure, more other equivalent embodiments can be included, and the scope of the present disclosure is determined by the scope of the appended claims.

Claims

1. A method for estimating the joint angular acceleration of a robotic arm, characterized in that, The robotic arm includes a plurality of arm bodies and a plurality of joints connecting the plurality of arm bodies, and the method includes: Obtaining at least one driving torque of the plurality of joints; Obtaining the joint angles of the plurality of joints; Obtaining the joint angular velocities of the plurality of joints; and Calculating a first estimate of the joint angular accelerations of the plurality of joints based on the at least one driving torque, the joint angles, the joint angular velocities, and the dynamic model of the robotic arm; Wherein, the dynamic model includes at least one of the following: the dynamic driving relationship of the linkage mechanism in the robotic arm, the gravity offset of the robotic arm, or the compensation of the gravity compensation mechanism of the robotic arm.

2. The method according to claim 1, wherein The method includes: Calculating the joint angular velocities of the plurality of joints based on the joint angles of the plurality of joints.

3. The method according to claim 1, wherein The dynamic model includes the balance of at least one driving torque of the plurality of joints with one or more of at least one resistance torque, at least one compensation torque, and at least one external force equivalent torque.

4. The method according to claim 3, wherein The linkage mechanism includes at least one active joint and at least one passive joint, the at least one active joint is driven by a joint driving device, and the at least one active joint is linked with the at least one passive joint; The dynamic driving relationship of the linkage mechanism includes the dynamic driving relationship between the at least one active joint and the at least one passive joint.

5. The method according to claim 4, wherein The linkage mechanism includes a parallelogram mechanism, and the dynamic driving relationship between the at least one active joint and the at least one passive joint includes at least one of the following: The angle of the at least one active joint has the same absolute value as the angle of the at least one passive joint but opposite in direction; The angular velocity of the at least one active joint has the same absolute value as the angular velocity of the at least one passive joint but opposite in direction; The angular acceleration of the at least one active joint has the same absolute value as the angular acceleration of the at least one passive joint but opposite in direction; The balance relationship of the joint torques of the at least one active joint and the at least one passive joint includes the balance of the driving torque of the at least one active joint with one or more of at least one resistance torque, at least one compensation torque, and at least one external force equivalent torque of the at least one active joint and the at least one passive joint.

6. The method according to claim 3, wherein The method includes: Calculating at least one external force equivalent torque of the plurality of joints based on the at least one driving torque, the joint angles, and the joint angular velocities; and Calculating a first estimate of the joint angular accelerations of the plurality of joints based on the at least one driving torque, the at least one external force equivalent torque, the joint angles, the joint angular velocities, and the dynamic model.

7. The method according to claim 6, wherein The method includes: Calculating the inertia matrix of the plurality of joints based on the joint angles; Calculating the momentum of the plurality of joints based on the inertia matrix and the joint angular velocities; and Calculating at least one external force equivalent torque of the plurality of joints based on the at least one driving torque, the joint angles, the joint angular velocities, and the momentum of the plurality of joints.

8. The method according to claim 6, characterized in that, The method includes: Calculating a first partial joint torque of the plurality of joints based on the joint angles; Calculate second partial joint torques of the plurality of joints based on the joint angles and the joint angular velocities; Calculate third partial joint torques based on the at least one driving torque, the at least one external force equivalent torque, the first partial joint torques, and the second partial joint torques; and Calculate a first estimated value of the joint angular acceleration based on the joint angles and the third partial joint torques; Wherein, the first partial joint torques include one or more of a first resistance torque, a second resistance torque, and the at least one compensation torque, the second partial joint torques include a third resistance torque, and the third partial joint torques include a fourth resistance torque.

9. The method according to claim 8, wherein The first resistance torque has a functional relationship with the joint angles, and the first partial joint torques include the first resistance torque; The method includes: calculating the first resistance torque of the plurality of joints based on the joint angles.

10. The method according to claim 8, characterized in that, The second resistance torque includes the resistance torque caused by the gravity offset of the robotic arm, the second resistance torque has a functional relationship with the joint angles, and the first partial joint torques include the second resistance torque.

11. The method according to claim 10, wherein The robotic arm is mounted on a base, and the gravity offset of the robotic arm includes the gravity offset caused by uneven installation of the base. The method further includes: Obtaining base unevenness parameters, where the base unevenness parameters include an inclination angle vector of the base relative to a reference plane; and Calculating the second resistance torque of the plurality of joints based on the joint angles and the base unevenness parameters.

12. The method according to claim 11, wherein The plurality of joints include a first joint, and the robotic arm is connected to the base through the first joint; The reference plane is a horizontal plane, and the inclination angle vector is the angle between the plane perpendicular to the rotation axis of the first joint and the reference plane.

13. The method according to claim 8, characterized in that, The at least one compensation torque includes the gravity compensation torque of the gravity compensation mechanism of the robotic arm for the plurality of joints, the gravity compensation torque has a functional relationship with the joint angles, and the first partial joint torques include the gravity compensation torque.

14. The method according to claim 13, wherein The gravity compensation mechanism includes an elastic balance mechanism, and the elastic balance mechanism includes an elastic member. The method includes: Obtaining elastic parameters of the elastic member; and Calculating the gravity compensation torque of the plurality of joints based on the joint angles and the elastic parameters.

15. The method according to claim 8, wherein The third resistance torque has a functional relationship with the joint angles and the joint angular velocities; The method includes: calculating the third resistance torque of the plurality of joints based on the joint angles and the joint angular velocities.

16. The method according to claim 8, wherein The fourth resistance torque has a functional relationship with the joint angles and the joint angular accelerations; The method includes: calculating the fourth resistance torque of the plurality of joints based on the at least one driving torque, the at least one external force equivalent torque, the first partial joint torques, and the second partial joint torques.

17. The method according to claim 1, wherein The method further includes: Calculating a second estimated value of the joint angular acceleration based on the first estimated value of the joint angular acceleration and an acceleration observer.

18. The method according to claim 17, wherein The method includes a plurality of estimation cycles. The method includes: in each estimation cycle, Calculate a first estimate of the joint angular accelerations of the plurality of joints; and Based on the first estimate of the joint angular accelerations and an acceleration observer, calculate a second estimate of the joint angular accelerations.

19. The method according to claim 18, characterized in that, Calculating a second estimate of the joint angular accelerations based on the first estimate of the joint angular accelerations and an acceleration observer includes: in each estimation cycle, Obtain a previous observation error of the joint angular velocities based on the previous observed values of the joint angular velocities of the plurality of joints in the previous estimation cycle and the previous actual values of the joint angular velocities of the plurality of joints in the previous estimation cycle; Based on the previous observation error of the joint angular velocities, the first estimate of the joint angular accelerations in the current estimation cycle, and the acceleration observer, calculate a current observed value of the joint angular velocities in the current estimation cycle; and Based on the current observed value of the joint angular velocities, the first estimate of the joint angular accelerations in the current estimation cycle, and the acceleration observer, calculate a second estimate of the joint angular accelerations.

20. A control method for a robotic arm, characterized in that, The robotic arm includes a plurality of arm bodies and a plurality of joints of the plurality of arm bodies, and the control method includes: Obtain the joint parameters of the plurality of joints to form a set of joint parameters; the joint parameters include joint angles, joint angular velocities, and joint angular accelerations; the joint angular accelerations are calculated based on the method according to any one of claims 1-19; Based on the set of joint parameters and the dynamic model of the robotic arm, calculate control signals for the plurality of joints; and Send the control signals to the plurality of joints.

21. The method according to claim 20, wherein The control method further includes: controlling the movement of the plurality of joints based on the control signals.

22. A robot system, characterized in that, Includes: At least one robotic arm, the at least one robotic arm including a plurality of arm bodies and a plurality of joints connecting the plurality of arm bodies; And A control device, connected to the at least one robotic arm, for executing the method according to any one of claims 1-19 or the method according to any one of claims 20-21.

23. A computer device, characterized in that, The computer device includes: A memory for storing at least one instruction; and A processor, coupled to the memory and for executing the at least one instruction to execute the method according to any one of claims 1-19 or the method according to any one of claims 20-21.

24. A computer-readable storage medium for storing at least one instruction, characterized in that, When the at least one instruction is executed by the computer device, the computer is caused to implement the method according to any one of claims 1-19 or the method according to any one of claims 20-21.

Images