CN116026323A - 一种用于发动机油加注机定位与区域防错方法 - Google Patents

一种用于发动机油加注机定位与区域防错方法 Download PDF

Info

Publication number
CN116026323A
CN116026323A CN202211664619.3A CN202211664619A CN116026323A CN 116026323 A CN116026323 A CN 116026323A CN 202211664619 A CN202211664619 A CN 202211664619A CN 116026323 A CN116026323 A CN 116026323A
Authority
CN
China
Prior art keywords
positioning
uwb
data
imu
information
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.)
Pending
Application number
CN202211664619.3A
Other languages
English (en)
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.)
Individual
Original Assignee
Individual
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 Individual filed Critical Individual
Priority to CN202211664619.3A priority Critical patent/CN116026323A/zh
Publication of CN116026323A publication Critical patent/CN116026323A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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

  • Navigation (AREA)

Abstract

本发明公开了一种用于发动机油加注机定位与区域防错方法，包括如下：S1：系统初始化；S2：获取UWB数据和IMU测量数据，S3：将导航结果与UWB输出的位置信息采用松耦合的方式进行一次卡尔曼滤波，得到最优估计下的定位结果；S4：通过三角化过程获得二维图像上对应点的世界三维坐标；S5：利用UWB和相机的坐标信息进行二次卡尔曼滤波融合；S6：将S3得到的数据与S5得到的滤波结果进行无重置联邦滤波，得到最终的定位结果；S7：将松耦合UWB和IMU数据的融合定位信息与视觉图像定位信息进行松耦合，形成最终的定位结果。本发明利用多源信息融合，弥补了单一定位方法的不足，整体系统提升了机器人定位方面的鲁棒性和精确性。

Description

一种用于发动机油加注机定位与区域防错方法
技术领域
本发明涉及汽车发动机加油技术领域，具体涉及一种用于发动机油加注机定位与区域防错方法。
背景技术
发动机在整机厂生产完成之后需要加注定量燃油，为后续发动机和整机测试提供动力燃油，传统的发动机燃油加注是通过人工方法手动加注，依次对生产线上的发动机实施加注，但是加注过程中存在由于人为失误导致漏加或者重复加注的情况，且此方法自动化程度不高，加注效率低。为了防止加注工程中的漏加或错加情况发生，提高发动机加注机的智能化水平，需要获取加油枪的实时位置，当超过加注区域的时候发出报警提示。实现以上功能的关键在于对加油枪的实时定位。位置信息服务是智能制造设备的重要信息，传统的位置信息服务由全球定位系统（Global Positioning System，GPS）和惯性测量单元（Inertial Measurement Unit，IMU）提供，然而室内的定位由于特殊环境的限制，导致GPS无法接收信号，限制了其进一步应用，超宽带(ultra-wideband，UWB)定位技术具有带宽宽、定位精度高、穿透力强等特点。在单路径传播环境中，节点之间没有干扰信号且没有障碍物，可以执行极其精确的距离估计。然而，在实际工厂车间环境中，信号经由多个路径到达接收器，并且环境中存在金属设备干扰信源和障碍物。IMU利用陀螺仪和加速度计量测载体的角速度和比力信息，短期导航定位精度较高，但其误差随时间累积。UWB／IMU组合既能利用IMU解算的结果减少UWB的干扰误差影响，又能利用UWB的测距或解算信息抑制INS中随时间累积的误差迅速，从而获得更高的导航定位精度以及更丰富的导航信息方法和原理。双目视觉利用图像配准和跟踪技术获取实时位移和速度，不受现场障碍物的影响，但是会受到质地和光线的影响。IMU和双目视觉都是获取相对位置，会产生积累误差，而UWB则是绝对位置信息，没有时间累积误差。因此本发明提出将UWB-IMU-视觉组合定位用于加油枪的实时定位，提高加油枪室内定位的精度和稳定性。
发明内容
本发明的目的在于提供一种用于发动机油加注机定位与区域防错方法，以解决当前室内发动机加油枪定位的非视距误差和鲁棒性差的问题。
本发明是通过以下技术方案来实现：一种用于发动机油加注机定位与区域防错方法，其特征在于，包括以下步骤：
S1：系统初始化。根据现场的环境，安装好UWB定位基站，测量基站之间的相对距离，建立UWB位置坐标体系，确定各基站的绝对坐标。对双目视觉系统进行标定，获取相机的内参矩阵、外参矩阵、畸变矩阵，建立相机坐标系和UWB系的转换矩阵。根据现场发动机的分布区域和油枪的位置范围确定定位的边界。
S2：将UWB-IMU-视觉装置安装于加油枪顶部，静置（静置时间5-10分钟），获取当前时刻的UWB位置数据和IMU测量数据，其中，所述IMU测量数据包括加速度计输出数据和陀螺仪输出数据。其中根据加速度计静止时间段内的输出均值，可获得初始姿态信息；
S3：在S2中获取到初始姿态角和初始位置信息后，惯导进入导航状态，并且，将IMU导航结果与UWB输出的位置数据采用松耦合的方式进行一次卡尔曼滤波，得到最优估计下的初步定位结果；
S4：获取双目视觉的图像数据，建立视觉即时定位与地图构建(SimultaneousLocalization and Mapping, SLAM)模型，根据采集到的图像来计算出相机的运动姿态。对相机采集到的图像帧进行特征点的提取，此外还会进行特征匹配，即确定当前得到的特征点与之前获得的特征点之间的对应关系。当获得到已经通过算法进行匹配好的特征点对后，则依据他们对相机的运动进行估计。通过两帧图像中的信息估计出物体的实际位姿，即通过三角化过程获得二维图像上对应点的世界三维坐标；
S5：将相机坐标系的坐标转换为UWB坐标下的坐标位置，利用UWB和相机的坐标信息进行二次卡尔曼滤波融合；
S6：将S3得到的数据与S5得到的滤波结果进行无重置联邦滤波，得到最终的定位结果；
S7：将松耦合UWB和IMU数据的融合定位信息与视觉图像定位信息进行松耦合，形成最终的定位结果，当定位结果超出S1中确定的定位的边界时，产生警示信息。
本发明UWB定位数据为绝对坐标位置，用以抑制其他两种传感器的误差积累，IMU和视觉图像获取相对定位信息，为UWB绝对位置信息进行定位坐标优化。
UWB系统定位基站在室内遮掩物较少时，可放置于离地2.8m高及以上的位置，当高处障碍物较多时，可根据实际情况调整定位基站的位置和姿态，以取得较好的信号接收效果。
定位系统启动时，需要静置5-10分钟，用以惯性导航模块数据稳定和导航初始姿态确定。
在扩展卡尔曼滤波框架下，UWB和IMU原始数据融合需要根据时间戳信息实现时钟同步。
本发明可以解决当前室内发动机加油枪定位的非视距误差和鲁棒性差的问题，提高加油枪室内定位的精度和稳定性。当定位结果超出S1中确定的定位的边界时，产生警示信息，进行区域防错。
附图说明
图1是本发明一种应用实例的模型俯视示意图；
图2是本发明一种应用实例的模型斜侧45度角示意图；
图3是本发明实施的原理流程图；
图4是本发明实施例的UWB定位结果和真实路径对比；
图5是本发明实施例的IMU定位结果和真实路径对比；
图6是本发明实施例的双目视觉定位结果和真实路径对比；
图7是本发明实施例的UWB-IMU-视觉融合定位结果和真实路径对比；
图8是四种定位方法的定位误差累计分布函数对比图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明了，以下结合附图及实施例对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明，即所描述的实施例仅仅为本发明一部分实施例，而不是全部实施例。
本发明包括以下步骤：
S1：系统初始化。根据现场的环境，安装好UWB定位基站，测量基站之间的相对距离，建立UWB位置坐标体系，确定各基站的绝对坐标。对双目视觉系统进行标定，获取相机的内参矩阵、外参矩阵、畸变矩阵，建立相机坐标系和UWB系的转换矩阵。根据现场发动机的分布区域和油枪的位置范围确定定位的边界，确定已加油区、加油区和未加油区的位置范围。
S11相机标定采用张正友标定法，利用棋盘格标定板，在得到一张标定板的图像之后，可以利用相应的图像检测算法得到每一个角点的像素坐标 ( u,v)，根据设定好的标定板的世界坐标系计算得到每一个角点在世界坐标系下的物理坐标，利用像素坐标和物理坐标来完成相机的标定，获得相机的内外参矩阵、畸变参数。
矩阵 K表示相机的内参矩阵， P表示点P在相机坐标下的运动， P W 表示P在世界坐标系下的坐标， R和 t表示相机的外参矩阵。Z表示物体到镜头之间的距离， u v表示物体成像在像素平面的坐标。
S12对相机进行畸变标定
首先对图像平面进行归一化，然后进行投影。设归一化坐标为。
第二步:对归一化平面上的点计算径向畸变和切向畸变
和是畸变后点的归一化坐标。 r表示P在极坐标下与坐标系原点之间的距离， k 1 , k 2 , k 3, p 1,p2为畸变参数。
第三步:将畸变后的点通过内参数矩阵投影到像素平面，即可得到该点在图像上的位置。
S2：将UWB-IMU-视觉装置安装于加油枪顶部，静置5-10分钟，获取当前时刻的UWB数据和IMU测量数据，其中，所述IMU测量数据包括加速度计输出数据和陀螺仪输出数据。其中根据加速度计静止时间段内的输出均值，可获得初始姿态信息，具体方法如下：
式中，分别为俯仰角和横滚角；分别为X轴Y轴和Z轴加速度计静止阶段的输出均值；为重力加速度计值，可取为9.8。
将UWB静止阶段的位置输出均值作为惯导的初始位置信息，记为。
S3：在S2中获取到初始姿态角和初始位置信息后，惯导进入导航状态，并且，将导航结果与UWB输出的位置信息采用松耦合的方式进行一次卡尔曼滤波，得到最优估计下的定位结果；
选择惯导解算相关误差状态包括平台失准角、速度误差、位置误差、陀螺零偏和加速度计零位；建立15维卡尔曼滤波的状态变量
在此基础上建立卡尔曼滤波器状态方程
式中为导航坐标系n下等效的陀螺和加速度计零位噪声；FI 为根据经典捷联惯导误差方程建立的15×15维状态转移矩阵。
选取导航解算位置误差作为量测值，建立卡尔曼滤波器量测方程
式中分别为惯导输出位置结果和UWB的位置输出， 为位置量测噪声。
S4：获取双目视觉的图像数据，建立视觉即时定位与地图构建(SimultaneousLocalization and Mapping, SLAM)模型，根据采集到的图像来计算出相机的运动姿态。对相机采集到的图像帧进行特征点的提取，然后进行特征匹配，即确定当前得到的特征点与之前获得的特征点之间的对应关系。当获得到已经通过算法进行匹配好的特征点对后，则依据他们对相机的运动进行估计。通过两帧图像中的信息估计出物体的实际位姿，即通过三角化过程获得二维图像上对应点的世界三维坐标；
双目摄像头的姿态估计需要两个步骤：
S41 双目图像ORB（Oriented FAST and Rotated BRIEF）特征点的提取和匹配。寻找FAST特征点，提取图像中的“角点”，对特征点周围图像区域进行BRIEF(Binary RobustIndependent Elementary Feature)描述。然后通过快速近似最邻近法实现双目摄像头两幅图像之间的匹配。
S42根据两幅图中匹配好的特征点，求解对极几何约束得到相机的姿态信息。
S5：将相机坐标系的坐标转换为UWB坐标下的坐标位置，利用UWB和相机的坐标信息进行二次卡尔曼滤波融合；
组合过程中，状态仍选取15维卡尔曼滤波的状态变量。
选取导航解算位置误差作为量测值，建立卡尔曼滤波器量测方程
式中分别为惯导输出位置结果和UWB的位置输出， 为位置量测噪声。
S6：将S3得到的数据与S5得到的滤波结果进行无重置联邦滤波，得到最终的定位结果：
图3中，为系统局部滤波器公共状态的局部最优估计，该估计的协方差阵为；而和则分别表示融合后的系统公共状态的全局最优估计和协方差矩阵，为SINS输出的导航参数，为输出校正后的导航结果（姿态、速度和位置）。
将主滤波器的滤波结果对导航误差进行反馈修正，可得到更为精确的定位结果：
对速度和位置修正：
为时刻惯导导航的速度和位置结果；为时刻的卡尔曼滤波结果。
对导航姿态进行修正：
S7：将松耦合UWB和IMU数据的融合定位信息与视觉图像定位信息进行松耦合，形成最终的定位结果，当定位结果超过油枪的设定范围时，发出警示信息。
根据本发明实施例的UWB-IMU-视觉融合的油枪室内定位方法。图1和图2为本发明实施例提供的一种油枪室内定位方法的安装示意图。在该应用实施例中，包括设置于车间四个角落一定高度的4个定位基站和融合UWB-IMU-视觉定位模块的加油枪，详述如下。
在本发明实施例中，所述UWB定位系统与现有的UWB定位系统相似，通常包括设置于需定位物体上的定位标签、POE交换机、定位服务器以及设置于环境中的多个定位基站，定位标签通过向多个定位基站发送信号，之后在固定响应时间后再向定位标签反馈信号，服务器通过时间差(TDOA)计算出定位标签与各个定位基站之间的距离。由于定位基站的位置是固定的，因此可以确定定位标签的位置信息。应知晓，该定位信息与实际待定位物的真实位置信息存在一定的误差。
本发明实施例中，所述惯性导航单元（IMU），通常是设置于定位物体上的惯性导航模块，该模块集成有三轴加速度计和陀螺仪，一共能够得到比力和角速度的6组数据，根据惯性导航解算模块的定位结果，可以融合到UWB绝对位置信息中，并在扩展卡尔曼滤波框架下，预测待定位物体的定位信息，并根据更新的坐标信息对坐标位置进行更新。同样，该定位信息与实际待定位物的真实定位信息存在一定的误差。
本发明实施例中，双目相机为现有的常用双目深度相机，通常设置于定位物体光线充足的前端，主要通过两个摄像头拍摄的两张图片（彩色RGB或者灰度图）来算距离，在对相机进行标定且图像矫正后，进行像素点匹配，根据匹配结果计算每个像素的深度。应知晓，该移动信息与实际障碍物的真实距离信息存在一定的误差。
本发明实施例中，
IMU姿态角误差：航向角20角分、俯仰角0.5角分、横滚角0.5角分；
IMU速度误差：三轴均为0.01m/s；
IMU位置误差：x轴5cm，y轴5cm，z轴10cm
UWB位置误差：x轴0.1m、y轴0.1m、z轴0.15m
双目视觉参数：
相机尺寸：22 * 80 毫米
 相机参数：双摄2560*720分辨率
 相机视角：100度 / 2.1MM焦距
 相机帧率：30FPS - 60FPS (MJPEG)
为了验证本发明专利的实施效果，分别采用UWB、IMU、双目视觉单独定位，对比本发明专利提出的UWB-IMU-视觉融合定位效果。图4是本发明实施例的UWB定位结果和真实路径对比；图5是本发明实施例的IMU定位结果和真实路径对比；图6是本发明实施例的双目视觉定位结果和真实路径对比；图7是本发明实施例的UWB-IMU-视觉融合定位结果和真实路径对比。
由图7可以看出，UWB-IMU-视觉融合定位更接近真实运动路径，表1给出了四种方法的定位误差对比，融合定位方法无论是最大误差还是平均误差都要优于单独定位的结果。为了进一步评价四种方法的定位误差，计算四种定位方法的定位误差累计分布函数对比图，如图8所示，从图8可以看出，融合后的定位方法误差100%的概率在0.3m以下，并且要优于UWB、IMU、双目视觉单独定位的结果。说明本专利提出的方法能有效抑制非视距误差给定位估计带来的影响，能用于发动机油加注机定位与区域防错。
表1不同定位方法的误差对比
定位方案 最大误差 平均绝对误差 均方根误差
UWB 0.342 6 0.156 2 0.143 2
惯导定位 1.102 2 0.245 3 0.352 1
视觉定位 0.932 5 0.223 6 0.323.1
融合定位 0.305 2 0.103 6 0.133 5
最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。

Claims (5)

1.一种用于发动机油加注机定位与区域防错方法，其特征在于，包括以下步骤：
S1：系统初始化：根据现场的环境，安装好UWB定位基站，测量基站之间的相对距离，建立UWB位置坐标体系，确定各基站的绝对坐标；对双目视觉系统进行标定，获取相机的内参矩阵、外参矩阵和畸变矩阵，建立相机坐标系和UWB系的转换矩阵；根据现场发动机的分布区域和油枪的位置范围确定定位的边界；
S2：将UWB-IMU-视觉装置安装于加油枪顶部，静置后获取当前时刻的UWB位置数据和IMU测量数据，其中，所述IMU测量数据包括加速度计输出数据和陀螺仪输出数据；其中根据加速度计静止时间段内的输出均值，获得初始姿态角和初始位置信息；
S3：在S2中获取到初始姿态角和初始位置信息后，惯导进入导航状态，并且，将IMU导航结果与UWB输出的位置数据采用松耦合的方式进行一次卡尔曼滤波，得到最优估计下的初步定位结果；
S4：获取双目视觉的图像数据，建立视觉即时定位与地图构建模型，根据采集到的图像计算出相机的运动姿态；对相机采集到的图像帧进行特征点的提取，并进行特征匹配，确定当前得到的特征点与之前获得的特征点之间的对应关系；当获得到已经通过算法进行匹配好的特征点对后，则依据他们对相机的运动进行估计；通过两帧图像中的信息估计出物体的实际位姿，即通过三角化过程获得二维图像上对应点的世界三维坐标；
S5：将相机坐标系的坐标转换为UWB坐标下的坐标位置，利用UWB和相机的坐标信息进行二次卡尔曼滤波融合；
S6：将S3得到的数据与S5得到的滤波结果进行无重置联邦滤波，得到最终的定位结果；
S7：将松耦合UWB和IMU数据的融合定位信息与视觉图像定位信息进行松耦合，形成最终的定位结果，当定位结果超出S1中确定的定位的边界时，产生警示信息。
2.根据权利要求1所述的用于发动机油加注机定位与区域防错方法，其特征在于，融合了UWB-IMU-视觉的定位数据，该方法解决了UWB定位存在的信号遮挡、人体吸收信号问题，IMU导航存在的误差累计问题，视觉导航存在的尺度模糊和尺度漂移问题，充分发挥各自定位的优势。
3.根据权利要求1所述的用于发动机油加注机定位与区域防错方法，其特征在于，UWB定位基站在室内遮掩物较少时，可放置于离地2.8m高以上的位置，当高处障碍物较多时，可根据实际情况调整定位基站的位置和姿态，以取得较好的信号接收效果。
4.根据权利要求1所述的用于发动机油加注机定位与区域防错方法，其特征在于，定位系统启动时，需要静置5-10分钟，用以惯性导航模块数据稳定和导航初始姿态确定。
5.根据权利要求1所述的用于发动机油加注机定位与区域防错方法，其特征在于，在一次卡尔曼滤波框架下，UWB和IMU原始数据融合需要调整数据采集的频率，对IMU的数据进行降采样。
CN202211664619.3A 2022-12-23 2022-12-23 一种用于发动机油加注机定位与区域防错方法 Pending CN116026323A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211664619.3A CN116026323A (zh) 2022-12-23 2022-12-23 一种用于发动机油加注机定位与区域防错方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211664619.3A CN116026323A (zh) 2022-12-23 2022-12-23 一种用于发动机油加注机定位与区域防错方法

Publications (1)

Publication Number Publication Date
CN116026323A true CN116026323A (zh) 2023-04-28

Family

ID=86077138

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211664619.3A Pending CN116026323A (zh) 2022-12-23 2022-12-23 一种用于发动机油加注机定位与区域防错方法

Country Status (1)

Country Link
CN (1) CN116026323A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116642468A (zh) * 2023-05-31 2023-08-25 交通运输部天津水运工程科学研究所 基于无人机航空摄影和无人船水上水下一体化扫描方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116642468A (zh) * 2023-05-31 2023-08-25 交通运输部天津水运工程科学研究所 基于无人机航空摄影和无人船水上水下一体化扫描方法
CN116642468B (zh) * 2023-05-31 2024-05-17 交通运输部天津水运工程科学研究所 基于无人机航空摄影和无人船水上水下一体化扫描方法

Similar Documents

Publication Publication Date Title
CN110859044B (zh) 自然场景中的集成传感器校准
KR100728377B1 (ko) 레이저 스캐너 및 무선인터넷을 이용한 변경된 지역시설물의 ｇｉｓ 실시간 업데이트 방법
JP6002126B2 (ja) 画像ベースの測位のための方法および装置
CN112629431B (zh) 土木结构变形监测方法及相关设备
CN108269286B (zh) 基于组合立体标志的多相机位姿关联方法
US20160238394A1 (en) Device for Estimating Position of Moving Body and Method for Estimating Position of Moving Body
US7149346B2 (en) Three-dimensional database generating system and method for generating three-dimensional database
CN111220120B (zh) 一种动平台双目测距自校准方法及装置
CN111091076B (zh) 基于立体视觉的隧道限界数据测量方法
JPH11230745A (ja) 高度計測装置
CN106370160A (zh) 一种机器人室内定位系统和方法
CN113551665A (zh) 一种用于运动载体的高动态运动状态感知系统及感知方法
EP3911968A2 (en) Locating system
CN110595374B (zh) 基于传递像机的大型结构件实时形变监测方法
CN114485654A (zh) 一种基于高精地图的多传感器融合定位方法及装置
CN114674311B (zh) 一种室内定位与建图方法及系统
CN116026323A (zh) 一种用于发动机油加注机定位与区域防错方法
CN113340272B (zh) 一种基于无人机微群的地面目标实时定位方法
CN111105467A (zh) 一种图像标定方法、装置及电子设备
CN111402315B (zh) 一种自适应调整双目摄像机基线的三维距离测量方法
CN112016568B (zh) 一种目标对象的图像特征点的跟踪方法及装置
CN114252099B (zh) 一种智能车辆多传感器融合自标定方法及系统
CN115930948A (zh) 一种果园机器人融合定位方法
CN113240749B (zh) 一种面向海上舰船平台无人机回收的远距离双目标定与测距方法
Liao et al. Automatic camera calibration and rectification methods

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