9、定点模式-相关参数调试

1、概述：

Loiter模式相当于在定高模式下，再假如了一层位置控制！ 当无人机的Loiter悬停模式打开后，飞行器会自动保持当前位置、方向和高度。但是GPS定位效果、 罗盘干扰情况 、 IMU的振动都会直接影响一个悬停效果。

2、Loiter如何控制？

• 操控者使用遥控器控制杆，控制飞行器水平位置和垂直高度。
• 水平位置可以用Roll和Pitch控制杆调节，当飞手放开摇杆，飞行器会缓慢降速，直到停止。
• 定高模式 一样，通过油门杆控制高度，通过Yaw控制杆控制方向。
• 在Ardupilot3.1（或更高版本），当GPS 3D锁定状态或HDOP降到2.0或更低，才可成功切换到Loiter模式，否则无法成功切换到Loiter模式，并提示切换失败，需要位置信息，如下图所示：    

位置控制器，处于PID环路的外环；又分为位置环与速度环！

在串级PID中，在源码层面可以拆分为四阶：位置环->速度环->姿态角度环->角速率环！每一环的输出，又是下一环的输入！

3、Loiter模式-扩展调参中显示的参数

Loiter模式属于位置控制环，在位置控制环同样有外环和内环，如下：

但是，参数并不止下述，详见后文。

4、悬停定点模式-位置控制器外环(位置控制外环)：PSC_POSXY_P(水平)、PSC_POSZ_P(垂直)

位置控制器的外环只有2个参数如下，分别控制的是水平方向和垂直方向，只有一个P项控制！

PSC_POSXY_P参数的含义：位置控制器比例增益。它把「飞机当前位置和目标位置的距离差」，转换成一个「目标水平速度」，再传给水平速度控制器去执行。

PSC_POSZ_P参数的含义：位置控制器比例增益。它把「飞机当前位置和目标位置的距离差」，转换成一个「目标垂直速度」，再传给水平速度控制器去执行。

以水平方向为例，假如飞机偏离了1m，它会告诉飞控：我需要以多少m/s的速度往回飞，才能修正这个误差，可通过以下例子进行辅助理解！

• 如果误差是 1 米，PSC_POSXY_P=0.5，那么目标速度就是 1 × 0.5 = 0.5 m/s。
• 如果PSC_POSXY_P=1.0，同样 1 米误差，目标速度就是 1.0 m/s，回正的 “力度” 大一倍。

调大：飞机回正目标的速度更快，抗风能力更强，遇到气流偏一点就立刻拉回来；定点精度更高，漂移范围更小，在强风下也能死死钉住位置。

调大的风险：飞机的修正动作会非常猛，导致悬停时来回 “画圈”、左右晃动，出现明显的振荡；修正时动作生硬，有明显的来回摆荡，久久不收敛；甚至会引发水平方向的高频抖动，影响云台和画面。

调小：修正动作非常柔和，飞机不会来回晃荡，手感顺滑。不会因为修正过猛引发振荡，稳定性好。

调小的风险：飞机回正目标的速度很慢，抗风能力变弱，遇到气流会慢慢飘走，定点精度差。风大时，飞机甚至无法修正误差，一直被吹着飘，需要手动频繁打杆修正。

配合参数：LOIT_ACC_MAX，Loiter 模式最大修正加速度。

5、悬停定点模式-位置控制器速度环(位置控制内环)：PSC_VELXY_*(水平)、PSC_VELZ_*(垂直)

悬停PID中速率内环相关的参数，有水平方向和垂直方向两个轴可调试，可在全部参数列表中，搜索：PSC_VELXY_开头的参数、PSC_VELZ_开头的参数！

Z轴垂直方向的PID参数，则需要用户自行在全部参数中进行搜索PSC_VELZ_开头的参数！

如下图所示，悬停PID-速率内环相关的参数主要有XY水平轴的PID参数，以及Z轴的PID参数；

此外，在扩展调参的悬停速率中的P参数、I参数、D参数、I最大参数都对应的是XY轴水平方向的参数。

5.1 位置控制器-速率内环-XY轴水平方向参数

XY轴速度环 (PID) → 把水平方向的「目标速度 和 实际速度的差」转成目标倾斜加速度。

PSC_VELXY_P，P比例项

PSC_VELXY_I，I积分项

PSC_VELXY_D，D微分项

PSC_VELXY_IMAX，I积分限幅项，一般不用去调整。

PSC_VELXY_FF，前馈项，默认=0未开启，最大为6，单次建议增量0.01

作用：把目标速度的变化快慢，直接提前转换成目标加速度，不用等速度产生误差再去修正。

调大：水平打杆极度跟手，无滞后、不肉；快速横移、转弯很利落，不拖泥带水机动响应干脆

调大的风险：太冲、太贼，轻轻推杆飞机就猛窜；容易出现水平前后左右小抖动、点头晃机身；过大直接引发水平高频振荡！

调小或关闭：机身极其稳、顺滑、不窜不抖；定点更安静，没有多余小动作

调小的风险：打杆明显延迟、发肉；快速横移反应慢半拍，转弯拖沓；手感迟钝，像拖着东西在飞！

PSC_VELXY_FLTE水平速度环 P、I 共用输入低通滤波（单位Hz）

作用：把速度误差信号磨平滑，再给 P 项、I 项做计算。

调大：滤波变弱，信号几乎直通、反应灵敏、跟手度高。

调大的风险：容易带进高频毛刺，造成水平微晃、机身小抖动！

调小：滤波变强，水平动作顺滑，抑制抖动能力强

调小的风险：滞后变大，打杆变肉、回正变慢、抗风变弱，容易慢悠悠飘

PSC_VELXY_FLTD水平速度环 D 项专用输入低通滤波（单位Hz）

作用：专门给速度环 D 项 滤波降噪，是抑制水平高频抖动、机身微颤的关键。【因为增大D增加阻尼的同时，也会让信号噪声放大！】

             太大就抖、太小就晃

调大：滤波变弱，D 项阻尼更足、刹车更干脆、不超调。

调大的风险：噪声全进 D 项，最终可能导致机身高频微抖、电机吱吱啸叫！

调小：滤波变强，强力压制 D 项噪声，机身很安静、不抖不颤

调小的风险：D 项被滤得太糊 → 阻尼滞后、刹车变差、松杆容易往前冲、左右晃荡不收敛。

PSC_VELXY_FLTE 由于主要滤的是PI项(对噪音并不敏感)，所以比 PSC_VELXY_FLTD可以设更高。例：PSC_VELXY_FLTE=25～30Hz ，PSC_VELXY_FLTD=15～20Hz

5.2位置控制器-速率内环-Z轴垂直方向参数

Z轴速度环 (PID) → 把垂直方向的「目标速度 和 实际速度的差」转成「目标倾斜加速度.

PSC_VELZ_P
PSC_VELZ_I
PSC_VELZ_D
PSC_VELZ_IMAX
PSC_VELZ_FF
PSC_VELZ_FLTE
PSC_VELZ_FLTD

上述参数的调参思路，与XY水平轴的思路完全一致，此处不再赘述！

6、Loiter模式下，手感相关参数的调试

加速度是：描述速度变化快慢的物理量；

加加速度是：描述加速度变化快慢的物理量；

加加速度相关参数：

PSC_JERK_XY位置控制回路下，达到水平最大加速度的快慢。

PSC_JERK_Z位置控制回路下，达到垂直最大加速度的快慢！

作用：限制目标加速度变化有多快，加速度从 0 涨到最大值的速度有多快。

控制着无人机从静止状态开始移动的的快速程度，以及无人机接近最大速度时如何快速完成加速。

如果增大 PSC_JERK_XY 、PSC_JERK_Z无人机将响应非常干脆，如果设置的太高，无人机可能会抽搐震荡。或者当你开始动摇杆打算让无人机运动的时候，它会非常突然过度的反应。

Loiter相关的参数，可通过missionplaner的全部参数中，搜索 “LOIT_” 相关的参数，如下：

LOIT_SPEED，Loiter模式下，打满杆能够飞行的最大速度，默认是12.5m/s。(最大速度受限于无人机允许倾斜角度以及机体性能)

如果速度设置的很快，但是 LOIT_ACC_MAX设置的很小，那么加速就会非常慢需要很久的时间！

PILOT_SPEED_DN，Loiter 模式下，手动打杆的最大下降速度！

PILOT_SPEED_UP，Loiter 模式下，手动打杆的最大上升速度！

PILOT_ACCEL_Z，Loiter 模式下，手动打杆的 最大垂直加速度！

RTL_LOIT_TIME，返航到头顶时，悬停多少秒以后再开始降落？

LOIT_ANG_MAX，Loiter模式下，允许的最大倾斜角度。默认是设置为0，取参数PSC_ANGLE_MAX /或参数ATC_ANGLE_MAX的2/3；此外哪怕将LOIT_ANG_MAX设的很大，也不会超PSC_ANGLE_MAX /或参数ATC_ANGLE_MAX的值！

PSC_ANG_MAX，在位置模式下所允许的角度，设0则使用ANGLE_MAX！

LOIT_ACC_MAX，Loiter 模式下，修正位置误差所允许的最大修正加速度，值越大修正位置误差的速度就越快，动作也更加激进！

单位：cm/s/s    

作用：限制飞行器在 Loiter 模式下为了修正位置误差而能够产生的最大水平加速度。

数值越高，飞行器的加速和停止速度就越快。当出现位置偏差时（比如被风吹离原位），飞控系统会计算需要施加的加速度来回到目标位置。LOIT_ACC_MAX 就是这个加速度的上限值。

例如：如果设置为 200 cm/s/s = 2 m/s/s；当飞行器被风吹离目标位置 2 米时，飞控最多只会以 2 m/s/s 的加速度来修正位置。

调大：纠偏猛、抗风强 → 容易晃、超调。

调小：动作柔、机身稳 → 容易飘、回正慢。

新手用默认参数即可，航拍求稳稍微调小一点，大风环境适当调大一点。

LOIT_BRK_ACCEL，打杆松杆回中时，Loiter模式使用的最大刹车加速度！(不受LOIT_ACC_MAX的限制)

单位：cm/s/s    

调大：摇杆一回中，减速力度大、刹停非常快、滑行距离短；但是刹车太猛，容易俯冲点头、机身顿挫、超调晃荡

调小：刹车减速很平缓，慢慢滑行停下；松杆后滑很远才停，刹车拖沓

LOIT_BRK_JERK，打杆松杆后的最大刹车加加速度。此参数控制无人机，能够多快能达到最大刹车加速度。值越大，达到最大刹车加速度的时间也就越快！(该值越大停得越干脆)

单位：cm/s/s/s  

调大：能很快拉满最大刹车倾角，快速建立完整刹车力度。但是加速度瞬间突变，刹车生硬、顿挫感强、机身突然晃 / 点头

调小：刹车加速度缓慢爬升，渐进式建立刹车力度。整个减速过程极度顺滑、过渡自然、无顿挫。但是达到最大刹车角度偏慢，刹车响应稍柔一点

LOIT_BRK_DELAY，Loiter 模式下打杆并松杆的制动时间，LOIT_BRK_DELAY=0代表：松杆后立即制动。(默认 =1，1s后才制动)

如何通过分析日志，调试Loiter模式？

将无人机用loiter模式，让无人机分别处于东西方向、南北方向，然后反复满杆测试，先左满杆然后快到末端时再右满杆，来回多次；横滚和俯仰方向都是一样的满杆操作！

同时在做一下垂直方向的急速轰满 油以及急速拉低油测试，完成日志记录！

1）先分析loiter模式的速度控制(内环)

水平方向PID控制器跟随情况分析：

PIDN，南北方向速度的 PID 比例 / 积分 / 微分增益，分析南北向的速度日志PIDN.Act和PIDN.Tar的曲线跟随情况。

PIDE，东西方向速度的 PID 比例 / 积分 / 微分增益，分析东西向速度日志PIDE.Act和PIDE.Tar的曲线跟随情况。

垂直方向PID控制器跟随情况分析：

PIDA，垂直方向加速度的 PID 比例 / 积分 / 微分增益参数分析南北向的速度日志PIDA.Act和PIDA.Tar的曲线跟随情况。

2）再调试loiter模式的位置控制(外环)

水平方向位置跟随分析：

分析南北向的位置日志PSCN.TPN(期望位置)和PSCN.PN(实际位置)的曲线跟随情况。

分析东西向的位置日志PSCE.TPE(期望位置)和PSCE.PE(实际位置)的曲线跟随情况。

垂直方向位置跟随分析：

分析垂直方向的位置日志PSCD.TPD(期望位置)和PSCD.PD(实际位置)的曲线跟随情况。

日志中的正值对应 负高程方向(日志中高度为负值)

7、位置控制的日志分析详解

Loiter定点模式的飞行性能，关键是分析日志中，以下这三组参数的曲线拟合程度!  拟合的越好，代表定点效果越佳！

水平北方向的日志：PSCN，日志中的正值对应地理北向。

水平东方向的日志：PSCE，日志中的正值对应地理东向

垂直向下方向的日志：PSCD，日志中的正值对应 负高程方向(日志中高度为负值)

上述三组日志，分别记录了位置控制模式，水平方向和垂直方向的期望速度、期望位置、期望加速度以及实际速度、实际位置、实际加速度！

• NED坐标系、机头坐标系：

上述参数的日志只看最后一位字母，就能看到N、E、D坐标系，在定点、自动、引导等涉及到位置控制的模式都会使用NED坐标系！在NED坐标系中的位置是相对于飞机初始化成功后的EKF原点的运动！

还有一个坐标系是机体坐标系：XYZ；其中X的正值为前进方向，Y的正值为机体右倾方向，Z的正值为机体向下方向！

• 两个坐标系会相互转换使用：

陀螺仪、加速度计、电机推力、姿态角、内环 PID 全在机体坐标系上！

GPS 位置、轨迹导航、定点悬停、返航、位置控制外环全在NED坐标系上！

两者在AP固件里面来回转换，传感器原始数据在机体系 → 转换成 NED 地理系做导航定位 → 再从NED系转回机体系，从而控制飞机姿态和电机。

分析1：通过位置控制模式下的东西、南北、竖直方向的相对速度来查看悬停效果！

如下图所示：3个轴跟随效果较好的是两条曲线基本拟合！

通过调节，悬停速度环(位置内环)，相关 PID参数可以上述决定PSCN、PSCE、PSCD的日志跟随效果！

分析2：通过位置控制模式下的东西、南北、竖直方向的相对位置，来查看悬停效果！

分析3：通过对比GPS的经纬高、XKF1的

分析3：通过对比GPS的经纬高，XKF1的PN、PE、PD来查看悬停效果！