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

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

地标识别方法同样参照了我上篇博文里提到的另一个人的博文的方法，但稍有改动，在这给出参考博文的链接
链接在此
本篇文章最后的运行效果我发布到了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)
        if len(result) < len(resultnow):
            result = resultnow
    rects = np.array(rects)[result]
    # 如果嵌套的矩形数量大于2才算提取成功
    if len(result) > 2:
		centers = np.sum(np.array(centers)[result], axis=0).astype(np.double) / len(result)
		publish(centers, width, height) # 发布消息
    else:
    	center_temp = center()
        center_temp.iffind = False
		center_publish.publish(center_temp)
    
    # 下面注释掉的部分将画出提取出的轮廓
    #cv2.polylines(frame, rects, True, (0,0,255), 2)
    #cv2.imshow('w',frame)
    #cv2.waitKey(1)

# 发布中心点消息
def publish(centers, width, height):
    center_temp = center()
    center_temp.width = width
    center_temp.height = height
    center_temp.x = centers[1]
    center_temp.y = centers[0]
    center_temp.iffind = True
    center_publish.publish(center_temp)

if __name__ == '__main__':
    listener()

下面是识别效果：

2、控制算法

通过矩形中心点距离图片中心的距离控制，使用pid算法：

#include 
#include 
#include 
#include 
#include 
#include 
#include  // 导入自定义消息类型

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

geometry_msgs::PoseStamped local_position;
void position_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    local_position = *msg;
}

landing::center landmark;
void center_cb(const landing::center::ConstPtr& msg){
    landmark = *msg;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "landing_node");
    ros::NodeHandle nh;
 	
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Subscriber position_sub = nh.subscribe<geometry_msgs::PoseStamped>
            ("mavros/local_position/pose", 10, position_cb);
    ros::Subscriber center_sub = nh.subscribe<landing::center>
            ("center", 10, center_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    ros::Publisher local_vel_pub = nh.advertise<geometry_msgs::TwistStamped>
            ("mavros/setpoint_velocity/cmd_vel", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");
 
    ros::Rate rate(20.0);
 
    while(ros::ok() && current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }
 
    geometry_msgs::PoseStamped pose;//姿态控制
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;
    
    geometry_msgs::TwistStamped vel;//速度控制
 
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }
 
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";
 
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
 
    ros::Time last_request = ros::Time::now();
 	
 	//起飞
    while(ros::ok())
    {
        if( current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0)))
        {
            if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
            {
                ROS_INFO("Offboard enabled");
            }
           	last_request = ros::Time::now();
       	}
        else if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0)))
        {
            if( arming_client.call(arm_cmd) && arm_cmd.response.success)
            {
                ROS_INFO("Vehicle armed");
            }
            last_request = ros::Time::now();
        }
        else if(ros::Time::now() - last_request > ros::Duration(5.0))
        	break;
 
        local_pos_pub.publish(pose);
 
        ros::spinOnce();
        rate.sleep();
    }
    
    //逛一圈
	last_request = ros::Time::now();
	while(ros::ok())
	{
		if(ros::Time::now() - last_request > ros::Duration(5.0))
			break;
	    vel.twist.linear.x = 1;
		vel.twist.linear.y = 0;
		vel.twist.linear.z = 0;
		local_vel_pub.publish(vel);
		ros::spinOnce();
    	rate.sleep();
	}
	last_request = ros::Time::now();
	while(ros::ok())
	{
		if(ros::Time::now() - last_request > ros::Duration(5.0))
			break;
	    vel.twist.linear.x = 0;
		vel.twist.linear.y = 1;
		vel.twist.linear.z = 0;
		local_vel_pub.publish(vel);
		ros::spinOnce();
    	rate.sleep();
	}
	last_request = ros::Time::now();
	while(ros::ok())
	{
		if(ros::Time::now() - last_request > ros::Duration(5.0))
			break;
	    vel.twist.linear.x = -1;
		vel.twist.linear.y = 0;
		vel.twist.linear.z = 0;
		local_vel_pub.publish(vel);
		ros::spinOnce();
    	rate.sleep();
	}
	last_request = ros::Time::now();
	while(ros::ok())
	{
		if(ros::Time::now() - last_request > ros::Duration(5.0))
			break;
	    vel.twist.linear.x = 0;
		vel.twist.linear.y = -1;
		vel.twist.linear.z = 0;
		local_vel_pub.publish(vel);
		ros::spinOnce();
    	rate.sleep();
	}
	
	//控制降落部分
	while(ros::ok())
	{
		//高度低于0.3时转为降落模式
		if(local_position.pose.position.z < 0.3)
			break;
		//如果找到地标，控制方向
		if(landmark.iffind)
		{
			//计算两方向err
			double err_x = landmark.height/2.0 - landmark.x;
			double err_y = landmark.width/2.0 - landmark.y;
			ROS_INFO_STREAM("state="<<err_x<<" "<<err_y);
			//速度控制
			vel.twist.linear.x = err_x/400;
			vel.twist.linear.y = err_y/400;
			//如果位置很正开始降落
			if(err_x < 10 && err_y < 10)
				vel.twist.linear.z = -0.2;
			else
				vel.twist.linear.z = 0;
			local_vel_pub.publish(vel);
			ros::spinOnce();
			rate.sleep();
    	}
    	//如果找不到矩形地标，回到2m高度
    	else
    	{
    		pose.pose.position.x = local_position.pose.position.x;
			pose.pose.position.y = local_position.pose.position.y;
			pose.pose.position.z = 2;
			local_pos_pub.publish(pose);
			ros::spinOnce();
			rate.sleep();
    	}
	}
    
    offb_set_mode.request.custom_mode = "AUTO.LAND";
    if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
    {
        ROS_INFO("Offboard enabled");
        last_request = ros::Time::now();
    }
 
    return 0;
}

这个代码的问题在于找不到地标的情况不够完整，后续我会改进。

3、剩余工作

CmakeLists.txt
需要注意添加的我已经用注释标出

cmake_minimum_required(VERSION 2.8.3)
project(landing)
find_package(catkin REQUIRED COMPONENTS
  mavros
  message_generation
  roscpp
  rospy
  std_msgs
)
add_message_files(FILES center.msg) # 这句话要注意添加
generate_messages(DEPENDENCIES std_msgs) # 这句话要注意添加
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES landing
#  CATKIN_DEPENDS roscpp rospy std_msgs
#  DEPENDS system_lib
)
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)
add_executable(landing_node src/control.cpp) # 这句话要注意添加
# 这句话要注意添加
target_link_libraries(landing_node 
${catkin_LIBRARIES}
)

package.xml
需要注意的地方我已经用注释标出

<package format="2">
  <name>landingname>
  <version>0.0.0version>
  <description>The landing packagedescription>
  <maintainer email="[email protected]">cyrilsterlingmaintainer>
  <license>TODOlicense>
    <buildtool_depend>catkinbuildtool_depend>
  <build_depend>roscppbuild_depend>
  <build_depend>rospybuild_depend>
  <build_depend>std_msgsbuild_depend>
  <build_depend>geometry_msgsbuild_depend>
  <build_depend>mavrosbuild_depend>
  
  <build_depend>message_generationbuild_depend>
  <build_export_depend>roscppbuild_export_depend>
  <build_export_depend>rospybuild_export_depend>
  <build_export_depend>std_msgsbuild_export_depend>
  <build_export_depend>geometry_msgsbuild_export_depend>
  <build_export_depend>mavrosbuild_export_depend>
  <exec_depend>roscppexec_depend>
  <exec_depend>rospyexec_depend>
  <exec_depend>std_msgsexec_depend>
  <exec_depend>geometry_msgsexec_depend>
  <exec_depend>mavrosexec_depend>
  
  <exec_depend>message_generationexec_depend>
  <export>
  export>
package>

之后返回catkin_ws编译运行就好，记得source，运行顺序如下：

roslaunch px4 mavros_posix_sitl.launch
python src/landing/scripts/track.py
rosrun landing landing_node

也可以自己写一个launch文件一次性运行
仿真到此结束，下一篇将是实物尝试与运用在实物上时算法的变化
希望疫情早点结束鸭！