我拼命加速,但贫穷始终快我一步
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
其实这一步可以省略,因为通过网线接入MK32天空端后 Jetson Orin Nano 会自动获得一个192.168.144.x
的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仿真:
其中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就会出现编译报错!!
解决方式:
修改后的 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空间坐标旋转矩阵得到,如:
对于单应矩阵的估计请参考:这里或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
-