借助px4飞控日志简便估计SLAM位姿时延操作方法

sunflower2018

我们基于SLAM进行无人机的定点飞行时，如果想保证好的定点飞行效果，往往还需要对SLAM位姿的时延进行一个评估。如果SLAM位姿延时偏大(如超过500毫秒甚至达到1秒)，可能会造成飞控控制的紊乱，无人机是个高实时的系统，对实时性要求较高，如果不能及时得到准确的位姿反馈，飞控的闭环控制可能就难以达到有效的效果，无人机定点时表现为容易晃动，甚至控制不住飘走。我们理想中的无人机定点应该是纹丝不动的。

SLAM位姿时延主要是因为SLAM计算耗时造成的，SLAM计算出某一图像帧的位姿，可能实际是此图像帧的时间的几百毫秒以后了，此时位姿再通过/mavros/vision_pose/pose话题传给飞控，飞控得到的实际是几百毫秒前的位姿。

对SLAM位姿时延估计评估是我们飞好无人机的SLAM定点的前提，如果时延较大，造成无人机定点效果不好，我们需要优化SLAM的计算耗时，可以调整参数，优化程序代码或者更换更好的计算平台的方式实现。

对于SLAM位姿时延的估计，我们一种可以结合动捕，我们可以认为动捕的的位姿时延很小，主要是传输延时，在板载端同时录制SLAM位姿话题和动捕位姿话题，然后可以通过比如matlab绘制出轨迹曲线，把两者的轨迹进行对齐(比如基于拐弯处位姿曲线的顶点对齐)，对齐后对比相同位置时两个位姿的时间戳的差值可以认为是SLAM位姿的时延。这种方式相对严格，但是操作可能比较麻烦，一个是动捕设备比较昂贵，同时matlab数据处理也需要一定基础。

这里介绍一个相对简便一点的SLAM位姿时延估计方法，通过飞控日志来进行SLAM位姿时延的估计(此方法主要适用于带imu
的SLAM如视觉惯性SLAM或者激光惯性SLAM以及视觉激光惯性SLAM，主要是imu可以使得SLAM位姿里的横滚俯仰有较为准确的估计，不会出现什么累计漂移)。

需要先确认把飞控参数中的SDLOG_PROFILE设为131了，也就是选择第0 1和第7比特位置1 关键是保证第7比特位置1，这样会保证把传入飞控的视觉位姿也记录到飞控日志里，SDLOG_PROFILE每个比特位含义如下图所示
https://docs.px4.io/main/zh/advanced_config/parameter_reference.html#SDLOG_PROFILE



飞控日志会记录下视觉位姿对应的uORB消息，和飞控基于自身imu得到的姿态对应的uORB消息，飞控基于自身imu得到的姿态主要是用的横滚俯仰，偏航靠磁力计或者视觉位姿偏航进行修正。由于视觉惯性SLAM也会用imu所以其估计的位姿的横滚和俯仰是可观量，所以也是相对可靠，可以认为和真值是接近的，不会发生大的累计漂移，飞控内部基于imu的加速度计和陀螺仪融合估计的横滚俯仰也同理，SLAM的偏航维度虽然不可观，但是由于飞控的偏航就是用的视觉偏航，所以两者的横滚俯仰偏航基本是一致的，就算视觉位姿的偏航有累计漂移，两者由于偏航相同，横滚俯仰也是基本正确无累计漂移的，所以两者的位姿四元数实际依旧基本一致，偏航层面的累计漂移在这里没什么影响，总之某种程度上可以认为SLAM位姿的四元数和飞控自身基于imu以及视觉偏航得到的姿态四元数，在同一时刻应该基本一致，特别是横滚俯仰应该基本接近于真值，基本不存在xyz层面SLAM估计时存在的累计漂移的问题，这也是为什么用姿态四元数，不用xyz做对比，那么我们可以对比日志中视觉位姿四元数和飞控自身姿态四元数的变化曲线，两者曲线应该基本一致，但是会存在一个固定时间偏差，两者的时间戳差值就可以认为是视觉位姿的时延(这里认为飞控内部基于imu和视觉偏航估计出的姿态的计算耗时和SLAM计算耗时相比较小，故忽略)。

具体操作如下，我们可以用flightplot打开基于SLAM定点飞行的PX4日志，选择日志中的vehicle_attitude_0.q[0]和vehicle_visual_odometry_0.q[0]这两项参数，进行对比，vehicle_attitude_0对应飞控基于自身imu及视觉偏航估计出的姿态四元数，vehicle_visual_odometry_0对应SLAM位姿中的四元数，q[0]是对应四元数中的第一位数字w（The quaternion uses the Hamilton convention, and the order is q(w, x, y, z)）。

为什么选择这两个日志参数进行显示呢，这里做下说明，PX4中所有日志均记录的是uORB消息里的数据，具体有哪些uORB消息呢，我们可以到 https://docs.px4.io/main/zh/msg_docs/ 这里进行查看，这里列举了所有的uORB消息类型，当然会有多个uORB消息是同一msg类型，但是名称不同，这在每个msg类型里面有列出来。

比如这是VehicleOdometry这个uORB消息类型的内容，它最后的注释有写VehicleOdometry这个类型对应的uORB消息名称，有vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry estimator_odometry。 我们这次用的就是日志中记录的vehicle_visual_odometry这个uORB消息，对应SLAM位姿。
https://docs.px4.io/main/zh/msg_docs/VehicleOdometry.html
ounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(line# Vehicle odometry data. Fits ROS REP 147 for aerial vehicles
uint32 MESSAGE_VERSION = 0
uint64 timestamp                # time since system start (microseconds)uint64 timestamp_sample
uint8 POSE_FRAME_UNKNOWN = 0uint8 POSE_FRAME_NED     = 1 # NED earth-fixed frameuint8 POSE_FRAME_FRD     = 2 # FRD world-fixed frame, arbitrary heading referenceuint8 pose_frame            # Position and orientation frame of reference
float32[3] position         # Position in meters. Frame of reference defined by local_frame. NaN if invalid/unknownfloat32[4] q                # Quaternion rotation from FRD body frame to reference frame. First value NaN if invalid/unknown
uint8 VELOCITY_FRAME_UNKNOWN  = 0uint8 VELOCITY_FRAME_NED      = 1 # NED earth-fixed frameuint8 VELOCITY_FRAME_FRD      = 2 # FRD world-fixed frame, arbitrary heading referenceuint8 VELOCITY_FRAME_BODY_FRD = 3 # FRD body-fixed frameuint8 velocity_frame        # Reference frame of the velocity data
float32[3] velocity         # Velocity in meters/sec. Frame of reference defined by velocity_frame variable. NaN if invalid/unknown
float32[3] angular_velocity # Angular velocity in body-fixed frame (rad/s). NaN if invalid/unknown
float32[3] position_variancefloat32[3] orientation_variancefloat32[3] velocity_variance
uint8 reset_counterint8 quality
# TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry# TOPICS estimator_odometry
这是VehicleAttitude这个uORB消息类型的内容，对应的uORB消息有vehicle_attitude vehicle_attitude_groundtruth external_ins_attitude estimator_attitude
我们这次对比用到了日志中记录的vehicle_attitude这个uORB消息，对应飞控基于自身imu及视觉偏航估计出的姿态四元数。
https://docs.px4.io/main/zh/msg_docs/VehicleAttitude.html
ounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(lineounter(line# This is similar to the mavlink message ATTITUDE_QUATERNION, but for onboard use# The quaternion uses the Hamilton convention, and the order is q(w, x, y, z)
uint32 MESSAGE_VERSION = 0
uint64 timestamp                # time since system start (microseconds)
uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
float32[4] q                    # Quaternion rotation from the FRD body frame to the NED earth framefloat32[4] delta_q_reset        # Amount by which quaternion has changed during last resetuint8 quat_reset_counter        # Quaternion reset counter
# TOPICS vehicle_attitude vehicle_attitude_groundtruth external_ins_attitude# TOPICS estimator_attitude
用flightplot显示日志中这个两个参数后，可以看到两个参数的曲线基本是重合的，和我们前面分析的一致，没有说出现两个参数出现比较大的累计漂移的情况，也应该不会出现。
而且两个参数的四元数对应的坐标系，参考上面对应消息文件里的说明，vehicle_attitude里的四元数是对应FRD坐标系在NED坐标系下的姿态，vehicle_visual_odometry里的呢 虽然它写的是FRD坐标系在参考坐标系下的姿态，这里的参考坐标系呢我们传视觉位姿时一般没有指定，vehicle_visual_odometry这个uORB消息里的pose_frame这一项的值实际也是0，实际默认一般也是NED(飞控端) ，也就是飞控端的视觉位姿也是FRD在NED坐标系下的位姿(可以参考这篇文章 https://mp.weixin.qq.com/s/12fLFqkDi1FVMVQ45mU4Ug )，所以vehicle_visual_odometry里的四元数是可以直接和vehicle_attitude里的四元数进行对比的，不用做额外的坐标系转换。



进一步放大可以看到vehicle_visual_odometry_0.q[0]的绿色曲线和vehicle_attitude_0.q[0]的红色曲线相差的一个相对固定的时间差



为了看清具体时延，可以进一步放大，对比时间戳差值，如下图所示，可以看到横轴的时间相差大概在200ms左右，可以粗略认为是SLAM位姿的时延是200ms左右，如果我们飞PX4的SLAM定位时，想修改PX4飞控中的EKF2_EV_DELAY参数，使得它更为准确，也可以基于此进行填写，EKF2_EV_DELAY参数含义是Vision Position Estimator delay relative to IMU measurements，它的可修改范围是0-300ms。