一种机场复杂环境下的地勤车辆精准定位方法与流程

：
1.本发明涉及复杂环境下得车辆定位方法，尤其是涉及一种机场非开阔区域环境下得地勤车辆精准定位方法。
技术背景：
2.目前，机场尚无行之有效的技术手段减少安全事故。而事故很大程度上是由于工作人员存在视线盲区，对飞机、车辆、人员的精确位置不清楚，从而造成作业或指挥上的失误，最终导致不安全事件的发生。如果精准获取飞机、车辆、人员的实时位置信息，针对风险态势，提前发出危警报，从技术层面上可有效减少剐蹭事故的发生，减少经济损失，实现机场智能化的安全管理。
3.机场地勤车辆的工作环境一般可以分为室外环境、半室内环境和室内环境。室外环境相对空旷，卫星信号相对较好，半室内环境和室内环境相对复杂，卫星信号容易被遮挡，定位偏差较大。就目前来说，还没有一套能够满足机场地勤车辆在不同工作环境下高精度定位要求的系统出现。
4.上述问题，值得解决。
5.

技术实现要素：

6.鉴于此，本发明提出一种机场复杂环境下的地勤车辆精准定位方法，用于解决地勤车辆在半室内及卫星信号遮挡严重，定位结果精度不高的问题。
7.为达到上述目的，本发明提供如下技术方案：
8.本发明提供一种机场复杂环境下的地勤车辆精准定位方法，其特征在于，包括以下步骤：
9.s1，车辆自主定位；
10.s2，车辆进入非理想区域时路侧摄像头结合车辆自主定位信息进行目标识别与跟踪；
11.s3，路侧激光雷达背景滤除算法；
12.s4，路侧激光雷达基于概率数据关联的卡尔曼位姿估计车辆定位算法；
13.s5，建立协同加权融合定位算法；
14.依据车辆在非理性环境中的位置及距离激光雷达的距离设定融合定位权值，通过权值结合自主定位与路侧激光雷达定位进行协同加权融合定位，根据定位算法获取车辆精确位置信息，将定位结果反馈自主定位系统，对车辆状态信息进行修正。
15.进一步，所述的一种机场复杂环境下的地勤车辆精准定位方法，其特征在于，步骤2所述的路侧摄像头对目标车辆的识别与跟踪算法，具体如下：
16.在光照及通视范围等条件较好时，采用车牌识别方式，通过摄像头捕捉车辆车牌进行车辆识别；
17.当光线条件较差或视线受阻时，车辆运行至摄像头捕捉区域时，将自主定位信息发送给摄像头，摄像头根据位置锁定该位置区域，进一步根据车辆惯性导航模块所提供速
度、加速度以及航向等信息对该区域车辆的运行状态进行自相关比对，通过比对结果锁定目标车辆。
18.进一步，所述的一种机场复杂环境下的地勤车辆精准定位方法，其特征在于，步骤3所述的路侧激光雷达背景滤除算法，具体如下：
19.激光雷达的扫描点包括扫描区域内的全部信息，但固定的场景信息对动态的车辆捕获与跟踪产生了干扰，因此，需要先定义车辆运行定位区域，然后对该空间区域内的干扰点进行排除。
20.采用背景差分法进行路侧激光雷达背景滤除。
21.将激光雷达任意数据帧m中的每个扫描点利用极坐标的方式进行扇形分割建立对应关系，如下：
[0022][0023][0024]
其中，θ角为扫描角度，grid为扫描长度，n为扇区个数，m为栅格个数；
[0025]
每个栅格中扫描点与原点的距离为：
[0026][0027]
其中，d
ij
扇形分割中任意i行j列的取值，nk为每个栅格中点的个数，dk为改点到原点的距离；
[0028]
判断该扫描点是否属于背景：
[0029][0030]
其中，ik为1，表示该点为非背景的有效点，ik为0，表示该点为背景点，b
ij
为每个栅格中到原点距离的平均值，t为克服阈值选择随距离发生变化的比例关系。
[0031]
进一步，所述的一种机场复杂环境下的地勤车辆精准定位方法，其特征在于，步骤4所述的路侧激光雷达对目标车辆的定位算法，具体如下：
[0032]
为t时刻激光雷达获取目标车辆卡尔曼滤波的系统状态量,与为路侧激光雷达在 t-1时刻的最优估计预测t时刻的车辆位姿与协方差矩阵，如下：
[0033][0034][0035]
式中：a
t
为状态转移矩阵，ψ
t
为系统状态误差，q
t
为系统状态误差的方差。
[0036]
t时刻第i个观测量为目标观测量的概率如下：
[0037][0038]
式中：为激光雷达在t时刻观测量集合，其中为t时刻第i个观测量，m
t
为激光雷达在t时刻观测量的总数；φ
t
＝{z1,z2,
…
,z
t
}为直到t时刻激光雷达累积观测量集合；表示t时刻第i个观测量为正确观测量；为的联合
概率密度函数；为z
t
的联合概率密度函数。
[0039]
设定跟踪门，求取z
t
与的联合概率密度函数；
[0040]
获取t时刻第i个观测量为正确观测量的概率和t时刻无正确观测量的概率
[0041]
根据卡尔曼滤波原理，可得t时刻目标状态的位姿估计和协方差，进而获取激光雷达目标车辆定位信息。
[0042]
进一步，所述的一种机场复杂环境下的地勤车辆精准定位方法，其特征在于，步骤5所述的协同容错融合定位算法，具体如下：
[0043]
k时刻，惯性与gnss融合子滤波器和惯性与激光雷达融合子滤波器利用k-1时刻主滤波器分配和重置的参数进行更新；
[0044]
惯性与gnss融合的故障检测，利用主滤波器一步预测协方差矩阵结算残差协方差和故障检测函数值；
[0045]
激光雷达的故障检测根据摄像头的影像信息判断目标车辆是否有遮挡，当遮挡存在时激光雷达的测距将发生故障；
[0046]
故障检测函数与设定阈值进行对比，若小于阈值则认定子系统无故障，通过反馈系数结合量测噪声对子系统进行调节；若大于阈值认定为子系统发生故障，该子系统不参与融合估计，目标定位结果由另一子系统进行估计；若两子系统均发生故障，选择与阈值相差最小的子系统来进行估计，获取目标该时刻定位结果。
[0047]
进一步，所述的一种机场复杂环境下的地勤车辆精准定位方法，其特征在于，车辆、摄像头、激光雷达的坐标系为：
[0048]
车辆坐标系为世界坐标系，且坐标点为车辆的质心；
[0049]
车载惯性导航在车辆坐标系中的相对位姿为其中，v指车辆，i指车载惯性导航；
[0050]
摄像头坐标系原点为摄像头在空间区域所处的位姿γc＝[xc,yc,θc]
t
，其中，c指摄像头，摄像头所捕获的目标观测量为该相机坐标下的相对位姿，通过γc矢量叠加可以将目标坐标转换成世界坐标系中的绝对位置
[0051]
激光雷达坐标系原点为激光雷达在空间区域所安装的位姿γ
l
＝[x
l
,y
l
,θ
l
]
t
，其中，l指激光雷达，激光雷达所捕获的目标观测量为该雷达坐标下的相对位姿，通过γ
l
矢量叠加可以将目标坐标转换成世界坐标系中的绝对位置
[0052]
摄像头与激光雷达的固定位置在不影响功能的前提下应相邻安放，使得摄像头补足的画面与激光雷达扫描点云信息方向角大致吻合，以便判断目标车辆是否被其他移动物体遮挡。
[0053]
进一步，所述的一种机场复杂环境下的地勤车辆精准定位方法，其特征在于，所述车路通信模块采用v2x 通信方式获取车辆的自主定位信息与路侧摄像头及激光雷达测量信息，包括：gnss测量绝对位置信息、车辆速度加速度和航向信息、视觉相对位置信息、激光雷达相对位置信息，最终获取所述位置信息；所述v2x通信方式为dsrc、蓝牙、wi-fi、5g中的至少一种。
[0054]
本发明与现有技术相比，其显著优点为：
[0055]
(1)可有效解决地勤车辆在机场航空器遮挡、半室外及建筑物附近区域gnss信号受到遮挡及多路径问题而导致的定位不准确问题；
[0056]
(2)设备成本低，远由于每辆车辆安装激光雷达来提高定位精度的方案；
[0057]
(3)根据区域特点设定子系统权重，自适应融合北斗、惯性和激光多种定位方法提高定位精度和可靠性。
附图说明：
[0058]
图1为定位方法原理图
[0059]
图2为摄像头对目标车辆识别流程图
[0060]
图3为协同定位系统框图
具体实施方式：
[0061]
为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图及具体实施例对本发明进行详细描述。
[0062]
实施例1复杂环境下的地勤车辆精准定位方法原理
[0063]
定位算法流程如图1所示，
[0064]
车辆进入摄像头视阈范围内，车辆将自主定位(gnss 惯导)信息通过v2x通信发送给处理器；处理器根据车辆位置信息指导摄像头锁定目标车辆；摄像头成功捕获车辆后进行目标跟踪，并将车辆坐标信息传递给激光雷达；激光雷达根据摄像头提供的车辆相对位置对目标进行锁定，通过激光点云扫描信息对目标车辆位置进行精确获取，并将结果发送至处理器；处理器依据车辆在非理性环境中的位置及距离激光雷达的距离设定融合定位权值，通过权值结合自主定位与路侧激光雷达定位进行协同加权融合定位，根据定位算法获取车辆精确位置信息，将定位结果反馈自主定位系统，对车辆状态信息进行修正，进而在该复杂区域下获取目标车辆的精确位置信息。
[0065]
实施例2路侧摄像头对目标车辆的识别与跟踪
[0066]
如图2所示，在光照及通视范围等条件较好时，采用车牌识别方式，通过摄像头捕捉车辆车牌进行车辆识别；
[0067]
当光线条件较差或视线受阻时，车辆运行至摄像头捕捉区域时，将自主定位信息发送给摄像头，摄像头根据位置锁定该位置区域，进一步根据车辆惯性导航模块所提供速度、加速度以及航向等信息对该区域车辆的运行状态进行自相关比对，通过比对结果锁定目标车辆。
[0068]
实施例3路侧激光雷达背景滤除算法
[0069]
激光雷达的扫描点包括扫描区域内的全部信息，但固定的场景信息对动态的车辆捕获与跟踪产生了干扰，因此，需要先定义车辆运行定位区域，然后对该空间区域内的干扰点进行排除。
[0070]
采用背景差分法进行路侧激光雷达背景滤除。
[0071]
将激光雷达任意数据帧m中的每个扫描点利用极坐标的方式进行扇形分割建立对应关系，如下：
[0072][0073][0074]
其中，θ角为扫描角度，grid为扫描长度，n为扇区个数，m为栅格个数；
[0075]
每个栅格中扫描点与原点的距离为：
[0076][0077]
其中，d
ij
扇形分割中任意i行j列的取值，nk为每个栅格中点的个数，dk为改点到原点的距离；
[0078]
判断该扫描点是否属于背景：
[0079][0080]
其中，ik为1，表示该点为非背景的有效点，ik为0，表示该点为背景点，b
ij
为每个栅格中到原点距离的平均值，t为克服阈值选择随距离发生变化的比例关系。
[0081]
实施例4路侧激光雷达对目标车辆的定位算法
[0082]
设为t时刻激光雷达获取目标车辆卡尔曼滤波的系统状态量,与为路侧激光雷达在 t-1时刻的最优估计预测t时刻的车辆位姿与协方差矩阵，如下：
[0083][0084][0085]
式中：a
t
为状态转移矩阵，ψ
t
为系统状态误差，q
t
为系统状态误差的方差。
[0086]
t时刻第i个观测量为目标观测量的概率如下：
[0087][0088]
式中：为激光雷达在t时刻观测量集合，其中为t时刻第i个观测量，m
t
为激光雷达在t时刻观测量的总数；φ
t
＝{z1,z2,
…
,z
t
}为直到t时刻激光雷达累积观测量集合；表示t时刻第i个观测量为正确观测量；为的联合概率密度函数；为z
t
的联合概率密度函数。
[0089]
设定椭圆跟踪门，跟踪体积为v
t
，干扰信号为正态分布：
[0090][0091][0092][0093]
其中，为落入跟踪门中的正确观测值的概率；为t时刻激光雷达第i个观测量与系统状态量测试值之差；为观察量与系统预测量误差的协方差矩阵；求取z
t
的联合概率密度函数如下：
[0094][0095]
当检测目标落入椭圆球跟踪门中，干扰信号的观测量减一，否则不变，的联合概率密度函数如下：
[0096][0097]
其中，pd为t时刻至多有一个正确观测量的概率；
[0098]
可得t时刻第i个观测量为正确观测量的概率为：
[0099][0100][0101]
根据卡尔曼滤波原理，可得t时刻目标状态的位姿估计和协方差:
[0102][0103][0104][0105][0106]
其中，为观测新息；k
t
为卡尔曼增益矩阵；h
t
为观测矩阵；i为单位矩阵；根据卡尔曼滤波进而获取激光雷达目标车辆定位信息。
[0107]
协同容错融合定位算法，具体如下：
[0108]
融合导航系统状态方程为：
[0109]
xk＝φ
k-1
x
k-1
γ
k-1
ω
k-1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(18)
[0110]
其中，xk为系统k时刻状态估计值，包含15维误差矢量；φ
k-1
为k-1时刻系统状态转移矩阵；γ
k-1
为k-1时刻系统输入噪声；ω
k-1
为k-1时刻过程噪声；
[0111]
惯导/gnss子系统量测方程为：
[0112]
z1(k)＝h1(k)x(k) v1(k)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(19)
[0113]
其中，h1(k)量测状态转移矩阵；v1(k)为量测误差；
[0114]
同理，惯导/激光雷达子系统量测方程为：
[0115]
z2(k)＝h2(k)x(k) v2(k)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(20)
[0116]
子滤波器残差协方差矩阵为：
[0117][0118]
其中，为主滤波器的一步预测协方差；hk为子滤波器的状态转移矩阵；ri为量测误差的协方差矩阵；故障检测函数为：
[0119][0120]
其中，λ
ik
为第i个子滤波器的残差向量；
[0121]
设第i个自滤波器的动态信息分配βi为：
[0122][0123]
其中，m
ik
为子系统故障门限设定值，值的大小根据误差拟合规律进行设定。

技术特征：
1.一种机场复杂环境下的地勤车辆精准定位方法，其特征在于，包括以下步骤：s1，车辆自主定位；s2，车辆进入非理想区域时路侧摄像头结合车辆自主定位信息进行目标识别与跟踪；s3，路侧激光雷达背景滤除算法；s4，路侧激光雷达基于概率数据关联的卡尔曼位姿估计车辆定位算法；s5，建立协同加权融合定位算法；依据车辆在非理性环境中的位置及距离激光雷达的距离设定融合定位权值，通过权值结合自主定位与路侧激光雷达定位进行协同加权融合定位，根据定位算法获取车辆精确位置信息，将定位结果反馈自主定位系统，对车辆状态信息进行修正。2.根据权利要求1所述的一种机场复杂环境下的地勤车辆精准定位方法，其特征在于，步骤2所述的路侧摄像头对目标车辆的识别与跟踪算法，具体如下：在光照及通视范围条件较好时，采用车牌识别方式，通过摄像头捕捉车辆车牌进行车辆识别；当光线条件较差或视线受阻时，车辆运行至摄像头捕捉区域时，将自主定位信息发送给摄像头，摄像头根据位置锁定该位置区域，进一步根据车辆惯性导航模块所提供速度、加速度以及航向信息对该区域车辆的运行状态进行自相关比对，通过比对结果锁定目标车辆。3.根据权利要求1所述的一种机场复杂环境下的地勤车辆精准定位方法，其特征在于，步骤3所述的路侧激光雷达背景滤除算法，具体如下：激光雷达的扫描点包括扫描区域内的全部信息，但固定的场景信息对动态的车辆捕获与跟踪产生了干扰，因此，需要先定义车辆运行定位区域，然后对该空间区域内的干扰点进行排除；采用背景差分法进行路侧激光雷达背景滤除；将激光雷达任意数据帧m中的每个扫描点利用极坐标的方式进行扇形分割建立对应关系，如下：系，如下：其中，θ角为扫描角度，grid为扫描长度，n为扇区个数，m为栅格个数；每个栅格中扫描点与原点的距离为：其中，d
ij
扇形分割中任意i行j列的取值，n
k
为每个栅格中点的个数，d
k
为改点到原点的距离；判断该扫描点是否属于背景：其中，i
k
为1，表示该点为非背景的有效点，i
k
为0，表示该点为背景点，b
ij
为每个栅格中到原点距离的平均值，t为克服阈值选择随距离发生变化的比例关系。
4.根据权利要求1所述的一种机场复杂环境下的地勤车辆精准定位方法，其特征在于，步骤4所述的路侧激光雷达对目标车辆的定位算法，具体如下：为t时刻激光雷达获取目标车辆卡尔曼滤波的系统状态量,与为路侧激光雷达在t-1时刻的最优估计预测t时刻的车辆位姿与协方差矩阵，如下：1时刻的最优估计预测t时刻的车辆位姿与协方差矩阵，如下：式中：a
t
为状态转移矩阵，ψ
t
为系统状态误差，q
t
为系统状态误差的方差；t时刻第i个观测量为目标观测量的概率如下：式中：为激光雷达在t时刻观测量集合，其中为t时刻第i个观测量，m
t
为激光雷达在t时刻观测量的总数；φ
t
＝{z1,z2,
…
,z
t
}为直到t时刻激光雷达累积观测量集合；表示t时刻第i个观测量为正确观测量；为的联合概率密度函数；为z
t
的联合概率密度函数；设定跟踪门，求取z
t
与的联合概率密度函数；获取t时刻第i个观测量为正确观测量的概率和t时刻无正确观测量的概率根据卡尔曼滤波原理，可得t时刻目标状态的位姿估计和协方差，进而获取激光雷达目标车辆定位信息。5.根据权利要求1所述的一种机场复杂环境下的地勤车辆精准定位方法，其特征在于，步骤5所述的协同容错融合定位算法，具体如下：k时刻，惯性与gnss融合子滤波器和惯性与激光雷达融合子滤波器利用k-1时刻主滤波器分配和重置的参数进行更新；惯性与gnss融合的故障检测，利用主滤波器一步预测协方差矩阵结算残差协方差和故障检测函数值；激光雷达的故障检测根据摄像头的影像信息判断目标车辆是否有遮挡，当遮挡存在时激光雷达的测距将发生故障；故障检测函数与设定阈值进行对比，若小于阈值则认定子系统无故障，通过反馈系数结合量测噪声对子系统进行调节；若大于阈值认定为子系统发生故障，该子系统不参与融合估计，目标定位结果由另一子系统进行估计；若两子系统均发生故障，选择与阈值相差最小的子系统来进行估计，获取目标该时刻定位结果。6.根据权利要求1所述的一种机场复杂环境下的地勤车辆精准定位方法，其特征在于，车辆、摄像头、激光雷达的坐标系为：车辆坐标系为世界坐标系，且坐标点为车辆的质心；车载惯性导航在车辆坐标系中的相对位姿为其中，v指车辆，i指车载惯
性导航；摄像头坐标系原点为摄像头在空间区域所处的位姿γ
c
＝[x
c
,y
c
,θ
c
]
t
，其中，c指摄像头，摄像头所捕获的目标观测量为该相机坐标下的相对位姿，通过γ
c
矢量叠加可以将目标坐标转换成世界坐标系中的绝对位置激光雷达坐标系原点为激光雷达在空间区域所安装的位姿γ
l
＝[x
l
,y
l
,θ
l
]
t
，其中，l指激光雷达，激光雷达所捕获的目标观测量为该雷达坐标下的相对位姿，通过γ
l
矢量叠加可以将目标坐标转换成世界坐标系中的绝对位置摄像头与激光雷达的固定位置在不影响功能的前提下应相邻安放，使得摄像头补足的画面与激光雷达扫描点云信息方向角大致吻合，以便判断目标车辆是否被其他移动物体遮挡。7.根据权利要求1所述的一种机场复杂环境下的地勤车辆精准定位方法，其特征在于，所述车路通信模块采用v2x通信方式获取车辆的自主定位信息与路侧摄像头及激光雷达测量信息，包括：gnss测量绝对位置信息、车辆速度加速度和航向信息、视觉相对位置信息、激光雷达相对位置信息，最终获取所述位置信息；所述v2x通信方式为dsrc、蓝牙、wi-fi、5g中的至少一种。

技术总结
本发明公开一种机场复杂环境下的地勤车辆精准定位方法，以降低自主定位成本为前提，重点解决机场半室外及航空器遮挡环境下地勤车辆定位精度差的问题。该算法为：车辆进入非理想区域路侧摄像头根据车辆自主定位信息及运动状态识别车辆；路侧激光雷达锁定目标，基于概率数据关联的卡尔曼滤波位姿估计消除非检测目标的干扰对车辆进行定位；根据环境特点采用协同容错融合算法实现高精度定位。本发明在减少车辆定位设备成本的基础上可为车辆在非理想环境下提供了精确的位置信息，提升车辆运行的安全性与经济性，同时该方法具有精度高和鲁棒性强的优点。和鲁棒性强的优点。


技术研发人员：张昊 王庆 阳媛 牛运丰 汤立凡
受保护的技术使用者：江苏集萃未来城市应用技术研究所有限公司
技术研发日：2021.10.28
技术公布日：2022/5/25