Chain structure attitude measurement system and measurement method

Abstract

Attitude measurement is one of the crucial technologies in sea, road, air and sky navigation systems, and provides basic support for safety detection of the motion state of an active carrier. The invention provides a chained structure attitude measurement system and a measurement method. Each chained subsystem to be measured comprises two satellite antennas and two laser range finders. Attitude measurement is carried out on the chain type structure through five steps of establishing a measurement coordinate system, acquiring satellite antenna data, calculating a pitch angle and a yaw angle, acquiring data of a laser range finder and calculating a roll angle. Compared with the prior art, the hardware cost is greatly reduced, and the measurement precision is improved.

Description

Technical Field

[0001] The present invention relates to the field of carrier attitude measurement, and in particular to a low-cost and high-precision chain structure attitude measurement system and a measurement method. Background Art

[0002] Attitude measurement is one of the most important technologies in sea, land, air and space navigation systems. It provides basic support for the safe detection of the motion state of active carriers. The acquisition of attitude information can not only accurately reflect the current state of the carrier, but also help predict its future motion trajectory. It is of great significance in tasks such as navigation of aircraft and ships and automatic driving of land vehicles. With the development of informatization of mechanical equipment and the continuous expansion of application scenarios, attitude measurement has become a research hotspot in the field of navigation and intelligent control.

[0003] A variety of attitude measurement methods can be used for different application environments. For low-dynamic and static platforms, optical and infrared instruments are often used. Although these sensors are highly accurate, they are easily affected by external factors such as weather and terrain. Dynamic carriers often use inertial navigation systems to obtain attitude information. Although these systems have high-frequency dynamic measurement capabilities, inertial devices will drift over time, thereby reducing measurement accuracy. Global satellite navigation system technology (GNSS) provides lateral and attitude measurement functions based on satellite signals. In the document "Method and Implementation of EMU Car Body Attitude Detection Based on Beidou Satellite", two sets of satellite antennas are used to achieve two-dimensional attitude measurement. For three-dimensional attitude measurement, at least three sets of satellite antennas are required, and the measurement accuracy is limited by the installation distance between the antennas. Summary of the invention

[0004] In order to solve the deficiencies in the prior art, the present invention proposes a set chain structure posture measurement system and a measurement method to reduce the measurement cost and improve the measurement accuracy.

[0005] See also Figure 1 A low-cost and high-precision chain structure posture measurement system includes: a chain structure system 1 to be measured and a measurement system 2. Among them, the number of chain subsystems is n, and the number of measurement subsystems is n-1. The measurement subsystem 21 on a section of the chain subsystem 11 to be measured includes: two satellite antennas (satellite antenna 211 and satellite antenna 212), and two laser rangefinders (laser rangefinder 213 and laser rangefinder 214). The positional relationship between them is: the satellite antennas 211 and 212 are respectively installed at the two ends of the midline position at the top of the chain subsystem 11, and the laser rangefinders 213 and 214 are inclined at a certain angle and installed between the two chain subsystems 11 and 12.

[0006] The chain structure system 11 can be a train or a car. Preferably, it is a train, and the chain subsystem 11 is a train section 111. When the two chain subsystems are parallel, the laser ranging length will not change with the pitch rotation of the chain subsystem, resulting in the inability to calculate the pitch angle. Therefore, it is necessary to process a measurement plane 112, which is rotated around the z axis of the B coordinate system by θ bb1 The result is installed at the front end 121 of the next chain subsystem illuminated by the laser rangefinder 213.

[0007] The posture measurement method applied to the high-precision and low-cost chain structure posture measurement system takes two adjacent chain structures as an example. The specific steps of the method are as follows:

[0008] Step 1: Establish a measurement coordinate system.

[0009] Step 2: Acquire data from satellite antennas 211 and 212.

[0010] Step 3: Calculate the pitch angle θ y and yaw angle θ z .

[0011] Step 4: Acquire data from the laser rangefinders 213 and 214 .

[0012] Step 5: Calculate the roll angle θ x .

[0013] Step 1: The implementation method of establishing the measurement coordinate system is:

[0014] The forward direction of the chain structure is the x-axis direction, the vertical direction is the z-axis direction, and the y-axis direction is determined according to the right-hand rule, where the angle of rotation around the x-axis is the roll angle θ x , the rotation angle around the y-axis is the pitch angle θ y , the rotation angle around the z-axis is the yaw angle θ z , the origin of the coordinate is the center of two adjacent chain structures, and the coordinate system O is established A and O B , O B The coordinate system is rotated about its z-axis by θ bb1 Angle, defined as O B1 Coordinate system, parallel to the measurement plane. Figure 2 shown.

[0015] Step 2: The method for obtaining the data of satellite antenna 211 and satellite antenna 212 is as follows:

[0016] Get the antenna position information, that is, the satellite antenna 211 (x′ 1 , y′ 1 , z′ 1 ), satellite antenna 212 (x′ 2 , y′ 2 , z′2 ). Using satellite antenna 211 as the reference antenna, the new antenna position is obtained, satellite antenna 211 (0, 0, 0), and the coordinate position of satellite antenna 212 (x 2 ,y 2 , z 2 ).

[0017] Step 3 Calculate the pitch angle θ y and yaw angle θ z The implementation method is:

[0018] Obtain satellite antenna data according to the above steps and calculate the angle.

[0019]

[0020] Step 4: The method for obtaining the data of the laser rangefinder 213 and the laser rangefinder 214 is as follows:

[0021] See also Figure 2 , i represents the serial number of the laser rangefinder (i=1, 2), F i is the installation position of the i-th laser rangefinder, is the installation direction of the i-th laser in the A coordinate system, that is L i is the i-th laser ranging length, Z i is the position of the ith irradiation point. Get the laser rangefinder information, including the laser rangefinder installation position, installation direction and laser ranging length.

[0022] Step 5: Calculate the roll angle θ x The implementation method is:

[0023] According to the rotation and translation coordinate transformation relationship in space, find θ x About L 1 , L 2 The coordinate change principle of a point in space rotating or translating around an arbitrary axis is given by the irradiation position Z. 1 Point, Z 2 The coordinate position of the point in the A coordinate system can be established by θ x About L 1 , L 2 The equation is:

[0024]

[0025] in, is the rotation matrix from the A coordinate system to the B coordinate system; Y is the rotation matrix from the B coordinate system to the B1 coordinate system. 21 (X b2 ,Y b2 ,Z b2) is the irradiated point Z 1 In B 1 Position in the coordinate system, Y 22 (X b2 ,Y b21 ,Z b21 ) is the irradiated point Z 2 In B 1 The positions in the coordinate system, both have the same x coordinate. is the coordinate of laser rangefinder 1 in the A coordinate system, that is is the coordinate of laser rangefinder 2 in the A coordinate system, that is is the coordinate of the origin of the B coordinate system in the A coordinate system, that is, According to the established coordinate system, X b2 Indicates that the distance between the two chain subsystems is a known quantity, Y b2 , Z b2 , Y b21 , Z b21 is the unknown quantity, the pitch angle θ y and yaw angle θ 2 It can be obtained from step 4.

[0026] Rewrite formula (3) and formula (4) as

[0027]

[0028] To simplify the expression, intermediate variables are defined to represent the left side of formula (5) and formula (6):

[0029]

[0030] Expanding formula (7) and formula (8) yields:

[0031]

[0032] Expanding formula (3) and formula (4) yields two sets of six equations in the x, y, and z directions, which can be combined to obtain θ x The expression is as follows:

[0033]

[0034]

[0035] The beneficial effects of the present invention are

[0036] 1. For three-dimensional attitude measurement, the hardware cost is greatly reduced compared with the same precision inertial navigation and three-satellite antenna attitude measurement solutions.

[0037] 2. Compared with the three-satellite antenna attitude measurement solution, the measurement accuracy of the pitch angle is improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 This is a schematic diagram of the attitude measurement system layout.

[0039] Figure 2 Schematic diagram of the coordinate system of the attitude measurement system.

[0040] Figure 3 This is the flow chart of the posture measurement method.

[0041] Figure 4 This is a schematic diagram showing the comparison between the input value and the theoretical calculated value.

[0042] Figure 5 Schematic diagram of the error results between the input value and the theoretical calculated value. DETAILED DESCRIPTION

[0043] The following will be combined with the attached Figure 1-5 The technical solutions in the embodiments of the present invention are described clearly and completely. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0044] See also Figure 3 The specific process of the high-precision and low-cost chain structure posture measurement method of the present invention is as follows:

[0045] Step 1: Establish a measurement coordinate system.

[0046] Taking the train as an example, the length of the train is 12m, the distance between the two trains is 0.5m, and the coordinate system O is established at the center of the two adjacent chain structures. A and O B Let θ bb1 =5°, B The coordinate system z axis rotates a certain angle to establish the coordinate system O B1 ,like Figure 2 As shown, the coordinates of the origin of the train 121 coordinate system in the train 111 coordinate system are =[12500,100,0]. Then X b2 =-6000*cosd(5).

[0047] Step 2: Acquire data from satellite antennas 211 and 212.

[0048] Step 3: Calculate the pitch angle θ y and yaw angle θ z ;

[0049] In step 2 and step 3, after obtaining the satellite antenna data, the LAMBDA algorithm is preferably used to fix the carrier phase integer ambiguity, and then the baseline vector is solved to calculate the pitch angle θ y and yaw angle θ z On this basis, the rolling angle θ x Verification of solution.

[0050] Step 4: Acquire data from the laser rangefinders 213 and 214 .

[0051] Select Laser Distance Meter 1 in O A Coordinates in the coordinate system Laser installation direction Laser Rangefinder 2 in O A Coordinates in the coordinate system Laser installation direction

[0052] Step 5: Calculate the roll angle θ x .

[0053] In addition, the displacement and angle changes during the train's movement must also be considered. According to the survey, the maximum relative displacement between train carriages is as follows:

[0054] Height direction: displacement ±75mm; angle: ±5°.

[0055] Travel direction: displacement ±32mm; angle: ±1°.

[0056] Left and right direction: displacement ±167mm; angle: ±2°.

[0057] Turning angle:3.12°.

[0058] Climbing angle:1.12°.

[0059] Therefore, the actual working condition of the train corresponds to the variable value in the model: y The angle range is [-5,6.12], θ z The angle range is [-5.12, 5.12], θ x The angle range is [-1,1], in degrees. The displacement of the center of mass of the train car (Δx, Δy, Δz) only affects Take the value, that is The value of Δx is in the range of [-32, 32], Δy is in the range of [-167, 167], and Δz is in the range of [-75, 75], and the unit is mm.

[0060] When the Euler angle θ of the train carriage motion is known x ,θy ,θ z and the train car center of mass displacement (Δx, Δy, Δz), the laser ranging length L can be calculated 1 and the y coordinate of the irradiation point Z in the B coordinate system (Y Bz ) and the z coordinate (Z Bz ), and define this process as seeking the correct solution of the model, namely:

[0061]

[0062] When the Euler angle θ of the train carriage motion is known y ,θ z , train car center of mass displacement and laser ranging length L 1 , find the rolling angle θ of the train carriage x And the y coordinate of the irradiation point Z in the B coordinate system (Y Bz ) and the z coordinate (Z Bz ) is used to obtain the inverse solution of the model, namely:

[0063]

[0064] By substituting the calculated positive solution into the inverse solution expression, the obtained θ x By comparing with the input value, we can verify θ x Correctness of analytical solution.

[0065] Δx, Δy, Δz are traversed at intervals of 30mm, 100mm, and 50mm respectively within their range of motion, θ y ,θ z The range is traversed every 1°, θ x The range is traversed every 0.5°. y and yaw angle θ z , and the laser rangefinder measures the length and calculates θ x . x The input value is compared with the theoretical calculated value, and the result is as follows Figure 4 and Figure 5 As shown, it is verified that θ x Correctness of analytical solution.

[0066] The present invention is not limited to the above specific implementations, and the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made to the above implementations based on the technical essence of the present invention shall be included in the protection scope of the present invention.

Claims

1. A chain structure posture measurement system, characterized in that: It includes a chain structure system to be measured and a measuring system, wherein the number of chain subsystems to be measured of the chain structure system to be measured is n, and the number of measuring subsystems of the measuring system is n-1; the measuring subsystem on one section of the chain subsystem to be measured includes: two satellite antennas and two laser rangefinders, and the position relationship is: the two satellite antennas are respectively installed at the two ends of the top midline position of the chain subsystem to be measured, and the two laser rangefinders are obliquely installed between the two chain subsystems to be measured.

2. A chain structure posture measurement system according to claim 1, characterized in that: The utility model also comprises a measuring plane which is installed at the front end of the next chain subsystem to be measured and illuminated by the laser rangefinder.

3. A chain structure posture measurement method, applied to the chain structure posture measurement system according to claim 1 or 2, characterized in that: The specific steps include: Step 1: Establish the measurement coordinate system; Step 2: Get data from two satellite antennas; Step 3: Calculate the pitch angle and yaw angle of the chain structure; Step 4: Obtain data from two laser rangefinders; Step 5: Calculate the rolling angle of the chain structure based on the pitch angle, yaw angle and laser ranging data.

4. A chain structure posture measurement method according to claim 3, characterized in that: The step 1 specifically includes: taking the chain structure forward direction as the x-axis direction, the vertical direction as the z-axis direction, and determining the y-axis direction according to the right-hand rule, wherein the angle of rotation around the x-axis is the roll angle θ x , the rotation angle around the y-axis is the pitch angle θ y , the rotation angle around the z-axis is the yaw angle θ z , taking the centers of two adjacent chain structures as the coordinate origin, establish coordinate systems O A and O B , O B The coordinate system is rotated about its z-axis by θ bb1 Angle, defined as O B1 Coordinate system, parallel to the measurement plane.

5. A chain structure posture measurement method according to claim 4, characterized in that: The step 2 specifically includes: obtaining the position information of two satellite antennas respectively, which are defined as (x′1, y′1, z′1) and (x′2, y′2, z′2) respectively; and then taking one of the satellite antennas as the reference antenna to obtain the new antenna position information under the reference antenna, which are defined as (0, 0, 0) and (x2, y2, z2) respectively.

6. A chain structure posture measurement method according to claim 5, characterized in that: The step three specifically includes: calculating the elevation angle θ according to the satellite antenna data obtained in step two y and yaw angle θ z :

7. A chain structure posture measurement method according to claim 6, characterized in that: The step 4 specifically includes: defining i to represent the serial number of the laser rangefinder, with a value of 1 or 2, F i is the installation position of the i-th laser rangefinder, O A The installation direction of the i-th laser rangefinder in the coordinate system is L i is the i-th laser ranging length, Z i is the irradiation point position of the i-th photometric rangefinder; obtain the laser rangefinder information, including the laser rangefinder installation position, installation direction, laser ranging length and irradiation point position.

8. A chain structure posture measurement method according to claim 7, characterized in that: The step five specifically includes: according to the irradiation positions Z1 and Z2, A The coordinate position in the coordinate system establishes the roll angle θ x The equations for L1 and L2 are: in, O A Coordinate system to O B The rotation matrix of the coordinate system; O B Coordinate system to O B1 The rotation matrix of the coordinate system; The irradiated point Z1 is at O B1 The position in the coordinate system, The irradiated point Z2 is at O B1 The positions in the coordinate system, both have the same x-axis coordinate; The first laser rangefinder F1 in O A The coordinates in the coordinate system, that is is the coordinate of the second laser rangefinder F2 in the A coordinate system, that is, O B The origin of the coordinate system is at O A The coordinates in the coordinate system, that is According to the established coordinate system, X b2 Represents the distance between two chain subsystems to be tested; Rewrite formula (3) and formula (4) as follows: To simplify the expression, intermediate variables are defined to represent the left side of formula (5) and formula (6): Expanding formula (7) and formula (8) yields: Expanding formula (3) and formula (4) yields two sets of six equations in the x, y, and z directions, which can be combined to obtain θ x The expression is as follows:

Images