汇总



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