CN111380557A - 一种无人车全局路径规划方法及装置 - Google Patents

一种无人车全局路径规划方法及装置 Download PDF

Info

Publication number
CN111380557A
CN111380557A CN202010212807.7A CN202010212807A CN111380557A CN 111380557 A CN111380557 A CN 111380557A CN 202010212807 A CN202010212807 A CN 202010212807A CN 111380557 A CN111380557 A CN 111380557A
Authority
CN
China
Prior art keywords
satellite
time
coordinate system
road
calculating
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202010212807.7A
Other languages
English (en)
Inventor
李子月
刘玉超
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Individual
Original Assignee
Individual
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Individual filed Critical Individual
Priority to CN202010212807.7A priority Critical patent/CN111380557A/zh
Publication of CN111380557A publication Critical patent/CN111380557A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3453—Special cost functions, i.e. other than distance or default speed limit of road segments
    • G01C21/3492—Special cost functions, i.e. other than distance or default speed limit of road segments employing speed data or traffic data, e.g. real-time or historical
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/03—Cooperating elements; Interaction or communication between different cooperating elements or between cooperating elements and receivers
    • G01S19/08—Cooperating elements; Interaction or communication between different cooperating elements or between cooperating elements and receivers providing integrity information, e.g. health of satellites or quality of ephemeris data
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Computer Security & Cryptography (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种无人车全局路径规划方法及装置，该方法包括：根据高精度地图、起始点、目的地信息进行静态全局路径规划，获得距离最短的方案；在距离最短的轨迹方案中，增加实时路况信息，通过行驶速度获得动态行驶时间最短方案；利用GPS/BDS卫星定位精度因子对行驶速度进行调整，形成适合于无人车的时间最短全局路径规划。和传统方法相比，利用本发明方法获得的最短时间路径更适用于无人车使用，且无需增加无人车硬件成本。

Description

一种无人车全局路径规划方法及装置
技术领域
本发明涉及无人车路径规划领域，尤其涉及一种无人车全局路径规划方法及装置。
背景技术
随着DARPA重启无人车技术研究，并先后于2004年、2005年和2007年举办了三届智能车挑战赛，谷歌大力投入自动驾驶技术研究，近年来全球范围内掀起了自动驾驶技术研究的热潮。参与者既有谷歌、苹果、Uber、BAT等顶尖互联网企业，也有特斯拉、奔驰、宝马、通用、凯迪拉克等传统车企，同时，英特尔、英伟达、博世、Mobileye等零部件供应商也参与其中，另外自动驾驶领域创业公司也如雨后春笋般应运而生。
自动驾驶技术经过十多年发展，逐渐从实验室研究、到封闭环境技术竞赛、再到固定场景示范应用，虽然取得了长足的进展，但距离实际应用仍有较大距离。车企普遍采用的由低级辅助驾驶到高级辅助驾驶，再到限定场景自动驾驶，最后到达全自动驾驶的路途并不十分顺利，例如特斯拉公司车辆配备的辅助驾驶系统，至今仍事故频出。谷歌、BAT互联网公司及创业公司采用的由低速、封闭场景逐渐向外延伸的技术路线发展也并不理想，目前仍然只能在封闭场景低速驾驶状态下执行简单确定性任务，并且可靠性较低。
路径规划是一项重要的无人车技术，主要实现由起始点到目标点的最优轨迹规划。传统全局路径规划主要基于高精度地图、实时路况等信息，设定起始点和目标点后，规划算法根据地图中的道路长度、实时路况等信息，按照距离最短、时间最短等原则，进行最优道路选择。但是，区别于人工驾驶车辆，无人车行驶状态很大程度上受GPS/BDS卫星导航定位状态影响。GPS/BDS卫星导航设备是无人车感知系统重要的传感器，可以为无人车提供高精度位置、速度等信息。由于GPS/BDS卫星导航信号容易受到遮挡、干扰等环境因素影响，难以为无人车提供持续、稳定的导航定位数据，尤其在城市环境中，遮挡、弱信号、多路径效应影响明显，GPS/BDS卫星导航精度和状态的不确定性给无人车的运行带来很大影响。因此，为了实现更科学的轨迹规划，有必要将GPS/BDS卫星定位状态参数作为启发函数或者代价函数的一部分，实现无人车考虑导航状态下的最优轨迹规划。
发明内容
本发明的目的是提供一种无人车全局路径规划方法及装置，以解决GPS/BDS卫星定位状态不确定性影响轨迹规划效果的问题。
为了达到上述目的，第一方面，本发明的实施例提供一种无人车全局路径规划方法，包括：
根据高精度地图、起始点、目的地信息进行静态全局路径规划，获得距离最短的方案；
在距离最短的轨迹方案中，增加实时路况信息，通过行驶速度获得动态行驶时间最短方案；
利用GPS/BDS卫星定位精度因子对行驶速度进行调整，形成适合于无人车的时间最短全局路径规划。
进一步地，获得距离最短的方案，包括：
将静态最短路径规划离散化，并根据道路实时路况，获得每个路段对应的车辆行驶速度，对代价函数进行调整，更新路径规划方案排序，获得行使时间最短路径规划。
进一步地，获得每个路段对应的车辆行驶速度，包括：
将全局规划路径的路段，按照距离离散化为m个单元，根据道路拥堵状况、红绿灯、道路限速、自然天气，获取离散化的每个单元路段对应的实时路况的车辆行驶速度vi。
进一步地，获得行使时间最短路径规划，包括：
利用速度参数vi，对代价函数进行调整，获取最短时间路径规划，具体见式(2)；
Figure BDA0002423382770000021
根据传传统全局路径规划中的路径搜索，按照式(2)计算方法，重新更新最优路径，获得最短时间路径规划。
进一步地，GPS/BDS卫星定位精度因子的计算，包括：
初始状态获取步骤，获取对应于当前时刻和路段单元位置参数值；
卫星星座分布实时计算步骤，根据卫星导航接收机获取导航电文，提取卫星轨道相关关键参数，获取当前时刻卫星星座分布，并计算卫星实时位置；
载体坐标系中卫星仰角计算步骤，通过姿态转移矩阵和平移向量对卫星进行坐标系变换，计算得到的地心地固坐标系变换到以道路单元中心为原点的地理坐标系，然后再根据道路单元姿态，利用地理坐标系至载体坐标系姿态矩阵，将其变换到载体坐标系，根据卫星在载体坐标系中的位置，计算对应于每个轨迹点的卫星实时仰角；
根据高精度地图的可视卫星选取步骤，根据高精度地图信息，计算道路单元相关环境信息中，建筑物轮廓对应的经度、纬度和高度信息，通过姿态矩阵和平移向量进行坐标转换，得到建筑物轮廓在以道路单元为原点的载体坐标系中的坐标；
卫星导航定位精度因子计算步骤，根据计算得到的可视卫星坐标信息，计算路段单元位置对应的卫星定位精度因子。
进一步地，卫星星座分布实时计算步骤，包括：
(1)计算归一化时间，因为卫星的星历数据都是相对于参考时刻toe而言的，所以需要将观测时刻做归一化处理，
tk＝ti-toe
其中，ti为当前时刻，tk为归一化之后的时间；
(2)计算卫星的平均角速度n：
将导航电文中的扰动修正项带入：
Figure BDA0002423382770000031
式中μ为地心引力常数，a为椭圆半长轴，由星历数据提供，再利用星历数据中提供的修正项Δn得到平均角速度：
n＝n0+Δn
(3)计算信号发射时刻的平近点角Mk
Mk＝M0+ntk
式中M0为参考时间的平近点角；
(4)计算卫星在信号发射时刻的偏近点角Ek
E＝Mk+essinEk
式中es为卫星椭圆轨道的偏心率，这是个超越方程，需要用迭代法求解，E为偏近点角；
(5)计算信号发射时刻的真近点角νk；
Figure BDA0002423382770000032
式中，e为卫星轨道离心率；
(6)计算卫星的地心向径r
r＝a(1-escosEk)
(7)计算信号发射时刻的升交点角距φk；利用卫星星历中提供的卫星轨道的近地点角距ω，代入如下公式：
φk＝vk+ω
(8)计算信号发射时刻摄动升交点角距修正项δμk、卫星地心向径修正项δrk和卫星轨道倾角修正项δik：
升交点角距修正项δμk：
δμk＝Cuccos2φk+Cussin2φk
卫星地心向径修正项δrk：
δrk＝Crccos2φk+Crssin2φk
卫星轨道倾角修正项δik：
δik＝Ciccos2φk+Cissin2φk
Cuc为纬度辐角的余弦调和修正项的幅度，Cus为纬度辐角的正弦调和修正项的幅度，Crc为卫星地心距的余弦调和修正值，Crs为卫星地心矩的正弦调和修正值，Cic为轨道倾角的余弦调和改正值，Cis为轨道倾角的正弦调和改正值，Cuc、Cus、Crc、Crs、Cic、Cis都来自于卫星星历数据；得到的这三个修正项用来校正φk，rk和ik:
μk＝φk+δμk
rk＝r+δrk
Figure BDA0002423382770000041
式中φk、rk和ik分别k时刻升交点角距、卫星地心向径和卫星轨道倾角，μk为修正后的升交点角距；i0为参考时刻的轨道倾角，
Figure BDA0002423382770000042
为轨道倾角变化率；
(9)计算卫星信号发射时刻在椭圆轨道平面直角坐标系中的位置(x'k，y'k)：
x′k＝rkcosμk
y'k＝rksinμk
(10)计算信号发射时刻的升交点赤经Ωk，计算公式如下：
Figure BDA0002423382770000043
式中Ω0、
Figure BDA0002423382770000051
分别为参考时刻升交点赤经和升交点赤经变化率，
Figure BDA0002423382770000052
等于7.292115146×10-5[rad/s]，是地球自转角速度；
xk＝x'kcosΩk-y'kcosiksinΩk
yk＝y'ksinΩk-y'kcosikcosΩk
zk＝y'ksinik
其中xk，yk，zk为卫星在WGS-84地心地固直角坐标系中的位置，Ωk为卫星信号发射时刻升交点赤经，ik为卫星轨道倾角。
进一步地，载体坐标系中卫星仰角计算步骤，包括：
通过姿态转移矩阵
Figure BDA0002423382770000053
和
Figure BDA0002423382770000054
平移向量对卫星进行坐标系变换，计算得到的地心地固坐标系变换到以道路单元中心为原点的地理坐标系，其中，
Figure BDA0002423382770000055
为地心地固坐标系到地理坐标系的姿态转移矩阵，
Figure BDA0002423382770000056
为地球中心到道路单元中心的平移向量，然后再根据道路单元姿态，利用地理坐标系至载体坐标系姿态矩阵
Figure BDA0002423382770000057
将其变换到载体坐标系，根据卫星在载体坐标系中的位置，计算对应于每个轨迹点的卫星实时仰角，具体计算表达式如下：
Figure BDA0002423382770000058
其中，
Figure BDA0002423382770000059
分别表示载体坐标系b中第i颗卫星仰角的三轴坐标；
Figure BDA00024233827700000510
和
Figure BDA00024233827700000511
具体表达式如下：
Figure BDA00024233827700000512
Figure BDA00024233827700000513
Figure BDA00024233827700000514
其中，λ、L分别为经度和纬度，θ、γ、φ分别为道路单元的俯仰角、横滚角和航向角从高精度地图中的道路信息中获得，
Figure BDA00024233827700000515
分别为地球中心到道路单元中心的平移向量在x、y、z方向的分量；
对应于每个轨迹点的卫星仰角计算公式如下：
Figure BDA0002423382770000061
其中，
Figure BDA0002423382770000062
表示载体坐标系b中第i颗卫星仰角。
进一步地，根据高精度地图的可视卫星选取步骤，包括：
根据高精度地图信息，计算道路单元相关环境信息中，建筑物轮廓对应的经度、纬度和高度信息，通过姿态矩阵和平移向量进行坐标转换，得到建筑物轮廓在以道路单元为原点的载体坐标系中的坐标；具体计算公式如下：
Figure BDA0002423382770000063
Figure BDA0002423382770000064
Figure BDA0002423382770000065
Figure BDA0002423382770000066
其中，n为椭球的卯酉圈曲率半径，e为椭球的第一偏心率，
Figure BDA0002423382770000067
λ、h分别为高楼对应的经度、纬度和高度，
Figure BDA0002423382770000068
分别为高楼对应的地心地固坐标系的坐标，
Figure BDA0002423382770000069
分别为高楼对应的载体坐标系的坐标；
在载体坐标系中，以道路单元切线方向为Y轴，并定义为前向，计算建筑物轮廓前边沿和后边沿在坐标系O-XY平面的投影坐标，然后据此计算前后边沿和无人车在O-XY平面的夹角，具体计算公式如下：
Figure BDA00024233827700000610
Figure BDA00024233827700000611
其中
Figure BDA00024233827700000612
分别为前后边沿在载体坐标系O-XY平面投影点坐标，
Figure BDA00024233827700000613
Figure BDA00024233827700000614
分别为前后边沿和道路单元切线的夹角；当
Figure BDA00024233827700000615
和
Figure BDA00024233827700000616
之差绝对值大于10°时，认为是对卫星影响不可忽略的建筑物，计算上边沿和道路边沿切线之间的夹角，具体公式如下：
Figure BDA00024233827700000617
当
Figure BDA00024233827700000618
大于5°时，计算所有可视卫星和载体坐标系X轴之间的夹角，同时满足在
Figure BDA00024233827700000619
区间内，并且仰角小于
Figure BDA00024233827700000620
两个条件，则被认为是卫星信号被遮挡。
进一步地，卫星导航定位精度因子计算步骤，包括：
根据计算得到的可视卫星坐标信息，计算ti路段单元位置Posii对应的卫星定位精度因子Pdopi；
计算当前可视卫星在载体坐标系中的坐标：
Figure BDA0002423382770000071
其中B、L为观测站大地纬度和大地经度，Xi、Yi、Zi为卫星地心固定坐标系坐标；Xr、Yr、Zr为载体在地心固定坐标系坐标，
Figure BDA0002423382770000072
为可视卫星在载体坐标系中的坐标；
可视卫星的载体极坐标为：
Figure BDA0002423382770000073
Figure BDA0002423382770000074
Figure BDA0002423382770000075
设α、β、γ分别为载体到卫星的直线与X、Y、Z轴的夹角：
Figure BDA0002423382770000076
Figure BDA0002423382770000077
Figure BDA0002423382770000078
如果观测到n颗卫星，星座矩阵为：
Figure BDA0002423382770000079
令DOP＝(QTQ)-1，主对角线元素分别为a、b、c、d，因此求得空间位置精度因子：
Figure BDA00024233827700000710
进一步地，利用GPS/BDS卫星定位精度因子对行驶速度进行调整，形成适合于无人车的时间最短全局路径规划，包括：
(1)以当前起始点位置和当前时刻基准为参考，计算卫星导航定位精度因子，获取实时路况信息；
(2)获取对应于当前时刻ti和路段单元的一组轨迹规划参数，包括路段单元位置Posii、行驶速度vi、代价函数；
(3)计算对应于ti时刻和Posii位置的卫星定位精度因子Pdopi参数值；
(4)将参数vi更新为v′i，计算公式为
v′i＝vi*ki
Figure BDA0002423382770000081
(5)计算对应于该路段单元行驶时间Δti，然后累加至ti，公式为：
Figure BDA0002423382770000082
ti+1＝ti+Δti
dLi为单元路段长度。
(6)按照步骤(2)-(5)，以ti+1为时间基准计算v′i+1，循环计算，直至到达目的地；
(7)以v′代替式(2)中的参数v，再次更新全局路径规划，获得适合无人车应用场景的最短时间路径。
第二方面，本发明实施例还提供一种无人车全局路径规划装置，包括：
静态规划模块，用于根据高精度地图、起始点、目的地信息进行静态全局路径规划，获得距离最短的方案；
时间最短规划模块，用于在距离最短的轨迹方案中，增加实时路况信息，通过行驶速度获得动态行驶时间最短方案；
无人车最优规划模块，用于利用GPS/BDS卫星定位精度因子对行驶速度进行调整，形成适合于无人车的时间最短全局路径规划。
根据上述的技术方案，本发明的实施例在GPS/BDS(Global Positioning System，GPS)/(BeiDou Navigation Satellite System，BDS)卫星导航定位对无人车运行的影响基础之上，将定位精度因子PDOP(Position Dilution of Precision，PDOP)参数引入到路径规划代价函数中。本发明在传统路径最短、时间最短路径规划方法基础之上，增加了GPS/BDS卫星导航设备工作状态对无人车运行的影响，更加符合无人车应用技术特点。和传统方法相比，利用本发明方法获得的最短时间路径更适用于无人车使用，且无需增加无人车硬件成本。
附图说明
此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1是根据本发明实施例的一种无人车全局路径规划方法的流程图；
图2是根据本发明实施例中卫星遮挡情况计算流程图；
图3是根据本发明实施例的一种无人车全局路径规划装置的示意图。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
图1是根据本发明实施例的一种无人车全局路径规划方法的流程图，本发明的实施例提供一种无人车全局路径规划方法，包括：
步骤S101，根据高精度地图、起始点、目的地信息进行静态全局路径规划，获得距离最短的方案；
步骤S102，在距离最短的轨迹方案中，增加实时路况信息，通过行驶速度获得动态行驶时间最短方案；
步骤S103，利用GPS/BDS卫星定位精度因子对行驶速度进行调整，形成适合于无人车的时间最短全局路径规划。
针对GPS/BDS卫星定位状态不确定性影响轨迹规划效果这一问题，根据上述的技术方案，本发明的实施例在GPS/BDS(Global Positioning System，GPS)/(BeiDouNavigation Satellite System，BDS)卫星导航定位对无人车运行的影响基础之上，将定位精度因子PDOP(Position Dilution of Precision，PDOP)参数引入到路径规划代价函数中。本发明在传统路径最短、时间最短路径规划方法基础之上，增加了GPS/BDS卫星导航设备工作状态对无人车运行的影响，更加符合无人车应用技术特点。和传统方法相比，利用本发明方法获得的最短时间路径更适用于无人车使用，且无需增加无人车硬件成本。
下面对上述实施例的方案做详细的描述。
1.基于A*算法的距离最近静态全局路径规划
基于高精度地图、起始点和目的地，进行传统全局路径规划，获得静态最短路径规划。传统路径规划算法种类很多，例如A*、Dijkstra等算法，下面内容将以A*算法为例进行描述。
1.1代价函数
A*算法是一种全局路径规划算法，利用全局地图栅格信息定义代价函数，并基于代价函数搜索最优路径节点。代价函数定义如式(1)所示：
f(n)＝g(n)+h(n) (1)
其中g(n)为起始点到达当前节点的实际代价值，h(n)为当前节点n到目标点的代价估计值，也称为启发函数。
常用曼哈顿距离或者欧拉距离定义代价值，即：
LM＝|nx-gx|+|ny-gy|
LE＝[(nx-gx)2+(ny-gy)2]1/2
1.2 A*算法静态全局路径搜索
基于高精度地图、起始点和目的地，利用A*算法进行全局静态最优路径规划，路径搜索步骤具体如下：
1)从起始点开始，并把它就加入到一个由路段组成的开放列表中。开放列表里的路段是路径可能会是沿途经过的，也有可能不经过，是一个待检查的路段列表；
2)查看与起始点相邻的路段(忽略障碍)，把其中可走的或可到达的路段，加入到开放列表中，并把起始点设置为这些路段的父节点；
3)把起始点从开放列表中移除，加入到封闭列表中，封闭列表中的每个路段都是现在不需要再关注的。
4)按照式(1)，计算开放列表中所有和起始点相邻路段对应的代价函数f(n)，并选择f(n)值最小的路段节点，然后将其从开放列表里取出，放到封闭列表中；
5)检查所有与上一步中代价最小节点相邻的路段，忽略其中在封闭列表中或是障碍，如果路段不在开放列表中，则把它们加入到开放列表中，并把上一步中代价最小节点设置为这些新加入的路段的父节点，记录其代价值；
6)如果某个相邻的路段已经在开放列表中，则检查经由当前节点的路径是否更优，如果代价更小，则把当前节点设为其父节点，重新计算g(n)和h(n)；
7)循环执行步骤5-6，直至目的地进入开放列表，此时，从目的地沿父节点到起始点，即为静态全局最优路径。
2.基于实时路况的时间最短路径规划
将1中获得的距离最短的静态全局路径规划方案离散化，并根据道路实时路况，获得每个路段对应的车辆行驶速度，对代价函数进行调整，更新路径规划方案排序，获得行使时间最短路径规划。
2.1行驶速度获取
将全局规划路径的路段，按照距离离散化为m个单元，根据道路拥堵状况、红绿灯、道路限速、自然天气等，获取离散化的每个单元路段对应的实时路况的车辆行驶速度vi。
2.2时间最短路径规划更新
利用速度参数vi，对式(1)进行调整，获取最短时间路径规划计算方法，具体见式(2)；
Figure BDA0002423382770000111
根据1中的路径搜索步骤，按照式(2)计算方法，重新更新最优路径，获得最短时间路径规划。
3.卫星导航定位精度因子计算
根据GPS/BDS卫星星历数据，计算每个单元路段对应时刻的卫星星座分布，然后，结合高精度地图中建筑物等因素，分析卫星信号遮挡情况，进一步计算精度因子PDOP参数。
3.1初始状态获取
获取对应于当前时刻ti、路段单元位置Posii等参数值，其中，路段单元位置Posii取起始点位置参数均值。
3.2卫星星座分布实时计算
根据卫星导航接收机获取导航电文，提取卫星轨道相关关键参数，获取ti时刻卫星星座分布，并计算卫星实时位置，具体计算过程如下：
(1)计算归一化时间，因为卫星的星历数据都是相对于参考时刻toe而言的，所以需要将观测时刻做归一化处理
tk＝ti-toe
归一化之后的时间tk控制在-302400到302400之间；
(2)计算卫星的平均角速度n：
将导航电文中的扰动修正项带入：
Figure BDA0002423382770000112
式中μ为地心引力常数，a为椭圆半长轴，由星历数据提供，再利用星历数据中提供的修正项Δn得到平均角速度：
n＝n0+Δn
(3)计算信号发射时刻的平近点角Mk
Mk＝M0+ntk
式中M0为参考时间的平近点角；
(4)计算卫星在信号发射时刻的偏近点角Ek
E＝Mk+essinEk
式中es为卫星椭圆轨道的偏心率，这是个超越方程，需要用迭代法求解，E为偏近点角；
(5)计算信号发射时刻的真近点角νk；
Figure BDA0002423382770000121
式中，e为卫星轨道离心率；
(6)计算卫星的地心向径r
r＝a(1-escosEk)
(7)计算信号发射时刻的升交点角距φk；利用卫星星历中提供的卫星轨道的近地点角距ω，代入如下公式：
φk＝vk+ω
(8)计算信号发射时刻摄动升交点角距修正项δμk、卫星地心向径修正项δrk和卫星轨道倾角修正项δik：
升交点角距修正项δμk：
δμk＝Cuccos2φk+Cussin2φk
卫星地心向径修正项δrk：
δrk＝Crccos2φk+Crssin2φk
卫星轨道倾角修正项δik：
δik＝Ciccos2φk+Cissin2φk
Cuc为纬度辐角的余弦调和修正项的幅度，Cus为纬度辐角的正弦调和修正项的幅度，Crc为卫星地心距的余弦调和修正值，Crs为卫星地心矩的正弦调和修正值，Cic为轨道倾角的余弦调和改正值，Cis为轨道倾角的正弦调和改正值，Cuc、Cus、Crc、Crs、Cic、Cis都来自于卫星星历数据；得到的这三个修正项用来校正φk，rk和ik:
μk＝φk+δμk
rk＝r+δrk
Figure BDA0002423382770000131
式中φk、rk和ik分别k时刻升交点角距、卫星地心向径和卫星轨道倾角，μk为修正后的升交点角距；i0为参考时刻的轨道倾角，
Figure BDA0002423382770000132
为轨道倾角变化率；
(9)计算卫星信号发射时刻在椭圆轨道平面直角坐标系中的位置(x'k，y'k)：
x′k＝rkcosμk
y'k＝rksinμk
(10)计算信号发射时刻的升交点赤经Ωk，计算公式如下：
Figure BDA0002423382770000133
式中Ω0、
Figure BDA0002423382770000134
分别为参考时刻升交点赤经和升交点赤经变化率，
Figure BDA0002423382770000135
等于7.292115146×10-5[rad/s]，是地球自转角速度；
xk＝x'kcosΩk-y'kcosiksinΩk
yk＝y'ksinΩk-y'kcosikcosΩk
zk＝y'ksinik
其中xk，yk，zk为卫星在WGS-84地心地固直角坐标系中的位置，Ωk为卫星信号发射时刻升交点赤经，ik为卫星轨道倾角；
3.3在载体坐标系中计算卫星仰角
通过姿态转移矩阵
Figure BDA0002423382770000136
和
Figure BDA0002423382770000137
平移向量对卫星进行坐标系变换，计算得到的地心地固坐标系变换到以道路单元中心为原点的地理坐标系，其中，
Figure BDA0002423382770000138
为地心地固坐标系到地理坐标系的姿态转移矩阵，
Figure BDA0002423382770000139
为地球中心到道路单元中心的平移向量，然后再根据道路单元姿态，利用地理坐标系至载体坐标系姿态矩阵
Figure BDA0002423382770000141
将其变换到载体坐标系，根据卫星在载体坐标系中的位置，计算对应于每个轨迹点的卫星实时仰角，具体计算表达式如下：
Figure BDA0002423382770000142
其中，
Figure BDA0002423382770000143
分别表示载体坐标系b中第i颗卫星仰角的三轴坐标；
Figure BDA0002423382770000144
和
Figure BDA0002423382770000145
具体表达式如下：
Figure BDA0002423382770000146
Figure BDA0002423382770000147
Figure BDA0002423382770000148
其中，λ、L分别为经度和纬度，θ、γ、φ分别为道路单元的俯仰角、横滚角和航向角从高精度地图中的道路信息中获得，
Figure BDA0002423382770000149
分别为地球中心到道路单元中心的平移向量在x、y、z方向的分量；
对应于每个轨迹点的卫星仰角计算公式如下：
Figure BDA00024233827700001410
其中，
Figure BDA00024233827700001411
表示载体坐标系b中第i颗卫星仰角；
3.4根据高精度地图选取可视卫星
根据高精度地图信息，计算道路单元相关环境信息中，建筑物轮廓对应的经度、纬度和高度信息，通过姿态矩阵和平移向量进行坐标转换，得到建筑物轮廓在以道路单元为原点的载体坐标系中的坐标；具体计算公式如下：
Figure BDA00024233827700001412
Figure BDA00024233827700001413
Figure BDA00024233827700001414
Figure BDA0002423382770000151
其中，n为椭球的卯酉圈曲率半径，e为椭球的第一偏心率，
Figure BDA0002423382770000152
λ、h分别为高楼对应的经度、纬度和高度，
Figure BDA0002423382770000153
分别为高楼对应的地心地固坐标系的坐标，
Figure BDA0002423382770000154
分别为高楼对应的载体坐标系的坐标；
在载体坐标系中，以道路单元切线方向为Y轴，并定义为前向，计算建筑物轮廓前边沿和后边沿在坐标系O-XY平面的投影坐标，然后据此计算前后边沿和无人车在O-XY平面的夹角，具体计算公式如下：
Figure BDA0002423382770000155
Figure BDA0002423382770000156
其中
Figure BDA0002423382770000157
分别为前后边沿在载体坐标系O-XY平面投影点坐标，
Figure BDA0002423382770000158
Figure BDA0002423382770000159
分别为前后边沿和道路单元切线的夹角；当
Figure BDA00024233827700001510
和
Figure BDA00024233827700001511
之差绝对值大于10°时，认为是对卫星影响不可忽略的建筑物，计算上边沿和道路边沿切线之间的夹角，具体公式如下：
Figure BDA00024233827700001512
当
Figure BDA00024233827700001513
大于5°时，计算所有可视卫星和载体坐标系X轴之间的夹角，同时满足在
Figure BDA00024233827700001514
区间内，并且仰角小于
Figure BDA00024233827700001515
两个条件，则被认为是卫星信号被遮挡。高楼对卫星遮挡情况具体计算流程和算法如图2所示，该方法首先将建筑物轮廓对应坐标转换到以道路单元中心为原点的载体坐标系中，然后计算前后轮廓在O-XY平面内投影与X轴夹角、上轮廓和O-XY平面之间的夹角，当载体坐标系中卫星仰角低于建筑物上轮廓，且处于前后轮廓之间时，认为该颗卫星被遮挡。
3.5卫星导航定位精度因子计算
根据3.4计算得到的可视卫星坐标信息，计算ti路段单元位置Posii对应的卫星定位精度因子Pdopi。
计算当前可视卫星在载体坐标系中的坐标：
Figure BDA00024233827700001516
其中B、L为观测站大地纬度和大地经度，Xi、Yi、Zi为卫星地心固定坐标系坐标；Xr、Yr、Zr为载体在地心固定坐标系坐标，
Figure BDA0002423382770000161
为可视卫星在载体坐标系中的坐标。
可视卫星的载体极坐标为：
Figure BDA0002423382770000162
Figure BDA0002423382770000163
Figure BDA0002423382770000164
设α、β、γ分别为载体到卫星的直线与X、Y、Z轴的夹角：
Figure BDA0002423382770000165
Figure BDA0002423382770000166
Figure BDA0002423382770000167
如果观测到n颗卫星，星座矩阵为：
Figure BDA0002423382770000168
令DOP＝(QTQ)-1，主对角线元素分别为a、b、c、d，因此求得空间位置精度因子：
Figure BDA0002423382770000169
4.基于卫星导航定位精度因子获得适合无人车的时间最短路径规划
基于上述计算得到的卫星导航定位精度因子，改进2中的车辆行驶速度，获得符合无人车技术特点的时间最短路径规划。针对无人车系统运行场景，该参数会影响式(2)的最短时间路径计算方法，其表现形式可以是作为速度vi的修正系数，也可以是作为时间参数修正量和式(2)右侧部分相加，此处以修正系数为例，描述计算过程，具体如下。
(1)以当前起始点位置和当前时刻基准为参考，计算卫星导航定位精度因子，获取实时路况信息；
(2)获取对应于当前时刻ti和路段单元的一组轨迹规划参数，包括路段单元位置Posii、行驶速度vi、代价函数；
(3)计算对应于ti时刻和Posii位置的卫星定位精度因子Pdopi参数值；
(4)将参数vi更新为v′i，计算公式为
v′i＝vi*ki
Figure BDA0002423382770000171
(5)计算对应于该路段单元行驶时间Δti，然后累加至ti，公式为：
Figure BDA0002423382770000172
ti+1＝ti+Δti
dLi为单元路段长度。
(6)按照步骤(2)-(5)，以ti+1为时间基准计算v′i+1，循环计算，直至到达目的地；
(7)以v′代替式(2)中的参数v，再次更新全局路径规划，获得适合无人车应用场景的最短时间路径。
图3为本发明实施例提供的一种无人车全局路径规划装置的结构示意图，该装置可以执行任意本发明任意实施例所提供的一种无人车全局路径规划方法，具备执行该方法相应的功能模块和有益效果。如图3所示，该装置包括：
静态规划模块91，用于根据高精度地图、起始点、目的地信息进行静态全局路径规划，获得距离最短的方案；
时间最短规划模块92，用于在距离最短的轨迹方案中，增加实时路况信息，通过行驶速度获得动态行驶时间最短方案；
无人车最优规划模块93，用于利用GPS/BDS卫星定位精度因子对行驶速度进行调整，形成适合于无人车的时间最短全局路径规划。
上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。
在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。
在本申请所提供的几个实施例中，应该理解到，所揭露的技术内容，可通过其它的方式实现。其中，以上所描述的设备实施例仅仅是示意性的，例如所述单元的划分，可以为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可为个人计算机、服务器或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种无人车全局路径规划方法，其特征在于，包括：
根据高精度地图、起始点、目的地信息进行静态全局路径规划，获得距离最短的方案；
在距离最短的轨迹方案中，增加实时路况信息，通过行驶速度获得动态行驶时间最短方案；
利用GPS/BDS卫星定位精度因子对行驶速度进行调整，形成适合于无人车的时间最短全局路径规划。
2.根据权利要求1所述的一种无人车全局路径规划方法，其特征在于，获得距离最短的方案，包括：
将静态最短路径规划离散化，并根据道路实时路况，获得每个路段对应的车辆行驶速度，对代价函数进行调整，更新路径规划方案排序，获得行使时间最短路径规划。
3.根据权利要求2所述的一种无人车全局路径规划方法，其特征在于，获得每个路段对应的车辆行驶速度，包括：
将全局规划路径的路段，按照距离离散化为m个单元，根据道路拥堵状况、红绿灯、道路限速、自然天气，获取离散化的每个单元路段对应的实时路况的车辆行驶速度vi；
获得行使时间最短路径规划，包括：
利用速度参数vi，对代价函数进行调整，获取最短时间路径规划，具体见式(2)；
Figure FDA0002423382760000011
根据传传统全局路径规划中的路径搜索，按照式(2)计算方法，重新更新最优路径，获得最短时间路径规划。
4.根据权利要求1所述的一种无人车全局路径规划方法，其特征在于，GPS/BDS卫星定位精度因子的计算，包括：
初始状态获取步骤，获取对应于当前时刻和路段单元位置参数值；
卫星星座分布实时计算步骤，根据卫星导航接收机获取导航电文，提取卫星轨道相关关键参数，获取当前时刻卫星星座分布，并计算卫星实时位置；
载体坐标系中卫星仰角计算步骤，通过姿态转移矩阵和平移向量对卫星进行坐标系变换，计算得到的地心地固坐标系变换到以道路单元中心为原点的地理坐标系，然后再根据道路单元姿态，利用地理坐标系至载体坐标系姿态矩阵，将其变换到载体坐标系，根据卫星在载体坐标系中的位置，计算对应于每个轨迹点的卫星实时仰角；
根据高精度地图的可视卫星选取步骤，根据高精度地图信息，计算道路单元相关环境信息中，建筑物轮廓对应的经度、纬度和高度信息，通过姿态矩阵和平移向量进行坐标转换，得到建筑物轮廓在以道路单元为原点的载体坐标系中的坐标；
卫星导航定位精度因子计算步骤，根据计算得到的可视卫星坐标信息，计算路段单元位置对应的卫星定位精度因子。
5.根据权利要求4所述的一种无人车全局路径规划方法，其特征在于，卫星星座分布实时计算步骤，包括：
(1)计算归一化时间，因为卫星的星历数据都是相对于参考时刻toe而言的，所以需要将观测时刻做归一化处理，
tk＝ti-toe
其中，ti为当前时刻，tk为归一化之后的时间；
(2)计算卫星的平均角速度n：
将导航电文中的扰动修正项带入：
Figure FDA0002423382760000021
式中μ为地心引力常数，a为椭圆半长轴，由星历数据提供，再利用星历数据中提供的修正项Δn得到平均角速度：
n＝n0+Δn
(3)计算信号发射时刻的平近点角Mk
Mk＝M0+ntk
式中M0为参考时间的平近点角；
(4)计算卫星在信号发射时刻的偏近点角Ek
E＝Mk+es sin Ek
式中es为卫星椭圆轨道的偏心率，这是个超越方程，需要用迭代法求解，E为偏近点角；
(5)计算信号发射时刻的真近点角νk；
Figure FDA0002423382760000022
式中，e为卫星轨道离心率；
(6)计算卫星的地心向径r
r＝a(1-es cos Ek)
(7)计算信号发射时刻的升交点角距φk；利用卫星星历中提供的卫星轨道的近地点角距ω，代入如下公式：
φk＝vk+ω
(8)计算信号发射时刻摄动升交点角距修正项δμk、卫星地心向径修正项δrk和卫星轨道倾角修正项δik：
升交点角距修正项δμk：
δμk＝Cuc cos2φk+Cus sin2φk
卫星地心向径修正项δrk：
δrk＝Crc cos2φk+Crs sin2φk
卫星轨道倾角修正项δik：
δik＝Cic cos2φk+Cis sin2φk
Cuc为纬度辐角的余弦调和修正项的幅度，Cus为纬度辐角的正弦调和修正项的幅度，Crc为卫星地心距的余弦调和修正值，Crs为卫星地心矩的正弦调和修正值，Cic为轨道倾角的余弦调和改正值，Cis为轨道倾角的正弦调和改正值，Cuc、Cus、Crc、Crs、Cic、Cis都来自于卫星星历数据；得到的这三个修正项用来校正φk，rk和ik:
μk＝φk+δμk
rk＝r+δrk
Figure FDA0002423382760000031
式中φk、rk和ik分别k时刻升交点角距、卫星地心向径和卫星轨道倾角，μk为修正后的升交点角距；i0为参考时刻的轨道倾角，
Figure FDA0002423382760000032
为轨道倾角变化率；
(9)计算卫星信号发射时刻在椭圆轨道平面直角坐标系中的位置(x'k，y'k)：
x’k＝rk cosμk
y'k＝rk sinμk
(10)计算信号发射时刻的升交点赤经Ωk，计算公式如下：
Figure FDA0002423382760000041
式中Ω0、
Figure FDA0002423382760000042
分别为参考时刻升交点赤经和升交点赤经变化率，
Figure FDA0002423382760000043
等于7.292115146×10-5[rad/s]，是地球自转角速度；
xk＝x'k cosΩk-y'k cosik sinΩk
yk＝y'k sinΩk-y'k cosikcosΩk
zk＝y'ksin ik
其中xk，yk，zk为卫星在WGS-84地心地固直角坐标系中的位置，Ωk为卫星信号发射时刻升交点赤经，ik为卫星轨道倾角。
6.根据权利要求4所述的一种无人车全局路径规划方法，其特征在于，载体坐标系中卫星仰角计算步骤，包括：
通过姿态转移矩阵
Figure FDA0002423382760000044
和
Figure FDA0002423382760000045
平移向量对卫星进行坐标系变换，计算得到的地心地固坐标系变换到以道路单元中心为原点的地理坐标系，其中，
Figure FDA0002423382760000046
为地心地固坐标系到地理坐标系的姿态转移矩阵，
Figure FDA0002423382760000047
为地球中心到道路单元中心的平移向量，然后再根据道路单元姿态，利用地理坐标系至载体坐标系姿态矩阵
Figure FDA0002423382760000048
将其变换到载体坐标系，根据卫星在载体坐标系中的位置，计算对应于每个轨迹点的卫星实时仰角，具体计算表达式如下：
Figure FDA0002423382760000049
其中，
Figure FDA00024233827600000410
分别表示载体坐标系b中第i颗卫星仰角的三轴坐标；
Figure FDA00024233827600000411
和
Figure FDA00024233827600000412
具体表达式如下：
Figure FDA00024233827600000413
Figure FDA00024233827600000414
Figure FDA0002423382760000051
其中，λ、L分别为经度和纬度，θ、γ、φ分别为道路单元的俯仰角、横滚角和航向角从高精度地图中的道路信息中获得，
Figure FDA0002423382760000052
分别为地球中心到道路单元中心的平移向量在x、y、z方向的分量；
对应于每个轨迹点的卫星仰角计算公式如下：
Figure FDA0002423382760000053
其中，
Figure FDA0002423382760000054
表示载体坐标系b中第i颗卫星仰角。
7.根据权利要求4所述的一种无人车全局路径规划方法，其特征在于，根据高精度地图的可视卫星选取步骤，包括：
根据高精度地图信息，计算道路单元相关环境信息中，建筑物轮廓对应的经度、纬度和高度信息，通过姿态矩阵和平移向量进行坐标转换，得到建筑物轮廓在以道路单元为原点的载体坐标系中的坐标；具体计算公式如下：
Figure FDA0002423382760000055
Figure FDA0002423382760000056
Figure FDA0002423382760000057
Figure FDA0002423382760000058
其中，n为椭球的卯酉圈曲率半径，e为椭球的第一偏心率，
Figure FDA0002423382760000059
λ、h分别为高楼对应的经度、纬度和高度，
Figure FDA00024233827600000510
分别为高楼对应的地心地固坐标系的坐标，
Figure FDA00024233827600000511
分别为高楼对应的载体坐标系的坐标；
在载体坐标系中，以道路单元切线方向为Y轴，并定义为前向，计算建筑物轮廓前边沿和后边沿在坐标系O-XY平面的投影坐标，然后据此计算前后边沿和无人车在O-XY平面的夹角，具体计算公式如下：
Figure FDA00024233827600000512
Figure FDA00024233827600000513
其中
Figure FDA00024233827600000514
分别为前后边沿在载体坐标系O-XY平面投影点坐标，
Figure FDA00024233827600000515
Figure FDA00024233827600000516
分别为前后边沿和道路单元切线的夹角；当
Figure FDA00024233827600000517
和
Figure FDA00024233827600000518
之差绝对值大于10°时，认为是对卫星影响不可忽略的建筑物，计算上边沿和道路边沿切线之间的夹角，具体公式如下：
Figure FDA0002423382760000061
当
Figure FDA0002423382760000062
大于5°时，计算所有可视卫星和载体坐标系X轴之间的夹角，同时满足在
Figure FDA0002423382760000063
区间内，并且仰角小于
Figure FDA0002423382760000064
两个条件，则被认为是卫星信号被遮挡。
8.根据权利要求4所述的一种无人车全局路径规划方法，其特征在于，卫星导航定位精度因子计算步骤，包括：
根据计算得到的可视卫星坐标信息，计算ti路段单元位置Posii对应的卫星定位精度因子Pdopi；
计算当前可视卫星在载体坐标系中的坐标：
Figure FDA0002423382760000065
其中B、L为观测站大地纬度和大地经度，Xi、Yi、Zi为卫星地心固定坐标系坐标；Xr、Yr、Zr为载体在地心固定坐标系坐标，
Figure FDA0002423382760000066
为可视卫星在载体坐标系中的坐标；
可视卫星的载体极坐标为：
Figure FDA0002423382760000067
Figure FDA0002423382760000068
Figure FDA0002423382760000069
设α、β、γ分别为载体到卫星的直线与X、Y、Z轴的夹角：
Figure FDA00024233827600000610
Figure FDA00024233827600000611
Figure FDA00024233827600000612
如果观测到n颗卫星，星座矩阵为：
Figure FDA0002423382760000071
令DOP＝(QTQ)-1，主对角线元素分别为a、b、c、d，因此求得空间位置精度因子：
Figure FDA0002423382760000072
9.根据权利要求1所述的一种无人车全局路径规划方法，其特征在于，利用GPS/BDS卫星定位精度因子对行驶速度进行调整，形成适合于无人车的时间最短全局路径规划，包括：
(1)以当前起始点位置和当前时刻基准为参考，计算卫星导航定位精度因子，获取实时路况信息；
(2)获取对应于当前时刻ti和路段单元的一组轨迹规划参数，包括路段单元位置Posii、行驶速度vi、代价函数；
(3)计算对应于ti时刻和Posii位置的卫星定位精度因子Pdopi参数值；
(4)将参数vi更新为v′i，计算公式为
v′i＝vi*ki
Figure FDA0002423382760000073
(5)计算对应于该路段单元行驶时间Δti，然后累加至ti，公式为：
Figure FDA0002423382760000074
ti+1＝ti+Δti
dLi为单元路段长度。
(6)按照步骤(2)-(5)，以ti+1为时间基准计算v′i+1，循环计算，直至到达目的地；
(7)以v′代替式(2)中的参数v，再次更新全局路径规划，获得适合无人车应用场景的最短时间路径。
10.一种无人车全局路径规划装置，其特征在于，包括：
静态规划模块，用于根据高精度地图、起始点、目的地信息进行静态全局路径规划，获得距离最短的方案；
时间最短规划模块，用于在距离最短的轨迹方案中，增加实时路况信息，通过行驶速度获得动态行驶时间最短方案；
无人车最优规划模块，用于利用GPS/BDS卫星定位精度因子对行驶速度进行调整，形成适合于无人车的时间最短全局路径规划。
CN202010212807.7A 2020-03-24 2020-03-24 一种无人车全局路径规划方法及装置 Pending CN111380557A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010212807.7A CN111380557A (zh) 2020-03-24 2020-03-24 一种无人车全局路径规划方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010212807.7A CN111380557A (zh) 2020-03-24 2020-03-24 一种无人车全局路径规划方法及装置

Publications (1)

Publication Number Publication Date
CN111380557A true CN111380557A (zh) 2020-07-07

Family

ID=71215520

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010212807.7A Pending CN111380557A (zh) 2020-03-24 2020-03-24 一种无人车全局路径规划方法及装置

Country Status (1)

Country Link
CN (1) CN111380557A (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113686349A (zh) * 2021-10-27 2021-11-23 深圳市羽翼数码科技有限公司 一种感知特定环境的适应性路径规划导航系统
CN113819917A (zh) * 2021-09-16 2021-12-21 广西综合交通大数据研究院 自动驾驶路径规划方法、装置、设备及存储介质
CN113935516A (zh) * 2021-09-07 2022-01-14 亚太卫星宽带通信（深圳）有限公司 一种机载天线快速寻星跟踪定位方法
CN114040328A (zh) * 2021-11-26 2022-02-11 成都耀塔科技有限公司 基于轨迹路程信息的机坪自绑定无动力设备定位推算方法
CN114659531A (zh) * 2022-05-16 2022-06-24 苏州挚途科技有限公司 车辆的地图定位方法、装置和电子设备
CN115909789A (zh) * 2022-11-02 2023-04-04 云控智行科技有限公司 结合路径规划和行驶状态规划的网联自动驾驶方法及系统

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1955639A (zh) * 2005-10-24 2007-05-02 何铁 车载速度测量仪的校准方法
CN201331348Y (zh) * 2009-01-06 2009-10-21 山东科沐华信息技术有限公司 惯导组合gps车载终端
CN101907714A (zh) * 2010-06-25 2010-12-08 陶洋 基于多传感器数据融合的gps辅助定位系统及其定位方法
CN202837553U (zh) * 2012-01-19 2013-03-27 康讯科技股份有限公司 距离及方向校正的位置估测装置
CN104457789A (zh) * 2014-11-26 2015-03-25 深圳市华颖泰科电子技术有限公司 基于惯性导航的参数修正方法和装置
CN105866812A (zh) * 2016-03-24 2016-08-17 广东机电职业技术学院 一种新型车辆组合定位算法
CN107084733A (zh) * 2017-04-10 2017-08-22 广东数相智能科技有限公司 一种基于无人驾驶的路径规划的方法、装置及系统
CN107272038A (zh) * 2017-07-25 2017-10-20 不灭科技（上海）有限公司 一种高精度定位的方法及设备
CN108827311A (zh) * 2018-08-02 2018-11-16 大连理工江苏研究院有限公司 一种制造车间无人搬运系统路径规划方法
CN109238270A (zh) * 2018-10-21 2019-01-18 浙江浙大中控信息技术有限公司 基于改进的a星算法的智能导航方法
CN109813306A (zh) * 2019-01-17 2019-05-28 李子月 一种无人车规划轨迹卫星定位数据可信度计算方法
CN109855617A (zh) * 2019-02-28 2019-06-07 深圳市元征科技股份有限公司 一种车辆定位方法、车辆定位装置及终端设备
US20190301876A1 (en) * 2018-03-30 2019-10-03 Telogis Inc. Dynamic reporting of location data for a vehicle based on a fitted history model

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1955639A (zh) * 2005-10-24 2007-05-02 何铁 车载速度测量仪的校准方法
CN201331348Y (zh) * 2009-01-06 2009-10-21 山东科沐华信息技术有限公司 惯导组合gps车载终端
CN101907714A (zh) * 2010-06-25 2010-12-08 陶洋 基于多传感器数据融合的gps辅助定位系统及其定位方法
CN202837553U (zh) * 2012-01-19 2013-03-27 康讯科技股份有限公司 距离及方向校正的位置估测装置
CN104457789A (zh) * 2014-11-26 2015-03-25 深圳市华颖泰科电子技术有限公司 基于惯性导航的参数修正方法和装置
CN105866812A (zh) * 2016-03-24 2016-08-17 广东机电职业技术学院 一种新型车辆组合定位算法
CN107084733A (zh) * 2017-04-10 2017-08-22 广东数相智能科技有限公司 一种基于无人驾驶的路径规划的方法、装置及系统
CN107272038A (zh) * 2017-07-25 2017-10-20 不灭科技（上海）有限公司 一种高精度定位的方法及设备
US20190301876A1 (en) * 2018-03-30 2019-10-03 Telogis Inc. Dynamic reporting of location data for a vehicle based on a fitted history model
CN108827311A (zh) * 2018-08-02 2018-11-16 大连理工江苏研究院有限公司 一种制造车间无人搬运系统路径规划方法
CN109238270A (zh) * 2018-10-21 2019-01-18 浙江浙大中控信息技术有限公司 基于改进的a星算法的智能导航方法
CN109813306A (zh) * 2019-01-17 2019-05-28 李子月 一种无人车规划轨迹卫星定位数据可信度计算方法
CN109855617A (zh) * 2019-02-28 2019-06-07 深圳市元征科技股份有限公司 一种车辆定位方法、车辆定位装置及终端设备

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113935516A (zh) * 2021-09-07 2022-01-14 亚太卫星宽带通信（深圳）有限公司 一种机载天线快速寻星跟踪定位方法
CN113819917A (zh) * 2021-09-16 2021-12-21 广西综合交通大数据研究院 自动驾驶路径规划方法、装置、设备及存储介质
CN113686349A (zh) * 2021-10-27 2021-11-23 深圳市羽翼数码科技有限公司 一种感知特定环境的适应性路径规划导航系统
CN114040328A (zh) * 2021-11-26 2022-02-11 成都耀塔科技有限公司 基于轨迹路程信息的机坪自绑定无动力设备定位推算方法
CN114040328B (zh) * 2021-11-26 2024-05-03 成都耀塔科技有限公司 基于轨迹路程信息的机坪自绑定无动力设备定位推算方法
CN114659531A (zh) * 2022-05-16 2022-06-24 苏州挚途科技有限公司 车辆的地图定位方法、装置和电子设备
CN115909789A (zh) * 2022-11-02 2023-04-04 云控智行科技有限公司 结合路径规划和行驶状态规划的网联自动驾驶方法及系统

Similar Documents

Publication Publication Date Title
CN111380557A (zh) 一种无人车全局路径规划方法及装置
CN109934920B (zh) 基于低成本设备的高精度三维点云地图构建方法
CN110631593B (zh) 一种用于自动驾驶场景的多传感器融合定位方法
Ochieng et al. Map-matching in complex urban road networks
US9222786B2 (en) Methods and systems for creating digital transportation networks
CN106871932B (zh) 基于金字塔搜索地形匹配的星载激光在轨指向检校方法
CN107328410A (zh) 用于定位自动驾驶车辆的方法和汽车电脑
US20060142943A1 (en) Navigation service method and terminal of enabling the method
US9411822B2 (en) System and method of generating and using open sky data
CN109813306A (zh) 一种无人车规划轨迹卫星定位数据可信度计算方法
CN113126618A (zh) 一种越野环境下的无人驾驶全局路径规划及重规划方法
CN107015246A (zh) 一种基于场景共享的导航协助方法及终端
CN110031012A (zh) 汽车实时匹配高精地图的方法、系统、设备及存储介质
CN110018503B (zh) 车辆的定位方法及定位系统
CN115326084A (zh) 车辆定位方法、装置、计算机设备、存储介质
CN115655257A (zh) 一种高精地图的更新方法、装置、设备及存储介质
CN108710145A (zh) 一种无人机定位系统及方法
Mandel et al. Particle filter-based position estimation in road networks using digital elevation models
CN111123306A (zh) Gps信号强度检测方法、系统及道路规划方法
CN115542277A (zh) 一种雷达法线标定方法、装置、系统、设备和存储介质
CN101943584A (zh) 基于ccd星敏感器的对准方法
CN113484843A (zh) 一种激光雷达与组合导航间外参数的确定方法及装置
CN115202383B (zh) 一种无人机多维航迹表达及生成方法
CN111928863A (zh) 一种高精地图数据采集方法、装置及系统
CN114279436B (zh) 自动驾驶用高精度地图的制作方法和装置

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20200707