A method for calculating a motion path of a servo robot arm

Abstract

The application discloses a kind of servo mechanical arm movement path calculation method, this method with the motion point of mechanical arm end on target straight line path as virtual main shaft, swing shaft and telescopic shaft are all set as slave shaft, and trajectory is calculated with two positioning points geometric constraint, and with speed projection decomposition distribution two shaft speed;The application cancels the dependence of fixed main shaft and fixed zero point, simplifies the path calculation of composite motion straight line, ensures that end speed is uniform, and realizes that zero point is offset without special tool, can be quickly and automatically compensated.

Description

Technical Field

[0001] This invention relates to the field of motion control technology for servo robotic arms, and in particular to a method for calculating the motion path of a servo robotic arm. Background Technology

[0002] When a servo robotic arm performs a combined telescopic and swinging motion in a straight line, the movement trajectories of the swing and telescopic axes need to be controlled in a coordinated manner. Existing technologies already have relevant solutions, such as the motion synthesis method and motion device disclosed in patent CN102692930A. This method uses the swing axis of the servo robotic arm as the master axis and the telescopic axis as the slave axis. By causing the master axis to swing at a constant speed and calculating the corresponding telescopic length and speed of the telescopic axis in real time based on the coordinate position of the swing axis, the ends of the two servo axes can move along a straight path.

[0003] The existing solution described above sets the swing axis as the master axis with uniform motion, while the telescopic axis passively follows as the slave axis. To ensure that the final composite trajectory is a straight line, the speed of the telescopic axis needs to be continuously changed to fit the position of the master axis. This results in the resultant velocity of the robotic arm's end effector fluctuating throughout the entire linear motion, making it impossible to achieve uniform motion. Furthermore, the motion trajectory calculation of this solution is highly dependent on the accuracy of the zero-point positions of the swing and telescopic axes. If either servo axis experiences a zero-point shift during debugging or maintenance, the motion trajectory calculated based on the original zero-point calibration will deviate from the predetermined straight path and become an arc trajectory. Accurately restoring the zero point to its original position under on-site conditions without high-precision measuring instruments is extremely difficult and time-consuming.

[0004] Therefore, in response to the problems mentioned above, this invention proposes a method for calculating the motion path of a servo robotic arm. Summary of the Invention

[0005] To overcome the problem of difficulty in achieving both speed uniformity and zero-point tolerance caused by the fixed master-slave axis allocation mode, this invention proposes a method for calculating the motion path of a servo robotic arm. The method uses the motion point on the virtual linear axis as the virtual master axis and the swing axis and telescopic axis as slave axes to cooperate in coordination. Path planning and speed allocation can be completed without relying on a fixed physical zero point.

[0006] The technical solution of this invention is: a method for calculating the motion path of a servo robotic arm, comprising the following steps: S1, take the point on the target straight path where the end of the servo robotic arm moves as the virtual master axis, and take the swing axis and telescopic axis of the servo robotic arm as slave axes. The swing axis and telescopic axis move in coordination with the virtual master axis. The target straight path is the straight path of the drill rod held by the end of the robotic arm moving in the vertical direction at the target column position of the drill rod box when the servo robotic arm performs drill rod gripping or drill rod placement operations. S2, control the end effector of the servo robotic arm to move to the first positioning point and the second positioning point on the target straight path, and read and record the first swing axis angle value and the first telescopic axis length value corresponding to the first positioning point, and the second swing axis angle value and the second telescopic axis length value corresponding to the second positioning point, wherein the first positioning point and the second positioning point are any two different positions on the target straight path, the first swing axis angle value and the second swing axis angle value are both angle values ​​relative to the current zero position of the servo robotic arm, and the first telescopic axis length value and the second telescopic axis length value are both length values ​​relative to the current zero position of the servo robotic arm; S3, based on the coordinate values ​​of the first positioning point and the second positioning point, take the rotation center of the servo robotic arm swing axis as point O, the target straight path as line L, the first positioning point as point A, the second positioning point as point B, point E as the foot of the perpendicular line connecting point O and line L, and point D as any target point on the target straight path. Let the length of the telescopic axis of point A be a, the length of the telescopic axis of point B be b, the difference between the swing axis angles of the two positioning points A and B be c, the swing axis angle of point D be y, and the length of the telescopic axis of point D be d. Point E is the point perpendicular to the line connecting point O and line L.

[0007] According to the cosine formula, we can obtain: ; The angle x of the oscillation axis corresponding to point E can be obtained from the coordinate relationship between points A and B using the cosine formula: because: ; so: ; Simplifying, we get: ; Divide both sides by have to: ; Simplifying, we get: ; Find: ; Calculate the length d of the extension axis of any target point D on the target straight path based on the swing axis angle value x corresponding to point E: Substitute the swing axis angle y and the extension axis value d of the target point D into... We can obtain: ; Finally, we obtained: ; S4, set the linear velocity of the servo robotic arm's end effector on the target straight path to be v, and the linear velocity component in the swing axis direction to be v. The linear velocity component in the direction of the telescoping axis is The oscillation radius of the servo oscillation axis is d, therefore the oscillation speed of the servo oscillation axis is... .

[0008] Preferably, when the zero position of the swing axis or telescopic axis of the servo robotic arm is offset, the end effector of the servo robotic arm is re-controlled to move to two positions on the target straight path. The swing axis angle value and telescopic axis length value corresponding to the two offset positions are read and recorded as new positioning point coordinates. The swing axis angle value and telescopic axis length value of any point on the target straight path are recalculated based on the new positioning point coordinates.

[0009] Preferably, the servo robotic arm is a multi-axis servo robotic arm installed on an intelligent drilling rig, used to perform automatic addition and removal of drill rods. When the servo robotic arm grabs a drill rod from the drill rod box or puts a drill rod into the drill rod box, the calculation method is used to control the combined motion of the swing axis and the telescopic axis of the servo robotic arm, so that the drill rod held at the end of the robotic arm moves along a straight path.

[0010] The beneficial effects of this invention are: 1. This invention uses the motion point on the virtual straight axis as the virtual master axis, and sets the swing axis and extension axis as slave axes. Based on the geometric constraints of the two positioning points, the axis coordinates of any target point are directly calculated. This invention abandons the dependence of the traditional master axis and slave axis fixed allocation mode on the assumption of uniform speed of the master axis, effectively simplifies the path calculation of composite motion in a straight line, and ensures that the deviation between the actual trajectory and the theoretical straight line is minimal.

[0011] 2. This invention does not specify a fixed zero point for the swing axis and telescopic axis. It only needs to collect the coordinates of any two current position points on the straight path to reconstruct the trajectory parameters of the entire path. After the servo axis zero point shifts, zero point compensation can be automatically completed without the need for any high-precision measuring tools, which greatly shortens the zeroing time during debugging and maintenance.

[0012] 3. This invention decomposes the linear velocity of the end effector along the swing and extension directions by geometric projection, and calculates the angular velocity of the swing axis and the axial velocity of the extension axis respectively. This enables the two slave axes to cooperate in the virtual master axis motion, avoiding nonlinear changes in the slave axis velocity caused by the uniform speed of the master axis, thereby ensuring that the speed of the end effector of the robotic arm is constant and uniform throughout the entire linear motion process. Attached Figure Description

[0013] Figure 1 The diagram shows the relative positions of the servo robotic arm and the drill rod box of the present invention. Figure 2The diagram shown is an equivalent geometric model of the present invention; Figure 3 The diagram shown is a schematic diagram of the linear motion velocity decomposition of the present invention. Detailed Implementation

[0014] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are some embodiments of the present invention, but not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0015] This invention provides an embodiment: This embodiment describes an intelligent drilling rig equipped with drill rod boxes arranged in columns, which utilize a multi-axis servo robotic arm for automatic drill rod loading and unloading. During automatic drilling operations, the servo robotic arm uses multi-axis composite motion to sequentially grab drill rods from the drill rod boxes and load them into the main unit of the drilling rig. During automatic unloading operations, the servo robotic arm uses multi-axis composite motion to remove drill rods from the main unit of the drilling rig and sequentially place them back into the drill rod boxes.

[0016] When the servo robotic arm grabs and puts drill rods into the drill rod box, it needs to use the combined motion of the servo robotic arm's swing axis and telescopic axis to ensure that the drill rod grabbed by the robotic arm moves in a straight line in the vertical direction at the target column position in the drill rod box, so that the grabbed drill rod can be successfully taken out and put in.

[0017] Please see Figure 1 The diagram shows the relative positions of the servo robotic arm and the drill rod box of the intelligent drilling rig. In this application scenario, the drill rods in the drill rod box are arranged vertically in columns, and the center line connecting the centers of each column of drill rods forms a straight line in the vertical direction. The servo robotic arm needs to sequentially grasp the drill rods from each column in the drill rod box. This requires the end effector of the robotic arm to move along the direction of the center line connecting the drill rods in that column (i.e., the vertical straight line direction) to ensure that the drill rods can smoothly enter and exit the drill rod box without interfering with the box. Due to the structural characteristics of the servo robotic arm itself, the movement of its end effector in space is a combination of the rotational motion of the swing axis and the linear motion of the telescopic axis. Therefore, it is necessary to calculate the angular position and extension length of the swing axis and the telescopic axis at each moment so that the combined end effector trajectory is exactly a straight line in space.

[0018] Please see Figure 2To calculate the aforementioned composite motion path, this invention establishes an equivalent geometric model of a servo robotic arm. In this model, the rotation center of the servo robotic arm's swing axis is point O, the virtual main axis is the target straight path L, points A and B are two positioning points on the virtual main axis L, point E is the foot of the perpendicular line connecting the rotation center O and the virtual main axis L, and point D is any target point on the virtual main axis L.

[0019] In the above geometric model, a is the length of the telescopic axis of the servo robotic arm at positioning point A, b is the length of the telescopic axis of the servo robotic arm at positioning point B, c is the difference between the swing axis angles at positioning points A and B, x is the swing axis angle at the perpendicular point E, y is the swing axis angle at the target point D, and d is the telescopic axis length at the target point D.

[0020] In the above geometric model, points A and B are two known positioning points on the virtual principal axis L, and their corresponding telescopic axis lengths a and b, and the oscillation axis angle difference c, are all known quantities. The perpendicular point E is the foot of the perpendicular from the rotation center O to the straight line L, therefore OE⊥L. The target point D is any point on the straight line L, and its corresponding oscillation axis angle y is the independent variable, while the telescopic axis length d is the dependent variable to be determined.

[0021] After establishing the equivalent geometric model, the core step of this invention is to calculate the functional relationship between the swing axis angle value y and the telescopic axis length value d of any target point D on the target straight path L, based on the known coordinate values ​​of the two positioning points A and B.

[0022] Since point E is the foot of the perpendicular line connecting the center of rotation O and line L, OE⊥L, and points A, E, B, and D all lie on line L. In triangle OAE, based on the law of cosines and the side-angle relationships of a right triangle, the following geometric constraint equations can be established: Since points A and B both lie on line L, and OE⊥L, point E is the point on line L that is closest to point O. In right triangle OAE, we have: ; In triangle OBE, we have: ; Since both of the above equations are equal to OE, we can obtain: ; According to the formula for the sum of trigonometric functions: ; Substituting the above expression into the previous equation, we get: ; Expand and organize: ; Move the terms containing cos x to the left side of the equals sign: ; Dividing both sides of the equation by cos x, we get: ; Further analysis reveals: ; Therefore, the angle of the swing axis corresponding to the perpendicular foot point E can be calculated: ; Please see Figure 3 After obtaining x, for any target point D on the target straight path L, since point D is also on line L, in right triangle OED, we also have: ; Therefore, the length of the telescoping axis corresponding to the target point D can be obtained: ; Substituting the expression for x, we finally get: ; The above formula represents the precise functional relationship between the servo telescopic axis length value d and the servo swing axis angle value y at the target position when the end effector of the servo robotic arm moves along the virtual linear spindle. In actual control, once the swing axis angle value y of the target point D is given, the corresponding telescopic axis length value d can be uniquely determined using the above formula, thereby achieving coordinated control of the swing axis and the telescopic axis.

[0023] Furthermore, during initial use or system initialization, the operator controls the end effector of the servo robotic arm to move to the first positioning point A on the target straight path. At this time, the control system reads and records the current swing axis angle value and telescopic axis length value, respectively denoted as... and Then, the servo robotic arm's end effector moves to the second positioning point B on the target straight path. Similarly, the current swing axis angle value and telescopic axis length value are read and recorded, denoted as follows: and Based on the coordinates of these two positioning points, the difference c between the swing axis angles of positioning points A and B can be calculated. - The length of the telescopic shaft at positioning point A is a = The length of the telescopic shaft at positioning point B, b = .

[0024] By substituting a, b, and c into the aforementioned trajectory calculation formula, the functional relationship between the swing axis angle value and the telescopic axis length value of any target point on the target straight path L can be completely determined, thereby achieving precise control of any position on the straight path.

[0025] When a servo robotic arm experiences zero-point offset during subsequent debugging or maintenance, existing technologies typically require recalibrating the zero-point position using high-precision measuring instruments. This process is not only time-consuming and labor-intensive but also demands a high level of skill from the operator. However, this invention eliminates the need for zero-point restoration. The operator simply repositions the servo robotic arm's end effector to any two positions on the target straight path (not necessarily coinciding with the original positioning points A and B), reads and records the swing axis angle and extension axis length values ​​corresponding to the two offset positions as the new positioning point coordinates. The control system can then recalculate the swing angle and extension length values ​​at any point on the virtual straight line based on these new coordinates.

[0026] This automatic zero-point offset compensation function allows the trajectory calculation method of this invention to be completely independent of the absolute zero-point position of the swing axis or telescopic axis, relying only on the relative geometric relationship between two positioning points. Regardless of how the servo axis's zero point offsets, as long as the two positioning points remain on the same straight path, the relative angle difference c and relative length values ​​a and b between them contain all the geometric information of that straight path. Therefore, even if the zero point offsets, precise control of the straight path can be restored simply by re-acquiring the current coordinate values ​​of the two positioning points.

[0027] To further verify the effectiveness of the invention, this example conducted systematic simulation comparison experiments and actual bench tests. The experiments included trajectory accuracy verification experiments, velocity uniformity comparison experiments, and zero-point offset compensation verification experiments.

[0028] (1) First, a trajectory accuracy verification experiment was conducted. The purpose of this experiment was to verify the deviation between the actual motion trajectory of the end of the robotic arm and the theoretical straight path when the servo robotic arm was controlled by the calculation method of the present invention to perform a combined telescopic and swinging motion.

[0029] This experiment constructed a servo robotic arm experimental platform, which includes a swing axis and a telescopic axis, both driven by servo motors and equipped with a high-precision absolute encoder for reading the actual position. A laser displacement sensor and a high-precision vision measurement system were installed at the end of the robotic arm to detect the actual position coordinates of the end in space in real time. A target straight path was defined, with two endpoints, P1 and P2. According to the calculation method of this invention, the coordinate values ​​(a, b, c) of the two positioning points P1 and P2 were collected, and the functional relationship between the swing axis angle value y and the telescopic axis length value d of each target point on the entire path was calculated. Then, the end of the robotic arm was controlled to move from point P1 to point P2 along the straight path. During the movement, the actual spatial coordinate values ​​of the end were recorded at fixed time intervals. The results are shown in Table 1. Table 1. Trajectory accuracy verification experiment

[0030] Table 1 shows that the maximum radial deviation between the actual position of the robotic arm's end effector and the theoretical straight path is 0.085 mm, and the average radial deviation is 0.052 mm, along the entire linear motion path. This result indicates that, using the calculation method of this invention, the deviation between the actual trajectory of the end effector and the theoretical straight path is extremely small when the servo robotic arm performs a combined telescopic and swinging motion, fully meeting the application requirements for high-precision linear motion.

[0031] (2) This example is a trajectory accuracy verification experiment. The purpose of this experiment is to compare the speed uniformity of the servo robot arm end moving along the straight path when using the method of this invention and the existing technology (spindle uniform speed method).

[0032] In this experiment, on the same servo robotic arm experimental platform, the method of this invention and the spindle constant speed method were used to control the end effector of the robotic arm to move along the same straight path from the starting point to the ending point, with the target linear velocity set at v = 100 mm / s. During the movement, a laser velocimeter was used to continuously measure the instantaneous velocity of the end effector of the robotic arm at a sampling frequency of 1 kHz, and the velocity data throughout the entire movement process were recorded. The results are shown in Table 2. Table 2 Comparison of Velocity Uniformity Experiments

[0033] As shown in Table 2, when using the method of this invention, the end effector speed of the robotic arm fluctuates between 98.56 mm / s and 101.23 mm / s, with a fluctuation range of only ±2.67% and a speed standard deviation of 0.78 mm / s, exhibiting excellent speed uniformity. In contrast, when using the spindle constant speed method, the end effector speed fluctuates drastically between 82.34 mm / s and 115.67 mm / s, with a fluctuation range as high as ±33.33% and a speed standard deviation of 9.62 mm / s.

[0034] The above comparison results show that the existing constant-speed spindle method, by setting the oscillating axis as the main axis and forcing it to move at a constant speed, requires the telescopic axis, as the slave axis, to continuously change its speed to compensate for the geometric offset caused by the main axis's movement. When the rate of change of the oscillating axis angle is constant, the rate of change of the telescopic axis's speed is actually non-linear, which leads to significant fluctuations in the end-effector velocity after the two axes are combined. In contrast, this invention uses both axes as slave axes to cooperate with the virtual main axis movement, and the speed distribution is based on the geometric projection relationship, thereby ensuring a constant end-effector velocity.

[0035] (3) This experiment is a zero-point offset compensation verification experiment. This experiment is to verify the effectiveness of the present invention in automatically completing zero-point compensation by re-acquiring two positioning points after the servo axis zero point offset occurs.

[0036] This invention first acquires the coordinate values ​​of positioning points A and B according to the calculation method of this invention, under the initial zero-point state of the servo robotic arm, establishes the trajectory parameters of the target straight path, and controls the robotic arm to move along the straight path once, recording the trajectory accuracy as benchmark data. Then, a zero-point offset is artificially created, simulating a zero-point offset of +2° for the swing axis and -5mm for the telescopic axis. After the zero-point offset occurs, without performing any traditional zero-point calibration operation, the automatic zero-point offset compensation steps of this invention are directly followed to re-control the robotic arm's end effector to move to the two positions on the target straight path, acquire new positioning point coordinate values, recalculate the trajectory parameters, and then control the robotic arm to move along the same straight path again, measuring the trajectory accuracy. The results are shown in Table 3. Table 3 Zero-point offset compensation verification experiment

[0037] As shown in Table 3, after the swing axis and telescopic axis experience zero-point offset, without any compensation, the trajectory of the robotic arm's end effector moving along the target straight path will deviate significantly, with a maximum radial deviation of 8.670 mm. The trajectory completely deviates from the predetermined straight path, becoming an arc. Using the automatic zero-point offset compensation method of this invention, only about 15 seconds of operation time is required, without any special measuring tools, to restore the maximum radial deviation to 0.092 mm and the average radial deviation to 0.058 mm, essentially matching the trajectory accuracy in the initial zero-point state. In contrast, the traditional zero-point recalibration method requires high-precision measuring instruments, takes approximately 600 seconds (10 minutes) of operation, and demands a very high level of operator skill.

[0038] The above are merely preferred embodiments of the present invention and are not intended to limit the present invention in any other way. Any person skilled in the art may make changes or modifications to the above-disclosed technical content to create equivalent embodiments that can be applied to other fields. However, any simple modifications, equivalent changes, and modifications made to the above embodiments based on the technical essence of the present invention without departing from the scope of the present invention shall still fall within the protection scope of the present invention.

Claims

1. A method for calculating the motion path of a servo robotic arm, characterized in that, Includes the following steps: S1, taking the point where the end of the servo robotic arm moves on the target straight path as the virtual master axis, and taking the swing axis and telescopic axis of the servo robotic arm as slave axes, and the swing axis and telescopic axis move in coordination with the virtual master axis; S2, control the end effector of the servo robotic arm to move to the first positioning point and the second positioning point on the target straight path, and read and record the first swing axis angle value and the first telescopic axis length value corresponding to the first positioning point, and the second swing axis angle value and the second telescopic axis length value corresponding to the second positioning point; S3, based on the coordinate values ​​of the first positioning point and the second positioning point, calculate the swing axis angle value and the extension axis length value of any target point on the target straight path; S4. Based on the swing axis angle value and telescopic axis length value of the target point on the target straight path, and the set linear velocity of the robotic arm end linear motion, calculate the angular velocity of the servo swing axis and the linear velocity of the servo telescopic axis.

2. The method for calculating the motion path of a servo robotic arm according to claim 1, characterized in that: In step S3, the rotation center of the servo robotic arm swing axis is taken as point O, the target straight path is taken as straight line L, the first positioning point is taken as point A, the second positioning point is taken as point B, point E is taken as the foot of the perpendicular line connecting point O and straight line L, and point D is taken as any target point on the target straight path. Let the length of the telescopic axis of point A be a, the length of the telescopic axis of point B be b, the difference between the swing axis angles of the two positioning points A and B be c, the swing axis angle of point D be y, and the length of the telescopic axis of point D be d. Point E is the perpendicular point of the line connecting point O and line L. According to the cosine formula, we can obtain: 。 3. The method for calculating the motion path of a servo robotic arm according to claim 2, characterized in that, In step S3, the angle value x of the swing axis corresponding to point E is obtained from the coordinate relationship between point A and point B using the cosine formula: because: ； so: ； Simplifying, we get: ； Divide both sides by have to: ； Simplifying, we get: ； Find: 。 4. The method for calculating the motion path of a servo robotic arm according to claim 3, characterized in that, In step S3, the length d of the extension axis of any target point D on the target straight path is obtained based on the swing axis angle value x corresponding to point E: Substitute the swing axis angle y and the extension axis value d of the target point D into... We can obtain: ； Finally, we obtained: 。 5. The method for calculating the motion path of a servo robotic arm according to claim 1, characterized in that: In step S4, the linear velocity of the servo robotic arm's end effector on the target straight path is set to v, and the linear velocity component in the swing axis direction is... The linear velocity component in the direction of the telescoping axis is The oscillation radius of the servo oscillation axis is d, therefore the oscillation speed of the servo oscillation axis is... .

6. The method for calculating the motion path of a servo robotic arm according to claim 1, characterized in that: In step S2, the first positioning point and the second positioning point are any two different positions on the target straight path. The angle values ​​of the first swing axis and the second swing axis are both angle values ​​relative to the current zero position of the servo robot arm. The length values ​​of the first telescopic axis and the second telescopic axis are both length values ​​relative to the current zero position of the servo robot arm.

7. The method for calculating the motion path of a servo robotic arm according to claim 1, characterized in that: When the zero position of the swing axis or telescopic axis of the servo robotic arm is offset, the end effector of the servo robotic arm is re-controlled to move to two positions on the target straight path. The swing axis angle value and telescopic axis length value corresponding to the two offset positions are read and recorded as the new positioning point coordinates. The swing axis angle value and telescopic axis length value of any point on the target straight path are recalculated based on the new positioning point coordinates.

8. The method for calculating the motion path of a servo robotic arm according to claim 1, characterized in that: The target straight path is the straight path along which the drill rod held by the end of the servo robotic arm moves in the vertical direction at the target column position in the drill rod box when the servo robotic arm performs drill rod gripping or drill rod placement operations.

9. A method for calculating the motion path of a servo robotic arm according to any one of claims 1-8, characterized in that: The servo robotic arm is a multi-axis servo robotic arm installed on an intelligent drilling rig, used to perform the addition and removal of drill rods.

10. The method for calculating the motion path of a servo robotic arm according to claim 9, characterized in that: When the servo robotic arm picks up or puts a drill rod into the drill rod box, the calculation method described above is used to control the combined motion of the swing axis and the telescopic axis of the servo robotic arm, so that the drill rod held at the end of the robotic arm moves along a straight path.

Images