四旋翼位置控制之-GPS水平位置控制

mark一下，争取一周内抽时间写完。感兴趣的朋友可以加我qq讨论。

最近事情太多，今天争取写完。先给大家介绍一些基本知识

GPS精度因子：PDOP（Position Dilution of Precision）位置精度因子、HDOP（Horzontal Dilution of Precision）水平分量精度因子。精度因子是位置质量的评判标准，精度因子的大小与GPS 定位的误差成正比，精度因子的值越大，定位误差也就越大，定位精度就越低。

GPS水平位置控制流程

1.通过解析协议读取GPS的信息

目前GPS的协议有两种NMEA协议和UBLOX协议。博主两种方式都用了，亲身经历得出经验，UBLOX协议更加简单，方便。

NMEA协议：选用常用的NMEA标准语句RMC来分析，如下为RMC格式

$xxRMC,time,status,lat,NS,long,EW,spd,cog,date,mv,mvEW,posMode,navStatus*cs<CR><LF>举例如下

$GPRMC,083559.00,A,4717.11437,N,00833.91522,E,0.004,77.52,091202,,,A,V*57

这条语句代表的意思我就不说了，对应上面的标准格式大家应该一看就明白了。这里我说下解析方法，因为这里看标准格式好像是固定的，但是实际在读取的时候会发现，NMEA协议的语句不是定字节长度的，既你的GPS 在静态和动态是，读取的语句长度不一样，你如你在静态时，RMC语句中就不出现航向角的cog的信息，而你的GPS在动态时，会出现航向角的信息，因此这给解析方法带来了难度。因为我们都想定字节长度的通过串口读取GPS信息，然后以$找到语句的头，然后数逗号的方式来解析出每一部分的信息。但是非定长 的语句，让这种方法行不通了。

UBLOX协议：选用标准语句UBX-NAV-POLLSH来分析，如下为POLLSH的结构

  Header       Class     ID     Length(Bytes)   Payload       Checksum

0XB5 0X62     0X01   0X02          28                              CK_A  CK_B

payload包含内容如下所示：

字节位         数据格式              名称       单位                      内容

   0            Usigned Long         iTOW       ms      GPS time of week of the navigation epoch

   4              Signed Long         lon          deg      Longitude

   8              Signed Long         lat           deg      Latitude

  12             Signed Long         height      mm      Height above ellipsoid

  16             Signed Long         hMSL      mm      Height above mean sea level

  20            Usigned Long        hAcc       mm      Horizontal accuracy estimate

  24            Usigned Long        vAcc        mm      Vertical accuracy estimate

有上述格式可以知道，UBLOX协议的优点，解析的时候语句开头固定为0XB5 0X62+语句ID然后紧接着就是语句的长度，这个长度是固定的所以很方便处理，并且有格式知道数据都是16进制的很方便处理，直接读取即可。

2.对解析出来的经纬度进行坐标转换

由于对GPS解析出来的经纬度是在GPS坐标系WGS84中的,所以我们要把经纬度信息转换到地球坐标系（导航坐标系n）下变成x,y。用了圆柱投影的方法直接将经纬度转换为x和y坐标。

3.将GPS融合惯导估计出位置

这就和之前介绍的高度估计算法一样了，只不过是吧观测器件转换成了GPS，位置估计算法框图如下所示

4.水平位置控制算法

位置控制算法是将位置控制输出的加速度输出转换为姿态的角度给定，所以水平位置控制采用的位置换-速度环-姿态环的三级PID控制。控制框图如下，

以上即为博主写GPS悬停算法的思路。欢迎大家加我QQ交流。写的不好。