无人机实验平台(七) 实验平台的坐标转换（上）

为什么要做坐标转换？

大疆提供的sdk中指出，无人机通常情况下通过两个坐标系来控制方向：自身坐标系（Body）和地面坐标系（N-E-D）。这两个坐标系可以相互转换，这种转换可以在无人机内部完成，我们只需要设定坐标系的类型，不需要关心如何转换。

首先要说明的是，这两个坐标系其实都是以自身为原点的，定位采用的坐标系则是GPS的WGS84坐标系，参数为一个double类型的经度和double类型的纬度，当然，这是一个平面坐标系。通常情况下GPS的定位精度可以达到米级，在配合RTK等特殊设备的情况下可以实现分米级甚至更高精度的定位，当然对于本实验来说是无法做到的。

除了WGS世界坐标系之外，刚才说的NED和body坐标系都是以自身为原点的，只是坐标轴含义不同。对于自身坐标系而言，机体正前方（前进方向）为x轴，右侧为y轴，z轴由右手原则确定。对于N-E-D坐标系而言，以自身为原点，正北方向为x轴，正东方向为y轴，正下方（大地）为z轴（同样符合右手原则）。

这三个坐标系的转换是贯穿于无人机控制的流程中的。

坐标系简述

Body

对于底层控制而言，无人机能够接收的控制信号和imu中获取的加速度信息都是基于自身坐标系的，也就是上一篇最后提到的Yaw，Pitch和Roll三个参数（对于高度则是通过Throttle单独控制，坐标系有所不同）。

Pitch即俯仰姿态，当状态为前倾，无人机自然会向前飞行，也就是沿着body坐标系下的x轴前进，反之，会沿着x轴后退。

Roll为左右滚转，Roll>0时会沿着横向（y轴）正方向前进，即向右前进，反之向左前进。

Pitch和Roll操作并不会改变x轴和y轴的方向，而是使得原点平移。

Yaw即旋转，不会改变自身坐标系的原点而是使得坐标轴旋转。Yaw>0会顺时针旋转，Yaw<0反之。

Body坐标系是最简单也最为直观的坐标系，它可以很直接地表示出无人机“动态”的坐标变换。由于可以通过Yaw操作来旋转坐标系，在body坐标系中无法实现非常精准的定位（因为没有固定的坐标轴，只能通过角度的差分来控制，无法指定某个角度使机头指向特定方向），因此大多数时候这个坐标系是用于“控制”而不是“定位”。

Body坐标系可以有很多种，只要坐标轴遵循右手原则即可，不同无人机可能采用不同的形式，例如经过测试Mavic2zoom的坐标轴与上述有一定差别，是以正前方为y，右侧为x，正上方为z的（我知道这很奇怪，这意味着pitch反而控制的是左右前进，而不是前后，这和“俯仰”的原意明显不同。roll也是如此。）

Body坐标系的局限性使得必须有一种能够准确指定方向的坐标系辅助控制，于时NED坐标系被采用了。

NED

NED即北东地坐标系，由于严格制定了正北方，正东方作为坐标轴，这个坐标系中的坐标轴不会产生任何旋转变化，因此我们可以通过一个角度来唯一标识一个方向。指南针在所有无人机中都内置了（当然，有时候数据未必准确，甚至让人不知道相信无人机的数据还是收集的数据。）

NED是非常稳定的，因为它不会旋转，但是无人机可以旋转，在该坐标系中正北方向（x轴方向）被定义为0°，范围为(-180,180]，当指定一个角度的时候，无人机的机头会严格指向这个方向。

实际控制旋转的时候，系统会先测定在NED坐标系下机头的方向，然后根据机头方向和目标方向的差，在body坐标系下旋转无人机，实现精准的姿态控制。这一重转换无人机已经帮我们完成，只需要根据自己的需求设定相应的坐标系即可。

            mFlightController = aircraft.getFlightController();
            mFlightController.setRollPitchControlMode(RollPitchControlMode.VELOCITY);//set mode as velocity where we use meters/s to control.
            mFlightController.setYawControlMode(YawControlMode.ANGLE);//change to ANGLE mode(for now)
            mFlightController.setVerticalControlMode(VerticalControlMode.VELOCITY);
            mFlightController.setRollPitchCoordinateSystem(FlightCoordinateSystem.BODY);

 WGS-84坐标系

无人机在利用GPS定位时采用的坐标系为WGS84坐标系，但是在实际控制的时候仍然需要转换为NED坐标系，通过NED的速度向量来控制无人机。

 在实验中，由于缺少GPS信号，且机体中已经携带WGS84坐标系的转换模块，这里暂且略过。

实验室坐标系

在实验室中，我们通过Nokov光学动作捕捉系统可以建立一个新的坐标系，该坐标系是由软件标定产生的，仅分布在室内约4m*4m的空间内，能够实现毫米级精确定位。该坐标系是三维立体坐标系，尽管在本实验中仅需要两个维度。

由于标定时可以随意选择原点和坐标轴方向，如何将该坐标系的坐标点转换为NED坐标系下的坐标点将是实验中必须考虑的问题。从一开始就沿着NE方向建立坐标系或许是最好的选择，但是为了扩展实验的范围，这里还是有必要找到坐标转换的方法，并在设备允许的范围内进行坐标系转换参数的确认（标定）。