CN110187372B - 一种低速无人车园区内组合导航方法及系统 - Google Patents

一种低速无人车园区内组合导航方法及系统 Download PDF

Info

Publication number
CN110187372B
CN110187372B CN201910538148.3A CN201910538148A CN110187372B CN 110187372 B CN110187372 B CN 110187372B CN 201910538148 A CN201910538148 A CN 201910538148A CN 110187372 B CN110187372 B CN 110187372B
Authority
CN
China
Prior art keywords
map
path
module
navigation
sensor
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.)
Active
Application number
CN201910538148.3A
Other languages
English (en)
Other versions
CN110187372A (zh
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.)
Beijing Union University
Original Assignee
Beijing Union University
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 Beijing Union University filed Critical Beijing Union University
Priority to CN201910538148.3A priority Critical patent/CN110187372B/zh
Publication of CN110187372A publication Critical patent/CN110187372A/zh
Application granted granted Critical
Publication of CN110187372B publication Critical patent/CN110187372B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • 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
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/46—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being of a radio-wave signal type

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明提供一种低速无人车园区内组合导航方法及系统，其中方法包括使用雷达SLAM模块生成路径信息1，还包括以下步骤：进行联合地图采集，生成路径地图；使用RTK导航模块生成路径信息2；融合模块接受所述路径信息1和路径信息2，加权融合后生成目标轨迹。本发明将移动站定位天线改为前天线，同时将雷达坐标原点平移到导航前天线位置，保证了采集地图轨迹的一致性；其次联合采集地图，地图中同一个路点既包括雷达SLAM定位坐标也包括RTK‑导航经纬度坐标；然后雷达SLAM模块与RTK‑导航模块加载同一个地图进行一次路径规划，所得路径点均以车辆坐标系为原点；最后通过融合模块处理两个定位模块的局部路径，最终生成无人车的目标路径。

Description

一种低速无人车园区内组合导航方法及系统
技术领域
本发明涉及智能导航的技术领域，特别是一种低速无人车园区内组合导航方法及系统。
背景技术
无人驾驶技术已然成为了当下最热门的研究方向之一，精确定位是车辆实现自动驾驶最基本的环节。在国内，对于商用车而言自动驾驶受到法律的约束，同时存在很多技术壁垒，短时间内基本无法形成真正意义上的自动驾驶商品车。然而对于低速车，自动驾驶可以寻求很多落地的突破口。本发明依据低速车运行环境的特点(半开放、路程短、楼宇树木遮挡严重等)发明了一种基于雷达SLAM与RTK-导航的组合导航方法。对于园区内自动驾驶场地车而言，单独依赖RTK-导航会受环境制约，很难实现持续的自动驾驶。
申请号为CN201810266168.5的发明专利公开了一种基于2D激光雷达和北斗定位的郁闭果园导航系统，包括2D激光雷达,北斗定位移动站、北斗定位基站、2个电源模块、PC机和控制器。2D激光雷达安装在履带拖拉机前端,北斗定位移动站安装在履带拖拉机顶端,北斗定位基站安装在郁闭果园内；一个电源模块为2D激光雷达、北斗定位移动站、PC机和控制器提供电能,另一个电源模块为北斗定位基站提供电能；2D激光雷达、北斗定位移动站和控制器均与PC机连接。该方法采用非A即B的融合方式，当RTK-导航失效采用道路边缘点导航，并没有对二者进行融合，没有对局部路径进行深度融合。
申请号为CN201710801741.3的发明专利公开了一种基于RTK北斗和激光雷达的巡检机器人导航系统及方法，包括机器人移动站和后台管理服务器,所述机器人移动站包括机器人机体以及设置于机器人机体上的控制模块、定位导航模块、无线通信模块和电源管理模块,所述定位导航模块包括激光雷达和RTK/SINS单元；导航地图采用全局地图与局部地图相结合的设计方案,全局地图构建采用机器人记录轨迹方式绘制,导航方式采用预瞄PID算法,局部地图构建采用激光雷达记录障碍物离散数据点,经聚类、曲线拟合等步骤还原障碍物边缘信息,采用人工势场路径规划避障方式。该方法利用RTK-导航进行定位，适用范围比较窄。
发明内容
为了解决上述的技术问题，本发明提出一种低速无人车园区内组合导航方法及系统，将移动站定位天线改为前天线，同时将雷达坐标原点平移到导航前天线位置，保证了采集地图轨迹的一致性；其次联合采集地图，地图中同一个路点既包括雷达SLAM定位坐标也包括RTK-导航经纬度坐标；然后雷达SLAM模块与RTK-导航模块加载同一个地图进行一次路径规划，所得路径点均以车辆坐标系为原点；最后通过融合模块处理两个定位模块的局部路径，最终生成无人车的目标路径。
本发明的第一目的是提供了一种低速无人车园区内组合导航方法，包括使用雷达SLAM模块生成路径信息1，还包括以下步骤：
步骤1：进行联合地图采集，生成路径地图；
步骤2：使用RTK导航模块生成路径信息2；
步骤3：融合模块接受所述路径信息1和路径信息2，加权融合后生成目标轨迹。
优选的是，所述步骤1包括以下子步骤：
步骤11：定义地图信息中的单个路点信息；
步骤12：采集地图时，实时将所述单个路点信息存储在txt文本中；
步骤13：采集完毕后，得到地图路径信息并生成所述路径地图。
在上述任一方案中优选的是，所述单个路点信息由采集时间、精度、维度、方向角、差分状态、纵向路点属性、横向路点属性、雷达SLAM定位点X、雷达SLAM定位点Y和SLAM定位置信度中至少一种组成。
在上述任一方案中优选的是，所述步骤2包括以下子步骤：
步骤21：加载所述路径地图；
步骤22：实时接收当前经纬度、航向角信息；通过UTM投影将车身经纬度点转换为笛卡尔坐标；
步骤23：将所述路径地图中的路径点通过UTM投影转换成笛卡尔坐标；
步骤24：在笛卡尔坐标系下计算出车身点前方K米的所述路径点，其中K为自然数；
步骤25：将得到的所述路径点转换成以车辆坐标系原点为原点的一次目标轨迹点S0。
在上述任一方案中优选的是，所述RTK导航模块还用于将当前RTK-导航定位状态实时发布给融合模块。
在上述任一方案中优选的是，所述使用雷达SLAM模块生成路径信息1包括以下子步骤：
步骤2A：加载所述路径地图；
步骤2B：通过雷达SLAM定位实时计算车辆的位置信息和/或航向信息；
步骤2C：将当前车身点与地图中所述路径点进行比较，结合航向角搜索出车辆前方K米的点，其中K为自然数；
步骤2D：将得到的路径点转换成以车辆坐标系原点为远点的一次目标轨迹点S1。
在上述任一方案中优选的是，所述雷达SLAM模块还用于将雷达SLAM定位结果的置信度实时发布给融合模块。
在上述任一方案中优选的是，所述步骤3包括以下子步骤：
步骤31：递推计算出采样时刻k的Rii(k)和Rij(k)，其中，Rii(k)表示Xi的自协方差函数，Rij(k)表示Xi、Xj的互协方差函数，Xi表示传感器i的测量值，Xj表示传感器j的测量值；
步骤32：计算采样时刻k的
Figure BDA0002101733310000031
其中，
Figure BDA0002101733310000032
表示Rij(k)的均值函数；
步骤33；计算采样时刻k的σi 2，其中，σi 2表示各传感器的方差；
步骤34：计算各传感器多次测量值的均值
Figure BDA0002101733310000033
步骤35：计算此时各传感器的最优加权因子Wi *；
步骤36：计算此时的估计值。
在上述任一方案中优选的是，所述采样时刻k的Rii(k)的计算公式为：
Figure BDA0002101733310000041
其中，m为采样时刻，i为独立的传感器。
在上述任一方案中优选的是，所述采样时刻k的Rij(k)的计算公式为：
Figure BDA0002101733310000042
其中，j为独立的传感器。
在上述任一方案中优选的是，所述采样时刻k的
Figure BDA0002101733310000043
的计算公式为：
Figure BDA0002101733310000044
其中，n为采样时刻。
在上述任一方案中优选的是，所述采样时刻k的σi 2的计算公式为：
σi 2＝E[ei 2]＝Rii-Rij
其中，E为期望因子，ei为零均值平稳噪声。
在上述任一方案中优选的是，由于Xi、Xj互不相关，且均值为零，与X也不相关，故Xi、Xj的互协方差函数Rij满足：Rij＝E[XiXj]＝E[X2]。
在上述任一方案中优选的是，Xi的自协方差函数Rii满足：Rii＝E[Xi 2]＝E[X2]+E[ei 2]。
在上述任一方案中优选的是，所述各传感器多次测量值的均值
Figure BDA0002101733310000045
的计算公式为：
Figure BDA0002101733310000046
其中，m为历史数据的个数。
在上述任一方案中优选的是，所述最优加权因子Wi *的计算公式为：
Figure BDA0002101733310000051
此时，对应的总方差最小值为
Figure BDA0002101733310000052
在上述任一方案中优选的是，所述估算值的计算方法为：
Figure BDA0002101733310000053
其中，
Figure BDA0002101733310000054
为融合后的真值，Wi为各传感器的加权因子。
发明的第二目的是提供了一种低速无人车园区内组合导航系统，包括用于生成路径信息1的雷达SLAM模块，还包括以下模块：
地图采集模块：用于进行联合地图采集，生成路径地图；
RTK导航模块：用于生成路径信息2；
融合模块：用于接受所述路径信息1和路径信息2，加权融合后生成目标轨迹；
所述系统使用如权利要求1所述的方法进行组合导航。
优选的是，所述雷达SLAM模块还用于规划出一次目标轨迹点s0，所述一次轨迹为在车辆坐标系下的目标路点。
在上述任一方案中优选的是，所述RTK导航模块还用于规划出一次目标轨迹点S1。
在上述任一方案中优选的是，所述RTK导航模块为双天线导航，前天线连接到导航接收机的后天线位置，后天线连接到导航接收机的前天线位置。
在上述任一方案中优选的是，Ol-XlYlZl为所述雷达SLAM的坐标系，Xl轴正方向为雷达数据输出线缆的相反方向，Yl轴正方向为Xl轴正方向逆时针旋转90°，Zl轴正方向指向天空。
27.在上述任一方案中优选的是，所述雷达SLAM模块的安装位置距地面高度为hl，距车辆坐标系原点的前向位移为xl，水平方向无偏为yl，则雷达相对于车辆坐标系的偏移向量表示为Tl＝[xl 0 hl]。因此雷达坐标系与车辆坐标的转换关系为：
Figure BDA0002101733310000061
其中，Ov-XvYtZv为前雷达的坐标系。
在上述任一方案中优选的是，所述RTK导航模块具有如下功能：
1)加载联合采集的所述路径地图；
2)通过UTM投影将车身经纬度点转换为笛卡尔坐标；
3)将地图中的路径点通过UTM投影转换成笛卡尔坐标；
4)在笛卡尔坐标系下计算出车身点前方K米的所述路径点，其中K为自然数；
5)将得到的所述路径点转换成以车辆坐标系原点为原点的一次目标轨迹点S0。
在上述任一方案中优选的是，所述雷达SLAM模块具有如下功能：
1)加载联合采集的所述路径地图；
2)通过雷达SLAM定位实时计算车辆的位置信息和/或航向信息；
3)将当前车身点与地图中所述路径点进行比较，结合航向角搜索出车辆前方K米的点，其中K为自然数；
4)将得到的路径点转换成以车辆坐标系原点为远点的一次目标轨迹点S1。
在上述任一方案中优选的是，所述融合模块用于接收所述RTK导航模块实时发布的当前RTK-导航定位状态和/或所述雷达SLAM模块实时发布的雷达SLAM定位结果的置信度。
本发明提出了一种低速无人车园区内组合导航方法，增加了园区内低速车自动驾驶的可靠性与系统的鲁棒性。
附图说明
图1为按照本发明的低速无人车园区内组合导航方法的一优选实施例的流程图。
图2为按照本发明的低速无人车园区内组合导航系统的一优选实施例的模块图。
图3为按照本发明的低速无人车园区内组合导航系统的另一实施例的雷达坐标系与导航坐标系标定图。
图4为按照本发明的低速无人车园区内组合导航方法的如图3所示实施例的联合地图采集方法流程图。
图5为按照本发明的低速无人车园区内组合导航方法的如图3所示实施例的组合导航方法流程图。
图6为按照本发明的低速无人车园区内组合导航系统的如图3所示实施例的模块间关系图。
图7为按照本发明的低速无人车园区内组合导航系统的如图3所示实施例的多传感器自适应加权融合模型图。
具体实施方式
下面结合附图和具体的实施例对本发明做进一步的阐述。
实施例一
如图1所示，执行步骤100，进行联合地图采集，生成路径地图。在步骤100中执行步骤101，定义地图信息中的单个路点信息，单个路点信息由采集时间、精度、维度、方向角、差分状态、纵向路点属性、横向路点属性、雷达SLAM定位点X、雷达SLAM定位点Y和SLAM定位置信度中至少一种组成。执行步骤102，采集地图时，实时将所述单个路点信息存储在txt文本中。执行步骤103，采集完毕后，得到地图路径信息并生成所述路径地图。
执行步骤110，使用RTK导航模块生成路径信息2，RTK导航模块还用于将当前RTK-导航定位状态实时发布给融合模块。在步骤100中执行步骤111，加载所述路径地图。执行步骤112，实时接收当前经纬度、航向角信息，通过UTM投影将车身经纬度点转换为笛卡尔坐标。执行步骤113，将所述路径地图中的路径点通过UTM投影转换成笛卡尔坐标。执行步骤114，在笛卡尔坐标系下计算出车身点前方K米的所述路径点，其中K为自然数。执行步骤115，将得到的所述路径点转换成以车辆坐标系原点为原点的一次目标轨迹点S0。
执行步骤120，使用雷达SLAM模块生成路径信息1，雷达SLAM模块还用于将雷达SLAM定位结果的置信度实时发布给融合模块。在步骤120中执行步骤121，加载所述路径地图。执行步骤122，通过雷达SLAM定位实时计算车辆的位置信息和/或航向信息。执行步骤123，将当前车身点与地图中所述路径点进行比较，结合航向角搜索出车辆前方K米的点，其中K为自然数。执行步骤124，将得到的路径点转换成以车辆坐标系原点为远点的一次目标轨迹点S1。执行步骤130，融合模块接受所述路径信息1和路径信息2，加权融合后生成目标轨迹。在步骤100中执行步骤31，递推计算出采样时刻k的Rii(k)和Rij(k)，计算公式为：
Figure BDA0002101733310000081
Figure BDA0002101733310000082
其中，Rii(k)表示Xi的自协方差函数，Rij(k)表示Xi、Xj的互协方差函数，Xi表示传感器i的测量值，Xj表示传感器j的测量值，m为采样时刻，i为独立的传感器，j为独立的传感器。执行步骤32，计算采样时刻k的
Figure BDA0002101733310000083
计算公式为：
Figure BDA0002101733310000084
其中，其中，
Figure BDA0002101733310000085
表示Rij(k)的均值函数，n为采样时刻。执行步骤33，计算采样时刻k的σi 2，计算公式为：
σi 2＝E[ei 2]＝Rii-Rij
其中，σi 2表示各传感器的方差，E为期望因子，ei为零均值平稳噪声。由于Xi、Xj互不相关，且均值为零，与X也不相关，故Xi、Xj的互协方差函数Rij满足：Rij＝E[XiXj]＝E[X2]，Xi的自协方差函数Rii满足：Rii＝E[Xi 2]＝E[X2]+E[ei 2]。执行步骤34，计算各传感器多次测量值的均值
Figure BDA0002101733310000091
计算公式为：
Figure BDA0002101733310000092
其中，m为历史数据的个数。执行步骤35，计算此时各传感器的最优加权因子
Figure BDA0002101733310000093
计算公式为：
Figure BDA0002101733310000094
此时，对应的总方差最小值为
Figure BDA0002101733310000095
执行步骤36，计算此时的估计值，计算公式为：
Figure BDA0002101733310000096
其中，
Figure BDA0002101733310000097
为融合后的真值，Wi为各传感器的加权因子。
实施例二
如图2所示，一种低速无人车园区内组合导航系统包括地图采集模块200、RTK导航模块210、雷达SLAM模块220和融合模块230。
地图采集模块200：用于进行联合地图采集，生成路径地图。
RTK导航模块210：用于生成路径信息2，规划出一次目标轨迹点S1。RTK导航模块210具有如下功能：1)加载联合采集的所述路径地图；2)通过UTM投影将车身经纬度点转换为笛卡尔坐标；3)将地图中的路径点通过UTM投影转换成笛卡尔坐标；4)在笛卡尔坐标系下计算出车身点前方K米的所述路径点，其中K为自然数；5)将得到的所述路径点转换成以车辆坐标系原点为原点的一系列点S0。RTK导航模块210为双天线导航，前天线连接到导航接收机的后天线位置，后天线连接到导航接收机的前天线位置。Ol-XlYlZl为所述雷达SLAM的坐标系，Xl轴正方向为雷达数据输出线缆的相反方向，Yl轴正方向为Xl轴正方向逆时针旋转90°，Zl轴正方向指向天空。雷达SLAM的安装位置距地面高度为hl，距车辆坐标系原点的前向位移为xl，水平方向无偏为yl，则雷达相对于车辆坐标系的偏移向量表示为Tl＝[xl 0 hl]。因此雷达坐标系与车辆坐标的转换关系为：
Figure BDA0002101733310000101
其中，Ov-XvYvZv为前雷达的坐标系。
雷达SLAM模块220：用于生成路径信息1，规划出一次目标轨迹点S0，一次轨迹为在车辆坐标系下的目标路点。雷达SLAM模块220具有如下功能：1)加载联合采集的所述路径地图；2)通过雷达SLAM定位实时计算车辆的位置信息和/或航向信息；3)将当前车身点与地图中所述路径点进行比较，结合航向角搜索出车辆前方K米的点，其中K为自然数；4)将得到的路径点转换成以车辆坐标系原点为远点的一系列点s1.
融合模块230：用于接受所述路径信息1和路径信息2，加权融合后生成目标轨迹。融合模块230用于接收所述RTK导航模块实时发布的当前RTK-导航定位状态和/或所述雷达SLAM模块实时发布的雷达SLAM定位结果的置信度。
实施例三
本发明基于雷达SLAM与RTK-导航的规划轨迹实现无人车的组合导航，即雷达SLAM模块规划出一次目标轨迹点S0，RTK-导航模块规划出一次目标轨迹点S1，本发明通过加权融合S0与S1得到无人车最终的目标轨迹，地图采集方法如图4所示，组合导航方法如图5所示。
本发明所指的一次轨迹为：在车辆坐标系下的目标路点。为了保证雷达SLAM轨迹与RTK-导航轨迹的一致性，需要对二者的安装位置进行标定。本发明采用平移矩阵将雷达坐标原点转换到导航坐标原点。本发明采用的RTK-导航装置为双天线导航，一般的双天线导航后天线为定位天线，前天线为定向天线，本发明为了将雷达坐标原点与导航定位点统一故将前天线连接到导航接收机的后天线位置，后天线连接到导航接收机的前天线位置。前天线位置即为车辆坐标系原点。
二者的位置关系如图3所示。其中，Ol-XlYlZl为雷达坐标系，Xl轴正方向为雷达数据输出线缆的相反方向，Yl轴正方向为Xl轴正方向逆时针旋转90°，Zl轴正方向指向天空；假设雷达的安装位置距地面高度为hl，距车辆坐标系原点的前向位移为xl，水平方向无偏为yl，则雷达相对于车辆坐标系的偏移向量表示为Tl＝[xl 0 hl]。因此雷达坐标系与车辆坐标的转换关系为：
Figure BDA0002101733310000111
联合地图采集
首先定义地图信息。如表1所示，自动驾驶地图中单个路点信息由：采集时间、经度、纬度、方位角(采集地图时采集车的航向角)、差分状态(地图采集点定位评判依据)、路点属性(路段标记：主要标记路段纵向变化，如弯道、直道、U_turn等)与路点属性(路段标记：主要标记路段横向变化，如单行线、多车道等)，雷达SLAM定位点X(全局坐标)，雷达SLAM定位点Y(全局坐标)，SLAM定位置信度(评价当前SLAM定位可靠性)组成。
Figure BDA0002101733310000112
表1 单个路点信息
采集地图时，实时将单个路点信息存储到txt文本中，采集完毕后得到地图路径信息。
子模块规划得到两条基于车辆坐标系轨迹
本发明子模块关系图如图6所示：
RTK-导航模块主要功能如下：加载联合采集的路径地图；实时接收当前经纬度、航向角信息；通过UTM投影将车身经纬度点转换为笛卡尔坐标；同时将地图中的路径点即表1中(Longitude，Latitude)通过UTM投影转换成笛卡尔坐标；在笛卡尔坐标系下计算出车身点前方30米的路径点；将得到的路径点转换成以车辆坐标系原点为原点的一系列点S0；另外，该模块还需实时发布当前RTK-导航定位状态以供融合模块使用。
雷达SLAM模块主要功能如下：加载联合采集的路径地图；通过雷达SLAM定位实时计算车辆的位置信息、航向信息；将当前车身点与地图中路径点即表1中(Lidar X，LidarY)进行比较，结合航向角搜索出车辆前方30米的点；将得到的路径点转换成以车辆坐标系原点为远点的一系列点S1；另外，该模块还需实时发布雷达SLAM定位结果的置信度以供融合模块使用。
两条轨迹加权融合并生成最终目标轨迹
一般的，RTK-导航定位评价如表2，本发明在评价RTK-导航定位状态同样采用表2的评价方式。
RTK状态 0 1 2 4 5
定位精度 无定位 单点定位 伪距定位 差分定位 浮点定位
备注 无定位 定位不可用 定位不可用 定位精确 定位可参考
表2 RTK-导航定位指标评价表
本发明雷达SLAM模块定位评价如表3，依据点云匹配率定义定位置信度。
Figure BDA0002101733310000121
Figure BDA0002101733310000131
表3 雷达SLAM定位指标评价表
融合模块如图6所示，融合模块需同时接受RTK-导航模块与雷达SLAM模块规划的局部路径信息，依据各自的定位状态进行两个路径信息加权融合。依据二者实时定位状态本发明采用表4的融合决策方案，表中4中A表示选用RTK-导航规划的轨迹为目标轨迹；B表示选用雷达SLAM规划的轨迹为目标轨迹；C表示无法生成目标轨迹-对于同时雷达SLAM定位与RTK-导航定位不可靠的情况，本发明为了无人车安全起见不再考虑轨迹生成；AB表示二者加权融合后的得到的目标轨迹。
Figure BDA0002101733310000132
表4 轨迹融合方案表
对于表4中AB的情况，本发明采用基于自适应加权的决策融合模型，如图7所示。设有n个传感器对某一对象参数进行测量，其测量值分别为x1、x2、…、xn，然后进行加权融合。该算法的总体思想是在保证总方差最小的条件下，根据各传感器的测量值，通过自适应的方式寻找与之对应的最优加权因子，使融合后的
Figure BDA0002101733310000133
值最优。
具体实施过程如下：
步骤一：用式(1)和式(2)递推地算出采样时刻k的Rii(k)和Rij(k)；
Figure BDA0002101733310000141
Figure BDA0002101733310000142
步骤二：根据式(3)求出采样时刻k的
Figure BDA0002101733310000143
Figure BDA0002101733310000144
步骤三：根据式(4)求得采样时刻k的σi 2；
σi 2＝E[ei 2]＝Rii-Rij (4)
因为Xi、Xj互不相关，且均值为零，与X也不相关，故Xi、Xj的互协方差函数Rij满足下式：
Rij＝E[XiXj]＝E[X2] (5)
Xi的自协方差函数Rii满足：
Rii＝E[Xi 2]＝E[X2]+E[ei 2] (6)
步骤四：根据式(7)求取各传感器多次测量值的均值；
Figure BDA0002101733310000145
其中m为历史数据的个数。
步骤五：根据式(8)求出此时各传感器的最优加权因子Wi *；
Figure BDA0002101733310000146
此时，对应的总方差最小值为：
Figure BDA0002101733310000151
步骤六：根据式(10)求得此时的估计值。
Figure BDA0002101733310000152
通过表4的对RTK-导航模块与雷达SLAM模块规划的轨迹进行融合最终得到无人车的目标轨迹。
实施例四
设两相互独立的传感器i、j，其测量值分别为Xi、Xj，对应的测量误差(即零均值平稳噪声)为ei、ej，待估计真值为X，则有：Xi＝X+ei和Xj＝X+ej，。传感器i的方差为：
Figure BDA0002101733310000153
为了更好地理解本发明，以上结合本发明的具体实施例做了详细描述，但并非是对本发明的限制。凡是依据本发明的技术实质对以上实施例所做的任何简单修改，均仍属于本发明技术方案的范围。本说明书中每个实施例重点说明的都是与其它实施例的不同之处，各个实施例之间相同或相似的部分相互参见即可。对于系统实施例而言，由于其与方法实施例基本对应，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。

Claims (9)

1.一种低速无人车园区内组合导航方法，包括使用雷达SLAM模块生成路径信息1，其特征在于，还包括以下步骤：
步骤1：进行联合地图采集，生成路径地图；
步骤2：使用RTK导航模块生成路径信息2；
步骤3：融合模块接受所述路径信息1和路径信息2，加权融合后生成目标轨迹；
所述步骤3包括以下子步骤：
步骤31：递推计算出采样时刻k的Rii(k)和Rij(k)，其中，Rii(k)表示Xi的自协方差函数，Rij(k)表示Xi、Xj的互协方差函数，Xi表示传感器i的测量值，Xj表示传感器j的测量值；
步骤32：计算采样时刻k的
Figure FDA0003126272190000011
其中，
Figure FDA0003126272190000012
表示Rij(k)的均值函数；
步骤33；计算采样时刻k的σi 2，其中，σi 2表示各传感器的方差；
步骤34：计算各传感器多次测量值的均值
Figure FDA0003126272190000013
步骤35：计算此时各传感器的最优加权因了
Figure FDA0003126272190000014
最优加权因子
Figure FDA0003126272190000015
的计算公式为：
Figure FDA0003126272190000016
此时，对应的总方差最小值为
Figure FDA0003126272190000017
其中，n为采样时刻；
步骤36：计算此时的估计值，计算方法为：
Figure FDA0003126272190000018
其中，
Figure FDA0003126272190000019
为融合后的真值，Wi为各传感器的加权因子。
2.如权利要求1所述的低速无人车园区内组合导航方法，其特征在于，所述步骤1包括以下子步骤：
步骤11：定义地图信息中的单个路点信息；
步骤12：采集地图时，实时将所述单个路点信息存储在txt文本中；
步骤13：采集完毕后，得到地图路径信息并生成所述路径地图。
3.如权利要求2所述的低速无人车园区内组合导航方法，其特征在于，所述单个路点信息由采集时间、精度、维度、方向角、差分状态、纵向路点属性、横向路点属性、雷达SLAM定位点X、雷达SLAM定位点Y和SLAM定位置信度中至少一种组成。
4.如权利要求2所述的低速无人车园区内组合导航方法，其特征在于，所述步骤2包括以下子步骤：
步骤21：加载所述路径地图；
步骤22：实时接收当前经纬度、航向角信息；通过UTM投影将车身经纬度点转换为笛卡尔坐标；
步骤23：将所述路径地图中的路径点通过UTM投影转换成笛卡尔坐标；
步骤24：在笛卡尔坐标系下计算出车身点前方K米的所述路径点，其中K为自然数；
步骤25：将得到的所述路径点转换成以车辆坐标系原点为原点的一次目标轨迹点S0。
5.如权利要求4所述的低速无人车园区内组合导航方法，其特征在于，所述RTK导航模块还用于将当前RTK-导航定位状态实时发布给融合模块。
6.如权利要求4所述的低速无人车园区内组合导航方法，其特征在于，所述使用雷达SLAM模块生成路径信息1包括以下子步骤：
步骤2A：加载所述路径地图；
步骤2B：通过雷达SLAM定位实时计算车辆的位置信息和/或航向信息；
步骤2C：将当前车身点与地图中所述路径点进行比较，结合航向角搜索出车辆前方K米的点，其中K为自然数；
步骤2D：将得到的路径点转换成以车辆坐标系原点为远点的一次目标轨迹点S1。
7.如权利要求6所述的低速无人车园区内组合导航方法，其特征在于，所述雷达SLAM模块还用于将雷达SLAM定位结果的置信度实时发布给融合模块。
8.如权利要求7所述的低速无人车园区内组合导航方法，其特征在于，所述采样时刻k的Rii(k)的计算公式为：
Figure FDA0003126272190000031
其中，m为采样时刻，i为独立的传感器。
9.一种低速无人车园区内组合导航系统，包括用于生成路径信息1的雷达SLAM模块，其特征在于，还包括以下模块：
地图采集模块：用于进行联合地图采集，生成路径地图；
RTK导航模块：用于生成路径信息2；
融合模块：用于接受所述路径信息1和路径信息2，加权融合后生成目标轨迹，融合方法包括以下子步骤：
步骤31：递推计算出采样时刻k的Rii(k)和Rij(k)，其中，Rii(k)表示Xi的自协方差函数，Rij(k)表示Xi、Xj的互协方差函数，Xi表示传感器i的测量值，Xj表示传感器j的测量值；
步骤32：计算采样时刻k的
Figure FDA0003126272190000041
其中，
Figure FDA0003126272190000042
表示Rij(k)的均值函数；
步骤33；计算采样时刻k的σi 2，其中，σi 2表示各传感器的方差；
步骤34：计算各传感器多次测量值的均值
Figure FDA0003126272190000043
步骤35：计算此时各传感器的最优加权因子
Figure FDA0003126272190000044
最优加权因子
Figure FDA0003126272190000045
的计算公式为：
Figure FDA0003126272190000046
此时，对应的总方差最小值为
Figure FDA0003126272190000047
其中，n为采样时刻；
步骤36：计算此时的估计值，计算方法为：
Figure FDA0003126272190000048
其中，
Figure FDA0003126272190000049
为融合后的真值，Wi为各传感器的加权因子；
所述系统使用如权利要求1所述的方法进行组合导航。
CN201910538148.3A 2019-06-20 2019-06-20 一种低速无人车园区内组合导航方法及系统 Active CN110187372B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910538148.3A CN110187372B (zh) 2019-06-20 2019-06-20 一种低速无人车园区内组合导航方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910538148.3A CN110187372B (zh) 2019-06-20 2019-06-20 一种低速无人车园区内组合导航方法及系统

Publications (2)

Publication Number Publication Date
CN110187372A CN110187372A (zh) 2019-08-30
CN110187372B true CN110187372B (zh) 2021-11-02

Family

ID=67722579

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910538148.3A Active CN110187372B (zh) 2019-06-20 2019-06-20 一种低速无人车园区内组合导航方法及系统

Country Status (1)

Country Link
CN (1) CN110187372B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112197783B (zh) * 2020-09-30 2022-08-02 哈尔滨工程大学 一种考虑车头指向的两阶段多抽样的rrt路径规划方法
CN112558130B (zh) * 2020-12-07 2023-12-19 安徽江淮汽车集团股份有限公司 定位数据的同步方法、装置、设备及存储介质
CN115248045A (zh) * 2021-04-25 2022-10-28 华为技术有限公司 一种地图、地图生成方法、地图使用方法及装置
CN114200926B (zh) * 2021-11-12 2023-04-07 河南工业大学 一种无人驾驶车辆的局部路径规划方法及系统
CN114360093B (zh) * 2021-12-31 2024-03-08 江苏鸣实纯钧科技有限公司 基于北斗rtk、slam定位和图像分析的路侧停车位巡检方法
CN116660916B (zh) * 2023-05-26 2024-02-02 广东省农业科学院设施农业研究所 一种用于果园移动机器人的定位方法、建图方法及电子设备

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107091648A (zh) * 2017-05-11 2017-08-25 江苏保千里视像科技集团股份有限公司 一种激光雷达与差分gps的数据融合方法及系统
CN107817509A (zh) * 2017-09-07 2018-03-20 上海电力学院 基于rtk北斗和激光雷达的巡检机器人导航系统及方法
WO2018186990A1 (en) * 2017-04-06 2018-10-11 Delphi Technologies, Inc. Hidden satellite signal rejection for automated vehicle navigation system
CN108646748A (zh) * 2018-06-05 2018-10-12 北京联合大学 一种场地无人车轨迹跟踪方法及系统
CN108645420A (zh) * 2018-04-26 2018-10-12 北京联合大学 一种基于差分导航的自动驾驶车辆多路径地图的创建方法
CN109445441A (zh) * 2018-12-14 2019-03-08 上海安吉四维信息技术有限公司 3d激光导航系统、自动引导运输车及工作方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018186990A1 (en) * 2017-04-06 2018-10-11 Delphi Technologies, Inc. Hidden satellite signal rejection for automated vehicle navigation system
CN107091648A (zh) * 2017-05-11 2017-08-25 江苏保千里视像科技集团股份有限公司 一种激光雷达与差分gps的数据融合方法及系统
CN107817509A (zh) * 2017-09-07 2018-03-20 上海电力学院 基于rtk北斗和激光雷达的巡检机器人导航系统及方法
CN108645420A (zh) * 2018-04-26 2018-10-12 北京联合大学 一种基于差分导航的自动驾驶车辆多路径地图的创建方法
CN108646748A (zh) * 2018-06-05 2018-10-12 北京联合大学 一种场地无人车轨迹跟踪方法及系统
CN109445441A (zh) * 2018-12-14 2019-03-08 上海安吉四维信息技术有限公司 3d激光导航系统、自动引导运输车及工作方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于激光雷达在无人车中的定位与建图的应用研究综述;王庆闪等;《计算机科学》;20181031;第45卷(第10A期);全文 *

Also Published As

Publication number Publication date
CN110187372A (zh) 2019-08-30

Similar Documents

Publication Publication Date Title
CN110187372B (zh) 一种低速无人车园区内组合导航方法及系统
US11294060B2 (en) System and method for lidar-based vehicular localization relating to autonomous navigation
CN106840179B (zh) 一种基于多传感器信息融合的智能车定位方法
Wang et al. Intelligent vehicle self-localization based on double-layer features and multilayer LIDAR
CN105509738B (zh) 基于惯导/多普勒雷达组合的车载定位定向方法
Milanés et al. Autonomous vehicle based in cooperative GPS and inertial systems
CN107015238A (zh) 基于三维激光雷达的无人车自主定位方法
CN101285686B (zh) 一种农业机械导航分级定位的方法和系统
Pfaff et al. Towards mapping of cities
CN113822944B (zh) 一种外参标定方法、装置、电子设备及存储介质
CN113155124B (zh) 一种多源辅助导航方法及装置
WO2024012006A1 (zh) 定位精度估计的方法、装置、电子设备及存储介质
Bounini et al. Real time cooperative localization for autonomous vehicles
Meichen et al. Dynamic obstacle detection based on multi-sensor information fusion
Lu et al. Robot indoor location modeling and simulation based on Kalman filtering
Wang et al. UGV‐UAV robust cooperative positioning algorithm with object detection
JP2023533140A (ja) 自動運転車の位置推定精度を向上させる方法
Abdelkader et al. Localization based on DGPS for autonomous robots in precision agriculture
Igneczi et al. Node point optimization for local trajectory planners based on human preferences
Niknejad et al. Multi-sensor data fusion for autonomous vehicle navigation and localization through precise map
Xiang et al. Where am I? SLAM for mobile machines on a smart working site
CN116263332A (zh) 一种基于在线自寻优的无人机通信中继位置规划方法
Eckelmann et al. Position determination based on sensor data fusion with digital maps and 3D point cloud data
CN116202534B (zh) 一种隧道内定位方法、装置、设备及存储介质
Matute et al. Sensor Fusion-Based Localization Framework for Autonomous Vehicles in Rural Forested Environments

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
GR01 Patent grant
GR01 Patent grant