我拼命加速,但贫穷始终快我一步
- 0~19
- 20. visp-d455:基于IBVS的Pixhawk无人机视觉伺服
- 20.1 基础
- 关于连接、通讯、UDP forward服务:
- 一些相关的、有用的例程
- Linux C++程序的gdb断点调试
- 搭建仿真
- 解决【testPixhawkDroneTakeoff.cpp例程能解锁但起飞命令无响应,断点模式下却有响应】问题
- 解决【使用最新的mavsdk版本导致visp编译报错】问题
- visp中的异步编程:std::promise和std::future
- 20.2 视觉伺服
- 知识储备
- 分析与实践
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仿真:
其中127.0.0.1:14550可用连接windows端的mission planner,便于查看飞行状态。sim_vehicle.py -v ArduCopter --console --out "192.168.100.197:14550" --out "127.0.0.1:14550" -w
- 4)在Jetson Orin Nano上启动MavProxy
注意:MavProxy生成的缓存文件mav.parm, mav.tlog ,mav.tlog.raw 要定时清理,这些文件保存在运行命令时的所在目录!不然会占用机载计算机的内存空间!mavproxy.py --master=udp:192.168.100.197:14550 --out=udpout:127.0.0.1:14552
- 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);
- 1)先使用setPositioningIncertitude()设置定位和偏航的不确定性参数,如:
解决【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空间坐标旋转矩阵得到,如:
对于单应矩阵的估计请参考:这里或Tutorial: Homography estimation from pointsvpRxyzVector 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图像(平面)坐标旋转矩阵,即单应矩阵
- 图像矩(Image Moments)
-
源自:数学学科中随机变量的矩
-
领域:数字图像处理、计算机视觉、深度学习
-
对象:二值图像、灰度图像
-
概念:图像的矩可以概括为图像的某些像素点的灰度值(像素值,在灰度图中像素值即灰度值)的加权平均值,或者是图像具有类似功能或意义的属性的加权平均值。
意义:可以通过图像的矩来获得图像的一些有用的性质,包括面积、轮廓、几何中心(重心、质心)、方向(特征向量、主轴)等信息 ,这些信息对于图像识别、形状匹配和目标跟踪等应用非常有用。 -
分类:
- 原点矩
(p+q)阶原点矩:
- 中心距
(p+q)阶原点矩:
- 中心归一化距
(p+q)阶中心归一化距:
- 原点矩
-
矩的应用:
- 目标区域的面积 = 目标区域的像素个数 = m00
与零阶混合原点矩有关! - 目标区域的质心坐标 =【m01/m00, m10/00】
与 一阶混合原点矩有关! - 目标区域的方向
与二阶混合中心距有关!
主轴斜率 = mu20/mu11
副轴斜率 = mu11/mu20
- 目标区域的面积 = 目标区域的像素个数 = m00
-
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