CN110132309B - 一种采煤机摇臂惯性/视觉组合定姿装置标定方法 - Google Patents

Abstract

一种采煤机摇臂惯性/视觉组合定姿装置标定方法，将惯性测量单元与单目视觉测量单元连接，将惯性测量单元与单目视觉测量单元安装在转台上；把信息处理单元安装在转台周边；将目标特征点单元安装在转台周边，调节内环轴，使单目视觉测量单元指向目标特征点单元；惯性/视觉组合定姿装置通电工作，调节双外环轴，信息处理单元记录第一位置的各个角；保持内环轴角位置不变，调节外环轴；信息处理单元记录第二位置的各个角；信息处理单元利用视觉测量姿态和惯性测量姿态，计算误差；本发明能够利用转台作为辅助工具，解决采煤机摇臂惯性/视觉组合定姿装置标定问题，具有易于实施，提高标定效率和精度的优点。

Description

一种采煤机摇臂惯性/视觉组合定姿装置标定方法

技术领域

本发明属于采煤机定位定姿技术领域，特别涉及一种采煤机摇臂惯性/视觉组合定姿装置标定方法。

背景技术

自动化采煤过程中，采煤机是综采工作面落煤和装煤的主要设备，是一种高集成度综采设备。采煤机摇臂是采煤机的重要组成部分，通过测量摇臂姿态，可以实现采煤机截煤轨迹自动测量和自动控制。采煤机摇臂定姿是综采工作面自动化、远程自动控制的关键技术。

现有方法主要采用基于标准六面体的标定方法，在惯性测量单元和单目视觉测量单元均安装标准六面体，利用自准直仪、基准平面镜等仪器在精密转台上测量安装误差，该方法操作复杂，对于仪器设备(如自准直仪、转台)和操作人员要求高，需要外部辅助仪器设备提供姿态信息，存在基准传递误差、仪器设备误差等误差，影响了标定效率和精度。

发明内容

为了解决上述现有技术的不足，本发明提出了一种采煤机摇臂惯性/视觉组合定姿装置标定方法，能够利用转台作为辅助工具，解决采煤机摇臂惯性/视觉组合定姿装置标定问题，具有易于实施，提高标定效率和精度的优点。

为了实现上述目的，本发明采用了如下的技术方案：

一种采煤机摇臂惯性/视觉组合定姿装置标定方法，

步骤一：将惯性测量单元3通过工装与单目视觉测量单元2刚性连接，确保惯性测量单元3与单目视觉测量单元2之间不发生相对线位移和相对角位移，将刚性连接好的惯性测量单元3与单目视觉测量单元2安装在双轴转台5上。

步骤二：根据信息处理单元4通讯电缆长度，把信息处理单元4安装在双轴转台5上或安装在双轴转台5周边，应确保通讯电缆长度不妨碍双轴转台5的转动。

步骤三：将目标特征点单元1安装在双轴转台5周边，调节双轴转台5的转台内环轴6，使单目视觉测量单元2指向目标特征点单元1，确保单目视觉测量单元2能够对光学特征点8成像，记录双轴转台5内环角数值，后续保持内环角不变。

步骤四：给惯性/视觉组合定姿装置通电，调节双轴转台5的转台外环轴7，使转台外环轴7的角度为3°～5°，标记为第一位置。

步骤五：信息处理单元4记录第一位置的惯性测量俯角、仰角、航向角θ(0)，ψ(0)，γ(0)和滚动角、视觉测量俯仰角、航向角和滚动角θ_c(0)，ψ_c(0)，γ_c(0)。

步骤六：保持转台内环轴6角位置不变，调节转台外环轴7，使转台外环轴7的角度为8°～10°，标记为第二位置。

步骤七：信息处理单元4记录第二位置的惯性测量俯角、仰角、航向角和滚动角θ(1)，ψ(1)，γ(1)和视觉测量俯仰角、航向角和滚动角θ_c(1)，ψ_c(1)，γ_c(1)。

步骤八：信息处理单元4利用视觉测量姿态和惯性测量姿态，根据计算公式

计算航向安装误差Φ。

进一步的，所述的目标特征点单元1由4个光学特征点组成，4个特征点共面分布，且呈正方形，目标特征点单元1安装在双轴转台5之外；单目视觉测量单元2与惯性测量单元3通过工装刚性连接好后，安装于双轴转台5上，确保目标特征点单元1的光学特征点8全部位于单目视觉测量单元2的视场范围内；单目视觉测量单元2利用单个相机拍摄目标特征点单元1的图像，利用图像上多个特征点之间的相对位置约束关系，求解特征坐标系与单目视觉测量单元体2坐标系之间的姿态关系，并将求解出的姿态传送给信息处理单元4；惯性测量单元3包含了三个单轴的加速度计和三个单轴的陀螺，加速度计测量物体在惯性测量单元3体坐标系三个轴向上的加速度信号，陀螺测量惯性测量单元3体坐标系相对于当地水平地理坐标系的角速度信号，并将角速度和加速度信号传送给信息处理单元4。

进一步的，所述的信息处理单元4接收载体三轴角速度和三轴加速度信号，利用捷联惯导算法，解算惯性测量单元3相对于当地水平地理坐标系的姿态；信息处理单元4根据视觉姿态和惯性姿态，计算出惯性测量单元3体坐标系与单目视觉测量单元2体坐标系的航向安装误差；具体计算方法如下：定义图像坐标系：图像左下点为图像坐标系原点，横轴为u轴，纵轴为v轴，图像坐标系以像素数为单位；定义特征坐标系O_tX_tY_tZ_t：坐标系原点O_t固连在4个目标特征点的几何中心，以4个目标特征点所在平面为YOZ，X_t轴垂直于目标特征点平面向外，Y_t轴指天，Z_t轴与X_t轴、Y_t轴构成右手坐标系；定义单目视觉测量单元2体坐标系O_cX_cY_cZ_c：坐标系原点O_c固连在相机透视投影中心，当镜头光轴指向目标特征单元1时，X_c轴正向沿镜头的光轴指向目标特征点单元1，Y_c轴垂直向上，Z_c轴与X_c轴、Y_c轴构成右手坐标系；定义惯性测量单元3体坐标系O_bX_bY_bZ_b：坐标系原点O_b固连在惯性测量单元3中心，X_b轴正向指向惯性测量单元3前方，Y_b轴垂直向上，Z_b轴与X_b轴、Y_b轴构成右手坐标系，前上右坐标系；定义当地水平地理坐标系O_nX_nY_nZ_n：北天东地理坐标；目标特征点单元1，由4个光学特征点组成，4个特征点共面分布，且呈正方形，安装在双轴转台5周边，使特征坐标系与当地水平地理坐标系O_nX_nY_nZ_n的轴向重合。

进一步的，惯性测量单元体坐标系O_bX_bY_bZ_b与单目视觉测量单元2体坐标系O_cX_cY_cZ_c的航向安装误差为Φ，有矩阵关系如下：

进一步的，所述的惯性/视觉组合定姿装置开始通电工作，三轴陀螺测量惯性测量单元3体坐标系三个轴的角速度向量

三轴加速度计测量惯性测量单元体坐标系三个轴向的加速度向量

并将测量的角速度

和加速度f^b发送给信息处理单元；

初始k＝0，信息处理单元4接收外部装订信息：经度λ(0)，纬度

高度h(0)和初始姿态

同时初始速度V＝[0 0 0]^T；

在时刻k，信息处理单元4首先更新惯性测量单元3体坐标系相对于当地水平地理坐标系的角速度向量

其中，

表示纬度值，

为姿态矩阵，ω_ie是地球自转角速度，R为地球半径，V_E和V_N表示东向速度和北向速度；

然后，更新四元数q＝[q₀ q₁ q₂ q₃]^T

其中，

ΔT表示姿态计算周期；

然后，更新惯性测量单元3相对于当地水平地理坐标系的惯性姿态矩阵

令

其中i，j＝1，2，3

根据

惯性姿态角计算如下：

俯仰角θ＝sin^-1(C₁₂)

航向角

滚动角

利用

将加速度计输出的加速度向量

投影到当地水平地理坐标系fⁿ＝[f_N f_U f_E]^T，

然后，更新惯性测量单元3的速度V＝[V_N V_U V_E]^T，经度λ，纬度

和高度h：

h(k+1)＝h(k)+0.5ΔT(V_U(k)+V_U(k+1))

令k←k+1，继续下一个时刻的计算；

单目视觉测量单元2利用单个相机采集目标特征点单元1的图像，4个特征点在图像坐标系上的像素坐标分别为(u_i，v_i)，i＝1，2，3，4；根据小孔成像比例关系，有：

其中f为相机焦距，d为单个正方形像素的边长物理尺寸，(u₀，v₀)为光轴与图像坐标系交点的像素坐标；f，d，(u₀，v₀)均为已知量，(u_i，v_i)为实测值，因此(k_xi，k_yi)根据公式(1)计算可得；

特征点P_i，i＝1，2，3，4在特征坐标系下的坐标为(X_ti，Y_ti，Z_ti)，同时在单目视觉测量单元2体坐标系下的坐标(X_ci，Y_ci，Z_ci)，那么两个坐标系的转换关系如下：

其中，(T_x，T_y，T_z)^T表示特征坐标系原点O_t到单目视觉测量单元2体坐标系原点O_c平移矢量；

表示特征坐标系t到单目视觉测量单元2体坐标系c的状态转移矩阵(又称方向余弦矩阵)；目标坐标系t绕Y_t轴旋转航向角ψ，再绕Z_t轴旋转俯仰角θ，再绕X_t轴旋转滚动角γ后与单目视觉测量单元2体坐标系c重合，即

令

因为4个特征点成正方形分布，所以记

将公式(2)代入公式(1)，得：

将公式(4)代入公式(5)，可得：

定义r_i＝c_iK，i＝1，2，4，5，7，8代入公式(6)，将公式(6)变化为8元线性方程组，以(k_xi，k_yi)作为已知量，利用公知的线性方程解法求解，可得未知数

再根据

是单位正交矩阵，满足

可以计算K。根据r_i，i＝1，2，4，5，7，8和K，可计算c_i，i＝1，2，4，5，7，8；

根据

是单位正交矩阵，则有单位矢量叉乘关系如下：

c₀＝c₄c₈-c₇c₅

c₃＝c₇c₂-c₁c₈

c₆＝c₁c₅-c₄c₂

中9个元素完全已知。根据公式(3)，视觉姿态角计算如下：

俯仰角θ_c＝sin^-1(c₁)

航向角

滚动角

本发明的有益效果是：由于标定出惯性测量单元体坐标系与单目视觉测量单元体坐标系的航向安装误差，使本发明具有操作方法高效，易于实施；由于巧妙利用了俯仰角改变时，航向安装误差对水平姿态投影不同的工作原理，来标定航向安装误差角；所有姿态均使用惯性测量单元和单目视觉测量单元的输出信息，无需精密转台等外部仪器设备提供姿态，提高了标定效率和精度，本发明为将惯性/视觉组合定姿装置应用于采煤机摇臂高精度定姿奠定基础，使其满足综采工作面自动化、远程自动控制的需求。

附图说明

图1为本发明的方法示意图。

图2为本发明实例1的特征坐标系、单目视觉测量单元体坐标系和惯性测量单元体坐标系。

图3为本发明的实例1的惯性测量单元体坐标系与采煤机体坐标系的航向安装误差表示图。

图4为本发明的实施例1的目标特征点单元示意图。

图中：1、目标特征点单元；2、单目视觉测量单元；3、惯性测量单元；4信息处理单元；5、双轴转台；6、转台内环轴；7、转台外环轴；8、光学特征点。

具体实施方式

如图1所示，实施例1：该采煤机摇臂惯性/视觉组合定姿装置包括：目标特征点单元1、单目视觉测量单元2、惯性测量单元3、信息处理单元4、双轴转台5。

本发明的采煤机摇臂惯性/视觉组合定姿装置标定方法，包括如下步骤：

1)目标特征点单元1由4个光学特征点组成，4个特征点共面分布，且呈正方形，目标特征点单元1安装在双轴转台5之外。

2)单目视觉测量单元2与惯性测量单元3通过工装刚性连接好后，安装于双轴转台5上，确保目标特征点单元1的光学特征点全部位于单目视觉测量单元2的视场范围内。

3)单目视觉测量单元2利用单个相机拍摄目标特征点单元1的图像，利用图像上特征点的二维图像坐标和特征点在特征坐标系中三维坐标，求解特征坐标系与单目视觉测量单元体坐标系之间的姿态关系，并将求解出的姿态(以下称为视觉姿态)传送给信息处理单元4，具体详见步骤17)。

4)惯性测量单元3是一种测量物体三轴角速率和三轴加速度的装置。一般的，惯性测量单元3包含了三个单轴的加速度计和三个单轴的陀螺，加速度计测量物体在惯性测量单元体坐标系三个轴向上的加速度信号，陀螺测量惯性测量单元体坐标系相对于当地水平地理坐标系的角速度信号，并将角速度和加速度信号传送给信息处理单元4。

5)信息处理单元4接收载体三轴角速度和三轴加速度信号，利用捷联惯导算法，解算惯性测量单元3相对于当地水平地理坐标系的姿态(以下称为惯性姿态)，详见步骤16)。

6)信息处理单元4根据视觉姿态和惯性姿态，计算出惯性测量单元体坐标系与单目视觉测量单元体坐标系的航向安装误差。

具体计算方法如下：

7)定义图像坐标系：图像左下点为图像坐标系原点，横轴为u轴，纵轴为v轴，图像坐标系以像素数为单位。

8)定义特征坐标系O_tX_tY_tZ_t：坐标系原点O_t固连在4个目标特征点的几何中心，以4个目标特征点所在平面为YOZ，X_t轴垂直于目标特征点平面向外，Y_t轴指天，Z_t轴与X_t轴、Y_t轴构成右手坐标系，如图2所示。

9)定义单目视觉测量单元体坐标系O_cX_cY_cZ_c：坐标系原点O_c固连在相机透视投影中心，当镜头光轴指向目标特征单元时，X_c轴正向沿镜头的光轴指向目标特征点单元，Y_c轴垂直向上，Z_c轴与X_c轴、Y_c轴构成右手坐标系，如图2所示。

10)建立惯性测量单元体坐标系O_bX_bY_bZ_b：坐标系原点O_b固连在惯性测量单元中心，X_b轴正向指向惯性测量单元前方，Y_b轴垂直向上，Z_b轴与X_b轴、Y_b轴构成右手坐标系，前上右坐标系，如图2所示。

11)建立当地水平地理坐标系O_nX_nY_nZ_n：北天东地理坐标系。

12)目标特征点单元1，由4个光学特征点组成，4个特征点共面分布，且呈正方形，如图2所示，安装在双轴转台5周边，特征坐标系与当地水平地理坐标系O_nX_nY_nZ_n的轴向重合。

13)惯性测量单元3和单目视觉测量单元2通过工装刚性连接，并安装在双轴转台5上。

14)惯性测量单元体坐标系O_bX_bY_bZ_b与单目视觉测量单元体坐标系O_cX_cY_cZ_c的航向安装误差为Φ，如图3所示，有矩阵关系如下，

15)信息处理单元4安装在双轴转台5上。

16)调节双轴转台5的内环轴6，使单目视觉测量单元2指向目标特征点单元1，确保光学特征点全部位于视场范围内；记录此时双轴转台5内环角数值。

17)惯性/视觉组合定姿装置开始通电工作；惯性测量单元3中的三轴陀螺测量惯性测量单元体坐标系三个轴的角速度向量

惯性测量单元3中的三轴加速度计测量惯性测量单元体坐标系三个轴向的加速度向量

并将测量的角速度

和加速度f^b发送给信息处理单元；

初始k＝0，信息处理单元接收外部装订信息：经度λ(0)，纬度

高度h(0)和初始姿态

同时初始速度V＝[0 0 0]^T；

在时刻k，信息处理单元首先更新惯性测量单元体坐标系相对于当地水平地理坐标系的角速度向量

其中，

表示纬度值，

为姿态矩阵，ω_ie是地球自转角速度，R为地球半径，V_E和V_N表示东向速度和北向速度；

然后，更新四元数q＝[q₀ q₁ q₂ q₃]^T

其中，

ΔT表示姿态计算周期；

然后，更新惯性测量单元3相对于当地水平地理坐标系的惯性姿态矩阵

令

其中i，j＝1，2，3

根据

惯性姿态角计算如下：

俯仰角θ＝sin^-1(C₁₂)

航向角

滚动角

利用

将加速度计输出的加速度向量

投影到当地水平地理坐标系fⁿ＝[f_N f_U f_E]^T，

然后，更新惯性测量单元3的速度V＝[V_N V_U V_E]^T，经度λ，纬度

和高度h：

h(k+1)＝h(k)+0.5ΔT(V_U(k)+V_U(k+1))

令k←k+1，继续下一个时刻的计算；

18)单目视觉测量单元2利用单个相机采集目标特征点单元1的图像，4个特征点在图像坐标系上的像素坐标分别为(u_i，v_i)，i＝1，2，3，4；根据小孔成像比例关系，有：

其中f为相机焦距，d为单个正方形像素的边长物理尺寸，(u₀，v₀)为光轴与图像坐标系交点的像素坐标；f，d，(u₀，v₀)均为已知量，(u_i，v_i)为实测值，因此(k_xi，k_yi)根据公式(1)计算可得；

特征点P_i，i＝1，2，3，4在特征坐标系下的坐标为(X_ti，Y_ti，Z_ti)，同时在单目视觉测量单元体坐标系下的坐标(X_ci，Y_ci，Z_ci)，那么两个坐标系的转换关系如下：

其中，(T_x，T_y，T_z)^T表示特征坐标系原点O_t到单目视觉测量单元体坐标系原点O_c平移矢量，

表示特征坐标系t到单目视觉测量单元体坐标系c的状态转移矩阵(又称方向余弦矩阵)，目标坐标系t绕Y_t轴旋转航向角ψ_c，再绕Z，轴旋转俯仰角θ_c，再绕X_t轴旋转滚动角γ_c后与单目视觉测量单元体坐标系c重合，即

令

因为4个特征点成正方形分布，所以记

将公式(2)代入公式(1)，可得：

将公式(4)代入公式(5)，可得：

定义r_i＝c_iK，i＝1，2，4，5，7，8代入公式(6)，将公式(6)变化为8元线性方程组，以(k_xi，k_yi)作为已知量，利用公知的线性方程解法求解，可得未知数

根据

是单位正交矩阵，满足

可以计算K；根据r_i，i＝1，2，4，5，7，8和K，可计算c_i，i＝1，2，4，5，7，8；

根据

是单位正交矩阵，则有单位矢量叉乘关系如下：

c₀＝c₄c₈-c₇c₅

c₃＝c₇c₂-c₁c₈

c₆＝c₁c₅-c₄c₂

于是，

中9个元素完全已知。根据公式(3)，视觉姿态角计算如下：

俯仰角θ_c＝sin^-1(c₁)

航向角

滚动角

19)调节双轴转台5的外环轴7，使外环轴7的角度为3°～5°；标记此时双轴转台5为第一位置；信息处理单元4记录第一位置惯性测量姿态矩阵

提取惯性测量俯仰角、航向角和滚动角分别为θ(0)，ψ(0)，γ(0)。

20)信息处理单元4记录第一位置的视觉测量姿态矩阵

提取视觉测量俯仰角、航向角和滚动角分别为θ_c(0)，ψ_c(0)，γ_c(0)；

21)双轴转台5保持内环轴6角位置不变，调节外环轴7，使外环轴7的角度为8°～10°；标记此时双轴转台5为第二位置；信息处理单元4记录第一位置惯性测量姿态矩阵

提取惯性测量俯仰角、航向角和滚动角分别为θ(1)，ψ(1)，γ(1)。

22)信息处理单元4记录第一位置的视觉测量姿态矩阵

提取视觉测量俯仰角、航向角和滚动角分别为θ_c(1)，ψ_c(1)，γ_c(1)。

23)信息处理单元4根据第一和第二位置的两组姿态角，计算航向安装误差为Φ，如下：

本发明的有益效果是：提出了一种采煤机摇臂惯性/视觉组合定姿装置标定方法，利用转台辅助，标定出惯性测量单元体坐标系与单目视觉测量单元体坐标系的航向安装误差Φ，为将惯性/视觉组合定姿装置应用于采煤机摇臂高精度定姿奠定基础，使其满足综采工作面自动化、远程自动控制的需求。

本发明方法高效，易于实施，对操作人员要求低；以转台改变装置的俯仰角，使用惯性测量单元和单目视觉测量单元的输出姿态信息，根据航向安装误差对水平姿态投影不同的工作原理，通过简单计算即可标定出航向安装误差角，无需精密转台等外部仪器设备提供姿态，提高了标定效率和精度。

Claims (5)

1.一种采煤机摇臂惯性/视觉组合定姿装置标定方法，其特征在于：

步骤一：将惯性测量单元通过工装与单目视觉测量单元刚性连接，确保惯性测量单元与单目视觉测量单元之间不发生相对线位移和相对角位移，将刚性连接好的惯性测量单元与单目视觉测量单元安装在双轴转台上；

步骤二：根据信息处理单元通讯电缆长度，把信息处理单元安装在双轴转台上或安装在双轴转台的周边，应确保通讯电缆长度不妨碍转台的转动；

步骤三：将目标特征点单元安装在双轴转台周边，调节双轴转台的转台内环轴，使单目视觉测量单元指向目标特征点单元，确保单目视觉测量单元能够对光学特征点成像，记录双轴转台内环角数值，后续保持内环角不变；

步骤四：惯性/视觉组合定姿装置开始通电工作，调节双轴转台的转台外环轴，使转台外环轴的角度为3°～5°，标记为第一位置；

步骤五：信息处理单元记录第一位置的惯性测量俯仰角、航向角θ(0)，ψ(0)，γ(0)和滚动角和视觉测量俯仰角、航向角和滚动角θ_c(0)，ψ_c(0)，γ_c(0)；

步骤六：双轴转台保持转台内环轴角位置不变，调节转台外环轴，使转台外环轴的角度为8°～10°，标记为第二位置；

步骤七：信息处理单元记录第二位置的惯性测量俯仰角、航向角和滚动角θ(1)，ψ(1)，γ(1)和视觉测量俯仰角、航向角和滚动角θ_c(1)，ψ_c(1)，γ_c(1)；

步骤八：信息处理单元利用视觉测量姿态和惯性测量姿态，根据计算公式

计算航向安装误差Φ。

2.根据权利要求1所述的一种采煤机摇臂惯性/视觉组合定姿装置标定方法，其特征在于：所述的目标特征点单元由4个光学特征点组成，4个特征点共面分布，且呈正方形；单目视觉测量单元与惯性测量单元通过工装刚性连接好后，安装于转台上，确保目标特征点单元的光学特征点全部位于单目视觉测量单元的视场范围内；单目视觉测量单元利用单个相机拍摄目标特征点单元的图像，利用图像上多个特征点之间的相对位置约束关系，求解特征坐标系与单目视觉测量单元体坐标系之间的姿态关系，并将求解出的姿态传送给信息处理单元；惯性测量单元包含了三个单轴的加速度计和三个单轴的陀螺，加速度计测量物体在惯性测量单元体坐标系三个轴向上的加速度信号，陀螺测量惯性测量单元体坐标系相对于当地水平地理坐标系的角速度信号，并将角速度和加速度信号传送给信息处理单元。

3.根据权利要求1所述的一种采煤机摇臂惯性/视觉组合定姿装置标定方法，其特征在于：所述的信息处理单元接收载体三轴角速度和三轴加速度信号，利用捷联惯导算法，解算惯性测量单元相对于当地水平地理坐标系的姿态；信息处理单元根据视觉姿态和惯性姿态，计算出惯性测量单元体坐标系与单目视觉测量单元体坐标系的航向安装误差；具体计算方法如下：定义图像坐标系：图像左下点为图像坐标系原点，横轴为u轴，纵轴为v轴，图像坐标系以像素数为单位；定义特征坐标系O_tX_tY_tZ_t：坐标系原点O_t固连在4个目标特征点的几何中心，以4个目标特征点所在平面为YOZ，X_t轴垂直于目标特征点平面向外，Y_t轴指天，Z_t轴与X_t轴、Y_t轴构成右手坐标系；定义单目视觉测量单元体坐标系O_cX_cY_cZ_c：坐标系原点O_c固连在相机透视投影中心，X_c轴正向沿镜头的光轴指向目标特征点单元，Y_c轴垂直向上，Z_c轴与X_c轴、Y_c轴构成右手坐标系；定义惯性测量单元体坐标系O_bX_bY_bZ_b：坐标系原点O_b固连在惯性测量单元中心，X_b轴正向指向惯性测量单元前方，Y_b轴垂直向上，Z_b轴与X_b轴、Y_b轴构成右手坐标系，前上右坐标系；定义当地水平地理坐标系O_nX_nY_nZ_n：北天东地理坐标系；目标特征坐标系与当地水平地理坐标系O_nX_nY_nZ_n的轴向重合。

4.根据权利要求2或3所述的一种采煤机摇臂惯性/视觉组合定姿装置标定方法，其特征在于：所述的惯性测量单元体坐标系O_bX_bY_bZ_b与单目视觉测量单元体坐标系O_cX_cY_cZ_c的航向安装误差为Φ，有矩阵关系如下，

5.根据权利要求1所述的一种采煤机摇臂惯性/视觉组合定姿装置标定方法，其特征在于：

惯性测量单元的三轴陀螺测量惯性测量单元体坐标系三个轴的角速度向量

惯性测量单元的三轴加速度计测量惯性测量单元体坐标系三个轴向的加速度向量

并将测量的角速度

和加速度f^b发送给信息处理单元：

初始k＝0，信息处理单元接收外部装订信息：经度λ(0)，纬度

高度h(0)和初始姿态

同时初始速度V＝[0 0 0]^T；

在时刻k，信息处理单元首先更新惯性测量单元体坐标系相对于当地水平地理坐标系的角速度向量

其中，

表示纬度值，

为姿态矩阵，ω_ie是地球自转角速度，R为地球半径，V_E和V_N表示东向速度和北向速度；

然后，更新四元数q＝[q₀ q₁ q₂ q₃]^T

其中，

ΔT表示姿态计算周期；

然后，更新惯性测量单元相对于当地水平地理坐标系的惯性姿态矩阵

令

其中i，j＝1，2，3

根据

惯性姿态角计算如下：

俯仰角θ＝sin^-1(C₁₂)

航向角

滚动角

利用

将加速度计输出的加速度向量

投影到当地水平地理坐标系fⁿ＝[f_N f_U f_E]^T，

然后，更新惯性测量单元的速度V＝[V_N V_U V_E]^T，经度λ，纬度

和高度h：

h(k+1)＝h(k)+0.5ΔT(V_U(k)+V_U(k+1))

令k←k+1，继续下一个时刻的计算；

单目视觉测量单元利用单个相机采集目标特征点单元的图像，4个特征点在图像坐标系上的像素坐标分别为(u_i，v_i)，i＝1，2，3，4；根据小孔成像比例关系，有：

其中f为相机焦距，d为单个正方形像素的边长物理尺寸，(u₀，v₀)为光轴与图像坐标系交点的像素坐标；f，d，(u₀，v₀)均为已知量，(u_i，v_i)为实测值，因此(k_zi，k_yi)根据公式(1)计算可得；

特征点P_i，i＝1，2，3，4在特征坐标系下的坐标为(X_ti，Y_ti，Z_ti)，同时在单目视觉测量单元体坐标系下的坐标(X_ci，Y_ci，Z_ci)，那么两个坐标系的转换关系如下：

其中，(T_x，T_y，T_z)^T表示特征坐标系原点O_t到单目视觉测量单元体坐标系原点O_c平移矢量；

表示特征坐标系t到单目视觉测量单元体坐标系c的状态转移矩阵又称方向余弦矩阵，目标坐标系t绕Y_t轴旋转航向角ψ，再绕Z_t轴旋转俯仰角θ，再绕X_t轴旋转滚动角γ后与单目视觉测量单元体坐标系c重合，即

令

因为4个特征点成正方形分布，所以记

将公式(2)代入公式(1)，

将公式(4)代入公式(5)，可得

定义r_i＝c_iK，i＝1，2，4，5，7，8代入公式(6)，将公式(6)变化为8元线性方程组，以(k_xi，k_yi)作为已知量，利用公知的线性方程解法求解，得未知数

再根据

是单位正交矩阵，满足

计算K；根据r_i，i＝1，2，4，5，7，8和K，计算c_i，i＝1，2，4，5，7，8；

根据

是单位正交矩阵，则有单位矢量叉乘关系如下：

c₀＝c₄c₈-c₇c₅

c₃＝c₇c₂-c₁c₈

c₆＝c₁c₅-c₄c₂

中9个元素完全已知，根据公式(3)，视觉姿态角计算如下：

俯仰角θ_c＝sin^-1(c₁)

航向角

滚动角

Images