DJI串级PID算法架构

最新推荐文章于 2025-06-03 16:07:55 发布

weixin_34234829

最新推荐文章于 2025-06-03 16:07:55 发布

阅读量340

点赞数

CC 4.0 BY-SA版权

文章标签： python

原文链接：https://my.oschina.net/u/2252538/blog/847595

本文详细介绍了机器人在特定环境中如何实现精准定位与导航的过程。包括获取当前位置坐标、角度偏差及与目标位置之间的距离计算等内容。此外，还给出了移动距离的确定方法及整个移动过程的控制策略。

2019独角兽企业重金招聘Python工程师标准>>>

1.得到当前位置xyz

2.获取当前poll，pitch，yaw

3.求出当前位置离一开始的位置是否有偏差，偏差距离为，xyz

4.根据设定的值看看还要走多远，用设定的值减去xyz表示剩下的距离。

5.设置角度偏差的极限值。0.01745329252*yawDesired

  double yawDesiredRad = DEG2RAD*yawDesired;  //角度偏差
  double yawThresholdInRad = DEG2RAD*yawThresholdInDeg;  //角度最大偏差
  float32_t posThresholdInM = posThresholdInCm/100;   //位置最大偏差，默认为5
  int elapsedTime = 0;            //执行的次数
  int speedFactor = 2;            //定义的xy的阈值，单位米

6.期望偏差绝对值大于2米则取移动距离为2米，小于两米等于本身。高度的话无限制，直接当前高度+期望高度就行了。

7.根据第4步计算的值去和我们定的最小距离0.05进行比较，如果比这个误差大就进行调整。

8.如果比误差大就进行移动。移动的距离就是第六步的结果。

9.然后获取当前位置，并和初始位置比较，看下仍需要走多远。计算出已经走得xyz向量。

10.求出还需要走的距离向量xyz，用总距离减去已经走过的。

11.最后看剩下的距离是否大于2米，大于两米继续，否则进行微调。

timeoutInMs默认10000,10000/20*20=10000ms=10s，所以一次得移动最多执行500次，耗时10s。

转载于:https://my.oschina.net/u/2252538/blog/847595