Kinematic solution method for cantilevered crushing device
By constructing a kinematic model and iterative solution method for the cantilever crusher, the linkage of the boom, stick, and end effector is realized, which solves the problems of low motion efficiency and low precision of the cantilever crusher and improves its control accuracy and flexibility.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- NINGXIA TIANDI BENNIU IND GRP
- Filing Date
- 2023-08-30
- Publication Date
- 2026-05-29
AI Technical Summary
The cantilever crusher has low motion efficiency and low control precision during control, and cannot achieve three-arm linkage, resulting in insufficient movement flexibility.
By establishing a kinematic model of the cantilever crusher, including DH parameters, link coordinate system and homogeneous transformation matrix, the robot transformation equation set and link constraint equation set are constructed. The Jacobi matrix and Newton-Raphson method are used for iterative solution to realize the three-arm linkage control of boom, stick and end effector.
This improves the motion control precision and efficiency of the cantilever crusher, making its movement more flexible.
Smart Images
Figure CN117171489B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the fields of coal mining and robotics, and in particular to a method for solving the kinematics of a cantilever crusher. Background Technology
[0002] Cantilever crushers are widely used in mechanized coal mining due to their ability to flexibly and efficiently crush large coal pieces and gangue. During coal mining, the control system of a cantilever crusher receives user commands and controls the base, boom, stick, and end effector of the crusher by adjusting the extension and retraction of the hydraulic cylinders, thereby controlling the movement of the cantilever crusher.
[0003] However, currently, when controlling cantilever crushers, the base, boom, stick, and end effector of the cantilever crusher robot are controlled separately. That is, when controlling the end effector of the cantilever crusher robot to move to the working position, the boom, stick, and end effector need to be moved sequentially through their respective hydraulic cylinders. This not only results in low movement efficiency, but also in low control precision and insufficient movement flexibility of the cantilever crusher because it cannot achieve the coordinated movement of the three arms. Summary of the Invention
[0004] In view of this, and to address the above shortcomings, it is necessary to propose a kinematic solution method for cantilever crushing devices, which can improve the control accuracy and motion efficiency of cantilever crushing devices, and make the movement of cantilever crushing devices more flexible.
[0005] This invention provides a method for solving the kinematics of a cantilever crusher, comprising the following steps:
[0006] Step 1: Obtain the mechanical parameters of the cantilever crusher; wherein, the mechanical parameters include the DH parameters of the three-bar linkage robot of the cantilever crusher, as well as the lengths of the links of the boom, stick and end effector.
[0007] Step 2: Establish the link coordinate system of the three-bar robot and the homogeneous transformation matrix of the end effector based on the DH parameters, and obtain the robot transformation equations for coordinate transformation between joint space and pose space based on the homogeneous transformation matrix of the link coordinate system and the end effector.
[0008] Step 3: Based on the lengths of the links in the boom, stick, and end effector and the form of the kinematic pairs in each part, establish the corresponding linkage constraint equations for each part of the boom, stick, and end effector.
[0009] Step 4: Read the current drive space coordinates of the cantilever crusher from the displacement sensors on the three hydraulic cylinders of the boom, stick, and end effector;
[0010] Step 5: Calculate the current pose space coordinates of the end effector using the robot transformation equations, link constraint equations, and the current drive space coordinates;
[0011] Step 6: Obtain the target pose space coordinates generated after the user moves the end effector of the cantilever crusher to the target working position based on the current pose space coordinates;
[0012] Step 7: Solve for the target drive space coordinates of the hydraulic cylinder of the cantilever crusher using the robot transformation equations, the link constraint equations, the current pose space coordinates, and the target pose space coordinates.
[0013] Preferably, step 2 specifically includes:
[0014] Define the joint space coordinates as (θ1, θ2, θ3) and the pose space coordinates of the end effector as (x, y, ξ); where θ1, θ2, θ3 are the relative rotation angles of the boom, stick and end effector of the cantilever crusher, respectively; x is the x-direction displacement of the end effector in the base coordinate system; y is the y-direction displacement of the end effector in the base coordinate system; and ξ is the relative rotation angle between the end effector and the Z-axis of the base coordinate system.
[0015] According to robot kinematics theory, p in link coordinate system i i The coordinates of the point are represented as coordinates in the link coordinate system i-1, which yields the coordinates p of the end effector in the base coordinate system. 0 As shown in the following formula:
[0016]
[0017] in, The homogeneous transformation matrix of the three-bar linkage robot is as follows:
[0018]
[0019] Then, based on equations (1) and (2), the following set of robot transformation equations for coordinate transformation between the joint space and pose space of the cantilever crusher can be obtained:
[0020]
[0021] Where a0, a1, a2, and a3 are the lengths of each link in the DH parameters.
[0022] Preferably, the establishment of the linkage constraint equations corresponding to the boom portion in step 3 includes:
[0023] Define the members constituting the boom as 0-1, 0-2, 0-3, and 0-4, l 0-1 l 0-2 l 0-3 and l 0-4 Let be the lengths of the corresponding members, and take the intersection of members 0-1 and 0-2 as the origin of the coordinate system, and establish a coordinate system O0 along the 0-2 direction on the x-axis;
[0024] Therefore, the corresponding linkage constraint equations for the boom section are as follows:
[0025]
[0026] Where (x2, y2) are the coordinates of the slider frame in the O0 coordinate system, (x 0-3 ,y 0-3 ), (x 0-4 ,y 0-4 The coordinates of the first ends of links 0-3 and 0-4 are respectively. and These represent the rotation angles of connecting rod 0-3, connecting rod 0-4, and the hydraulic cylinder in the O0 coordinate system.
[0027] Preferably, the establishment of the linkage constraint equations for the stick section in step 3 includes:
[0028] The members constituting the boom are defined as 1-0, 1-1, 1-2, and 1-3. 1-0 l 1-1 l 1-2 、 and l 1-3 These represent the lengths of the corresponding members; a coordinate system O1 is established with the intersection of members 1-0 and 1-1 as the origin, and the x-axis along the 1-1 direction;
[0029] Therefore, the corresponding set of link constraint equations for the stick section are as follows:
[0030]
[0031] Where (x5, y5) are the coordinates of the slider frame in the O1 coordinate system, (x 1-2 ,y 1-2 ), (x 1-3 ,y 1-3 The coordinates of the first ends of links 1-2 and 1-3 are respectively. and These represent the rotation angles of connecting rod 1-2, connecting rod 1-3, and the hydraulic cylinder in the O1 coordinate system.
[0032] Preferably, the establishment of the linkage constraint equations corresponding to the end effector in step 3 includes:
[0033] The links constituting the end effector are defined as 2-0, 2-1, 2-2, 2-3, 3, 3-0, and 3-1, l. 2-0 l 2-1 l 2-2 l 2-3 l3, l 3-0 and l 3-1 These represent the lengths of the corresponding members; with the intersection of members 2-0 and 2-3 as the origin, a coordinate system O2 is established with the x-axis along the direction of 2-0;
[0034] Therefore, the link constraint equations corresponding to the end effector are as follows:
[0035]
[0036] Where (x6, y6) are the coordinates of the slider frame in the O2 coordinate system, (x 2-1 ,y 2-1 ),
[0037] (x 2-2 ,y 2-2 ), (x 3-1 ,y 3-1 ), (x 3-0 ,y 3-0 These are the coordinates of the ends of connecting rods 2-1, 2-2, 3-1, and 3-0, respectively. These represent the rotation angles of the hydraulic cylinder and connecting rods 2-1, 2-2, 3-1, and 3-0 in the O2 coordinate system.
[0038] Preferably, step 5 includes:
[0039] The linkage constraint equations are simplified based on the current drive space coordinates (s1, s2, s3) to obtain the simplified linkage constraint equations; where s1, s2, and s3 are the extension and retraction of the hydraulic cylinders of the boom, stick, and end effector, respectively.
[0040] Solve for the first Jacobian matrix of the simplified linkage constraint equations;
[0041] The iterative formula is determined based on the first Jacobian matrix, and the relative rotation angles of the boom, stick, and end effector are obtained by iteratively solving the problem using the Newton-Raphson method.
[0042] Substituting the relative rotation angles of the boom, stick, and end effector into the robot transformation equations, we obtain the current pose space coordinates of the end effector.
[0043] Preferably, step 7 includes:
[0044] The second Jacobian matrix is constructed based on the robot transformation equations, and an iterative formula is constructed using the second Jacobian matrix, the current pose space coordinates, and the target pose space coordinates.
[0045] The iterative formula constructed by the second Jacobian matrix is iteratively solved to obtain the target joint space coordinates in the joint space; wherein, the target joint space coordinates are composed of the relative rotation angles of the corresponding boom, stick and end effector;
[0046] The extension and retraction of the hydraulic cylinder driving the boom is calculated using the relative rotation angle of the boom and the corresponding linkage constraint equations; the extension and retraction of the hydraulic cylinder driving the stick is calculated using the relative rotation angle of the stick and the corresponding linkage constraint equations; and the extension and retraction of the hydraulic cylinder driving the end effector is calculated using the relative rotation angle of the end effector and the corresponding linkage constraint equations.
[0047] As can be seen from the above technical solution, in the kinematic solution method of the cantilever crusher provided in the embodiments of the present invention, a set of robot transformation equations for coordinate transformation between joint space and pose space is established through DH parameters, and a set of link constraint equations for the boom, stick and end effector are established. Based on this, when the cantilever crusher is controlled by the drive space coordinates, the boom, stick and end effector can achieve three-arm linkage movement. Compared with the method of controlling the boom, stick and end effector separately, it can not only improve the movement speed and control accuracy, but also make the movement of the cantilever crusher more flexible. Attached Figure Description
[0048] Figure 1 A flowchart illustrating the kinematic solution method for a cantilever crusher;
[0049] Figure 2 This is a schematic diagram of a cantilever crushing device.
[0050] Figure 3 This is a schematic diagram of the boom section of a cantilever crusher;
[0051] Figure 4 This is a schematic diagram of the boom section of a cantilever crusher;
[0052] Figure 5 This is a schematic diagram of the end effector section of a cantilever crusher.
[0053] In the diagram: base frame 0, boom 1, stick 2, end effector 3. Detailed Implementation
[0054] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.
[0055] See Figure 1 This invention provides a method for solving the kinematics of a cantilever crusher, which may include the following steps:
[0056] Step 1: Obtain the mechanical parameters of the cantilever crusher; wherein, the mechanical parameters include the DH parameters of the three-bar linkage robot of the cantilever crusher, as well as the lengths of the links of the boom, stick and end effector.
[0057] Step 2: Establish the link coordinate system of the three-bar robot and the homogeneous transformation matrix of the end effector based on the DH parameters, and obtain the robot transformation equations for coordinate transformation between joint space and pose space based on the homogeneous transformation matrix of the link coordinate system and the end effector.
[0058] Step 3: Based on the lengths of the links in the boom, stick, and end effector and the form of the kinematic pairs in each part, establish the corresponding linkage constraint equations for each part of the boom, stick, and end effector.
[0059] Step 4: Read the current drive space coordinates of the cantilever crusher from the displacement sensors on the three hydraulic cylinders of the boom, stick, and end effector;
[0060] Step 5: Calculate the current pose space coordinates of the end effector using the robot transformation equations, link constraint equations, and the current drive space coordinates;
[0061] Step 6: Obtain the target pose space coordinates generated after the user moves the end effector of the cantilever crusher to the target working position based on the current pose space coordinates;
[0062] Step 7: Solve for the target drive space coordinates of the hydraulic cylinder of the cantilever crusher using the robot transformation equations, the link constraint equations, the current pose space coordinates, and the target pose space coordinates.
[0063] Based on the above steps, the kinematic solution method for the cantilever crusher provided in this embodiment of the invention can mainly include two parts: the first part: the construction of the kinematic model of the cantilever crusher; and the second part: the kinematic solution of the cantilever crusher.
[0064] In the first part, the kinematic model construction of the cantilever crusher involves building a kinematic model of the cantilever crusher based on its mechanical parameters. This kinematic model includes a linkage kinematic model and a three-bar robot kinematic model. The linkage kinematic model is the linkage constraint equation set, and the three-bar robot kinematic model is the robot transformation equation set.
[0065] In the second part, the kinematics solution of the cantilever crusher includes both forward and inverse kinematics solutions. The forward kinematics solution involves reading the drive space coordinates of the cantilever crusher from the displacement sensor on the hydraulic cylinder, and then solving for the current pose of the end effector. The inverse kinematics solution involves using the current pose of the end effector obtained from the forward kinematics solution, and the target pose after the operator moves the end effector to the target working position, to solve for the drive space coordinates of the corresponding target working position.
[0066] The following section provides a more detailed explanation of each step in the two parts mentioned above.
[0067] In step 1, as Figure 2 As shown, parts 1, 2, and 3 constitute a three-link robot, 0 is the base frame, and parts 1, 2, and 3 are the boom, stick, and end effector, respectively. Mechanical parameters may include DH parameters such as the length of each link, the relative rotation angle of each revolute joint, and the lengths of the links that make up the boom, stick, and end effector.
[0068] In step 2, the present invention first establishes the link coordinate system of the three-link robot according to the DH parameter method, then establishes the homogeneous transformation matrix of the end effector, and finally obtains the robot transformation equations. Specifically, step 2 can be implemented in the following way:
[0069] Define the joint space coordinates as (θ1, θ2, θ3) and the pose space coordinates of the end effector as (x, y, ξ); where θ1, θ2, θ3 are the relative rotation angles of the boom, stick and end effector of the cantilever crusher, respectively; x is the x-direction displacement of the end effector in the base coordinate system; y is the y-direction displacement of the end effector in the base coordinate system; and ξ is the relative rotation angle between the end effector and the Z-axis of the base coordinate system.
[0070] According to robot kinematics theory, p in link coordinate system i i The coordinates of the point are represented as coordinates in the link coordinate system i-1, which yields the coordinates p of the end effector in the base coordinate system. 0 As shown in the following formula:
[0071]
[0072] in, The homogeneous transformation matrix of the three-bar linkage robot is as follows:
[0073]
[0074] Then, based on equations (1) and (2), the following set of robot transformation equations for coordinate transformation between the joint space and pose space of the cantilever crusher can be obtained:
[0075]
[0076] Where a0, a1, a2, and a3 are the lengths of each link in the DH parameters.
[0077] In this embodiment, based on robot kinematics theory, The matrix is a transformation matrix between link coordinate system i and link coordinate system i-1, which can transform p in link coordinate system i. i The coordinates of a point are represented as coordinates in the link coordinate system i-1, that is: in,
[0078] In the formula, α is the torsion angle of the link, a is the length of the link, and d is the joint distance.
[0079] Therefore, in the base coordinate system, the hammer tip position coordinate p of the end effector 0 The above equation can be used to derive equation 1, which leads to the robot transformation equation set of equation 3. Thus, the kinematic model of the three-bar robot of the cantilever crusher is completed.
[0080] In step 3, the boom, stick, and end effector are all driven by their own hydraulic cylinders. This step establishes the corresponding set of motion constraint equations based on the kinematic pair forms of these driving parts.
[0081] 1) Linkage constraint equations for the boom section
[0082] like Figure 3 The diagram shows the link structure of the boom section. Step 3, establishing the corresponding link constraint equations for the boom section, includes:
[0083] Define the members constituting the boom as 0-1, 0-2, 0-3, and 0-4, l 0-1 l 0-2 l 0-3 and l 0-4 Let P0, P1, and P2 be the lengths of the corresponding rods, and P0, P1, and P2 be the revolute joints. The coordinate origin is the intersection of rods 0-1 and 0-2, and a coordinate system O0 is established along the 0-2 direction on the x-axis.
[0084] Therefore, the corresponding linkage constraint equations for the boom section are as follows:
[0085]
[0086] Where (x2, y2) are the coordinates of the slider frame in the O0 coordinate system, (x 0-3 ,y 0-3 ), (x 0-4 ,y 0-4 The coordinates of the first ends of links 0-3 and 0-4 are respectively. and These represent the rotation angles of connecting rod 0-3, connecting rod 0-4, and the hydraulic cylinder in the O0 coordinate system.
[0087] 2) The linkage constraint equations of the boom section
[0088] like Figure 4 The diagram shows the rod structure of the boom section. Step 3, establishing the corresponding set of link constraint equations for the boom section, may include:
[0089] The members constituting the boom are defined as 1-0, 1-1, 1-2, and 1-3. 1-0 l 1-1 l 1-2 、 and l 1-3 These represent the lengths of the corresponding rods, with P3, P4, and P5 being revolute joints; a coordinate system O1 is established with the intersection of rods 1-0 and 1-1 as the origin, and the x-axis along the 1-1 direction.
[0090] Therefore, the corresponding set of link constraint equations for the stick section are as follows:
[0091]
[0092] Where (x5, y5) are the coordinates of the slider frame in the O1 coordinate system, (x 1-2 ,y 1-2 ), (x 1-3 ,y 1-3 The coordinates of the first ends of links 1-2 and 1-3 are respectively. and These represent the rotation angles of connecting rod 1-2, connecting rod 1-3, and the hydraulic cylinder in the O1 coordinate system.
[0093] 3) Link constraint equations for the end effector section
[0094] like Figure 5 The diagram shows the link structure of the end effector. Therefore, establishing the link constraint equations for the corresponding end effector in step 3 can include:
[0095] The links constituting the end effector are defined as 2-0, 2-1, 2-2, 2-3, 3, 3-0, and 3-1, l. 2-0 l 2-1 l 2-2 l 2-3 l3, l 3-0 and l 3-1 These represent the lengths of the corresponding members, with P6-P10 being a revolute joint; a coordinate system O2 is established with the intersection of members 2-0 and 2-3 as the origin, and the x-axis along the direction of 2-0;
[0096] Therefore, the link constraint equations corresponding to the end effector are as follows:
[0097]
[0098] Where (x6, y6) are the coordinates of the slider frame in the O2 coordinate system, (x 2-1 ,y 2-1 ),
[0099] (x 2-2 ,y 2-2 ), (x 3-1 ,y 3-1 ), (x 3-0 ,y 3-0 These are the coordinates of the ends of connecting rods 2-1, 2-2, 3-1, and 3-0, respectively. These represent the rotation angles of the hydraulic cylinder and connecting rods 2-1, 2-2, 3-1, and 3-0 in the O2 coordinate system.
[0100] At this point, the kinematic model of the cantilever crusher linkage is complete, and the kinematic model of the cantilever crusher in the first part is complete.
[0101] The boom, stick, and end effector are all controlled by hydraulic cylinders. Each hydraulic cylinder can be equipped with a displacement sensor to read the extension and retraction amount, thereby obtaining the drive space coordinates. For example, in step 4, the values of the displacement sensors on the hydraulic cylinders of the boom, stick, and end effector are read as s1, s2, and s3, respectively. Then, the current drive space coordinates are (s1, s2, s3). Further, based on these current drive space coordinates, the current pose space coordinates can be calculated in step 5, i.e., the position of the tip of the current end effector can be calculated.
[0102] In one embodiment, step 5 can be implemented as follows:
[0103] The linkage constraint equations are simplified based on the current drive space coordinates (s1, s2, s3) to obtain the simplified linkage constraint equations; where s1, s2, and s3 are the extension and retraction of the hydraulic cylinders of the boom, stick, and end effector, respectively.
[0104] Solve for the first Jacobian matrix of the simplified linkage constraint equations;
[0105] The iterative formula is determined based on the first Jacobian matrix, and the relative rotation angles of the boom, stick, and end effector are obtained by iteratively solving the problem using the Newton-Raphson method.
[0106] Substituting the relative rotation angles of the boom, stick, and end effector into the robot transformation equations, we obtain the current pose space coordinates of the end effector.
[0107] In this embodiment, equations 4, 5, and 6 are first simplified based on the hydraulic cylinder extension and retraction amounts s1, s2, and s3 to obtain a simplified set of link constraint equations. Then, the Jacobian matrix of the link constraint equations is solved, and an iterative formula is obtained based on the Jacobian matrix. The Newton-Raphson method is then used for iterative solving. During the solution process, an initial value is given. If convergence is achieved, the iteration ends; otherwise, iterative solving continues. After the solution is completed, the relative rotation angles θ1, θ2, and θ3 of the boom, stick, and hammer are obtained. Substituting θ1, θ2, and θ3 into the robot transformation equations for joint space and pose space yields the pose (x, y, ξ) of the hammer. Specifically,
[0108] 1) For the boom section, the extension / retraction of the boom's hydraulic cylinder is s1. Based on the geometric relationship of the boom mechanism, we have:
[0109]
[0110] Combining equations (4) and (7), we have:
[0111]
[0112] make Then we have:
[0113]
[0114] right Solving it using the Newton-Raphson iterative method, the first Jacobian matrix J in Equation 9 is obtained. b :
[0115]
[0116] Its iterative formula is:
[0117]
[0118]
[0119] Based on the above formula, a specific initial value is selected for iteration, when |(α n+1 ,β n+1 When )≤δ|, the root of Equation 8 is (α) n ,β n ), that is, at this time For α n , For β n Then, combining this with the geometric relationship of the link rotation angle... This gives us the relative rotation angle θ1 of the boom section.
[0120] 2) For the stick section, the extension / retraction of the hydraulic cylinder of the stick is s2. Based on the geometric relationship of the stick mechanism, we have:
[0121]
[0122] Combining equations (5) and (11), we have:
[0123]
[0124] make Then we have:
[0125]
[0126] right Solving it using the Newton-Raphson iterative method, the first Jacobian matrix J in Equation 13 is obtained. b :
[0127]
[0128] Its iterative formula is:
[0129]
[0130]
[0131] Based on the above formula, a specific initial value is selected for iteration, when |(α n+1 ,β n+1 When α ≤ δ|, the root of equation 12 is (α) n ,β n ), that is, at this time For α n , For β n Then, combining this with the geometric relationship of the link rotation angle... This gives us the relative rotation angle θ2 of the boom section.
[0132] 3) For the end effector, it can be decomposed into a four-bar linkage and a crank-slider mechanism. Equations 11 and 12 below are the link constraint equations for the crank-slider mechanism and the four-bar linkage in the end effector, respectively:
[0133]
[0134]
[0135] At this point, the extension / retraction of the hydraulic cylinder of the end effector is s3. Similar to the boom and stick parts, this can be calculated using the Newton-Raphson method as described above. and in, Substitute these known quantities into Equation 15, and let Let α be the unknown quantity. If β is the unknown quantity, then we can obtain:
[0136]
[0137] Similarly, the Newton-Raphson iteration method can be used to obtain the value at this time. and At this point, we can further consider the geometric relationship of the connecting rod rotation angle. The relative rotation angle θ3 of the end effector can then be obtained.
[0138] θ1, θ2, and θ3 are obtained from the above equations. Then, θ1, θ2, and θ3 are substituted into the robot transformation equations in equation 3 to obtain the current pose space coordinates (x, y, ξ) of the end effector.
[0139] In step 6, the user moves the end effector to the target working position based on the current pose space coordinates, which will form a new target pose space coordinate. As a result, there will be an error between the current pose space coordinate and the target pose space coordinate. In step 7, this error is taken into account in the calculation process, thereby improving the accuracy of the kinematic solution and thus improving the control precision of the cantilever crusher.
[0140] In one implementation, step 7 can be achieved as follows:
[0141] The second Jacobian matrix is constructed based on the robot transformation equations, and an iterative formula is constructed using the second Jacobian matrix, the current pose space coordinates, and the target pose space coordinates.
[0142] The iterative formula constructed by the second Jacobian matrix is iteratively solved to obtain the target joint space coordinates in the joint space; wherein, the target joint space coordinates are composed of the relative rotation angles of the corresponding boom, stick and end effector;
[0143] The extension and retraction of the hydraulic cylinder driving the boom is calculated using the relative rotation angle of the boom and the corresponding linkage constraint equations; the extension and retraction of the hydraulic cylinder driving the stick is calculated using the relative rotation angle of the stick and the corresponding linkage constraint equations; and the extension and retraction of the hydraulic cylinder driving the end effector is calculated using the relative rotation angle of the end effector and the corresponding linkage constraint equations.
[0144] In this embodiment, the Jacobian matrix is first constructed based on the robot's transformation equations. Then, an iterative formula is built using this Jacobian matrix, and the joint space coordinates (θ1, θ2, θ3) are solved using the Newton-Raphson method. Subsequently, the hydraulic cylinder extension amounts s1 and s2 of the boom and stick can be directly obtained by combining the relative rotation angles of the boom and stick with their link constraint equations. For the end effector, its link constraint equations are first simplified based on the relative rotation angle θ3 of the end effector. Then, the Jacobian matrix and iterative formula are constructed. The relevant unknowns are then solved using the Newton-Raphson method, and finally, the hydraulic cylinder extension amount s3 of the end effector is obtained based on the solution results. Specifically,
[0145] (1) When solving for the joint space coordinates, we have:
[0146]
[0147] Therefore, the Jacobian matrix Jr is:
[0148]
[0149] The constructed iterative formula is as follows:
[0150]
[0151] Where e1, e2, and e3 represent the errors between the current pose and the target pose. That is, during the iteration process, the spatial coordinates of the current pose are first calculated using Equation 3, and then the difference between the current pose spatial coordinates and the target pose spatial coordinates is taken to obtain the error. J is the Jacobian matrix r The inverse matrix.
[0152] From the above, the relative rotation angles θ1, θ2, and θ3 are finally obtained through iteration, thus yielding the target joint spatial coordinates. After obtaining the relative rotation angles, the target extension / retraction of the hydraulic cylinder needs to be further calculated using the linkage constraint equations.
[0153] (2) Solving for the driving space coordinates
[0154] 1) Solving for the extension and retraction of the hydraulic cylinder in the boom section:
[0155] Assuming the relative rotation angle of the boom is θ1 at this point, based on the geometric relationships of the boom section, we have: Then we have:
[0156]
[0157] Substituting it into Equation 4, we can obtain the result. and Therefore, the extension and retraction of the boom's hydraulic cylinder at the target working position can be calculated as s1.
[0158] 2) Calculate the extension / retraction of the hydraulic cylinder in the boom section:
[0159] Assuming the relative rotation angle of the stick is θ2 at this moment, based on the geometric relationship of the stick section... Then we have:
[0160]
[0161] Substituting it into equation 5, we can obtain the result. and Therefore, the extension and retraction of the hydraulic cylinder of the boom can be calculated as s2 when the target working position is reached.
[0162] 3) Solving for the extension and retraction of the hydraulic cylinder in the end effector section:
[0163] For the end effector part, at this time Substitute it into equation 16 and let Let α be the value of α. If β is , then:
[0164]
[0165] Similarly, using the Newton-Raphson method, we can obtain α and β in the above equation, i.e. and Substituting the known quantities into equation 15, we can obtain:
[0166]
[0167] Therefore, x can be obtained. 2-2 y 2-2 and Then the extension and retraction of the hydraulic cylinder
[0168] Therefore, through the above processes 1), 2), and 3), the target drive space coordinates (s1, s2, s3) of the hydraulic cylinder of the cantilever crusher can be calculated.
[0169] The modules or units in the device of this invention can be merged, divided, and deleted according to actual needs. The above-disclosed embodiments are merely preferred embodiments of the present invention and should not be construed as limiting the scope of the invention. Those skilled in the art will understand that implementing all or part of the processes of the above embodiments and making equivalent changes according to the claims of this invention still fall within the scope of the invention.
Claims
1. A method for solving the kinematics of a cantilever crusher, characterized in that, Includes the following steps: Step 1: Obtain the mechanical parameters of the cantilever crusher; wherein, the mechanical parameters include the DH parameters of the three-bar linkage robot of the cantilever crusher, as well as the lengths of the links of the boom, stick and end effector. Step 2: Establish the link coordinate system of the three-bar robot and the homogeneous transformation matrix of the end effector based on the DH parameters, and obtain the robot transformation equations for coordinate transformation between joint space and pose space based on the homogeneous transformation matrix of the link coordinate system and the end effector. Step 3: Based on the lengths of the links in the boom, stick, and end effector and the form of the kinematic pairs in each part, establish the corresponding linkage constraint equations for each part of the boom, stick, and end effector. Step 4: Read the current drive space coordinates of the cantilever crusher from the displacement sensors on the three hydraulic cylinders of the boom, stick, and end effector; Step 5: Calculate the current pose space coordinates of the end effector using the robot transformation equations, link constraint equations, and the current drive space coordinates; Step 6: Obtain the target pose space coordinates generated after the user moves the end effector of the cantilever crusher to the target working position based on the current pose space coordinates; Step 7: Solve for the target drive space coordinates of the hydraulic cylinder of the cantilever crusher using the robot transformation equations, the link constraint equations, the current pose space coordinates, and the target pose space coordinates.
2. The kinematic solution method for the cantilever crusher according to claim 1, characterized in that, Step 2 specifically includes: Define the joint space coordinates as (θ1, θ2, θ3) and the pose space coordinates of the end effector as (x, y, ξ); where θ1, θ2, θ3 are the relative rotation angles of the boom, stick and end effector of the cantilever crusher, respectively; x is the x-direction displacement of the end effector in the base coordinate system; y is the y-direction displacement of the end effector in the base coordinate system; and ξ is the relative rotation angle between the end effector and the Z-axis of the base coordinate system. According to robot kinematics theory, p in link coordinate system i i The coordinates of the point are represented as coordinates in the link coordinate system i-1, which yields the coordinates p of the end effector in the base coordinate system. 0 As shown in the following formula: in, The homogeneous transformation matrix of the three-bar linkage robot is as follows: Then, based on equations (1) and (2), the following set of robot transformation equations for coordinate transformation between the joint space and pose space of the cantilever crusher can be obtained: Where a0, a1, a2, and a3 are the lengths of each link in the DH parameters.
3. The kinematic solution method for the cantilever crusher according to claim 2, characterized in that, The establishment of the linkage constraint equations for the boom section in step 3 includes: Define the members constituting the boom as 0-1, 0-2, 0-3, and 0-4, l 0-1 l 0-2 l 0-3 and l 0-4 Let be the lengths of the corresponding members, and take the intersection of members 0-1 and 0-2 as the origin of the coordinate system, and establish a coordinate system O0 along the 0-2 direction on the x-axis; Therefore, the corresponding linkage constraint equations for the boom section are as follows: Where (x2, y2) are the coordinates of the slider frame in the O0 coordinate system, (x 0-3 ,y 0-3 ), (x 0-4 ,y 0-4 The coordinates of the first ends of links 0-3 and 0-4 are respectively. and These represent the rotation angles of connecting rod 0-3, connecting rod 0-4, and the hydraulic cylinder in the O0 coordinate system.
4. The kinematic solution method for the cantilever crusher according to claim 2, characterized in that, The establishment of the link constraint equations for the boom section in step 3 includes: The members constituting the boom are defined as 1-0, 1-1, 1-2, and 1-3. 1-0 l 1-1 l 1-2 、 and l 1-3 These represent the lengths of the corresponding members; a coordinate system O1 is established with the intersection of members 1-0 and 1-1 as the origin, and the x-axis along the 1-1 direction; Therefore, the corresponding set of link constraint equations for the stick section is as follows: Where (x5, y5) are the coordinates of the slider frame in the O1 coordinate system, (x 1-2 ,y 1-2 ), (x 1-3 ,y 1-3 The coordinates of the first ends of links 1-2 and 1-3 are respectively. and These represent the rotation angles of connecting rod 1-2, connecting rod 1-3, and the hydraulic cylinder in the O1 coordinate system.
5. The kinematic solution method for the cantilever crusher according to claim 2, characterized in that, The establishment of the link constraint equation set corresponding to the end effector in step 3 includes: The links constituting the end effector are defined as 2-0, 2-1, 2-2, 2-3, 3, 3-0, and 3-1, l. 2-0 l 2-1 l 2-2 l 2-3 l3, l 3-0 and l 3-1 These represent the lengths of the corresponding members; with the intersection of members 2-0 and 2-3 as the origin, a coordinate system O2 is established with the x-axis along the direction of 2-0; Therefore, the link constraint equations corresponding to the end effector are as follows: Where (x6, y6) are the coordinates of the slider frame in the O2 coordinate system, (x 2-1 ,y 2-1 ), (x 2-2 ,y 2-2 ), (x 3-1 ,y 3-1 ), (x 3-0 ,y 3-0 These are the coordinates of the ends of connecting rods 2-1, 2-2, 3-1, and 3-0, respectively. These represent the rotation angles of the hydraulic cylinder and connecting rods 2-1, 2-2, 3-1, and 3-0 in the O2 coordinate system.
6. The kinematic solution method for the cantilever crusher according to claim 1, characterized in that, Step 5 includes: The linkage constraint equations are simplified based on the current drive space coordinates (s1, s2, s3) to obtain the simplified linkage constraint equations; where s1, s2, and s3 are the extension and retraction of the hydraulic cylinders of the boom, stick, and end effector, respectively. Solve for the first Jacobian matrix of the simplified linkage constraint equations; The iterative formula is determined based on the first Jacobian matrix, and the relative rotation angles of the boom, stick, and end effector are obtained by iteratively solving the problem using the Newton-Raphson method. Substituting the relative rotation angles of the boom, stick, and end effector into the robot transformation equations, we obtain the current pose space coordinates of the end effector.
7. The kinematic solution method for the cantilever crusher according to claim 6, characterized in that, Step 7 includes: The second Jacobian matrix is constructed based on the robot transformation equations, and an iterative formula is constructed using the second Jacobian matrix, the current pose space coordinates, and the target pose space coordinates. The iterative formula constructed by the second Jacobian matrix is iteratively solved to obtain the target joint space coordinates in the joint space; wherein, the target joint space coordinates are composed of the relative rotation angles of the corresponding boom, stick and end effector; The extension and retraction of the hydraulic cylinder driving the boom is calculated using the relative rotation angle of the boom and the corresponding linkage constraint equations; the extension and retraction of the hydraulic cylinder driving the stick is calculated using the relative rotation angle of the stick and the corresponding linkage constraint equations; and the extension and retraction of the hydraulic cylinder driving the end effector is calculated using the relative rotation angle of the end effector and the corresponding linkage constraint equations.