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：板载计算机、机载计算机

软件：

• 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。

一些相关的、有用的例程

• 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就会出现编译报错！！
解决方式：

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

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

20.2 视觉伺服

知识储备

• 坐标变换矩阵 = 坐标旋转矩阵+坐标平移矩阵
  Rotation Matrix
  本质：坐标变换本质上是一种投影变换，是一种投影关系！从系a转换到系b即把系a中的坐标投影到系b.
  应用：2维，3维，n维
  举例：Xb = Tr * Xa + Tt * 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/

分析与实践

• 代码：servoPixhawkDroneIBVS.cpp
  根据相机的位置来调整位置转换矩阵，例如：
  FLU body frame 和 相机坐标系之间的转换矩阵：
  
  飞机机体坐标系，记作e，即终端坐标系：FLU
  最终的控制量：机体速度，也是要转换到FLU坐标系下。
  相机坐标，记作c
  中间坐标系：原点与c重合，轴与e同向，记作c1
  那么，c1与c的转换矩阵是由自己的安装位置决定的，是已知的。
  e跟c1的转换矩阵：只有位移关系，也是已知的。
  上面的转换矩阵最终求得e到c的转换关系矩阵：cVe
  
  Xn：重心矩视觉特征的x分量误差！！！！！
  Yn：重心矩视觉特征的y分量误差！！！！！
  an：目标区域面积视觉特征误差！！！！！
  atan(1/rho)")：灭点视觉特征误差！！！！！
  
  待续…

运行

cd ~/shd/visp-ws/visp-build/example/servo-pixhawk
./servoPixhawkDroneIBVS --tag-size 0.12 --co udp://:14550

在代码中引用外部库xxxx.h xxxx.so
CMakeLists.txt中：

pkg-config --cflags --libs gstreamer-1.0

等待解决的问题：

• cVe坐标系的问题，为什么是相机系到FLU的而不是飞机机体系FRD？为什么视觉伺服计算得到的ve是FRD下表示的？
• 推流如显示检测结果和误差曲线？
• 二维码颠倒之后伺服控制量是否正常？？？
  与摆放位置无关！伺服控制量都是正常的！以第一次识别到二维码时的二维码姿态来计算初始灭点水平线！！！！！
• 相机坐标系，图像坐标系的轴向？视觉伺服控制量坐标系的轴向？？关系？？？
• 由guided模式切换到其他模式如loiter和atlHold为什么会自动下降高度直到降落？？？？
• 如何将鼠标点击事件转换成命令行输入的形式？左击开始，再左击停止，右击停止并降落！！
  
  
  
  一些疑问：
  位置估计和姿态估计是基于哪个坐标系下的？
  地固坐标系：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