A robot arm posture control method based on inclination sensor

Abstract

The present application relates to the technical field of engineering machinery, and discloses a method for controlling the posture of a mechanical arm based on an inclination sensor, comprising the following steps: S1, constructing a mathematical model for measuring the rotation angle of the mechanical arm according to the inclination sensor; S2, obtaining the output value of the inclination sensor, wherein the inclination sensor is installed at the joint of the mechanical arm; S3, calculating the rotation angle of the mechanical arm through the output value of the inclination sensor and the mathematical model; S4, calculating the yaw angle of the mechanical arm according to the rotation angle of the mechanical arm; S5, optimizing the posture control of the mechanical arm according to the yaw angle of the mechanical arm. The measurement results of the method for controlling the posture of the mechanical arm of the present application are stable and reliable, which brings great convenience to engineering personnel.

Description

Technical Field

[0001] The invention belongs to the field of engineering machinery, and in particular relates to a mechanical arm posture control method based on an inclination sensor. Background Art

[0002] In modern technology, there are many ways to measure the spatial rotation angle of the robot arm, such as gyroscopes. The gyroscope can detect the current rotation rate, obtain the current rotation angle through rate integration, and then perform all-round angle detection on the robot arm. However, the gyroscope has wandering errors. Even if the angle of the robot arm remains unchanged, the gyroscope still outputs signals and cannot accurately measure the rotation angle. In the prior art, there is a scheme for measuring the rotation angle of the mechanical wall using a tilt sensor. For example, in the Chinese utility model patent with the authorization announcement number CN205219117U, a posture self-correcting robot system based on a tilt sensor is disclosed. The actual motion angle of the robot is measured by the tilt sensor, and then the motion angle compensation is judged and controlled by a microprocessor to achieve self-calibration of the robot. However, the patent only discloses structural features such as the tilt sensor, the motor driver and the power module. It is unknown to those skilled in the art how to solve the relationship between the output value of the tilt sensor and the motion angle of the robot in order to achieve motion control of the robot arm. Summary of the invention

[0003] The present invention provides a robot arm posture control method based on a tilt sensor to solve at least one of the above technical problems.

[0004] The technical solution adopted by the present invention is:

[0005] The present invention provides a method for controlling the position and posture of a mechanical arm based on an inclination sensor, which is characterized by comprising the following steps:

[0006] S1, constructing a mathematical model for measuring the rotation angle of the robotic arm according to the inclination sensor;

[0007] S2, obtaining an output value of the inclination sensor, wherein the inclination sensor is installed at a joint of the mechanical arm;

[0008] S3, calculating the rotation angle of the mechanical arm by using the output value of the inclination sensor and the mathematical model;

[0009] S4, calculating the yaw angle of the robotic arm according to the rotation angle of the robotic arm;

[0010] S5. Optimizing the posture control of the robotic arm according to the yaw angle of the robotic arm.

[0011] As a preferred embodiment of the present invention, the inclination sensor is a dual-axis inclination sensor, and step S1 comprises:

[0012] Setting a horizontal plane as a reference plane for the measuring axis of the inclination sensor, wherein the measuring axes X and Y of the inclination sensor are perpendicular to each other;

[0013] According to the reference plane, a first coordinate system oxyz is established, wherein the y-axis is the projection of the rotation axis of the robot arm on the horizontal plane, the x-axis is perpendicular to the y-axis and is in the same horizontal plane as the y-axis, and the z-axis is perpendicular to the horizontal plane and intersects the x-axis and the y-axis at point o;

[0014] After the robot arm is rotated by a certain angle, a second coordinate system ouvw is established, where the ou axis coincides with the x axis, the ov axis coincides with the rotation axis of the robot arm, and the ow axis is the normal vector of the uov plane passing through point o;

[0015] A mathematical model of the measurement value of the inclination sensor and the rotation angle of the robot arm is established according to the first coordinate system oxyz and the second coordinate system ouvw.

[0016] As a preferred embodiment of the present invention, in step S1, according to the first coordinate system oxyz and the second coordinate system ouvw, a mathematical model of the measured value of the inclination sensor and the rotation angle of the robotic arm is established as follows:

[0017] tx=arcsin(sinθcosβ) (1)

[0018] ty=β (2)

[0019] Among them, tx is the X-axis output value of the inclination sensor measuring axis after the robotic arm rotates by an angle θ, ty is the Y-axis output value of the inclination sensor measuring axis, θ is the rotation angle of the robotic arm, and β is the angle between the rotation axis of the robotic arm and the horizontal plane.

[0020] As a preferred embodiment of the present invention, in step S4, the specific method for calculating the yaw angle of the robotic arm according to the rotation angle of the robotic arm is as follows:

[0021] Assuming tx and ty are the output values ​​of the inclination sensor on the X and Y axes, the initial rotation angle θ1′ when the robot arm is not rotating is obtained from formula (1) as follows:

[0022]

[0023] Wherein, θ1′ is the rotation angle calculated due to the tilt of the robot carrier when it is not rotated, tx1 is the output value of the tilt sensor X1 axis when it is not rotated, and ty1 is the angle between the tilt sensor Z1 axis and the plane;

[0024] After the tilt sensor rotates by an angle θ, the expression of the rotation angle θ1″ is as follows:

[0025]

[0026] Wherein, θ1″ is the rotation angle calculated by the rotation angle θ, and tx1′ is the output value of the measuring axis X-axis of the inclination sensor;

[0027] The expression for calculating the yaw angle according to the rotation angle of the robotic arm is:

[0028]

[0029]

[0030] Wherein, α' is the yaw angle of the robotic arm relative to the horizontal plane when the robotic arm is not rotated, and α" is the yaw angle of the robotic arm relative to the horizontal plane after the robotic arm rotates by an angle θ;

[0031] Processing (3), (4), (5), and (6) yields:

[0032]

[0033]

[0034] α=α”-α'

[0035] Wherein, α is the actual yaw angle of the robotic arm.

[0036] As a preferred implementation manner of the present invention, in step S2, the measuring axis of the inclination sensor is parallel to the rotation axis of the robot arm and is in the same direction.

[0037] As a preferred embodiment of the present invention, the angle between the rotation axis of the robotic arm and the horizontal plane is β, and the value range of β is β∈(0,90°).

[0038] Due to the adoption of the above technical solution, the beneficial effects achieved by the present invention are as follows:

[0039] The present invention relates to a method for measuring the yaw angle of a mechanical arm by using a tilt sensor and controlling the mechanical arm to reach a target position according to the yaw angle. The tilt sensor can detect the angle between any detection position that is not perpendicular to the horizontal plane and the horizontal plane, and the measurement result is stable and reliable, which is more suitable for engineering applications. The yaw position of the mechanical arm can be measured by using the tilt sensor, and then the posture of the mechanical arm can be controlled to reach the target state. The method uses the tilt sensor as a measuring tool, and forms an unknown spatial angle according to the spatial relationship between the installation mode of the tilt sensor and the rotation axis that is not perpendicular to the horizontal plane, thereby establishing a mathematical model of the output value of the tilt sensor and the rotation angle, and the rotation angle is calculated according to the established mathematical model and known conditions, and finally derives a formula for solving the yaw angle of the mechanical arm. According to the yaw angle, the deviation of the mechanical arm from the target position is controlled, and then the posture control of the mechanical arm is optimized. The method of the present invention avoids the problem of reduced accuracy and increased cumulative error caused by the use of gyroscopes and other methods to measure the yaw angle of engineering machinery, which brings great convenience to engineering personnel. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] The drawings described herein are used to provide a further understanding of the present invention and constitute a part of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present application and do not constitute an improper limitation of the present invention. In the drawings:

[0041] Figure 1 Schematic diagram of the flow of a method for controlling a mechanical arm posture based on a tilt sensor according to an embodiment of the present invention;

[0042] Figure 2 A schematic diagram of a coordinate system of a space model according to an embodiment of the present invention;

[0043] Figure 3 A schematic diagram of the structure of a mechanical arm provided by an embodiment of the present invention;

[0044] Figure 4 It is a schematic diagram of measuring the yaw angle by applying a space model in an embodiment of the present invention.

[0045] in,

[0046] 1-Robotic arm;

[0047] 2-Tilt sensor;

[0048] 3- Vehicle body. DETAILED DESCRIPTION

[0049] In order to more clearly illustrate the overall concept of the present application, a detailed description is given below in an illustrative manner in conjunction with the accompanying drawings.

[0050] In the following description, many specific details are set forth to facilitate a full understanding of the present invention. However, the present invention may also be implemented in other ways different from those described herein. Therefore, the protection scope of the present invention is not limited to the specific embodiments disclosed below.

[0051] In addition, in the description of the present invention, it should be understood that the orientations or positional relationships indicated by the terms "top", "bottom", "inside", "outside", "axial", "radial", "circumferential", etc. are based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present invention.

[0052] In the present invention, unless otherwise clearly specified and limited, the terms "installed", "connected", "connected", "fixed" and the like should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection, an electrical connection, or a communication; it can be a direct connection, or an indirect connection through an intermediate medium, it can be the internal connection of two elements or the interaction relationship between two elements. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0053] In the present invention, unless otherwise clearly specified and limited, the first feature "on" or "under" the second feature may be that the first and second features are in direct contact, or the first and second features are in indirect contact through an intermediate medium. In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in an appropriate manner in any one or more embodiments or examples.

[0054] There are many types of robotic arms 1, most of which are spraying robotic arms 1. The robotic arm 1 in this patent takes the spraying robotic arm 1 in tunnel support as an example.

[0055] The present invention provides a method for controlling the posture of a mechanical arm 1 based on a tilt sensor 2, such as Figure 1 As shown, the following steps are included:

[0056] S1, constructing a mathematical model for measuring the rotation angle of the robotic arm 1 according to the inclination sensor 2;

[0057] S2, obtaining an output value of the inclination sensor 2, wherein the inclination sensor 2 is installed at a joint of the robotic arm 1;

[0058] S3, calculating the rotation angle of the mechanical arm 1 by using the output value of the inclination sensor 2 and the mathematical model;

[0059] S4, calculating the yaw angle of the robotic arm 1 according to the rotation angle of the robotic arm 1;

[0060] S5. Optimize the posture control of the robotic arm 1 according to the yaw angle of the robotic arm 1.

[0061] As a preferred embodiment of the present invention, the tilt sensor is a dual-axis tilt sensor, and step S1 comprises:

[0062] Setting a horizontal plane as a reference plane for the measuring axis of the inclination sensor, wherein the measuring axes X and Y of the inclination sensor are perpendicular to each other;

[0063] According to the reference plane, a first coordinate system oxyz is established, wherein the y-axis is the projection of the rotation axis of the robot arm on the horizontal plane, the x-axis is perpendicular to the y-axis and is in the same horizontal plane as the y-axis, and the z-axis is perpendicular to the horizontal plane and intersects the x-axis and the y-axis at point o;

[0064] After the robot arm is rotated by a certain angle, a second coordinate system ouvw is established, where the ou axis coincides with the x axis, the ov axis coincides with the rotation axis of the robot arm, and the ow axis is the normal vector of the uov plane passing through point o;

[0065] A mathematical model of the measurement value of the inclination sensor and the rotation angle of the robot arm is established according to the first coordinate system oxyz and the second coordinate system ouvw.

[0066] As a preferred embodiment, Figure 2-Figure 3 As shown, in step S1, the inclination sensor 2 is a dual-axis inclination sensor 2, and the mathematical model for measuring the rotation angle of the robot arm 1 using the inclination sensor 2 is as follows:

[0067] Make the measuring axes X and Y of the dual-axis inclination sensor 2 perpendicular to each other, and its output signal is an angle with the horizontal plane H as a reference; let the rotation axis of the inclination sensor be L, counterclockwise rotation is positive, the rotation angle is θ, let the angle between the rotation axis L and the horizontal plane be β, and the projection of the rotation axis L on the horizontal plane be the y-axis; the x-axis is perpendicular to the y-axis and is in the same plane H as the y-axis, the z-axis is perpendicular to the horizontal plane and intersects the x-axis and y-axis at point o, forming a Cartesian coordinate system oxyz; let the measuring axis X of the inclination sensor 2 coincide with the x-axis of the Cartesian coordinate system, and the measuring axis Y coincide with the rotation axis L, and let the measuring axis X of the inclination sensor 2 rotate to X' after a certain angle; establish a new Cartesian coordinate system ouvw, let ou coincide with the x-axis, ov coincide with the rotation axis L, let the normal vector of the uov plane passing through point o be ow, and the normal vector of the XOY plane be M;

[0068] Take M on the yoz plane as the initial reference zero position;

[0069] The coordinate system ouvw is obtained by rotating the coordinate system oxyz clockwise around the ox axis by an angle β. Its rotation matrix is:

[0070]

[0071] Assume that the unit vector of the z-axis in the oxyz coordinate system is Z = [0 0 1], and its representation in the ouvw coordinate system is:

[0072] Z′=Z·R OX (β) = [0 - sinβ cosβ]

[0073] The unit vectors of the rotation axis L and the measurement axis X in the ouvw coordinate system are:

[0074] L = [0 1 0]

[0075] X=[1 0 0]

[0076] The inclination sensor 2 rotates around the ov axis by an angle θ in the ouvw coordinate system, and its rotation matrix is:

[0077]

[0078] Then, the vector measuring the axis X' is:

[0079] X'=R v (θ).X T =[cosθ 0 -sinθ]

[0080] The angle between the measuring axis Y and the horizontal plane is always ty = β;

[0081] Assume that the angle between the measuring axis X' and the OZ axis is remember<X',Z′> , the angle between X' and the horizontal plane is tx, then and The cosine of is:

[0082]

[0083] The measured value of the inclination sensor 2 is:

[0084] tx=arcsin(sinθcosβ) (1)

[0085] ty=β (2)

[0086] Thus, a mathematical model between the X-axis and Y-axis output values ​​of the inclination sensor 2 and the rotation angle θ is established.

[0087] It can be understood that the tilt sensor 2 can be dual-axis or single-axis. In this embodiment, a dual-axis tilt sensor 2 is taken as an example.

[0088] In one embodiment, Figure 3 As shown, in the step S2, in the predetermined installation method, the measuring axis of the inclination sensor 2 is parallel to the rotation axis of the robot 1 and is in the same direction, so that the output of the inclination sensor 2 remains unchanged during the rotation process and is always equal to the angle β between the rotation axis of the robot 1 and the horizontal plane. If the rotation axis L is perpendicular to the horizontal plane, the inclination sensor cannot measure the yaw angle of the robot arm, and the value range of β is β∈(0,90°).

[0089] like Figure 4 As shown, in step S3, the specific method for calculating the yaw angle of the robotic arm 1 according to the rotation angle of the robotic arm 1 is as follows:

[0090] Assume tx and ty are the output values ​​of the measuring axis X and Y of the inclination sensor 2, α is the actual yaw angle of the shotcrete robot 1, tx1 is the output value of the measuring axis X1 of the inclination sensor 2 when it is not rotated, α' is the yaw angle of the robot 1 relative to the horizontal plane at this time; Assume the rotation angle θ of the robot 1, the inclination X1 axis after rotation is recorded as the inclination X1', the new output value of the measuring axis X1 of the inclination sensor 2 is tx1', α" is the yaw angle of the robot 1 relative to the horizontal plane at this time; θ1' is the solution angle caused by the inclination of the robot carrier such as the car body 3 when it is not rotated, ty1 is the angle between the Z1 axis and the plane, θ1" is the angle obtained by the formula after rotation from the initial value, that is, the X axis coincides with the coordinate axis X;

[0091] From formula (1), we can get θ1' when not rotated

[0092]

[0093] After rotating by angle θ,

[0094]

[0095] Depend on Figure 4 The geometric relationship is deduced:

[0096]

[0097] Likewise:

[0098]

[0099] Substituting equations (3) and (4) into equations (5) and (6), we obtain:

[0100]

[0101]

[0102] Depend on Figure 4 The geometric relationship in the equation can be deduced as:

[0103] α=α”-α'

[0104] Therefore, each time the inclination sensor 2 measures a set of data, it can calculate the corresponding yaw angle. After the yaw angle of the robotic arm 1 is solved, the size of the yaw angle of the robotic arm 1 is controlled, and finally the purpose of optimizing the posture of the robotic arm 1 is achieved, thereby improving the accuracy of the posture control of the robotic arm 1.

[0105] Anything not described in the present invention can be achieved by adopting or drawing on existing technologies.

[0106] The various embodiments in this specification are described in a progressive manner, and the same or similar parts between the various embodiments can be referenced to each other, and each embodiment focuses on the differences from other embodiments.

[0107] The above description is only an embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent substitution, improvement, etc. made within the spirit and principle of the present invention should be included in the scope of the claims of the present invention.

Claims

1. A method for controlling the posture of a robotic arm based on an inclination sensor, characterized in that: The following steps are involved: S1, constructing a mathematical model for measuring the rotation angle of the robotic arm according to the inclination sensor, including: Setting a horizontal plane as a reference plane for measuring axes X and Y of the inclination sensor, wherein the measuring axes X and Y of the inclination sensor are perpendicular to each other; According to the reference plane, a first coordinate system oxyz is established, wherein the y-axis is the projection of the rotation axis of the robot arm on the horizontal plane, the x-axis is perpendicular to the y-axis and is in the same horizontal plane as the y-axis, and the z-axis is perpendicular to the horizontal plane and intersects the x-axis and the y-axis at point o; After the robot arm is rotated by a certain angle, a second coordinate system ouvw is established, where the ou axis coincides with the x axis, the ov axis coincides with the rotation axis of the robot arm, and the ow axis is the normal vector of the uov plane passing through point o; According to the first coordinate system oxyz and the second coordinate system ouvw, a mathematical model of the measured value of the inclination sensor and the rotation angle of the robotic arm is established, as follows: tx=arcsin(sinθcosβ) (1) ty=β (2) Wherein, tx is the output value of the X-axis of the tilt sensor measuring axis after the mechanical arm rotates by an angle θ, ty is the output value of the Y-axis of the tilt sensor measuring axis, θ is the rotation angle of the mechanical arm, and β is the angle between the rotation axis of the mechanical arm and the horizontal plane; S2, obtaining an output value of the inclination sensor, wherein the inclination sensor is installed at a joint of the robotic arm; wherein a measuring axis of the inclination sensor is parallel to a rotation axis of the robotic arm and is in the same direction; S3, calculating the rotation angle of the mechanical arm by using the output value of the inclination sensor and the mathematical model; S4, calculating the yaw angle of the robotic arm according to the rotation angle of the robotic arm, comprising: From formula (1), the initial rotation angle θ1′ when the robot arm is not rotating is expressed as follows: Wherein, θ1′ is the rotation angle calculated due to the tilt of the robot carrier when it is not rotated, tx1 is the output value of the measuring axis X axis of the inclination sensor when it is not rotated, and ty1 is the angle between the Y1 axis of the inclination sensor and the plane; After the tilt sensor rotates by an angle θ, the expression of the rotation angle θ1″ is as follows: Wherein, θ1″ is the rotation angle calculated by the rotation angle θ of the mechanical arm, and tx1′ is the output value of the measuring axis X-axis of the inclination sensor; The expression for calculating the yaw angle according to the rotation angle of the robotic arm is: Wherein, α' is the yaw angle of the robotic arm relative to the horizontal plane when the robotic arm is not rotated, and α" is the yaw angle of the robotic arm relative to the horizontal plane after the robotic arm rotates by an angle θ; Processing (3), (4), (5), and (6) yields: α＝α”-α' Wherein, α is the actual yaw angle of the robotic arm; S5. Optimizing the posture control of the robotic arm according to the yaw angle of the robotic arm.

2. A method for controlling the position and posture of a robotic arm based on an inclination sensor according to claim 1, characterized in that: The value range of β is β∈(0,90°).

Images