Control system, method, break-in robot, and medium for a break-in robot

Abstract

The disclosure provides a control system and method of a forced-entry robot, the forced-entry robot and a storage medium, wherein an expected trajectory generation module is configured to determine an expected position of a forced-entry head end according to a current position of the forced-entry head end and a position of a target to be forcedly entered; an inverse kinematics solving module is configured to solve joint angles between links by using an inverse kinematics solving method with a constraint condition, and obtain a first length of a hydraulic cylinder; an environment interaction module obtains a second length of the hydraulic cylinder; and a feedback control module controls movement of a piston rod of the hydraulic cylinder based on a difference between the first length and the second length of the hydraulic cylinder. The disclosure can realize automatic control of the forced-entry robot, improve forced-entry work efficiency, solve the inverse of a planar multi-degree-of-freedom arm by using the inverse kinematics solving method with the constraint condition, effectively protect a robot body structure, improve forced-entry work safety, and improve user experience.

Description

Technical Field

[0001] This invention relates to the field of engineering machinery technology, and in particular to a control system, control method, demolition robot, and storage medium for a demolition robot. Background Technology

[0002] In recent years, robots designed for specific fields and applications (special-purpose robots) have developed rapidly. Special-purpose robots possess the engineering attributes of construction machinery and can be used in explosion protection, firefighting, excavation, demolition, and other fields. Demolition robots are a widely used type of special-purpose robot. Currently, demolition robots designed for complex environments (such as mines, post-earthquake areas, and steel mills) have a low degree of automation due to environmental factors and structural limitations. Their booms have many degrees of freedom, and analytical solutions cannot be derived from robotics. Experienced operators must visually observe and judge, sequentially operating remote control handles to position the end effector, resulting in low efficiency. Furthermore, during movement and demolition, neither the operator nor the robot itself can obtain information about the forces acting on the boom joints, increasing the risk of excessive stress and damage to the robot itself. Summary of the Invention

[0003] In view of this, one technical problem to be solved by the present invention is to provide a control system, control method, demolition robot, and storage medium for a demolition robot.

[0004] According to a first aspect of this disclosure, a control system for a demolition robot is provided, wherein the demolition robot includes: a rotating base, a robotic arm, and a demolition head; the rotating base is mounted on a chassis; the robotic arm consists of a plurality of sequentially hinged links, and hydraulic cylinders are provided on the links for controlling the movement of the links; one end of the robotic arm is hinged to the rotating base, and the other end is mounted with the demolition head; the control system includes: a desired trajectory generation module, used to determine the desired trajectory of the end of the demolition head based on the current position of the end of the demolition head and the position of the target to be demolished. The system includes: a position module; an inverse kinematics solution module for solving the joint angles between the connecting rods using a constrained inverse kinematics solution method, and determining the first length of the hydraulic cylinder based on the joint angles; an environment interaction module for acquiring the tilt angle of the connecting rod collected by the tilt sensor, and obtaining the second length of the hydraulic cylinder based on the transformation relationship between the tilt angle and the joint angle, and the transformation relationship between the joint angle and the length of the hydraulic cylinder; and a feedback control module for controlling the movement of the piston rod of the hydraulic cylinder based on the difference between the first and second lengths of the hydraulic cylinder.

[0005] Optionally, the desired trajectory generation module includes: a tool posture adjustment unit, used to obtain the normal of the breach surface of the target to be breached based on data collected by a vision sensor, and adjust the axis of the breaching head to coincide with or be parallel to the normal of the breach surface; and an end-point desired position input unit, used to obtain the position of the target to be breached based on data collected by a vision sensor, and perform spatial trajectory planning processing based on the current position of the end of the breaching head and the position of the target to be breached to determine the desired position of the end of the breaching head.

[0006] Optionally, the desired trajectory generation module further includes: an end-position initialization unit, used to establish multiple DH coordinate systems corresponding to the demolition robot and determine DH parameters corresponding to the demolition robot; and to determine the transformation matrix between the DH coordinate systems based on the DH parameters and the multiple coordinate systems.

[0007] Optionally, the demolition robot is a five-degree-of-freedom demolition robot; the number of the multiple links is four, and the relative rotation between each link is located in the same plane; the inverse kinematics solution module includes: a waist inverse kinematics unit, used to determine the rotation angle θ1 between the rotating base and the chassis based on the desired position of the end of the demolition head and the transformation matrix.

[0008] Optionally, the robotic arm includes: a second link hinged to a rotating base, a third link hinged to one end of the second link, a fourth link hinged to one end of the third link, and a fifth link hinged to one end of the fourth link; the demolition head is mounted on one end of the fifth link; the joint angle between the second link and the rotating base is θ2, the joint angle between the second link and the third link is θ3, the joint angle between the third link and the fourth link is θ4, and the joint angle between the fourth link and the fifth link is θ5. θ5 is a fixed value; the inverse kinematics solution module includes: a three-joint inverse kinematics unit for the robotic arm, used to solve θ3 and θ4 when θ2 is known, θ2 and θ4 when θ3 is known, and θ2 and θ3 when θ4 is known, based on the desired position of the end of the demolition head, the transformation matrix, and the constraints, in order to obtain θ2, θ3, and θ4; wherein, the constraints include: the normal of the demolition head coincides with or is parallel to the normal of the demolition surface, and the sum of the lever arms of the hydraulic cylinder with respect to the rotating joint is maximized.

[0009] Optionally, the inverse kinematics solving module includes: a hydraulic cylinder length solving unit, used to obtain the first length of the hydraulic cylinder based on θ2, θ3, θ4 and θ5 and the transformation relationship between the joint angle and the hydraulic cylinder length.

[0010] Optionally, the feedback control module is used to control the movement of the piston rod of the hydraulic cylinder using PID control or model predictive control strategies based on the difference between the first length and the second length of the hydraulic cylinder.

[0011] According to a second aspect of this disclosure, a control method for a demolition robot is provided, wherein the demolition robot includes: a rotating base, a robotic arm, and a demolition head; the rotating base is mounted on a chassis; the robotic arm consists of a plurality of links hinged sequentially, and a hydraulic cylinder is provided on the links for controlling the movement of the links; one end of the robotic arm is hinged to the rotating base, and the other end is mounted with the demolition head; the method includes: determining a desired position of the end of the demolition head based on the current position of the end of the demolition head and the position of the target to be demolished; solving for the joint angles between the links using an inverse kinematics solution method with constraints, and determining a first length of the hydraulic cylinder based on the joint angles; acquiring the tilt angle of the links collected by a tilt sensor, and obtaining a second length of the hydraulic cylinder based on the transformation relationship between the tilt angle and the joint angle, and the transformation relationship between the joint angle and the length of the hydraulic cylinder; and controlling the movement of the piston rod of the hydraulic cylinder based on the difference between the first length and the second length of the hydraulic cylinder.

[0012] Optionally, determining the desired position of the end of the demolition head based on the current position of the end of the demolition head and the position of the target to be demolished includes: obtaining the normal of the demolition surface of the target to be demolished based on data collected by a vision sensor, and adjusting the axis of the demolition head to coincide with or be parallel to the normal of the demolition surface; obtaining the position of the target to be demolished based on data collected by a vision sensor, and performing spatial trajectory planning processing based on the current position of the end of the demolition head and the position of the target to be demolished to determine the desired position of the end of the demolition head.

[0013] Optionally, multiple DH coordinate systems corresponding to the demolition robot are established and DH parameters corresponding to the demolition robot are determined; the transformation matrix between the DH coordinate systems is determined based on the DH parameters and the multiple coordinate systems.

[0014] Optionally, the demolition robot is a five-degree-of-freedom demolition robot; the number of the multiple links is four, and the relative rotation between each link is located in the same plane; the method of solving the joint angle between the links using the inverse kinematics solution method with constraints includes: determining the rotation angle θ1 between the rotating base and the chassis based on the desired position of the end of the demolition head and the transformation matrix.

[0015] Optionally, the robotic arm includes: a second link hinged to a rotating base, a third link hinged to one end of the second link, a fourth link hinged to one end of the third link, and a fifth link hinged to one end of the fourth link; the demolition head is mounted on one end of the fifth link; the joint angle between the second link and the rotating base is θ2, the joint angle between the second link and the third link is θ3, the joint angle between the third link and the fourth link is θ4, and the joint angle between the fourth link and the fifth link is θ5; θ5 is a fixed... The method of solving the joint angles between the links using inverse kinematics with constraints includes: solving θ3 and θ4 when θ2 is known, solving θ2 and θ4 when θ3 is known, and solving θ2 and θ3 when θ4 is known, based on the desired position of the end of the breaking head, the transformation matrix, and the constraints, to obtain θ2, θ3, and θ4; wherein the constraints include: the normal of the breaking head coincides with or is parallel to the normal of the breaking surface, and the sum of the lever arms of the hydraulic cylinder with respect to the rotating joint is maximized.

[0016] Optionally, determining the first length of the hydraulic cylinder based on the joint angle includes: obtaining the first length of the hydraulic cylinder based on θ2, θ3, θ4 and θ5 and the transformation relationship between the joint angle and the length of the hydraulic cylinder.

[0017] Optionally, controlling the movement of the piston rod of the hydraulic cylinder based on the difference between the first and second lengths of the hydraulic cylinder includes: controlling the movement of the piston rod of the hydraulic cylinder using a PID control or model predictive control strategy based on the difference between the first and second lengths of the hydraulic cylinder.

[0018] According to a third aspect of this disclosure, a control system for a demolition robot is provided, comprising: a memory; and a processor coupled to the memory, the processor being configured to perform the method described above based on instructions stored in the memory.

[0019] According to a fourth aspect of this disclosure, a demolition robot is provided, comprising: a control system for the demolition robot as described above.

[0020] According to a fifth aspect of this disclosure, a computer-readable storage medium is provided that stores computer instructions which, when executed by a processor, perform the method described above.

[0021] The control system, control method, demolition robot, and storage medium disclosed herein enable automatic control of the demolition robot, improving demolition work efficiency. By using an inverse kinematics solution method with constraints to solve the inverse kinematics of the planar multi-degree-of-freedom arm, the robot's main structure can be effectively protected, improving the safety of demolition work. Furthermore, it has good compatibility with the original control system, enhancing the user experience. Attached Figure Description

[0022] To more clearly illustrate the technical solutions in the embodiments of this disclosure or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this disclosure. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0023] Figure 1 This is a schematic diagram of a module of one embodiment of the control system of the demolition robot according to the present disclosure;

[0024] Figure 2A A schematic diagram of a desired trajectory generation module in one embodiment of the control system of the demolition robot according to the present disclosure;

[0025] Figure 2B This is a schematic diagram of the inverse kinematics solving module in one embodiment of the control system of the demolition robot according to the present disclosure;

[0026] Figure 3A A schematic diagram of the robotic arm of a demolition robot; Figure 3B A schematic diagram showing the structural parameters and coordinate system definition of the robotic arm of the demolition robot; Figure 3C A schematic diagram defining the hinge point and tilt angle of the robotic arm of a demolition robot; Figure 3D and Figure 3E A schematic diagram illustrating the working constraints of the demolition head;

[0027] Figure 4 This is a schematic diagram of the control logic of a demolition robot control system according to an embodiment of the present disclosure;

[0028] Figure 5 This is a flowchart illustrating one embodiment of the control method for a demolition robot according to the present disclosure;

[0029] Figure 6 This is a schematic diagram of a module of another embodiment of the control system of the demolition robot according to the present disclosure. Detailed Implementation

[0030] Exemplary embodiments of the present disclosure will be described below with reference to the accompanying drawings. For clarity and brevity, not all features of the embodiments are described in the specification. However, it should be understood that many implementation-specific settings must be made in carrying out the embodiments to achieve the developer's specific goals, such as complying with constraints related to the device and business, and these constraints may vary depending on the implementation. Furthermore, it should be understood that while development work can be very complex and time-consuming, such development work is merely a routine task for those skilled in the art who benefit from the present disclosure.

[0031] It should be noted that, unless otherwise specifically stated, the relative arrangement, numerical expressions, and values ​​of the components and steps set forth in these embodiments do not limit the scope of this disclosure.

[0032] Those skilled in the art will understand that the terms "first," "second," etc., in the embodiments of this disclosure are only used to distinguish different steps, devices, or modules, and do not represent any specific technical meaning, nor do they indicate a necessary logical order between them.

[0033] It should also be understood that in the embodiments disclosed herein, "a plurality of" may refer to two or more, and "at least one" may refer to one, two or more.

[0034] It should also be understood that any component, data or structure mentioned in the embodiments of this disclosure can generally be understood as one or more unless expressly defined or given to the contrary in the context.

[0035] Furthermore, the term "and / or" in this disclosure is merely a description of the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A existing alone, A and B existing simultaneously, or B existing alone. Additionally, the character " / " in this disclosure generally indicates that the preceding and following related objects have an "or" relationship.

[0036] It should also be understood that the description of the various embodiments in this disclosure emphasizes the differences between the various embodiments, and the similarities or similarities can be referred to each other. For the sake of brevity, they will not be described in detail.

[0037] At the same time, it should be understood that, for ease of description, the dimensions of the various parts shown in the accompanying drawings are not drawn according to actual scale.

[0038] The following description of at least one exemplary embodiment is merely illustrative and is in no way intended to limit this disclosure or its application or use.

[0039] Techniques, methods, and equipment known to those skilled in the art may not be discussed in detail, but where appropriate, such techniques, methods, and equipment should be considered part of the specification.

[0040] It should be noted that similar labels and letters in the following figures indicate similar items; therefore, once an item is defined in one figure, it does not need to be discussed further in subsequent figures.

[0041] Furthermore, to avoid obscuring this disclosure with unnecessary detail, only processing steps and / or apparatus structures closely related to at least the solutions according to this disclosure are shown in the accompanying drawings, while other details less relevant to this disclosure are omitted. It should also be noted that similar reference numerals and letters in the drawings indicate similar items, and therefore once an item is defined in one drawing, it need not be discussed again in subsequent drawings.

[0042] In the related technologies known to the inventor, demolition robots can be five-degree-of-freedom (DOF) or six-degree-of-freedom (DOF) robots. Since there are more than two DDFs in the same plane, the inverse kinematics cannot be solved analytically, making automated control impossible. Existing control methods rely on on-site remote control by an operator. The operator switches between controlled joints via a remote control, making multiple adjustments to bring the demolition head to the desired position. This method is inefficient and lacks precision, entirely dependent on the operator's observational skills and proficiency. Furthermore, increasing the DDF and range of motion of the demolition robot arm also increases the stress on the joints during movement and demolition operations, easily damaging the robot body or sensors mounted on the joints.

[0043] In one embodiment, this disclosure provides a control system for a demolition robot. The demolition robot includes: a rotating base, a robotic arm, and a demolition head. The rotating base is mounted on the chassis of the demolition robot and is rotatable. The robotic arm consists of multiple links hinged sequentially, and hydraulic cylinders are installed on the links to control their movement. One end of the robotic arm is hinged to the rotating base, and the other end is fitted with the demolition head. The demolition robot can be a five-degree-of-freedom, six-degree-of-freedom, or other degree-of-freedom demolition robot.

[0044] like Figure 1 As shown, the control system of the demolition robot includes: a desired trajectory generation module 11, an inverse kinematics solution module 12, an environment interaction module 14, and a feedback control module 13. The desired trajectory generation module 11 determines the desired position of the demolition head end based on the current position of the demolition head end and the position of the target to be demolished. The inverse kinematics solution module 12 uses a constrained inverse kinematics solution method to solve the joint angles between the links and determines the first length of the hydraulic cylinder based on the joint angles.

[0045] The desired trajectory generation module 11 can automatically plan the trajectory based on the current position of the end of the demolition head and the position of the target to be demolished, and output the end position of the demolition head (desired position) at each sampling time. This end position is input into the inverse kinematics solution module 12. The inverse kinematics solution module 12 calculates the optimal solution (first length) of the hydraulic cylinder length in each step of the operation through the inverse kinematics solution method with force constraints.

[0046] The environmental interaction module 14 acquires the tilt angle of the connecting rod collected by the tilt sensor. Based on the transformation relationship between the tilt angle and the joint angle, and the transformation relationship between the joint angle and the length of the hydraulic cylinder, it obtains the second length of the hydraulic cylinder. The feedback control module 13 controls the movement of the piston rod of the hydraulic cylinder based on the difference between the first length and the second length of the hydraulic cylinder.

[0047] The environmental interaction module 14 obtains the tilt angle collected by the tilt sensor installed on the robotic arm link, and transforms it between the tilt angle, joint angle, and hydraulic cylinder length to obtain real-time feedback (the second length of the hydraulic cylinder). In the feedback control module 13, the underlying control of the demolition robot is completed based on the expected value (first length) of the hydraulic cylinder length and the feedback value (second length), for example, controlling the movement of the piston rod of the hydraulic cylinder.

[0048] The feedback control module 13 can control the movement of the piston rod of the hydraulic cylinder based on the difference between the first and second lengths of the hydraulic cylinder, using PID control or model predictive control strategies. The PID control or model predictive control strategy can be any existing PID control or model predictive control strategy.

[0049] In one embodiment, such as Figure 2A As shown, the desired trajectory generation module 11 includes: a tool posture adjustment unit 111, an end-effector desired position input unit 112, and an end-effector position initialization unit 113. The tool posture adjustment unit 111 obtains the normal of the breach surface of the target to be breached based on the data collected by the vision sensor, and adjusts the axis of the breaching head to coincide with or be parallel to the normal of the breach surface.

[0050] The end-expected position input unit 112 obtains the position of the target to be demolished based on the data collected by the vision sensor, performs spatial trajectory planning processing based on the current position of the end of the demolition head and the position of the target to be demolished, and determines the expected position of the end of the demolition head.

[0051] The end position initialization unit 113 establishes multiple DH coordinate systems corresponding to the demolition robot and determines the DH parameters corresponding to the demolition robot. Based on the DH parameters and multiple coordinate systems, it determines the transformation matrix between the DH coordinate systems.

[0052] In one embodiment, the tool posture adjustment module 111 can include two input methods. In manual mode, the operator needs to observe the surface to be broken and adjust the axis of the breaking head to be parallel or coincident with the normal of the surface to be broken. In automatic mode, the normal vector of the surface to be broken can be obtained using a vision sensor (e.g., binocular vision, lidar, or other surface reconstruction methods, or multiple measurements using a laser rangefinder to determine the plane). The tool posture adjustment module 111 can be executed multiple times, ensuring that the angle deviation is within ±5° before the breaking head reaches the target object to perform the breaking operation. Figure 3D and 3E As shown.

[0053] Demolition operations require the axis of the demolition head to be parallel to or coincide with the normal to the demolition surface, i.e., posture adjustment. The posture needs to be adjusted for each operation. Once the posture of the end demolition head is determined, the joint angle θ5 is fixed during the inverse kinematics calculation of the inverse kinematics solution model 12. The optimal solution for the hydraulic cylinder length is independent of θ5.

[0054] The spatial trajectory planning process performed by the end-expected position input unit 112 based on the current position of the end of the demolition head and the position of the target to be demolished can be one of the existing spatial trajectory planning processes. The existing spatial trajectory planning process can be used to obtain the trajectory curve from the current position of the end of the demolition head to the position of the target to be demolished, and a point is determined on the trajectory curve as the expected position of the end of the demolition head.

[0055] In one embodiment, the demolition robot is a demolition robot with five degrees of freedom. The five degrees of freedom refer to the rotation between the entire robotic arm (rotating base) and the chassis, the rotation of the base linkage of the robotic arm (rotating base), and the rotation between the three motion linkages, etc. Except for the rotation between the entire robotic arm and the chassis, the other rotations are in the same plane.

[0056] like Figure 2B As shown, the inverse kinematics solution module 12 includes: a waist inverse kinematics unit 121, a robotic arm three-joint inverse kinematics unit 122, and a hydraulic cylinder length solution unit 123. Figures 3A to 3C As shown, the demolition robot is a five-degree-of-freedom demolition robot with four links, and the relative rotations between the links are located in the same plane. The waist inverse kinematics unit 121 determines the rotation angle θ1 of the rotary base based on the desired position of the demolition head end and the transformation matrix. The rotary base is equivalent to the base link of the robotic arm, and is mounted on the chassis of the demolition robot and can rotate between the robot and the chassis.

[0057] The robotic arm includes: a second link hinged to a rotating base, a third link hinged to one end of the second link, a fourth link hinged to one end of the third link, and a fifth link hinged to one end of the fourth link; a demolition head is mounted on one end of the fifth link. Figure 3A As shown, the slewing base can be regarded as the first link, the second link as link 2, the third link as link 3, the fourth link as link 4, and the fifth link as link 5; hydraulic cylinders 1 to 4 are used to drive link 2 to link 5 to run.

[0058] The rotations between the second link and the slewing base, between the second link and the third link, between the third link and the fourth link, and between the fourth link and the fifth link all occur in the same plane. For example... Figure 3B and Figure 3C As shown, the joint angle between the second link and the slewing base is θ2, the joint angle between the second link and the third link is θ3, the joint angle between the third link and the fourth link is θ4, and the joint angle between the fourth link and the fifth link is θ5; θ5 is a fixed value.

[0059] The inverse kinematics unit 122 of the robotic arm, based on the desired position of the end of the breaking head, the transformation matrix, and the constraints, solves for θ3 and θ4 when θ2 is known, solves for θ2 and θ4 when θ3 is known, and solves for θ2 and θ3 when θ4 is known, to obtain θ2, θ3, and θ4. The constraints include: the normal to the breaking head coincides with or is parallel to the normal to the breaking surface, and the total lever arm of the hydraulic cylinder with respect to the rotating joint is maximized. The hydraulic cylinder length calculation unit 113 obtains the first length of the hydraulic cylinder based on θ2, θ3, θ4, and θ5 and the transformation relationship between the joint angle and the hydraulic cylinder length.

[0060] Since θ2-θ5 are in the same plane, θ1 can be solved independently, such as Figure 4 As shown, the waist inverse kinematics unit 121 obtains the spatial coordinates of the end of the breaking head from the desired trajectory generation module 11, and solves and outputs the waist rotation angle θ1 and the position of the end of the breaking head Q in the {1} system. Since θ5 can be directly given by the tool posture adjustment unit 111, the robotic arm three-joint inverse kinematics unit 122 needs to perform the following solutions respectively: "assuming θ2 is known, solve θ3 and θ4", "assuming θ3 is known, solve θ2 and θ4", and "assuming θ4 is known, solve θ2 and θ3".

[0061] During the movement and demolition process of the demolition robot, each joint is driven by a hydraulic cylinder. When the distance between the joint hinge point and the hydraulic cylinder is at its maximum, i.e., when the lever arm is at its maximum, the joint torque can be considered to be at its minimum. Therefore, this principle can be used for the mechanical constraints in the constraint conditions. The joint lever arms corresponding to the three calculated cases are selected, and the optimal inverse solution corresponding to the maximum sum of the three joint lever arms is selected. The hydraulic cylinder length calculation unit 123 obtains the length of each hydraulic cylinder (first length) by combining the joint angles obtained through the above process with the transformation relationship between joint angles and hydraulic cylinder lengths.

[0062] like Figure 4 As shown, the environment interaction module includes multiple processing nodes such as the environment interaction interface, tilt angle transformation, and hydraulic cylinder length calculation. The environment interaction interface defines variables for data exchange between the control system and the demolition robot, and consists of two parts: the control system output (demolition robot input) and the control system input (demolition robot output). The control system output variables are determined according to the actual needs of the controlled demolition robot, such as hydraulic cylinder position, speed, force, or solenoid valve voltage and current. The control system input variables are determined by the sensor type. To avoid damage and facilitate installation, tilt sensors can be used, and the tilt angle transformation unit can complete the tilt angle-joint angle transformation.

[0063] In one embodiment, such as Figures 3A to 3C As shown, the end-position initialization unit establishes multiple DH coordinate systems corresponding to the demolition robot and determines the DH parameters corresponding to the demolition robot based on the forward kinematics calculation of the demolition robot.

[0064] DH coordinate system establishment rules: The link coordinate system is fixed to the link, and the z-axis of the DH coordinate system {i} is... i The axis coincides with the joint axis i, and the origin is located on the common perpendicular a. i At the intersection with joint axis i. x i Axis along a i The direction is from joint i to joint i+1. The link parameters are defined as follows:

[0065] a i Along x i axis, from z i Move to z i+1 Distance; α i : around x i axis, from z i Rotate to z i+1 Angle; d i Along z i axis, from x i-1 Move to x i The distance; θ i : around z i axis, from x i-1Rotate to x i The angle.

[0066] The form of coordinate transformation is:

[0067]

[0068] In this disclosure, the function cos() is abbreviated as c, and sin() is abbreviated as s. The θ symbol is omitted, and only the subscript i represents the angle of the i-th joint angle. When the subscript c or s contains multiple characters, such as c... 123 , it means cos(θ1+θ2+θ3).

[0069] The links of the robotic arm are sequentially linked 2 to 5. The upper part (rotating base) is defined as link 1, and the lower part (base or chassis) as base 0. For example, the link coordinate system {4} is fixed to link 4, with its origin located on the hinge axis between link 3 and link 4, i.e., at the intersection of the common perpendicular a4 and joint axis 4. The z4 axis of coordinate system {4} coincides with joint axis 4, and the x4 axis points from joint 4 to joint 5 along the a4 direction. a4: the distance from z4 to z5 along the x4 axis; α4: the angle of rotation from z4 to z5 around the x4 axis; d4: the distance from x3 to x4 along the z4 axis; θ4: the angle of rotation from x3 to x4 around the z4 axis. Counterclockwise rotation is defined as positive for the angle.

[0070] Based on the above DH coordinate establishment method, establish as follows: Figure 3B The coordinate system shown below, and the DH parameters of the demolition robot's robotic arm, are shown in Table 1 below:

[0071] Link arm i <![CDATA[a i / mm]]> <![CDATA[d i / mm]]> <![CDATA[α i / °]]> <![CDATA[θ i / °]]> Base 0 0 <![CDATA[l 01 ]]> 0 0 Link 1 <![CDATA[l 12 ]]> 0 <![CDATA[α1=90]]> <![CDATA[θ1]]> Link 2 <![CDATA[l 23 ]]> 0 0 <![CDATA[θ2]]> Link 3 <![CDATA[l 34 ]]> 0 0 <![CDATA[θ3]]> Link 4 <![CDATA[l 45 ]]> 0 0 <![CDATA[θ4]]> Link 5 <![CDATA[l 56 ]]> 0 0 <![CDATA[θ5]]>

[0072] Table 1 - DH Parameter Table for Robotic Arm

[0073] Substituting the DH parameters into (1-1), we obtain the transformations (transformation matrices) of each coordinate system. Therefore, the forward kinematics result is:

[0074] (1-2);

[0075] in,

[0076] σ1=s 2345

[0077] σ2=l 45 c 234 +l 56 σ3+l 34 c 23 +l 23 c2+l 12

[0078] σ3=c2345

[0079] σ4=l 45 s 234 +l 56 σ1+l 34 s 23 +l 23 s2+l 01

[0080] The solution process for the inverse element of the waist is as follows:

[0081]

[0082] Combining equations (1-2) and (1-3), and selecting terms (1,4) and (2,4) of the matrix, we obtain:

[0083]

[0084] Where, p x and p y The specific value is given by the trajectory planning value at each moment.

[0085] The solution process of the three-joint inverse kinematic unit of the robotic arm is as follows: The robotic arm consists of four consecutive arm sections in a plane. The target control quantity, namely the two coordinates of the position of the demolition head in the arm plane, is equivalent to known quantity. However, what needs to be solved are the four joint angles. Two quantities must be selected from the four joint angles as known and then solved. Select two joint angles as known angles and add reasonable constraints.

[0086] For demolition robots, firstly, it is necessary to add a constraint that the demolition head can coincide with the normal of the demolition plane, so as to provide the maximum demolition force at the demolition point; secondly, the movement of the demolition robot needs to ensure that the output of the boom's driving hydraulic cylinder is minimized at all times. Therefore, a constraint that maximizes the lever arm of the hydraulic cylinder relative to the rotating joint after movement can be added.

[0087] As can be seen from the above, the constraint of "the normal of the demolition head coinciding with the demolition plane" is equivalent to θ5 being known; the constraint of the maximum relative rotational joint lever arm of the hydraulic cylinder after movement is equivalent to assuming θ2, θ3, and θ4 are known respectively, solving for the remaining two joint angles, then calculating the sum of the stress arms of the three joints, and selecting the joint angle with the largest sum of lever arms.

[0088] Multiply equations (1-3) by the left side simultaneously. have to:

[0089]

[0090] The left side of equation (1-5) is known at this point, so it can be simplified as follows:

[0091]

[0092] Selecting terms (1,4) and (2,4) from equation (1-6), we have:

[0093]

[0094] Multiply equation (1-6) on the right We can obtain:

[0095] (1-8);

[0096] in,

[0097] u x =q x -l 56 r 32

[0098] u y =q y -l 56 r 31

[0099] Assuming θ2 and θ5 are known, find θ3 and θ4. Assuming θ2 and θ5 are known, multiply formula (1-8) by its left side. have to:

[0100] (1-9);

[0101] in

[0102] v x =u x c2-l 23 +u y s2

[0103] v y =u y c2-u x s2

[0104] Taking the terms (1,4) and (2,4) from the above expression, we have

[0105]

[0106] Therefore, we can solve this.

[0107]

[0108] in,;

[0109]

[0110]

[0111] Assuming θ3 and θ5 are known, find θ2 and θ4 as follows: Figures 3A-3C As shown, assuming θ3 and θ5 are known, then:

[0112] ∠BCG=π-θ3, ∠GMP=π-θ5;

[0113] Where ∠BCG is the angle between link 2 and link 3, and is the complementary angle of θ3. ∠GMP is the angle between link 2 and link 3, and is the complementary angle of θ5.

[0114] l 23 l 34 l 45 l 56 Given that, then we have:

[0115]

[0116] The problem is transformed into a problem where the centers of two circles are (0,0) and (q). x ,q y Given two circles with radii BG and GP, find the coordinates of their intersection point G. The algorithm code for finding the intersection point is as follows:

[0117]

[0118] Assume the coordinates of point G are (x G ,y G From the forward kinematics formula, we can obtain:

[0119]

[0120] Therefore, we can conclude that:

[0121]

[0122] The solution is:

[0123]

[0124] The conditions are:

[0125]

[0126] The conditions are:

[0127] (k∈Z)∧((q x -x G ) 2 ≤l 45 2 +2l 45 l 56 c5+l 56 2 c5 2 +l56 2 s5 2 )∧(l 45 +q x +l 56 c5≠x G );

[0128] or,

[0129]

[0130] The conditions are:

[0131]

[0132] The conditions are:

[0133] (k∈Z)∧((q y -y G ) 2 ≤l 45 2 +2l 45 l 56 c5+l 56 2 c5 2 +l 56 2 s5 2 )∧(q y +l 56 s5≠y G )

[0134] Assuming θ4 and θ5 are known, find θ2 and θ3. Assuming θ4 and θ5 are known, multiply equation (1-8) on the right simultaneously. have to:

[0135] (1-14);

[0136] in,

[0137] w x =u x -l 45 (r 32 c5+r 31 s5)

[0138] w y =u y -l 45 (r 31 c5-r 32 s5)

[0139] Taking the terms (1,4) and (2,4) from the above expression, we have

[0140]

[0141] Therefore, we can solve this.

[0142]

[0143] in,

[0144]

[0145]

[0146] like Figure 3B and 3C The definition of the hinge point and the specific mechanical constraints are as follows:

[0147] The method for solving the lever arm is: the equation of the line between two points, and the distance from the point to the line. The specific algorithm implementation is as follows: The two-point expression is:

[0148]

[0149] The distance from a point to a line is:

[0150]

[0151] Combining the two, we can conclude that:

[0152]

[0153] Therefore, it is necessary to solve for the coordinates of each joint and hydraulic cylinder hinge point in the {2} system. From the forward kinematics formula (1-1), we know that:

[0154] In coordinate system {1}, coordinates B are (0, 0) and coordinates A are (x, y). A ,y A );

[0155] The coordinates of C, D, and E in the {2} system are (l 23 ,0) T , (x D ,y D ) T , (x E ,y E ) T Therefore, in the {1} system, the coordinates of C, D, and E are (l 23 c2,l 23 s2), (x D c2-y D s2,x D s2+y D c2), (x E c2-y E s2,x E s2+y E c2);

[0156] The coordinates of G, F, and H in the {3} system are (l 34 ,0) T , (x F ,y F ) T , (x H ,y H ) T Therefore, in the {1} system, the coordinates of G, F, and H are (l 34 c 23 +l 23 c2,l 34 s 23 +l 23 s2) T 、(x F c 23 -y F s 23 +l 23 c2,x F s 23 +y F c 23 +l 23 s2) T 、(x H c 23 -y H s 23 +l 23 c2,x H s 23 +y H c 23 +l 23 s2) T ;

[0157] The coordinates of I in the {4} system are (x I ,y I ) T Therefore, in the {1} system, the coordinates of I are (x... I c 234 -y I s 234 +l 34 c 23 +l 23 c2,x I s 234 +y I c 234 +l 34 s 23 +l 23 s2) T .

[0158] Where A is the hinge point between hydraulic cylinder 1 and the upper vehicle, B is the hinge point between connecting rod 2 and the upper vehicle, C is the hinge point between connecting rod 2 and connecting rod 3, D is the hinge point between hydraulic cylinder 1 and connecting rod 2, E is the hinge point between hydraulic cylinder 2 and connecting rod 2, F is the hinge point between hydraulic cylinder 2 and connecting rod 3, G is the hinge point between connecting rod 2 and connecting rod 3, H is the hinge point between hydraulic cylinder 3 and connecting rod 3, and I is the hinge point between hydraulic cylinder 3 and connecting rod 4.

[0159] Therefore, substituting the coordinates of the points above into formula (1-16), we can obtain the distances from B to AD, C to EF, and G to HI. From this, we can calculate the lever arms of each joint. The hydraulic cylinder length calculation module determines the relationship between the hydraulic cylinder length and the joint angle as follows:

[0160] The relationship between the length of the hydraulic cylinder AD and θ2 is as follows:

[0161]

[0162] in

[0163]

[0164] Where AB is the length of the line segment connecting points A and B; BC is the length of the line segment connecting points B and C; CD is the length of the line segment connecting points C and D; and ∠BCD is the angle between line segments BC and CD.

[0165] The relationship between the length of the hydraulic cylinder EF and θ3 is as follows:

[0166]

[0167] in,

[0168]

[0169] Given the angles ∠CGH and ∠IGM, the relationship between the length of the hydraulic cylinder HI and θ4 is:

[0170]

[0171] Given the angles ∠OMP, ∠JLG, and ∠NLK, the relationship between the length of hydraulic cylinder JK and θ5 is:

[0172]

[0173] in,

[0174]

[0175] The relationship between joint angle and tilt angle is as follows:

[0176]

[0177] Among them, a2-a5 correspond to the tilt angles collected by the tilt sensors installed on each rod.

[0178] like Figure 4 As shown, the planned values ​​and feedback values ​​of each physical quantity are controlled by feedback in the feedback control module. The specific control is determined by the input of the control system. The underlying control logic can be implemented using methods such as PID and model predictive control. Among them, the planned value and feedback value refer to the planned value and feedback value of θ1-θ5, x is conventionally represented as position, xc represents the expected value of the input position, and fb represents feedback, i.e., the feedback quantity.

[0179] The control system of the demolition robot disclosed herein can directly plan and control the spatial position of the end-effector demolition head, automate the demolition robot, reduce dependence on operators, and simplify control. It realizes the inverse kinematics solution of the planar multi-degree-of-freedom arm and adopts an optimization algorithm with force constraints to minimize the stress on the joints of the robotic arm during movement and demolition, thus protecting the robot's main structure. It uses tilt sensors, which are simple and convenient to install and reduce modifications to the original structure. The tilt sensors can be externally mounted on the robotic arm links to reduce the possibility of damage due to joint stress. The system does not conflict with the original operating logic and can quickly switch functions as needed. The entire system achieves low-cost, quick installation and good compatibility.

[0180] Figure 5 This is a flowchart illustrating one embodiment of the control method for a demolition robot according to the present disclosure, as follows: Figure 5 As shown:

[0181] Step 501: Determine the desired position of the end of the demolition head based on the current position of the end of the demolition head and the position of the target to be demolished.

[0182] Step 502: Solve the joint angle between the connecting rods using the inverse kinematics solution method with constraints, and determine the first length of the hydraulic cylinder based on the joint angle.

[0183] Step 503: Obtain the tilt angle of the connecting rod collected by the tilt sensor. Based on the transformation relationship between the tilt angle and the joint angle, and the transformation relationship between the joint angle and the length of the hydraulic cylinder, obtain the second length of the hydraulic cylinder.

[0184] Step 504: Based on the difference between the first and second lengths of the hydraulic cylinder, control the movement of the piston rod of the hydraulic cylinder.

[0185] In one embodiment, based on data collected by a vision sensor, the normal to the breach surface of the target to be breached is obtained, and the axis of the breaching head is adjusted to coincide with or be parallel to the normal to the breach surface. The position of the target to be breached is obtained based on the data collected by the vision sensor. Spatial trajectory planning is performed based on the current position of the breaching head's end and the position of the target to be breached to determine the desired position of the breaching head's end. Multiple DH coordinate systems corresponding to the breaching robot are established, and DH parameters corresponding to the breaching robot are determined. The transformation matrix between the DH coordinate systems is determined based on the DH parameters and the multiple coordinate systems.

[0186] Based on the desired position of the dismantling head end and the transformation matrix, the rotation angle θ1 between the robotic arm and the rotating base is determined. Based on the desired position of the dismantling head end, the transformation matrix, and the constraints, θ3 and θ4 are solved when θ2 is known, θ2 and θ4 are solved when θ3 is known, and θ2 and θ3 are solved when θ4 is known, to obtain θ2, θ3, and θ4. The constraints include: the normal to the dismantling head coincides with or is parallel to the normal to the dismantling surface, and the total lever arm of the hydraulic cylinder with respect to the rotating joint is maximized. Based on θ2, θ3, θ4, and θ5 and the transformation relationship between the joint angle and the hydraulic cylinder length, the first length of the hydraulic cylinder is obtained. Based on the difference between the first and second lengths of the hydraulic cylinder, PID control or model predictive control strategies are used to control the movement of the piston rod of the hydraulic cylinder.

[0187] Figure 6 This is a schematic diagram of a module of yet another embodiment of the control system for a demolition robot according to the present disclosure. Figure 6 As shown, the model processing device may include a memory 601, a processor 602, a communication interface 603, and a bus 604. The memory 601 is used to store instructions, and the processor 602 is coupled to the memory 601. The processor 602 is configured to execute the above-described method based on the instructions stored in the memory 601.

[0188] The memory 601 can be high-speed RAM, non-volatile memory, or a memory array. The memory 601 may also be divided into blocks, and these blocks can be combined into virtual volumes according to certain rules. The processor 602 can be a central processing unit (CPU), an application-specific integrated circuit (ASIC), or one or more integrated circuits configured to implement the methods of this disclosure.

[0189] In one embodiment, this disclosure provides a demolition robot, including a control system for the demolition robot as described in any of the above embodiments.

[0190] In one embodiment, this disclosure provides a computer-readable storage medium storing computer instructions that, when executed by a processor, implement the model processing method as described in any of the preceding embodiments.

[0191] Computer-readable storage media may take the form of any combination of one or more readable media. A readable medium may be a readable signal medium or a readable storage medium. A readable storage medium may, for example, include, but is not limited to, electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatuses, or devices, or any combination thereof. More specific examples (not an exhaustive list) of readable storage media may include: electrical connections having one or more wires, portable disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof.

[0192] Embodiments of this disclosure may also be computer program products, including computer program instructions that, when executed by a processor, cause the processor to perform the steps in the methods according to various embodiments of this disclosure as described in the "Exemplary Methods" section above.

[0193] The control system, control method, demolition robot, and storage medium of the demolition robot in the above embodiments can realize automatic control of the demolition robot and improve the efficiency of demolition work. By using the inverse kinematics solution method with constraints to solve the inverse kinematics of the planar multi-degree-of-freedom arm, the robot body structure can be effectively protected, the safety of demolition work can be improved, and it has good compatibility with the original control system, improving the user experience.

[0194] The basic principles of this disclosure have been described above with reference to specific embodiments. However, it should be noted that the advantages, benefits, and effects mentioned in this disclosure are merely examples and not limitations, and should not be considered as essential features of each embodiment of this disclosure. Furthermore, the specific details disclosed above are for illustrative and facilitative purposes only, and are not limitations. These details do not limit the scope of this disclosure to the necessity of employing the aforementioned specific details for implementation.

[0195] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For system embodiments, since they largely correspond to method embodiments, the description is relatively simple; relevant parts can be referred to the descriptions in the method embodiments.

[0196] The block diagrams of devices, apparatuses, devices, and systems disclosed herein are merely illustrative examples and are not intended to require or imply that they must be connected, arranged, or configured in the manner shown in the block diagrams. As those skilled in the art will recognize, these devices, apparatuses, devices, and systems can be connected, arranged, and configured in any manner. Words such as “comprising,” “including,” “having,” etc., are open-ended terms meaning “including but not limited to,” and are used interchangeably with them. The terms “or” and “and” as used herein refer to the terms “and / or,” and are used interchangeably with them unless the context clearly indicates otherwise. The term “such as” as used herein refers to the phrase “such as but not limited to,” and is used interchangeably with it.

[0197] It should also be noted that in the apparatus, devices, and methods of this disclosure, the components or steps can be disassembled and / or recombined. These disassemblies and / or recombinations should be considered as equivalent solutions to this disclosure.

[0198] The above description of the disclosed aspects is provided to enable any person skilled in the art to make or use this disclosure. Various modifications to these aspects will be readily apparent to those skilled in the art, and the general principles defined herein can be applied to other aspects without departing from the scope of this disclosure. Therefore, this disclosure is not intended to be limited to the aspects shown herein, but rather to be carried out within the widest scope consistent with the principles and novel features disclosed herein.

[0199] The above description has been given for illustrative and descriptive purposes. Furthermore, this description is not intended to limit the embodiments of this disclosure to the forms disclosed herein. Although several exemplary aspects and embodiments have been discussed above, those skilled in the art will understand that the above embodiments are illustrative only and do not limit the scope of this disclosure. Those skilled in the art will understand that the above embodiments can be combined, modified, or replaced without departing from the scope and spirit of this disclosure.

Claims

1. A control system for a demolition robot, wherein, The demolition robot includes: a rotating base, a robotic arm, and a demolition head; the rotating base is mounted on a chassis; the robotic arm consists of multiple sequentially hinged links, and hydraulic cylinders are installed on the links to control their movement; one end of the robotic arm is hinged to the rotating base, and the demolition head is mounted on the other end; the control system includes: The desired trajectory generation module is used to determine the desired position of the end of the demolition head based on the current position of the end of the demolition head and the position of the target to be demolished. The inverse kinematics solution module is used to solve the joint angle between the connecting rods using an inverse kinematics solution method with constraints, and to determine the first length of the hydraulic cylinder based on the joint angle. An environmental interaction module is used to acquire the tilt angle of the connecting rod collected by the tilt sensor, and to obtain the second length of the hydraulic cylinder based on the transformation relationship between the tilt angle and the joint angle, and the transformation relationship between the joint angle and the length of the hydraulic cylinder. A feedback control module is used to control the movement of the piston rod of the hydraulic cylinder based on the difference between the first length and the second length of the hydraulic cylinder; The demolition robot is a five-degree-of-freedom demolition robot. The robotic arm includes: a second link hinged to a rotating base, a third link hinged to one end of the second link, a fourth link hinged to one end of the third link, and a fifth link hinged to one end of the fourth link. The demolition head is mounted on one end of the fifth link. The joint angle between the second link and the rotating base is θ2, the joint angle between the second link and the third link is θ3, the joint angle between the third link and the fourth link is θ4, and the joint angle between the fourth link and the fifth link is θ5. θ5 is a fixed value. The relative rotation between each link is within the same plane. The inverse kinematics solution module includes: The waist inverse kinematics unit is used to determine the rotation angle θ1 between the slewing base and the chassis based on the desired position of the end of the breaking head and the transformation matrix between the DH coordinate system; The three-joint inverse kinematics unit of the robotic arm is used to solve for θ3 and θ4 when θ2 is known, θ2 and θ4 when θ3 is known, and θ2 and θ3 when θ4 is known, based on the desired position of the end of the demolition head, the transformation matrix, and the constraints, in order to obtain θ2, θ3, and θ4. The constraints include: the axis of the demolition head coincides with or is parallel to the normal of the demolition surface of the target to be demolished, and the total lever arm of the hydraulic cylinder with respect to the rotating joint is maximized.

2. The system as claimed in claim 1, wherein, The desired trajectory generation module includes: The tool posture adjustment unit is used to obtain the normal of the demolition surface of the target to be demolished based on the data collected by the vision sensor, and adjust the axis of the demolition head to coincide with or be parallel to the normal of the demolition surface. The end-point desired position input unit is used to obtain the position of the target to be breached based on the data collected by the vision sensor, and to perform spatial trajectory planning processing based on the current position of the breaching head end and the position of the target to be breached to determine the desired position of the breaching head end.

3. The system as described in claim 2, wherein, The desired trajectory generation module further includes: The end-effector initialization unit is used to establish multiple DH coordinate systems corresponding to the demolition robot and determine the DH parameters corresponding to the demolition robot; and to determine the transformation matrix between the DH coordinate systems based on the DH parameters and the multiple coordinate systems.

4. The system as described in claim 3, wherein, The inverse kinematics solution module includes: The hydraulic cylinder length calculation unit is used to obtain the first length of the hydraulic cylinder based on θ2, θ3, θ4 and θ5 and the transformation relationship between the joint angle and the hydraulic cylinder length.

5. The system according to any one of claims 1 to 4, wherein, The feedback control module is used to control the movement of the piston rod of the hydraulic cylinder based on the difference between the first length and the second length of the hydraulic cylinder, using PID control or model predictive control strategies.

6. A control method for a demolition robot, wherein, The demolition robot includes: a rotating base, a robotic arm, and a demolition head; the rotating base is mounted on a chassis; the robotic arm consists of multiple sequentially hinged links, and hydraulic cylinders are installed on the links to control their movement; one end of the robotic arm is hinged to the rotating base, and the demolition head is mounted on the other end; the method includes: Based on the current position of the end of the demolition head and the position of the target to be demolished, determine the desired position of the end of the demolition head; The joint angles between the links are solved using an inverse kinematics method with constraints, and the first length of the hydraulic cylinder is determined based on the joint angles. The tilt angle of the connecting rod is acquired by the tilt sensor. Based on the transformation relationship between the tilt angle and the joint angle, and the transformation relationship between the joint angle and the length of the hydraulic cylinder, the second length of the hydraulic cylinder is obtained. The movement of the piston rod of the hydraulic cylinder is controlled based on the difference between the first length and the second length of the hydraulic cylinder. The demolition robot is a five-degree-of-freedom demolition robot. The robotic arm includes: a second link hinged to a rotating base, a third link hinged to one end of the second link, a fourth link hinged to one end of the third link, and a fifth link hinged to one end of the fourth link. The demolition head is mounted on one end of the fifth link. The joint angle between the second link and the rotating base is θ2, the joint angle between the second link and the third link is θ3, the joint angle between the third link and the fourth link is θ4, and the joint angle between the fourth link and the fifth link is θ5. θ5 is a fixed value. The relative rotation between each link is within the same plane. The method of solving the joint angles between links using inverse kinematics with constraints includes: Based on the desired position of the end of the demolition head and the transformation matrix between the DH coordinate system, the rotation angle θ1 between the rotary base and the chassis is determined. The method of solving the joint angles between links using inverse kinematics with constraints includes: Based on the desired position of the end of the demolition head, the transformation matrix, and the constraints, θ3 and θ4 are solved when θ2 is known, θ2 and θ4 are solved when θ3 is known, and θ2 and θ3 are solved when θ4 is known, in order to obtain θ2, θ3, and θ4. The constraints include: the axis of the demolition head coincides with or is parallel to the normal of the demolition surface of the target to be demolished, and the total lever arm of the hydraulic cylinder with respect to the rotating joint is maximized.

7. The method of claim 6, wherein determining the desired position of the end of the demolition head based on the current position of the end of the demolition head and the position of the target to be demolished comprises: Based on the data collected by the vision sensor, the normal of the demolition surface of the target to be demolished is obtained, and the axis of the demolition head is adjusted to coincide with or be parallel to the normal of the demolition surface. The location of the target to be breached is obtained based on data collected by a visual sensor. Spatial trajectory planning is performed based on the current location of the breaching head end and the location of the target to be breached to determine the desired location of the breaching head end.

8. The method of claim 7, further comprising: Establish multiple DH coordinate systems corresponding to the demolition robot and determine the DH parameters corresponding to the demolition robot; determine the transformation matrix between the DH coordinate systems based on the DH parameters and the multiple coordinate systems.

9. The method of claim 8, wherein, Determining the first length of the hydraulic cylinder based on the joint angle includes: The first length of the hydraulic cylinder is obtained based on θ2, θ3, θ4, and θ5 and the transformation relationship between the joint angle and the length of the hydraulic cylinder.

10. The method of any one of claims 6 to 9, wherein controlling the movement of the piston rod of the hydraulic cylinder based on the difference between the first length and the second length of the hydraulic cylinder comprises: Based on the difference between the first and second lengths of the hydraulic cylinder, the movement of the piston rod of the hydraulic cylinder is controlled using PID control or model predictive control strategies.

11. A control system for a demolition robot, comprising: Memory; and a processor coupled to the memory, the processor being configured to perform the method as described in any one of claims 6 to 10 based on instructions stored in the memory.

12. A demolition robot, comprising: The control system of the demolition robot as described in any one of claims 1 to 5, or the control system of the demolition robot as described in claim 11.

13. A computer-readable storage medium storing computer instructions that are executed by a processor according to any one of claims 6 to 10.

Images