CN114644315A - 一种自动加油用油箱盖精确定位装置及方法 - Google Patents

一种自动加油用油箱盖精确定位装置及方法 Download PDF

Info

Publication number
CN114644315A
CN114644315A CN202210241756.XA CN202210241756A CN114644315A CN 114644315 A CN114644315 A CN 114644315A CN 202210241756 A CN202210241756 A CN 202210241756A CN 114644315 A CN114644315 A CN 114644315A
Authority
CN
China
Prior art keywords
point
camera
coordinate system
points
fuel tank
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.)
Granted
Application number
CN202210241756.XA
Other languages
English (en)
Other versions
CN114644315B (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.)
Jiangyin Furen High Tech Co Ltd
Original Assignee
Jiangyin Furen High Tech Co Ltd
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 Jiangyin Furen High Tech Co Ltd filed Critical Jiangyin Furen High Tech Co Ltd
Priority to CN202210241756.XA priority Critical patent/CN114644315B/zh
Publication of CN114644315A publication Critical patent/CN114644315A/zh
Application granted granted Critical
Publication of CN114644315B publication Critical patent/CN114644315B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B67—OPENING, CLOSING OR CLEANING BOTTLES, JARS OR SIMILAR CONTAINERS; LIQUID HANDLING
    • B67D—DISPENSING, DELIVERING OR TRANSFERRING LIQUIDS, NOT OTHERWISE PROVIDED FOR
    • B67D7/00—Apparatus or devices for transferring liquids from bulk storage containers or reservoirs into vehicles or into portable containers, e.g. for retail sale purposes
    • B67D7/04—Apparatus or devices for transferring liquids from bulk storage containers or reservoirs into vehicles or into portable containers, e.g. for retail sale purposes for transferring fuels, lubricants or mixed fuels and lubricants
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B67—OPENING, CLOSING OR CLEANING BOTTLES, JARS OR SIMILAR CONTAINERS; LIQUID HANDLING
    • B67D—DISPENSING, DELIVERING OR TRANSFERRING LIQUIDS, NOT OTHERWISE PROVIDED FOR
    • B67D7/00—Apparatus or devices for transferring liquids from bulk storage containers or reservoirs into vehicles or into portable containers, e.g. for retail sale purposes
    • B67D7/06—Details or accessories
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B67—OPENING, CLOSING OR CLEANING BOTTLES, JARS OR SIMILAR CONTAINERS; LIQUID HANDLING
    • B67D—DISPENSING, DELIVERING OR TRANSFERRING LIQUIDS, NOT OTHERWISE PROVIDED FOR
    • B67D7/00—Apparatus or devices for transferring liquids from bulk storage containers or reservoirs into vehicles or into portable containers, e.g. for retail sale purposes
    • B67D7/04—Apparatus or devices for transferring liquids from bulk storage containers or reservoirs into vehicles or into portable containers, e.g. for retail sale purposes for transferring fuels, lubricants or mixed fuels and lubricants
    • B67D7/0401—Apparatus or devices for transferring liquids from bulk storage containers or reservoirs into vehicles or into portable containers, e.g. for retail sale purposes for transferring fuels, lubricants or mixed fuels and lubricants arrangements for automatically fuelling vehicles, i.e. without human intervention
    • B67D2007/0403—Fuelling robots
    • B67D2007/0417—Manipulator arms
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B67—OPENING, CLOSING OR CLEANING BOTTLES, JARS OR SIMILAR CONTAINERS; LIQUID HANDLING
    • B67D—DISPENSING, DELIVERING OR TRANSFERRING LIQUIDS, NOT OTHERWISE PROVIDED FOR
    • B67D7/00—Apparatus or devices for transferring liquids from bulk storage containers or reservoirs into vehicles or into portable containers, e.g. for retail sale purposes
    • B67D7/04—Apparatus or devices for transferring liquids from bulk storage containers or reservoirs into vehicles or into portable containers, e.g. for retail sale purposes for transferring fuels, lubricants or mixed fuels and lubricants
    • B67D7/0401—Apparatus or devices for transferring liquids from bulk storage containers or reservoirs into vehicles or into portable containers, e.g. for retail sale purposes for transferring fuels, lubricants or mixed fuels and lubricants arrangements for automatically fuelling vehicles, i.e. without human intervention
    • B67D2007/0444—Sensors
    • B67D2007/0455—Sensors recognising the position
    • B67D2007/0467—Sensors recognising the position of the fuel tank flap and/or fuel tank opening
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B67—OPENING, CLOSING OR CLEANING BOTTLES, JARS OR SIMILAR CONTAINERS; LIQUID HANDLING
    • B67D—DISPENSING, DELIVERING OR TRANSFERRING LIQUIDS, NOT OTHERWISE PROVIDED FOR
    • B67D7/00—Apparatus or devices for transferring liquids from bulk storage containers or reservoirs into vehicles or into portable containers, e.g. for retail sale purposes
    • B67D7/04—Apparatus or devices for transferring liquids from bulk storage containers or reservoirs into vehicles or into portable containers, e.g. for retail sale purposes for transferring fuels, lubricants or mixed fuels and lubricants
    • B67D7/0401—Apparatus or devices for transferring liquids from bulk storage containers or reservoirs into vehicles or into portable containers, e.g. for retail sale purposes for transferring fuels, lubricants or mixed fuels and lubricants arrangements for automatically fuelling vehicles, i.e. without human intervention
    • B67D2007/0444—Sensors
    • B67D2007/0455—Sensors recognising the position
    • B67D2007/0467—Sensors recognising the position of the fuel tank flap and/or fuel tank opening
    • B67D2007/0471—Sensors recognising the position of the fuel tank flap and/or fuel tank opening by calculating from a reference position, e.g. the car's silhouette, reference marks
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Mechanical Engineering (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明公开了一种自动加油用油箱盖精确定位装置及方法，所述装置包括机械臂、相机、基准点、控制系统，所述机械臂与所述相机相连，所述控制系统与所述机械臂、相机相连，所述基准点为预设在待加油车附近地面和/或空中的标记点位，控制系统通过控制机械臂运动将待加油车身和基准点共同置于相机视场内，控制相机拍照，并通过对车型信息和图像信息的解析，计算油箱盖和加油口的精确位置，控制夹具开关外盖、加油枪启停加油等操作，从而无需深度相机、测距传感器、压力传感器等专用设备，利用普通相机即可实现油箱盖精确定位、提高加油效率、降低成本的目的。

Description

一种自动加油用油箱盖精确定位装置及方法
技术领域
本发明涉及汽车自动加油技术领域，具体涉及一种自动加油用油箱盖精确定位装置及方法。
背景技术
随着经济和社会发展，汽车自动加油技术应用越来越广泛。
当前汽车自动加油，包括机器人加油，一般首先通过图像传感器、压力传感器、光电限位传感器或者测距传感器等初步确定车身位置，较有代表性的就是确定车轮的位置，一般使用车轮与地面接触点或者车轮轴心作为位置基准，也可使用车身其他特征点作为位置基准；然后通过图像传感器如普通相机或者物联网技术完成对车牌的识别，通过车牌信息提取车型信息，从而获知车轮与油箱外盖的相对位置(该步骤也可以先完成)，自动加油系统则可根据此相对位置和已经获取的车轮位置，驱动机械臂和加油枪运动到油箱盖附近；此后，再依托加油枪或机械臂上携带的图像传感器或测距传感器等获取油箱盖近距离清晰图像或距离信息等，从而可精确定位油箱盖和/或加油口位置(这里称之为二次测量定位)，驱动机械臂执行开关盖操作，驱动加油枪执行加油操作。
以上方案存在的主要问题是车身初定位误差较大，一般数个厘米甚至更高，因此不能保证机械臂及加油枪一次到位，需要如深度相机类的设备进行再次成像、精确定位；另一方面，深度相机白天成像受环境影响较大，其他类型传感器如双目相机计算数据稳定性不足、激光扫描安全性和准确性业内尚存在质疑，总之精确定位的现有方案仍有很多待改进之处；此外，现有初定位方案中传感器布局一般也较为复杂或者数量较多，性价比不高。
发明内容
考虑到当前的图像处理技术已经较为成熟，特别是利用已知基准点，通过不同角度拍摄多幅图像可以精确确定目标点相对基准点的位置。鉴于此以及汽车自动加油领域存在的上述问题，本发明提出了一种自动加油用油箱盖精确定位装置及方法，可以利用普通相机成像，实现精确定位车身位置和姿态，从而精确定位油箱盖和/或加油口位置，无需深度相机、测距传感器等类设备的二次测量定位。
本发明设计的一种自动加油用油箱盖精确定位装置，包括机械臂、相机、基准点、控制系统，所述相机至少为一部，若为一部，其相机安装于所述机械臂上，2部以上，其相机安装在待加油车周边不同位置，机械臂是优选的安装位置之一；所述机械臂、相机与所述控制系统相连，受控制系统控制；所述基准点包括预设在待加油车附近地面和/或空中的标记点位。所述油箱盖包括汽车油箱外盖和/或内盖，在所述机械臂上可以安装有夹具、加油枪等常规加油装置，为成熟技术。
进一步的，所述基准点为光学标志点，区别于周围环境，用于相机拍照定位识别，车辆待加油区域至少布置1个基准点，若为多个基准点，则每个基准点分别标记；基准点位置确定并已知有利于简化计算；基准点一般用显著的颜色如黑色、红色、蓝色、橙色或绿色等标记，基准点形状可以为是任意形状，一般可优先选择十字交叉点、星形点或圆形点，形状和颜色设置，均需考虑能够明显区别于周围环境，便于图像识别；为便于相机夜间识别，可以在基准点中心设置发光二极管等点光源；基准点相互间可区分，一般可以通过在每个点周边设置数字编号的方式或者其他方式来区分；基准点位置选择在加油区域地面或空中皆可，一般在地面和空中同步布设多个基准点，便于多角度拍照均有基准点进入相机视场；空中基准点可以在加油机上、建筑物上、立柱上、加油站顶棚下或从加油站顶棚垂吊，总之不同位置、不同高度的多个空中基准点有利于更加精确的定位待加油车油箱盖和/或加油口位置；基准点实际设置无需长期固定，只需要在有车辆加油时，拍摄所有照片过程中保持位置固定不变即可，拍摄完成后可以设置自动回收的装置，如此基准点配置将更加灵活多样。
进一步的，所述相机包括普通相机和/或结构光相机、双目相机，相机输出图像位数字图像或可数字化的图像，安装位置便于同时拍摄至少一个基准点和待加油车的至少2个特征点；如果仅配置一部相机，则相机拍照位置应便于同时拍摄至少1个基准点和待加油车的至少2个特征点；或同时拍摄至少1个基准点和一个油箱盖上或油箱盖边缘相关的特征点，所述油箱盖边缘相关点包括油箱盖上的点或车身与油箱盖边缘相近的点。所述汽车特征点包括待加油汽车上固有的、不变的、易图像识别的标志性点位，如车轮的轴心、车窗的角点、前后挡风玻璃的角点、车身前后的顶点、油箱盖的中心等，其中，车轮轴心可以通过车轮或车轴边缘识别精确提取并定位，推荐使用，而车身前后的顶点图像特征一般并不明显，不推荐使用。鉴于普通相机既方便又廉价，一般推荐使用普通相机，可实现较高的性价比；也可以配置多部相机，推荐在机械臂上固连一部相机，可随机械臂运动，另在不同部位固定设置一部以上普通相机，可以实现对待加油车辆的全方位拍照，有利于提高车身位置和姿态测量精度，也就是提高油箱盖和/或加油口定位精度。
进一步的，所述控制系统包括计算机类控制系统。控制系统一方面接收相机图像，进行图像处理、识别，计算待加油车油箱盖和/或加油口位置，另一方面根据控制程序发出机器人或机械臂运动、相机拍照、夹具开关油箱盖、加油枪加油等指令。
本发明设计的采用自动加油用油箱盖精确定位装置的精确定位方法之一，包括以下步骤：
S1：拍照，移动机械臂控制其上相机和/或控制其它不同位置上的相机，从不同角度拍摄2张以上照片，使至少一个基准点和待加油车至少2个特征点同时出现在所拍照片的视场中；一般优先选择2个以上车轮的轴心、1个以上车窗或前后挡风玻璃的角点作为特征点，精确计算车身姿态一般拍摄特征点累计不少于4个，且至少有一个特征点不在其他特征点的平面内，基准点累计不少于2个；
S2：图像识别，提取基准点和待加油汽车上的特征点在图像中的坐标，图像处理和识别技术为现有成熟技术；
S3：特征点位置和汽车姿态计算，根据相机成像原理、基准点位置已知、汽车特征点在车体坐标系中相对位置已知的特点，利用特征点位置计算方法或模型，计算待加油汽车特征点在加油站坐标系中的位置和/或在相机坐标系中的位置以及相对机械臂的位置；利用姿态计算方法或模型，计算待加油车简化姿态或精确姿态；一般选择加油口同侧的2个车轮轴心作为特征点，可以计算汽车简化姿态；选择3个以上车轮轴心加1个车窗或前后挡风玻璃的角点，累计4个特征点可以计算待加油车的精确姿态；
针对基准点数量大于等于2个和/或不同位置拍摄图像数量大于等于2张的情形，利用特征点位置优化方法或模型优化计算特征点位置；针对特征点个数大于等于4个的情形，利用车身姿态优化方法或模型优化计算车身姿态；优化模型包括统计方法或其他数学方法，统计方法包括最小二乘法、数学平均法等；特征点数量不足4个时，可以只优化汽车简化姿态角的值；特征点数量超过4个时，可以同时优化汽车精确姿态值；基准点数量大于等于2个、照片大于等于2张时，可以优化计算每个特征点的位置；
一般来讲，基准点数量多，可以提高汽车特征点定位精度；特征点数量多，可以提高车身姿态确定精度；不同角度的照片数量多，可以同时提高特征点定位精度和姿态确定精度，最终都对提高油箱盖或加油口定位精度有益；
S4：油箱盖定位，根据车型信息获知汽车特征点相对油箱盖的位置，结合步骤S3所述特征点和姿态计算结果，利用油箱盖位置计算方法或模型，计算油箱盖在加油站坐标系中的位置和/或在相机坐标系中的位置以及相对机械臂的位置，实现精确定位；车型信息、汽车各特征点及油箱盖各自在车体坐标系下相对位置信息获知为成熟技术，如通过车牌信息提取查询数据库的方式得到；本步骤方法同样适用于加油口或车身其它需确定位置的部位的精确定位。
优选的，油箱盖精确定位方法之一所述特征点位置计算方法或模型包括：
考虑只拍摄2张不同角度照片，记第一基准点在加油站坐标系中的坐标为(xE,31,yE,31,zE,31)，第一基准点在2次不同拍照位置相机坐标系下的坐标分别为(x21,31,y21,31,z21,31)、(x22,31,y22,31,z22,31)，依据坐标转换方法或模型，可以用加油站坐标系下的坐标(xE,31,yE,31,zE,31)来表示；第一基准点在2张照片图像坐标系下的坐标分别为(xS1,31,yS1,31)、(xS2,31,yS2,31)；第一特征点在2张照片图像坐标系下的坐标分别为(xS1,51,yS1,51)、(xS2,51,yS2,51)，该坐标值可通过图像识别测量得到，以上均为已知量；
第一特征点在加油站坐标系中的坐标为(xE,51,yE,51,zE,51)，有3个未知量，需要求解，依据坐标转换方法或模型，第一特征点在相机2次拍照坐标系下的坐标(x21,51,y21,51,z21,51)、(x22,51,y22,51,z22,51)可以用(xE,51,yE,51,zE,51)来表示；
所述坐标转换方法或模型包括：
Figure BDA0003542456370000051
即(x21,51,y21,51,z21,51)通过公式(1)得到，其中(x021,E,y021,E,z021,E)为加油站坐标系原点在相机拍照坐标系中的坐标，为平移量，Q21,E为加油站坐标系到第一次拍照的相机坐标系的旋转矩阵，
Figure BDA0003542456370000061
其中，3个欧拉角俯仰α、偏航β、滚动γ用来表征旋转矩阵，Q21,E和(x021,E,y021,E,z021,E)均为已知量。所有特征点、基准点的坐标转换方法或模型类同公式(1)；
依据相机简化成像模型有
Figure BDA0003542456370000062
Figure BDA0003542456370000063
Figure BDA0003542456370000064
Figure BDA0003542456370000065
将由公式(1)和公式(2)方法或模型得到的(x21,51,y21,51,z21,51)、(x22,51,y22,51,z22,51)、(x21,31,y21,31,z21,31)、(x22,31,y22,31,z22,31)分别代入公式(3)～公式(6)，4个方程求解3个未知数，采用忽略任意一个方程直接计算解析结果或用最小二乘法或计算机数值法求解均可得到(xE,51,yE,51,zE,51)；
同理，求得所有特征点在加油站坐标系下的坐标。
优选的，油箱盖精确定位方法之一所述车身简化姿态计算方法或模型包括：
定义第一特征点、第二特征点分别为待加油车右后轮轴心和右前轮轴心，计算车身简化姿态，假设车身前后左右均处于平衡状态，即4个车轮轴心形成的平面与加油站路面平行，不失一般性，均认为水平状态，因此车身姿态主要是车身方向即x轴方向与加油站坐标系x轴间存在夹角θ，定义该夹角以加油站坐标系x轴为基准，绕z轴逆时针旋转为正，顺时针旋转为负，有
Figure BDA0003542456370000071
其中，L为前后轮的轮距。
优选的，油箱盖精确定位方法之一所述特征点位置和车身姿态优化方法或模型包括：
特征点位置优化方法或模型：记n为不同角度照片张数，n≥2；m为定义的特征点个数，m≥4；w为设计的基准点个数，w≥2；采用考虑相机主点与成像中心点偏差的成像模型，横纵向主点偏差分别记为xp0,21、yp0,21、…、xp0,2i、yp0,2i、…、xp0,2n、yp0,2n，焦距记为f21、…、f2i、…、f2n，图像横纵向比例因子分别为mx21、my21、…、mx2i、my2i、…、mx2n、my2n，n≥i≥1，每张照片二维像素坐标系用Si来标识；特征点序列号用5j来标识，m≥j≥1；基准点序列号用3k来标识，w≥k≥1；每张照片的每一特征点和每一个基准点分别写出2个方程：
Figure BDA0003542456370000072
Figure BDA0003542456370000073
Figure BDA0003542456370000074
Figure BDA0003542456370000075
其中(x2i,5j,y2i,5j,z2i,5j)、(x2i,3k,y2i,3k,z2i,3k)通过公式(1)和公式(2)的方法或模型用坐标转换来表示，即
Figure BDA0003542456370000076
Figure BDA0003542456370000081
旋转矩阵Q2i,E和坐标平移量(x021,E,y021,E,z021,E)事先可测或可计算，均为已知量；特征点和基准点像素坐标(xSi,5j,ySi,5j)、(xSi,3k,ySi,3k)为图像识别目标提取后的结果，亦为已知量；
考虑每张照片相机参数包括主点偏差、比例因子、焦距共5个未知数，每个特征点3个未知数，对于n张照片、m个特征点、w个基准点共有(5n+3m)个未知数，参考公式(8)～公式(11)可以建立(2q+2p)个方程，其中q为所有照片拍摄识别的特征点总数，p为所有照片拍摄识别的基准点总数量；在保证(2q+2p)>＝(5n+3m)的情况下，即可采用最小二乘或其他数学优化方法对方程进行求解。
车身姿态优化方法或模型：用车体坐标系相对加油站坐标系的旋转欧拉角表征车身姿态，累计3个未知数，依据车体坐标系向加油站坐标系转换的模型
Figure BDA0003542456370000082
其中C表示车体坐标系，QE,C为车体坐标系到加油站坐标系的旋转矩阵，有3个欧拉角未知量；(xC,5j,yC,5j,zC,5j)为第j个特征点在车体坐标系下的坐标，为已知量；(x0E,C,y0E,C,z0E,C)为车体坐标系原点在加油站坐标系下的坐标，以特征点之一如第一特征点作为坐标原点，此亦为已知量；根据特征点位置优化方法或模型，所有特征点位置已求解完成，则除作为原点的第一特征点外所有特征点形成的如公式(14)的方程联立，累计大于等于4个特征点的情况下，采用最小二乘或数学优化方法对联立方程进行求解；一般4个特征点可以求得解析解，5个及以上特征点可以求得优化解，二者方法可统一。
优选的，油箱盖精确定位方法之一所述油箱盖位置计算方法或模型包括：
根据车身简化姿态定义，记车体坐标系下，油箱盖相对后轮轴心在x、y、z方向的位移分别为xC,g、yC,g、zC,g，其中C表示车体坐标系，g表示油箱盖，则油箱盖在加油站坐标系下的坐标为：
Figure BDA0003542456370000091
或者根据车身精确姿态利用公式(14)的方法或模型即可得到油箱盖精确位置。
加油口或车身其他部位的位置计算方法同油箱盖位置计算方法，只是它们相对后轮轴心在车体坐标系x、y、z方向的位移与油箱盖不同。
本发明设计的采用自动加油用油箱盖精确定位装置的精确定位方法之二，以油箱盖为特征点，直接定位，包括以下步骤：
B1：拍照，移动机械臂控制其上相机和/或控制其它不同位置上的相机，从不同角度拍摄2张以上照片，使至少一个基准点和待加油车油箱盖同时出现在所拍照片的视场中，将油箱盖中心作为特征点或者油箱盖上其他易识别的点作为特征点；一般拍照条件允许时，直接拍摄油箱盖并提取其中心点位置，将油箱盖中心点直接作为特征点，可大大简化油箱盖位置计算方法；通常拍摄油箱外盖即可，通过内盖与外盖、以及加油口与外盖在车体坐标系下的相对位置关系，可以得到满足加油操作精度要求的油箱内盖及加油口位置，特殊情况下可以单独补充拍摄2张以上油箱内盖或加油口的照片，所有照片均需包含至少一个基准点。
B2：图像识别，提取基准点和待加油汽车油箱盖上的特征点在图像中的坐标，一般通过图像边缘提取方式推算油箱盖中心点的图像坐标；
B3：油箱盖位置计算，根据相机成像原理和基准点已知的特点，利用特征点位置计算方法或模型，计算待加油车油箱盖的位置；针对基准点数量超过1个和/或不同位置拍摄图像数量超过2张的情形，利用特征点位置优化方法或模型，计算油箱盖位置结果，一般利用最小二乘等统计方法或其他数学方法。
优选的，油箱盖精确定位方法之二所述特征点位置计算方法或模型包括：
考虑只拍摄2张不同角度照片，记第一基准点在加油站坐标系中的坐标为(xE,31,yE,31,zE,31)，第一基准点在2次不同拍照位置相机坐标系下的坐标分别为(x21,31,y21,31,z21,31)、(x22,31,y22,31,z22,31)，依据坐标转换方法或模型，与加油站坐标系下的坐标(xE,31,yE,31,zE,31)存在坐标转换关系；第一基准点在2张照片图像坐标系下的坐标分别为(xS1,31,yS1,31)、(xS2,31,yS2,31)；第一特征点在2张照片图像坐标系下的坐标分别为(xS1,51,yS1,51)、(xS2,51,yS2,51)，该坐标值通过图像识别测量得到，以上均为已知量；
第一特征点在加油站坐标系中的坐标为(xE,51,yE,51,zE,51)，有3个未知量，需要求解，依据坐标转换方法或模型，第一特征点在相机2次拍照坐标系下的坐标(x21,51,y21,51,z21,51)、(x22,51,y22,51,z22,51)可以用(xE,51,yE,51,zE,51)来表示；
所述坐标转换方法或模型包括：
Figure BDA0003542456370000101
即(x21,51,y21,51,z21,51)通过公式1得到，其中(x021,E,y021,E,z021,E)为加油站坐标系原点在相机拍照坐标系中的坐标，为平移量，Q21,E为加油站坐标系到第一次拍照的相机坐标系的旋转矩阵，
Figure BDA0003542456370000111
其中，3个欧拉角俯仰α、偏航β、滚动γ用来表征旋转矩阵，Q21,E和(x021,E,y021,E,z021,E)均为机械臂受控运动过程中的已知量；所有特征点、基准点的坐标转换方法或模型类同公式(1)；
依据相机简化成像模型有
Figure BDA0003542456370000112
Figure BDA0003542456370000113
Figure BDA0003542456370000114
Figure BDA0003542456370000115
将由公式(1)和公式(2)方法或模型得到的(x21,51,y21,51,z21,51)、(x22,51,y22,51,z22,51)、(x21,31,y21,31,z21,31)、(x22,31,y22,31,z22,31)分别代入公式(3)～公式(6)，4个方程求解3个未知数，采用忽略任意一个方程直接计算解析结果或用最小二乘法或计算机数值法求解均可得到(xE,51,yE,51,zE,51)。
优选的，油箱盖精确定位方法之二所述特征点位置优化方法或模型包括：
记n为不同角度照片张数，n≥2；w为设计的基准点个数，w≥2；采用考虑相机主点与成像中心点偏差的成像模型，横纵向主点偏差分别记为xp0,21、yp0,21、…、xp0,2i、yp0,2i、…、xp0,2n、yp0,2n，焦距记为f21、…、f2i、…、f2n，图像横纵向比例因子分别为mx21、my21、…、mx2i、my2i、…、mx2n、my2n，n≥i≥1，每张照片二维像素坐标系用Si来标识；仅一个特征点，用序列号用51来标识；基准点序列号用3k来标识，w≥k≥1；每张照片的每一特征点和每一个基准点分别写出2个方程：
Figure BDA0003542456370000121
Figure BDA0003542456370000122
Figure BDA0003542456370000123
Figure BDA0003542456370000124
其中(x2i,51j,y2i,51,z2i,51)、(x2i,3k,y2i,3k,z2i,3k)通过公式(1)和公式(2)的方法或模型用坐标转换来表示，即
Figure BDA0003542456370000125
Figure BDA0003542456370000126
旋转矩阵Q2i,E和坐标平移量(x021,E,y021,E,z021,E)均为已知量；特征点和基准点像素坐标(xSi,51,ySi,51)、(xSi,3k,ySi,3k)为图像识别目标提取后的结果，亦为已知量；
每张照片相机参数包括主点偏差、比例因子、焦距共5个未知数，每个特征点3个未知数，对于n张照片、1个特征点、w个基准点共有(5n+3)个未知数，参考公式(8)～公式(11)可以建立(2n+2p)个方程，其中p为所有照片拍摄识别的基准点总数量；在保证(2n+2p)＞＝(5n+3)的情况下，采用最小二乘或数学优化方法对方程进行求解。
本发明的优点和有益效果在于：普通相机是一般加油站包括人工加油站和自动加油站的基本配置，主要用于安全监视和车牌识别。本发明利用普通相机一次拍摄多张不同角度照片可以实现代替机构光相机等特殊相机在自动加油领域的应用，既克服了特殊相机在工程应用中的特殊困难、避免了特殊需求、提高了性价比，还简化了汽车自动加油系统配置和操作流程，更明显的优势是通过普通相机的一次性测量，就能够精确定位汽车油箱盖和/或加油口，可有效提高自动加油效率。
附图说明
图1是一种单基准点、单相机自动加油用油箱盖定位装置；
图2是一种多基准点、多相机自动加油用油箱盖定位装置；
图3是自动加油用油箱盖精确定位方法流程图；
图4是简化汽车姿态确定原理图；
图中标记如下：
机械臂1、相机2、基准点3、控制系统4、汽车5、车轮6、油箱盖7；
图2中：第一相机21、第二相机22、第三相机23、第四相机24、第五相机25；
图2、4中：第一基准点31、第二基准点32、第三基准点33、第四基准点34、第一特征点51、第二特征点52。
具体实施方式
下面结合附图和实施例，对本发明的具体实施方式作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。
实施例1：单相机安装于机械臂的一种油箱盖精确定位装置
如图1所示，一种单基准点、单相机自动加油用油箱盖定位装置，包括机械臂1、相机2、基准点3、控制系统4，所述相机为一部，安装于所述机械臂上，所述机械臂、相机与所述控制系统相连，受控制系统控制；所述基准点包括预设在待加油车附近地面和/或空中的标记点位，图1示意了1个地面基准点。所述油箱盖包括汽车油箱外盖和/或内盖，在所述机械臂上可以安装有夹具、加油枪等常规装置，为成熟技术，未再示意，其他实施例同此。
实施例2：1部相机安装于机械臂、4部相机安装于待加油车外围上空的一种油箱盖精确定位装置
如图2所示，一种多基准点、多相机自动加油用油箱盖定位装置，包括机械臂1、第一相机21、第二相机22、第三相机23、第四相机24、第五相机25、第一基准点31、第二基准点32、第三基准点33、第四基准点34、控制系统4，所述相机为5部，1部安装于所述机械臂上，其余4部分别安装于停车区域四周上空，从四个方向对待加油车拍照，每部相机均能够拍摄至少2个特征点和2个基准点；所述机械臂、相机与所述控制系统相连，受控制系统控制；所述基准点包括预设在待加油车附近地面和/或空中的标记点位，本实施例在待加油车两侧地面各有2个基准点，停车位4角上方空中从加油站顶棚各垂吊了1个基准点，即待加油车周边共设置了8个基准点，图2所示为便于查看，只给出了车身一侧和上空各2个基准点，编号分别为31、32、33、34，同时示意了右侧2个车轮的特征点，分别为第一特征点51、第二特征点52。
优选的，关于实施例1、2，所述基准点为光学标志点，区别于周围环境，用于相机拍照定位识别，车辆待加油区域至少布置有1个基准点，若为多个基准点，则每个基准点分别标记，位置确定并已知，有利于简化计算。基准点一般用显著的颜色如黑色、红色、蓝色、橙色或绿色等标记，基准点形状可以为是任意形状，一般可优先选择十字交叉点、星形点或圆形点，形状和颜色设置，均需考虑能够明显区别于周围环境，便于图像识别；为便于相机夜间识别，可以在基准点中心设置发光二极管等点光源；基准点相互间可区分，一般可以通过在每点周边设置数字编号的方式或者其他方式来区分；基准点位置选择在加油区域地面或空中皆可，一般在地面和空中同步布设多个基准点，便于多角度拍照均有基准点进入相机视场；空中基准点可以在加油机上、建筑物上、立柱上、加油站顶棚下或从加油站顶棚垂吊，总之不同位置、不同高度的多个空中基准点有利于更加精确的定位待加油车油箱盖和/或加油口位置；基准点实际设置无需长期固定，只需要在有车辆加油时，拍摄所有照片过程中保持位置固定不变即可，拍摄完成后可以设置自动回收的装置，如此基准点配置将更加灵活多样。
优选的，关于实施例1、2，所述相机包括普通相机和/或结构光相机、双目相机，相机输出图像位数字图像或可数字化的图像，安装位置便于同时拍摄至少一个基准点和待加油车的至少2个特征点；如果仅配置一部相机，则相机拍照位置应便于同时拍摄至少1个基准点和待加油车的至少2个特征点；或同时拍摄至少1个基准点和一个油箱盖上或油箱盖边缘相关的特征点，所述油箱盖边缘相关点包括油箱盖上的点或车身与油箱盖边缘相近的点。所述汽车特征点包括待加油汽车上固有的、不变的、易图像识别的标志性点位，如车轮的轴心、车窗的角点、前后挡风玻璃的角点、车身前后的顶点、油箱盖的中心等，其中，车轮轴心可以通过车轮或车轴边缘识别精确提取并定位，推荐优先使用，而车身前后的顶点图像特征一般并不明显，不推荐优先使用。鉴于普通相机既方便又廉价，一般推荐使用普通相机，可实现较高的性价比；配置多部相机时，推荐在机械臂上固连一部相机，可随机械臂运动，其余几部相机在不同部位固定设置，一般根据通视特征点和基准点的需求选择点位，可以实现对待加油车量的全方位拍照，有利于提高车身位置和姿态测量精度，也就是提高油箱盖和/或加油口定位精度。
优选的，关于实施例1、2，所述控制系统包括计算机类控制系统。控制系统一方面接收相机图像，进行图像处理、识别，计算待加油车油箱盖和/或加油口位置，另一方面根据控制程序发出机器人或机械臂运动、相机拍照、夹具开关油箱盖、加油枪加油等指令。
实施例3：实施例1的一种油箱盖精确定位方法
如图3所示。
S1：拍照，移动机械臂控制其上相机和/或控制其它不同位置上的相机，从不同角度拍摄2张以上照片，使至少一个基准点和待加油车至少2个特征点同时出现在所有照片的视场中；本实施例优先选择加油口同侧的2个车轮的轴心作为特征点，图示加油口同侧为车身右侧，加油口在车身左侧的不影响实施方法。
S2：图像识别，提取基准点和待加油汽车上的特征点在图像中的坐标，图像处理和识别技术为现有成熟技术。
S3：特征点位置和汽车姿态计算，根据相机成像原理、基准点位置已知、汽车特征点在车体坐标系中相对位置已知的特点，利用特征点位置计算方法或模型，计算待加油汽车特征点在加油站坐标系中的位置和/或在相机坐标系中的位置以及相对机械臂的位置；利用姿态计算方法或模型，计算待加油车简化姿态或精确姿态；一般选择加油口同侧的2个车轮轴心作为特征点，可以计算汽车简化姿态；选择3个以上车轮轴心加1个车窗或前后挡风玻璃的角点，累计4个特征点可以计算待加油车的精确姿态；
S4：油箱盖定位，根据车型信息获知汽车特征点相对油箱盖的位置，结合本实施例步骤S3所述特征点和姿态计算结果，利用油箱盖位置计算方法或模型，计算油箱盖在加油站坐标系中的位置和/或在相机坐标系中的位置以及相对机械臂的位置，实现精确定位；车型信息、汽车各特征点及油箱盖和/或加油口各自在车体坐标系下相对位置信息获知为成熟技术，如通过车牌信息提取查询数据库的方式得到。
本实施例重点计算加油口一侧两个车轮轴心第一特征点51、第二特征点52的位置和汽车简化姿态，并基于此简化姿态计算油箱盖位置；加油口或车身其它需确定位置的部位，其定位实施步骤可参照本实施例进行。
实施例4：实施例2的一种油箱盖精确定位方法
S1：拍照，移动机械臂控制其上相机和/或控制其他不同位置上的相机，至少拍摄2张不同角度照片，使至少2个基准点和待加油车至少2个特征点同时出现在所有照片的视场中；本实施例采用实施例2的5个相机各拍摄1张照片，累计5张照片，安装于机械臂的可移动相机可以拍摄多张不同角度的照片，均可有效，本实施例以拍摄1张为例进行计算；以4个车轮的轴心和汽车前挡风玻璃的右上角、后挡风玻璃的左下角为特征点，累计6个特征点，汽车左右两侧路面各预设2个基准点，停车上方4个顶角各预设1个垂吊基准点，累计8个基准点，事实上在相机的可视且可识别范围内可以布置更多的基准点，对工作量和成本几乎不影响；每张照片一般可同时拍摄3个特征点、6个基准点。
S2：图像识别，提取基准点和待加油汽车上的特征点在图像中的坐标，图像处理和识别技术为现有成熟技术。
S3：特征点位置和汽车姿态计算，根据相机成像原理、基准点位置已知、汽车特征点在车体坐标系中相对位置已知的特点，针对基准点数量大于等于2个和/或不同位置拍摄图像数量大于等于2张的情形，利用特征点位置优化方法或模型优化计算特征点位置；针对特征点个数大于等于4个的情形，利用车身姿态优化方法或模型优化计算车身姿态；优化模型包括统计方法或其他数学方法，统计方法包括最小二乘法、数学平均法等；特征点数量不足4个时，可以用求数学平均的方法只优化汽车简化姿态角的值；特征点数量为4时，可以求得车身精确姿态的解析解；特征点数量超过4个时，可以优化车身精确姿态值，且可与解析求解用统一的方法或模型；基准点数量大于等于2个、照片大于等于2张时，可以优化计算每个特征点的位置；优化特征点结果本质上也是对车身姿态结果的优化，只不过是间接的。
S4：油箱盖定位，根据车型信息获知汽车特征点相对油箱盖和/或加油口的位置，结合本实施例步骤S3所述特征点和姿态计算结果，利用油箱盖位置计算方法或模型，计算油箱盖在加油站坐标系中的位置和/或在相机坐标系中的位置以及相对机械臂的位置，实现精确定位。车型信息、汽车各特征点及油箱盖的各自在车体坐标系下相对位置信息获知为成熟技术，如通过车牌信息提取查询数据库的方式得到；
加油口或车身其它需确定位置的部位，其精确或优化定位实施步骤可参照本实施例进行。
实施例5：以油箱盖中心为特征点的一种油箱盖精确定位方法
B1：拍照，移动机械臂控制其上相机和/或控制其它不同位置上的相机，从不同角度拍摄2张以上照片，使至少一个基准点和待加油车油箱盖同时出现在所拍照片的视场中，将油箱盖中心作为特征点，也可将油箱盖上其他易识别的点作为特征点，但最终都需要折算到油箱盖中心点去；一般拍照条件允许时，直接拍摄油箱盖并提取其中心点位置，将油箱盖中心点直接作为特征点，可大大简化油箱盖位置计算方法；通常拍摄油箱外盖即可，通过内盖与外盖、以及加油口与外盖在车体坐标系下的相对位置关系，可以得到满足加油操作精度要求的油箱内盖及加油口位置，特殊情况下可以单独补充拍摄2张以上油箱内盖或加油口的照片，所有照片均需包含至少一个基准点。相比结构光相机，普通相机在一般光学条件下对类似油箱盖的成像会更稳定、清晰，当然极亮和极暗也不合适，但解决此类光学成像条件的方法，普通相机应用更加成熟。
B2：图像识别，提取基准点和待加油汽车油箱盖上的特征点在图像中的坐标，一般以油箱盖中心为特征点，通过油箱盖边缘特征提取后用对称判决的方法计算中心点像素坐标。
B3：油箱盖位置计算，根据相机成像原理和基准点已知的特点，利用特征点位置计算方法或模型，计算待加油车油箱盖的位置；针对基准点数量超过1个和/或不同位置拍摄图像数量超过2张的情形，利用特征点位置优化方法或模型，计算油箱盖位置结果，一般利用最小二乘等统计方法或其他数学方法。
本实施例方法与实施例3、4不同点主要在于省略了车身姿态的计算过程，此省略对于通过油箱外盖获取内盖或加油口位置会有一个小的误差。一般汽车加油时车身与加油站坐标系x轴基本平行，即使有一个小的姿态角，对于油箱外盖精确定位后，直接利用车体坐标系下的相对位置计算油箱内盖和加油口位置，忽略姿态所造成的误差也较小，因为油箱外盖、内盖和加油口三者紧密相连，姿态角带来的位置计算误差也很小。特殊情况下，油箱外盖开启后，可以再增拍2张以上包含基准点和油箱内盖的照片，用同样的处理方法可以获取精确位置信息。
为了进一步阐述实施例3、4、5所述方法和模型，这里对通用概念和符号进行定义或说明。
坐标系定义：加油站坐标系——以加油站中任一固定点为原点，不失一般性，以停车区域右后角点为原点，坐标轴按东北天坐标系定义，x轴朝东，待加油车一般也按此方向前进，y轴朝北，z轴朝天，该坐标系用字母E表示；相机坐标系——以相机光心为原点，图像横向为x轴，向右为正，纵轴为y轴，向上为正，光轴为z轴，指向目标方向为正，由于机械臂与相机为固定安装关系，本发明简化认为机械臂坐标系与相机坐标系重合，该坐标系用字母J或图示相机编号表示；车体坐标系——以右后轮轴心为原点，车身方向为x轴，向前为正，车横向为y轴，向左为正，z轴垂直向上为正，该坐标系用字母C表示。以上所有三维直角坐标系均满足右手螺旋关系。图像坐标系为平面坐标系，用字母S表示，坐标轴与相机坐标系xy平面平行，原点为图像中点。
相机、机械臂、基准点坐标已知，基准点在加油站坐标系下的坐标可以事先转换为相机坐标系下的坐标。
进一步的，实施例3、5所述特征点位置计算方法或模型包括：
考虑只拍摄2张不同角度照片，记第一基准点31在加油站坐标系中的坐标为(xE,31,yE,31,zE,31)，相机2次拍照分别用编号21、22标记，第一基准点在2次不同拍照位置相机坐标系下的坐标分别为(x21,31,y21,31,z21,31)、(x22,31,y22,31,z22,31)，依据坐标转换方法或模型，可以用加油站坐标系下的坐标(xE,31,yE,31,zE,31)来表示；第一基准点在2张照片图像坐标系下的坐标分别为(xS1,31,yS1,31)、(xS2,31,yS2,31)；第一特征点51在2张照片图像坐标系下的坐标分别为(xS1,51,yS1,51)、(xS2,51,yS2,51)，该坐标值可通过图像识别测量得到，以上均为已知量；
第一特征点51在加油站坐标系中的坐标为(xE,51,yE,51,zE,51)，有3个未知量，需要求解，依据坐标转换方法或模型，第一特征点51在相机2次拍照坐标系下的坐标(x21,51,y21,51,z21,51)、(x22,51,y22,51,z22,51)可以用(xE,51,yE,51,zE,51)来表示；
所述坐标转换方法或模型包括：
Figure BDA0003542456370000201
即(x21,51,y21,51,z21,51)可以通过公式(1)得到，其中(x021,E,y021,E,z021,E)为加油站坐标系原点在相机拍照坐标系中的坐标，为平移量，Q21,E为加油站坐标系到第一次拍照的相机坐标系的旋转矩阵，
Figure BDA0003542456370000202
其中，3个欧拉角俯仰α、偏航β、滚动γ用来表征旋转矩阵，由于相机固连于机械臂或安装于已知固定位置，且机械臂运动为受控运动，Q21,E和(x021,E,y021,E,z021,E)均为已知量。所有特征点、基准点的坐标转换方法或模型类同公式(1)；
依据相机简化成像模型有
Figure BDA0003542456370000211
Figure BDA0003542456370000212
Figure BDA0003542456370000213
Figure BDA0003542456370000214
将由公式(1)和公式(2)方法或模型得到的(x21,51,y21,51,z21,51)、(x22,51,y22,51,z22,51)、(x21,31,y21,31,z21,31)、(x22,31,y22,31,z22,31)分别代入公式(3)～公式(6)，4个方程求解3个未知数，采用忽略任意一个方程直接计算解析结果或用最小二乘法或计算机数值法求解均可得到(xE,51,yE,51,zE,51)；
同理，求得所有特征点在加油站坐标系下的坐标。
进一步的，实施例3所述车身简化姿态计算方法或模型包括：
如图4所示，定义第一特征点51、第二特征点52分别为待加油车右后轮轴心和右前轮轴心，计算车身简化姿态，假设车身前后左右均处于平衡状态，即4个车轮轴心形成的平面与加油站路面平行，不失一般性，均认为水平状态，因此车身姿态主要是车身方向即x轴方向与加油站坐标系x轴间存在夹角θ，定义该夹角以加油站坐标系x轴为基准，绕z轴逆时针旋转为正，顺时针旋转为负，有
Figure BDA0003542456370000215
其中，L为前后轮的轮距，为已知量。
进一步的，实施例4、5所述特征点位置优化方法或模型和实施例4所述车身姿态优化方法或模型包括：
特征点位置优化方法或模型：记n为不同角度照片张数，n≥2；m为定义的特征点个数，对于实施例4，m≥4，对于实施例5，m＝1；w为设计的基准点个数，w≥2；采用考虑相机主点与成像中心点偏差的成像模型，横纵向主点偏差分别记为xp0,21、yp0,21、…、xp0,2i、yp0,2i、…、xp0,2n、yp0,2n，焦距记为f21、…、f2i、…、f2n，图像横纵向比例因子分别为mx21、my21、…、mx2i、my2i、…、mx2n、my2n，n≥i≥1，每张照片二维像素坐标系用Si来标识；特征点序列号用5j来标识，m≥j≥1；基准点序列号用3k来标识，w≥k≥1；每张照片的每一特征点和每一个基准点分别写出2个方程：
Figure BDA0003542456370000221
Figure BDA0003542456370000222
Figure BDA0003542456370000223
Figure BDA0003542456370000224
其中(x2i,5j,y2i,5j,z2i,5j)、(x2i,3k,y2i,3k,z2i,3k)通过公式(1)和公式(2)的方法或模型用坐标转换来表示，即
Figure BDA0003542456370000225
Figure BDA0003542456370000226
旋转矩阵Q2i,E和坐标平移量(x02i,E,y02i,E,z02i,E)事先可测或可计算，均为已知量；特征点和基准点像素坐标(xSi,5j,ySi,5j)、(xSi,3k,ySi,3k)为图像识别目标提取后的结果，亦为已知量；
考虑每张照片相机参数包括主点偏差、比例因子、焦距共5个未知数，每个特征点3个未知数，对于n张照片、m个特征点、w个基准点共有(5n+3m)个未知数，参考公式(8)～公式(11)可以建立(2q+2p)个方程，其中q为所有照片拍摄识别的特征点总数，p为所有照片拍摄识别的基准点总数量；在保证(2q+2p)＞＝(5n+3m)的情况下，即可采用最小二乘或其他数学优化方法对方程进行求解；
根据实施例2的设置和实施例4的步骤，累计设置8个基准点、6个特征点，一般每张照片可拍摄6个基准点、3个特征点，按拍摄5张照片计算，累计有43个未知数、90个方程；事实上由于特征点两两距离已知，还有w＊(w－1)/2个方程可用，即实际方程总数为105个，因此可以优化求解特征点位置；
根据实施例5的步骤，n＞＝2，m＝1，q＝n，在保证(2n+2p)＞＝(5n+3)的情况下，可以采用最小二乘或数学优化方法对方程进行求解；如拍摄2张照片、设置3个基准点，则有16个方程，求解13个未知数。
车身姿态优化方法或模型：用车体坐标系相对加油站坐标系的旋转欧拉角表征车身姿态，累计3个未知数，依据车体坐标系向加油站坐标系转换的模型
Figure BDA0003542456370000231
其中C表示车体坐标系，QE,C为车体坐标系到加油站坐标系的旋转矩阵，有3个欧拉角未知量；(xC,5j,yC,5j,zC,5j)为第j个特征点在车体坐标系下的坐标，为已知量；(x0E,C,y0E,C,z0E,C)为车体坐标系原点在加油站坐标系下的坐标，可以用特征点之一如第一特征点51作为坐标原点，此亦为已知量；根据特征点位置优化方法或模型，所有特征点位置已求解完成，则除作为原点的第一特征点外所有特征点形成的如公式(14)的方程联立，累计大于等于4个特征点的情况下，采用最小二乘或数学优化方法对联立方程进行求解；一般4个特征点可以求得解析解，5个及以上特征点可以求得优化解，二者方法可统一。
另根据公式(8)～公式(11)的特点，还可以采用变量替换法、方程分类联立法等数学方法进一步简化求解过程。
进一步的，实施例3、4所述油箱盖位置计算方法或模型包括：
根据车身简化姿态定义，记车体坐标系下，油箱盖相对后轮轴心在x、y、z方向的位移分别为xC,g、yC,g、zC,g，其中C表示车体坐标系，g表示油箱盖，则油箱盖在加油站坐标系下的坐标为：
Figure BDA0003542456370000241
或者根据车身精确姿态计算油箱盖精确位置，由于QE,C、(x0E,C,y0E,C,z0E,C)以及油箱盖在车体坐标系下的坐标(xC,g,yC,g,zC,g)均已知，利用公式(14)的方法或模型即可得到油箱盖的精确位置。
加油口或车身其他部位的位置计算方法同油箱盖位置计算方法，只是它们相对后轮轴心在车体坐标系x、y、z方向的位移与油箱盖不同。
本发明的基本原理是利用相机多角度拍照成像，根据成像目标空间相对位置固定不变的特性和相机成像原理，可以实现空间目标的三维重构。本发明将图像三维重构技术应用于汽车自动加油领域，通过设置已知的基准点、定义车身若干特征点，利用普通相机从多个不同角度成像，根据同一目标点在不同照片、不同视向交叉重叠的原理，即可实现车身特别是重点部位如油箱盖和加油口等的精确定位，从而可简化自动加油领域现有的操作流程和设备配置，提高性价比。本发明提出了汽车特征点的概念，2个不同的特征点可以确定车身的二维简化姿态，4个不在同一平面内的特征点可以确定车身三维精确姿态，即将汽车定位和姿态确定问题转换为一个明确的数学问题，并且对汽车特征点选择给出明确建议，可以进一步提高图像处理的效率。
综上所述，本发明提供了一种自动加油用油箱盖精确定位装置及方法，可以简化汽车自动加油领域定位油箱盖的设备配置、降低成本，处理方法以软件技术为主，可实现机械臂或加油机器人对开关油箱盖以及加油操作更加简单、精准的控制。本发明中基准点的选取和确定简单、易实施，定位采用普通相机拍照即可完成，无需压力传感器、深度相机、测距传感器等复杂设备，当然采用此类设备可以进一步提高定位精度，但实际意义不大，基本方法也不脱离本发明的保护范围。本发明的应用可以提高自动加油的效率和设备性价比。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和润饰，包括采用不同的相机成像模型可以得出不同的定位计算方法，这些改进和润饰也应视为本发明的保护范围。

Claims (12)

1.一种自动加油用油箱盖精确定位装置，其特征在于，所述装置包括机械臂(1)、相机(2)、基准点(3)、控制系统(4)，所述相机至少为一部，若为一部，其相机安装于所述机械臂上，2部以上，其相机安装在待加油车周边不同位置；所述机械臂、相机与所述控制系统相连，受控制系统控制；所述基准点包括预设在待加油车附近地面和/或空中的标记点位。
2.根据权利要求1所述的一种自动加油用油箱盖精确定位装置，其特征在于，所述基准点为光学标志点，区别于周围环境，用于相机拍照定位识别，车辆待加油区域至少布置有一个基准点，若为多个基准点，则每个基准点分别标记。
3.根据权利要求1所述的一种自动加油用油箱盖精确定位装置，其特征在于，所述相机包括普通相机和/或结构光相机、双目相机，相机安装位置便于同时拍摄至少1个基准点和待加油车的至少2个特征点，或同时拍摄至少1个基准点和一个油箱盖上或油箱盖边缘相关的特征点；所述特征点包括待加油汽车上固有的、不变的、易图像识别的标志性点位。
4.根据权利要求1所述的一种自动加油用油箱盖精确定位装置，其特征在于，所述控制系统包括计算机类控制系统。
5.一种采用自动加油用油箱盖精确定位装置的精确定位方法，其特征在于，所述方法包括以下步骤：
S1：拍照，移动机械臂控制其上相机和/或控制其它不同位置上的相机，从不同角度拍摄2张以上照片，使至少一个基准点和待加油车至少2个特征点同时出现在所拍照片的视场中；
S2：图像识别，提取基准点和待加油汽车上的特征点在图像中的坐标；
S3：特征点位置和汽车姿态计算，根据相机成像原理、基准点位置已知、汽车特征点在车体坐标系中相对位置已知的特点，利用特征点位置计算方法或模型，计算待加油汽车特征点在加油站坐标系中的位置和/或在相机坐标系中的位置以及相对机械臂的位置；利用姿态计算方法或模型，计算待加油车简化姿态或精确姿态；针对基准点数量大于等于2个和/或不同位置拍摄图像数量大于等于2张的情形，利用特征点位置优化方法或模型优化计算特征点位置；针对特征点个数大于等于4个的情形，利用车身姿态优化方法或模型优化计算车身姿态；
S4：油箱盖定位，根据车型信息获知汽车特征点相对油箱盖的位置，结合步骤S3所述特征点和姿态计算结果，利用油箱盖位置计算方法或模型，计算油箱盖在加油站坐标系中的位置和/或在相机坐标系中的位置以及相对机械臂的位置。
6.根据权利要求5所述的一种自动加油用油箱盖精确定位方法，其特征在于，所述特征点位置计算方法或模型包括：
考虑只拍摄2张不同角度照片，记第一基准点(31)在加油站坐标系中的坐标为(xE,31,yE,31,zE,31)，相机2次拍照分别用编号(21)、(22)标记，第一基准点在不同拍照位置相机坐标系下的坐标分别为(x21,31,y21,31,z21,31)、(x22,31,y22,31,z22,31)，依据坐标转换方法或模型，与加油站坐标系下的坐标(xE,31,yE,31,zE,31)存在坐标转换关系；第一基准点在2张照片图像坐标系下的坐标分别为(xS1,31,yS1,31)、(xS2,31,yS2,31)；第一特征点(51)在2张照片图像坐标系下的坐标分别为(xS1,51,yS1,51)、(xS2,51,yS2,51)，该坐标值通过图像识别测量得到，以上均为已知量；
第一特征点(51)在加油站坐标系中的坐标为(xE,51,yE,51,zE,51)，有3个未知量，需要求解，依据坐标转换方法或模型，第一特征点(51)在相机2次拍照坐标系下的坐标(x21,51,y21,51,z21,51)、(x22,51,y22,51,z22,51)用(xE,51,yE,51,zE,51)来表示；
所述坐标转换方法或模型包括：
Figure FDA0003542456360000031
即(x21,51,y21,51,z21,51)通过公式1得到，其中(x021,E,y021,E,z021,E)为加油站坐标系原点在相机拍照坐标系中的坐标，为平移量，Q21,E为加油站坐标系到第一次拍照的相机坐标系的旋转矩阵，
Figure FDA0003542456360000032
其中，3个欧拉角俯仰α、偏航β、滚动γ用来表征旋转矩阵，Q21,E和(x021,E,y021,E,z021,E)均为机械臂受控运动过程中的已知量；所有特征点、基准点的坐标转换方法或模型类同公式(1)；
依据相机简化成像模型有
Figure FDA0003542456360000033
Figure FDA0003542456360000034
Figure FDA0003542456360000035
Figure FDA0003542456360000036
将由公式(1)和公式(2)方法或模型得到的(x21,51,y21,51,z21,51)、(x22,51,y22,51,z22,51)、(x21,31,y21,31,z21,31)、(x22,31,y22,31,z22,31)分别代入公式(3)～公式(6)，4个方程求解3个未知数，采用忽略任意一个方程直接计算解析结果或用最小二乘法或计算机数值法求解均可得到(xE,51,yE,51,zE,51)；
同理，求得所有特征点在加油站坐标系下的坐标。
7.根据权利要求5所述的一种自动加油用油箱盖精确定位方法，其特征在于，所述车身简化姿态计算方法或模型包括：
定义第一特征点(51)、第二特征点(52)分别为待加油车后轮轴心和前轮轴心，车身简化姿态主要是车身方向即x轴方向与加油站坐标系x轴间存在夹角θ，定义该夹角以加油站坐标系x轴为基准，绕z轴逆时针旋转为正，顺时针旋转为负，有
Figure FDA0003542456360000041
其中，L为前后轮的轮距。
8.根据权利要求5所述的一种自动加油用油箱盖精确定位方法，其特征在于，所述特征点位置和车身姿态优化方法或模型包括：
特征点位置优化方法或模型：记n为不同角度照片张数，n≥2；m为定义的特征点个数，m≥4；w为设计的基准点个数，w≥2；采用考虑相机主点与成像中心点偏差的成像模型，横纵向主点偏差分别记为xp0,21、yp0,21、…、xp0,2i、yp0,2i、…、xp0,2n、yp0,2n，焦距记为f21、…、f2i、…、f2n，图像横纵向比例因子分别为mx21、my21、…、mx2i、my2i、…、mx2n、my2n，n≥i≥1，每张照片二维像素坐标系用Si来标识；特征点序列号用5j来标识，m≥j≥1；基准点序列号用3k来标识，w≥k≥1；每张照片的每一特征点和每一个基准点分别写出2个方程：
Figure FDA0003542456360000042
Figure FDA0003542456360000043
Figure FDA0003542456360000044
Figure FDA0003542456360000045
其中(x2i,5j,y2i,5j,z2i,5j)、(x2i,3k,y2i,3k,z2i,3k)通过公式(1)和公式(2)的方法或模型用坐标转换来表示，即
Figure FDA0003542456360000051
Figure FDA0003542456360000052
旋转矩阵Q2i,E和坐标平移量(x021,E,y021,E,z021,E)均为已知量；特征点和基准点像素坐标(xSi,5j,ySi,5j)、(xSi,3k,ySi,3k)为图像识别目标提取后的结果，亦为已知量；
每张照片相机参数包括主点偏差、比例因子、焦距共5个未知数，每个特征点3个未知数，对于n张照片、m个特征点、w个基准点共有(5n+3m)个未知数，参考公式(8)～公式(11)可以建立(2q+2p)个方程，其中q为所有照片拍摄识别的特征点总数，p为所有照片拍摄识别的基准点总数量；在保证(2q+2p)＞＝(5n+3m)的情况下，采用最小二乘或数学优化方法对方程进行求解。
车身姿态优化方法或模型：用车体坐标系相对加油站坐标系的旋转欧拉角表征车身姿态，累计3个未知数，依据车体坐标系向加油站坐标系转换的模型
Figure FDA0003542456360000053
其中C表示车体坐标系，QE,C为车体坐标系到加油站坐标系的旋转矩阵，有3个欧拉角未知量；(xC,5j,yC,5j,zC,5j)为第j个特征点在车体坐标系下的坐标，为已知量；(x0E,C,y0E,C,z0E,C)为车体坐标系原点在加油站坐标系下的坐标，以特征点之一如第一特征点(51)作为坐标原点，此亦为已知量；根据特征点位置优化方法或模型，所有特征点位置已求解完成，则除作为原点的第一特征点外所有特征点形成的如公式(14)的方程联立，累计大于等于4个特征点的情况下，采用最小二乘或数学优化方法对方程进行求解。
9.根据权利要求5所述的一种自动加油用油箱盖精确定位方法，其特征在于，所述油箱盖位置计算方法或模型包括：
根据车身简化姿态定义，记车体坐标系下，油箱盖相对后轮轴心在x、y、z方向的位移分别为xC,g、yC,g、zC,g，其中C表示车体坐标系，g表示油箱盖，则油箱盖在加油站坐标系下的坐标为：
Figure FDA0003542456360000061
10.一种采用自动加油用油箱盖精确定位装置的精确定位方法，其特征在于，所述方法包括以下步骤：
B1：拍照，移动机械臂控制其上相机和/或控制其它不同位置上的相机，从不同角度拍摄2张以上照片，使至少一个基准点和待加油车油箱盖同时出现在所拍照片的视场中；
B2：图像识别，提取基准点和待加油汽车油箱盖上的特征点在图像中的坐标；
B3：油箱盖位置计算，根据相机成像原理和基准点已知的特点，利用特征点位置计算方法或模型，计算待加油车油箱盖的位置；针对基准点数量超过1个和/或不同位置拍摄图像数量超过2张的情形，利用特征点位置优化方法或模型，计算油箱盖位置结果。
11.根据权利要求10所述的一种自动加油用油箱盖精确定位方法，其特征在于，所述特征点位置计算方法或模型包括：
考虑只拍摄2张不同角度照片，记第一基准点(31)在加油站坐标系中的坐标为(xE,31,yE,31,zE,31)，相机2次拍照分别用编号(21)、(22)标记，第一基准点在不同拍照位置相机坐标系下的坐标分别为(x21,31,y21,31,z21,31)、(x22,31,y22,31,z22,31)，依据坐标转换方法或模型，与加油站坐标系下的坐标(xE,31,yE,31,zE,31)存在坐标转换关系；第一基准点在2张照片图像坐标系下的坐标分别为(xS1,31,yS1,31)、(xS2,31,yS2,31)；第一特征点(51)在2张照片图像坐标系下的坐标分别为(xS1,51,yS1,51)、(xS2,51,yS2,51)，该坐标值通过图像识别测量得到，以上均为已知量；
第一特征点(51)在加油站坐标系中的坐标为(xE,51,yE,51,zE,51)，有3个未知量，需要求解，依据坐标转换方法或模型，第一特征点(51)在相机2次拍照坐标系下的坐标(x21,51,y21,51,z21,51)、(x22,51,y22,51,z22,51)用(xE,51,yE,51,zE,51)来表示；
所述坐标转换方法或模型包括：
Figure FDA0003542456360000071
即(x21,51,y21,51,z21,51)通过公式1得到，其中(x021,E,y021,E,z021,E)为加油站坐标系原点在相机拍照坐标系中的坐标，为平移量，Q21,E为加油站坐标系到第一次拍照的相机坐标系的旋转矩阵，
Figure FDA0003542456360000072
其中，3个欧拉角俯仰α、偏航β、滚动γ用来表征旋转矩阵，Q21,E和(x021,E,y021,E,z021,E)均为机械臂受控运动过程中的已知量；所有特征点、基准点的坐标转换方法或模型类同公式(1)；
依据相机简化成像模型有
Figure FDA0003542456360000073
Figure FDA0003542456360000074
Figure FDA0003542456360000075
Figure FDA0003542456360000076
将由公式(1)和公式(2)方法或模型得到的(x21,51,y21,51,z21,51)、(x22,51,y22,51,z22,51)、(x21,31,y21,31,z21,31)、(x22,31,y22,31,z22,31)分别代入公式(3)～公式(6)，4个方程求解3个未知数，采用忽略任意一个方程直接计算解析结果或用最小二乘法或计算机数值法求解均可得到(xE,51,yE,51,zE,51)。
12.根据权利要求10所述的一种自动加油用油箱盖精确定位方法，其特征在于，所述特征点位置优化方法或模型包括：
记n为不同角度照片张数，n≥2；w为设计的基准点个数，w≥2；采用考虑相机主点与成像中心点偏差的成像模型，横纵向主点偏差分别记为xp0,21、yp0,21、…、xp0,2i、yp0,2i、…、xp0,2n、yp0,2n，焦距记为f21、…、f2i、…、f2n，图像横纵向比例因子分别为mx21、my21、…、mx2i、my2i、…、mx2n、my2n，n≥i≥1，每张照片二维像素坐标系用Si来标识；仅一个特征点，用序列号用51来标识；基准点序列号用3k来标识，w≥k≥1；每张照片的每一特征点和每一个基准点分别写出2个方程：
Figure FDA0003542456360000081
Figure FDA0003542456360000082
Figure FDA0003542456360000083
Figure FDA0003542456360000084
其中(x2i,51,y2i,51,z2i,51)、(x2i,3k,y2i,3k,z2i,3k)通过公式(1)和公式(2)的方法或模型用坐标转换来表示，即
Figure FDA0003542456360000085
Figure FDA0003542456360000086
旋转矩阵Q2i,E和坐标平移量(x021,E,y021,E,z021,E)均为已知量；特征点和基准点像素坐标(xSi,51,ySi,51)、(xSi,3k,ySi,3k)为图像识别目标提取后的结果，亦为已知量；
公式每张照片相机参数包括主点偏差、比例因子、焦距共5个未知数，每个特征点3个未知数，对于n张照片、1个特征点、w个基准点共有(5n+3)个未知数，参考公式(8)～公式(11)可以建立(2n+2p)个方程，其中p为所有照片拍摄识别的基准点总数量；在保证(2n+2p)＞＝(5n+3)的情况下，采用最小二乘或数学优化方法对方程进行求解。
CN202210241756.XA 2022-03-11 2022-03-11 一种自动加油用油箱盖精确定位装置及方法 Active CN114644315B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210241756.XA CN114644315B (zh) 2022-03-11 2022-03-11 一种自动加油用油箱盖精确定位装置及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210241756.XA CN114644315B (zh) 2022-03-11 2022-03-11 一种自动加油用油箱盖精确定位装置及方法

Publications (2)

Publication Number Publication Date
CN114644315A true CN114644315A (zh) 2022-06-21
CN114644315B CN114644315B (zh) 2023-06-06

Family

ID=81992714

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210241756.XA Active CN114644315B (zh) 2022-03-11 2022-03-11 一种自动加油用油箱盖精确定位装置及方法

Country Status (1)

Country Link
CN (1) CN114644315B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115239855A (zh) * 2022-06-23 2022-10-25 安徽福斯特信息技术有限公司 一种基于移动终端的虚拟手语主播生成方法、装置及系统
CN116228890A (zh) * 2023-05-05 2023-06-06 深圳市拓普泰克技术股份有限公司 一种基于加油设备的智能控制系统

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0796819A (ja) * 1993-09-30 1995-04-11 Tokico Ltd 給油装置
JPH07101500A (ja) * 1993-09-30 1995-04-18 Tokico Ltd 給油ロボット装置
JP2002255291A (ja) * 2001-02-23 2002-09-11 Tatsuno Corp 自動給油装置
JP2006069549A (ja) * 2004-08-31 2006-03-16 Shibaura Institute Of Technology 自動給油装置
CN104180808A (zh) * 2014-08-05 2014-12-03 南京航空航天大学 一种用于自主空中加油的圆形锥套视觉位姿解算方法
CN110340887A (zh) * 2019-06-12 2019-10-18 西安交通大学 一种基于图像的加油机器人视觉引导的方法
CN110713161A (zh) * 2018-07-12 2020-01-21 杭州海康威视数字技术股份有限公司 加油系统
CN111646420A (zh) * 2020-05-25 2020-09-11 无锡八英里电子科技有限公司 基于人工智能的智能加油方法
CN113610922A (zh) * 2021-08-06 2021-11-05 北京理工大学 一种用于自动加油机器人加油盖位姿估计的方法
CN114022551A (zh) * 2021-10-28 2022-02-08 北京理工大学 一种燃油车加油盖精确识别与位姿估计的方法

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0796819A (ja) * 1993-09-30 1995-04-11 Tokico Ltd 給油装置
JPH07101500A (ja) * 1993-09-30 1995-04-18 Tokico Ltd 給油ロボット装置
JP2002255291A (ja) * 2001-02-23 2002-09-11 Tatsuno Corp 自動給油装置
JP2006069549A (ja) * 2004-08-31 2006-03-16 Shibaura Institute Of Technology 自動給油装置
CN104180808A (zh) * 2014-08-05 2014-12-03 南京航空航天大学 一种用于自主空中加油的圆形锥套视觉位姿解算方法
CN110713161A (zh) * 2018-07-12 2020-01-21 杭州海康威视数字技术股份有限公司 加油系统
CN110340887A (zh) * 2019-06-12 2019-10-18 西安交通大学 一种基于图像的加油机器人视觉引导的方法
CN111646420A (zh) * 2020-05-25 2020-09-11 无锡八英里电子科技有限公司 基于人工智能的智能加油方法
CN113610922A (zh) * 2021-08-06 2021-11-05 北京理工大学 一种用于自动加油机器人加油盖位姿估计的方法
CN114022551A (zh) * 2021-10-28 2022-02-08 北京理工大学 一种燃油车加油盖精确识别与位姿估计的方法

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115239855A (zh) * 2022-06-23 2022-10-25 安徽福斯特信息技术有限公司 一种基于移动终端的虚拟手语主播生成方法、装置及系统
CN116228890A (zh) * 2023-05-05 2023-06-06 深圳市拓普泰克技术股份有限公司 一种基于加油设备的智能控制系统
CN116228890B (zh) * 2023-05-05 2023-08-11 深圳市拓普泰克技术股份有限公司 一种基于加油设备的智能控制系统

Also Published As

Publication number Publication date
CN114644315B (zh) 2023-06-06

Similar Documents

Publication Publication Date Title
CN114644315A (zh) 一种自动加油用油箱盖精确定位装置及方法
CN110148169B (zh) 一种基于ptz云台相机的车辆目标三维信息获取方法
JP4201059B2 (ja) １つまたは複数の移動カメラの初期位置および向きを校正するための方法ならびに固定対象の３次元的位置測定に対してのこの方法の応用
CN106651953B (zh) 一种基于交通指示牌的车辆位姿估计方法
CN104200086B (zh) 宽基线可见光相机位姿估计方法
CN109074668A (zh) 路径导航方法、相关装置及计算机可读存储介质
ES2549962T3 (es) Procedimiento de calibrado de un sistema de visión por ordenador de a bordo en un móvil
CN109685855B (zh) 一种道路云监控平台下的摄像机标定优化方法
CN108733039A (zh) 一种机器人室内导航定位的方法与装置
CN108226938A (zh) 一种agv小车的定位系统和方法
CN107862719A (zh) 相机外参的标定方法、装置、计算机设备和存储介质
CN101556152B (zh) 交通事故现场图像数据信息采集及处理方法
CN110595476A (zh) 一种基于gps和图像视觉融合的无人机降落导航方法及装置
CN109446973A (zh) 一种基于深度神经网络图像识别的车辆定位方法
CN108230393A (zh) 一种智能车前方运动车辆的测距方法
US10964059B2 (en) Static camera calibration using motion of vehicle portion
CN109871739B (zh) 基于yolo-sioctl的机动站自动目标检测与空间定位方法
CN113129280B (zh) 一种基于建筑物轮廓特征的目标落点测量方法
CN112819711B (zh) 一种基于单目视觉的利用道路车道线的车辆反向定位方法
CN113610922A (zh) 一种用于自动加油机器人加油盖位姿估计的方法
CN115343299B (zh) 一种轻量化公路隧道集成检测系统及方法
CN113409396A (zh) 一种adas单目相机的标定方法
CN105424059A (zh) 宽基线近红外相机位姿估计方法
CN110415299B (zh) 一种运动约束下基于设定路牌的车辆位置估计方法
CN116520295A (zh) 一种基于自动建图的多激光雷达标定方法

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