基于OpenHarmony的3D姿态解算及展示

大家都有接触过体感游戏么，即使没亲身体验过，也一定看到过，体感游戏最基础的一种情形就是现实的动作同步展现在屏幕上。本Demo将带你亲密接触，这种现实与虚拟的结合。Demo的底层数据采用Geek_Lite_Board开发板采集，它内部使用了OpenHarmony3.0操作系统，显示端则采用Processing编写上位机。最终实现开发板与上位机软件同步姿态，如下动图展示。

MPU9250传感器简介

实现虚拟与现实的同步，用到的最重要的传感器是我们平时简称为陀螺仪的传感器，本Demo使用的陀螺仪MPU9250属于9轴陀螺仪，通过软件运算，它能够实时反馈自身的姿态。MPU9250传感器，它有如下特性：

1 MPU9250 内部集成有 3 轴陀螺仪、3 轴加速度计和 3 轴磁力计，输出都是 16 位的数字量;

2 可以通过集成电路总线( IIC) 接口和MCU进行数据交互，传输速率可达 400 kHz /s；

陀螺仪的角速度测量范围最高达±2000（° /s），具有良好的动态响应特性。加速度计的测量范围最大为±16g( g 为重力加速度)，静态测量精度高。磁力计采用高灵度霍尔型传感器进行数据采集，磁感应强度测量范围为±4800μT，可用于对偏航角的辅助测量。

以下从感性认识及使用的角度讲解什么是陀螺仪、加速度计及磁力计：

三轴陀螺仪：又叫角速度传感器，描述的物理量是偏转、倾斜时的转动角速度。

读取传感器芯片的数据寄存器的原始数据后，经过换算能得出三个轴方向的角速度大小，它们以“度/秒”为单位；角速度数据再与时间相乘，再积分（累加求和），就能得到物体旋转的角度值（物体倾斜的角度值）。

三轴加速度计：也叫重力感应器，实际上是感应三个轴方向上的加速度（重力加速度则只是地表垂直方向加速度），描述物体速度变化快慢的物理量。陀螺仪更多关注自身旋转情况（原位运动），加速计则主要是测量设备的受力情况，也就是三轴运动情况。

通过读取数据寄存器的原始数据后，经过换算后能得出三个轴方向加速度m/s2 （米每二次方秒）；设备静止时受到的地球引力为1g，1g是物体在地球的海平面上受到的下拉力(9.8米/秒²)。假如设备从高处掉落，其加速计测量到的加速度将为0g。假如设备水平放在桌面上，则加速计测量出的加速度为1g(9.8米/秒²)。

三轴磁力计：也叫地磁、磁感器，可用于测试磁场强度和方向，定位设备的方位，磁力计的原理跟指南针原理类似，可以测量出当前设备与东南西北四个方向上的夹角。磁力计得出的数据为三个轴方向的磁场大小，单位为高斯，通常我们用各个轴的数值通过三角函数计算出角度，推算出目前方向与地球北极方向的偏差。但是由于地球磁场比较微弱，数据很容易受到干扰。本样例也未使用磁力计数据。

姿态角简介

贯穿姿态结算都会用到姿态角，那么姿态角是什么东西？有什么作用？

姿态角也叫欧拉角，是物体绕坐标系三个坐标轴(x,y,z轴）的旋转角度，即绕x轴的滚转角(Roll)、绕y轴的俯仰角(Pitch)和绕z轴的偏航角(Yaw)。

它是用来表达物体旋转后的姿态，我们通过三个姿态角获得旋转矩阵，旋转矩阵能够描述物体的姿态。

我们描述一个能表示确定姿态/旋转的欧拉角需要三个元素：1 旋转角度；2 旋转顺序；3 内旋或外旋；

内外旋：内旋是以自身坐标系的三个轴进行旋转（本文章内容默认是内旋），外旋是以世界坐标系进行旋转。

1pitch_roll_yaw

我们分四步来感官理解姿态角（内旋）：

1 最初，机体坐标与世界坐标重合，即机器的自身坐标，与客观外界一致；

2 第一次旋转：绕z轴旋转一个角度，通常称为航向角；

3 第二次旋转：再继续绕第一次旋转后的y轴旋转一个角度，通常称为俯仰角；

4 第三次旋转：再继续绕第二次旋转后x轴旋转一个角度，我们通常称为滚转角。

用这三个轴角度的正余弦函数值，组成旋转矩阵，也叫方向余弦阵，这个矩阵可以描述刚体旋转后的姿态。方向余弦阵也能用四元数表达，即用三个轴角度值描述的旋转矩阵等价于用四元数描述的旋转矩阵。根据对应关系，就能通过四元数算出三个轴的角度值，同理反之亦然。姿态角在理解上比较感官，但是运算比较复杂，四元数不好理解，但是运算方便，所以姿态解算通常要熟悉两者关系及意义。

推荐网址：

https://zhuanlan.zhihu.com/p/98320567

https://blog.csdn.net/sinolover/article/details/90671784

https://zhuanlan.zhihu.com/p/359738184

四元数简介

四元数也是用来描述刚体的旋转，可以理解为它是一种数学工具，用这种数学工具能够更简单的进行各种数学运算。它比欧拉角更有优势，可以避免万向节锁，可以提供平滑插值等。在本样例中，只需要理解四元数和欧拉角都是用来表示旋转，并且他们的旋转矩阵有一一对应的关系即可。

下图是用欧拉角表示的旋转矩阵（根据旋转顺序不同，会有多种欧拉角旋转矩阵，这里选用最常见的绕zyx的顺序得到如下旋转矩阵）：

下图是四元数表示的旋转矩阵：

用欧拉角和用四元数表示的旋转矩阵是等效的，它们一一对应，故有如下转换公式：

本样例中除了涉及基础的四元数算法，数据融合时还涉及矩阵运算，叉积，点积以及正交矩阵的性质等，本文只呈现如何实现，具体知识背景请按需自行补充。

推荐网址：

https://zhuanlan.zhihu.com/p/27471300

https://blog.csdn.net/Leyvi_Hsing/article/details/54293690

https://zhuanlan.zhihu.com/p/52335076

开发步骤简介

1 Geek_Lite_Board开发板获取MPU9250传感器陀螺仪及加速度计原始数据，把原始数据换算为物理单位的数值，例如陀螺仪三个轴数据（把开发板平放桌面）：处理后三个轴的数据应该都接近0，单位是度/秒（详细请看关键代码分析章节）；

2 通过四元数算法，把已经转化为物理单位的数据拟合成三个姿态角：Pitch（倾斜角）、Roll（翻滚角）、Yaw（航向角），再把数据串口发送给上位机；

3 上位机解析出三个姿态角，根据姿态角计算出旋转矩阵；

4 更新上位机坐标系，实现上位机软件同步开发板实时姿态。

关键代码分析

/************************************************************************************
*一 创建两个任务,分别执行处理数据及发送数据
*************************************************************************************/
    static UINT32 Task_Init(void) 
    {
        UINT32 Status = LOS_OK;
		//第一个任务是传感器数据获取
        Status = Start_System_Task();
        if (Status != LOS_OK) {
            return LOS_NOK;
        }
		//第二个任务是按照协议发送Pitch、Roll、Yaw数据给上位机
        Status = Start_User_Task();
        if (Status != LOS_OK) {
            return LOS_NOK;
        }
        return Status;
    }
/************************************************************************************
*二 把原始数据处理换算为物理单位数据
*************************************************************************************/
	{
        //根据寄存器数据手册提供的地址，获取原始数据
        Mpu_Data.accel_x   = MPU_BUFF[0] << 8 | MPU_BUFF[1];
        Mpu_Data.accel_y   = MPU_BUFF[2] << 8 | MPU_BUFF[3];
        Mpu_Data.accel_z   = MPU_BUFF[4] << 8 | MPU_BUFF[5];
        Mpu_Data.temperature = MPU_BUFF[6] << 8 | MPU_BUFF[7];
        Mpu_Data.gyro_x = ((MPU_BUFF[8]  << 8 | MPU_BUFF[9])  - Mpu_Offset.gyro_x);
        Mpu_Data.gyro_y = ((MPU_BUFF[10] << 8 | MPU_BUFF[11]) - Mpu_Offset.gyro_y);
        Mpu_Data.gyro_z = ((MPU_BUFF[12] << 8 | MPU_BUFF[13]) - Mpu_Offset.gyro_z);
        
		//加速度计数据后面做了归一化处理，所以这里没有把原始数据换算为有具体物理意义的数据
        memcpy(&Mpu_Calc.accel_x, &Mpu_Data.accel_x, 3 * sizeof(int16_t));

		//根据配置计算出角速度,以下处理转化为弧度/秒
        Mpu_Calc.gyro_x = Mpu_Data.gyro_x / 16.384f / 57.3f;
        Mpu_Calc.gyro_y = Mpu_Data.gyro_y / 16.384f / 57.3f;
        Mpu_Calc.gyro_z = Mpu_Data.gyro_z / 16.384f / 57.3f;
    }
/************************************************************************************
*三 调用四元数运算函数
*************************************************************************************/		
	void Mpu_Attitude_Calc(void)//四元数算法及数据融合
	{	
        //加速度计的数据归一化处理，因为四元数的数据已经是归一化数据
        norm = inv_sqrt(ax * ax + ay * ay + az * az);
        ax = ax * norm;
        ay = ay * norm;
        az = az * norm;
		//根据四元数旋转矩阵对应位置，如下是四元数推算的重力加速度的各轴分量
        vx = 2.0f * (q1q3 - q0q2);
        vy = 2.0f * (q0q1 + q2q3);
        vz = q0q0 - q1q1 - q2q2 + q3q3;
		//加速度计重力的分量值与四元数的分量值存在误差，误差主要由于四元数是用陀螺仪（陀螺仪存在温漂误差）获得，
        //误差的相关性特征可以通过它们之间做叉乘运算的算出
        ex = (ay * vz - az * vy) ;
        ey = (az * vx - ax * vz) ;
        ez = (ax * vy - ay * vx) ;
        //把误差比例放大或缩小到与陀螺仪数据同一数量级，KpKi需要实际调参
        if(ex != 0.0f && ey != 0.0f && ez != 0.0f)
        {
            exInt = exInt + ex * Ki * halfT;
            eyInt = eyInt + ey * Ki * halfT;
            ezInt = ezInt + ez * Ki * halfT;
            gx = gx + Kp * ex + exInt;
            gy = gy + Kp * ey + eyInt;
            gz = gz + Kp * ez + ezInt;
        }
        //根据四元数运算算法
        tempq0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;
        tempq1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;
        tempq2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;
        tempq3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT;
        norm = inv_sqrt(tempq0 * tempq0 + tempq1 * tempq1 + tempq2 * tempq2 + tempq3 * tempq3);
        q0 = tempq0 * norm;
        q1 = tempq1 * norm;
        q2 = tempq2 * norm;
        q3 = tempq3 * norm;
	}
/************************************************************************************
*四 根据欧拉角旋转矩阵与四元数旋转矩阵一一对应的关系，用四元数推算欧拉角角度值
*************************************************************************************/
	{
        Mpu_Read_Data.yaw = -atan2(2*q1*q2 + 2*q0*q3, -2*q2*q2 - 2*q3*q3 + 1)* 57.3;
        Mpu_Read_Data.pit = -asin(-2*q1*q3 + 2*q0*q2)* 57.3;
        Mpu_Read_Data.rol =  atan2(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1)* 57.3;
    }

3D姿态展示上位机代码介绍

本Demo上位机采用Processing解析Geek_lite_board开发板传回的陀螺仪数据，实现上位机与开发板同步3D姿态。关于Processing的进一步消息，请自行在网络上搜索。推荐两个学习网址，本人也是通过这两个网站的学习，得以实现本次项目初步功能。

https://zhuanlan.zhihu.com/p/104872344

https://www.arduino.cn/thread-42852-1-1.html

以下着重讲述，上位机代码关键部分：

1 选取正确的端口号：

开发板串口正确接入电脑后，查看对应端口号，并修改Processing代码。

void setup()
{
  size(800, 600, P3D);

  myPort = new Serial(this, "COM15", 115200);//端口号设置为15，波特率115200

  img = loadImage("openharmony800x600.jpg");//加载背景图
  background(img);
  textSize(60); 
  textMode(SHAPE); 
}

2 绘制Geek_lite_board外形图

void drawGeekLiteBoard()
{
  int i=0;
 
  stroke(0, 90, 90); 
  fill(90, 89, 86); 
  box(300, 10, 200); //绘制大的底板，注意圆心
  stroke(0); 
  fill(242, 232, 191);
    
  //绘制开发板连接口
  for (i=0; i<10; i++)
  {
    pushMatrix();
    translate(140-30*i, -10, 90); 
    box(20, 10, 10); 
    popMatrix();
  }

  for (i=0; i<10; i++)
  {
    pushMatrix();
    translate(140-30*i, -10, -90);
    box(20, 10, 10);
    popMatrix();
  }

  for (i=0; i<5; i++)
  {
    pushMatrix();
    translate(-140, -10, -60+30*i);
    box(10, 10, 20);
    popMatrix();
  }
}

3 通过陀螺仪传来的三个姿态角（roll、pitch、yaw），旋转坐标系

void draw()
{
  serialEvent();  
  background(img);
  lights();

  translate(width/2, height/2); 
  
  pushMatrix(); //因为每一次的旋转会影响到下一次旋转，使用pushMatrix保存当前的坐标为基准
  //旋转矩阵的元素  
  float c1 = cos(radians(roll));
  float s1 = sin(radians(roll));
  float c2 = cos(radians(pitch));
  float s2 = sin(radians(pitch));
  float c3 = cos(radians(yaw));
  float s3 = sin(radians(yaw));
  //矩阵相乘，即坐标系根据该矩阵进行旋转
  applyMatrix( c2*c3, s1*s3+c1*c3*s2, c3*s1*s2-c1*s3, 0,
    -s2, c1*c2, c2*s1, 0,
    c2*s3, c1*s2*s3-c3*s1, c1*c3+s1*s2*s3, 0,
    0, 0, 0, 1);

  drawGeekLiteBoard();
  popMatrix();//回归到pushMatrix之前的坐标基准
}

样例操作步骤

1 开发板上电，usb接入电脑，会听到嗡鸣器，”嘀“一声，表示板子已经正确供电并运行；

2 查看电脑端正确的端口号；

3 打开Processing代码，修改对应的端口号，开启上位机，转动开发板，即能看到上位机图形与开发板同步姿态。

总结：

本项目是基于OpenHarmony操作系统开发的3D姿态结算，涉及OpenHarmony及姿态计算的背景知识。本Demo样例关于3D姿态解算的知识主要是个人理解角度描述，有不准确的地方还望大家参与指正，以及欢迎大家把样例提交到OpenHarmony知识体系SIG仓，共建开发样例，请参考https://gitee.com/openharmony-sig/knowledge/blob/master/docs/co-construct_demos/README_zh.md

本代码下载地址：

git clone git@gitee.com:geekros/OpenHarmony_For_STM32F427.git --depth=1

参考样例代码下载地址：

git clone git@gitee.com:openharmony-sig/knowledge_demo_temp.git --depth=1

Geek_Lite_Board相关网址：www.geekros.com

金永生 / knowledge_demo_temp

基于OpenHarmony的3D姿态解算及展示

MPU9250传感器简介

姿态角简介

四元数简介

开发步骤简介

关键代码分析

3D姿态展示上位机代码介绍

1 选取正确的端口号：

2 绘制Geek_lite_board外形图

3 通过陀螺仪传来的三个姿态角（roll、pitch、yaw），旋转坐标系

样例操作步骤

总结：

简介

发行版

贡献者

近期动态

金永生 / knowledge_demo_temp .gitee-modal { width: 500px !important; }

基于OpenHarmony的3D姿态解算及展示

MPU9250传感器简介

姿态角简介

四元数简介

开发步骤简介

关键代码分析

3D姿态展示上位机代码介绍

1 选取正确的端口号：

2 绘制Geek_lite_board外形图

3 通过陀螺仪传来的三个姿态角（roll、pitch、yaw），旋转坐标系

样例操作步骤

总结：

简介

发行版

贡献者

近期动态

搜索帮助

金永生 / knowledge_demo_temp