1.3　惯性单元

假设我们被封闭在一个靠近地球的盒子里，看不见外面的环境，在盒子里，我们可以进行一些惯性实验，比如观察物体的平移或旋转。这些实验使我们能够间接地测量切向加速度和盒子在其自身坐标系内的角速度，惯性单元的原理是在假设初始姿态已知的条件下，仅根据这些惯性测量（称为本体感觉测量）来估计盒子的姿态（位置和方向）。

为此，我们将用一个运动学模型来描述盒子的运动，它的输入是切向加速度和角速度，这些量在盒子的坐标系中是可以直接测量的。其状态向量是由向量P=（p_x，p_y，p_z）、三个欧拉角（ψ，θ，φ）和速度向量v_r组成的，其中向量P=（p_x，p_y，p_z）给出了绝对惯性坐标系R₀中的盒子中心坐标；而机器人的速度向量v_r则是表示在其自身坐标系R₁内的。该系统的输入有两个，其一为表示在其自身坐标系内的机器人中心的加速度，其二则是表示在机器人坐标系R₁内机器人相对于坐标系R₀的旋转向量。因为机器人自身可通过其上安装的传感器去测量a，w，所以较为常见的就是将这些量表示在机器人坐标系中。第一个状态方程为：

对该方程求导可得：

式中，R=R（ψ，θ，φ），则将其用另一种形式表示为：

上式构成了第二个状态方程。图1.6给出了一个机器人以恒定速度沿圆周运动的情况。速度向量v_r为一常量而a_r不为0。

图1.6　当=0时，式=a_r-ω_r∧V_r的图解
a）机器人跟随一个圆；b）机器人坐标系下的向量表示

此外，还需将表示为一个关于状态变量的方程，根据方程式（1.11）可得下式：

转换为：

通过在该表达式中提取向量，可得第三个状态方程。将如上三个状态方程联立起来，便可得到该机器人航星机械化方程[FAR 08]：

在水平面上。对于一个机器人在水平面上运动，有ψ=θ=0。由式（1.11）可知=w_r3，=w_r2以及=w_r1。在这种情况下，便有了一个w_r各元素和欧拉角的微分之间的完美对应关系。同时也会有一些奇异的情况，例如当时（这是机器人指向上方时的情形），无法定义欧拉角的微分。但旋转向量通常不会存在这种奇异性，因此成了首选。

航位推测法。对于航位推测法（即没有外部传感器），通常会有一些非常精密的激光陀螺测试仪（旋转速度为0.001deg/s），这些仪器利用的是萨格纳克效应（在一个环形光纤绕其自身回转的过程中，光循行一个完整来回所花费的时间是依赖于其路径方向的）。使用三种光纤，那么这些陀螺测试仪就会产生向量w_r=（w_x，w_y，w_z），同时也会有一些能够以很高精度测量加速度a_r的加速度计。在纯惯性模型中，仅利用表示在机器人坐标系内的加速度a_r和旋转速度w_r对式（1.12）进行求导便可得到位置信息。在利用DVL测量v_r（同样表示在机器人坐标系内）时，仅需对这三个方程中的第一个和第三个方程求积分即可。最后，如果该机器人是一个有合适压载的潜艇或者一个在相对平整地面上移动的地面机器人时，可以得出一个推论，即横滚角和俯仰角的平均值等于0。因此，为了限制定位偏移，利用卡尔曼滤波器来处理该信息。而一个有效的惯性单元包含了所有可用信息的集合。

惯性单元。一个纯惯性单元（没有杂质也不考虑地球引力）代表了一个如图1.7所示的运动学模型的机器人，而其自身状态方程由式（1.12）给出。该系统可以写为=f（x,u）的形式，其中u=（a_r，w_r）为所测量的惯性输入向量（通过地面的一个观测器检测其加速度和旋转速度，并将其表示在机器人坐标系内），x=（p，v_r，ψ，θ，φ）为状态向量。此时，可利用类似于欧拉法的数值积分方法，将微分方程=f（x,u）用如下循环代替：

图1.7　航行机械化方程

重力作用下。在存在依赖于p的重力g（p）的情况下，实际加速度a_r为重力加上测得的加速度，此时，惯性单元可由以下状态方程描述：

这种情况下，系统的输入是由加速度计和陀螺仪获得的和w_r。对该状态方程进行积分以实现定位，则需知道初始状态和一个重力图g（p）。

用旋转矩阵代替欧拉角：由式（1.4）可得=R·Ad（R^Tw），因此，在无任何欧拉角的前提下，可将状态方程（1.13）写为：

这种表示方法的优势在于不存在cosθ=0时式（1.13）中的奇异点。但相反，因利用9维矩阵R替代了这三个欧拉角，导致存在冗余。由于数值原因，当长时积分时，矩阵R（t）有可能会丢失R·R^T=I的性质。为避免这种情况，每次迭代都需要进行归一化处理，即当前矩阵R应投影到SO（3）。一种可能性就是利用QR因式分解。方阵M的QR分解可得两个矩阵Q，R，使得M=Q·R，其中Q为旋转矩阵，R为三角矩阵。当M近似为旋转矩阵且R近似为一个三角旋转矩阵时，即在R的对角线上，所有项均近似为±1，除此之外的项均为0；如下Python代码执行了M到SO（3）的投影，并生成了旋转矩阵M₂。

---

Q,R = numpy.linalg.qr(M)
v=diag(sign(R))
M2=Q@diag(v)

---