A six-axis parallel robot with measurement capabilities and its measurement method

By using linear actuators and motor drivers in a six-axis parallel robot, combined with kinematic calculations, the problem of the inability to measure external forces in active robots was solved, enabling accurate measurement of equivalent force and displacement of the mobile platform and compliant motion.

CN123103583APending Publication Date: 2026-11-17DELTA ELECTRONICS INC(CN)
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202510632900.6
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2025-05-16
Publication Date
2026-11-17

AI Technical Summary

Technical Problem

Existing active six-axis parallel robots cannot perform compliant movements when subjected to external forces and cannot measure the external forces, resulting in the inability to effectively measure the displacement and torque of the moving platform.

Method used

By using linear actuators and motor drivers, combined with inverse and forward kinematics calculations, the current value and displacement change of the linear motor are monitored, and the equivalent force and displacement of the moving platform are calculated.

Benefits of technology

It enables accurate measurement of the equivalent force and displacement of a mobile platform under external force, and can be used as a six-axis force gauge and displacement meter, providing compliant motion to adapt to external forces.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN123103583A_ABST
    Figure CN123103583A_ABST
Patent Text Reader

Abstract

A six-axis parallel robot with measurement capabilities and its measurement method are disclosed. The robot includes a mobile platform, a base platform, and six actuators connected between the mobile platform and the base platform via multiple joints. The mobile platform has a target pose and multiple resistance forces. Each actuator has a linear motor and a driver, wherein each driver performs the following actions: reading the current value of each linear motor when subjected to an external force less than or equal to a threshold; performing compliant motion and reading the displacement change of each linear motor when subjected to an external force greater than the threshold; calculating the forward kinematics based on the displacement change to obtain the current pose of the mobile platform; calculating the equivalent force and equivalent torque of the mobile platform relative to the external force based on the current value and the current pose; and comparing the current pose with the target pose to calculate the displacement and angular change of the mobile platform.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] This application relates to a six-axis parallel robot, and more particularly to a six-axis parallel robot and its measurement method that can measure the equivalent force / equivalent torque and displacement of a moving platform when subjected to external forces. Background Technology

[0002] In the assembly process of automated production lines, six-axis (or six degrees of freedom) parallel robots are often used to assist in assembly. These robots generally include active robots and passive devices.

[0003] Active robots have active motors that can provide active compensation, but because of their high rigidity, they cannot perform compliant motion when subjected to external forces.

[0004] Generally, the mobile platform and actuators on an active robot do not respond to received external forces; therefore, active robots lack the ability to measure external forces. Furthermore, some active robots can achieve compliant movement by modifying their internal structure or by adding additional sensors (such as force / torque sensors or cameras). However, while the mobile platform and / or actuators of such active robots can yield to received external forces, they still cannot measure those forces. Therefore, those skilled in the art have a genuine need to improve six-axis parallel robots. Summary of the Invention

[0005] This application discloses a six-axis parallel robot with measurement function and its measurement method, which can measure the equivalent force / equivalent torque of the external force and the displacement of the moving platform caused by the external force when it is subjected to external force.

[0006] In one embodiment, the six-axis parallel robot of this application includes a mobile platform having a first surface, a base platform having a second surface, a plurality of first joint elements respectively disposed on the first surface, a plurality of second joint elements respectively disposed on the second surface, and six linear actuators disposed between the mobile platform and the base platform through the plurality of first and second joint elements. Each linear actuator has a linear motor and a motor driver. The mobile platform has a target pose and a plurality of resistance forces relative to multiple directions. In this application, each motor driver is configured to perform the following actions:

[0007] Based on a motion command, the operation of each linear motor is controlled to provide a thrust so that the mobile platform maintains the target posture;

[0008] When the mobile platform receives an external force that is less than or equal to a thrust threshold, a current value of each linear motor is read and a current change is calculated.

[0009] When the mobile platform receives the external force and the external force is greater than the thrust threshold, control each linear actuator to perform compliant movement and read the linear displacement change of each linear motor;

[0010] A positive kinematics is calculated based on the linear displacement change of each linear motor to obtain the current pose of the mobile platform.

[0011] Based on the current change of each linear motor and the current pose, calculate the first-order force and first-order force moment of the moving platform relative to the external force; and

[0012] The current pose and the target pose are compared to calculate a displacement and an angle change of the moving platform.

[0013] In one embodiment, the measurement method of this application is applied to the above-described six-axis parallel robot and includes the following steps:

[0014] Step a) Each of the motor drivers controls the operation of each linear motor based on a motion command to provide a thrust so that the moving platform maintains the target posture;

[0015] Step b) When the mobile platform receives an external force and the external force is less than or equal to a thrust threshold, each motor driver reads a current value of each linear motor and calculates a current change.

[0016] Step c) When the mobile platform receives the external force and the external force is greater than the thrust threshold, each motor driver controls each linear actuator to perform compliant movement and reads the linear displacement change of each linear motor.

[0017] Step d) Each motor driver calculates a positive kinematics based on the linear displacement change of each linear motor to obtain the current pose of the moving platform.

[0018] Step e) The motor driver calculates the equivalent force and equivalent moment of the moving platform relative to the external force based on the current change of each linear motor and the current pose; and

[0019] Step f) The current pose and the target pose are compared by each motor driver to calculate a displacement and an angle change of the moving platform.

[0020] Compared to related technologies, the six-axis parallel robot of this application can be used as a six-axis force gauge and displacement meter / attitude meter, thereby measuring the equivalent force / equivalent torque of the external force and the displacement of the moving platform caused by the external force when subjected to external force. Attached Figure Description

[0021] Figure 1 This is an embodiment of the six-axis parallel robot of this application;

[0022] Figure 2 This is an embodiment of a block diagram of the linear actuator of this application;

[0023] Figure 3 This is an embodiment of the measurement flowchart of this application;

[0024] Figure 4A This is an embodiment of the displacement measurement schematic diagram of this application;

[0025] Figure 4B This is an embodiment of an angle measurement diagram of this application;

[0026] Figure 4C This is an embodiment illustrating the force and torque measurement diagram of this application;

[0027] Figure 5 This is an embodiment of a schematic diagram illustrating the forward kinematics calculation of this application;

[0028] Figure 6 This is an embodiment of the external force calculation diagram of this application;

[0029] Figure 7 This is an example illustrating the measurement results of this application.

[0030] Explanation of reference numerals in the attached figures

[0031] 1: Six-axis parallel robot

[0032] 2: Mobile Platform

[0033] 21: First Surface

[0034] 3: Base Platform

[0035] 31: Second Surface

[0036] 41: First joint element

[0037] 42: Second joint element

[0038] 5: Linear actuator

[0039] 51: Motor driver

[0040] 52: Linear Motor

[0041] S31~S39: Measurement Procedures

[0042] B1~B6: The coordinate positions of the first joint represented by the base platform coordinate system.

[0043] T1~T6: The coordinate positions of the second joint represented by the base platform coordinate system.

[0044] ra: First distance

[0045] rb: Second distance

[0046] α: First included angle

[0047] β: Second included angle

[0048] Current position

[0049] Target linear displacement

[0050] Current linear displacement

[0051] Linear displacement velocity array of linear actuator

[0052] The second linear displacement velocity array at the center point of the moving platform Detailed Implementation

[0053] This application discloses a six-axis parallel robot with measurement capabilities (hereinafter referred to as a six-axis parallel robot). The six-axis parallel robot of this application is an active robot. By modifying the actuator in the six-axis parallel robot to use a linear actuator with a linear motor to reduce the actuator's rigidity, and based on the inherent reverse drive characteristic of the linear motor, the linear actuator can provide translational and / or rotational compensation when receiving external forces, thus enabling compliant motion. During compliant motion, the equivalent force / equivalent torque of the external force received by the moving platform and the displacement of the moving platform can be calculated by changing the linear displacement and current value of the linear actuator. Therefore, the six-axis parallel robot can be directly used as a six-axis force gauge and displacement / attitude meter.

[0054] Six-axis generally refers to a robot that achieves operations in six degrees of freedom (DoF) through the coordinated movement of six drive elements (six linear actuators in this application). Therefore, the six-axis parallel robot of this application can also be called a six-DOF parallel robot. For ease of understanding, the following description will consistently use the term "six-axis parallel robot," but it is not limited to this.

[0055] Please see Figure 1This is an embodiment of the six-axis parallel robot of this application. Figure 1 A schematic diagram of the structure of the six-axis parallel robot 1 of this application is disclosed. (See attached diagram.) Figure 1 As shown, the six-axis parallel robot 1 of this application includes a mobile platform 2, a base platform 3, a plurality of first joint elements 41, a plurality of second joint elements 42, and six linear actuators 5. The mobile platform 2 is disposed relative to the base platform 3. The mobile platform 2 has a first surface 21 in the direction toward the base platform 3, and the base platform 3 has a second surface 31 in the direction toward the mobile platform 2. The number of the plurality of first joint elements 41 is six, and they are respectively disposed on the first surface 21 of the mobile platform 2. The number of the plurality of second joint elements 42 is six, and they are respectively disposed on the first surface 21 of the base platform 3. In one embodiment, the plurality of first joint elements 41 and the plurality of second joint elements 42 are universal joints or ball joints.

[0056] The linear actuators 5 are elongated. One end of each of the six linear actuators 5 is connected to a first joint element 41, and the other end is connected to a second joint element 42. Thus, the six linear actuators 5 are respectively positioned between the moving platform 2 and the base platform 3 via the six first joint elements 41 and the six second joint elements 42.

[0057] In one embodiment, the base platform 3 is positioned in a relatively stable location (e.g., fixed to the ground or other supporting structure), and the mobile platform 2 is a movable component on the six-axis parallel robot 1, for example, used to mount an end effector. When the six-axis parallel robot 1 is used for contact-type assembly processes, the mobile platform 2 may be subjected to external forces. As mentioned above, the six-axis parallel robot of this application uses a linear actuator 5 that can be driven in reverse. Therefore, when the mobile platform 2 is subjected to external forces exceeding a preset thrust threshold, the linear actuator 5 can retract within the limits of the mechanism to achieve compliant movement of the six-axis parallel robot 1. Furthermore, the linear actuator 5 can calculate the equivalent force / equivalent torque of the external force received by the mobile platform 2, as well as the displacement and angular change of the mobile platform 2 caused by the external force, through an algorithm while retracting, thereby allowing the six-axis parallel robot 1 to be directly used as a six-axis force gauge, displacement meter, and / or attitude indicator.

[0058] Please also refer to Figure 2 This is a block diagram embodiment of the linear actuator of this application. Figure 2As shown, each linear actuator 5 in this application has a motor driver 51 and a linear motor 52. The motor driver 51 is connected to the linear motor 52 to control the rotation of the linear motor 52 via motion commands. The linear motor 52 is connected to a first joint element 41 and a second joint element 42 at both ends of the linear actuator 5. When the linear motor 52 operates, it can drive the first joint element 41 and / or the second joint element 42 to rotate, so that the moving platform 2 is maintained in the desired target pose. The motor driver 51 can also be connected to the first joint element 41 and the second joint element 42. When the moving platform 2 receives an external force, the motor driver 51 obtains the coordinates of the first joint element 41 and the second joint element 42 and the linear displacement of the linear motor 52, thereby measuring the external force.

[0059] It is worth mentioning that the motor driver 51 can also monitor the current value of the linear motor 52. Specifically, when the moving platform 2 receives an external force, the linear motor 52 in each linear actuator 5 will increase its current value to resist the external force (i.e., increase the thrust toward the moving platform 2). By continuously monitoring the current value of the linear motor 52 and calculating the change in current value, the motor driver 51 can calculate the equivalent force and equivalent torque relative to the external force (described in detail later).

[0060] After the six-axis parallel robot 1 is started, each motor driver 51 controls the corresponding linear motor 52 to operate based on motion commands, so that the mobile platform 2 maintains a preset target pose. When the mobile platform 2 maintains the target pose, all six linear actuators 5 have a preset linear displacement (i.e., a preset leg length). In one embodiment, the linear displacement is calculated based on the preset target pose. On the other hand, the six-axis parallel robot 1 can be operated by the user to set multiple resistance forces of the mobile platform 2 relative to multiple directions (e.g., the movement direction along the X-axis, Y-axis, or Z-axis, or the rotation direction along the X-axis, Y-axis, or Z-axis), and set one or more thrust thresholds based on these resistance forces. When the mobile platform 2 receives an external force but the force does not exceed the thrust threshold, the mobile platform 2 will not be pushed, and each motor driver 51 will continuously control each linear motor 52 to be at the linear displacement (however, at this time the thrust required increases, so the current value of each linear motor 52 will increase), so that the mobile platform 2 maintains the target posture; when the mobile platform 2 receives an external force and the external force exceeds one or more thrust thresholds (i.e., exceeds one of multiple resistance forces), each motor driver 51 will control each linear motor 52 to yield in order to achieve compliant movement.

[0061] In one embodiment, when the six-axis parallel robot 1 receives the external force on the mobile platform 2, it performs the analysis and calculation of the external force as described in the following paragraphs through the respective motor drivers 51 in each linear actuator 5. In another embodiment, the six-axis parallel robot 1 performs the analysis and calculation of the external force as described in the following paragraphs through one or more controllers (not shown in the figure). For ease of understanding, the following description will use the execution of algorithms by each motor driver 51 to analyze the external force as an example, but it is not limited to this.

[0062] Please refer to the following as well. Figure 3 This is an embodiment of the measurement flowchart of this application. First, the six-axis parallel robot 1 communicates with the human-machine interface (e.g., ...). Figure 7 The human-machine interface shown accepts external operations to set the target pose of the mobile platform 2. Furthermore, each motor driver 51 can generate motion commands to control each linear motor 52 based on this target pose (step S31). Additionally, the six-axis parallel robot can simultaneously set multiple resistance forces on the mobile platform 2 relative to multiple directions through the external operations, and set a thrust threshold based on the multiple resistance forces (step S32). In one embodiment, the resistance forces include three-axis forces and three-axis torques in a Caliber coordinate system. The three axes include the X-axis, Y-axis, and Z-axis. When the external force received by the mobile platform 2 exceeds the thrust threshold, each motor driver 51 controls each linear actuator 5 to retract to achieve compliant motion. Furthermore, each motor driver 51 detects the linear displacement change and current change of each linear motor 52 to measure the external force.

[0063] It is worth mentioning that since the target pose set by the user is a pose in the Cassette coordinate system, this target pose must first be converted into a motion command that can directly control each linear motor 52. To achieve this, this application uses inverse kinematics for the conversion. In one embodiment, after the user sets the target pose of the mobile platform 2, each motor driver 51 performs inverse kinematics calculations based on the target pose to obtain the linear displacement that each linear actuator 5 needs to follow to maintain the mobile platform 2 at this target pose, and then generates a corresponding motion command based on this linear displacement. Thus, when each motor driver 51 controls the corresponding linear motor 52 to rotate based on the motion command, the thrust generated by each linear motor 52 can maintain the mobile platform 2 at the target pose.

[0064] In one embodiment, each motor driver 51 primarily performs the following steps to perform inverse kinematics calculations. First, the structure of the six-axis parallel robot 1 needs to be defined (e.g., Figure 6The structure definition shown includes the target pose of the mobile platform 2 based on the base platform coordinate system used by the base platform (i.e., the target pose input in the aforementioned step S31), and the first distance r between the center point of the mobile platform and each first joint element 41. a The second distance r between the center point of the base platform 3 and each of the second joint elements 42 b Here, a first included angle α represents half the included angle between two relatively close first joint elements 41 on the mobile platform 2, and a second included angle β represents half the included angle between two relatively close second joint elements 42 on the base platform 3. In this application, the controller or each motor driver 51 inputs the above information into the inverse kinematics formula for calculation.

[0065] Next, based on inverse kinematics, the controller or each motor driver 51 determines the distance r according to the second distance. b The second coordinate positions B1 to B6 of each second joint element 42 in the base platform coordinate system are calculated using the second included angle β, and the calculation formula is as follows:

[0066] B1 = [r] b cos(β),r b sin(β), 0];

[0067] B2 = [r b cos(-β),r b sin(-β), 0];

[0068]

[0069] Next, the controller or each motor driver 51 determines the distance r based on the first distance. a The first coordinate positions T1 to T6 of each first joint element 41 in the base platform coordinate system are calculated using the first included angle α. It should be noted that the calculated first coordinate positions T1 to T6 have not undergone a coordinate transformation process and therefore are not the actual positions of each first joint element 41. The formulas for calculating the first coordinate positions T1 to T6 are as follows:

[0070]

[0071] Next, the controller or each motor driver 51, based on the target pose of the mobile platform 2 with the base platform coordinate system as a reference, sequentially performs coordinate transformation matrix multiplication on the first coordinate positions T1 to T6 of each first joint element 41, and takes the Euclidean distance between the transformed coordinate position of each first joint element 41 and the corresponding second joint element 42 to obtain the linear displacement of each corresponding linear actuator 5. The specific formula is described as follows:

[0072] for i = 0 to 5 do:

[0073]

[0074] Among them, H 4x4 The target pose is [XYZABC]. T The homogeneous transformation matrix, T[i] represents the first coordinate position of each first joint element 41 in the base platform coordinate system, l leg This represents the linear displacement of each linear actuator 5. It is the set of linear displacements of the six linear actuators 5.

[0075] Through the aforementioned inverse kinematics calculations, the controller or each motor driver 51 can convert the target pose of the user-set mobile platform 2 into the linear displacement that each linear actuator 5 needs to follow. Therefore, the controller or each motor driver 51 can generate motion commands to control each linear motor 52 based on the linear displacement.

[0076] After steps S31 and S32, each motor driver 51 controls the operation of each linear motor 52 based on motion commands to provide thrust toward the mobile platform 2, thereby maintaining the mobile platform 2 in a preset target pose (step S33). Simultaneously, each motor driver 51 continuously monitors the position (i.e., linear displacement) and current value of each linear motor 52. Furthermore, each motor driver 51 continuously performs servo regression control during the operation of the six-axis parallel robot 1 (step S34), thereby performing compliant motion when receiving external force that is greater than the thrust threshold, and restoring the mobile platform 2 to the target pose when the external force is less than the thrust threshold or disappears.

[0077] Specifically, when the mobile platform 2 receives an external force, and the external force is less than or equal to the thrust threshold, each motor driver 51 reads the current value of each linear motor 52 and calculates the current change of each linear motor 52 (step S35). Based on the current change of each linear motor 52, each motor driver 51 can calculate the equivalent force and equivalent torque of the external force.

[0078] On the other hand, when the external force received by the mobile platform 2 exceeds the thrust threshold, each motor driver 51 reverses the direction of each linear motor 52 to cause each linear actuator 5 to perform compliant movement, and reads the linear displacement change of each linear motor 52 (step S36). That is, the motor driver 51 calculates the difference between the first linear displacement of the corresponding linear motor 52 when the mobile platform 2 is in the target pose and the second linear displacement of the mobile platform 2 after it moves due to the external force. It is worth mentioning that when the external force occurs, each motor driver 51 will simultaneously and continuously execute steps S35 and S36, so there is no sequential relationship between steps S35 and S36.

[0079] Next, each motor driver 51 calculates forward kinematics based on the linear displacement change of each linear motor 52 to obtain the current pose of the mobile platform 2 (step S37). Since the external force received by the mobile platform 2 exceeds the thrust threshold at this time, each linear actuator 5 begins to retract, therefore the current pose of the mobile platform 2 will differ from the target pose. In this embodiment, the current pose includes the displacement and angle of the mobile platform 2 after it has moved due to the external force; that is, the difference between the current pose and the target pose includes both the displacement change and the angle change.

[0080] After obtaining the current change of each linear motor 52 in step S35 and the current pose of the moving platform 2 in step S37, each motor driver 51 further calculates the equivalent force and equivalent torque of the moving platform 2 relative to the external force based on the current change of each linear motor 52 and the current pose of the moving platform (step S38). On the other hand, each motor driver 51 compares the current pose of the moving platform 2 with the target pose to calculate the displacement and angle change of the moving platform 2 caused by the external force (step S39).

[0081] As shown in the steps above, during the process of the mobile platform 2 receiving external force, each motor driver 51 continuously monitors the changes in linear displacement and current of each linear motor 52, and calculates the equivalent force and equivalent torque of the external force based on these changes, as well as the displacement and angular change of the current pose of the mobile platform 2 relative to the target pose. When the external force disappears or changes to a value below the thrust threshold, each motor driver 51 controls the rotation of each linear motor 52 based on the motion command to provide thrust and restore the mobile platform 2 to the target pose. In this way, the servo return control can be achieved.

[0082] Through the above steps, the six-axis parallel robot 1 of this application can measure the equivalent force and equivalent torque of the external force when the mobile platform 2 is subjected to external force, and at the same time determine the displacement and angle changes of the mobile platform 2 caused by the external force. Thus, the six-axis parallel robot can be directly used as a six-axis force gauge and displacement meter / attitude instrument.

[0083] Please see below. Figures 4A to 4C ,in Figure 4A This is an embodiment of the displacement measurement schematic diagram of this application. Figure 4B This is an embodiment of the angle measurement schematic diagram of this application. Figure 4C This is an embodiment of a schematic diagram illustrating the force and torque measurement of this application.

[0084] like Figure 4A As shown, when the mobile platform 2 receives an external force exceeding a preset thrust threshold, each linear actuator 5 will be pushed by the external force, causing the mobile platform 2 to leave the target pose. At this time, the linear displacement of each linear actuator 5 (i.e., leg length) will differ from the linear displacement indicated by the motion command. Based on this difference, the motor driver 51 of each linear actuator 5 can calculate the forward kinematics to obtain the displacement of the mobile platform 2, thereby using the six-axis parallel robot 1 as a displacement meter.

[0085] like Figure 4B As shown, when the mobile platform 2 receives an external force exceeding a preset thrust threshold, each linear actuator 5 will be pushed by the external force, causing the mobile platform 2 to leave the target pose. At this time, the mobile platform 2 will experience an angular change θ relative to the horizontal plane parallel to the base platform 3, corresponding to each axis. Based on this angular change, the motor driver 51 of each linear actuator 5 can calculate the forward kinematics to determine the current attitude of the mobile platform 2, thereby using the six-axis parallel robot 1 as an attitude indicator.

[0086] like Figure 4C As shown, when the mobile platform 2 receives an external force, the linear motors 52 of each linear actuator 5 will increase their thrust to resist the external force, resulting in a change in the current value. Similarly, if each linear actuator 5 is pushed by an external force, each linear motor 52 will also produce a change in linear displacement. Based on these changes, the motor driver 51 of each linear actuator 5 can calculate the positive kinematics combined with the current information to convert the equivalent force / equivalent torque of the external force, thereby using the six-axis parallel robot 1 as a six-axis force gauge.

[0087] Please see below. Figure 5This is an embodiment of the forward kinematics calculation schematic diagram of this application. As mentioned above, this application detects the linear displacement change of each linear actuator 5 when the mobile platform 2 is pushed by an external force, and after forward kinematics calculation, obtains the current pose of the mobile platform 2 after receiving the external force. Specifically, this application performs forward kinematics calculation by the controller or each motor driver 51 through the following steps. For ease of understanding, the following explanation uses the execution of forward kinematics calculation by each motor driver 51 as an example.

[0088] like Figure 5 As shown, when each motor driver 51 performs forward kinematics, it calculates iteratively until the target result converges, and then outputs the final pose. The input to the iterative calculation is the current linear displacement of the six linear actuators 5, and the output is the current pose of the mobile platform 2. More specifically, the iterative calculation in this application is a virtual movement process in which the six linear actuators 5 gradually move from their initial length (i.e., the linear displacement before receiving an external force) to the linear displacement after receiving an external force. During this virtual movement, the Jacobian matrix is ​​calculated based on the speed of movement (i.e., the change in linear displacement) to obtain a small position change. The estimated pose of the mobile platform 2 is adjusted based on this small position change, and then feedback is performed. Finally, the iterative calculation ends and the final estimated pose of the mobile platform 2 is output when the entire result converges. Because the linear displacement of the six linear actuators 5 gets closer and closer to the target linear displacement after receiving an external force during the feedback process, the final output predicted pose can be close to or equal to the true pose of the mobile platform 2 after receiving an external force.

[0089] First, each motor driver 51 obtains the current pose of the moving platform 2. And based on the current pose Perform inverse kinematics calculations to obtain the current linear displacement of each linear motor 52. The method for calculating inverse kinematics is as described above and will not be repeated here.

[0090] Next, each motor driver 51 calculates the current linear displacement of each linear motor 52. and the target linear displacement caused by external forces displacement error value And based on this displacement error value The time constant (time_coefficient) of the iterative algorithm is used to calculate the linear displacement velocity array required for each linear actuator 5. Next, each motor driver 51 is based on the current pose of the mobile platform 2. Calculate the Jacobian matrix and use it to represent the linear displacement-velocity array. The second linear displacement velocity array required to convert to the center point of the mobile platform 2

[0091] Furthermore, each motor driver 51 will generate a second linear displacement velocity array required for the center point of the moving platform 2. Add the current pose of the center point of mobile platform 2 during the iteration process To generate the updated current pose during the iteration process (i.e., ).in, Figure 5 As shown That is, to execute and In other words, the equation will eventually become Figure 5 The physical phenomena shown are calculated to represent Therefore, the attitude change of the six-axis parallel robot 1 during the iteration process can be obtained.

[0092] During the iteration process, each motor driver 51 further adjusts its position based on the updated current pose. The inverse kinematics calculation is performed to update the current linear displacement of each linear motor 52 during the iteration process. Furthermore, each motor driver 51 repeats the above actions to perform iterative update calculations until the displacement error value of each linear motor 52 converges. up to the preset accuracy criteria accuracy The iterative calculation ends here. At the end of the iterative calculation, each motor driver 51 outputs the last updated current pose. This is taken as the current pose of the mobile platform 2 (i.e., the estimated pose).

[0093] Specifically, the calculation steps for the above-mentioned forward kinematics can be represented by the following pseudo code:

[0094]

[0095] As shown in the above code, this application iteratively calculates the current linear displacement of each linear actuator 5. Up to now, linear displacement The linear displacement of the target caused by external force Error value of displacement between leg Less than the preset accuracy value criteria accuracy At this point, convergence is considered complete. Furthermore, each motor driver 51 is configured with its current pose based on the last update calculated before convergence. To serve as the estimated pose of the mobile platform 2 after receiving external forces. (That is, the current pose of the final output).

[0096] In one embodiment, the controller or individual motor drivers 51 determine the position of the mobile platform 2 based on the following steps. This is used to generate the Jacobian matrix. For ease of understanding, the following explanation uses the generation of the Jacobian matrix by each motor driver 51 as an example.

[0097] In this application, the Jacobian matrix is ​​defined as [XYZABC]. T As a 6x6 matrix arranged as first-order partial derivatives. Assuming J is a Jacobian matrix, then we can define... in The linear velocity Cassette coordinate system represents the mobile platform 2 (end effector); This represents the linear velocity of each linear actuator 5. In the Jacobian matrix, X, Y, and Z represent the positions of the endpoints of the mobile platform 2 in the Calibre coordinate system, including X along the X-axis, Y along the Y-axis, and Z along the Z-axis; A, B, and C represent the orientation of the endpoints of the mobile platform 2, for example, A, B, and C represent roll, pitch, and yaw in Euler angles, respectively. T represents transpose.

[0098] First, each motor driver 51 inputs the current pose of the mobile platform 2. And input a preset small gain change (Increment) small This tiny gain change can be, for example, 0.1, 0.2, etc., and is not limited. Next, each motor driver 51 establishes a pose array [XYZABC]. T And any value in the pose array is designated as the small gain change. For example, each motor driver 51 establishes a pose array such as [0 0 0 0 00]. T Furthermore, a preset small gain change (e.g., 0.1) is assigned to the value of any matrix element (e.g., the first element) in this pose array to obtain [0.1 0 0 0 0 0]. T The pose array. Where 0.1 is the matrix element of the i-th term (i = 1 to 6). The value.

[0099] Next, each motor driver 51 calculates its current pose using inverse kinematics. The linear displacement of each linear actuator 5 before change (i.e., ), and the current pose After adding a small gain change, the changed pose is generated. Then, the changed linear displacement of each linear actuator 5 under the changed pose is calculated (i.e., Next, each motor driver 51 calculates the difference between the linear movement before and after the change (i.e., in (This is the variation difference). Finally, using this variation difference and the values ​​of the matrix elements of the pose array after adding the small gain change, the final value of the corresponding matrix element in the Jacobian matrix is ​​calculated. Taking the above Jacobian matrix J as an example, for:

[0100] J 1(1X6) The values ​​of the matrix elements of the pose matrix are the result of adding a small gain change.

[0101] Finally, each motor driver 51 repeats the above action six times until the final values ​​of the six matrix elements in the Jacobian matrix are obtained. Specifically, each time the controller or each motor driver 51 performs the above action, a 1x6 matrix element (i.e., J) is obtained. 1(1X6) After repeating the above steps six times, six sets of 1x6 matrix elements are obtained. Stacking these six sets of matrix elements yields a complete 6x6 Jacobian matrix. Using the Jacobian matrix, each motor driver 51 can obtain the force distribution of each linear actuator 5. Therefore, in the next step, the Jacobian matrix can be multiplied by the linear displacement velocity array of each linear actuator 5. To obtain the second linear displacement velocity array of the center point of the mobile platform 2

[0102] Please see below. Figure 6 This is an embodiment of the external force calculation schematic diagram of this application. As mentioned above, when the mobile platform 2 receives an external force, the controller or each motor driver 51 can calculate the equivalent force and equivalent torque of the external force based on the current value of each linear motor 52 and the current pose of the mobile platform 2. For ease of understanding, the following explanation will take the calculation of the equivalent force and equivalent torque by each motor driver 51 performing the following steps as an example.

[0103] First, each motor driver 51 obtains the linear thrust vector f of each linear actuator 5. iThis linear thrust vector is directly related to the current value of each linear motor 52. Specifically, to resist external forces, the thrust provided by the linear motor 52 to the moving platform 2 will increase, and the current value will increase accordingly. That is, when the current value of the linear motor 52 changes, the thrust also changes accordingly. Next, each motor driver 51 calculates the first coordinate position T of each first joint element 41 in the base platform coordinate system used by the base platform 3 through inverse kinematics. i and the second coordinate position B of each second joint element 42 expressed in the base platform coordinate system. i It should be noted that the base platform 3 of this application is set in a relatively stable position (e.g., fixed to the ground), therefore the second coordinate position B of each second joint element 42 is... i It remains unchanged; in contrast, the first coordinate position T of each first joint element 41 remains unchanged. i It will change its current position after the mobile platform 2 receives an external force.

[0104] In one embodiment, the initial coordinate position T of each first joint element 41 iinit It can be represented as follows:

[0105]

[0106] Among them, each motor driver 51 sets the initial coordinate position T of each first joint element 41 i,init Multiply by the homogeneous transformation matrix of the current pose of the mobile platform 2 to obtain the first coordinate position T of each first joint element 41. i That is: T i =H 4x4 ·T i,init H 4x4 Let be the homogeneous transformation matrix of the current pose of mobile platform 2.

[0107] The inverse kinematics used here is the same as the inverse kinematics described above, and will not be repeated here.

[0108] Next, each motor driver 51, based on these first coordinate positions T i and these second coordinate positions B i Calculate the Euclidean distance between the lines connecting each second joint element 42 and its corresponding first joint element 41. And based on these Euclidean distances Calculate the unit vector of the line connecting each second joint element 42 to its corresponding first joint element 41. Next, each motor driver 51, based on the linear thrust vector f of each linear actuator 5,i and the unit vector Calculate the equivalent force F of the mobile platform 2 relative to the external force it receives.

[0109] On the other hand, each motor driver 51 further calculates the line vector connecting the center position of the base platform 3 and each of the second joint elements 42, as the lever arm required for calculating the torque. And based on the lever arm And the equivalent moment T of the mobile platform 2 relative to the external force is calculated using the equivalent force F.

[0110] In one embodiment, each motor driver 51 can calculate the unit vector, equivalent force, and equivalent moment according to the following first to third formulas:

[0111]

[0112] The formula described above, where The unit vector is the line connecting each second joint element 42 to its corresponding first joint element 41. F is the Euclidean distance between the lines connecting each second joint element 42 and its corresponding first joint element 41, where F is the equivalent force. i Here is the linear thrust vector matrix for each linear actuator 5, and T is the equivalent torque. The vector (i.e., lever arm) connecting the center position of the base platform 3 to each of the second joint elements 42.

[0113] Please also refer to Figure 7 This is an embodiment illustrating the measurement results of this application. Figure 7 The human-machine interface display screen corresponding to the six-axis parallel robot 1 of this application is shown. For example... Figure 7 As shown, the control system of the six-axis parallel robot 1 (not shown) can have a simulation model of the six-axis parallel robot 1, which reflects all the changes that occur when the six-axis parallel robot 1 is subjected to external forces. For example, Figure 7 The human-machine interface shown displays a simulation model corresponding to the six-axis parallel robot 1, and displays the displacement and angle changes of the mobile platform 2 after receiving external force as: diff: [-0 0.01 0.01 0 0.01 -0.01], and displays the equivalent force as: net_force: [-0.07 0.08 -0.05] (N), and the equivalent torque as: net_torque: [-22.9 -24.34 8.4] (N*mm). However, Figure 7 This is merely one specific embodiment of this application, but the control system can present the calculated equivalent force, equivalent moment, and displacement and angular changes of the platform in any different form, without using... Figure 7 The human-computer interface shown is limited to this.

[0114] Through the above-mentioned technical means of this application, users can directly use a six-axis parallel robot as a six-axis force gauge and displacement meter / attitude meter, thereby measuring the equivalent force / equivalent torque of the external force and the displacement of the moving platform caused by the external force when it is subjected to external force.

Claims

1. A six-axis parallel robot with measurement function, comprising: The mobile platform has a first surface; The base platform has a second surface; Multiple first joint elements are respectively disposed on the first surface; Multiple second joint elements are respectively disposed on the second surface; and Six linear actuators are respectively disposed between the moving platform and the base platform via the plurality of first joint elements and the plurality of second joint elements, and each of the linear actuators has a linear motor and a motor driver, wherein the moving platform has a target pose and a plurality of resistance forces relative to multiple directions, and each of the motor drivers is configured to perform the following actions: Action a) Based on motion commands, control the operation of each of the linear motors to provide thrust so that the mobile platform maintains the target pose; Action b) When the mobile platform receives an external force and the external force is less than or equal to the thrust threshold, read the current value of each linear motor and calculate the current change. Action c) When the mobile platform receives the external force and the external force is greater than the thrust threshold, control each of the linear actuators to perform compliant movement and read the linear displacement change of each of the linear motors; Action d) Calculate the positive kinematics based on the linear displacement change of each of the linear motors to obtain the current pose of the moving platform; Action e) Calculate the equivalent force and equivalent moment of the moving platform relative to the external force based on the current change of each of the linear motors and the current pose; and Action f) Compare the current pose and the target pose to calculate the displacement and angle change of the mobile platform.

2. The six-axis parallel robot of claim 1, wherein each of the motor drivers is further configured to perform the following actions respectively: Action a01) Accepts external operation to set the target pose of the mobile platform, and generates motion commands based on the target pose to control each of the linear motors; and Action a02) receives the external operation to set the multiple resistance forces of the mobile platform relative to multiple directions, and sets the thrust threshold based on the multiple resistance forces, wherein the multiple resistance forces include three-axis forces and three-axis torques in the Caliber coordinate system.

3. The six-axis parallel robot according to claim 2, wherein in the action a01), each of the motor drivers is configured to perform inverse kinematics calculations based on the target pose to obtain the linear displacement amount required by each of the linear actuators to maintain the mobile platform following the target pose, and to generate the corresponding motion command based on the linear displacement amount.

4. The six-axis parallel robot of claim 3, wherein each of the motor drivers is further configured to perform the following actions to calculate the linear displacement of each of the linear actuators: Input the target pose of the mobile platform with the base platform coordinate system as a reference, the first distance between the center point of the mobile platform and each of the first joint elements, the second distance between the center point of the base platform and each of the second joint elements, the first included angle representing half the included angle between two closest first joint elements on the mobile platform, and the second included angle representing half the included angle between two closest second joint elements on the base platform. Calculate the second coordinate position of each of the second joint elements in the coordinate system of the base platform based on the second distance and the second included angle; Calculate the first coordinate position of each of the first joint elements in the coordinate system of the base platform based on the first distance and the first included angle; and Based on the target pose of the mobile platform with the base platform coordinate system as a reference, coordinate transformation matrix multiplication is performed sequentially on the first coordinate positions of each first joint element, and Euclidean distance is taken with the corresponding second joint elements to obtain the linear displacement of each corresponding linear actuator.

5. The six-axis parallel robot of claim 1, wherein each of the motor drivers is further configured to iteratively perform the following actions to achieve action d): The current pose of the mobile platform is obtained, and inverse kinematics calculation is performed based on the current pose to obtain the current linear displacement of each linear motor. Calculate the current linear displacement of each of the linear motors and the displacement error value of the target linear displacement caused by the external force; Calculate the required linear displacement velocity array for each of the linear actuators based on the displacement error value and the time constant of the iterative algorithm. The Jacobian matrix is ​​calculated based on the current pose of the mobile platform, and the linear displacement velocity array is converted into a second linear displacement velocity array required by the mobile platform using the Jacobian matrix. The second linear displacement velocity array is added to the current pose of the moving platform to generate the updated current pose during the iteration process; The inverse kinematics calculation is performed based on the updated current pose to obtain the current linear displacement of each of the linear motors during the iteration process; and The above actions are repeated for iterative update calculations until the displacement error value of each linear motor converges to a preset accurate value, at which point the calculation ends and the updated current pose is output as the current pose of the mobile platform.

6. The six-axis parallel robot of claim 1, wherein each of the motor drivers is further configured to iteratively perform the following actions to achieve action e): Multiple linear thrust vector matrices are obtained from the six linear actuators, wherein each linear thrust vector matrix is ​​directly related to the change in current. Multiple first coordinate positions and multiple second coordinate positions are obtained by calculating the first coordinate position of each first joint element in the base platform coordinate system and the second coordinate position of each second joint element in the base platform coordinate system through inverse kinematics calculation. Based on the plurality of first coordinate positions and the plurality of second coordinate positions, the Euclidean distance between each second joint element and the corresponding first joint element is calculated to obtain the plurality of second coordinate positions; Based on the multiple Euclidean distances, calculate the unit vector of the line connecting each second joint element and the corresponding first joint element to obtain multiple unit vectors; The equivalent force of the mobile platform relative to the external force is calculated based on the plurality of linear thrust vector matrices and the plurality of unit vectors. Calculate the line vector connecting the center position of the base platform to each of the second joint elements to serve as the lever arm; and The equivalent moment of the moving platform relative to the external force is calculated based on the lever arm and the equivalent force.

7. The six-axis parallel robot according to claim 6, wherein each of the motor drivers calculates the unit vector based on a first formula, calculates the equivalent force based on a second formula, and calculates the equivalent moment based on a third formula, wherein the first formula is: The second formula is: The third formula is: in Let be the unit vector. Let f be the Euclidean distance, F be the equivalent force, and f be the Euclidean distance. i Let T be the linear thrust vector matrix, and T be the equivalent torque. Let be the connecting vector.

8. A measurement method for a six-axis parallel robot, applied to a six-axis parallel robot having a mobile platform, a base platform, a plurality of first joint elements respectively disposed on a first surface of the mobile platform, a plurality of second joint elements respectively disposed on a second surface of the base platform, and six linear actuators respectively disposed between the mobile platform and the base platform via the plurality of first joint elements and the plurality of second joint elements, wherein the mobile platform has a target pose and a plurality of resistance forces relative to a plurality of directions, each of the linear actuators has a linear motor and a motor driver, and the measurement method includes the following steps: Step a) The motor drivers control the operation of the linear motors based on motion commands to provide thrust so that the mobile platform maintains the target pose; Step b) When the mobile platform receives an external force and the external force is less than or equal to the thrust threshold, each of the motor drivers reads the current value of each of the linear motors and calculates the current change. Step c) When the mobile platform receives the external force and the external force is greater than the thrust threshold, each of the motor drivers controls each of the linear actuators to perform compliant movement and reads the linear displacement change of each of the linear motors; Step d) The motor drivers calculate the positive kinematics based on the linear displacement change of each linear motor to obtain the current pose of the mobile platform; Step e) The motor drivers calculate the equivalent force and equivalent torque of the moving platform relative to the external force based on the current change of each of the linear motors and the current pose; and Step f) The current pose and the target pose are compared by each of the motor drivers to calculate the displacement and angle change of the moving platform.

9. The measurement method according to claim 8, further comprising: Step a01) Accept external operation to set the target pose of the mobile platform, and generate motion commands to control each of the linear motors based on the target pose; and Step a02) Receive the external operation to set the multiple resistance forces of the mobile platform relative to multiple directions, and set the thrust threshold based on the multiple resistance forces, wherein the multiple resistance forces include three-axis forces and three-axis torques in the Caliber coordinate system.

10. The measurement method according to claim 9, wherein step a01) includes each of the motor drivers performing inverse kinematics calculations based on the target pose to obtain the linear displacement amount required by each of the linear actuators to maintain the moving platform following the target pose, and generating the corresponding motion command based on the linear displacement amount.

11. The measurement method according to claim 10, wherein step a01) further comprises: Step a011) Input the target pose of the mobile platform with the base platform coordinate system as a reference, the first distance between the center point of the mobile platform and each of the first joint elements, the second distance between the center point of the base platform and each of the second joint elements, the first included angle representing half the included angle between two closest first joint elements on the mobile platform, and the second included angle representing half the included angle between two closest second joint elements on the base platform; Step a012) Calculate the second coordinate position of each of the second joint elements in the coordinate system of the base platform based on the second distance and the second included angle; Step a013) Calculate the first coordinate position of each of the first joint elements in the coordinate system of the base platform based on the first distance and the first included angle; and Step a014) Based on the target pose of the mobile platform with the base platform coordinate system as the reference, perform coordinate transformation matrix multiplication on the first coordinate position of each first joint element in sequence, and take Euclidean distance with each corresponding second joint element to obtain the linear displacement of each corresponding linear actuator.

12. The measurement method according to claim 8, wherein step d) comprises: Step d1) Obtain the current pose of the mobile platform and perform inverse kinematics calculation based on the current pose to obtain the current linear displacement of each linear motor; Step d2) Calculate the current linear displacement of each of the linear motors and the displacement error value of the target linear displacement caused by the external force; Step d3) Calculate the required linear displacement velocity array for each of the linear actuators based on the displacement error value and the time constant of the iterative algorithm; Step d4) Calculate the Jacobian matrix based on the current pose of the mobile platform, and use the Jacobian matrix to convert the linear displacement velocity array into the second linear displacement velocity array required by the mobile platform; Step d5) Add the second linear displacement velocity array to the current pose of the moving platform to generate the updated current pose during the iteration process; Step d6) Perform the inverse kinematics calculation based on the updated current pose to obtain the current linear displacement of each linear motor during the iteration process; and Step d7) Repeat the above steps to perform iterative update calculations until the displacement error value of each linear motor converges to a preset accurate value, and then output the updated current pose as the current pose of the mobile platform.

13. The measurement method according to claim 8, wherein step e) comprises: Step e1) The six linear actuators obtain multiple linear thrust vector matrices, wherein each linear thrust vector matrix is ​​directly related to the change in current; Step e2) Calculate the first coordinate position of each first joint element in the base platform coordinate system and the second coordinate position of each second joint element in the base platform coordinate system by inverse kinematics to obtain multiple first coordinate positions and multiple second coordinate positions; Step e3) Calculate the Euclidean distance between each second joint element and the corresponding first joint element based on the plurality of first coordinate positions and the plurality of second coordinate positions, so as to obtain a plurality of Euclidean distances; Step e4) Calculate the unit vector of the line connecting each second joint element and the corresponding first joint element based on the multiple Euclidean distances to obtain multiple unit vectors; Step e5) Calculate the equivalent force of the mobile platform relative to the external force based on the plurality of linear thrust vector matrices and the plurality of unit vectors; Step e6) Calculate the line vector connecting the center position of the base platform and each of the second joint elements as the lever arm; and Step e7) Calculate the equivalent moment of the moving platform relative to the external force based on the lever arm and the equivalent force.

14. The measurement method according to claim 13, wherein step e4) includes calculating the unit vector based on a first formula, step e5) includes calculating the equivalent force based on a second formula, and step e7) includes calculating the equivalent moment based on a third formula, wherein the first formula is: The second formula is: The third formula is: in Let be the unit vector. Let f be the Euclidean distance, F be the equivalent force, and f be the Euclidean distance. i Let T be the linear thrust vector matrix, and T be the equivalent torque. Let be the connecting vector.