无人机基于目标检测的路径规划任务

无人机目标检测相机配置

目标检测基于darknet-yolo框架，darknet用于C/C++代码实现，适用于嵌入式设备上的目标检测
更改目标检测相机
使用RGB-D相机的左摄像头而不是单目相机，修改配置如下

在indoor.launch启动文件中模型使用realsense深度相机模型

<arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/iris_realsense_camera/iris_realsense_camera.sdf"/>

在~/catkin_ws/src/darknet_ros/darknet_ros/launch/task1.launch修改

  <!-- <arg name="image" default="/typhoon_h480_0/cgo3_camera/image_raw" /> -->
  <arg name="image" default="/iris_0/stereo_camera/left/image_raw" />

在~/catkin_ws/src/darknet_ros/darknet_ros/config/uav_0.yaml修改

    # topic: /typhoon_h480_0/cgo3_camera/image_raw
    topic: /iris_0/stereo_camera/left/image_raw

检测目标坐标系转换

浅析相机坐标系的相关转换
https://blog.csdn.net/qq_15029743/article/details/90215104

1.像素坐标系转相机坐标系
需要知道像素物理单元距离以及相机的内参矩阵

其中fx应该是f/dx，将相机的像素物理距离考虑在内
查看相机内参矩阵，camera/realsense_camera/camera_info话题，其中K为内参矩阵

intrinsic_M = np.array([[554.254691191187, 0.0, 320.5],  [0.0, 554.254691191187, 240.5],  [0.0, 0.0, 1.0]])

2.相机坐标系转世界坐标系
需要获得R、T矩阵
从/mavros/local_position/pose话题中获得偏移量,并转化为R T

# 构造四元数，并将之转化为旋转矩阵
    q = Quaternion(data.pose.orientation.w, data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z)
    R = q.rotation_matrix
 #构造平移矩阵
    T = np.array([[data.pose.position.x ],[data.pose.position.y ],[data.pose.position.z ]])

3.坐标转换流程

 #目标中心点的xy坐标(u,v)
    u = int(u)-1
    v = int(v)-1
    #中心点深度值
    Zc = box_ave(depth_image, u, v) 
    #相机内参矩阵
    intrinsic_M = np.array([[554.254691191187, 0.0, 320.5],  [0.0, 554.254691191187, 240.5],  [0.0, 0.0, 1.0]])
    
    # 像素坐标系
    uv_M = np.array([[u],[v],[1]])
    
    # 相机坐标系
    pos_C = np.dot(np.linalg.inv(intrinsic_M), uv_M)*Zc
    # print('pos_C',pos_C)
    
    #相机坐标系ENU 有时需要将相机坐标转换为以ENU系为基准
    
    # 世界坐标系(两种方法),经实验验证,这是以无人机为原点的ENU世界坐标系
    T = np.array([[X_w],[Y_w],[Z_w]])
    # pos_W = np.dot(np.linalg.inv(Rot), (pos_C_ENU - T))
    pos_W = np.dot(Rot, pos_C_ENU) + T
    print('pos_W',pos_W)

无人机目标检测地图配置

在forest.world文件中加入人物casual_female，在insert里面加入模型

d435i相机目标检测