自动驾驶高精定位与路径规划方法.pdf

《自动驾驶高精定位与路径规划方法.pdf》由会员分享，可在线阅读，更多相关《自动驾驶高精定位与路径规划方法.pdf（36页完成版）》请在专利查询网上搜索。

1、(19)中华人民共和国国家知识产权局 (12)发明专利申请 (10)申请公布号 (43)申请公布日 (21)申请号 201911027832.1 (22)申请日 2019.10.28 (71)申请人 张少军 地址 130033 吉林省长春市南关区净月街 道力旺委124组 (72)发明人 邱少波张少军李骏程洪 (74)专利代理机构 吉林长春新纪元专利代理有 限责任公司 22100 代理人 白冬冬 (51)Int.Cl. G01C 21/28(2006.01) G01C 21/34(2006.01) (54)发明名称 自动驾驶高精定位与路径规划方法 (57)摘要 一种自动驾驶高精定位与路径规划方法。

2、， 属 于智能交通技术领域。 本发明的目的是依靠观察 车与静止障碍物之间的相对距离， 将路侧设施与 环视图像信息有效融合的自动驾驶高精定位与 路径规划方法。 本发明包括以下步骤： 在道路节 点周围的道路两侧布置RSU， RSU分类信息编码， RSU的安装， 车辆光学定位。 本发明可以解决目前 ADAS系统所解决不了的纵向精准定位问题、 规则 感知问题、 路口中央区域路径规划问题。 权利要求书5页 说明书17页 附图13页 CN 111076731 A 2020.04.28 CN 111076731 A 1.一种自动驾驶高精定位与路径规划方法， 其特征在于： 包括以下步骤： 步骤S1： 在道路节。

3、点周围的道路两侧布置RSU， RSU由V2X信号发射器和标识照明两个部 分组成； 步骤S2： RSU分类信息编码， RSU一共有7类， 其中， 城区定位RSU分为行驶路段、 灯控交叉 路口、 无灯路口、 环岛等4类， 高速公路定位RSU分为行驶路段、 入口、 出口3类； 步骤S3： RSU的安装， RSU装在杆柱上、 装在横杆上、 利用现有的路灯杆或交通灯立柱原 有地面设施； RSU外壳上应涂有矩形鲜艳彩色标识， RSU同时充当光学定位的靶点； 步骤S4： 车辆光学定位方法， 即PCM光学测距， 用仰角测量和方位角测量两种方法进行 本车定位测量。 2.根据权利要求1所述的自动驾驶高精定位与路径。

4、规划方法， 其特征在于： 步骤S1中的 道路节点包括： 道路交叉点、 道路结构更改点、 交通规则改变点、 道路几何变更点。 3.根据权利要求1所述的自动驾驶高精定位与路径规划方法， 其特征在于： 步骤S2中7 类类别区分： 表1类别编号 全部RSU播报信息由11段内容信息构成， 见表2RSU播报信息内容协议 权利要求书 1/5 页 2 CN 111076731 A 2 其中， 车道线类别识别码一共3位， 第一位代表离RSU最远车道的第1条， 其它依次类推； 第2位代表车道左线类别， 第3位代表车道右线类别； 车道线种类规定见表3 表3 车道线类别编码 权利要求书 2/5 页 3 CN 1110。

5、76731 A 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相对位置参数， 本车即可计算出目前本车的中 心向与各个车道的。

6、中心线横向距离为多少、 距前方停车线的纵向距离为多远等各种定位参 数； 2)横向方位角测量定位法 车载PCM系统可以精确测得 1、 2， 因为L是已知， 所以 权利要求书 3/5 页 4 CN 111076731 A 4 由于S、 N、 W为已知的RSU广播参数， 所以根据L2、 M可进而推之本车所在的车道和距离停 车线的距离等自动驾驶所需参数； 3)纵向方位角测量定位法 车载PCM系统可以精确测得 1、 2， 因为L 是已知， 所以 其余定位方法与横向方位角测量定位法相同。 5.根据权利要求4所述的自动驾驶高精定位与路径规划方法， 其特征在于： 因为环视图 像定位相机能够在100米范围内对3。

7、60 图像进行全景解析， 视野没有指向性， 所以能捕捉到 周边环境内的所有可见RSU， 因此还可以利用多个RSU的计算结果进行互相修正。 6.根据权利要求1所述的自动驾驶高精定位与路径规划方法， 其特征在于： 定位测量的 精度的系统保证方法： 1)对PCM系统的精度要求 俯仰角测量定位法要求PCC相机对30m以外物体的像素分辨率应能达到1m； 假设标高为 10m， 则相当于要求PCM的标高测量精度误差为10以内， 相应的本机定位误差约为50厘米； 2)PCM测量数据与INS的信息融合 本车距标识点的距离x测量看成是遵循线性随机差分方程的时间离散量测量： 运动方程中xk+1|k是INS系统根据前。

8、一个状态预测出的运动过程状态向量， xk|k是k状态 向量的最优结果； k+1|k是从前一个离散状态k到随后下一个新状态k+1之间的转移矩阵； wk 是转移过程噪声向量， 测量方程中zk+1是PCM系统的测量向量， Hk+1|k是测量矩阵； vk+1是PCM系 统的测量过程噪声矩阵； 利用卡尔曼滤波器对INS和PCM的测量结果进行融合， 过程如下： (1)利用已知的k-1状态预测出现在的先验状态估计值和先验误差协方差矩阵： 权利要求书 4/5 页 5 CN 111076731 A 5 初始化和Pk-1可以任意给定初值； 然后， 根据上述INS预测结果， 同时再收集现在状 态的PCM测量量值， 。

9、二者结合利用以下三个公式进行测量更新； (2)修正矩阵 (3)更新观测值 (4)更新误差协方差 其中，是滤波器估算出来的状态向量， Pk是滤波器估算的状态协方差矩阵， 是预测 的状态协方差矩阵， Q是动态噪声矩阵， K是卡尔曼增益矩阵， z是PCM测量向量。 7.根据权利要求1所述的自动驾驶高精定位与路径规划方法， 其特征在于： RSU-PCM定 位系统在自动驾驶控制中的应用； 城市道路RSU部署： RSU只布置在道路节点附近， 道路节点是指： 道路交叉点、 道路结 构更改点、 交通规则改变点、 道路几何变更点， 其中道路结构更改点包括车道数量变化、 车 道类别变化； 交通规则改变点包括限速的。

10、变化、 车道线类别更改； 道路几何变更点包括曲率 的变化、 坡度的变化； 节点间均匀城市路段的自动驾驶模式： 道路节点与节点之间的道路结构是均匀一致 化的， 即使没有RSU， 车辆也能在驶过本路段入口处的RSU点以后始终记住自己所处的车道， 也能根据车道导航要求换到理想车道； 驶经中途节点： 中途节点的RSU-PCM单元只负责播报上述行车参数的更改信息， 而不 必提供高精定位光学标识功能； 城市路口节点的自动驾驶： 1)十字路口中央区域路径规划： 路口的RSU布置在入口和出口处每个路端的两侧， 双侧 的RSU连线为车道中心线的法线方向， 每条RSU连线都构成一个虚拟的道路入口 “门关” ； 2。

11、)复杂路口路径规划： 较复杂路口也按照RSU-环视信息融合原理完成路口通行任务； 圆形环岛自动驾驶绕行路径规划： 圆形环岛是一种大型的综合节点， 环岛中心的RSU 需要表明自己是环岛中心U4类RSU， 内容包括： 环岛车道数量、 每条车道中心线相对于环岛 中心的半径； 高速公路驾驶： 高速公路自动驾驶任务可由4项最基本的行驶状态组合而成， 分别 为： 恒态路段高速自动巡航状态、 上路匝道汇流状态、 下路匝道分流状态、 状态转移。 权利要求书 5/5 页 6 CN 111076731 A 6 自动驾驶高精定位与路径规划方法 技术领域 0001 本发明属于智能交通技术领域。 背景技术 0002 像。

12、人类驾驶一样， 自动驾驶系统并不是时时刻刻都需要知道自己的精准位置坐 标， 而是只需要知道自己与静止边界、 移动障碍之间的相对位置。 0003 移动障碍的探测主要依靠激光雷达、 毫米波雷达、 超声雷达、 摄像头等车载传感器 进行探测， 目前技术已经成熟到大批量产品应用的阶段。 相比之下， 对静止边界的精准感知 目前还面临巨大的挑战。 对静止边界感知的内容包括： 行驶区域边界、 交通规则、 信息标识、 道路几何特性(形态、 横向/纵向坡度)等。 0004 静态障碍的探测方法包括激光雷达、 高精地图等技术， 其中高精地图+基于卫星基 站的高精定位是保证自动驾驶车辆大批量投放最有潜力的解决方案， 但。

13、是目前面临着几大 挑战。 一是某些高精地图的内容与国家法规要求不相符合， 其次地图扫描与更新成本很高， 同时卫星信号地面接收基站布置成本很高， 另外还有大面积的卫星信号丢失问题(比如在 涵洞内和城市高楼峡谷之间)， 目前都没有找到理想的解决方案。 0005 为了摆脱对卫星定位系统的依赖， 目前也有很多根据路侧单元定位的研究， 主要 是是依靠信号接收时间、 多RSU之间的信号时间A Elmarady,2013A Khattab,2015、 信 号接收强度等原理A Shojaifar,2015， 存在的问题是定位精度最高只能达到1.5m左右， 用于车道导航还勉强， 但是用于自动驾驶控制与避撞的精度。

14、还远远不够C Ou,2019,J Xu,2012,M Aatique,1997,R Reza,2000,S Chaabene,2019。 0006 自动驾驶需要依靠高精定位系统解决如下六大问题： -需要了解身处哪个车道和需要换到哪个车道(车道导航问题) -需要知道距离车道边缘的距离(横向定位问题) -需要知道停车线的位置(纵向定位) -相对位置精度小于30cm(定位高精问题) -需要知道路口中央区域没有车道线时的行驶路线(路口规划问题) -需要了解交通规则(规则感知问题)。 发明内容 0007 本发明的目的是依靠观察车与静止障碍物之间的相对距离， 将路侧设施与环视图 像信息有效融合的自动驾驶高。

15、精定位与路径规划方法。 0008 本发明包括以下步骤： 步骤S1： 在道路节点周围的道路两侧布置RSU， RSU由V2X信号发射器和标识照明两个部 分组成； 步骤S2： RSU分类信息编码， RSU一共有7类， 其中， 城区定位RSU分为行驶路段、 灯控交叉 路口、 无灯路口、 环岛等4类， 高速公路定位RSU分为行驶路段、 入口、 出口3类； 说明书 1/17 页 7 CN 111076731 A 7 步骤S3： RSU的安装， RSU装在杆柱上、 装在横杆上、 利用现有的路灯杆或交通灯立柱原 有地面设施； RSU外壳上应涂有矩形鲜艳彩色标识， RSU同时充当光学定位的靶点； 步骤S4： 车。

16、辆光学定位方法， 即PCM光学测距， 用仰角测量和方位角测量两种方法进行 本车定位测量。 0009 本发明步骤S1中的道路节点包括： 道路交叉点、 道路结构更改点、 交通规则改变 点、 道路几何变更点。 0010 本发明步骤S2中7类类别区分： 表1类别编号 全部RSU播报信息由11段内容信息构成， 见表2RSU播报信息内容协议 说明书 2/17 页 8 CN 111076731 A 8 其中， 车道线类别识别码一共3位， 第一位代表离RSU最远车道的第1条， 其它依次类推； 第2位代表车道左线类别， 第3位代表车道右线类别； 车道线种类规定见表3 表3车道线类别编码 0011 本发明步骤S4。

17、中仰角测量和方位角测量两种方法： 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)横向方位角测量定位法 车载。

18、PCM系统可以精确测得 1、 2， 因为L是已知， 所以 说明书 3/17 页 9 CN 111076731 A 9 由于S、 N、 W为已知的RSU广播参数， 所以根据L2、 M可进而推之本车所在的车道和距离停 车线的距离等自动驾驶所需参数； 3)纵向方位角测量定位法 车载PCM系统可以精确测得 1、 2， 因为L 是已知， 所以 其余定位方法与横向方位角测量定位法相同。 0012 本发明因为环视图像定位相机能够在100米范围内对360 图像进行全景解析， 视 野没有指向性， 所以能捕捉到周边环境内的所有可见RSU， 因此还可以利用多个RSU的计算 结果进行互相修正。 0013 本发明定位测。

19、量的精度的系统保证方法： 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系统的测量向量， H。

20、k+1|k是测量矩阵； vk+1是PCM系 统的测量过程噪声矩阵； 利用卡尔曼滤波器对INS和PCM的测量结果进行融合， 过程如下： 利用已知的k-1状态预测出现在的先验状态估计值和先验误差协方差矩阵： 说明书 4/17 页 10 CN 111076731 A 10 初始化和Pk-1可以任意给定初值； 然后， 根据上述INS预测结果， 同时再收集现在状 态的PCM测量量值， 二者结合利用以下三个公式进行测量更新； 修正矩阵 更新观测值 更新误差协方差 其中，是滤波器估算出来的状态向量， Pk是滤波器估算的状态协方差矩阵， 是预测 的状态协方差矩阵， Q是动态噪声矩阵， K是卡尔曼增益矩阵， z。

21、是PCM测量向量。 0014 本发明RSU-PCM定位系统在自动驾驶控制中的应用； 城市道路RSU部署： RSU只布置在道路节点附近， 道路节点是指： 道路交叉点、 道路结 构更改点、 交通规则改变点、 道路几何变更点， 其中道路结构更改点包括车道数量变化、 车 道类别变化； 交通规则改变点包括限速的变化、 车道线类别更改； 道路几何变更点包括曲率 的变化、 坡度的变化； 节点间均匀城市路段的自动驾驶模式： 道路节点与节点之间的道路结构是均匀一致 化的， 即使没有RSU， 车辆也能在驶过本路段入口处的RSU点以后始终记住自己所处的车道， 也能根据车道导航要求换到理想车道； 驶经中途节点： 中途。

22、节点的RSU-PCM单元只负责播报上述行车参数的更改信息， 而不 必提供高精定位光学标识功能； 城市路口节点的自动驾驶： 1)十字路口中央区域路径规划： 路口的RSU布置在入口和出口处每个路端的两侧， 双侧 的RSU连线为车道中心线的法线方向， 每条RSU连线都构成一个虚拟的道路入口 “门关” ； 2)复杂路口路径规划： 较复杂路口也按照RSU-环视信息融合原理完成路口通行任务； 圆形环岛自动驾驶绕行路径规划： 圆形环岛是一种大型的综合节点， 环岛中心的RSU 需要表明自己是环岛中心U4类RSU， 内容包括： 环岛车道数量、 每条车道中心线相对于环岛 中心的半径； 高速公路驾驶： 高速公路自动。

23、驾驶任务可由4项最基本的行驶状态组合而成， 分别 为： 恒态路段高速自动巡航状态、 上路匝道汇流状态、 下路匝道分流状态、 状态转移。 本系统像人类驾驶一样， 只靠观察本车与静止障碍物之间的相对距离， 而不关心障碍 物的绝对位置坐标， 不用依赖高精卫星定位和高精地图系统， 为解决上述自动驾驶 “六大问 题” 提供一种技术途径。 说明书 5/17 页 11 CN 111076731 A 11 0015 本发明依靠路侧单元、 环视相机图像分析技术， 不依靠激光雷达、 高精卫星定位地 面设施(比如DGPS基站、 RTK基站)和自动驾驶高精地图， 无需对车载感知硬件与算法提出过 高的计算要求， 即可实。

24、现城区道路自动驾驶的分米级高精定位。 本系统仍然需要配合使用 普通精度的卫星导航与定位系统。 0016 本发明中路侧单元RSU是指车辆互联系统中的地面通讯单元， 安装在路侧， 可以与 车载单元(OBU)进行通讯， 实现车辆身份识别、 电子扣分、 自动缴费、 信息播报等功能。 附图说明 0017 图1是本发明城市道路依靠RSU-PCM的自动驾驶过程图； 图2是本发明系统构成图； 图3是本发明RSU外壳上涂色范围示例图； 图4是本发明俯仰角测量定位法中车与RSU示例图； 图5是本发明定位计算方法图； 图6是本发明横向方位角测量定位法中车与RSU示例图； 图7是本发明纵向方位角测量定位法中车与RSU。

25、示例图； 图8是本发明定位测量精度的系统路线图； 图9是本发明城市路口节点的自动驾驶中十字路口中央区域路径规划图； 图10是具有左转待转区域十字路口图； 图11是具有直行待转区域十字路口图； 图12是本发明复杂路口路径规划图， RSU1-12类别U2； 图13是本发明复杂路口路径规划图， 环岛中心采用U4类RSU， 其余类别均为U2； 图14是本发明图13的局部放大图。 具体是实施方式 0018 本发明 “全景反折光学测量” (PCM)系统是指， 利用反折射式360 环视相机(PCC)进 行无缝的全景图像感知， 同时可对变形的图像进行全方位信息还原， 满足分米级高精的的 内容识别和测量要求， 。

26、相关专利见发明人张少军于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)进行通 讯， 实现。

27、车辆身份识别， 电子扣分的装置。 0020 4.1本发明原理 如果地面上一个固定的静止设施与道路的相对位置是已知的， 并且本车可以随时测量 出这个固定设施与本车之间的相对方位， 那么通过这个静止设施， 就可以计算出本车和道 路之间的相对位置关系， 而不需要通过本车位置和道路几何的绝对位置坐标来计算。 0021 本系统把RSU当作上述固定的静止设备。 RSU与道路要素之间的相对位置(例如距 车道线的横向距离、 距停车线的纵向距离等)在安装RSU的安装施工过程中进行厘米级别精 度测量。 说明书 6/17 页 12 CN 111076731 A 12 0022 本系统用环视图像定位技术对RSU装置相。

28、对于本车的距离和方位角进行分米精度 的实时测量。 0023 RSU将本单元的相对位置信息播报给过往车辆， 车辆据此计算出本车相对于车道 的横向和纵向分米级定位信息， 进而完成路径规划。 0024 RSU除了播报相对位置信息以外， 还播报道路结构信息(比如转弯曲率、 纵向与横 向斜率、 车道数量等)， 规则信息(比如车道线类型、 限速、 流向、 路权规定、 红绿灯状态等)。 0025 4.2系统构成， 见图2 4.2.1 RSU分布 在道路节点周围以不大于50米的间距在道路两侧布置一定数量的RSU。 RSU由V2X信号 发射器和标识照明两个部分组成。 0026 节点包括： 道路交叉点、 道路结构。

29、更改点(车道数量变化、 车道类别变化等)、 交通 规则改变点(比如限速的变化、 车道线类别更改)， 道路几何变更点(比如曲率的变化、 坡度 的变化等)。 0027 4.2.2 RSU分类信息编码 RSU的信息、 道路结构和交通规则这三类信息用编码和汉语文本形式向过往车辆播报。 不同位置的RSU播放的信息内容有所不同， 所以首先需要根据应用地点的不同对RSU进行类 别区分。 0028 RSU一共有7类， 其中， 城区定位RSU分为行驶路段、 灯控交叉路口、 无灯路口、 环岛 等4类， 高速公路定位RSU分为行驶路段、 入口、 出口3类(见表1)。 0029 表1 全部RSU播报信息由11段内容信。

30、息构成， 协议见表2 0030 表2RSU播报信息内容协议 说明书 7/17 页 13 CN 111076731 A 13 *车道线类别识别码 *交通规则包括： 限速、 禁左、 禁右、 单行、 让行、 前方待驶.， 等。 0031 其中， 车道线类别识别码一共3位， 第一位代表离RSU最远车道的第1条， 其它依次 类推。 第2位代表车道左线类别， 第3位代表车道右线类别。 车道线种类规定见表3 表3 说明书 8/17 页 14 CN 111076731 A 14 表3车道线类别编码 根据上述协议， 本车所处区域也分为相应7类(U1U4， H1H3)。 0032 4.2.3RSU的安装 RSU可。

31、以装在杆柱上， 也可以装在横杆上， 可以利用现有的路灯杆、 交通灯立柱等原有 地面设施。 RSU外壳上应涂有尺寸尽量大的矩形鲜艳彩色标识， RSU同时充当光学定位的靶点， 示 例见图3。 0033 为了便于车载环视图像定位系统对靶点的捕捉， 色标的尺寸应当按照同一的标准 进行涂装。 安装完成后， 将色标左下角的精准高度输入到RSU播报信息的第10段。 色标需要 增加专用的照明灯， 以便车载定位相机在夜间光线不好的时候也能捕捉到彩色标识板。 0034 路口RSU杆柱的数量不限， 位置不限。 原则是让个方向的车辆最少能同时看到两个 以上的标识。 理论上讲， 一个标识记即可以提供定位服务， 在标识影。

32、像消失的时候， 车载INS 系统也可以在一定范围以内继续提供定位测算。 但是在视野范围内增加一个RSU标识会进 一步增加系统的安全冗余度。 0035 4.2.4车辆光学定位方法 PCM可以采用仰角测量和方位角测量两种方法进行本车定位计算。 0036 1)俯仰角测量定位法 采用俯仰角定位法时， PCC视野内只需要跟踪一对RSU-光学标识组合单元， 但是单元的 说明书 9/17 页 15 CN 111076731 A 15 精确安装高度必须为已知。 0037 由于彩色图标的标高已知， 所以环视图像定位系统能够计算出本车与过RSU垂线 在地面垂足点之间的距离。 同时由于环视图像定位系统能够直接读取本。

33、车与RSU连线与前 进方向之间的夹角 ， 所以能够计算出横向定位参数L与纵向定位参数M， 由于S、 N、 W为已知 的RSU广播参数， 所以可进而推之本车所在的车道和距离停车线的距离等自动驾驶所需参 数， 见图4。 0038 S、 W、 L、 N等标识立柱与道路几何之间的静止关系在布置安装RSU时进行人工测取。 当RSU标识布置在横杆上的某一个位置时， 则用横杆标识垂线与地面的交点取代标杆立柱 的安装点。 无论是横杆安装还是竖杆安装， 均应按照先安装后测量的方法， 用激光垂线仪等 工具测出RSU在地面的垂足点F和垂线高度h， 定位计算方法见图5。 0039 假设车辆沿x轴线方向前进， 视野内出。

34、现了道路前方右侧的RSU， 垂线高度已知为 h， PIPC可根据与RSU安装在一起的彩色标识测出RSU的仰角 ， 本车与RSU垂足F之间的距离R 为： 车道横向法线上本车x轴线距RSU垂足F的距离L为： 沿车道纵向上本车y轴距离RSU垂足F的距离为： 根据计算出的L、 M， 和RSU广播的W、 N、 S等相对位置参数， 本车即可计算出目前本车的中 心向与各个车道的中心线横向距离为多少、 距前方停车线的纵向距离为多远等各种定位参 数。 0040 因为环视图像定位相机能够在100米范围内对360 图像进行全景解析， 视野没有 指向性， 所以能捕捉到周边环境内的所有可见RSU， 因此还可以利用多个R。

35、SU的计算结果进 行互相修正。 0041 使用环视图像解析相机的另一个必要性是， 由于普通相机的视角有限， 如果RSU标 识涂装位置过高(比如高于10米)， 当车辆行驶到杆柱的下方时， 定位标识就会从相机视野 里消失。 环视图像解析相机可以避免光学标识位置过高而消失， 也可以防止在横向视野的 盲区里消失。 0042 当视野内出现多个RSU时， 可根据RSU信号源区别出RSU的方位， 然后与图像中的 RSU像匹配， 将每个RSU信号广播内容正确地匹配到图像内的RSU对象上去。 0043 2)横向方位角测量定位法 方位角测量定位法需要PCC视野之内同时捕捉两个RSU-光学标识组合单元。 组合单元 。

36、安装在道路两侧， 其连线与道路轴线垂直， 二者精确的水平距离L为已知(见图6)。 安装高度 不限， 对PCM测量单元来讲可以是未知， 但是两个组合单元的安装高度需要一致。 方位角定位原理如上图所示。 0044 车载PCM系统可以精确测得 1、 2， 因为L是已知， 所以 说明书 10/17 页 16 CN 111076731 A 16 LM(tan 1+tan 2) 1、 2的计算量较小， 如果预先建立查找表， 把所有的计算都统一成加法， 定位信息可 以根据 1、 2通过索引的方法获取， 那么计算量会进一步降低。 如果 1、 2在表格中不存在， 则可以采用线性插值法、 INS补偿法、 卡尔曼滤。

37、波等辅助方法计算出其准确位置。 这种 角测 量方法与PCC在车上安装的高度无关， 不论大车小车， 都可以采用同样的定位数据表。 0045 由于S、 N、 W为已知的RSU广播参数， 所以根据L2、 M可进而推之本车所在的车道和距 离停车线的距离等自动驾驶所需参数。 0046 3)纵向方位角测量定位法当道路过宽， 比如高速公路， 横跨公路与道路轴线垂直 的成对RSU布置方式不容易实现， 或者， 在使用的时候容易受视线遮挡， 这时单元组可以采 用沿道路方向的纵向排列布局， 其连线与道路轴线平行， 二者精确的水平距离L为已知(见 图7)。 安装高度不限， 对PCM测量单元来讲可以是未知， 但是两个组。

38、合单元的安装高度需要 一致。 单元组安装在直线路段。 0047 车载PCM系统可以精确测得 1、 2， 因为L 是已知， 所以 其余定位方法与横向方位角测量定位法相同。 0048 4.2.5定位测量精度的系统保证方法 系统精度的保证方法有两方面， 一是保证光学测距系统的最低精度要求， 二是用惯导 单元对光学测量结果进行融合修正， 并用数字滤波方法进行测距最优估值并对干扰信号进 行过滤， 同时补偿由于遮挡、 光线不足等因素引起的PCM信号中断和丢失。 0049 1)对PCM系统的精度要求 俯仰角测量定位法要求PCC相机对30m以外物体的像素分辨率应能达到1m。 假设标高为 10m， 则相当于要求。

39、PCM的标高测量精度误差为10以内， 相应的本机定位误差约为50厘米。 0050 方位角测量定位法如果采用4K的CMOS感光元件， 相当于效利用像素达到3K。 如果 在路口每30米布置一对RSU， 每个像素代表的空间应当是1厘米， 考虑到相机成像和图像处 理误差， 按30个像素代表一个空间单位， 即理论的空间定位精度在30厘米。 0051 2)PCM测量数据与INS的信息融合 本车距标识点的距离x测量可以看成是遵循线性随机差分方程的时间离散量测量： xk+1|kk+1|kxk|k+wk 说明书 11/17 页 17 CN 111076731 A 17 zk+1Hk+1|kxk|k+vk+1 (。

40、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可以任意给定。

41、初值。 然后， 根据上述INS预测结果， 同时再收集现在状 态的PCM测量量值， 二者结合利用以下三个公式进行测量更新(修正)。 0053 计算修正矩阵 0054 更新观测值 0055 更新误差协方差 其中，是滤波器估算出来的状态向量， Pk是滤波器估算的状态协方差矩阵， 是预测 的状态协方差矩阵， Q是动态噪声矩阵， K是卡尔曼增益矩阵， z是PCM测量向量， 具体见图8。 0056 当下一个数据采集周期到来的时候重复上述过程。 本系统规定INS单元工作频率 为100Hz， 零漂精度10 /hr， PCM系统工作频率为10Hz。 0057 4.3RSU-PCM定位系统在自动驾驶控制中的应用 。

42、4.3.1城市道路RSU部署(道路节点以4.2.1为准) RSU只布置在道路节点附近， 这样可以节省大量的基础设施投资。 道路节点是指： -道路汇合点 -道路分流点 -车道数量变更点 -车道宽度变更点 -道路交叉点 -曲率变更点 0058 4.3.2节点间均匀城市路段的自动驾驶模式 道路节点与节点之间的道路结构是均匀一致化的， 比如车道数量是一致的， 宽度是一 致的， 道路曲率是接近的， 等等， 另外， 也没有路权和规则更改。 因此， 即使没有RSU， 车辆也 可以在驶过本路段入口处的RSU点以后始终记住自己所处的车道， 也可以根据车道导航要 求换到理想车道。 说明书 12/17 页 18 C。

43、N 111076731 A 18 0059 在节点之间的均匀道路结构上驾驶时， 横向定位可依靠现在成熟的视觉LKS系统， 可以实现分米级高精定位。 纵向定位依靠普通GNSS利用1030m低精度定位即可。 这时的纵 向定位只应用于发放换道指令， 而不用于停车指令， 所以这个精度足够使用。 一般性的超车 动作靠车载ADAS系统完成。 0060 4.3.3驶经中途节点中途节点是指从此点往后的道路结构或者行车规则将要发生 变化， 但是车辆无需停车， 比如车道数目改变、 限速改变、 即将进入弯道、 即将驶入匝道等 等。 0061 中途节点的RSU-PCM单元只负责播报上述行车参数的更改信息， 而不必提供。

44、高精 定位光学标识功能。 过往车辆只是根据RSU广播更新车道结构信息和交通规则信息。 0062 4.3.4城市路口节点的自动驾驶 进入路口节点区域以后， 车辆根据路口RSU更新车道结构信息和交通规则信息， 同时切 换成高精度纵向定位， 必要时进行停车辅助控制。 0063 在路口节点内， 由于车辆停靠距离较近， 车道线被遮挡， LKS系统的视觉横向定位 系统有可能断续失效。 这时可以依靠RSU提供高精度横向定位。 如果前方有车辆减速， 本车 依靠ADAS的跟停系统(全速ACC系统)实现自动跟车停走操作； 如果前方没有其它车辆， 本车 根据RSU纵向定位系统和信号灯识别系统实现路口自动停车。 00。

45、64 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系统跟随车道线前行； 0。

46、068 ii)本车寻找与感知转入道路 “入口门关” RSU 1、 RSU2位置， 在自动驾驶参考坐标 系里建立RSU 1-2线段方程； 坐标系可以采用相对于地面的静止坐标系， 也可以采用相对车 身固定的随动坐标系； 可以采用笛卡尔坐标， 也可以采用极坐标； 0069 iii)根据已知RSU参数L1、 L2， 建立车道中心线C1(或C2)的线段方程(方向与RSU1- 2连线垂直) 0070 iv)用最佳转弯半径弧线A1(或A2)连接C1(或C2)和本车转向前的前行中心线， 并 与二者相切。 最佳转弯半径与转弯角速度范围参考下表。 如果道路结构要求小于最小转弯 半径， 则利用轨迹(trajecto。

47、ry)规划按照舒适性原则降低转弯速度； 表4 V(mph)f(摩擦系数)下限转弯半径R(ft) 100.3818 150.3247 200.2799 250.22174 0071 v)如果没有规则化的弧线可供选用， 可使用成熟的常用路径与轨迹规划方法将本 说明书 13/17 页 19 CN 111076731 A 19 车轨迹平滑连接到C1(或C2)上； 0072 vi)在使用自行规划路径(非转弯车道线跟随模式)时， 如果有其它车辆切入本车 的规划路径， 则根据ADAS设计的安全原则进行避让。 0073 vii)如果路口划有待驶区域， 本车则应该按照待驶车道行驶， 并在待驶停车线处 停车等待左。

48、转灯。 待驶区分为直行待驶区和左转待转区两种(图11， 图10)。 0074 RSU通知车辆本车道是否有待转区。 如果有待转区， 当直行灯变绿以后， 本车依靠 LKS系统沿待转车道线行驶， 捕捉到待转区停车线以后在停在停车线上。 0075 viii)如果此路口没有左转红绿灯， 同时没有待弯区域， 以美国加州交规为例， 如 果有本道路对面相对而来的直行车辆， 本车应当沿原来方向行驶至路口中央， 然后停止给 对方让行， 且在此之前不允许向左转动方向盘。 本路径规划方法规定， 在路口中央让行的停 车点为转弯弧线A与本车前行轴线之间的切点T。 如果本车选择转向后占用右车道就停在T1 点， 如果选择做车。

49、道就停止在T2点， 且在汽车之前不允许转动方向盘。 0076 其它地区的具体驾驶行为规划需要符合当地交通法规的规定。 0077 2)复杂路口路径规划(见图12) 较复杂路口也可以按照上述RSU-环视信息融合原理完成路口通行任务。 0078 在图中所示的上海市世纪大道-张扬路-东方路交汇处， 需要设置6道 “入门关口” 。 每个RSU都带有道路名称属性， 所以车辆转入路口之前需要寻找相应的正确 “关口” 。 比如， 从东方路上行左拐进入世纪大道的车辆， 需要首先RSU3和RSU4时正确的门关， 然后才能进 入建立门关线段方程程序。 0079 这个左拐进入世纪大道的指令是车道导航地图发出的， 因此。

50、， 车道导航地图需要 将转入道路的名称用文本形式发给本定位导航系统。 0080 在这种大型路口， 极有可能因为距离过远或者建筑、 车辆互相遮挡而造成RSU视线 丢失。 这时可采用两种办法加以补偿和备份。 第一是增加标杆高度， 比如增加到20米高， 这 时要求RSU本身发射特殊标高通知， 以便本系统对方位计算方法进行修正。 第二种方法是用 1.5米1.8米的精度， 依靠RFID信号对RSU进行定位(方法见)。 当RSU重新出现在视野以 内之后再进行精准定位， 修正轨迹。 这个逐渐精细化修正过程也是和人类驾驶习惯相一致 的。 0081 使用高精度的惯导单元也可以补偿地标信号的暂时缺失。 0082 。