随着智能交通系统和5G通讯的发展，无人驾驶车辆定位的研究方向由车载传感器定位逐渐演变为车路协同定位，如Vehicle to Infrastructure（V2I）技术. 在校园、社区等非结构化场景中，一般有树木遮挡、环境特征少及缺乏道路信息等特点，持续横向运动时，定位存在累积误差大、精度低等问题. 研究人员从不同方向探索以实现高精度定位.

一个方向是车载多传感器融合，包括全球定位系统（global positioning system，GPS）、惯性传感器、激光雷达以及摄像头等多个组合^[1-4]，但是GPS和惯性传感器很难平衡成本与性能，激光雷达性价比较高，但存在累积误差，无法持续定位. V2I协同定位技术成为另一个热门方向，包括车载传感器与路侧地图的协同地图匹配^[5-7]、车载传感器与路侧设备的融合，例如专用短程通讯设备^[8]、射频识别^[9]等，但在横向运动时均无法很好地获取航向角，横向定位误差大. Mousavian等^[10-12]利用摄像头进行三维目标检测，获取车辆位姿（即位置和航向角），但实时性差，易受虚假目标的干扰.

车载传感器与路侧设备的位姿融合本质上为多源信息融合，广泛采用紧耦合，例如采用扩展卡尔曼滤波（extended Kalman filter，EKF）^[13-14]、无迹卡尔曼滤波^[15]、贝叶斯融合^[16]以及结合地图的证据理论^[17]，但对于紧耦合类型，抗干扰和自主调节效果较差. 刘江等^[18]结合联邦卡尔曼滤波，增强信息融合后的调节能力，这种松耦合类型的子滤波器一般为卡尔曼滤波器（Kalman filter，KF），应用范围有限. 夏楠等^[19]结合粒子滤波与卡尔曼滤波的非线性，给不同类型的子滤波器耦合提供了启发.

本文提出双层融合协同定位算法，采用激光雷达作为车载传感器，双目相机作为路侧摄像头. 下层为2个并行的子滤波器，分别为自适应蒙特卡洛定位（adaptive Monte Carlo localization，AMCL）和基于概率数据关联（probability data association，PDA）的KF，相应地处理激光雷达与路侧摄像头获取的位姿. 上层为全局融合估计，实现了位姿融合和结果的反馈. 该算法融合基于双地图切换机制的AMCL和基于PDA的KF滤波器，建立反馈机制，以低成本实现了车辆在非结构化场景中的高精度定位；利用多种定位算法进行横向对比，结合摄像头数量进行纵向对比，验证了双层融合协同定位算法的定位精度. 该算法对车路协同在路径规划、避障等无人驾驶领域中的应用具有重大意义.

协同定位系统如图1所示，由车辆位姿（位置和航向角）测量模块和双层融合协同定位算法组成.

车辆位姿测量模块包含激光雷达和路侧摄像头. 其中，车载激光雷达利用点到线距离的迭代最近点（point to line iterative closest point，PLICP）算法，实时计算车辆位姿；路侧摄像头通过三维目标检测算法获取车辆位姿，将目标检测与阿克曼转向运动模型结合，周期性地发布车辆位姿.

在双层融合协同定位中，下层包含2个并行位姿估计. AMCL位姿估计结合双地图切换机制，利用短期和长期估计，实现静态全局地图和动态局部地图的切换，修正PLICP的累积误差；基于PDA的KF位姿估计消除虚假目标干扰，实现路侧摄像头的目标跟踪. 上层为全局融合估计，将位姿和协方差反馈至AMCL、KF及PLICP，以实现自主调节.

如图2所示为坐标系变换示意图，结合图1的变量作以下说明.

1）地图坐标系视为世界坐标系，位姿估计结果 $\hat {{X}}_t^{\rm{L}} {\text{、}}\hat {{X}}_t^{\rm{c}}{\text{、}}{\hat {{X}}_t}$为地图坐标系下的绝对位姿.

2）里程计坐标系原点为运动起始时刻激光雷达在地图坐标系下的绝对位姿 ${{{\varGamma}} _{\rm{L}}} = {\left[ {{x_{\rm{L}}},{y_{\rm{L}}},{\theta _{\rm{L}}}} \right]^{\rm{T}}}$，激光雷达的位姿观测量为里程计坐标系下的相对位姿，与 ${{{\varGamma}} _{\rm{L}}}$矢量叠加可以转换为地图坐标系下的绝对位姿 ${{Z}}_t^{\rm{L}}$，L指激光雷达.

3）相机坐标系原点为路侧摄像头在地图坐标系中的位姿 ${{{\varGamma}} _{\rm{c}}} = {\left[ {{x_{\rm{c}}},{y_{\rm{c}}},{\theta _{\rm{c}}}} \right]^{\rm{T}}}$. 路侧摄像头的位姿观测量为相机坐标系下的相对位姿，与 ${{{\varGamma}} _{\rm{c}}}$矢量叠加可以转换为地图坐标系中的绝对位姿 ${{Z}}_t^{\rm{c}}$，c指摄像头.

4）车辆坐标系原点为车辆质心，激光雷达坐标系原点为激光雷达质心，激光雷达在车辆坐标系中的相对位姿为 ${{\varGamma}} _{\rm{v}}^{\rm{L}} = {\left[ {x_{\rm{v}}^{\rm{L}},y_{\rm{v}}^{\rm{L}},\theta _{\rm{v}}^{\rm{L}}} \right]^{\rm{T}}}$，v指车辆.

激光雷达利用PLICP算法^[20]，计算一组平移和旋转变换，使得前、后两帧点云达到最大重叠.

根据已知的位姿变换 ${{{q}}_{k{\rm{ - }}1}} = \left[ {{{R}}\left( {{\theta _{k{\rm{ - }}1}}} \right),{{{T}}_{k{\rm{ - }}1}}} \right]$，将t时刻当前扫描帧 ${{{F}}_t}$的点云坐标 ${{{p}}_i}$映射到参考帧 ${{{F}}_{t - 1}}$坐标系下，可得映射点坐标 ${{p}}_i^{\rm{map}}$，如下：

(1) ${{p}}_i^{\rm{map}} \triangleq {{{p}}_i} \oplus {{{q}}_{k - 1}} \triangleq {{R}}\left( {{\theta _{k - 1}}} \right){{{p}}_i} + {{{T}}_{k - 1}}.$

式中： $i$为当前扫描帧中点云序号， $k$为迭代次数.

根据欧式距离，在参考帧里匹配2个离映射点 ${{p}}_i^{\rm{map}}$最近的对应点，构成向量 $\left[ {{{p}}_i^{{\rm{map}}},\;{{p}}_{j1}^i,\;{{p}}_{j2}^i} \right]$，其中 $j$为参考扫描帧中点云序号. 若干上述向量组成 ${{{C}}_k}$.

目标函数为当前扫描帧的点 ${{{p}}_i}$到对应线段 ${{p}}_{j1}^i{{p}}_{j2}^i$的距离平方和，根据最小化误差原则，利用拉格朗日求解位姿 ${{{q}}_k} = \left[ {{{R}}\left( {{\theta _k}} \right),{{{T}}_k}} \right]$，如下：

(2) $\left.\begin{array}{c} \varepsilon \left( {{{{q}}_{k - 1}},{{{C}}_k}} \right) = \sum\limits_i {{{\left( {{{v}}_i^{\rm{T}}\left[ {{{R}}\left( {{\theta _k}} \right){{{p}}_i} + {{{T}}_k} - {{p}}_{j1}^i} \right]} \right)}^2}} ; \\ \mathop {\min \; }\limits_{{{{q}}_k}} \varepsilon \left( {{{{q}}_{k - 1}},{{{C}}_k}} \right). \end{array} \right\}$

根据Censi^[20]的证明可知，必有闭环解. 当 $k \leqslant N$且 $\varepsilon \left( {{{{q}}_{k - 1}},{{{M}}_k}} \right) \leqslant {\varepsilon ^{'}}$时，扫描匹配成功，其中 $N$为最大迭代次数， ${\varepsilon ^{'}}$为最小误差阈值.

将位姿变换 ${{{q}}_k}$累加，可得 $t$时刻激光雷达观测量，通过与 ${{{\varGamma}} _{\rm{L}}}$矢量叠加，将激光雷达观测量转换为地图坐标系下的绝对位姿 ${{Z}}_t^{\rm{L}}$，如下：

(3) ${{Z}}_t^{\rm{L}}{\rm{ = }}{\left[ {x_t^{\rm{L}},y_t^{\rm{L}},z_t^{\rm{L}}} \right]^{\rm{T}}}{\rm{ = }}\sum\limits_{i = 1}^t {{{{q}}_{k,i}}} + {{{\varGamma}} _{\rm{L}}}.$

式中： ${{{q}}_{k,i}}$为 $t$时刻闭环解对应的位姿变换 ${{{q}}_k}$.

相比于单目和RGBD相机，双目相机具有可间接获取深度信息、室外适应性好等特点，适合作为路侧摄像头. 双目相机利用Stereo R-CNN神经网络模型^[12]，实现三维目标检测，获取车辆航向角和尺度信息. 根据像素坐标系和相机坐标系的投影变换关系，求解车辆位置. 左目二维检测框的左边界 ${u_{\rm{l}}}$、上边界 ${v_{\rm{t}}}$投影关系如下：

(4) $\left[ {\begin{array}{*{20}{c}} {{u_{\rm{l}}}} \\ {{v_{\rm{t}}}} \\ 1 \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} x \\ y \\ z \end{array}} \right] + \left[ {\begin{array}{*{20}{c}} {\cos \; \theta }&0&{\sin \; \theta } \\ 0&1&0 \\ {\sin \; \theta }&0&{\cos \; \theta } \end{array}} \right]\left[ {\begin{array}{*{20}{c}} { - {w}/{2}} \\ { - {h}/{2}} \\ { - {l}/{2}} \end{array}} \right].$

同理可得左目二维检测框的右边界 ${u_{\rm{r}}}$、下边界 ${v_{\rm{b}}}$和右目二维检测框的左右边界 $u_{\rm{l}}^{'}$、 $u_{\rm{r}}^{'}$以及透视关键点的u向坐标 ${u_{\rm{p}}}$等投影关系，整理后如下：

(5) $\left.\!\!\!\!\! \begin{array}{l} {u_{\rm{l}}} = \left( {x - w\cos\theta /2 - l\sin\theta /2} \right)/\left( {z + w\sin\theta /2 - l\cos\theta /2} \right), \\ {v_{\rm{t}}} = \left( {y - h/2} \right)/\left( {z + w\sin\theta /2 - l\cos\theta /2} \right), \\ {u_{\rm{r}}} = \left( {x + w\cos\theta /2 + l\sin\theta /2} \right)/\left( {z - w\sin\theta /2 + l\cos\theta /2} \right), \\ {v_{\rm{b}}} = \left( {y + h/2} \right)/\left( {z - w\sin\theta /2 + l\cos\theta /2} \right), \\ u_{\rm{l}}^{'} = \left( {x - {{b}} - w\cos\theta /2 - l\sin\theta /2} \right)/\left( {z + w\sin\theta /2 - l\cos\theta /2} \right), \\ u_{\rm{r}}^{'} = \left( {x - {{b}} + w\cos\theta /2 + l\sin\theta /2} \right)/\left( {z - w\sin\theta /2 + l\cos\theta /2} \right), \\ {u_{\rm{p}}} = \left( {x + w\cos\theta /2 - l\sin\theta /2} \right)/\left( {z - w\sin\theta /2 - l\cos\theta /2} \right). \end{array} \!\!\right\}$

式中： $\left( {l,w,h} \right)$为获取的车辆尺度信息，b为双目摄像头的基线，利用高斯牛顿法求解式（5）. ${\left[ {x,y,z,\theta } \right]^{\rm{T}}}$为车辆在相机坐标系下的位姿观测量，通过与 ${{{\varGamma}} _{\rm{c}}}$矢量叠加，将 $t$时刻路侧摄像头位姿观测量转换为地图坐标系下的绝对位姿 ${{Z}}_t^{\rm{c}}$，如下：

(6) ${{Z}}_t^{\rm{c}}{\rm{ = }}\left[ {\begin{array}{*{20}{c}} {x_t^{\rm{c}}} \\ {y_t^{\rm{c}}} \\ {\theta _t^{\rm{c}}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {\cos{\theta _{\rm{c}}}}&{\sin{\theta _{\rm{c}}}}&0 \\ {\sin{\theta _{\rm{c}}}}&{\cos{\theta _{\rm{c}}}}&0 \\ 0&0&1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} x \\ y \\ \theta \end{array}} \right] + \left[ {\begin{array}{*{20}{c}} {{x_{\rm{c}}}} \\ {{y_{\rm{c}}}} \\ {{\theta _{\rm{c}}}} \end{array}} \right].$

神经网络算法的实时性较差，为了提升算法的实时性，将三维目标检测与阿克曼转向运动模型结合. 已知目标检测的位姿发布周期为T，t时刻路侧摄像头位姿观测量见式（6），在t+1~t+T时刻，路侧摄像头位姿观测量如下：

(7) ${{Z}}_t^{\rm{c}}{\rm{ = }}{{Z}}_{t{\rm{ - }}1}^{\rm{c}}{\rm{ + }}\left[\!\!\! {\begin{array}{*{20}{c}} {\cos\theta _{t - 1}^{\rm{c}}}&0&0 \\ 0&{\sin\theta _{t - 1}^{\rm{c}}}&0 \\ 0&0&{{w^{- 1}}\arctan\;({{wd}}/{v})} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} v \\ v \\ w \end{array}} \!\!\!\right].$

式中：v为车辆纵向线速度，w为车辆角速度，d为车辆轴距.

设t时刻AMCL位姿估计的系统状态量为 $\hat {{X}}_t^{\rm{L}}{\rm{ = }}{\left[ {\hat x_t^{\rm{L}},\hat y_t^{\rm{L}},\hat \theta _t^{\rm{L}}} \right]^{\rm{T}}}$，流程如图3所示.

初始位姿为 $\hat {{X}}_0^{\rm{L}}{\rm{ = }}{{{\varGamma}} _{\rm{L}}} - {{\varGamma}} _{\rm{lidar}}^{\rm{car}}$，初始化粒子群. 基于采样的里程计运动模型模拟粒子运动，求解每个粒子位姿；根据似然域测量模型对每个粒子打分，以粒子中心作为AMCL的位姿估计. 利用粒子分数的短期与长期估计，实现双地图切换.

车辆模型为阿克曼模型，根据激光雷达位姿观测量 ${{Z}}_t^{\rm{L}}$、 ${{Z}}_{t - 1}^{\rm{L}}$，可得相邻两时刻的相对航向角 $\Delta \theta $和相对平移 $\Delta s$.

(8) $\Delta \theta \; {\rm{ = }} \; \theta _{t - 1}^{\rm{L}} - {\rm{arctan}} \; \left( {\frac{{y_t^{\rm{L}} - y_{t - 1}^{\rm{L}}}}{{x_t^{\rm{L}} - x_{t - 1}^{\rm{L}}}}} \right),$

(9) $\Delta s = \sqrt {{{\left( {x_t^{\rm{L}} - x_{t - 1}^{\rm{L}}} \right)}^2} + {{\left( {y_t^{\rm{L}} - y_{t - 1}^{\rm{L}}} \right)}^2}} .$

根据采样算法建立运动误差模型，以经验值取误差参数 ${{\rm{\alpha}} _1}{\text{、}}{{\rm{\alpha}} _2}{\text{、}}{{\rm{\alpha}} _3}{\text{、}}{{\rm{\alpha}} _4}$，第 $j$个粒子的误差及 $t$时刻第 $j$个粒子的位姿 $\hat {{X}}_{t,j}^{\rm{L}}{\rm{ = }}{\left[ {x_{t,j}^{\rm{L}},y_{t,j}^{\rm{L}},\theta _{t,j}^{\rm{L}}} \right]^{\rm{T}}}$如下：

(10) $\left.\begin{array}{l} \Delta {{\hat \theta }_j} = \Delta \theta - {\rm{sample}} \; \left( {{{\rm{\alpha}} _1}{{\left( {\Delta \theta } \right)}^2} + {{\rm{\alpha}} _2}{{\left( {\Delta s} \right)}^2}} \right), \\ \Delta {{\hat s}_j} = \Delta s - {\rm{sample}} \; \left( {{{\rm{\alpha}} _3}{{\left( {\Delta \theta } \right)}^2} + {{\rm{\alpha}} _4}{{\left( {\Delta s} \right)}^2}} \right); \\ \end{array} \right\}$

(11) $\left. \begin{array}{l} x_{t,j}^{\rm{L}} = \hat x_{t - 1}^{\rm{L}}{\rm{ + }}\Delta \hat s {\rm{cos}} \left( {\theta _{t - 1}^{\rm{L}} - \Delta \hat \theta } \right), \\ y_{t,j}^{\rm{L}} = \hat y_{t - 1}^{\rm{L}}{\rm{ + }}\Delta \hat s {\rm{sin}} \left( {\theta _{t - 1}^{\rm{L}} - \Delta \hat \theta } \right), \\ \theta _{t,j}^{\rm{L}} = \hat \theta _{t - 1}^{\rm{L}} + \Delta \hat \theta . \\ \end{array} \right\}$

利用谷歌Cartographer建图算法^[21]构建静态全局地图 ${\rm{MAP}}$，坐标系为地图坐标系. 激光雷达点云数据如下：

(12) ${{\bf{lidar}}_t}{\rm{ = }}\left [{\rm{lidar}}_t^1,{\rm{lidar}}_t^2, \cdots ,{\rm{lidar}}_t^k, \cdots ,{\rm{lidar}}_t^n\right]. $

式中： ${\rm{lidar}}_t^k$表示t时刻第k个点云包含的障碍物距离信息，n为返回的点云总个数.

假设第 $j$个粒子为车辆真实位姿，根据激光雷达在车辆坐标系中的位姿 ${{\varGamma}} _{\rm{v}}^{\rm{L}}$，可得t时刻第k个点云映射到 ${\rm{MAP}}$上的坐标 ${\left[ {x_{t,k}^{\rm{L}},y_{t,k}^{\rm{L}}} \right]^{\rm{T}}}$，如下：

(13) $\begin{split} \left[ {\begin{array}{*{20}{c}} {x_{t,k}^{\rm{L}}} \\ {y_{t,k}^{\rm{L}}} \end{array}} \right] = & \left[ {\begin{array}{*{20}{c}} {x_{t,j}^{\rm{L}}} \\ {y_{t,j}^{\rm{L}}} \end{array}} \right] + \left[ {\begin{array}{*{20}{c}} {\cos\theta _{t,j}^{\rm{L}}}&{\sin\theta _{t,j}^{\rm{L}}} \\ {\sin\theta _{t,j}^{\rm{L}}}&{\cos\theta _{t,j}^{\rm{L}}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {x_{\rm{v}}^{\rm{L}}} \\ {y_{\rm{v}}^{\rm{L}}} \end{array}} \right] + \\ & {\rm{lidar}}_t^k\left[ {\begin{array}{*{20}{c}} {\cos \; (\theta _{t,j}^{\rm{L}} + \theta _{\rm{v}}^{\rm{L}})} \\ {\sin \; (\theta _{t,j}^{\rm{L}} + \theta _{\rm{v}}^{\rm{L}})} \end{array}} \right]. \end{split} $

设 $d_t^k$为t时刻第k个点云映射坐标与 ${\rm{MAP}}$中障碍物的最小欧式距离，服从 $d_t^k\sim N\left( {0,{{\rm{\sigma}} ^2}} \right)$，可得t时刻第 $j$个粒子的得分 $\xi _t^j$，如下：

(14) $\left. \begin{split} & d_t^k = \mathop {\min }\limits_{(x,y) \in {\rm{MAP}}} \left( {\sqrt {{{\left( {x_{t,k}^{\rm{L}} - x} \right)}^2} + {{\left( {y_{t,k}^{\rm{L}} - y} \right)}^2}} } \right), \\ & {\xi _j} = \prod\limits_{k = 1}^n {\left[ {{\varphi _{\rm{noise}}}p\left( {d_t^k,{\rm{\sigma}} } \right) + {{{\varphi _{\rm{rand}}}}}/{{{\varphi _{\rm{max}}}}}} \right]} . \end{split}\right\} $

式中： $p\left( {d_t^k,{\rm{\sigma}} } \right)$指 $d_t^k - \sigma \sim d_t^k + \sigma $所占概率； ${\varphi _{\rm{noise}}}$为测量值与地图最近障碍物的欧式距离测量误差的概率权值； ${\varphi _{\rm{rand}}}$为测量值为无法解释的随机测量的概率权值； ${\varphi _{{\rm{max}}}}$为由于测量失败导致返回测量值为最大测距的概率权值，利用极大似然估计确定参数值； $N$为粒子总数量，根据库尔贝克-莱布勒散度采样原理^[22]动态调整粒子数目.

根据式（14）可得粒子平均得分 $\xi _t^{\rm{avg}} = \sum\nolimits_{j = 1}^N {\xi _t^j} /N$. 短期估计和长期估计参数 ${{\rm{\beta}} _{{\rm{short}}}} {\text{、}}{{\rm{\beta}} _{{\rm{long}}}}$分别为短期估计和长期估计的衰减率，以经验值取值，则t时刻粒子得分的短期估计 $\xi _t^{{\rm{short}}}$和长期估计 $\xi _t^{{\rm{long}}}$如下：

(15) $\left.\begin{split} & \xi _t^{{\rm{short}}} = \xi _{t{\rm{ - }}1}^{{\rm{short}}} + {{\rm{\beta}} _{{\rm{short}}}}\left( {\xi _t^{\rm{avg}} - \xi _{t{\rm{ - }}1}^{{\rm{short}}}} \right), \\ & \xi _t^{{\rm{long}}} = \xi _{t{\rm{ - }}1}^{{\rm{long}}} + {{\rm{\beta}} _{{\rm{long}}}}\left( {\xi _t^{\rm{avg}} - \xi _{t{\rm{ - }}1}^{{\rm{long}}}} \right). \\ \end{split}\right\} $

对于t= ${t_0}$，若 $\xi _{{t_0}}^{{\rm{short}}} \geqslant \xi _{{t_0}}^{{\rm{long}}}$，则默认选用地图 ${\rm{MAP}}$；若 $\xi _{{t_0}}^{{\rm{short}}} < \xi _{{t_0}}^{{\rm{long}}}$，则说明定位累积误差过大，出现明显漂移，此时缩小激光雷达测距距离，利用Gmapping算法^[23]实时构建动态局部地图 ${\rm{map}}$，用于似然域测量模型，原点坐标为 ${t_0}$时刻AMCL位姿估计 $\hat {{X}}_{{t_0}}^{\rm{L}}$. 与 ${\rm{MAP}}$相比， ${\rm{map}}$覆盖范围小，包含实时动态障碍物，具有更加准确的障碍物地图模型，可以更好地修正累积误差. 此时 $d_t^k$如下：

(16) $d_t^k = \mathop {\min }\limits_{(x,y) \in {\rm{map}}} \left( {\sqrt {{{\left( {x_{t,k}^{\rm{L}} - x_{{t_0}}^{\rm{L}} - x} \right)}^2} + {{\left( {y_{t,k}^{\rm{L}} - y_{{t_0}}^{\rm{L}} - y} \right)}^2}} } \right).$

综上所述，根据加权最小二乘法^{[19, 24]}，选定粒子中心作为 $t$时刻AMCL位姿估计 $\hat {{X}}_t^{\rm{L}}$， ${{P}}_t^{\rm{L}}$为AMCL位姿估计的协方差，如下：

(17) $\left.\begin{array}{l} \hat {{X}}_t^{\rm{L}}{\rm{ = }}\sum\limits_{j = 1}^N {\left( {{\xi _j}\Big/\sum\limits_{j = 1}^N {{\xi _j}} } \right)\hat {{X}}_{t,j}^{\rm{L}}} , \\ {{P}}_t^{\rm{L}}{\rm{ = max}}\left( {{\xi _1},{\xi _2}, \cdots ,{\xi _N}} \right){{P}}_{t{\rm{ - }}1}^{\rm{L}}{\rm{ + }} \\ \left( {{\rm{1 - max}}\left( {{\xi _1},{\xi _2}, \cdots ,{\xi _N}} \right)} \right)E\left( {\Delta {{z}}_t^{\rm{L}}{{\left( {\Delta {{z}}_t^{\rm{L}}} \right)}^{\rm{T}}}} \right). \end{array}\right\} $

式中： $\Delta {{z}}_t^{\rm{L}} $为激光里程计累积误差, $\Delta {{z}}_t^{\rm{L}} ={\left[ {\Delta x_t^{\rm{L}},\Delta y_t^{\rm{L}},\Delta \theta _t^{\rm{L}}} \right]^{\rm{T}}}{\rm{ = }}$ $ \hat {{X}}_t^{\rm{L}} - {{Z}}_t^{\rm{L}} - {{X}}_0^{\rm{L}}$.

设t时刻KF的系统状态量为 $\hat {{X}}_t^{\rm{c}} = \left[{\hat x_t^{\rm{c}},\hat y_t^{\rm{c}},\hat \theta _t^{\rm{c}}} \right]$. 利用路侧摄像头在t−1时刻的最优估计 $\hat {{X}}_{t{\rm{ - }}1}^{\rm{c}}$预测t时刻的车辆位姿 $\hat {{X}}_{t|t{\rm{ - 1}}}^{\rm{c}}$与协方差矩阵 ${{P}}_{t|t - 1}^{\rm{c}}$，如下：

(18) $\begin{split} & \hat {{X}}_{t|t{\rm{ - 1}}}^{\rm{c}}{\rm{ = }}{{{A}}_t}\hat {{X}}_{t{\rm{ - 1}}}^{\rm{c}} + {{{\psi}} _t}, \\ & {{P}}_{t|t - 1}^{\rm{c}}{\rm{ = }}{{{A}}_t}{{P}}_{t - 1}^{\rm{c}} {{A}}_t^{{{\rm{T}}}} + {{{Q}}_t}. \end{split} $

式中： ${{{A}}_t}$为状态转移矩阵， ${{{\psi}} _t}$为系统状态误差， ${{{Q}}_t}$为系统状态误差的方差.

基于PDA的KF位姿估计可以消除路侧摄像头目标检测中的干扰信号，利用PDA算法^[25]将上一帧与下一帧图像中的目标进行关联，实现目标跟踪.

根据贝叶斯公式和乘法定理，可得t时刻第i个观测量为正确观测量的概率 ${{P}}\left( {{{z}}_{t,i}^{\rm{c}}|{{{\varPhi}} _t}} \right)$，如下：

(19) $\begin{split} \\ P\left( {{{z}}_{t,i}^{\rm{c}}|{{{\varPhi}} _t}} \right) = P\left( {{{z}}_{t,i}^{\rm{c}}|{{{Z}}_t},{m_t},{{{\varPhi}} _{t{\rm{ - }}1}}} \right) = \\ \\ \frac{{P\left( {{{{Z}}_t}|{{z}}_{t,i}^{\rm{c}},{m_t},{{{\varPhi}} _{t{\rm{ - }}1}}} \right)P\left( {{{z}}_{t,i}^{\rm{c}}|{m_t},{{{\varPhi}} _{t{\rm{ - }}1}}} \right)}}{{\sum\limits_{i = 0}^{{m_t}} {P\left( {{{{Z}}_t}|{{z}}_{t,i}^{\rm{c}},{m_t},{{{\varPhi}} _{t{\rm{ - }}1}}} \right)P\left( {{{z}}_{t,i}^{\rm{c}}|{m_t},{{{\varPhi}} _{t{\rm{ - }}1}}} \right)} }}. \end{split} $

式中： ${{{Z}}_t}{\rm{ = }}\left\{ {{{Z}}_{t,1}^{\rm{c}},{{Z}}_{t,2}^{\rm{c}}, \cdots ,{{Z}}_{t,i}^{\rm{c}}, \cdots ,{{Z}}_{t,{m_t}}^{\rm{c}}} \right\}$为摄像头在t时刻观测量集合，其中 ${{Z}}^{\rm{c}}_{t,i} $为t时刻第i个观测量， ${m_t}$为摄像头在t时刻观测量个数； ${{{\varPhi}} _t}{\rm{ = }}\left\{ {{{{Z}}_1},\;{{{Z}}_2},\; \cdots ,\;{{{Z}}_t}} \right\}$为直到t时刻摄像头累积观测量集合； ${{z}}_{t,i}^{\rm{c}}$表示t时刻第i个观测量 ${{Z}}_{t,i}^{\rm{c}}$为正确观测量； $P\left( {{{z}}_{t,i}^{\rm{c}}|{m_t},{{{\varPhi}} _{t{\rm{ - }}1}}} \right)$为 ${{z}}_{t,i}^{\rm{c}}$的联合概率密度函数； $P\left( {{{{Z}}_t}|{{z}}_{t,i}^{\rm{c}},{m_t},{{{\varPhi}} _{t{\rm{ - }}1}}} \right)$为 ${{{Z}}_t}$的联合概率密度函数.

假设干扰信号服从均匀分布，正确观测量服从正态分布，如下：

(20) $\begin{array}{*{20}{c}} {P\left( {{{Z}}_{t,j}^{\rm{c}}|{{z}}_{t,i}^{\rm{c}},{{{\varPhi}} _{t - 1}}} \right) = V_t^{ - 1}};&{j \ne } \end{array}i.$

(21) $P\left( {{{Z}}_{t,i}^{\rm{c}}|{{z}}_{t,i}^{\rm{c}},{{{\varPhi}} _{t - 1}}} \right) = P_{\rm{G}}^{ - 1}\frac{{{\rm{exp}}\left[ { - 0.5{{\left( {\Delta {{z}}_{t,i}^{\rm{c}}} \right)}^{\rm{T}}}{{S}}_t^{ - 1}\Delta {{z}}_{t,i}^{\rm{c}}} \right]}}{{{{\left( {\sqrt {2{\text{π}} } } \right)}^3}\sqrt {|{{{S}}_t}|} }}.$

式中： ${V_t}$为椭球跟踪门体积； ${{Z}}_{t,j}^{\rm{c}}$均为干扰信号； $P_{\rm{G}}^{ - 1}$为正确观测值落入跟踪门的概率； $\Delta {{z}}_{t,i}^{\rm{c}}$为t时刻第i个摄像头观测量与系统状态量预测值之差， $\Delta {{z}}_{t,i}^{\rm{c}} = {{Z}}_{t,i}^{\rm{c}} - \hat {{X}}_{t|t{\rm{ - 1}}}^{\rm{c}}$；S_t为观测量与系统预测量误差的协方差矩阵.

可得 ${{{Z}}_t}$的联合概率密度函数，如下：

(22) $\begin{array}{c} P\left( {{{{Z}}_t}|{{z}}_{t,i}^{\rm{c}},{m_t},{{{\varPhi}} _{t{\rm{ - }}1}}} \right) =\qquad\qquad\qquad\qquad\qquad\qquad \\ P\left( {{{Z}}_{t,i}^{\rm{c}}|{{z}}_{t,i}^{\rm{c}},{m_t},{{{\varPhi}} _{t{\rm{ - }}1}}} \right)\prod\limits_{j = 1,j \ne i}^{{m_t}} {P\left( {{{Z}}_{t,j}^{\rm{c}}|{{z}}_{t,i}^{\rm{c}},{m_t},{{{\varPhi}} _{t{\rm{ - }}1}}} \right)} = \\ V_t^{ - {m_t} + 1}P\left( {{{Z}}_{t,i}^{\rm{c}}|{{z}}_{t,i}^{\rm{c}},{{{\varPhi}} _{t{\rm{ - }}1}}} \right). \end{array} $

若目标被检测到且正确观测量落入到椭球跟踪门内，则干扰信号观测量的个数为 ${m_{\rm{f}}} = {m_t} - 1$，否则 ${m_{\rm{f}}} = {m_t}$. 假设 ${m_{\rm{f}}}$服从参数为 $\lambda {V_t}$的泊松分布，

(23) $\begin{array}{*{20}{c}} {P\left( {{m_{\rm{f}}} = {m_t}} \right) = - \dfrac{{{\exp\;({ - \lambda V_t})}{{\left( {\lambda {V_t}} \right)}^{{m_t}}}}}{{{m_t}!}}};&{{m_t} = 0,1, \cdots .} \end{array}$

可得 ${{z}}_{t,i}^{\rm{c}}$的联合概率密度函数：

(24) $\begin{split} & P\left( {{{z}}_{t,i}^{\rm{c}}|{m_t},{{{\varPhi}} _{t{\rm{ - }}1}}} \right){\rm{ = }}P\left( {{{z}}_{t,i}^{\rm{c}}|{m_t}} \right){\rm{ = }} \\ & P\left( {{{z}}_{t,i}^{\rm{c}}|{m_{\rm{f}}} = {m_t} - 1} \right)P\left( {{m_{\rm{f}}} = {m_t} - 1} \right) + \\ & P\left( {{{z}}_{t,i}^{\rm{c}}|{m_{\rm{f}}} = {m_t}} \right)P\left( {{m_{\rm{f}}} = {m_t}} \right){\rm{ = }} \\ & \left\{ {\begin{array}{*{20}{c}} {\dfrac{{{P_{\rm{D}}}{P_{\rm{G}}}}}{{{P_{\rm{D}}}{P_{\rm{G}}}{m_t} + \left( {1 - {P_{\rm{D}}}{P_{\rm{G}}}} \right)\lambda {V_t}}}},&{i = 1,2, \cdots ,{m_t}}; \\ {\dfrac{{\left( {1 - {P_{\rm{D}}}{P_{\rm{G}}}} \right)\lambda {V_t}}}{{{P_{\rm{D}}}{P_{\rm{G}}}{m_t} + \left( {1 - {P_{\rm{D}}}{P_{\rm{G}}}} \right)\lambda {V_t}}}},&{i = 0}. \end{array}} \right. \\ \end{split} $

式中： ${P_{\rm{D}}}{\rm{ = }}P\left( {{m_{\rm{f}}} = {m_t} - 1} \right){\rm{ + }}P\left( {{m_{\rm{f}}} = {m_t}} \right)$为t时刻至多有一个正确观测量的概率， $i = 0$指无正确观测量.

综上所述，根据式（19）、（22）及（24），可得t时刻第i个观测量为正确观测量的概率 $P\left( {{{z}}_{t,i}^{\rm{c}}|{{{\varPhi}} _t}} \right)$和 $t$时刻无正确观测量的概率 $P\left( {{{z}}_{t,0}^{\rm{c}}|{{{\varPhi}} _t}} \right)$，如下：

(25) $P\left( {{{z}}_{t,i}^{\rm{c}}|{{{\varPhi}} _t}} \right){\rm{ = }}\frac{{{\rm{exp}}\left[ { - 0.5{{\left( {\Delta {{z}}_{t,i}^{\rm{c}}} \right)}^{\rm{T}}}{{S}}_t^{ - 1}\Delta {{z}}_{t,i}^{\rm{c}}} \right]}}{{{b_t}{\rm{ + }}\sum\limits_{j = 1}^{{m_t}} {{\rm{exp}}\left[ { - 0.5{{\left( {\Delta {{z}}_{t,j}^{\rm{c}}} \right)}^{\rm{T}}}{{S}}_t^{ - 1}\Delta {{z}}_{t,j}^{\rm{c}}} \right]} }},$

(26) $P\left( {{{z}}_{t,0}^{\rm{c}}|{{{\varPhi}} _t}} \right){\rm{ = }}\frac{{{b_t}}}{{{b_t}{\rm{ + }}\sum\limits_{j = 1}^{{m_t}} {{\rm{exp}}\left[ { - 0.5{{\left( {\Delta {{z}}_{t,j}^{\rm{c}}} \right)}^{\rm{T}}}{{S}}_t^{ - 1}\Delta {{z}}_{t,j}^{\rm{c}}} \right]} }}.$

式中：

$ {b_t} = \frac{{\lambda {{\left( {\sqrt {2{\text{π}} } } \right)}^3}\sqrt {|{{{S}}_t}|} \left( {1 - {P_{\rm{D}}}{P_{\rm{G}}}} \right)}}{{{P_{\rm{D}}}}};\;i = 1,2, \cdots ,{m_t}. $

根据KF滤波原理，可得t时刻目标状态的位姿估计和协方差：

27) $\left.\begin{split} & \hat {{X}}_t^{\rm{c}} = \hat {{X}}_{t|t{\rm{ - }}1}^{\rm{c}} + {{{K}}_t}\sum\limits_{i = 1}^{{m_t}} {P\left( {{{z}}_{t,i}^{\rm{c}}|{{{\varPhi}} _t}} \right)\Delta {{z}}_{t,i}^{\rm{c}}} , \\& {{P}}_t^{\rm{c}} = P\left( {{{z}}_{t,0}^{\rm{c}}|{{{\varPhi}} _t}} \right){{P}}_{t|t - 1}^{\rm{c}} + \\& \left( {1 - P\left( {{{z}}_{t,0}^{\rm{c}}|{{{\varPhi}} _t}} \right)} \right)\left( {{{I}} - {{{K}}_t}{{{H}}_t}} \right){{P}}_{t|t - 1}^{\rm{c}} + \\& {{{K}}_t}\left( {\sum\limits_{i = 1}^{{m_t}} {P\left( {{{z}}_{t,i}^{\rm{c}}|{{{\varPhi}} _t}} \right)\Delta {{z}}_{t,i}^{\rm{c}}} ,{{\left( {\Delta {{z}}_{t,i}^{\rm{c}}} \right)}^{\rm{T}}} - \Delta {{z}}_t^{\rm{c}}{{\left( {\Delta {{z}}_t^{\rm{c}}} \right)}^{\rm{T}}}} \right){{K}}_t^{\rm{T}}. \end{split}\right\} $

式中： $\Delta {{z}}_t^{\rm{c}} $为观测新息， $\Delta {{z}}_t^{\rm{c}} ={\left[{\Delta x_t^{\rm{c}},\Delta y_t^{\rm{c}},\Delta \theta _t^{\rm{c}}} \right]^{\rm{T}}}{\rm{ = }}$ $ {{z}}_{t,i}^{\rm{c}} - {{{H}}_t}\hat {{X}}_{t|t{\rm{ - }}1}^{\rm{c}}$，其中 ${{{H}}_t}$为观测矩阵； ${{{K}}_t}$为卡尔曼增益矩阵； ${{I}}$为单位矩阵.

AMCL位姿估计消除激光雷达扫描匹配累积误差，得到地图坐标系下的全局位姿 $\hat {{X}}_t^{\rm{L}}$及其协方差 ${{P}}_t^{\rm{L}}$；KF位姿估计消除虚假目标对路侧摄像头的干扰，得到位姿 $\hat {{X}}_t^{\rm{c}}$及其协方差 ${{P}}_t^{\rm{c}}$. 参考联邦卡尔曼滤波^[26-28]和带反馈的最优分布式估计融合^[29]，以分布式的形式，将2个位姿估计进行融合，可得t时刻全局最优估计 ${\hat {{X}}_t}$及协方差 ${{{P}}_t}$，如下：

(28) $\left.\begin{split} & {{P}}_t^{ - 1}{\rm{ = }}{\left( {{{P}}_t^{\rm{L}}} \right)^{ - 1}}{\rm{ + }}{\left( {{{P}}_t^{\rm{c}}} \right)^{ - 1}} - {{P}}_{t - 1}^{ - 1}, \\ & {{P}}_t^{ - 1}{{\hat {{X}}}_t} = {\left( {{{P}}_t^{\rm{L}}} \right)^{ - 1}}\hat {{X}}_t^{\rm{L}} + {\left( {{{P}}_t^{\rm{c}}} \right)^{ - 1}}\hat {{X}}_t^{\rm{c}} - {{P}}_{t - 1}^{ - 1}{{\hat {{X}}}_{t{\rm{ - }}1}}. \end{split}\right\} $

通过式（28）可知，位姿估计对应的协方差越小，置信度越高，其位姿估计在全局估计中的比重越大. 将全局最优估计 ${\hat {{X}}_t}$反馈到各个位姿估计中作为下一时刻迭代开始的初始值，如下：

(29) $\begin{array}{l} {{Z}}_t^{\rm{L}} = {{\hat {{X}}}_t} - \hat {{X}}_0^{\rm{L}} - \Delta {{z}}_t^{\rm{L}}, \; \hat {{X}}_t^{\rm{L}} = {{\hat {{X}}}_t}, \; \hat {{X}}_t^{\rm{c}}{\rm{ = }}{{\hat {{X}}}_t}. \end{array} $

根据激光雷达与路侧摄像头的累积误差，可以求出协方差矩阵：

(30) ${{P}}_t^{{\rm{L}},{\rm{c}}}{\rm{ = }}E\left( {\Delta {{z}}_t^{\rm{L}}{{\left( {\Delta {{z}}_t^{\rm{c}}} \right)}^{\rm{T}}}} \right){\rm{ = }}{\left( {{{P}}_t^{{\rm{c}},{\rm{L}}}} \right)^{\rm{T}}}.$

由协方差的数学性质可知， ${{P}}_t^{{\rm{L}},{\rm{L}}}{\rm{ = }}{{P}}_t^L,\;{{P}}_t^{{\rm{c}},{\rm{c}}}{\rm{ = }}{{P}}_t^{\rm{c}}$. 根据矩阵加权线性最小方差最优融合准则^[30]可知，自主调节2个位姿估计的协方差：

(31) $\left.\begin{split} & {{P}}_t^{\rm{L}}{\rm{ = }}\left( {{{P}}_t^{\rm{L}} - {{P}}_t^{{\rm{L}},{\rm{c}}}} \right){\left( {{{P}}_t^{\rm{L}} + {{P}}_t^{\rm{c}} - {{P}}_t^{{\rm{L}},{\rm{c}}} - {{P}}_t^{{\rm{c}},{\rm{L}}}} \right)^{ - 1}}{{{P}}_t}, \\ & {{P}}_t^{\rm{c}}{\rm{ = }}\left( {{{P}}_t^{\rm{c}} - {{P}}_t^{{\rm{c}},{\rm{L}}}} \right){\left( {{{P}}_t^{\rm{L}} + {{P}}_t^{\rm{c}} - {{P}}_t^{{\rm{L}},{\rm{c}}} - {{P}}_t^{{\rm{c}},{\rm{L}}}} \right)^{ - 1}}{{{P}}_t}. \end{split}\right\} $

式（29）、（31）体现了全局融合估计对下一时刻迭代初值的影响，将全局最优估计作为下一时刻下层的2个位姿估计的初值，并校准激光雷达的位姿观测量，减小扫描匹配累积误差；利用矩阵加权性质，合理反馈协方差. 全局融合估计通过位姿和协方差的反馈实现动态融合和自主调节，最大限度地降低各种动态干扰对定位的影响.

实验车辆为无人驾驶平台308S，如图4所示. 16线激光雷达置于车辆前端，IMU置于车辆质心，路侧摄像头置于路侧，通过ROS搭建通信框架，利用局域网实现车与路侧的通讯. 标定上述传感器，可得起点处激光雷达位姿 ${{{\varGamma}} _{\rm{L}}}$、摄像头位姿 ${{{\varGamma}} _{\rm{c}}}$、激光雷达在车辆坐标系的位姿 ${{\varGamma}} _{\rm{v}}^{\rm{L}}$. 实验场景为武汉理工大学校园道路，如图5（a）所示为卫星地图，如图5（b）所示为利用Cartographer算法^[21]构建的二维地图. 该场景存在大量树木遮挡，GPS效果差，包含非标准化道路.

图5（b）中黑色曲线为参考轨迹，由项目数据和实地测绘获得. 起点为左下角五角星，以逆时针方向经多次转向，回到起点，总里程为500 m. 车辆以20 km/h的速度匀速行驶实现纵向运动，以10 km/h的线速度、5°转向角实现横向运动，标记纵向运动与横向运动的分界点，以实现2类运动的切换；位姿发布频率为10 Hz. 假设相同运动时间车辆到达相同地点，则位姿发布频率一致的前提下可视车辆位姿、运动时间和位姿采集点一一对应.

分析下述4种定位算法的定位精度如下. 1）PLICP：以PLICP实现激光雷达的扫描匹配定位，无滤波和车路协同；2）AMCL：仅利用针对激光雷达的AMCL位姿估计，无车路协同；3）EKF：在摄像头覆盖区域将激光雷达与摄像头观测量进行EKF融合，其余区域为AMCL；4）双层融合：在摄像头覆盖区域的定位为提出的双层融合协同定位，其余区域为AMCL.

该部分实验中路侧摄像头位于图6所示的转向区域，每次试验均在同一个转向区域放置一个路侧摄像头，并且摄像头位姿不变. 每次车辆转向时，纵向定位误差会以累积纵向误差的形式转化为横向定位误差，因此定位精度主要分析横向定位精度、航向角精度和终点处的累积纵向误差.

如图6所示为4种定位算法的轨迹对比图. PLICP环境的适应性较差，定位误差随时间逐渐累积，无法持续精确定位，因此逐渐偏离参考轨迹，定位误差很大，不对PLICP定位算法作误差分析. 如图7所示为其余3种算法的定位误差，如图7（a）所示为横向定位误差，如图7（b）所示为航向角误差. 图中，n为位姿采集点的个数，与运动时间相对应； $\Delta L$为横向定位误差； $\Delta \theta $为航向角误差. 如表1所示为3种算法终点处的累积纵向误差. 表中， $\Delta d$为累积纵向误差.

结合图6左下角局部放大图和表1可知，双层融合累积纵向误差小于AMCL和EKF，累积纵向误差比AMCL减小34.12%，比EKF减小46.15%.

如图6、7所示，EKF和双层融合协同定位算法的定位精度明显优于AMCL. AMCL的横向定位误差和航向角误差大，稳定性差，幅值高. 随着时间的增加，AMCL无法很好地消除扫描匹配的累积误差. 相比于EKF，双层融合稳定性好，未出现类似EKF曲线中的较大浮动.

为了分析上述3种定位算法的定位精度与稳定性，计算相关统计学指标. 如图8所示为3种算法的定位误差分布，图8（a）所示为横向定位误差分布，图8（b）所示为航向角误差分布. 图中， $\eta $为百分比. 如表2所示为3种算法的横向定位误差和航向角误差统计学指标. 表中， ${\mu _{\rm{L}}}$、 ${\sigma _{\rm{L}}}$、 $\Delta {L_{\max }}$为横向定位误差的均值、标准差及最大值， ${\mu _{\rm{\theta}} }$、 ${\sigma _{\rm{\theta}} }$、 $\Delta {\theta _{\max }}$为航向角误差的均值、标准差及最大值.

根据表2可知，AMCL的定位精度为m级，EKF和双层融合的定位精度为dm级. 针对横向定位误差，双层融合的定位精度比EKF提升38.8%，比AMCL提升79.2%. AMCL的 ${\sigma _{\rm{L}}}$很大，结合图8（a）可知，约有50%位姿采集点的横向定位误差大于0.5 m；EKF与双层融合的 ${\sigma _{\rm{L}}}$虽然接近，但是EKF约有30%位姿采集点的横向定位误差大于0.5 m，在0~0.2 m内的误差分布劣于双层融合.

针对航向角误差，双向融合的航向角精度比EKF提升33.2%，比AMCL提升55.3%. 虽然3种定位的航向角精度均小于5°，但是结合 ${\sigma _{\rm{\theta}} }$和图8（b）可知，双层融合的航向角误差均分布在0~15°，且绝大部分分布在0~2°，优于EKF和AMCL的航向角误差分布.

综上可知，双层融合的定位精度为0.199 m，航向角精度为2.179°，相比于AMCL和EKF，定位精度和稳定性有大幅度提升，说明双层融合协同定位消除了激光雷达扫描匹配的累积误差，体现了较好的自我调节能力.

由于摄像头作用范围有限，合理布置摄像头可以提升定位精度. 由图6可知，转向区域一般存在较大的定位误差，因此路侧摄像头仅在转向区域工作. 分析摄像头数量对于双层融合协同定位的定位精度影响. 摄像头的数量和位置见表3. 表中，a、b、c、d、e均为转向区域，每个区域至多设有一个路侧摄像头. 实验环境和车辆工况与前述实验过程保持一致.

如图9所示为1cam、2cams、3cams这3种类型定位轨迹的对比. 如图10所示为3种类型定位误差，图10（a）所示为横向定位误差，图10（b）所示为航向角误差，其中局部放大图从左到右依次为b、c、d区域. 如图11所示为3种类型在各区域的横向定位误差的标准差，即 $\sigma $.

从图9~11可知，3种类型的轨迹基本重合，航向角精度和稳定性基本一致. 针对横向定位误差，3cams在b、c、d、e区域的误差浮动小于2cams和1cams，随着路侧摄像头数量的增加，在转向区域的横向定位误差标准差随之减小. 对比图11的曲线趋势可知，摄像头数量增加有利于定位的整体稳定性.

利用统计学指标，分析摄像头数量对定位精度与稳定性的影响. 如表4所示为3种类型的横向定位误差和航向角误差的统计学指标. 如图12所示为3种类型的定位误差分布，图12（a）所示为横向定位误差分布，图12（b）所示为航向角误差分布.

根据表4可知，1cams和2cams的定位精度均为分米级，3cams为厘米级. 针对横向定位误差，3cams类型比2cams定位精度提升53.0%，2cams类型比1cam定位精度提升16.5%；3cams横向定位误差的标准差小，且多分布在0.1 m内，虽然1cam和2cams的 ${\sigma _{\rm{L}}}$接近，结合图12（a）可知，2cams在0~0.3 m内的误差分布优于1cams，定位算法的稳定性随着摄像头数量的增加而提升. 结合图12（b）可知，3种类型航向角精度均约为2°，浮动基本一致.

综上可知，随着路侧摄像头数量的增加，可以提升横向定位稳定性，在一定程度上提升定位精度. 在该实验场景中设置3个路侧摄像头，可以实现cm级定位，精度为7.8 cm. 路侧摄像头数量增加对航向角精度的影响不大，航向角精度均约为2°.

（1）双层融合协同定位以低成本实现了高精度定位，稳定性好. 相比车载激光雷达定位，如AMCL和PLICP，协同定位可以很好地消除累积误差，定位精度提升79.2%，航向角精度提升55.3%；相比无反馈的紧融合定位，如EKF，分布式结构具有更好的抗干扰能力，利用反馈实现自主调节，定位精度提升38.8%，航向角精度提升33.2%.

（2）路侧摄像头数量的增加有益于定位稳定性的提升，定位精度可以实现cm级定位. 对航向角精度的影响不大，均约为2°. 相比于GPS、惯性传感器等，摄像头成本很低，工程应用广，增加摄像头数量以提升定位精度的可行性高.

（3）横向运动的定位误差大于纵向运动，必须提升横向运动区域的定位精度，消除过大的累积误差. 航向角误差虽然小，但具有重要意义.

本文提出的双层融合协同定位算法将AMCL与KF滤波器相融合并实现反馈，极大提升了在校园、社区等非结构化场景中的定位精度，给无人驾驶在封闭园区内落地提供辅助，说明车路协同技术是未来智能交通中的关键环节. 后续将进一步解决定位算法的实时性问题，包括摄像头覆盖区域与未覆盖区域间的良好自主衔接，同时开展协同定位在车路协同系统中更深层次的应用.