A multi-channel six degree of freedom electromagnetic position tracking device for robot navigation

Abstract

This invention relates to a multi-channel six-degree-of-freedom electromagnetic positioning and tracking device for robot navigation, comprising: a magnetic field transmitter, multiple magnetic field receivers, and a comprehensive control terminal; the magnetic field transmitter is used to transmit triaxial magnetic field signals into the workspace in a time-division manner and sample the time-domain data of the excitation current of the transmitting antenna; the magnetic field receivers are used to sense the triaxial magnetic field signals and sample the time-domain data of the induced voltage of the receiving antenna; the comprehensive control terminal is used to control the working timing of the magnetic field transmitter and the magnetic field receivers; receive the time-domain data of the excitation current and the time-domain data of the induced voltage; calculate the magnetic field vector matrix of the magnetic field receivers, correct the inter-axis non-orthogonality of the transmitting and receiving antennas, calculate the position and attitude information of each receiving antenna in the transmitting antenna coordinate system, and perform positioning and tracking on each magnetic field receiver. This invention achieves real-time positioning and tracking of multiple targets and effectively improves the accuracy of position and attitude measurement in electromagnetic positioning and tracking.

Description

Technical Field

[0001] This invention belongs to the field of electromagnetic positioning technology, specifically relating to a multi-channel six-degree-of-freedom electromagnetic positioning and tracking device for robot navigation. Background Technology

[0002] With the continuous development of technology, positioning technology is widely used in fields such as assisted medical care, human-computer interaction, and motion detection and analysis. Compared with ultrasonic and photoelectric positioning devices, devices designed based on six-degree-of-freedom electromagnetic positioning technology have advantages such as high speed, low cost, and good portability. Six-degree-of-freedom electromagnetic positioning devices can track the target's motion trajectory while accurately measuring the target's attitude, and most importantly, the positioning is not affected by obstacles. These advantages make multi-target six-degree-of-freedom electromagnetic positioning devices well-suited to meet research needs.

[0003] However, current six-degree-of-freedom electromagnetic positioning technologies are all based on the transmission and reception of ideal orthogonal magnetic fields. In reality, the magnetic field transmitting and receiving antennas cannot achieve an ideal orthogonal state. Therefore, using an ideal orthogonal magnetic field to solve electromagnetic positioning problems in practice will affect the accuracy of positioning and tracking. Summary of the Invention

[0004] Based on the above analysis, the present invention aims to disclose a multi-channel six-degree-of-freedom electromagnetic positioning and tracking device for robot navigation, which realizes high-precision electromagnetic positioning and tracking across multiple channels.

[0005] This invention discloses a multi-channel six-degree-of-freedom electromagnetic positioning and tracking device for robot navigation, comprising: a magnetic field transmitter, multiple magnetic field receivers that move with the robot, and an integrated control terminal;

[0006] The magnetic field transmitting end is used to transmit triaxial magnetic field signals into the workspace in a time-division manner to form a dipole magnetic field; and to sample the time-domain data of the excitation current of the transmitting antenna;

[0007] The magnetic field receiving end is used to sense a three-axis magnetic field signal using a three-axis receiving antenna within the workspace; and to sample the time-domain data of the induced voltage of the receiving antenna.

[0008] The integrated control terminal is used to control the working timing of the magnetic field transmitter and the magnetic field receiver; receive and process the time-domain data of the excitation current and the time-domain data of the induced voltage; calculate the magnetic field vector matrix of the magnetic field receiver, correct the interaxial non-orthogonality of the transmitting and receiving antennas, calculate the position and attitude information of each receiving antenna in the transmitting antenna coordinate system, and realize the positioning and tracking of each magnetic field receiver that moves with the robot.

[0009] Furthermore, the magnetic field transmitting end includes a transmitting excitation circuit and a transmitting antenna;

[0010] The transmitting antenna is a triaxial transmitting antenna, including a frame and three transmitting coils; the frame is a triaxial orthogonal square structure; and a transmitting coil is wound around each of the three axes.

[0011] The transmitting excitation circuit includes an excitation signal generation circuit, a switching circuit, and a current acquisition circuit;

[0012] The excitation signal generation circuit is used to generate a single-frequency sinusoidal voltage signal required for magnetic field emission under the excitation control of the integrated control terminal.

[0013] The switching circuit is used to excite each coil of the triaxial transmitting antenna in a time-division excitation manner according to a fixed timing sequence under the channel timing control of the integrated control terminal.

[0014] The current acquisition circuit is used to acquire time-domain data of the excitation current and output it to the integrated control terminal.

[0015] Furthermore, the magnetic field receiving end includes a receiving processing circuit and a receiving antenna;

[0016] The receiving antenna is a triaxial receiving antenna, including a frame and three receiving coils; the frame is a triaxial orthogonal square structure; and a receiving coil is wound around each of the three axes.

[0017] The receiving processing circuit includes three receiving conditioning circuits and a receiving sampling circuit;

[0018] The three receiving conditioning circuits are each connected to a receiving coil and are used to condition and amplify the signal received by each receiving coil.

[0019] The receiving sampling circuit is used to perform analog-to-digital conversion on the conditioned and amplified received signal to obtain the time-domain data of the induced voltage under the receiving sampling control of the integrated control terminal.

[0020] Furthermore, the integrated control terminal includes an excitation data calculation module and a sensing data calculation module;

[0021] The excitation data calculation module is used to perform FFT calculation on the obtained excitation current time-domain data to obtain the excitation current amplitude and phase of the current transmitting axis, and to calculate the magnetic moment and current phase information of the transmitting antenna based on the excitation current amplitude and phase.

[0022] The inductive data calculation module is used to perform FFT calculation on the time-domain data of the induced voltage obtained by the receiving antenna of each channel to obtain the amplitude and phase of the induced voltage of the receiving antenna.

[0023] Furthermore, the integrated control terminal includes a magnetic field vector matrix calculation module. In this module, the magnetic field strength matrix of the magnetic field measured by the receiving antenna is calculated based on the conditioning circuit gain of the three axes of the receiving antenna, the antenna sensitivity, and the amplitude of the induced voltage of the receiving antenna. The sign matrix of the magnetic field measured by the receiving antenna is determined based on the phase difference between the induced voltage of the receiving antenna and the excitation current of the transmitting antenna. The matrix of the measured magnetic field vector is obtained by multiplying the matrix elements at the same position in the magnetic field strength matrix and the magnetic field sign matrix.

[0024] Furthermore, the transformation matrix from the orthogonal coordinate system to the non-orthogonal coordinate system for each antenna stored in the integrated control terminal. Wherein, angles u1, u2, and u3 are the degrees of non-orthogonality of the actual three magnetic axes of each antenna relative to the ideally orthogonal three magnetic axes.

[0025] Furthermore, the integrated control terminal includes a first correction module and a second correction module;

[0026] The first correction module obtains the magnetic field vector matrix in an orthogonal coordinate system by correcting the interaxial non-orthogonality of the receiving antenna on the magnetic field vector matrix measured at the magnetic field receiving end.

[0027] The corrected formula is:

[0028]

[0029] In the formula, F'_R i Let F_i be the magnetic field vector matrix of the i-th channel's magnetic field receiver in the orthogonal coordinate system; FxTx', FxTy', and FxTz' are the magnetic field intensities of the receiving antenna of this channel's magnetic field receiver on the three axes of the orthogonal coordinate system, respectively; FxTx, FyTy, and FzTy are the measured magnetic field intensities of the receiving antenna of this channel's magnetic field receiver on the three axes of the non-orthogonal coordinate system, respectively; P_R i The transformation matrix from the orthogonal coordinate system to the non-orthogonal magnetic axis coordinate system of the receiving antenna at the i-th channel magnetic field receiving end; "Inverse" is the matrix inversion operation;

[0030] The second correction module is used to correct the interaxial non-orthogonality of the magnetic moment of the transmitting antenna to obtain the magnetic moment of the orthogonal coordinate system; the correction formula is: M'=Transpose(P_T)*M;

[0031] In the formula, M is the magnetic moment matrix of the transmitting antenna in the actual non-orthogonal coordinate system; P_T is the transformation matrix of the transmitting antenna from the orthogonal coordinate system to the non-orthogonal magnetic axis coordinate system.

[0032] Furthermore, the integrated control terminal includes a pose iteration calculation module;

[0033] The pose iteration calculation module is used to establish a coupling equation based on the magnetic moment and magnetic field vector matrix of the transmitting antenna in the orthogonal coordinate system, and to calculate the position and orientation of the receiving antenna of the magnetic field receiving end by iteratively solving the equation.

[0034] The coupling equation F'_R i =Lm*Rm*M'; where F'_R i M' is the magnetic field vector matrix measured by the magnetic field receiver of the i-th channel in the orthogonal coordinate system; Lm is the magnetic moment of the transmitting antenna in the orthogonal coordinate system; Rm is the attitude matrix of the receiving antenna of the magnetic field receiver; and Rm is the distance coupling matrix between the receiving antenna of the magnetic field receiver and the transmitting antenna of the magnetic field transmitter.

[0035] Furthermore, the attitude matrix Lm is:

[0036]

[0037] fPitch and fRoll are the azimuth, elevation, and roll angles of the receiving antenna, respectively.

[0038] Furthermore, the distance coupling matrix Rm is:

[0039]

[0040] In the formula, rx, ry, and rz are the X, Y, and Z axis position coordinates of the receiving antenna, respectively;

[0041] This invention can achieve at least one of the following beneficial effects:

[0042] This invention can simultaneously measure the position and attitude of multiple channel receivers, enabling real-time positioning and tracking of multiple targets.

[0043] This invention effectively improves the accuracy of position and attitude measurement in electromagnetic positioning and tracking by correcting the interaxial non-orthogonality of the transmitting and receiving antennas. Attached Figure Description

[0044] The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Throughout the drawings, the same reference numerals denote the same parts.

[0045] Figure 1 This is a block diagram illustrating the principle of the multi-channel six-degree-of-freedom electromagnetic positioning and tracking device in an embodiment of the present invention.

[0046] Figure 2 This is a block diagram illustrating the principle of the magnetic field transmitting end in an embodiment of the present invention;

[0047] Figure 3 This is a schematic diagram of the transmitting antenna frame structure in an embodiment of the present invention;

[0048] Figure 4 This is a block diagram illustrating the principle of the magnetic field receiver in an embodiment of the present invention.

[0049] Figure 5 This is a schematic diagram of the receiving antenna frame structure in an embodiment of the present invention;

[0050] Figure 6 This is a block diagram illustrating the principle of the integrated control terminal in this embodiment of the invention.

[0051] Figure 7 This is a diagram showing the coordinate relationship between the actual magnetic axis and the ideal orthogonal three axes of the antenna in this embodiment of the invention. Detailed Implementation

[0052] Preferred embodiments of the present invention will now be described in detail with reference to the accompanying drawings, which form part of this application and, together with the embodiments of the present invention, serve to illustrate the principles of the present invention.

[0053] One embodiment of the present invention discloses a multi-channel six-degree-of-freedom electromagnetic positioning and tracking device for robot navigation, such as... Figure 1 As shown, it includes: a magnetic field transmitter, multiple magnetic field receivers that move with the robot, and an integrated control terminal;

[0054] The magnetic field transmitting end is used to transmit triaxial magnetic field signals into the workspace in a time-division manner to form a dipole magnetic field; and to sample the time-domain data of the excitation current of the transmitting antenna;

[0055] The magnetic field receiving end is used to sense a three-axis magnetic field signal using a three-axis receiving antenna within the workspace; and to sample the time-domain data of the induced voltage of the receiving antenna.

[0056] The integrated control terminal is used to control the working timing of the magnetic field transmitter and the magnetic field receiver; receive and process the time-domain data of the excitation current and the time-domain data of the induced voltage; calculate the magnetic field vector matrix of the magnetic field receiver, correct the interaxial non-orthogonality of the transmitting and receiving antennas, calculate the position and attitude information of each receiving antenna in the transmitting antenna coordinate system, and realize the positioning and tracking of each magnetic field receiver that moves with the robot.

[0057] Specifically, such as Figure 2 As shown, the magnetic field transmitting end includes a transmitting excitation circuit and a transmitting antenna;

[0058] The transmitting antenna is a triaxial transmitting antenna, including a frame and three transmitting coils; the frame has a triaxial orthogonal square structure, such as... Figure 3 As shown; a transmitting coil is wound along each of the three axes;

[0059] The transmitting excitation circuit includes an excitation signal generation circuit, a switching circuit, and a current acquisition circuit;

[0060] The excitation signal generation circuit is used to generate a single-frequency sinusoidal voltage signal required for magnetic field emission under the excitation control of the integrated control terminal.

[0061] The switching circuit is used to excite each coil of the triaxial transmitting antenna in a time-division excitation manner according to a fixed timing sequence under the channel timing control of the integrated control terminal.

[0062] The current acquisition circuit is used to acquire time-domain data of the excitation current and output it to the integrated control terminal.

[0063] More specifically, the excitation signal generation circuit includes a DDS circuit, a filtering and gain circuit, and a power amplifier;

[0064] The DDS circuit is used to generate an initial sinusoidal voltage signal with the characteristic frequency required to excite the transmitting antenna under the excitation control of the integrated control terminal.

[0065] Filtering and gain circuitry is used to filter and gain-adjust the initial sinusoidal voltage signal to generate a low-noise single-frequency sinusoidal voltage signal with the required amplitude.

[0066] A power amplifier is used to amplify a low-noise single-frequency sinusoidal voltage signal to generate the single-frequency sinusoidal voltage signal required for magnetic field emission.

[0067] More specifically, the current acquisition circuit includes a current sampler, a current sampling conditioning circuit, and an ADC circuit;

[0068] The current sampler is used to sample the excitation current and output it to the sampling current conditioning circuit by means of a current sampling resistor; the current sampling resistor is connected to the power amplifier in the excitation signal generation circuit and is used to sample the excitation current.

[0069] The current sampling and conditioning circuit is used to condition the amplitude of the sampled signal of the excitation current.

[0070] The ADC circuit is used to perform analog-to-digital conversion on the amplitude-conditioned excitation current to obtain the time-domain data of the excitation current.

[0071] Specifically, such as Figure 4 As shown, the magnetic field receiving end includes a receiving processing circuit and a receiving antenna;

[0072] The receiving antenna is a triaxial receiving antenna, including a frame and three receiving coils; the frame has a triaxial orthogonal square structure, such as... Figure 5 As shown; a receiving coil is wound along each of the three axes;

[0073] The receiving processing circuit includes three receiving conditioning circuits and a receiving sampling circuit;

[0074] The three receiving conditioning circuits are each connected to a receiving coil and are used to condition and amplify the signal received by each receiving coil.

[0075] The receiving sampling circuit is used to perform analog-to-digital conversion on the conditioned and amplified received signal to obtain the time-domain data of the induced voltage under the receiving sampling control of the integrated control terminal.

[0076] Specifically, the integrated control terminal adopts an embedded software radio architecture including FPGA+DSP or FPGA+ARM, such as... Figure 6 The system communicates with the magnetic field transmitter, each magnetic field receiver, and the robot through corresponding interfaces.

[0077] The integrated control terminal includes a transmission excitation control module, a reception sampling control module, an excitation data calculation module, an induction data calculation module, a magnetic field vector matrix calculation module, a first correction module, a second correction module, a pose iteration calculation module, and corresponding data storage modules.

[0078] in,

[0079] The emission excitation control module is used to control the excitation and channel timing of the magnetic field transmitter, so that the magnetic field transmitter emits triaxial magnetic field signals into the workspace in a time-division manner to form a dipole magnetic field.

[0080] The receiving sampling control module is used to control the receiving and acquisition of the magnetic field receiving end of each channel, so that the receiving end samples the time-domain data of the induced voltage of the receiving antenna.

[0081] The excitation data calculation module is used to perform FFT calculation on the obtained excitation current time-domain data to obtain the excitation current amplitude and phase of the current transmitting axis, and to calculate the magnetic moment and current phase information of the transmitting antenna based on the excitation current amplitude and phase.

[0082] In the excitation data calculation module, the current amplitudes of the X, Y, and Z axes of the magnetic field transmitter obtained by FFT calculation during time-division excitation are denoted as Ix, Iy, and Iz, respectively, and the current phases are denoted as PhaIx, PhaIy, and PhaIz, respectively.

[0083] Based on the obtained current amplitudes Ix, Iy, and Iz, the magnetic moments of the transmitting antenna along the X, Y, and Z axes can be calculated as follows:

[0084] Mx=Ix*Areax, My=Iy*Areay, Mz=Iz*Areaz;

[0085] Areax, Areay, and Areaz represent the coil areas along the X, Y, and Z axes, respectively.

[0086] The magnetic moment matrix M for constructing the transmitting antenna is:

[0087]

[0088] The current phase matrix PhaI is:

[0089] PhaI=(PhaIx PhaIx PhaIx PhaIy PhaIy PhaIy PhaIz PhaIz PhaIz).

[0090] The inductive data calculation module corresponds to the magnetic field receiving module of each channel. It is used to perform FFT calculation on the time-domain data of the induced voltage obtained by the receiving antenna of each channel to obtain the amplitude and phase of the induced voltage of the receiving antenna.

[0091] For ease of description, the multiple receiving channels are denoted as R1 / R2 / R3 / ...R i …Taking channel R1 as an example (other measurement channels are similar), after the transmitting antenna completes one cycle of excitation, the amplitude matrix VoltR1 and phase matrix PhR1 of the induced voltage of each receiving antenna in channel R1 can be obtained.

[0092] Voltage amplitude matrix:

[0093] VoltR1=(VxTx VyTx VzTx VxTy VyTy VzTy VxTz VyTz VzTz);

[0094] in:

[0095] VxTx, VyTx, and VzTx represent the induced voltage amplitudes of the X-axis receiving antenna, Y-axis receiving antenna, and Z-axis receiving antenna of the R1 measurement channel when the transmitting antenna is excited along the X-axis.

[0096] VxTy, VyTy, and VzTy represent the induced voltage amplitudes of the X-axis receiving antenna, Y-axis receiving antenna, and Z-axis receiving antenna of the R1 measurement channel when the transmitting antenna is excited along the Y-axis.

[0097] VxTz, VyTz, and VzTz represent the induced voltage amplitudes of the X-axis, Y-axis, and Z-axis receiving antennas of the R1 measurement channel when the transmitting antenna is excited along the Z-axis.

[0098] Voltage phase matrix:

[0099] PhR1=(PhxTx PhyTx PhzTx PhxTy PhyTy PhzTy PhxTz PhyTz PhzTz);

[0100] in:

[0101] PhxTx, PhyTx, and PhzTx represent the phase of the induced voltage of the X-axis receiving antenna, Y-axis receiving antenna, and Z-axis receiving antenna of the R1 measurement channel when the transmitting antenna is excited along the X-axis.

[0102] PhxTy, PhyTy, and PhzTy represent the phases of the induced voltages of the X-axis receiving antenna, Y-axis receiving antenna, and Z-axis receiving antenna of the R1 measurement channel when the transmitting antenna is excited along the Y-axis.

[0103] PhxTz, PhyTz, and PhzTz represent the phases of the induced voltages of the X-axis, Y-axis, and Z-axis receiving antennas of the R1 measurement channel when the transmitting antenna is excited along the Z-axis.

[0104] The magnetic field vector matrix calculation module calculates the magnetic field strength matrix of the magnetic field measured by the receiving antenna based on the conditioning circuit gain, antenna sensitivity, and amplitude of the induced voltage of the receiving antenna on each channel's three axes; it determines the sign matrix of the magnetic field measured by the receiving antenna based on the phase difference between the induced voltage of the receiving antenna and the excitation current of the transmitting antenna; and it obtains the matrix of the measured magnetic field vector by multiplying the matrix elements at the same positions in the magnetic field strength matrix and the magnetic field sign matrix respectively.

[0105] Specifically, the methods for calculating the magnetic field vector matrix include:

[0106] 1) Calculate the magnetic field strength matrix of the magnetic field measured by the receiving antenna based on the conditioning circuit gain of the X, Y, and Z axes of the receiving antenna, the antenna sensitivity, and the amplitude of the induced voltage of the receiving antenna.

[0107] The gains of the conditioning circuits along the X, Y, and Z axes of the receiving antenna are denoted as GainRx, GainRy, and GainRz, respectively. The gain matrix of the receiving channel is then constructed as follows:

[0108] GainR1=(GainRx GainRy GainRz GainRx GainRy GainRz GainRx GainRyGainRz);

[0109] The sensitivities of the receiving antenna along the X, Y, and Z axes are denoted as SensRx, SensRy, and SensRz, respectively. The sensitivity matrix of the receiving antenna is then constructed as follows:

[0110] SensR1=(SensRx SensRy SensRz SensRx SensRy SensRz SensRx SensRySensRz);

[0111] Dividing the amplitude measurement result of the induced voltage of the receiving antenna, VoltR1, by the gain of each measurement channel and the sensitivity of the receiving antenna, yields the magnetic field strength matrix of the magnetic field measured by the receiving antenna.

[0112] Fn_R1=VoltR1. / GainR1. / SensR1.

[0113] 2) Determine the sign matrix of the magnetic field measured by the receiving antenna based on the phase difference between the induced voltage of the receiving antenna and the excitation current of the transmitting antenna;

[0114] Based on Faraday's law of electromagnetic induction, the phase difference between the induced voltage of the receiving antenna and the excitation current of the transmitting antenna is either +90° or -90°. Therefore, the sign matrix of the magnetic field measured by the receiving antenna is determined based on the phase information of the induced voltage of the receiving antenna and the phase information of the excitation current of the transmitting antenna as follows:

[0115] Fsgn_R1=sgn(sin(PhR1-PhaI));

[0116] Where “sgn” indicates the sign of the calculated value of the sine function. If the calculated value is greater than 0, the sign is “+1”, and if the calculated value is less than 0, the sign is “-1”.

[0117] 3) The matrix of the measured magnetic field vector can be obtained by multiplying the matrix elements in the same position in the magnetic field intensity matrix and the magnetic field symbol matrix respectively.

[0118] The matrix of the measured magnetic field vector is: F_R1 = Fn_R1.*Fsgn_R1;

[0119] F_R1 has the following form:

[0120] F_R1=(FxTx FyTx FzTx FxTy FyTy FzTy FxTz FyTz FzTz).

[0121] Ideally, transmitting and receiving antennas are orthogonal triaxial structures. However, errors inevitably exist in antenna frame fabrication and winding. Therefore, the magnetic axes of actual transmitting and receiving antennas are not ideally orthogonal triaxial structures, and a certain degree of interaxial non-orthogonality is unavoidable. This interaxial non-orthogonality severely affects the positioning accuracy of electromagnetic positioning devices. It is necessary to correct the interaxial non-orthogonality of both transmitting and receiving antennas simultaneously, converting the magnetic field measurement matrix into a magnetic field measurement matrix in an orthogonal coordinate system to ensure positioning and navigation accuracy.

[0122] The relationship between the antenna's actual magnetic axis and its ideal orthogonal triaxial axes is as follows: Figure 7As shown, let xyz be the actual magnetic axis coordinate system and x'y'z' be the orthogonal coordinate system. The angles u1, u2, and u3 represent the degree of non-orthogonality of the three magnetic axes of the actual antenna relative to the three ideal orthogonal magnetic axes.

[0123] Preferably, in this embodiment, the integrated control terminal stores the transformation matrix data from the orthogonal coordinate system to the non-orthogonal coordinate system for each antenna, and the number of transformation matrices is... Wherein, angles u1, u2, and u3 are the degrees of non-orthogonality of the actual three magnetic axes of each antenna relative to the ideally orthogonal three magnetic axes.

[0124] The first correction module obtains the magnetic field vector matrix of the orthogonal coordinate system by correcting the interaxial non-orthogonality of the receiving antenna by measuring the magnetic field vector matrix at the magnetic field receiving end of each channel.

[0125] The corrected formula is:

[0126]

[0127] In the formula, F'_R i Let F_i be the magnetic field vector matrix of the i-th channel's magnetic field receiver in the orthogonal coordinate system; FxTx', FxTy', and FxTz' are the magnetic field intensities of the receiving antenna of this channel's magnetic field receiver on the three axes of the orthogonal coordinate system, respectively; FxTx, FyTy, and FzTy are the measured magnetic field intensities of the receiving antenna of this channel's magnetic field receiver on the three axes of the non-orthogonal coordinate system, respectively; P_R i The transformation matrix from the orthogonal coordinate system to the non-orthogonal magnetic axis coordinate system of the receiving antenna at the i-th channel magnetic field receiving end; "Inverse" is the matrix inversion operation;

[0128] The corrected magnetic field strength on the three axes of the orthogonal coordinate system of channel R1:

[0129]

[0130] The second correction module is used to correct the interaxial non-orthogonality of the magnetic moment of the transmitting antenna to obtain the magnetic moment of the orthogonal coordinate system;

[0131] The corrected formula is:

[0132] In the formula, M is the magnetic moment matrix of the actual non-orthogonal coordinate system of the transmitting antenna; P_T is the transformation matrix of the transmitting antenna from the orthogonal coordinate system to the non-orthogonal magnetic axis coordinate system; angles u1, u2, and u3 characterize the degree of non-orthogonality of the three magnetic axes of the actual transmitting antenna relative to the three ideal orthogonal magnetic axes.

[0133] The pose iteration calculation module is used to establish a coupling equation based on the magnetic moment and magnetic field vector matrix of the transmitting antenna in the orthogonal coordinate system, and to calculate the position and orientation of the receiving antenna of the magnetic field receiving end by iteratively solving the equation.

[0134] The coupling equation F'_R i =Lm*Rm*M'; where F'_R i M' is the magnetic field vector matrix measured by the magnetic field receiver of the i-th channel in the orthogonal coordinate system; Lm is the magnetic moment of the transmitting antenna in the orthogonal coordinate system; Rm is the attitude matrix of the receiving antenna of the magnetic field receiver; and Rm is the distance coupling matrix between the receiving antenna of the magnetic field receiver and the transmitting antenna of the magnetic field transmitter.

[0135] Specifically, the attitude matrix Lm is:

[0136] In the formula, fYaw, fPitch, and fRoll are the azimuth, elevation, and roll angles of the receiving antenna, respectively.

[0137] Specifically, the distance coupling matrix Rm is:

[0138]

[0139] In the formula, rx, ry, and rz are the X, Y, and Z axis position coordinates of the receiving antenna, respectively;

[0140] By substituting the attitude matrix Lm and range coupling matrix Rm into the coupling equations of the receiving antenna's magnetic field and the transmitting antenna's magnetic moment, the receiver's position and attitude information can be obtained iteratively using a classical iterative algorithm for solving nonlinear equations (such as the gradient descent method). This enables six-degree-of-freedom electromagnetic positioning and tracking of the magnetic field receiver of receiving channel R1.

[0141] For other measurement channels, the method of this embodiment can be used to calculate the three-axis position and three-axis attitude information of the receiving antenna in the orthogonal coordinate system for each channel.

[0142] In summary, the embodiments of the present invention can simultaneously measure the position and attitude of multiple channel receivers, enabling real-time positioning and tracking of multiple targets. Furthermore, by correcting the interaxial non-orthogonality of the transmitting and receiving antennas, the accuracy of position and attitude measurement in electromagnetic positioning and tracking is effectively improved.

[0143] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. A multi-channel six-degree-of-freedom electromagnetic positioning and tracking device for robot navigation, characterized in that, include: One magnetic field transmitter, multiple magnetic field receivers that move with the robot, and an integrated control terminal; The magnetic field transmitting end is used to transmit triaxial magnetic field signals into the workspace in a time-division manner to form a dipole magnetic field; and to sample the time-domain data of the excitation current of the transmitting antenna; The magnetic field receiver is used to sense a three-axis magnetic field signal using a three-axis receiving antenna within the workspace. And sample the time-domain data of the induced voltage of the receiving antenna; The integrated control terminal is used to control the working timing of the magnetic field transmitter and the magnetic field receiver; and to receive and process the excitation current time-domain data and the induced voltage time-domain data. Calculate the magnetic field vector matrix of the magnetic field receiver, correct the interaxial non-orthogonality of the receiving and transmitting antennas, calculate the position and attitude information of each receiving antenna in the transmitting antenna coordinate system, and realize the positioning and tracking of each magnetic field receiver that moves with the robot. The transformation matrix from the orthogonal coordinate system to the non-orthogonal coordinate system for each antenna is stored in the integrated control terminal. Among them, angle , , The degree of non-orthogonality of the actual three magnetic axes of each antenna relative to the ideally orthogonal three magnetic axes; The integrated control terminal includes a first correction module and a second correction module; The first correction module obtains the magnetic field vector matrix in an orthogonal coordinate system by correcting the interaxial non-orthogonality of the receiving antenna on the magnetic field vector matrix measured at the magnetic field receiving end. The corrected formula is: ； In the formula, For the first i The magnetic field vector matrix of the magnetic field receiving end of the channel in an orthogonal coordinate system; , , These represent the magnetic field strength of the receiving antenna at the magnetic field receiving end of the channel on the three axes of the orthogonal coordinate system. , , These represent the measured magnetic field strength of the receiving antenna at the magnetic field receiving end of the channel on the three axes of a non-orthogonal coordinate system. For the first i The transformation matrix from the orthogonal coordinate system to the non-orthogonal coordinate system of the receiving antenna at the channel magnetic field receiver; "Inverse" is the matrix inversion operation; The second correction module is used to correct the interaxial non-orthogonality of the magnetic moment of the transmitting antenna to obtain the magnetic moment in an orthogonal coordinate system; the correction formula is: ; In the formula, M is the magnetic moment matrix of the transmitting antenna in the actual non-orthogonal coordinate system; This is the transformation matrix from the orthogonal coordinate system to the non-orthogonal coordinate system of the transmitting antenna.

2. The multi-channel six-degree-of-freedom electromagnetic positioning and tracking device according to claim 1, characterized in that, The magnetic field transmitting end includes a transmitting excitation circuit and a transmitting antenna; The transmitting antenna is a triaxial transmitting antenna, including a frame and three transmitting coils; the frame is a triaxial orthogonal square structure; and a transmitting coil is wound around each of the three axes. The transmitting excitation circuit includes an excitation signal generation circuit, a switching circuit, and a current acquisition circuit; The excitation signal generation circuit is used to generate a single-frequency sinusoidal voltage signal required for magnetic field emission under the excitation control of the integrated control terminal. The switching circuit is used to excite each coil of the triaxial transmitting antenna in a time-division excitation manner according to a fixed timing sequence under the channel timing control of the integrated control terminal. The current acquisition circuit is used to acquire time-domain data of the excitation current and output it to the integrated control terminal.

3. The multi-channel six-degree-of-freedom electromagnetic positioning and tracking device according to claim 1, characterized in that, The magnetic field receiving end includes a receiving and processing circuit and a receiving antenna; The receiving antenna is a triaxial receiving antenna, including a frame and three receiving coils; the frame is a triaxial orthogonal square structure; and a receiving coil is wound around each of the three axes. The receiving processing circuit includes three receiving conditioning circuits and a receiving sampling circuit; The three receiving conditioning circuits are each connected to a receiving coil and are used to condition and amplify the signal received by each receiving coil. The receiving sampling circuit is used to perform analog-to-digital conversion on the conditioned and amplified received signal to obtain the time-domain data of the induced voltage under the receiving sampling control of the integrated control terminal.

4. The multi-channel six-degree-of-freedom electromagnetic positioning and tracking device according to claim 1, characterized in that, The integrated control terminal includes an excitation data calculation module and a sensing data calculation module; The excitation data calculation module is used to perform FFT calculation on the obtained excitation current time-domain data to obtain the excitation current amplitude and phase of each axis of the current magnetic field transmitter, and to calculate the magnetic moment and current phase information of the transmitting antenna based on the excitation current amplitude and phase. The inductive data calculation module is used to perform FFT calculation on the time-domain data of the induced voltage obtained by the receiving antenna of each channel to obtain the amplitude and phase of the induced voltage of the receiving antenna.

5. The multi-channel six-degree-of-freedom electromagnetic positioning and tracking device according to claim 4, characterized in that, The integrated control terminal includes a magnetic field vector matrix calculation module. In the magnetic field vector matrix calculation module, the magnetic field strength matrix of the magnetic field measured by the receiving antenna is calculated based on the conditioning circuit gain of the three axes of the receiving antenna, the antenna sensitivity, and the amplitude of the induced voltage of the receiving antenna. The sign matrix of the magnetic field measured by the receiving antenna is determined based on the phase difference between the induced voltage of the receiving antenna and the excitation current of the transmitting antenna. The matrix of the measured magnetic field vector can be obtained by multiplying the matrix elements at the same position in the magnetic field strength matrix and the magnetic field sign matrix respectively.

6. The multi-channel six-degree-of-freedom electromagnetic positioning and tracking device according to claim 5, characterized in that, The integrated control terminal includes a pose iteration calculation module; The pose iteration calculation module is used to establish a coupling equation based on the magnetic moment and magnetic field vector matrix of the transmitting antenna in the orthogonal coordinate system, and to calculate the position and orientation of the receiving antenna of the magnetic field receiving end by iteratively solving the equation. The coupling equation ;in, In an orthogonal coordinate system, the first i The magnetic field vector matrix of the magnetic field receiving end of the channel; Lm is the magnetic moment of the transmitting antenna in the orthogonal coordinate system; Rm is the attitude matrix of the receiving antenna of the magnetic field receiving end; and Rm is the distance coupling matrix between the receiving antenna of the magnetic field receiving end and the transmitting antenna of the magnetic field transmitting end.

7. The multi-channel six-degree-of-freedom electromagnetic positioning and tracking device according to claim 6, characterized in that, The attitude matrix Lm is: In the formula, fYaw, fPitch, and fRoll are the azimuth, elevation, and roll angles of the receiving antenna, respectively.

8. The multi-channel six-degree-of-freedom electromagnetic positioning and tracking device according to claim 6, characterized in that, The distance coupling matrix Rm is: ； In the formula, rx, ry, and rz are the X, Y, and Z axis position coordinates of the receiving antenna, respectively; .

Images