一种基于车载POS的公路线形特征识别与参数计算方法与流程

本发明属于组合导航定位系统和测量技术领域，具体涉及一种基于车载POS的公路线形特征识别与参数计算方法。

背景技术：

公路线形数据采集与计算是公路普及调查过程中的重要工作。目前，国内外对道路线形计算的研究方法主要分两种：(1)以数学模型为基础研究公路设计基本元素的选取组合及线形拟合；(2)结合CAD技术研究公路设计基本元素的选取组合及线形拟合。国外相关学者倾向于先进行公路线形识别再研究，包括弧长-曲率法，斜率法与综合法。国内相关学者倾向于基于国外研究理论的基础上对道路平面线形进行的深入的研究，其中具有代表性的是：

(1)武汉大学的童小华教授提出基于曲率-弧长、斜率判别法的道路线形识别方法，该方法分别建立道路曲线基本单元的基本平差模型，并建立以基本单元组合的道路曲线的联合平差模型，以及分段的道路曲线的整体平差模型。

(2)同济大学姚连璧教授提出基于曲率判别法的方位角法的道路线形识别方法，该方法应用于道路无缓和曲线情况下，利用车载GPS获取公路离散坐标，利用方位角法对线形进行识别。计算直线及圆曲线的重心化参数，并利用平差后的直线及圆曲线参数计算缓和曲线参数，最后对三种线形连接处应用加权平均法和道路设计思路相结合得到比较精确的道路平面线形几何参数。

(3)同济大学杨轸博士在道路平面线形的研究中，将拟合曲线或最小时得到的拟合曲线作为最佳拟合曲线，其中h为采样点到拟合曲线的垂距。

(4)采用CAD样条曲线法，输入测设里程坐标绘制样条曲线，剔除误差较大点后，粗略判断基本元素的位置，绘制CAD直线、圆曲线与样条曲线对比，得到拟合曲线参数。

综上所述，采用CAD样条法有较多人为因素干扰，不利于公路线形自动识别与计算理论的研究。建立在曲率法、方位角法及综合法基础之上的联合平差法、方位角法及最小二乘法均采用车载GPS数据进行公路线形参数计算方法研究，相对于传统方法简单有效，但在隧道等复杂环境下，卫星信号受遮挡或多路径影响，且车体无法保证严格按照中线行驶和车体抖动等原因，计算出的曲率或方位角会基于一定趋势波动，即便选取特定的采样步长计算，也会损失判断精度，不利于线形的识别与计算。

技术实现要素：

针对上述现有技术存在的不足，本发明提供一种基于车载POS的公路线形特征识别与参数计算方法。

本发明的技术方案：

一种基于车载POS的公路线形特征识别与参数计算方法，包括如下步骤：

步骤1：在车辆行进过程中，车载POS(Position and Orientation System)实时采集陀螺角速率、比例、伪距及相位信息，并通过后处理解算生成车辆位置参数、速度参数及姿态参数的时间序列；

步骤2：根据车辆姿态参数提取航向角信息，并利用车辆坐标参数和车辆速度推算采样点里程；

步骤3：利用航向角信息和采样点里程计算全路段概略曲率分布即初始公路线形特征分布，并将初始公路线形暂时分为直线部分和圆曲线部分：

步骤3.1：采用均值加速中值滤波方法以20m为步长分段平滑航向角，得到全路段航向角分布：

步骤3.1.1：计算滤波子窗口内航向角的均值；

步骤3.1.2：将滤波子窗口内航向角分为大于等于该航向角的均值和小于该航向角的均值的两个航向角集合；

步骤3.1.3：舍弃元素个数少的航向角集合，对元素个数大于滤波子窗口内航向角的总个数一半的航向角集合重复步骤3.1.1和步骤3.1.2，直到分解得到的两个航向角集合中的元素个数均小于滤波子窗口内原航向角总个数的一半；

步骤3.1.4：选择得到的两个航向角集合中元素个数少的航向角集合，对该集合中航向角进行快速排序，并取中间航向角作为该滤波子窗口即步长内的航向角；

步骤3.1.5：依次对各滤波子窗口重复步骤3.1.1到步骤3.1.4，得到各滤波子窗口的航向角，统计并生成全路段航向角随里程分布；

步骤3.2：根据航向角与里程的关系，以20m为步长求得全路段概略曲率分布，得到初始公路线形特征分布；

步骤3.3：设定直线曲率阈值，暂时将初始公路线形分为直线部分和圆曲线部分，所述直线部分和圆曲线两部分均可能包括缓和曲线部分。

步骤4：分别调整直线部分中各直线的窗口大小和圆曲线部分中各圆曲线的窗口大小，提取窗口识别过程中筛选出的缓和曲线，并根据直线线形参数与圆曲线线形参数计算缓和曲线线形参数；

步骤4.1：取初始公路线形中直线部分中一段直线，构建直线方程：yi＝axi+b，其中，(xi,yi)为该段直线的平面坐标即该段直线上采样点的平面坐标，a、b分别为直线方程斜率和截据；

步骤4.2：利用最小二乘法计算参数a、b，得到最小二乘法拟合直线方程；

步骤4.3：计算(xi,yi)在拟合直线上的投影坐标、拟合残差，并根据拟合残差计算该段直线拟合中误差；

步骤4.4：根据拟合中误差大小及直线两端航向角变化确定直线窗口伸缩方向：

若拟合中误差小于阈值，说明该直线窗口识别不完整，需要扩展，反之需要缩小，理论上直线曲率为零，则窗口可以向航向角变化较小的一端扩展或在航向角变化较大的一端缩小，并判断拟合中误差是否大于阈值，是，恢复上次窗口扩展程度，执行步骤4.6，否则，执行步骤4.1，所述航向角变化大小即采样点间的航向角差值大小。

步骤4.5：重复步骤4.1至步骤4.4，直到完成全路段直线部分中所有直线的线形特征识别与参数计算；

步骤4.6：取初始公路线形圆曲线部分中一段圆曲线，构建拟合圆曲线方程：其中，(xj,yj)为该圆曲线的平面坐标即该段圆曲线上采样点的平面坐标，(c，d)为圆心坐标，r为曲率半径；

步骤4.7：利用最小二乘法计算参数c、d和c²+d²+r²，得到最小二乘法拟合圆曲线方程；

步骤4.8：计算(xj,yj)在最小二乘法拟合圆曲线上的投影坐标、拟合残差和该段圆曲线拟合中误差：

步骤4.9：根据拟合中误差大小及该圆曲线两端航向角变化确定圆曲线窗口伸缩方向：

若拟合中误差小于阈值，说明该圆曲线窗口识别不完整，需要扩展，反之需要缩小，理论上圆曲线曲率为常值，则窗口可以向航向角变化较大的一端扩展或在航向角变化较小的一端缩小，并判断拟合中误差是否大于阈值，是，恢复上次窗口扩展程度，认为该段线形为圆曲线，计算该圆曲线的曲率为1/r，执行步骤4.10，否则，执行步骤4.6；

步骤4.10：重复步骤4.6至步骤4.9，直到完成全路段圆曲线部分中所有圆曲线的线形特征识别与参数计算；

步骤4.11：识别计算直线及圆曲线过程中筛选出的连接圆曲线与直线的缓和曲线，并根据直缓点与缓圆点的里程及曲率，建立缓和曲线的曲率-里程方程：k＝ms+n，其中，k、s分别表示缓和曲线的曲率和里程；所述直缓点为直线与缓和曲线交点，所述缓圆点为缓和曲线与圆曲线交点；

步骤4.12：根据曲率-里程方程，计算该段缓和曲线内采样点曲率，并根据采样点曲率占缓和曲线曲率变化比例结合直缓点与缓圆点平差后的坐标参数内插出缓和曲线离散坐标；

步骤5：根据平差后的直线线形参数、平差后的圆曲线线形参数及缓和曲线线形参数统计全路段曲率随里程的变化，得到全路段最优化线形特征及参数。

有益效果：一种基于车载POS数据采集的公路线形特征识别与参数计算方法与现有技术相比，具有如下优势：

1、采用的数据采集方法在提供了高精度的坐标信息的同时，还能提供载体的姿态信息，提高了公路线形初步识别与线形窗口调节的效率及精度；

2、利用的坐标及姿态信息有车载POS系统差分GNSS伪距及相位与INS的紧组合处理获得。两种技术的优势互补，以长时间提供高精度位置、速度及姿态信息；

3、利用均值加速中值滤波方法(MFM)平滑航向角信息，该方法即能最大程度降低白噪声影响又能有效剔除颗粒噪声，还能克服中值滤波庞大计算量的问题，为线性初步识别提供指导；

4、提出了一种基于航向角变化及拟合中误差的线形窗口自动识别方法，可以在粗略识别线形的基础上更准确地识别出直线、圆曲线及缓和曲线的区间，相对于CAD法，更大程度降低了人为因素影响，并降低工作量。

附图说明

图1为本发明一种实施方式的一种基于车载POS数据采集的公路线形特征识别与参数计算方法流程图；

图2为本发明一种实施方式的平滑航向角的方法流程图；

图3为本发明一种实施方式的航向角平滑结果与初始公路线形示意图，其中，(a)为航向角平滑结果图，(b)为初始公路线形图；

图4为本发明一种实施方式的直线窗口识别过程流程图；

图5为本发明一种实施方式的圆曲线窗口识别过程流程图；

图6为本发明一种实施方式的某段圆曲线窗口自动识别过程示意图，其中，(a)为圆曲线窗口大小示意图，(b)为圆曲线窗口调整过程中拟合误差图，(c)为为圆曲线窗口起始位置图；(d)为圆曲线窗口末端位置图；

图7为本发明一种实施方式的最终生成最优拟合轨迹图；

图8为本发明一种实施方式的全路段计算曲率分布图；

图9为本发明一种实施方式的全路段拟合残差图。

具体实施方式

下面结合附图对本发明的一种实施方式作详细说明。

本实施方式中，以京承公路某路段(全程20km)的线形特征识别与参数计算为例，针对本发明中的步骤，分别从线形初步识别结果、其中一段直线的窗口自动识别结果、全路段线形特征分布及全路段拟合偏差来为说明本方法的可行性和有效性。

POS(Position Orientation System)是基于GNSS(Global Navigation Satellite System)/INS(Inertial Navigation System)组合导航原理的一种定位定姿系统，其组合方式主要分为松组合、紧组合与深组合3种。相对于INS和GNSS的优势是导航定位的误差不随时间积累，且可提供较高精度的定位、测速及授时信息；不依赖于外部信息，完全独立自主、全天候且不存在信号失锁问题的导航系统，并可提供更加丰富的导航信息，且短期内载体的位置、速度及姿态信息精度较高。将二者组合在一起可以实现两种技术的优势互补，以长时间提供精度均较高的完整导航参数，包括位置、速度和姿态。在GNSS/INS组合导航系统中，高精度的GNSS信息，用以估计INS误差参数，控制其误差随时间的积累，另一方面INS对GNSS导航定位结果进行了平滑并弥补了其信号中断。本发明所利用的车载POS系统获得的坐标与姿态信息由差分GNSS伪距及相位与INS的紧组合处理获得，位置精度可达厘米级，航向角精度可达0.001度，俯仰角及横滚角精度可达0.0005度。较高精度的位置和姿态信息对线形的识别与计算工作有重要意义。

如图1所示，一种基于车载POS的公路线形特征识别与参数计算方法，包括如下步骤：

步骤1：在车辆行进过程中，车载POS(Position and Orientation System)实时采集陀螺角速率、比例、伪距及相位信息，并通过后处理解算生成车辆位置参数、速度参数及姿态参数的时间序列；

步骤2：根据车辆姿态参数提取航向角信息，并利用车辆坐标参数和车辆速度推算采样点里程；

步骤3：利用航向角信息和采样点里程计算全路段概略曲率分布即初始公路线形特征分布，并将初始公路线形暂时分为直线部分和圆曲线部分：

步骤3.1：如图2所示，采用均值加速中值滤波方法以20m为步长分段平滑航向角，得到全路段航向角分布：

步骤3.1.1：计算滤波子窗口内航向角的均值；

步骤3.1.2：将滤波子窗口内航向角分为大于等于该航向角的均值和小于该航向角的均值的两个航向角集合；

步骤3.1.3：舍弃元素个数少的航向角集合，对元素个数大于滤波子窗口内航向角的总个数一半的航向角集合重复步骤3.1.1和步骤3.1.2，直到分解得到的两个航向角集合中的元素个数均小于滤波子窗口内原航向角总个数的一半；

步骤3.1.4：选择得到的两个航向角集合中元素个数少的航向角集合，对该集合中航向角进行快速排序，并取中间航向角作为该滤波子窗口即步长内的航向角；

步骤3.1.5：依次对各滤波子窗口重复步骤3.1.1到步骤3.1.4，得到各滤波子窗口的航向角，统计并生成全路段航向角随里程分布；

步骤3.2：根据里程、航向角及曲率的关系以20m为步长求得全路段概略曲率分布，得到初始公路线形特征分布，其中，α为航向角，s为单位里程，K为曲率；

本实施方式中，线形初步识别结果如图3所示；根据曲率分布的初步识别结果可以较清晰地区分出线形特征；

步骤3.3：设定直线曲率阈值，暂时将初始公路线形分为直线部分和圆曲线部分，所述直线部分和圆曲线两部分均可能包括缓和曲线部分。

本实施方式中，拟定直线段初始曲率的绝对值不大于0.02，此时的曲率分布不代表真实道路曲率。如图3所示，在700至1400m之间的曲率分布存在3处粗差，原因可能是车体靠边停车或变换车道所致。这类粗差影响会在线形窗口自动识别与参数计算过程中被剔除。

步骤4：对全路段平面坐标即车辆平面坐标参数进行拟合，统计拟合残差，计算每段直线和圆曲线的拟合中误差，并根据拟合中误差大小及初始公路线形的航向角变化调整直线和圆曲线的窗口的大小和窗口伸缩方向，最后根据平差后的直线线形参数与圆曲线线形参数计算缓和曲线线形参数：

步骤4.1：如图4所示，结合实验环境及实验流程，设定拟合中误差阈值，取初始公路线形直线部分中一段直线，构建直线方程：yi＝axi+b，其中，(xi,yi)为该段直线的平面坐标即该直线上采样点的平面坐标，a、b分别为直线方程斜率和截据；

步骤4.2：则构建等精度最小二乘法方程(B^TB)X＝B^TL，即：

其中：n为直线上采样点个数，i＝1,2，…，n；

由上式解得参数a、b：

X＝(a b)^T＝(B^TB)^-1B^TL (2)

步骤4.3：计算(xi,yi)在最小二乘法拟合直线上的投影坐标(xi0,yi0)、拟合残差errori和该段直线拟合中误差E：

步骤4.4：根据拟合中误差大小及直线两端航向角变化确定直线窗口伸缩方向：

若拟合中误差小于阈值，说明该直线窗口识别不完整，需要扩展，反之需要缩小，理论上直线段曲率为零，则窗口可以向航向角变化较小的一端扩展或在航向角变化较大的一端缩小，并判断拟合中误差是否大于阈值，是，恢复上次窗口扩展程度，执行步骤4.6，否则，执行步骤4.1，所述航向角变化大小即采样点间的航向角差值大小。

本实施方式中，以某段直线线形窗口线形识别过程为例，如图4所示，随着该直线线形首段和末端位置的变化线形窗口大小及拟合中误差不断增大，当拟合中误差大于一定阈值时，窗口大小会恢复到上次扩展的程度，并记录拟合中误差、窗口间隔的历元数及首末端的位置，此时，线形窗口识别已达到最大程度上的吻合，其中，图4中扩展因子e的作用就是记录线形窗口的调节状态，避免窗口在接近阈值附近重复扩展和缩小；

步骤4.5：重复步骤4.1至4.4，直到完成全路段直线部分中所有直线的线形特征识别与参数计算；

步骤4.6：如图5所示，取初始公路线形圆曲线部分中一段圆曲线，构建拟合圆曲线方程：其中，(xj,yj)为该圆曲线的平面坐标即该段圆曲线上采样点的平面坐标，(c，d)为圆心坐标，r为曲率半径；

步骤4.7：构建参数为c、d、c²+d²+r²的观测方程BX＝L，其中：

其中，m为圆曲线段上采样点个数，j＝1,2，…，m；

构建等精度法方程(B^TB)X＝B^TL，其中：

根据等精度法方程计算参数c、d和c²+d²+r²，得到最小二乘法拟合圆曲线方程；

步骤4.8：计算(xj,yj)在最小二乘法拟合圆曲线上的投影坐标(xj0,yj0)、拟合残差errorj和该段圆曲线拟合中误差E'：

其中，Δy＝yj-yj0，Δx＝xj-xj0。

步骤4.9：根据拟合中误差大小及圆曲线两端航向角变化确定圆曲线窗口伸缩方向：

若拟合中误差小于阈值，说明该圆曲线窗口识别不完整，需要扩展，反之需要缩小，理论上圆曲线段曲率为常值，则窗口可以向航向角变化较大的一端扩展或在航向角变化较小的一端缩小，并判断拟合中误差是否大于阈值，是，恢复上次窗口扩展程度，认为该段线形为圆曲线，计算该段圆曲线的曲率为1/r，执行步骤4.10，否则，执行步骤4.6；

本实施方式中，以某段某段圆曲线窗口线形识别过程为例如图5-6所示，随着该圆曲线线形首段和末端位置的变化线形窗口大小及拟合中误差不断增大，当拟合中误差大于一定阈值时，窗口大小会恢复到上次扩展的程度，并记录拟合中误差、窗口间隔的历元数及首末端的位置，此时，线形窗口识别已达到最大程度上的吻合，其中，图5中扩展因子e的作用就是记录线形窗口的调节状态，避免窗口在接近阈值附近重复扩展和缩小；

步骤4.10：重复步骤4.6至4.9，直到完成全路段圆曲线部分中所有圆曲线的线形特征识别与参数计算；

步骤4.11：识别计算直线及圆曲线过程中筛选出的连接圆曲线与直线的缓和曲线，根据直缓点与缓圆点的里程及曲率，建立缓和曲线的曲率-里程方程：k＝ms+n，其中，k、s分别表示缓和曲线的曲率和里程；所述直缓点为直线与缓和曲线交点，所述缓圆点为缓和曲线与圆曲线交点；

步骤4.12：根据采样点曲率占缓和曲线曲率变化比例内插出缓和曲线离散坐标：令缓和曲线起始端曲率与里程分别为KS和SS，末端的曲率与里程分别为KE和SE，计算系数m、n后，结合采样点里程计算采样点的曲率Kl及Kl占曲率变化的比例scalel：

scalel＝(Kl-KS)/(KE-KS) (6)

令ΔX＝XE-XS,ΔY＝YE-YS，缓和曲线起点S坐标为(XS,YS)，则计算缓曲线内采样点i内插坐标为：

为了保证缓和曲线平滑过渡，可以通过增加采样率的方法来加密缓和曲线内插点；

步骤5：根据平差后的直线线形参数、平差后的圆曲线线形参数及缓和曲线线形参数统计全路段曲率随里程的变化，得到全路段最优化线形特征及参数；

本实施方式中，最终生成最优化拟合轨迹即全路段最优化线形特征如图7所示，全路段计算曲率如图8所示。

步骤6：统计拟合中误差以反映基于车载POS数据采集公路线形识别与参数计算效果。

本实施方式中，全路段拟合残差分布如图9所示，根据拟合残差分布，统计直线、圆曲线及缓和曲线的拟合中误差，直线段拟合中误差±0.610m，圆曲线段拟合中误差±0.499m，缓和曲线段拟合中误差±0.740m。整体轨迹±0.573m，最大拟合偏差0.657m，最小拟合偏差0.021m。拟合残差呈现出较大的随机性而非系统性，可以认为这种随机误差主要由车体不能严格按照中线行驶所致。若能让车体严格沿中线行驶，拟合精度不仅局限于此。

以上所述，仅为本发明中最基础的具体实施方式，但本发明的保护范围并不局限于此，任何本技术领域人士在本发明所揭露的技术范围内，可理解到的替换，都应涵盖在本发明的包含范围之内。因此，本发明的保护范围应该以权利要求书的保护范围为准。