1_竞赛无人机相关的坐标系入门知识——零基础学习竞赛无人机搭积木式编程指南

竞赛无人机相关的坐标系入门知识

1. 导航系与载体坐标系的关系

导航坐标系（简称N系）

 

 

 

载体坐标系（简称B系）

取右手笛卡尔直角坐标系为载体坐标系，绕X轴逆时针转动为俯仰运动正方向，绕Y轴逆时针转动为横滚运动正方向，绕Z轴逆时针转动为偏航运动正方向，即图中分别表示俯仰角、横滚角、偏航角。

萌新新手容易将三个角度弄混，这里根据字面意思直观的理解记忆：

1. 俯——形容身体或者头向下弯曲，从高处俯看；仰——形容面朝上，仰望、仰视，俯仰就是指的飞机相对地平面向上仰视或者向下俯视
2. 横滚——指的是飞机相对纵轴（机头到机位所在轴）左右发生旋转后与水平面投影之间的夹角
3. 偏航——航向指的是飞机机头的朝向，注意偏航角并不是指的实际飞机运动方向。

下面以MPU6050为例，来展示加速度计、陀螺仪数据三轴数据XYZ方向与飞控中B系对应关系。

                                        

        上图中带直线箭头沿坐标轴方向标注+X、+Y、+Z指示的是三轴加速度计原始数据正方向，绕轴旋转带环状箭头+X、+Y、+Z为三轴角速度的正方向。三轴加速度计XYZ正方向分别与载体坐标系的XYZ一致，故绕传感器自身X、Y、Z轴旋转分别表示俯仰、横滚、偏航角速度，其中逆时针旋转为正方向。

1. 载体坐标系XY平面在导航系EN水平面方向的二维投影坐标系（简称RP坐标系）             

        当飞机在三维空间内发生俯仰、横滚、横滚角度运动时，导航坐标系和载体系统不再重合，其中由于偏航运动造成的载体XY平面在导航系上投影方向随之游动。

 

在无人机进行位置、速度控制时，需要将EN方向的期望位置、速度、加速度转换到RP坐标系上来，最后将期望的加速度映射成期望姿态角度。姿态控制环（角度、角速度）在这里可以理解成为外层控制回路的执行机构。

在这里需要理解的是RP坐标系只是作为系统中间坐标系，用于转换导航EN方向与载体XY方向关系，坐标轴R(roll)、P(pitch)相对导航系统EN方向来讲，相差一个偏航方向运动。

 

1. 光流水平位置控制坐标系

光流传感器原始数据输出的是采样间隔时间内相对地面的像素移动，通过相机参数与旋转补偿后可以得到速度观测量。由于光流是直接固定在飞机上，故光流得到的观测速度是在RP坐标系下。通过B系到N系的旋转矩阵得到的是飞机相对于地理正北（SLAM开始建图时刻朝向、无磁力计情况下的开机初始机头模拟正北）的惯导加速度，所以要想将二者进行融合有两种途径：

• 将RP坐标系下的观测速度，通过偏航角旋转关系将坐标基底从RP映射到EN方向上来，然后与EN方向的惯导加速度进行融合；
• 将EN坐标系下的惯导加速度，通过偏航角旋转关系从EN方向映射到RP方向上来，然后与RP方向的观测速度进行融合；

        采用方式1时，在无人机进行位置、速度控制时，又需要将EN方向的期望加速度通过偏航映射又需要重新转到RP方向。为了简化运算流程，飞控代码在前期采用的是方式2，惯导融合直接在RP坐标系下融合，位置、速度控制也是在RP方向，减少了运算流程。

        在这里需要注意的是前期飞控光流方向XY方向与RP方向在X方向上刚好反向，部分旧版本的SDK函数为了兼容老客户，X方向相反这部分一直有保留下来，下面给出光流控制坐标系与RP坐标系关系。

 

        根据上面图示可知：当希望控制飞机向前移动时，可以直接在光流速度控制中，给定Y方向期望速度为+即可，同理希望左侧移动时，给定X方向速度期望为+，速度控制相关函数在后面教程中给出接收。

1. SLAM定位下的坐标系

        激光雷达SLAM定位能输出飞机在地图中的位置与航向角度，注意这里的位置和航向是相对SLAM建图初始时刻飞机的位置与朝向为基准的，直观上来理解就是飞机初始时刻朝向哪里，那个方位就是SLAM得到航向角的“正北”，此时SLAM航向角度为0。SLAM输出的位置原点也是基于初始时刻飞机的坐标位置的。我们将SLAM的“正北”称为模拟正北(SN),垂直右侧的坐标轴称为模拟正东(SN)，初始时刻的位置为导航原点O。

 

        上图中SE-O-SN为SLAM定位下的等效导航坐标系（EN），O’(x,y)和Yaw分别表示SLAM建图输出的位置与航向信息。细心的同学肯定可以发现，当我们把SN、SE当做导航坐标系下的East、North等效来看时，二者在导航融合和控制方法上是一致的，算法在处理是只需要把原来由磁力计或者GPS运动航向得到的偏航角度信息替换为由SLAM定位算法输出的航向信息就可以了。需要注意的是由于SLAM定位数据有延时、数据精度不够，SLAM定位的航向角度信息同样需要和飞控IMU融合才能使用。

 

        在SLAM定位模式下，配套有专用的高效搭积木式二次开发教程，针对新手用户控制无人机飞行有专用的API函数取实现，可以直接控制无人机位置、速度、角度、角速度等接口函数，初学者可以无需了解具体内部实现（也有配套教程讲解实现原理），只需要理解飞行器的基本控制流程，API函数功能、接口参数定义等。每个API函数都有配套的精简demo例程让客户逐步学习来加深理解，直到融会贯通。

 

 

        用户二次开发时根据已经编写好的API函数，按实际比赛要求配合视觉传感器，编写按一定流程动作飞行的API指令即可高效率的完成比赛内容，同时新增Subtask_Demo.c里面有提供2021年国赛植保无人机G题基础题、附加题完整竞赛学习方案。以往竞赛常规赛题元素例如自主循迹、移动追踪、绕杆避障、AprilTag定位等demo都有提供。