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

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

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

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

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

声明新变量 期望航向角度
在这里插入图片描述

在初始化函数里加入 计算初始的姿态角度
并将初始航向角度设定为 期望航向角度
ros仿真 无人机航向控制算法优化_第1张图片

在原来的 控制器参数文件里 增加一个 yaw的控制器 原来只有 yawrate的控制
在这里插入图片描述
ros仿真 无人机航向控制算法优化_第2张图片

航向锁定 计算 力矩
ros仿真 无人机航向控制算法优化_第3张图片

调整 航向控制pid

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

生成msg头文件时报这个错误
ros仿真 无人机航向控制算法优化_第4张图片

把double换成 float32后错误消失
ros仿真 无人机航向控制算法优化_第5张图片
ros仿真 无人机航向控制算法优化_第6张图片

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

ROS把这些基本类型重新定义了一番
ros仿真 无人机航向控制算法优化_第7张图片

所以把消息类型改成
ros仿真 无人机航向控制算法优化_第8张图片

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

生成了头文件
ros仿真 无人机航向控制算法优化_第9张图片

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

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

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

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

当有 航向角速度控制指令时 进行角速度控制 并更新期望的航向角度 但此时存在更新偏慢的问题 ,先不管
没有 航向角速度控制指令时 则进行期望航向角度的控制
ros仿真 无人机航向控制算法优化_第10张图片

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

你可能感兴趣的:(ros)