﻿ 协同定位中的坐标配准策略研究
«上一篇
 文章快速检索 高级检索

 智能系统学报  2021, Vol. 16 Issue (3): 459-465  DOI: 10.11992/tis.202012015 0

### 引用本文

FAN Yingsheng, QI Xiaogang, LIU Lifang. Coordinate registration strategy in cooperative localization[J]. CAAI Transactions on Intelligent Systems, 2021, 16(3): 459-465. DOI: 10.11992/tis.202012015.

### 文章历史

1. 西安电子科技大学 数学与统计学院，陕西 西安 710071;
2. 浙江警察学院 公共基础部，浙江 杭州 310053;
3. 西安电子科技大学 计算机学院，陕西 西安 710071

Coordinate registration strategy in cooperative localization
FAN Yingsheng 1,2, QI Xiaogang 1, LIU Lifang 3
1. School of Mathematics and Statistics, Xidian University, Xi’an 710071, China;
2. Basic Courses Department, Zhejiang Police College, Hangzhou 310053, China;
3. School of Computer Science and Technology, Xidian University, Xi’an 710071, China
Abstract: Coordinate registration is an integral part of cooperative localization. A good coordinate registration system can improve the performance of collaborative location algorithms; otherwise, it may increase their errors. This paper carefully compares the design ideas and applicable conditions of coordinate registration methods based on the least square (LS) method and Procrustes analysis (PA). Detailed steps of the coordinate registration algorithm based on PA are provided. Using the experimental data obtained from the cooperative localization algorithms (classical MDS and Levenberg-Marquardt algorithm), the effects of the number of anchor nodes, range error, and average connectivity of the network nodes on the registration accuracy were analyzed in detail. The experimental results showed that in 2D and 3D environments, the PA-based algorithm has better registration accuracy and stability than the LS-based algorithm, with registration error reduced by approximately 20%.
Key words: cooperative localization    positioning algorithm    coordinate registration    least square algorithm    Procrustes analysis    anchor node    average connectivity    range error

1 坐标配准模型 1.1 坐标配准模型与符号说明

1.2 基于最小二乘的坐标配准

 ${{Q}} = {({{C'C}})^{{\rm{ - }}1}}({{C'd}})$

2 基于普氏分析的坐标配准

 $[s,{{Q}},{{T}}] = \arg\min \left\| {s{{BQ}} + {{T}} - {{A}}} \right\|$ (1)

2.1 平移

 ${{{l}}_{ci}} = {{{l}}_i} - {\frac{1}{m}}\sum\limits_{j = 1}^m {{{{l}}_j}}$ (2)
 ${\hat{ l}}_{ci}^{{\rm{re}}} = {\hat{ l}}_i^{{\rm{re}}} - \frac{1}{m}\sum\limits_{j = 1}^m {{\hat{ l}}_j^{{\rm{re}}}}$ (3)

2.2 归一化

 ${{{A}}_0} = \frac{1}{{\left\| {{{{A}}_c}} \right\|}}{{{A}}_c}$ (4)
 ${{{B}}_0} = \frac{1}{{\left\| {{{{B}}_c}} \right\|}}{{{B}}_c}$ (5)
2.3 求解旋转变换Q

 ${{Q}} = {\rm{argmin}} \left\| {{{{B}}_0}{{Q}} - {{{A}}_0}} \right\|$ (6)

${\rm{tr}}({{A}})$ 表示矩阵 ${{A}}$ 的迹，由矩阵迹的定义以及“循环性质”，也即 ${\rm{tr}}({{ABC}}) = {\rm{tr}}({{CAB}}) = {\rm{tr}}({{BCA}})$ ，可知：

 $\begin{array}{l} {\left\| {{{{B}}_0}{{Q}} - {{{A}}_0}} \right\|^2} = {\rm{tr}}\left( {({{{B}}_0}{{Q}} - {{{A}}_0})^{\rm{T}}({{{B}}_0}{{Q}} - {{{A}}_0})} \right) =\\ \quad{\rm{tr}}({{Q}}^{\rm{T}}{{{B}}_0}^{\rm{T}} {{{B}}_0}{{Q}}) + {\rm{tr}}({{{A}}_0}^{\rm{T}} {{{A}}_0}) - 2{\rm{tr}}({{{A}}_0}^{\rm{T}} {{{B}}_0}{{Q}}) = \\ \quad\quad{\rm{tr}}({{{B}}_0}^{\rm{T}} {{{B}}_0}) + {\rm{tr}}({{{A}}_0}^{\rm{T}} {{{A}}_0}) - 2{\rm{tr}}({{{A}}_0}^{\rm{T}} {{{B}}_0}{{Q}}) \\ \end{array}$ (7)

 ${\rm{tr}}({{{A}}_0}^{\rm{T}} {{{B}}_0}{{Q}}) = {\rm{tr}}({{UWV}}^{\rm{T}}{{Q}}) = {\rm{tr}}({{WV}}^{\rm{T}}{{QU}}) = {\rm{tr}}({{WH}})$ (8)

 ${\rm{tr}}({{WH}}) = \sum\limits_{i = 1}^p {{\sigma _i}{h_{ii}}}$ (9)

 ${{Q}} = {{VU}}^{\rm{T}}$ (10)
2.4 在A0B0Q下求解放缩因子s0

 ${s_0} = {\rm{argmin}} \left\| {{s_0}{{{B}}_0}{{Q}} - {{{A}}_0}} \right\|$ (11)

 $\begin{array}{l} {\left\| {{s_0}{{{B}}_0}{{Q}} - {{{A}}_0}} \right\|^2} = {\rm{tr}}\left( {({s_0}{{{B}}_0}{{Q}} - {{{A}}_0})^{\rm{T}}({s_0}{{{B}}_0}{{Q}} - {{{A}}_0})} \right) = \\ \quad {\rm{tr}}({{Q}}^{\rm{T}}{{{B}}_0}^{\rm{T}} {{{B}}_0}{{Q}}){s_0}^2 - 2{\rm{tr}}({{{A}}_0}^{\rm{T}} {{{B}}_0}{{Q}}){s_0} + {\rm{tr}}({{{A}}_0}^{\rm{T}} {{{A}}_0}) \\ \end{array}$ (12)

 ${s_0} = \frac{{{\rm{tr}}({{{A}}_0}^{\rm{T}} {{{B}}_0}{{Q}})}}{{{\rm{tr}}({{Q}}^{\rm{T}}{{{B}}_0}^{\rm{T}} {{{B}}_0}{{Q}})}} = \frac{{{\rm{tr}}({{UWV}^{\rm{T}}{{VU}^{\rm{T}}}})}}{{{\rm{tr}}({{QQ}^{\rm{T}}}{{{B}}_0}^{\rm{T}} {{{B}}_0})}} = {\rm{tr}}({{W}})$ (13)

${s_0}$ 即为矩阵 ${{{A}}_0}^{\rm{T}} {{{B}}_0}$ 奇异值分解后的奇异值之和。

2.5 求解平移向量T

 ${s_0}\frac{{{{B}} - {{{T}}_B}}}{{\left\| {{{{B}}_c}} \right\|}}{{Q}} \approx \frac{{{{A}} - {{{T}}_A}}}{{\left\| {{{{A}}_c}} \right\|}}$ (14)
 $\left({s_0}\frac{{\left\| {{{{A}}_c}} \right\|}}{{\left\| {{{{B}}_c}} \right\|}}\right){{BQ}} + \left({{{T}}_A} - {s_0}\frac{{\left\| {{{{A}}_c}} \right\|}}{{\left\| {{{{B}}_c}} \right\|}}{{{T}}_B}{{Q}}\right) \approx {{A}}$ (15)

 $s = {s_0}\frac{{\left\| {{{{A}}_c}} \right\|}}{{\left\| {{{{B}}_c}} \right\|}}$ (16)
 ${{T}} = {{{T}}_A} - {s_0}\frac{{\left\| {{{{A}}_c}} \right\|}}{{\left\| {{{{B}}_c}} \right\|}}{{{T}}_B}{{Q}}$ (17)
2.6 基于普氏分析坐标配准的计算流程

1)根据式(2)、(3)对矩阵 ${{A}}$ ${{B}}$ 进行平移，求得平移后的矩阵 ${{{A}}_c}$ ${{{B}}_c}$ 以及 ${{{T}}_A}$ ${{{T}}_B}$

2)根据式(4)、(5)对 ${{{A}}_c}$ ${{{B}}_c}$ 进行归一化，求得归一化后的矩阵 ${{{A}}_0}$ ${{{B}}_0}$

3)根据式(7)~(9)求得旋转矩阵 ${{Q}}$

4)若考虑放缩因子 $s$ ，则根据式(12)、(13)求得式(12)中的 ${s_0}$ ，转5)；若不考虑放缩因子 $s$ ，则令 $s = 1$ ，转至6)；

5)根据式(16)求得放缩因子 $s$

6)根据式(17)求得平移向量 ${{T}}$

3 仿真实验

 ${\rm{MAE}} = \frac{1}{N}\sum\limits_{i = 1}^N {\left\| {{{{\hat{ X}}}_i} - {{{X}}_i}} \right\|}$ (18)
3.1 实验设置

3.2 不同锚节点数量对配准精度的影响

 Download: 图 1 不同锚节点数量对配准精度的影响 Fig. 1 Influence of the number of anchor nodes on the registration accuracy

3.3 不同测距误差对配准精度的影响

 Download: 图 2 不同测距误差对配准精度的影响 Fig. 2 Influence of ranging errors on the registration accuracy
3.4 不同连通度对配准精度的影响

 Download: 图 3 网络平均连通度为6时的坐标配准 Fig. 3 Coordinate registration of ${\rm{deg}} = 6$
 Download: 图 4 网络平均连通度为8时的坐标配准 Fig. 4 Coordinate registration of ${\rm{deg}} = 8$

 Download: 图 5 网络平均连通度为10时的坐标配准 Fig. 5 Coordinate registration of ${\rm{deg}} = 10$
 Download: 图 6 网络平均连通度为16时的坐标配准 Fig. 6 Coordinate registration of ${\rm{deg}} = 16$
4 结论

 [1] WANASINGHE T R, MANN G K I, GOSINE R G. Distributed leader-assistive localization method for a heterogeneous multirobotic system[J]. IEEE transactions on automation science and engineering, 2015, 12(3): 795-809. DOI:10.1109/TASE.2015.2433014 (0) [2] YAN Yuan, MOSTOFI Y. Robotic router formation in realistic communication environments[J]. IEEE transactions on robotics, 2012, 28(4): 810-827. DOI:10.1109/TRO.2012.2188163 (0) [3] SUN Qian, TIAN Yu, DIAO Ming. Cooperative localization algorithm based on Hybrid topology architecture for multiple mobile robot system[J]. IEEE internet of things journal, 2018, 5(6): 4753-4763. DOI:10.1109/JIOT.2018.2812179 (0) [4] WANG Chunxiang, HUANG Hairu, JI Yang, et al. Vehicle localization at an intersection using a traffic light map[J]. IEEE transactions on intelligent transportation systems, 2019, 20(4): 1432-1441. DOI:10.1109/TITS.2018.2851788 (0) [5] DENTLER J, KANNAN S, BEZZAOUCHA S, et al. Model predictive cooperative localization control of multiple UAVs using potential function sensor constraints: a workflow to create sensor constraint based potential functions for the control of cooperative localization scenarios with mobile robots[J]. Autonomous robots, 2019, 43(1): 153-178. DOI:10.1007/s10514-018-9711-z (0) [6] WANG Xianpeng, YANG L T, MENG Dandan, et al. Multi-UAV cooperative localization for marine targets based on weighted subspace fitting in SAGIN environment[EB/OL]. (2021−3−17)[2021−4−30]https://ieeexplore.ieee.org/document/9380358. (0) [7] 朱健楠, 虞梦苓, 杨益新. 无人水面艇感知技术发展综述[J]. 哈尔滨工程大学学报, 2020, 41(10): 1486-1492. ZHU Jiannan, YU Mengling, YANG Yixin. Overview of development of unmanned-surface-vehicle sensing technology[J]. Journal of Harbin Engineering University, 2020, 41(10): 1486-1492. (0) [8] SHIT R C. Precise localization for achieving next- generation autonomous navigation: State-of-the-Art, taxonomy and future prospects[J]. Computer communications, 2020, 160: 351-374. DOI:10.1016/j.comcom.2020.06.007 (0) [9] ZHU Yanbo, LIU Yuan, WANG Zhipeng. Evaluation of GBAS flight trials based on BDS and GPS[J]. IET radar, sonar & navigation, 2020, 14(2): 233-241. (0) [10] 席志红, 占梦奇. 基于位置范围限定的WiFi-KNN室内定位算法[J]. 应用科技, 2020, 47(4): 66-70. XI Zhihong, ZHAN Mengqi. WiFi-KNN indoor positioning algorithm based on location range limitation[J]. Applied science and technology, 2020, 47(4): 66-70. (0) [11] JIN Bonan, XU Xiaosu, ZHU Yongyun, et al. Single-source aided semi-autonomous passive location for correcting the position of an underwater vehicle[J]. IEEE sensors journal, 2019, 19(9): 3267-3275. DOI:10.1109/JSEN.2019.2892840 (0) [12] WYMEERSCH H, LIEN J, WIN M Z. Cooperative localization in wireless networks[J]. Proceedings of the IEEE, 2009, 97(2): 427-450. DOI:10.1109/JPROC.2008.2008853 (0) [13] FAN Yingsheng, QI Xiaogang, LIU Lifang. Fault-tolerant cooperative localization of 3D mobile networks via two-layer filter multidimensional scaling[J]. IEEE sensors journal, 2021, 21(6): 8354-8366. DOI:10.1109/JSEN.2020.3046736 (0) [14] SHIT R C, SHARMA S, PUTHAL D, et al. Ubiquitous Localization (UbiLoc): a survey and taxonomy on device free localization for smart world[J]. IEEE communications surveys & tutorials, 2019, 21(4): 3532-3564. (0) [15] PIOVESAN N, ERSEGHE T. Cooperative localization in WSNs: a hybrid convex/nonconvex solution[J]. IEEE transactions on signal and information processing over networks, 2018, 4(1): 162-172. DOI:10.1109/TSIPN.2016.2639442 (0) [16] SHANG Y, RUMI W, ZHANG Y, et al. Localization from connectivity in sensor networks[J]. IEEE transactions on parallel and distributed systems, 2004, 15(11): 961-974. DOI:10.1109/TPDS.2004.67 (0) [17] COSTA J A, PATWARI N, HERO A O. Distributed weighted multi-dimensional scaling for node localization in sensor networks[J]. ACM transactions on sensor networks, 2006, 2(1): 39-64. DOI:10.1145/1138127.1138129 (0) [18] NOCEDAL J, WRIGHT S J. Numerical optimization[M]. 2nd ed. Berlin: Springer, 2006: 250−274. (0) [19] FAN Yingsheng, QI Xiaogang, LI Bo, et al. Fast clustering-based multidimensional scaling for mobile networks localisation[J]. IET communications, 2020, 14(1): 135-143. DOI:10.1049/iet-com.2019.0444 (0) [20] 马兰芝, 管亮, 冯新泸, 等. 基于普鲁克分析的润滑油红外光谱指纹区相似度评价方法[J]. 石油学报(石油加工), 2013, 29(5): 891-898. MA Lanzhi, GUAN Liang, FENG Xinlu, et al. The method of similarity determination for lubricating Oil infrared fingerprint by Procrustes Analysis[J]. Acta petrolei sinica (petroleum processing section), 2013, 29(5): 891-898. (0) [21] 陈智强, 王作伟, 方龙伟, 等. 基于机器学习和几何变换的实时2D/3D脊椎配准[J]. 自动化学报, 2018, 44(7): 1183-1194. CHEN Zhiqiang, WANG Zuowei, FANG Longwei, et al. Real-time 2D/3D registration of vertebra via machine learning and geometric transformation[J]. Acta automatica sinica, 2018, 44(7): 1183-1194. (0) [22] 成翔昊, 达飞鹏, 汪亮. 基于融合约束局部模型的三维人脸特征点定位[J]. 浙江大学学报(工学版), 2019, 53(4): 770-776. CHENG Xianghao, DA Feipeng, WANG Liang. Feature fusion based constrained local model for three- dimensional facial landmark localization[J]. Journal of Zhejiang University (engineering science), 2019, 53(4): 770-776. DOI:10.3785/j.issn.1008-973X.2019.04.018 (0) [23] 熊玉雪, 杨慧芳, 赵一姣, 等. 两种评价面部三维表面数据不对称度方法的比较[J]. 北京大学学报(医学版), 2015, 47(2): 340-343. XIONG Yuxue, YANG Huifang, ZHAO Yijiao, et al. Comparison of two kinds of methods evaluating the degree of facial asymmetry by three-dimensional data[J]. Journal of Peking University (health sciences), 2015, 47(2): 340-343. (0) [24] BAI Xuefeng, CAO Hailong, CHEN Kehai, et al. A bilingual adversarial autoencoder for unsupervised bilingual lexicon induction[J]. IEEE/ACM transactions on audio, speech, and language processing, 2019, 27(10): 1639-1648. DOI:10.1109/TASLP.2019.2925973 (0) [25] GOWER J C, DIJKSTERHUIS G B. Procrustes problems[M]. Oxford: Oxford University Press, 2004: 29−48. (0) [26] MOHAMMADREZA D. Minimal driver nodes for structural controllability of large-scale dynamical systems: node classification[J]. IEEE systems journal, 2020, 14(3): 4209-4216. DOI:10.1109/JSYST.2019.2956501 (0)