Method and device for restraining self-motion of mechanical arm in null space

Abstract

The invention provides a method for restraining self-motion of a mechanical arm in a null space. The method comprises the steps that the mechanical arm is controlled to be in a zero-force dragging mode; a plane formed by a shoulder joint, a wrist joint and an elbow joint of the mechanical arm is set as an arm plane; setting a reference plane, and setting an angle between the arm plane and the reference plane as an arm angle; a virtual wall surface is set, and the reference plane, the arm plane and the virtual wall surface intersect on the same straight line; when the mechanical arm moves, the arm angle changes in the first direction, and the arm plane is made to be close to the virtual wall face. And when the arm plane coincides with the virtual wall surface, the movement of a shoulder joint in the arm plane of the mechanical arm is restrained so as to hinder the movement of the arm plane when crossing the virtual wall surface in the first direction and after crossing the virtual wall surface. The invention further provides a device for restraining the self-motion of the mechanical arm in the null space, electronic equipment and a readable storage medium.

Description

Technical Field

[0001] The present invention relates to a method, a device, an electronic device and a readable storage medium for constraining the self-motion of a robot arm in zero space, and belongs to the technical field of robots. Background Art

[0002] With the continuous development of engineering technology, the application of robotic arms is becoming more and more extensive, and the tasks that robotic arms need to complete are becoming more and more complex. At present, industrial robotic arms have been maturely applied in industrial production, and can replace humans to complete tasks such as grasping, gluing, and painting. In order to improve the ability of human-machine collaboration of robotic arms, seven-degree-of-freedom collaborative robotic arms are increasingly attracting attention from the industry. Seven-degree-of-freedom collaborative robotic arms can achieve performance indicators such as avoiding singularities, avoiding obstacles, and optimizing joint torque.

[0003] There will be an infinite number of solutions when solving the inverse solution of a seven-degree-of-freedom manipulator, so a virtual parameter arm angle needs to be introduced in the process of solving the inverse solution. The arm angle describes the angle between the triangle formed by the elbow, wrist, and shoulder of the manipulator and the reference plane. For SRS-configured manipulators, the inverse solution calculated using the arm angle is already quite mature. For non-SRS-configured manipulators with offsets at the elbow and wrist, the wrist offset can be simplified and constructed into an SRS type before solving. The self-motion characteristics of the seven-degree-of-freedom redundant manipulator are parameterized by the arm angle, that is, if the manipulator has self-motion, its arm plane will rotate accordingly, and the arm angle will also change accordingly.

[0004] In order to improve the safety of humans during collaborative work, collaborative robotic arms will add a virtual wall function to ensure that the robotic arm works in a specific workspace and will not pass through the virtual wall to threaten personal safety. In the prior art, the virtual wall generally constrains the end flange or tool of the robotic arm in Cartesian space. By restricting the three directions of the Cartesian coordinate system, the robotic arm workspace is constrained within a certain range. In the zero-force drag mode, the seven-degree-of-freedom robotic arm will exhibit its self-motion characteristics, which will affect the user's use of the robotic arm. Summary of the invention

[0005] In order to solve one of the above technical problems, the present disclosure provides a method, a device, an electronic device and a readable storage medium for constraining the self-motion of a robot arm in zero space.

[0006] According to one aspect of the present disclosure, a method for constraining the self-motion of a robot arm in null space is provided, comprising: Control the robot arm in zero-force drag mode; The plane formed by the shoulder joint, wrist joint and elbow joint of the robot arm is set as the arm plane; the reference plane is set, and the angle between the arm plane and the reference plane is set as the arm angle; the virtual wall is set, wherein the reference plane, the arm plane and the virtual wall intersect on the same straight line; When the robotic arm moves, the arm angle changes along a first direction and makes the arm plane approach the virtual wall; when the arm plane coincides with the virtual wall, the movement of the shoulder joint in the arm plane of the robotic arm is constrained so as to hinder the movement of the arm plane when crossing the virtual wall in the first direction and after crossing the virtual wall.

[0007] According to a method for constraining the self-motion of a robotic arm in zero space according to at least one embodiment of the present disclosure, a preset arm angle threshold range is set based on the arm angle, and a plane corresponding to the preset arm angle threshold range is set as a virtual wall.

[0008] According to a method for constraining the self-motion of a robotic arm in zero space of at least one embodiment of the present disclosure, when the arm angle reaches a preset arm angle threshold range, based on the gravity term, friction term and resistance factor of each joint of the robotic arm, the required driving torque of each joint and the resistance torque to prevent the arm angle from changing along a first direction are calculated, and the driving torque and resistance torque are applied to the joints.

[0009] According to a method for constraining the self-motion of a robotic arm in zero space of at least one embodiment of the present disclosure, when the forearm angle is within a preset arm angle threshold range, the resistance factor is 0; when the forearm angle is outside the preset arm angle threshold range, the resistance factor is calculated; if the calculated resistance factor is greater than a first preset threshold, the resistance factor is taken as the first preset threshold; if the calculated resistance factor is less than a second preset threshold, the resistance factor is taken as the second preset threshold; if the calculated resistance factor is between the first preset threshold and the second preset threshold, the resistance factor remains unchanged.

[0010] According to the method for constraining the self-motion of a robot arm in null space of at least one embodiment of the present disclosure, the first preset threshold is greater than the second preset threshold.

[0011] According to the method for constraining the self-motion of a robot arm in zero space of at least one embodiment of the present disclosure, the first preset threshold is greater than 0; and / or the second preset threshold is less than 0.

[0012] According to another aspect of the present disclosure, there is provided a device for constraining the self-motion of a robot arm in zero space, comprising: A control module, the control module is used to control the robot arm in a zero-force drag mode; and The parameter setting module sets the plane formed by the shoulder joint, wrist joint and elbow joint of the robot arm as the arm plane; sets the reference plane, and sets the angle between the arm plane and the reference plane as the arm angle; sets the virtual wall, wherein the reference plane, the arm plane and the virtual wall intersect on the same straight line; In which, when the robotic arm moves, the arm angle changes along a first direction and makes the arm plane approach a virtual wall; when the arm plane coincides with the virtual wall, the control module is also used to constrain the movement of the shoulder joint in the arm plane of the robotic arm so as to hinder the movement of the arm plane in the first direction when it crosses the virtual wall and after crossing the virtual wall.

[0013] According to another aspect of the present disclosure, there is provided an electronic device, comprising: a memory storing execution instructions; and A processor executes the execution instructions stored in the memory, so that the processor executes the above method.

[0014] According to another aspect of the present disclosure, a readable storage medium is provided, wherein the readable storage medium stores execution instructions, and the execution instructions are used to implement the above method when executed by a processor. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] The accompanying drawings illustrate exemplary embodiments of the present disclosure and together with the description serve to explain the principles of the present disclosure. These drawings are included to provide a further understanding of the present disclosure and are incorporated in and constitute a part of this specification.

[0016] Figure 1 It is a flowchart of a method for constraining the self-motion of a robot arm in zero space according to an embodiment of the present disclosure.

[0017] Figure 2 It is a schematic diagram of an arm plane, a reference plane, a virtual wall and an arm angle according to an embodiment of the present disclosure.

[0018] Figure 3 It is a schematic diagram of an arm plane with an offset robotic arm according to one embodiment of the present disclosure.

[0019] Figure 4 It is a schematic diagram of the structure of a device for constraining the self-motion of a robot arm in zero space according to an embodiment of the present disclosure. DETAILED DESCRIPTION

[0020] The present disclosure is further described in detail below in conjunction with the accompanying drawings and implementations. It is understood that the specific implementations described herein are only used to explain the relevant content, rather than to limit the present disclosure. It should also be noted that, for ease of description, only the parts related to the present disclosure are shown in the accompanying drawings.

[0021] It should be noted that, in the absence of conflict, the embodiments and features of the embodiments in the present disclosure can be combined with each other. The technical solution of the present disclosure will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0022] Unless otherwise specified, the exemplary embodiments / embodiments shown will be understood as providing exemplary features of various details of some ways in which the technical concept of the present disclosure can be implemented in practice. Therefore, unless otherwise specified, the features of the various embodiments / embodiments can be combined, separated, interchanged and / or rearranged without departing from the technical concept of the present disclosure.

[0023] The use of cross-hatching and / or shading in the accompanying drawings is generally used to make the boundaries between adjacent components clear. As such, unless otherwise specified, the presence or absence of cross-hatching or shading does not convey or indicate any preference or requirement for the specific materials, material properties, dimensions, proportions, commonalities between the components shown, and / or any other characteristics, attributes, properties, etc. of the components. In addition, in the accompanying drawings, the sizes and relative sizes of the components may be exaggerated for clarity and / or descriptive purposes. When the exemplary embodiments can be implemented differently, the specific process sequence can be performed in a different order than described. For example, two successively described processes can be performed substantially simultaneously or in an order opposite to the described order. In addition, the same figure numbers represent the same components.

[0024] When a component is referred to as being "on" or "over," "connected to," or "coupled to" another component, the component may be directly on, directly connected to, or directly coupled to the other component, or intervening components may be present. However, when a component is referred to as being "directly on," "directly connected to," or "directly coupled to" another component, there are no intervening components. For this purpose, the term "connected" may refer to a physical connection, an electrical connection, etc., with or without intervening components.

[0025] For descriptive purposes, the present disclosure may use spatially relative terms such as "under," "beneath," "under," "down," "over," "upper," "above," "higher," and "side (e.g., as in "sidewall")," to describe the relationship of one component to another (other) component as shown in the accompanying drawings. The spatially relative terms are intended to encompass different orientations of the device in use, operation, and / or manufacture in addition to the orientation depicted in the accompanying drawings. For example, if the device in the accompanying drawings is turned over, components described as "under" or "beneath" other components or features would subsequently be positioned "over" the other components or features. Thus, the exemplary term "under" can encompass both the "above" and "below" orientations. Furthermore, the device may be otherwise oriented (e.g., rotated 90 degrees or at other orientations), and the spatially relative descriptors used herein should be interpreted accordingly.

[0026] The terms used here are for the purpose of describing specific embodiments, and are not intended to be restrictive. As used here, unless the context clearly indicates otherwise, the singular forms "one (kind, person)" and "said (the)" are also intended to include plural forms. In addition, when the terms "comprise" and / or "include" and their variations are used in this specification, it is explained that there are stated features, integral bodies, steps, operations, parts, assemblies and / or their groups, but it is not excluded that there are or add one or more other features, integral bodies, steps, operations, parts, assemblies and / or their groups. It should also be noted that, as used here, the terms "substantially", "approximately" and other similar terms are used as approximate terms and not as degree terms, so that they are used to explain the inherent deviations of the measured values, calculated values ​​and / or the values ​​provided that will be recognized by those of ordinary skill in the art.

[0027] Figure 1 It is a flowchart of a method for constraining the self-motion of a robot arm in zero space according to an embodiment of the present disclosure.

[0028] like Figure 1As shown, a method S100 for constraining the self-motion of a robot arm in zero space includes: S102, controlling the robot arm in a zero-force dragging mode; S104, setting a plane formed by the shoulder joint, wrist joint and elbow joint of the robot arm as an arm plane; setting a reference plane, and setting the angle between the arm plane and the reference plane as an arm angle; setting a virtual wall, wherein the reference plane, the arm plane and the virtual wall intersect on the same straight line; and S106, when the robot arm moves, the arm angle changes along a first direction, and the arm plane approaches the virtual wall; when the arm plane coincides with the virtual wall, constraining the movement of the shoulder joint in the arm plane of the robot arm to hinder the movement of the arm plane in the first direction when it crosses the virtual wall and after it crosses the virtual wall.

[0029] In the present disclosure, when the robot arm has a null space, it generally has more than six degrees of freedom and is formed as a redundant robot arm. At this time, when the posture of the end of the robot arm remains unchanged, each joint of the robot arm can have multiple relative postures, that is, the movement of these joints can be formed into a null space self-motion.

[0030] The zero-force drag mode is a control type of the robot arm when it is in a teaching or other working state. The method of constraining the self-motion of the robot arm in zero space disclosed in the present invention is a method applied in the zero-force drag mode of the robot arm.

[0031] In the present disclosure, the plane where the shoulder joint, wrist joint and elbow joint of the robot arm are located can be used as the arm plane, that is, the plane formed by the coordinate points of the 2, 4, and 6 joints of the robot arm is set as the arm plane.

[0032] For a 7-DOF manipulator of any configuration, including an SRS configuration and a non-SRS configuration, the plane formed by the coordinate points of the second joint, the fourth joint and the sixth joint of the manipulator can be set as the arm plane.

[0033] A reference plane is set, and the angle between the arm plane and the reference plane is set as the arm angle. In the present disclosure, when the robot arm moves, the arm angle changes with the arm plane. That is, at this time, the reference plane is a plane with a fixed position, and preferably, the reference plane is a vertical plane.

[0034] For example, when the plane where the shoulder, elbow and wrist of the robot arm are located is used as the arm plane, the reference plane is a vertical plane passing through the shoulder and wrist of the robot arm. At this time, the straight line where the reference plane intersects with the arm plane passes through the wrist and shoulder of the robot arm.

[0035] In the present disclosure, a preset arm angle threshold range is set based on the arm angle, and the plane corresponding to the preset arm angle threshold range is set as a virtual wall. At this time, the reference plane, the arm plane and the virtual wall intersect on the same straight line, which passes through the wrist and shoulder of the robotic arm.

[0036] Figure 2 It is a schematic diagram of an arm plane, a reference plane, a virtual wall and an arm angle according to an embodiment of the present disclosure.

[0037] Figure 3 It is a schematic diagram of an arm plane with an offset robotic arm according to one embodiment of the present disclosure.

[0038] like Figure 3 As shown, for any configuration of seven-DOF manipulator, its offset configuration can be simplified. The specific simplified form is as follows Figure 3 As shown in , axis O2 can be regarded as shoulder Ps, axis O4 as elbow Pe, and axis O6 as wrist Pw. d3, a4, a5, d5, a6, a7, d7 represent the DH parameters of the third, fourth, fifth, sixth, and seventh joints.

[0039] In the present disclosure, a specific implementation of the arm angle calculation includes the following steps: The position of each joint coordinate point of the robot arm is obtained by the forward kinematics calculation method. The specific calculation method is as follows.

[0040] , , in, Indicates the method of calculating the pose information of the i-th joint through the i-1th joint, where i is the joint number. For example, , which means that the posture information of the seventh joint is calculated by the posture information of the first to sixth joints. The calculation method of the posture information of other joints is similar.

[0041] , Among them, R represents the rotation matrix of the joint, and P represents the position matrix of the joint.

[0042] Next, the arm angle is calculated based on the positions of the joint coordinate points.

[0043] like Figure 3 As shown, , , the second joint coordinate system is used as the base coordinate system, and V is used as the vertical unit vector of the base coordinate system. Define the reference plane unit vector ,in , projecting Lse onto Lsw yields , , is the unit vector of Lsw. The perpendicular distance vector from point Pe to Lsw . , is the unit vector of Lse, , is the unit vector of P.

[0044] Arm angle tangent formula , the arm angle calculation formula is as follows: .

[0045] Typically, the vertical direction is selected as the reference plane, that is, the plane formed by V(z) and (Y) shown in the figure is used as the reference plane, and the calculated arm angle is used as a parameter for setting the virtual wall.

[0046] in The results can be obtained in the arm angle calculation process, that is, through the calculation method of the above steps. The conversion matrix is ​​calculated according to the DH parameters, and the position P of each axis relative to the base can be calculated according to the following formula, which can also be used to calculate the expression of the above parameter vector.

[0047] When the robot arm moves, the arm angle changes along a first direction, and the first direction can be Figure 2 In the present disclosure, the arm angle may also change in the opposite direction of the first direction, and in this case, virtual walls may be set on both sides of the arm plane. The following description is only for the case where a virtual wall is set on one side of the arm plane. The case where virtual walls are set on both sides of the arm plane is similar to the case where a virtual wall is set on one side of the arm plane, and will not be repeated here.

[0048] When the arm angle changes, it approaches the virtual wall. When the arm plane coincides with the virtual wall, the movement of the shoulder joint in the arm plane of the robot arm is constrained to hinder the movement of the arm plane when and after crossing the virtual wall in the first direction.

[0049] That is to say, when the robot arm is moving, after it passes over the virtual wall, a resistance torque will be generated through the control of the joints, making it difficult for the robot arm to move further. In the present disclosure, the virtual wall is not a rigid wall, but a flexible wall that can be passed over. At this time, the movement of the arm plane when passing over the virtual wall in the first direction and after passing over the virtual wall is hindered, that is, the arm plane can pass over the virtual wall, but due to the existence of the resistance torque, after passing over the virtual wall, a larger force / torque is required to drive the robot arm to move further.

[0050] like Figure 2 As shown, the preset arm angle threshold range can be [ ], accordingly, in and Two virtual walls are set up.

[0051] When the arm angle reaches a preset arm angle threshold range, based on the gravity term, friction term and resistance factor of each joint of the robotic arm, the required driving torque of each joint and the resistance torque to prevent the arm angle from changing along the first direction are calculated, and the driving torque and resistance torque are applied to the joints.

[0052] Specifically, for Figure 2 and Figure 3 The dynamic model of the robot arm shown can be simplified as follows: In the robot arm dynamics model, the meanings of each expression are as follows: is the joint driving torque, is the inertia matrix, is the Coriolis force related term, is the gravity term, and f is the friction term.

[0053] In zero-force drag mode, you only need to set the gravity term The smoothness of dragging can be ensured by compensating the friction term f.

[0054] When the arm angle rotates to the virtual wall, a resistance perpendicular to the arm plane needs to be added and joint resistance torque To prevent the arm angle from continuing to change (rotate), Therefore, during zero-force dragging, the joint torque needs to be added as shown below Therefore, the specific calculation method of the torque applied to each joint is as follows: in, represents the resisting moment of each axis, , the resistance applied to each axis is calculated through the Jacobian matrix J Converted into the resistance torque of the corresponding joint. represents the gravity term. C represents the drag factor. Represents the resistance applied to each joint, which can be set to a fixed value during initialization.

[0055] in, Represents the resistance torque, which can be calculated by the joint impedance control principle to equate the robot arm joint to a spring-mass-damper system.

[0056] Resistance torque In practical applications, the resistance moment It can be simplified as follows: Due to the existence of resistance factor, Simplified to a fixed value.

[0057] Based on the buffer arm angle interval , Current arm angle , preset arm angle threshold range Calculate the drag factor C; when the current arm angle is within the preset arm angle threshold range Internal time (i.e. when the current arm angle is greater than or equal to , less than or equal to ), the resistance factor is 0, that is, no additional resistance is generated, that is, no resisting torque is applied.

[0058] When the current arm angle is outside the preset arm angle threshold range, the drag factor is calculated; if the current arm angle is greater than hour, ; If the current arm angle is less than hour, .

[0059] If the calculated resistance factor is greater than the first preset threshold, the resistance factor is taken as the first preset threshold; if the calculated resistance factor is less than the second preset threshold, the resistance factor is taken as the second preset threshold; if the calculated resistance factor is between the first preset threshold and the second preset threshold (including the calculated resistance factor being equal to the first preset threshold or equal to the second preset threshold), the resistance factor remains unchanged (the current calculated value).

[0060] In a preferred embodiment, the first preset threshold is greater than the second preset threshold. More preferably, the absolute values ​​of the first preset threshold and the second preset threshold are the same.

[0061] In the present disclosure, the first preset threshold is greater than 0 and less than or equal to 3; for example, the first preset threshold may be 1; accordingly, the second preset threshold is greater than or equal to -3 and less than 0, for example, the second preset threshold may be -1.

[0062] In the present disclosure, the preset arm angle threshold range is a constant representing the arm angle size, and the buffer arm angle interval represents the arm angle virtual wall buffer zone, that is, the virtual wall thickness.

[0063] In zero-force dragging and self-motion control, the change of the arm angle of the seven-degree-of-freedom robot arm is mainly determined by joints 2 and 3, so the change of the arm angle can be limited by limiting the rotation of joints 2 and 3.

[0064] At this point, we can get the resistance torque given to the joint when the arm angle rotates to the range of the virtual wall. When the arm angle of the robot arm rotates to the plane of the virtual wall, the resistance torque is applied to joints 2 and 3 to limit the arm angle and ensure its posture stability during dragging and movement.

[0065] The method for constraining the zero-space self-motion of a redundant robotic arm in a zero-force dragging mode provided by the present disclosure has the following technical advantages. Compared with the prior art, which adds a Cartesian space virtual wall based on the end of the robotic arm to limit the end workspace in order to ensure user safety, the method provided by the present disclosure, based on the virtual wall technology set by the arm angle, can further limit the posture of the redundant robotic arm within the arm plane range according to its self-motion characteristics, while enhancing the safety of user operation.

[0066] Figure 4 It is a schematic diagram of the structure of a device for constraining the self-motion of a robot arm in zero space according to an embodiment of the present disclosure.

[0067] like Figure 4 As shown, the device 1000 for constraining the self-motion of a robot arm in zero space includes a control module 1002 and a parameter setting module 1004, etc.

[0068] Among them, the control module 1002 is used to control the robotic arm in zero-force drag mode; and the parameter setting module 1004 is used to set the plane formed by the shoulder joint, wrist joint and elbow joint of the robotic arm as the arm plane; set the reference plane, and set the angle between the arm plane and the reference plane as the arm angle; set the virtual wall, wherein the reference plane, the arm plane and the virtual wall intersect on the same straight line; wherein, when the robotic arm moves, the arm angle changes along a first direction, and makes the arm plane approach the virtual wall; when the arm plane coincides with the virtual wall, the control module is also used to constrain the movement of the shoulder joint in the arm plane of the robotic arm, so as to hinder the movement of the arm plane when and after crossing the virtual wall in the first direction.

[0069] In the present disclosure, when the robot arm has a null space, it generally has more than six degrees of freedom and is formed as a redundant robot arm. At this time, when the posture of the end of the robot arm remains unchanged, each joint of the robot arm can have multiple relative postures, that is, the movement of these joints can be formed into a null space self-motion.

[0070] The zero-force drag mode is a control type of the robot arm when it is in a teaching or other working state. The method of constraining the self-motion of the robot arm in zero space disclosed in the present invention is a method applied in the zero-force drag mode of the robot arm.

[0071] In the present disclosure, the plane formed by the shoulder joint, wrist joint and elbow joint of the robot arm can be set as the arm plane. Preferably, the plane formed by the coordinate points of the shoulder, elbow and wrist of the robot arm is set as the arm plane.

[0072] For a 7-DOF manipulator of any configuration, including an SRS configuration and a non-SRS configuration, the plane formed by the coordinate points of the second joint, the fourth joint and the sixth joint of the manipulator can be set as the arm plane.

[0073] A reference plane is set, and the angle between the arm plane and the reference plane is set as the arm angle. In the present disclosure, when the robot arm moves, the arm angle changes with the arm plane. That is, at this time, the reference plane is a plane with a fixed position, and preferably, the reference plane is a vertical plane.

[0074] For example, when the plane where the shoulder, elbow and wrist of the robot arm are located is used as the arm plane, the reference plane is a vertical plane passing through the shoulder and wrist of the robot arm. At this time, the straight line where the reference plane intersects with the arm plane passes through the wrist and shoulder of the robot arm.

[0075] In the present disclosure, a preset arm angle threshold range is set based on the arm angle, and the plane corresponding to the preset arm angle threshold range is set as a virtual wall. At this time, the virtual wall, the arm plane and the virtual wall intersect on the same straight line, which passes through the wrist and shoulder of the robotic arm.

[0076] like Figure 3 As shown, for any configuration of seven-DOF manipulator, its offset configuration can be simplified. The specific simplified form is as follows Figure 3 As shown in , axis O2 can be regarded as shoulder Ps, axis O4 as elbow Pe, and axis O6 as wrist Pw. d3, a4, a5, d5, a6, a7, d7 represent the DH parameters of the third, fourth, fifth, sixth, and seventh joints.

[0077] In the present disclosure, a specific implementation of the arm angle calculation includes the following steps: The position of each joint coordinate point of the robot arm is obtained by the forward kinematics calculation method. The specific calculation method is as follows.

[0078] , , in, Indicates the method of calculating the pose information of the i-th joint through the i-1th joint, where i is the joint number. For example, , which means that the posture information of the seventh joint is calculated by the posture information of the first to sixth joints. The calculation method of the posture information of other joints is similar.

[0079] , Among them, R represents the rotation matrix of the joint, and P represents the position matrix of the joint.

[0080] Next, the arm angle is calculated based on the positions of the joint coordinate points.

[0081] like Figure 3 As shown, , , the second joint coordinate system is used as the base coordinate system, and V is used as the vertical unit vector of the base coordinate system. Define the reference plane unit vector ,in , projecting Lse onto Lsw yields , , is the unit vector of Lsw. The perpendicular distance vector from point Pe to Lsw . , is the unit vector of Lse, , is the unit vector of P.

[0082] Arm angle tangent formula , the arm angle calculation formula is as follows: .

[0083] Typically, the vertical direction is selected as the reference plane, that is, the plane formed by V(z) and (Y) shown in the figure is used as the reference plane, and the calculated arm angle is used as a parameter for setting the virtual wall.

[0084] in The results can be obtained in the arm angle calculation process, that is, through the calculation method of the above steps. The conversion matrix is ​​calculated according to the DH parameters, and the position P of each axis relative to the base can be calculated according to the following formula, which can also be used to calculate the expression of the above parameter vector.

[0085] When the robot arm moves, the arm angle changes along a first direction, and the first direction can be Figure 2 In the present disclosure, the arm angle may also change in the opposite direction of the first direction, and in this case, virtual walls may be set on both sides of the arm plane. The following description is only for the case where a virtual wall is set on one side of the arm plane. The case where virtual walls are set on both sides of the arm plane is similar to the case where a virtual wall is set on one side of the arm plane, and will not be repeated here.

[0086] When the arm angle changes, it approaches the virtual wall. When the arm plane coincides with the virtual wall, the movement of the shoulder joint in the arm plane of the robot arm is constrained to hinder the movement of the arm plane when and after crossing the virtual wall in the first direction.

[0087] That is to say, when the robot arm is moving, after it passes over the virtual wall, a resistance torque will be generated through the control of the joints, making it difficult for the robot arm to move further. In the present disclosure, the virtual wall is not a rigid wall, but a flexible wall that can be passed over. At this time, the movement of the arm plane when passing over the virtual wall in the first direction and after passing over the virtual wall is hindered, that is, the arm plane can pass over the virtual wall, but due to the existence of the resistance torque, after passing over the virtual wall, a larger force / torque is required to drive the robot arm to move further.

[0088] like Figure 2 As shown, the preset arm angle threshold range can be [ ], accordingly, in and Two virtual walls are set up.

[0089] When the arm angle reaches a preset arm angle threshold range, based on the gravity term, friction term and resistance factor of each joint of the robotic arm, the required driving torque of each joint and the resistance torque to prevent the arm angle from changing along the first direction are calculated, and the driving torque and resistance torque are applied to the joints.

[0090] Specifically, for Figure 2 and Figure 3 The dynamic model of the robot arm shown can be simplified as follows: In the robot arm dynamics model, the meanings of each expression are as follows: is the joint driving torque, is the inertia matrix, is the Coriolis force related term, is the gravity term, and f is the friction term.

[0091] In zero-force drag mode, you only need to set the gravity term The smoothness of dragging can be ensured by compensating the friction term f.

[0092] When the arm angle rotates to the virtual wall, a resistance perpendicular to the arm plane needs to be added and joint resistance torque To prevent the arm angle from continuing to change (rotate), Therefore, during zero-force dragging, the joint torque needs to be added as shown below Therefore, the specific calculation method of the torque applied to each joint is as follows: in, represents the resisting moment of each axis, , the resistance applied to each axis is calculated through the Jacobian matrix J Converted into the resistance torque of the corresponding joint. represents the gravity term. C represents the drag factor. Represents the resistance applied to each joint, which can be set to a fixed value during initialization.

[0093] in, Represents the resistance torque, which can be calculated by the joint impedance control principle to equate the robot arm joint to a spring-mass-damper system.

[0094] Resistance torque In practical applications, the resistance moment It can be simplified as follows: Due to the existence of resistance factor, Simplified to a fixed value.

[0095] Based on the buffer arm angle interval , Current arm angle , preset arm angle threshold range Calculate the drag factor C; when the current arm angle is within the preset arm angle threshold range Internal time (i.e. when the current arm angle is greater than or equal to , less than or equal to ), the resistance factor is 0, that is, no additional resistance is generated, that is, no resisting torque is applied.

[0096] When the current arm angle is outside the preset arm angle threshold range, the drag factor is calculated; if the current arm angle is greater than hour, ; If the current arm angle is less than hour, .

[0097] If the calculated resistance factor is greater than the first preset threshold, the resistance factor is taken as the first preset threshold; if the calculated resistance factor is less than the second preset threshold, the resistance factor is taken as the second preset threshold; if the calculated resistance factor is between the first preset threshold and the second preset threshold (including the calculated resistance factor being equal to the first preset threshold or equal to the second preset threshold), the resistance factor remains unchanged (the current calculated value).

[0098] In a preferred embodiment, the first preset threshold is greater than the second preset threshold. More preferably, the absolute values ​​of the first preset threshold and the second preset threshold are the same.

[0099] In the present disclosure, the first preset threshold is greater than 0 and less than or equal to 3; for example, the first preset threshold may be 1; accordingly, the second preset threshold is greater than or equal to -3 and less than 0, for example, the first preset threshold may be -1.

[0100] In the present disclosure, the preset arm angle threshold range is a constant representing the arm angle size, and the buffer arm angle interval represents the arm angle virtual wall buffer zone, that is, the virtual wall thickness.

[0101] In zero-force dragging and self-motion control, the change of the arm angle of the seven-degree-of-freedom robot arm is mainly determined by joints 2 and 3, so the change of the arm angle can be limited by limiting the rotation of joints 2 and 3.

[0102] At this point, we can get the resistance torque given to the joint when the arm angle rotates to the range of the virtual wall. When the arm angle of the robot arm rotates to the plane of the virtual wall, the resistance torque is applied to joints 2 and 3 to limit the arm angle and ensure its posture stability during dragging and movement.

[0103] According to another aspect of the present disclosure, there is provided an electronic device, including: A memory, the memory stores execution instructions; The processor executes the execution instructions stored in the memory, so that the processor executes any one of the above methods.

[0104] According to another aspect of the present disclosure, a readable storage medium is provided, in which execution instructions are stored. When the execution instructions are executed by a processor, they are used to implement any of the above methods.

[0105] Figure 4 An example diagram of an apparatus using a hardware implementation of a processing system is shown. The apparatus may include a corresponding module that performs each or several steps in the above flowchart. Therefore, each step or several steps in the above flowchart may be performed by a corresponding module, and the apparatus may include one or more of these modules. The module may be one or more hardware modules specifically configured to perform the corresponding steps, or implemented by a processor configured to perform the corresponding steps, or stored in a computer-readable medium for implementation by a processor, or implemented by some combination.

[0106] The hardware structure can be implemented using a bus architecture. The bus architecture can include any number of interconnecting buses and bridges, depending on the specific application and overall design constraints of the hardware. The bus 1100 connects various circuits including one or more processors 1200, memory 1300, and / or hardware modules together. The bus 1100 can also connect various other circuits 1400 such as peripherals, voltage regulators, power management circuits, external antennas, etc.

[0107] The bus 1100 may be an Industry Standard Architecture (ISA) bus, a Peripheral Component Interconnect (PCI) bus, or an Extended Industry Standard Component (EISA) bus, etc. The bus may be divided into an address bus, a data bus, a control bus, etc. For ease of representation, the figure only uses one connecting line, but does not mean that there is only one bus or one type of bus.

[0108] Any process or method description in the flowchart or otherwise described herein can be understood as a module, fragment or portion of a code representing one or more executable instructions for implementing the steps of a specific logical function or process, and the scope of the preferred embodiments of the present disclosure includes other implementations, in which the functions may not be performed in the order shown or discussed, including performing the functions in a substantially simultaneous manner or in a reverse order according to the functions involved, which should be understood by a person skilled in the art of the present disclosure. The processor performs the various methods and processes described above. For example, the method implementation in the present disclosure can be implemented as a software program, which is tangibly contained in a machine-readable medium, such as a memory. In some embodiments, part or all of the software program can be loaded and / or installed via a memory and / or a communication interface. When the software program is loaded into the memory and executed by the processor, one or more steps in the method described above can be performed. Alternatively, in other embodiments, the processor can be configured to perform one of the above methods in any other appropriate manner (e.g., by means of firmware).

[0109] The logic and / or steps represented in the flowchart or otherwise described herein may be embodied in any readable storage medium for use by an instruction execution system, apparatus, or device (such as a computer-based system, a system including a processor, or other system that can fetch instructions from and execute instructions on an instruction execution system, apparatus, or device), or in combination with such instruction execution systems, apparatuses, or devices.

[0110] For the purposes of this specification, a "readable storage medium" may be any device that can contain, store, communicate, propagate or transmit a program for use with or in conjunction with an instruction execution system, device or apparatus. More specific examples of readable storage media (a non-exhaustive list) include the following: an electrical connection with one or more wirings (electronic device), a portable computer disk case (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable and editable read-only memory (EPROM or flash memory), a fiber optic device, and a portable read-only memory (CDROM). In addition, the readable storage medium may even be paper or other suitable medium on which the program can be printed, since the program can be obtained electronically, for example, by optically scanning the paper or other medium, followed by editing, deciphering or, if necessary, processing in another suitable manner, and then stored in a memory.

[0111] It should be understood that the various parts of the present disclosure can be implemented in hardware, software, or a combination thereof. In the above-mentioned embodiments, multiple steps or methods can be implemented in software stored in a memory and executed by a suitable instruction execution system. For example, if implemented in hardware, as in another embodiment, it can be implemented by any one of the following technologies known in the art or their combination: a discrete logic circuit having a logic gate circuit for implementing a logic function for a data signal, a dedicated integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0112] A person skilled in the art may understand that all or part of the steps of implementing the above-mentioned implementation method may be completed by instructing related hardware through a program, and the program may be stored in a readable storage medium, which, when executed, includes one or a combination of the steps of the implementation method.

[0113] In addition, each functional unit in each embodiment of the present disclosure may be integrated into a processing module, or each unit may exist physically separately, or two or more units may be integrated into one module. The above-mentioned integrated module may be implemented in the form of hardware or in the form of a software functional module. If the integrated module is implemented in the form of a software functional module and sold or used as an independent product, it may also be stored in a readable storage medium. The storage medium may be a read-only memory, a disk or an optical disk, etc.

[0114] In the description of this specification, the description with reference to the terms "one embodiment / method", "some embodiments / methods", "example", "specific example", or "some examples" etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment / method or example are included in at least one embodiment / method or example of the present application. In this specification, the schematic representations of the above terms are not necessarily the same embodiment / method or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments / methods or examples in a suitable manner. In addition, those skilled in the art may combine and combine the different embodiments / methods or examples described in this specification and the features of the different embodiments / methods or examples, unless they are contradictory.

[0115] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of the features. In the description of this application, the meaning of "plurality" is at least two, such as two, three, etc., unless otherwise clearly and specifically defined.

[0116] Those skilled in the art should understand that the above embodiments are only for the purpose of clearly illustrating the present disclosure, and are not intended to limit the scope of the present disclosure. For those skilled in the art, other changes or modifications may be made based on the above disclosure, and these changes or modifications are still within the scope of the present disclosure.

Claims

1. A method for constraining the self-motion of a robot arm in null space, characterized in that: include: Control the robot arm in zero-force drag mode; The plane formed by the shoulder joint, wrist joint and elbow joint of the robot arm is set as the arm plane; A reference plane is set, and the angle between the arm plane and the reference plane is set as the arm angle; Setting a virtual wall, wherein the reference plane, the arm plane and the virtual wall intersect on the same straight line; When the robot arm moves, the arm angle changes along the first direction, and makes the arm plane approach the virtual wall surface; When the arm plane coincides with the virtual wall, the movement of the shoulder joint in the arm plane of the robot arm is constrained so as to hinder the movement of the arm plane when and after crossing the virtual wall in the first direction.

2. The method for constraining the self-motion of a robot arm in null space as claimed in claim 1, characterized in that: A preset arm angle threshold range is set based on the arm angle, and a plane corresponding to the preset arm angle threshold range is set as a virtual wall.

3. The method for constraining the self-motion of a robot arm in null space as claimed in claim 2, characterized in that: When the arm angle reaches a preset arm angle threshold range, based on the gravity term, friction term and resistance factor of each joint of the robotic arm, the required driving torque of each joint and the resistance torque to prevent the arm angle from changing along the first direction are calculated, and the driving torque and resistance torque are applied to the joints.

4. The method for constraining the self-motion of a robot arm in null space as claimed in claim 3, characterized in that: When the current arm angle is within the preset arm angle threshold range, the resistance factor is 0; when the current arm angle is outside the preset arm angle threshold range, the resistance factor is calculated; if the calculated resistance factor is greater than the first preset threshold, the resistance factor is set to the first preset threshold; If the calculated resistance factor is less than the second preset threshold, the resistance factor is set to the second preset threshold; if the calculated resistance factor is between the first preset threshold and the second preset threshold, the resistance factor remains unchanged.

5. The method for constraining the self-motion of a robot arm in null space as claimed in claim 4, characterized in that: The first preset threshold is greater than the second preset threshold.

6. The method for constraining the self-motion of a robot arm in null space as claimed in claim 5, characterized in that: The first preset threshold is greater than 0; and / or the second preset threshold is less than 0.

7. A device for constraining the self-motion of a robot arm in zero space, characterized in that: include: A control module, the control module is used to control the robot arm in a zero-force drag mode; as well as The parameter setting module sets the plane formed by the shoulder joint, wrist joint and elbow joint of the robot arm as the arm plane; sets the reference plane, and sets the angle between the arm plane and the reference plane as the arm angle; sets the virtual wall, wherein the reference plane, the arm plane and the virtual wall intersect on the same straight line; In which, when the robotic arm moves, the arm angle changes along a first direction and makes the arm plane approach a virtual wall; when the arm plane coincides with the virtual wall, the control module is also used to constrain the movement of the shoulder joint in the arm plane of the robotic arm so as to hinder the movement of the arm plane in the first direction when it crosses the virtual wall and after crossing the virtual wall.

8. An electronic device, characterized in that: include: A memory storing execution instructions; as well as A processor, wherein the processor executes the execution instruction stored in the memory, so that the processor executes the method according to any one of claims 1 to 6.

9. A readable storage medium, characterized in that: The readable storage medium stores execution instructions, which are used to implement the method according to any one of claims 1 to 6 when executed by a processor.

Images