15-无人机视觉自主飞行:VINS+Fast-Planner

转载自:https://zhuanlan.zhihu.com/p/257236655?utm_source=wechat_session

15-无人机视觉自主飞行:VINS+Fast-Planner

张碧辉

张碧辉

飞行器设计,马拉松

0.对港科大Fast-Planner项目的复现:

https://github.com/HKUST-Aerial-Robotics/Fast-Planner​

github.com

 

1.硬件:

淘宝买的四旋翼以及Pixhawk飞控板,自己订做了碳板用来安装Upboard和Realsense:

与飞机一起购买的这种老款的Pixhawk质量差,并且没有电源保护功能:

后来买的新版本Pixhawk更好一些,但是仍不理想。

与飞机一起购买的廉价螺旋桨应该是没有做动平衡方面的品控,使用时会造成飞机严重的震动,进而导致Realsense的内置IMU数据存在严重噪声。但是在相同的减震措施下,飞控的IMU却能正常工作,这说明Realsense这一类相机产品并不是为了飞机这种高频震动平台制造的。另外手里有小觅双目相机,但是它的数据线实在是愚蠢而笨重,就直接放弃了。

2.Palnner指令的执行:

Fast-Planner给出的航迹指令包含位置、速度、加速度以及Yaw,港科大用的飞控是大疆的,但是我在大疆Onboard SDK的网站上只找到了位置控制指令。所以很想知道Boyu Zhou大佬是怎么实现的……

遂改用亲切的MavROS+Offboard模式,在仿真环境下试了下"/mavros/setpoint_accel/accel"以及"mavros/setpoint_raw/local"中的加速度项,效果很差。无奈只能用位置("mavros/setpoint_raw/local")和速度("/mavros/setpoint_velocity/cmd_vel_unstamped")控制了,这就导致忽略了Fast-Planner给出的加速度指令,理论上飞行效果肯定无法达到标准水平。

大佬在仿真程序里开源的控制逻辑是:无人机实际位置、速度、加速度值与Planner理论位置、速度、加速度值之差,分别乘以各项的修正系数,然后求和并且记入重力,作为无人机推力指令输出给飞控。

我的控制逻辑是:无人机实际位置、速度与Planner理论位置、速度之差,分别乘以各项的修正系数,然后求和作为无人机速度指令输出给飞控。

另外大佬有这么一段代码,经过验证其作用是确保飞行指令方向与竖直方向不超过45度。我很想知道这段代码所依据的理论出自何处……

// Limit control angle to 45 degree
double theta = M_PI / 2;
double c = cos(theta);
Eigen::Vector3d f;
f.noalias() = kx.asDiagonal() * (des_pos - pos_) + kv.asDiagonal() * (des_vel - vel_) + //
mass_ * des_acc + mass_ * ka.asDiagonal() * (des_acc - acc_);
if (Eigen::Vector3d(0, 0, 1).dot(force_ / force_.norm()) < c)
{
double nf = f.norm();
double A = c * c * nf * nf - f(2) * f(2);
double B = 2 * (c * c - 1) * f(2) * mass_ * g_;
double C = (c * c - 1) * mass_ * mass_ * g_ * g_;
double s = (-B + sqrt(B * B - 4 * A * C)) / (2 * A);
force_.noalias() = s * f + mass_ * g_ * Eigen::Vector3d(0, 0, 1);
}

3.坐标系:

PX4飞控默认的惯性坐标系方向是“北东下”,Offboard指令中的local坐标系方向是“东北上”,Offboard指令中的setpoint_velocity指令是相对于机体坐标系的。

在视觉惯性导航坐标系下:使用Realsense时,VINS初始化以后默认相机前方是朝向Y轴的;Fast-Planner输出的航迹指令在初始时刻总是朝向X轴的。我简单粗暴地把后者的初始角度加了90度,这样飞机每次初始时刻都会直接向前飞。

4.gazebo仿真:

仿真设置有时间可以单独写一篇,此处略过。

fastplanner-vins-realsense-gazebo

5.四旋翼飞行试验::

四旋翼轴距是550,桨是14寸,障碍物是足球训练用的绕桩标志杆,高度2.4m,直径2.5cm。地图膨胀值设为0.6m,Planner最大速度0.5m/s。试验场地为室外,1-2级风。

fastplanner-vins-realsense-pix4-post

6.存在问题:

飞控存在偶尔不能解锁的状况(MavROS与遥控器均不能),原因未知。

Offborad位置控制模式下,单纯依靠飞控加速度计进行积分得到的位置准确性差、很不稳定。

realsense偶尔存在不能检测到障碍物深度的情况,导致飞机撞杆。

 

编辑于 2020-09-22

已标记关键词 清除标记
相关推荐
©️2020 CSDN 皮肤主题: 编程工作室 设计师:CSDN官方博客 返回首页