一种城市峡谷环境下激光里程计辅助的快速优化选星方法

摘要

本发明公开了一种城市峡谷环境下激光里程计辅助的快速优化选星方法。本方法首先利用激光里程计准确推算智能车的先验绝对位置，其次，依托准确的智能车位置信息精确计算卫星的高度角和方位角，接着，计算自适应截止高度角，并利用模糊规则进行快速优化选星，最后，依据选择卫星的数量对智能车实施多模式定位策略。本发明公开的快速优化选星方法，克服了现有方法卫星高度角和方位角计算不精准、适应性不强、选星效率低、定位误差大等难题，保障了城市峡谷环境下智能车的高精定位。

权利要求

1.一种城市峡谷环境下激光里程计辅助的快速优化选星方法，其特征在于，在城市峡谷环境下，根据激光里程计确定智能车的先验绝对位置，进而计算准确的卫星高度角和方位角，结合优化选星算法，并根据选星结果选用不同定位策略，实现智能车在城市峡谷环境下准确可靠的定位，所述方法包括以下步骤：步骤一：利用雷达里程计推算智能车先验绝对位置记k‑1时刻在地心大地坐标下对智能车的纬度、经度和高度定位结果为通过下式将其转换至地心空间直角坐标系下为(xk‑1,yk‑1,zk‑1)：上式中，e为地球椭球偏心率，N为地球基准椭球体曲率半径，记k时刻通过激光里程计方法在地心空间直角坐标系下得到的智能车位置增量为(Δxk,Δyk,Δzk)，由此得到k时刻智能车在地心空间直角坐标系下的位置(xk,yk,zk)：再将智能车的位置由地心空间直角坐标系转向地心大地坐标系：式中， 分别表示k时刻的智能车先验经度、纬度和高度，由此得到智能车的先验绝对位置信息；

步骤二：通过车载GNSS接收机接收卫星星历数据，得到各卫星位置坐标通过卫星星历，获取k时刻各卫星的坐标信息，记为 其中，k表示当前时刻，n表示第n颗卫星；

步骤三：基于准确的车辆先验位置信息，精确计算卫星的高度角与方位角卫星的高度角和方位角需要通过智能车和卫星之间的数学关系算出，具体公式如下：式中， 表示k时刻第n颗卫星的高度角， 表示k时刻第n颗卫星的方位角，r为地球半径，R为卫星轨道半径；

步骤四：快速优化选星

子步骤一：依据信噪比进行筛选首先剔除信噪比小于30db/Hz的卫星；

子步骤二：计算自适应截止高度角计算城市峡谷环境下卫星的自适应截止高度角：其中， 表示k时刻的截止高度角，n为卫星序列，当卫星的高度角大于截止高度角时为视距卫星，H为两侧建筑物的高度，d为智能车到右侧建筑物的距离，由于d越小，角度越大，而智能车到右侧建筑物的距离肯定小于左侧建筑物的距离，因此，只计算右侧的截止高度角，设定城市峡谷周围大楼的平均高度为100m；取智能车距道路边缘平均距离为一个半车道的宽度，即5.25m，同时，计算该时刻平均截止高度角记每帧时间间隔为Δk，若连续5*Δk个时刻出现三分之二的卫星高度角都大于平均截止高度角的情况，则修改建筑物平均高度与智能车到右侧建筑物的平均距离为：若连续5*Δk个时刻出现三分之二的卫星高度角都小于平均截止高度角的情况，则修改建筑物平均高度与智能车到右侧建筑物的平均距离为：计算下一时刻的截止高度角时，将使用修改后的参数进行，实现截止高度角的自适应变化；

子步骤三：剔除高度角小于自适应截止高度角的卫星根据计算得到的卫星高度角，剔除其中小于自适应截止高度角的卫星；

子步骤四：依据剩余卫星个数模糊选星借鉴模糊选星思想并进行优化，对剩余卫星进行选星：(1)若剩余卫星数目小于等于五颗，则全部选中；

(2)若剩余卫星数目大于五颗，则先选中高度角最大、第二大和最小的三颗卫星，记剩余卫星个数为m，对剩余卫星按方位角从小到大进行排序，记为 对应的高度角为 计算剩余卫星方位角平均值 对方位角处于 和 之间的q颗卫星构造模糊向量：

构造两者间的模糊关系：

T T T

Z＝[Z1 Z2]           (10)改进的自适应权重：

P＝[p1 p2]                        (11)上式中，有：

最后进行模糊变换：

Q＝P·Z                          (13)Q中元素最小的即第四颗卫星，接着，对方位角处于 和 之间的卫星再进行一次模糊选星，选出第五颗星；

步骤五：基于选中的卫星与激光里程计对智能车进行定位子步骤一：读取选中卫星数量

根据选中卫星的数量，采取不同的定位策略；

子步骤二：多模式定位策略

(1)若选取的卫星数大于等于四颗，则将卫星的经纬高用作观测量，激光里程计的递推数据为状态量，构建卡尔曼滤波方程，获取定位结果；

(2)若选取卫星数量等于三颗，此时可以解算出智能车的经纬度信息，但无法获取高度信息，将卫星计算的经纬度用作观测量，激光里程计递推数据作为状态量，构建卡尔曼滤波方程，获取定位结果；

(3)若选取卫星数小于三颗，则使用激光里程计递推数据作为定位结果；

通过选取不同模式，最终得到智能车k时刻在地心大地坐标下的定位结果

说明书

一种城市峡谷环境下激光里程计辅助的快速优化选星方法

技术领域

[0001] 本发明涉及车辆导航定位领域，尤其涉及一种城市峡谷环境下激光里程计辅助的快速优化选星方法。

背景技术

[0002] 近年来，随着传感器、电子信息、深度学习等技术的不断成熟以及相关政策法规的逐步完善，如自适应巡航(Adaptive Cruise Control，ACC)、自动紧急制动(Autonomous 
Emergency Braking，AEB)、车道保持(Lane keep assistance，LKA)等智能驾驶技术有了长
足进步，推动智能车行业迅速发展。智能驾驶技术作为新一轮科技革命的前沿，不仅提升了
车辆的智能化程度，也提高了交通出行的效率与安全。现有的智能驾驶技术依赖于准确的
智能车位置信息。当智能车位置信息存在较大误差甚至错误时，会给后续的加减速、方向控
制等智能驾驶策略带来误导，严重时甚至可能引发重大安全问题，危及社会公共安全与人
民群众生命财产安全。目前，智能车定位方法主要依靠全球导航卫星系统(Global 
Navigation Satellite System，GNSS)。该系统通过车载接收机接收不少于四颗卫星信号
对智能车进行空间定位，可以实现大多数情况下的智能车高精定位。

[0003] 城市是人类生活和智能车运行的主要区域。随着城市化进程的加快，城市道路两旁经常会出现稠密高耸的建筑物，形成类似自然峡谷的城市峡谷环境。此类环境下卫星定
位存在着非视距效应和多路径效应两类问题。非视距效应是指高楼等阻碍卫星信号，使信
号无法到达车载接收机；多路径效应是指卫星信号有可能经过多条路径到达车载接收机，
从而产生信号混叠。城市峡谷环境下有极大的人/车流量，而非视距效应或多路径效应会严
重降低卫星对智能车的定位精度，影响智能驾驶控制策略，给社会公共安全和人民生命财
产安全带来极大隐患。因此，实现智能车在城市峡谷环境下的高精可靠定位及其关键。

[0004] 目前，城市峡谷环境下智能车高精定位的解决方案是基于卫星/惯导的优化选星和组合导航。依托惯导辅助选星，选用视距范围内和受多路径效应影响较小的卫星，利用此
类卫星数据融合惯导数据进行定位；当卫星数不足四颗时，利用惯导来确定位置。然而，现
有方法存在一定问题：(1)卫星高度角和方位角计算不准确。高度角和方位角是选星的基
础，需要通过智能车与卫星之间的数学关系进行计算。虽然卫星的位置是精确已知的，但城
市峡谷环境下车辆的位置信息中夹杂着由非视距和多路径引起的噪声以及惯导的累计误
差，因此并不准确。这就造成卫星的高度角和方位角计算不准，导致每次选星结果不一定最
优；(2)选星算法适用性不强。现有选星算法中截止高度角通常为定值，难以适应多种场景，
容易选中非视距卫星或丢弃部分视距卫星；(3)选星算法计算量大。现有选星算法存在大量
的矩阵乘法和矩阵逆运算，算法所耗时间约占据单次位置解算时间的一半以上，影响定位
输出频率，难以满足城市环境下的智能车定位需求；(4)惯导单独定位的累计误差。当卫星
数不足四颗时，依靠惯导定位会带来累计误差，车载惯导不足以支撑智能车在卫星信号缺
失下的高精定位。

发明内容

[0005] 本发明提出了一种激光里程计辅助快速优化选星方法，实现智能车在城市峡谷环境下的高精定位，主要特征如下：(1)准确的高度角和方位角计算。配备车载激光雷达传感
器，依托激光雷达高精度的测量结果，利用雷达里程计方法获取车辆位置的增量信息，为优
化选星提供准确的车辆先验位置。雷达里程计通过计算雷达前后帧点云的旋转和平移矩
阵，得到车辆位置的增量信息，配合时间序列上最近的智能车绝对位置，从而得到智能车准
确的先验绝对位置信息，确保卫星高度角和方位角的准确性；(2)自适应截止高度角。截止
高度角根据实际情况可以实现自适应调整，避免漏掉视距卫星或选中非视距卫星；(3)快速
优化选星方法。基于准确的卫星高度角和方位角，采取一种改进的模糊选星算法，对卫星进
行优化选星。此算法不涉及大量的矩阵乘法和矩阵逆运算，提高了选星效率；(4)多模式定
位策略。当选中卫星数目大于等于四颗时，利用选星后的经纬高数据与激光里程计融合对
车辆进行定位；当选中卫星数目等于三颗时，融合卫星计算的经纬信息与激光里程计信息；
当选中卫星数目不足三颗时，利用比惯导精度更高的激光里程计进行递推，实现车辆位置
解算。

[0006] 本发明提出的优化选星方法解决了目前卫星高度角和方位角计算不精准的问题，提升了选星速率与质量，并克服了惯导累计误差的影响，为智能驾驶技术的发展奠定了坚
实基础，为城市峡谷环境下智能车辆的高精定位及安全性提供了充分保障，对智能车行业
的发展有积极重要的意义。

[0007] 下面对本发明的思路做进一步说明：

[0008] 本发明在城市峡谷环境下，根据激光里程计确定智能车的先验绝对位置，进而计算准确的卫星高度角和方位角，结合优化选星算法，有效减小非视距效应和多路径效应带
来的干扰，克服了惯导累计误差的影响，并根据选星结果选用不同定位策略，实现智能车在
城市峡谷环境下准确可靠的定位，具体包括以下步骤：

[0009] 步骤一：利用雷达里程计推算智能车先验绝对位置

[0010] 激光雷达传感器测距精度高，鲁棒性好，在智能车行业已有较为广泛的应用。激光雷达通过对前后两帧点云的匹配，可以获取前后两帧之间的旋转和平移矩阵，进而得出增
量信息。激光雷达的这种功能与汽车里程计类似，因此也称为激光里程计，激光里程计的详
细原理可见参考文献(JiZhang,SanjivSingh.Low‑drift and real‑time lidar odometry 
and mapping[J].Autonomous Robots,2017.)。记本发明k‑1时刻在地心大地坐标下对智能
车的纬度、经度和高度定位结果为 通过下式将其转换至地心空间直角坐标
系下为(xk‑1,yk‑1,zk‑1)：

[0011]

[0012] 上式中，e为地球椭球偏心率，N为地球基准椭球体曲率半径。记k时刻通过激光里程计方法在地心空间直角坐标系下得到的智能车位置增量为(Δxk,Δyk,Δzk)，由此得到k
时刻智能车在地心空间直角坐标系下的位置(xk,yk,zk)：

[0013]

[0014] 再将智能车的位置由地心空间直角坐标系转向地心大地坐标系：

[0015]

[0016] 式中， 分别表示k时刻的智能车先验经度、纬度和高度，由此得到智能车的先验绝对位置信息；

[0017] 步骤二：通过车载GNSS接收机接收卫星星历数据，得到各卫星位置坐标

[0018] 通过卫星星历，获取k时刻各卫星的坐标信息，记为 其中，k表示当前时刻，n表示第n颗卫星。

[0019] 步骤三：基于准确的车辆先验位置信息，精确计算卫星的高度角与方位角

[0020] 卫星的高度角和方位角需要通过智能车和卫星之间的数学关系算出，具体公式如下：

[0021]

[0022] 式中， 表示k时刻第n颗卫星的高度角， 表示k时刻第n颗卫星的方位角，r为地球半径，R为卫星轨道半径。

[0023] 步骤四：快速优化选星

[0024] 子步骤一：依据信噪比进行筛选

[0025] 首先剔除信噪比小于30db/Hz的卫星；

[0026] 子步骤二：计算自适应截止高度角

[0027] 计算城市峡谷环境下卫星的自适应截止高度角：

[0028]

[0029] 其中， 表示k时刻的截止高度角，n为卫星序列，当卫星的高度角大于截止高度角时为视距卫星，H为两侧建筑物的高度，d为智能车到右侧建筑物的距离，由于d越小，角度越
大，而智能车到右侧建筑物的距离肯定小于左侧建筑物的距离，因此，本发明中只计算右侧
的截止高度角，根据我国《民用建筑设计通则(GB50352‑2005)》，本发明中设定城市峡谷周
围大楼的平均高度为100m；根据《城市道路交叉口规划规范(GB50647)》、《城市道路交叉口
设计规程(CJJ152)》、《城市道路工程设计规范(CJJ37)》、《城市快速路设计规范(CJJ129)》
等，取智能车距道路边缘平均距离为一个半车道的宽度，即5.25m，同时，计算该时刻平均截
止高度角

[0030]

[0031] 记每帧时间间隔为Δk，若连续5*Δk个时刻出现三分之二的卫星高度角都大于平均截止高度角的情况，则修改建筑物平均高度与智能车到右侧建筑物的平均距离为：

[0032]

[0033] 若连续5*Δk个时刻出现三分之二的卫星高度角都小于平均截止高度角的情况，则修改建筑物平均高度与智能车到右侧建筑物的平均距离为：

[0034]

[0035] 计算下一时刻的截止高度角时，将使用修改后的参数进行，实现截止高度角的自适应变化；

[0036] 子步骤三：剔除高度角小于自适应截止高度角的卫星

[0037] 根据计算得到的卫星高度角，剔除其中小于自适应截止高度角的卫星；

[0038] 子步骤四：依据剩余卫星个数模糊选星

[0039] 借鉴模糊选星思想并进行优化，对剩余卫星进行选星：

[0040] (1)若剩余卫星数目小于等于五颗，则全部选中；

[0041] (2)若剩余卫星数目大于五颗，则先选中高度角最大、第二大和最小的三颗卫星，记剩余卫星个数为m，对剩余卫星按方位角从小到大进行排序，记为 对
应的高度角为 计算剩余卫星方位角平均值 对方位角处于 和 之
间的q颗卫星构造模糊向量：

[0042]

[0043] 构造两者间的模糊关系：

[0044] Z＝[Z1T Z2T]T   (10)

[0045] 改进的自适应权重：

[0046] P＝[p1 p2]   (11)

[0047] 上式中，有：

[0048]

[0049] 最后进行模糊变换：

[0050] Q＝P·Z    (13)

[0051] Q中元素最小的即第四颗卫星。接着，对方位角处于 和 之间的卫星再进行一次模糊选星，选出第五颗星。

[0052] 步骤五：基于选中的卫星与激光里程计对智能车进行定位

[0053] 子步骤一：读取选中卫星数量

[0054] 根据选中卫星的数量，采取不同的定位策略。

[0055] 子步骤二：多模式定位策略

[0056] (1)若选取的卫星数大于等于四颗，则将卫星的经纬高用作观测量，激光里程计的递推数据为状态量，构建卡尔曼滤波方程，获取定位结果，具体卡尔曼滤波方法详见参考文
献(秦永元、张洪钺、汪叔华.卡尔曼滤波与组合导航[M].西北工业大学出版社，2012)；

[0057] (2)若选取卫星数量等于三颗，此时可以解算出智能车的经纬度信息，但无法获取高度信息。将卫星计算的经纬度用作观测量，激光里程计递推数据作为状态量，构建卡尔曼
滤波方程，获取定位结果；

[0058] (3)若选取卫星数小于三颗，则使用激光里程计递推数据作为定位结果。

[0059] 通过选取不同模式，最终得到智能车k时刻在地心大地坐标下的定位结果

附图说明

[0060] 图1是本发明的技术方案图；

[0061] 图2是本发明的选星方法流程图。

具体实施方式

[0062] 近年来，随着传感器、电子信息、深度学习等技术的不断成熟以及相关政策法规的逐步完善，如自适应巡航(Adaptive Cruise Control，ACC)、自动紧急制动(Autonomous 
Emergency Braking，AEB)、车道保持(Lane keep assistance，LKA)等智能驾驶技术有了长
足进步，推动智能车行业迅速发展。智能驾驶技术作为新一轮科技革命的前沿，不仅提升了
车辆的智能化程度，也提高了交通出行的效率与安全。现有的智能驾驶技术依赖于准确的
智能车位置信息，当智能车位置信息存在较大误差甚至错误时，会给后续的加减速、方向控
制等智能驾驶策略带来误导，严重时甚至可能引发重大安全问题，危及社会公共安全与人
民群众生命财产安全。目前，智能车定位方法主要依靠全球导航卫星系统(Global 
Navigation Satellite System，GNSS)。该系统通过车载接收机接收不少于四颗卫星信号
对智能车进行空间定位，可以实现大多数情况下的智能车高精定位。

[0063] 城市是人类生活和智能车运行的主要区域。随着城市化进程的加快，城市道路两旁经常会出现稠密高耸的建筑物，形成类似自然峡谷的城市峡谷环境。此类环境下卫星定
位存在着非视距效应和多路径效应两类问题。非视距效应是指高楼等阻碍卫星信号，使信
号无法到达车载接收机；多路径效应是指卫星信号有可能经过多条路径到达车载接收机，
从而产生信号混叠。城市峡谷环境下有极大的人/车流量，而非视距效应或多路径效应会严
重降低卫星对智能车的定位精度，影响智能驾驶控制策略，给社会公共安全和人民生命财
产安全带来极大隐患。因此，实现智能车在城市峡谷环境下的高精可靠定位及其关键。

[0064] 目前，城市峡谷环境下智能车高精定位的解决方案是基于卫星/惯导的优化选星和组合导航。依托惯导辅助选星，选用视距范围内和受多路径效应影响较小的卫星，利用此
类卫星数据融合惯导数据进行定位；当卫星数不足四颗时，利用惯导来确定位置。然而，现
有方法存在一定问题：(1)卫星高度角和方位角计算不准确。高度角和方位角是选星的基
础，需要通过智能车与卫星之间的数学关系进行计算。虽然卫星的位置是精确已知的，但城
市峡谷环境下车辆的位置信息中夹杂着由非视距和多路径引起的噪声以及惯导的累计误
差，因此并不准确。这就造成卫星的高度角和方位角计算不准，导致每次选星结果不一定最
优；(2)选星算法适用性不强。现有选星算法中截止高度角通常为定值，难以适应多种场景，
容易选中非视距卫星或丢弃部分视距卫星；(3)选星算法计算量大。现有选星算法存在大量
的矩阵乘法和矩阵逆运算，算法所耗时间约占据单次位置解算时间的一半以上，影响定位
输出频率，难以满足城市环境下的智能车定位需求；(4)惯导单独定位的累计误差。当卫星
数不足四颗时，依靠惯导定位会带来累计误差，车载惯导不足以支撑智能车在卫星信号缺
失下的高精定位。

[0065] 针对现有问题，本发明提出了一种激光里程计辅助快速优化选星方法，实现智能车在城市峡谷环境下的高精定位，主要特征如下：(1)准确的高度角和方位角计算。配备车
载激光雷达传感器，依托激光雷达高精度的测量结果，利用雷达里程计方法获取车辆位置
的增量信息，为优化选星提供准确的车辆先验位置。雷达里程计通过计算雷达前后帧点云
的旋转和平移矩阵，得到车辆位置的增量信息，配合时间序列上最近的智能车绝对位置，从
而得到智能车准确的先验绝对位置信息，确保卫星高度角和方位角的准确性；(2)自适应截
止高度角。截止高度角根据实际情况可以实现自适应调整，避免漏掉视距卫星或选中非视
距卫星；(3)快速优化选星方法。基于准确的卫星高度角和方位角，采取一种改进的模糊选
星算法，对卫星进行优化选星。此算法不涉及大量的矩阵乘法和矩阵逆运算，提高了选星效
率；(4)多模式定位策略。当选中卫星数目大于等于四颗时，利用选星后的经纬高数据与激
光里程计融合对车辆进行定位；当选中卫星数目等于三颗时，融合卫星计算的经纬信息与
激光里程计信息；当选中卫星数目不足三颗时，利用比惯导精度更高的激光里程计进行递
推，实现车辆位置解算。

[0066] 本发明提出的优化选星方法解决了目前卫星高度角和方位角计算不精准的问题，提升了选星质量与速度，并克服了惯导累计误差的影响，为智能驾驶技术的发展奠定了坚
实基础，为城市峡谷环境下智能车辆的高精定位及安全性提供了充分保障，对智能车行业
的发展有积极重要的意义。

[0067] 下面对本发明的思路做进一步说明：

[0068] 步骤一：利用雷达里程计推算智能车先验绝对位置

[0069] 激光雷达传感器测距精度高，鲁棒性好，在智能车行业已有较为广泛的应用。激光雷达通过对前后两帧点云的匹配，可以获取前后两帧之间的旋转和平移矩阵，进而得出增
量信息。激光雷达的这种功能与汽车里程计类似，因此也称为激光里程计，激光里程计的详
细原理可见参考文献(JiZhang,SanjivSingh.Low‑drift and real‑time lidar odometry 
and mapping[J].Autonomous Robots,2017.)。记本发明k‑1时刻在地心大地坐标下对智能
车的纬度、经度和高度定位结果为 通过下式将其转换至地心空间直角坐
标系下为(xk‑1,yk‑1,zk‑1)：

[0070]

[0071] 上式中，e为地球椭球偏心率，N为地球基准椭球体曲率半径。记k时刻通过激光里程计方法在地心空间直角坐标系下得到的智能车位置增量为(Δxk,Δyk,Δzk)，由此得到k
时刻智能车在地心空间直角坐标系下的位置(xk,yk,zk)：

[0072]

[0073] 再将智能车的位置由地心空间直角坐标系转向地心大地坐标系：

[0074]

[0075] 式中， 分别表示k时刻的智能车先验经度、纬度和高度，由此得到智能车的先验绝对位置信息；

[0076] 步骤二：通过车载GNSS接收机接收卫星星历数据，得到各卫星位置坐标

[0077] 通过卫星星历，获取k时刻各卫星的坐标信息，记为 其中，k表示当前时刻，n表示第n颗卫星。

[0078] 步骤三：基于准确的车辆先验位置信息，精确计算卫星的高度角与方位角

[0079] 卫星的高度角和方位角需要通过智能车和卫星之间的数学关系算出，具体公式如下：

[0080]

[0081] 式中， 表示k时刻第n颗卫星的高度角， 表示k时刻第n颗卫星的方位角，r为地球半径，R为卫星轨道半径。

[0082] 步骤四：快速优化选星

[0083] 子步骤一：依据信噪比进行筛选

[0084] 首先剔除信噪比小于30db/Hz的卫星；

[0085] 子步骤二：计算自适应截止高度角

[0086] 计算城市峡谷环境下卫星的自适应截止高度角：

[0087]

[0088] 其中， 表示k时刻的截止高度角，n为卫星序列，当卫星的高度角大于截止高度角时为视距卫星，H为两侧建筑物的高度，d为智能车到右侧建筑物的距离，由于d越小，角度越
大，而智能车到右侧建筑物的距离肯定小于左侧建筑物的距离，因此，本发明中只计算右侧
的截止高度角，根据我国《民用建筑设计通则(GB50352‑2005)》，本发明中设定城市峡谷周
围大楼的平均高度为100m；根据《城市道路交叉口规划规范(GB50647)》、《城市道路交叉口
设计规程(CJJ152)》、《城市道路工程设计规范(CJJ37)》、《城市快速路设计规范(CJJ129)》
等，取智能车距道路边缘平均距离为一个半车道的宽度，即5.25m，同时，计算该时刻平均截
止高度角

[0089]

[0090] 记每帧时间间隔为Δk，若连续5*Δk个时刻出现三分之二的卫星高度角都大于平均截止高度角的情况，则修改建筑物平均高度与智能车到右侧建筑物的平均距离为：

[0091]

[0092] 若连续5*Δk个时刻出现三分之二的卫星高度角都小于平均截止高度角的情况，则修改建筑物平均高度与智能车到右侧建筑物的平均距离为：

[0093]

[0094] 计算下一时刻的截止高度角时，将使用修改后的参数进行，实现截止高度角的自适应变化；

[0095] 子步骤三：剔除高度角小于自适应截止高度角的卫星

[0096] 根据计算得到的卫星高度角，剔除其中小于自适应截止高度角的卫星；

[0097] 子步骤四：依据剩余卫星个数模糊选星

[0098] 借鉴模糊选星思想并进行优化，对剩余卫星进行选星：

[0099] (1)若剩余卫星数目小于等于五颗，则全部选中；

[0100] (2)若剩余卫星数目大于五颗，则先选中高度角最大、第二大和最小的三颗卫星，记剩余卫星个数为m，对剩余卫星按方位角从小到大进行排序，记为 对
应的高度角为 计算剩余卫星方位角平均值 对方位角处于 和 之
间的q颗卫星构造模糊向量：

[0101]

[0102] 构造两者间的模糊关系：

[0103] Z＝[Z1T Z2T]T   (23)

[0104] 改进的自适应权重：

[0105] P＝[p1 p2]   (24)

[0106] 上式中，有：

[0107]

[0108] 最后进行模糊变换：

[0109] Q＝P·Z   (26)

[0110] Q中元素最小的即第四颗卫星。接着，对方位角处于 和 之间的卫星再进行一次模糊选星，选出第五颗星。

[0111] 步骤五：基于选中的卫星与激光里程计对智能车进行定位

[0112] 子步骤一：读取选中卫星数量

[0113] 根据选中卫星的数量，采取不同的定位策略。

[0114] 子步骤二：多模式定位策略

[0115] (1)若选取的卫星数大于等于四颗，则将卫星的经纬高用作观测量，激光里程计的递推数据为状态量，构建卡尔曼滤波方程，获取定位结果，具体卡尔曼滤波方法详见参考文
献(秦永元、张洪钺、汪叔华.卡尔曼滤波与组合导航[M].西北工业大学出版社，2012)；

[0116] (2)若选取卫星数量等于三颗，此时可以解算出智能车的经纬度信息，但无法获取高度信息。将卫星计算的经纬度用作观测量，激光里程计递推数据作为状态量，构建卡尔曼
滤波方程，获取定位结果；

[0117] (3)若选取卫星数小于三颗，则使用激光里程计递推数据作为定位结果。

[0118] 通过选取不同模式，最终得到智能车k时刻在地心大地坐标下的定位结果