A fast positioning method for mobile targets in a portable mobile range based on UWB and inertial navigation fusion

Abstract

The present invention discloses a method for quickly positioning a mobile target in a portable mobile shooting range based on UWB and inertial navigation fusion, comprising: step S1: constructing a mobile shooting range; placing four positioning rods around to construct a convex quadrilateral shooting range; step S2: quickly positioning a mobile target robot; placing the target on the top of the robot, controlling the target to move autonomously in the shooting range through the autonomous movement of the robot, and adopting UWB and inertial navigation fusion positioning; step S3: building an electronic fence. The present invention has the advantages of simple principle and high positioning accuracy.

Description

Technical Field

[0001] The invention mainly relates to the technical field of intelligent shooting simulation, and in particular to a method for quickly positioning a mobile target in a portable mobile shooting range based on UWB and inertial navigation fusion. Background Art

[0002] In a simulated shooting training environment, shooters must adapt to different shooting environments. Therefore, the traditional fixed or manually calibrated shooting range environment is not suitable for frequently moving the shooting range. When moving the training shooting range, a lot of manpower and material resources are wasted to build the shooting range and recalibrate the positioning rod position. At the same time, traditional targets are placed on mobile racks and moved manually or placed on vehicles and moved remotely, which lacks continuity and autonomy.

[0003] Traditional outdoor positioning often uses GPS or Beidou systems, which requires corresponding time calibration for all navigation modules in the system, which is not convenient in actual shooting or other exercise environments.

[0004] There are technical solutions such as "shooting range theodolite arrangement and target positioning calculation" and "mobile target intelligent control system" in the prior art. In the first shooting range arrangement and target positioning solution, the range measurement module in the shooting range requires three theodolites, and a laser is required to measure the distance between any two installation points. Figure 1 The ABC triangle shown is a distance measurement module composed of three theodolites. First, the theodolites need to be installed and calibrated for distance measurement, and then the position of M is calculated by the azimuth between the target M and the triangle side lengths AB, BC, and AC measured by the theodolite. In the second mobile target control system solution, the target vehicle is controlled by several remote controllers, and the target vehicle and the remote controller realize two-way data interaction with the remote controller through the AP wireless LAN. The mobile target vehicle transmits the positioning information to the data center for processing through the Beidou satellite, and the data processing center then sends the information to the target vehicle through the 4G module.

[0005] In summary, the prior art has the defects that the fixed shooting range is not convenient for carrying and changing the shooting range position, and the target cannot move continuously and autonomously. Summary of the invention

[0006] The technical problem to be solved by the present invention is: in view of the technical problems existing in the prior art, the present invention provides a method for quickly positioning a mobile target in a portable mobile target range based on UWB and inertial navigation fusion, which has a simple principle, is easy to operate and has high positioning accuracy.

[0007] In order to solve the above technical problems, the present invention adopts the following technical solutions:

[0008] A method for quickly locating a mobile target in a portable mobile range based on UWB and inertial navigation fusion, comprising:

[0009] Step S1: construct a mobile shooting range; place four positioning rods around to construct a convex quadrilateral shooting range;

[0010] Step S2: Rapid positioning of the mobile target robot; placing the target on the top of the robot, controlling the target to move autonomously in the target range through the autonomous movement of the robot, and adopting UWB and inertial navigation fusion positioning;

[0011] Step S3: Building an electronic fence.

[0012] As a further improvement of the present invention: the process of step S1 includes:

[0013] Step S101: First, the position of the No. 1 positioning rod is used as the origin of the global coordinate system, and the line connecting the No. 1 positioning rod with the right side is used as the X-axis (OA direction), and the direction perpendicular to the X-axis is used as the Y-axis to establish the global coordinate system; connect DA as X', and establish the O'X'Y' coordinate system perpendicular to X' as Y';

[0014] Step S102: positioning between the positioning poles by using UWB ultra-wideband ranging and applying asymmetric bilateral two-way ranging technology.

[0015] As a further improvement of the present invention: when measuring the distance between the positioning poles in step S101, one pole acts as a base station and the other pole acts as a tag.

[0016] As a further improvement of the present invention: the step S102 includes:

[0017] Step S1021: The ranging process begins with the tag sending a request frame to the base station. The base station starts counting after receiving the request frame. The delay is T d1 After that, the base station sends a response signal to the tag. After receiving the response signal, the tag writes the time points of sending and receiving the signal into the termination frame. After a delay of T d2 Then, a termination frame is sent to the base station, and the base station ends the measurement after receiving the termination signal;

[0018] Step S1022: The distances between adjacent positioning rods are measured in sequence by the asymmetric bilateral bidirectional distance measurement method, and the shortest diagonal lines of the quadrilateral are connected to form two triangles. As shown in the figure, the side length of the triangle OAD has been measured, and the angle α can be calculated, and then the position of point D relative to the global coordinate system (X4, Y4) and the position of point C relative to the O'X'Y' coordinate system (X'3, Y'3) are calculated.

[0019] As a further improvement of the present invention: the step S2 comprises:

[0020] Step S201: First, place the robot in the shooting range and locate it according to UWB as the initial position of the robot.

[0021] Step S202: Calculate the change matrix from the navigation coordinate system to the global coordinate system based on the initial position and inertial navigation posture;

[0022] Step S203: After the inertial navigation positioning at time T, UWB position calibration is performed, and the positioning is continuously cycled.

[0023] As a further improvement of the present invention: in step S2, the robot UWB positioning adopts a triangulation positioning method to determine the triangle where the robot is located, and the three positioning poles corresponding to the triangle are used as base stations. The robot is used as a tag to measure the distance between the robot and each base station.

[0024] As a further improvement of the present invention: in the step S2, the distance between the robot target and each base station is first measured by asymmetric bilateral bidirectional measurement, and the three nearest base stations where the robot is located are determined as a triangle, and the distance information is transmitted by WIFI; the position of the robot is calculated by the three-side positioning method; the positions of the three base stations O (X1, Y1), A (X2, Y2), and D (X4, Y4) are known, and the positions of the robot tag from the base stations are d r1 d r2 d r3 , the position of the robot is R(X, Y); use the least squares method to solve the position of the robot in the closed area.

[0025] As a further improvement of the present invention: the robot coordinate system adopts the right-front-up coordinate system, and the east-north-sky heading coordinate system is calculated according to the robot positioning information and the robot heading information measured by inertial navigation; then the rotation matrix R from the global coordinate system to the heading coordinate system is solved. g Then change the robot position (P x ,P y ) as a translation transformation.

[0026] As a further improvement of the present invention: in the step S3, the quadrilateral area surrounded by four positioning rods is used as an electronic fence; the robot controls its own movement according to its own positioning information and electronic fence information; the obtained robot position is used to determine whether the robot is in the quadrilateral area using the inner angle method; for a convex quadrilateral, lines are connected from the point to the four corner points of the quadrilateral respectively, thereby forming four vectors, and if the angle formed by the four vectors is equal to 360°, then it is in the quadrilateral shooting range area, otherwise it is outside the shooting range area; at the same time, it is determined that the robot must not move to the shooting range fence, that is, any side of the quadrilateral, and the judgment condition is that the vector angle is 180°. In order to increase a certain threshold range, the vector angle is restricted to not be greater than 170°, and the sum of the angles is 360°.

[0027] Compared with the prior art, the advantages of the present invention are:

[0028] The invention discloses a method for quickly positioning a mobile target in a portable mobile shooting range based on UWB and inertial navigation fusion. The method has the advantages of simple principle, simple operation and high positioning accuracy. The shooting range is surrounded by four positioning rods, which are equivalent to small base stations. The relative positions between the positioning rods are measured by UWB ultra-wideband, and the distance information is transmitted by WIFI. Finally, each positioning rod can obtain the distance between each positioning rod. The positioning rods are positioned by the relevant algorithm, and the four positioning rods are easy to carry and can be used to arbitrarily arrange the convex polygonal shooting range without additional calibration and manual measurement of the target position. The target is placed on an autonomous mobile robot to realize continuous movement of the target. An inertial navigation component is installed on the robot to form a strapdown inertial navigation system, and the mobile robot is positioned by the fusion of UWB and inertial navigation. Finally, an electronic fence is formed by the positioning rods, and the robot is controlled not to rush out of the shooting range according to the fence position information and the robot positioning information. Finally, the whole system realizes the construction of an electronic fence in a portable mobile shooting range, and the mobile target robot can move continuously and be quickly positioned. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 This is the schematic diagram of the theodolite layout at the shooting range.

[0030] Figure 2 It is a diagram of a positioning rod in a specific application example of the present invention.

[0031] Figure 3 It is a global coordinate system diagram of the shooting range in a specific application example of the present invention.

[0032] Figure 4 It is a schematic diagram of the asymmetric bilateral two-way ranging principle of the present invention in a specific application example.

[0033] Figure 5 It is a flow chart of robot positioning in a specific application example of the present invention.

[0034] Figure 6 It is a diagram of the three-side positioning method in a specific application example of the present invention.

[0035] Figure 7 It is a strapdown inertial navigation principle diagram of the present invention in a specific application example. DETAILED DESCRIPTION

[0036] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0037] like Figures 1 to 7As shown, the present invention is based on the design ability of individual or quasi-individual shooting, by moving the training range to different environments, autonomously moving the target to train and improve the shooter. Therefore, a method for quickly positioning a portable mobile range and a mobile target is designed. The present invention adopts a range based on UWB ultra-fast band technology to build, and UWB and inertial navigation are integrated to quickly position the mobile target robot.

[0038] Quick construction of mobile shooting range: The shooting range is constructed by four portable positioning rods. The shooter places the four positioning rods around to build a convex quadrilateral shooting range. There is no need for manual measurement and calibration to arrange a regular quadrilateral shooting range for positioning. The global coordinate system of the shooting range is constructed as follows: Figure 3 As shown, the main solution is to first use the position of the No. 1 positioning rod as the origin of the global coordinate system, and the line connecting the right positioning rod as the X-axis (OA direction), and the direction perpendicular to the X-axis as the Y-axis to establish the global coordinate system. Connect DA as X', and perpendicular to X' as Y' to establish the O'X'Y' coordinate system.

[0039] Each positioning rod uses UWB ultra-wideband ranging and asymmetric double side two-way range (ADS-TWR) technology for positioning. There is no need to synchronize the clock between the positioning rod and the robot tag, which greatly reduces the difficulty of operation and calculation. When measuring the distance between the positioning rods, one rod is equivalent to the base station and the other rod acts as the tag. The ranging process begins with the tag sending a request frame to the base station. After the base station receives the request frame, it starts counting the time. The delay T d1 After that, the base station sends a response signal to the tag. After receiving the response signal, the tag writes the time points of sending and receiving the signal into the termination frame. After a delay of T d2 Then it sends a termination frame to the base station, and the base station ends the measurement after receiving the termination signal.

[0040] Distance calculation derivation:

[0041] T r1 =T d1 +2T tof (1)

[0042] T r2 =T d1 +2T tof (2)

[0043]

[0044] d=T tof ×c

[0045] The distances between adjacent positioning rods are measured in sequence by the asymmetric bilateral bidirectional distance measurement method, and the shortest diagonal lines of the quadrilateral are connected to form two triangles. As shown in the figure, the side length of triangle OAD has been measured, and the angle α can be calculated, and then the position of point D relative to the global coordinate system (X4, Y4) and the position of C relative to the O'X'Y' coordinate system (X'3, Y'3) are calculated.

[0046]

[0047]

[0048]

[0049] X4=d1cosα (8)

[0050] Y4=d1sinα (9)

[0051] X'3=d5cosθ (8)

[0052] Y3'=d5sinθ (9)

[0053] The coordinate system O'X'Y' is rotated by -β and translated by (X4, Y4) relative to the global coordinate system. So the transformation matrix is

[0054]

[0055] Find the position of C in the global coordinate system

[0056] [X3, Y3]'=P·[X'3, Y'3]' (11)

[0057] The coordinates of (X2, Y2) are (d2, 0), and the coordinates of (X1, Y1) are (0, 0). Thus, the target range can be quickly constructed. (2) Rapid positioning of mobile target robot:

[0058] The target is placed on the top of the robot, and the robot's autonomous movement is used to control the target's autonomous movement in the shooting range. To prevent the robot from moving out of the shooting range, rapid positioning of the robot is essential. UWB positioning has high accuracy, but poor real-time and continuity, while inertial navigation positioning has good real-time continuity, but there will be cumulative errors. The present invention adopts UWB and inertial navigation fusion positioning.

[0059] In the method of the present invention, the robot is first placed in the shooting range and positioned according to UWB as the initial position of the robot. Then, the change matrix from the navigation coordinate system to the global coordinate system is calculated based on the initial position and the inertial navigation posture. After the inertial navigation positioning at time T, the UWB position calibration is performed. The continuous cycle positioning can ensure the accuracy, continuity and real-time performance of the robot positioning.

[0060] The robot UWB positioning adopts the triangulation positioning method to determine the triangle where the robot is located (OAD triangle or ADC triangle), and the three positioning poles corresponding to the triangle are used as base stations. The robot is used as a tag to measure the distance between the robot and each base station.

[0061] like Figure 4 As shown in the figure, the distance between the robot target and each base station is first measured by asymmetric bilateral bidirectional measurement, and the three nearest base stations where the robot is located are determined as a triangle, and the distance information is transmitted using WIFI. The position of the robot is calculated using the three-sided positioning method. The positions of the three base stations O(X1, Y1), A(X2, Y2), and D(X4, Y4) are known. The positions of the robot tag from the base station are d r1 ,d r2 ,d r3 The robot's position is R(X, Y). The equations are:

[0062]

[0063] In non-ideal situations, the measured distance between the robot and the base station may have errors with the true value. Therefore, it is impossible to make the three circles intersect at one point. Often a closed area is formed. The least squares method is used to solve the position of the robot in the closed area.

[0064] From formula (12), we can get the following using least square method:

[0065] AX=b (13)

[0066]

[0067]

[0068] Where X represents the robot position matrix [X, Y]', and the final solution is:

[0069] X=(A T A) -1 A T b (16)

[0070] The robot coordinate system uses the right-front-up coordinate system, and the east-north-sky heading coordinate system is calculated based on the robot positioning information and the robot heading information measured by inertial navigation. Then solve the rotation matrix R from the global coordinate system to the heading coordinate system g Then change the robot position (P x ,P y ) as the translation transformation. The robot's position updates can thus be transformed into the global coordinate system.

[0071] Angular velocity through gyroscope and calculate the angular velocity The difference continuously updates the attitude matrix The attitude information is solved by the attitude matrix. Acceleration specific force information After coordinate conversion, the acceleration information in the navigation coordinate system is obtained. After integral calculation, the velocity information and position information are calculated respectively. x ,P y ) is converted to the position in the global coordinate system established by the positioning rod The position change matrix is ​​the position of the robot relative to the original coordinate system at the last moment. The speed information obtained is To update This iterates and updates the position continuously.

[0072] Electronic fence construction: In order to prevent the robot from running out of the shooting range, a quadrilateral area surrounded by four positioning poles is used as an electronic fence. The robot controls its own movement based on its own positioning information and electronic fence information. According to the robot position obtained in the previous step, the inner angle method is used to determine whether the robot is in the quadrilateral area. For a convex quadrilateral, lines are connected from the point to the four corner points of the quadrilateral to form four vectors. If the angle formed by the four vectors is equal to 360°, it is within the quadrilateral shooting range area, otherwise it is outside the shooting range area. At the same time, it is determined that the robot must not move to the shooting range fence, that is, any side of the quadrilateral. The judgment condition is that the vector angle is 180°. In order to increase a certain threshold range, the vector angle is restricted to not be greater than 170°, and the sum of the angles is 360°.

[0073] Angle calculation (only the calculation of α1 is listed, the rest of the calculations are similar):

[0074]

[0075] The conditions for determining whether the robot moves within the electronic fence are:

[0076] α1&α2&α3&α4<170° (18)

[0077] α1+α2+α3+α4=360 (19)

[0078] First, when the shooting range is moved to accommodate the shooter's training environment, the movement of a fixed shooting range or a traditional shooting range requires a lot of manpower and material resources to build and calibrate. The present invention uses four positioning poles as base stations to enclose the shooting range, uses UWB ultra-wideband to measure the distance between the positioning poles, transmits distance information through WIFI communication, and uses the three sides of a triangle to solve the angle to locate and construct the global coordinate system of the shooting range. The present invention can well overcome the complex calibration of building a shooting range coordinate system, and can efficiently and quickly build a shooting range coordinate system.

[0079] Secondly, traditional targets are often placed on shelves or on remote control cars, and the target movement is not autonomous enough and not continuous enough. Therefore, the present invention uses an autonomous robot as a target carrier, places the target on the robot, and can achieve autonomous movement in the shooting range through autonomous positioning and navigation. One important step is robot positioning. Traditionally, GPS or Beidou positioning is used, but the system needs to be time-calibrated, and the positioning system layout is time-consuming. The present invention adopts the method of UWB and inertial navigation fusion positioning, integrating the precise positioning of UWB and the rapid positioning effect of inertial navigation. The robot moves autonomously in the shooting range, of course, it also needs an electronic fence. The present invention uses a convex polygon with an internal angle of 360° to determine whether it is inside the shooting range, thereby limiting and planning the movement of the target robot.

[0080] Finally, the method for quickly positioning mobile targets in a portable mobile shooting range based on the fusion of UWB and inertial navigation of the present invention can quickly build a shooting range positioning system for single-soldier or quasi-soldier combat shooting training scenarios, can move targets autonomously, and can quickly, efficiently and accurately position targets, thereby realizing autonomous movement of targets within the electronic fence of the shooting range.

[0081] The above are only preferred embodiments of the present invention, and the protection scope of the present invention is not limited to the above embodiments. All technical solutions under the concept of the present invention belong to the protection scope of the present invention. It should be pointed out that for ordinary technicians in this technical field, some improvements and modifications without departing from the principle of the present invention should be regarded as the protection scope of the present invention.

Claims

1. A method for quickly locating a mobile target in a portable mobile range based on UWB and inertial navigation fusion, characterized in that: include: Step S1: construct a mobile shooting range; place four positioning rods around to construct a convex quadrilateral shooting range; Step S2: Rapid positioning of the mobile target robot; The target is placed on the top of the robot, and the robot's autonomous movement is used to control the target's autonomous movement in the shooting range, using UWB and inertial navigation fusion positioning; Step S3: Building an electronic fence; The step S2 comprises: Step S201: firstly place the robot in the shooting range and locate it according to UWB as the initial position of the robot; Step S202: Calculate the change matrix from the navigation coordinate system to the global coordinate system based on the initial position and inertial navigation posture; Step S203: After the inertial navigation positioning at time T, UWB position calibration is performed, and positioning is continuously cycled; In the step S3, a quadrilateral area surrounded by four positioning rods is used as an electronic fence; the robot controls its own movement according to its own positioning information and electronic fence information; the obtained robot position is used to determine whether the robot is in the quadrilateral area using the inner angle method; for a convex quadrilateral, lines are connected from a point to the four corner points of the quadrilateral respectively, thereby forming four vectors, and if the angle formed by the four vectors is equal to 360°, then it is in the quadrilateral shooting range area, otherwise it is outside the shooting range area; at the same time, it is determined that the robot must not move to the shooting range fence, that is, any side of the quadrilateral, and the judgment condition is that the vector angle is 180°; in order to increase a certain threshold range, the vector angle is restricted to not be greater than 170°, and the sum of the angles is 360°.

2. The method for quickly locating a mobile target in a portable mobile shooting range based on UWB and inertial navigation fusion according to claim 1 is characterized in that: The process of step S1 includes: Step S101: Use the position of the No. 1 positioning rod as the origin of the global coordinate system, the line connecting the No. 1 positioning rod with the right positioning rod as the X-axis, and the direction perpendicular to the X-axis as the Y-axis to establish a global coordinate system; connect DA as ,vertical Do Establish Coordinate system; Step S102: positioning between the positioning poles by using UWB ultra-wideband ranging and an asymmetric bilateral two-way ranging method.

3. The method for quickly locating a mobile target in a portable mobile shooting range based on UWB and inertial navigation fusion according to claim 2 is characterized in that: When measuring the distance between the positioning poles in step S101, one pole acts as a base station and the other pole acts as a tag.

4. The method for quickly locating a mobile target in a portable mobile shooting range based on UWB and inertial navigation fusion according to claim 3 is characterized in that: The step S102 includes: Step S1021: The ranging process begins with the tag sending a request frame to the base station. The base station starts counting after receiving the request frame. After that, the base station sends a response signal to the tag; after receiving the response signal, the tag writes the time points of sending and receiving signals into the termination frame. Then, a termination frame is sent to the base station, and the base station ends the measurement after receiving the termination signal; Step S1022: The distances between adjacent positioning rods are measured in sequence by the asymmetric bilateral bidirectional distance measurement method, and the shortest diagonal lines of the quadrilateral are connected to form two triangles; the side length of the triangle OAD has been measured, and the angle α can be calculated, and then the position of point D relative to the global coordinate system is calculated ( ), C relative to The position of the coordinate system ( ).

5. The method for quickly locating a mobile target in a portable mobile shooting range based on UWB and inertial navigation fusion according to claim 1 is characterized in that: In step S2, the robot UWB positioning adopts a triangulation positioning method to determine the triangle where the robot is located, and the three positioning poles corresponding to the triangle are used as base stations. The robot is used as a tag to measure the distance between the robot and each base station.

6. The method for rapid positioning of mobile targets in a portable mobile shooting range based on UWB and inertial navigation fusion according to claim 1 is characterized in that: In step S2, the distance between the robot target and each base station is first measured by asymmetric bilateral bidirectional measurement, and the three nearest base stations where the robot is located are determined as a triangle, and the distance information is transmitted by WIFI; the position of the robot is calculated by the three-side positioning method; if the three base stations O ( )、A( )、D( ), the robot tag’s position from the base station is , , , the position of the robot is R (X, Y); use the least squares method to solve the position of the robot in the closed area.

7. The method for quickly locating a mobile target in a portable mobile shooting range based on UWB and inertial navigation fusion according to claim 1 is characterized in that: The robot coordinate system adopts the right-front-up coordinate system. The east-north-sky heading coordinate system is calculated based on the robot positioning information and the robot heading information measured by inertial navigation; then the rotation matrix from the global coordinate system to the heading coordinate system is solved. ; Then change the robot position ( ) as a translation transformation.

Images