ros仿真 无人机航向控制算法优化

ros仿真 无人机航向控制算法优化

当前算法：有输入航向角速度时 就进行扭矩计算 ，当外界干扰时 不进行处理

算法优化思想 根据 航向角速度输入 决定机头是否锁定当前航向角度

锁定算法 思想， 期望角度 与实际角度进行pid 计算

声明新变量 期望航向角度

在初始化函数里加入 计算初始的姿态角度
并将初始航向角度设定为 期望航向角度

在原来的 控制器参数文件里 增加一个 yaw的控制器 原来只有 yawrate的控制

航向锁定 计算 力矩

调整 航向控制pid

将航向角度发布出来 以topic形式

生成msg头文件时报这个错误

把double换成 float32后错误消失

那ros 不能把double数据作为topic吗
看这篇博客 可以看到 double用float64代替了

ROS把这些基本类型重新定义了一番

所以把消息类型改成

编译没有问题
可以看到用double时 是白色的， 用float64是是蓝色的。所以类型一定要用蓝色的

生成了头文件

程序运行 ，正常的发布了 想要的消息

通过rqt的plot工具和在线调整pid参数的工具 来整定pid参数

经过调整pid 值 现在受 云台的影响小了
目前还存在一个问题 无人机自身航向自稳 与 外界航向控制并存，现在外界控制不了呢

将整个无人机的航向控制算法 调整为如下所示

当有 航向角速度控制指令时 进行角速度控制 并更新期望的航向角度 但此时存在更新偏慢的问题 ，先不管
没有 航向角速度控制指令时 则进行期望航向角度的控制

至此无人机航向受 云台转动的影响基本解决