CN1270162C - 用变更四元数数据表示对可倾斜物体中的方位角估算 - Google Patents

Abstract

基于陀螺和倾斜传感器输出的对可倾斜物体的方位角进行跟踪和控制。对由陀螺输出的信号作变换和积分产生变更四元数形式的估算位置信息，此变更四元数的偏航角分量限定为零值。倾斜传感器的输出也产生同样形式的变更四元数，并用于去探测和校正估算位置信息的误差分量。陀螺的漂移也以倾斜传感器的输出为基础进行校正。

Description

用变更四元数数据表示对可 倾斜物体中的方位角估算

技术领域

本发明涉及可倾斜物体的跟踪和控制。

背景技术

使用四元数去表示一个物体在空间的取向是已知的。通常的四元数记号比广泛采用的欧拉角数据表示在计算上更有效。此外，四元数记号不像欧拉角记号那样遇到奇异点。下面的美国专利公开了采用四元数去控制、确定、和/或显示一物体在空间的取向：其专利号为：5,875,993；5,212,480；4,797,836；4,742,836；和4,737,794。

                     四元数的一般讨论

一个四元数有四个元素，它是由Sir William Rowan Hamilton在1843年首先想出的一个超复数。一个四元数由标量部分和矢量部分组成。矢量部分是三个实分量的排序三重数(矢量)，这三个实分量被指定沿着三个复单位矢量í，j，k的方向。一个一般四元数的例子Q表示如下：

Q＝q0+iq1+jq2+kq3                              (方程1)

四元数相加由相同方向的分量相加来实现，四元数相乘可采用下列单位基矢量的成积来实现：

i2＝j2＝k2＝ijk＝-1                            (方程2)

ij＝-ji＝k                                     (方程3)

jk＝-kj＝i                                     (方程4)

ki＝-ik＝j                                     (方程5)

因四元数是超复数，它有复数共轭，即将其矢量部分的方向反向。一个例子表式如下：

Q*＝q₀-iq₁-jq₂-kq₃                        (方程6)

如下所示，四元数的平方值(模的平方)可由计算四元数与它的复数共轭的乘积来得到：

Q²＝QQ^*＝q₀ ²+q₁ ²+q₂ ²+q₃ ²                  (方程7)

数值(模)为单位的1(Q²＝1)的四元数有特别的意义。特别是，它的作用像一个两面转动算符。注意到如下事实是有价值的，即哈密顿(Hamilton)通过努力研究一个复数乘以形式为exp(iθ)的单位复数在复数平面上产生的转动效应的三维推广而发现了四元数。exp(iθ)的转动效应来自复数相乘要求它们各自的模相乘而各自的相位相加。因exp(iθ)的模为单位1，它只影响乘积的相位，在复数平面上这表现为绕原点转动角度θ。当试图将这种效应推广到矢量转动时，哈密顿最初试图用三个元素的超复数。直到他意识到需要四个元素去考虑在三维空间的‘相位’改变时，他才得到了想要的结果。

通常，一个矢量的转动采用单面转动算符R来完成，在三维空间中此转动算符可用一个3×3的正交矩阵来表示。用如所示的左乘：

x’＝Rx这里x∈R^3×1和R∈R^3×3                 (方程8)

这个变换矩阵将矢量x转动到x’。

一个两面转动必需施加左乘和右乘两者。在四元数算符的情况，一给定标量部分为零(即矢量)的四元数(X)用一单位四元数和它的复数共轭先乘和后乘：

X’＝QXQ^*                                     (方程9)

则完成转动。

产生的矢量X’是绕着一般的轴转动一特定的角，而此轴和角都由单位四元数Q确定。如果转动轴用单位矢量n表示，转动角用θ表示，则单位四元数的分量可写为：

q₀＝cos(θ/2)和q＝nsin(θ/2)这里q＝(q₁，q₂，q₃)

                                    (方程10)

这些分量满足归一化条件：

1＝q₀ ²+q₁ ²+q₂ ²+q₃ ²                  (方程11)

这样定义的四元数分量也称欧拉参数。这些参数包含导出转动轴和角所需的全部信息。由单位矢量n定义的转动轴称为本征轴，因为它是单面转动矩阵R相应于本征值λ＝+1的本征矢量。因为转动轴对原来的参照系和转动后的参照系是公共的而必需不被转动算符改变，所以出现这种情况。注意，所谓本征轴转动是一绕一般轴的单个转动，它是与由分别绕轴x，y，和z实施三次分开的转动：偏航角，俯仰角和滚动角，来完成的欧拉角转动比较而言的。

四元数复单位矢量(I，j，k)与泡利自旋矩阵的关系表示为：

i＝-iσ₁                                (方程12)

j＝-iσ₂                                (方程13)

k＝-iσ₃                                (方程13A)

其中

$i=\sqrt{-1}$ (方程14)

${\mathrm{\σ}}_0=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$ (方程15)

${\mathrm{\σ}}_1=\left[\begin{array}{cc}0& 1\\ 1& 0\end{array}\right]$ (方程16)

${\mathrm{\σ}}_2=\left[\begin{array}{cc}0& -i\\ i& 0\end{array}\right]$ (方程17)

${\mathrm{\σ}}_3=\left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]$ (方程18)

(见E.Cartan的旋量理论)

采用泡利自旋矩阵和单位四元数系数的定义，单位四元数可以写为：

Q＝σ₀cos(θ/2)-i(n·σ)sin1(θ/2)                (方程19)

这里

n·σ＝n₁σ₁+n₂σ₂+n₃σ₃                          (方程20)

可以证明，这与下面的矩阵指数等价：

Q＝e^-iσ·θ/2其中θ＝nθ                         (方程21)

请注意，四元数的这种形式与前面讨论的单位复数指数形式相似。这种形式暗示出哈密顿原来所寻找的‘相位’的三维改变。半角的出现由Q是两面变换这一事实来顾及。左因子和右因子Q和Q*，每一个贡献所需空间相位移动的一半。

通常，用泡利自旋矩阵形式比用传统的四元数的哈密顿形式更方便。例如，一个矢量可用构成内积来表示为所示矩阵：

X＝x·σ＝x₁σ₁+x₂σ₂+x₃σ₃                       (方程22)

它给出

$X=\left[\begin{array}{cc}{x}_3& x_1-i{x}_2\\ x_1+i{x}_2& -x_3\end{array}\right]$ (方程23)

这种矩阵形式有很多有用的性质。例如，可以证明，一个矢量x通过由单位法线a定义平面的反射容易用矢量的矩阵形式产生并表示为：

X’＝-AXA这里A＝a·σ                    (方程24)

还可证明，任何转动可用两次反射产生。如果两个反射平面以角

相交且交线用单位矢量n定义，这样合成变换将任何矢量x绕本征轴n转动角度θ。下面示例说明平面的单位法线是a和b的情况。

X’＝BAXAB                               (方程25)

这个实施转动的两面算符与上边描述的四元数转动极为相似。事实上可以证明，Q＝BA。下面的积型恒等式来自矢量的矩阵形式的性质：

AB＝σ₀a·b+i(a×b)·σ                  (方程26)

在转动θ角的情况，单位法线a和b的交角必需是θ/2。因此，它们的点乘和叉乘给出a·b＝cos(θ/2)和a ×b＝n sin(θ/2)，这里n与交线平行。将这些值代入方程26给出想要的结果：

Q＝BA＝σ₀cos(θ/2)-i(n·σ)sin(θ/2)＝e^-iσ·θ/2    (方程27)

此推导的一个有趣特征是四元数的系数可用适当单位矢量的点乘和叉乘得到。特别是，这两个矢量a和b必需垂直于转动轴并且分开θ/2角。可以证明，已知在一个参照系中的两个任意矢量α和β和它们在转动参照系中的对应α′和β′，则四元数和与导致这种变换相联系的转动矩阵可被唯一确定。因此，在基本参照系中给定两个从原点到两个外参照点定义的矢量，则系统的方位角可由在转动系中对这两个外参照点作额外的‘观测’并比较这两个参照矢量在最终参照系与初始参照系中的坐标来唯一确定。

还可设想直接积分四元数的速度来计算四元数。这需要将四元数的导数表示为角速度的函数。这可对方程21微分导出。所得到的四元数系数导数的每一个是用已有四元数分量来加权的角速度分量的线性组合。如果四元数和角速度排列为矢量形式，则得到如下矩阵方程：

$\left[\begin{array}{c}{\stackrel{\·}{q}}_0\\ {\stackrel{\·}{q}}_1\\ {\stackrel{\·}{q}}_2\\ {\stackrel{\·}{q}}_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{ccc}{-q}_1& -q_2& -q_3\\ q_0& {-q}_3& q_2\\ q_3& q_0& -q_1\\ {-q}_2& q_1& q_0\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_1\\ {\mathrm{\ω}}_2\\ {\mathrm{\ω}}_3\end{array}\right]$ (方程28)

这里带点的q_i是四元数速度分量而ω_i是角速度ω(＝θ)的分量。

一个物体的取向和/或转动可在多个参照系中表示。例如，可相对感兴趣的物体本身或相对外部一固定物体来定义参照系。在方程28中角速度是参照本体系。参照地球系的角速度的类似矩阵方程表示为下面的方程29。请注意，参照系的移动在四元数矩阵中产生了若干符号变化。

$\left[\begin{array}{c}{\stackrel{\·}{q}}_0\\ {\stackrel{\·}{q}}_1\\ {\stackrel{\·}{q}}_2\\ {\stackrel{\·}{q}}_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{ccc}{-q}_1& -q_2& -q_3\\ q_0& q_3& {-q}_2\\ -q_3& q_0& q_1\\ q_2& {-q}_1& q_0\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_{E1}\\ {\mathrm{\ω}}_{E2}\\ {\mathrm{\ω}}_{E3}\end{array}\right]$ (方程29)

方程29可采用矩阵变数表示为如下所示的更紧致形式：

dq/dt＝Qω_E/2                                (方程30)

注意到Q^TQ＝I，方程30可对ω_E求解并表示如下：

ω_E＝2Q^T(dq/dt)                              (方程31)

用所讨论物体的瞬时角速度对四元数速度方程积分，得到的四元数含有从初始参照系到最终参照系坐标变换的所需信息。因此，四元数分量可用于去构成前面所描述的转动矩阵R(方程8)。借助四元数分量将本体系坐标变换到地球系坐标的转动矩阵表示如下：

$R_{\mathrm{EB}}=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_3{q}_0)& 2(q_1{q}_3+q_2{q}_0)\\ 2(q_2{q}_1+q_3{q}_0)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_1{q}_0)\\ 2(q_3{q}_1-q_2{q}_0)& 2(q_3{q}_2+q_1{q}_0)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$ (方程32)

因变换矩阵R_EB是正交的，它的转置是它的逆变换R_BE。因此，可用四元数通过一个物体的任何角运动来跟踪该物体的方位角。

为了对比的目的，计算欧拉角速度的方程也表示在下面。相联系的转动矩阵R也作为欧拉角的函数表示出来。

d/dt＝P+(dψ/dt)sinθ

dθ/dt＝Qcos-Rsin

dψ/dt＝(Qsin+Rcos)secθ

其中

＝Roll

θ＝Pitch

ψ＝Yaw

ω＝Angular Rate Vector＝[PQR]^T

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

正如已经知道的，欧拉方法大量地依赖于三角函数并且计算相当繁重。此外，欧拉速度方程依赖俯仰角的割线，它来自俯仰角到达±90度时出现的奇异点。相反，四元数方法不包含三角函数且只与乘和加有关。结果是四元数方法在计算上相当有效。正如上面已注意到的，给定两个在本体系中定义的矢量和两个外部参照点，一物体的方位角可由四元数形式唯一表示出来。然而，去确立两个外部参照点是困难和/或费力的，特别是位于地球上物体的情况，不论是在陆地上或水中物体的表面上或表面下。

本发明人相信本发明人是第一个注意到如何对四元数形式作有用的变更使得只需一个外部参照点去产生一变更四元数形式，并且此变更四元数形式能应用于自由倾斜的物体，但该物体的航向(偏航角)要受到约束或者不需要用采用变更四元数的系统来控制。

发明内容

本发明的一个目的是对可倾斜物体提供改进的跟踪和/或控制。

本发明的一个进一步的目的是提供跟踪和控制系统，此系统所需计算量比以前的技术系统小很多。

本发明的一个更进一步的目的是对只有一个外部参照点可利用的系统提供四元数处理。

本发明的另一个进一步的目的是对物体的取向提供可靠的高带宽跟踪。

本发明的一个方面是提供估算可倾斜物体方位角的方法，这里物体包括像射流倾斜传感器这样的倾斜探测设备和像陀螺这样的角速度探测设备。此方法包含步骤：从角速度探测设备输出角速度信息，变换和积分此输出的角速度信息去产生第1四元数位置信息，使得此第1四元数位置信息被约束为代表绕地球参照系中水平轴的转动，从倾斜探测设备输出倾斜信息，处理此输出的倾斜信息去产生第2四元数位置信息，使得这第2四元数位置信息被约束为代表绕地球参照系中水平轴的转动，与第1四元数位置信息作比较去产生误差信息并用此误差信息去补偿在角速度探测设备中的漂移。

本发明的另一个方面是提供估算可倾斜物体方位角的方法，这里物体包括像射流倾斜传感器这样的倾斜探测设备和像陀螺这样的角速度探测设备。此方法包含步骤：从角速度探测设备输出角速度信息，从此输出的速度信息去产生带有标量和二分量矢量形式的变更四元数速度信息，积分此变更四元数速度信息去产生第1变更四元数方位角信息，该产生变更四元数速度信息的步骤受到约束使得变更四元数方位角信息有形式e＝e₀+e₁i+e₂j，这里í，j是虚单位矢量，它们彼此及与第三个虚单位矢量k满足下列关系：

i²＝j²＝k²＝ijk＝-1，ij＝-ji＝k，jk＝-kj＝i，ki＝-ik＝j；

从倾斜探测设备输出倾斜信息，从此输出的倾斜信息去产生第2变更的四元数位置信息而使得第2四元数位置信息有形式e＝e₀+e₁i+e₂j，对第1变更四元数位置信息和第2变更四元数位置信息作比较去产生变更四元数位置误差信息，变换此变更四元数位置误差信息到角速度误差信号并用此角速度误差信号去补偿输出的角速度信息的误差。

用按照本发明提供的方法，从像射流倾斜传感器或类似仪器输出的信息被用于去补偿像一组陀螺这样的高带宽的漂移和其它移动。通过倾斜参照数据提供的误差补偿许可对校正的高带宽角速度数据积分去提供实时的方位角跟踪估算，因而不产生积累移动误差。航向角速度信息(偏航角速度信息)被略掉而产生一变更(3分量)四元数表示，此变更四元数表示可与由倾斜信息导出并以单个的重力参照点为基础的类似变更四元数速度信息相比。采用四元数记号来实施误差补偿和滤波去提供计算效率。

本发明的再一个方面是估算可倾斜物体方位角的方法，该方法包含以下步骤：在本体的系中产生角速度信息；对角速度信息施加漂移校正信号去产生校正角速度信息；用变更四元数估算位置信息处理该校正角速度信息去产生对地球参照系的变更四元数估算速度信息，该变更四元数估算速度信息由标量分量和两个矢量分量构成；以加权的位置误差信号为基础调整该变更四元数估算速度信息去产生估算位置差信号；积分该估算位置差信号去产生变更四元数估算位置信息，该变更四元数估算位置信息由标量分量和两个矢量分量构成；在本体的系中产生倾斜信息；用该倾斜信息产生变更四元数参照位置信息，该变更四元数参照位置信息由标量分量和两个矢量分量构成；由变更四元数估算位置信息减去变更四元数参照位置信息去产生误差位置信号；对该误差位置信号加权去产生加权的误差位置信号；用该变更四元数估算位置信息将误差位置信号转换到倾斜角误差信号；由该倾斜角信号的俯仰角和滚动角分量以及角速度信号的偏航角分量构成角误差矢量，此角速度信号是用变更四元数估算位置信息将校正的角速度信息转换到地球系产生的；用变更四元数估算位置信息将角误差矢量从地球系转换到本体的系；并且对转换的角误差矢量作加权积分产生漂移校正信号。

本发明的又一个方面是估算可倾斜物体方位角的仪器，该仪器包括：装配在该物体上用于输出倾斜信息的倾斜探测设备；装配在该物体上用于输出角速度信息的角速度探测设备；用于从角速度探测设备输出的角速度信息来产生变更四元数速度信息的设备，该变更四元数速度信息有标量和二分量矢量的形式；用于积分变更四元数速度信息去产生第1变更四元数方位角信息的设备，该变更四元数方位角信息有形式e＝e₀+e₁i+e₂j，其中í，j是虚单位矢量，它们彼此及与第三个虚单位矢量k满足下列关系：

i²＝j²＝k²＝ijk＝-1，ij＝-ji＝k，jk＝-kj＝i，ki＝-ik＝j；

用于从倾斜探测设备输出的倾斜信息来产生第2变更四元数位置信息的设备，该第2变更四元数位置信息有形式e＝e₀+e₁i+e₂j；用于比较该第1变更四元数位置信息到该第2变更四元数位置信息去产生变更四元数位置误差信息的设备；用于变换变更四元数位置误差信息到角速度误差信号的设备；和用于根据该角速度误差信号补偿从该角速度探测设备输出的角速度信息中的误差的设备。

本发明的以上和其它方面的特征和优点将由下列描述和附图陈述明白。

附图说明

图1是采用本发明的可倾斜物体10的示意和概括侧视图。

图2是图1中可倾斜物体的上视平面图。

图3是图1中可倾斜物体的控制系统的方块图示例说明。

图4是按本发明给出的图3中控制系统的方位角和转动部分的方块图表示。

具体实施方式

图1是适用本发明的一个一般可倾斜物体10的示意侧视图。

图2是同一物体10的上视平面图。物体10可以是任何物体，它从水平状态的倾斜被跟踪，表示和/或控制。例如，物体10可以是一个机器人，一个可倾斜的用马达控制的轮椅，一个离岸的钻探平台，水上舰艇和小船只和/或水下潜艇，可倾斜的有轨机车或汽车，在娱乐驾驶中的乘客室或运输工具，飞行模拟机或一个在化学反应或加工过程用于盛放或选择性地倾倒出物质的容器。物体10也可以是一辆小汽车。与物体10相联系的是控制系统12，它从一个或多个倾斜探测设备14和一个或多个角速度探测设备16接收输入。倾斜探测设备14可以是常用的射流倾斜传感器和/或过荷传感器。可设想这里有一个或多个倾斜传感器。角速度传感器16可由常用的陀螺或其它已知角速度探测设备。可设想这里有一个或多个角速度传感器。

为了以后的讨论可建立如图1和2所示的坐标系。如箭头20指示的，水平朝前方向视为x轴的正方向。如箭头22(图2)指示的，水平指向右并与x轴垂直视为正y轴。如箭头24(图1)指示的，垂直指向下方视为正z轴。倾斜被定义为离开垂线的角度偏转，它有从正到负180°的限定范围。倾斜方向由x和y轴定义。绕x轴的转动视为滚动角，向右倾斜滚动角为正。俯仰角定义为绕y轴的转动，反向倾斜时俯仰角为正。偏航角定义为绕z轴(垂直)的转动，向右转动偏航角为正。可以看出，这种定义与右手定则相连。除了欧拉角以特定的顺序实施并产生中间的参照系外，以上关于俯仰角和滚动角的定义与欧拉俯仰角和滚动角的定义类似。在此处讨论的基于倾斜的系统中，假定绕垂直轴的转动不存在或可略去。例如，物体10的航向可能是固定的，或与倾斜的跟踪和控制无关，或者已被手动操作控制，或者已用不考虑或不需考虑倾斜信息的系统作了控制。

图3用方块图的形式示例说明图1所示控制系统12的组件。控制系统12包含连接体40，通过它控制系统12接收从倾斜传感器14和陀螺16输出的信号。这些信号由方位角/转动估算方块42按照本发明作处理。以从陀螺和传感器输出的信号为基础，方块42以下面将描述的方式对物体10的方位角和/或角转动速度提供估算。以由方块42提供的方位角和/或转动信息为基础，方块44产生用于可控执行器46的控制信号去控制物体10的方位角。执行器可包括马达，螺线管，喷水箱，或其它的机械，电动机械，用于控制以上所指物体方位角的液动或汽动仪器。至少实行方块42和44的一部分的电子硬件可含有一个或多个微处理器。另外，控制信号产生方块44可优选地包含驱动电路，它为执行器46提供合适的驱动信号。

虽然图1指明控制系统12装在物体10上，但应当理解为至少控制系统12的某些部分是与物体10分开的。例如，实行方块42和44的一些或全部的处理电路可能远离物体10而通过遥测装置从倾斜传感器14和陀螺16接收输出，并且也可能用合适的无线通讯道传送控制信号回到物体10。

                方位角和转动的估算

现在描述方位角/转动估算方块42的操作。

像常用射流传感器这样的倾斜传感器可以相对地免除漂移，因而对重力矢量可提供出可靠读数。因此静态系统的方位角可用这种传感器相对于重力来确定。然而，在动态系统中，这种类型的传感器对角加速度和振动加速度是敏感的，因而必需作低通滤波去衰减重力以外的加速效应。所产生的数据提供重力在有限时间间隔内的平均方向，此时间间隔由滤波器的带宽确定。然而，在一要求快速反应时间的系统中，只有重力传感器是不适用的。

另一方面，陀螺或其它角速度传感器可提供高带宽和快速反应而没有加速的不利影响。虽然用这种传感器测定的角速度不直接指明系统的方位角，但对速度数据积分可产生位置信息。然而，速度传感器容易遭受漂移，作积分时它会引起重大误差。由方块42提供的估算处理将分别由陀螺16和倾斜传感器14给出的高和低带宽信息结合起来并采用以模型为基础的估算器布局去提供精确的位置和转动数据。

在以模型为基础的估算器中，真实位置估算由比较传感器数据与系统动力学的内部模型来产生。测量数据与预言数据之间的误差不断地用于去精确这种估算。此误差影响估算的程度由权重矩阵H确定，它反馈一个合适的状态误差到估算器。这样一个估算器的状态变数方程表示如下：

dx_e/dt＝Ax_e+Bu+H(y-y_e)

y_e    ＝Cx_e+Du

dx/dt ＝Ax+Bu

y     ＝Cx+Du

其中

x＝系统状态矢量

x_e＝估算状态矢量

y＝系统输出矢量

y_e＝估算输出矢量

u＝系统/估算器输入矢量

A＝系统矩阵

B＝输入矩阵

C＝输出矩阵

D＝输入/输出矩阵

H＝反馈矩阵

注意，下标e表示估算的参数。内部系统模型由矩阵(A，B，C，D)定义。反馈矩阵H确定估算器的稳定性。长时间以后，一个稳定估算器将趋于平衡，即真实的和估算的状态相同(x_e＝x)。内部模型经常以简单积分器为基础。这种简单模型的有效性与积分和微分对物理现象是基本的这一事实有关。例如，速度是位置的微分：v＝dx/dt。因此，给定一简单的参数为A＝0。B＝1，C＝1，D＝0和反馈增益H＝k₁的—阶积分器，则能够产生一个估算器，它将测量的系统静态位置θ₀和测量的动态位置θ结合起来，使得估算值的θ。在整个频率区间是精确的。其效应为，估算是对两种测量作频率相关的加权求和。这一结果的拉拉斯变换表示如下：

θe(s)＝θ₀(s)[k₁/(s+k₁)]+θ(s)[s/(s+k₁)]           (方程33)

注意，在低频率(s＝0)估算位置趋于低带宽位置测量θ₀，在高频率估算位置趋于高带宽位置测量θ。因此，高和低带宽数据被结合起来。

如果估算器是以简单的二阶积分器为基础，则可得到一较有用的估算。在这种情况，可采用形式H＝[k₁k₂]^T的反馈矩阵通过输出矩阵C＝[10]去反馈低带宽位置测量θ₀。产生的位置和速度估算的拉拉斯变换表示如下：

θe(s)＝ω(s)[s/(s²+k₁s+k₂)]+θ₀(s)[(k₁s+k₂)/(s²+k₁s+k₂)]      (方程34)

ωe(s)＝ω(s)[(s²+k₁s)/(s²+k₁s+k₂)]+θ₀(s)[k₂s/(s²+k₁s+k₂)]    (方程35)

这里ω(s)是dθ/dt在s-定义域的测量值。因此，这种估算器布局将低带宽位置θ₀和高带宽速度ω结合起来。在低频率，和以前一样估算位置趋于低带宽位置测量θ₀，在高频率估算位置趋于对高带宽速度测量ω的积分，此积分等价于高带宽位置θ。对估算速度可得类似结果。因此，这种类型的估算器很适合于从低带宽倾斜数据和高带宽角速度数据去产生角位置和角速度的估算。选取增益k₁和k₂使得可得到合适的滤波器带宽和稳定性。注意，在方程34中令k₂＝0和ω(s)＝sθ(s)，则产生与方程33所示相同的一阶位置估算。

已经知道，四元数可用于通过任意角运动去跟踪系统的方位角。此外，四元数在计算上是有效表记并且不经受欧拉角所遇到的奇异点。也已经知道，采用估算器可将来自高和低带宽传感器的数据结合起来去提供在整个带宽都精确的数据。然而，四元数不适合直接物理测量，这使得从倾斜传感器数据到绝对四元数基准的推导很复杂。没有这样一个基准，估算器不可能防止陀螺漂移对积分得到的四元数的损坏并且最终失去对系统角位置的跟踪。

由前面的讨论可回忆起，为了唯一地确定一个转动，常用四元数需要两个转动矢量和两个外参照点。然而，仅仅用倾斜传感器时，只有到地球中心的单个参照点可利用。本发明对基于倾斜的系统用提供一个变更四元数记号解决了这一问题，在此变更四元数记号中，常用四个元素中只有三个是非零，并且为了唯一地表示方位角(倾斜)时这种四元数形式只需要一个参照点。这种解决办法假定，系统不需要它的航向的知识因而不需要跟踪偏航向角(绕垂直轴的转动)的变化。

基于前面的发展，由单个参照点计算四元数系数需要在每一参照系中参照矢量与转动轴垂直。对于一个基于倾斜以重力作为参照的系统，这等价于要求在所有时间转动轴保持水平。如果转动轴n一直水平，则轴的z分量必需恒等为零。引用单位四元数分量的定义，这意味着四元数q₃分量必需恒等为零。因此，q₃＝0成为约束条件。因此产生的四元数被变更并且只含有三个有意义的分量。

当四元数速度方程(方程28或方程29)被积分时，约束q₃＝0也必需被满足。只要条件dq₃/dt＝0保持不变，在积分过程中条件q₃＝0也保持不变。由方程28和方程29，这意味着下列条件之一必需成立：

0＝dq₃/dt＝-q₂ω₁+q₁ω₂+q₀ω₃(对在本体系中积分)(方程36)

0＝dq₃/dt＝q₂ω_E1-q₁ω_E2+q₀ω_E3(对在地球系中积分)(方程37)

为了满足这些约束之一，如所要求的那样，至少一个参数许可任意变化。在本体系中，三个角速度分量和三个四元数分量都含有所需信息。然而，在地球系中因假定系统以倾斜为基础，假定了角速度的偏航角分量不重要。因此，真实的偏航角分量可用满足以上约束的虚假偏航角分量来代替。实际上，这可以将测量角速度从本体系变换到地球系来完成。所得到的地球偏航角用满足下面约束方程的虚假地球偏航角来代替：

ω′_E3＝(q₁ω_E2-q₂ω_E1)/q₀(满足dq₃/dt＝0的虚假地球偏航角)(方程38)

可将这个地球偏航角分量代回原来的四元数速度方程(方程29)去消除它对偏航角的显示依赖关系。为了区分变更的三元素四元数与其他四元数，现在变更四元数将用“e”而不用“q”表示。这些分量可以写为：

e₀＝cos(θ/2)和e＝[e₁e₂]^T＝nsin(θ/2)          (方程39)

where n＝[n₁n₂]^T和e₃＝n₃＝0

消除ω_E3和e₃以后，变更四元数速度方程成为：

$\left[\begin{array}{c}{\stackrel{\·}{e}}_0\\ \stackrel{\·}{e}\end{array}\right]=\frac{1}{2e_0}\left[\begin{array}{c}-e_0{e}^T\\ I-{\mathrm{ee}}^T\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_{E1}\\ {\mathrm{\ω}}_{E2}\end{array}\right]$ 这里I＝2×2恒等矩阵          (方程40)

转动矩阵R_EB(方程32)也可由消除ω_E3和e₃来简化而表示为：

R_EB＝[I-2ff^T 2fe₀]这里f＝[e₂-e₁]^T              (方程41)

将这些结果结合起来并注意到ω_E＝R_EBω，以地球系为参照的变更四元数速度可写为本体系中角速度的函数而表示为：

$\left[\begin{array}{c}{\stackrel{\·}{e}}_0\\ \stackrel{\·}{e}\end{array}\right]=\frac{1}{2e_0}\left[\begin{array}{cc}-e_0{e}^T& 0\\ {\mathrm{Ie}}_0^2-{\mathrm{ff}}^T& {2e}_{0}f\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_1\\ {\mathrm{\ω}}_2\\ {\mathrm{\ω}}_3\end{array}\right]$ (方程42)

这可简明地用矩阵形式表示为：

de/dt＝Eω/2                                (方程43)

采用这一结果，可以积分变更四元数速度去产生变更四元数位置。然而，这个方程假定理想的角速度矢量ω，在来自陀螺的实际角速度数据中没有对漂移提供补偿。一般说来，为了校正漂移必需将原始的角速度数据减去一个偏移项。在上面的方程中，用形式为ω′＝ω-ω_drift的校正角速度来代替ω。情况由陀螺漂移是时间的函数这一事实而变得进一步复杂化。因此，漂移校正项必需不断地更新。这需要采用以前描述的二阶估算器布局。

估算器的目的是对由积分四元数速度得到的四元数和由倾斜数据得到的四元数进行比较。假定任何定态误差只由陀螺漂移引起，这一误差的大小可用于调整陀螺漂移校正项。选取估算器的增益使得长时间以后定态误差趋于零。估算器增益值的选取是本领域的技术人员熟知的，因而不需要作进一步讨论。

为了合适地调整校正项，四元数误差必需被转换回角速度误差。这可采用原来的四元数速度方程来完成，此方程已对地球系中的角速度解出并表示为：

ω_E＝2Q^T(dq/dt)                           (方程31，重复)

对时间积分这一表示产生以下结果：

Δθ_E＝2Q^T(Δq)＝2Q^T(q’-q)               (方程44)

因此，上面的方程可用于去转换估算四元数和倾斜四元数之间的误差(δq＝q’-q)到倾斜角误差Δθ_E。可以回忆起，在这种情况下我们采用在q和Q两者中都满足q₃＝0的变更四元数。然而，用这种方法得到的地球偏航角分量是不正确的，因四元数由采用虚假地球偏航角产生。为了给出正确的角误差，原来的地球偏航角必需用误差分量Δθ_E3代替。地球偏航角的表达式可由ω_E＝R_EBω在q₃＝0时导出并在给出如下：

θ_E3＝(-2q₂q₀)ω₁+(2q₁q₀)ω₂+(q₀ ²-q₁ ²-q₂ ²)ω₃        (方程45)

在作替代以后，产生的在地球系中的角误差矢量由Δθ_E′＝[Δθ_E1，Δθ_E2，Δθ_E3]^T给出。因为陀螺是在本体系中，在地球系中的角误差必需用Δθ＝R_BEΔθ_E′变换到本体系中。然后，估算器使用得到的角误差去产生陀螺漂移校正项。

现在讨论将倾斜数据转换为变更四元数的方法。已经证明，四元数分量可由两个垂直于转动轴且分离角为θ/2的单位矢量构成点乘和叉乘来产生。两个这种矢量可用地球中心为外部参照点来构成(既重力方向与倾斜传感器测量的相同)。令初始重力矢量用G_i表记，终了重力矢量用G_f表记。(相应于初始重力矢量G_i的数据可在初始化过程中取得并存储起来作进一步使用。当估算器工作时终了重力矢量G_f被不断更新。)

令这两个矢量被归一化为：

g_i＝G_i/|G_i|和g_f＝G_f/|G_f|                (方程46)

用这些归一化的重力矢量，想得到的单位矢量可构成如下：

a＝(g_i+g_f)/|g_i+g_f|和b＝g_i               (方程47)

最后，四元数分量可由计算如下所示的点乘和叉乘来产生：

e₀＝a·b和e＝a×b                       (方程48)

按照构成，矢量a和b的叉乘产生一个垂直于地球系中重力矢量的矢量。结果，这个矢量必需是水平的。这意味着四元数的z分量正如所要求的那样恒等于零。因此，只有四元数的前三个分量是非平凡的并满足变更四元数的形式。

以上描述的整个估算过程在图4的方块图中作示例说明。

估算器的布局分为两个回路：k₁回路和k₂回路。k₁回路对来自倾斜传感器的数据提供低带宽滤波，并确定用于调整估算四元数速度的误差大小。k₂回路对陀螺数据提供漂移校正。因陀螺是在本体系中，k₂回路必需处在坐标变换的本体一边。k₂增益确定用于校正陀螺数据漂移的误差大小。对加权的误差作积分，结果是长时间以后回路取得零定态误差。换句话说，当四元数误差达到零时，k₂积分器的输出停止变化并且漂移校正项保持为常数。

在图4中，k₂回路中的求和方块50对由陀螺16输出的角速度信息施加加权误差信号(它也可视为漂移校正信号)去产生校正角速度信息。校正速度信息被转换到地球参照系并在方块52变换成变更四元数。正如方块52中标记E/2所指示的，在此方块中的计算是按照上面讨论过的方程43和42来实施的。所产生的估算速度信息以上面描述的变更四元数形式被提供为求和方块54的输入。在方块54，由k₁回路提供的校正施加到估算四元数速度信息去产生估算位置变化的数据指示。在方块56依次对此数据(它也可引证为估算位置误差信号)积分而提供变更四元数估算位置数据。在方块58，由方块56输出的变更四元数估算位置数据和方块52输出的估算变更四元数速度信息转换为欧拉角而被输出到控制信号产生方块44(图3)。采用下列方程这种转换容易完成。(虽然这些方程使用三角函数，计算不复杂并可用查表来实施。)

θ＝asin(2e₂e₀)和＝asin(2e₁e₀/cosθ)     (方程49)

对于基于倾斜的系统的运动是有限的事件，这些方程也可用小角度展开来简化。

如果控制信号发生器44的类型为输出四元数信号而不输出欧拉角信号，则方块58可略去，估算四元数位置和速度信息(按照本发明提供为变更四元数的形式)可直接向控制信号发生器提供。

在任何情况下，变更四元数的形式的估算位置信息提供为求和方块60的输入，并在此方块与由倾斜传感器14输出在方块62转换为变更四元数形式的当前倾斜信息作比较。(方块62产生的信号可引证为变更四元数参照位置信号。)在方块62实现的转换与上面关于方程46-48描述的程序是一致的。由求和方块60输出的合成误差信号在方块62用增益因子k₁加权，然后作为上面提到的校正信号施加在方块54。由方块60的输出(它可视为一个误差位置信号)也为方块66提供输入。按照方程44，方块66将估算四元数与倾斜四元数之间的误差转换为倾斜角误差。由方块66输出的合成信号的错误偏航角分量用地球系偏航角分量代替，此地球系偏航角分量经方块52的处理是可采用的，合成角误差矢量在方块68被转换到本体参照系。然后方块68的输出在方块70被积分并用k₂增益加权，并且合成信号作为上面提到的漂移校正信号施加到求和方块50。

应当注意到，正如在图4中在72所指示的，在方块52，66和68的过程所需要的系数取自积分方块56所输出的变更四元数位置信息。

正如本领域的技术人员所知道的，图4中用方块图形式作示例说明的过程可用一个或多个合适的程序计算设备(例如一个或多个微处理器)来便利的实现，而这些计算设备是控制系统12的一部分。

在按本发明提供的方位角和转动估算器中，由转动传感器提供的高带宽信息和来自倾斜传感器的低带宽信息被结合为新型的四元数记号，它含三个元素而不是通常的四个元素。产生的估算四元数在整个的设计带宽上是正确的并且不随时间漂移。要实行的计算可使用像乘，加和平方根这种简单的运算来处理。变更四元数估算器可应用于要跟踪和/或控制倾斜的广泛系统。

发明的以上描述只是示例说明而不是限定不变的。在所描述实施方案中，本领域的技术人员可能会作各种变化或修改而不偏离本发明的构思或范围。

Claims (11)

1.估算可倾斜物体方位角的方法，该物体包括倾斜探测设备和角速度探测设备，此方法包含步骤：

从该角速度探测设备输出角速度信息；

变换和积分该输出的角速度信息去产生第1四元数位置信息使得该第1四元数位置信息被约束为表示绕地球参照系中水平轴的转动；

从倾斜探测设备输出倾斜信息；

处理该输出的倾斜信息去产生第2四元数位置信息使得该第2四元数位置信息被约束为表示绕该地球参照系中水平轴的转动；

比较该第1四元数位置信息与该第2四元数位置信息去产生误差信息；并且

用该误差信息去补偿该角速度探测设备的漂移。

2.估算可倾斜物体方位角的方法，该物体包括倾斜探测设备和角速度探测设备，此方法包含步骤：

从该角速度探测设备输出角速度信息；

从该输出的角速度信息产生变更四元数速度信息，该变更四元数速度信息有标量和二分量矢量的形式；

积分该变更四元数速度信息去产生第1变更四元数方位角信息，产生变更四元数速度信息的步骤被约束，使得该第1变更四元数方位角信息有形式e＝e₀+e₁i+e₂j，这里i，j是虚单位矢量，它们彼此及与第三个虚单位矢量k满足下列关系：

i²＝j²＝k²＝ijk＝-1，ij＝-ji＝k，jk＝-kj＝i，ki＝-ik＝j；从该倾斜探测设备输出倾斜信息；

从该输出的倾斜信息产生第2变更四元数位置信息，该第2变更四元数位置信息有形式e＝e₀+e₁i+e₂j；

比较该第1变更四元数位置信息到该第2变更四元数位置信息去产生变更四元数位置误差信息；

变换该变更四元数位置误差信息到角速度误差信号；并且

用该角速度误差信号去补偿在该输出的角速度信息中的误差。

3.按照权利要求2的方法，进一步包含对该第2变更四元数位置信息作低通滤波的步骤。

4.按照权利要求2的方法，其中：

该角速度信息在第1参照系中提供，倾斜信息在不同与该第1参照系的第2参照系中产生；

产生该变更四元数速度信息的步骤包括将该输出的角速度信息从该第1参照系转换到第2参照系；并且

还包括在使用该步骤之前将该角速度误差信号从该第2参照系转换到该第1参照系的步骤。

5.按照权利要求2的方法，其中该第2变更四元数位置信息是以由倾斜探测设备输出并在第1时刻储存的第1倾斜信息和由倾斜探测设备输出并在与第1时刻不同的第2时刻储存的第2倾斜信息为基础产生的。

6.估算可倾斜物体方位角的方法，包含步骤：

在本体的系中产生角速度信息；

对角速度信息施加漂移校正信号去产生校正角速度信息；

用变更四元数估算位置信息处理该校正角速度信息去产生对地球参照系的变更四元数估算速度信息，该变更四元数估算速度信息由标量分量和两个矢量分量构成；

以加权的位置误差信号为基础调整该变更四元数估算速度信息去产生估算位置差信号；

积分该估算位置差信号去产生变更四元数估算位置信息，该变更四元数估算位置信息由标量分量和两个矢量分量构成；

在本体的系中产生倾斜信息；

用该倾斜信息产生变更四元数参照位置信息，该变更四元数参照位置信息由标量分量和两个矢量分量构成；

由变更四元数估算位置信息减去变更四元数参照位置信息去产生误差位置信号；

对该误差位置信号加权去产生加权的误差位置信号；

用该变更四元数估算位置信息将误差位置信号转换到倾斜角误差信号；

由该倾斜角信号的俯仰角和滚动角分量以及角速度信号的偏航角分量构成角误差矢量，此角速度信号是用变更四元数估算位置信息将校正的角速度信息转换到地球系产生的；

用变更四元数估算位置信息将角误差矢量从地球系转换到本体的系；并且

对转换的角误差矢量作加权积分产生漂移校正信号。

7.按照权利要求6的方法，进一步包含步骤：

转换变更四元数估算速度信息和变更四元数估算位置信息去产生欧拉角输出信息；和

输出该欧拉角输出信息到控制信号产生装置。

8.估算可倾斜物体方位角的仪器，包括：

装配在该物体上用于输出倾斜信息的倾斜探测设备；

装配在该物体上用于输出角速度信息的角速度探测设备；

用于从角速度探测设备输出的角速度信息来产生变更四元数速度信息的设备，该变更四元数速度信息有标量和二分量矢量的形式；

用于积分变更四元数速度信息去产生第1变更四元数方位角信息的设备，该变更四元数方位角信息有形式e＝e₀+e₁i+e₂j，其中i，j是虚单位矢量，它们彼此及与第三个虚单位矢量k满足下列关系：

i²＝j²＝k²＝ijk＝-1，ij＝-ji＝k，jk＝-kj＝i，ki＝-ik＝j；

用于从倾斜探测设备输出的倾斜信息来产生第2变更四元数位置信息的设备，该第2变更四元数位置信息有形式e＝e₀+e₁i+e₂j；

用于比较该第1变更四元数位置信息到该第2变更四元数位置信息去产生变更四元数位置误差信息的设备；

用于变换变更四元数位置误差信息到角速度误差信号的设备；和

用于根据该角速度误差信号补偿从该角速度探测设备输出的角速度信息中的误差的设备。

9.按照权利要求8的仪器，其中该倾斜探测设备至少包括一个射流倾斜传感器。

10.按照权利要求9的仪器，其中该角速度探测设备至少包括一个陀螺。

11.按照权利要求8的仪器，进一步包含用于将该变更四元数速度信息转换到欧拉角速度信息和将该第1变更四元数方位角信息转换到欧拉角位置信息的设备。

Images