汇总
-
无人机目标跟踪与运动控制⑥——多无人机编队 [待更新]
0. 前言
首先说明一下本文所涉及的跟踪控制,是建立在无人机已经具有稳定姿态的飞控的基础上进行的(如bebop或大疆tello这种)。因此仅将无人机视作二阶系统,不涉及无人机自身的稳定控制,控制输入为XY方向的加速度(姿态倾角的角度)以及Z方向的速度。
通俗来说就是不考虑那些复杂的飞控算法,直接通过控制无人机的速度和加速度来让他听话,算法也采用简单好用的PID控制,更适合快速入手无人机的运动控制。
在上一篇文章 轨迹跟踪控制理论篇 中已经简要的说明PID控制的基本原理的无人机串级PID控制的基本框架。
接下来是本文实验的简要说明:
-
定位方法:通过动作捕捉外部定位系统来获取无人机的当前位置与姿态(Gazebo仿真环境下可以直接获取位置和姿态,室外环境可以尝试通过GPS定位,本人没有做过尝试,只要能获取到无人机实时的三维位置坐标xyz即可)
-
控制方法:通过串级反馈PID分别对无人机的速度环和位置换进行控制
-
最终效果:1.可以快速且稳定的到达目标点,2.可以对曲线轨迹进行跟踪(示例采用螺旋曲线轨迹)。
-
编程语言:Python
-
实验效果视频已上传B站:https://www.bilibili.com/video/BV1fL4y1t7bA/
-
实验图片:
本文采用的无人机是 Parrot 家的 Bebop 2 无人机,通过Parrot官方的SDK接口,可以和ROS进行很好的通讯,可以直接控制无人机XY平面的加速度以及Z方向的速度。
由于我现在已经毕业了,没有时间和条件再重新做一次仿真,就将之前实物实验的过程总结一下分享给大家,可以根据本系的列前两节仿真环境搭建的说明进行尝试。
本文暂不介绍动捕定位的流程和访问动捕数据的过程,本实验采用的是通过ROS_VRPN接口的方式获取动捕中无人机位姿的数据流,除此之外只要能够得到无人机的位姿信息并且替换代码中的 PoseStamped
消息即可。
1. 无人机位置控制
接下来本节将通过Python语言对Bebop进行位置控制,没有实物的同学可以根据《无人机目标跟踪与运动控制①——Gazebo仿真环境搭建》和《无人机目标跟踪与运动控制②——运动指令与视觉图像》两篇搭建仿真环境,只需要尝试获取无人机在环境内的XYZ位置信息即可。
前文说过,在串级控制中,首先调节内环的速度PID控制,再调节外环的位置PD控制。
在实际测试时发现,内环速度控制中,I起到的作用并不是特别大,因此本实验为了方便,内外环均为PD控制。
1.1 调试界面
为了调试方便,我们先用 PyQt 写一个简单的调试界面
这里简单介绍一下 PyQt 环境的安装,为了防止后续项目多,依赖发生问题,我们用 Anaconda 来安装 PyQt 的环境。(Anaconda 的好处就是每新建一个env都独有一套环境,不会和其他项目的环境相互冲突)Anaconda的安装自行查阅。
# 创建名称为pyqt的环境,Python版本为3.7.6
conda create -n pyqt python=3.7.6
# 进入名称为pyqt的环境
conda activate pyqt
# 安装qt的依赖
conda install pyqt
接下来写一个主程序负责调用窗口:
#!/usr/bin/env python
# -*- coding: utf-8 -*-
from PyQt5.QtWidgets import *
from window_bebop_ctrl import DataWindow
import rospy
import sys
if __name__ == '__main__':
rospy.init_node('bebop_ctrl_node')
app = QApplication(sys.argv)
window = DataWindow()
window.show()
sys.exit(app.exec_())
子程序 window_bebop_ctrl
负责建立调试qt界面以及一些功能函数:
#!/usr/bin/env python
# -*- coding: utf-8 -*-
from PyQt5.QtWidgets import *
from PyQt5.QtCore import *
from PyQt5.QtGui import *
from geometry_msgs.msg import Twist, PoseStamped
from turtlesim.msg import Pose
from std_msgs.msg import Empty
from nav_msgs.msg import Odometry
import rospy
import math
class DataWindow(QWidget):
def __init__(self):
# 定义全局变量
self.i = 0
self.x_last = 0
self.y_last = 0
self.z_last = 0
self.x = 0
self.y = 0
self.z = 0
self.vx = 0
self.vy = 0
self.vz = 0
self.x_err = 0
self.y_err = 0
self.z_err = 0
# 初始化QT
super(DataWindow, self).__init__()
self.setWindowTitle('Bebop Ctrl')
self.resize(400, 400)
# 设置更新频率
update_timer = QTimer(self)
update_timer.setInterval(16)
update_timer.start()
update_timer.timeout.connect(self.on_update)
# 以下为配置QT窗口界面的设置
wlayout = QVBoxLayout()
layout1 = QFormLayout()
layout2 = QHBoxLayout()
layout3 = QHBoxLayout()
layout4 = QHBoxLayout()
layout5 = QHBoxLayout()
layout6 = QHBoxLayout()
layout7 = QHBoxLayout()
self.linear_x = QLineEdit()
self.linear_y = QLineEdit()
self.linear_z = QLineEdit()
self.lb_x = QLabel()
self.lb_y = QLabel()
self.lb_z = QLabel()
self.lb_linear_x = QLabel()
self.lb_linear_y = QLabel()
self.lb_linear_z = QLabel()
btn_send = QPushButton('Send !')
btn_takeoff = QPushButton('TakeOff !')
btn_land = QPushButton('Land !')
label1 = QLabel()
label1.setText('Vx')
label2 = QLabel()
label2.setText(' Vy ')
label3 = QLabel()
label3.setText(' Vz ')
label4 = QLabel()
label4.setText(' ')
label_text2 = QLabel()
label_text2.setText('手动输入速度指令:')
label_text3 = QLabel()
label_text3.setText('手动输入速度指令:')
layout4.addWidget(label_text2)
layout3.addWidget(label1)
layout3.addWidget(self.linear_x)
layout3.addWidget(label2)
layout3.addWidget(self.linear_y)
layout3.addWidget(label3)
layout3.addWidget(self.linear_z)
layout3.addWidget(label4)
layout3.addWidget(btn_send)
layout5.addWidget(label4)
layout6.addWidget(label4)
layout7.addWidget(label4)
layout1.addRow('当前位置 X:', self.lb_x)
layout1.addRow('当前位置 Y:', self.lb_y)
layout1.addRow('当前位置 Z:', self.lb_z)
layout1.addRow('当前速度 Vx:', self.lb_linear_x)
layout1.addRow('当前速度 Vy:', self.lb_linear_y)
layout1.addRow('当前速度 Vz:', self.lb_linear_z)
layout2.addWidget(btn_takeoff)
layout2.addWidget(btn_land)
wlayout.addLayout(layout5)
wlayout.addLayout(layout4)
wlayout.addLayout(layout3)
wlayout.addLayout(layout6)
wlayout.addLayout(layout1)
wlayout.addLayout(layout7)
wlayout.addLayout(layout2)
self.setLayout(wlayout)
btn_send.clicked.connect(self.data_send)
btn_takeoff.clicked.connect(self.takeoff_send)
btn_land.clicked.connect(self.land_send)
# 通过vrpn接口获取动捕系统里的无人机位姿信息
pose_topic_name = '/vrpn_client_node/UAV_01/pose'
# 注意此时订阅的更新频率需要设置,不然会造成消息队列的堆积
rospy.Subscriber(pose_topic_name, PoseStamped, self.pose_callback, queue_size=1)
此时会得到如下简易的qt调试界面:
1.2 串级PID控制算法代码
由于控制器为串级PID控制,那么在实际的调试中首先对内环的速度控制进行调试,先达到可以控制无人机速度的目的,进而再调节外环参数。
前文说了,内环的速度控制采用PID控制,那么我们可以进一步的完善代码,首先是起飞降落按钮的命令:
def takeoff_send(self):
# 起飞按钮命令
print 'UAV Takeoff !!!'
takeoff_pub = rospy.Publisher('/bebop/takeoff', Empty, queue_size=10)
takeoff_pub.publish(Empty())
def land_send(self):
# 降落按钮命令
print 'UAV Land !!!'
land_pub = rospy.Publisher('/bebop/land', Empty, queue_size=10)
land_pub.publish(Empty())
接下来就是发送按钮了,在触发发送按钮之后,不仅发送期望的速度数据信息,同时也将控制算法包含在该函数中:
def data_send(self):
# 发送按钮命令(速度控制输入)
print 'Send Data !!!'
topic_name = '/bebop/cmd_vel'
data_pub = rospy.Publisher(topic_name, Twist, queue_size=100)
x_target_2 = float(self.linear_x.text())
y_target_2 = float(self.linear_y.text())
z_target_2 = float(self.linear_z.text())
x_last_err = self.uav2_x_err
y_last_err = self.uav2_y_err
z_last_err = self.uav2_z_err
self.uav2_x_err = x_target_2 - self.uav2_x
self.uav2_y_err = y_target_2 - self.uav2_y
self.uav2_z_err = z_target_2 - self.uav2_z
sum_err = math.sqrt(math.pow(self.uav2_x_err, 2) + math.pow(self.uav2_y_err, 2))
linear_x = 0.0014 * self.uav2_x_err + 0.0012 * (self.uav2_x_err - x_last_err)
linear_y = 0.0014 * self.uav2_y_err + 0.0012 * (self.uav2_y_err - y_last_err)
self.vvx_tar_2 = linear_x
self.vvy_tar_2 = linear_y
vx_last_err = self.uav2_vx_err
vy_last_err = self.uav2_vy_err
self.uav2_vx_err = linear_x - self.uav2_vx
self.uav2_vy_err = linear_y - self.uav2_vy
if sum_err <= 20:
angle_x = 0
angle_y = 0
else:
angle_x = 0.2 * self.uav2_vx_err + 0.012 * (self.uav2_vx_err - vx_last_err)
angle_y = 0.2 * self.uav2_vy_err + 0.012 * (self.uav2_vx_err - vy_last_err)
twist = Twist()
twist.linear.x = angle_x
twist.linear.y = angle_y
twist.linear.z = 0
if twist.linear.x > 0.3:
twist.linear.x = 0.3
if twist.linear.x < -0.3:
twist.linear.x = -0.3
if twist.linear.y > 0.3:
twist.linear.y = 0.3
if twist.linear.y < -0.3:
twist.linear.y = -0.3
self.control_vx_2 = twist.linear.x
self.control_vy_2 = twist.linear.y
data_pub.publish(twist)
def pose_callback(self, msg):
if not isinstance(msg, PoseStamped):
return
x = - msg.pose.position.x
y = msg.pose.position.y
z = msg.pose.position.z - 58
self.x = round(x, 1)
self.y = round(y, 1)
self.z = round(z, 1)
if self.i > 10:
vx = (self.x - self.x_last)*10
vy = (self.y - self.y_last)*10
vz = (self.z - self.z_last)*10
self.vx = round(vx, 1)
self.vy = round(vy, 1)
self.vz = round(vz, 1)
self.i = 0
self.lb_x.setText(str(self.x))
self.lb_y.setText(str(self.y))
self.lb_z.setText(str(self.z))
self.lb_linear_x.setText(str(self.vx))
self.lb_linear_y.setText(str(self.vy))
self.lb_linear_z.setText(str(self.vz))
self.x_last = self.x
self.y_last = self.y
self.z_last = self.z
self.i = self.i + 1
# print(self.i)
def on_update(self):
# render UI
self.update()
if rospy.is_shutdown():
self.close()
1.3 绘制图像信息
通过 Matplotlib 工具绘制图像,并储存数据,需要在前面的qt函数中,添加 data_draw
按钮,绘图参考代码如下:
def data_draw(self):
self.flag_draw = 1
fig, ax1 = plt.subplots(figsize=(4, 7))
fig2 = plt.figure()
ax2 = fig2.add_subplot(311)
ax3 = fig2.add_subplot(312)
ax8 = fig2.add_subplot(313)
now_x = []
now_y = []
controlv = []
x_tar = []
y_tar = []
vvvx = []
vvvy = []
vx_tar = []
vy_tar = []
t = []
i = 0
while True:
if self.flag_draw == 1:
t.append(i)
x_target_2 = float(self.linear_x.text())
y_target_2 = float(self.linear_y.text())
x2 = self.uav2_x
y2 = self.uav2_y
now_x.append(x2)
now_y.append(y2)
x_tar.append(x_target_2)
y_tar.append(y_target_2)
vx2 = self.uav2_vx
vy2 = self.uav2_vy
vvvx.append(vx2)
vvvy.append(vy2)
vx_tar.append(self.vvx_tar_2)
vy_tar.append(self.vvy_tar_2)
control_vx_2 = self.control_vx_2
controlv.append(control_vx_2)
ax1.cla()
ax1.set_title("UAVs")
ax1.set_xlabel("y / mm")
ax1.set_ylabel("x / mm")
ax1.set_xlim(-1000, 1000)
ax1.set_ylim(-1800, 1800)
ax1.grid()
ax1.scatter(y2, x2, s=10, c='g')
ax1.scatter(y_target_2, x_target_2, s=10, c='r')
i += 1
plt.pause(0.01)
else:
time = self.GetNowTime()
x2_target = np.array([x_tar]).T
y2_target = np.array([y_tar]).T
now_xx2 = np.array([now_x]).T
now_yy2 = np.array([now_y]).T
vx2_target = np.array([vx_tar]).T
vy2_target = np.array([vy_tar]).T
now_vvvx = np.array([vvvx]).T
now_vvvy = np.array([vvvy]).T
t_list = np.array([t]).T
control_uav1 = np.array([controlv]).T
# print x2_target
uav2_data_save = np.concatenate((t_list, x2_target, y2_target, now_xx2, now_yy2, vx2_target, vy2_target, now_vvvx, now_vvvy, control_uav1), axis=1)
np.savetxt("/home/chenan/bebop_ws/uav_data_"+time+".txt", uav2_data_save, fmt='%f', delimiter=', ')
plt.savefig("/home/chenan/bebop_ws/uav_"+time+".png")
plt.close(fig)
plt.close(fig2)
break
1.4 调试
打开 bebop 无人机(实物),开机之后连续按开机键三下,使无人机连接上WiFi,查看是否能够与无人机的网络 Ping 通。
运行程序,点击 Take off 按钮,等待无人机起飞后,输入分别对x和y方向的期望速度进行输入,场地有限的话,建议对目标速度取反(即加负号直接发送),查看无人机对速度阶跃信号的跟踪情况。
本次调试中的实验结果如图:
在速度控制达预期目标后,将qt中的速度输入更改为位置输入,即可对位置控制的参数进行调节,如图:
本次调试中的实验结果如图:
2. 无人机轨迹跟踪
若要进行轨迹跟踪实验,首先需要设定期望轨迹,以下以螺旋曲线轨迹为例
在代码中,只需要将 x_target
,y_target
,z_target
部分的输入更改为带有时序的轨迹即可,参考代码如下:
self.theta = j * 0.04
self.x_target_2 = 600 * np.cos(self.theta)
self.y_target_2 = 600 * np.sin(self.theta)
self.z_target_2 = 500 + j
j += 2.5
最终的实验结果如图
将实验结果绘制成三维曲线:
完整代码参考
完整代码参考:https://github.com/Chenan-W/Python-Trajectory-Tracking-Control-for-UAV
评论(3)
您还未登录,请登录后发表或查看评论