车辆位置姿势角推定装置及车辆位置姿势角推定方法
【技术领域】
[0001]本发明涉及使用质点过滤器(particlefilter)推定车辆的位置及姿势角的车辆位置姿势角推定装置及其方法。
【背景技术】
[0002]作为使用质点过滤器计算移动体的位置和姿势角的技术，目前公开有专利文献I。该技术中，在根据里程(odometry)而计算出的移动体的位置和姿势角的附近散布多个质点。而且，将这些质点中与搭载于移动体的激光传感器的测定值最吻合的质点(particle)作为该移动体的位置和姿势角的真实值进行计算。
[0003]此时，散布质点的存在分布范围通过时时刻刻计算使用质点过滤器计算出的位置和姿势角如何进行修正，并根据该修正量的履历进行设定。
[0004]现有技术文献
[0005]专利文献
[0006]专利文献I:(日本)特开2010 — 224755号公报
[0007]但是，在上述的现有技术中，考虑到向车辆的行进方向的误差，但向车宽方向的误差未充分考虑。特别是，在车速高时，如果车辆转弯，则在车辆上产生向车宽方向的大的移动，因此，向车宽方向的误差增大，但目前这种情况的对应未能充分进行。因此，存在对质点的存在分布范围的设定产生延迟，不能适当地进行设定的问题点。
【发明内容】
[0008]于是，本发明是鉴于上述的情况而提案的，其目的在于，提供一种即使在车辆的速度变高而在车辆上产生向车宽方向的移动的情况下，也能够适当地设定质点的存在分布范围的车辆位置姿势角推定装置及其方法。
[0009]为解决上述的课题，本发明一方式的车辆位置姿势角推定装置及其方法，使用质点过滤器设定规定范围的质点的存在分布范围。而且，在所设定的存在分布范围内散布质点，根据拍摄车辆的周围环境的图像推定车辆的位置及姿势角，当车辆的速度变高时，将所设定的存在分布范围向车辆的车宽方向扩展。
【附图说明】
[0010]图1是表示具备本发明第一实施方式的车辆位置姿势角推定装置的车辆位置姿势角推定系统的构成的框图；
[0011]图2是表示本发明第一实施方式的车辆位置姿势角推定装置的车辆的位置姿势角推定处理的处理顺序的流程图；
[0012]图3是示意性地表示定速圆转弯中的车辆的转弯中心和各轮胎的侧滑角的图；
[0013]图4是用于说明根据本发明第一实施方式的车辆位置姿势角推定装置推定的速度将质点的存在分布范围向车辆的前后方向扩展的方法的图；
[0014]图5是用于说明根据本发明第一实施方式的车辆位置姿势角推定装置推定的速度将质点的存在分布范围向车辆的车宽方向扩展的方法的图；
[0015]图6是用于说明根据本发明第一实施方式的车辆位置姿势角推定装置推定的速度将质点的存在分布范围向车辆的偏航角方向扩展的方法的图；
[0016]图7是表示具备本发明第二实施方式的车辆位置姿势角推定装置的车辆位置姿势角推定系统的构成的框图；
[0017]图8是用于说明车辆转舵时的轮胎横力和偏航力矩的图；
[0018]图9是用于说明根据本发明第二实施方式的车辆位置姿势角推定装置推定的偏航率将质点的存在分布范围向车辆的车宽方向及偏航角方向扩展的方法的图；
[0019]图10是表示具备本发明第三实施方式的车辆位置姿势角推定装置的车辆位置姿势角推定系统的构成的框图；
[0020]图11是用于说明根据本发明第三实施方式的车辆位置姿势角推定装置推定的转向操舵角将质点的存在分布范围向车辆的前后方向、车宽方向及偏航角方向扩展的方法的图；
[0021]图12是用于说明本发明的车辆位置姿势角推定装置的质点的散布方法的图。
【具体实施方式】
[0022]以下，参照【附图说明】应用了本发明的第一?第三实施方式。
[0023][第一实施方式]
[0024][车辆位置姿势角推定系统的构成]
[0025]图1是表示搭载有本实施方式的车辆位置姿势角推定装置的车辆位置姿势角推定系统的构成的框图。如图1所示，本实施方式的车辆位置姿势角推定系统具备ECU1、摄像机
2、三维地图数据库3、车辆传感器群4。
[0026]在此，E⑶I为由R0M、RAM、运算电路等构成的电子控制单元，具备本实施方式的车辆位置姿势角推定装置10。此外，E⑶I也可以兼用于其它控制的E⑶。
[0027]摄像机2使用例如CCD等固体摄像元件，本实施方式中，以其光轴水平且以可拍摄车辆前方的方式设置在车辆的前保险杠上。拍摄到的图像被送入ECU1。
[0028]三维地图数据库3存储有例如包含路面显示的周围环境的边缘等三维位置信息。本实施方式中，至少存储有表示道路端部的区划线或路牙等位置和其方向的三维信息，另夕卜，除白线、停止线、人行横道、路面标识等路面显示之外，还包含建筑物等构造物的边缘信息。此外，作为三维地图数据库3的数据格式，道路端部等各地图信息被作为边缘的集合体而定义。在边缘为长的直线的情况下，例如以每Im进行区分，因此，不存在极长的边缘。在直线的情况下，各边缘具有表示直线的两端点的三维位置信息。在曲线的情况下，各边缘具有表示曲线的两端点和中央点的三维位置信息。
[0029]车辆传感器群4具备GPS接收机41、加速传感器42、转向传感器43、制动传感器44、车速传感器45、加速度传感器46、车轮速传感器47、偏航率传感器48。车辆传感器群4与ECUl连接，将由各传感器41?48检测出的各种检测值向ECUl供给。E⑶I通过使用车辆传感器群4的输出值，计算车出辆的大概位置，或计算出表示单位时间内车辆行进的移动量的里程(odometry)。
[0030]车辆位置姿势角推定装置10是用于将拍摄车辆的周围环境得到的图像和三维地图数据进行匹配来推定车辆的位置及姿势角的装置。而且，通过执行特定的程序，作为边缘图像计算部12、里程计算部14、车速检测部15、质点存在分布范围设定部16、质点散布部18、投影图像生成部20、相似度计算部22及位置姿势角推定部24而进行动作。
[0031]边缘图像计算部12由摄像机2取得拍摄车辆的周围环境而得到的图像，根据该图像检测边缘，并计算边缘图像。在由摄像机2拍摄到的图像中，作为用于推定自车辆的位置及姿势角所需的路面信息，至少拍摄表示道路端部的区划线或路牙。另外，也可以拍摄其它白线、停止线、人行横道、路面标识等路面显示。
[0032]里程计算部14使用由车辆传感器群4得到的各种传感器值计算出车辆的每单位时间的移动量即里程。
[0033]车速检测部15通过取得由车速传感器45测量出的传感器值，检测车辆的速度。
[0034]质点存在分布范围设定部16在仅移动由里程计算部14计算出的里程量的位置和姿势角的附近设定规定范围的质点的存在分布范围，根据车辆的行驶状态修正质点的存在分布范围。具体而言，如图12所示，使I循环前推定的车辆V(tl)的位置及姿势角的质点P和周围的质点Pl?P5移动里程量，设定质点的存在分布范围进行修正。本实施方式中，作为车辆的行驶状态，当速度变高时，进行使质点的存在分布范围在车辆的前后方向、车宽方向及偏航角方向扩展的修正。
[0035]质点散布部18在由质点存在分布范围设定部16设定的质点的存在分布范围内散布质点。如图12所示，质点散布部18为推定新的车辆V (12)的位置及姿势角而设定质点P1
?15ο
[0036]投影图像生成部20对于由质点散布部18散布的质点分别生成投影图像。例如，将存储于三维地图数据库3的边缘等三维位置信息以成为根据各质点的位置及姿势角用摄像机拍摄的图像的方式进行投影变换，生成投影图像。
[0037]相似度计算部22将由投影图像生成部20生成的投影图像和由边缘图像计算部12计算出的边缘图像进行比较，对质点分别计算出相似度(尤度I ikel ihood)。该相似度是表示各质点的位置及姿势角相对于实际的车辆的位置及姿势角相似多少的指标，以投影图像和边缘图像的一致度越高，相似度越高的方式进行设定。
[0038]位置姿势角推定部24基于由相似度计算部22计算出的相似度来推定车辆的位置及姿势角。例如，将相似度最高的质点的位置及姿势角作为车辆的实际的位置及姿势角的推定结果来进行计算。
[0039]此外，本实施方式中，求出车辆的6自由度的位置(前后方向、车宽方向、上下方向)及姿势角(侧倾、俯仰、偏航)。但是，例如如不具备悬架等的工厂等所使用的无人输送车，即使在推定3自由度的位置(前后方向、横方向)及姿势角(偏航)的情况下，也可以应用本技术。具体而言，在这样的车辆中，由于上下方向的位置和姿势角的侧倾及俯仰固定，所以只要预先测量这些参数并利用即可。
[0040][车辆的位置姿势角推定处理的顺序]
[0041]其次，参照图2的流程图说明本实施方式的车辆的位置姿势角推定处理的顺序。此夕卜，本实施方式中，作为车辆的位置信息推定前后方向、车宽方向、上下方向，作为姿势角信息推定侧倾、俯仰、偏航的合计6自由度的位置和姿势角。但是，侧倾是以车辆的前后方向为轴的旋转方向，俯仰是以车辆的车宽方向为轴的旋转方向，偏航是以车辆的上下方向为轴的旋转方向(参照图12)。
[0042]另外，以下说明的车辆的位置姿势角推定处理例如以100msec左右的间隔连续进行。
[0043]如图2所示，首先，在步骤SllO中，边缘图像计算部12根据摄像机2的图