Method for Calculating End Points

Abstract

The present invention relates to a method for calculating an end point. A method for calculating an end point according to one embodiment of the present invention comprises: a conveyor device having one or more joints and a SCARA device coupled to the conveyor device having one or more joints, wherein the method calculates an end point of a unloading robot for unloading cargo, and comprises the steps of: defining DH parameters (Denavit-Hartenberg Parameters) of each joint; analyzing the inverse kinematics of the conveyor device based on the DH parameters; and analyzing the overall inverse kinematics of the unloading robot through Lagrange's Multiplier Estimation. By optimizing and simultaneously finding the optimal conveyor position and grip point that enable cargo unloading, the present invention can improve the cargo unloading volume using the robot and improve the stability of cargo unloading using the robot.

Description

Technology Field

[0001] The present invention relates to a method for calculating an end point, and more specifically, to a method for calculating an end point that analyzes the inverse kinematics of a conveyor device based on defined DH parameters and analyzes the overall inverse kinematics of an unloading robot through Lagrange's Multiplier Estimation to simultaneously find the optimal conveyor position and grip point that enable cargo unloading. Background Technology

[0003] The term "logistics" is an abbreviation of "physical distribution" and refers to the collective function or activities of effectively moving products and goods from producers to consumers. Generally, it encompasses various activities such as packaging, loading and unloading, transportation, storage, and information.

[0004] Typically, the transportation of products and goods involves various processes such as packaging, storage, collection / loading, transport, unloading / delivery, and storage. Regardless of the means of transportation used, the movement of products and goods is impossible without going through these processes. Physical distribution (logistics) is the comprehensive view of this entire movement.

[0005] Recently, mass production, mass sales, and mass consumption have become the trends of the times, and as the need to streamline the flow of goods connecting them has grown, the importance of logistics is gradually increasing.

[0006] A logistics warehouse generally refers to a storage facility designed to temporarily or for long-term store all everyday goods, such as various foodstuffs, beverages, clothing, home appliances, miscellaneous goods, and industrial supplies, that are mass-produced at factories or production sites. Due to the rapid development of the logistics industry in recent years, these warehouses are being designed and constructed to move beyond simple logistics management. They are now designed to facilitate the creation of new business opportunities, ranging from the placement of stored inventory to efficient inbound and outbound operations and inventory management.

[0007] Logistics loading and unloading operations are notoriously harsh, leading to frequent musculoskeletal disorders and safety accidents among workers, which have become a significant social issue. In response to this need, we have proposed and manufactured an unloading robot utilizing a SCARA (Selective Compliance Assembly Robot Arm) mounted on a conveyor system to unload cargo from a vehicle onto the conveyor or load cargo from the conveyor onto a vehicle, and this robot is currently undergoing testing.

[0008] Typically, to estimate the end point position of a robot, the Denavit-Hartenberg parameters (DH parameters) of each joint must be defined and kinematics analyzed. In particular, such an unloading robot must calculate the end point of the SCARA robot, which serves as the grip point for unloading cargo, and the end point of the conveyor, which pulls the cargo to the optimal position. The calculated end point position is the position of the end point corresponding to the length or angle change value caused by each joint, and a separate control unit controls each joint so that each end point can move to the calculated position. By repeatedly performing this end point calculation and joint control, cargo unloading using the unloading robot can be performed by moving the end point to the target position so that the end point position gradually approaches the target position.

[0009] Accordingly, research is required on an end point calculation method capable of simultaneously optimizing and finding the optimal conveyor position and grip point that enable cargo unloading. The problem to be solved

[0011] The present invention aims to solve the problems of the aforementioned prior art, and the objective of the present invention is to provide an end point calculation method that analyzes the inverse kinematics of a conveyor device based on defined DH parameters, analyzes the overall inverse kinematics of an unloading robot through Lagrange's Multiplier Estimation, and simultaneously optimizes and finds the optimal conveyor position and grip point that enable cargo unloading. means of solving the problem

[0013] An end point calculation method according to one embodiment of the present invention is a method for calculating an end point of an unloading robot for unloading cargo, comprising a conveyor device having one or more joints and an unloading robot having one or more joints coupled to the conveyor device, and includes the steps of defining DH parameters (Denavit-Hartenberg Parameters) of each joint, analyzing the inverse kinematics of the conveyor device based on the DH parameters, and analyzing the overall inverse kinematics of the unloading robot through Lagrange's Multiplier Estimation.

[0014] At this time, the conveyor device comprises a first joint positioned above the robot origin and having a length that can be extended or shortened along a first axis direction which is the direction of transport of cargo horizontal to the ground, a second joint positioned spaced apart from the first joint along the first axis direction and having a rotation axis in a second axis direction which is vertical to the ground, a third joint having a rotation axis in a third axis direction which is horizontally perpendicular to the first axis direction, and a third-fifth joint positioned spaced apart from the third joint along the first axis direction and corresponding to the end point of the conveyor device, and the step of analyzing the inverse kinematics of the conveyor device may include the step of calculating the rotation angle of the third joint from the position coordinates of the end point of the given conveyor device, the step of calculating the rotation angle of the second joint from the position coordinates of the end point of the given conveyor device, and the step of calculating the travel distance of the first joint from the position coordinates of the end point of the given conveyor device.

[0015] In addition, the step of calculating the rotation angle of the third joint is the following mathematical formula <1> It is calculated using,

[0017] [Mathematical Formula 1]

[0018]

[0020] Here, x3 is the rotation angle of the third joint, d0 is the distance in the second axis direction from the robot origin to the first joint, d2 is the distance in the second axis direction from the second joint to the third joint, Conv z is the z-coordinate value of the end point of the conveyor device, and a3 may mean the first axis direction separation distance from the third joint to the third-fifth joint.

[0021] In addition, the step of calculating the rotation angle of the second joint is the following mathematical formula <2> It is calculated using,

[0023] [Mathematical Formula 2]

[0024]

[0026] Here, x2 is the rotation angle of the second joint, Conv y y-coordinate value of the end point of the conveyor device, a2 is the first axis direction separation distance from the second joint to the third joint, a3 is the first axis direction separation distance from the third joint to the 3.5 joint, and x3 may represent the rotation angle of the third joint.

[0027] In addition, the step of calculating the travel distance of the first joint is the following mathematical formula <3> It is calculated using,

[0029] [Mathematical Formula 3]

[0030]

[0032] Here, x1 is the displacement distance of the first joint, a2 is the first axis-direction separation distance from the second joint to the third joint, x2 is the rotation angle of the second joint, a3 is the first axis-direction separation distance from the third joint to the 3.5 joint, x3 is the rotation angle of the third joint, Conv x can mean the x-coordinate value of the end point of the conveyor device.

[0033] Additionally, the SCARA device comprises a fourth joint spaced apart from the third joint in the direction of the third axis and having a rotation axis in the direction of the third axis, a fifth joint spaced apart from the fourth joint along the direction of the first axis and having a rotation axis in the direction of the second axis, a sixth joint spaced apart from the fifth joint in the direction of horizontal to the ground and having a rotation axis in the direction of the second axis, and a seventh joint spaced apart from the sixth joint in the direction of horizontal to the ground and spaced apart along the horizontal direction and having a rotation axis in the direction of the second axis, and the end point of the unloading robot may be spaced apart from the seventh joint along the direction of the first axis.

[0034] In addition, the step of analyzing the overall inverse kinematics of the unloading robot can analyze the inverse kinematics of the conveyor device such that the position error for the target position of the end point of the conveyor device calculated in the step of analyzing the inverse kinematics of the conveyor device has an error precision within a reference distance. This step of analyzing the overall inverse kinematics of the unloading robot calculates that the end point of the unloading robot approaches the surface of the unloading cargo perpendicularly, calculates that the 7th joint is positioned further away from the unloading cargo than the end point of the unloading robot, and calculates that the 6th joint is positioned offset in the direction of the centerline of the conveyor device than the 4th joint, thereby analyzing the inverse kinematics of the conveyor device.

[0035] In addition, the step of analyzing the overall inverse kinematics of the disembarking robot is the following mathematical formula <4> Based on, the inverse kinematics of the conveyor device is analyzed, and

[0037] [Mathematical Formula 4]

[0038]

[0040] Here, is the kinematics from the robot origin to the end point of the disembarking robot with respect to x, y, z coordinates, is the kinematics from the robot origin to the end point of the disembarking robot with respect to the y-coordinate, is the kinematics from the robot origin to the 7th joint with respect to the y-coordinate, a7 is the first-axis separation distance from the 7th joint to the end point of the disembarking robot, x3 is the rotation angle of the 3rd joint, x4 is the rotation angle of the 4th joint, is the kinematics from the robot origin to the endpoint with respect to the x-coordinate, can mean the kinematics from the robot origin to the 7th joint with respect to the x-coordinate. Effects of the invention

[0042] According to the present invention, the inverse kinematics of a conveyor device is analyzed based on defined DH parameters, and the overall inverse kinematics of an unloading robot is analyzed through Lagrange's Multiplier Estimation. By optimizing and simultaneously finding the optimal conveyor position and grip point that enable cargo unloading, the cargo unloading volume using the robot can be improved, and the stability of cargo unloading using the robot can be improved. Brief explanation of the drawing

[0044] FIG. 1 is a functional block diagram of a disembarking robot system to which an endpoint calculation method according to one embodiment of the present invention is applied. FIG. 2 is a schematic diagram illustrating an unloading robot according to one embodiment of the present invention. FIG. 3 is a drawing illustrating the operational characteristics of a disembarking robot according to one embodiment of the present invention. FIG. 4 is a drawing illustrating an example of an angle conversion view of a second conveyor unit according to an embodiment of the present invention. FIG. 5 is a drawing illustrating a SCARA device according to one embodiment of the present invention. FIG. 6 is a drawing illustrating an example of an angle conversion view of a support unit according to one embodiment of the present invention. FIG. 7 is a flowchart schematically illustrating an endpoint calculation method according to an embodiment of the present invention. FIG. 8 is a diagram illustrating an example of DH parameter switching of an unloading robot according to an embodiment of the present invention. FIG. 9 is a diagram illustrating an example of a DH parameter table of a disembarking robot according to one embodiment of the present invention. Specific details for implementing the invention

[0045] It should be noted that the technical terms used in this invention are used merely to describe specific embodiments and are not intended to limit the invention. Furthermore, unless specifically defined otherwise in this invention, the technical terms used in this invention should be interpreted in the sense generally understood by those skilled in the art to which this invention pertains, and should not be interpreted in an overly broad or overly narrow sense. Additionally, if a technical term used in this invention is an incorrect technical term that fails to accurately express the concept of the invention, it should be understood as being replaced by a technical term that can be correctly understood by those skilled in the art.

[0046] Furthermore, singular expressions used in the present invention include plural expressions unless the context clearly indicates otherwise. In the present invention, terms such as "composed of" or "comprising" should not be interpreted as necessarily including all of the various components or steps described in the invention, and should be interpreted as meaning that some of the components or steps may not be included, or that additional components or steps may be included.

[0047] In addition, it should be noted that the attached drawings are intended only to facilitate an understanding of the concept of the present invention, and should not be interpreted as limiting the concept of the present invention.

[0049] The method for calculating an endpoint according to the present invention will be examined in more detail below with reference to the attached drawings.

[0051] FIG. 1 is a functional block diagram of a disembarking robot system to which an endpoint calculation method according to one embodiment of the present invention is applied.

[0052] Referring to FIG. 1, an unloading robot system to which an end point calculation method according to one embodiment of the present invention is applied may be configured to include an unloading robot (1) that operates to unload cargo, a calculation unit (3) that calculates the end point of the unloading robot (1), and a control unit (5) that controls the unloading robot (1).

[0053] The unloading robot (1) may be equipped with multiple joints (axes) to enable various movements. The number of joints may vary depending on the use of the unloading robot (1), but the unloading robot (1) according to one embodiment of the present invention will be described as having a total of 7 joints.

[0054] The calculation unit (3) can calculate and estimate the end point according to the movement of each joint. To do this, the calculation unit can define the DH parameters (Denavit-Hartenberg Parameters) of the unloading robot (1) and analyze the kinematics. At this time, the end point calculated by the calculation unit (5) includes the length or angle change values ​​of each joint for the end point to be located at the corresponding coordinates.

[0055] And the control unit (5) receives the calculated value from the calculation unit (3) and can control the operation of each joint of the unloading robot (1) based on the received calculated value.

[0057] FIG. 2 is a schematic diagram illustrating an unloading robot according to an embodiment of the present invention, FIG. 3 is a diagram illustrating the operational characteristics of an unloading robot according to an embodiment of the present invention, FIG. 4 is a diagram illustrating an example of an angle transformation of a second conveyor unit according to an embodiment of the present invention, FIG. 5 is a diagram illustrating a SCARA device according to an embodiment of the present invention. FIG. 6 is a diagram illustrating an example of an angle transformation of a support unit according to an embodiment of the present invention.

[0058] Hereinafter, an unloading robot (10) according to an embodiment of the present invention will be described with reference to FIGS. 2 to 6.

[0059] As illustrated in FIG. 2, the unloading robot (10) comprises a conveyor device (10) having one or more joints and a SCARA device (30) coupled to the conveyor device (10) and having one or more joints.

[0060] The conveyor device (10) can transport unloaded cargo in one direction. At this time, the conveyor device (10) can extend or shorten the length in the transport direction, at least a portion of the section can rotate at a predetermined angle around a vertical axis, and at least a portion of the section can rotate at a predetermined angle around a horizontal axis.

[0061] Referring to FIG. 3, the conveyor device (10) is described in more detail. The conveyor device (10) may be configured to include a first conveyor unit (11) that is adjustable in length, a second conveyor unit (12) that is connected to the first conveyor unit (11) and whose left-right and up-down positioning angles relative to the first conveyor unit (11) are adjustable, and a conveyor unit (13) capable of conveying the first conveyor unit (11) and the second conveyor unit (12).

[0062] The first conveyor unit (11) is equipped with a conveyor to transport cargo in one direction and can be configured to extend or shorten in length along the cargo transport direction.

[0063] The second conveyor unit (12) is arranged to extend from the first conveyor unit (11) and is equipped with a conveyor to transport cargo in one direction together with the first conveyor unit (11). At this time, among the two longitudinal ends of the second conveyor unit (12), the end connected to the first conveyor unit (11) is provided with a vertical rotation axis (ax1), and the position angle of the second conveyor unit (12) relative to the first conveyor unit (11) can be adjusted to a predetermined angle in the left and right directions by the vertical rotation axis (ax1) of the connection end.

[0064] Additionally, the connecting end of the second conveyor unit (12) is provided with a horizontal rotation axis (ax2) perpendicular to the cargo transport direction, and the second conveyor unit (12) can adjust the positioning angle relative to the first conveyor unit (11) by a predetermined angle in the up and down direction by the horizontal rotation axis (ax2) of the connecting end.

[0065] Accordingly, the unloading robot (10) according to the present embodiment can rotate the second conveyor unit (12) by a predetermined angle in the left-right or up-down direction depending on the position of the cargo being unloaded, thereby bringing the end point of the second conveyor unit (12) closer to or aligned with the cargo unloading position. If the cargo unloading position is positioned far from the second conveyor unit (12), the length of the first conveyor unit (11) can be extended, or conversely, if the cargo unloading position is positioned close to the second conveyor unit (12), the length of the first conveyor unit (11) can be shortened to bring the end of the second conveyor unit (12) closer to or aligned with the cargo unloading position. Here, the end center of the second conveyor unit (12) corresponds to the end point of the conveyor device (10). Meanwhile, the first conveyor unit (11) and the second conveyor unit (12) can be moved by being coupled to a transport unit (13) equipped with wheels.

[0066] The SCARA device (30) is coupled to the second conveyor unit (12) to move integrally with the second conveyor unit (12), is configured to grasp cargo and unload it, and can operate to load the unloaded cargo onto the end point side end of the second conveyor unit (12). At this time, the cargo being unloaded may be cargo loaded in a cargo container, a cargo vehicle, another conveyor device, etc.

[0067] The SCARA device (30) may be configured to include a SCARA unit (31) for gripping cargo and a support unit (33) for connecting the SCARA unit (31) to a second conveyor unit (12).

[0068] Referring to FIG. 5, the SCARA unit (31) can be configured to include a first arm (311) which is connected to a support unit (33) through a vertical rotation axis (ax4), a second arm (313) which is connected to the other end of the first arm (311) through a vertical rotation axis (ax5), and an end portion (315) which is connected to the other end of the second arm (313) through a vertical rotation axis (ax6). At this time, the other end portion of the end portion (315) may be equipped with a suction module, etc., to enable the gripping of cargo. Here, the center of the other end portion of the end portion (315) corresponds to the end point of the unloading robot (1).

[0069] A support unit (33) can support the SCARA unit (31) from the second conveyor unit (12) by having one end connected to the other end of the second conveyor unit (12) and the other end connected to one end of the first arm via a vertical rotation axis (ax4). At this time, a horizontal rotation axis (ax3) perpendicular to the conveyor's transport direction is provided at one end of the support unit (33), and the support unit (33) can be connected to the second conveyor unit (12) via the vertical rotation axis (ax3). Accordingly, the positioning angle of the support unit (33) relative to the second conveyor unit (12) can be adjusted to a predetermined angle in the vertical direction.

[0071] Meanwhile, the second conveyor unit (12) according to the present embodiment can rotate at a predetermined angle in the up and down direction around a horizontal rotation axis (ax2) that is orthogonal to the conveyor transport direction. However, the second conveyor unit (12) has a considerable weight depending on the length of the conveyor, so it may be difficult to lift the other end by driving the rotation axis placed at one end.

[0072] To solve such problems, the second conveyor unit (12) according to the present embodiment can generate a driving force for vertical rotation by a separate first servo cylinder (14).

[0073] One end of the first servo cylinder (14) may be rotatably connected to the lower side of the middle or other end of the second conveyor unit (12), and the other end may be spaced apart from the lower side of the one end of the second conveyor unit (12) by a predetermined distance. At this time, the other end of the first servo cylinder (14) may be spaced apart from the lower side of the one end of the second conveyor unit (12) by a separate first spacing frame (15). The first spacing frame (15) has a predetermined length in the vertical direction, and the one end of the second conveyor unit (12) is connected to the upper end, and the other end of the first servo cylinder (14) is rotatably connected to the lower end.

[0074] Accordingly, as shown in the example illustrated in FIG. 4, by adjusting the length of the first servo cylinder (14) to raise or lower the other end of the second conveyor unit (12), the second conveyor unit (12) can be rotated at a predetermined angle in the up and down direction.

[0075] Additionally, as described above, the support unit (33) can rotate at a predetermined angle in the up and down direction with respect to a horizontal rotation axis (ax3) that is orthogonal to the cargo transport direction. The load supported by the support unit (33) is the sum of the weight of the support unit (33), the weight of the SCARA unit (31), and the weight of the cargo being unloaded by the SCARA unit (31). This weight is substantial, and as a result, the support unit (33) may also have difficulty lifting the other end by driving the rotation axis placed at one end, just like the second conveyor unit (12) described above.

[0076] To solve this problem, the support unit (33) according to the present embodiment can generate a driving force for vertical rotation by a separate second servo cylinder.

[0077] One end of the second servo cylinder (16) may be rotatably connected to the lower side of the middle or other end of the support unit (33), and the other end may be spaced apart from the support unit (33) by a predetermined distance. At this time, the other end of the second servo cylinder (16) may be spaced apart from the support unit (33) by a second spacing frame (17). The second spacing frame (17) has a predetermined length in the vertical direction, and the middle or other end of the second conveyor unit (12) may be connected to the upper end, and the other end of the second servo cylinder (16) may be rotatably connected to the lower end.

[0078] Accordingly, as shown in the example illustrated in FIG. 5, by adjusting the length of the second servo cylinder (16) to raise or lower the other end of the support unit (33), the support unit (33) can be rotated at a predetermined angle in the up and down direction.

[0080] FIG. 7 is a flowchart schematically illustrating an endpoint calculation method according to an embodiment of the present invention, FIG. 8 is a diagram illustrating an example of DH parameter conversion of an unloading robot according to an embodiment of the present invention, and FIG. 9 is a diagram illustrating an example of a DH parameter table of an unloading robot according to an embodiment of the present invention.

[0081] Below, we will examine the process of calculating the endpoint of the calculation unit (3) with reference to FIGS. 7 to 9.

[0082] An end point calculation method according to one embodiment of the present invention can simultaneously find the optimal conveyor position and grip point that enable cargo unloading by optimizing them through a process (S5) of defining the DH parameters (Denavit-Hartenberg Parameters) of each joint (S1), analyzing the inverse kinematics of the conveyor device based on the defined DH parameters (S3), and analyzing the overall inverse kinematics of the unloading robot (1) through Lagrange's Multiplier Estimation.

[0083] Referring to FIG. 8, an example of DH parameter conversion of an unloading robot (1) according to an embodiment of the present invention can be described. First, the conveyor device (10) of the unloading robot may have a first joint that is positioned above the robot origin and can be extended or shortened along a first axis direction which is the direction of cargo transport horizontal to the ground, a second joint that is spaced apart from the first joint along the first axis direction and has a rotation axis in a second axis direction which is vertical to the ground, a third joint that has a rotation axis in a third axis direction which is horizontal and orthogonal to the first axis direction, and a third-fifth joint that is spaced apart from the third joint along the first axis direction and corresponds to the end point of the conveyor device (10).

[0084] Additionally, the SCARA device (30) of the unloading robot is provided with a fourth joint that is spaced apart from the third joint in the direction of the third axis and has a rotation axis in the direction of the third axis, a fifth joint that is spaced apart from the fourth joint along the direction of the first axis and has a rotation axis in the direction of the second axis, a sixth joint that is spaced apart from the fifth joint in the direction horizontal to the ground and has a rotation axis in the direction of the second axis, and a seventh joint that is spaced apart from the sixth joint in the direction horizontal to the ground and spaced apart along the horizontal direction and has a rotation axis in the direction of the second axis. At this time, the end point of the SCARA device (30) is the end point of the unloading robot, and the end point of the unloading robot (1) can be spaced apart from the seventh joint along the direction of the first axis.

[0085] Accordingly, the end point of the conveyor device (10) is determined by the change in length or angle of the first to third joints, and the end point of the SCARA device (30), i.e., the end point of the unloading robot (1), will be determined by the change in length or angle of the first to third joints and the fourth to seventh joints.

[0086] Additionally, in Fig. 8, the z-axis of each joint is positioned to correspond to the axis of rotation.

[0087] Meanwhile, the DH parameters transformed as in the example of Fig. 8 can be summarized as shown in the table of Fig. 9. Furthermore, the formula for the rotation transformation matrix of the DH parameters is the following mathematical equation. <1> It can be organized as follows.

[0089] [Mathematical Formula 1]

[0090]

[0091] Here, T i is the kinematics of joint i (i is 7 from 0 (origin)), θ i is the angle change value due to rotation of joint i, d i is the height change value according to joint i, a iα is the change in length in the direction of connection progression according to joint i. i can mean the change in the angle of the connection direction according to joint i.

[0093] At this time, the kinematics from the robot origin to the end point of the disembarking robot is given by the following mathematical formula <2> It can be calculated based on.

[0095] [Mathematical Formula 2]

[0096]

[0097] Here, is the kinematics from the robot origin to the end point of the disembarking robot, T base is the kinematics of the robot origin, T1 is the kinematics of the first joint, T2 is the kinematics of the second joint, T3 is the kinematics of the third joint, T 3. 5 may mean the kinematics of the 3.5 joint, T4 may mean the kinematics of the 4th joint, T5 may mean the kinematics of the 5th joint, T6 may mean the kinematics of the 6th joint, and T7 may mean the kinematics of the 7th joint.

[0098] At this time, the kinematics from the robot origin to the end point of the disembarking robot is given by the following mathematical formula <3> The kinematics from the robot base to the end point of the conveyor device, expressed as, and the following mathematical formula <4> The kinematics from the end point of the conveyor device represented by to the origin of the SCARA device, and the following mathematical formula <5> It can be distinguished by the kinematics from the origin of the SCARA device, expressed as [ ], to the end point of the disembarking robot.

[0100] [Mathematical Formula 3]

[0101]

[0103] [Mathematical Formula 4]

[0104]

[0106] [Mathematical Formula 5]

[0107]

[0109] However, here, the kinematics from the end point of the conveyor device to the origin of the SCARA device correspond to a fixed constant value determined by the numerical value of the unloading robot (1). Therefore, the kinematics from the robot origin to the end point of the unloading robot can be analyzed through the kinematics from the robot base to the end point of the conveyor device and the kinematics from the origin of the SCARA device to the end point of the unloading robot.

[0111] Meanwhile, the process of analyzing the inverse kinematics of a conveyor device can calculate the end point of a conveyor device by sequentially performing the process of calculating the rotation angle of a third joint from the position coordinates of the end point of a given conveyor device, the process of calculating the rotation angle of a second joint from the position coordinates of the end point of a given conveyor device, and the process of calculating the travel distance of a first joint from the position coordinates of the end point of a given conveyor device.

[0112] At this time, the kinematics from the origin of the unloading robot to the end point of the conveyor device is given by the following mathematical formula <6> It can be expressed as such, and the coordinates of the end point of the conveyor device are given by the following mathematical formula <7> It can be calculated through. And the rotation angle of the third joint is the following mathematical formula <8> It can be calculated through, and the rotation angle of the second joint is the following mathematical formula <9> It can be calculated using, and the travel distance of the first joint is the following mathematical formula <10> It can be calculated using .

[0114] [Mathematical Formula 6]

[0115]

[0116] Here, x1 represents the rotation angle of the first joint, x2 represents the rotation angle of the second joint, x3 represents the rotation angle of the third joint, a1 represents the first axis direction separation distance from the first joint to the second joint, a2 represents the first axis direction separation distance from the second joint to the third joint, a3 represents the first axis direction separation distance from the third joint to the 3.5 joint, d0 represents the second axis direction separation distance from the robot origin to the first joint, and d2 represents the second axis direction separation distance from the second joint to the third joint.

[0118] [Mathematical Formula 7]

[0119]

[0120] Here, Conv x,y,z can mean the coordinates of the end point of the conveyor device.

[0122] [Mathematical Formula 8]

[0123]

[0124] Here, x3 is the rotation angle of the third joint, Conv z can mean the z-coordinate value of the end point of the conveyor device.

[0126] [Mathematical Formula 9]

[0127]

[0128] Here, x2 is the rotation angle of the second joint, Conv y can mean the y-coordinate value of the end point of the conveyor device.

[0130] [Mathematical Formula 10]

[0131]

[0132] Here, x1 is the travel distance of the first joint Conv x can mean the x-coordinate value of the end point of the conveyor device.

[0134] Meanwhile, the position error of the unloading robot's endpoint for the target position (cargo unloading position) is the error precision of the reference distance endurance (e.g., 10 -6 In order to calculate the robot's end point with an error precision of within mm, when analyzing the overall inverse kinematics of the unloading robot, the end point of the unloading robot is calculated to approach the surface of the unloading cargo perpendicularly, the seventh joint is calculated to be positioned further from the unloading cargo than the end point of the unloading robot, and the sixth joint is calculated to be positioned offset in the direction of the centerline of the conveyor device than the fourth joint, thereby allowing the inverse kinematics of the conveyor device to be analyzed.

[0135] If we express these conditions as equation f(x), the following mathematical equation <11> It is the same as.

[0137] [Mathematical Formula 11]

[0138]

[0139] Here, is the kinematics from the robot origin to the end point of the disembarking robot with respect to x, y, z coordinates, is the kinematics from the robot origin to the end point of the disembarking robot with respect to the y-coordinate, is the kinematics from the robot origin to the 7th joint with respect to the y-coordinate, a7 is the first-axis separation distance from the 7th joint to the end point of the disembarking robot, x3 is the rotation angle of the 3rd joint, x4 is the rotation angle of the 4th joint, is the kinematics from the robot origin to the endpoint with respect to the x-coordinate, can mean the kinematics from the robot origin to the 7th joint with respect to the x-coordinate.

[0140] The output unit (3) can analyze the inverse kinematics of the conveyor device through the above equation based on the Jacobian.

[0142] Through the above configurations, the end point calculation method according to the present embodiment optimizes and simultaneously finds the optimal conveyor position and grip point that enable cargo unloading, thereby improving the cargo unloading volume using a robot and improving the stability of cargo unloading using a robot.

[0144] Meanwhile, it goes without saying that the technical concept of the present invention may also be applied to a computer-readable recording medium containing a computer program that enables the device and method according to the present embodiment to perform their functions. Furthermore, the technical concept according to various embodiments of the present invention may be implemented in the form of computer-readable code recorded on a computer-readable recording medium. A computer-readable recording medium may be any data storage device that can be read by a computer and store data. For example, a computer-readable recording medium may be a ROM, RAM, CD-ROM, magnetic tape, floppy disk, optical disk, hard disk drive, etc. Additionally, computer-readable code or a program stored on a computer-readable recording medium may be transmitted through a network connected between computers.

[0145] Furthermore, the foregoing description is merely an illustrative explanation of the technical concept of the present invention, and those skilled in the art to which the present invention pertains will be able to make various modifications and variations within the scope of the essential characteristics of the present invention. Accordingly, the embodiments disclosed in the present invention are intended to explain, not limit, the technical concept of the present invention, and the scope of the technical concept of the present invention is not limited by these embodiments. The scope of protection of the present invention shall be interpreted by the claims below, and all technical concepts within an equivalent scope shall be interpreted as being included within the scope of rights of the present invention. Explanation of the symbols

[0147] 1: Disembarking robot 3: Output section 5: Control unit 10: Conveyor device 11: 1st conveyor unit 12: 2nd conveyor unit 13: Transfer unit 14: 1st servo cylinder 14: First separation frame 16: Second servo cylinder 17: Second separation frame 30: SCARA device 31: Skara Unit 311: 1st Amnesia 313: Second Cancer 315: Distal part 33: Base unit

Claims

Claim 1 A method for calculating an end point of an unloading robot for unloading cargo, comprising: a conveyor device having one or more joints and an unloading robot coupled to the conveyor device and having one or more joints, wherein the method comprises: a step of defining DH parameters (Denavit-Hartenberg Parameters) of each joint; a step of analyzing the inverse kinematics of the conveyor device based on the DH parameters; and a step of analyzing the overall inverse kinematics of the unloading robot through Lagrange's Multiplier Estimation, wherein the conveyor device comprises: a first joint positioned above the robot origin and capable of extending or contracting in length along a first axis direction which is the cargo transport direction horizontal to the ground; a second joint positioned spaced apart from the first joint along the first axis direction and having a rotation axis in a second axis direction which is perpendicular to the ground; a third joint having a rotation axis in a third axis direction which is a horizontal direction orthogonal to the first axis direction; and a joint positioned spaced apart from the third joint along the first axis direction and corresponding to the end point of the conveyor device. The step of providing a third and fifth joint and analyzing the inverse kinematics of the conveyor device comprises: a step of calculating the rotation angle of the third joint from the position coordinates of the end point of the given conveyor device; a step of calculating the rotation angle of the second joint from the position coordinates of the end point of the given conveyor device; and a step of calculating the travel distance of the first joint from the position coordinates of the end point of the given conveyor device, wherein the step of calculating the rotation angle of the third joint is based on the following mathematical formula <1> It is calculated using [Mathematical Formula 1] Here, x3 is the rotation angle of the third joint, d0 is the distance in the second axis direction from the robot origin to the first joint, d2 is the distance in the second axis direction from the second joint to the third joint, Conv z A method for calculating an end point where a3 represents the z-coordinate value of the end point of the conveyor device and the first axis direction separation distance from the third joint to the third-fifth joint. Claim 2 delete Claim 3 delete Claim 4 A method for calculating an end point of an unloading robot for unloading cargo, comprising: a conveyor device having one or more joints and an unloading robot coupled to the conveyor device and having one or more joints, wherein the method comprises: a step of defining DH parameters (Denavit-Hartenberg Parameters) of each joint; a step of analyzing the inverse kinematics of the conveyor device based on the DH parameters; and a step of analyzing the overall inverse kinematics of the unloading robot through Lagrange's Multiplier Estimation, wherein the conveyor device comprises: a first joint positioned above the robot origin and capable of extending or contracting in length along a first axis direction which is the cargo transport direction horizontal to the ground; a second joint positioned spaced apart from the first joint along the first axis direction and having a rotation axis in a second axis direction which is perpendicular to the ground; a third joint having a rotation axis in a third axis direction which is a horizontal direction orthogonal to the first axis direction; and a joint positioned spaced apart from the third joint along the first axis direction and corresponding to the end point of the conveyor device. The step of providing a third and fifth joint and analyzing the inverse kinematics of the conveyor device comprises: a step of calculating the rotation angle of the third joint from the position coordinates of the end point of the given conveyor device; a step of calculating the rotation angle of the second joint from the position coordinates of the end point of the given conveyor device; and a step of calculating the travel distance of the first joint from the position coordinates of the end point of the given conveyor device, wherein the step of calculating the rotation angle of the second joint is based on the following mathematical formula <2> It is calculated using [Mathematical Formula 2] Here, x2 is the rotation angle of the second joint, Conv y A method for calculating an end point, wherein y-coordinate value of the end point of the conveyor device, a2 is the first axis direction separation distance from the second joint to the third joint, a3 is the first axis direction separation distance from the third joint to the third-fifth joint, and x3 is the rotation angle of the third joint. Claim 5 A method for calculating an end point of an unloading robot for unloading cargo, comprising: a conveyor device having one or more joints and an unloading robot coupled to the conveyor device and having one or more joints, wherein the method comprises: a step of defining DH parameters (Denavit-Hartenberg Parameters) of each joint; a step of analyzing the inverse kinematics of the conveyor device based on the DH parameters; and a step of analyzing the overall inverse kinematics of the unloading robot through Lagrange's Multiplier Estimation, wherein the conveyor device comprises: a first joint positioned above the robot origin and capable of extending or contracting in length along a first axis direction which is the cargo transport direction horizontal to the ground; a second joint positioned spaced apart from the first joint along the first axis direction and having a rotation axis in a second axis direction which is perpendicular to the ground; a third joint having a rotation axis in a third axis direction which is a horizontal direction orthogonal to the first axis direction; and a joint positioned spaced apart from the third joint along the first axis direction and corresponding to the end point of the conveyor device. The step of providing a third and fifth joint and analyzing the inverse kinematics of the conveyor device comprises: a step of calculating the rotation angle of the third joint from the position coordinates of the end point of the given conveyor device; a step of calculating the rotation angle of the second joint from the position coordinates of the end point of the given conveyor device; and a step of calculating the travel distance of the first joint from the position coordinates of the end point of the given conveyor device, wherein the step of calculating the travel distance of the first joint is based on the following mathematical formula <3> It is calculated using [Mathematical Formula 3] Here, x1 is the displacement distance of the first joint, a2 is the separation distance in the first axis direction from the second joint to the third joint, x2 is the rotation angle of the second joint, a3 is the separation distance in the first axis direction from the third joint to the 3.5 joint, x3 is the rotation angle of the third joint, Conv x An endpoint calculation method that means the x-coordinate value of the endpoint of the above-mentioned conveyor device. Claim 6 delete Claim 7 delete Claim 8 A method for calculating an end point of an unloading robot for unloading cargo, comprising: a conveyor device having one or more joints and a SCARA device coupled to the conveyor device having one or more joints, wherein the method comprises: a step of defining DH parameters (Denavit-Hartenberg Parameters) of each joint; a step of analyzing the inverse kinematics of the conveyor device based on the DH parameters; and a step of analyzing the overall inverse kinematics of the unloading robot through Lagrange's Multiplier Estimation, wherein the conveyor device comprises: a first joint positioned above the robot origin and capable of extending or contracting in length along a first axis direction which is the cargo transport direction horizontal to the ground; a second joint positioned spaced apart from the first joint along the first axis direction and having a rotation axis in a second axis direction which is perpendicular to the ground; a third joint having a rotation axis in a third axis direction which is a horizontal direction orthogonal to the first axis direction; and a joint positioned spaced apart from the third joint along the first axis direction and at the end point of the conveyor device The step of providing a corresponding third and fifth joint and analyzing the inverse kinematics of the conveyor device comprises: a step of calculating the rotation angle of the third joint from the position coordinates of the end point of the given conveyor device; a step of calculating the rotation angle of the second joint from the position coordinates of the end point of the given conveyor device; and a step of calculating the travel distance of the first joint from the position coordinates of the end point of the given conveyor device, wherein the SCARA device comprises the third.A fourth joint spaced apart from the fifth joint in the direction of the third axis and having a rotation axis in the direction of the third axis; a fifth joint spaced apart from the fourth joint along the direction of the first axis and having a rotation axis in the direction of the second axis; a sixth joint spaced apart from the fifth joint in a direction horizontal to the ground and having a rotation axis in the direction of the second axis; and a seventh joint spaced apart from the sixth joint in a direction horizontal to the ground, spaced apart along the horizontal direction, and having a rotation axis in the direction of the second axis, wherein the end point of the unloading robot is spaced apart from the seventh joint along the direction of the first axis, and the step of analyzing the overall inverse kinematics of the unloading robot such that the position error for the target position of the end point of the conveyor device calculated in the step of analyzing the inverse kinematics of the conveyor device has an error precision within a reference distance, wherein the end point of the unloading robot is calculated to approach the surface of the unloading cargo perpendicularly, and the seventh joint is calculated to be located further from the unloading cargo than the end point of the unloading robot, and the sixth joint The step of analyzing the inverse kinematics of the conveyor device by calculating that it is positioned offset in the direction of the centerline of the conveyor device compared to the fourth joint, and analyzing the overall inverse kinematics of the unloading robot, is as follows: <4> Based on [Equation 4], the inverse kinematics of the above conveyor device is analyzed. Here, is the kinematics from the robot origin to the end point of the disembarking robot with respect to x, y, z coordinates, is the kinematics from the robot origin to the end point of the disembarking robot with respect to the y-coordinate, is the kinematics from the robot origin to the 7th joint with respect to the y-coordinate, a7 is the separation distance in the 1st axis direction from the 7th joint to the end point of the unloading robot, x3 is the rotation angle of the 3rd joint, x4 is the rotation angle of the 4th joint, is the kinematics from the above robot origin to the end point with respect to the x-coordinate, An end point calculation method representing the kinematics from the robot origin to the seventh joint with respect to the x-coordinate.

Images