一种矿用掘进多方式组合定位方法及系统

1.本发明涉及矿山掘进装备定位技术领域，特别是涉及一种基于多传感器数据融合的矿用掘进多方式组合定位方法及系统。


背景技术：

2.随着煤矿井下开采的无人化、智能化发展，解决掘进机的位姿自主精确定位的问题是实现煤矿智能化精准开采的核心和主要任务。目前，悬臂式掘进机机身位姿测量主要分为三大类：人工测量，半自动测量和自主测量，但是，人工测量是根据施工人员的肉眼观察，并借助人工经验进行判断，无法获取掘进机的位姿信息；半自动测量是以激光指向仪为基准，借助视觉测量或者其他的测量方法获得掘进机相对激光指向仪的位姿参数，可以获得较为精准的掘进机位姿，但还是需要地测人员进行激光指向仪的定位，与完全的自动化、无人化、智能化掘进有一定的距离；自主测量可以不依靠外物，自主检测悬臂式掘进机的机身位姿参数，但目前大部分自主测量系统结构复杂，很难应用于实际生产中。因此，依靠单一的检测手段难以实现悬臂式掘进机的位姿自主精确测量，提出一种矿用掘进多方式组合定位方法实现掘进机位姿自主测量是未来的发展趋势。


技术实现要素：

3.本发明的目的是提供一种矿用掘进多方式组合定位方法及系统，通过将多传感器数据进行数据融合的方式提高定位精度，实现掘进机机身位姿的自主精确测量。
4.为实现上述目的，本发明提供了如下方案：
5.一种矿用掘进多方式组合定位方法，所述组合定位方法包括：
6.分别获取机器视觉系统和光纤惯导系统测量掘进机机身所得到的第一位姿和第二位姿；
7.对所述第一位姿和所述第二位姿进行时间和空间的数据对准，得到第一对准后位姿和第二对准后位姿；
8.利用卡尔曼滤波方法对所述第一对准后位姿和所述第二对准后位姿进行数据融合，得到融合后位姿。
9.一种矿用掘进多方式组合定位系统，所述组合定位系统包括：
10.位姿测量模块，用于分别获取机器视觉系统和光纤惯导系统测量掘进机机身所得到的第一位姿和第二位姿；
11.数据对准模块，用于对所述第一位姿和所述第二位姿进行时间和空间的数据对准，得到第一对准后位姿和第二对准后位姿；
12.数据融合模块，用于利用卡尔曼滤波方法对所述第一对准后位姿和所述第二对准后位姿进行数据融合，得到融合后位姿。
13.根据本发明提供的具体实施例，本发明公开了以下技术效果：
14.本发明用于提供一种矿用掘进多方式组合定位方法及系统，先分别获取机器视觉
系统和光纤惯导系统测量掘进机机身所得到的第一位姿和第二位姿，然后对第一位姿和第二位姿进行时间和空间的数据对准，得到第一对准后位姿和第二对准后位姿，最后利用卡尔曼滤波方法对第一对准后位姿和第二对准后位姿进行数据融合，得到融合后位姿，进而基于多传感器数据的融合来精确定位掘进机机身位姿，解决了数据不相关的问题，使掘进机机身定位更加精准。
附图说明
15.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
16.图1为本发明实施例1所提供的组合定位方法的方法流程图；
17.图2为本发明实施例1所提供的组合定位方法的技术路线图；
18.图3为本发明实施例1所提供的组合定位方法的原理图；
19.图4为本发明实施例1所提供的对准过程的误差曲线图；
20.图5为本发明实施例1所提供的机器视觉与光纤惯导组合定位测量坐标系的示意图；
21.图6为本发明实施例1所提供的数据融合策略的流程图；
22.图7为本发明实施例1所提供的运动轨迹、观测轨迹和滤波估计图；
23.图8为本发明实施例1所提供的误差结果图；
24.图9为本发明实施例2所提供的组合定位系统的系统框图。
具体实施方式
25.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
26.本发明的目的是提供一种矿用掘进多方式组合定位方法及系统，将机器视觉系统与光纤惯导系统获得的掘进机机身位姿检测数据进行时间和空间的数据对准，运用卡尔曼滤波方法进行数据融合，解决了数据不相关的问题，使掘进机机身定位更加精准，用以解决现有的矿用悬臂式掘进机定位方法无法准确定位的问题。
27.为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
28.实施例1：
29.本实施例用于提供一种矿用掘进多方式组合定位方法，适用于实时监测煤矿井下掘进机运行状态，可获得掘进机机身运行的精准位置。如图1、图2和图3所示，所述组合定位方法包括：
30.s1：分别获取机器视觉系统和光纤惯导系统测量掘进机机身所得到的第一位姿和第二位姿；
31.机器视觉系统可为机器单目视觉测量系统。第一位姿包括掘进机机身位姿信息，第二位姿包括掘进机机身位姿的姿态角和空间位置信息。
32.具体的，s1中，获取机器视觉系统测量掘进机机身所得到的第一位姿可以包括：利用固定于掘进机机身上的摄像机采集由激光指向仪发出的平行激光束图像，并根据平行激光束图像进行位姿求解，得到第一位姿。摄像机可为防爆摄像机。
33.s1中，光纤惯导系统包括三个光纤陀螺仪和三轴加速度计，获取光纤惯导系统测量掘进机机身所得到的第二位姿可以包括：利用光纤惯导系统测量得到掘进机机身三个方向上的角速度和加速度，根据三个方向的角速度和加速度进行位姿求解，得到第二位姿。
34.更为具体的，采用旋转矢量法的三子样算法对光纤惯导系统所测得的加速度和角速度进行姿态解算。掘进机在运动过程中，利用光纤惯导系统获取了[tk，t
k+1
]时间段内的运动参数，将得到的运动参数等间隔取出三个数据作为一组，组成新的连续的运动数据，采用二次曲线拟合掘进机角速度，即
[0035][0036]
式(1)中，表示掘进机从tk时刻至t
k+1
的转动速度；tk表示初始时间；τ表示传感器a(即机器视觉系统)的采样周期。
[0037][0038]
式(2)中，δθi表示三个时间段的角度变化量；h表示步长。
[0039]
联立求得：
[0040][0041]
式(3)中，φ(h)表示掘进机机体坐标系从tk时刻至t
k+1
时刻的角位置变化对应的等效旋转矢量。
[0042]
在掘进机运动过程中，将光纤惯导系统输出的k时刻三个坐标轴x，y，z上的加速度信息表示为：
[0043][0044]
式(4)中，ab(k)表示k时刻三个坐标轴的加速度信息的矩阵形式；a
bx
表示x轴加速度；a
by
表示y轴加速度；a
bz
表示z轴加速度。
[0045]
根据解算原理先将其转化到导航坐标系中，可得：
[0046][0047]
式(5)中，an(k)表示k时刻三个坐标轴的加速度信息在导航坐标系的矩阵形式。
[0048]
表示k时刻惯性坐标系到导航坐标系的旋转矩阵，由陀螺仪的数据进行姿态的解算更新。为了消除地球重力加速度的影响，需要补偿重力加速度分量后进行积分运算，最后再积分计算速度和位移信息。
[0049]
[0050]
式(6)中，g表示重力加速度，ag(k)表示除去重力加速度影响的加速度值。
[0051]
首先对加速度ag(k)积分得到掘进机在导航坐标系中的速度：
[0052]
vn(k+1)＝vn(k)+ag(k)δt；
ꢀꢀꢀꢀꢀꢀꢀ
(7)
[0053]
式(7)中，vn(k+1)表示k+1时刻掘进机在导航坐标系中的速度；vn(k)表示k时刻掘进机在导航坐标系中的速度；δt表示从k时刻至k+1时刻的时间。
[0054]
式(7)中，δt＝t(k+1)-t(k)，对式(7)积分即可得到位置信息：
[0055][0056]
式(8)中，s(k+1)表示k+1时刻掘进机在导航坐标系中的位移。
[0057]
将位置信息在导航坐标系上进行分解，利用该方法即可解算出掘进机在当前时刻的位姿信息，结合初始位姿信息，按照时间顺序进行迭代计算就可得到连续的运动轨迹。
[0058]
利用s1可以获得机器视觉系统和光纤惯导系统所获取的位姿信息，后续步骤则是对两个位姿信息进行对准和融合，以对掘进机机身进行精确定位。
[0059]
s2：对所述第一位姿和所述第二位姿进行时间和空间的数据对准，得到第一对准后位姿和第二对准后位姿；
[0060]
s2中，对s1所获得的两组位姿数据进行时间和空间对准，通过时间对准保证机器视觉系统与光纤惯导系统时间同步，通过空间对准将不在同一坐标系下的位姿数据同步到同一坐标系，保证悬臂式掘进机机身坐标系的统一。
[0061]
s2可以包括：
[0062]
(1)对第一位姿和第二位姿进行时间对准，得到第一中间位姿和第二中间位姿；
[0063]
具体的，本实施例考虑机器视觉系统和光纤惯导系统采样频率不同来进行时间同步，以采样频率低的机器视觉系统的采样时间为基准时间，利用最小二乘法将采样频率是机器视觉系统的采样频率的整倍数的光纤惯导系统的姿态数据对准到基准时间。即先设定机器视觉系统的采样周期为光纤惯导系统的采样周期的整数倍，然后以第一位姿作为第一中间位姿，并以机器视觉系统的采样时间作为基准时间，利用最小二乘法将第二位姿对准到基准时间，得到第二中间位姿。
[0064]
其中，以机器视觉系统的采样时间作为基准时间，利用最小二乘法将第二位姿对准到基准时间，得到第二中间位姿可以包括：对于机器视觉系统的每一个第一采样周期，获取光纤惯导系统在第一采样周期内所得到的n个第二位姿，第n个第二位姿的采样时间与第一采样周期的终止时间相同，n为机器视觉系统的采样周期与光纤惯导系统的采样周期的比值。然后利用最小二乘法对n个第二位姿进行拟合，得到虚拟值，并以虚拟值作为光纤惯导系统在第一采样周期内的第二中间位姿，从而可以使光纤惯导系统的数据周期与机器视觉系统的采样周期相同，实现时间对准。
[0065]
更为具体的，将机器视觉系统记为传感器a，将光纤惯导系统记为传感器b，a，b两传感器的采样周期分别为τ和t，并且有n＝τ/t。若传感器a的第一姿态的更新时刻为(k-1)τ时刻，则下一更新时刻为kτ＝[(k-1)τ+nt]时刻，即传感器a连续测量两次，传感器b会进行n次测量。利用最小二乘法对传感器b在(k-1)τ时刻到kτ＝[(k-1)τ+nt]时刻的n个第二姿态进行时间对准，拟合为一个虚拟值，代替传感器b在kτ＝[(k-1)τ+nt]时刻的第二姿态值，即得到传感器b在该周期内的第二中间姿态，将此虚拟值与传感器a的测量值进行融合，就可
以消除时间不同步导致对掘进机状态估计出现偏差的问题。举例而言，假设τ＝4，t＝2，那么，n＝2，则1s时传感器a获得一个数据，5s时传感器a获得一个数据；1s时传感器b获得一个数据，3s时传感器b获得一个数据，5s时传感器b获得一个数据，本实施例的时间对准就是把3s和5s时传感器b的数据相融合，得到一个虚拟值，以替换5s时传感器b的真实数据，从而使传感器a和传感器b的数据周期相等，都相隔4s，完成时间对准。
[0066]
用zn＝(z1，z2，...zn)
t
表示(k-1)τ至kτ时刻传感器b的n个第二姿态，则对传感器b的n个第二姿态进行融合所得的虚拟值为：
[0067][0068]
式(9)中，为虚拟值；
[0069]
以下，对虚拟值的误差进行分析：
[0070]
若用表示(k-1)τ至kτ时刻传感器b的第二姿态z1，z2，...zn融合后的虚拟值及其导数，通过式(10)就可以准确得到传感器b的实际测量值zi：
[0071][0072]
式(10)中，vi是噪声方程。
[0073]
将式(10)转化为向量形式，可得：
[0074]
z＝wnuz+vn，i＝1,2,3,...,n；
ꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0075]
式(11)中，z表示传感器b的测量值。
[0076]
对传感器b的n个第二姿态进行融合所得的噪声方差为：
[0077][0078]
式(12)中，表示噪声方差。
[0079]
虚拟值相当于拟合后的观测值，通过将传感器a、b的观测值运用最小二乘法进行对准，通过噪声方差的结果来判断误差的大小，以便修正，通过判断误差的大小来确定虚拟值的准确性。
[0080]
具体的，运用最小二乘原则进一步减小误差，修正虚拟值，该过程可以包括：结合测量噪声vn＝(v1，v2，...vn)
t
，其均值为零，融合以前的量测噪声方程。先拟合传感器b的虚拟值代替测量值，再将虚拟值与传感器a的测量值进行融合，该处是用u进行表示，为了通过噪声方差以及最小二乘法减少误差：
[0081][0082]
式(13)中，n是指(k-1)τ至kτ时刻传感器b的n个测量集合；t是传感器b的采样周期。
[0083]
再根据最小二乘准则，要使j最小，j两边对求偏导并令其等于零得：
[0084][0085]
式(14)中，j是损失定义函数；wn是转化过程的计算系数；表示(k-1)τ至kτ时刻传
感器b的测量值z1，z2，...zn融合后的虚拟值及其导数。
[0086]
如图4所示，其为对准后的误差曲线图。由图4可知，本实施例的对准方法误差比较小。
[0087]
(2)对第一中间位姿和第二中间位姿进行空间对准，得到第一对准后位姿和第二对准后位姿。
[0088]
具体的，(2)可以包括：
[0089]
(2.1)通过外参标定获得机器视觉系统的第一坐标系到机体坐标系之间的第一旋转矩阵和第一平移矩阵以及光纤惯导系统的第二坐标系到机体坐标系之间的第二旋转矩阵和第二平移矩阵；
[0090]
(2.2)通过第一旋转矩阵和第一平移矩阵将第一坐标系转换到机体坐标系，通过第二旋转矩阵和第二平移矩阵将第二坐标系转换到机体坐标系，以对第一中间位姿和第二中间位姿进行空间对准，第一中间位姿在机体坐标系中的位姿即为第一对准后位姿，第二中间位姿在机体坐标系中的位姿即为第二对准后位姿。
[0091]
结合悬臂式掘进机机器视觉模型和悬臂式掘进机光纤惯导模型，通过外参标定，实现悬臂式掘进机机体坐标系的统一，其中机体坐标系、时间坐标系完全重合，具体如图5所示。设机器视觉系统的测量坐标系为c系，光纤惯导系统的测量坐标系o
ins
x
insyinszins
为m系，导航坐标系为n系。机器视觉系统和光纤惯导系统均与悬臂式掘进机机身刚性固连，且位置固定。空间对准的目标是将两个系统的坐标系转化到统一的机体坐标系b系，具体通过下边的旋转矩阵和平移矩阵进行坐标系的转换，都转换到机体坐标系上。
[0092][0093][0094]
上式中，pc是机器视觉系统的测量坐标系；p
ins
是光纤惯导系统的测量坐标系；是机体坐标系；是机器视觉系统的测量坐标系到机体坐标系之间的旋转矩阵和平移矩阵；是光纤惯导系统的测量坐标系到机体坐标系之间的旋转矩阵和平移矩阵。
[0095]
以上这些参数均可通过外参标定获得，此时两种定位系统实现了坐标系的统一，即统一到了悬臂式掘进机机体坐标系。
[0096]
s3：利用卡尔曼滤波方法对所述第一对准后位姿和所述第二对准后位姿进行数据融合，得到融合后位姿。
[0097]
将机器视觉系统与光纤惯导系统测量掘进机机身位姿进行时间对准和空间对准后，即对其进行时间和空间上的统一后，对掘进机机身位姿进行解算，实现机器视觉位姿测量与光纤惯导自主定位的数据融合，达到提高定位精度与高可靠性的目的。
[0098]
在此，先对本实施例所用的数据融合过程的原理予以介绍：
[0099]
利用数据融合策略实现运动精确估计，融合策略流程如图6所示。通常采用松耦合和紧耦合两种融合策略实现多传感器数据融合处理。松耦合是指用传感器的数据结果作为观测值对预测值进行更新，相对简单且可扩展性强，可以加入多种传感器信息进行融合，故本实施例采用松耦合的方式对机器视觉与光纤惯导的姿态数据进行融合，实现悬臂式掘进
机组合定位。
[0100]
组合定位系统的状态变量表示为15维列向量如下式：
[0101]
x＝[p
k φ
k v
k δ
k εk]
t
ꢀꢀ
(17)
[0102]
式(17)中，pk、φk、vk分别是光纤惯导解算得到的位置、姿态和速度；δk、εk分别是光纤惯导中速度计和陀螺仪的零漂。
[0103]
过程模型(也即本实施例中悬臂式掘进机的运动模型)由光纤惯导系统的输出数据建立，该步骤中选取的加速度和角速度两个参数是由包含在上述状态向量里的两个参数所求得的，具体分别对位姿和姿态进行求导可得出加速度和角速度，然后将加速度ab和角速度作为输入，用uk表示：
[0104][0105]
考虑到噪声的实际测量模型以及对重力影响进行补偿后的加速度，机器视觉系统可以测量目标载体的位置和姿态，以其输出构建扩展卡尔曼滤波器的观测方程，如下：
[0106][0107]
式(18)中，φ
c，k
表示目标载体的姿态，p
c，k
表示目标载体的位置，vc是机器视觉系统的测量噪声。
[0108]
通过以上公式可以建立符合扩展卡尔曼滤波的形式：
[0109]
状态转移矩阵：
[0110][0111]
式(19)中，f是机器视觉系统在k时刻相对于位移和速度的函数；uk是k时刻传感器a的测量值；x是位移；xk是k时刻的位移矩阵。
[0112]
控制输入矩阵：
[0113][0114]
式(20)中，ω表示角速度。
[0115]
观测矩阵：
[0116][0117]
观测噪声：
[0118][0119]
式(22)中，v是k时刻的观测噪声矩阵。
[0120]
通过扩展卡尔曼滤波解算方程进行解算，实现单目视觉位姿测量与光纤惯导自主定位的数据融合。
[0121]
基于上述融合原理可知组合定位时状态方程和观测方程所包含的参数，以下，将悬臂式掘进机的运动方式近似为变加速运动，以进行数据融合分析。为了更精确的表征掘进机的各个运动状态，以变加速运动模型作为悬臂式掘进机的运动模型，进行卡尔曼数据
融合，实现高精度组合定位。
[0122]
在开始融合之前，先建立融合过程中所涉及的状态方程、预测方程、观测方程和更新方程。具体的，根据掘进机的运动模型建立运动方程，根据运动方程建立状态矩阵和观测矩阵，根据状态矩阵建立状态方程，根据观测矩阵建立观测方程。以卡尔曼滤波器时间更新方程作为预测方程，以卡尔曼滤波器状态更新方程作为更新方程，状态方程是为了描述设备运行状态，预测方程是在数据融合过程中，根据卡尔曼滤波数据融合方法预测设备运行的下一个点，观测方程是为了描述数据融合之后的设备运行状态，更新方程是为了更新卡尔曼增益等参数来提高数据融合结果的可靠性。
[0123]
在建立好上述四个方程后，具体的，s3可以包括：
[0124]
(1)利用状态方程分别对第一对准后位姿和第二对准后位姿进行状态描述，得到第一状态和第二状态；
[0125]
(2)以第一状态和第二状态作为输入，利用预测方程得到预测位姿；
[0126]
(3)以预测位姿作为输入，利用观测方程得到融合后位姿；
[0127]
(4)利用更新方程对预测方程进行更新，得到更新后的预测方程，并以更新后的预测方程作为下一循环的预测方程，对下一时刻的第一对准后位姿和第二对准后位姿进行数据融合。
[0128]
以下，通过坐标的变化和运动方式进行运动分析，对两个同类型的数据进行融合的过程予以说明：
[0129]
假设位移为x(k)，速度为加速度为加加速度为建立如下运动方程：
[0130][0131]
式(23)中，k为当前时刻，(k-1)为上一时刻，t为时间。该运动方程是为了描述掘进机的运动方式，后续的状态方程和观测方程是根据本运动方程建立的，主要是按照运动方程中各参数的系数进行建立的。
[0132]
上式可简写为：
[0133][0134]
通过机器视觉和惯导组合定位系统中x，y，z三个方向的位置数据，则状态转移矩阵为：
[0135][0136]
则状态方程为：
[0137][0138]
利用组传感器方法，将所有的测量值融合进一个合成的观测方程中。假设观测值为z(k)，观测矩阵为：
[0139]
[0140]
则观测方程为：
[0141][0142]
本实施例中，卡尔曼滤波器时间更新方程(即预测方程)如下：
[0143][0144]
式(29)中，是在时刻k的预测系统状态向量，是预测方程的结果；是k-1时刻的后验状态估计值，是滤波的结果之一；a是系统动态转移矩阵；b是获得矩阵；u
k-1
是状态高斯白噪声序列；是在时刻k的先前估计不确定性方差矩阵，是滤波的中间计算结果；p
k-1
是在当前状态下先前估计不确定性协方差矩阵；q是过程噪声协方差矩阵。
[0145]
运用该预测方程计算得出掘进机的预测坐标。
[0146]
本实施例中，卡尔曼滤波器状态更新方程(即更新方程)如下：
[0147][0148]
式(30)中，kk是卡尔曼增益；h是观察矩阵；r是测量不确定性；是在时刻k的估计系统状态向量；zk是测量值；pk是当前状态的估计不确定性协方差矩阵。
[0149]
更新方程是通过更新kk、pk三个参数来减小误差，使得数据融合结果更接近实际情况。
[0150]
本实施例还通过matlab对组合定位进行仿真分析，图7为运动轨迹、观测轨迹和滤波估计图，图8为误差结果图，根据结果显示，随着时间或者采样过程的增加，滤波效果是稳定的，且相比于两种观测结果，与实际的运动轨迹误差较小，更接近于真实值。
[0151]
实施例2：
[0152]
本实施例用于提供一种矿用掘进多方式组合定位系统，如图9所示，所述组合定位系统包括：
[0153]
位姿测量模块m1，用于分别获取机器视觉系统和光纤惯导系统测量掘进机机身所
得到的第一位姿和第二位姿；
[0154]
数据对准模块m2，用于对所述第一位姿和所述第二位姿进行时间和空间的数据对准，得到第一对准后位姿和第二对准后位姿；
[0155]
数据融合模块m3，用于利用卡尔曼滤波方法对所述第一对准后位姿和所述第二对准后位姿进行数据融合，得到融合后位姿。
[0156]
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
[0157]
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。