Servo模式滤波器设置说明
Servo模式滤波器设置说明
简介
用户可在usersetting配置文件SERVOMOVE中，更改参数FILTER_TYPE的值来选择滤波器类型。
该值可设置为：
- 0：代表不使用滤波器；
- 1：使用关节空间的一阶低通滤波器，LPF_1ST；
- 2：使用关节空间的三阶非线性滤波器，NLF_JOINT；
- 3：使用关节空间的多阶均值滤波器，MMF_JOINT；
- 4：使用笛卡尔空间的三阶非线性滤波器，NLF_CART；
- 5：使用笛卡尔空间的速度前瞻滤波器，NLF_MMF_COMB。
关节空间的滤波器对关节运动、笛卡尔运动都生效。笛卡尔空间的滤波器仅对笛卡尔运动生效。
调用方法：
- 通过SDK调用；
- 通过Python脚本执行。
一阶低通滤波器（LPF_1ST）
使用一阶低通滤波器只需设置截止频率LPF_CUTOFF_FREQ：LPF_CUTOFF_FREQ越大，精度越高，平稳性降低；LPF_CUTOFF_FREQ越小，精度越低，平稳性提高。
从真机实测效果，LPF_CUTOFF_FREQ设为0.1时运动很顺滑，精度很差，运动严重失真，无法精准到位，随着参数慢慢增大，精度确实会越来越高，平稳性越来越低，顿挫感较强，40之后往上调参没有明显区别。
所以如果没有特别要求精准到位但对平滑和稳定性有要求的工况，可以使用小参数，反之，可以尝试使用比较大的参数。
三阶非线性滤波器（NLF_JOINT）
三阶非线性滤波器的本质是根据最大加加速度、加速度与速度限制跟踪目标的在线S曲线规划器。
当使用关节空间的三阶非线性滤波器时，需设置参数：
NLF_MAX_VR：关节速度限制，单位 °/s。关节速度限制和滤波器规划的速度快慢有关，设置越大速度越快。
NLF_MAX_AR：关节加速度限制，单位 °/s2。
NLF_MAX_JR：关节加加速度限制，单位 °/s3。
默认参数下的滤波器规划（NLF_MAX_VR=120; NLF_MAX_AR=180; NLF_MAX_JR=720）速度曲线较为平滑，如图所示：
加加速度、加速度、速度限制越大跟踪目标的能力越大，但较大的加加速度限制会导致速度震荡的发生和速度震荡幅度的增加。加加速度限制小于2000为宜，如下图所示，机器人速度震荡不止：
示例：
若参数值为：
- NLF_MAX_VR=120
- NLF_MAX_AR=720
- NLF_MAX_JR=5000
则速度曲线为：
从真机实测效果来看，使用这种模式时，运动过程精度普遍不是很高。参数小，精度高一些；参数越大，精度越差，但是平滑性会越来越好。
三阶非线性滤波可以适用于对运动过程中的轨迹精度没有要求、但对平滑性要求较高的场合。
多阶均值滤波器（MMF_JOINT）
使用该类型的滤波器时，需要设置的参数如下：
MMF_MAX_BUF：表示均值滤波器缓冲区的大小。如果滤波器缓冲区设置的太小（< 3），则很可能会造成规划失败。缓冲值也不宜过大（> 100），会给控制器带来负担，还会造成规划滞后，随着缓冲值的增大，规划滞后时间越长。
MMF_KP：位置滤波系数
MMF_KV：速度滤波系数
MMF_KA：加速度滤波系数
三个滤波系数是主要的参数，通过调节位置滤波系数KP、速度滤波系数KV、加速度滤波系数KA来调节平滑后指令与原始指令的位置误差。
参数在正常范围内时的速度曲线：
参数不在正常范围内时的速度曲线：
从真机实测效果来看，各滤波系数不能为负数，需要在0-1之间，否则可能会出现抖动剧烈、关节过流以及控制器断电等情况。
在合理参数范围内调参，不同参数间效果并无明显差异，精度相对较好，响应比较快，缺点是不够顺滑，启停冲击较大。
非线性滤波器（NLF_CART）（笛卡尔空间）
该滤波器的实现原理是一个在线的、按照最大加加速度跟踪目标的S曲线规划器，仅适用于servop。
使用该类型的滤波器时，需要设置的参数如下：
- NLF_MAX_VP： 末端线速度限制值，单位mm/s。
- NLF_MAX_AP：末端线加速度限制值，单位mm/s2。
- NLF_MAX_JP：末端线加加速度限制值，单位mm/s3。
- NLF_MAX_VR：末端姿态速度限制值，单位°/s。
- NLF_MAX_AR：末端姿态加速度限制值，单位°/s2。
- NLF_MAX_JR：末端姿态加加速度限制值，单位°/s3。
示例：
若参数值为：
- NLF_MAX_VP=1000
- NLF_MAX_AP=500
- NLF_MAX_JP=800
- NLF_MAX_VR=120
- NLF_MAX_AR=180
- NLF_MAX_JR=720
则速度曲线为：
和三阶非线性滤波一样，速度、加速度、加加速度限制越大跟踪目标的能力越大，但较大的速度、加速度和加加速度限制会导致速度波动的更加剧烈。加加速度5000以下为宜。
示例：
若参数值为：
- NLF_MAX_VP=1000
- NLF_MAX_AP=3500
- NLF_MAX_JP=5000
- NLF_MAX_VR=120
- NLF_MAX_AR=180
- NLF_MAX_JR=720
则速度曲线为：
由于此模式通过积分得到位置，存在累计误差，参数设置较大的话，可能会造成持续小幅运动，在目标位置上下波动，无法停下来。
用户需要根据实际情况选择合适的参数。
速度前瞻滤波器（NLF_MMF_COMB）（笛卡尔空间）
使用该类型的滤波器时，需要设置的参数如下：
- MMF_KP：位置滤波系数，减小该系数，滤波效果更光滑，但位置精度损失更大；增大该系数，则响应更快，精度更高，但有可能出现运行不稳定/抖动的问题，尤其是在原始数据噪声较大的情况下。
- MMF_MAX_BUF：表示均值滤波器缓冲区的大小；缓冲区越大越光滑，但精度损失更高，规划滞后时间越长。
具体参数设置根据实际数据情况调整。
真机实测反馈
从真机实测效果来看，跑servoj时，效果最好的是一阶低通滤波器；跑servop时，效果相对最好的是速度前瞻。
说明：
绿色：轻微变化
黄色：中等变化
粉色：剧烈变化
| 滤波器 | 参数 | 点位个数变化（原1352个） | 轨迹波形变化 |
|---|---|---|---|
| 一阶低通滤波（LPF） | 截止频率（cutoffFreq）：0.2 | 1377（+25） | 变化很大，但波形平滑 |
| 截止频率（cutoffFreq）：0.5 | 变化较大，波形还算平滑 | ||
| 截止频率（cutoffFreq）：0.8 | 变化较大 | ||
| 非线性滤波（NLF） | 速度上限值（max_vr）：2 | 1356（+4） | 非常大，失去原波形，轨迹趋于直线 |
| 加速度上限值（max_ar）：2 | |||
| 加加速度上限值（max_jr）：4 | |||
| 速度上限值（max_vr）：10 | 1364（+12） | 非常大，失去原波形，轨迹趋于斜线 | |
| 加速度上限值（max_ar）：10 | |||
| 加加速度上限值（max_jr）：20 | |||
| 速度上限值（max_vr）：40 | 1369（+17） | 非常大，失去原波形，轨迹仅存原轨迹趋势 | |
| 加速度上限值（max_ar）：40 | |||
| 加加速度上限值（max_jr）：80 | |||
| 多阶均值滤波（MMF） | 缓冲区大小（max_buff）：4 | 1371（+19） | 较小 |
| 位置滤波系数（kp）：0.25 | |||
| 速度滤波系数（kv）：0.25 | |||
| 加速度滤波系数（ka）：0.25 | |||
| 缓冲区大小（max_buff）：4 | 1367（+15） | 较小 | |
| 位置滤波系数（kp）：0.5 | |||
| 速度滤波系数（kv）：0.5 | |||
| 加速度滤波系数（ka）：0.5 | |||
| 缓冲区大小（max_buff）：4 | 1357（+5） | 较小 | |
| 位置滤波系数（kp）：1 | |||
| 速度滤波系数（kv）：1 | |||
| 加速度滤波系数（ka）：1 | |||
| 速度前瞻滤波（speed_foresight） | 缓冲区大小（max_buff）：2 | 1353（+1） | 较小 |
| 位置滤波系数（kp）：0.5 | |||
| 缓冲区大小（max_buff）：4 | |||
| 位置滤波系数（kp）：1 | |||
| 缓冲区大小（max_buff）：6 | 1354（+2） | ||
| 位置滤波系数（kp）：2 |