﻿ 基于移动最小二乘法的轨迹拟合切线方位角计算
 文章快速检索
 同济大学学报(自然科学版)  2018, Vol. 46 Issue (11): 1589-1593.  DOI: 10.11908/j.issn.0253-374x.2018.11.018 0

### 引用本文

YAO Lianbi, QIAN Jinfei. Trajectory Tangent Azimuth Calculation Based on Moving Least Square Fitting[J]. Journal of Tongji University (Natural Science), 2018, 46(11): 1589-1593. DOI: 10.11908/j.issn.0253-374x.2018.11.018

### 基金项目

“十三五”国家重点研发计划(2016YFB1200602-02)；国家自然科学基金(41771482)

### 文章历史

Trajectory Tangent Azimuth Calculation Based on Moving Least Square Fitting
YAO Lianbi , QIAN Jinfei
College of Surveying and Geo-Informatics, Tongji University, Shanghai 200092, China
Abstract: Based on moving least squares, an algorithm of trajectory tangent azimuth calculation is presented in this paper. The feasibility of the algorithm is tested by measured data and the setting of key parameters including dilatation parameter and weight function is discussed. The results show: the algorithm is practical and can be applied to small bended trajectory; the value of dilatation parameter should satisfy the requirement of calculation, but it should not be too big; the precision of trajectory fitting can be promoted by weight function but the precision of azimuth will not be affected.
Key words: azimuth    moving least squares    trajectory fitting

1 移动最小二乘理论

 $u\left( \mathit{\boldsymbol{x}} \right) \simeq \hat u\left( \mathit{\boldsymbol{x}} \right) = {\mathit{\boldsymbol{P}}^{\rm{T}}}\left( \mathit{\boldsymbol{x}} \right)\mathit{\boldsymbol{\alpha }} = \sum\limits_{j = 1}^m {{\alpha _j}{p_j}\left( \mathit{\boldsymbol{x}} \right), \mathit{\boldsymbol{x}} \in \mathit{\Omega }}$ (1)

 $\begin{array}{l} M\left( \mathit{\boldsymbol{\alpha }} \right) = \sum\limits_{i \in {I_{\mathit{\boldsymbol{x}}, \delta }}} {{{\left( {u\left( {{\mathit{\boldsymbol{x}}_i}} \right) - \sum\limits_{j = 1}^m {{\alpha _j}{p_j}\left( {{\mathit{\boldsymbol{x}}_i}} \right)} } \right)}^2}w\left( {\mathit{\boldsymbol{x}} - } \right.} \\ \;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\left. {{\mathit{\boldsymbol{x}}_i}} \right) = \min \end{array}$ (2)

 $\begin{array}{l} M\left( \mathit{\boldsymbol{\alpha }} \right) = \mathit{\boldsymbol{U}}_{{\mathit{\Omega }_\mathit{\boldsymbol{x}}}}^{\rm{T}}{\mathit{\boldsymbol{W}}_{{\mathit{\Omega }_\mathit{\boldsymbol{x}}}}}{\mathit{\boldsymbol{U}}_{{\mathit{\Omega }_\mathit{\boldsymbol{x}}}}} - 2{\mathit{\boldsymbol{\alpha }}^{\rm{T}}}{\mathit{\boldsymbol{P}}_{{\mathit{\Omega }_\mathit{\boldsymbol{x}}}}}{\mathit{\boldsymbol{W}}_{{\mathit{\Omega }_\mathit{\boldsymbol{x}}}}}{\mathit{\boldsymbol{U}}_{{\mathit{\Omega }_\mathit{\boldsymbol{x}}}}} + \\ \;\;\;\;\;\;\;\;\;\;\;\;\;{\mathit{\boldsymbol{\alpha }}^{\rm{T}}}{\mathit{\boldsymbol{P}}_{{\mathit{\Omega }_\mathit{\boldsymbol{x}}}}}{\mathit{\boldsymbol{W}}_{{\mathit{\Omega }_\mathit{\boldsymbol{x}}}}}\mathit{\boldsymbol{P}}_{{\mathit{\Omega }_\mathit{\boldsymbol{x}}}}^{\rm{T}}\mathit{\boldsymbol{\alpha }} + \min \end{array}$ (3)

 $\left[ {{\mathit{\boldsymbol{P}}_{{\mathit{\Omega }_\mathit{\boldsymbol{x}}}}}\;\;{\mathit{\boldsymbol{W}}_{{\mathit{\Omega }_\mathit{\boldsymbol{x}}}}}\;\;\mathit{\boldsymbol{P}}_{{\mathit{\Omega }_\mathit{\boldsymbol{x}}}}^{\rm{T}}} \right]\mathit{\boldsymbol{\alpha }} = {\mathit{\boldsymbol{P}}_{{\mathit{\Omega }_\mathit{\boldsymbol{x}}}}}{\mathit{\boldsymbol{W}}_{{\mathit{\Omega }_\mathit{\boldsymbol{x}}}}}{\mathit{\boldsymbol{U}}_{{\mathit{\Omega }_\mathit{\boldsymbol{x}}}}}$ (4)

 $\mathit{\boldsymbol{\alpha }} = {\left[ {{\mathit{\boldsymbol{P}}_{{\mathit{\Omega }_\mathit{\boldsymbol{x}}}}}\;\;{\mathit{\boldsymbol{W}}_{{\mathit{\Omega }_\mathit{\boldsymbol{x}}}}}\;\;\mathit{\boldsymbol{P}}_{{\mathit{\Omega }_\mathit{\boldsymbol{x}}}}^{\rm{T}}} \right]^{ - 1}}{\mathit{\boldsymbol{P}}_{{\mathit{\Omega }_\mathit{\boldsymbol{x}}}}}{\mathit{\boldsymbol{W}}_{{\mathit{\Omega }_\mathit{\boldsymbol{x}}}}}{\mathit{\boldsymbol{U}}_{{\mathit{\Omega }_\mathit{\boldsymbol{x}}}}}$ (5)

2 轨迹拟合切线方位角算法

 图 1 算法流程 Fig.1 Flow chart of the algorithm
3 算法测试

 图 2 四平路实验及逸仙路实验轨迹 Fig.2 Trajectory of Siping road experiment and Yixian road experiment

 图 3 车辆掉头处轨迹 Fig.3 Trajectory of vehicle turning around

 图 4 转弯处IMU方位角与拟合方位角之差 Fig.4 Difference between IMU azimuth and fitting azimuth around the bend

 $\delta \ge 2.5\frac{{{v_{\max }}}}{f}$ (5)

 ${W_1}\left( {\Delta s} \right) = 1 - \frac{{\Delta s}}{\delta }$ (6)
 ${W_2}\left( {\Delta s} \right) = \frac{1}{{1 + {{\left( {\Delta s/\delta } \right)}^2}}}$ (7)
 $\begin{array}{l} {W_3}\left( {\Delta s} \right) = \\ \left\{ \begin{array}{l} \frac{2}{3} - 4{\left( {\frac{{\Delta s}}{\delta }} \right)^{ - 2}} + 4{\left( {\frac{{\Delta s}}{\delta }} \right)^{ - 3}}, \;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;0 < \frac{{\Delta s}}{\delta } \le \frac{1}{2}\\ \frac{3}{4} - 4{\left( {\frac{{\Delta s}}{\delta }} \right)^{ - 1}} + 4{\left( {\frac{{\Delta s}}{\delta }} \right)^{ - 2}} + \frac{4}{3}{\left( {\frac{{\Delta s}}{\delta }} \right)^{ - 3}}, \;\;\;\;\frac{1}{2} < \frac{{\Delta s}}{\delta } \le 1 \end{array} \right. \end{array}$ (8)

4 结论

(1) 轨迹形状测试说明本文算法能处理小曲率及大曲率半径下的轨迹拟合.在大曲率半径下，拟合方位角与IMU方位角有较大的差异，所以这种情况下不建议使用该方法拟合的数据；在轨迹弯曲不大的路线上，该算法能够较好地逼近轨迹并计算方位角，为移动测量车的IMU方位角标定提供一个比较精准的原始值.

(2) 随着紧支系数δ的增大，移动最小二乘坐标拟合的精度(标准差)逐渐下降，而方位角拟合的精度逐渐上升.

(3) 权函数能够提高坐标拟合的精度，但对方位角拟合的作用十分有限.

 [1] GROVES P D. GNSS与惯性及多传感器组合导航系统原理[M]. 北京: 国防工业出版社, 2015 GROVES P D. Principles of GNSS, inertial, and multisensor integrated navigation systems[M]. Beijing: National Defense Industry Press, 2015 [2] 张玲.基于三次样条曲线拟合公路平面线形方法研究[D].武汉: 武汉理工大学, 2007. ZHANG Ling. Analysis of fitting method on plan curve based on cubic spline[D]. Wuhan: Wuhan University of Technology, 2007. [3] 姚连璧, 周小平. 基于MATLAB的控制网平差程序设计[M]. 上海: 同济大学出版社, 2006 YAO Lianbi, ZHOU Xiaoping. Design of control net adjustment software based on MATLAB[M]. Shanghai: Tongji University Press, 2006 [4] 曾清红, 卢德唐. 基于移动最小二乘法的曲线曲面拟合[J]. 工程图学学报, 2004(1): 84 ZENG Qinghong, LU Detang. Curve and surface fitting based on moving least squares[J]. Journal of Engineering Graphics, 2004(1): 84 DOI:10.3969/j.issn.1003-0158.2004.01.017 [5] 齐林, 张芳, 陈恩庆. 基于移动最小二乘曲线拟合的LFM信号参数估计[J]. 郑州大学学报(工学版), 2011(3): 95 QI Lin, ZHANG Fang, CHEN Enqing. Parameter estimation of LFM signal via MLS approximation[J]. Journal of Zhengzhou University(Engineering Science), 2011(3): 95 DOI:10.3969/j.issn.1671-6833.2011.03.023 [6] SHEPARD D. A two dimensional interpolation function for irregularly spaced data[C]//Proceedings of the Twenty Third ACM National Conference. New York: ACM, 1968: 517-524. [7] LANCASTER P, ALKAUSKAS K. Curve and surface fitting[M]. New York: Academic Press, 1986 [8] 程玉民. 移动最小二乘法研究进展与述评[J]. 计算机辅助工程, 2009(6): 5 CHENG Yumin. Advances and review on moving least square methods[J]. Computer Aided Engineering, 2009(6): 5 [9] AMIRFAKHRIAN M, MAFIKANDI H. Approximation of parametric curves by Moving Least Squares method[J]. Applied Mathematics and Computation, 2016, 283: 290 DOI:10.1016/j.amc.2016.02.039