四元数、欧拉角与旋转矩阵

Salierilym
1,313 阅读5分钟

1. 起因

在游戏开发的过程中难免会遇到欧拉角和四元数直接的转换问题，比如Airsim中不管是通过自带的数据记录还是Python API采集的数据，例如Vx,Vy,Vz等，描述的都是相对世界坐标系下的速度方向，而不能直接获取到车辆运行过程中的轴向、横向和纵向车速。于是就需要想方设法想想怎么根据已有数据来获取需要的轴向、横向和纵向数据，那么入手点就是Airsim导出的四元数ow、ox、oy和oz了。

2. 欧拉角和四元数

在讲四元数之前，可以先了解简单的欧拉角和轴向角。

欧拉角 欧拉角使用最简单的x,y,z值来分别表示在x，y，z轴上的旋转角度，其取值为0-360(或者0-2pi），一般使用roll，pitch，yaw来表示这些分量的旋转值。需要注意的是，这里的旋转是针对世界坐标系的，这意味着第一次的旋转不会影响第二、三次的转轴。

欧拉角容易出现的问题是 1）不易在任意方向的旋转轴插值； 2）万向节死锁；3）旋转的次序无法确定。

轴角 轴角用一个以单位矢量定义的旋转角，再加上一个标量定义的旋转角来表示旋转。通常的表示[x,y,z,theta]，前面三个表示轴，最后一个表示角度。表示非常直观，也很紧凑。

轴角最大的一个局限就是不能进行简单的插值，此外，轴角形式的旋转不能直接施于点或矢量，必转换为矩阵或者四元素。

四元数 四元数是由爱尔兰数学家Hamilton发明的，感觉上就是轴角的进化，也是使用一个3维向量表示转轴和一个角度分量表示绕此转轴的旋转角度，即(w,x,y,z)，对于一个物体的旋转，其实我们只需要知道四个值：一个旋转的向量 + 一个旋转的角度。而四元数也正是这样的设计：其中x,y,z 代表的是向量的三维坐标，w代表的是角度。

w = cos(theta/2)
x = ax * sin(theta/2)
y = ay * sin(theta/2)
z = az * sin(theta/2)

其中(ax,ay,az)表示轴的矢量，theta表示绕此轴的旋转角度。四元数中的每个数都是经过“处理”的轴和角，轴角描述的“四元组”并不是一个空间下的东西，首先（ax,ay,az）是一个3维坐标下的矢量，而theta则是级坐标下的角度，简单的将他们组合到一起并不能保证他们插值结果的稳定性，因为他们无法归一化，所以不能保证最终插值后得到的矢量长度（经过旋转变换后两点之间的距离）相等，而四元数在是在一个统一的4维空间中，方便归一化来插值，又能方便的得到轴、角这样用于3D图像的信息数据，所以用四元数再合适不过了。相比于矩阵，四元素也只要存储4个浮点数，优势很明显。

3. 旋转矩阵

目前为止，描述清楚姿态信息的方式有三种：欧拉角、旋转矩阵、四元数。 image.png 其实关于这几个概念之间的关系网上已经有很多人做过详细的介绍，比如wiki，这里就不多赘述，仅从自身应用的需求方面进行一个平铺直叙。 已知欧拉角、四元数、旋转矩阵之间是可以相互转化的，那么根据四元数即可求出欧拉角或者旋转矩阵，也就是说，在Airsim中根据ow、ox、oy、oz即可将坐标系旋转到当前坐标系，获得车辆运行时的朝向。 这里引用知乎中的一张图，来说明四元数到旋转矩阵的计算。 image.png 于是可以编写相关的Python脚本，来粗略计算车辆的轴向数据、横向数据和纵向数据记录在Excel中。这里给出一个简单的例子，用来计算Airsim自带的数据采集处理方案。

import csv, os, xlwt
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
import autolab_core
import math

df = pd.read_csv(r'D:\Airsim\AirSim\PythonClient\car\data\2020-08-04-15-05.csv')
v = df.iloc[:, 11:14]
a = df.iloc[:, 8:11]
orientation = df.iloc[:, 14:18]
v_axial = []
a_axial = []
v_lateral = []
a_lateral = []
v_longitudinal = []
a_longitudinal = []
for i in range(df.shape[0]):
    T_qua2rota = autolab_core.RigidTransform(np.array(orientation.iloc[i, :]), np.array(v.iloc[i, :]))
    dot = np.dot(v.iloc[i, :], T_qua2rota.rotation)
    v_axial.append(dot[0])
    v_lateral.append(dot[1])
    v_longitudinal.append(dot[2])
    T_qua2rota1 = autolab_core.RigidTransform(np.array(orientation.iloc[i, :]), np.array(a.iloc[i, :]))
    dot1 = np.dot(a.iloc[i, :], T_qua2rota1.rotation)
    a_axial.append(dot1[0])
    a_lateral.append(dot1[1])
    a_longitudinal.append(dot1[2])

book = xlwt.Workbook(encoding='utf-8') #创建Workbook，相当于创建Excel

# 创建sheet，Sheet1为表的名字，cell_overwrite_ok为是否覆盖单元格
sheet1 = book.add_sheet(u'Sheet1', cell_overwrite_ok=True)
sheet1.write(0, 0, '轴向车速')
sheet1.write(0, 1, '横向车速')
sheet1.write(0, 2, '纵向车速')
sheet1.write(0, 3, '车速')
sheet1.write(0, 4, '轴向加速度')
sheet1.write(0, 5, '横向加速度')
sheet1.write(0, 6, '纵向加速度')

#向表中添加数据
for i in range(df.shape[0]):
    sheet1.write(i+1, 0, v_axial[i])
    sheet1.write(i+1, 1, v_lateral[i])
    sheet1.write(i+1, 2, v_longitudinal[i])
    sheet1.write(i+1, 3, math.sqrt(v_axial[i]**2 + v_lateral[i]**2 + v_longitudinal[i]**2))
    sheet1.write(i+1, 4, a_axial[i])
    sheet1.write(i+1, 5, a_lateral[i])
    sheet1.write(i+1, 6, a_longitudinal[i])

book.save('C:/Users/HP/Desktop/shuju/data.xls')

4. 用Pyinstaller打包.py脚本

当然如果结合Airsim的Python API采集到的数据，得到的结果会更加准确。那么问题来了，如何在其他电脑上使用此脚本进行模拟驾驶的数据采集呢，从0开始搭建一个对应的环境可太麻烦了，于是想到了几种方式：

  1. 在对应的电脑上装个Python,离线环境下安装相应的依赖包。有点麻烦，耗时耗力。
  2. 用Docker for Windows整个镜像。感觉有点大材小用，且实验室电脑万一被我整崩了，那就尴尬了。
  3. 用Pyinstaller进行打包。一句命令行搞定，方便快捷。

所以选择用Pyinstaller进行打包，直接通过CMD控制台cd到py文件所在目录，输入pyinstaller -F record.py,就完事儿了，其中生成的dist文件夹存储.exe文件，build文件夹存储中间文件，.spec文件用来描述打包方法，可以自己进行编辑修改再pyinstaller -F record.spec。 期间遇到了各种小的问题，但通过面向谷歌学习的方法都很快解决了，最后把打包的.exe和项目复制到任何一台电脑上，都可以愉快地进行模拟驾驶和数据采集了。

avatar
文章
阅读
粉丝