树莓派控制无人机实现定点降落（六）：地标识别及控制算法实现

目录

• 树莓派控制无人机实现定点降落（六）——地标识别及控制算法的实现
• 1、地标识别 2、控制算法 3、剩余工作

树莓派 控制 无人机实现定点降落（六）——地标识别及控制算法的实现

地标识别方法同样参照了我上篇博文里提到的另一个人的博文的方法，但稍有改动，在这给出参考博文的链接
链接在此

   本篇文章最后的运行效果我发布到了b站，大家可以去看看[传送门]

通过我上篇博文的介绍，地标识别只需要识别嵌套最多的矩形即可，而树莓派安装opencv比较麻烦，所以地标识别部分我使用cv2写，并将识别好的矩形中心点发布到ros节点上用于控制。

   注：树莓派快速安装cv2：

sudo apt-get install python-opencv

使用pip安装将很慢，这种安装方法虽然版本久了点，但还是可以用的。

   接下来实现地标识别

1、地标识别

使用轮廓提取，多边形抽象，过滤面积较小的图形，然后过滤出四边形，再过滤掉非凸形。得到的四边形里通过简单的聚 类  方法寻找中心距离最近的一类，其中心的平均值即为地标中心。

   （1）首先，建立工作空间

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_create_pkg landing roscpp rospy std_msgs geometry_msgs mavros
cd ~/catkin_ws/src/landing
mkdir msg #存放自定义消息类型
mkdir scripts #存放python脚本

（2）自定义消息类型

   由cv2识别出的矩形中心点将依照某种自定义格式发布到topic上，自定义center消息类型如下：

   在msg 文件夹  下创建center.msg文件，消息格式我定义为图片的宽和高，矩形中心的位置，以及是否检测到矩形，内容如下：

uint32 width
uint32 height
float64 x
float64 y
bool iffind

（3）创建识别脚本

   在scripts下创建track.py。

   原理上面讲了，直接上代码：

import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import Image
from landing.msg import center # 自定义消息类型
import os
import cv2
import numpy as np
import time

center_publish=rospy.Publisher('/center', center, queue_size=1) # 发布矩形中心
# 获得摄像头图像的回调函数
def callback(Image):
    img = np.fromstring(Image.data, np.uint8)
    img = img.reshape(240,320,3)
    track(img, Image.width, Image.height) # 寻找矩形

# 订阅获得摄像头图像
def listener():
    rospy.init_node('track')
    rospy.Subscriber('/iris/usb_cam/image_raw', Image, callback)
    rospy.spin()

# 寻找矩形中心
def track(frame, width, height):
    img = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
    _, img = cv2.threshold(img, 127, 255, cv2.THRESH_BINARY)
    contours = cv2.findContours(img, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE) # 轮廓提取
    rects = [] # 存放四边形
    centers = [] # 存放中心点
    for contour in contours[1]:
        if cv2.contourArea(contour) < 100: # 过滤掉矩形面积小的
            continue
        epsilon = 0.02 * cv2.arcLength(contour, True)
        approx = cv2.approxPolyDP(contour, epsilon, True)
        if approx.shape[0] == 4 and cv2.isContourConvex(approx): # 过滤掉非四边形与非凸形
            rects.append(approx)
            centers.append((approx[0]+approx[1]+approx[2]+approx[3]).squeeze()/4)

	# 以下部分为聚类算法
    center_iter = list(range(len(centers)))
    result = []
    threshold = 20
    while len(center_iter) is not 0:
        j = 1
        resultnow = []
        while j < len(center_iter):
            if np.sum((centers[center_iter[0]] - centers[center_iter[j]])**2) < threshold:
                resultnow.append(center_iter[j])
                center_iter.pop(j)
                j = j-1
            j = j+1
        resultnow.append(center_iter[0])
        center_iter.pop(0)
        
免责声明：本文系网络转载或改编，未找到原创作者，版权归原作者所有。如涉及版权，请联系删