一种附加外方位元素约束的采煤机定位定姿方法技术

本发明专利技术涉及采煤机控制领域，特别是一种附加外方位元素约束的采煤机定位定姿方法，所述方法包括如下步骤：步骤一、惯导系统提供实时参数；步骤二、视觉定位定姿方法；步骤三、融合算法；本方法具有实时性强、采样频率高、数据丰富、定姿定位精度高，用多传感器进行采煤机状态参数的采集、结算与反馈，便于控制井下采煤机的工作状态，有大量冗余数据，系统的容错性强，便于对采煤机的状态控制。

A method of orientation and orientation of shearer with external orientation element constraint

【技术实现步骤摘要】
一种附加外方位元素约束的采煤机定位定姿方法
本专利技术涉及采煤机控制领域，特别是一种附加外方位元素约束的采煤机定位定姿方法。
技术介绍
传统的采煤机定向定姿方法依赖于已知的基站坐标或行进轨道形态等外界参数，不能满足采煤机定位的实时性、自主性和精确性要求。优势：无误差累积，姿态解算过程相对容易。劣势：不能实时定姿，受外界的影响干扰大，难以达到高精度的姿态参数，加上受时间的影响，难以确定某一时刻的姿态参数，外部对其运行过程的控制极其困难。利用陀螺仪、加速度计等自感知定位，存在误差累积，不能自主纠偏等问题，优势：实时、采样频率高、精度更高，抗干扰能力强，适合在恶劣环境中运行。劣势：由于陀螺仪在长时间的输出过程中受零偏不稳定性的影响，姿态解算过程中有误差逐步累积；传感器数据的综合使用会产生耦合度不够的问题；地下空间环境低纹理的特点，自适应能力差，不便控制采煤机的运动。
技术实现思路
本专利技术主要针对现有技术中劣势，将视觉与惯导融合，一方面通过摄像机拍摄照片提供环境信息，在运动过程构建环境场景的三维模型，计算相机的位置姿态，为惯导系统提供位姿误差补偿信息，修正了IMU由于惯性误差随着时间的漂移造成的不准确问题，另一方面IMU惯导系统凭借其数据更新率高、定位精度高，不受光照、温度等环境影响的优势，弥补了视觉导航系统实时性和稳定性的缺陷，此外，为了解决惯性传感器测量的角速度和比力进行数次积分时，误差会随时间进行积累，以及因为相机采集照片的频率低、光照条件的不足等，产生位置信息的偏差的问题，提出了一种附加外方位元素约束的采煤机定位定姿方法。本专利技术解决其技术问题所采用的技术方案为：一种附加外方位元素约束的采煤机定位定姿方法，其特征在于，所述方法包括如下步骤：步骤一、惯导系统提供实时参数；步骤二、视觉定位定姿方法；步骤三、融合算法。所述惯导系统提供实时参数在IMU定位定姿中以东北天坐标系作为导航坐标系，具体解算步骤为：2.1计算载体坐标系至导航坐标系的转换矩阵：通过陀螺仪测得的角度变化率，经过四元数的更新，若俯仰角和横滚角κ很小，可以忽略不计，可以得到载体坐标系至导航坐标系的转换矩阵：其中，ω为航向角2.2计算载体的位置和速度：由加速度计测得的采样时间Δt里的载体坐标系中相应于应力的速度增量通过转换矩阵转换成导航坐标系中的增量即则加速度增量为：其中，为地球自转角速度的反对称矩阵，为位置角速度的反对称矩阵，gL为重力向量，VL为采煤机的速度；经过积分，和表示采煤机在k时刻和k+1时刻的速度，和表示采煤机K-1到K时刻和K时刻到K+1时刻的速度变化量，则载体的速度和位置分别为：2.3计算载体速度增量的误差：对速度增量进行求导，得到载体速度增量的误差：其中，Ω为真实转换矩阵与计算的转换矩阵之间失准角误差的反对称阵，若忽略前一个时刻误差δVL和δgL的影响，速度增量的误差与两个因素有关，当前时刻转换矩阵误差和加速度计误差，即由于地球引力的作用，比较大，由此引起的水平位置分量为B，L的速度增量误差也会很大；2.4计算位置增量误差：其中，δ是位置增量的积分误差。所述视觉定位定姿方法包括如下具体步骤：3.1利用影像立体像对解算目标点坐标在移动测量系统中，双目相机固连在载体上，且相机的内参已知，在此基础上，不考虑设备安装误差，直接利用双目相机立体像对解求目标点坐标，并计算其在移动测量系统中的位置和姿态变化，即求解外方位元素：XT＝λRMXM+X0XT＝λ′R′MX′M+X′0XM＝RPXPX′M＝R′PX′P式中，XT＝(xT,yT,zT)T为地面点A在大地空间直角坐标系下的坐标，X0＝(x0,y0,z0)T和X′0＝(x′0,y′0,z′0)T分别是左右地面摄影测量坐标系原点在大地坐标系中坐标原点的平移量，XM＝(xm,ym,zm)T和X′M＝(x′m,y′m,z′m)T是地面点A在左右像空间辅助坐标系下坐标的平移量；XP＝(xp,yp,-f)T和X′P＝(x′p,y′p,-f′)T是地面点A在左右像片上的投影点在各自像空间坐标系下的坐标的平移量；RP和R′P是像空间坐标系到摄影测量坐标系间的转换矩阵，均可通过前期标校精确给出；RM和R′M是摄影测量坐标系相对于大地坐标系的旋转矩阵，由相机姿态参数航向角俯仰角ω和翻滚角κ决定，旋转矩阵中的每个元素均为上述三参数的函数；在测量作业中，当两个像空间辅助坐标系的三个对称轴相互平行时，RM＝R′M；λ、λ′是尺度因子，表示像空间坐标系和像空间辅助坐标系的长度比，由基线长度和地物点A在左右两个像空间辅助坐标系中的坐标计算得到，公式如下：3.2计算各时刻下的双目相机定位定姿测量：在k时刻下，基于同名点Ai(i＝0···n)的观测方程：λ(i,k-1)表示k-1时刻第i个相机的尺度因子，表示k-1时刻第i个相机的摄影测量坐标系相对于大地坐标系的旋转矩阵。系数矩阵A(i,k)的元素可描述为：an,m＝f(λ(i,k),αk-1,ωk-1,κk-1,xk-1,yk-1,zk-1,x(i,p,k),y(i,p,k),-f)f为摄影机主距，e(i,L)为观测误差向量，表示左侧双目相机在k-1至k时刻间的姿态与位置增量，为待估参数，当同名点不少于两个时，可采用最小二乘法对系数矩阵A(i,k)求解，并在此基础上，通过连续的递推实现各时刻下的双目相机定位定姿测量；所述融合算法的具体步骤为：在移动测量系统中，利用观测信息对载体定位定姿参数进行抗差估计，IMU、双目相机可基于抗差估计构成自适应融合滤波：其中，Lik计算参照(1)式：i取值为1,2，分别代表IMU和双目相机传感器，为各传感器的抗差等价权；基于自适应卡尔曼滤波原理，构造基于各传感器抗差估计得自适应融合滤波解为：式中：上式中，αk为自适应因子，状态估计值用多传感器融合解本专利技术的有益效果是：该方法具有实时性强、采样频率高、数据丰富、定姿定位精度高，用多传感器进行采煤机状态参数的采集、结算与反馈，便于控制井下采煤机的工作状态，有大量冗余数据，系统的容错性强，便于对采煤机的状态控制。附图说明图1为本专利技术所述一种附加外方位元素约束的采煤机定位定姿方法流程示意图；图2为本专利技术所述一种附加外方位元素约束的采煤机相机安装示意图；图3为本专利技术所述一种附加外方位元素约束的采煤机相机定位定姿相关坐标系示意图；具体实施方式为更进一步阐述本专利技术为达成预定专利技术目的所采取的技术手段及功效，以下结合附图及较佳的实施例，对依据本专利技术申请的具体实施方式、结构、特征及其功效，详细说明如下。实施例一如图1所示，一种附加外方位元素约束本文档来自技高网...

【技术保护点】
1.一种附加外方位元素约束的采煤机定位定姿方法，其特征在于，所述方法包括如下步骤：/n步骤一、惯导系统提供实时参数；/n步骤二、视觉定位定姿方法；/n步骤三、融合算法。/n

【技术特征摘要】
1.一种附加外方位元素约束的采煤机定位定姿方法，其特征在于，所述方法包括如下步骤：
步骤一、惯导系统提供实时参数；
步骤二、视觉定位定姿方法；
步骤三、融合算法。


2.根据权利要求1所述一种附加外方位元素约束的采煤机定位定姿方法，其特征在于：所述惯导系统提供实时参数在IMU定位定姿中以东北天坐标系作为导航坐标系，具体解算步骤为：
2.1计算载体坐标系至导航坐标系的转换矩阵：通过陀螺仪测得的角度变化率，经过四元数的更新，若俯仰角和横滚角κ很小，可以忽略不计，可以得到载体坐标系至导航坐标系的转换矩阵：

其中，ω为航向角；
2.2计算载体的位置和速度：由加速度计测得的采样时间Δt里的载体坐标系中相应于应力的速度增量通过转换矩阵转换成导航坐标系中的增量即
则加速度增量为：
其中，为地球自转角速度的反对称矩阵，为位置角速度的反对称矩阵，gL为重力向量，VL为采煤机的速度；
经过积分，和表示采煤机在k时刻和k+1时刻的速度，和表示采煤机K-1到K时刻和K时刻到K+1时刻的速度变化量，则载体的速度和位置分别为：



2.3计算载体速度增量的误差：对速度增量进行求导，得到载体速度增量的误差：



其中，为转换矩阵的误差阵，为求导过程中速度的误差矩阵，Ω为真实转换矩阵与计算的转换矩阵之间失准角误差的反对称阵，若忽略前一个时刻误差δVL和δgL的影响，速度增量的误差与两个因素有关，当前时刻转换矩阵误差和加速度计误差，即



由于地球引力的作用，比较大，由此引起的水平位置分量为B，L的速度增量误差也会很大；
2.4计算位置增量误差：



其中，δ是位置增量的积分误差。


3.根据权利要求1所述一种附加外方位元素约束的采煤机定位定姿方法，其特征在于：所述视觉定位定姿方法包括如下具体步骤：
3.1利用影像立体像对解算目标点坐标
在移动测量系统中，双目相机固连在载体上，且相机的内参已知，在此基础上，不考虑设备安装误差，直接利用双目相机立体像对解求目标点坐标，并计算其在移动测量系统中的位置和姿态变化，即求解外方位元素：
XT＝λRMXM+X0XT＝λ′R′MX′M+X′0XM＝RPXP
X′M＝R′PX′P
式中，XT＝(xT,yT,zT)T为地面点A在大地空间直角坐标系下的坐标，X0＝(x0,y0,z0)T和X′0＝(x′0...

【专利技术属性】
技术研发人员：龚云，南守琎，石稳欣，王浩森，杨梦佳，高涵，赵吉庆，
申请(专利权)人：西安科技大学，
类型：发明
国别省市：陕西;61