ardupilot开发 --- Jetson Orin Nano 后篇

在这里插入图片描述

0~19

请参考前篇

20 visp-d455:基于IBVS的Pixhawk无人机视觉伺服

20.1 基础

在这里插入图片描述

  • 参考:Tutorial: Image-based visual-servoing on a drone equipped with a Pixhawk

  • 第三方库: MavSDK,librealsense

  • 一些概念:

    • onboard computer:板载计算机、机载计算机
    • IBVS:基于图像的视觉伺服
    • Homogeneous Matrix:齐次变换矩阵
    • rotation 、translations、transformation:旋转、平移、转换
    • Image Moments:图像矩
  • 软件:

    • QGC 或 Mission Planner,安装在地面站PC上,windows系统或Ubuntu系统。
    • MavProxy ,安装在机载计算机上。

关于连接、通讯、UDP forward服务:

  • 板载计算机与飞控用USB连接。
  • 板载计算机与地面站PC:通过局域网连接,进行SSH。例如使用思翼科技的链路产品MK32实现局域网连接,使用MobaXterm实现SSH。
  • MavProxy
    运行在:板载计算机上,Ubuntu20.04,安装教程
    与飞控的连接:USB,即--master=/dev/ttyACM0
    创建3个UDP forward服务,使得其他应用或程序能过通过UDP连接到飞控(与飞控通讯)!
    启动命令如:
    mavproxy.py --master=/dev/ttyACM0 --out=udpout:192.168.30.111:14550 --out=udpout:192.168.30.111:14551 --out=udpout:127.0.0.1:14552
    
  • Mission Planner 或 QGC
    运行在:地面站PC,windows系统或Ubuntu系统都可。
    与飞控的连接:通过mavproxy创建的UDP forward服务:192.168.30.111:14550,其中192.168.30.111是地面站PC的IP地址。
  • keyboard Control app:
    运行在:地面站PC,windows系统或Ubuntu系统都可。
    与飞控的连接:通过mavproxy创建的UDP forward服务:192.168.30.111:14551,其中192.168.30.111是app所在系统的IP地址。通讯使用MavSDK。
  • IBVS app
    视觉伺服程序,visp程序。
    运行在:板载计算机上,Ubuntu20.04.
    与飞控的连接:通过mavproxy创建的UDP forward服务127.0.0.1:14552,其中127.0.0.1是app所在系统的IP地址。通讯使用MavSDK。

Jetson Orin Nano 与飞控的连接和测试

  • 使用usb线连接飞控的Type-C口与Jetson Orin Nano的USB口,用mavproxy测试是否能正常连接到飞控:mavproxy.py --master=/dev/ttyACM0
  • 错误提示:
    在这里插入图片描述
  • 解决方法:
    卸载 modemmanager 然后重启 Jetson Orin Nano.
    在这里插入图片描述

通过MK32提供的局域网SSH到Jetson Orin Nano

  • 1)将Jetson Orin Nano的网口与MK32天空端的LAN口用网线进行连接,注意Jetson Orin Nano的接口是RJ45,MK32天空端的接口是GH1.25-8P。若需要制作连接线,MK32天空端的接口定义请参考:A8mini云台相机篇,RJ45接口定义请百度查询。
    在这里插入图片描述
  • 2)Jetson Orin Nano设置静态IP
    IP:192.168.144.79(11,12,20,25,26已被占用)
    网关:192.168.144.1
    掩码:255.255.255.0
    MK32的局域网设置请参考:MK32使用手册
    在这里插入图片描述
  • 3)用网线将PC的网口连接到MK32遥控器的LAN口。
  • 4)PC设置静态IP
    IP:192.168.144.80(11,12,20,25,26已被占用)
    网关:192.168.144.1
    掩码:255.255.255.0
  • 5)PC端使用SSH工具进行远程登录到 Jetson Orin Nano
    SSH前先ping一下局域网内的其他IP,验证一下网络状况。
    在这里插入图片描述

一些相关的、有用的例程

  • testPixhawkDroneTakeoff.cpp
    简单的起飞然后降落。
  • testPixhawkDronePositionAbsoluteControl.cpp
    起飞,方形轨迹飞行
  • testPixhawkDronePositionRelativeControl.cpp
    起飞,以起飞点为中心进行方形轨迹飞行
  • testPixhawkDroneVelocityControl.cpp
    起飞之后是一个简单的轨迹,使用速度控制测试一些不同的动作。
  • autopilot_server.cpp

Linux C++程序的gdb断点调试

  • 1)用VSCode打开代码
  • 2)开始调试 在这里插入图片描述
    注意:
    【1】"program": "/home/jetson/shd/visp-ws/visp-build/modules/robot/testPixhawkDroneTakeoff"中不要使用~来表示用户目录,因为VSCode无法识别!!
    【2】编译时:cmake -DCMAKE_BUILD_TYPE=debug否则无法识别断点!

搭建仿真

  • 前提:
    Ardupilot仿真运行在WSL上,visp程序和mavproxy运行在Jetson Orin Nano上!
    WSL和Jetson Orin Nano在同一个局域网!
  • 1)在Jetson Orin Nano上安装mavproxy:
    sudo apt-get install python3-dev python3-pip python3-matplotlib python3-lxml python3-pygame
    pip3 install PyYAML mavproxy --user
    echo 'export PATH="$PATH:$HOME/.local/bin"' >> ~/.bashrc
    . ~/.bashrc
    
  • 2)已知Jetson Orin Nano的IP地址为:192.168.100.197
  • 3)在WSL上运行SITL仿真:
    sim_vehicle.py -v ArduCopter --console --out "192.168.100.197:14550" --out "127.0.0.1:14550" -w
    
    其中127.0.0.1:14550可用连接windows端的mission planner,便于查看飞行状态。
  • 4)在Jetson Orin Nano上启动MavProxy
    mavproxy.py --master=udp:192.168.100.197:14550 --out=udpout:127.0.0.1:14552
    
    注意:MavProxy生成的缓存文件mav.parm, mav.tlog ,mav.tlog.raw 要定时清理,这些文件保存在运行命令时的所在目录!不然会占用机载计算机的内存空间!
  • 5)testPixhawkDroneTakeoff.cpp例程调试和分析
    • /home/jetson/shd/visp-ws/visp/modules/robot/test/servo-pixhawk/testPixhawkDroneTakeoff.cpp
    • 构造一个mavsdk 对象;
    • 飞控已启动后,局域网已正常连接;
    • 建立一个TCP、UDP、串口连接,不成功会抛出异常并终止程序。
    • 发送指令之前先调用isRunning()函数,查看飞机是否在运行。
    • mavsdk的析构函数会导致飞机降落??
    • 使用类vpRobotMavsdk控制飞机飞行的方式:
      • 1)先使用setPositioningIncertitude()设置定位和偏航的不确定性参数,如:
        drone.setPositioningIncertitude(0.10, vpMath::rad(5.));
        位置和偏航不确定范围为0.1m和5deg。
        这些参数用于你判断飞机是否到设定的期望位置或偏航角期望值,可以用
      • 2)调用takeControl()让飞机进入guided(ardupilot)或off-board(PX4)模式。
      • 3)使用 setPosition() 或 setVelocity() 去控制飞机移动 。
      • 其他
        设置起飞高度:drone.setTakeOffAlt(5.);
        是否打印详细信息:drone.setVerbose(true);

解决【testPixhawkDroneTakeoff.cpp例程能解锁但起飞命令无响应,断点模式下却有响应】问题

在这里插入图片描述
解决:先用地面站将飞控设置为guided,再运行testPixhawkDroneTakeoff.cpp例程。

解决【使用最新的mavsdk版本导致visp编译报错】问题

背景:visp中的视觉伺服例程使用的固件是px4,跟本人正在使用Ardupilot有较大区别,例如px4中的off-board而Ardupilot中是guided模式,等等。
通过查阅mavsdk源码可知:mavsdk中对guided模式的一些定义和使用在src\mavsdk\core\ardupilot_custom_mode.h中,并且必须是最新的mavsdk源码版本即main分支。
使用最新的mavsdk代码,编译visp时报错,分析和解决如下:
在这里插入图片描述
解决方式:使用合适的构造函数代替:
cpp //mavsdk::Mavsdk m_mavsdk {}; mavsdk::Mavsdk m_mavsdk{mavsdk::Mavsdk::Configuration{mavsdk::Mavsdk::ComponentType::GroundStation}};
在这里插入图片描述
解决方式:
cpp #if (VISP_HAVE_MAVSDK_VERSION > 0x010412) passthrough.unsubscribe_message(MAVLINK_MSG_ID_HEARTBEAT,handle); #else passthrough.subscribe_message_async(MAVLINK_MSG_ID_HEARTBEAT, nullptr); #endif
一些条件编译中的报错处理方法大同小异:
大部分都是#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)下面的代码报错,因为visp作者使用的是mavsdk1.4版本,在例程中执行的是else下面的代码if下的代码则不会被执行所以不会被发现,更新mavsdk到最新版本后再编译visp就会出现编译报错!!
解决方式:
在这里插入图片描述
修改后的 vpRobotMavsdk.cpp .

visp中的异步编程:std::promise和std::future

std::promise和 std::future 是怎么配合工作的?以及它们在异步编程中扮演的角色是什么?参考百度。

20.2 视觉伺服

知识储备

  • 坐标变换矩阵即坐标齐次变换矩阵,包括:旋转变换、平移变换。
    Rotation Matrix
    本质:坐标变换本质上是一种投影变换,是一种投影关系!从系a转换到系b即把系a中的坐标投影到系b.
    应用:2维,3维,n维
    举例:Xb = 旋转矩阵*Xb + 系a原点在系b中的坐标 = Tr * Xa + Ao_b = 齐次变换矩阵 * Xa
    旋转译自rotation平移译自translations转换译自transformation
    坐标旋转的表示方式有多种:方向余弦矩阵、欧拉角、四元素等等,可参考坐标旋转篇
    在这里插入图片描述
  • 单应性矩阵(单应矩阵、齐次矩阵、投影变换矩阵)
    Homogeneous Matrix
    本质:是一种坐标转换关系,是两个图像坐标系之间的转换关系矩阵。
    应用:图像投影。
    什么是图像投影?有什么作用?
    对于3D空间的一系列点(某个场景),在不同视觉角度拍摄得到的图像分别记作P1、P2,P1P2中的同一个点p在P1P2的图像坐标系(2D的)的坐标记作【x1,y1】、【x2,y2】,那么它们之间的转换关系可以用单应矩阵表示:【x2,y2】= H * 【x1,y1】
    应用场景:交通场景中,例如需要获得俯视视角下的街道平面各智能体的坐标。而摄像机的位置可能无法做到完全俯视。此时可以标注一些地标,例如地板正方形瓷砖的角作为对应点,之后通过确定的H矩阵来将摄像机的影像转换为俯视视角的坐标。
    参考:Homography matrix(单应矩阵)简介
    计算方式:
    在visp中单应矩阵可以通过3D空间坐标旋转矩阵得到,如:
    vpRxyzVector c1_rxyz_c(vpMath::rad(-10.0), vpMath::rad(0), 0);
    vpRotationMatrix c1Rc(c1_rxyz_c); // Rotation between (c1) and (c)  //c1->c的3d空间坐标旋转矩阵
    vpHomogeneousMatrix c1Mc(vpTranslationVector(), c1Rc); // Homogeneous matrix between (c1) and (c) //c1->c的2d图像(平面)坐标旋转矩阵,即单应矩阵
    
    对于单应矩阵的估计请参考:这里Tutorial: Homography estimation from points
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
  • 图像矩(Image Moments)
    • 源自:数学学科中随机变量的矩

    • 领域:数字图像处理、计算机视觉、深度学习

    • 对象:二值图像、灰度图像

    • 概念:图像的矩可以概括为图像的某些像素点的灰度值(像素值,在灰度图中像素值即灰度值)的加权平均值,或者是图像具有类似功能或意义的属性的加权平均值。
      意义:可以通过图像的矩来获得图像的一些有用的性质,包括面积、轮廓、几何中心(重心、质心)、方向(特征向量、主轴)等信息 ,这些信息对于图像识别、形状匹配和目标跟踪等应用非常有用。

    • 分类:

      • 原点矩
        (p+q)阶原点矩:
        在这里插入图片描述
      • 中心距
        (p+q)阶原点矩:
        在这里插入图片描述
      • 中心归一化距
        (p+q)阶中心归一化距:
        在这里插入图片描述
    • 矩的应用:

      • 目标区域的面积 = 目标区域的像素个数 = m00
        与零阶混合原点矩有关!
      • 目标区域的质心坐标 =【m01/m00, m10/00】
        与 一阶混合原点矩有关!
      • 目标区域的方向
        与二阶混合中心距有关!
        主轴斜率 = mu20/mu11
        副轴斜率 = mu11/mu20
        在这里插入图片描述
    • opencv中的图像矩介绍
      空间矩:double m00,m10,m01,m20,m11,m02,m30,m21,m12,m03
      中心矩:double mu20,mu11,mu02,mu30,mu21,mu12,mu03
      中心归一化矩:double nu20,nu11,nu02,nu30,nu21,nu12,nu03

    • 参考文献
      https://zh.wikipedia.org/wiki//wiki/矩_(图像)
      https://zhuanlan.zhihu.com/p/519713049
      https://blog.csdn.net/weixin_43212588/article/details/132674015
      https://blog.csdn.net/shuiyixin/article/details/104646531/

分析与实践

  • 相关概念
    IBVS、视觉伺服、图像矩视觉特征、坐标变换、mavsdk、guided模式、Visp…

  • IBVS视觉伺服理论请参考:视觉伺服篇

  • 坐标变换理论请参考:坐标变换篇

  • 图像矩视觉特征的介绍请参考:
    【0】Image-based visual-servoing
    【1】硕士论文:基于图像的四旋翼无人机视觉伺服控制-郑栋梁
    【2】硕士论文:基于无标定视觉伺服的定位研究-王博

  • 坐标系示意图:
    在这里插入图片描述
    注意:文档 https://visp-doc.inria.fr/doxygen/visp-daily/tutorial-pixhawk-vs.html 中画出的坐标系示意图(下图)是错误的!!因此原源代码中的一些注释也是错误的!!
    在这里插入图片描述

  • 需要根据相机的安装位置调整的坐标系变换矩阵
    终端坐标系e:视觉伺服中的重要坐标系,视觉伺服计算得到的运动控制量最终要转换到该坐标系下,在该案例中终端坐标系即FRD飞机机体坐标系。
    相机坐标系c:IBVS视觉伺服推导的重要坐标系。
    中间坐标系c1,原点与c重合,轴向与c同向但与e平行。
    例如:相机的安装位置如下图所示,并且相机的镜头微微向下倾斜10°,则e到c的坐标齐次变换矩阵cMe计算方式如下:
    在这里插入图片描述

    //! [compute cMe]
    vpRxyzVector c1TOc_xyz(vpMath::rad(-10.0), vpMath::rad(0), vpMath::rad(0)); // c1 ~> c 的xyz轴旋转量!
    vpRotationMatrix c1Rc(c1TOc_xyz); // 旋转矩阵:c -> c1 
    vpRotationMatrix cRc1 = c1Rc.inverse(); // 旋转矩阵:c1 -> c, 旋转矩阵是正交的,因此 c1Rc^-1==c1Rc^T
    vpHomogeneousMatrix cMc1(vpTranslationVector(), cRc1); // 齐次变换矩阵:c1 -> c
    vpRotationMatrix c1Re {0, 1, 0, -1, 0, 0, 0, 0, 1}; // 旋转矩阵:e -> c1 
    vpTranslationVector e0_c1(0, 0, -0.1); // 平移关系:e系原点在c1系中的坐标
    vpHomogeneousMatrix c1Me(e0_c1, c1Re); // 齐次变换矩阵:e -> c1
    vpHomogeneousMatrix cMe = cMc1 * c1Me; // 齐次变换矩阵:e -> c
    vpVelocityTwistMatrix cVe(cMe);  // 伺服系统控制量Vc=[vx,vy,vz,wx,wy,wz]的坐标变换矩阵:e -> c,从飞机机体系FRD到相机系RDF
    //! [compute cMe]
    task.set_cVe(cVe);
    //eJe推导:全自由度控制量Vc=[vx,vy,vz,wx,wy,wz]^T,只允许vx,vy,vz,wz可控,令[vx,vy,vz,wx,wy,wz]=[vx,vy,vz,0,0,wz]=eJe*[vx,vy,vz,wz]
    //则可知eJe=[1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 0; 0 0 0 0; 0 0 0 1]
    //那么IBVS伺服系统的状态空间变为:dot_S = L * cVe * eJe * Ve = L * cVe * eJe * [vx,vy,vz,wz]
    //其中:cVe是6x6矩阵,eJe是6x4矩阵,Ve是终端系的4x1速度矢量
    vpMatrix eJe(6, 4, 0);//哪些速度自由度是可控的!!!!!!!!!!!!
    eJe[0][0] = 1;//vx可控
    eJe[1][1] = 1;//vy可控
    eJe[2][2] = 1;//vz可控
    eJe[5][3] = 1;//wz可控
    task.set_eJe(eJe);
    

    visp对于 c M e , c V e , e J e {^cM}_e,{^cV}_e,{^eJ}_e cMe,cVe,eJe的解释请参考:Image-based visual-servoing.

  • 定义视觉特征期望值
    在相机系下定义4个期望特征点,然后将期望值转换到图像系下:

    // Desired distance to the target
    double Z_d = (opt_has_distance_to_tag ? opt_distance_to_tag : 1.); //相机到二维码的 期望控制距离!
    // Define the desired polygon corresponding the the AprilTag CLOCKWISE  //视觉特征期望值!
    double X[4] = { tagSize / 2., tagSize / 2., -tagSize / 2., -tagSize / 2. }; //定义4个【相机系下的特征点期望值】!
    double Y[4] = { tagSize / 2., -tagSize / 2., -tagSize / 2., tagSize / 2. };
    std::vector<vpPoint> vec_P, vec_P_d;
    vpHomogeneousMatrix cdMo(0, 0, Z_d, 0, 0, 0);//AprilTag机体系到相机系的【期望齐次变换矩阵】!
    for (int i = 0; i < 4; i++) {
      vpPoint P_d(X[i], Y[i], 0);//相机系下的视觉特征期望值!
      P_d.track(cdMo); //计算【图像系下的视觉特征期望值】,结果保存在 P_d 的祖父类 pTracker 的 cP 中!
      vec_P_d.push_back(P_d);//视觉特征期望值向量!
    }
    
  • 起飞

    #ifdef CONTROL_UAV//这个宏定义是本例程中所有控制飞机指令的 使能
      //drone.doFlatTrim();//自动校正加速度计和陀螺仪
      drone.takeOff(false,15,true);//起飞,takeOff包含了:guided -> 解锁 -> 起飞 -> 位置保持
    #endif
    
  • 获取当前帧

    rs.acquire(I);//获取当前图像
    
  • 二维码识别

    //使用当前帧检测AprilTags的位姿,结果保存在cMo_vec
    std::vector<vpHomogeneousMatrix> cMo_vec;
    detector.detect(I, tagSize, cam, cMo_vec); 
    
  • 在计算IBVS控制量前更新当前帧的图像矩视觉特征
    略…

  • 计算飞机机体系下的IBVS控制量(速度矢量)

    if (detector.getNbObjects() != 0) 
    {
      vpColVector ve = task.computeControlLaw();//根据IBVS理论计算得到飞机的6D速度控制量[u,v,w,p,q,r]
    }
    
  • 飞机运动控制
    发送速度指令:

    #ifdef CONTROL_UAV
      drone.setVelocity(ve);//发送控制飞机速度的指令,ve是飞机FRD body坐标系下的速度矢量。4dim,vx,vy,vz,wz.
    #endif
    

    发送停止运动指令:

    #ifdef CONTROL_UAV
      drone.stopMoving(); // In this case, we stop the drone  //没有检测到AprilTags则停止飞行防止失控!
    #endif
    
  • RTSP推流
    使用ffmpeg推流,主要逻辑:

    std::string rtsp_server_url = "rtsp://127.0.0.1:554/live/0";
    std::stringstream command;
    command << "ffmpeg -loglevel error ";//log打印等级设置为:有报错时才打印到窗口
    // inputfile options
    command << "-y "  // overwrite output files
    << "-an " // disable audio
    << "-f rawvideo " // force format to rawvideo
    << "-vcodec rawvideo "  // force video rawvideo ('copy' to copy stream)
    << "-pix_fmt bgr24 "  // set pixel format to bgr24 or gray //灰度流和彩色流这个参数要变一变不然会显示异常!!
    << "-s 640x480 "  // set frame size (WxH or abbreviation)  640x480
    << "-r 10 "; // set frame rate (Hz value, fraction or abbreviation)
    command << "-i - ";
    // outputfile options
    command 
    << "-c:v libx264 "  // Hyper fast Audio and Video encoder
    << "-pix_fmt yuv420p "  // set pixel format to yuv420p
    << "-tune:v zerolatency "
    << "-preset ultrafast " // set the libx264 encoding preset to ultrafast
    << "-f rtsp "; // force format to flv for rtmp, rtsp for rtsp
    command << rtsp_server_url;
    FILE *fp = nullptr;
    // 在子进程中调用 ffmpeg 进行推流
    fp = popen(command.str().c_str(), "w");
    if (fp != nullptr) 
      std::cout << "fp open success!" << std::endl;
    else 
    {
      std::cout << "fp open fail!" << std::endl;
      pclose(fp);
    }
    while(1) {
    //识别结果
    vpImage<vpRGBa> Ioverlay;
    vpDisplay::getImage(I, Ioverlay);//获取识别结果图像
    cv::Mat frame;
    vpImageConvert::convert(Ioverlay, frame);
    if(!frame.empty()) 
      fwrite(frame.data, sizeof(char), frame.total() * frame.elemSize(), fp);//推流
    }
    

在这里插入图片描述

  • 图像叠加显示
    //识别结果 I
    vpImage<vpRGBa> Ioverlay;
    vpDisplay::getImage(I, Ioverlay);//获取识别结果图像
    cv::Mat frame;
    vpImageConvert::convert(Ioverlay, frame);
    //IBVS误差曲线
    vpImage<vpRGBa> I_plot;
    plotter.getImage(I_plot);//获取当前误差曲线图像
    //将误差曲线和识别结果进行图像叠加显示
    cv::Mat frame_plot;
    cv::Mat frame_overlay;
    vpImageConvert::convert(I_plot, frame_plot);
    cv::addWeighted(frame, 0.6, frame_plot, 0.4, 0, frame_overlay);//opencv中的图像叠加函数
    vpImageConvert::convert(frame_overlay, overlayImg);
    display2.display(overlayImg);
    display2.flush(overlayImg);
    

在这里插入图片描述

  • 编译和运行
    使用帮助请查阅README.md

  • 结果分析
    在这里插入图片描述
    该例程中使用的图像矩视觉特征解释请参考:Image-based visual-servoing
    Xn:重心矩视觉特征的x分量误差!
    Yn:重心矩视觉特征的y分量误差!
    an:目标区域面积视觉特征误差!控制相机到AprilTag中心的距离!
    atan(1/rho)"):灭点视觉特征误差!两条灭点直线相互平行时(不要求水平只要求相互平行)该误差为0!
    在这里插入图片描述

  • 其他
    在代码中引用外部库xxxx.h xxxx.so
    CMakeLists.txt中:
    在这里插入图片描述
    pkg-config --cflags --libs gstreamer-1.0

  • 一些疑惑

    • 二维码颠倒之后伺服控制量是否正常??
      与摆放位置无关!伺服控制量都是正常的!以第一次识别到二维码时的二维码姿态来计算初始灭点水平线!!!
    • 在SITL中由guided模式切换到手动模式如 loiter 或 atlHold 为什么会自动降落?
      因为在SITL中,虚拟遥控器的油门遥感是默认保持最低位的,因此在由guided模式切换到手动模式如loiter或atlHold时会表现为自动下高度。
    • 如何将鼠标点击事件转换成命令行输入的形式?左击开始,再左击停止,右击停止并降落!!
      待解决…
    • Ardupilot的位置估计和姿态估计是基于哪个坐标系下的?
      地固坐标系:NED local frame(或称 NED Ardupilot reference frame)
      飞机机体坐标系1:FRD body frame
      飞机机体坐标系2:FLU body frame
      位置估计:FRD body frame 原点在 NED local frame 中的坐标【posX,posY,posZ】
      姿态估计:FRD body frame 与 NED local frame 间的旋转关系,一般用欧拉角表示:【俯仰角,横滚角,偏航角】,航天航空领域一般使用东北天的旋转顺序来计算欧拉角。
      运动控制量是基于哪个坐标系下计算得到的?
      最终的控制量是基于按个坐标系下的量 ?
      姿态角、姿态角速度控制:body frame FRD
      速度控制:body frame FRD or Local frame NED ?
      位置控制:body frame FRD or Local frame NED ?
      位置、姿态测量传感器是基于哪个坐标系下的量?
      GPS:WGS84
      视觉定位Vicon:Vicon ENU reference frame
      动作捕捉器MoCAP:MoCAP ENU reference frame
    • 如何解决一开始就检测不到Apriltag报的段错误?
      在这里插入图片描述
      解决:添加判断检测到Apriltag再绘制误差曲线if (condition && (detector.getNbObjects() != 0))
  • 项目源码:https://github.com/huangyangl/visp

  • 代码:servoPixhawkDroneIBVS-test.cpp
    有多个感叹号“!!!!!”的注释是需要留意的地方!

  • 代码 vpRobotMavsdk.cpp

  • vpPlot添加一个函数

    //debug 20240628
    void vpPlot::getImage(vpImage<vpRGBa> &Idest)
    {
      display->flush(I);
      display->getImage(I, Idest);
    }
    
  • README.md
    README.md

    # cmake 
    cd ~/shd/visp-ws/visp-build
    cmake ../visp -DCMAKE_BUILD_TYPE=debug -DUSE_BLAS/LAPACK=GSL
    cmake ../visp -DCMAKE_BUILD_TYPE=release -DUSE_BLAS/LAPACK=GSL
    # make 
    cd ~/shd/visp-ws/visp-build
    make -j6 servoPixhawkDroneIBVS-test
    # run servo-pixhawk in jetson
    cd ~/shd/visp-ws/visp-build/example/servo-pixhawk
    ./servoPixhawkDroneIBVS-test --help
    ./servoPixhawkDroneIBVS-test --tag-size 0.12 --co udp://:14550 --distance-to-tag 2 --verbose --rtsp --cambottom
    # run mavproxy in jetson
    mavproxy.py --master=/dev/ttyACM0 --out=udpout:127.0.0.1:14550
    # wsl sitl
    wsl
    cd ~/code/hyl-git-repo-ws/sitl-master
    sim_vehicle.py -v ArduCopter --console --out "192.168.100.197:14550" --out "127.0.0.1:14550" -w
    # RTSP server
    cd ZLMediaKit/release/linux/Debug
    ./MediaServer -h
    sudo ./MediaServer -d -l 0 &
    sudo killall -2 MediaServer
    # RTSP pull
    ffplay -rtsp_transport tcp -i rtsp://127.0.0.1:554/live/0
    # user guid
    1. 叠加图像显示的开启和关闭
    由宏定义 #define DISP_OverLayImg 决定。
    2. 叠加图像的RTSP推流
    开启:--rtsp
    关闭:缺省--rtsp,默认关闭
    如果开启RTSP,必须先运行MediaServer以开启RTSP服务器再运行程序servoPixhawkDroneIBVS-test,否则servoPixhawkDroneIBVS-test程序会运行报错。
    默认推流地址(目前只支持在代码中修改):rtsp://127.0.0.1:554/live/0
    3. 二维码的尺寸
    --tag-size 0.12
    4. 飞机到二维码的期望距离
    默认为1
    --distance-to-tag 2
    5. 与飞控的连接方式
    --co udp://:14550
    6. 运行程序过程中显示冗余打印信息
    --verbose
    7. 默认图像处理频率为30fps
    目前只支持在代码中改动后重新编译。
    8. 相机的安装位置
    安转在飞机正下方,相机x轴与飞机机体y轴同向: --cambottom
    安转在飞机正左方,相机x轴与飞机机体x轴同向: 缺省--cambottom,默认关闭
    9. 开启、关闭视觉伺服、降落
    开启、关闭视觉伺服:左击图像,或者在命令行输入 0关闭,1开启
    关闭视觉伺服并降落:右击图像,或命令行输入7
    10. 运行顺序
    (1)在机载计算机上运行mavproxy:mavproxy.py --master=/dev/ttyACM0 --out=udpout:127.0.0.1:14550
    (2)在机载计算机上运行 RTSP server:cd ZLMediaKit/release/linux/Debug & sudo ./MediaServer -d -l 0 &
    (3)在机载计算机上运行servo-pixhawk app:./servoPixhawkDroneIBVS-test --tag-size 0.12 --co udp://:14550 --distance-to-tag 2 --verbose --rtsp --cambottom
    
  • 测试
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    请添加图片描述

  • 结论
    硬件方案:英伟达 jetson orin nano 机载计算机 +因特尔D455深度相机。
    软件方案:ViSP。
    通讯工具:orin nano 通过网线接入MK32天空端的局域网,PC通过网线或热点接入MK32地面端安卓遥控器的局域网,PC就可以通过SSH工具(如MobaXterm)连接到 orin nano 以控制IBVS程序的启停。
    测试目标:实现无人机对二维码的实时跟踪。
    测试结果:视觉伺服控制结果基本符合预期,基本能实现二维码的识别、定位和跟踪,测试过程中发现经常出现二维码识别失败的问题,初步推断可能是由于太阳光照太强、相机抖动频繁、二维码尺寸太小、识别算法参数不适当导致,这需要后续继续优化代码和实施更多的重复测试来解决这些问题。

21 visp_ros 软件包

代码:https://github.com/lagadic/visp_ros
在这里插入图片描述

21.1 一些概念

  • why?
    为什么要使用ros?直接使用传统的Visp程序实现IBVS视觉伺服不好吗?
    传统的Visp程序是一个以main函数为主(main中包含一个while死循环)的C++程序,可运行在Linux上。所有功能都要写在main函数中,包括图像帧获取、RTSP推流、IBVS控制量计算、发送pixhawk平台控制指令等,这样杂揉在一起,但凡某个环节出现异常都会导致整个进程的崩溃,不合理。
    传统的ViSP程序控制流如下图:
    在这里插入图片描述
    ros 是一个分布式的平台,ros可以将传统的ViSP程序进行分布式化,将各个子功能模块编译成一个个的ros节点,节点间可以很容易地通过主题的发布和订阅进行信息交换!某个节点的崩溃不会影响其他节点的功能,分布式的好处就是强大的信息共享机制和模块化的运行机制。例程参考这里:Using visp_ros to produce a ROS node.
    完全ros化的visp程序的控制流如下图:
    在这里插入图片描述
    当然,还有一种半ros化的视觉伺服实现方式,就是利用visp_ros 软件包中提供的visp_ros/vpROSGrabber.h可以让传统的ViSP程序与 ros 节点之间进行信息交换,注意此处的ViSP程序并不是一个ros节点!例程参考这里:How to grab images from ROS.
    该法的控制流如下图:
    在这里插入图片描述

  • 使用ROS的意义
    便于分布式部署由不同语言编写的、实现不同功能的节点程序。例如以python编写的视线GPIO控制的节点、以C++编写的IBVS节点、以C++编写的读取D455相机的节点、RTSP推流节点等等。

  • 包含的Visp类
    使用方式与ViSP classes类似。
    vpROSGrabber 用于获取图像帧
    vpROSRobot 用于控制机器人平台
    vpROSRobotPioneer
    vpROSRobotFrankaCoppeliasim

  • 包含的节点
    visp_ros 软件包包含了一些允许控制特定机器人平台的ros节点,平台如Pioneer mobile robot、Parrot bebop2 drone、pixhawk等…

  • 包含一些有用的demo

21.2 ViSP ROS package

ROS package的用法和解释请参看:ViSP ROS package

  • ViSP C++库的ROS package
    安装后就可以在ROS调用,根据版本来安装对应的包ros--visp,如:ros-<noetic>-visp
  • 其他ViSP相关的ROS package
    • vision_visp,如用于ViSP与ROS的数据转换等功能。注意vision_visp是一个元包,包含多个包。
    • visp_ros,如用于ViSP中的视觉伺服计算等功能。
    • demo_pioneer,Pioneer robot相关的功能等。

vision_visp 的编译和用法;
visp_ros 的编译和用法;
demo_pioneer 的编译和用法。

21.2 视觉伺服demo

  • 安装 ROS

  • 创建并初始化 catkin 工作空间

    cd ~/shd
    mkdir -p catkin-ws/src
    cd catkin-ws
    catkin_make
    echo "source ~/shd/catkin-ws/devel/setup.bash" >> ~/.bashrc
    source ~/.bashrc
    printenv | grep ROS
    
  • 编译 ViSP(安装ViSP库)
    从package.xml可知ViSP库是软件包vision_visp和visp_ros的依赖库,所以需要将ViSP源码编译成为库文件(xxx.so, xxx.h),并且需要将ViSP库的存放路径在环境变量VISP_DIR中指定。
    如果你在ViSP源码中做了某些代码改动,你就必须以编译源码的方式安装ViSP库才能应用这些改动。否则你可以使用apt的方式来安装ViSP的预编译库:apt-get install ros-noetic-visp,若如此,在后续的步骤中你就可以去掉这些命令参数:-DVISP_DIR=${VISP_DIR} --skip-keys=visp
    编译ViSP:

    cd ~/shd/visp-ws/visp-build
    cmake ../visp -DUSE_BLAS/LAPACK=GSL -DCMAKE_BUILD_TYPE=release -DBUILD_SHARED_LIBS=ON
    make -j4
    echo "export VISP_DIR=~/shd/visp-ws/visp-build" >> ~/.bashrc # 如果已存在则略过这步,用printenv | grep visp
    source ~/.bashrc
    

    以上编译命令执行完毕后ViSP库文件会被安装到~/shd/visp-ws/visp-build目录中!
    如果你想将ViSP库文件安装到ros包默认安装目录/opt/ros/noetic,请这样编译ViSP:

    cd ~/shd/visp-ws/visp-build
    cmake ../visp -DUSE_BLAS/LAPACK=GSL -DCMAKE_BUILD_TYPE=release -DCMAKE_INSTALL_PREFIX=/opt/ros/noetic #指定安装目录
    make -j4
    sudo make install
    
  • 编译 vision_visp
    从package.xml可知vision_visp是visp_ros的依赖,因此在编译visp_ros之前要先编译vision_visp。
    (1)下载vision_visp源码

    cd ~/shd/catkin-ws/src
    git clone https://github.com/lagadic/vision_visp.git -b $ROS_DISTRO
    

    (2)安装vision_visp的依赖
    vision_visp是一个元包其中包含的多个相关的软件包。catkin元包的编译和依赖安装和catkin普通软件包的方法类似。

    cd ~/shd/catkin-ws
    sudo rosdep init # 若之前执行过则跳过这步!
    rosdep update # 这个命令不要使用sudo
    rosdep install --from-paths src --ignore-src --rosdistro $ROS_DISTRO --skip-keys visp
    

    各个包的依赖在各自的 package.xml中 查看。
    其中--skip-keys=visp表示跳过安装官方预编译的ViSP库(官方预编译的ViSP库的包名为ros-noetic-visp),因为我们已经在前面步骤中通过编译自己的ViSP源码来生成ViSP库,只需要在后续的编译中是指明ViSP库的存放路径即可。--from-paths src表示先在指定的目录中查找依赖包,若没找到再从ros官网安装这些依赖包。
    (3)编译 vision_visp
    编译vision_visp元包中的所有软件包:

    cd ~/shd/catkin-ws
    catkin_make -DVISP_DIR=${VISP_DIR} 
    

    编译vision_visp元包中的某些软件包:

    cd ~/shd/catkin-ws
    catkin_make -DVISP_DIR=${VISP_DIR} --pkg visp_bridge visp_camera_calibration 
    

    注意1:因为ViSP库并没有安装到ros默认目录/opt/ros/noetic所以编译vision_visp时需要指定ViSP库的安装路径,如-DVISP_DIR=${VISP_DIR}
    注意2:由于版本问题,最新的ViSP源码可能与当前vision_visp元包中的某些包不兼容,因此编译vision_visp元包时建议只编译visp_bridge、visp_camera_calibration 这两个包,因为visp_ros的依赖也只是visp_bridge而已!!

  • 编译 visp_ros
    (1)下载visp_ros软件包源码
    下载到与vision_visp同一个catkin空间中。

    cd ~/shd/catkin-ws/src
    git clone https://github.com/lagadic/visp_ros.git -b $ROS_DISTRO
    

    (2)安装visp_ros的依赖
    其实可以跳过这一步,因为在用rosdep安装vision_visp的依赖后已经满足visp_ros的依赖要求。

    cd ~/shd/catkin-ws/
    rosdep update
    rosdep install --from-paths src --ignore-src --rosdistro $ROS_DISTRO  --skip-keys visp
    

    其中--skip-keys=visp表示跳过安装官方预编译的ViSP库,因为我们已经在前面步骤中通过编译自己的ViSP源码来生成ViSP库并安装在${VISP_DIR}目录中。--from-paths src表示先在指定的目录中查找依赖包,若没找到再从ros官网安装这些依赖包。
    (3)编译visp_ros

    cd ~/shd/catkin-ws
    catkin_make -DVISP_DIR=${VISP_DIR} --pkg visp_ros
    
  • 其他

    • 查看已安装的ViSP包:
      cd /opt/ros/noetic
      find . -name *visp*
      dpkg -l "*visp*"
      
    • 卸载通过rosdep install安装的官方预编译ViSP库:
      sudo apt-get --purge remove ros-noetic-visp
      
      visp的其他ros版本的包名格式为:ros-rosdistro-visp
  • 运行visp_ros中的节点

    source ~/shd/catkin-ws/devel/setup.bash
    roslaunch visp_ros bebop_servo_node.launch
    # 或:
    roslaunch visp_ros bebop_servo_nodelet.launch
    
  • 代码分析
    主要实现代码是:
    catkin-ws/src/visp_ros/tutorial/bebop2/visual_servo_node.cpp
    catkin-ws/src/visp_ros/tutorial/bebop2/visual_servo_nodelet.cpp
    其中visual_servo_nodelet.cpp以nodelet节点的形式启动!
    关于nodelet节点的用法请参考:
    https://index.ros.org/p/nodelet_tutorial_math/github-ros-common_tutorials/#noetic
    https://blog.csdn.net/qq_34493401/article/details/128890508

  • 参考文献
    https://visp-doc.inria.fr/doxygen/visp-daily/tutorial-bridge-ros.html
    https://wiki.ros.org/vision_visp
    https://wiki.ros.org/visp_ros#Controlling_a_Parrot_Bebop2_drone_by_Visual-Servoing
    https://wiki.ros.org/visp_ros/Tutorials/Howto_install_visp_ros
    How to do visual servoing with Parrot Bebop 2 drone using visp_ros
    https://github.com/lagadic/visp_ros/blob/rolling/README.md
    https://github.com/lagadic/vision_visp/blob/rolling/visp_bridge/README.md

bebop_autonomy 包分析:

package.xml注释:
<!-- build_depend version_gte="3.12.6">parrot_arsdk</build_depend -->
安装依赖:
rosdep update
rosdep install --from-paths src -i  --rosdistro $ROS_DISTRO  --skip-keys visp
编译:
visp_ros只用到了bebop_driver包,因此只需要编译bebop_driver包
catkin_make -DVISP_DIR=${VISP_DIR} --pkg bebop_driver
需要运行bebop_driver包的哪些节点?
src/bebop_autonomy/bebop_driver/launch/bebop_nodelet.launch
节点加载过程:
(1)继承Nodelet的类会被编译成为一个Nodelet插件(实际上是一个动态库)。
(2)class BebopDriverNodelet : public nodelet::Nodelet  
可知 BebopDriverNodelet类会被编译成一个Nodelet插件(动态库)。
(3)在BebopDriverNodelet::onInit()中写你的项目逻辑。
(4)先运行一个nodelet管理器。
<node pkg="nodelet" type="nodelet" name="bebop_nodelet_manager" args="manager" output="screen"/> 
(5)通过nodelet管理器启动一个nodelet节点,并让该节点加载一个名为BebopDriverNodelet的nodelet插件。
    <node pkg="nodelet" type="nodelet" name="bebop_nodelet"
      args="load bebop_driver/BebopDriverNodelet bebop_nodelet_manager"> 
    </node>

查看geometry_msgs/Twist消息:rosmsg show geometry_msgs/Twist
cd ~/shd/catkin-ws/src
catkin_create_pkg ardupilot_driver std_msgs rospy roscpp
cd ~/shd/catkin-ws/
catkin_make -DVISP_DIR=${VISP_DIR} --pkg ardupilot_driver
创建一个读取D455的节点:
待续…




在这里插入图片描述

点赞、收藏、关注哟!
  • 10
    点赞
  • 13
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值