A robot arm path planning and automatic control method

Abstract

The application discloses a mechanical arm path planning and automatic control method, comprising the following steps: installing an inclination sensor on a swing arm, a bucket rod and a rocker position of an excavator respectively, and measuring a bucket relative angle, a bucket rod relative angle and a swing arm relative angle after calibration; establishing a vehicle body reference coordinate system with a swing arm rear hinge point as an origin; establishing a local coordinate system of each degree of freedom according to a right-hand rule, and converting to the vehicle body reference coordinate system; recording Xa coordinates and Za coordinates of a current position of a bucket tooth tip in the vehicle body reference coordinate system, confirming Xb coordinates and Zb coordinates of a target position, obtaining a plurality of intermediate process points after discretization, calculating a control amount through an error solution of the target position and the current position, controlling the movement of each degree of freedom, and realizing the vertical movement of the excavator bucket tooth tip position. The application can control the swing arm lifting, the bucket rod retraction and swing, and the bucket retraction and swing, realize the automatic vertical lifting function of the excavator tooth tip position, and effectively improve the working efficiency of the excavator.

Description

Technical Field

[0001] This invention belongs to the field of excavator control technology, specifically relating to a method for robotic arm path planning and automatic control. Background Technology

[0002] Excavators are construction machinery that use hydraulic oil to drive the working device and dig with a bucket. They are multi-purpose earthmoving and rock excavation machines, mainly used for earth and rock excavation and loading, but can also be used for land leveling, slope repair, hoisting, crushing, demolition, ditching and other operations. Therefore, they are widely used in road construction such as highways and railways, bridge construction, urban construction, airport and port construction and water conservancy construction.

[0003] Furthermore, excavators can meet various work requirements beyond excavation by changing their working devices. Replacing the excavator's bucket with a solidification mixing rod enables sludge solidification treatment. The automatic vertical lifting and lowering function of the solidification mixing rod reduces operating steps and improves work efficiency.

[0004] To achieve the above functions, it is necessary to precisely control three sets of actions: boom lifting, stick retraction and outward swing, and bucket retraction and outward swing. Traditionally, these actions are controlled manually, resulting in poor linkage between the three sets of actions, low control precision, slow adjustment of the robotic arm's movement, and reduced excavator working efficiency. Summary of the Invention

[0005] The purpose of this invention is to provide a robotic arm path planning and automatic control method that can precisely control the boom lifting, stick retraction and outward swing, and bucket retraction and outward swing, with three sets of actions linked and controlled to achieve automatic vertical lifting and lowering of the excavator's tooth tip position, effectively improving the excavator's working efficiency.

[0006] To achieve the above objectives, the present invention provides a method for robotic arm path planning and automatic control, comprising the following steps:

[0007] S1. Install tilt sensors at the boom, stick, and rocker arm positions of the excavator respectively, and measure the relative angles of the bucket (Theta1), stick (Theta2), and boom (Theta3) after calibration.

[0008] S2. Establish the upper car body reference coordinate system with the boom rear hinge point O1 as the origin, which conforms to the right-hand rule;

[0009] S3. Each degree of freedom establishes its own local coordinate system according to the right-hand rule and transforms it to the vehicle body reference coordinate system established in S2;

[0010] S4. In automatic mode, record the Xa and Za coordinates of the current position of the bucket tooth tip in the vehicle body reference coordinate system, confirm the Xb and Zb coordinates of the target position, and obtain several intermediate process points (X1, Z1)...(Xn, Zn) after discretization. Calculate the control quantity through the error solution between the target position and the current position, control the movement of each degree of freedom, and realize the vertical movement of the excavator bucket tooth tip position.

[0011] As a further aspect of the present invention: calibration of each tilt sensor:

[0012] Control the excavator to enter the calibrated posture and obtain the angle values ​​displayed by the corresponding tilt sensors of the boom, stick, and rocker arm;

[0013] The angular offset of the boom, stick, and rocker tilt sensors under the calibration posture is calculated using inverse trigonometric functions.

[0014] The actual angle of the tilt sensor is calculated based on the angle value displayed by the tilt sensor and the angle offset of the tilt sensor, thereby obtaining the actual angles of the boom, stick, rocker arm and horizontal line.

[0015] As a further aspect of this invention: the establishment and transformation of coordinate systems for each degree of freedom:

[0016] A reference coordinate system for the upper vehicle body is established with the boom rear hinge point O1 as the origin, conforming to the right-hand rule. The positive X-axis is defined as the direction of track forward movement; the positive Y-axis is perpendicular to the X-axis, and the plane containing the Z-axis points to the left; the positive Z-axis is perpendicular to the horizontal plane containing the bottom surface of the track and points upward.

[0017] The other degrees of freedom establish their own local coordinate systems according to the right-hand rule and then transform them to the vehicle body reference coordinate system.

[0018] As a further aspect of the present invention: In S4, the calculated control quantity is adjusted in real time through a PID algorithm to control the stability and accuracy of the robotic arm's movements, thereby achieving vertical motion control of the excavator bucket teeth.

[0019] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0020] By reducing manual operation steps and realizing automated system control, the system can precisely control the boom lifting, stick retraction and outward swing, and bucket retraction and outward swing. The three sets of actions are linked and controlled to realize the automatic vertical lifting and lowering function of the excavator tooth tip position, effectively improving the excavator's working efficiency.

[0021] Compared to traditional manual control, which requires real-time observation of the equipment's position and adjustment of the handle ratio to adjust the corresponding movement speed, resulting in lower control precision, this invention collects data from the tilt sensor and the established coordinate system in real time during operation. Based on the PID control algorithm, it automatically controls the movement of the robotic arm and adjusts the speed of each movement, thereby effectively improving control precision. Attached Figure Description

[0022] Figure 1 This is a schematic diagram of the coordinate system and multiple angles of the present invention;

[0023] Figure 2 This is a schematic diagram illustrating an embodiment of the present invention. Detailed Implementation

[0024] The invention will now be further described with reference to the accompanying drawings.

[0025] like Figure 1 and Figure 2 As shown, a method for path planning and automatic control of a robotic arm includes the following steps:

[0026] S1. Install tilt sensors at the boom, stick, and rocker arm positions of the excavator respectively, and measure the relative angles of the bucket (Theta1), stick (Theta2), and boom (Theta3) after calibration.

[0027] Because the installation position of the tilt sensor can introduce deviations, the tilt sensor needs to be calibrated. Specifically, the calibration of each tilt sensor is as follows:

[0028] Ref = Sensor + Offset (1)

[0029] Control the excavator to enter the calibration posture and obtain the angle values ​​displayed by the corresponding tilt angle sensors of the boom, stick, and rocker arm;

[0030] The angular offset of the boom, stick, and rocker tilt sensors under the calibration posture is calculated using inverse trigonometric functions.

[0031] The actual angle of the tilt sensor is calculated based on the angle value displayed by the tilt sensor and the angle offset of the tilt sensor, thereby obtaining the actual angles of the boom, stick, rocker arm and horizontal line.

[0032] S2. Establish the upper vehicle body reference coordinate system with the boom rear hinge point O1 as the origin, which conforms to the right-hand rule.

[0033] S3. Each degree of freedom establishes its own local coordinate system according to the right-hand rule and transforms it to the vehicle body reference coordinate system established in S2.

[0034] Specifically, the establishment and transformation of coordinate systems for each degree of freedom:

[0035] A reference coordinate system for the upper vehicle body is established with the boom rear hinge point O1 as the origin, conforming to the right-hand rule. The positive X-axis is defined as the direction of track forward movement; the positive Y-axis is perpendicular to the X-axis, and the plane containing the Z-axis points to the left; the positive Z-axis is perpendicular to the horizontal plane containing the bottom surface of the track and points upward.

[0036] The other degrees of freedom establish their own local coordinate systems according to the right-hand rule and then transform them to the vehicle body reference coordinate system.

[0037] S4. In automatic mode, record the Xa and Za coordinates of the current position of the bucket tooth tip in the vehicle body reference coordinate system, confirm the Xb and Zb coordinates of the target position, and obtain several intermediate process points (X1, Z1)...(Xn, Zn) after discretization. Calculate the control quantity Y using the error solution between the target position and the current position. k Control quantity Y k The calculation formula is as follows:

[0038]

[0039] Where: Y k Let K be the control output at time k, e(k) be the error at time k, e(k-1) be the error at time k-1, and K be the control output at time k. p T is the proportionality coefficient. n T is the integration constant. v is a differential constant, and T is the sampling period.

[0040] Then, by controlling the movement of each degree of freedom, the vertical movement of the excavator bucket tooth tip is achieved.

[0041] Furthermore, based on the calculated control quantity, the PID algorithm is used to adjust in real time (Formula 2) to control the stability and accuracy of the robotic arm's movements, thereby achieving vertical motion control of the excavator bucket teeth.

Claims

1. A method for path planning and automatic control of a robotic arm, characterized in that, Includes the following steps: S1. Install tilt sensors at the boom, stick, and rocker arm positions of the excavator respectively, and measure the relative angles of the bucket (Theta1), stick (Theta2), and boom (Theta3) after calibration. S2. Establish the upper car body reference coordinate system with the boom rear hinge point O1 as the origin, which conforms to the right-hand rule; S3. Each degree of freedom establishes its own local coordinate system according to the right-hand rule and transforms it to the vehicle body reference coordinate system established in S2; S4. In automatic mode, record the Xa and Za coordinates of the current position of the bucket tooth tip in the vehicle body reference coordinate system, confirm the Xb and Zb coordinates of the target position, and obtain several intermediate process points (X1, Z1) ... (Xn, Zn) after discretization. Calculate the control quantity through the error solution between the target position and the current position, control the movement of each degree of freedom, and realize the vertical movement of the excavator bucket tooth tip position. The formula for calculating the control quantity is as follows: ； in: To control the output at time k, Let k be the error at time k. The error at time k-1, This is the proportionality coefficient. Let be the integration constant. is a differential constant, and T is the sampling period.

2. The robotic arm path planning and automatic control method according to claim 1, characterized in that, Calibration of each tilt sensor: Control the excavator to enter the calibrated posture and obtain the angle values ​​displayed by the corresponding tilt sensors of the boom, stick, and rocker arm; The angular offset of the boom, stick, and rocker tilt sensors under the calibration posture is calculated using inverse trigonometric functions. The actual angle of the tilt sensor is calculated based on the angle value displayed by the tilt sensor and the angle offset of the tilt sensor, thereby obtaining the actual angles of the boom, stick, rocker arm and horizontal line.

3. The robotic arm path planning and automatic control method according to claim 1, characterized in that, Establishment and transformation of coordinate systems for each degree of freedom: A reference coordinate system for the upper vehicle body is established with the boom rear hinge point O1 as the origin, conforming to the right-hand rule. The positive X-axis is defined as the direction of track forward movement; the positive Y-axis is perpendicular to the X-axis, and the plane containing the Z-axis points to the left; the positive Z-axis is perpendicular to the horizontal plane containing the bottom surface of the track and points upward. The other degrees of freedom establish their own local coordinate systems according to the right-hand rule and then transform them to the vehicle body reference coordinate system.

4. The robotic arm path planning and automatic control method according to claim 1, characterized in that, In S4, based on the calculated control quantity, the PID algorithm is used to adjust in real time to control the stability and accuracy of the robotic arm's movements, thereby achieving vertical motion control of the excavator bucket teeth.

Images