Method for controlling movement of mechanical arm, electronic device and computer readable storage medium

Abstract

The application discloses a motion control method of a mechanical arm, an electronic device and a computer readable storage medium. The motion control method comprises the following steps: acquiring end position data of a plurality of path points of the mechanical arm; obtaining interpolation position data of a plurality of interpolation points based on the plurality of end position data; obtaining interpolation attitude data of the plurality of interpolation points based on the plurality of end position data and the interpolation position data of the plurality of interpolation points; obtaining corresponding Cartesian velocities of adjacent two interpolation points in the plurality of interpolation points based on the interpolation position data of the adjacent two interpolation points; obtaining joint velocities of each sub-arm of the mechanical arm based on the Cartesian velocities; if the joint velocity of a target sub-arm is greater than a preset joint velocity threshold, multiplying the Cartesian velocities by a correction parameter to obtain corrected Cartesian velocities, and then determining the corrected joint velocity of the target sub-arm; and performing motion control on the target sub-arm based on the corrected joint velocity. Based on the above method, the stability of the mechanical arm during motion can be improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of control, in particular to a motion control method of a mechanical arm, an electronic device and a computer readable storage medium. BACKGROUND

[0002] At present, the existing motion control method is used to plan a path and a speed of a mechanical arm, and motion control of the mechanical arm is performed based on a result of the path planning and the speed planning. For example, coordinates of a plurality of path points required to be passed through by an end of the mechanical arm in a target scene are acquired in advance, path fitting is performed based on the coordinates of the plurality of path points, a corresponding path curve is planned, and speed planning is performed on the path curve to determine a time of occurrence and a moving speed of the mechanical arm at each point in the path curve, and the motion control of the mechanical arm can be performed based on this.

[0003] The defect of the prior art is that the motion control of the mechanical arm is performed by using the existing motion control method, which is prone to cause frequent shaking or uneven fast and slow of the mechanical arm during motion due to excessive joint speed between at least part of sub-arms of the mechanical arm, so that the stability of the mechanical arm during motion is weak, that is, the stability of the mechanical arm during motion is low. SUMMARY

[0004] The technical problem to be mainly solved by the present application is how to improve the stability of the mechanical arm during motion.

[0005] In order to solve the above technical problem, the first technical solution adopted by the present application is a motion control method of a mechanical arm, comprising: acquiring end pose data of a plurality of path points of the mechanical arm, the end pose data comprising end position data and end attitude data of a corresponding path point; performing path fitting and speed planning based on the plurality of end pose data to obtain interpolation position data of a plurality of interpolation points; performing attitude fitting based on the plurality of end attitude data and the interpolation position data of the plurality of interpolation points to obtain interpolation attitude data of the plurality of interpolation points; obtaining corresponding Cartesian speed based on interpolation pose data of two adjacent interpolation points in the plurality of interpolation points, wherein the interpolation pose data comprises the interpolation position data and the interpolation attitude data of the corresponding interpolation point; obtaining joint speed of each sub-arm of the mechanical arm based on the Cartesian speed; if joint speed of a target sub-arm is greater than a preset joint speed threshold, multiplying the Cartesian speed of the target sub-arm by a correction parameter to obtain corrected Cartesian speed, and determining corrected joint speed of the target sub-arm based on the corrected Cartesian speed, wherein the target sub-arm is one of all sub-arms of the mechanical arm; and performing motion control on the target sub-arm in a motion process of the mechanical arm between the two adjacent interpolation points based on the corrected joint speed.

[0006] The corresponding Cartesian velocity is obtained based on the interpolation pose data of two adjacent interpolation points in the plurality of interpolation points, including: determining a first Cartesian velocity corresponding to the process of the robot arm moving from the first interpolation point to the second interpolation point based on the interpolation pose data of the first interpolation point and the second interpolation point, and determining a second Cartesian velocity corresponding to the process of the robot arm moving from the second interpolation point to the third interpolation point based on the interpolation pose data of the second interpolation point and the third interpolation point, wherein the interpolation pose data includes interpolation position data and interpolation attitude data of the corresponding interpolation point; the joint speed of each sub-arm of the robot arm is obtained based on the Cartesian velocity, including: determining the first joint speed of each sub-arm of the robot arm based on the first Cartesian velocity, and determining the second joint speed of each sub-arm of the robot arm based on the second Cartesian velocity; if the joint speed of the target sub-arm is greater than the preset joint speed threshold, the Cartesian speed of the target sub-arm is multiplied by a correction parameter to obtain a corrected Cartesian speed, and the corrected joint speed of the target sub-arm is determined based on the corrected Cartesian speed, including: if the first joint speed of the target sub-arm is greater than the preset joint speed threshold, the first Cartesian speed of the target sub-arm is multiplied by a correction parameter to obtain a corrected Cartesian speed, and the corrected joint speed of the target sub-arm is determined based on the corrected Cartesian speed, wherein the target sub-arm is one of the sub-arms of the robot arm, the correction parameter is a difference between one and a reduction parameter, the reduction parameter is one of the to-be-determined reduction parameters, each to-be-determined reduction parameter corresponds to a sub-arm, the to-be-determined reduction parameter is a quotient of a first value divided by a second value, the first value is the absolute value of the difference between the first joint speed of the corresponding sub-arm and the preset joint speed threshold, and the second value is the absolute value of the difference between the first joint speed of the corresponding sub-arm and the second joint speed of the corresponding sub-arm; the target sub-arm is controlled to move in the process of the robot arm moving between the two adjacent interpolation points based on the corrected joint speed, including: the target sub-arm is controlled to move in the process of the robot arm moving from the first interpolation point to the second interpolation point based on the corrected joint speed.

[0007] The reduction parameter is the largest one of the to-be-determined reduction parameters.

[0008] The interpolation attitude data of the plurality of interpolation points is obtained by performing attitude fitting based on the plurality of end attitude data and the interpolation position data of the plurality of interpolation points, including: constructing a quaternion curve formula based on the quaternion form of the end attitude data of the four path points, the curve parameter corresponding to the target interpolation point on the path curve obtained by path fitting, the curve parameters corresponding to the two path points with the second and third orders in the four path points on the path curve obtained by path fitting, and the interpolation attitude data of the target interpolation point in the form of quaternion; the target interpolation point is an interpolation point located between the two path points with the second and third orders; and the interpolation attitude data of the target interpolation point is obtained based on the quaternion curve formula.

[0009] The first Cartesian speed corresponding to the process of the mechanical arm moving from the first interpolation point to the second interpolation point is determined based on the interpolation pose data of the first interpolation point and the second interpolation point, including: obtaining an interpolation distance based on the interpolation position data of the first interpolation point and the interpolation position data of the second interpolation point, and dividing the interpolation distance by the interpolation time length between the first interpolation point and the second interpolation point to obtain a first linear speed; obtaining a rotation attitude in the form of a quaternion by dividing the interpolation pose data of the second interpolation point by the interpolation pose data of the first interpolation point, and multiplying half of the real part value of the rotation attitude by three imaginary part values of the rotation attitude to obtain three product values, obtaining a first angular speed based on the sum of the three product values; and determining the first Cartesian speed based on the first linear speed and the first angular speed.

[0010] The first joint speed of each sub-arm of the mechanical arm is determined based on the first Cartesian speed, including: determining the first joint speed of each sub-arm of the mechanical arm based on the first Cartesian speed and a Jacobian matrix, wherein the Jacobian matrix is used to represent the relationship between the first Cartesian speed and the first joint speed of each sub-arm of the mechanical arm.

[0011] The interpolation position data of the plurality of interpolation points is obtained by path fitting and speed planning based on the plurality of end pose data, including: performing path fitting based on a cubic B-spline algorithm and the end pose data of the plurality of path points to obtain a path curve; and performing speed planning on the path curve based on a preset speed planning model to determine the interpolation position data of the plurality of interpolation points on the path curve.

[0012] The interpolation position data of the plurality of interpolation points is obtained by performing speed planning on the path curve based on a preset speed planning model, including: determining the curve parameter of the interpolation point corresponding to each time node in the time sequence based on the time sequence, the speed planning model and the path curve; and determining the interpolation position data of the corresponding interpolation point based on the path curve and the curve parameter of the corresponding interpolation point.

[0013] To solve the above technical problems, a second technical solution adopted by the present application is an electronic device, comprising: a memory and a processor; the memory is used to store program instructions, and the processor is used to execute the program instructions to realize the above method.

[0014] To solve the above technical problems, a third technical solution adopted by the present application is a computer readable storage medium, which stores program instructions, and the program instructions are executed by a processor to realize the above method.

[0015] The beneficial effects of the present application are that: different from the prior art, in the technical solution of the present application, by acquiring end position data and end attitude data of a plurality of path points required to be passed through by the end of the mechanical arm, performing path fitting based on the plurality of end position data to obtain a path curve and performing velocity planning on the path curve to obtain interpolation position data of a plurality of interpolation points, performing attitude fitting based on the plurality of end position data and interpolation attitude data of the plurality of interpolation points to obtain interpolation attitude data of the plurality of interpolation points, obtaining corresponding Cartesian velocity based on interpolation attitude data of adjacent two interpolation points, and determining joint velocity of each sub-arm of the mechanical arm based on the Cartesian velocity, when joint velocity of a target sub-arm is greater than a preset joint velocity threshold, multiplying Cartesian velocity of the target sub-arm by a correction Cartesian velocity, and determining a correction joint velocity of the target sub-arm based on the correction Cartesian velocity, performing motion control on the target sub-arm based on the correction joint velocity during motion of the mechanical arm between adjacent two interpolation points, based on the above manner, when joint velocity of the target sub-arm during motion between any two interpolation points among all interpolation points is greater than the preset joint velocity threshold, the Cartesian velocity of the target sub-arm is corrected based on a correction parameter to obtain a correction Cartesian velocity, and then a correction joint velocity is obtained based on the corrected correction Cartesian velocity to control the target sub-arm, so that the phenomenon of instability of the mechanical arm during motion caused by too large joint velocity of the target sub-arm is avoided, and the stability of the mechanical arm during motion is improved. BRIEF DESCRIPTION OF DRAWINGS

[0016] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed to be used in the embodiment description will be briefly introduced as follows. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor.

[0017] Figure 1 is a flowchart of an embodiment of the motion control method of the present application;

[0018] Figure 2 is a flowchart of another embodiment of the motion control method of the present application;

[0019] Figure 3 is a flowchart of an embodiment of the electronic device of the present application;

[0020] Figure 4 is a structural schematic diagram of an embodiment of the computer readable storage medium of the present application. DETAILED DESCRIPTION

[0021] The application will be described in further detail below with reference to the drawings and embodiments. It is particularly pointed out that the following embodiments are only for illustration of the application and do not limit the scope of the application. Similarly, the following embodiments are only part of the embodiments of the application, and all other embodiments obtained by those of ordinary skill in the art without creative labor are within the scope of protection of the application.

[0022] Reference herein to "embodiment" means that the particular feature, structure, or characteristic described in connection with the embodiment can be included in at least one embodiment of the application. The appearance of the phrase in various places in the specification does not necessarily all refer to the same embodiment, nor is it necessarily mutually exclusive of other embodiments, or alternative or alternative embodiments. Those skilled in the art explicitly and implicitly understand that the embodiments described herein can be combined with other embodiments.

[0023] In the description of the application, it should be noted that, unless otherwise specifically defined and limited, the terms "mounting", "setting", "connecting", "connecting" should be understood broadly, for example, it can be fixedly connected, it can be detachably connected, or integrally connected; it can be mechanically connected, or electrically connected; it can be directly connected, or connected through an intermediate medium. For those of ordinary skill in the art, the above can be connected according to the specific meaning of the application.

[0024] The application first proposes a motion control method of a mechanical arm, see Figure 1 , Figure 1 is a flowchart of an embodiment of the motion control method of the application, as shown in Figure 1 The motion control method of the mechanical arm comprises:

[0025] Step S11: obtaining end pose data of a plurality of path points of the mechanical arm, the end pose data comprising end position data and end attitude data of the corresponding path point.

[0026] Among them, the method of obstacle avoidance planning or offline teaching can be used to obtain a plurality of points in the working space of the mechanical arm that need to be moved by the end of the mechanical arm, which is recorded as a plurality of path points. The end position data corresponding to each path point can be determined by the coordinates of the end of the mechanical arm at each path point, and the end attitude data corresponding to each path point can be determined by the attitude of the end of the mechanical arm when passing through each path point, so as to obtain the end pose data of the end of the mechanical arm at each path point.

[0027] Specifically, the end position data can be the spatial coordinate position of each path point, and the end attitude data can be the Euler angle attitude data of the mechanical arm at each path point, which can be determined according to actual needs, which is not limited here.

[0028] Step S12: based on the plurality of end pose data, path fitting and speed planning are performed to obtain interpolation position data of the plurality of interpolation points.

[0029] Wherein, the path planning can be performed based on the end pose data of the plurality of path points respectively corresponding to the mechanical arm, to obtain the corresponding path curve, and the speed planning is performed on the path curve, so as to determine the speed of the mechanical arm to the corresponding point of the path curve at each time, so that a point on the path curve can be selected as an interpolation point every preset time interval, and the interpolation position data of each selected interpolation point can be determined according to the path curve.

[0030] Specifically, the above method of selecting a point on the path curve as an interpolation point according to a preset time interval is only an example, and other methods can also be used to determine the interpolation points on the path curve. The interpolation position data of any interpolation point can be obtained through the path planning result and the speed planning result, which can be determined according to actual needs, which is not limited here.

[0031] Step S13: based on the plurality of end pose data and the interpolation position data of the plurality of interpolation points, pose fitting is performed to obtain interpolation pose data of the plurality of interpolation points.

[0032] Wherein, based on the end pose data respectively corresponding to each two path points, and the interpolation position data of the interpolation points between the corresponding two path points, the interpolation points between the corresponding two path points are fitted to determine the pose of the mechanical arm end when moving to each interpolation point, that is, the interpolation pose data.

[0033] Step S14: based on the interpolation pose data of the two adjacent interpolation points in the plurality of interpolation points, the corresponding Cartesian speed is obtained.

[0034] Wherein, the interpolation pose data includes the interpolation position data and the interpolation pose data of the corresponding interpolation point.

[0035] After obtaining the interpolation position data and the interpolation pose data corresponding to each interpolation point based on the above steps, the linear speed and the angular speed of the two adjacent interpolation points in the plurality of interpolation points can be calculated, that is, the linear speed is determined based on the coordinate difference between the two interpolation points and the time difference under the speed planning, and the angular speed is determined based on the pose difference / Euler angle difference between the two interpolation points and the time difference under the speed planning, and the corresponding Cartesian speed is determined based on the linear speed and the angular speed.

[0036] Specifically, the linear speed can include the linear speed in the x-axis, y-axis and z-axis directions of the Cartesian coordinate system, and the angular speed can include the angular speed of rotation along the x-axis, y-axis and z-axis of the Cartesian coordinate system, which is not limited here.

[0037] Step S15: Obtain the joint speed of each sub-arm of the robot arm based on the Cartesian speed.

[0038] In the formula, the Jacobian matrix can be used to associate the joint speed of each sub-arm of the robot arm with the Cartesian speed of the end of the robot arm, so that the joint speed of any sub-arm of the robot arm can be obtained by conversion under the condition of known Cartesian speed.

[0039] Step S16: If the joint speed of the target sub-arm is greater than the preset joint speed threshold, multiply the Cartesian speed by a correction parameter to obtain a corrected Cartesian speed, and determine the corrected joint speed of the target sub-arm based on the corrected Cartesian speed.

[0040] In the formula, the target sub-arm is one of all sub-arms of the robot arm.

[0041] When it is detected that the joint speed of the target sub-arm during movement between the above two adjacent interpolation points is greater than the preset joint speed threshold, it can be determined that the joint speed of the target sub-arm at this time is too large, which is likely to cause the risk of making the robot arm unable to run smoothly. Therefore, the joint speed needs to be corrected. The Cartesian speed can be multiplied by a correction parameter to obtain a corrected Cartesian speed, and the corrected joint speed of the target sub-arm during movement between the above two adjacent interpolation points can be determined based on the corrected Cartesian speed. Subsequent movement control of the target sub-arm based on the corrected joint speed can avoid the situation that the robot arm cannot run smoothly due to the joint speed of the target sub-arm being too large.

[0042] Step S17: Based on the corrected joint speed, the movement of the target sub-arm is controlled during movement of the robot arm between the two adjacent interpolation points.

[0043] In the formula, each sub-arm of the robot arm can be regarded as the target sub-arm for processing in the previous step, and the corrected joint speed corresponding to each sub-arm whose joint speed is greater than the preset joint speed threshold is calculated. Then, the movement of the sub-arm whose joint speed does not need to be corrected is controlled by using the joint speed, and the movement of the sub-arm whose joint speed needs to be corrected is controlled by using the corrected joint speed. In this way, the robot arm can smoothly move between the two adjacent interpolation points, and the stability of the robot arm during movement is improved.

[0044] In an application scenario, the robot arm can be used for cleaning and wiping. The plurality of path points can be path points required to be wiped by the end of the robot arm. In addition, the robot arm can also be applied to other application scenarios to perform other work. The specific application can be determined according to actual needs, which is not limited here.

[0045] Differing from the prior art, in the technical scheme of the application, end position data and end attitude data of a plurality of path points required to be passed through by the end of the mechanical arm are acquired, path fitting is performed based on the plurality of end position data to obtain a path curve and velocity planning is performed on the path curve to obtain interpolation position data of a plurality of interpolation points, attitude fitting is performed based on the plurality of end position data and interpolation attitude data of the plurality of interpolation points to obtain interpolation attitude data of the plurality of interpolation points, corresponding Cartesian velocities are obtained based on interpolation attitude data of adjacent two interpolation points, and joint velocities of each sub-arm of the mechanical arm are determined based on the Cartesian velocities, when the joint velocity of a target sub-arm is greater than a preset joint velocity threshold, the Cartesian velocity of the target sub-arm is multiplied by a correction Cartesian velocity, and a correction joint velocity of the target sub-arm is determined based on the correction Cartesian velocity, and the target sub-arm is controlled in a movement process between adjacent two interpolation points based on the correction joint velocity. Based on the above manner, when the joint velocity of the target sub-arm is greater than the preset joint velocity threshold in a movement process between any two interpolation points among all the interpolation points, the Cartesian velocity of the target sub-arm is corrected based on a correction parameter to obtain a correction Cartesian velocity, and then a correction joint velocity is obtained based on the corrected correction Cartesian velocity to control the target sub-arm, so that the phenomenon of instability of the mechanical arm in the movement process caused by the excessively large joint velocity of the target sub-arm is avoided, and the stability of the mechanical arm in the movement process is improved.

[0046] In an embodiment, referring to Figure 2 , Figure 2 is a flowchart of another embodiment of the motion control method of the application, as shown in Figure 2 , the step S14 can specifically include:

[0047] Step S141: based on the interpolation position data of the first interpolation point and the second interpolation point, a first Cartesian velocity corresponding to a movement process of the mechanical arm from the first interpolation point to the second interpolation point is determined, and based on the interpolation position data of the second interpolation point and the third interpolation point, a second Cartesian velocity corresponding to a movement process of the mechanical arm from the second interpolation point to the third interpolation point is determined.

[0048] Wherein, the second interpolation point can refer to the next interpolation point of the first interpolation point in the path curve obtained by path fitting, and the third interpolation point can refer to the next interpolation point of the second interpolation point in the path curve obtained by path fitting.

[0049] The step S15 can specifically include:

[0050] Step S151: based on the first Cartesian velocity, a first joint velocity of each sub-arm of the mechanical arm is determined, and based on the second Cartesian velocity, a second joint velocity of each sub-arm of the mechanical arm is determined.

[0051] The first joint speed of each sub-arm of the robot arm when the robot arm end moves from the first interpolation point to the second interpolation point can be determined based on the first Cartesian speed and the Jacobian matrix, and the second joint speed of each sub-arm of the robot arm when the robot arm end moves from the second interpolation point to the third interpolation point can be determined based on the second Cartesian speed and the Jacobian matrix.

[0052] The step S16 can specifically include:

[0053] The step S161 can specifically include: if the first joint speed of the target sub-arm is greater than the preset joint speed threshold, multiplying the first Cartesian speed of the target sub-arm by a correction parameter to obtain a corrected Cartesian speed, and determining a corrected joint speed of the target sub-arm based on the corrected Cartesian speed.

[0054] The correction parameter is a difference between one and a reduction parameter, and the reduction parameter is one of all to-be-determined reduction parameters, each to-be-determined reduction parameter corresponds to a sub-arm, and the to-be-determined reduction parameter is a quotient of a first value divided by a second value, the first value is an absolute value of a difference between the first joint speed of the corresponding sub-arm and the preset joint speed threshold, and the second value is an absolute value of a difference between the first joint speed of the corresponding sub-arm and the second joint speed of the corresponding sub-arm.

[0055] The step S17 can specifically include:

[0056] The step S171 can specifically include: based on the corrected joint speed, performing motion control on the target sub-arm during the movement of the robot arm from the first interpolation point to the second interpolation point.

[0057] Specifically, the calculation formula of the to-be-determined reduction parameter can be as follows:

[0058] (1)

[0059] In formula (1), is the preset joint speed threshold, is the first joint speed of the kth sub-arm in the robot arm, is the second joint speed of the kth sub-arm in the robot arm, is the to-be-determined reduction parameter.

[0060] The calculation formula of the correction parameter can be as follows:

[0061] (2)

[0062] In formula (2), is the first Cartesian speed of the target sub-arm, is one of all to-be-determined reduction parameters, that is, the reduction parameter, is the corrected Cartesian speed of the target sub-arm.

[0063] Based on the above manner, the joint speed of each sub-arm in the mechanical arm can be corrected in advance or in real time during the motion control of the mechanical arm, so as to avoid the situation that the whole mechanical arm cannot operate smoothly due to the excessive joint speed of one or more sub-arms in the mechanical arm, and improve the stability of the mechanical arm during motion.

[0064] Optionally, the reduction parameter is a parameter with the largest value among all the to-be-determined reduction parameters.

[0065] Specifically, the reduction parameter can be a parameter with the largest value among all the to-be-determined reduction parameters obtained by each sub-arm based on formula (1).

[0066] Based on the above manner, the joint speed of each sub-arm in the mechanical arm can be corrected in advance or in real time during the motion control of the mechanical arm, so as to avoid the situation that the whole mechanical arm cannot operate smoothly due to the excessive joint speed of one or more sub-arms in the mechanical arm, and improve the stability of the mechanical arm during motion.

[0067] Optionally, step S13 specifically can include:

[0068] Based on the end pose data in quaternion form of the four path points, a quaternion curve formula is constructed:

[0069] (3)

[0070] wherein, , , and are end pose data in quaternion form of the four path points arranged in sequence, is a curve parameter corresponding to the target interpolation point on the path curve obtained by path fitting, and are curve parameters corresponding to the two path points with the second and third orders respectively among the four path points on the path curve obtained by path fitting, is interpolation pose data in quaternion form of the target interpolation point, and the target interpolation point is an interpolation point between the two path points with the second and third orders respectively.

[0071] Based on the quaternion curve formula, i.e., formula (3), interpolation pose data of the target interpolation point is obtained.

[0072] Specifically, the path curve can be a curve formula about curve parameter , and the curve parameter is a curve parameter corresponding to the position of the corresponding interpolation point on the path curve.

[0073] Specifically, the end pose data of the corresponding path point in the Euler angle form of the end of the mechanical arm in the Cartesian coordinate system can be obtained first, and then the end pose data in the Euler angle form can be converted into the end pose data in the quaternion form, which is substituted into formula (3) to calculate the interpolation pose data of each interpolation point. The obtained interpolation pose data is also in the quaternion form.

[0074] Based on the above manner, the interpolation pose data of each interpolation point is determined by constructing a quaternion curve formula to fit the poses of the plurality of path points, so that the poses of adjacent path points are second-order continuous, thereby improving the smoothness of the pose conversion between the interpolation points and the path points, between the interpolation points, and between the path points, and further improving the stability of the mechanical arm during movement.

[0075] Further, based on the interpolation pose data of the first interpolation point and the second interpolation point, a first Cartesian velocity corresponding to the process of the mechanical arm moving from the first interpolation point to the second interpolation point is determined, including:

[0076] Based on the interpolation position data of the first interpolation point and the interpolation position data of the second interpolation point, an interpolation distance is obtained, and the interpolation distance is divided by the interpolation time length between the first interpolation point and the second interpolation point to obtain a first linear velocity.

[0077] The interpolation pose data of the second interpolation point is divided by the interpolation pose data of the first interpolation point to obtain a rotational pose in the quaternion form, and half of the real part value of the rotational pose is multiplied by three imaginary part values of the rotational pose to obtain three product values. Based on the sum of the three product values, a first angular velocity is obtained.

[0078] The first Cartesian velocity is determined based on the first linear velocity and the first angular velocity.

[0079] Specifically, the calculation formula of the first linear velocity is as follows:

[0080] (4)

[0081] In formula (4), is the interpolation time length between any two adjacent interpolation points, is the interpolation position data of the first interpolation point, is the interpolation position data of the second interpolation point, is the first linear velocity of the end of the mechanical arm when moving from the first interpolation point to the second interpolation point.

[0082] The calculation formula of the first angular velocity is as follows:

[0083] (5)

[0084] In formula (5), is the interpolation pose data of the first interpolation point in the quaternion form, the interpolation pose data in quaternion form of the second interpolation point, a rotation attitude in quaternion form, a real part value of the rotation attitude in quaternion form, three imaginary part values of the rotation attitude in quaternion form, a first angular velocity when the robot arm end moves from the first interpolation point to the second interpolation point.

[0085] The first Cartesian velocity is composed of the first linear velocity and the first angular velocity, so the first Cartesian velocity when the robot arm end moves from the first interpolation point to the second interpolation point can be determined by the first linear velocity and the first angular velocity when the robot arm end moves from the first interpolation point to the second interpolation point.

[0086] Based on the above manner, the first Cartesian velocity of the movement of the robot arm corresponding to the adjacent interpolation points can be determined based on the interpolation pose data of the adjacent interpolation points, and subsequently the corresponding joint velocity can be determined based on the first Cartesian velocity and the Jacobian matrix of the robot arm for corresponding processing, thereby improving the accuracy of the obtained Cartesian velocity of the robot arm end and the joint velocity of each sub-arm of the robot arm.

[0087] Similarly, the calculation process of the second Cartesian velocity is similar to that of the first Cartesian velocity, and only the two adjacent interpolation points are changed from the first interpolation point and the second interpolation point to the second interpolation point and the third interpolation point, which will not be described here.

[0088] Optionally, determining the first joint velocity of each sub-arm of the robot arm based on the first Cartesian velocity comprises:

[0089] determining the first joint velocity of each sub-arm of the robot arm based on the first Cartesian velocity and the Jacobian matrix, wherein the Jacobian matrix is used to represent the relationship between the first Cartesian velocity and the first joint velocity of each sub-arm of the robot arm.

[0090] Specifically, the DH parameters between each adjacent sub-arm of the robot arm can be obtained by using the MDH method, and based on the DH parameters and the inverse kinematics algorithm of the robot arm, the correlation between the joint velocity of each sub-arm of the robot arm and the Cartesian velocity of the robot arm end can be constructed, that is, the corresponding Jacobian matrix can be constructed.

[0091] The relationship between the joint velocity of each sub-arm of the robot arm, the Cartesian velocity of the robot arm end and the Jacobian matrix is as follows:

[0092] (6)

[0093] In formula (6), ​​is the joint velocity of the n th sub-arm in the mechanical arm, is the pseudo-inverse matrix of the Jacobian matrix, is the Cartesian velocity of the end of the mechanical arm, is the x-axis line velocity of the end of the mechanical arm, is the y-axis line velocity of the end of the mechanical arm, is the z-axis line velocity of the end of the mechanical arm, is the x-axis angular velocity of the end of the mechanical arm, is the y-axis angular velocity of the end of the mechanical arm, is the z-axis angular velocity of the end of the mechanical arm.

[0094] The pseudo-inverse matrix of the Jacobian matrix can be obtained by using the Eigen or armadillo open source library interface, or other databases or methods can be used to calculate the pseudo-inverse matrix, which is not limited here.

[0095] The Jacobian matrix can specifically include a position Jacobian matrix and a pose Jacobian matrix of the mechanical arm relative to the base, and the pose Jacobian matrix can also be referred to as an angle Jacobian matrix.

[0096] Based on the above method, the joint velocity of any sub-arm of the mechanical arm can be converted based on the known Cartesian velocity of the end of the mechanical arm, thereby improving the accuracy of the motion control method.

[0097] Optionally, the step S12 can specifically include:

[0098] Based on the cubic B-spline algorithm and the end pose data of the plurality of path points, path fitting is performed to obtain a path curve.

[0099] Based on the preset speed planning model, the speed of the path curve is planned to determine the interpolation position data of the plurality of interpolation points on the path curve.

[0100] Specifically, the cubic B-spline algorithm and the end pose data of the plurality of path points are used to perform path fitting, so that a path curve containing the plurality of path points can be fitted.

[0101] And based on the preset speed planning model, the speed of the path curve is planned to determine the speed and position that the mechanical arm should reach at the corresponding time, so as to determine the time difference between the interpolation points through the preset interpolation period, and then determine the interpolation position data of each interpolation point on the path curve through the time corresponding to each interpolation point.

[0102] The curve formula of the path curve of the cubic B-spline algorithm is as follows:

[0103] (7)

[0104] In formula (7), is the curve parameter corresponding to the coordinate on the path curve, is the coordinate corresponding to the curve parameter , is the coordinate of the i-th control point.

[0105] In formula (7), is not a known quantity, and the method for calculating is as follows:

[0106] (8)

[0107] Formula (8) is a set of formulas associated with the curve formula of the path curve of the n+2 cubic B-spline algorithm constructed by using the natural boundary condition, in formula (8), is the n-th curve parameter obtained by discretizing the curve parameter of the path curve by using the chord length parameterization method, is the coordinate of the n-th path point, is the coordinate of the n-th control point.

[0108] Based on formula (8), the value of can be calculated, so that the unknown variable in formula (7) is only the curve parameter , and the value range of the curve parameter is [0, 1], the curve parameter takes 0, is the coordinate of the starting point of the curve path, and the curve parameter takes 1, is the coordinate of the end point of the curve path.

[0109] Based on the above method, formula (7) for describing the fitted path curve can be obtained, and by substituting different values of the curve parameter, the coordinates of the corresponding points on the corresponding curve can be obtained.

[0110] It should be noted that the above method for calculating the control point coordinates by using the natural boundary condition can be replaced by other methods for calculating the control point coordinates by using other boundary conditions or other methods to obtain the curve formula, which is not limited here.

[0111] The above path fitting by using the cubic B-spline algorithm is only an example, and other algorithms can also be used for path fitting, which is not limited here.

[0112] Further, the path curve is speed planned based on a preset speed planning model to determine interpolation position data of a plurality of interpolation points on the path curve, including:

[0113] Based on the time sequence, the speed planning model and the path curve, the curve parameter of the interpolation point corresponding to each time node in the time sequence is determined.

[0114] Based on the path curve and the curve parameter of the corresponding interpolation point, interpolation position data of the corresponding interpolation point is determined.

[0115] Specifically, the total length of the curve path corresponding to formula (7) can be calculated based on the Simpson integral formula:

[0116] (9)

[0117] In formula (9), is the total length of the curve path when the path curve is divided into n curve segments for length calculation, is the curve parameter described in the foregoing embodiments , is the curve parameter described in the foregoing embodiments , a is 0, and b is 1. .

[0118] After the total length corresponding to the curve path is calculated, a preset speed planning model, that is, an S-shaped speed curve model, can be used to plan the speed of the path curve.

[0119] Based on the preset maximum speed parameter, the maximum acceleration parameter and the maximum jerk parameter, a curve formula of the S-shaped speed curve can be constructed, and the segmented speed planning curve formula in the kth S-shaped speed curve model is as follows:

[0120] (10)

[0121] In formula (10), t is the movement time of the mechanical arm, is the acceleration of the end of the mechanical arm at t, is the speed of the end of the mechanical arm at t, is the displacement of the end of the mechanical arm at t, is the maximum jerk, , , .

[0122] In an example, the S-shaped speed curve model described above can be a 7-segment S-shaped speed curve model, and the first and last speeds and accelerations of the curve are all 0. In other examples, other numbers of S-shaped speed curve models can also be used for speed planning, which are not limited here.

[0123] The time sequence can be determined according to a preset interpolation time length, a time difference between each two adjacent time nodes in the time sequence is the interpolation time length, and through the curve formula and the piecewise speed planning curve formula, the speed, the acceleration and the displacement of the robot arm end corresponding to each time node in the time sequence can be determined.

[0124] The curve parameter is second-order Taylor expanded:

[0125] (11)

[0126] In formula (11), is a curve parameter of an interpolation point corresponding to the ith time node in the time sequence on the path curve, is 0, is a speed of the robot arm end of the interpolation point corresponding to the ith time node in the time sequence, is an acceleration of the robot arm end of the interpolation point corresponding to the ith time node in the time sequence, is the interpolation time length, is a first-order derivative of the described in the foregoing embodiment, is a second-order derivative of the described in the foregoing embodiment.

[0127] Based on the above manner, the curve parameter corresponding to each interpolation point can be obtained, and the curve parameter is substituted into formula (7) to obtain the coordinates of the corresponding interpolation point, that is, interpolation position data of the interpolation point.

[0128] Further, the obtained curve parameter can be estimated and corrected by using an estimation and correction method to eliminate the residual.

[0129] The first target interpolation point and the second target interpolation point are two adjacent interpolation points in the plurality of interpolation points, the first target interpolation point is an interpolation point of the first time node, and the second target interpolation point is an interpolation point of the second time node.

[0130] The first displacement of the robot arm end of the path curve at the first time node and the second displacement of the robot arm end of the path curve at the second time node can be determined based on formula (10), and the corresponding undetermined interpolation step distance is obtained by subtracting the first displacement from the second displacement.

[0131] The curve parameters of the path curves of the first target interpolation point and the second target interpolation point are substituted into formula (7) respectively to obtain two corresponding coordinates, and the actual interpolation step distance is determined based on the distance between the two coordinates.

[0132] The estimation error formula is as follows:

[0133] (12)

[0134] In formula (12), is the estimation error, is the actual interpolation step distance, is the to-be-determined interpolation step distance.

[0135] The estimation error threshold is set as , and iteration is performed based on the following formula:

[0136] (13)

[0137] In formula (13), is the iteration value, is the curve parameter of the path curve of the first target interpolation point, is the curve parameter of the path curve of the second target interpolation point.

[0138] The iteration value is iterated based on formula (13) until the estimation error is less than the estimation error threshold .

[0139] The final iteration value is taken as the curve parameter of the path curve of the second target interpolation point to perform motion control, so that the estimation correction is realized, the phenomenon that the mechanical arm moves to an area other than the expected area is avoided, and the reliability of the motion control method is improved.

[0140] The present application also provides an electronic device, referring to Figure 3 , Figure 3 is a flowchart of an embodiment of the electronic device of the present application, as shown in Figure 3 , the electronic device 20 includes a processor 21, a memory 22 and a bus 23.

[0141] The processor 21 and the memory 22 are respectively connected with the bus 23, the memory 22 stores program instructions, and the processor 21 is used to execute the program instructions to realize the motion control method of the mechanical arm in the above-mentioned embodiment.

[0142] In this embodiment, the processor 21 can also be referred to as a CPU (Central Processing Unit). The processor 21 can be an integrated circuit chip with processing capability. The processor 21 can also be a general purpose processor, a digital signal processor (DSP), an application specific integrated circuit (ASIC), a field programmable gate array (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components. The general purpose processor can be a microprocessor or the processor 21 can also be any conventional processor.

[0143] Distinguishing from the prior art, in the technical solution of the present application, the end position data and the end pose data of a plurality of path points required to be passed through by the end of the mechanical arm are acquired, path fitting is performed based on the plurality of end pose data to obtain a path curve and the path curve is subjected to speed planning to obtain interpolation position data of a plurality of interpolation points, pose fitting is performed based on the plurality of end pose data and the interpolation pose data of the plurality of interpolation points to obtain interpolation pose data of the plurality of interpolation points, the corresponding Cartesian speed is obtained based on the interpolation pose data of two adjacent interpolation points, and the joint speed of each sub-arm of the mechanical arm is determined based on the Cartesian speed. When the joint speed of the target sub-arm is greater than the preset joint speed threshold, the Cartesian speed of the target sub-arm is multiplied by the correction Cartesian speed, and the correction joint speed of the target sub-arm is determined based on the correction Cartesian speed. The target sub-arm is subjected to motion control based on the correction joint speed during the movement of the mechanical arm between two adjacent interpolation points. Based on the above manner, when the joint speed of the target sub-arm during the movement between any two interpolation points among all the interpolation points is greater than the preset joint speed threshold, the Cartesian speed of the target sub-arm is corrected based on the correction parameter to obtain the correction Cartesian speed, and then the correction joint speed is obtained based on the corrected correction Cartesian speed to control the target sub-arm, thereby avoiding the phenomenon that the mechanical arm is unstable during the movement due to the excessive joint speed of the target sub-arm, and improving the stability of the mechanical arm during the movement.

[0144] The present application also provides a computer readable storage medium, such as Figure 4 as shown in the figure, Figure 4 is a structural schematic diagram of an embodiment of the computer readable storage medium of the present application. The computer readable storage medium 30 has program instructions 31 stored thereon, and the program instructions 31 are executed by a processor (not shown in the figure) to realize the motion control method of the mechanical arm in the above embodiment.

[0145] The computer readable storage medium 30 of the present embodiment can be, but is not limited to, a storage unit in a U disk, an SD card, a PD optical drive, a mobile hard disk, a large-capacity floppy disk drive, a flash memory, a multimedia memory card, a server, an FPGA or an ASIC, etc.

[0146] Differing from the prior art, in the technical scheme of the application, end position data and end attitude data of a plurality of path points required to be passed through by the end of the mechanical arm are acquired, path fitting is performed based on the plurality of end position data to obtain a path curve and velocity planning is performed on the path curve to obtain interpolation position data of a plurality of interpolation points, attitude fitting is performed based on the plurality of end position data and interpolation attitude data of the plurality of interpolation points to obtain interpolation attitude data of the plurality of interpolation points, corresponding Cartesian velocities are obtained based on interpolation attitude data of adjacent two interpolation points, and joint velocities of each sub-arm of the mechanical arm are determined based on the Cartesian velocities, when the joint velocity of a target sub-arm is greater than a preset joint velocity threshold, the Cartesian velocity of the target sub-arm is multiplied by a correction Cartesian velocity, and a correction joint velocity of the target sub-arm is determined based on the correction Cartesian velocity, and the target sub-arm is controlled in a movement process between adjacent two interpolation points based on the correction joint velocity. Based on the above manner, when the joint velocity of the target sub-arm is greater than the preset joint velocity threshold in a movement process between any two interpolation points among all the interpolation points, the Cartesian velocity of the target sub-arm is corrected based on a correction parameter to obtain a correction Cartesian velocity, and then a correction joint velocity is obtained based on the corrected correction Cartesian velocity to control the target sub-arm, so that the phenomenon of instability of the mechanical arm in the movement process caused by the excessively large joint velocity of the target sub-arm is avoided, and the stability of the mechanical arm in the movement process is improved.

[0147] In the description of the present application, the description of the terms "one embodiment", "some embodiments", "an example", "a specific example", or "some examples" and the like means that the specific features, structures, materials or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present application. In the present specification, the illustrative description of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any appropriate manner in any one or more embodiments or examples. In addition, different embodiments or examples described in the specification and the features of different embodiments or examples can be combined and combined by those skilled in the art without contradiction.

[0148] In addition, the terms "first", "second" are only for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the number of indicated technical features. Therefore, the features defined with "first", "second" can explicitly or implicitly include at least one of the features. In the description of the present application, the meaning of "a plurality of" is at least two, for example, two, three, etc., unless otherwise explicitly specified.

[0149] Any process or method described in the flowchart or otherwise herein can be understood as representing a module, segment, or portion of code comprising one or more executable instructions for implementing a particular logical function or process, and the scope of the preferred embodiments of this application includes additional implementations in which functions may be performed not in the order shown or discussed, including substantially simultaneously or in reverse order depending on the function involved, as will be understood by those skilled in the art to which embodiments of this application pertain.

[0150] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a ordered list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (which may be a personal computer, server, network device, or other system that can fetch and execute instructions from, an instruction execution system, apparatus, or device). For the purposes of this specification, "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transmit programs for use by, or in conjunction with, an instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of computer-readable media include: an electrical connection having one or more wires (electronic device), a portable computer disk drive (magnetic device), random access memory (RAM), read-only memory (ROM), erasable and editable read-only memory (EPROM or flash memory), fiber optic devices, and portable optical disc read-only memory (CDROM). Alternatively, the computer-readable medium may be paper or other suitable media 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, interpreting, or otherwise processing as necessary, and then stored in a computer memory.

[0151] The above description is merely an embodiment of this application and does not limit the patent scope of this application. Any equivalent structural or procedural transformations made using the content of this application's specification and drawings, or direct or indirect applications in other related technical fields, are similarly included within the patent protection scope of this application.

Claims

1. A motion control method for a robotic arm, characterized in that, The method comprises: acquiring end pose data of a plurality of path points of a robot arm, the end pose data comprising end position data and end attitude data of the corresponding path points; based on the plurality of end pose data, path fitting and velocity planning are performed to obtain interpolation position data of a plurality of interpolation points; based on the plurality of end attitude data and the interpolation position data of the plurality of interpolation points, attitude fitting is performed to obtain interpolation attitude data of the plurality of interpolation points; based on interpolation pose data of a first interpolation point and a second interpolation point, a first Cartesian velocity corresponding to a process in which the robot arm moves from the first interpolation point to the second interpolation point is determined, and based on interpolation pose data of the second interpolation point and a third interpolation point, a second Cartesian velocity corresponding to a process in which the robot arm moves from the second interpolation point to the third interpolation point is determined; wherein the interpolation pose data comprises the interpolation position data and the interpolation attitude data of the corresponding interpolation points; based on the first Cartesian velocity, a first joint velocity of each sub-arm of the robot arm is determined, and based on the second Cartesian velocity, a second joint velocity of each sub-arm of the robot arm is determined; if the first joint velocity of a target sub-arm is greater than a preset joint velocity threshold, a correction parameter is multiplied by the first Cartesian velocity to obtain a corrected Cartesian velocity, and based on the corrected Cartesian velocity, a corrected joint velocity of the target sub-arm is determined, wherein the correction parameter is a difference value obtained by subtracting a reduction parameter from 1, the reduction parameter is a parameter in all to-be-determined reduction parameters, each to-be-determined reduction parameter corresponds to a sub-arm, the to-be-determined reduction parameter is a quotient value obtained by dividing a first value by a second value, the first value is an absolute value of a difference between the first joint velocity of the corresponding sub-arm and the preset joint velocity threshold, and the second value is an absolute value of a difference between the first joint velocity of the corresponding sub-arm and the second joint velocity of the corresponding sub-arm; wherein the target sub-arm is one of all the sub-arms of the robot arm. based on the corrected joint velocity, the target sub-arm is controlled to move in the process in which the robot arm moves from the first interpolation point to the second interpolation point.

2. The motion control method according to claim 1, characterized by, The reduction parameter is the largest parameter in all to-be-determined reduction parameters.

3. The motion control method according to claim 1 or 2, characterized by, The attitude fitting based on the plurality of end attitude data and the interpolation position data of the plurality of interpolation points to obtain the interpolation attitude data of the plurality of interpolation points comprises: based on the quaternion form of the end attitude data of four path points, the curve parameter of a target interpolation point on the path curve obtained by the path fitting, the curve parameters of two path points with second and third orders in the four path points on the path curve obtained by the path fitting, and the interpolation attitude data of the target interpolation point in the form of a quaternion, a quaternion curve formula is constructed; wherein the target interpolation point is an interpolation point located between the two path points with the second and third orders; based on the quaternion curve formula, the interpolation attitude data of the target interpolation point is obtained.

4. The motion control method according to claim 3, wherein The interpolation pose data based on the first interpolation point and the second interpolation point is used to determine a first Cartesian speed corresponding to a movement of the robot arm from the first interpolation point to the second interpolation point, including: The interpolation position data of the first interpolation point and the interpolation position data of the second interpolation point are used to obtain an interpolation distance, and the interpolation distance is divided by an interpolation time length between the first interpolation point and the second interpolation point to obtain a first linear speed; The interpolation pose data of the second interpolation point is divided by the interpolation pose data of the first interpolation point to obtain a rotation attitude in the form of a quaternion, and half of a real part value of the rotation attitude is multiplied by three imaginary part values of the rotation attitude to obtain three product values, and a first angular speed is obtained based on a sum of the three product values. The first Cartesian speed is used to determine a first joint speed of each sub-arm of the robot arm, including:

5. The motion control method according to claim 1 or 2, characterized by, The first Cartesian speed and a Jacobian matrix are used to determine the first joint speed of each sub-arm of the robot arm, wherein the Jacobian matrix is used to represent a relationship between the first Cartesian speed and the first joint speed of each sub-arm of the robot arm. The path fitting and speed planning are performed based on the plurality of end pose data to obtain interpolation position data of a plurality of interpolation points, including:

6. The motion control method according to claim 1 or 2, characterized by, The path fitting is performed based on a cubic B-spline algorithm and the end pose data of the plurality of path points to obtain a path curve; The speed planning is performed on the path curve based on a preset speed planning model to determine the interpolation position data of the plurality of interpolation points on the path curve. The speed planning is performed on the path curve based on a preset speed planning model to determine the interpolation position data of the plurality of interpolation points on the path curve, including:

7. The motion control method according to claim 6, wherein Curve parameters of an interpolation point corresponding to each time node in a time sequence are determined based on the time sequence, the speed planning model and the path curve; Interpolation position data of the corresponding interpolation point is determined based on the path curve and the curve parameters of the corresponding interpolation point. including:

8. An electronic device, comprising: a memory and a processor; the memory is used to store program instructions, and the processor is used to execute the program instructions to implement the method according to any one of claims 1 to 7. The computer readable storage medium stores program instructions, and the program instructions are executed by the processor to implement the method according to any one of claims 1 to 7.

9. A computer-readable storage medium, characterized in that, ​

Images