自动驾驶高精定位与路径规划方法
阅读：814发布：2020-05-08
专利汇可以提供自动驾驶高精定位与路径规划方法专利检索，专利查询，专利分析的服务。并且一种自动驾驶高精 定位 与路径规划方法，属于智能交通技术领域。本 发明 的目的是依靠观察车与静止障碍物之间的相对距离，将路侧设施与环视图像信息有效融合的自动驾驶高精定位与路径规划方法。本发明包括以下步骤：在道路 节点 周围的道路两侧布置RSU，RSU分类信息编码，RSU的安装，车辆光学定位。本发明可以解决目前ADAS系统所解决不了的纵向精准定位问题、规则 感知 问题、路口中央区域路径规划问题。，下面是自动驾驶高精定位与路径规划方法专利的具体信息内容。
1.一种自动驾驶高精定位与路径规划方法，其特征在于：包括以下步骤：
步骤S1：在道路节点周围的道路两侧布置RSU，RSU由V2X信号发射器和标识照明两个部分组成；
步骤S2：RSU分类信息编码，RSU一共有7类，其中，城区定位RSU分为行驶路段、灯控交叉路口、无灯路口、环岛等4类，高速公路定位RSU分为行驶路段、入口、出口3类；
步骤S3：RSU的安装，RSU装在杆柱上、装在横杆上、利用现有的路灯杆或交通灯立柱原有地面设施；RSU外壳上应涂有矩形鲜艳彩色标识，RSU同时充当光学定位的靶点；
步骤S4：车辆光学定位方法，即PCM光学测距，用仰角测量和方位角测量两种方法进行本车定位测量。
2.根据权利要求1所述的自动驾驶高精定位与路径规划方法，其特征在于：步骤S1中的道路节点包括：道路交叉点、道路结构更改点、交通规则改变点、道路几何变更点。
3.根据权利要求1所述的自动驾驶高精定位与路径规划方法，其特征在于：步骤S2中7类类别区分：表1类别编号
全部RSU播报信息由11段内容信息构成，见表2RSU播报信息内容协议
其中，车道线类别识别码一共3位，第一位代表离RSU最远车道的第1条，其它依次类推；
第2位代表车道左线类别，第3位代表车道右线类别；车道线种类规定见表3表3 车道线类别编码
4.根据权利要求1所述的自动驾驶高精定位与路径规划方法，其特征在于：步骤S4中仰角测量和方位角测量两种方法：
1)俯仰角测量定位法：
①假设车辆沿x轴线方向前进，视野内出现了道路前方右侧的RSU，垂线高度已知为h，PIPC可根据与RSU安装在一起的彩色标识测出RSU的仰角θ，本车与RSU垂足F之间的距离R为：
②车道横向法线上本车x轴线距RSU垂足F的距离L为：
α为本车与RSU连线与前进方向之间的夹角；
③沿车道纵向上本车y轴距离RSU垂足F的距离为：
④根据计算出的L、M，和RSU广播的W、N、S相对位置参数，本车即可计算出目前本车的中心向与各个车道的中心线横向距离为多少、距前方停车线的纵向距离为多远等各种定位参数；
2)横向方位角测量定位法
车载PCM系统可以精确测得α1、α2，因为L是已知，所以
由于S、N、W为已知的RSU广播参数，所以根据L2、M可进而推之本车所在的车道和距离停车线的距离等自动驾驶所需参数；
3)纵向方位角测量定位法
车载PCM系统可以精确测得α’1、α’2，因为L’是已知，所以
其余定位方法与横向方位角测量定位法相同。
5.根据权利要求4所述的自动驾驶高精定位与路径规划方法，其特征在于：因为环视图像定位相机能够在100米范围内对360°图像进行全景解析，视野没有指向性，所以能捕捉到周边环境内的所有可见RSU，因此还可以利用多个RSU的计算结果进行互相修正。
6.根据权利要求1所述的自动驾驶高精定位与路径规划方法，其特征在于：定位测量的精度的系统保证方法：
1)对PCM系统的精度要求
俯仰角测量定位法要求PCC相机对30m以外物体的像素分辨率应能达到1m；假设标高为
10m，则相当于要求PCM的标高测量精度误差为10％以内，相应的本机定位误差约为50厘米；
2)PCM测量数据与INS的信息融合
①本车距标识点的距离x测量看成是遵循线性随机差分方程的时间离散量测量：
运动方程中xk+1|k是INS系统根据前一个状态预测出的运动过程状态向量，xk|k是k状态向量的最优结果；Φk+1|k是从前一个离散状态k到随后下一个新状态k+1之间的转移矩阵；wk是转移过程噪声向量，测量方程中zk+1是PCM系统的测量向量，Hk+1|k是测量矩阵；vk+1是PCM系统的测量过程噪声矩阵；
②利用卡尔曼滤波器对INS和PCM的测量结果进行融合，过程如下：
(1)利用已知的k-1状态预测出现在的先验状态估计值和先验误差协方差矩阵：
初始化 和Pk-1可以任意给定初值；然后，根据上述INS预测结果，同时再收集现在状态的PCM测量量值，二者结合利用以下三个公式进行测量更新；
(2)修正矩阵
(3)更新观测值
(4)更新误差协方差
其中，是滤波器估算出来的状态向量，Pk是滤波器估算的状态协方差矩阵， 是预测的状态协方差矩阵，Q是动态噪声矩阵，K是卡尔曼增益矩阵，z是PCM测量向量。
7.根据权利要求1所述的自动驾驶高精定位与路径规划方法，其特征在于：RSU-PCM定位系统在自动驾驶控制中的应用；
①城市道路RSU部署：RSU只布置在道路节点附近，道路节点是指：道路交叉点、道路结构更改点、交通规则改变点、道路几何变更点，其中道路结构更改点包括车道数量变化、车道类别变化；交通规则改变点包括限速的变化、车道线类别更改；道路几何变更点包括曲率的变化、坡度的变化；
②节点间均匀城市路段的自动驾驶模式：道路节点与节点之间的道路结构是均匀一致化的，即使没有RSU，车辆也能在驶过本路段入口处的RSU点以后始终记住自己所处的车道，也能根据车道导航要求换到理想车道；
③驶经中途节点：中途节点的RSU-PCM单元只负责播报上述行车参数的更改信息，而不必提供高精定位光学标识功能；
④城市路口节点的自动驾驶：
1)十字路口中央区域路径规划：路口的RSU布置在入口和出口处每个路端的两侧，双侧的RSU连线为车道中心线的法线方向，每条RSU连线都构成一个虚拟的道路入口“门关”；
2)复杂路口路径规划：较复杂路口也按照RSU-环视信息融合原理完成路口通行任务；
⑤圆形环岛自动驾驶绕行路径规划：圆形环岛是一种大型的综合节点，环岛中心的RSU需要表明自己是环岛中心U4类RSU，内容包括：环岛车道数量、每条车道中心线相对于环岛中心的半径；
⑥高速公路驾驶：高速公路自动驾驶任务可由4项最基本的行驶状态组合而成，分别为：恒态路段高速自动巡航状态、上路匝道汇流状态、下路匝道分流状态、状态转移。
自动驾驶高精定位与路径规划方法
技术领域
[0001] 本发明属于智能交通技术领域。
背景技术
[0002] 像人类驾驶一样，自动驾驶系统并不是时时刻刻都需要知道自己的精准位置坐标，而是只需要知道自己与静止边界、移动障碍之间的相对位置。
[0003] 移动障碍的探测主要依靠激光雷达、毫米波雷达、超声雷达、摄像头等车载传感器进行探测，目前技术已经成熟到大批量产品应用的阶段。相比之下，对静止边界的精准感知目前还面临巨大的挑战。对静止边界感知的内容包括：行驶区域边界、交通规则、信息标识、道路几何特性(形态、横向/纵向坡度)等。
[0004] 静态障碍的探测方法包括激光雷达、高精地图等技术，其中高精地图+基于卫星基站的高精定位是保证自动驾驶车辆大批量投放最有潜力的解决方案，但是目前面临着几大挑战。一是某些高精地图的内容与国家法规要求不相符合，其次地图扫描与更新成本很高，同时卫星信号地面接收基站布置成本很高，另外还有大面积的卫星信号丢失问题(比如在涵洞内和城市高楼峡谷之间)，目前都没有找到理想的解决方案。
[0005] 为了摆脱对卫星定位系统的依赖，目前也有很多根据路侧单元定位的研究，主要是是依靠信号接收时间、多RSU之间的信号时间[A Elmarady,2013][A Khattab,2015]、信号接收强度等原理[A Shojaifar,2015]，存在的问题是定位精度最高只能达到1.5m左右，用于车道导航还勉强，但是用于自动驾驶控制与避撞的精度还远远不够[C Ou,2019],[J Xu,2012],[M Aatique,1997],[R Reza,2000],[S Chaabene,2019]。
[0006] 自动驾驶需要依靠高精定位系统解决如下六大问题：-需要了解身处哪个车道和需要换到哪个车道(车道导航问题)
-需要知道距离车道边缘的距离(横向定位问题)
-需要知道停车线的位置(纵向定位)
-相对位置精度小于30cm(定位高精问题)
-需要知道路口中央区域没有车道线时的行驶路线(路口规划问题)
-需要了解交通规则(规则感知问题)。
-需要知道距离车道边缘的距离(横向定位问题)
-需要知道停车线的位置(纵向定位)
-相对位置精度小于30cm(定位高精问题)
-需要知道路口中央区域没有车道线时的行驶路线(路口规划问题)
-需要了解交通规则(规则感知问题)。
发明内容
[0007] 本发明的目的是依靠观察车与静止障碍物之间的相对距离，将路侧设施与环视图像信息有效融合的自动驾驶高精定位与路径规划方法。
[0008] 本发明包括以下步骤：步骤S1：在道路节点周围的道路两侧布置RSU，RSU由V2X信号发射器和标识照明两个部分组成；
步骤S2：RSU分类信息编码，RSU一共有7类，其中，城区定位RSU分为行驶路段、灯控交叉路口、无灯路口、环岛等4类，高速公路定位RSU分为行驶路段、入口、出口3类；
步骤S3：RSU的安装，RSU装在杆柱上、装在横杆上、利用现有的路灯杆或交通灯立柱原有地面设施；RSU外壳上应涂有矩形鲜艳彩色标识，RSU同时充当光学定位的靶点；
步骤S4：车辆光学定位方法，即PCM光学测距，用仰角测量和方位角测量两种方法进行本车定位测量。
步骤S2：RSU分类信息编码，RSU一共有7类，其中，城区定位RSU分为行驶路段、灯控交叉路口、无灯路口、环岛等4类，高速公路定位RSU分为行驶路段、入口、出口3类；
步骤S3：RSU的安装，RSU装在杆柱上、装在横杆上、利用现有的路灯杆或交通灯立柱原有地面设施；RSU外壳上应涂有矩形鲜艳彩色标识，RSU同时充当光学定位的靶点；
步骤S4：车辆光学定位方法，即PCM光学测距，用仰角测量和方位角测量两种方法进行本车定位测量。
[0009] 本发明步骤S1中的道路节点包括：道路交叉点、道路结构更改点、交通规则改变点、道路几何变更点。
[0010] 本发明步骤S2中7类类别区分：表1类别编号全部RSU播报信息由11段内容信息构成，见表2RSU播报信息内容协议
其中，车道线类别识别码一共3位，第一位代表离RSU最远车道的第1条，其它依次类推；
第2位代表车道左线类别，第3位代表车道右线类别；车道线种类规定见表3
表3车道线类别编码
其中，车道线类别识别码一共3位，第一位代表离RSU最远车道的第1条，其它依次类推；
第2位代表车道左线类别，第3位代表车道右线类别；车道线种类规定见表3
表3车道线类别编码
[0011] 本发明步骤S4中仰角测量和方位角测量两种方法：1)俯仰角测量定位法：
①假设车辆沿x轴线方向前进，视野内出现了道路前方右侧的RSU，垂线高度已知为h，PIPC可根据与RSU安装在一起的彩色标识测出RSU的仰角θ，本车与RSU垂足F之间的距离R为：
②车道横向法线上本车x轴线距RSU垂足F的距离L为：
α为本车与RSU连线与前进方向之间的夹角；
③沿车道纵向上本车y轴距离RSU垂足F的距离为：
④根据计算出的L、M，和RSU广播的W、N、S相对位置参数，本车即可计算出目前本车的中心向与各个车道的中心线横向距离为多少、距前方停车线的纵向距离为多远等各种定位参数；
2)横向方位角测量定位法
车载PCM系统可以精确测得α1、α2，因为L是已知，所以
由于S、N、W为已知的RSU广播参数，所以根据L2、M可进而推之本车所在的车道和距离停车线的距离等自动驾驶所需参数；
3)纵向方位角测量定位法
车载PCM系统可以精确测得α’1、α’2，因为L’是已知，所以
其余定位方法与横向方位角测量定位法相同。
①假设车辆沿x轴线方向前进，视野内出现了道路前方右侧的RSU，垂线高度已知为h，PIPC可根据与RSU安装在一起的彩色标识测出RSU的仰角θ，本车与RSU垂足F之间的距离R为：
②车道横向法线上本车x轴线距RSU垂足F的距离L为：
α为本车与RSU连线与前进方向之间的夹角；
③沿车道纵向上本车y轴距离RSU垂足F的距离为：
④根据计算出的L、M，和RSU广播的W、N、S相对位置参数，本车即可计算出目前本车的中心向与各个车道的中心线横向距离为多少、距前方停车线的纵向距离为多远等各种定位参数；
2)横向方位角测量定位法
车载PCM系统可以精确测得α1、α2，因为L是已知，所以
由于S、N、W为已知的RSU广播参数，所以根据L2、M可进而推之本车所在的车道和距离停车线的距离等自动驾驶所需参数；
3)纵向方位角测量定位法
车载PCM系统可以精确测得α’1、α’2，因为L’是已知，所以
其余定位方法与横向方位角测量定位法相同。
[0012] 本发明因为环视图像定位相机能够在100米范围内对360°图像进行全景解析，视野没有指向性，所以能捕捉到周边环境内的所有可见RSU，因此还可以利用多个RSU的计算结果进行互相修正。
[0013] 本发明定位测量的精度的系统保证方法：1)对PCM系统的精度要求
俯仰角测量定位法要求PCC相机对30m以外物体的像素分辨率应能达到1m；假设标高为
10m，则相当于要求PCM的标高测量精度误差为10％以内，相应的本机定位误差约为50厘米；
2)PCM测量数据与INS的信息融合
①本车距标识点的距离x测量看成是遵循线性随机差分方程的时间离散量测量：
运动方程中xk+1|k是INS系统根据前一个状态预测出的运动过程状态向量，xk|k是k状态向量的最优结果；Φk+1|k是从前一个离散状态k到随后下一个新状态k+1之间的转移矩阵；wk是转移过程噪声向量，测量方程中zk+1是PCM系统的测量向量，Hk+1|k是测量矩阵；vk+1是PCM系统的测量过程噪声矩阵；
②利用卡尔曼滤波器对INS和PCM的测量结果进行融合，过程如下：
⑴利用已知的k-1状态预测出现在的先验状态估计值和先验误差协方差矩阵：
初始化 和Pk-1可以任意给定初值；然后，根据上述INS预测结果，同时再收集现在状态的PCM测量量值，二者结合利用以下三个公式进行测量更新；
⑵修正矩阵
⑶更新观测值
⑷更新误差协方差
其中，是滤波器估算出来的状态向量，Pk是滤波器估算的状态协方差矩阵， 是预测的状态协方差矩阵，Q是动态噪声矩阵，K是卡尔曼增益矩阵，z是PCM测量向量。
俯仰角测量定位法要求PCC相机对30m以外物体的像素分辨率应能达到1m；假设标高为
10m，则相当于要求PCM的标高测量精度误差为10％以内，相应的本机定位误差约为50厘米；
2)PCM测量数据与INS的信息融合
①本车距标识点的距离x测量看成是遵循线性随机差分方程的时间离散量测量：
运动方程中xk+1|k是INS系统根据前一个状态预测出的运动过程状态向量，xk|k是k状态向量的最优结果；Φk+1|k是从前一个离散状态k到随后下一个新状态k+1之间的转移矩阵；wk是转移过程噪声向量，测量方程中zk+1是PCM系统的测量向量，Hk+1|k是测量矩阵；vk+1是PCM系统的测量过程噪声矩阵；
②利用卡尔曼滤波器对INS和PCM的测量结果进行融合，过程如下：
⑴利用已知的k-1状态预测出现在的先验状态估计值和先验误差协方差矩阵：
初始化 和Pk-1可以任意给定初值；然后，根据上述INS预测结果，同时再收集现在状态的PCM测量量值，二者结合利用以下三个公式进行测量更新；
⑵修正矩阵
⑶更新观测值
⑷更新误差协方差
其中，是滤波器估算出来的状态向量，Pk是滤波器估算的状态协方差矩阵， 是预测的状态协方差矩阵，Q是动态噪声矩阵，K是卡尔曼增益矩阵，z是PCM测量向量。
[0014] 本发明RSU-PCM定位系统在自动驾驶控制中的应用；①城市道路RSU部署：RSU只布置在道路节点附近，道路节点是指：道路交叉点、道路结构更改点、交通规则改变点、道路几何变更点，其中道路结构更改点包括车道数量变化、车道类别变化；交通规则改变点包括限速的变化、车道线类别更改；道路几何变更点包括曲率的变化、坡度的变化；
②节点间均匀城市路段的自动驾驶模式：道路节点与节点之间的道路结构是均匀一致化的，即使没有RSU，车辆也能在驶过本路段入口处的RSU点以后始终记住自己所处的车道，也能根据车道导航要求换到理想车道；
③驶经中途节点：中途节点的RSU-PCM单元只负责播报上述行车参数的更改信息，而不必提供高精定位光学标识功能；
④城市路口节点的自动驾驶：
1)十字路口中央区域路径规划：路口的RSU布置在入口和出口处每个路端的两侧，双侧的RSU连线为车道中心线的法线方向，每条RSU连线都构成一个虚拟的道路入口“门关”；
2)复杂路口路径规划：较复杂路口也按照RSU-环视信息融合原理完成路口通行任务；
⑤圆形环岛自动驾驶绕行路径规划：圆形环岛是一种大型的综合节点，环岛中心的RSU需要表明自己是环岛中心U4类RSU，内容包括：环岛车道数量、每条车道中心线相对于环岛中心的半径；
⑥高速公路驾驶：高速公路自动驾驶任务可由4项最基本的行驶状态组合而成，分别为：恒态路段高速自动巡航状态、上路匝道汇流状态、下路匝道分流状态、状态转移。
本系统像人类驾驶一样，只靠观察本车与静止障碍物之间的相对距离，而不关心障碍物的绝对位置坐标，不用依赖高精卫星定位和高精地图系统，为解决上述自动驾驶“六大问题”提供一种技术途径。
②节点间均匀城市路段的自动驾驶模式：道路节点与节点之间的道路结构是均匀一致化的，即使没有RSU，车辆也能在驶过本路段入口处的RSU点以后始终记住自己所处的车道，也能根据车道导航要求换到理想车道；
③驶经中途节点：中途节点的RSU-PCM单元只负责播报上述行车参数的更改信息，而不必提供高精定位光学标识功能；
④城市路口节点的自动驾驶：
1)十字路口中央区域路径规划：路口的RSU布置在入口和出口处每个路端的两侧，双侧的RSU连线为车道中心线的法线方向，每条RSU连线都构成一个虚拟的道路入口“门关”；
2)复杂路口路径规划：较复杂路口也按照RSU-环视信息融合原理完成路口通行任务；
⑤圆形环岛自动驾驶绕行路径规划：圆形环岛是一种大型的综合节点，环岛中心的RSU需要表明自己是环岛中心U4类RSU，内容包括：环岛车道数量、每条车道中心线相对于环岛中心的半径；
⑥高速公路驾驶：高速公路自动驾驶任务可由4项最基本的行驶状态组合而成，分别为：恒态路段高速自动巡航状态、上路匝道汇流状态、下路匝道分流状态、状态转移。
本系统像人类驾驶一样，只靠观察本车与静止障碍物之间的相对距离，而不关心障碍物的绝对位置坐标，不用依赖高精卫星定位和高精地图系统，为解决上述自动驾驶“六大问题”提供一种技术途径。
[0015] 本发明依靠路侧单元、环视相机图像分析技术，不依靠激光雷达、高精卫星定位地面设施(比如DGPS基站、RTK基站)和自动驾驶高精地图，无需对车载感知硬件与算法提出过高的计算要求，即可实现城区道路自动驾驶的分米级高精定位。本系统仍然需要配合使用普通精度的卫星导航与定位系统。
[0017] 图1是本发明城市道路依靠RSU-PCM的自动驾驶过程图；图2是本发明系统构成图；
图3是本发明RSU外壳上涂色范围示例图；
图4是本发明俯仰角测量定位法中车与RSU示例图；
图5是本发明定位计算方法图；
图6是本发明横向方位角测量定位法中车与RSU示例图；
图7是本发明纵向方位角测量定位法中车与RSU示例图；
图8是本发明定位测量精度的系统路线图；
图9是本发明城市路口节点的自动驾驶中十字路口中央区域路径规划图；
图10是具有左转待转区域十字路口图；
图11是具有直行待转区域十字路口图；
图12是本发明复杂路口路径规划图，RSU1-12类别U2；
图13是本发明复杂路口路径规划图，环岛中心采用U4类RSU，其余类别均为U2；
图14是本发明图13的局部放大图。
具体是实施方式
图3是本发明RSU外壳上涂色范围示例图；
图4是本发明俯仰角测量定位法中车与RSU示例图；
图5是本发明定位计算方法图；
图6是本发明横向方位角测量定位法中车与RSU示例图；
图7是本发明纵向方位角测量定位法中车与RSU示例图；
图8是本发明定位测量精度的系统路线图；
图9是本发明城市路口节点的自动驾驶中十字路口中央区域路径规划图；
图10是具有左转待转区域十字路口图；
图11是具有直行待转区域十字路口图；
图12是本发明复杂路口路径规划图，RSU1-12类别U2；
图13是本发明复杂路口路径规划图，环岛中心采用U4类RSU，其余类别均为U2；
图14是本发明图13的局部放大图。
具体是实施方式
[0018] 本发明“全景反折光学测量”(PCM)系统是指，利用反折射式360°环视相机(PCC)进行无缝的全景图像感知，同时可对变形的图像进行全方位信息还原，满足分米级高精的的内容识别和测量要求，相关专利见发明人张少军于2016、2018.7、2018.9、2018.10、2018.11.13、2018.11.27、2018.11.30、2019.1.25申请的相关专利。PCM定位系统不会因为镜头的视角限制而丢失地标或漏检目标。PCC相机的光学与定位原理见S Zhang、张少军、M Schonbein等文献。
[0019] RSU是指Road Side Unit路侧单元，是ETC系统中，安装在路侧，采用DSRC(Dedicated Short Range Communication)技术，与车载单元(OBU，On Board Unit)进行通讯，实现车辆身份识别，电子扣分的装置。
[0020] 4.1本发明原理如果地面上一个固定的静止设施与道路的相对位置是已知的，并且本车可以随时测量出这个固定设施与本车之间的相对方位，那么通过这个静止设施，就可以计算出本车和道路之间的相对位置关系，而不需要通过本车位置和道路几何的绝对位置坐标来计算。
[0021] 本系统把RSU当作上述固定的静止设备。RSU与道路要素之间的相对位置(例如距车道线的横向距离、距停车线的纵向距离等)在安装RSU的安装施工过程中进行厘米级别精度测量。
[0022] 本系统用环视图像定位技术对RSU装置相对于本车的距离和方位角进行分米精度的实时测量。
[0023] RSU将本单元的相对位置信息播报给过往车辆，车辆据此计算出本车相对于车道的横向和纵向分米级定位信息，进而完成路径规划。
[0024] RSU除了播报相对位置信息以外，还播报道路结构信息(比如转弯曲率、纵向与横向斜率、车道数量等)，规则信息(比如车道线类型、限速、流向、路权规定、红绿灯状态等)。
[0025] 4.2系统构成，见图24.2.1 RSU分布
在道路节点周围以不大于50米的间距在道路两侧布置一定数量的RSU。RSU由V2X信号发射器和标识照明两个部分组成。
在道路节点周围以不大于50米的间距在道路两侧布置一定数量的RSU。RSU由V2X信号发射器和标识照明两个部分组成。
[0026] 节点包括：道路交叉点、道路结构更改点(车道数量变化、车道类别变化等)、交通规则改变点(比如限速的变化、车道线类别更改)，道路几何变更点(比如曲率的变化、坡度的变化等)。
[0027] 4.2.2 RSU分类信息编码RSU的信息、道路结构和交通规则这三类信息用编码和汉语文本形式向过往车辆播报。
不同位置的RSU播放的信息内容有所不同，所以首先需要根据应用地点的不同对RSU进行类别区分。
不同位置的RSU播放的信息内容有所不同，所以首先需要根据应用地点的不同对RSU进行类别区分。
[0028] RSU一共有7类，其中，城区定位RSU分为行驶路段、灯控交叉路口、无灯路口、环岛等4类，高速公路定位RSU分为行驶路段、入口、出口3类(见表1)。
[0029] 表1全部RSU播报信息由11段内容信息构成，协议见表2
[0030] 表2RSU播报信息内容协议*车道线类别识别码
**交通规则包括：限速、禁左、禁右、单行、让行、前方待驶......，等。
**交通规则包括：限速、禁左、禁右、单行、让行、前方待驶......，等。
[0031] 其中，车道线类别识别码一共3位，第一位代表离RSU最远车道的第1条，其它依次类推。第2位代表车道左线类别，第3位代表车道右线类别。车道线种类规定见表3表3表3车道线类别编码
根据上述协议，本车所处区域也分为相应7类(U1～U4，H1～H3)。
根据上述协议，本车所处区域也分为相应7类(U1～U4，H1～H3)。
[0032] 4.2.3RSU的安装RSU可以装在杆柱上，也可以装在横杆上，可以利用现有的路灯杆、交通灯立柱等原有地面设施。
RSU外壳上应涂有尺寸尽量大的矩形鲜艳彩色标识，RSU同时充当光学定位的靶点，示例见图3。
RSU外壳上应涂有尺寸尽量大的矩形鲜艳彩色标识，RSU同时充当光学定位的靶点，示例见图3。
[0033] 为了便于车载环视图像定位系统对靶点的捕捉，色标的尺寸应当按照同一的标准进行涂装。安装完成后，将色标左下角的精准高度输入到RSU播报信息的第10段。色标需要增加专用的照明灯，以便车载定位相机在夜间光线不好的时候也能捕捉到彩色标识板。
[0034] 路口RSU杆柱的数量不限，位置不限。原则是让个方向的车辆最少能同时看到两个以上的标识。理论上讲，一个标识记即可以提供定位服务，在标识影像消失的时候，车载INS系统也可以在一定范围以内继续提供定位测算。但是在视野范围内增加一个RSU标识会进一步增加系统的安全冗余度。
[0035] 4.2.4车辆光学定位方法PCM可以采用仰角测量和方位角测量两种方法进行本车定位计算。
[0036] 1)俯仰角测量定位法采用俯仰角定位法时，PCC视野内只需要跟踪一对RSU-光学标识组合单元，但是单元的精确安装高度必须为已知。
[0037] 由于彩色图标的标高已知，所以环视图像定位系统能够计算出本车与过RSU垂线在地面垂足点之间的距离。同时由于环视图像定位系统能够直接读取本车与RSU连线与前进方向之间的夹角α，所以能够计算出横向定位参数L与纵向定位参数M，由于S、N、W为已知的RSU广播参数，所以可进而推之本车所在的车道和距离停车线的距离等自动驾驶所需参数，见图4。
[0038] S、W、L、N等标识立柱与道路几何之间的静止关系在布置安装RSU时进行人工测取。当RSU标识布置在横杆上的某一个位置时，则用横杆标识垂线与地面的交点取代标杆立柱的安装点。无论是横杆安装还是竖杆安装，均应按照先安装后测量的方法，用激光垂线仪等工具测出RSU在地面的垂足点F和垂线高度h，定位计算方法见图5。
[0039] 假设车辆沿x轴线方向前进，视野内出现了道路前方右侧的RSU，垂线高度已知为h，PIPC可根据与RSU安装在一起的彩色标识测出RSU的仰角θ，本车与RSU垂足F之间的距离R为：车道横向法线上本车x轴线距RSU垂足F的距离L为：
沿车道纵向上本车y轴距离RSU垂足F的距离为：
根据计算出的L、M，和RSU广播的W、N、S等相对位置参数，本车即可计算出目前本车的中心向与各个车道的中心线横向距离为多少、距前方停车线的纵向距离为多远等各种定位参数。
沿车道纵向上本车y轴距离RSU垂足F的距离为：
根据计算出的L、M，和RSU广播的W、N、S等相对位置参数，本车即可计算出目前本车的中心向与各个车道的中心线横向距离为多少、距前方停车线的纵向距离为多远等各种定位参数。
[0040] 因为环视图像定位相机能够在100米范围内对360°图像进行全景解析，视野没有指向性，所以能捕捉到周边环境内的所有可见RSU，因此还可以利用多个RSU的计算结果进行互相修正。
[0041] 使用环视图像解析相机的另一个必要性是，由于普通相机的视角有限，如果RSU标识涂装位置过高(比如高于10米)，当车辆行驶到杆柱的下方时，定位标识就会从相机视野里消失。环视图像解析相机可以避免光学标识位置过高而消失，也可以防止在横向视野的盲区里消失。
[0042] 当视野内出现多个RSU时，可根据RSU信号源区别出RSU的方位，然后与图像中的RSU像匹配，将每个RSU信号广播内容正确地匹配到图像内的RSU对象上去。
[0043] 2)横向方位角测量定位法方位角测量定位法需要PCC视野之内同时捕捉两个RSU-光学标识组合单元。组合单元安装在道路两侧，其连线与道路轴线垂直，二者精确的水平距离L为已知(见图6)。安装高度不限，对PCM测量单元来讲可以是未知，但是两个组合单元的安装高度需要一致。
方位角定位原理如上图所示。
方位角定位原理如上图所示。
[0044] 车载PCM系统可以精确测得α1、α2，因为L是已知，所以L＝M(tanα1+tanα2)
α1、α2的计算量较小，如果预先建立查找表，把所有的计算都统一成加法，定位信息可以根据α1、α2通过索引的方法获取，那么计算量会进一步降低。如果α1、α2在表格中不存在，则可以采用线性插值法、INS补偿法、卡尔曼滤波等辅助方法计算出其准确位置。这种α角测量方法与PCC在车上安装的高度无关，不论大车小车，都可以采用同样的定位数据表。
α1、α2的计算量较小，如果预先建立查找表，把所有的计算都统一成加法，定位信息可以根据α1、α2通过索引的方法获取，那么计算量会进一步降低。如果α1、α2在表格中不存在，则可以采用线性插值法、INS补偿法、卡尔曼滤波等辅助方法计算出其准确位置。这种α角测量方法与PCC在车上安装的高度无关，不论大车小车，都可以采用同样的定位数据表。
[0045] 由于S、N、W为已知的RSU广播参数，所以根据L2、M可进而推之本车所在的车道和距离停车线的距离等自动驾驶所需参数。
[0046] 3)纵向方位角测量定位法当道路过宽，比如高速公路，横跨公路与道路轴线垂直的成对RSU布置方式不容易实现，或者，在使用的时候容易受视线遮挡，这时单元组可以采用沿道路方向的纵向排列布局，其连线与道路轴线平行，二者精确的水平距离L为已知(见图7)。安装高度不限，对PCM测量单元来讲可以是未知，但是两个组合单元的安装高度需要一致。单元组安装在直线路段。
[0047] 车载PCM系统可以精确测得α’1、α’2，因为L’是已知，所以其余定位方法与横向方位角测量定位法相同。
[0048] 4.2.5定位测量精度的系统保证方法系统精度的保证方法有两方面，一是保证光学测距系统的最低精度要求，二是用惯导单元对光学测量结果进行融合修正，并用数字滤波方法进行测距最优估值并对干扰信号进行过滤，同时补偿由于遮挡、光线不足等因素引起的PCM信号中断和丢失。
[0049] 1)对PCM系统的精度要求俯仰角测量定位法要求PCC相机对30m以外物体的像素分辨率应能达到1m。假设标高为
10m，则相当于要求PCM的标高测量精度误差为10％以内，相应的本机定位误差约为50厘米。
10m，则相当于要求PCM的标高测量精度误差为10％以内，相应的本机定位误差约为50厘米。
[0050] 方位角测量定位法如果采用4K的CMOS感光元件，相当于效利用像素达到3K。如果在路口每30米布置一对RSU，每个像素代表的空间应当是1厘米，考虑到相机成像和图像处理误差，按30个像素代表一个空间单位，即理论的空间定位精度在30厘米。
[0051] 2)PCM测量数据与INS的信息融合本车距标识点的距离x测量可以看成是遵循线性随机差分方程的时间离散量测量：
xk+1|k＝Φk+1|k[xk|k]+wk
zk+1＝Hk+1|k[xk|k]+vk+1 (6)
运动方程中xk+1|k是INS系统根据前一个状态预测出的运动过程状态向量，包含位移、速度、航向角等信息；xk|k是k状态向量的最优结果；Φk+1|k是从前一个离散状态k到随后下一个新状态k+1之间的转移矩阵；wk是转移过程噪声向量，假设为白噪声，均值为0；测量方程中zk+1是PCM系统的测量向量，Hk+1|k是测量矩阵；vk+1是PCM系统的测量过程噪声矩阵，假设为白噪声。
xk+1|k＝Φk+1|k[xk|k]+wk
zk+1＝Hk+1|k[xk|k]+vk+1 (6)
运动方程中xk+1|k是INS系统根据前一个状态预测出的运动过程状态向量，包含位移、速度、航向角等信息；xk|k是k状态向量的最优结果；Φk+1|k是从前一个离散状态k到随后下一个新状态k+1之间的转移矩阵；wk是转移过程噪声向量，假设为白噪声，均值为0；测量方程中zk+1是PCM系统的测量向量，Hk+1|k是测量矩阵；vk+1是PCM系统的测量过程噪声矩阵，假设为白噪声。
[0052] 本系统利用卡尔曼滤波器对INS和PCM的测量结果进行融合，过程如下：首先，利用已知的k-1状态预测出现在的先验状态估计值和先验误差协方差矩阵：
初始化 和Pk-1可以任意给定初值。然后，根据上述INS预测结果，同时再收集现在状态的PCM测量量值，二者结合利用以下三个公式进行测量更新(修正)。
初始化 和Pk-1可以任意给定初值。然后，根据上述INS预测结果，同时再收集现在状态的PCM测量量值，二者结合利用以下三个公式进行测量更新(修正)。
[0053] 计算修正矩阵
[0054] 更新观测值
[0055] 更新误差协方差其中，是滤波器估算出来的状态向量，Pk是滤波器估算的状态协方差矩阵， 是预测的状态协方差矩阵，Q是动态噪声矩阵，K是卡尔曼增益矩阵，z是PCM测量向量，具体见图8。
[0057] 4.3RSU-PCM定位系统在自动驾驶控制中的应用4.3.1城市道路RSU部署(道路节点以4.2.1为准)
RSU只布置在道路节点附近，这样可以节省大量的基础设施投资。道路节点是指：
-道路汇合点
-道路分流点
-车道数量变更点
-车道宽度变更点
-道路交叉点
-曲率变更点
RSU只布置在道路节点附近，这样可以节省大量的基础设施投资。道路节点是指：
-道路汇合点
-道路分流点
-车道数量变更点
-车道宽度变更点
-道路交叉点
-曲率变更点
[0058] 4.3.2节点间均匀城市路段的自动驾驶模式道路节点与节点之间的道路结构是均匀一致化的，比如车道数量是一致的，宽度是一致的，道路曲率是接近的，等等，另外，也没有路权和规则更改。因此，即使没有RSU，车辆也可以在驶过本路段入口处的RSU点以后始终记住自己所处的车道，也可以根据车道导航要求换到理想车道。
[0059] 在节点之间的均匀道路结构上驾驶时，横向定位可依靠现在成熟的视觉LKS系统，可以实现分米级高精定位。纵向定位依靠普通GNSS利用10～30m低精度定位即可。这时的纵向定位只应用于发放换道指令，而不用于停车指令，所以这个精度足够使用。一般性的超车动作靠车载ADAS系统完成。
[0060] 4.3.3驶经中途节点中途节点是指从此点往后的道路结构或者行车规则将要发生变化，但是车辆无需停车，比如车道数目改变、限速改变、即将进入弯道、即将驶入匝道等等。
[0061] 中途节点的RSU-PCM单元只负责播报上述行车参数的更改信息，而不必提供高精定位光学标识功能。过往车辆只是根据RSU广播更新车道结构信息和交通规则信息。
[0062] 4.3.4城市路口节点的自动驾驶进入路口节点区域以后，车辆根据路口RSU更新车道结构信息和交通规则信息，同时切换成高精度纵向定位，必要时进行停车辅助控制。
[0063] 在路口节点内，由于车辆停靠距离较近，车道线被遮挡，LKS系统的视觉横向定位系统有可能断续失效。这时可以依靠RSU提供高精度横向定位。如果前方有车辆减速，本车依靠ADAS的跟停系统(全速ACC系统)实现自动跟车停走操作；如果前方没有其它车辆，本车根据RSU纵向定位系统和信号灯识别系统实现路口自动停车。
[0064] 1)十字路口中央区域路径规划(见图9)路口的RSU布置在入口和出口处每个路端的两侧。双侧的RSU连线为车道中心线的法线方向，比如RSU 1-2线、RSU 5-6线与斜向道路垂直，RSU 3-4线、RSU 7-8线与纵向道路的中心线垂直。每条RSU连线都构成一个虚拟的道路入口“门关”。
[0065] 先以红色车辆通过路口为例，红车(以下称“本车”)站在左转线上欲从纵向道路转往倾斜路段，驶入路口中央区域以后的路径规划过程如下。
[0066] i)本车根据车道导航指令决定转入C1车道还是C2车道；
[0067] ii)如果有转弯路线，本车利用LKS系统跟随车道线前行；
[0068] ii)本车寻找与感知转入道路“入口门关”RSU 1、RSU2位置，在自动驾驶参考坐标系里建立RSU 1-2线段方程；坐标系可以采用相对于地面的静止坐标系，也可以采用相对车身固定的随动坐标系；可以采用笛卡尔坐标，也可以采用极坐标；
[0069] iii)根据已知RSU参数L1、L2，建立车道中心线C1(或C2)的线段方程(方向与RSU1-2连线垂直)
[0070] iv)用最佳转弯半径弧线A1(或A2)连接C1(或C2)和本车转向前的前行中心线，并与二者相切。最佳转弯半径与转弯角速度范围参考下表。如果道路结构要求小于最小转弯半径，则利用轨迹(trajectory)规划按照舒适性原则降低转弯速度；表4
V(mph) f(摩擦系数)下限 转弯半径R(ft)
10 0.38 18
15 0.32 47
20 0.27 99
25 0.22 174
V(mph) f(摩擦系数)下限 转弯半径R(ft)
10 0.38 18
15 0.32 47
20 0.27 99
25 0.22 174
[0071] v)如果没有规则化的弧线可供选用，可使用成熟的常用路径与轨迹规划方法将本车轨迹平滑连接到C1(或C2)上；
[0072] vi)在使用自行规划路径(非转弯车道线跟随模式)时，如果有其它车辆切入本车的规划路径，则根据ADAS设计的安全原则进行避让。
[0073] vii)如果路口划有待驶区域，本车则应该按照待驶车道行驶，并在待驶停车线处停车等待左转灯。待驶区分为直行待驶区和左转待转区两种(图11，图10)。
[0074] RSU通知车辆本车道是否有待转区。如果有待转区，当直行灯变绿以后，本车依靠LKS系统沿待转车道线行驶，捕捉到待转区停车线以后在停在停车线上。
[0075] viii)如果此路口没有左转红绿灯，同时没有待弯区域，以美国加州交规为例，如果有本道路对面相对而来的直行车辆，本车应当沿原来方向行驶至路口中央，然后停止给对方让行，且在此之前不允许向左转动方向盘。本路径规划方法规定，在路口中央让行的停车点为转弯弧线A与本车前行轴线之间的切点T。如果本车选择转向后占用右车道就停在T1点，如果选择做车道就停止在T2点，且在汽车之前不允许转动方向盘。
[0076] 其它地区的具体驾驶行为规划需要符合当地交通法规的规定。
[0077] 2)复杂路口路径规划(见图12)较复杂路口也可以按照上述RSU-环视信息融合原理完成路口通行任务。
[0078] 在图中所示的上海市世纪大道-张扬路-东方路交汇处，需要设置6道“入门关口”。每个RSU都带有道路名称属性，所以车辆转入路口之前需要寻找相应的正确“关口”。比如，从东方路上行左拐进入世纪大道的车辆，需要首先RSU3和RSU4时正确的门关，然后才能进入建立门关线段方程程序。
[0079] 这个左拐进入世纪大道的指令是车道导航地图发出的，因此，车道导航地图需要将转入道路的名称用文本形式发给本定位导航系统。
[0080] 在这种大型路口，极有可能因为距离过远或者建筑、车辆互相遮挡而造成RSU视线丢失。这时可采用两种办法加以补偿和备份。第一是增加标杆高度，比如增加到20米高，这时要求RSU本身发射特殊标高通知，以便本系统对方位计算方法进行修正。第二种方法是用1.5米～1.8米的精度，依靠RFID信号对RSU进行定位(方法见～)。当RSU重新出现在视野以内之后再进行精准定位，修正轨迹。这个逐渐精细化修正过程也是和人类驾驶习惯相一致的。
[0081] 使用高精度的惯导单元也可以补偿地标信号的暂时缺失。
[0082] RSU的方向可以防止本车将行进路线规划到逆行一侧。
[0084] 4.3.5圆形环岛自动驾驶绕行路径规划圆形环岛是一种大型的综合节点。典型结构见图13，环岛中心采用U4类RSU，其余类别均为U2。
[0085] 环岛中心的RSU需要表明自己是环岛中心U4类RSU。环岛中心RSU的数据格式与路边RSU有所不同，内容包括：-环岛车道数量
[0086] 每条车道中心线相对于环岛中心的半径，比如R1、R2
[0087] U4类RSU定位只能采用俯仰角测量定位法。环岛驾驶的路径规划过程如下。
[0088] i)以V1为例，本车根据车道导航指令决定转入Ri车道(i＝1～n)；
[0089] ii)如果有弯道车道线，本车利用LKS系统跟随车道线前行；
[0090] iii)感知环岛中心RSU(类别U4)，计算其位置，在自动驾驶参考坐标系里建立Ri车道圆方程；坐标系可以采用相对于地面的静止坐标系，也可以采用相对车身固定的随动坐标系；可以采用笛卡尔坐标，也可以采用极坐标；
[0091] iv)用最佳转弯半径弧线Ai连接Ri和本车入岛前的前行中心线，并与二者相切。最佳转弯半径与转弯角速度范围参考下表。如果道路结构要求小于最小转弯半径，则利用轨迹(trajectory)规划按照舒适性原则降低转弯速度；
[0092] 表5V(mph) f(摩擦系数)下限 转弯半径R(ft)
10 0.38 18
15 0.32 47
20 0.27 99
25 0.22 174
10 0.38 18
15 0.32 47
20 0.27 99
25 0.22 174
[0093] v)进入环路以后，如果车道清晰，应当利用LKS视觉系统或着激光雷达系统进行循线驾驶。如果车道线不清晰，则根据Ri半径虚拟车道中心线进行循线路径规划；
[0094] vi)以V2为例，驶入环岛以后，根据路名文本寻找出口“门关”，然后用最佳转弯半径弧线Ai连接Ri和门关的垂直线L，并与二者相切。
[0095] vii)如果没有规则化的弧线可供选用，可使用成熟的常用路径与轨迹规划方法将本车轨迹平滑连接到L上；
[0096] viii)在使用自行规划路径(非转弯车道线跟随模式)时，如果有其它车辆切入本车的规划路径，则根据ADAS设计的安全原则进行避让；
[0097] ix)驶出关口以后转入到直线路段的定位与导航模式。
[0098] 城市道路依靠RSU-PCM的自动驾驶过程如图1所示。
[0099] 4.3.7高速公路驾驶高速公路自动驾驶任务可由4项最基本的行驶状态组合而成，分别为：
[0100] 1)恒态路段高速自动巡航状态：路段没有影响安全的结构、几何改变，也没有行车规则改变。车辆的自动驾驶任务包括：定速巡航、自适应巡航、自动超车、自动换线、自动避险刹车等功能；
[0101] 2)上路匝道汇流状态；
[0102] 3)下路匝道分流状态；
[0103] 4)状态转移：按照新的规则行驶(比如新的速度限制等)，或者考虑新的道路特征因素(比如转弯曲率等)
[0104] 4种行驶状态所需要的感知信息如下：表6 恒态路段高速自动巡航状态
[0105] 表7 上路匝道汇流状态
[0106] 表8 下路匝道分流状态高速公路上按照表6～8，根据功能的不同需要，在不同的路段分别部署H1～H3类别的RSU-PCM单元。
[0107] 发明的效果：本发明可以解决目前ADAS系统所解决不了的纵向精准定位问题、规则感知问题、路口中央区域路径规划问题。
如果在道路节点上适当布置本专利定位技术，在现有的ADAS技术基础之上，不依靠GNSS信号和高精地图，在高速公路和城市街道都能逐步实现自动驾驶功能，可以解决GNSS信号漂移机信号丢失、卫星地面基站成本过高、高精地图合规性、激光三维点云定位成本过高等目前自动驾驶面临的障碍。
本发明技术需要配合车道导航地图一起使用。车道导航地图不在本专利叙述范围之内。车道导航信息只要在现有地图基础上加以标注车道数量就可以，不用重新扫描街道。其次，需要将道路名称输出给本定位系统，然后本系统根据RSU信号在复杂路口寻找正确的“门关”。
如果在道路节点上适当布置本专利定位技术，在现有的ADAS技术基础之上，不依靠GNSS信号和高精地图，在高速公路和城市街道都能逐步实现自动驾驶功能，可以解决GNSS信号漂移机信号丢失、卫星地面基站成本过高、高精地图合规性、激光三维点云定位成本过高等目前自动驾驶面临的障碍。
本发明技术需要配合车道导航地图一起使用。车道导航地图不在本专利叙述范围之内。车道导航信息只要在现有地图基础上加以标注车道数量就可以，不用重新扫描街道。其次，需要将道路名称输出给本定位系统，然后本系统根据RSU信号在复杂路口寻找正确的“门关”。
[0108] ADAS：先进辅助驾驶系统(AdvancedDrivingAssistantSystem)DGPS：GPS差分定位系统
GNSS：全球卫星导航系统(Global Navigation Satellite System)
INS：惯性导航系统(InertialNavigationSystem)
LKS：道路保持系统(LaneKeepingSystem)
PCC：全景反折式相机(PanoramaCatadioptricCamera)
PCM：全景反折光学测量(PanoramaCatadioptricMeasurement)
RSU：路侧单元(RoadSideUnit)。
GNSS：全球卫星导航系统(Global Navigation Satellite System)
INS：惯性导航系统(InertialNavigationSystem)
LKS：道路保持系统(LaneKeepingSystem)
PCC：全景反折式相机(PanoramaCatadioptricCamera)
PCM：全景反折光学测量(PanoramaCatadioptricMeasurement)
RSU：路侧单元(RoadSideUnit)。
| 标题 | 发布/更新时间 | 阅读量 |
|---|---|---|
| 一种红外探测器的驱动电路及红外探测器系统 | 2020-05-08 | 577 |
| 视频编码装置 | 2020-05-08 | 863 |
| 视频转码方法、装置、计算机设备和存储介质 | 2020-05-08 | 673 |
| 基于自同步混沌密码的铁路应答器报文安全增强方法 | 2020-05-08 | 109 |
| 并联式混动汽车混合模式扭矩信号分解及扭矩分配方法 | 2020-05-08 | 25 |
| 一种基于PSD的信号处理方法 | 2020-05-08 | 725 |
| 集成三态电磁干扰滤波和线路调节模块 | 2020-05-08 | 451 |
| 一种水下鱼类图像的超分辨率重建方法、系统及装置 | 2020-05-08 | 777 |
| 一种基于卷积神经网络的泥浆脉冲信号识别方法 | 2020-05-08 | 789 |
| 一种指纹图像增强方法 | 2020-05-08 | 288 |
高效检索全球专利专利汇是专利免费检索,专利查询,专利分析-国家发明专利查询检索分析平台，是提供专利分析，专利查询，专利检索等数据服务功能的知识产权数据服务商。
我们的产品包含105个国家的1.26亿组数据，免费查、免费专利分析。
分析报告
专利汇分析报告产品可以对行业情报数据进行梳理分析，涉及维度包括行业专利基本状况分析、地域分析、技术分析、发明人分析、申请人分析、专利权人分析、失效分析、核心专利分析、法律分析、研发重点分析、企业专利处境分析、技术处境分析、专利寿命分析、企业定位分析、引证分析等超过60个分析角度，系统通过AI智能系统对图表进行解读，只需1分钟，一键生成行业专利分析报告。
QQ群二维码
意见反馈
意见反馈