自动驾驶感知——导航与定位

文章目录

1. 汽车定位技术

1.1 汽车定位技术的定义

• 定义：相对某一坐标系，确定自动驾驶车辆位置与姿态(6自由度，

    X
   
   
    ,
   
   
    Y
   
   
    ,
   
   
    Z
   
   
    ,
   
   
    Y
   
   
    a
   
   
    w
   
   
    ,
   
   
    P
   
   
    i
   
   
    t
   
   
    c
   
   
    h
   
   
    ,
   
   
    R
   
   
    o
   
   
    l
   
   
    l
   
  
  
   X,Y,Z,Yaw,Pitch,Roll
  
 
X,Y,Z,Yaw,Pitch,Roll)。

• 评定方法：精度、鲁棒性、覆盖场景等

1.2 汽车定位技术实现方式

    在任何驾驶条件下，自动驾驶汽车均依赖于精准位姿信息。
定位技术包括三大类：

• 基于电子信号定位：GNSS,UWB,Wi-Fi,Cell Phone等
• 基于轨迹推算：IMU、Odometry、轮速计等
• 环境特征匹配：视觉、激光、融合定位等

1.3 定位源

    在不同交通场景下，每种定位源各有优缺，为提高车辆定位系统的整体性能，可以将定位方法进行优势互补，在提高定位精度的同时，定位方法具备一定的泛化能力。
卫星导航惯性导航视觉里程计激光里程计地图匹配定位绝对定位相对定位相对定位相对定位绝对定位遮挡复杂环境,存在定位失锁长时间运行存在累积误差易受光照影响，对环境结构要求较高空旷及特征缺乏环境下定位精度降低地图构建与实时更新是难点所在

1.3.1 不同定位源之间的联系

2. 卫星定位技术

2.1 全球四大导航卫星系统

美国全球定位系统（Global Navigation System，GPS）俄罗斯格洛纳斯卫星导航系统 (Global Navigation Satellite System，GLONASS)中国北斗卫星导航系统（BeiDou Navigation Satellite System，BDS）欧洲伽利略卫星导航系统（Galileo Satellite Navigation System，GALILEO)距离地面20200公里，24颗卫星，6条轨道距离地面24126公里，30颗卫星，3条轨道21500公里，35颗卫星

2.1.1 GPS

GPS的系统组成：由空间部分、地面控制部分和用户设备部分三部分组成
GPS卫星星座：设计星座：21+3颗； 当前星：31颗

➢21颗正式的工作卫星+3颗活动的备用卫星
➢6个轨道面，平均轨道高度20200km，轨道倾角55 ，周期11h 58min（顾及地球自转，地球-卫星的几何关系每天提前4min重复一次
➢保证在24小时，在高度角15以上，能够同时观测到4至8颗卫星

2.1.2 GLONASS

    格洛纳斯（GLONASS），是俄语“全球卫星导航系统GLOBAL NAVIGATION SATELLITE SYSTEM”的缩写。格洛纳斯卫星导航系统作用类似于美国的GPS、欧洲的伽利略卫星定位系统和中国的北斗卫星导航系统。

2.1.3 BDS

组成：35颗卫星。5颗静止轨道，30颗中轨道。在轨高度21500公里。2020年底共发射了55颗卫星

    北斗卫星导航系统可在全球范围内全天候、全天时为各类用户提供高精度、高可靠定位、导航、授时服务，并具短报文通信能力，已经初步具备区域导航、定位和授时能力。短报文通信：北斗系统用户终端具有双向报文通信功能，在远洋航行中有重要的应用价值。精密授时：北斗系统具有精密授时功能，可向用户提供20ns-100ns时间同步精度。

2.1.4 GALILEO

    伽利略卫星导航系统（Galileo satellite navigation system），是由欧盟研制和建立的全球卫星导航定位系统，该计划于1999年2月由欧洲委员会公布，欧洲委员会和欧空局共同负责。系统由轨道高度为23616km的30颗卫星组成，其中27颗工作星，3颗备份星。卫星轨道高度约2.4万公里，位于3个倾角为56度的轨道平面内。截止2016年12月，已经发射了18颗工作卫星，具备了早期操作能力（EOC），并计划在2019年具备完全操作能力（FOC）。全部30颗卫星（调整为24颗工作卫星，6颗备份卫星）计划于2020年发射完毕。

2.2 卫星定位原理

卫星定位系统组成：由空间部分、地面控制部分和用户设备部分等三部分组成    卫星定位至少需要同时观测4颗卫星。接下来说说原因。    如图所示，可以列出以下方程：

             (
            
            
             
              x
             
             
              1
             
            
            
             −
            
            
             x
            
            
             )
            
           
           
            2
           
          
          
           +
          
          
           
            
             (
            
            
             
              y
             
             
              1
             
            
            
             −
            
            
             y
            
            
             )
            
           
           
            2
           
          
          
           +
          
          
           
            
             (
            
            
             
              z
             
             
              1
             
            
            
             −
            
            
             z
            
            
             )
            
           
           
            2
           
          
         
        
        
         =
        
        
         c
        
        
         (
        
        
         
          v
         
         
          
           t
          
          
           1
          
         
        
        
         −
        
        
         
          v
         
         
          
           t
          
          
           0
          
         
        
        
         )
        
        
         =
        
        
         
          d
         
         
          1
         
        
       
      
     
    
    
     
      
       
        
         
          
           
            
             (
            
            
             
              x
             
             
              2
             
            
            
             −
            
            
             x
            
            
             )
            
           
           
            2
           
          
          
           +
          
          
           
            
             (
            
            
             
              y
             
             
              2
             
            
            
             −
            
            
             y
            
            
             )
            
           
           
            2
           
          
          
           +
          
          
           
            
             (
            
            
             
              z
             
             
              2
             
            
            
             −
            
            
             z
            
            
             )
            
           
           
            2
           
          
         
        
        
         =
        
        
         c
        
        
         (
        
        
         
          v
         
         
          
           t
          
          
           2
          
         
        
        
         −
        
        
         
          v
         
         
          
           t
          
          
           0
          
         
        
        
         )
        
        
         =
        
        
         
          d
         
         
          2
         
        
       
      
     
    
    
     
      
       
        
         
          
           
            
             (
            
            
             
              x
             
             
              3
             
            
            
             −
            
            
             x
            
            
             )
            
           
           
            2
           
          
          
           +
          
          
           
            
             (
            
            
             
              y
             
             
              3
             
            
            
             −
            
            
             y
            
            
             )
            
           
           
            2
           
          
          
           +
          
          
           
            
             (
            
            
             
              z
             
             
              3
             
            
            
             −
            
            
             z
            
            
             )
            
           
           
            2
           
          
         
        
        
         =
        
        
         c
        
        
         (
        
        
         
          v
         
         
          
           t
          
          
           3
          
         
        
        
         −
        
        
         
          v
         
         
          
           t
          
          
           0
          
         
        
        
         )
        
        
         =
        
        
         
          d
         
         
          3
         
        
       
      
     
    
    
     
      
       
        
         
          
           
            
             (
            
            
             
              x
             
             
              4
             
            
            
             −
            
            
             x
            
            
             )
            
           
           
            2
           
          
          
           +
          
          
           
            
             (
            
            
             
              y
             
             
              4
             
            
            
             −
            
            
             y
            
            
             )
            
           
           
            2
           
          
          
           +
          
          
           
            
             (
            
            
             
              z
             
             
              4
             
            
            
             −
            
            
             z
            
            
             )
            
           
           
            2
           
          
         
        
        
         =
        
        
         c
        
        
         (
        
        
         
          v
         
         
          
           t
          
          
           4
          
         
        
        
         −
        
        
         
          v
         
         
          
           t
          
          
           0
          
         
        
        
         )
        
        
         =
        
        
         
          d
         
         
          4
         
        
       
      
     
    
   
   
    \begin{array}{l}\sqrt {{{({x_1} - x)}^2} + {{({y_1} - y)}^2} + {{({z_1} - z)}^2}} = c({v_{{t_1}}} - {v_{{t_0}}}) = {d_1}\\\sqrt {{{({x_2} - x)}^2} + {{({y_2} - y)}^2} + {{({z_2} - z)}^2}} = c({v_{{t_2}}} - {v_{{t_0}}}) = {d_2}\\\sqrt {{{({x_3} - x)}^2} + {{({y_3} - y)}^2} + {{({z_3} - z)}^2}} = c({v_{{t_3}}} - {v_{{t_0}}}) = {d_3}\\\sqrt {{{({x_4} - x)}^2} + {{({y_4} - y)}^2} + {{({z_4} - z)}^2}} = c({v_{{t_4}}} - {v_{{t_0}}}) = {d_4}\end{array}
   
  
 (x1​−x)2+(y1​−y)2+(z1​−z)2​=c(vt1​​−vt0​​)=d1​(x2​−x)2+(y2​−y)2+(z2​−z)2​=c(vt2​​−vt0​​)=d2​(x3​−x)2+(y3​−y)2+(z3​−z)2​=c(vt3​​−vt0​​)=d3​(x4​−x)2+(y4​−y)2+(z4​−z)2​=c(vt4​​−vt0​​)=d4​​四个方程式中各个参数意义如下：
 
  
   
    x
   
   
    、
   
   
    y
   
   
    、
   
   
    z
   
  
  
   x、y、z
  
 
x、y、z为待测点坐标的空间直角坐标。
 
  
   
    
     x
    
    
     i
    
   
   
    、
   
   
    
     y
    
    
     i
    
   
   
    、
   
   
    
     z
    
    
     i
    
   
   
    (
   
   
    i
   
   
    =
   
   
    1
   
   
    、
   
   
    2
   
   
    、
   
   
    3
   
   
    、
   
   
    4
   
   
    )
   
  
  
   x_i 、y_i 、z_i (i=1、2、3、4)
  
 
xi​、yi​、zi​(i=1、2、3、4)分别为卫星1、卫星2、卫星3、卫星4在t时刻的空间直角坐标，可由卫星导航电文求得。
 
  
   
    
     V
    
    
     
      t
     
     
      i
     
    
   
   
    (
   
   
    i
   
   
    =
   
   
    1
   
   
    、
   
   
    2
   
   
    、
   
   
    3
   
   
    、
   
   
    4
   
   
    )
   
  
  
   V_{t_i} (i=1、2、3、4)
  
 
Vti​​(i=1、2、3、4) 分别为卫星1、卫星2、卫星3、卫星4的卫星钟的钟差，由卫星星历提供。
 
  
   
    
     V
    
    
     
      t
     
     
      o
     
    
   
  
  
   V_{t_o}
  
 
Vto​​为接收机的钟差。

存在4个未知数：3个点位坐标分类与一个接收机钟差参数，因此至少需要四颗同步伪距测量值，即需要同步观测四颗卫星。

伪距就是由卫星发射的测距码信号到达GPS接收机的传播时间乘以光速所得出的量测距离。由于卫星时钟、接收机时钟的误差以及无线电信号经过电离层和对流层中的延迟，实际测出的距离与卫星到接收机的几何距离有一定差值，因此一般称量测出的距离为伪距（有以下三种类型）。

• C/A码伪距：用C/A码进行测量的伪距
• P码伪距：用P码测量的伪距。
• 载波相位: L1/L2载波相位

2.3 卫星定位方法分类

    无论采取何种GPS定位方法，都是通过观测GPS卫星而获得某种观测量来实现的。GPS卫星信号含有多种定位信息，根据不同的要求，可以从中获得不同的观测量，主要包括：

• 根据码相位观测得出的伪距
• 根据载波相位观测得出的位置
• 由积分多普勒计数得出的伪距
• 由干涉法测量得出的时间延迟

    前两种应用较为广泛，伪距测量方法可直接测定卫星到接收机之间的距离，不存在模糊度的问题，抗干扰能力强但精度低；载波相位测量方法精度高，但存在着存在模糊度问题，无法直接测定卫星到接收机之间的距离、抗干扰能力弱的问题。多普勒法观测时间较长，一般数小时，并要求接收机的震荡器保持高度稳定。干涉法所需设备较昂贵，数据处理复杂。

3. 卫星组合惯导定位

3.1 惯性导航系统 IMU

原理：惯性导航，利用惯性元件来测量运载体本身的方向和加速度，经过积分和运算得到速度（或者直接利用测速结果）和位置，从而达到对运载体导航定位的目的。

组成：惯性测量装置包括加速度计和陀螺仪，又称惯性测量单元。
• 3个自由度陀螺仪用来测量运载体的3个转动运动；
• 3个加速度计用来测量运载体的3个平移运动的加速度。

    捷联惯导系统（SINS）中惯性测量器件（陀螺和加速度计）直接与运载体固联，通过导航计算机采集惯性器件的输出信息并进行数值积分求解运载体的姿态、速度和位置等导航参数，这三组参数的求解过程即所谓的姿态更新算法、速度更新算法和位置更新算法。

3.2 GPS+惯导定位技术

    GPS遇到被屏蔽、遮挡或者高速运动的情况时，信号容易失锁，难以定位；同时采样频率低，难以满足高速运动状态下高精定位需求。INS难以克服的缺点是其导航定位误差随时间累加,难以长时间独立工作。将GPS与INS互补，就诞生了GPS/INS组合。

    GPS/INS组合系统利用高精度的 GPS信息作为外部量测输入,在运动过程中修正 INS,以控制其误差随时间的累积；而短时间内高精度的 INS 定位结果,又可以解决GPS 动态环境中的信号失锁和周跳问题。INS 还可以辅助 GPS 接收机增强其抗干扰能力,提高捕获和跟踪卫星信号的能力。

    GPS/INS有三种常见的组合方式：紧耦合、松耦合、深耦合。

3.2.1 紧耦合

    紧耦合系统是将由GNSS码环与载波跟踪环解算得到的伪距、伪距率与由惯性导航系统结合自身信息与卫星星历进行计算得到的伪距、伪距率做差，得到伪距与伪距率的测量残差，将其作为融合滤波器的输入观测量，得到惯性导航系统计算误差以及传感器偏差以完成对惯性导航系统的校正并获得位置与速度的最优估计值。

3.2.2 松耦合

    在松耦合系统中，GNSS给INS提供位置信息，二者硬件上相互独立，而且可以随时断开连接，分别输出定位信息与速度信息到融合滤波器，融合滤波器进行优化处理后将结果反馈给惯性导航系统对其修正后输出。

3.2.3 深耦合

    深耦合系统相对于紧耦合系统，增加了INS单元对GNSS接收机的辅助。利用INS单元结合星历信息可以对伪距与载波的多普勒频移进行估计，利用估计结果辅助接收机的捕获与跟踪环路，可以有效提高GNSS接收机跟踪环路的动态性与灵敏性。

4. 多传感器融合定位

    充分利用多种不同传感器，进行优势互补，动态自适应组合多源融合定位方法是目前的研究趋势。

4.1 多传感器融合定位系统原理

    多传感器融合定位输入主要来自GNSS-RTK、惯性导航系统和地图匹配定位系统，融合定位系统对进行预处理、数据配准(数据配准包括时间配准与空间配准)和数据融合等处理后，可输出汽车自身的速度、位置和姿态信息。

5. 同步定位与地图构建(SLAM) 介绍

5.1 SLAM 基本概念

• SLAM是同步定位与地图构建(Simultaneous Localization And Mapping)；实时动态扫描在周围环境，解决自主定位的同时，同步动态构建周围环境地图。
• SLAM主要用于解决移动机器人在未知环境中运行时定位导航与地图构建的问题；
• 目前，SLAM技术被广泛运用于机器人、无人机、无人驾驶、AR、VR等领域，依靠传感器可实现机器的自主定位、建图、路径规划等功能。
• 由于传感器不同，SLAM的实现方式也有所不同，按传感器来分，SLAM主要包括激光SLAM和视觉SLAM两大类；

5.2 激光SLAM

• 激光SLAM采用2D或3D激光雷达，2D激光雷达一般用于室内机器人上（如扫地机器人），而3D激光雷达一般使用于无人驾驶领域。
• 激光SLAM系统通过对不同时刻两片点云的匹配与比对，计算激光雷达相对运动的距离和姿态的改变，完成自定位。
• 激光SLAM理论研究也相对成熟，落地产品更丰富。

5.3 视觉SLAM

    随着计算机视觉的迅速发展，视觉SLAM因为信息量大，适用范围广等优点受到广泛关注。

• 基于深度摄像机的视觉SLAM，跟激光SLAM类似，通过收集到的点云数据，能直接计算障碍物距离；
• 基于单目、鱼眼相机的视觉SLAM方案，利用多帧图像来估计自身的位姿变化，再通过累计位姿变化来计算距离物体的距离，并进行定位与地图构建；

5.4 SLAM 基本框架

▪ 传感器数据：主要用于采集实际环境中的各类型原始数据。包括激光扫描数据、视频图像数据、点云数据等；
▪ 前端：主要用于不同时刻间移动目标相对位置的估算。包括特征匹配、直接配准等算法的应用；
▪ 后端：主要用于优化视觉里程计带来的累计误差。包括滤波器、图优化等算法应用；
▪ 建图：它根据估计的轨迹，建立与任务要求对应的地图。
▪ 回环检测：主要用于空间累积误差消除；

声明

本人所有文章仅作为自己的学习记录，若有侵权，联系立删。本系列文章主要参考了清华大学、北京理工大学、哈尔滨工业大学、深蓝学院、百度Apollo等相关课程。