1 引言（Introduction）

精准平滑的GNSS路径是户外机器人执行行驶任务的重要先验信息，高效获取、处理和存储GNSS路径是地图制作^[1]、机器人导航技术^[2-3]和实时决策规划^[4]的研究基础。其典型应用场景有军用机器人自主返航和简易地图采集，这类场景需要人工操作轮式移动机器人行驶，利用其定位设备采集准确有效的GNSS路径^[5]。但由于道路复杂，人为操作时受到诸多因素影响，极易出现走错道路及不断“倒车”的现象，形成简易的交错折线和复杂的“之字形”折线等无效路径，无法在机器人导航中使用，机器人需要从所采集路径中自动提取平滑有效的路径。

杨伟等^[6]基于速度序列对采集的GNSS轨迹线噪声进行分割滤选，能够筛选出相同速度下的突变路径，但针对众源轨迹的滤选方法均不能够对无效轨迹进行精细化处理。精细化路径处理方法主要包括曲线拟合法和数值优化法2类。Dolgov等^[7]通过建立多目标优化函数，使用共轭梯度下降法进行路径平滑，并在半结构化环境下进行无人车路径平滑，但该方法对各目标函数的权重选择较为敏感，容易出现不收敛的情况；杜广泽等^[8]使用MPC（model predictive control）算法直接优化得到可用于无人车的平滑轨迹；Liu等^[9]提出CFS（convex feasible set）算法，利用凸约束迭代得到平滑路径；Zhou等^[10]提出一种DL-IAPS（dual-loop iterative anchoring path smoothing）算法，对无人车泊车路径进行优化，将问题转化为二次规划问题求解，为诸多泊车场景提供了可靠的路径。Vailland等^[11]使用3次贝塞尔曲线对CBB-RRT*（cubic Bezier based-rapidly-exploring random tree*）算法得到的采样路径进行平滑，得到可用于无人平台平滑行驶的路径；B样条拟合法具备拟合度较高的特点，杜明博等^[12]、Zhou等^[13]和He等^[14]使用B样条曲线分別在无人车、无人机和无人机群中对规划路径进行拟合平滑；文[15]提出使用SDM（squared distance minimization）算法将离散点云拟合为B样条曲线；谢德胜等^[16]通过标准卡尔曼滤波器对跳变点进行平滑滤波，但该方法未关注滤波结果与原有路径的位置差异。以上路径处理方法能够得到平滑路径，但是其在处理存在无效路段的路径时存在一些缺陷：(1) 仅能对折线进行平滑处理，无法自动标识和删除无效路段；(2) 局部路段优化时无法与连接点的位置、朝向和曲率保持连续；(3) 拟合度高的方法容易出现过拟合现象，拟合度低的方法得到的结果与原有路径位置差异较大。

为有效解决轮式移动机器人在采集RTK-GNSS（real-time kinematic-GNSS）点集过程中出现的无效路段问题（由走错道路导致），特别是反复倒车前行产生的难以用包络线进行拟合的“之字形”路段，本文提出一种简单高效的路径处理方法，自适应地对多次调整和单次调整路段中的无效路段予以自动识别、剔除和重组，以解决现有路径平滑方法在无效路段剔除方面的不足。为获得适用于轮式移动机器人的平滑路径，构建多目标优化函数并转化为二次规划型求解，达到路径曲率连续、与原有路径保持较小位置差异的目的。整体算法流程如图 1所示。

2 路径分段重组算法（Path segmentation and rearrangement algorithm） 2.1 DRD分段法

路径分段之前提出如下假设：1) 定义沿车头方向行驶为前进档，逆车头方向行驶为倒退档；2) 定义非前进档采集的路径为误采集路段，称为无效路段，但前进档行驶时采集的路径点集并非都有效。

基于以上假设，根据计算得到的采集点航向信息对GNSS路径中各点的档位信息进行自动识别，并根据档位分布情况将整条路径进行第1次分段。划分的多个路段中，每一个路段中各点对应的档位属性应当一致，每一个路段与相邻路段的档位属性应当不一致，整条路径划分为“前进档—倒退档—前进档”形式的路段组合，如将图 2(a)(c) 划分为图 2(b)(d) 所示路段组合。将这种形式的路段定义为DRD路段组合，划分方法称为DRD分段法。其中D段为前进档时采集的路径点集合，R段为倒档时采集的路径点集合。能够得出如下推论：1) 每一个R段的前一段和后一段都是D段，即不存在2个连续的R段组合，同理也不存在连续的D段组合；2) 存在没有R段的路径且该路径仍为有效路径；3) 不存在D段的路径必然为无效路径。

2.2 DRD路段重组

DRD分段法将GNSS路径划分为多条前进和倒退路段组合，但前进路段中所有点并非都是有效的，如图 2(b)中的D$ _{2} $段、D$ _{3} $段和图 2(d)中靠近R$ _{1} $段的部分前进路段仍然属于无效路段。DRD路段重组的目的是从DRD路段组合中找到有效的路段集合，按照如下步骤对其中的有效路段进行快速提取和重组：首先对部分路段按照邻近倒退路段长度进行第2次自适应分段，其次对无效路段组合进行剔除，最后将有效路段重新组合，并在组合时保持原有路径的形状。算法具体步骤如下：

步骤1：基于邻域长度的自适应分段。自适应分段的目的是将长距离的有效（D段）路段划分为多个小段，便于寻找步骤3中的正确交叉点。如图 3(a)所示，若R段的相邻2个D段为闭合形式曲线或者存在多个交点的曲线，在后续步骤3中寻找与相邻D段的交叉点时，将会找到如图 3(a)所示的错误交叉点。对每一个R段相邻的D段按照式(1) 进行第2次分段，即根据R段的长度对相邻D段进行第2次自适应分段处理。对图 2(d)中D$ _{1} $R$ _{1} $D$ _{2} $路段按照步骤1进行自适应分段处理后，得到图 3(a)所示结果，其中D$ _{12} $R$ _{1} $D$ _{21} $路段中仅存在唯一的正确交叉点。

$ \begin{align} d_{\rm d} \geqslant d_{\rm r} +d_{\text{buffer}} \end{align} $

(1)

式中，$ d_{\rm d} $为D段的长度，$ d_{\rm r} $为R段的长度，$ d_{\text{buffer}} $为长度差的阈值。除此之外，按照式(1) 分段能够将部分RDR组合中的长距离D段划分为多个子线段，将RDR形式转换为RD$ _{1} $D$ _{2} $D$ _{3} $R，避免在步骤2中将其中的D$ _{2} $段误识别为无效路段。

步骤2：无效路段剔除。按照2.1节假设2) 设定无效路段剔除规则：(1) R形式路段，所有R段的点均为无效点；(2) RDR形式路段，经过自适应分段法得到的路段中存在RDR式的路段组合，这类组合表示需要连续调整行驶方向区域，该路段组合均为无效路段。

步骤3：路段重组，对剔除无效路段后的剩余路段进行重新组合。每次重组均仅针对2个路段，以$ \boldsymbol{x}_{i} =(x_{i}, y_{i} ) $表示第$ i $个采集点的直角坐标，$ S_{j} :\{ \boldsymbol{x}_{i} =(x_{i}, y_{i} ) |i=0, 1, \cdots, N \} $表示第$ j $段点集，$ S_{j+1} :\{ \boldsymbol{x}_{i} =(x_{i}, y_{i} ) |i=0 , 1, \cdots, M \} $表示第$ j+1 $段点集，则第$ j $段点集按照$ \boldsymbol{x}_{i} -\boldsymbol{x}_{i-1} $组成$ N-1 $个线段，同理第$ j+1 $段点集组成$ M-1 $个线段，按照需要重组的所有线段中任意2个线段是否存在交叉，设定如下路段重组规则：

1) 若存在交叉点，即$ S_{j} \cap S_{j+1} != \varnothing $，设交叉点在2个点集中的序号分别为$ p $、$ q $，则可通过式(2) 对其进行更新和重组，如图 3(a)所示。

$ \begin{align} S_{j} & \to \{ \boldsymbol{x}_{i} =(x_{i}, y_{i} ) |i=0, 1, \cdots, p \} \\ S_{j+1} & \to \{ \boldsymbol{x}_{i} =(x_{i}, y_{i} ) |i=q, q+1, \cdots, N \} \\ S_{j} \cup S_{j+1} & \to \{ \boldsymbol{x}_{i} =(x_{i}, y_{i} ) |i=0, 1, \cdots, p, q, \cdots, N \} \end{align} $

(2)

2) 若不存在交叉点，即$ S_{j} \cap S_{j+1} =\varnothing $，则通过式(3) 进行重组：第$ j $段利用其路段终点在第$ j+1 $段中正向寻找最近点$ q $，第$ j+1 $段利用其路段起点在第$ j $段中逆向寻找最近点$ p $，如图 3(b)所示。

$ \begin{align} & \boldsymbol{x}_{N} \in S_{j} \xrightarrow[{ q\in S_{j+1}}]{\text{寻找最近点}} \{\boldsymbol{x}_{i} =(x_{i}, y_{i} ) |i=0 , 1, \cdots, M \} \\ & \boldsymbol{x}_{0} \in S_{j+1} \xrightarrow[p\in S_{j}]{\text{寻找最近点}} \{ \boldsymbol{x}_{i} =(x_{i}, y_{i} ) |i=N, \cdots, 0 \} \\ & S_{j} \cup S_{j+1} \to \{ \boldsymbol{x}_{i} =(x_{i}, y_{i})|i=0, 1, \cdots, p, q, \cdots, N \} \end{align} $

(3)

3 后处理方法（Post-processing method） 3.1 优化目标建立

为解决重组结果航向和曲率不连续变化的问题，将经过分段重组处理后的路径进行后处理，以得到可直接用于机器人轨迹跟随和高精度地图制作的平滑路径。从决策变量、目标函数和约束条件3个方面建立多目标优化模型，优化目标为得到使目标函数最小的决策变量。

建立优化目标函数的数学模型，首先需要确定问题的决策变量$ {\boldsymbol{X}} $。优化函数的目标为对初始路径进行优化，以各个离散路径点横纵坐标作为决策变量。将分段重组处理后由$ N $个路径点组成的路径表示为

$ \begin{align} {\boldsymbol{X}}=[{\boldsymbol{x}_{1}, \boldsymbol{x}_{2}, \cdots, \boldsymbol{x}_{N}}]^{\rm T} \end{align} $

(4)

根据决策变量设定优化模型的目标函数，构建式(5) 所示的2个子目标函数：路径平滑函数$ J_{\rm s} $和原始路径形状保持函数$ J_{\rm d} $。

$ \begin{align} \min {\boldsymbol{Y}}=J({\boldsymbol{X}}) =\sum _2^N ({\omega_{\rm s} J_{\rm s} (\boldsymbol{x}_{i})+\omega_{\rm d} J_{\rm d} (\boldsymbol{x}_{i} ) }) \end{align} $

(5)

式中，$ \omega_{\rm s} $和$ \omega_{\rm d} $分别为目标路径平滑度优化项和距离原始点距离优化项的权重系数。

文[7]使用$ (\Delta \boldsymbol{x}_{i+1} -\Delta \boldsymbol{x}_{i} ) ^{2} $对路径的平滑度进行了定义，其中$ \Delta \boldsymbol{x}_{i} =\boldsymbol{x}_{i} -\boldsymbol{x}_{i-1} $，此平滑项直观上为最小化各点之间的位移差向量的差值，目的是使得相邻2个向量保持最均匀的位置差和方向差，以达到平滑的目的，据此构造平滑度优化目标函数：

$ \begin{align} J_{\rm s} (\boldsymbol{x}_{i} )=(\Delta \boldsymbol{x}_{i+1} -\Delta \boldsymbol{x}_{i} ) ^{2}, \; \; i=2, \cdots, N-1 \end{align} $

(6)

令$ {\boldsymbol{X}}_{\text{origin}} =[\boldsymbol{x}_{\rm o1} , \boldsymbol{x}_{\rm o2} , \cdots, \boldsymbol{x}_{{\rm o}N}] ^{\rm T} $，其中$ \boldsymbol{x}_{{\rm o}i} =(x_{{\rm o}i}, y_{{\rm o}i} ) $（$ i=0, 1, \cdots, N $）表示优化前的路径点坐标，为达到优化后路径和原始路径保持较高拟合度的目的，为每一个路径点设置距离原始点的距离优化项，其计算方法为距离二次项：

$ \begin{align} J_{\rm d} (\boldsymbol{x}_{i})=( \boldsymbol{x}_{i} -\boldsymbol{x}_{{\rm o}i} ) ^{2}, \; \; i=1, \cdots, N \end{align} $

(7)

采用为每一个决策变量设置矩形优化区域的方式约束优化范围。设置各点的调整区域，以保证优化结果与原路径保持形状一致。设矩形优化区域的长度和宽度分别为$ L $和$ W $，如图 4所示，用$ {\varOmega}_{i} $ $ = $ $ \{{(x, y) \big| {| {{\boldsymbol{x}}-\boldsymbol{x}_{{\rm o}i}} |\leqslant L, \; |{\boldsymbol{y}}-{\boldsymbol{y}}_{{\rm o}i} |\leqslant W}} \} $表示第$ i $个决策变量的调整区域，则约束条件可表示为$ \boldsymbol{x}_{i} -\boldsymbol{x}_{{\rm o}i} $ $ \in $ $ {\varOmega}_{i} $。

为了保证端点（2段轨迹的连接点）处的曲率连续，在端点处构建渐近约束：以$ d_{\rm r} $表示距离端点的距离，构建函数$ f(d_{\rm r} )=( d_{\max} -d_{\rm r} ) ^{8}+1 $，分别以$ L/f(d_{\rm r} ) $和$ W/f(d_{\rm r} ) $表示端点附近（$ d_{\rm r}\leqslant d_{\max} $）路径点的矩形优化区域的长和宽。得到矩形区域的长度（宽度）的变化趋势，如图 5所示，在逐渐接近起点和终点时（$ d_{\rm r} $趋近0），调整区域趋向于0，在调整点逐渐接近$ d_{\max} $时调整区域趋向于矩形调整区域$ {\varOmega} $，以确保起点和终点处的位置和朝向的渐近变化。

3.2 二次规划型求解

由式(4)~式(7) 可知，优化目标函数最高次均为二次，约束条件均可转化为线性。因此，将以上目标函数转化为式(8) 所示的二次规划型求解。

$ \begin{align} \min \frac{1}{2}{\boldsymbol{X}}^{\rm T}{\boldsymbol{QX}}+{\boldsymbol{C}}^{\rm T}{\boldsymbol{X}}, \quad \text{s.t. }\; \; {\boldsymbol{L}}\leqslant {\boldsymbol{AX}}\leqslant {\boldsymbol{U}} \end{align} $

(8)

式中，$ \boldsymbol{Q} $为$ N\times N $的矩阵，$ \boldsymbol{C} $为$ N $维向量。$ \boldsymbol{A} $为$ M\times N $的矩阵，$ \boldsymbol{L} $、$ \boldsymbol{U} $分别为优化区域的上、下边界向量，为$ M $维向量。其中$ M $为约束条件的个数，$ N $为路径点的数量。根据式(5)~式(7)，$ \boldsymbol{Q} $可表示为

$ \begin{align} {\boldsymbol{Q}} = \begin{bmatrix} {\boldsymbol{Q}_{1}} & {\boldsymbol{Q}_{2}} & & & {\boldsymbol{0}} \\ & {\boldsymbol{Q}_{3}} & {\boldsymbol{Q}_{2}} & & \\ & & \ddots & \ddots& \\ & & & {\boldsymbol{Q}_{3}} & {\boldsymbol{Q}_{2}} \\ {\boldsymbol{0}} & & & & {\boldsymbol{Q}_{4}} \end{bmatrix} \end{align} $

(9)

式中，矩阵$ {\boldsymbol{Q}}_{1} =\begin{bmatrix} {\omega_{\rm s} +\omega_{\rm d}} & {-4\omega_{\rm s}-2\omega_{\rm d}} \\ {0} & {5\omega_{\rm s} +2\omega_{\rm d}} \end{bmatrix} $，$ {\boldsymbol{Q}}_{2} =\begin{bmatrix} {2\omega_{\rm s}} & 0 \\ {-8\omega_{\rm s}-2\omega_{\rm d}} & {2\omega_{\rm s}} \end{bmatrix} $，$ {\boldsymbol{Q}}_{3} =\begin{bmatrix} {6\omega_{\rm s} +2\omega_{\rm d}} & {-8\omega_{\rm s}-2\omega_{\rm d}} \\ {0} & {6\omega_{\rm s} +2\omega_{\rm d}} \end{bmatrix} $，$ {\boldsymbol{Q}}_{4} =\begin{bmatrix} {5\omega_{\rm s} +2\omega_{\rm d}} & {-4\omega_{\rm s}-2\omega_{\rm d}} \\ 0 & {\omega_{\rm s} +\omega_{\rm d}} \end{bmatrix} $。

一次项矩阵$ \boldsymbol{C} $仅与式(7) 相关：

$ \begin{align} {\boldsymbol{C}} =[-2\omega_{\rm d} x_{{\rm o}1} , \cdots , -2\omega_{\rm d} x_{{\rm o}N} ]^{\rm T} \end{align} $

(10)

根据矩形优化区域，以$ {\boldsymbol{X}}_{\text{bound}} $表示约束区域矩阵，可得约束条件中的各矩阵：

$ \begin{align} \begin{cases} {\boldsymbol{L}}={\boldsymbol{X}}_{\text{origin}} -{\boldsymbol{X}}_{\text{bound}} \\ {\boldsymbol{U}}={\boldsymbol{X}}_{\text{origin}} +{\boldsymbol{X}}_{\text{bound}} \end{cases} \end{align} $

(11)

4 实验与分析（Experiment and analysis）

为了验证所提出的路径重组和优化方法，对多段具有代表性的路径进行采集，分别使用曲线拟合法、数值优化法和本文方法进行处理，并对实验结果进行定性和定量分析。使用的地图采集车为图 6所示的“军交猛狮”无人车，该车搭载RTK-GNSS设备，通过地基增强和微惯性导航系统以20 Hz频率输出精度可达厘米级的定位信息，以固定间距存储经度和纬度，达到对密集型定位数据的清洗作用。

4.1 定性分析

为了验证算法的整体性能，对不同倒车次数和多种道路形态下的实车行驶路径进行采集，分别采集1次倒车、2次倒车、3次倒车情况下的GNSS路径，采集场景覆盖直角弯、弧线弯和直线等不同道路形态。分别使用本文重组优化方法、B样条拟合法^[15]和多目标优化方法^[10]对初始采集路径进行处理，并使用本文优化方法和B样条拟合法对重组结果进行优化后处理。

将“军交猛狮”无人平台采集的原始GNSS路径转化到直角坐标系中，计算路径点航向，然后按照航向识别路径行驶方向，使用DRD分段法将其划分为D、R路段组合，得到如图 7中实线所示路径，图中箭头方向为采集路径点的方向。其中场景1为单次倒车的路段，其倒车路段长度为31 m；场景2和场景3中存在2次倒车，其中场景3的2次倒车路径长度存在差异（分别为8 m和18 m）；场景4中存在3次倒车，并对其调整行驶方向的区域进行了局部放大。按照本文方法对DRD分段后的路段进行无效路段剔除、自适应分段重组和路径后处理，得到如图 7中点虚线所示的处理结果。根据实验结果可知DRD分段法能够有效处理不同倒车距离下的无效路段，不同长度的无效路段经过第2次自适应分段后不再受到无效路段长度的影响，按照相同的规则进行剔除和重组。在处理过程中，场景2和场景3中的重组路段相互“交叉”，按照式(2) 进行重组；场景1和场景4中的重组路段按照式(3) 进行重组。场景2、场景3、场景4为存在多次倒车而产生“之字形”形式的无效路段，其中场景2与场景3在经过第2次自适应分段后均形成了DRDRD形式的路段组合，场景4分段为DRDRDRD形式，根据剔除规则和重组规则均能得到准确有效的路径。

采用不同的方法对原始采集路径进行处理，结果如图 8所示。由图 8可知，直接通过B样条拟合和数值优化方法仅能够对初始路径进行平滑和对所采集路径中的“尖角”进行处理，但得到的弧线仍然存在圆弧锐角，在单次倒车和“之字形”场景中均未能有效地剔除无效路段，随着无效路段的长度增加，平滑效果逐渐变差，且所得的结果不符合机器人使用要求。以上实验结果说明，B样条拟合和数值优化法均不能达到剔除无效路段、平滑有效路段的目的。

使用文[15]方法和本文方法对重组后的路径进行优化，并对其优化路径的曲率进行计算，如图 9所示。文[15]和本文方法均能够很好地拟合重组路径。但由图 9中的曲率对比曲线可知，文[15]方法中起点和终点与原有路径曲率产生了较大差异，本文方法中起点和终点位置均与原有路径保持一样的曲率；文[15]得到的B样条曲率是连续变化的，但其多处位置存在曲率变化“尖角”的情况，曲率峰值和变化速度均大于本文方法，表明本文方法得到的优化结果更加平滑。

其次，将本文方法处理后的路径用于2种不同大小的轮式无人车，2种平台质量分别为9.5 kg（电动车）和2000 kg（汽油车），轴距分别为40 cm和295 cm，转向方式均为前轮阿克曼式转向^{[16, 18]}。实验车速分别为2.4 m/s和10 m/s，采用文[16, 18]的控制方法及其无人平台对本文处理轨迹进行跟随，横向偏差均控制在0.6 m以内，反映了该路径的可行性。

4.2 定量分析

参考文[17]中的路径平滑性指标，本文对路径离散采样点的曲率绝对值之和$ f_{\rm c} $进行计算，以作为评估算法平滑效果的依据：同一路径，$ f_{\rm c} $越小，平滑效果越好。本文优化前后均为离散点，各点间存在一一对应关系，按照式(12) 对路径优化前后各点位置差$ e_{i} $进行计算，将位置平均误差值$ e_{\rm a} $作为原路径形状保持能力的评价指标。B样条拟合结果和优化前的离散点不存在一一对应关系，因此不对其平均位置误差值进行计算。

$ \begin{align} \begin{cases} e_{i} =| {\boldsymbol{x}_{i} -\boldsymbol{x}_{{\rm o}i}} | \\ e_{\rm a} =\dfrac{\sum\limits_1^N {e_{i}}} {N} \end{cases} \end{align} $

(12)

图 10为优化结果和重组结果中各点的位置差，本文方法的位置差最大值小于0.35 m。由图 9和图 10可得，本文方法的优化结果中起点和终点的位置、曲率和航向角均与相邻点保持了一致，达到了保持原路径形状的目的，能够与所采集路径有效衔接。

分别计算各点在优化前的曲率绝对值之和$ f_{\text{c1}} $、优化后的曲率绝对值之和$ f_{\text{c2}} $、位置平均误差值$ e_{\rm a} $，结果如表 1所示。由表 1可得，文[15]方法具备对杂乱点快速拟合的优点，但根据$ f_{\rm c} $可知文[15]方法容易出现过拟合问题，导致场景1、场景2和场景4中路径优化后总曲率数值反而大于优化前；本文方法在各场景下路径优化后$ f_{\rm c} $均小于优化前，与重组路径的平均位置误差均在0.2 m以内，各场景下本文得到的$ f_{\text{c2}} $均小于文[15]的$ f_{\text{c2}} $。本文使用矩形优化区域缩小了求解空间，并转化为二次规划求解，故耗时均远小于文[15]方法。

5 结论（Conclusion）

提出了一种基于分段重组和后处理的GNSS路径处理方法，为达到保持采集路径原有形态的目的，提出DRD分段法对采集路径进行路段划分，考虑多次反复倒车、前行情景和不同道路形态场景设定无效路段剔除规则，在DRD路段中根据邻域长度进行第2次自适应分段并重组，从采集路径中高效地筛选到可用的有效路段；为实现筛选路段与原有路段平滑衔接的目的，构建优化目标和渐近端点约束，对重组路径进行平滑后处理。多种场景下的实验结果表明该方法能够解决直接拟合法和数值优化法无法处理的无效路径问题，并与原有路径保持曲率连续，与文[15]方法的对比结果说明，本文方法能够得到更为平滑的路径，符合轮式移动机器人的运动学特征，且耗时仅为文[15]方法的1/5。

该方法在民用中可应用于高精度地图更新和车辆编队行驶，在军用中可用于无人平台自主返航和简易地图采集等场景。下一步尝试使用优化方法对部分丢失卫星信号的定位数据进行滤波和平滑处理，以更好地满足实际应用中的需要。