CN116633037A - 一种用于机器人导航的多通道六自由度电磁定位跟踪装置 - Google Patents
一种用于机器人导航的多通道六自由度电磁定位跟踪装置 Download PDFInfo
- Publication number
- CN116633037A CN116633037A CN202210132787.1A CN202210132787A CN116633037A CN 116633037 A CN116633037 A CN 116633037A CN 202210132787 A CN202210132787 A CN 202210132787A CN 116633037 A CN116633037 A CN 116633037A
- Authority
- CN
- China
- Prior art keywords
- magnetic field
- receiving
- antenna
- transmitting
- matrix
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 239000011159 matrix material Substances 0.000 claims abstract description 92
- 239000013598 vector Substances 0.000 claims abstract description 31
- 238000005070 sampling Methods 0.000 claims abstract description 27
- 230000001939 inductive effect Effects 0.000 claims abstract description 4
- 230000005284 excitation Effects 0.000 claims description 53
- 238000004364 calculation method Methods 0.000 claims description 30
- 230000006698 induction Effects 0.000 claims description 21
- 230000003750 conditioning effect Effects 0.000 claims description 19
- 238000012937 correction Methods 0.000 claims description 18
- 230000008878 coupling Effects 0.000 claims description 14
- 238000010168 coupling process Methods 0.000 claims description 14
- 238000005859 coupling reaction Methods 0.000 claims description 14
- 238000006243 chemical reaction Methods 0.000 claims description 11
- 238000012545 processing Methods 0.000 claims description 10
- 230000007274 generation of a signal involved in cell-cell signaling Effects 0.000 claims description 8
- 230000035945 sensitivity Effects 0.000 claims description 7
- 238000004804 winding Methods 0.000 claims description 7
- 230000001143 conditioned effect Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 238000005259 measurement Methods 0.000 abstract description 7
- 238000010586 diagram Methods 0.000 description 6
- 230000036544 posture Effects 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 4
- 238000001914 filtration Methods 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 239000004973 liquid crystal related substance Substances 0.000 description 2
- 238000000034 method Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 125000004122 cyclic group Chemical group 0.000 description 1
- 238000013500 data storage Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000005674 electromagnetic induction Effects 0.000 description 1
- 238000011478 gradient descent method Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- H—ELECTRICITY
- H02—GENERATION; CONVERSION OR DISTRIBUTION OF ELECTRIC POWER
- H02J—CIRCUIT ARRANGEMENTS OR SYSTEMS FOR SUPPLYING OR DISTRIBUTING ELECTRIC POWER; SYSTEMS FOR STORING ELECTRIC ENERGY
- H02J50/00—Circuit arrangements or systems for wireless supply or distribution of electric power
- H02J50/40—Circuit arrangements or systems for wireless supply or distribution of electric power using two or more transmitting or receiving devices
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
- G01S5/0205—Details
- G01S5/021—Calibration, monitoring or correction
-
- H—ELECTRICITY
- H02—GENERATION; CONVERSION OR DISTRIBUTION OF ELECTRIC POWER
- H02J—CIRCUIT ARRANGEMENTS OR SYSTEMS FOR SUPPLYING OR DISTRIBUTING ELECTRIC POWER; SYSTEMS FOR STORING ELECTRIC ENERGY
- H02J50/00—Circuit arrangements or systems for wireless supply or distribution of electric power
- H02J50/20—Circuit arrangements or systems for wireless supply or distribution of electric power using microwaves or radio frequency waves
-
- H—ELECTRICITY
- H02—GENERATION; CONVERSION OR DISTRIBUTION OF ELECTRIC POWER
- H02J—CIRCUIT ARRANGEMENTS OR SYSTEMS FOR SUPPLYING OR DISTRIBUTING ELECTRIC POWER; SYSTEMS FOR STORING ELECTRIC ENERGY
- H02J50/00—Circuit arrangements or systems for wireless supply or distribution of electric power
- H02J50/60—Circuit arrangements or systems for wireless supply or distribution of electric power responsive to the presence of foreign objects, e.g. detection of living beings
-
- H—ELECTRICITY
- H02—GENERATION; CONVERSION OR DISTRIBUTION OF ELECTRIC POWER
- H02J—CIRCUIT ARRANGEMENTS OR SYSTEMS FOR SUPPLYING OR DISTRIBUTING ELECTRIC POWER; SYSTEMS FOR STORING ELECTRIC ENERGY
- H02J50/00—Circuit arrangements or systems for wireless supply or distribution of electric power
- H02J50/90—Circuit arrangements or systems for wireless supply or distribution of electric power involving detection or optimisation of position, e.g. alignment
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/30—Assessment of water resources
Landscapes
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Power Engineering (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Variable-Direction Aerials And Aerial Arrays (AREA)
Abstract
本发明涉及一种用于机器人导航的多通道六自由度电磁定位跟踪装置，包括：一个磁场发射端、多个磁场接收端和综合控制终端；磁场发射端用于向工作空间内分时发射三轴磁场信号；并采样发射天线的激励电流时域数据；磁场接收端用于感应三轴磁场信号；并采样接收天线的感应电压时域数据；综合控制终端，用于控制磁场发射端和磁场接收端的工作时序；接收激励电流时域数据和感应电压时域数据；计算磁场接收端的磁场矢量矩阵，修正收发天线的轴间不正交度，计算每个接收天线在发射天线坐标系下的位置信息和姿态信息，对每个磁场接收端进行定位跟踪。本发明实现多个目标的实时定位跟踪，并有效提高了电磁定位跟踪的位置和姿态测量准确度。
Description
技术领域
本发明属于电磁定位技术领域，具体涉及一种用于机器人导航的多通道六自由度电磁定位跟踪装置。
背景技术
随着科技的不断发展，定位技术在辅助医疗、人机交互、运动检测分析等领域应用广泛。与超声式、光电式等定位装置相比，基于六自由度电磁定位技术设计的装置具有速度快、成本低、便携性好等优点。六自由度电磁定位装置追踪目标的运动轨迹的同时，还可以对目标姿态精确测量，最重要的是定位不受障碍物遮挡影响。这些优点使多目标六自由度电磁定位装置可以很好的满足研究需求。
但目前的六自由度电磁定位技术均是基于理想正交磁场的发射接收，而实际的磁场由于磁场发射天线和接收天线无法达到理想的正交状态，因此采用基于理想正交磁场在解决实际中的电磁定位会影响定位跟踪的精度。
发明内容
鉴于上述的分析，本发明旨在公开了一种用于机器人导航的多通道六自由度电磁定位跟踪装置，实现多通道的高精度电磁定位跟踪。
本发明公开了一种用于机器人导航的多通道六自由度电磁定位跟踪装置，包括：一个磁场发射端、多个随机器人运动的磁场接收端和综合控制终端；
所述磁场发射端，用于向工作空间内分时发射三轴磁场信号，形成偶极子磁场；并采样发射天线的激励电流时域数据；
所述磁场接收端，用于在工作空间内采用三轴接收天线感应三轴磁场信号；并采样接收天线的感应电压时域数据；
所述综合控制终端，用于控制磁场发射端和磁场接收端的工作时序；接收并处理所述激励电流时域数据和感应电压时域数据；计算磁场接收端的磁场矢量矩阵，修正收、发天线的轴间不正交度，计算每个接收天线在发射天线坐标系下的位置信息和姿态信息，实现对每个随机器人运动的磁场接收端的定位跟踪。
进一步地，所述磁场发射端包括发射激励电路和发射天线；
所述发射天线为三轴发射天线，包括骨架和三个发射线圈；所述骨架为三轴正交方型结构；在三个轴向上分别缠绕一个发射线圈；
发射激励电路包括激励信号产生电路、开关电路和电流采集电路；
所述激励信号产生电路，用于在综合控制终端的激励控制下，产生磁场发射所需的单频正弦电压信号；
所述开关电路，用于在综合控制终端的通道时序控制下，按固定时序以分时激励的方式分时激励三轴发射天线的每个线圈；
所述电流采集电路，用于采集激励电流时域数据输出到综合控制终端。
进一步地，所述磁场接收端包括接收处理电路和接收天线；
所述接收天线为三轴接收天线，包括骨架和三个接收线圈；所述骨架为三轴正交方型结构；在三个轴向上分别缠绕一个接收线圈；
接收处理电路包括三个接收调理电路和接收采样电路；
所述三个接收调理电路分别与一个接收线圈连接，用于对每个接收线圈接收的信号进行调理放大；
所述接收采样电路，用于在综合控制终端的接收采样控制下，对经过调理放大的接收信号进行模数变换得到感应电压的时域数据。
进一步地，所述综合控制终端包括激励数据计算模块和感应数据计算模块；
激励数据计算模块，用于对所得激励电流时域数据进行FFT计算获得当前发射轴的激励电流幅值和相位，并根据激励电流幅值和相位计算发射天线的磁矩和电流相位信息；
感应数据计算模块，用于对每个通道的接收天线所得感应电压的时域数据进行FFT计算，获得接收天线的感应电压幅值和相位。
进一步地，所述综合控制终端中包括磁场矢量矩阵计算模块，在磁场矢量矩阵计算模块中，根据接收天线三轴的调理电路增益、天线灵敏度和接收天线感应电压的幅值，计算接收天线所测磁场的磁场强度矩阵；基于接收天线感应电压和发射天线激励电流的相位差确定接收天线所测磁场的符号矩阵；磁场强度矩阵和磁场符号矩阵中相同位置的矩阵元分别相乘后可以得到所测磁场矢量的矩阵。
进一步地，所述综合控制终端中存储的每个天线的正交坐标系向非正交坐标系的转换矩阵其中，角度u1、u2、u3为每个天线的实际三个磁轴相对于理想正交的三个磁轴的不正交度。
进一步地，所述综合控制终端中包括第一修正模块和第二修正模块；
所述第一修正模块，通过对磁场接收端测量的磁场矢量矩阵进行接收天线的轴间不正交度修正得到正交坐标系的磁场矢量矩阵；
修正公式为：
式中，F'_Ri为第i通道的磁场接收端在正交坐标系的磁场矢量矩阵；FxTx'、FxTy'、FxTz'分别为该通道磁场接收端的接收天线在正交坐标系三轴上的磁场强度；FxTx、FyTy、FzTy分别为该通道磁场接收端的接收天线在非正交坐标系三轴上的测量磁场强度；P_Ri第i通道磁场接收端的接收天线的正交坐标系向非正交磁轴坐标系的转换矩阵；“Inverse”为对矩阵取逆运算；
所述第二修正模块，用于对发射天线的磁矩进行轴间不正交度修正得到正交坐标系的磁矩；修正公式为：M'＝Transpose(P_T)*M；
式中，M为发射天线实际非正交坐标系下的磁矩矩阵；P_T为发射天线的正交坐标系向非正交磁轴坐标系的转换矩阵。
进一步地，所述综合控制终端中包括位姿迭代计算模块；
所述位姿迭代计算模块，用于根据正交坐标系下发射天线的磁矩和磁场矢量矩阵建立耦合方程，通过迭代求解计算磁场接收端的接收天线的位置和姿态；
所述耦合方程F'_Ri＝Lm*Rm*M'；其中，F'_Ri为正交坐标系下第i通道的磁场接收端测量的磁场矢量矩阵；M'为正交坐标系下发射天线的磁矩；Lm为所述磁场接收端的接收天线姿态矩阵，Rm为所述磁场接收端的接收天线和磁场发射端的发射天线的距离耦合矩阵。
进一步地，姿态矩阵Lm为：
fPitch、fRoll分别为接收天线的方位角、俯仰角、横滚角。
进一步地，距离耦合矩阵Rm为：
式中，rx、ry、rz分别为接收天线的X、Y、Z轴位置坐标；
本发明至少可实现以下有益效果之一：
本发明可以同时测量多个通道接收端的位置和姿态，实现多个目标的实时定位跟踪。
本发明通过对发射天线和接收天线的轴间不正交度进行修正，有效提高了电磁定位跟踪的位置和姿态测量准确度。
附图说明
附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。
图1为本发明实施例中的多通道六自由度电磁定位跟踪装置原理框图；
图2为本发明实施例中的磁场发射端原理框图；
图3为本发明实施例中的发射天线骨架结构示意图；
图4为本发明实施例中的磁场接收端原理框图；
图5为本发明实施例中的接收天线骨架结构示意图；
图6为本发明实施例中的综合控制终端原理框图；
图7为本发明实施例中的天线的实际磁轴和理想正交三轴之间的坐标关系图。
具体实施方式
下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理。
本发明的一个实施例公开了一种用于机器人导航的多通道六自由度电磁定位跟踪装置，如图1所示，包括：一个磁场发射端、多个随机器人运动的磁场接收端和综合控制终端；
所述磁场发射端，用于向工作空间内分时发射三轴磁场信号，形成偶极子磁场；并采样发射天线的激励电流时域数据；
所述磁场接收端，用于在工作空间内采用三轴接收天线感应三轴磁场信号；并采样接收天线的感应电压时域数据；
所述综合控制终端，用于控制磁场发射端和磁场接收端的工作时序；接收并处理所述激励电流时域数据和感应电压时域数据；计算磁场接收端的磁场矢量矩阵，修正收、发天线的轴间不正交度，计算每个接收天线在发射天线坐标系下的位置信息和姿态信息，实现对每个随机器人运动的磁场接收端的定位跟踪。
具体的，如图2所示，所述磁场发射端包括发射激励电路和发射天线；
所述发射天线为三轴发射天线，包括骨架和三个发射线圈；所述骨架为三轴正交方型结构，如图3所示；在三个轴向上分别缠绕一个发射线圈；
发射激励电路包括激励信号产生电路、开关电路和电流采集电路；
所述激励信号产生电路，用于在综合控制终端的激励控制下，产生磁场发射所需的单频正弦电压信号；
所述开关电路，用于在综合控制终端的通道时序控制下，按固定时序以分时激励的方式分时激励三轴发射天线的每个线圈；
所述电流采集电路，用于采集激励电流时域数据输出到综合控制终端。
更具体的，所述激励信号产生电路包括DDS电路、滤波及增益电路、功率放大器；
DDS电路，用于在综合控制终端的激励控制下，产生激励发射天线所需的特征频率的初始正弦电压信号；
滤波和增益电路，用于对初始正弦电压信号进行滤波和增益调节产生所需幅值的低噪声单频正弦电压信号；
功率放大器，用于对低噪声单频正弦电压信号进行功率放大产生磁场发射所需的单频正弦电压信号。
更具体的，所述电流采集电路包括电流采样器、电流采样调理电路以及ADC电路；
所述电流采样器，用于采用电流采样电阻的方式，采样激励电流输出到采样电流调理电路；所述电流采样电阻与激励信号产生电路中的功率放大器连接，用于采激励电流。
所述电流采样调理电路，用于对激励电流的采样信号进行幅值调理；
所述ADC电路，用于对幅值调理后的激励电流进行模数变换得到激励电流时域数据。
具体的，如图4所示，所述磁场接收端包括接收处理电路和接收天线；
所述接收天线为三轴接收天线，包括骨架和三个接收线圈；所述骨架为三轴正交方型结构，如图5所示；在三个轴向上分别缠绕一个接收线圈；
接收处理电路包括三个接收调理电路和接收采样电路；
所述三个接收调理电路分别与一个接收线圈连接，用于对每个接收线圈接收的信号进行调理放大；
所述接收采样电路，用于在综合控制终端的接收采样控制下，对经过调理放大的接收信号进行模数变换得到感应电压的时域数据。
具体的，所述综合控制终端采用包括FPGA+DSP或FPGA+ARM在内的嵌入式软件无线电结构，如图6所述，通过相应的接口与磁场发射端、每个磁场接收端和机器人进行通信连接。
其中，综合控制终端包括发射激励控制模块、接收采样控制模块、激励数据计算模块、感应数据计算模块、磁场矢量矩阵计算模块、第一修正模块、第二修正模块、位姿迭代计算模块以及相应的数据存储模块；
其中，
所述发射激励控制模块，用于对所述磁场发射端的激励和通道时序进行控制，使磁场发射端向工作空间内分时发射三轴磁场信号，形成偶极子磁场。
所述接收采样控制模块，用于对每个通道的所述磁场接收端进行接收采集控制，使接收端采样接收天线的感应电压时域数据。
激励数据计算模块，用于对所得激励电流时域数据进行FFT计算获得当前发射轴的激励电流幅值和相位，并根据激励电流幅值和相位计算发射天线的磁矩和电流相位信息；
在激励数据计算模块中，经FFT计算得到的磁场发射端X、Y、Z轴分时激励时的电流幅值分别记为Ix、Iy、Iz，电流相位分别记为PhaIx、PhaIy、PhaIz；
基于所得电流幅值Ix、Iy、Iz，可以计算得到发射天线的X、Y、Z轴的磁矩如下：
Mx＝Ix*Areax,My＝Iy*Areay,Mz＝Iz*Areaz；
Areax、Areay、Areaz分别为X轴、Y轴、Z轴的线圈面积。
构建发射天线的磁矩矩阵M为：
电流相位矩阵PhaI为：
PhaI＝(PhaIx PhaIx PhaIx PhaIy PhaIy PhaIy PhaIz PhaIz PhaIz)。
感应数据计算模块与每个通道的磁场接收模块对应，用于对每个通道的接收天线所得感应电压的时域数据进行FFT计算，获得接收天线的感应电压幅值和相位。
为方便叙述多个接收通道分别记为R1/R2/R3/…Ri…，则以R1通道为例(其他测量通道类似)，在发射天线完成一次循环激励后，可以得到R1通道各接收天线感应电压的幅值矩阵VoltR1和相位矩阵PhR1。
电压幅值矩阵：
VoltR1＝(VxTx VyTx VzTx VxTy VyTy VzTy VxTz VyTz VzTz)；
其中：
VxTx、VyTx、VzTx分别表示发射天线的X轴激励时，R1测量通道的X轴接收天线、Y轴接收天线、Z轴接收天线的感应电压幅值；
VxTy、VyTy、VzTy分别表示发射天线的Y轴激励时，R1测量通道的X轴接收天线、Y轴接收天线、Z轴接收天线的感应电压幅值；
VxTz、VyTz、VzTz分别表示发射天线的Z轴激励时，R1测量通道的X轴接收天线、Y轴接收天线、Z轴接收天线的感应电压幅值。
电压相位矩阵：
PhR1＝(PhxTx PhyTx PhzTx PhxTy PhyTy PhzTy PhxTz PhyTz PhzTz)；
其中：
PhxTx、PhyTx、PhzTx分别表示发射天线的X轴激励时，R1测量通道的X轴接收天线、Y轴接收天线、Z轴接收天线的感应电压的相位；
PhxTy、PhyTy、PhzTy分别表示发射天线的Y轴激励时，R1测量通道的X轴接收天线、Y轴接收天线、Z轴接收天线的感应电压的相位；
PhxTz、PhyTz、PhzTz分别表示发射天线的Z轴激励时，R1测量通道的X轴接收天线、Y轴接收天线、Z轴接收天线的感应电压的相位。
所述磁场矢量矩阵计算模块，在磁场矢量矩阵计算模块中，根据每个通道的接收天线三轴的调理电路增益、天线灵敏度和接收天线感应电压的幅值，计算接收天线所测磁场的磁场强度矩阵；基于接收天线感应电压和发射天线激励电流的相位差确定接收天线所测磁场的符号矩阵；磁场强度矩阵和磁场符号矩阵中相同位置的矩阵元分别相乘后可以得到所测磁场矢量的矩阵。
具体的，磁场矢量矩阵的计算方法包括：
1)根据接收天线X、Y、Z轴的调理电路增益、天线灵敏度和接收天线感应电压的幅值，计算接收天线所测磁场的磁场强度矩阵；
接收天线X、Y、Z轴的调理电路的增益分别记为GainRx、GainRy、GainRz，构建接收通道的增益矩阵：
GainR1＝(GainRx GainRy GainRz GainRx GainRy GainRz GainRx GainRyGainRz)；
接收天线X、Y、Z轴的灵敏度分别记为SensRx、SensRy、SensRz，构建接收天线的灵敏度矩阵：
SensR1＝(SensRx SensRy SensRz SensRx SensRy SensRz SensRx SensRySensRz)；
接收天线感应电压的幅值测量结果VoltR1除以各自测量通道的增益和接收天线灵敏度，即得到接收天线所测磁场的磁场强度矩阵：
Fn_R1＝VoltR1./GainR1./SensR1。
2)基于接收天线感应电压和发射天线激励电流的相位差确定接收天线所测磁场的符号矩阵；
基于法拉第电磁感应定律，接收天线感应电压的相位相对发射天线激励电流的相位差值为“+90°”或“-90°”。因此，基于接收天线感应电压的相位信息和发射天线激励电流的相位信息确定接收天线所测磁场的符号矩阵如下：
Fsgn_R1＝sgn(sin(PhR1-PhaI))；
其中“sgn”表示对正弦函数的计算值取符号，计算值大于0，符号取“+1”，计算值小于0，符号取“-1”。
3)磁场强度矩阵和磁场符号矩阵中相同位置的矩阵元分别相乘后可以得到所测磁场矢量的矩阵。
所测磁场矢量的矩阵：F_R1＝Fn_R1.*Fsgn_R1；
F_R1具有如下形式：
F_R1＝(FxTx FyTx FzTx FxTy FyTy FzTy FxTz FyTz FzTz)。
理想的发射天线和接收天线为正交三轴结构，但天线骨架加工和天线绕制必然存在误差，因此实际的发射天线和接收天线的磁轴不是理想的正交三轴结构，必然存在一定的轴间不正交度。天线的轴间不正交度严重影响电磁定位装置的定位精度，必须同时对发射天线和接收天线的轴间不正交度进行修正，将磁场测量矩阵转换为正交坐标系下的磁场测量矩阵才能保证定位导航精度。
天线的实际磁轴和理想正交三轴之间的关系如图7所示，设xyz为实际磁轴坐标系，x'y'z'为正交坐标系，用角度u1、u2、u3表征了实际天线三个磁轴相对于理想的正交的三个磁轴的不正交度。
优选的，在本实施例的综合控制终端中存储每个天线的正交坐标系向非正交坐标系的转换矩阵数据，转换矩阵数其中，角度u1、u2、u3为每个天线的实际三个磁轴相对于理想正交的三个磁轴的不正交度。
所述第一修正模块，通过对每个通道的磁场接收端测量的磁场矢量矩阵进行接收天线的轴间不正交度修正得到正交坐标系的磁场矢量矩阵；
修正公式为：
式中，F'_Ri为第i通道的磁场接收端在正交坐标系的磁场矢量矩阵；FxTx'、FxTy'、FxTz'分别为该通道磁场接收端的接收天线在正交坐标系三轴上的磁场强度；FxTx、FyTy、FzTy分别为该通道磁场接收端的接收天线在非正交坐标系三轴上的测量磁场强度；P_Ri第i通道磁场接收端的接收天线的正交坐标系向非正交磁轴坐标系的转换矩阵；“Inverse”为对矩阵取逆运算；
R1通道的修正后的正交坐标系三轴上的磁场强度：
所述第二修正模块，用于对发射天线的磁矩进行轴间不正交度修正得到正交坐标系的磁矩；
修正公式为：
式中，M为发射天线实际非正交坐标系下的磁矩矩阵；P_T为发射天线的正交坐标系向非正交磁轴坐标系的转换矩阵；角度u1、u2、u3表征了实际发射天线三个磁轴相对于理想的正交的三个磁轴的不正交度。
所述位姿迭代计算模块，用于根据正交坐标系下发射天线的磁矩和磁场矢量矩阵建立耦合方程，通过迭代求解计算磁场接收端的接收天线的位置和姿态；
所述耦合方程F'_Ri＝Lm*Rm*M'；其中，F'_Ri为正交坐标系下第i通道的磁场接收端测量的磁场矢量矩阵；M'为正交坐标系下发射天线的磁矩；Lm为所述磁场接收端的接收天线姿态矩阵，Rm为所述磁场接收端的接收天线和磁场发射端的发射天线的距离耦合矩阵。
具体的，姿态矩阵Lm为：
式中，fYaw、fPitch、fRoll分别为接收天线的方位角、俯仰角、横滚角。
具体的，距离耦合矩阵Rm为：
式中，rx、ry、rz分别为接收天线的X、Y、Z轴位置坐标；
将姿态矩阵Lm和距离耦合矩阵Rm代入接收天线磁场和发射天线磁矩的耦合方程，采用求解非线性方程组的经典迭代算法(例如梯度下降法)即可迭代求解得到接收器的位置和姿态信息。从而实现对接收通道R1的磁场接收端的六自由度电磁定位跟踪。
对于其他的测量通道，均可以采用本实施例的方法计算得到每个通道接收天线在正交坐标系下的三轴位置和三轴姿态信息。
综上所述，本发明实施例可以同时测量多个通道接收端的位置和姿态，实现多个目标的实时定位跟踪。并通过对发射天线和接收天线的轴间不正交度进行修正，有效提高了电磁定位跟踪的位置和姿态测量准确度。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。
Claims (10)
1.一种用于机器人导航的多通道六自由度电磁定位跟踪装置，其特征在于，包括：一个磁场发射端、多个随机器人运动的磁场接收端和综合控制终端；
所述磁场发射端，用于向工作空间内分时发射三轴磁场信号，形成偶极子磁场；并采样发射天线的激励电流时域数据；
所述磁场接收端，用于在工作空间内采用三轴接收天线感应三轴磁场信号；并采样接收天线的感应电压时域数据；
所述综合控制终端，用于控制磁场发射端和磁场接收端的工作时序；接收并处理所述激励电流时域数据和感应电压时域数据；计算磁场接收端的磁场矢量矩阵，修正收、发天线的轴间不正交度，计算每个接收天线在发射天线坐标系下的位置信息和姿态信息，实现对每个随机器人运动的磁场接收端的定位跟踪。
2.根据权利要求1所述的多通道六自由度电磁定位跟踪装置，其特征在于，
所述磁场发射端包括发射激励电路和发射天线；
所述发射天线为三轴发射天线，包括骨架和三个发射线圈；所述骨架为三轴正交方型结构；在三个轴向上分别缠绕一个发射线圈；
发射激励电路包括激励信号产生电路、开关电路和电流采集电路；
所述激励信号产生电路，用于在综合控制终端的激励控制下，产生磁场发射所需的单频正弦电压信号；
所述开关电路，用于在综合控制终端的通道时序控制下，按固定时序以分时激励的方式分时激励三轴发射天线的每个线圈；
所述电流采集电路，用于采集激励电流时域数据输出到综合控制终端。
3.根据权利要求1所述的多通道六自由度电磁定位跟踪装置，其特征在于，所述磁场接收端包括接收处理电路和接收天线；
所述接收天线为三轴接收天线，包括骨架和三个接收线圈；所述骨架为三轴正交方型结构；在三个轴向上分别缠绕一个接收线圈；
接收处理电路包括三个接收调理电路和接收采样电路；
所述三个接收调理电路分别与一个接收线圈连接，用于对每个接收线圈接收的信号进行调理放大；
所述接收采样电路，用于在综合控制终端的接收采样控制下，对经过调理放大的接收信号进行模数变换得到感应电压的时域数据。
4.根据权利要求1所述的多通道六自由度电磁定位跟踪装置，其特征在于，所述综合控制终端包括激励数据计算模块和感应数据计算模块；
激励数据计算模块，用于对所得激励电流时域数据进行FFT计算获得当前发射轴的激励电流幅值和相位，并根据激励电流幅值和相位计算发射天线的磁矩和电流相位信息；
感应数据计算模块，用于对每个通道的接收天线所得感应电压的时域数据进行FFT计算，获得接收天线的感应电压幅值和相位。
5.根据权利要求4所述的多通道六自由度电磁定位跟踪装置，其特征在于，
所述综合控制终端中包括磁场矢量矩阵计算模块，在磁场矢量矩阵计算模块中，根据接收天线三轴的调理电路增益、天线灵敏度和接收天线感应电压的幅值，计算接收天线所测磁场的磁场强度矩阵；基于接收天线感应电压和发射天线激励电流的相位差确定接收天线所测磁场的符号矩阵；磁场强度矩阵和磁场符号矩阵中相同位置的矩阵元分别相乘后可以得到所测磁场矢量的矩阵。
6.根据权利要求5所述的多通道六自由度电磁定位跟踪装置，其特征在于，
所述综合控制终端中存储的每个天线的正交坐标系向非正交坐标系的转换矩阵其中，角度u1、u2、u3为每个天线的实际三个磁轴相对于理想正交的三个磁轴的不正交度。
7.根据权利要求4所述的多通道六自由度电磁定位跟踪装置，其特征在于，所述综合控制终端中包括第一修正模块和第二修正模块；
所述第一修正模块，通过对磁场接收端测量的磁场矢量矩阵进行接收天线的轴间不正交度修正得到正交坐标系的磁场矢量矩阵；
修正公式为：
式中，F'_Ri为第i通道的磁场接收端在正交坐标系的磁场矢量矩阵；FxTx'、FxTy'、FxTz'分别为该通道磁场接收端的接收天线在正交坐标系三轴上的磁场强度；FxTx、FyTy、FzTy分别为该通道磁场接收端的接收天线在非正交坐标系三轴上的测量磁场强度；P_Ri第i通道磁场接收端的接收天线的正交坐标系向非正交磁轴坐标系的转换矩阵；“Inverse”为对矩阵取逆运算；
所述第二修正模块，用于对发射天线的磁矩进行轴间不正交度修正得到正交坐标系的磁矩；修正公式为：M'＝Transpose(P_T)*M；
式中，M为发射天线实际非正交坐标系下的磁矩矩阵；P_T为发射天线的正交坐标系向非正交磁轴坐标系的转换矩阵。
8.根据权利要求7所述的多通道六自由度电磁定位跟踪装置，其特征在于，所述综合控制终端中包括位姿迭代计算模块；
所述位姿迭代计算模块，用于根据正交坐标系下发射天线的磁矩和磁场矢量矩阵建立耦合方程，通过迭代求解计算磁场接收端的接收天线的位置和姿态；
所述耦合方程F'_Ri＝Lm*Rm*M'；其中，F'_Ri为正交坐标系下第i通道的磁场接收端测量的磁场矢量矩阵；M'为正交坐标系下发射天线的磁矩；Lm为所述磁场接收端的接收天线姿态矩阵，Rm为所述磁场接收端的接收天线和磁场发射端的发射天线的距离耦合矩阵。
9.根据权利要求8所述的多通道六自由度电磁定位跟踪装置，其特征在于，姿态矩阵Lm为：
式中，fYaw、fPitch、fRoll分别为接收天线的方位角、俯仰角、横滚角。
10.根据权利要求8所述的多通道六自由度电磁定位跟踪装置，其特征在于，距离耦合矩阵Rm为：
式中，rx、ry、rz分别为接收天线的X、Y、Z轴位置坐标；
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210132787.1A CN116633037A (zh) | 2022-02-14 | 2022-02-14 | 一种用于机器人导航的多通道六自由度电磁定位跟踪装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210132787.1A CN116633037A (zh) | 2022-02-14 | 2022-02-14 | 一种用于机器人导航的多通道六自由度电磁定位跟踪装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116633037A true CN116633037A (zh) | 2023-08-22 |
Family
ID=87608655
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210132787.1A Pending CN116633037A (zh) | 2022-02-14 | 2022-02-14 | 一种用于机器人导航的多通道六自由度电磁定位跟踪装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116633037A (zh) |
-
2022
- 2022-02-14 CN CN202210132787.1A patent/CN116633037A/zh active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP0581434B1 (en) | Compensation method for an electromagnetic remote position and orientation sensor | |
| JPH08512125A (ja) | 妨害金属がある場所で物体の位置と向きを測定する方法および装置 | |
| US5646525A (en) | Three dimensional tracking system employing a rotating field | |
| CN106643792B (zh) | 惯性测量单元和地磁传感器整体标定装置及标定方法 | |
| EP0576187B1 (en) | Tracker employing a rotating electromagnetic field | |
| CA1164986A (en) | Locator for source of electromagnetic radiation having unknown structure or orientation | |
| US20040088136A1 (en) | Method of measuring position and orientation with improved signal to noise ratio | |
| Hehn et al. | High-accuracy localization and calibration for 5-DoF indoor magnetic positioning systems | |
| US10132636B2 (en) | Hybrid inertial/magnetic system for determining the position and orientation of a mobile body | |
| JPS5991311A (ja) | 電子コンパスを有する乗物ナビゲーション装置 | |
| CN106950528B (zh) | 一种基于线性调频信号的波达方向估计方法 | |
| CN114509069B (zh) | 基于蓝牙aoa和imu融合的室内导航定位*** | |
| JP2011033609A (ja) | 室内位置検出装置 | |
| JP2011227074A (ja) | 磁場測定値補償方法および装置、物***置特定方法およびシステム、ならびに、これらの方法の情報記録媒体 | |
| Paperno et al. | Three-dimensional magnetic tracking of biaxial sensors | |
| JP2011059091A (ja) | 室内位置検出装置 | |
| Kusche et al. | Indoor positioning via artificial magnetic fields | |
| CN116625363A (zh) | 一种多通道六自由度电磁定位跟踪方法 | |
| EP4365702A2 (en) | Device and method for determining an orientation of a magnet, and a joystick | |
| EP0038151B1 (en) | Remote object position and orientation locator | |
| CN116633037A (zh) | 一种用于机器人导航的多通道六自由度电磁定位跟踪装置 | |
| Moschitta et al. | Estimation of the magnetic dipole moment of a coil using AC voltage measurements | |
| CN114234958B (zh) | 一种基于磁场特征值的磁信标定向方法、存储介质及设备 | |
| KR101352245B1 (ko) | Ｍｅｍｓ 지자기 센서의 방위각 보정 방법 및 장치 | |
| CN117784259B (zh) | 单分量磁场定位方法及定位*** |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |