搜索
姿态解算初步——陈佳伟姿态解算理解1、姿态的描述方法前几天在论坛里偶尔看到一个帖子，帖子的内容是问的为什么不用倾斜角表示姿态，我认为他说的倾斜角是指的斜面与斜面的夹角，或者说是物体与垂线的夹角吧，这种想法可能来源于我们日常生活的思维。图1立方体比如有一个立方体，我们放在水平面上的时候它的底面是和水平面平行的，但是当我们把立方体的一个脚垫起一个角度时，这样一来，立方体的一条棱与水平面的垂线就有了一定的夹角了。我们所说倾斜了多少多少度就是指的这个夹角，这是我们直观的反应。我认为这样直观的反应甚至比欧拉角还要来的直观，因为欧拉角是基于旋转的，肯定不会说这个立方体X、Y轴各旋转了多少度（假设Z旋转无效），我们可能也没那个概念，我们直观的反应就是它倾斜了一定度数。但是我们在姿态解算的时候为啥不用的这种描述方法呢，个人认为是虽然方便我们直观的表达但不适合数学上的计算，还有就是我们仅仅知道这个倾斜角我们怎么施加控制量呢？高中物理学习物体运动的时候我们知道，物体的运动是合运动，我们可以把它的运动矢量正交分解为几个运动的合成（不正交也是可以的，但那不是在自找麻烦吗），同样道理，我们可以把刚体的旋转分解为三个轴上的旋转，这个旋转的角度就是欧拉角，如图2。图2zxz序规欧拉角欧拉角欧拉角的定义不仅仅和旋转角度有关系，还和旋转轴的旋转顺序有关系，任何一种旋转顺序都是合法的。根据定义，欧拉角有12种旋转顺序（维基），一个物体通过任意一个旋转顺序都可以达到同样的姿态，在各个学科里所以为了统一，航空航天领域规定XYZ为欧拉角的旋转顺序。上面已经说了欧拉角的定义。欧拉角的定义也是很直观而且容易理解的，也利于我们的计算，因为我们用的惯性器件也是按照单个轴向运动来测量的。定义上的欧拉角还和我们所说的Yaw、Pitch、Roll不是一回事。因为定义上的欧拉角就是刚体绕三个轴的旋转角度，姿态解算初步——陈佳伟欧拉旋转和外界的东西（参考系）是没有关系的。Yaw、Pitch、Roll就是载体对于参考系来说的了，这意味着第一次的旋转不会影响第二、三次的转轴，因为选的参考系都是地球了（这也是产生GimbalLock的原因，下面会讲到）。我们现在说的飞机都是在近地表附近飞行，所以我们习惯是拿地球作为参考系，我们的飞机总是在一点起飞在另一点降落。所以我们规定地理方位东、北、天为参考初始点，也就是说，我们的飞机头朝北水平放置时载体坐标系和参考坐标系是重合的，那么接下来我们绕飞机的Z轴旋转30°，这个旋转的欧拉角就是我们所说的Yaw，同样，绕飞机的X轴旋转30°，我们得到Pitch，绕飞机Y轴旋转得到Roll，如图3。图3这和规定欧拉角的旋转顺序是一样的，所以说是和欧拉角是一一对应的，要注意欧拉角是基于飞机本身轴旋转得到的，但是得到的姿态却是对于参考坐标系而说的，要不然对我们来说也没有实际的意义，来个更直观的：欧拉角是有很多优点的。但是也有致命的缺点，那就是GimbalLock（万向节死锁），要理解GimbalLock所说的情况，这可能有点不好理解，让我们看个现实中的场景。假如我们有一个望远镜和一个用来放望远镜的三脚架，（我们将）三脚架放在地面上，使支撑望远镜的三脚架的顶部是平行于地平面（参考平面）的，以便使得竖向的旋转轴（记为x轴）是完全地垂直于地平面的。现在，我们就可以将望远镜饶x轴旋转360度，从而观察（以望远镜为中心的）水平包围圈的所有方向。通常将正北朝向方位角度记为0度方位角。第二个坐标轴，即平行于地平面的横向的坐标轴（记为y轴）使得望远镜可以饶着它上下旋姿态解算初步——陈佳伟转，通常将地平面朝向的仰角记为0度，这样，望远镜可以向上仰+90度指向天顶，或者向下-90度指向脚底。好了，万事俱备。现在，天空中（包括地面上）的每个点只需要唯一的一对x和y度数就可以确定。比如x=90度,y=45度指向的点是位于正东方向的半天空上。现在，看看万向节死锁是怎么发生的。一次，我们探测到有一个飞行器贴地飞行，位于望远镜的正东方向（x=90度，y=10度），朝着我们直飞过来，我们跟踪它。飞行器飞行方向是保持x轴角度90度不变，而y向的角度在慢慢增大。随着飞行器的临近，y轴角增长的越来越快且当y向的角度达到90度时（即将超越），突然它急转弯朝南飞去。这时，我们发现我们不能将望远镜朝向南方，因为此时y向已经是90度，造成我们失去跟踪目标。这就是万向节死锁！为什么说不能将望远镜朝向南方呢，让我们看看坐标变化，从开始的（x=90度，y=10度）到（x=90度，y=90度），这个过程没有问题，望远镜慢慢转动跟踪飞行器。当飞行器到达（x=90度，y=90度）后，坐标突然变成（x=180度，y=90度）（因为朝南），x由90突变成180度,所以望远镜需要饶垂直轴向x轴旋转180-90=90度以便追上飞行器,但此时，望远镜已经是平行于x轴，我们知道饶平行于自身的中轴线的的旋转改变不了朝向，就象拧螺丝一样，螺丝头的指向不变。所以望远镜的指向还是天顶。而后由于飞行器飞远，坐标变成（x=180度，y90度）时，y向角减小，望远镜只能又转回到正东指向，望'器'兴叹。这说明用x,y旋转角（又称欧拉角）来定向物体有时并不能按照你想像的那样工作，象上面的例子中从（x=90度，y=10度）到（x=90度，y=90度），按照欧拉角旋转确实可以正确地定向，但从（x=90度，y=90度）到（x=180度，y=90度），再到（x=180度，y90度）,按照欧拉角旋转后的定向并非正确。我的理解是坐标值的变化和飞行器空间的位置变化一一对应，但是从（x=90度，y=90度）到（x=180度，y=90度），再到（x=180度，y90度）这个变化，飞行器位置是连续的变化，但坐标值的变化却不是连续的（从90突变到180），其原因在于（x=90度，y=90度）和（x=180度，y=90度）甚至和（x=任意度，y=90度）这些不同的坐标值对应空间同一个位置，这种多个坐标值对应同一个位置的不一致性是造成死锁的根源。上面是2维坐标系中的例子，同样，对于3维的也一样。比如有一个平行于x轴的向量，我们先将它饶y旋转直到它平行于z轴，这时，我们会发现任何饶z的旋转都改变不了向量的方向，即万向节死锁，所以说传统的欧拉角是不能做到全姿态解析的。（抄的）方向余弦一个向量在坐标系中的位置也可以用方向余弦表示，也就是这个向量分别到三个坐标轴的夹角余弦值，实际上就是这个向量到各个坐标轴的投影啦，角度范围是0~π（维基）。所以推广到载体坐标系和参考坐标系当中，我们就有了载体坐标轴xyz分别与参考轴XYZ的方向余弦，这里就是所说的方向余弦矩阵了，它是由两组不同的标准正交基的基底向量之间的方向余弦所形成的3x3矩阵。方向余弦矩阵可以用来表达一组标准正交基与另一组标准正交基之间的关系。余弦矩阵的列表示载体坐标系中的单位矢量在参考坐标系中的投影。分量形式如下:第i行、j列的元素表示参考坐标系i轴和载体坐标系j轴夹角的余弦。其实方向余弦和欧拉角没有本质上的区别，因为这是用欧拉角表示的方向余弦。一个坐标系到另一个坐标系的变换，在《捷联惯性导航技术》、和《惯性导航》中都是有介绍的，姿态解算初步——陈佳伟特别是《惯性导航》有推导的过程。推广到三轴的单次旋转，我们用矩阵表示为：这里要说一下矩阵的含义，C21表示坐标系1到坐标系2的变换矩阵，那么，这是绕Z轴的基本旋转，我们可以看到X2、Y2投影为0，Z2投影为1，这是为什么呢？自己想想。。。咱们看下面的单位1，这是什么？所以说这就是上面说的坐标系2到坐标系1轴的三个方向余弦！对吧。。。这在大家熟知的imu.c里面就有直接的计算。那么单独旋转各个轴，我们得到：实际上，两坐标系任何复杂的角位置关系都可以看做有限次基本旋转的组合，变换矩阵等于基本旋转确定的变换矩阵的连乘，要是不知道矩阵和矩阵乘法，那就看线性代数吧，连乘的基本顺序依据基本旋转的顺序向右排列。之所以有顺序是因为矩阵有“左乘”和“右乘”之分。那么我们得到：姿态解算初步——陈佳伟按上面推论，此矩阵的下面三个就是三轴对应的方向余弦，γ、θ、ψ就是欧拉角，现在明白了欧拉角和方向余弦矩阵的关系了吧。四元数四元数要说的实在太多，因为它的优点很多，利用起来很方便，但是理解起来就有点蹩脚了。我们百度四元数，一开始看到的就是四元数来历，还有就是四元数的基本计算。对于来历，还是想说一下，四元数（Quaternions）是由威廉·卢云·哈密尔顿(WilliamRowanHamilton,1805-1865）在1843年爱尔兰发现的数学概念（百度百科）。将实数域扩充到复数域，并用复数来表示平面向量，用复数的加、乘运算表示平面向量的合成、伸缩和旋，这就是我们熟知的复数的二维空间含义，所以人们会继续猜想，利用三维复数不就可以表达三维空间的变换了吗，历史上有很多数学家试图寻找过三维的复数，但后来证明这样的三维复数是不存在的。有关这个结论的证明，我没有查到更明确的版本，据《古今数学思想》中的一个理由，三维空间中的伸缩旋转变换需要四个变量来决定：两个变量决定轴的方向，一个变量决定旋转角度，一个变量决定伸缩比例。这样，只有三个变量的三维复数无法满足这样的要求。但是历史上得到的应该是比这个更强的结论，即使不考虑空间旋转，只从代数角度来说，三维的复数域作为普通复数域的扩张域是不存在的。并且，据《古今数学思想》叙述，即使像哈密尔顿后来引入四元数那样，牺牲乘法交换律，这样的三维复数也得不到。经过一些年的努力之后，Hamilton发现自己被迫应作两个让步，第一个是他的新数包含四个分量，而第二个是他必须牺牲乘法交换律。（《古今数学思想》第三册177页）但是四元数用作旋转的作用明显，简化了运算，而且避免了GimbalLock，四元数是最简单的超复数，我们不能把四元数简单的理解为3D空间的矢量，它是4维空间中的的矢量，也是非常不容易想像的（抄的）。1、四元数的表示方式：姿态解算初步——陈佳伟2、四元数的计算：关于四元数的基本运算，加减乘除，大家从别的地方看看吧，维基百科要注意的是，四元数的元的平方定义为-1，这点等同于复数，四元数不同元相乘得到另外一个，这点有点像向量叉乘。可见，四元数相乘不是虚数相乘也不是向量相乘，这一运算特点是由四元数的性质决定的。四元数由一个实数和三个虚数构成，所以是一个四维空间的向量，但是它的三个虚数又有三维空间的性质，因此，三维空间中的一个矢量，可以看作一个实部为0的四元数，这个四元数是这个三维空间的一个矢量在四维空间里的“映像”，或者叫做这个矢量的“四元数映像”。这样，我们就把三维空间和一个四维空间联系起来，用四维空间中四元数的性质和运算规律来研究三维空间中刚体定点转动的问题。四元数的乘法不可逆性，这里说的是四元数的外积，定义两个四元数:它们相乘得到：（看明白这个！既得到了平行又得到了垂直分量）四元数乘法的非可换性，pq并不等于qp。我们发现，两个四元数相乘，我们不仅仅的得到内机，而且还得到外积！这是由四元数运算性质决定的。qp乘积的向量部分是:，四元数乘法不可逆。下面要说四元数是如何表示旋转的，这个确实有点不好理解，我看到的资料有的说的还是有点出入的，或许只有自己理解了才能明白吧。“一个单位四元数可以表示一个旋转”，这句话的信息量确实够大的。它基于的思路是:一个坐标系到另一个坐标系的变换可以通过绕一个定义在参考坐标系中的矢量μ的单次转动来实现，四元数提供了这种数学描述。我们直观的想一下，我们可以把刚体仅仅旋转一次就能达到任意的姿态，前提是确定了旋转轴线和旋转的角度，大家想一想，感觉也是可以实现的吧。设Q=a+bi+cj+dk，我们可以写成[w，v],其中w=a，v=bi+cj+dk。那么，v是矢量，表示三维空间里的旋转轴。w标量，表示旋转角度。所以，一个四元数可以表示一个完整的旋转。但要注意只有单位四元数才可以表示旋转，至于为什么，那是因为这就是四元数表示旋转的约束条