一种角速度输入条件下的姿态解耦方法与流程

本发明属于导航制导与控制领域，具体是一种一种角速度输入条件下的姿态解耦方法。

背景技术：

刚体是指在外力作用下，其形状、大小及内部各点相对位置不变的物体。在导航、制导与控制领域，刚体姿态的精确测量对其自身的姿态稳定、运动控制具有重要意义。刚体在旋转时，常表现出俯仰和偏航通道之间的耦合效应(如马格努斯效应、陀螺效应等)，使其具有特殊的运动学和动力学特性，具体表现为锥运动及其它附加运动。这些复杂运动导致刚体姿态角相互耦合，出现诱导误差，将加剧导航系统的姿态误差发散，从而严重影响自旋刚体后续的导航、制导与控制。

例如制导弹药飞行过程中姿态参数的测量对弹药的气动参数辨识、外弹道特性研究、制导系统设计等都有重要的意义，而自旋弹丸的姿态测量一直存在很多困难。身管发射弹药的转速通常在30～100转/秒，以现有的导航手段难以得到满足制导控制要求的姿态参数。又比如在油气田勘探开发中，钻井的钻杆姿态控制问题一直是深层油气藏钻井施工中面临的主要难题和急需解决的技术，这一问题在我国塔里木、准噶尔和南方海相地区这尤为明显。对于高陡构造、大倾角底层等复合底层，常规的满眼钻具、偏心钻具等姿态防斜纠斜技术可以在一定程度上抑制井斜增加，但只能采用轻压吊打来控制井斜，导致机械钻速极低，直接影响着该地区油气藏的勘探开发速度，而造成这种情况的主要原因在于无法实现对井斜角的实时动态测量。

总之，目前缺少能够解决角速度输入条件下的自旋刚体姿态解耦问题的有效方法。

技术实现要素：

本发明公开了一种角速度输入条件下的姿态解耦方法，其主要目的在于利用数学的方法建立中间坐标系，并使其作为媒介，克服刚体自转的姿态耦合影响，提高姿态测量精度。

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

一种自旋刚体的姿态解耦方法，包括以下步骤：

步骤1，构建一个能够实时跟踪载体旋转角速度的动坐标系，将其作为中间坐标系；

步骤2，将姿态解算过程由载体坐标系转换到中间坐标系；

步骤3，待解算完成后再转换到导航坐标系，实现导航坐标系下的姿态解耦。

进一步的，所述步骤1中的中间坐标系能够使得下式中：

中φ和ω的方向一致，其中φ是tk时刻至tk+1时刻角位置变化所对应的等效旋转矢量，是φ的导数，φ是φ的绝对值φ＝|φ|，ω是tk时刻至tk+1时刻的载体角速度。即φ×ω＝0。

进一步的，所述步骤2中，由载体坐标系b系到中间坐标系s系的转换可表示为：

rs＝t^-1(ω·δt)＝t(-ω·δt)rb

其中rs表示中间坐标系下的矢量，rb表示载体坐标系下的矢量，t为方向余弦矩阵，t¹为t的逆矩阵，ω为圆锥运动角速度，δt为时刻差δt＝tk+1-tk。

进一步的，步骤3中间坐标系到导航坐标系的转换可表示为：

re＝t[ω(t)·δt+ω·δt]rs

其中，re表示中间导航坐标系下的矢量，ω(t)表示载体角速度。

与现有技术相比，本发明的有益效果是：

(1)本发明通过建立中间坐标系，实时跟踪载体旋转角速度，实现对自转刚体的旋转隔离，有效克服自转刚体的姿态耦合，提高姿态测量精度。

(2)本发明采用数学手段进行姿态解耦，避免了改造刚体自身结构进行减旋以避免姿态耦合的物理方法，降低了解决问题的难度和成本。

附图说明

图1整体流程图；

图2中间坐标系示意图；

图3姿态解耦方法示意图。

具体实施方式

本发明提供的一种角速度输入条件下的姿态解耦方法具体步骤如下：

首先，构建一个能够实时跟踪载体旋转角速度的动坐标系，将其作为中间坐标系，实现对高速自旋刚体的旋转隔离。

根据旋转矢量微分方程可知：

其中φ是tk时刻至tk+1时刻角位置变化所对应的等效旋转矢量，是φ的导数，φ是φ的绝对值φ＝|φ|，ω是tk时刻至tk+1时刻的载体角速度。

式(1)中后两项中的叉乘项，表明了刚体旋转的不可交换性，这也是造成圆锥误差以及姿态耦合的主要原因，因此，构建一个坐标系，使得φ和ω的方向一致，则有：

φ×ω＝0(2)

这样，式(1)中后两项就都等于零，旋转矢量的求解可直接对微分方程进行一次积分，即：

由此可知，构建一个可实时跟踪载体旋转角速度的旋转动坐标系，将其作为中间坐标系，实现对高速自旋刚体的旋转隔离，可从原理上消除刚体旋转不可交换性造成的圆锥误差以及姿态耦合误差。构建的中间坐标系如图2所示，图中，ω(t)表示载体角速度，ω表示圆锥运动角速度，b系为载体坐标系，s系为中间坐标系，δt为时刻差δt＝tk+1-tk。

利用方向余弦矩阵进行载体坐标系到中间坐标系的转换，从t时刻到t+δt时刻，载体坐标系上的角速率矢量变换可表示为：

ω(t+δt)＝t(ω·δt)ω(t)(4)

则由b系到s系的转换可表示为：

rs＝t^-1(ω·δt)＝t(-ω·δt)rb(5)

其中rs表示中间坐标系下的矢量，rb表示载体坐标系下的矢量，t为方向余弦矩阵，t¹为t的逆矩阵。方向余弦矩阵t可表示为：

其中，e＝[e1e2e3]^t为旋转轴方向的单位向量，c＝cosθ-1/θ²，s＝sinθ/θ，为绕轴转过的角度。

s系中的角速率矢量为：

ωs＝ω(t)+ω(7)

则由s系到导航坐标系e系的转换可表示为：

re＝t[ω(t)·δt+ω·δt]rs(8)

其中，re表示导航坐标系下的矢量。将式(5)代入式(8)可得到从b系到e系的转换：

re＝t[ω(t)·δt+ω·δt]t(-ω·δt)rb(9)

最后应说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。