第四章：姿态表示方法（中）——方向余弦矩阵（DCM）

好，咱们接着聊姿态表示。上一章讲了欧拉角，那个直观是直观，但有个万向锁的问题让我头疼了很久。今天要说的方向余弦矩阵（DCM），虽然看起来矩阵运算多了点，但它没有奇点，这是它最大的优势。

4.1 什么是方向余弦矩阵

说白了，方向余弦矩阵就是一个3×3的旋转矩阵。它描述了两个坐标系之间的相对姿态关系。为什么叫“方向余弦”？因为矩阵里的每个元素，其实就是两个坐标系对应轴之间的夹角余弦值。

举个例子。假设有个体坐标系b，还有个导航坐标系n。那么从n系到b系的旋转，就可以用一个3×3的矩阵C_n^b来表示：

C_n^b = [
    [cos(x_b, x_n), cos(x_b, y_n), cos(x_b, z_n)],
    [cos(y_b, x_n), cos(y_b, y_n), cos(y_b, z_n)],
    [cos(z_b, x_n), cos(z_b, y_n), cos(z_b, z_n)]
]

你看，每个元素都是两个轴夹角的余弦值。这就是“方向余弦”这个名字的由来。

4.2 DCM的基本性质

DCM有几个非常重要的性质，我建议你牢牢记住。我在做项目时，经常用这些性质来检查代码有没有写错。

4.2.1 正交性

DCM是一个正交矩阵。这意味着：

• C^T = C^-1 —— 转置等于逆
• det(C) = +1 —— 行列式为1（右手系）
• 每一行、每一列都是单位向量
• 不同行（或不同列）之间相互正交

为什么会这样？因为坐标系旋转不会改变向量的长度和夹角。你想想看，一个刚体旋转后，它上面任意两点的距离是不变的。这就是正交性的物理意义。

4.2.2 逆矩阵就是转置

这个性质太有用了。如果C_n^b表示从n系到b系的旋转，那么从b系回到n系就是：

C_b^n = (C_n^b)^T = C_n^b^T

不需要求逆！直接转置就行。这在实时计算中能省下不少运算量。我记得第一次写惯导程序时，还傻乎乎地去调矩阵求逆函数，后来被老工程师骂了一顿：“你转置一下不就行了？”

4.3 DCM的正交化

我曾经在一个无人机项目上遇到过这个问题。飞了十几分钟后，姿态开始漂移。查了半天，发现就是DCM的正交性被破坏了。从那以后，我每次更新DCM后都会做正交化处理。

最常用的正交化方法是Gram-Schmidt过程。这里我给出一个简单版本：

def orthogonalize_dcm(C):
    # 提取三个行向量
    r1 = C[0, :]
    r2 = C[1, :]
    r3 = C[2, :]
    
    # 第一步：归一化第一行
    r1 = r1 / np.linalg.norm(r1)
    
    # 第二步：从第二行中减去在第一行上的投影
    r2 = r2 - np.dot(r2, r1) * r1
    r2 = r2 / np.linalg.norm(r2)
    
    # 第三步：第三行由前两行的叉积得到
    r3 = np.cross(r1, r2)
    
    return np.array([r1, r2, r3])

嗯，这里要注意。第三步用叉积而不是继续做Gram-Schmidt，是为了保证右手系。如果你用Gram-Schmidt做第三步，可能会得到左手系，那行列式就变成-1了。

4.4 DCM的链式法则

链式法则是DCM最强大的性质之一。它说的是：多个旋转可以按顺序组合成一个总的旋转矩阵。

假设我们有三个坐标系：

• 地心地固系（e系）
• 导航坐标系（n系）
• 机体坐标系（b系）

那么从e系到b系的旋转可以写成：

C_e^b = C_n^b · C_e^n

注意顺序！是先转C_eⁿ，再转C_n^b。矩阵乘法不满足交换律，顺序搞反了结果就全错了。

我画了个图，帮你理解这个链式关系：

f^n = C_b^n · f^b

# 假设C = [c11, c12, c13; c21, c22, c23; c31, c32, c33]
roll  = atan2(c32, c33)        # 横滚角
pitch = -asin(c31)             # 俯仰角
yaw   = atan2(c21, c11)        # 航向角