Target following control method, device and system based on rectangular array of conical antennas

Abstract

The application discloses a target following control method, device and system based on a rectangular array of conical antennas, four conical antennas and a 1T4R UWB transceiving unit arranged on the same PCB are used to synchronously acquire four-way receiving phases of the same tag signal, a plurality of baseline phase differences after direction unification are circularly fused according to channel phase compensation parameters and effective phase center coordinate parameters corresponding to a current working channel, a target azimuth is obtained by using a four-quadrant inverse tangent function, and a target distance is obtained by combining UWB bidirectional ranging; and a following controller controls turning and speed of a mobile carrier according to an annular angle error and a distance error in a vehicle body coordinate system, and the following can be started when the tag is located at any initial direction in horizontal 360 degrees.

Description

Technical Field

[0001] This invention relates to the fields of wireless communication, UWB positioning, and mobile vehicle following control, and particularly to a target following control method, apparatus, and system based on a rectangular array of conical antennas. Background Technology

[0002] Traditional UWB positioning or tracking systems often rely on complex multi-base station deployments. When using multiple planar antenna arrays to achieve 360° coverage, there may still be issues such as sector handover delays, boundary angle jumps, and cross-board calibration errors. Although existing four-antenna angle measurement base stations can use two sets of orthogonal baselines for phase difference angle measurement, it is still necessary to address the differences in effective phase centers under different UWB operating channels, the consistency of directional baselines, and the continuity of +π / -π phase boundaries.

[0003] During actual movement, the signal is easily affected by multipath interference, and the hardware circuit itself has errors, which causes the measured phase data to jitter or deviate, resulting in the vehicle swaying left and right or the distance to the vehicle changing erratically when following.

[0004] When the target is obscured or the signal quality is extremely poor, if the system continues to use incorrect data for control, the vehicle may collide, lose the target, or make unpredictable and dangerous maneuvers.

[0005] In summary, there is a need for a target following control method and system that, while maintaining the rectangular array structure of the four-conical antenna used in the mechanism, can perform phase center and channel phase compensation for the current working channel, perform circular fusion of directed multi-baselines, and correctly convert the ranging and angle measurement results to the coordinate system of the moving vehicle. Summary of the Invention

[0006] To address the shortcomings of existing technologies, this invention provides a target following control method based on a conical antenna rectangular array, a UWB omnidirectional angle measurement and ranging device and system, aiming to solve problems such as discontinuous omnidirectional angle measurement boundaries, inconsistent parameters of different working channels, coordinate transformation of mobile vehicles, and low-confidence safety control.

[0007] To achieve the above objectives, the present invention provides the following technical solution: a target following control method based on a conical antenna rectangular array, comprising the following steps:

[0008] Step S1: Acquire data by using a UWB angle and distance measuring device installed on the mobile vehicle to obtain the azimuth angle θ, spatial distance D, and confidence level C of the tag relative to the device coordinate system.

[0009] Step S2: Steering control. After the azimuth angle θ is corrected by the installation deviation angle of the UWB angle measuring and ranging device relative to the moving vehicle, a circular angle error is formed in the coordinate system of the moving vehicle. The circular angle error is used as the steering control quantity to control the moving vehicle to turn to the target direction.

[0010] Step S3: Speed ​​control, using the deviation between the distance and the preset following distance as the speed control value to control the speed of the moving vehicle;

[0011] Step S4: Anomaly handling: Determine whether the measurement is valid based on the confidence level C and the data update time; reduce the linear speed of the moving vehicle when the measurement fails for a short time; control the moving vehicle to stop when the continuous failure exceeds the preset time.

[0012] Optionally, step S1 is implemented in the following manner:

[0013] Step S11: Signal reception. The phase information of the same tag UWB pulse signal received by the four conical antennas is synchronously acquired using the four receiving channels of the 1T4R UWB transceiver unit.

[0014] Step S12: Extract the phase difference. Determine four sets of directed baselines based on the effective phase center coordinates of the four conical antennas under the current working channel, and calculate the arrival phase difference of the four sets of directed baselines. The four sets of directed baselines include baseline A from antenna 3 to antenna 0, baseline B from antenna 1 to antenna 0, baseline C from antenna 2 to antenna 1, and baseline D from antenna 2 to antenna 3. Baseline A and baseline C are in the same direction, baseline B and baseline D are in the same direction, and the two sets of parallel baselines are orthogonal to each other.

[0015] Step S13: Joint processing, perform circular averaging or weighted circular averaging on the phase difference of two parallel baselines with the same direction to obtain the fused phase difference ΔΦx in the X direction and the fused phase difference ΔΦy in the Y direction, so as to reduce measurement jitter and avoid the phase difference located on both sides of the +π and -π boundary being linearly averaged as an erroneous result.

[0016] Step S14: Calculate the azimuth angle. Normalize ΔΦx and ΔΦy according to the wavenumber of the current working channel and the corresponding effective phase center baseline length. Calculate the azimuth angle θ of the target relative to the UWB angle measuring and ranging device using the four-quadrant arctangent function.

[0017] Step S15: Measure the distance. Use one of the conical antennas as the transmitting antenna to conduct two-way ranging communication with the tag and obtain the target distance D based on the flight time.

[0018] Step S16: Confidence assessment. Based on at least one parameter among the signal-to-noise ratio of the received signal, first-path power, or multipath index, assess the confidence C of the current measurement, and finally output the azimuth angle θ, distance D, and confidence C.

[0019] Optionally, step S12 may further include a channel-related phase calibration step:

[0020] The current operating channel of the UWB angle and distance measuring device is determined, and the channel phase compensation parameters and effective phase center coordinate parameters corresponding to the current operating channel are read from the non-volatile memory. The parameters are obtained by acquiring four receiving phases at multiple known azimuth and elevation angle positions and reducing the ring residual between the measured directional baseline phase difference and the theoretical directional baseline phase difference. The corresponding channel phase compensation parameters are subtracted from the original phase difference of each directional baseline measured in real time to obtain the calibrated phase difference.

[0021] Optionally, the circumferential fusion in step S13 is implemented in the following manner:

[0022] Define the directed phase difference according to the start and end points of each baseline, and perform sign transformation on the phase differences of the baselines that represent the same spatial direction but have opposite start and end orders, so that the two parallel baselines participating in the fusion have the same directed direction.

[0023] The phase difference between two parallel X-direction baselines, after being aligned and calibrated, is mapped onto the complex plane. The corresponding unit phasors are then weighted and summed to obtain the argument, yielding ΔΦx. The phase difference between two parallel Y-direction baselines is processed in the same way to obtain ΔΦy.

[0024] The weights used in the weighting are determined based on at least one of the signal-to-noise ratio of the received signal, first-path power, multipath index, and parallel baseline consistency; when the two phase differences are located on both sides of the +π and -π boundaries respectively, the continuity of the phase boundaries is maintained by the circumferential fusion.

[0025] Optionally, step S2 is implemented in the following manner:

[0026] Step S21: Receive azimuth data. Continuously receive the azimuth angle θ of the target relative to the device coordinate system output by the motion control module on the mobile vehicle through the motion control module on the mobile vehicle.

[0027] Step S22: Azimuth deviation calculation. Based on the installation deviation angle θm of the UWB omnidirectional angle measuring and ranging device relative to the forward direction of the moving vehicle, the azimuth angle θ is corrected by coordinate, and the steering error eθ=wrap(θ-θm) is calculated by the annular angle difference function, where wrap represents mapping the angle to a preset main value range.

[0028] Step S23: Generate steering command, using the steering error eθ as the steering control quantity, and generate angular velocity command for the moving vehicle accordingly;

[0029] Step S24: Execute steering. The motion control module sends steering commands to the drive system of the moving vehicle to control the vehicle to rotate and gradually align it with the target direction.

[0030] Optionally, step S3 is implemented in the following manner:

[0031] Step S31: Receive distance data. The motion control module on the mobile vehicle continuously receives the target spatial distance D output from the UWB omnidirectional angle measuring and ranging device.

[0032] Step S32: Distance deviation calculation. The motion control module compares the received real-time distance D with the user-preset ideal following distance D0 and calculates the deviation value between the two.

[0033] Step S33: Generate a speed command, using the calculated distance deviation as a speed control quantity to generate a linear speed command; when the absolute value of the steering error is greater than a preset steering threshold, reduce or limit the linear speed command;

[0034] Step S34: Execute speed control. The motion control module sends speed commands to the drive system of the moving vehicle to control the vehicle to move forward, decelerate, or move backward.

[0035] Optionally, step S4 is implemented in the following manner:

[0036] Step S41: Confidence assessment. While outputting the azimuth angle θ and distance D, the UWB omnidirectional angle and distance measuring device will assess the confidence C of the current measurement based on the quality parameters of the received signal.

[0037] Step S42: Threshold determination: The motion control module on the mobile vehicle receives the confidence level C and compares it with the preset threshold.

[0038] Step S43: Execute the security policy. When the confidence level C is lower than the failure threshold or the data is not updated within a preset time, trigger the graded anomaly handling logic:

[0039] During the first failure period, maintain the most recent effective steering trend and reduce linear speed at a preset slope; when consecutive failures exceed the second failure period, output a stop command and enter a safe waiting state; resume following when the confidence level of multiple consecutive measurement cycles is higher than the recovery threshold.

[0040] A target following control device based on a rectangular array of conical antennas, employing the target following control method based on a rectangular array of conical antennas, is characterized by comprising a circular PCB, four conical antennas, a 1T4R UWB transceiver unit, a processing unit, and a non-volatile memory.

[0041] The four conical antennas are positioned on the same side of the circular PCB and connected to the four receiving channels of the 1T4R UWB transceiver unit, with at least one conical antenna also connected to the transmitting channel. The non-volatile memory stores the channel phase compensation parameters corresponding to the current operating channel and the effective phase center coordinate parameters of each conical antenna. The processing unit is configured to construct at least two sets of non-parallel directed baseline phase differences based on the four receiving phases of the same tag signal, perform direction unification, phase compensation, and circumferential fusion on the directed baseline phase differences to obtain the 360° azimuth angle of the tag relative to the device, and perform UWB bidirectional ranging with the tag through the transmitting channel to obtain the spatial distance.

[0042] A target following control system based on a conical antenna rectangular array, employing the target following control method based on a conical antenna rectangular array, includes a mobile vehicle, a UWB omnidirectional angle and distance measuring device, a motion control module, a drive system, and a tag;

[0043] The UWB omnidirectional angle and distance measuring device is fixedly installed on the mobile vehicle and is used to obtain the azimuth angle θ, spatial distance D, confidence level C and data timestamp of the tag relative to the device coordinate system.

[0044] The motion control module is used to convert the azimuth angle θ into a ring angle error in the coordinate system of the mobile vehicle based on the installation deviation angle of the UWB omnidirectional angle measuring and ranging device relative to the forward direction of the mobile vehicle, generate an angular velocity command based on the ring angle error, generate a linear velocity command based on the distance error between the spatial distance D and the preset following distance D0, and perform deceleration and stopping control when the confidence level is low or the data timeout occurs.

[0045] The drive system receives steering and speed commands from the motion control module and directly controls the motor or steering mechanism of the moving vehicle to achieve the physical movement of the vehicle.

[0046] A non-volatile memory is installed in the UWB omnidirectional angle and distance measuring device to store channel phase compensation parameters and effective phase center coordinate parameters of each conical antenna according to the working channel.

[0047] The tag is used to send UWB pulse signals for the base station to receive, and to cooperate with the base station for two-way ranging communication.

[0048] The beneficial effects of this invention are:

[0049] 1. In this invention, four conical antennas are set on the same PCB and the four receiving phases of the same tag signal are obtained by the 1T4R UWB transceiver unit, avoiding sector switching between multiple planar arrays; continuous azimuth angles are obtained through directional baseline unification, channel correlation compensation and circular fusion, supporting horizontal 360° angle measurement and a maximum measurement frequency of 100Hz within the verified working range.

[0050] 2. In this invention, by fusing azimuth angle, spatial distance and confidence level, the mobile vehicle can automatically adjust its orientation and speed; the parallel redundant baseline is used to reduce random jitter, detect anomalies and maintain the continuity of +π / -π boundary, and the spacing between adjacent effective phase centers is still designed according to the half-wavelength constraint of the current working channel, thereby taking into account both no spatial aliasing and angle measurement sensitivity.

[0051] 3. In this invention, the relative azimuth angle output by the base station is converted into a ring angle error in the coordinate system of the moving vehicle after installation deviation correction. A graded safety strategy of short-term speed reduction, continuous failure shutdown and recovery hysteresis is adopted to reduce the risk of malfunction when the target is blocked or multipath is severe. Attached Figure Description

[0052] Figure 1 This is a schematic diagram of a method flow of the present invention.

[0053] Figure 2 This is a schematic diagram of step S1 of the present invention.

[0054] Figure 3 This is a schematic diagram of step S2 of the present invention.

[0055] Figure 4 This is a schematic diagram of step S3 of the present invention.

[0056] Figure 5 This is a schematic diagram of step S4 of the present invention.

[0057] Figure 6 This is a test curve diagram of the present invention.

[0058] Figure 7 This is a physical image of a UWB angle and distance measuring device according to the present invention.

[0059] Figure 8 This is a hardware connection block diagram of the present invention. Detailed Implementation

[0060] To more clearly illustrate the technical solutions in the embodiments of the invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0061] like Figures 1 to 7 As shown, a target following control method based on a conical antenna rectangular array includes the following:

[0062] This invention uses a solid cone made of copper or copper alloy as the antenna radiator, hereinafter referred to as a conical antenna. The key parameters of the conical antenna are: cone height 5~8mm, base diameter approximately φ6mm, weight approximately 0.72g, feed impedance 50Ω, and it is vertically mounted on a PCB board using a DIP connector method.

[0063] The tapered geometry of the cone mitigates abrupt changes in input impedance with frequency and, together with the PCB grounding structure, feed gap, RF traces, and optional matching network, forms a broadband UWB radiating element. Through electromagnetic simulation and prototype testing, the height, base diameter, and feed structure of the conical antenna are adjusted to ensure that the corresponding product model or configuration meets preset impedance matching specifications within UWB Channel 5 (center frequency 6489.6MHz, bandwidth 499.2MHz) and / or Channel 9 (center frequency 7987.2MHz, bandwidth 499.2MHz).

[0064] The conical antenna has approximately omnidirectional radiation characteristics in the horizontal plane, with a gain of about 2 dBi and linear polarization. Its specific gain fluctuation and effective elevation coverage are determined by the antenna's three-dimensional radiation pattern, PCB ground structure, installation environment, and test results.

[0065] Electromagnetic simulation and multi-angle calibration are used to extract the effective phase center coordinates of the conical antenna under the current operating channel and their variation with frequency and incident direction. The processing unit uses these effective phase center coordinates, rather than just the antenna's geometric center, for PDOA calculation to reduce the systematic angle measurement error introduced by phase center offset.

[0066] like Figure 7 As shown, this invention arranges four conical antennas (antenna 0, antenna 1, antenna 2, and antenna 3) in a rectangular array in the central area of ​​a circular PCB with a diameter of 70mm. The four antennas are located at the four vertices of the rectangle.

[0067] In this design, the rectangular array adopts an equidistant design (dx=dy), meaning the four antennas are arranged in a square. The four conical antennas use the same structure, and the differences in radiation pattern, gain, and phase center of each receiving channel are reduced through symmetrical layout, RF trace design, and factory calibration.

[0068] Four antennas form four PDOA angle measurement baselines:

[0069] Baseline A: Antenna 3 → Antenna 0 (top of rectangle, X direction);

[0070] Baseline B: Antenna 1 → Antenna 0 (Right side of the rectangle, Y direction);

[0071] Baseline C: Antenna 2 → Antenna 1 (bottom edge of rectangle, X direction, same as baseline A);

[0072] Baseline D: Antenna 2 → Antenna 3 (left side of the rectangle, Y direction, same as baseline B direction).

[0073] In this configuration, baselines A and C are parallel baselines in the X-direction, and baselines B and D are parallel baselines in the Y-direction, with the two sets of parallel baselines being orthogonal and of equal length. If the hardware output uses the reverse start and end order, the phase difference is inverted and remapped to the preset principal value range during processing. This arrangement forms two pairs of parallel redundant baselines of equal length, used for circular fusion, reducing random jitter, and detecting anomalies.

[0074] In addition, an auxiliary baseline can be formed along the diagonal direction (antenna 3→antenna 1, antenna 0→antenna 2). The length of the diagonal baseline is √2 times the side length, which can be used for further angle cross-checking or accuracy enhancement in specific scenarios.

[0075] The chip architecture is 1T4R (1 transmit, 4 receive): Antennas 0, 1, 2, and 3 are all connected to the four receive channels of the UWB chip for synchronous sampling; Antenna 3 is also connected to the transmit channel for TWR bidirectional ranging with the tag. Angle measurement and ranging share the same set of antennas, eliminating the need for an additional dedicated ranging antenna.

[0076] The spacing design of rectangular arrays follows these principles:

[0077] (1) The spacing between adjacent effective phase centers is preferably no greater than half of the shortest wavelength corresponding to the highest operating frequency (Channel 9 upper sideband 8236.8MHz), i.e. d≤λmin / 2≈18.2mm, in order to avoid spatial phase aliasing under single baseline conditions;

[0078] (2) The spacing should not be too small, otherwise it will reduce the angular resolution;

[0079] (3) Through electromagnetic simulation, the actual phase center position (rather than the physical geometric center) of each unit in the antenna array is used as the equivalent spacing for optimization design. The optimization objective is to minimize the position offset of the phase center in the entire working frequency band of CH5 and CH9, and to determine the final side length value.

[0080] (4) Parallel redundant baselines are used to reduce random jitter, detect anomalies and maintain the continuity of +π / -π boundaries. They cannot be used to determine the integer week ambiguity when the half-wavelength constraint is exceeded. Therefore, the array design still satisfies the above effective phase center spacing constraint.

[0081] A circular PCB and symmetrical layout help reduce the difference in the impact of the substrate edges on the four antennas. However, PCB traces, components, mounting holes, and the housing may still disrupt the ideal symmetry. These differences are compensated for through electromagnetic simulation and factory calibration. A ground copper layer is provided on the PCB. The 50Ω characteristic impedance of the RF traces is determined by the dielectric thickness, dielectric constant, copper thickness, trace width, and ground spacing.

[0082] In this invention, the design of the antenna array spacing not only affects the measurement accuracy of the horizontal azimuth angle, but also directly determines the effective working range of the device for azimuth angle measurement under different elevation angle conditions.

[0083] This device does not have an independent pitch angle measurement function and cannot be used alone to achieve three-dimensional positioning. The "effective working range of pitch angle -25° to +60°" mentioned here means that when the target tag is within this pitch angle range, the device can perform horizontal azimuth angle measurement and UWB ranging according to the specified test conditions.

[0084] For an array where the antenna phase centers are located on the same horizontal plane, under ideal far-field conditions, the horizontal baseline phase difference varies with the cosine factor of the elevation angle. Positive and negative elevation will not reverse the horizontal path difference simply because of different signs. Actual negative elevation performance is mainly affected by the antenna's three-dimensional radiation pattern, the circular PCB ground structure, obstruction from moving vehicles and the outer casing, effective phase center drift, signal-to-noise ratio, and multipath propagation. Therefore, the effective operating range is determined through three-dimensional electromagnetic simulation and calibration and testing at multiple azimuth and elevation angles.

[0085] The design uses the effective phase center position of each element in the antenna array, rather than just the physical center position, as the equivalent spacing. Electromagnetic simulation and calibration are used to extract the effective phase center position and its variation characteristics within the Channel 5 and / or Channel 9 operating frequency bands of the conical antenna, and the effective phase center spacing is used as an array optimization variable.

[0086] During the spacing optimization process, the constraint is that the spacing between adjacent effective phase centers should not be greater than half of the shortest wavelength corresponding to the current working channel. The phase difference, received signal quality and azimuth error of the two sets of orthogonal baselines are evaluated within the target elevation angle range of -25° to +60°.

[0087] The tapered structure of a conical antenna facilitates simultaneous optimization of impedance bandwidth, radiation pattern, and effective phase center changes; its performance differences relative to other antenna types are based on simulation or test results under the same conditions.

[0088] Through joint optimization of antenna size, effective phase center spacing, and channel phase compensation parameters, and product testing, this embodiment achieves an effective working range of pitch angle from -25° to +60° under specified conditions. This range describes the applicable area for horizontal azimuth angle measurement in scenarios such as different heights, slopes, or when people are bending over, and does not indicate that the device can output pitch angle.

[0089] During tracking, mobile vehicles may place the tag in a negative elevation angle due to terrain undulations and the height difference between the vehicle and the personnel. This invention reduces the probability of tracking failure caused by abnormal azimuth angles in negative elevation scenarios through antenna 3D performance optimization, effective phase center modeling, and multi-angle calibration.

[0090] like Figure 8 As shown, this invention uses the MK8000 UWB transceiver processing chip, which supports a 1T4R (1 transmit 4 receive) architecture, with one built-in transmit channel and four receive channels. The four conical antennas are labeled as antenna 0, antenna 1, antenna 2, and antenna 3, respectively. Antennas 0, 1, 2, and 3 all have receiving functions, while antenna 3 also has a transmitting function for TOF ranging.

[0091] Array baseline configuration:

[0092] Four antennas form multiple PDOA angle measurement baselines:

[0093] Baseline A: From antenna 3 to antenna 0;

[0094] Baseline B: Antenna 1 to Antenna 0;

[0095] Baseline C: From antenna 2 to antenna 1;

[0096] Baseline D: Antenna 2 to Antenna 3.

[0097] Of the four sets of baselines mentioned above, baselines A and C are in the same direction and parallel to each other, and baselines B and D are in the same direction and parallel to each other. The two sets of parallel baselines are orthogonal to each other. If the underlying chip output uses different phase difference start and end sequences, the processing unit first performs sign unification.

[0098] The 360° angle measurement method utilizes redundant information from two sets of parallel baselines to reduce jitter and maintain phase boundary continuity through orientation unification, channel correlation calibration, and circular fusion.

[0099] (1) The arrival phase difference of two parallel baselines after unifying the direction can be used as redundant measurement of the same direction component.

[0100] (2) Mapping the phase difference of two parallel baselines in the same direction to a complex plane unit phasor and performing a circular average or a circular average weighted according to signal quality can reduce random jitter.

[0101] (3) When the two phase differences are located on both sides of the +π and -π boundaries respectively, circular fusion can avoid the erroneous result of ordinary linear averaging approaching zero; the parallel baseline itself is not used to break the half-wavelength constraint of the effective phase center spacing.

[0102] (4) Based on the wavenumber of the current working channel and the effective phase center baseline length in the X and Y directions, normalize the fused phase differences ΔΦx and ΔΦy, and then calculate the 360° azimuth angle using the four-quadrant arctangent function.

[0103] TOF ranging:

[0104] Antenna 3 serves as the transmitting antenna, enabling two-way ranging communication with the tag via TWR (Two-Way Ranging) to obtain distance values ​​based on time-of-flight. Ranging and angle measurement share the same antenna array, eliminating the need for a dedicated ranging antenna and reducing system complexity and cost.

[0105] Compared with the scheme of using time-division switching or multi-chip phase acquisition, the 1T4R receiving scheme of the present invention has the following advantages: (1) Four receiving channels acquire the phase information of the same tag signal, reducing the target motion error introduced by time-division sampling; (2) Phase acquisition is completed in the same UWB transceiver unit, reducing cross-board synchronization and additional device errors; (3) The multi-baseline redundancy design achieves noise reduction, anomaly detection and continuous phase boundary processing through direction unification and circumferential fusion; (4) The single-module integration scheme is conducive to reducing hardware cost, size and power consumption. In this embodiment, the average operating current is about 7mA at the 20Hz measurement frequency.

[0106] The signal processing unit (which can be implemented by the chip's built-in processor or an external MCU) is responsible for:

[0107] (1) The four sets of baseline phase differences output by the receiving chip;

[0108] (2) Apply the factory-calibrated phase compensation values ​​to each channel for calibration compensation;

[0109] (3) Perform directional unification, channel correlation compensation and circular fusion on the two sets of parallel baselines respectively to obtain the X-direction fusion phase difference ΔΦx and the Y-direction fusion phase difference ΔΦy;

[0110] (4) Normalize ΔΦx and ΔΦy according to the wavenumber of the current working channel and the effective phase center baseline length, and calculate the 360° azimuth using the four-quadrant arctangent function;

[0111] (5) Obtain the target distance D by performing TWR two-way ranging through the transmission channel of antenna 3;

[0112] (6) Assess confidence level based on received signal quality (such as signal-to-noise ratio, first-path power, etc.);

[0113] (7) Output the three parameters of azimuth angle θ, distance D and confidence level at a frequency of not less than 20Hz through the UART interface.

[0114] This invention is applicable to both Channel 5 and Channel 9 product models, or, if hardware supports it, the corresponding operating channel can be selected by configuration. The processing unit calls the corresponding channel phase compensation parameters and effective phase center coordinate parameters according to the current operating channel. The center frequency of Channel 5 is 6489.6MHz, and the center frequency of Channel 9 is 7987.2MHz. The specific operating channel is determined according to the product model, application scenario, and local regulations.

[0115] Due to manufacturing tolerances, PCB trace length errors, antenna soldering position deviations, and inconsistent gain / phase across chip channels, each mass-produced device exhibits inherent phase deviations between its receiving channels. Without calibration and compensation, these inherent phase deviations will be directly added to the PDOA measurement values, resulting in a systematic shift in angle measurement.

[0116] The method of calibration based on the working channel is adopted:

[0117] (1) In a low multipath or repeatable calibration environment, position the tag or standard signal source at multiple known azimuth and multiple known pitch angles;

[0118] (2) For each working channel supported by the base station, the phase information of four receiving channels is collected, and the measured phase difference of the directed baseline after unification of direction is calculated;

[0119] (3) Establish the theoretical phase difference based on the known spatial direction, the wave number of the current working channel and the candidate phase center coordinates of each antenna. By reducing the ring residual between the measured phase difference and the theoretical phase difference, determine the channel phase compensation parameters and the effective phase center coordinate parameters.

[0120] (4) Write the channel phase compensation parameters and effective phase center coordinate parameters corresponding to the working channel into the non-volatile memory of the device, and make them correspond one-to-one with the device;

[0121] (5) During normal operation, the signal processing unit first determines the current working channel, reads the corresponding parameters, performs directional unification and channel phase compensation on the original phase difference, and then performs circular fusion and angle calculation.

[0122] This calibration method is used to reduce batch variations caused by inherent phase deviations in PCB traces, antenna mounting, and receiver channels, and to ensure that different operating channels use parameters that match their wavenumbers and effective phase centers.

[0123] This invention achieves 360° continuous azimuth angle calculation based on four sets of baselines (two pairs of parallel orthogonal baselines) in a rectangular array. The specific method is as follows:

[0124] Step 1: The four receiving channels of the UWB transceiver processing chip (corresponding to antenna 0, antenna 1, antenna 2, and antenna 3) synchronously receive the UWB pulse signal sent by the tag and obtain the receiving phase information of each channel.

[0125] Step 2: Extract the arrival phase difference of the four baselines:

[0126] The phase difference ΔΦ_A between baseline A (antenna 3 to antenna 0);

[0127] The phase difference ΔΦ_B between baseline B (antenna 1 to antenna 0);

[0128] The phase difference ΔΦ_C between baseline C (antenna 2 to antenna 1);

[0129] The phase difference ΔΦ_D between baseline D (antenna 2 to antenna 3).

[0130] Step 3: Apply the factory-calibrated compensation values ​​to calibrate and compensate for the phase difference of each baseline.

[0131] Step 4: Unifying parallel baseline directions and circumferential blending:

[0132] X direction: Baseline A (from antenna 3 to antenna 0) and baseline C (from antenna 2 to antenna 1) are in the same direction and parallel to each other. The two are circumferentially fused to obtain ΔΦx;

[0133] Y direction: Baseline B (antenna 1 to antenna 0) and baseline D (antenna 2 to antenna 3) are in the same direction and parallel to each other. The two are circularly fused to obtain ΔΦy.

[0134] Circular fusion is achieved by averaging or weighted averaging the complex plane unit phasors corresponding to the two phase differences and then taking the argument. The weights are determined based on the quality of the received signal and the consistency of the parallel baseline.

[0135] Step 5: Phase boundary continuity processing - When the phase difference between two parallel baselines in the same direction is located on both sides of the +π and -π boundaries respectively, circular fusion is used to avoid the near-zero erroneous phase obtained by ordinary linear averaging.

[0136] If the circumferential distance between two parallel baselines in the same direction is less than the consistency threshold, the circumferential fusion result is taken as the fusion phase difference in the corresponding direction.

[0137] If the circumferential distance between the two baselines exceeds the consistency threshold, the weight of the abnormal baseline is reduced based on the quality of the received signal; when neither baseline meets the quality threshold, the confidence of the current measurement is reduced.

[0138] The above mechanism is used for continuous phase boundary processing, noise reduction, and anomaly detection; the array still avoids basic spatial aliasing by constraining the effective phase center spacing to be no greater than half the shortest wavelength of the current working channel.

[0139] Step 6: Normalize ΔΦx and ΔΦy according to the wavenumber of the current working channel and the effective phase center baseline length in the X and Y directions, and then calculate the azimuth angle using the four-quadrant arctangent function; the output range is [-180°, 180°) or converted to [0°, 360°] via analog-to-digital conversion.

[0140] Step 7: Perform TWR two-way ranging with the tag through the transmission channel of antenna 3, and obtain the distance value D based on the time of flight.

[0141] Step 8: Evaluate the confidence level C of the current measurement based on the quality parameters of the received signal (signal-to-noise ratio, first path power, multipath index, etc.).

[0142] Step 9: Output the azimuth angle θ, distance D, and confidence level C.

[0143] This method utilizes parallel redundant baselines with unified direction for circular fusion, which can reduce random jitter, detect anomalies, and handle the continuity of +π / -π boundaries. It reduces systematic deviation by using the channel phase compensation parameters and effective phase center coordinate parameters corresponding to the current working channel. It determines the azimuth quadrant by using two sets of orthogonal direction components and the four-quadrant arctangent function, thereby achieving continuous 360° azimuth output within the verified working range.

[0144] A moving target tracking system based on the above-mentioned angle and distance measuring device includes:

[0145] (1) UWB omnidirectional angle and distance measuring device (base station): installed on the top of a mobile vehicle (such as a quadruped robot, wheeled robot, electric car, golf cart, smart wheelchair, smart suitcase, etc.), with the antenna array facing upwards and placed horizontally.

[0146] (2) UWB tag: worn or carried by the target (personnel) being followed, periodically emitting UWB signals.

[0147] (3) Motion control module: It is connected to the angle and distance measuring device via UART communication, receives azimuth, distance and confidence information, and generates steering and speed commands for the mobile vehicle accordingly.

[0148] Follow control method:

[0149] Step S1: The angle and distance measuring device continuously outputs the azimuth angle θ, spatial distance D, confidence level C, and data timestamp of the target relative to the device coordinate system at a preset frequency.

[0150] Step S2: The motion control module calculates the annular angular error eθ=wrap(θ-θm) in the vehicle coordinate system based on the installation deviation angle θm of the device relative to the forward direction of the moving vehicle, and generates angular velocity commands based on eθ.

[0151] Step S3: Use the deviation (D-D0) between the spatial distance D and the preset following distance D0 as the linear speed control quantity; when |eθ| is greater than the turning threshold, limit the linear speed so that the moving vehicle turns first or turns in a low-speed arc; when |eθ| decreases, move forward, decelerate or reverse according to the distance deviation.

[0152] Step S4: When the confidence level C is lower than the failure threshold or the data times out, reduce the linear speed during the first failure period; stop when consecutive failures exceed the second failure period; resume following after the confidence level is higher than the recovery threshold for multiple consecutive cycles.

[0153] Step S5: Repeat steps S1-S4 at a frequency of not less than 20Hz to achieve closed-loop real-time tracking.

[0154] Since the angle and distance measuring device covers 360° horizontally within its effective working range, the mobile vehicle can directly obtain the relative azimuth angle when the target is in front, to the side or behind, and then turn and follow accordingly, without having to use rotation search as a necessary step to obtain the target's azimuth.

[0155] The angle and distance measuring device of the present invention can also operate in single-base station positioning mode, replacing the traditional multi-base station TOF positioning scheme:

[0156] Operating method: The angle and distance measuring device is fixedly installed at a known location in the environment as a single positioning base station. The base station simultaneously obtains the azimuth angle θ and spatial slant range D of the tag; when the tag and the base station are at the same height, let the horizontal polar radius ρ = D; when the height difference Δh between the two is known, let ρ = √(D² - Δh²), and then convert it to horizontal rectangular coordinates.

[0157] x = ρ × cos(θ),

[0158] y = ρ × sin(θ).

[0159] This device does not have an independent pitch angle measurement function. When the tag and base station are at the same height or the height difference is known, the horizontal plane coordinates can be calculated as described above; when the height difference is unknown, the azimuth angle θ and spatial slant distance D are output as relative position observations. For scenarios requiring altitude information, a barometer or other altitude sensors can be used in conjunction.

[0160] This invention requires only one base station device to output the azimuth and spatial distance of the tag relative to the base station, reducing the equipment deployment, location calibration and coordinate management required for traditional multi-base station ranging and positioning. When the conditions of equal height or known height difference are met, it can be used for horizontal relative positioning in scenarios such as small rooms and warehouse shelving aisles, and can be configured and used between follow mode and fixed positioning mode.

[0161] The present invention is compared with a solution that achieves 360° by splicing multiple planar antenna arrays:

[0162] This solution is based on an omnidirectional conical antenna array on the same PCB and four-quadrant arctangent calculation. Within the verified operating range, there is no need to switch sectors between multiple planar arrays. The multi-planar splicing solution, on the other hand, needs to handle the coordinate calibration and boundary switching issues between arrays.

[0163] The azimuth angle measurement accuracy of this scheme is ±3°, and its statistical caliber is 85% coverage within the effective angle range. The accuracy comparison with other schemes should be carried out under the same channel, distance, elevation angle, environment and statistical caliber.

[0164] This solution uses four receiving channels to acquire the phase information of the same tag signal, with no sector RF switching delay, and the measurement frequency supports up to 100Hz; the specific effective output frequency is affected by the baud rate, ranging process and system configuration.

[0165] This solution can directly output the target's relative azimuth angle within a 360° effective working range in the horizontal direction, providing a directional basis for starting follow-up in any initial direction.

[0166] This solution uses four conical antennas, one 1T4R UWB transceiver module, and one circular PCB with a diameter of approximately 70mm. Compared to solutions that require multiple antenna boards and sector switching links, this solution helps to reduce size, number of components, and calibration requirements.

[0167] In this embodiment, the average operating current is approximately 7mA and the typical quiescent current is approximately 5.5μA under a measurement frequency of 20Hz and a specified transmit power configuration. The power consumption comparison of different schemes should be conducted under the same measurement frequency and transmit power conditions.

[0168] This solution stores calibration parameters for the same four-channel array according to the working channel; in addition to the calibration of each array itself, the multi-plane splicing solution usually needs to handle the coordinate and boundary consistency between arrays.

[0169] This embodiment integrates UWB PDOA angle measurement and UWB two-way ranging: azimuth coverage is 0°~360°; the effective working range of target elevation angle is -25°~+60°; the effective angle coverage is 85% with an azimuth error of ±3°; and the coverage is 90% with a ranging error of ±10cm. The above performance corresponds to the test conditions specified in the product.

[0170] The tapered geometry of the conical antenna, together with the PCB ground plane and feed design, achieves UWB impedance matching for Channel 5 and / or Channel 9. The product selects the operating channel based on its model or configuration; Channel 5, under specified transmit power and test conditions, can achieve a ranging distance of up to 15m.

[0171] Through antenna 3D performance optimization, effective phase center modeling, channel-related calibration, and array spacing constraints, this embodiment achieves an effective pitch operating range of -25° to +60° under specified conditions. This characteristic is used to ensure horizontal azimuth angle measurement within this pitch range and does not mean that the device can directly output the pitch angle.

[0172] The processing unit uses the effective phase center coordinates and channel phase compensation parameters corresponding to the current working channel to perform PDOA calculation, so as to reduce the impact of frequency difference, PCB traces, antenna installation and receiving channel deviation on the angle measurement results.

[0173] Compared to multi-plane antenna splicing schemes, this scheme does not require radio frequency switching between multiple angle measurement sectors and reduces the number of coordinate calibration items between arrays; compared to traditional multi-base station ranging and positioning schemes, this scheme outputs relative azimuth and spatial distance through a single device, making it suitable for robot following and small-space relative positioning scenarios.

[0174] When the target is in any initial horizontal orientation, the mobile vehicle can perform steering and initiate following based on the relative azimuth angle. This embodiment has an average operating current of approximately 7mA, a typical dormant current of approximately 5.5μA, a module diameter of approximately 70mm, and a height of approximately 15mm (including the antenna), making it suitable for battery-powered mobile platforms.

[0175] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions or improvements made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A target following control method based on a rectangular array of conical antennas, characterized in that, Includes the following steps: Step S1: Acquire data by using a UWB angle and distance measuring device installed on the mobile vehicle to obtain the azimuth angle θ, spatial distance D, and confidence level C of the tag relative to the device coordinate system. Step S2: Steering control. After the azimuth angle θ is corrected by the installation deviation angle of the UWB angle measuring and ranging device relative to the moving vehicle, a circular angle error is formed in the coordinate system of the moving vehicle. The circular angle error is used as the steering control quantity to control the moving vehicle to turn to the target direction. Step S3: Speed ​​control, using the deviation between the distance and the preset following distance as the speed control value to control the speed of the moving vehicle; Step S4: Anomaly handling: Determine whether the measurement is valid based on the confidence level C and the data update time; reduce the linear speed of the moving vehicle when the measurement fails for a short time; control the moving vehicle to stop when the continuous failure exceeds the preset time.

2. The target following control method based on a conical antenna rectangular array according to claim 1, characterized in that, Step S1 is implemented in the following manner: Step S11: Signal reception. The phase information of the same tag UWB pulse signal received by the four conical antennas is synchronously acquired using the four receiving channels of the 1T4R UWB transceiver unit. Step S12: Extract the phase difference. Determine four sets of directed baselines based on the effective phase center coordinates of the four conical antennas under the current working channel, and calculate the arrival phase difference of the four sets of directed baselines. The four sets of directed baselines include baseline A from antenna 3 to antenna 0, baseline B from antenna 1 to antenna 0, baseline C from antenna 2 to antenna 1, and baseline D from antenna 2 to antenna 3. Baseline A and baseline C are in the same direction, baseline B and baseline D are in the same direction, and the two sets of parallel baselines are orthogonal to each other. Step S13: Joint processing, perform circular averaging or weighted circular averaging on the phase difference of two parallel baselines with the same direction to obtain the fused phase difference ΔΦx in the X direction and the fused phase difference ΔΦy in the Y direction, so as to reduce measurement jitter and avoid the phase difference located on both sides of the +π and -π boundary being linearly averaged as an erroneous result. Step S14: Calculate the azimuth angle. Normalize ΔΦx and ΔΦy according to the wavenumber of the current working channel and the corresponding effective phase center baseline length. Calculate the azimuth angle θ of the target relative to the UWB angle measuring and ranging device using the four-quadrant arctangent function. Step S15: Measure the distance. Use one of the conical antennas as the transmitting antenna to conduct two-way ranging communication with the tag and obtain the target distance D based on the flight time. Step S16: Confidence assessment. Based on at least one parameter among the signal-to-noise ratio of the received signal, first-path power, or multipath index, assess the confidence C of the current measurement, and finally output the azimuth angle θ, distance D, and confidence C.

3. The target following control method based on a conical antenna rectangular array according to claim 2, characterized in that, Step S12 also includes a channel-related phase calibration step: The current operating channel of the UWB angle and distance measuring device is determined, and the channel phase compensation parameters and effective phase center coordinate parameters corresponding to the current operating channel are read from the non-volatile memory. The parameters are obtained by acquiring four receiving phases at multiple known azimuth and elevation angle positions and reducing the ring residual between the measured directional baseline phase difference and the theoretical directional baseline phase difference. The corresponding channel phase compensation parameters are subtracted from the original phase difference of each directional baseline measured in real time to obtain the calibrated phase difference.

4. The target following control method based on a conical antenna rectangular array according to claim 2, characterized in that, The circumferential fusion in step S13 is implemented in the following manner: Define the directed phase difference according to the start and end points of each baseline, and perform sign transformation on the phase differences of the baselines that represent the same spatial direction but have opposite start and end orders, so that the two parallel baselines participating in the fusion have the same directed direction. The phase difference between two parallel X-direction baselines, after being aligned and calibrated, is mapped onto the complex plane. The corresponding unit phasors are then weighted and summed to obtain the argument, yielding ΔΦx. The phase difference between two parallel Y-direction baselines is processed in the same way to obtain ΔΦy. The weighting weights are determined based on at least one of the signal-to-noise ratio of the received signal, first-path power, multipath index, and parallel baseline consistency; when the two phase differences are located on both sides of the +π and -π boundaries respectively, the phase boundaries are made continuous through the circumferential fusion.

5. The target following control method based on a conical antenna rectangular array according to claim 1, characterized in that, Step S2 is implemented in the following manner: Step S21: Receive azimuth data. Continuously receive the azimuth angle θ of the target relative to the device coordinate system output by the motion control module on the mobile vehicle through the motion control module on the mobile vehicle. Step S22: Azimuth deviation calculation. Based on the installation deviation angle θm of the UWB omnidirectional angle measuring and ranging device relative to the forward direction of the moving vehicle, the azimuth angle θ is corrected by coordinate, and the steering error eθ=wrap(θ-θm) is calculated by the annular angle difference function, where wrap represents mapping the angle to a preset main value range. Step S23: Generate steering command, using the steering error eθ as the steering control quantity, and generate angular velocity command for the moving vehicle accordingly; Step S24: Execute steering. The motion control module sends steering commands to the drive system of the moving vehicle to control the vehicle to rotate and gradually align it with the target direction.

6. The target following control method based on a conical antenna rectangular array according to claim 1, characterized in that, Step S3 is implemented in the following manner: Step S31: Receive distance data. The motion control module on the mobile vehicle continuously receives the target spatial distance D output from the UWB omnidirectional angle measuring and ranging device. Step S32: Distance deviation calculation. The motion control module compares the received real-time distance D with the user-preset ideal following distance D0 and calculates the deviation value between the two. Step S33: Generate a speed command, using the calculated distance deviation as a speed control quantity to generate a linear speed command; when the absolute value of the steering error is greater than a preset steering threshold, reduce or limit the linear speed command; Step S34: Execute speed control. The motion control module sends speed commands to the drive system of the moving vehicle to control the vehicle to move forward, decelerate, or move backward.

7. The target following control method based on a conical antenna rectangular array according to claim 1, characterized in that, Step S4 is implemented in the following manner: Step S41: Confidence assessment. While outputting the azimuth angle θ and distance D, the UWB omnidirectional angle and distance measuring device will assess the confidence C of the current measurement based on the quality parameters of the received signal. Step S42: Threshold determination: The motion control module on the mobile vehicle receives the confidence level C and compares it with the preset threshold. Step S43: Execute the security policy. When the confidence level C is lower than the failure threshold or the data is not updated within a preset time, trigger the graded anomaly handling logic: During the first failure period, maintain the most recent effective turning trend and reduce the linear velocity according to the preset slope; When consecutive failures exceed the second failure period, a stop command is output and the system enters a safe waiting state. Recovery tracking resumes when the confidence level is above the recovery threshold for multiple consecutive measurement periods.

8. A target following control device based on a conical antenna rectangular array, employing the target following control method based on a conical antenna rectangular array as described in any one of claims 1-7, characterized in that, It includes a circular PCB, four conical antennas, a 1T4R UWB transceiver unit, a processing unit, and non-volatile memory; The four conical antennas are positioned on the same side of the circular PCB and connected to the four receiving channels of the 1T4R UWB transceiver unit, with at least one conical antenna also connected to the transmitting channel. The non-volatile memory stores the channel phase compensation parameters corresponding to the current operating channel and the effective phase center coordinate parameters of each conical antenna. The processing unit is configured to construct at least two sets of non-parallel directed baseline phase differences based on the four receiving phases of the same tag signal, perform direction unification, phase compensation, and circumferential fusion on the directed baseline phase differences to obtain the 360° azimuth angle of the tag relative to the device, and perform UWB bidirectional ranging with the tag through the transmitting channel to obtain the spatial distance.

9. A target following control system based on a conical antenna rectangular array, employing the target following control method based on a conical antenna rectangular array as described in any one of claims 1-7, characterized in that, Includes mobile vehicle, UWB omnidirectional angle and distance measuring device, motion control module, drive system and tag; The UWB omnidirectional angle and distance measuring device is fixedly installed on the mobile vehicle and is used to obtain the azimuth angle θ, spatial distance D, confidence level C and data timestamp of the tag relative to the device coordinate system. The motion control module is used to convert the azimuth angle θ into a ring angle error in the coordinate system of the mobile vehicle based on the installation deviation angle of the UWB omnidirectional angle measuring and ranging device relative to the forward direction of the mobile vehicle, generate an angular velocity command based on the ring angle error, generate a linear velocity command based on the distance error between the spatial distance D and the preset following distance D0, and perform deceleration and stopping control when the confidence level is low or the data timeout occurs. The drive system receives steering and speed commands from the motion control module and directly controls the motor or steering mechanism of the moving vehicle to achieve the physical movement of the vehicle. A non-volatile memory is installed in the UWB omnidirectional angle and distance measuring device to store channel phase compensation parameters and effective phase center coordinate parameters of each conical antenna according to the working channel. The tag is used to send UWB pulse signals for the base station to receive, and to cooperate with the base station for two-way ranging communication.

Images