SLAM常见功能代码段整理汇总1

整理汇总了部分SLAM中常见功能的代码段实现，以备参考。所有代码均在本机测试通过。

1.位姿转换

主要包含四元数、旋转矩阵、欧拉角和旋转轴不同姿态表示之间的转换。

1.1 四元数归一化

描述：给定一个四元数(qw, qx, qy, qz)，将其进行归一化（模长为1）。

关键：按公式\(\sqrt{ {qx}^2+ {qy}^2+ {qz}^2+ {qw}^2}\)求解模长归一化即可，详见这篇博客。

代码：

import math


def normalizeQuat(qw, qx, qy, qz):
    ori_norm = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw)

    qx_norm = qx / ori_norm
    qy_norm = qy / ori_norm
    qz_norm = qz / ori_norm
    qw_norm = qw / ori_norm

    return qw_norm, qx_norm, qy_norm, qz_norm


if __name__ == '__main__':
    qw = 0.1
    qx = 0.34
    qy = 0.2
    qz = 0.6
    
    qw_norm, qx_norm, qy_norm, qz_norm = normalizeQuat(qw, qx, qy, qz)

    print(qw_norm, qx_norm, qy_norm, qz_norm)

1.2 四元数转旋转矩阵

描述：给定一个非归一化的四元数(qw, qx, qy, qz)，将其转换为对应的旋转矩阵。

关键：按照转换公式直接转换即可，注意转换前四元数要归一化，详见这篇博客。

代码：

import math
import numpy as np


def cvtQuat2Rot(qw, qx, qy, qz):
    q_mod = math.sqrt(qw * qw + qx * qx + qy * qy + qz * qz)
    qw_norm = qw / q_mod
    qx_norm = qx / q_mod
    qy_norm = qy / q_mod
    qz_norm = qz / q_mod

    r11 = 1 - 2 * (qy_norm * qy_norm + qz_norm * qz_norm)
    r12 = 2 * (qx_norm * qy_norm - qw_norm * qz_norm)
    r13 = 2 * (qx_norm * qz_norm + qw_norm * qy_norm)
    r21 = 2 * (qx_norm * qy_norm + qw_norm * qz_norm)
    r22 = 1 - 2 * (qx_norm * qx_norm + qz_norm * qz_norm)
    r23 = 2 * (qy_norm * qz_norm - qw_norm * qx_norm)
    r31 = 2 * (qx_norm * qz_norm - qw_norm * qy_norm)
    r32 = 2 * (qy_norm * qz_norm + qw_norm * qx_norm)
    r33 = 1 - 2 * (qx_norm * qx_norm + qy_norm * qy_norm)

    rot_mat = np.array([[r11, r12, r13], [r21, r22, r23], [r31, r32, r33]])

    return rot_mat


if __name__ == '__main__':
    qw = 0.1239
    qx = 0.6
    qy = 0.2
    qz = 0.7

    rot_mat = cvtQuat2Rot(qw, qx, qy, qz)

    print(rot_mat)

    # check identity matrix
    print(np.matmul(rot_mat, np.transpose(rot_mat)))

1.3 旋转矩阵转四元数

描述：给定一个旋转矩阵，将其转换为对应的归一化四元数(qw, qx, qy, qz)。

关键：按照转换公式直接转换即可，核心是旋转矩阵的迹是否大于0(矩阵主对角线上各个元素总和)，需要根据不同情况使用不同公式。详见这篇博客。

代码：

import numpy as np

def cvtRot2Quat(R):
    # 计算矩阵迹并选择分支
    trace = np.trace(R)
    
    if trace > 0:
        S = np.sqrt(trace + 1.0) * 2
        w = 0.25 * S
        x = (R[2, 1] - R[1, 2]) / S
        y = (R[0, 2] - R[2, 0]) / S
        z = (R[1, 0] - R[0, 1]) / S
    else:
        if R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
            S = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2
            w = (R[2, 1] - R[1, 2]) / S
            x = 0.25 * S
            y = (R[0, 1] + R[1, 0]) / S
            z = (R[0, 2] + R[2, 0]) / S
        elif R[1, 1] > R[2, 2]:
            S = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2
            w = (R[0, 2] - R[2, 0]) / S
            x = (R[0, 1] + R[1, 0]) / S
            y = 0.25 * S
            z = (R[1, 2] + R[2, 1]) / S
        else:
            S = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2
            w = (R[1, 0] - R[0, 1]) / S
            x = (R[0, 2] + R[2, 0]) / S
            y = (R[1, 2] + R[2, 1]) / S
            z = 0.25 * S

    quat = np.array([w, x, y, z])
    return quat / np.linalg.norm(quat)


if __name__ == '__main__':
    rat = np.identity(3)
    quat = cvtRot2Quat(rat)
    print(quat)

1.4 欧拉角转旋转矩阵

描述：给定三个欧拉角(均以角度表示)：绕Z轴旋转得到的偏航角(Yaw)、绕Y轴旋转得到的俯仰角(Pitch)、绕X轴旋转得到的滚转角(Roll)，旋转顺序为Z-Y-X，将其转换为对应的旋转矩阵R。

关键：分别构建围绕三轴的旋转矩阵，再相乘即可。需要注意的是旋转顺序，详见这篇博客。

代码：

import numpy as np


def euler2Rot(yaw, pitch, roll):
    yaw, pitch, roll = np.radians([yaw, pitch, roll])

    # 计算各轴旋转矩阵
    Rz = np.array([
        [np.cos(yaw), -np.sin(yaw), 0],
        [np.sin(yaw), np.cos(yaw), 0],
        [0, 0, 1]
    ])

    Ry = np.array([
        [np.cos(pitch), 0, np.sin(pitch)],
        [0, 1, 0],
        [-np.sin(pitch), 0, np.cos(pitch)]
    ])

    Rx = np.array([
        [1, 0, 0],
        [0, np.cos(roll), -np.sin(roll)],
        [0, np.sin(roll), np.cos(roll)]
    ])

    # 组合旋转矩阵：R = Rz * Ry * Rx
    return np.matmul(Rz, np.matmul(Ry, Rx))


if __name__ == "__main__":
    # 示例1：90度偏航（绕Z轴）
    print("Yaw 90°:\n", euler2Rot(90, 0, 0))

    # 示例2：航空航天常用角度测试
    aircraft_orientation = euler2Rot(30, 15, -10)
    print("\nAircraft orientation (30°,15°,-10°):\n", aircraft_orientation)

1.5 旋转矩阵转欧拉角

描述：给定旋转矩阵R，将其转换为三个欧拉角(均以角度表示)：绕Z轴旋转得到的偏航角(Yaw)、绕Y轴旋转得到的俯仰角(Pitch)、绕X轴旋转得到的滚转角(Roll)，旋转顺序为Z-Y-X。

关键：按照公式计算，需要注意旋转顺序影响结果，万向锁情况要特殊处理，详见这篇博客。

代码：

import numpy as np
import math

def rot2Euler(R):
    # 处理万向节死锁情况(pitch=±90°)
    if abs(R[2,0]) > 0.9999:
        yaw = math.atan2(-R[1,2], R[1,1])
        pitch = math.copysign(math.pi/2, -R[2,0])
        roll = 0.0
    else:
        yaw = math.atan2(R[1,0], R[0,0])
        pitch = math.asin(-R[2,0])
        roll = math.atan2(R[2,1], R[2,2])
    
    yaw, pitch, roll = np.degrees([yaw, pitch, roll])
    return yaw, pitch, roll

if __name__ == "__main__":
    # 示例1: 90度偏航的旋转矩阵
    R_yaw90 = np.array([[0, -1, 0],
                        [1, 0, 0],
                        [0, 0, 1]])
    yaw, pitch, roll = rot2Euler(R_yaw90)
    print("Yaw 90° conversion:", yaw, pitch, roll)

    # 示例2: 万向节死锁情况测试
    R_gimbal = np.array([[0, 0, 1],
                         [0, 1, 0],
                         [-1, 0, 0]])
    yaw, pitch, roll = rot2Euler(R_gimbal)
    print("Gimbal lock case:", yaw, pitch, roll)

1.6 欧拉角转四元数

描述：将旋转顺序为Z-Y-X的欧拉角(角度表示)：偏航角(Yaw)、俯仰角(Pitch)、滚转角(Roll)，转换为归一化四元数。

关键：按照公式计算，需要注意旋转顺序影响结果，详见这篇博客。

代码：

import numpy as np

def euler2Quat(yaw, pitch, roll):
    yaw = np.radians(yaw)
    pitch = np.radians(pitch)
    roll = np.radians(roll)
    
    cy = np.cos(yaw / 2)
    sy = np.sin(yaw / 2)
    cp = np.cos(pitch / 2)
    sp = np.sin(pitch / 2)
    cr = np.cos(roll / 2)
    sr = np.sin(roll / 2)
    
    w = cr * cp * cy + sr * sp * sy
    x = sr * cp * cy - cr * sp * sy
    y = cr * sp * cy + sr * cp * sy
    z = cr * cp * sy - sr * sp * cy

    return np.array([w, x, y, z]) / np.linalg.norm([w, x, y, z])

if __name__ == "__main__":
    yaw = 30
    pitch = 45
    roll = 60
    quaternion = euler2Quat(yaw, pitch, roll)
    print("Quaternion:", quaternion)

1.7 四元数转欧拉角

描述：将归一化四元数(qw, qx, qy, qz)转换为旋转顺序为Z-Y-X的欧拉角(角度表示)：偏航角(Yaw)、俯仰角(Pitch)、滚转角(Roll)。

关键：按照公式计算，详见这篇博客。

代码：

import numpy as np

def quat2Euler(qw, qx, qy, qz):
    qw_norm = np.sqrt(qw**2 + qx**2 + qy**2 + qz**2)
    w = qw / qw_norm
    x = qx / qw_norm
    y = qy / qw_norm
    z = qz / qw_norm
    
    # 计算yaw (Z轴)
    yaw = np.arctan2(2*(w*z + x*y), 1 - 2*(y**2 + z**2))
    # 计算pitch (Y轴)
    pitch = np.arcsin(2*(w*y - z*x))
    # 计算roll (X轴)
    roll = np.arctan2(2*(w*x + y*z), 1 - 2*(x**2 + y**2))
    
    yaw = np.degrees(yaw)
    pitch = np.degrees(pitch)
    roll = np.degrees(roll)
    
    return yaw, pitch, roll

if __name__ == "__main__":
    qw = 0.371
    qx = 0.1
    qy = 0.271
    qz = 0.5
    
    yaw, pitch, roll = quat2Euler(qw, qx, qy, qz)
    
    print("Yaw:", yaw)
    print("Pitch:", pitch)
    print("Roll:", roll)

1.8 旋转向量转旋转矩阵

描述：给定一个旋转向量，将其转化为对应的旋转矩阵。

关键：使用经典罗德里格斯公式可以实现，罗德里格斯公式描述了旋转向量和旋转矩阵之间的关系，详见这篇博客。这个转换是严密的，不会出现歧义情况。旋转向量的模长表示旋转角度的大小(弧度)，旋转向量的方向表示旋转轴的单位向量，这是角轴表示的核心。所以并不能随便对旋转向量进行归一化，否则会损失旋转角度这一信息。关于角轴表示旋转的详细信息，参考这篇博客。

代码：

import math
import numpy as np

def vector2Rot(rot_vec):
    # 旋转角度(弧度)
    theta = np.linalg.norm(rot_vec)

    # 零旋转情况
    if theta < 1e-10:
        return np.eye(3)
    
    # 单位旋转轴
    k = rot_vec / theta  
    
    # 计算叉乘矩阵K
    K = np.array([[0, -k[2], k[1]],
                  [k[2], 0, -k[0]],
                  [-k[1], k[0], 0]])
    
    # 罗德里格斯公式
    I = np.eye(3)
    R = I + math.sin(theta) * K + (1 - math.cos(theta)) * np.dot(K, K)
    
    return R

if __name__ == "__main__":
    rot_vec = np.array([0.1, 0.2, 0.3])
    
    R = vector2Rot(rot_vec)
    
    print("旋转向量:", rot_vec)
    print("旋转矩阵:\n", R)

1.9 旋转矩阵转旋转向量

描述：给定一个旋转矩阵，将其转化为对应的旋转向量。

关键：使用罗德里格斯公式做逆运算可以实现，详见这篇博客。这个转换是严密的，不会出现歧义情况。关于角轴表示旋转的详细信息，参考这篇博客。

代码：

import numpy as np
import math

def rot2Vec(R):
    # 计算旋转角度(弧度)
    theta = math.acos((np.trace(R) - 1) / 2)
    # 无旋转情况
    if abs(theta) < 1e-10:
        return np.zeros(3)
    
    # 计算旋转轴
    K = (R - R.T) / (2 * math.sin(theta))
    k = np.array([K[2,1], K[0,2], K[1,0]])
    
    # 组合为旋转向量
    return k * theta

if __name__ == "__main__":
    # 示例旋转矩阵（绕Z轴旋转45度）
    R = np.array([
        [0.707, -0.707, 0],
        [0.707,  0.707, 0],
        [0,      0,     1]
    ])
    
    print("旋转向量:", rot2Vec(R))

1.10 四元数转旋转向量

描述：给定一个单位四元数(qw, qx, qy, qz)，将其转化为对应的旋转向量。

关键：旋转角度\(\theta\)按四元数的实部\(qw\)计算\(\theta = 2 \cdot arccos(qw)\)，旋转轴\(\mathbf{K}\)按虚部计算\(\mathbf{K} = \frac{(qx, qy, qz)}{sin(\theta/2)}\)。

代码：

import numpy as np
import math

def quat2Vec(qw, qx, qy, qz):
    # 1. 计算旋转角度
    theta = 2 * math.acos(np.clip(qw, -1.0, 1.0))
    
    # 2. 处理无旋转情况
    if abs(theta) < 1e-10:
        return np.zeros(3)
    
    # 3. 计算虚部模长（避免除零）
    imag_norm = math.sqrt(qx*qx + qy*qy + qz*qz)
    # 虚部接近零
    if imag_norm < 1e-10:
        return np.zeros(3)
    
    # 4. 计算旋转轴
    k = np.array([qx, qy, qz]) / imag_norm
    
    # 5. 角度优化
    if theta > math.pi:
        theta = 2 * math.pi - theta
        k = -k
    
    # 6. 组合旋转向量
    return k * theta

# 测试示例
if __name__ == "__main__":
    # 绕Z轴旋转90度的四元数 (w=cos45°, xyz=k*sin45°)
    qw = 0.7071
    qx = 0
    qy = 0
    qz = 0.7071
    
    # 应接近 [0, 0, 1.57] (π/2≈1.57)
    print("旋转向量:", quat2Vec(qw, qx, qy, qz))  

1.11 旋转向量转四元数

描述：给定一个旋转向量，将其转化为对应的单位四元数(qw, qx, qy, qz)。

关键：根据角轴表示，旋转向量\(\mathbf{V}=\theta \mathbf{K}\)，其中，\(\theta\)为弧度表示的旋转角度(也即模长)，\(\mathbf{K}\)为单位旋转轴向量。在四元数表示中，实部\(qw\)对应旋转角度，按\(qw = cos(\theta/2)\)计算，虚部\((qx, qy, qz)\)对应旋转轴向量，按\((qx, qy, qz) = \mathbf{K}\cdot sin(\theta/2)\)计算。

代码：

import numpy as np

def vec2Quat(v):
    # 根据模长求旋转角度(弧度)
    theta = np.linalg.norm(v)
    # 零旋转
    if theta < 1e-10:
        return np.array([1.0, 0.0, 0.0, 0.0])
    
    k = v / theta
    half_theta = theta / 2
    w = np.cos(half_theta)
    xyz = k * np.sin(half_theta)
    return np.concatenate(([w], xyz))

# 测试示例
if __name__ == "__main__":
    # 绕Z轴旋转90度
    v = np.array([0, 0, np.pi/2])
    
    # 应接近 [0.707, 0, 0, 0.707]
    print("四元数:", vec2Quat(v))

1.12 欧拉角到旋转向量

描述：给定三个欧拉角(均以角度表示)：绕Z轴旋转得到的偏航角(Yaw)、绕Y轴旋转得到的俯仰角(Pitch)、绕X轴旋转得到的滚转角(Roll)，旋转顺序为Z-Y-X，将其转化为对应的旋转向量。

关键：根据已有的代码和转换路径，有欧拉角→旋转矩阵→旋转向量，这两步转换均已实现。

代码：

import numpy as np
import math

def euler2Rot(yaw, pitch, roll):
    yaw, pitch, roll = np.radians([yaw, pitch, roll])

    # 计算各轴旋转矩阵
    Rz = np.array([
        [np.cos(yaw), -np.sin(yaw), 0],
        [np.sin(yaw), np.cos(yaw), 0],
        [0, 0, 1]
    ])

    Ry = np.array([
        [np.cos(pitch), 0, np.sin(pitch)],
        [0, 1, 0],
        [-np.sin(pitch), 0, np.cos(pitch)]
    ])

    Rx = np.array([
        [1, 0, 0],
        [0, np.cos(roll), -np.sin(roll)],
        [0, np.sin(roll), np.cos(roll)]
    ])

    # 组合旋转矩阵：R = Rz * Ry * Rx
    return np.matmul(Rz, np.matmul(Ry, Rx))


def rot2Vec(R):
    # 计算旋转角度(弧度)
    theta = math.acos((np.trace(R) - 1) / 2)
    # 无旋转情况
    if abs(theta) < 1e-10:
        return np.zeros(3)
    
    # 计算旋转轴
    K = (R - R.T) / (2 * math.sin(theta))
    k = np.array([K[2,1], K[0,2], K[1,0]])
    
    # 组合为旋转向量
    return k * theta

if __name__ == "__main__":
    # 示例1：90度偏航（绕Z轴）
    rot1 = euler2Rot(90, 0, 0)
    vec1 = rot2Vec(rot1)
    print("旋转向量:", vec1)
    
    # 示例2：航空航天常用角度测试
    rot2 = euler2Rot(30, 15, -10)
    vec2 = rot2Vec(rot2)
    print("旋转向量:", vec2)

1.13 旋转向量到欧拉角

描述：给定旋转向量，将其转换为三个欧拉角(均以角度表示)：绕Z轴旋转得到的偏航角(Yaw)、绕Y轴旋转得到的俯仰角(Pitch)、绕X轴旋转得到的滚转角(Roll)，旋转顺序为Z-Y-X。

关键：根据已有的代码和转换路径，有旋转向量→旋转矩阵→欧拉角，这两步转换均已实现。

代码：

import math
import numpy as np

def vector2Rot(rot_vec):
    # 旋转角度(弧度)
    theta = np.linalg.norm(rot_vec)

    # 零旋转情况
    if theta < 1e-10:
        return np.eye(3)
    
    # 单位旋转轴
    k = rot_vec / theta  
    
    # 计算叉乘矩阵K
    K = np.array([[0, -k[2], k[1]],
                  [k[2], 0, -k[0]],
                  [-k[1], k[0], 0]])
    
    # 罗德里格斯公式
    I = np.eye(3)
    R = I + math.sin(theta) * K + (1 - math.cos(theta)) * np.dot(K, K)
    
    return R


def rot2Euler(R):
    # 处理万向节死锁情况(pitch=±90°)
    if abs(R[2,0]) > 0.9999:
        yaw = math.atan2(-R[1,2], R[1,1])
        pitch = math.copysign(math.pi/2, -R[2,0])
        roll = 0.0
    else:
        yaw = math.atan2(R[1,0], R[0,0])
        pitch = math.asin(-R[2,0])
        roll = math.atan2(R[2,1], R[2,2])
    
    yaw, pitch, roll = np.degrees([yaw, pitch, roll])
    return yaw, pitch, roll


if __name__ == "__main__":
    rot_vec = np.array([0.1, 0.2, 0.3])
    
    R = vector2Rot(rot_vec)
    yaw, pitch, roll = rot2Euler(R)
    
    print(yaw, pitch, roll)

2.位姿插值

主要包含位置和姿态表示的插值和外推。

2.1 平面二维位置插值

描述：给定t1时刻的位置P1、t2时刻的位置P2，线性内插求解t1到t2时刻内任意时刻t对应的位置P。

关键：按内插的原理和公式求解即可，详见这篇博客。

代码：

class Point2D:
    timestamp = 0
    pos_x = 0
    pos_y = 0

    def __init__(self, t, px, py):
        self.timestamp = t
        self.pos_x = px
        self.pos_y = py
    
    def print(self):
        print("Timestamp:" , self.timestamp)
        print("\t Position (x, y):" , self.pos_x, self.pos_y)
        

def interpolate(p1, p2, t):
    dx = (p2.pos_x - p1.pos_x) * ((t - p1.timestamp) / (p2.timestamp - p1.timestamp))
    dy = (p2.pos_y - p1.pos_y) * ((t - p1.timestamp) / (p2.timestamp - p1.timestamp))

    return Point2D(t, p1.pos_x + dx, p1.pos_y + dy)


if __name__ == '__main__':
    p1 = Point2D(100, 1, 2)
    p2 = Point2D(200, 2, 3)

    p3 = interpolate(p1, p2, 150)

    p1.print()
    p2.print()
    p3.print()

2.2 空间三维位置插值

描述：给定t1时刻的位置P1、t2时刻的位置P2，线性内插求解t1到t2时刻内任意时刻t对应的位置P。

关键：按内插的原理和公式求解即可，详见这篇博客。和二维位置流程一致，只是多了Z轴一个维度。

代码：

class Point3D:
    timestamp = 0
    pos_x = 0
    pos_y = 0
    pos_z = 0
    
    def __init__(self, t, px, py, pz):
        self.timestamp = t
        self.pos_x = px
        self.pos_y = py
        self.pos_z = pz
    
    def print(self):
        print("Timestamp:" , self.timestamp)
        print("\t Position (x, y, z):" , self.pos_x, self.pos_y, self.pos_z)
        

def interpolate(p1, p2, t):
    dx = (p2.pos_x - p1.pos_x) * ((t - p1.timestamp) / (p2.timestamp - p1.timestamp))
    dy = (p2.pos_y - p1.pos_y) * ((t - p1.timestamp) / (p2.timestamp - p1.timestamp))
    dz = (p2.pos_z - p1.pos_z) * ((t - p1.timestamp) / (p2.timestamp - p1.timestamp))

    return Point3D(t, p1.pos_x + dx, p1.pos_y + dy, p1.pos_z + dz)


if __name__ == '__main__':
    p1 = Point3D(100, 1, 2, 3)
    p2 = Point3D(200, 2, 3, 5)

    p3 = interpolate(p1, p2, 150)

    p1.print()
    p2.print()
    p3.print()

2.3 Heading航向角角度插值

描述：给定t1时刻的航向角H1、t2时刻的航向角H2，线性内插求解t1到t2时刻内任意时刻t对应的航向角H。

关键：按内插的原理和公式求解即可，详见这篇博客。Heading角度范围为0-360度，正北为0度，顺时针方向为正，也称为航向角、方位角、偏航角。对应欧拉角姿态表示中的Yaw(绕Z轴旋转)，详见这里。这里面有个问题需要注意。对于从航向角1到航向角2，事实上有两种方式，一种是始终选择小于180度的方向变化过去，另一种是永远以顺时针或逆时针方向旋转，变化过去。两种方式有两种不同计算方法。

代码：

class Heading:
    timestamp = 0
    head_angle = 0
    
    def __init__(self, t, angle):
        self.timestamp = t
        self.head_angle = angle
    
    def print(self):
        print("Timestamp:" , self.timestamp)
        print("\t Heading Angle:" , self.head_angle)


# 无论旋转方向(顺时针或逆时针)，单次角度变化绝对值不大于180，若大于180，按较小角度算
def interpolate1(h1, h2, t):
    ratio = (t - h1.timestamp) / (h2.timestamp - h1.timestamp)
    range_heading = h2.head_angle - h1.head_angle
    
    if range_heading < -180:
        range_heading = range_heading + 360
        new_heading = h1.head_angle + range_heading * ratio
    elif range_heading > 180:
        range_heading = 360 - range_heading
        new_heading = h1.head_angle + 360 - range_heading * ratio
    else:
        new_heading = h1.head_angle + range_heading * ratio
    
    if new_heading >= 360:
        new_heading = new_heading - 360

    return Heading(t, new_heading)

# 所有角度变化都是顺时针，无论角度变化大小
def interpolate2(h1, h2, t):
    ratio = (t - h1.timestamp) / (h2.timestamp - h1.timestamp)
    range_heading = h2.head_angle - h1.head_angle

    if range_heading < 0:
        range_heading = range_heading + 360
    new_heading = h1.head_angle + range_heading * ratio
    
    if new_heading >= 360:
            new_heading = new_heading - 360
    
    return Heading(t, new_heading)


if __name__ == '__main__':
    h1 = Heading(100, 10)
    h2 = Heading(200, 350)

    h3_1 = interpolate1(h1, h2, 150)
    h3_2 = interpolate2(h1, h2, 150)

    h1.print()
    h2.print()
    h3_1.print()
    h3_2.print()

2.4 Pitch俯仰角角度插值

描述：给定t1时刻的俯仰角P1、t2时刻的俯仰角P2，线性内插求解t1到t2时刻内任意时刻t对应的俯仰角P。

关键：按内插的原理和公式求解即可，详见这篇博客。Pitch角度范围为-90到90度，水平为0度，向上为正，向下为负，对应欧拉角姿态表示中的俯仰角，详见这里。因为俯仰角变化范围在180度以内，所以内插相比于Heading航向角容易一些，直接两个状态相减即可。

代码：

class Pitch:
    timestamp = 0
    pitch_angle = 0

    def __init__(self, t, angle):
        self.timestamp = t
        self.pitch_angle = angle
    
    def print(self):
        print("Timestamp:" , self.timestamp)
        print("\t Pitch Angle:" , self.pitch_angle)

def interpolate(p1, p2, t):
    ratio = (t - p1.timestamp) / (p2.timestamp - p1.timestamp)
    range_pitch = p2.pitch_angle - p1.pitch_angle

    new_pitch = p1.pitch_angle + range_pitch * ratio
    
    return Pitch(t, new_pitch)


if __name__ == '__main__':
    p1 = Pitch(100, -60)
    p2 = Pitch(200, 40)

    p3 = interpolate(p1, p2, 150)

    p1.print()
    p2.print()
    p3.print()

2.5 Roll翻滚角角度插值

描述：给定t1时刻的翻滚角R1、t2时刻的翻滚角R2，线性内插求解t1到t2时刻内任意时刻t对应的翻滚角R。

关键：按内插的原理和公式求解即可，详见这篇博客。Roll角度范围为-90到90度，水平为0度，向右为正，向下左为负，对应欧拉角姿态表示中的翻滚角，详见这里。因为翻滚角变化范围在180度以内，所以内插相比于Heading航向角容易一些，直接两个状态相减即可。

代码：

class Roll:
    timestamp = 0
    roll_angle = 0

    def __init__(self, t, angle):
        self.timestamp = t
        self.roll_angle = angle
    
    def print(self):
        print("Timestamp:" , self.timestamp)
        print("\t Roll Angle:" , self.roll_angle)

def interpolate(r1, r2, t):
    ratio = (t - r1.timestamp) / (r2.timestamp - r1.timestamp)
    range_roll = r2.roll_angle - r1.roll_angle

    new_roll = r1.roll_angle + range_roll * ratio
    
    return Roll(t, new_roll)


if __name__ == '__main__':
    r1 = Roll(100, -60)
    r2 = Roll(200, 40)

    r3 = interpolate(r1, r2, 150)

    r1.print()
    r2.print()
    r3.print()

2.6 平面位姿(位置+航向角)插值

描述：给定t1时刻的位置P1和航向角H1、t2时刻的位置P2和航向角H2，线性内插求解t1到t2时刻内任意时刻t对应的位置P和航向角H。

关键：按内插的原理和公式求解即可，本质上是位置插值+Heading航向角插值组合。注意事项详见各部分。

代码：

class Pose2D:
    timestamp = 0
    pos_x = 0
    pos_y = 0
    head_angle = 0
    
    def __init__(self, t, px, py, heading):
        self.timestamp = t
        self.pos_x = px
        self.pos_y = py
        self.head_angle = heading
    
    def print(self):
        print("Timestamp:" , self.timestamp)
        print("\t Pose (x, y, heading):" , self.pos_x, self.pos_y, self.head_angle)


# 无论旋转方向(顺时针或逆时针)，单次角度变化不大于180，若大于180，按较小角度算
def interpolate1(p1, p2, t):
    ratio = (t - p1.timestamp) / (p2.timestamp - p1.timestamp)

    dx = (p2.pos_x - p1.pos_x) * ratio
    dy = (p2.pos_y - p1.pos_y) * ratio

    new_x = p1.pos_x + dx
    new_y = p1.pos_y + dy

    range_heading = p2.head_angle - p1.head_angle
    
    if range_heading < -180:
        range_heading = range_heading + 360
        new_heading = p1.head_angle + range_heading * ratio
    elif range_heading > 180:
        range_heading = 360 - range_heading
        new_heading = p1.head_angle + 360 - range_heading * ratio
    else:
        new_heading = p1.head_angle + range_heading * ratio
    
    if new_heading >= 360:
        new_heading = new_heading - 360

    return Pose2D(t, new_x, new_y, new_heading)


# 所有角度变化都是顺时针，无论角度变化大小
def interpolate2(p1, p2, t):
    ratio = (t - p1.timestamp) / (p2.timestamp - p1.timestamp)
    
    dx = (p2.pos_x - p1.pos_x) * ratio
    dy = (p2.pos_y - p1.pos_y) * ratio

    new_x = p1.pos_x + dx
    new_y = p1.pos_y + dy

    range_heading = p2.head_angle - p1.head_angle

    if range_heading < 0:
        range_heading = range_heading + 360
    new_heading = p1.head_angle + range_heading * ratio
    
    if new_heading >= 360:
            new_heading = new_heading - 360

    return Pose2D(t, new_x, new_y, new_heading)


if __name__ == '__main__':
    p1 = Pose2D(100, 1, 2, 350)
    p2 = Pose2D(200, 2, 3, 10)

    p3 = interpolate1(p1, p2, 160)

    p1.print()
    p2.print()
    p3.print()

2.7 空间位姿(位置+三轴姿态)插值

描述：给定t1时刻的位置P1和航向角H1、俯仰角P1、翻滚角R1，t2时刻的位置P2和航向角H2、俯仰角P2、翻滚角R2，线性内插求解t1到t2时刻内任意时刻t对应的位置P和航向角H、俯仰角P、翻滚角R。

关键：按内插的原理和公式求解即可，本质上是位置插值+Heading航向角插值+Pitch俯仰角插值+Roll翻滚角插值组合。注意事项详见各部分。

代码：

class Pose3D:
    timestamp = 0

    pos_x = 0
    pos_y = 0
    pos_z = 0

    head_angle = 0
    pitch_angle = 0
    roll_angle = 0

    
    def __init__(self, t, px, py, pz, heading, pitch, roll):
        self.timestamp = t
        self.pos_x = px
        self.pos_y = py
        self.pos_z = pz
        self.head_angle = heading
        self.pitch_angle = pitch
        self.roll_angle = roll
        
    
    def print(self):
        print("Timestamp:" , self.timestamp)
        print("\t Pose (x, y, z, heading, pitch, roll):" , self.pos_x, self.pos_y, self.pos_z, self.head_angle, self.pitch_angle, self.roll_angle)


def interpolate(p1, p2, t):
    ratio = (t - p1.timestamp) / (p2.timestamp - p1.timestamp)

    # 位置插值
    dx = (p2.pos_x - p1.pos_x) * ratio
    dy = (p2.pos_y - p1.pos_y) * ratio
    dz = (p2.pos_z - p1.pos_z) * ratio

    new_x = p1.pos_x + dx
    new_y = p1.pos_y + dy
    new_z = p1.pos_z + dz

    # Heading插值(使用小于180角的变化方式)
    range_heading = p2.head_angle - p1.head_angle
    
    if range_heading < -180:
        range_heading = range_heading + 360
        new_heading = p1.head_angle + range_heading * ratio
    elif range_heading > 180:
        range_heading = 360 - range_heading
        new_heading = p1.head_angle + 360 - range_heading * ratio
    else:
        new_heading = p1.head_angle + range_heading * ratio
    
    if new_heading >= 360:
        new_heading = new_heading - 360

    # Pitch插值
    range_pitch = p2.pitch_angle - p1.pitch_angle
    new_pitch = p1.pitch_angle + range_pitch * ratio

    # Roll插值
    range_roll = p2.roll_angle - p1.roll_angle
    new_roll = p1.roll_angle + range_roll * ratio

    return Pose3D(t, new_x, new_y, new_z, new_heading, new_pitch, new_roll)


if __name__ == '__main__':
    p1 = Pose3D(100, 1, 2, 3, 350, 30, 40)
    p2 = Pose3D(200, 2, 3, 10, 10, 20, 60)

    p3 = interpolate(p1, p2, 150)

    p1.print()
    p2.print()
    p3.print()

2.8 三维空间位置+姿态状态外推

描述：给定t1时刻的位置P1和航向角H1、俯仰角P1、翻滚角R1，t2时刻的位置P2和航向角H2、俯仰角P2、翻滚角R2，线性内插求解t2时刻后任意时刻t对应的位置P和航向角H、俯仰角P、翻滚角R。

关键：本质上和内插的原理相同，采用线性方式计算，代码完全一致。和内插不同的是它的时间ratio是大于1的。位置插值+Heading航向角插值+Pitch俯仰角插值+Roll翻滚角插值组合。注意事项详见各部分。

代码：

class Pose3D:
    timestamp = 0

    pos_x = 0
    pos_y = 0
    pos_z = 0

    head_angle = 0
    pitch_angle = 0
    roll_angle = 0

    
    def __init__(self, t, px, py, pz, heading, pitch, roll):
        self.timestamp = t
        self.pos_x = px
        self.pos_y = py
        self.pos_z = pz
        self.head_angle = heading
        self.pitch_angle = pitch
        self.roll_angle = roll
        
    
    def print(self):
        print("Timestamp:" , self.timestamp)
        print("\t Pose (x, y, z, heading, pitch, roll):" , self.pos_x, self.pos_y, self.pos_z, self.head_angle, self.pitch_angle, self.roll_angle)


def interpolate(p1, p2, t):
    ratio = (t - p1.timestamp) / (p2.timestamp - p1.timestamp)

    # 位置插值
    dx = (p2.pos_x - p1.pos_x) * ratio
    dy = (p2.pos_y - p1.pos_y) * ratio
    dz = (p2.pos_z - p1.pos_z) * ratio

    new_x = p1.pos_x + dx
    new_y = p1.pos_y + dy
    new_z = p1.pos_z + dz

    # Heading插值(使用小于180角的变化方式)
    range_heading = p2.head_angle - p1.head_angle
    
    if range_heading < -180:
        range_heading = range_heading + 360
        new_heading = p1.head_angle + range_heading * ratio
    elif range_heading > 180:
        range_heading = 360 - range_heading
        new_heading = p1.head_angle + 360 - range_heading * ratio
    else:
        new_heading = p1.head_angle + range_heading * ratio
    
    if new_heading >= 360:
        new_heading = new_heading - 360

    # Pitch插值
    range_pitch = p2.pitch_angle - p1.pitch_angle
    new_pitch = p1.pitch_angle + range_pitch * ratio

    # Roll插值
    range_roll = p2.roll_angle - p1.roll_angle
    new_roll = p1.roll_angle + range_roll * ratio

    return Pose3D(t, new_x, new_y, new_z, new_heading, new_pitch, new_roll)


if __name__ == '__main__':
    p1 = Pose3D(100, 1, 2, 3, 350, 30, 40)
    p2 = Pose3D(200, 2, 3, 10, 10, 20, 60)

    p3 = interpolate(p1, p2, 300)

    p1.print()
    p2.print()
    p3.print()

3.空间位姿变换

主要包含二维和三维的空间变换。

3.1 二维位姿旋转变换

描述：给定t时刻的位置P(x, y)以及对应的Heading角h，求以C点(cx, cy)为中心，顺时针旋转alpha角度(规定逆时针旋转为正方向)，求旋转后的位姿P’(x’, y’, h’)。

关键：对于位置，按照平面旋转公式求解即可，详见这篇博客。对于角度，类似于前面提到的Heading角度内插，原理相同。这里需要注意的是旋转方向的问题，比较容易弄错。

代码：

import math

class Pose2D:
    timestamp = 0
    pos_x = 0
    pos_y = 0
    head_angle = 0

    def __init__(self, t, px, py, heading):
        self.timestamp = t
        self.pos_x = px
        self.pos_y = py
        self.head_angle = heading

    def print(self):
        print("Timestamp:", self.timestamp)
        print("\t Pose (x, y, heading):", self.pos_x, self.pos_y, self.head_angle)


def rotate(cx, cy, rotate_angle, point1):
    rotate_angle_rad = math.radians(rotate_angle)

    # 坐标旋转
    new_x = cx + math.cos(rotate_angle_rad) * point1.pos_x - math.sin(rotate_angle_rad) * point1.pos_y
    new_y = cy + math.sin(rotate_angle_rad) * point1.pos_x - math.cos(rotate_angle_rad) * point1.pos_y

    # Heading角度旋转
    if rotate_angle < 0:
        new_heading = point1.head_angle - rotate_angle
        if new_heading > 360:
            new_heading = new_heading - 360
    else:
        new_heading = point1.head_angle + rotate_angle
        if new_heading < 0:
            new_heading = new_heading + 360

    return Pose2D(point1.timestamp, new_x, new_y, new_heading)

if __name__ == '__main__':
    center_x = 0
    center_y = 0
    rotate_angle = -90 # 角度表示，逆时针为正，顺时针为负

    p1 = Pose2D(1, 0, 1, 30)
    p2 = rotate(center_x, center_y, rotate_angle, p1)

    p1.print()
    p2.print()

3.2 三维位置旋转变换

描述：给定t时刻的位置P(x, y, z)，分别绕Z轴、Y轴、X轴旋转alpha、beta、gamma，求旋转后的位置P’(x’, y’, z’)。

关键：按照旋转角度构建旋转矩阵并组合，最后应用到三维坐标点上即可，详见这篇博客。在构造旋转矩阵时，需要注意旋转方向问题。为了简单记住到底旋转后把谁当作横轴谁当作纵轴，总结了一个简单小方法。即以哪个轴旋转，就用手握住这个轴，然后逆时针转动，最先碰到的那个轴作为横轴，剩下的那个作为纵轴。

代码：

import math
import numpy as np

if __name__ == '__main__':
    init_x = 1
    init_y = 1
    init_z = 1

    rotate_z = 0
    rotate_y = 0
    rotate_x = 0

    pos1 = np.array([[init_x], [init_y], [init_z]])

    rotate_x_rad = math.radians(rotate_x)
    rotate_y_rad = math.radians(rotate_y)
    rotate_z_rad = math.radians(rotate_z)

    # 构造旋转矩阵
    rot_z = np.array(
        [[math.cos(rotate_z_rad), -math.sin(rotate_z_rad), 0],
         [math.sin(rotate_z_rad), math.cos(rotate_z_rad), 0],
         [0, 0, 1]])
    rot_y = np.array([[math.cos(rotate_y_rad), 0, math.sin(rotate_y_rad)],
                      [0, 1, 0],
                      [-math.sin(rotate_y_rad), 0, math.cos(rotate_y_rad)]])
    rot_x = np.array([[1, 0, 0],
                      [0, math.cos(rotate_x_rad), -math.sin(rotate_x_rad)],
                      [0, math.sin(rotate_x_rad), math.cos(rotate_x_rad)]])

    pos2 = np.matmul(rot_z, np.matmul(rot_y, np.matmul(rot_x, pos1)))

    print(pos1)
    print(pos2)

3.3 三维位姿旋转变换

描述：给定t时刻的位置P(x, y, z)以及对应的Heading、Pitch、Roll角度，将其分别绕Z轴、Y轴、X轴旋转alpha、beta、gamma(均为默认正方向)，求旋转后的位置和姿态。

关键：对于位置，按照旋转角度构建旋转矩阵并组合，最后应用到三维坐标点上即可，详见这篇博客。对于角度，各个角度与对应的旋转角累加即可。

代码：

import math
import numpy as np

if __name__ == '__main__':
    init_x = 1
    init_y = 1
    init_z = 1

    init_heading = 0
    init_pitch = 0
    init_roll = 0

    rotate_z = 10
    rotate_y = 0
    rotate_x = 0

    pos1 = np.array([[init_x], [init_y], [init_z]])

    rotate_x_rad = math.radians(rotate_x)
    rotate_y_rad = math.radians(rotate_y)
    rotate_z_rad = math.radians(rotate_z)

    # 构造旋转矩阵
    rot_z = np.array(
        [[math.cos(rotate_z_rad), -math.sin(rotate_z_rad), 0],
         [math.sin(rotate_z_rad), math.cos(rotate_z_rad), 0],
         [0, 0, 1]])
    rot_y = np.array([[math.cos(rotate_y_rad), 0, math.sin(rotate_y_rad)],
                      [0, 1, 0],
                      [-math.sin(rotate_y_rad), 0, math.cos(rotate_y_rad)]])
    rot_x = np.array([[1, 0, 0],
                      [0, math.cos(rotate_x_rad), -math.sin(rotate_x_rad)],
                      [0, math.sin(rotate_x_rad), math.cos(rotate_x_rad)]])

    pos2 = np.matmul(rot_z, np.matmul(rot_y, np.matmul(rot_x, pos1)))

    # Heading角
    new_heading = init_heading + rotate_z

    # Pitch角
    new_pitch = init_pitch + rotate_y

    # Roll角
    new_roll = init_roll + rotate_x

    print(pos1, init_heading, init_pitch, init_roll)
    print(pos2, new_heading, new_pitch, new_roll)

Menu