Six-degree-of-freedom parallel leg robot trajectory planning method based on semicircular interpolation algorithm

Abstract

The invention belongs to the technical field of six-degree-of-freedom parallel leg robot motion control, and discloses a six-degree-of-freedom parallel leg robot trajectory planning method based on a semicircular interpolation algorithm, and the method comprises the steps: selecting a reference motion unit; establishing a space rectangular coordinate system by taking the center of the upper surface of the public platform as an original point and taking the plane where the upper surface of the public platform is located as an XOY plane; the current position coordinates of the feet of the reference motion unit are obtained, and the target position coordinates of the feet of the reference motion unit in the next step are determined; taking a connecting line of the target position coordinate and the current position coordinate as a diameter, constructing a semicircle on a vertical plane perpendicular to the XOY surface, and taking the semicircle as a target motion track of the foot of the reference motion unit; setting an interpolation number, and determining coordinates of each interpolation point on the target motion track according to the interpolation number; and sequentially passing through the coordinates of the interpolation points from the current position coordinates to the target position coordinates to form the next motion track of the foot of the reference motion unit.

Description

Technical Field

[0001] The present invention belongs to the technical field of motion control of a six-degree-of-freedom parallel-legged robot, and in particular relates to a trajectory planning method for a six-degree-of-freedom parallel-legged robot based on a semicircle interpolation algorithm. Background Art

[0002] Six-degree-of-freedom parallel-legged robots, due to their high stiffness, high load capacity, and multi-degree-of-freedom motion characteristics, are widely used in precision positioning, driver simulators, aerospace, medical surgical robots, and high-precision industrial processing. However, existing technologies still face numerous challenges in practical applications. Traditional trajectory planning methods often use linear interpolation algorithms. While simple to implement, the resulting path is composed of a series of straight line segments, resulting in an uneven motion path. In particular, at inflection points, the velocity and acceleration of the end effector are prone to sudden changes. These changes can induce mechanical vibration, reducing motion accuracy and potentially causing additional wear on the mechanical structure, shortening the device's service life. Furthermore, linear interpolation algorithms struggle to generate complex curved trajectories, limiting their application in high-precision operations. Furthermore, dynamic adaptability is poor, as existing interpolation parameters are typically fixed and cannot be dynamically adjusted to actual working conditions. For example, when the robot's load changes (such as an increase or decrease in load) or encounters sudden external disturbances (such as collisions or external impacts), fixed interpolation parameters can significantly increase trajectory tracking errors and may even cause the robot to lose stability. This lack of dynamic adaptability severely limits the application of robots in complex environments, especially in scenarios requiring high precision and high reliability. Summary of the Invention

[0003] The purpose of the present invention is to overcome the defects of the prior art and provide a trajectory planning method for a six-degree-of-freedom parallel-legged robot based on a semicircular interpolation algorithm, which can convert the foot motion trajectory of the six-degree-of-freedom parallel-legged robot from three-dimensional space to a two-dimensional plane, and perform semicircular path planning on the robot's motion trajectory on the two-dimensional plane, which can significantly improve the smoothness and flexibility of the robot's motion.

[0004] The technical solution provided by the present invention is: A trajectory planning method for a six-degree-of-freedom parallel-legged robot based on a semicircle interpolation algorithm comprises the following steps: Step 1: Select any motion unit in the robot's outer leg as the reference motion unit; Step 2: Establish a spatial rectangular coordinate system with the center of the upper surface of the public platform as the origin and the plane where the upper surface of the public platform is located as the XOY plane; Step 3: Obtain the current position coordinates of the foot of the reference motion unit and determine the next target position coordinates of the foot of the reference motion unit; construct a semicircle on a vertical plane perpendicular to the XOY plane using the line connecting the target position coordinates and the current position coordinates as the diameter, and use the semicircle as the target motion trajectory of the foot of the reference motion unit; wherein the semicircle is located above the diameter; Step 4: setting an interpolation number, and determining the coordinates of each interpolation point on the target motion trajectory according to the interpolation number; From the current position coordinates, through the coordinates of each interpolation point in sequence, to the target position coordinates, a next movement trajectory of the foot of the reference motion unit is formed; Steps 2 to 4 are repeated in a loop to gradually plan the foot of the reference motion unit until the robot moves to the planning end point, thereby obtaining the motion trajectory of the robot.

[0005] Preferably, in step 3, the method for determining the next target position coordinates of the foot of the reference motion unit is: The projection of the line connecting the planned end point and the center of the upper surface of the common platform on the XOY plane is used as the baseline, and the direction toward the planned end point on the baseline is used as the travel direction; the position reached by the foot of the reference motion unit moving one step length from the current position along a distance parallel to the travel direction is used as the next target position coordinate of the foot of the reference motion unit.

[0006] Preferably, the step length is set in the range of 80 mm to 200 mm.

[0007] Preferably, in step 4, the interpolation number is set to: ; in, is the chord height error, is the radius of the target motion trajectory.

[0008] Preferably, in step 2, the projection of the bisector of the angle between the axes of the two electric cylinders in the reference motion unit on the upper surface of the common platform is used as the X-axis, and the direction pointing to the bisector is used as the positive direction of the X-axis; the plane where the X-axis is located on the upper surface of the common platform is rotated 90 degrees counterclockwise to obtain the Y-axis; and the upward direction perpendicular to the XOY plane is used as the positive direction of the Z-axis.

[0009] The beneficial effects of the present invention are: The trajectory planning method for a six-degree-of-freedom parallel-legged robot based on a semicircular interpolation algorithm provided by the present invention can convert the foot motion trajectory of the six-degree-of-freedom parallel-legged robot from three-dimensional space into a two-dimensional plane, and perform semicircular path planning on the robot's motion trajectory on the two-dimensional plane, which significantly improves the smoothness and flexibility of the robot's motion and improves the robot's adaptability in complex dynamic scenes; there are no operations such as integration and differentiation in the path planning process, which reduces the calculation difficulty and thus improves the path planning efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0010] Figure 1 This is a schematic diagram of the overall structure of the six-degree-of-freedom parallel leg robot described in the present invention.

[0011] Figure 2 Schematic diagram of a spatial rectangular coordinate system in an embodiment of the present invention.

[0012] Figure 3 Schematic diagram of the trajectory from K to K' in an embodiment of the present invention.

[0013] Figure 4 The foothold and the position of the related electric cylinder in the embodiment of the present invention are shown as follows: DETAILED DESCRIPTION

[0014] The present invention will be described in further detail below in conjunction with the accompanying drawings so that those skilled in the art can implement the invention with reference to the description.

[0015] The present invention provides a trajectory planning method for a six-degree-of-freedom parallel-legged robot based on a semicircle interpolation algorithm, which is used to plan the motion trajectory of the six-degree-of-freedom parallel-legged robot.

[0016] like Figure 1As shown, the six-degree-of-freedom parallel leg robot mainly includes: a common platform 1, motion units 2, electric cylinders 3, an upper arch 4, a lower arch 5, a Hooke's hinge 6, a universal joint 7, a connection end 8, a U-shaped connector 9 and a supporting electric cylinder 10. Six groups of motion units 2 are evenly distributed on the edge of the common platform 1 in a hinged manner, of which three groups of non-adjacent motion units 2 are connected to the three connection ends 8 of the upper arch 4 and form the outer legs; the remaining three groups of motion units 2 are connected to the three connection ends 8 of the lower arch 5 and form the inner legs; the upper arch 4 and the lower arch 5 are arranged crosswise and do not interfere with each other. Each group of motion units 2 includes two parallel electric cylinders 3, wherein the top of each electric cylinder 3 is connected to the common platform 1 by a Hooke's hinge 6; the bottom is connected to the positioning hole bolt of the connection end 8 of the upper arch 4 or the lower arch 5 by a universal joint 7. The distance between the tops of the two parallel electric cylinders 3 of the same motion unit 2 is greater than the distance between the bottoms of the two parallel electric cylinders 3, so that an angle is formed between the axes of the two parallel electric cylinders 3. Each of the connection ends 8 is provided with a U-shaped connector 9, and each of the U-shaped connectors 9 is bolted to a supporting electric cylinder 10 perpendicular to the ground. The bottom of the supporting electric cylinder 10 passes through the U-shaped notch of the U-shaped connector 9 to raise the height of the inner and outer legs from the ground. When encountering uneven road conditions, the supporting electric cylinder 10 in a lower position is extended and supported on the ground, so that the public platform can always remain on a flat road surface. The specific structure of the six-degree-of-freedom parallel leg robot has been disclosed by the patent (application number: 202321662910.7).

[0017] The specific implementation process of the trajectory planning method for a six-degree-of-freedom parallel-legged robot based on a semicircle interpolation algorithm is as follows.

[0018] Any motion unit in the robot's outer legs is selected as a reference motion unit, and the reference motion unit includes two parallel electric cylinders.

[0019] A spatial rectangular coordinate system is established with the center of the upper surface of the public platform as the origin and the plane where the upper surface of the public platform is located as the XOY plane.

[0020] Here, the position and positive direction of the X-axis and Y-axis can be selected according to actual conditions without specific limitation.

[0021] The current position coordinates of the foot of the reference motion unit are obtained, and the target position coordinates of the foot of the reference motion unit for the next step are determined; a semicircle is constructed on a vertical plane perpendicular to the XOY plane, using the line connecting the target position coordinates and the current position coordinates as the diameter, and the semicircle is used as the target motion trajectory of the foot of the reference motion unit. The semicircle is located above the diameter. This configuration converts trajectory planning from three-dimensional space to a two-dimensional plane.

[0022] like Figure 1 As shown, the foot M of the reference motion unit refers to the lowest end of the connection between the two parallel electric cylinders of the selected reference motion and the single foot of the upper arch.

[0023] An interpolation number is set, and the coordinates of each interpolation point on the target motion trajectory are determined based on the interpolation number. The next motion trajectory of the foot of the reference motion unit is formed by sequentially passing through the coordinates of each interpolation point from the current position coordinates to the target position coordinates. The entire motion trajectory consists of the starting point, the interpolation points, and the end point.

[0024] After planning the first step, the origin and target coordinates are re-determined. According to the above process, the foot of the reference motion unit is planned step by step until the robot moves to the planned end point, and the motion trajectory of the robot is obtained.

[0025] After the base motion unit's foot has completed trajectory planning, the motion units on the other two outer legs follow the base motion unit's movement, while the motion units on the inner legs also follow its displacement, returning the robot's configuration to its initial state (before the first movement). Therefore, once the base motion unit's trajectory planning is complete, the entire robot's trajectory planning is complete.

[0026] In this embodiment, the method for determining the next target position coordinates of the foot of the reference motion unit is as follows: the projection of the line connecting the planned end point and the center of the upper surface of the common platform on the XOY plane is used as the reference line, and the direction toward the planned end point on the reference line is used as the moving direction; the position reached by moving the foot of the reference motion unit from the current position along a distance of one step parallel to the moving direction is used as the next target position coordinates of the foot of the reference motion unit.

[0027] In each step, more interpolation points can improve trajectory smoothness, while fewer interpolation points will result in uneven motion. Small step size + more interpolation points can improve trajectory accuracy and smoothness, but require higher computing power and control frequency; large step size + fewer interpolation points can improve motion efficiency and speed, but may introduce trajectory errors and motion shocks.

[0028] As a preferred embodiment, the step length is set in the range of 80 mm to 200 mm. Setting the step length within this range can ensure the stability of the robot's walking process.

[0029] As a further optimization, when the step length range is set to 80mm~200mm, the interpolation number is set to 200~800. According to the test, if the interpolation number is less than 200, the motion trajectory is not smooth and there will be a sense of jitter. If the interpolation number is greater than 800, the running speed will be affected. At the same time, the bandwidth of the robot servo system may be exceeded due to the excessive density of interpolation points. If the interpolation period is T Too many interpolation points will result in a single motion time that is too short. t=T , the motor does not have time to respond to the command change, but instead introduces tracking error. No tracking error is introduced here, so the number of interpolation points in the range of 200~800 is also in line with the motor response time.

[0030] In one embodiment, a 90 mm step length is generally selected and the interpolation point is set to 360, so that the accuracy and smoothness of the motion trajectory can achieve relatively good results.

[0031] Since the number of interpolation points directly determines the approximation accuracy, it is specifically expressed as the chord height error (the maximum normal distance between the actual trajectory and the ideal trajectory). R If the interpolation number is m , then the chord lengths corresponding to adjacent interpolation points are ,but .

[0032] Due to the chord height error for ,but .

[0033] Number of interpolation points m The more, the longer the chord L The smaller the chord height error h Declining exponentially. m Approaching infinity, h When it approaches 0, that is, when the interpolation points are infinitely dense, the trajectory approaches the ideal semicircle infinitely.

[0034] In actual use, the interpolation number can be determined according to the acceptable chord height error based on the relationship between the above formula (interpolation number and chord height error).

[0035] In order to verify the feasibility of the above formula, the following experiments were conducted: Chord height error h In the range of 0.00077mm~0.0124mm, the step length is 80mm~200mm (radius R is 40~100mm), the interpolation quantity can be known from the above formula m Between 200 and 800, the electric cylinder operates stably, with no noticeable mechanical vibration and an appropriate step size. The formula's calculation results agree with the interpolation number range determined in the previous experiment, further verifying the formula's accuracy.

[0036] The trajectory planning process sequentially calculates the extension and contraction amounts of each electric cylinder in the reference motion unit from the current position to the adjacent interpolation point, the extension and contraction amounts of each electric cylinder in the reference motion unit from one interpolation point to the next, and the extension and contraction amounts of each electric cylinder in the reference motion unit from the adjacent interpolation point to the target position. The electric cylinders are controlled according to the corresponding extension and contraction amounts, thereby achieving the motion of the robot's reference motion unit. The extension and contraction of the electric cylinders in the other motion units in the robot's outer legs, excluding the reference unit, are also controlled according to this process, thereby achieving the overall motion of the robot.

[0037] The following is a further explanation of the trajectory planning method for a six-degree-of-freedom parallel-legged robot based on a semicircle interpolation algorithm provided by the present invention in conjunction with specific embodiments. Example

[0038] In this embodiment, the control system mainly includes QT interface design and Beckhoff PLC program design based on TwinCAT. The host computer is an industrial control computer that performs kinematic calculations and control program execution, and the lower computer is a motion controller. Visual Studio software and QT collaborative development are installed on the host computer as the control system center. The QT-based host computer software interface design adopts the VS2013+QT development environment. Beckhoff TwinCAT3 realizes the ADS communication between the host computer QT software and the Beckhoff PLC through the EtherCAT bus module. The communication of ADS in this control system is that the host computer software packages the operation and parameter data and sends it to the PLC. The PLC processes the data after receiving the data and sends the processed control instructions to each servo drive. At the same time, the PLC directly reads the motion parameters and status information of the electric cylinder from each servo drive. The read information is transmitted to the host computer. After processing in the host computer software, the device status of the electric cylinder is displayed in real time.

[0039] In the entire control system process, the host computer is initialized first, then the host computer detects the control signal and sends the control instruction. After the lower computer controller receives the instruction, it parses the communication instruction. If the instruction is correct, the control command is passed to the servo drive, so that the servo motor pushes the electric cylinder to extend and retract, thereby completing the robot's movement. Finally, the robot completes the corresponding action posture according to the control instruction.

[0040] The control system uses a semicircular interpolation algorithm to plan the robot's motion trajectory. TwinCAT3 replaces traditional programmable logic controllers for real-time control, systematically scheduling tasks for each device within the system. It can also send tasks to multiple logic controllers simultaneously, allowing the robot's 12 servo cylinders to operate independently without interfering with each other. The 12 servo cylinders of the six-degree-of-freedom parallel-leg robot are driven by 12 corresponding servo controllers and equipped with high-precision speed and displacement sensors for precise position control. The robot's posture and foot position can also be determined based on the position of the servo cylinders.

[0041] In this embodiment, Figure 2 The motion unit at position M in the middle is the reference motion unit. The projection of the angle bisector of the angle between the axes of the two electric cylinders in the reference motion unit onto the upper surface of the common platform is the X-axis, and the direction pointing toward the angle bisector is the positive X-axis direction. The Y-axis is obtained by rotating the plane of the X-axis on the upper surface of the common platform 90 degrees counterclockwise. The positive Z-axis direction is the direction perpendicular to the XOY plane and pointing upward. The coordinates of the lowest point of M in the figure are the coordinates of the foot of the reference motion unit.

[0042] The real-time position of the foot of the six-degree-of-freedom parallel leg robot can be obtained through the speed and displacement controller, so that the initial state information of the robot's foot, public platform and other mechanisms can be known. In the present invention, the positions of these initial states, as well as the target position, the trajectory points of the foot movement and other positions are all placed in a spatial coordinate system, which greatly simplifies the calculation.

[0043] The specific size parameters (coordinates) of the robot used in this embodiment are shown in Table 1. The positions of the components in Table 1 are as follows: Figure 4 shown.

[0044] Table 1

[0045] Note: The stroke (i.e., telescopic length) of the electric cylinder of the six-degree-of-freedom parallel-leg robot is 0~160mm. When the electric cylinder is extended to 80mm, it is called the neutral state. At the same time, the total length between the universal joint and the ball joint when the electric cylinders 1-6 are in the neutral position can also be obtained.

[0046] When the electric cylinder of the robot used in this embodiment is in the neutral state, the specific dimensional parameters of the total length between the universal joint and the ball joint are shown in Table 2: Table 2

[0047] During its movement, the six-degree-of-freedom parallel-leg robot's feet are always controlled by the extension and contraction of the electric cylinder, allowing it to move in any direction. At the same time, the trajectory of the robot's feet is similar to that of human walking, which is a parabola that approximates lifting and lowering. Semicircular interpolation is used to optimize and improve it.

[0048] like Figure 3 As shown, the present invention simplifies the trajectory of the foot M of the robot's reference motion unit into the process from M to M'. It can be seen that no matter how M moves to M', it is always on the circular plane where MM' is located. In this embodiment, the initial coordinates of the foot M of the reference motion unit are the outer leg landing point M3 (600, 0, -880), and M' is the landing point of our movement and the end point of the first step. Assume that the coordinates of M' are (x1+600, y1, -880). In order to simplify the demonstration calculation process, the process from M to M' is changed to the change increment from M plus K to K'. In this way, the coordinate change of M is more convenient to calculate. Here, the coordinate origin is moved to point K, while keeping the positive direction of the coordinate system xyz unchanged, then K is (0, 0, 0), then K' is (x1, y1, -880) and assuming that the radius of the arc where KK' is located is R, then: The coordinates of the highest point of the arc are ; The linear motion distance from K to K' is ; The coordinates of the center of the circle are ; 2D motion angle for ; Three-dimensional motion angle for ; Semicircular interpolation converts a spatial point to the plane formed by the points KK' of the foot M's trajectory, converting the three-dimensional problem into a two-dimensional one. The arc angle is then calculated and interpolated on this plane. The interpolation point is set to S. Through calculation, we can obtain the three-dimensional coordinates of the interpolation point on the two-dimensional plane: The x-axis coordinate of the first interpolation point S1: ; The y-axis coordinate of the first interpolation point S1: ; The z-axis coordinate of the first interpolation point S1: ; Among them, n is the interpolation point number, that is, the value of n corresponds to the interpolation point to be obtained. What is required here is the coordinates of the first interpolation point S1, and n=1.

[0049] Similarly, the coordinates of the second interpolation point S2, the third interpolation point S3, the fourth interpolation point S4, etc. can also be calculated (corresponding to n taking n=2, n=3 and n=4 respectively). After obtaining the coordinates of the interpolation points, the extension and contraction of the electric cylinder can be calculated by inverse solution. Here, electric cylinder D1 and electric cylinder 2 are taken as examples to calculate the extension and contraction.

[0050] The simplified formula for the extension and contraction of the electric cylinder D1 when the foot M moves to S1 (relative to the neutral position) is: ; At this time, n=1, and A is the total length between the universal joint and the ball joint where the electric cylinder D1 is located when in the neutral position, where A=917.5489.

[0051] When foot M moves to S2, n = 2 and A remains unchanged; When foot M moves to S3, n=3, and A remains unchanged; By analogy, the expansion and contraction amounts of other interpolation points and other electric cylinders are the same.

[0052] It is also important to note that the calculated motion angle is constrained according to the motion limitations of the robot's various mechanisms, that is, the value range of R will be limited: semicircular .

[0053] At the same time, the servo drive also needs to make relevant important settings, including the load inertia to load weight ratio of the servo motor: 30 times (range 0-200), the maximum output of the analog torque command: 100% (range -1000~1000), the maximum speed limit: 7876r / min (range 0-7876), the motor anti-collision protection function (protection time): 1ms (range 1-1000), and the motor overload output warning bit: 120% (range 0-120).

[0054] Although the embodiments of the present invention have been disclosed above, they are not limited to the applications listed in the description and implementation methods. They can be fully applied to various fields suitable for the present invention. For those familiar with the art, additional modifications can be easily implemented. Therefore, without departing from the general concept defined by the claims and the scope of equivalents, the present invention is not limited to the specific details and illustrations shown and described herein.

Claims

1. A trajectory planning method for a six-degree-of-freedom parallel-legged robot based on a semicircle interpolation algorithm, characterized in that: The steps include: Step 1: Select any motion unit in the robot's outer leg as the reference motion unit; Step 2: Establish a spatial rectangular coordinate system with the center of the upper surface of the public platform as the origin and the plane where the upper surface of the public platform is located as the XOY plane; Step 3: Obtain the current position coordinates of the foot of the reference motion unit and determine the next target position coordinates of the foot of the reference motion unit; construct a semicircle on a vertical plane perpendicular to the XOY plane using the line connecting the target position coordinates and the current position coordinates as the diameter, and use the semicircle as the target motion trajectory of the foot of the reference motion unit; wherein the semicircle is located above the diameter; Step 4: setting an interpolation number, and determining the coordinates of each interpolation point on the target motion trajectory according to the interpolation number; From the current position coordinates, through the coordinates of each interpolation point in sequence, to the target position coordinates, a next movement trajectory of the foot of the reference motion unit is formed; Steps 2 to 4 are repeated in a loop to gradually plan the foot of the reference motion unit until the robot moves to the planning end point, thereby obtaining the motion trajectory of the robot.

2. The trajectory planning method for a six-degree-of-freedom parallel-legged robot based on a semicircular interpolation algorithm according to claim 1, characterized in that: In step three, the method for determining the next target position coordinates of the foot of the reference motion unit is as follows: The projection of the line connecting the planned end point and the center of the upper surface of the common platform on the XOY plane is used as the baseline, and the direction toward the planned end point on the baseline is used as the travel direction; the position reached by the foot of the reference motion unit moving one step length from the current position along a distance parallel to the travel direction is used as the next target position coordinate of the foot of the reference motion unit.

3. The trajectory planning method for a six-degree-of-freedom parallel-legged robot based on a semicircular interpolation algorithm according to claim 2, characterized in that: The step length is set in the range of 80 mm to 200 mm.

4. The trajectory planning method for a six-degree-of-freedom parallel-legged robot based on a semicircular interpolation algorithm according to any one of claims 1 to 3, characterized in that: In step 4, the interpolation number is set to: ； in, is the chord height error, is the radius of the target motion trajectory.

5. The trajectory planning method for a six-degree-of-freedom parallel-legged robot based on a semicircular interpolation algorithm according to claim 4 is characterized in that: In step 2, the projection of the bisector of the angle between the axes of the two electric cylinders in the reference motion unit on the upper surface of the common platform is used as the X-axis, and the direction pointing to the bisector is used as the positive direction of the X-axis; the plane where the X-axis is located on the upper surface of the common platform is rotated 90 degrees counterclockwise to obtain the Y-axis; and the upward direction perpendicular to the XOY plane is used as the positive direction of the Z-axis.

Images