Method for measuring the attitude of a moving object based on double lines

Abstract

This invention discloses a method for measuring the attitude of a moving object based on a dual-line laser, belonging to the field of attitude measurement technology. A laser penetrates a reference laser receiving target and strikes a standard laser receiving target. One laser beam is fixedly illuminating a point on the reference laser receiving target. The line connecting the center points of the two targets is parallel to the laser beam and moves synchronously with the moving object. Coordinate systems are established at the center of the left side of both the reference and standard laser receiving targets. The coordinate values ​​of the laser illumination point in each target coordinate system are read, and the attitude angle of the moving object in space is calculated using spatial geometry. The attitude measurement method includes: measuring the yaw angle obtained by rotating the moving object around the Y-axis of the coordinate system, measuring the pitch angle obtained by rotating the moving object around the X-axis of the coordinate system, measuring the roll angle obtained by rotating the moving object around the Z-axis of the coordinate system, and calculating the coordinate value of one of the laser illumination points in the coordinate system of the standard laser receiving target (when no roll occurs).

Description

Technical Field

[0001] This invention discloses a method for measuring the attitude of a moving object based on a dual-line system, belonging to the field of attitude measurement technology. Background Technology

[0002] There are various methods for measuring attitude angles, typically using devices such as magnetic sensors, rotary potentiometers, angle sensors, machine vision, and inertial sensors. However, each method has its own advantages and limitations. Magnetic sensors are demanding in terms of environment, have a narrow temperature range, and poor reliability, and are usually only used as an auxiliary measurement tool. Rotary potentiometers are affected by the contact effect of the sliding contacts, mechanical resistance, and the conversion accuracy limitations of the AD conversion module, resulting in significant data fluctuations at high resolutions, making stable measurements difficult. Among angle sensors, incremental encoders are limited by the number of pulses per revolution, accumulate errors over long-term operation, and experience pulse loss upon power failure and restart, requiring manual reset. Absolute encoders, on the other hand, assign a specific digital code to each position, but are limited by their measurement range, have complex structures, and are expensive. Machine vision is easily affected by dust and other contaminants, camera resolution, and camera distance, and is subject to significant environmental constraints. Inertial sensors are affected by installation errors, initial alignment errors, and motion interference errors. Furthermore, with prolonged use, inertial sensors accumulate drift due to the integration of acceleration and angular velocity, affecting normal operation. Summary of the Invention

[0003] This invention proposes a method for measuring the attitude of moving objects based on dual lines, which solves the problem of inaccurate attitude measurement by instruments in the prior art.

[0004] A bilinear motion object attitude measurement method includes:

[0005] S1. Measure the heading angle obtained by rotating the moving object around the Y-axis of the coordinate system;

[0006] S2. Measure the pitch angle obtained by rotating the moving object around the X-axis of the coordinate system;

[0007] S3. Measure the roll angle obtained by rotating the moving object around the Z-axis of the coordinate system;

[0008] S4. Calculate the coordinates of one of the laser irradiation points in the reference laser receiving target coordinate system (without roll).

[0009] Preferably, a reference laser receiving target, a standard laser receiving target, and two lasers are used. The two lasers are at the same height and emit parallel lasers. The lasers penetrate the reference laser receiving target and strike the standard laser receiving target. One of the lasers is fixedly irradiating a point on the reference laser receiving target. Both targets are placed vertically, and the line connecting their center points is parallel to the laser and moves synchronously with the moving object.

[0010] Preferably, the reference laser receiving target and the reference laser receiving target establish a coordinate system at the center of their respective left sides, read the coordinate values ​​of the laser irradiation point in each target coordinate system, and calculate the attitude angle of the moving object in space through spatial geometric relationships.

[0011] Preferably, let A and C be the illumination points of the first laser line and the second laser line on the reference laser receiving target, respectively, and let B and D be the illumination points of the first laser line and the second laser line on the reference laser receiving target, respectively. Then, the formula for calculating the roll angle Roll is:

[0012] In the formula, O1 is the origin of the coordinate system on the left side of the reference laser receiving target. The line connecting O1 and A is represented by X and Y, which are the x and y coordinates of C in the coordinate system {O1}, respectively.

[0013] Preferably, the pitch angle is positive when rotating upward along the horizontal plane, and the value range is (-90°-90°). Let the coordinates of B in the coordinate system {reference O2} be (x, y). The BHM plane is perpendicular to the two targets and intersects the coordinate system {O1} and the coordinate system {reference O2} at H and M. Right now In the formula, l HM The distance between the two targets is denoted by , and pitch is the elevation angle. Coordinate system {reference O2}: The {O2} coordinate system when no roll occurs; Coordinate system {O2}: The coordinate system established at the center of the left side of the reference laser receiving target.

[0014] Preferably, the heading angle yaw is positive when rotated counterclockwise around the Y-axis at point A, and its value ranges from -90° to 90°.

[0015] Preferably, the process of solving the coordinates of B is as follows: based on the position of point B on the reference laser receiving target, the reference laser receiving target is divided into 8 regions, the equation of the dividing line between adjacent regions is solved, and the coordinates of B in the coordinate system {reference O2} are solved according to the 8 ranges of the roll angle.

[0016] Compared with existing technologies, the beneficial effects of this invention are: the invention has a simple structure, does not rely on external data, has a simple attitude angle calculation method, can work for a long time without cumulative errors, and can achieve stable measurement; the infrared laser irradiation interval is long, the product life is long, and infrared light has strong penetrating ability, is less affected by dust and other particulate matter, and has a high operating temperature range; the laser receiving target has a relatively small interval, is directly attached to the object, is lightweight, and does not require zeroing after power failure and power restoration. Attached Figure Description

[0017] Figure 1 The measuring device of the present invention;

[0018] Figure 2 Based on Figure 1 A schematic diagram of the trigonometric solution;

[0019] Figure 3 View in the Q direction;

[0020] Figure 4 This is a view in the N direction;

[0021] Figure 5 A region map showing the roll angle: Rall ∈ (-180° to 90°);

[0022] Figure 6 A region map for roll angles: Rall ∈ (-90°-0°);

[0023] Figure 7 A region map for roll angles: Rall ∈ (0°-90°);

[0024] Figure 8 A region map showing the roll angle: Rall ∈ (90°-180°);

[0025] The reference numerals in the attached figures include: 1-first laser, 2-second laser, 3-first laser line, 4-second laser line, 5-reference laser receiving target, and 6-reference laser receiving target. Detailed Implementation

[0026] The specific embodiments of the present invention will be further described below with reference to specific examples:

[0027] A bilinear motion object attitude measurement method includes:

[0028] S1. Measure the heading angle obtained by rotating the moving object around the Y-axis of the coordinate system;

[0029] S2. Measure the pitch angle obtained by rotating the moving object around the X-axis of the coordinate system;

[0030] S3. Measure the roll angle obtained by rotating the moving object around the Z-axis of the coordinate system;

[0031] S4. Calculate the coordinates of one of the laser irradiation points in the reference laser receiving target coordinate system (without roll).

[0032] The system employs a reference laser receiving target 5, a reference laser receiving target 6, and two lasers. The two lasers are at the same height and emit parallel lasers. The lasers penetrate the reference laser receiving target 5 and strike the reference laser receiving target 6. One of the lasers is fixedly irradiating a point on the reference laser receiving target 5. Both targets are placed vertically, and the line connecting their center points is parallel to the laser and moves synchronously with the moving object.

[0033] The reference laser receiving target 5 and the reference laser receiving target 6 establish coordinate systems at the center of their respective left sides, read the coordinate values ​​of the laser irradiation point in each target coordinate system, and calculate the attitude angle of the moving object in space through spatial geometric relationships.

[0034] Let A and C be the illumination points of the first laser line 3 and the second laser line 4 on the reference laser receiving target 5, respectively, and let B and D be the illumination points of the first laser line 3 and the second laser line 4 on the reference laser receiving target 6, respectively. In the roll angle calculation, the reference laser receiving target 5 is taken as the research object. Under the condition that the reference laser receiving target 5 has deflection and pitch, such as... Figure 2 Looking directly at the target (viewed from L in the figure), we analyze the roll angles at different angles. Figure 2 The coordinate system is as follows: Coordinate system {O0}: Rotation coordinate system: that is, the moving object rotates around its three axes in sequence; Coordinate system {O1}: Coordinate system established at the center of the left side of the reference laser receiving target 5; Rotation angle is the rotation angle of the reference laser receiving target 5 under the L-view: Roll; Rotation angle Roll is ∠O1AC; Point E is the perpendicular line drawn from point C to the x1 axis of coordinate system {O1}, with the foot of the perpendicular at point E; The BHM plane is perpendicular to the two targets and intersects coordinate system {O1} and coordinate system {reference O2} at H and M. According to the views in different directions, the accompanying drawings in the instruction manual include arrows in four directions: L, N, P, and Q, to indicate the corresponding visual effect viewed from that direction. The view in the Q direction is shown below. Figure 3 The view in the N direction is as follows Figure 4 ;

[0035] The formula for calculating the roll angle (Roll) is:

[0036] In the formula, O1 is the origin of the coordinate system on the left side of the reference laser receiving target 5. The line connecting O1 and A is represented by X and Y, which are the x and y coordinates of C in the coordinate system {O1}, respectively.

[0037] The coordinates of point C are: C: (X, 0), with the sign: (-, 0). At this time, the roll angle is a straight angle: ∠O1AC=±180°.

[0038] The coordinates of point C are C(X,Y), with the sign (-,-). At this point, the roll angle is obtuse and negative.

[0039]

[0040] Right now:

[0041] The coordinates of point C are C(X,Y), and the symbol is: At this point, the roll angle is a right angle: ∠O1AC=-90°;

[0042] The coordinates of point C are C(X,Y), with the sign (-,-). At this point, the roll angle is acute and negative.

[0043]

[0044] Right now:

[0045] Point C has coordinates C: (0, Y), sign: (0, -), and the roll angle is acute and negative.

[0046]

[0047] Right now:

[0048] The coordinates of point C are C(X,Y), with signs (+,-). At this point, the roll angle is acute and negative.

[0049]

[0050] Right now:

[0051] The coordinates of point C are C(X,0), with the sign (+,0). The roll angle at this point is Roll = 0°.

[0052] The coordinates of point C are C(X,Y), with the sign (+,+). At this point, the roll angle is acute and positive.

[0053]

[0054] Right now:

[0055] The coordinates of point C are C(0,Y), with the sign (0,+). At this point, the roll angle is acute and positive.

[0056]

[0057] Right now:

[0058] The coordinates of point C are C(X,Y), with signs (-,+). At this point, the roll angle is acute and positive.

[0059]

[0060] Right now:

[0061] The coordinates of point C are C(X,Y), and the symbol is: At this point, the roll angle is a right angle: Roll = 90°

[0062] The coordinates of point C are C(X,Y), with signs (-,+), and the roll angle is obtuse.

[0063]

[0064] Right now:

[0065] The pitch angle is positive when rotating upwards along the horizontal plane, and its value ranges from -90° to 90°. Let the coordinates of B in the coordinate system {reference O2} be (x, y). The BHM plane is perpendicular to the two targets and intersects the coordinate systems {O1} and {reference O2} at H and M. Right now In the formula, l HM The distance between the two targets is denoted by 'pitch', and 'pitch' is the elevation angle. Coordinate system {reference O2}: The {O2} coordinate system when no roll occurs; Coordinate system {O2}: The coordinate system established at the center of the left side of the reference laser receiving target 5.

[0066] The heading angle yaw is positive when rotated counterclockwise around the Y-axis at point A, and its value ranges from -90° to 90°.

[0067] The process of solving for the coordinates of point B is as follows: Based on the position of point B on the reference laser receiving target, the reference laser receiving target 6 is divided into 8 regions. The equations of the dividing lines between adjacent regions are solved. Based on the 8 ranges of the roll angle, the coordinates of B in the coordinate system {reference O2} are solved respectively. The 8 regions are as follows: Figures 5 to 8 As shown.

[0068] In solving for the coordinates of point B, the reference laser receiving target 6 is taken as the research object. The following figures are obtained under the condition that the reference laser receiving target 6 has deflection and elevation. Figure 2, facing the target (in the P viewing direction in the figure), the analysis and drawing are carried out under the roll angles of different angles. The specific coordinate system in the figure is: coordinate system {reference O2}: the {O2} coordinate system when there is no roll; coordinate system {O2}: the coordinate system established at the center position of the left side of the reference laser receiving target 5; the regional segmentation equation is the equation in the coordinate system {O2}; define The following calculations are all adopted, that is, B0(-25, 0); point B0 is the irradiation point of the first laser line 3 on the reference laser receiving target 6 (when the device does not rotate at all); the angle identifiers of each region are obtained by summarization; point F is the perpendicular from point B to the x-axis of the coordinate system {O2}, and the foot of the perpendicular is point F.

[0069] B1. Roll angle: When Rall = ±180°, the coordinates of point B are: B(-(50 + X), -Y), where 50 is the distance from O2 to the reference O2.

[0070] Such as Figure 5 , B2. Roll angle: When Rall ∈ (-180°--90°),

[0071] The dividing line equation between region 1 and region 2 is: y = tan(Roll)x + 25tan(Roll);

[0072] The dividing line equation between region 2 and region 3 is: x = -25;

[0073] The dividing line equation between region 3 and region 4 is:

[0074] The dividing line equation between region 4 and region 5 is: y = 0;

[0075] When X = -25 and Y = 0, x = -25 and y = 0;

[0076] When X > -25 and That is, in region 1:

[0077]

[0078] When X < -25 and That is, in region 5:

[0079]

[0080] When X > -25 and Y > tan(Roll)x + 25tan(Roll), that is, in region 2:

[0081]

[0082] When X < -25 and Y < tan(Roll)x + 25tan(Roll), that is, in region 6:

[0083]

[0084] When X < -25 and That is, in region 3:

[0085]

[0086] When X > -25 and That is, in region 7:

[0087]

[0088] When X < -25 and That is, in region 4:

[0089]

[0090] When X > -25 and That is, in region 8:

[0091]

[0092] The formulas can be applied to both sides of the dividing line under boundary conditions.

[0093] B3. Roll angle: When Rall = -90°: Point B is: B(-(25+Y), 25+X);

[0094] like Figure 6 B4. Roll angle: When Rall ∈ (-90°-0°):

[0095] The equation of the dividing line between region 1 and region 2 is: y = -tan(Roll)x - 25tan(Roll);

[0096] The equation of the dividing line between region 2 and region 3 is: x = -25;

[0097] The equation of the dividing line between region 3 and region 4 is:

[0098] The equation of the dividing line between region 4 and region 5 is: y = 0;

[0099] When X = -25 and Y = 0, x = -25 and y = 0;

[0100] When X > -25 and That is, in region 1:

[0101]

[0102] When X < -25 and That is, in region 5:

[0103]

[0104] When X > -25 and Y > -tan(Roll)X - 25tan(Roll), that is, in region 2:

[0105]

[0106] When X < -25 and Y < -tan(Roll)X - 25tan(Roll), that is, in region 6:

[0107]

[0108] When X < -25 and That is, in region 3:

[0109]

[0110] When X > -25 and That is, in region 7:

[0111]

[0112] When X < -25 and That is, in region 4:

[0113]

[0114] When X > -25 and That is, in region 8

[0115]

[0116] B5. Roll angle: When Rall = 0°: the coordinates of point B are: B(X, Y);

[0117] like Figure 7 B6. Roll angle: When Rall ∈ (0°-90°):

[0118] The equation of the dividing line between region 1 and region 2 is: y = tan(Roll)x + 25tan(Roll);

[0119] The equation of the dividing line between region 2 and region 3 is: x = -25;

[0120] The equation of the dividing line between region 3 and region 4 is:

[0121] The equation of the dividing line between region 4 and region 5 is: y = 0;

[0122] When X = -25 and Y = 0, x = -25 and y = 0;

[0123] When X > -25 and that is, in region 1:

[0124]

[0125] When X < -25 and that is, in region 5:

[0126]

[0127] When X > -25 and Y > tan(Roll)x + 25tan(Roll), that is, in region 2:

[0128] When X < -25 and Y < tan(Roll)x + 25tan(Roll), that is, in region 6:

[0129]

[0130] When X < -25 and that is, in region 3:

[0131]

[0132] When X > -25 and that is, in region 7:

[0133]

[0134] When X < -25 and that is, in region 4:

[0135]

[0136] When X > -25 and that is, in region 8:

[0137]

[0138] B7. Roll angle: When Rall = 90°, the coordinates of point B are: B(-25 + Y, -(25 + X));

[0139] Such as Figure 8 , B8. Roll angle: When Rall ∈ (90° - 180°):

[0140] The dividing line equation between region 1 and region 2 is: y = -tan(Roll)x - 25tan(Roll);

[0141] The dividing line equation between region 2 and region 3 is: x = -25;

[0142] The dividing line equation between Region 3 and Region 4 is:

[0143] The dividing line equation between Region 4 and Region 5 is: y = 0;

[0144] When X = -25 and Y = 0, x = -25, y = 0;

[0145] When X > -25 and That is, in Region 1:

[0146]

[0147] When X < -25 and That is, in Region 5:

[0148]

[0149] When X > -25 and Y > -tan(Roll)x - 25tan(Roll), that is, in Region 2:

[0150]

[0151] When X < -25 and Y < tan(Roll)x + 25tan(Roll), that is, in Region 6:

[0152]

[0153] When X < -25 and That is, in Region 3:

[0154] When X > -25 and That is, in Region 7:

[0155] When X < -25 and That is, in Region 4:

[0156]

Claims

1. A method for measuring the attitude of a moving object based on a dual-line system, characterized in that, The system includes a reference laser receiving target, a standard laser receiving target, and two lasers. The two lasers are at the same height and emit parallel lasers. The lasers penetrate the reference laser receiving target and strike the standard laser receiving target. One of the lasers is fixedly irradiating a point on the reference laser receiving target. Both targets are placed vertically, and the line connecting their center points is parallel to the laser and moves synchronously with the moving object. The reference laser receiving target and the reference laser receiving target establish a coordinate system at the center of their respective left sides. The coordinate values ​​of the laser irradiation point in each target coordinate system are read, and the attitude angle of the moving object in space is calculated through spatial geometric relationships. The coordinate system on the left side of the reference laser receiving target is a left-handed system. With the geometric center point O1 on the left side of the reference laser receiving target as the origin, a plane rectangular coordinate system is established in the left side of the reference laser receiving target. The horizontal direction to the right in the plane is the positive X-axis, the vertical direction upward in the plane is the positive Y-axis, and the laser emission direction is the positive Z-axis. The coordinate system on the left side of the reference laser receiving target is a left-handed system. With the geometric center point O2 on the left side of the reference laser receiving target as the origin, a plane rectangular coordinate system is established in the left side of the reference laser receiving target. The horizontal direction to the right in the plane is the positive X-axis, the vertical direction upward in the plane is the positive Y-axis, and the laser emission direction is the positive Z-axis. Let A and C be the illumination points of the first and second laser lines on the reference laser receiving target, respectively, and let B and D be the illumination points of the first and second laser lines on the reference laser receiving target, respectively. Then the formula for calculating the roll angle is: In the formula, The origin of the coordinate system is the left side of the reference laser receiving target. express The distance between the line connecting A and C, and X and Y are the horizontal and vertical coordinates of C in the coordinate system on the left side of the reference laser receiving target, respectively. The pitch angle is positive when rotating upwards along the horizontal plane. Let the coordinates of B in the coordinate system of the left side of the reference laser receiving target when no roll occurs be (x, y). The BHM plane is perpendicular to both targets and intersects the coordinate system of the left side of the reference laser receiving target and the coordinate system of the left side of the reference laser receiving target when no roll occurs at H and M. ,Right now In the formula, The distance between the two targets. The pitch angle; Heading angle Rotating counterclockwise around the Y-axis around point A results in positive rotation. ; The process of solving for the coordinates of B is as follows: Based on the position of point B on the reference laser receiving target, the reference laser receiving target is divided into 8 regions. The equations of the dividing lines between adjacent regions are solved. Based on the 8 ranges of the roll angle, the coordinates of B in the coordinate system of the left side of the reference laser receiving target when no roll occurs are solved respectively.

Images