空鼠遥控器软件设计

空鼠遥控器的设计思路

---

前言

写这篇文章主要是因为自己想要研究下其中的姿态解算算法原理，才有下面这篇。
通常的姿态解算是用到四元数处理，然而我用到的是基本旋转变换和基坐标变换。

---

提示：以下是本篇文章正文内容，下面案例可供参考

一、空鼠遥控器简介

1.遥控器功能

如下图所示，遥控器里有个传感器，操作遥控器空中上下左右移动，遥控器里面的陀螺仪会给出相应的数据，之后通过无线传输给智能显示设备，使设备里的鼠标指针移动，达到空中体感操控鼠标移动的目的。

2.六轴陀螺仪

六轴陀螺仪，三轴加速度和三轴角速度，一般规格书里都会给出陀螺仪的正方向，如下图，是右手坐标系。

陀螺仪测量的就是XYZ三个轴方向上的加速度以及绕着这三个轴转动的角速度。

二、姿态更新算法

1.算法前的分析

根据遥控器的功能需求，最后需求的结果是智能显示设备上鼠标指针的移动量，而这个移动量通过三维空间里的旋转角度得到，如下图，设定三维坐标系，Y轴水平指向显示设备，Z轴垂直指向天，X轴垂直与Y和Z轴的平面，方向符合右手定则，指向右侧。后面定义这个坐标系为R系 。

接下来给遥控器设定三维坐标系，由于陀螺仪IC贴合在遥控器上，遥控器的坐标轴与IC坐标轴重合，所以之下使用陀螺仪的三维系表示，如下图，后面定义这个坐标系为B系。

可以看出R系是不动的，而B系跟随手势变化而变化，陀螺仪直接输出的原始数据是根据B系来的的，所以最后的目地就是由在B系中的变化求在R系的变化。

2.算法基础知识

空间向量

坐标系上的点是可以用向量的形式表示。
例在一个二维坐标系上的一个点P， 并设定X、Y轴的单位向量i、j，如下图以向量的形式表示。

i、j是单位向量
矩阵形式：

那么三维坐标系的点P就表示为：

旋转矩阵

a、
首先是以二维坐标系分析，如下图。

坐标系上的一个点P绕原点逆时针旋转β角度到点P’，
点P的坐标可表示为：

点P’的坐标表示为：

可写成矩阵相乘的形式

则

由上式可以得出点P’的坐标可以由P左乘一个二阶矩阵得到，这个二阶矩阵就是变换矩阵，即在二维坐标中，点P绕点O逆时针（逆时针角度为正）转动β角度的变换矩阵为

下面看二维变换的另一个要点，如下图，

则点P’‘的坐标可以表示为点P先逆时针转动a角度到点P’

再由点P’逆时针转动b角度得到P’’

则

也可以直接由P逆时针转动c角度到P’’

通过矩阵的计算可得

由上面的结果可以得出，多个变换动作可以合成一个大的变换动作，一个变换动作也可以分解成多个小的变换动作。
b、
三维空间旋转矩阵的推倒与二维类似，如下图，点P在三维坐标系XYZ中绕Z轴以右手螺旋正方形旋转θ度。

先将点P的运动投影到XOY平面上，则绕Z轴旋转的θ度就是XOY平面上绕O点旋转的θ度，X、Y轴的坐标表示与二维平面一样

而绕Z轴运动，Z轴上的坐标是不变的，所以通过矩阵的计算可以简单得到P’的坐标：

则在三维空间中点P绕Z轴正方向旋转θ角度的旋转矩阵为：

同理，可得到，绕X轴正方向旋转ψ角度和绕Y轴旋转ω分别为：


根据上面二维变换的合成与分解，在三维空间的一个物体先绕X轴正方向旋转ψ，再绕Y轴旋转ω，最后再绕Z轴旋转θ后，可得到

计算后可得到一个三阶矩阵

设一个在三维坐标上的点绕XYZ轴旋转的旋转变换为

旋转矩阵

再结合上面计算的三阶矩阵，可以推出

由此可得出只要空间变换的旋转矩阵，就可由此矩阵得到绕XYZ轴运动的一个角度。

基向量变换

上节所讲的旋转矩阵都是基于在同一个坐标系下的，就以坐标的矩阵表达形式来看

矩阵里面的只是一个数字，是一个标量，要想由此确认坐标系里具体的位置是要有个具体的基向量，坐标的矩阵表示应该是

向量的表示更准确

假如这个基向量改变后，那么这个矩阵里数字也会改变，下面分析下这个怎么变。
还是先以二维空间分析，再过渡到三维中去，如下图，

由图中可看到，在b系由i、j构成的坐标系中，有一个点P坐标为，

这个点P在由r系m、n构成坐标系中的坐标为

下面分析下这两者的关系，为了好分析，可将r系转为我们熟悉的垂直水平的坐标轴，并将b系坐标轴去掉，只保留基向量，如下图

可以看出i、j在r系中都可由对应的坐标表示出来，则点P

又由于基向量都是单位向量，且这里用到的变换只有旋转并无拉伸，所以b系中各个基向量在r系中的坐标都可以由该向量与r系中的m、n夹角的余弦分量表示，则上式为
那么左边乘就是基变换矩阵。

同样的，在三维坐标系中，设r系基向量为i、j、k，b系基向量为m、n、p，那么r系到b系的基变换矩阵为

记从r坐标系变换到b坐标系的坐标变换为

基变换矩阵为

上面变换公式中等式两边同时乘上基变换矩阵的逆矩阵，由逆矩阵的定义可轻松得到从b系到r系的基变换矩阵为

3.开始算法

根据上面的分析，目的就是要求的R系的旋转矩阵，如下

而陀螺仪给出的是B系的旋转角度，所以需要对这个公式做出些变化。
根据之前讲的基变换，从R系到B系的表示为

然后在B系里做一个旋转变换

再由变换后的从B系转到R系，

最终得到R系中的坐标

上式红框中等价于R系中的变换矩阵，也就是将红框内的结果计算出来，再结合三阶矩阵的角度公式，即可得到对应的变换角度。
在这公式里的各矩阵都可有陀螺仪给出的原始数据经过算数处理求的。

三、补充

1、四元数

四元数这一部分不了解，要不然就用四元数去解了>_<|||。这里不讲四元数的基础概念，说下自己的理解，在之前的变换公式中，要求得角度变换就是要求旋转矩阵，而这个旋转矩阵用四元数表示，所以最终就是要确立这个四元数Q，但是我现在还不知到怎么去确定这个四元数Q。(T＿T)

2、卡尔曼滤波

这一块我也不是很了解，因为我这套解法是利用到了重力加速度，所以在陀螺仪收到的加速度不等于重力加速度时，计算出来的结果就不正确，也就是说要在测量出来的数据与预测出的数据中间需要滤出一个正确的数据。

总结

上面数学推倒过程有不严谨的地方，这里并不是研究数学公式，而是讲下求解思路。我这套求解算法在运算的时候比较复杂，需要求叉积、矩阵相乘和矩阵逆，而且还要考虑角度范围的问题。
我自己写过一套程序处理，验证这套算法是可以解算出角度变化的，只不过实现的有点麻烦，以后有空会继续研究下四元数是怎么处理的。