ROS与AR.Drone-tum_ardrone-drone_stateestimation(状态评估)

drone_stateestimation(状态评估)

说明

• 基于PTAM,navdata,控制命令来评估飞行器的状态。

注意

• 要求信息发送在 /ardrone/navdata (>100Hz) 和/ardrone/image_raw (>10Hz)
• 在运行ardrone_autonomy节点或回放 .bag 最起码有两个参数，运行ardrone_autonomy如：

rosrun ardrone_autonomy ardrone_driver _navdata_demo:=0 _loop_rate:=500

订阅的主题

• /ardrone/navdata
• /ardrone/image_raw
• /cmd_vel
• /tum_ardrone/com

已发布的主题

• /ardrone/predictedPose
• /tum_ardrone/com

参数

• PTAM，全称，Parallel Tracking and Mapping

• EKF, 全称ExtendedKalmanFilter，即扩展卡尔曼滤波器

• ~publishFreq: 频率，无人机估计位置计算和公布的频率。默认值：30hz

• ~calibFile: 相机校准文件，如没设置，默认使用 (camcalib/ardroneX_default.txt).

• UseControlGains: 是否使用 EKF 预测.

• UsePTAM: 是否使用PTAM姿态评估更新EKF

• UseNavdata:是否使用Navdata更新EKF

• UsePTAM 和UseNavdata 设置为Flase， EKF 不会更新作为一个纯仿真 ，基于控制命令预测(在/cmd_vel).

• PTAMMapLock: 锁定PTAM地图(不再KF)

• PTAMSyncLock: 锁定PTAM地图同步(修复范围或姿态偏移等.)

• PTAMMaxKF: KF PTAM的最大值

• PTAMMinKFDist: 最小的距离在两个KF之间（米位单位）

• PTAMMinKFWiggleDist: 最小的距离在两个KF之间 (相对于预定场景深度).

• PTAMMinKFTimeDiff: 两个KF间最小时间

  PTAM takes a new KF if (PTAMMinKFTimeDiff AND (PTAMMinKFDist OR PTAMMinKFWiggleDist)), and tracking is good etc.

• RescaleFixOrigin: If the scale of the Map is reestimated, only one point in the mapping PTAM <-> World remains fixed. If RescaleFixOrigin == false, this is the current pos. of the drone (to avoid sudden, large "jumps"). this however makes the map "drift". If RescaleFixOrigin == true, by default this is the initialization point where the second KF has been taken (drone pos. may jump suddenly, but map remains fixed.). The fixpoint may be set by the command "lockScaleFP".

• c1 ... c8: EKF的预测模型参数 see "Camera-Based Navigation of a Low-Cost Quadrocopter"

使用

• 如果需要正确评估PTAM的边界，最好是在初始化之后起飞和降落一下，（比如，飞起1m，降落1m）
• 出现两个窗口，一个视频窗口，显示视频和PTAM的地图点，一个是地图窗口。
• 发布一个key命令, 关注各自的窗口，点击一个Key. 这样生成一个命令发送到/tum_ardrone/com

Video Window:

如果在飞行中，点击视频窗口会生产一个航点，发送给飞行器。

• 左点击，飞 (x,y,0) 米相对目前的位置，图像的中心是(0,0), 边界分别为2米.
• 右点击，飞（0,0，y）米和旋转X度偏航，图像的中心是（0,0，边界分别为2米和90度

Map Window: