CN101419070A - 基于激光测距成像仪的相对位姿确定方法 - Google Patents

Abstract

一种基于激光测距成像仪的相对位姿确定方法，本发明属于导航定位技术领域。其特征在于包括以下步骤：(1)激光测距成像仪获取到三个特征测量点p_i的矢量(距离+方向)

；(2)构建参考坐标系o_rx_ry_rz_r；(3)构建相对位置和相对姿态。本方法可以有效克服传统的基于光学CCD相机的位姿确定算法中存在的计算量大、收敛速度慢、导航精度低等缺点。

Description

基于激光测距成像仪的相对位姿确定方法

技术领域

本发明属于导航定位技术领域。

背景技术

相对位姿确定方法在机器人导航定位、航天器交会对接、编队飞行等众多领域有着极其重要的应用价值。目前，相对位姿确定方法使用较多的传感器主要是光学CCD导航相机。通过多个非共线视线矢量(单位方向矢量)的测量，通过近似假设得到相对位置和姿态的近似解析解(参考文献1：N.KPhilip，Ananthasayanam MR.Relative position and attitude estimation and controlschemes for the final phase of an autonomous docking mission of spacecraft.Acta Astronautica，2003，52:511-522；参考文献2：王保丰，李广云，于志坚.飞行器自主交会对接逼近阶段单台CCD测量方法研究.宇航学报，2007，28(1):22-27)。或者借助于最小二乘算法得到相对位姿的最小方差(或者其他非线性估计方法)意义下的解(参考文件3：JU-YOUNG DU.Vision based NavigationSystem for Autonomous Proximity Operations：An Experimental andAnalytical Study.Ph.D.dissertation，Texas A&M University，December2004)。上述相对位置和姿态的近似解析解求取的过程中由于近似假设的存在，位姿导航误差也会一直存在解析解中；而基于非线性估计的相对位姿确定方法存在着计算量大、导航收敛困难和精度低的缺点。同时，由于计算机视觉中的模式识别图像处理需要很大的计算资源，所有使用CCD相机作为导航传感器的基于视觉的导航算法都存在数据计算量大、数据更新率低的缺点，这些缺点有时候将会导致暂时的跟踪识别目标丢失、从而最终导致导航失败。

发明内容

为了克服传统的基于光学CCD相机的位姿确定算法中存在的计算量大、收敛速度慢、导航精度低等缺点，本发明给出了一种采用基于高采样速率的激光测距成像仪的相对位姿确定方法。

具体的位姿流程如下：

1)激光测距成像仪获取到三个特征测量点p_i(i＝1，2，3)的矢量(距离+方向)

${\stackrel{\→}{L}}_i(i=\mathrm{1,2,3});$

$\stackrel{\→}{p_1{p}_2}=\stackrel{\→}{L_2}-\stackrel{\→}{L_1}$

$\stackrel{\→}{p_1{p}_3}=\stackrel{\→}{L_3}-\stackrel{\→}{L_1}$

2)构建参考坐标系o_rx_ry_rz_r；

$\stackrel{\→}{o_r{x}_r}=\frac{\stackrel{\→}{p_1{p}_2}}{\left|\right|\stackrel{\→}{p_1{p}_2}\left|\right|}=\frac{\stackrel{\→}{L_2}-\stackrel{\→}{L_1}}{\left|\right|\stackrel{\→}{L_2}-\stackrel{\→}{L_1}\left|\right|}$

$\stackrel{\→}{o_r{z}_r}=\stackrel{\→}{o_r{x}_r}\×\frac{\stackrel{\→}{p_1{p}_3}}{\left|\right|\stackrel{\→}{p_1{p}_3}\left|\right|}=\stackrel{\→}{o_r{x}_r}\×\frac{\stackrel{\→}{L_3}-\stackrel{\→}{L_1}}{\left|\right|\stackrel{\→}{L_3}-\stackrel{\→}{L_1}\left|\right|}$

$\stackrel{\→}{o_r{y}_r}=\stackrel{\→}{o_r{z}_r}\×\stackrel{\→}{o_r{x}_r}$

3)相对位置和姿态的构建

相对位置： $\stackrel{\→}{R_r}=-\stackrel{\→}{L_1}$

相对姿态矩阵(坐标变换矩阵)： $A={\left[\begin{array}{ccc}\stackrel{\→}{o_r{x}_r}& \stackrel{\→}{o_r{y}_r}& \stackrel{\→}{o_r{z}_r}\end{array}\right]}^T$

定义ψ，θ，φ是体固连坐标系相对与参考坐标系的姿态角，即相对姿态角，假设按照3(ψ)-2(θ)-1(φ)顺序旋转可以得到姿态矩阵：

$A=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\θ}\\ \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\cos \mathrm{\θ}\\ \cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}& \cos \mathrm{\φ\θ}\cos \end{array}\right]$

进而可以得到相对姿态角：

$\mathrm{\ψ}=\arctan \left(\frac{A\left(\mathrm{1,2}\right)}{A\left(\mathrm{1,1}\right)}\right)$

θ＝-arcsin(A(1，3))

$\mathrm{\φ}=\arctan \left(\frac{A\left(\mathrm{2,3}\right)}{A(3,3)}\right)$

和传统的基于光学CCD导航相机的相对位姿确定算法相比，基于激光测距成像仪的相对位姿确定算法有以下优点：

(1)该方法计算量小，计算速度快；这一优点可以保证比较好的导航实时性。

(2)由于激光测距成像仪可以直接提供距离测量信息，使位姿确定算法的可靠性较光学CCD/视觉导航有了很大的提高。

该方法是从经典的双矢量定姿原理得到启发，通过三矢量观测来直接确定相对位置和姿态等参数，是对双矢量定姿的发展和推广。本方法可以有效地解决单独使用光学CCD相机进行位姿确定过程中存在的问题。在航天器交会对接、编队飞行和机器人导航定位等众多领域都有着极其重要的应用价值。

附图说明

图1为相对位姿确定示意图。位姿确定的物理含义：通过对激光测距成像仪的测量输出进行相应的处理和运算，得到体固联坐标系相对于构建参考坐标系的位置矢量和位姿矢量[ψ，θ，φ]^T。

附图中相关标注说明：

o_rx_ry_rz_r—构建所得到的参考坐标系；

o_bx_by_bz_b—体固联参考坐标系(与导航仪器固联)；

p_i(i＝1，2，3)—特征测量点(测量对应点)；

—激光测距成像输出的三个测量矢量；

具体实施方式

具体实施方式一：合作目标间的相对位姿确定。航天器的交会对接、编队飞行等都属于这一类别。此时，测量对应点在参考系下的位置矢量已知，通过一个简单的坐标变换就可以得到两个航天器之间的相对位置和姿态。

具体实施方式二：非合作目标间的相对位姿确定。机器人的导航定位、着陆小行星等属于该类别。方式二区别于方式一的地方在于测量对应点在参考系下的位置矢量是未知的，所得到的相对位置和姿态是相对于构建参考坐标系的。

Claims (1)

1、一种基于激光测距成像仪的相对位姿确定方法，其特征在于包括以下步骤：

(1)、激光测距成像仪获取到三个特征测量点p_i的矢量(距离+方向)

其中i＝1，2，3

$\stackrel{\→}{p_1{p}_2}=\stackrel{\→}{L_2}-\stackrel{\→}{L_1}$

$\stackrel{\→}{p_1{p}_3}=\stackrel{\→}{L_3}-\stackrel{\→}{L_1};$

(2)、构建参考坐标系o_rx_ry_rz_r

$\stackrel{\→}{o_r{x}_r}=\frac{\stackrel{\→}{p_1{p}_2}}{\left|\right|\stackrel{\→}{p_1{p}_2}\left|\right|}=\frac{\stackrel{\→}{L_2}-\stackrel{\→}{L_1}}{\left|\right|\stackrel{\→}{L_2}-\stackrel{\→}{L_1}\left|\right|}$

$\stackrel{\→}{o_r{z}_r}=\stackrel{\→}{o_r{x}_r}\×\frac{\stackrel{\→}{p_1{p}_3}}{\left|\right|\stackrel{\→}{p_1{p}_3}\left|\right|}=\stackrel{\→}{o_r{x}_r}\×\frac{\stackrel{\→}{L_3}-\stackrel{\→}{L_1}}{\left|\right|\stackrel{\→}{L_3}-\stackrel{\→}{L_1}\left|\right|}$

$\stackrel{\→}{o_r{y}_r}=\stackrel{\→}{o_r{z}_r}\×\stackrel{\→}{o_r{x}_r};$

(3)、相对位置和姿态的构建

相对位置： $\stackrel{\→}{R_r}=-\stackrel{\→}{L_1}$

相对姿态矩阵(坐标变换矩阵)： $A={\left[\begin{array}{ccc}\stackrel{\→}{o_r{x}_r}& \stackrel{\→}{o_r{y}_r}& \stackrel{\→}{o_r{z}_r}\end{array}\right]}^T$

定义ψ，θ，φ是体固连坐标系相对与参考坐标系的姿态角，即相对姿态角，假设按照3(ψ)-2(θ)-1(φ)顺序旋转可以得到姿态矩阵：

$A=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\θ}\\ \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\cos \mathrm{\θ}\\ \cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]$

进而可以得到相对姿态角：

$\mathrm{\ψ}=\arctan \left(\frac{A\left(\mathrm{1,2}\right)}{A\left(\mathrm{1,1}\right)}\right)$

θ＝-arcsin(A(1，3))

$\mathrm{\φ}=\arctan \left(\frac{A\left(\mathrm{2,3}\right)}{A\left(\mathrm{3,3}\right)}\right).$

Images