Multi-target relative pose state monitoring method and system

Abstract

The invention provides a multi-target relative pose state monitoring method and system, and adopts a Beidou navigation positioning system and a six-degree-of-freedom inertial navigation unit to carry out relative pose state-based cooperative situation awareness measurement technology on modular multi-target equipment such as ship formation, unmanned ship cluster, unmanned aerial vehicle cluster, ship-ship cooperative formation and the like. According to the technical scheme of the invention, a distributed star topology structure is realized based on an ad hoc network wireless communication technology, so that the method can be more flexibly applied to target relative pose state measurement and comprehensive situation monitoring in a large-scale unmanned aerial vehicle and unmanned ship cluster or collaborative operation scene; the method is a key for carrying out researches such as unmanned equipment cluster, multi-target collaborative related control and performance evaluation at present.

Description

Technical Field

[0001] The present invention relates to the technical field of collaborative situation awareness measurement, and specifically relates to a method and system for monitoring the relative position and pose states of multiple targets. Background Art

[0002] At present, the real-time measurement of the relative position and relative pose between two objects is a key technology in the fields of unmanned system clusters, multi-target collaboration, etc. There are mainly three technical implementation methods, which are specifically as follows:

[0003] The first is the multi-point ranging method. Laser or microwave rangefinders are arranged on the target platform to construct a reference coordinate system, and multiple targets or beacons are arranged on the measured target. The relative position and pose between the two are calculated through geometric transformation, such as patents CN117570957A and CN115451920B. However, this technology has certain requirements for the relationship between the distance between targets and the measurement distance. If the distance between targets is relatively small compared to the measurement distance, it will affect the measurement accuracy of the attitude angle of the measured target.

[0004] The second is the pose measurement based on image recognition technology, including technologies such as camera sequential imaging and binocular vision. Its device arrangement method is similar to the multi-point ranging method. Perception and measurement devices are arranged on the main platform, and the three-dimensional pose of the target is obtained through calculation processes such as stereo calibration and depth extraction of the collected target images. To improve the efficiency of target recognition based on images, feature targets are usually arranged on the target, such as patents CN10657094B, CN106780607, and CN115690205B. However, its measurement accuracy depends on the pixels of the target imaging, and the real-time performance of the measurement depends on the image calculation and processing platform.

[0005] The third is to fuse the inertial navigation-based attitude measurement technology with other measurement technologies to obtain the pose information of a single target, and then obtain the relative pose state between targets through differential calculation in a unified coordinate system, such as patents CN113075713B and CN114383612B. However, this technology has high requirements for the spatio-temporal consistency of single-target measurement data, and it is difficult to accurately achieve synchronous measurement of the six-degree-of-freedom relative pose state.

[0006] Existing relative pose measurement technologies have different advantages and disadvantages due to differences in underlying technologies. In the implementation processes of the first two technologies, one of the target bodies is used as the reference platform for the measurement device, and the output of the pose measurement of the measured target is the relative pose state information between the two targets, and no further differential calculation is required. However, factors such as the motion state of the target, the distance between the two targets, image operation and processing, and rain and snow weather all affect the measurement accuracy and real-time performance, and it is not suitable for multi-target relative pose measurement and overall situation monitoring.

[0007] For the third relative pose measurement technology, Patent CN113075713B realizes the relative pose measurement between vehicles through methods such as comprehensive GPS positioning measurement, dead reckoning, and communication delay processing. Given the limited three-dimensional measurement accuracy of GPS and factors such as the position deviation caused by the superposition of communication delay and vehicle speed, its measurement dimension and accuracy cannot meet the measurement requirements of the unmanned equipment cluster and multi-target collaborative situation. Patent CN114383612B obtains relative attitude data through visual measurement assisted inertial measurement. This technology is more suitable for the relative attitude measurement between two targets and cannot meet the accurate measurement requirements of the relative position and relative attitude of multiple targets. Summary of the Invention

[0008] The technical problem to be solved by the technical solution of the present invention is: for the multi-target cluster control or collaborative operation scenario, the real-time and accurate measurement of the relative position and relative attitude between multiple targets has a direct and important impact on its control performance and collaborative efficiency. Due to the wide distribution range of multiple targets, and their respective external environments and motion states being different, how to achieve the accurate measurement of the six-degree-of-freedom relative state of multiple targets in this complex scenario.

[0009] The technical solution of the present invention provides a multi-target relative pose state monitoring system method. The multi-target relative pose state monitoring method is used to detect multiple measured target objects. The multi-target relative pose state monitoring method includes the following steps:

[0010] Multiple terminal devices are matched and connected with multiple measured target objects. A central device is set according to the positions of the multiple terminal devices. An RTK base station is provided inside the central device;

[0011] Taking the position where the RTK base station is located as the positioning measurement anchor point, multiple terminal devices continuously and accurately measure the longitude and latitude coordinate information of the measured target objects based on the RTK differential information and send it to the central device. The central device determines the accurate position coordinates of the multiple measured target objects through calculation and processing;

[0012] Synchronize the clocks of the multiple terminal devices and the central device. Determine the initial origin according to the positioning measurement anchor point, and construct a north-east-earth three-dimensional space coordinate system with the accurate position coordinates of the multiple measured target objects;

[0013] Multiple terminal devices accurately calculate the position data and attitude angles corresponding to the multiple measured target objects corresponding to the multiple terminal devices according to the set measurement data types and frequencies, in combination with the north-east-earth three-dimensional space coordinate system;

[0014] Multiple terminal devices align the position data and attitude angles according to the UTC time corresponding to the position data and attitude angles, transform the position data according to the position offsets of multiple measured target objects in the north-east-earth three-dimensional space coordinate system in combination with the attitude angles, obtain the pose data of multiple measured target objects, and pack and transmit the data of multiple terminal devices to the central device;

[0015] The central device performs data parsing and clock alignment processing on the received data packet and stores it;

[0016] When in use, the stored data is retrieved, and the relative pose and relative motion state between the targets are calculated based on the unified space-time reference. Taking the reference target as the center, the multi-target motion state and comprehensive situation are displayed on the monitoring interface.

[0017] Preferably, the precise position coordinates include longitude, latitude and elevation.

[0018] Preferably, during the multi-target cluster or collaborative operation process, the stored data is retrieved, and the position of the measured target object located at the central position among the precise position coordinates of multiple measured target objects is selected as the origin to calculate and process the relative positions and attitudes of other measured target objects.

[0019] Preferably, the measurement data types include longitude, latitude, elevation, speed magnitude, speed direction, position UTC time, linear accelerations in the longitudinal, lateral and vertical directions, angular velocities in the yaw, pitch and roll directions, and attitude UTC time.

[0020] Preferably, the resolution of the position UTC time is consistent with the measurement output frequency.

[0021] Preferably, the resolution of the attitude UTC time is consistent with the measurement output frequency.

[0022] The technical solution of the present invention also provides a multi-target relative pose state monitoring system, which adopts a multi-target relative pose state monitoring method as described above. The multi-target relative pose state monitoring system includes:

[0023] Multiple terminal devices, each terminal device includes a Beidou navigation and positioning measuring instrument, a six-degree-of-freedom inertial navigation unit and a terminal pose measurement data processing unit;

[0024] A central device, including a Beidou navigation and positioning RTK base station, a calculation processing unit, a data storage module and a multi-target comprehensive situation display unit;

[0025] Among them, the Beidou navigation and positioning measuring instrument continuously and precisely measures the longitude and latitude coordinate information of the measured target object based on the RTK differential information and sends it to the calculation processing unit, and the calculation processing unit determines the precise position coordinates of multiple measured target objects through calculation and processing;

[0026] The six - degree - of - freedom inertial navigation unit, the terminal pose measurement data processing unit, and the calculation processing unit all receive the second - pulse signals from the Beidou navigation positioning measurer and the Beidou navigation RTK base station for clock synchronization. The calculation processing unit determines the initial origin according to the positioning measurement anchor point and constructs a north - east - earth three - dimensional space coordinate system with the accurate position coordinates of multiple measured target objects;

[0027] The Beidou navigation positioning measurer accurately calculates the position data corresponding to multiple measured target objects corresponding to multiple terminal devices according to the set measurement data type and frequency, in combination with the north - east - earth three - dimensional space coordinate system;

[0028] The six - degree - of - freedom inertial navigation unit accurately calculates the attitude angles corresponding to multiple measured target objects corresponding to multiple terminal devices according to the set measurement data type and frequency, in combination with the north - east - earth three - dimensional space coordinate system;

[0029] The terminal pose measurement data processing unit aligns the position data and the attitude angles according to the UTC time corresponding to the position data and the attitude angles, performs transformation processing on the position data according to the position offsets of multiple measured target objects in the north - east - earth three - dimensional space coordinate system in combination with the attitude angles, obtains the pose data of multiple measured target objects, and packs and transmits the data of multiple terminal devices to the calculation processing unit;

[0030] The calculation processing unit performs data parsing and clock alignment processing on the received data packet and transmits it to the data storage module for storage;

[0031] Retrieve the stored data, calculate the relative pose and relative motion state between targets based on the unified space - time reference, and display the multi - target motion state and comprehensive situation on the monitoring interface with the reference target as the center.

[0032] Preferably, the multi - target relative pose state monitoring system is applicable to the measurement of the relative pose state and the monitoring of the comprehensive situation of targets in large - scale scenarios such as unmanned aerial vehicle clusters, unmanned boat clusters or collaborative operation scenarios.

[0033] Preferably, the multiple terminal devices and the central device are all equipped with self - organizing network wireless communication units for mutual communication.

[0034] Preferably, the multiple terminal devices and the central device achieve a distributed star - type topology structure through the self - organizing network wireless communication units.

[0035] The technical solution of the present invention proposes a multi-target relative posture state monitoring method and system, which uses the Beidou navigation and positioning system and the six-degree-of-freedom inertial navigation unit to measure modular multi-target equipment, such as ship formations, unmanned boat clusters, drone clusters, ship-ship collaborative formations, etc., based on the collaborative situation awareness measurement technology of relative posture state. The technical solution of the present invention is based on ad hoc network wireless communication technology to achieve a distributed star topology structure, which can be more flexibly applied to the relative posture state measurement and comprehensive situation monitoring of targets in a large-scale range of drones, unmanned boat clusters or collaborative operation scenarios. It is the key to the current research on unmanned equipment clusters, multi-target collaborative related control and performance evaluation. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 A flowchart of a multi-target relative posture state monitoring method provided by an embodiment of the present invention;

[0037] Figure 2 A schematic diagram of the structure of a multi-target relative posture state monitoring system provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0038] The present invention will be further described below in conjunction with specific embodiments. It should be understood that these embodiments are only used to illustrate the present invention and are not intended to limit the scope of the present invention. In addition, it should be understood that after reading the content taught by the present invention, those skilled in the art can make various changes or modifications to the present invention, and these equivalent forms fall within the scope limited by the appended claims of the application equally.

[0039] like Figure 1 As shown, a multi-target relative posture state monitoring method provided by an embodiment of the present invention includes the following steps:

[0040] (1) The terminal device is fixed and bound to the measured object, and the central device determines the layout position according to the position of the terminal device. According to the position of the terminal device and the central device, select a suitable fixed RTK base station as the positioning measurement anchor point. Use the Beidou navigation positioning measurement instrument to continuously measure and record the longitude and latitude coordinate information of the positioning measurement anchor point for a certain period of time. Through cross-comparison, the precise position coordinates (longitude and latitude and elevation) of the positioning measurement anchor point in the geographic coordinate system are finally accurately determined to obtain the anchor point coordinate information, and the anchor point coordinate information is used as the measurement benchmark of the positioning measurement instrument of each terminal device.

[0041] (2) The six-degree-of-freedom inertial navigation unit of the terminal device, the data processing unit of the terminal device, and the data processing unit of the central device all receive the second pulse signal (PPS) of the Beidou navigation and positioning measuring instrument in multiple terminal devices and automatically synchronize the clocks with it. Then, based on the anchor point position of the RTK base station as the initial origin, a north-east-earth three-dimensional space coordinate system is constructed. It should be noted that the origin of this coordinate system can be flexibly selected according to the actual operation scenario. During multi-target cluster or collaborative operations, the position of any target at the multi-target center can be selected as the origin, and based on this reference, the relative positions and postures of other targets are calculated and processed;

[0042] (3) Set the measurement data type and frequency of the Beidou navigation and positioning measuring instrument as needed. The measurement data type should include longitude, latitude, elevation, speed magnitude, speed direction, and UTC time. The resolution of the UTC time should be consistent with the measurement output frequency;

[0043] (4) Set the measurement data type and frequency of the six-degree-of-freedom inertial navigation unit as needed. The measurement data should include the linear accelerations in the longitudinal, lateral, and vertical directions in the body coordinate system of the target, the angular velocities and attitude angles in the directions of yaw, pitch, roll, etc., and UTC time. The UTC time resolution should be consistent with the measurement output frequency;

[0044] (5) The terminal pose measurement data processing unit collects the position measurement data and attitude measurement data through the internal communication network, aligns the position and attitude data according to the UTC time mark, and based on the position offset of the terminal device's Beidou measurement antenna in the body coordinate system of the target, comprehensively transforms the position measurement information with the attitude information to obtain the pose data of the measured target. Then, all the data is packed and sent to the central device through the ad-hoc wireless communication unit;

[0045] (6) The central device receives the pose measurement data of all targets through the wireless network. After completing data parsing and clock alignment processing, all the original data is stored in the storage medium of the computing and processing unit;

[0046] (7) The central device calculates the relative pose and relative motion state between targets based on the unified space-time reference. With the reference target as the center, the multi-target comprehensive situation is displayed through the monitoring interface.

[0047] Such as Figure 2As shown in the figure, an embodiment of the present invention further provides a multi-target relative pose state monitoring system. The monitoring system consists of several terminal devices and a set of central devices. The terminal devices are composed of main functional modules such as a Beidou navigation and positioning measuring instrument, a six-degree-of-freedom inertial navigation unit, a terminal pose measurement data processing unit, and a self-organizing network wireless communication unit. The central device is composed of main functional modules such as a Beidou navigation and RTK base station with RTK (Real Time Kinematic) base station function, a calculation and processing unit, a data storage module, a multi-target comprehensive situation display unit, and a self-organizing network wireless communication unit. Data interaction between the central device and the terminal device is realized through a self-organizing network wireless communication module.

[0048] There is no physical connection between the terminal device and the central device, and they can be flexibly arranged according to needs during test applications. Usually, the terminal device is fixedly bound to the object to be measured, so as to directly measure the pose change and motion state of the object to be measured. The central device can be flexibly arranged at any position according to the actual working conditions, so as to comprehensively monitor the pose states of multiple targets, and synchronously determine the position coordinates of the RTK base station as an anchor point to provide differential signals for the Beidou positioning measurement of the terminal device.

Claims

1. A multi-target relative pose state monitoring method, characterized in that, The described multi-target relative pose state monitoring method is used to detect multiple measured target objects. The described multi-target relative pose state monitoring method includes the following steps: Multiple terminal devices are matched and connected to multiple measured target objects. A central device is set according to the positions of the multiple terminal devices. An RTK base station is provided inside the central device; Taking the position where the RTK base station is located as the positioning measurement anchor point, multiple terminal devices continuously and accurately measure the longitude and latitude coordinate information of the measured target objects based on RTK differential information and send it to the central device. The central device determines the accurate position coordinates of the multiple measured target objects through calculation and processing; Synchronize the clocks of the multiple terminal devices and the central device. Determine the initial origin according to the positioning measurement anchor point, and construct a north-east-earth three-dimensional space coordinate system with the accurate position coordinates of the multiple measured target objects; Multiple terminal devices accurately calculate the position data and attitude angles corresponding to the multiple measured target objects corresponding to the multiple terminal devices according to the set measurement data types and frequencies, in combination with the north-east-earth three-dimensional space coordinate system; Multiple terminal devices align the position data and attitude angles according to the UTC time corresponding to the position data and attitude angles. According to the position offsets of the multiple measured target objects in the north-east-earth three-dimensional space coordinate system, and in combination with the attitude angles, perform transformation processing on the position data to obtain the pose data of the multiple measured target objects. Package and transmit the data of the multiple terminal devices to the central device; The central device performs data parsing and clock alignment processing on the received data packets and stores them; When in use, retrieve the stored data, calculate the relative pose and relative motion state between the targets based on a unified spatio-temporal reference. Taking the reference target as the center, display the multi-target motion state and comprehensive situation on the monitoring interface.

2. The multi-object relative pose state monitoring method according to claim 1, characterized in that The described accurate position coordinates include longitude, latitude and elevation.

3. The multi-object relative pose state monitoring method according to claim 1, characterized in that, During the multi-target cluster or collaborative operation process, retrieve the stored data, and select the position of the measured target object located at the central position among the accurate position coordinates of the multiple measured target objects as the origin to calculate and process the relative positions and postures of other measured target objects.

4. The multi-target relative pose state monitoring method according to claim 1, characterized in that The described measurement data types include longitude, latitude, elevation, speed magnitude, speed direction, position UTC time, linear accelerations in the longitudinal, lateral and vertical directions, angular velocities in the yaw, pitch and roll directions, and attitude UTC time.

5. The multi-objective relative pose state monitoring method according to claim 4, characterized in that, The resolution of the described position UTC time is consistent with the measurement output frequency.

6. The multi-objective relative pose state monitoring method according to claim 4, characterized in that The resolution of the described attitude UTC time is consistent with the measurement output frequency.

7. A multi-target relative pose state monitoring system, characterized in that, Adopt a multi-target relative pose state monitoring method as described in claim 1. The described multi-target relative pose state monitoring system includes: Multiple terminal devices, each terminal device includes a Beidou navigation positioning measuring instrument, a six-degree-of-freedom inertial navigation unit and a terminal pose measurement data processing unit; A central device, including a Beidou navigation RTK base station, a calculation processing unit, a data storage module and a multi-target comprehensive situation display unit; Among them, the Beidou navigation and positioning measuring instrument continuously and accurately measures the longitude and latitude coordinate information of the measured target based on RTK differential information and sends it to the calculation and processing unit. The calculation and processing unit determines the accurate position coordinates of multiple measured targets through calculation and processing; The six-degree-of-freedom inertial navigation unit, the terminal pose measurement data processing unit, and the calculation and processing unit all receive the second pulse signals of the Beidou navigation and positioning measuring instrument and the Beidou navigation and RTK base station for clock synchronization. The calculation and processing unit determines the initial origin according to the positioning measurement anchor point and constructs a north-east-earth three-dimensional space coordinate system with the accurate position coordinates of multiple measured targets; The Beidou navigation and positioning measuring instrument accurately calculates the position data corresponding to multiple measured targets corresponding to multiple terminal devices in combination with the north-east-earth three-dimensional space coordinate system according to the set measurement data type and frequency; The six-degree-of-freedom inertial navigation unit accurately calculates the attitude angles corresponding to multiple measured targets corresponding to multiple terminal devices in combination with the north-east-earth three-dimensional space coordinate system according to the set measurement data type and frequency; The terminal pose measurement data processing unit aligns the position data and the attitude angle in terms of clock according to the UTC time corresponding to the position data and the attitude angle, and performs transformation processing on the position data according to the position offset of multiple measured targets in the north-east-earth three-dimensional space coordinate system in combination with the attitude angle to obtain the pose data of multiple measured targets, and packs and transmits the data of multiple terminal devices to the calculation and processing unit; The calculation and processing unit performs data parsing and clock alignment processing on the received data packet and transmits it to the data storage module for storage; Retrieve the stored data, calculate the relative pose and relative motion state between targets based on a unified spatio-temporal reference, and display the multi-target motion state and comprehensive situation on the monitoring interface with the reference target as the center.

8. The multi-objective relative pose state monitoring method according to claim 7, wherein The multi-target relative pose state monitoring system is applicable to the measurement of the relative pose state and the monitoring of the comprehensive situation of targets in large-scale unmanned aerial vehicle, unmanned boat cluster or cooperative operation scenarios.

9. The multi-object relative pose state monitoring method according to claim 7, characterized in that The multiple terminal devices and the central device are all equipped with self-organizing network wireless communication units for communicating with each other.

10. A multi-objective relative pose state monitoring method according to claim 8, characterized in that, The multiple terminal devices and the central device realize a distributed star topology structure through the self-organizing network wireless communication unit.

Images