Quadcopter 小冬搞开发 ROS

[小冬搞开发]-基础篇-飞控开发系列(四)

小冬搞机之数据处理与源码赏析

Posted by Lordon on March 8, 2020

0.关于本文

上一篇中使用blender对m100结构等进行了精确设计,各个零件的导出文件以及设计工程已经放在了Github,算是整合了好几个工程中有用的部分,后面会跟同伴创建Organizations将工程一起放进去Github.

1.数据处理

所需采集数据类型

  • 0324网络输入数据： 维度：9维
    位置误差pos_err.x, pos_err.y, pos_err.z
    速度误差vel_err.x, vel_err.y, vel_err.z
    欧拉角误差eluer_err.x, eluer_err.y, eluer_err.z
  • 网络输出数据： 维度：4维
    四个电机的角速度rotor_0_vel, rotor_1_vel, rotor_2_vel, rotor_3_vel
  • 0331更新：
总结一下流程

odom+trajectony->lee_position_control->mottor_speed(angle)(vx,vy,vz)

保存ros话题数据

1 - /src同目录下创建/bagfile,并cd进入
2 - 启动launch文件同时保存所有话题数据

1
2
3

$ rosbag record -a  + $ roslaunch rotors_gazebo neu_m100_start.launch
	或者只保存部分
$ rosbag record /NEU_m100/imu /NEU_m100/gps /NEU_m100/motor_speed

3 - 查看数据包属性

1
2
3
4
5
6
7
8
9
10
11
12

$ rosbag info 2020-03-07-09-16-24.bag

dynamic_reconfigure/Config              [958f16a05573709014982821e6822580]
dynamic_reconfigure/ConfigDescription   [757ce9d44ba8ddd801bb30bc456f946f]
gazebo_msgs/LinkStates                  [48c080191eb15c41858319b4d8a609c2]
gazebo_msgs/ModelStates                 [48c080191eb15c41858319b4d8a609c2]
geometry_msgs/PointStamped              [c63aecb41bfdfd6b7e1fac37c7cbe7bf]
geometry_msgs/Pose                      [e45d45a5a1ce597b249e23fb30fc871f]
geometry_msgs/PoseWithCovarianceStamped [953b798c0f514ff060a53a3498ce6246]
geometry_msgs/TransformStamped          [b5764a33bfeb3588febc2682852579b0]
geometry_msgs/TwistStamped              [98d34b0043a2093cf9d9345ab6eef12e]
......

4 - 转换成txt保存[这里使用脚本solve_data.sh]

1

$ rostopic echo -b 2020-03-07-09-16-24.bag -p geometry_msgs/Pose > m100_data.txt

试想如果执行这条指令多快?这样就能保存多个话题数据了耶

1

$ rostopic echo -b 2020-03-07-09-16-24.bag -p geometry_msgs/Pose geometry_msgs/PointStamped ... > m100_data.txt
you may only specify one input topic

因为$rostopic echo..这指令一次只能对一个话题数据进行处理,我这十多个话题每次都这么搞岂不是麻烦死?

shell脚本处理

1 - 目录下创建sove_data.sh,使用vscode编写

1

touch sove_data.sh

2 - ctrl+shift+c调出命令行窗口运行之前要改权限

1

$ chmod +x sove_data.sh

3 - 运行

1

$ ./sove_data.sh

CODE solve_data.sh:

1
2
3
4
5
6
7
8
9
10

#! /bin/bash
ls -a
echo -e "Pls input the name of bag : \c "
read filename
echo output /NEU_m100/gps
rostopic echo -b $filename -p /NEU_m100/gps > $PWD/data/gps.txt
echo output /NEU_m100/imu
rostopic echo -b $filename -p /NEU_m100/imu > $PWD/data/imu.txt
......

txt数据处理方法

python

Ros保存的话题数据从.bag中提取出来之后所有数据均用逗号分隔开,所以这里关键用到readlines和split语句.一个是逐行读入数据为列表,一个是按指定方式将列表元素进行拆分成新的元素.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33

class Trajectory():
    def __init__(self,
        trajectory_fileName      = 'trajectory.txt',
        trajectory_time          = [],
        trajectory_seq           = [],
        trajectory_stamp         = [],
        trajectory_translation_x = [],
        trajectory_translation_y = [],
        trajectory_translation_z = []):

        self.trajectory_fileName      = trajectory_fileName
        self.trajectory_time          = trajectory_time   # 时间
        self.trajectory_seq           = trajectory_seq
        self.trajectory_stamp         = trajectory_stamp # 时间戳
        self.trajectory_translation_x = trajectory_translation_x  # 轨迹变换x
        self.trajectory_translation_y = trajectory_translation_y  # 轨迹变换y
        self.trajectory_translation_z = trajectory_translation_z  # 轨迹变换z

    def loadTrajectoryDataSet(self, trajectory_fileName):
        with open(trajectory_fileName, 'r') as f:
            data = f.readlines()  # txt中所有字符串读入data sum=14814

            for line in data:
                trajectory = line.strip().split(',')  # 将数据拆分易存储

                self.trajectory_time.append(trajectory[0])
                self.trajectory_seq.append(trajectory[1])
                self.trajectory_stamp.append(trajectory[2])
                self.trajectory_translation_x.append(trajectory[5])
                self.trajectory_translation_y.append(trajectory[6])
                self.trajectory_translation_z.append(trajectory[7])
        return 0

这里trajectory_变量定义太多,重复性复制粘贴特别麻烦,使不使用init初始化在后面确实没啥大的差别,但是为了规范代码,还是费力少点bug.

cpp

从十四讲例程中扣一段,做个记录.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

int main(int argc, char **argv) {
 string trajectory_file = 'path';
 ifstream fin(trajectory_file);
  if (!fin) {
    cout << "cannot find trajectory file at " << trajectory_file << endl;
    return 1;
  }

  while (!fin.eof()) {
    double time, tx, ty, tz, qx, qy, qz, qw;
    // 依次读入
    fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
    Eigen::Isometry3d Twr;
    Twr(Quaterniond(qw, qx, qy, qz));
    Twr.pretranslate(Vector3d(tx, ty, tz));
    poses.push_back(Twr);
  }
  cout << "read total " << poses.size() << " pose entries" << endl;
}

2.源码分析

rqt_graph 显示话题关系

CrazyS PositionController.cpp 分析

结构体构造函数鉴赏

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

struct EigenOdometry {
  EigenOdometry() 
      : position(0.0, 0.0, 0.0),
        orientation(Eigen::Quaterniond::Identity()),
        velocity(0.0, 0.0, 0.0),
        angular_velocity(0.0, 0.0, 0.0) {};
  
  EigenOdometry(const Eigen::Vector3d& _position,
                const Eigen::Quaterniond& _orientation,
                const Eigen::Vector3d& _velocity,
                const Eigen::Vector3d& _angular_velocity) {
    position = _position;
    orientation = _orientation;
    velocity = _velocity;
    angular_velocity = _angular_velocity;

  Eigen::Vector3d position;
  Eigen::Quaterniond orientation;
  Eigen::Vector3d velocity; 
  Eigen::Vector3d angular_velocity;
  };

初始化结构体,并设默认值

1

EigenOdometry odometry_;

使用话题数据给里程计赋值

1
2
3
4
5
6
7

inline void eigenOdometryFromMsg(const nav_msgs::OdometryConstPtr& msg,
                                 EigenOdometry* odometry) {
  odometry->position = mav_msgs::vector3FromPointMsg(msg->pose.pose.position);
  odometry->orientation = mav_msgs::quaternionFromMsg(msg->pose.pose.orientation);
  odometry->velocity = mav_msgs::vector3FromMsg(msg->twist.twist.linear);
  odometry->angular_velocity = mav_msgs::vector3FromMsg(msg->twist.twist.angular);
}

常量构造函数调用,不改变当前的里程计数值

1
2
3
4
5
6
7

void PositionController::SetOdometryWithoutStateEstimator(const EigenOdometry& odometry) {
    
    odometry_ = odometry; 

    // Such function is invoked when the ideal odometry sensor is employed
    SetSensorData();
}

RotorS 飞控电机控制

通过机载odom以及设定的目标点位置与目标速度计算加速度

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40

void LeePositionController::ComputeDesiredAngularAcc(const Eigen::Vector3d& acceleration,
                                                     Eigen::Vector3d* angular_acceleration) const {//作为常数函数 不可修改类中变量
  assert(angular_acceleration);//无则报错
  // 当前姿态4×1->三维旋转矩阵3×3  https://stackoverflow.com/questions/36501262/quaternion-to-rotation-matrix-incorrect-values-using-eigen-library/36502963#36502963
  // tf::Matrix3x3 m(q);相同  得到旋转矩阵 前者用tf类计算
  Eigen::Matrix3d R = odometry_.orientation.toRotationMatrix();
  // Get the desired rotation matrix.
  Eigen::Vector3d b1_des;
  // 偏航  yaw=0
  double yaw = command_trajectory_.getYaw();
  b1_des << cos(yaw), sin(yaw), 0;
  Eigen::Vector3d b3_des;
  // (44)
  b3_des = -acceleration / acceleration.norm();//2范数
  Eigen::Vector3d b2_des;
  // (43) 向量 |A||B|sim()  Return the cross product between this and another vector
  b2_des = b3_des.cross(b1_des);//
  b2_des.normalize();
  // Rc 3×1 列向量
  Eigen::Matrix3d R_des;
  R_des.col(0) = b1_des;
  // R_des.col(0) = b2_des.cross(b3_des);//不是b1_des?
  R_des.col(1) = b2_des;
  R_des.col(2) = b3_des;
  // (42) e_R
  Eigen::Matrix3d angle_error_matrix = 0.5 * (R_des.transpose() * R - R.transpose() * R_des);
  Eigen::Vector3d angle_error;
  vectorFromSkewMatrix(angle_error_matrix, &angle_error);
  Eigen::Vector3d angular_rate_des(Eigen::Vector3d::Zero());
  angular_rate_des[2] = command_trajectory_.getYawRate();
  // (42)e_omiga
  Eigen::Vector3d angular_rate_error = odometry_.angular_velocity - R.transpose() * R_des * angular_rate_des;
  // Eigen::Vector3d angular_rate_error = odometry_.angular_velocity - R_des.transpose() * R * angular_rate_des;
  
  // (41)M = 转动惯量*angular_acceleration
  // odometry_.angular_velocity.cross(odometry_.angular_velocity)=0
  *angular_acceleration = -1 * angle_error.cwiseProduct(normalized_attitude_gain_)
                           - angular_rate_error.cwiseProduct(normalized_angular_rate_gain_)
                           + odometry_.angular_velocity.cross(odometry_.angular_velocity); // no J,we don't need the inertia matrix here
}

通过计算得到的加速度计算角加速度-参考

1
2
3

void LeePositionController::ComputeDesiredAngularAcc(const Eigen::Vector3d& acceleration,
        Eigen::Vector3d* angular_acceleration)

由角加速度+计算的推力转化成四个电机转速发布到话题 motor_speed

1
2

void LeePositionController::CalculateRotorVelocities(Eigen::VectorXd* rotor_velocities)

n.附件

东大地理参数

1
2
3
4
5
6
7
8
9
10

https://www.ngdc.noaa.gov/geomag/calculators/magcalc.shtml#igrfwmm
东北大学
gps_update_freq="5.0"
lat:41° 45' 57"  lon:123° 25' 30" 
latitude=41.765833 
longitude=73.984000 
alt=50
reference_magnetic_field_north="0.26764"
reference_magnetic_field_east="-0.04413"
reference_magnetic_field_down="0.47029"
CATALOG
    FEATURED TAGS
    Memory 小冬搞开发 小冬传经验 Quadcopter 小冬找工作 小冬爱总结 ROS SLAM C++11 Deep learning Deeplearning Homework Jekyll Blog Object detection TEST Tensorflow 小冬学英语 Algorithm Blender Cross Entropy Error Func Github Kalman_filter Paper RL Raspberry Pi Ubuntu Wiki windows
    FRIENDS
    SHOWCASE