CN117359623A - Forced motion control method and system for mechanical arm - Google Patents

Abstract

The application provides a forced motion control method and system for a mechanical arm, and relates to the field of device control. The control method comprises the following steps: calculating a first external force matrix corresponding to external force received by the force sensor; the external force matrix comprises external force and external force moment; the force sensor is arranged at the end part of the mechanical arm; limiting the first external force by singular factors, and converting the first external force matrix from a force sensor coordinate system to a tool coordinate system to obtain a second external force matrix; the tool coordinate system is a coordinate system where an executing mechanism connected to the end part of the mechanical arm is located; admittance control is carried out on the second external moment array so as to obtain the instruction pose of the mechanical arm; and controlling the mechanical arm to move based on the mechanical arm instruction pose. By using the forced motion control method of the mechanical arm, which is provided by the embodiment of the application, the linear dragging and the limiting dragging of the mechanical arm can be realized, the singular point can be effectively avoided, and the motion stability of the mechanical arm is improved.

Description

Forced motion control method and system for mechanical arm

Technical Field

The application relates to the field of device control, in particular to a forced motion control method and system of a mechanical arm.

Background

In the fields of automatic production, surgical robots and the like, mechanical arms connected in series through multiple joints are common, and are mainly divided into industrial mechanical arms and cooperative mechanical arms. In order to freely, accurately and efficiently drag, push or pull the mechanical arm, the mechanical arm performs forced motion, the purposes of man-machine cooperation, track teaching and the like are achieved, and the forced motion of the mechanical arm can be controlled by a sensor based on the tail end of the mechanical arm.

The existing control method for forced movement of the mechanical arm based on the mechanical arm end sensor simply converts external force into the speed or the pose of the mechanical arm, and then the speed or the position is used for controlling the movement of the mechanical arm in a servo system. However, the existing mechanical arm control method does not avoid singular points, so that the joint is easy to rotate too fast, and a driver reports errors.

Disclosure of Invention

The embodiment of the application aims to provide a forced motion control method and a forced motion control system for a mechanical arm, wherein a singular factor can be calculated based on the joint state of the mechanical arm by calculating the external force received by a force sensor at the end part of the mechanical arm to judge or serve as a basis for adjusting the singular factor, and finally, a pose instruction for controlling the motion of the mechanical part is obtained according to admittance control; therefore, the linear motion and the limited motion of the mechanical arm can be realized, singular points can be effectively avoided, and the motion stability of the mechanical arm is improved.

In a first aspect, an embodiment of the present application provides a method for controlling a forced motion of a mechanical arm, where the method includes: calculating a first external force matrix corresponding to external force received by the force sensor; the external force matrix comprises external force and external force moment; the force sensor is arranged at the end part of the mechanical arm; limiting the first external force by singular factors, and converting the first external force matrix from a force sensor coordinate system to a tool coordinate system to obtain a second external force matrix; the tool coordinate system is a coordinate system where an executing mechanism connected to the end part of the mechanical arm is located; admittance control is carried out on the second external moment array so as to obtain the instruction pose of the mechanical arm; and controlling the mechanical arm to move based on the mechanical arm instruction pose.

In the implementation process, the method for controlling the forced movement of the mechanical arm provided by the embodiment of the application calculates the singular factor based on the joint state of the mechanical arm by calculating the external force received by the force sensor at the end part of the mechanical arm, judges or serves as a basis for adjusting the singular factor, and finally obtains the pose instruction for controlling the movement of the mechanical part according to admittance control. The mechanical arm forced motion control method provided by the embodiment of the application is used for controlling the mechanical arm, so that the mechanical arm can stably move under the action of external force; the mechanical arm has good flexibility and can avoid singular points of the mechanical arm.

Optionally, in an embodiment of the present application, limiting the first external force with a singular factor includes: calculating the actual condition number of the Jacobian matrix of the mechanical arm; wherein the actual condition number is inversely proportional to the degree of singular of the robotic arm; calculating singular factors according to the starting condition number threshold, the ending condition number threshold and the actual condition number; the first external force is limited based on the singular factor.

Optionally, in an embodiment of the present application, calculating the singular factor according to the start condition number threshold, the end condition number threshold, and the actual condition number includes: based on the formula:calculating singular factors; wherein scale is a singular factor, condJ ₁ To initiate condition number threshold, condJ ₂ To terminate the condition number threshold, condJ is the actual condition number.

In the implementation process, the method for controlling the forced motion of the mechanical arm judges the singular degree of the mechanical arm by calculating the actual condition number of the jacobian matrix of the mechanical arm in real time; and calculating singular factors according to the initial condition number threshold value, the final condition number threshold value and the calculated actual condition number corresponding to the actual demand, and limiting external force by using the singular factors, so that the purpose of avoiding singular is achieved, and the forced movement of the mechanical arm is more flexible and accurate.

Optionally, in an embodiment of the present application, admittance control is performed on the second external torque array to obtain a command pose of the mechanical arm, including:

in admittance model:calculating acceleration error matrix corresponding to external force>Speed error matrix->Position error matrix X _e The method comprises the steps of carrying out a first treatment on the surface of the Wherein M is an inertial parameter matrix, B isDamping parameter matrix, K is rigidity parameter matrix, F _ext Is a second external force matrix; a second external force matrix and an acceleration error matrix>Speed error matrix->And a position error matrix X _e Mapping to a straight line where a planned path of the forced movement is located; initial pose and position error matrix X based on mechanical arm _e And obtaining the instruction pose of the mechanical arm.

In the implementation process, when admittance control is performed on the second external force matrix, the second external force matrix and the acceleration error matrix are processedSpeed error matrix->And a position error matrix X _e And the limiting is carried out respectively, so that the command pose after admittance control can be kept on a straight line of navigation planning, and limiting dragging and straight line dragging are realized.

Optionally, in an embodiment of the present application, the mechanical arm command pose includes a command position and a command pose; based on the arm instruction pose, control arm motion includes: converting the instruction gesture into a unit quaternion gesture; and carrying out low-pass filtering on the quaternion gesture and the instruction position, and controlling the mechanical arm to move based on the quaternion gesture and the instruction position after filtering.

Optionally, in an embodiment of the present application, the instruction location includesp _x 、p _y 、p _z Cartesian command positions in the x-direction and the z-direction, respectively; the instruction gesture includes->The corresponding unit quaternion gesture is +.> r _x 、r _y 、r _z Cartesian command poses in the x direction and the z direction in the y direction respectively; (k) _x ，k _y ，k _z ) Is a rotation shaft, and theta is a rotation angle; low pass filtering the quaternion pose and the instruction position includes: for p _x 、p _y 、p _z 、q ₀ 、q ₁ 、q ₂ 、q ₃ And performing first-order low-pass filtering by using independent single channels respectively.

In the implementation process, the forced motion control method of the mechanical arm can overcome the problems that the command pose is discontinuous, the speed and the acceleration are suddenly changed, and avoid the vibration of the mechanical arm caused by the fact that the command pose output by admittance is directly used for motion control of the mechanical arm, so that the motion of the mechanical arm is more stable.

Optionally, in an embodiment of the present application, calculating a first external force matrix corresponding to an external force received by the force sensor includes: according to initial data acquired by the force sensor, carrying out load compensation and zero compensation on the force sensor to obtain zero point, load weight and mass center coordinates of the force sensor; the initial data are data of the mechanical arm under the condition of no external force; based on zero point, load weight and mass center coordinates of the force sensor, external force received by the force sensor and external moment corresponding to the external force are calculated to form a first external force matrix.

In the above implementation, the force sensor may have some initial deviation or zero drift, i.e. the output value is not zero when no external force is applied; zero compensation can correct these initial errors to zero, ensuring that the sensor output value approaches zero in the absence of external forces. Tools, loads or objects mounted on the robotic arm can create additional loads to the force sensor, which can lead to measurement errors; the load compensation may take into account and correct these additional loads, ensuring that the sensor output reflects the actual external force.

In a second aspect, embodiments of the present application provide a forced motion control system for a robotic arm, the control system comprising: the mechanical arm, the force sensor arranged at the end part of the mechanical arm, the controller and the executing mechanism; the controller is used for calculating a first external force matrix corresponding to the external force received by the force sensor; the external force matrix comprises external force and external force moment; the controller is also used for limiting the first external force by singular factors and converting the first external force matrix from the force sensor coordinate system to the tool coordinate system so as to obtain a second external force matrix; the tool coordinate system is a coordinate system where an executing mechanism connected to the end part of the mechanical arm is located; the controller is also used for admittance control of the second external moment array so as to obtain the instruction pose of the mechanical arm; and controlling the mechanical arm to move based on the mechanical arm instruction pose.

In a third aspect, an embodiment of the present application provides an electronic device, where the electronic device includes a memory and a processor, where the memory stores program instructions, and when the processor reads and executes the program instructions, the processor performs the steps in any of the foregoing implementation manners.

In a fourth aspect, embodiments of the present application further provide a computer readable storage medium having stored therein computer program instructions that, when read and executed by a processor, perform the steps of any of the above implementations.

Drawings

In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings that are needed in the embodiments of the present application will be briefly described below, it should be understood that the following drawings only illustrate some embodiments of the present application and should not be considered as limiting the scope, and other related drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.

FIG. 1 is a control flow chart of forced movement of a robotic arm provided in an embodiment of the present application;

fig. 2 is a singular avoidance flowchart provided in an embodiment of the present application;

fig. 3 is a flowchart of generating a pose of a mechanical arm instruction according to an embodiment of the present application;

FIG. 4 is a schematic diagram of a forced motion control system for a robotic arm provided in an embodiment of the present application;

fig. 5 is a schematic structural diagram of an electronic device according to an embodiment of the present application.

Icon: a forced motion control system-100 of the mechanical arm; a mechanical arm-110; a robot arm end-111; a force sensor-120; a controller-130; and an actuator 140.

Detailed Description

The technical solutions in the embodiments of the present application will be described below with reference to the drawings in the embodiments of the present application. For example, the flowchart and block diagrams in the figures illustrate the architecture, functionality, and operation of possible implementations of systems, methods and computer program products according to various embodiments of the present invention. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code, which comprises one or more executable instructions for implementing the specified logical function(s). It should also be noted that in some alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It will also be noted that each block of the block diagrams and/or flowchart illustration, and combinations of blocks in the block diagrams and/or flowchart illustration, can be implemented by special purpose hardware-based systems which perform the specified functions or acts, or combinations of special purpose hardware and computer instructions. In addition, functional modules in the embodiments of the present invention may be integrated together to form a single part, or each module may exist alone, or two or more modules may be integrated to form a single part.

The singular point processing method of the mechanical arm is one of the main technical difficulties in the application field of robots. The problem of singular points is a technical point which is inevitably encountered in the movement process of the multi-joint mechanical arm, and the problem is unavoidable due to the fact that the multi-joint mechanical arm is related to a structure.

Taking the current most common 6-joint mechanical arm as an example, the 6-joint mechanical arm at least comprises three singular points, namely a wrist singular point, an elbow singular point and a shoulder singular point. Wrist singularities may occur when the fourth and sixth joints are coaxial; elbow singularities may occur when the second, third and fifth joints are coaxial; and shoulder singularities may occur when the first and sixth joints are coaxial. And when the mechanical arm moves to a certain pose, the condition that a plurality of singular points appear simultaneously can be even encountered, and the phenomenon that the movement speed of the joint suddenly increases can occur, so that the robot is stopped and even the safety problem is brought.

The applicant finds that the current control method for forced movement of the mechanical arm based on the mechanical arm end sensor only converts the external force into the speed or the pose of the mechanical arm by simple drops in the research process, and further controls the movement of the mechanical arm by using the speed or the position in a servo system. However, the existing mechanical arm control method does not avoid singular points, so that the joint is easy to rotate too fast, and a driver reports errors; on the other hand, the mechanical arm is easy to vibrate in the movement process, and when the mechanical arm is applied to operations such as oral surgery, certain danger exists; in addition, the current control method can only control the mechanical arm to move freely, but can not control the mechanical arm to move according to a specific track, such as linear movement or limited movement.

Based on the above, the application provides a forced motion control method and a forced motion control system of a mechanical arm, wherein the forced motion control method of the mechanical arm realizes the functions of linear dragging and dragging limiting of a tool by limiting the direction and the size of external force and external moment under a tool coordinate system. Judging the singular degree of the mechanical arm by calculating the condition number of the jacobian matrix of the mechanical arm in real time; and then dynamically adjusting the singular factors of the external force and the external torque input admittance model by using the condition number, thereby achieving the purpose of avoiding singular. The mechanical arm instruction gesture (Euler angle) output by the admittance model is converted into a quaternion, and then a first-order low-pass filter is adopted, so that a smoother gesture can be obtained, abrupt change and singular caused by the filter represented by the Euler angle are avoided, and the dragging stability is improved. The control method for the forced movement of the mechanical arm is applied to operations such as oral surgery, an operator can freely control the mechanical arm, such as free dragging and linear dragging, and has good flexibility and can avoid singular points of the mechanical arm.

Referring to fig. 1, fig. 1 is a control flow chart of forced movement of a mechanical arm according to an embodiment of the present application; the first aspect of the present application provides a forced motion control method for a mechanical arm, including the steps of:

step S100: and calculating a first external force matrix corresponding to the external force received by the force sensor.

In the step S100, a first external force matrix corresponding to an external force received by a force sensor is calculated, the force sensor is disposed at an end of the mechanical portion, and the external force matrix in the embodiment of the present application includes a force and a moment corresponding to the force. For example, a first external force matrix F _ext The method comprises the following steps: [ F _{x_ext} F _{y_ext} F _{z_ext} M _{x_ext} M _{y_ext} M _{z_ext} ]The method comprises the steps of carrying out a first treatment on the surface of the Wherein F is _{x_ext} 、F _{y_ext} 、F _{z_ext} Respectively an x component, a y component and a z component of the external force under the coordinates of the force sensor; m is M _{x_ext} 、M _{y_ext} 、M _{z_ext} Respectively F _{x_ext} 、F _{y_ext} 、F _{z_ext} Corresponding moment. It should be noted that, the readings of the sensor at the end of the mechanical arm often have differences from the actual external force due to factors such as zero drift, for example, the first external force matrix [ F _{x_ext} F _{y_ext} F _{z_ext} M _{x_ext} M _{y_ext} M _{z_ext} ]Sensor reading of [ F ] _x F _y F _z M _x M _y M _z ]The sensor readings are calculated.

The sensor in embodiments of the present application may be a multi-dimensional force sensor, such as a six-dimensional force sensor.

Step S200: the first external force is limited by a singular factor and the first external force matrix is converted from the force sensor coordinate system to the tool coordinate system to obtain a second external force matrix.

In the step S200, the first external force is limited by a singular factor, and the first external force matrix is converted from the force sensor coordinate system to the tool coordinate system, so as to obtain a second external force matrix. The singular factor is a factor used for judging the singular degree of the mechanical arm in the embodiment of the application, and can adjust the force and the moment of the mechanical arm under the condition that the singular degree is generated, for example, so as to achieve the purpose of avoiding the singular degree.

Step S300: admittance control is carried out on the second external moment array so as to obtain the instruction pose of the mechanical arm.

Step S400: and controlling the mechanical arm to move based on the mechanical arm instruction pose.

In the step S300 to the step S400, admittance control is performed on the second external moment array to obtain a mechanical arm instruction pose, wherein the mechanical arm instruction pose is data for guiding the movement of the mechanical arm; and controlling the mechanical arm to move based on the instruction pose of the mechanical arm.

It should be noted that admittance control is generally considered to be a type of mathematical model that is converted from force commands to position commands, for example, a common model is a mass-spring-damping model, or other similar or derived models.

In the field of robots, the contact force between the robot and the outside and the reference force of upper layer planning, which are obtained by measuring or estimating the external force by a force sensor and the like, are used as the input of admittance control, and the input force command is converted into the position command of the robot by the admittance control and is sent to the corresponding joint position actuator.

As can be seen from fig. 1, in the method for controlling forced movement of a mechanical arm according to the embodiment of the present application, by calculating the external force received by a force sensor at the end of the mechanical arm, a singular factor can be calculated based on the joint state of the mechanical arm, so as to determine or serve as a basis for adjusting the singular factor, and finally, a pose instruction for controlling movement of the mechanical part is obtained according to admittance control. The mechanical arm forced motion control method provided by the embodiment of the application is used for controlling the mechanical arm, so that the mechanical arm can stably move under the action of external force; the mechanical arm has good flexibility and can avoid singular points of the mechanical arm.

Referring to fig. 2, fig. 2 is a singular avoidance flowchart provided in an embodiment of the present application; in an alternative implementation manner of the embodiment of the present application, the limiting of the first external force by the singular factor in the step S200 may be implemented in the following manner:

step S210: the actual condition number of the jacobian matrix of the robotic arm is calculated.

Wherein the actual condition number is inversely proportional to the degree of singular of the robotic arm.

In the step S210, the actual condition number of the jacobian matrix of the mechanical arm is calculated; it should be noted that jacobian Matrix (jacobian Matrix) is a common concept in the fields of robotics, mechanical engineering, mathematics, etc., and can be used to describe the relationship between position and velocity in a multi-degree of freedom mechanical system (e.g., a robot arm).

To calculate the Jacobian matrix of the mechanical arm, determining the end position expression of the mechanical arm, usually a vector; determining the variable of each joint of the mechanical arm, wherein the variable is usually expressed by a joint angle; for the end position vector, partial derivative calculations are performed for each joint variable separately, which will result in a jacobian row vector for the joint variable. And after the Jacobian row vector of each joint is calculated, a Jacobian matrix is constructed.

The Jacobian condition number (Jacobian matrix condition number) is an indicator for measuring the numerical stability and numerical error sensitivity of the Jacobian matrix. For a Jacobian matrix J, the condition number is calculated as follows: cond (J) = | J is |X|J ^-1 I; wherein J represents the norm of the Jacobian matrix J, I J ^-1 I represents the inverse matrix J of the Jacobian matrix ^-1 Is a norm of (c).

The smaller the condition number is, the closer the mechanical arm is to the odd pose; the larger the condition number, the farther the mechanical arm is from the odd pose. An increase in condition number indicates that the robotic arm is moving away from an odd pose; a reduced condition number indicates that the robotic arm is approaching an odd pose. In practical applications, if the condition number of the jacobian matrix is large, some measure of numerical stability is usually required.

Step S220: the singular factor is calculated based on the starting condition number threshold, the ending condition number threshold, and the actual condition number.

Step S230: the first external force is limited based on the singular factor.

In the above steps S220-S230, the singular factor is calculated based on the starting condition number threshold and the ending condition number threshold, and the real condition number threshold of the robot arm calculated in real time, thereby limiting or adjusting the first external force.

In an alternative embodiment, step S220 is: the calculation of the singular factor from the starting condition number threshold, the ending condition number threshold, and the actual condition number may be calculated by the following equation:

in the formulas (1) - (3), scale is a singular factor, condJ ₁ To initiate condition number threshold, condJ ₂ To terminate the condition number threshold, condJ is the actual condition number. Further, the method comprises the steps of,

scale＝limit(0，scale，1) (2)

limit is a limiting function that limits the scale value between (0 1). Further, the method comprises the steps of,

F′ _ext ＝scale·F _ext (3)

F _ext for the first external force matrix, using a singular factor scale between 0 and 1 for the first external force matrix F _ext And limiting and inputting an admittance control model.

Analysis based on the above formulas (1) to (3):

when condJ is greater than condJ2, the mechanical arm is not singular, scale=1, the external force is completely input into the admittance model, and dragging of the mechanical arm is controlled.

When condJ is between condJ1 and condJ2, the mechanical arm approaches to the odd ectopic pose, 0 < scale < 1, and only a part of the external force is input to the admittance model. The mechanical arm is shown to drag more and more along with scale reduction, so that the purpose of avoiding singular is achieved.

Illustratively, when the robotic arm forced motion control method provided by the embodiments of the present application is applied in an oral operation, such as a dental operation, the starting condition number threshold condJ ₁ Can take a value of 0.10, terminating condition number threshold condJ ₂ May start and stop at 0.06.

Therefore, the method for controlling the forced movement of the mechanical arm judges the singular degree of the mechanical arm by calculating the actual condition number of the jacobian matrix of the mechanical arm in real time; and calculating singular factors according to the initial condition number threshold value, the final condition number threshold value and the calculated actual condition number corresponding to the actual demand, and limiting external force by using the singular factors, so that the purpose of avoiding singular is achieved, and the forced movement of the mechanical arm is more flexible and accurate.

Referring to fig. 3, fig. 3 is a flowchart of generating a pose of a manipulator instruction according to an embodiment of the present application; in an optional implementation manner of the embodiment of the present application, the controlling of admittance of the second external moment array by the step S300 to obtain the command pose of the mechanical arm may be implemented by the following manner:

step S310: in admittance model:calculating acceleration error matrix corresponding to external force>Speed error matrix->Position error matrix X _e 。

In the above step S310, the admittance model is used:calculating acceleration error matrix corresponding to external force>Velocity error matrix/>Position error matrix X _e The method comprises the steps of carrying out a first treatment on the surface of the Wherein M is an inertial parameter matrix, B is a damping parameter matrix, K is a rigidity parameter matrix, F _ext Is a second external force matrix.

For initial t ₀ F at time instant _ext 、X _e 、M, B, K is a known parameter from which t can be calculated based on the admittance model ₁ Time->Integration to obtain t ₁ Time->Integration to obtain t ₁ X time of day _e 。

Step S320: a second external force matrix and an acceleration error matrixSpeed error matrix->And a position error matrix X _e Mapped onto a straight line along which the planned path of the forced movement is located.

In the above step S320, a second external force matrix and an acceleration error matrix are obtainedSpeed error matrix->And a position error matrix X _e Mapping to a straight line where a planned path of the forced movement is located; that is, the present application is carried out by limiting the external force F _ext Acceleration error->Speed error->The direction and the size of the position error Xe can achieve the functions of linear dragging and drag limiting.

Illustratively, external force F _ext Mapping to a straight line of the navigation plan, and removing forces outside the direction of the straight line. External force F after mapping _ext Parallel to the direction of the navigation planning straight line; the external moment is all assigned zero. From F after mapping according to admittance model _ext Generated acceleration errorThe linear acceleration error component of (2) is parallel to the planned straight line, and the angular acceleration error component is all zero.

Error of accelerationMapping the linear acceleration errors to the linear of the navigation planning, and removing linear acceleration errors except the linear direction. After mapping +.>Is parallel to the navigation line. />Integral +.>The linear velocity error components are parallel to the planned straight line, and the angular velocity error components are all zero.

Error of speedMapping the linear velocity error to a linear of the navigation planning, and removing linear velocity errors except the linear direction. After mapping +.>Is parallel to the navigation line. />The position error component of the Xe obtained by integration is parallel to the planning straight line, and the attitude error component is all zero.

The position error Xe is mapped to a straight line of the navigation plan, and the position error other than the straight line direction is removed. The position error component of Xe after mapping is parallel to the planned straight line, and the attitude error is all zero.

Step S330: initial pose and position error matrix X based on mechanical arm _e And obtaining the instruction pose of the mechanical arm.

In the above step S330, the process is performed by X _t1 ＝X ₀ +X _e Calculating the current t ₁ Command pose of mechanical arm at moment, X ₀ Representing the initial pose. New instruction pose X _t1 The consistency is maintained on the navigation planning straight line. The mechanical arm can only drag on a straight line, and the posture is unchanged.

In some embodiments, the mechanical arm can drag only at a certain position on the straight line by limiting the size of the projection of Xe on the straight line of the navigation planning.

If be applied to dental robot, in the in-process that bores for the tooth down, can realize that the arm drags along navigation planning's implant planting direction straight line, and after reaching navigation planning's implant bottom position, do not allow to continue to advance, can only drag along opposite direction straight line, withdraw from and bore the tooth hole.

As can be seen from FIG. 3, when admittance control is performed on the second external force matrix, the second external force matrix and the acceleration error matrixSpeed error matrix->And a position error matrix X _e Respectively restrict and realize admittance controlThe position and the posture of the command after the command are kept on a straight line of navigation planning, and the limiting dragging and the straight line dragging are realized.

In an alternative embodiment, step S400 the robotic arm command pose includes a command position and a command pose; based on the mechanical arm instruction pose, the mechanical arm movement is controlled by the following modes:

converting the instruction gesture into a unit quaternion gesture; and carrying out low-pass filtering on the quaternion gesture and the instruction position, and controlling the mechanical arm to move based on the quaternion gesture and the instruction position after filtering.

The mechanical arm command pose comprises a command position X _p And command pose Xr, mechanical arm command poseWherein, instruction gesture X _r Euler angles, shaft angles, and the like, may be used.

The parameters of the admittance model such as input external force, external moment, joint position, joint speed and the like have interference and errors, so that the generated command pose is discontinuous, and the speed and acceleration have abrupt changes. The command pose output by admittance is directly used for controlling the movement of the mechanical arm, so that the mechanical arm can vibrate. Therefore, it is necessary to perform a filtering process on the instruction pose of admittance output.

In an alternative embodiment, instruction position X _p Represented asWherein p is _x 、p _y 、p _z The three instruction positions are respectively Cartesian instruction positions in the x direction and the z direction in the y direction and are mutually independent, and a single-channel filter can be used for respectively carrying out filtering treatment on the instruction positions.

Instruction gesture X _r Represented as(shaft angle representation method). (k) _x ，k _y ，k _z ) The rotation axis, the unit vector, and θ the rotation angle. Wherein r is _x 、r _y 、r _z The instruction states are respectively Cartesian instruction states in the x direction and the z direction in the y direction, and can be Euler angles or rotation vectors, wherein the Euler angles represent states with singularities, and two similar states, and Euler angles represent states which can be quite different; therefore, it cannot be individually filtered using a single-channel filter.

On the basis, the embodiment of the application is applied to instruction gesture X _r Using quaternion notation, X is taken as _r Expressed as:

in the above formula (3), the unit quaternion represents the attitude information of the robot, and the entire space thereof is continuous without singularity. Thus, a single-channel filter may be used to filter its four channels separately. The unit quaternion has a double coverage relation with the rotation matrix, so that the inner product result of all quaternions participating in filtering needs to be ensured to be non-negative when filtering is performed. When there is a negative number, the adjustment is performed by multiplying the quaternion by-1. The quaternion obtained after filtering is not a unit quaternion, and unit processing is required.

Further, the first order low pass filter expression for a single channel is:

alpha＝(2×pi×0.004×fx)/(2×pi×0.004×fx+1)

y′ _k+1 ＝(y _k+1 -y _k )×alpha+y _k

wherein fx is the low-pass cut-off frequency, y _k For the last period input, y _k+1 For input of the present period, y' _k+1 Is the output of the present period.

After low-pass filtering, controlling the mechanical arm to move based on the filtered quaternion gesture and the instruction position; and calculating the joint instruction position by using a robot inverse kinematics formula according to the instruction Cartesian pose. And sending the instruction positions of all joints to a driver of the mechanical arm, and controlling all joint motors of the mechanical arm to move.

Therefore, the forced motion control method of the mechanical arm can overcome the problems that the command pose is discontinuous and the speed and the acceleration are suddenly changed, avoids the vibration of the mechanical arm caused by the fact that the command pose output by admittance is directly used for motion control of the mechanical arm, and enables the motion of the mechanical arm to be more stable.

In an alternative embodiment, step S100 calculates a first external force matrix corresponding to the external force applied by the force sensor, which may be implemented in the following manner:

and carrying out load compensation and zero compensation on the force sensor according to initial data acquired by the force sensor so as to obtain zero point, load weight and barycenter coordinates of the force sensor. The initial data is data when the mechanical arm is not subjected to external force.

Based on zero point, load weight and mass center coordinates of the force sensor, external force received by the force sensor and external moment corresponding to the external force are calculated to form a first external force matrix.

It follows that the force sensor may have some initial deviation or zero drift, i.e. the output value is not zero when no external force is acting; zero compensation can correct these initial errors to zero, ensuring that the sensor output value approaches zero in the absence of external forces. Tools, loads or objects mounted on the robotic arm can create additional loads to the force sensor, which can lead to measurement errors; the load compensation may take into account and correct these additional loads, ensuring that the sensor output reflects the actual external force.

The forced motion control method of the mechanical arm converts signals acquired by the force sensor at the tail end of the mechanical arm into force and moment; carrying out load compensation and zero point compensation on the force and the moment to obtain an external force and an external moment under a force sensor coordinate system; filtering the compensated external force and external torque, and equivalently converting the external force and external torque into a tool coordinate system; according to the admittance control model, converting external force and external torque into mechanical arm instruction pose; obtaining a joint instruction position by using a robot inverse kinematics algorithm; and sending the joint instruction position to a servo driver to control the mechanical arm to move. The manipulator can be easily dragged by an operator. The mechanical arm is controlled to drag freely and drag linearly to drill down and punch holes, and the mechanical arm has the advantages of good flexibility, singular avoidance and the like.

Referring to fig. 4, fig. 4 is a schematic diagram of a forced motion control system of a mechanical arm according to an embodiment of the present application; the second aspect of the present application also provides a forced motion control system of a mechanical arm, where the forced motion control system 100 of the mechanical arm includes a mechanical arm 110, a force sensor 120 disposed at an end 111 of the mechanical arm, a controller 130, and an actuator 140.

The controller 130 is configured to calculate a first external force matrix corresponding to the external force received by the force sensor 120; the external force matrix comprises external force and external force moment.

The controller 130 is further configured to limit the first external force by a singular factor and convert the first external force matrix from the force sensor 120 coordinate system to the tool coordinate system to obtain a second external force matrix; the tool coordinate system is a coordinate system where the actuator 140 connected to the arm end 111 is located.

The controller 130 is further configured to perform admittance control on the second external moment array to obtain a command pose of the mechanical arm 110; based on the robot arm 110 command pose, the robot arm 110 is controlled to move.

In an alternative embodiment, the controller 130 is further configured to calculate an actual condition number of the jacobian matrix of the robotic arm 110; wherein the actual condition number is inversely proportional to the degree of singular of the robotic arm 110; calculating singular factors according to the starting condition number threshold, the ending condition number threshold and the actual condition number; the first external force is limited based on the singular factor.

In an alternative embodiment, the controller 130 is further configured to, based on the formula: calculating singular factors; wherein scale is a singular factor, condJ ₁ To initiate condition number threshold, condJ ₂ To terminate the condition number threshold, condJ is the actual condition number.

In an alternative embodiment, the controller 130 is also configured to provide admittanceAnd (3) model: calculating acceleration error matrix corresponding to external force>Speed error matrix->Position error matrix X _e The method comprises the steps of carrying out a first treatment on the surface of the Wherein M is an inertial parameter matrix, B is a damping parameter matrix, K is a rigidity parameter matrix, F _ext Is a second external force matrix; a second external force matrix and an acceleration error matrix>Speed error matrix->And a position error matrix X _e Mapping to a straight line where a planned path of the forced movement is located; initial pose and position error matrix X based on mechanical arm 110 _e The commanded pose of the robotic arm 110 is obtained.

In an alternative embodiment, controller 130 is further configured to convert the commanded pose into a unit quaternion pose; the quaternion pose and the command position are subjected to low-pass filtering, and the movement of the mechanical arm 110 is controlled based on the filtered quaternion pose and the command position.

In an alternative embodiment, the instruction location includesp _x 、p _y 、p _z Cartesian command positions in the x-direction and the z-direction, respectively; the instruction gesture includes-> (shaft angle representation method). (kx, ky, kz) represents the rotation axis, the unit vector, θ represents the rotation angle, and the corresponding unit quaternion pose isr _x 、r _y 、r _z Cartesian command poses in the x direction and the z direction in the y direction respectively; the controller 130 is also configured to address p _x 、p _y 、p _z 、q ₀ 、q ₁ 、q ₂ 、q ₃ And performing first-order low-pass filtering by using independent single channels respectively.

In an alternative embodiment, the controller 130 is further configured to perform load compensation and zero compensation on the force sensor 120 according to the initial data collected by the force sensor 120, so as to obtain zero point, load weight and centroid coordinates of the force sensor 120; the initial data is data of the mechanical arm 110 under the condition of no external force; based on the zero point, load weight and centroid coordinates of the force sensor 120, an external force and an external moment corresponding to the external force applied to the force sensor 120 are calculated to form a first external force matrix.

Referring to fig. 5, fig. 5 is a schematic structural diagram of an electronic device according to an embodiment of the present application. An electronic device 300 provided in an embodiment of the present application includes: a processor 301 and a memory 302, the memory 302 storing machine-readable instructions executable by the processor 301, which when executed by the processor 301 perform the method as described above.

Based on the same inventive concept, embodiments of the present application also provide a computer readable storage medium, where a computer program instruction is stored, and when the computer program instruction is read and executed by a processor, the steps in any of the above implementations are performed.

The computer readable storage medium may be any of various media capable of storing program codes, such as random access Memory (Random Access Memory, RAM), read Only Memory (ROM), programmable Read Only Memory (Programmable Read-Only Memory, PROM), erasable Read Only Memory (Erasable Programmable Read-Only Memory, EPROM), electrically erasable Read Only Memory (Electric Erasable Programmable Read-Only Memory, EEPROM), and the like. The storage medium is used for storing a program, the processor executes the program after receiving an execution instruction, and the method executed by the electronic terminal defined by the process disclosed in any embodiment of the present invention may be applied to the processor or implemented by the processor.

In the embodiments provided in the present application, it should be understood that the disclosed apparatus and method may be implemented in other manners. The above-described apparatus embodiments are merely illustrative, for example, the division of the units is merely a logical function division, and there may be other manners of division in actual implementation, and for example, multiple units or components may be combined or integrated into another system, or some features may be omitted, or not performed. Alternatively, the coupling or direct coupling or communication connection shown or discussed with each other may be through some communication interface, device or unit indirect coupling or communication connection, which may be in electrical, mechanical or other form.

Further, the units described as separate units may or may not be physically separate, and units displayed as units may or may not be physical units, may be located in one place, or may be distributed over a plurality of network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

Furthermore, functional modules in various embodiments of the present application may be integrated together to form a single portion, or each module may exist alone, or two or more modules may be integrated to form a single portion.

Alternatively, it may be implemented in whole or in part by software, hardware, firmware, or any combination thereof. When implemented in software, may be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions. When loaded and executed on a computer, produces a flow or function in accordance with embodiments of the present invention, in whole or in part.

The computer may be a general purpose computer, a special purpose computer, a computer network, or other programmable apparatus. The computer instructions may be stored in or transmitted from one computer-readable storage medium to another, for example, by wired (e.g., coaxial cable, optical fiber, digital Subscriber Line (DSL)), or wireless (e.g., infrared, wireless, microwave, etc.).

In this document, relational terms such as first and second, and the like may be used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Moreover, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising" does not exclude the presence of additional identical elements in a process, method, article, or apparatus that comprises the element.

The foregoing is merely exemplary embodiments of the present application and is not intended to limit the scope of the present application, and various modifications and variations may be suggested to one skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principles of the present application should be included in the protection scope of the present application.

Claims (10)

1. A method of forced motion control of a robotic arm, the method comprising:

calculating a first external force matrix corresponding to external force received by the force sensor; wherein the external force matrix comprises external force and external force moment; the force sensor is arranged at the end part of the mechanical arm;

limiting the first external force by singular factors, and converting the first external force matrix from a force sensor coordinate system to a tool coordinate system to obtain a second external force matrix; the tool coordinate system is a coordinate system where an executing mechanism connected to the end part of the mechanical arm is located;

admittance control is carried out on the second external moment array so as to obtain the instruction pose of the mechanical arm;

and controlling the mechanical arm to move based on the mechanical arm instruction pose.

2. The method of claim 1, wherein the limiting the first external force by a singular factor comprises:

calculating the actual condition number of the Jacobian matrix of the mechanical arm; wherein the actual condition number is inversely proportional to the degree of singular of the robotic arm;

calculating a singular factor from the starting condition number threshold, the ending condition number threshold, and the actual condition number;

the first external force is limited based on the singular factor.

3. The method of claim 2, wherein said calculating a singular factor based on a start condition number threshold, an end condition number threshold, and said actual condition number comprises:

based on the formula:calculating the singular factors;

wherein scale is the singular factor, condJ ₁ For the starting condition number threshold, condJ ₂ For the termination condition number threshold, condJ is theActual condition number.

4. The method of claim 1, wherein admittance controlling the second external moment array to obtain a robotic arm command pose comprises:

in admittance model:calculating an acceleration error matrix corresponding to the external force>Speed error matrix->Position error matrix X _e The method comprises the steps of carrying out a first treatment on the surface of the Wherein M is an inertial parameter matrix, B is a damping parameter matrix, K is a rigidity parameter matrix, F _ext Is the second external force matrix;

the second external force matrix and the acceleration error matrixSpeed error matrix->And a position error matrix X _e Mapping to a straight line where the planned path of the forced movement is located;

based on the initial pose of the mechanical arm and the position error matrix X _e And obtaining the instruction pose of the mechanical arm.

5. The method of claim 1, wherein the robotic arm command pose comprises a command position and a command pose; the controlling the movement of the mechanical arm based on the mechanical arm instruction pose comprises the following steps:

converting the instruction gesture into a unit quaternion gesture;

and carrying out low-pass filtering on the quaternion gesture and the instruction position, and controlling the mechanical arm to move based on the quaternion gesture and the instruction position after filtering.

6. The method of claim 5, wherein the instruction location comprisesp _x 、p _y 、p _z Cartesian command positions in the x-direction and the z-direction, respectively; the instruction gesture includes->The instruction gesture X _r The corresponding unit quaternion gesture is +.>Wherein r is _x 、r _y 、r _z Cartesian command poses in the x direction and the z direction in the y direction respectively; (k) _x ,k _y ,k _z ) Is a rotation shaft, and theta is a rotation angle;

the low pass filtering of the quaternion pose and the instruction position includes:

for said p _x 、p _y 、p _z 、q ₀ 、q ₁ 、q ₂ 、q ₃ And performing first-order low-pass filtering by using independent single channels respectively.

7. The method of claim 1, wherein calculating a first external force matrix corresponding to an external force experienced by the force sensor comprises:

according to initial data acquired by the force sensor, carrying out load compensation and zero compensation on the force sensor to obtain zero point, load weight and barycenter coordinates of the force sensor; the initial data are data of the mechanical arm under the condition that the mechanical arm is not subjected to external force;

and calculating the external force applied to the force sensor and the external moment corresponding to the external force based on the zero point, the load weight and the barycenter coordinate of the force sensor so as to form the first external force matrix.

8. A forced motion control system for a robotic arm, the control system comprising: the device comprises a mechanical arm, a force sensor arranged at the end part of the mechanical arm, a controller and an executing mechanism;

the controller is used for calculating a first external force matrix corresponding to the external force received by the force sensor; wherein the external force matrix comprises external force and external force moment;

the controller is further configured to limit the first external force by a singular factor and convert the first external force matrix from a force sensor coordinate system to a tool coordinate system to obtain a second external force matrix; the tool coordinate system is a coordinate system where an executing mechanism connected to the end part of the mechanical arm is located;

the controller is also used for admittance control of the second external moment array so as to obtain the instruction pose of the mechanical arm; and controlling the mechanical arm to move based on the mechanical arm instruction pose.

9. An electronic device comprising a memory and a processor, the memory having stored therein program instructions which, when executed by the processor, perform the steps of the method of any of claims 1-7.

10. A computer readable storage medium, characterized in that the computer readable storage medium has stored therein computer program instructions which, when executed by a processor, perform the steps of the method of any of claims 1-7.