樹莓派控制無人機實現定點降落(六)——地標識別及控制算法的實現
地標識別方法同樣參照了我上篇博文裏提到的另一個人的博文的方法,但稍有改動,在這給出參考博文的鏈接
鏈接在此
本篇文章最後的運行效果我發佈到了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 <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <landing/center.h> // 導入自定義消息類型
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
需要注意的地方我已經用註釋標出
<?xml version="1.0"?>
<package format="2">
<name>landing</name>
<version>0.0.0</version>
<description>The landing package</description>
<maintainer email="[email protected]">cyrilsterling</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>mavros</build_depend>
<!-- 這句話要注意添加 -->
<build_depend>message_generation</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>mavros</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>mavros</exec_depend>
<!-- 這句話要注意添加 -->
<exec_depend>message_generation</exec_depend>
<export>
</export>
</package>
之後返回catkin_ws編譯運行就好,記得source,運行順序如下:
roslaunch px4 mavros_posix_sitl.launch
python src/landing/scripts/track.py
rosrun landing landing_node
也可以自己寫一個launch文件一次性運行
仿真到此結束,下一篇將是實物嘗試與運用在實物上時算法的變化
希望疫情早點結束鴨!