基于ROS搭建简易软件框架实现ROV水下目标跟踪（七）--程序解析之推力分配

      项目链接：https://github.com/cabinx/cabin_auv_ws

      现在开始介绍demo的基础控制模块程序。基础控制模块包含推力分配模块、PWM波计算模块，数据串口下发模块。每个模块的主要功能可以回头看软件框架简介的那一篇文章，在此不做介绍，后续主要介绍的是程序中的一些关键点。

      本文主要介绍的推力分配模块的几个关键函数。对应程序中的cabin_controllers/thruster_controller。这块可能写得杂且乱。

一、输入输出

      推力分配模块输入输出如下：

      监听topic：/command/netLoat，格式cabin_msgs::NetLoad，实际上即为总的合力和转矩[Fx，Fy，Fz，Mx，My，Mz]，注意合力和转矩为机器人坐标系；

      发布topic：/command/thrust，格式cabin_msgs::ThrustStamped，实际上为8个推进器推力。

      Yaml文件为机械模型文件（链接）。

      相关topic数据格式请查阅cabin_msgs/msg/下相关文件。

二、载入机械模型文件参数：

 ThrusterController::LoadParam<string>("properties_file", properties_file);
 properties = YAML::LoadFile(properties_file);
 ThrusterController::LoadVehicleProperties();

      这部分没有什么好介绍的。

三、ThrusterController::SetThrusterCoeffs（）函数     

      这个函数非常关键，所以我会花比较大篇幅介绍。函数主要实现推进器推力对机器人的作用因子矩阵（thrustCoeffs）的求取。该矩阵用于后续EOM方程组的构建，以通过非线性优化的方法求解满足总合力和转矩需求的各推进器的推力分配。

      我们以demo中x轴方向的运动为例。当机器人在x轴方向前进时，推进器推力怎么样才是合理的呢？竖直方向的推进器不用考虑，将水平方向推进器的推力在x轴及y轴方向分解，如下图：

      看起来非常理想，只需要考虑抵消y轴方向的分力，只余x轴方向的分力。起初我也是这样的想法，但这样描述是不准确的。此时只考虑了推进器推力对机器人x轴，y轴，z轴平移运动的影响，没有考虑推力对x轴、y轴，z轴转动的影响。

      来看维基百科上对转矩的描述。下图为在一个旋转系统里，作用力F、位置向量r、力矩τ，动量p，角动量L物理量间的关系。

      平时也可以靠右手来确定转矩。

      我们姑且认为转轴在机器人坐标系垂直于质心，则推进器推力的转矩为r X F，r为推进器位置相对于质心位置的向量，F为推进器推力方向,方向为Mz方向直线上。如推力分解图中右上角的推进器推力就会对机器人有一个Mz方向上向下的转矩，将会使得机器人绕z轴顺时针方向旋转。

      令推进器推力F与x轴方向的夹角为θ，显然推进器应满足：

      

      更进一步，记作用因子矩阵thrustCoeffs（6X8）为，则无论机器人怎样运动，均需满足：

      

      显然thrusterCoeffs矩阵每一行代表8个推进器推力在一个方向上的总的作用，如第一行代表8个推进器在x轴方向的推力影响；每一列代表每个推进器推力在各个方向上的分作用，如第一列代表1号推进器在Fx，Fy，Fz，Mx，My，Mz方向的影响。

      我们回到程序，在此只列几个关键点。

for(int i = 0; i < numThrusters; i++){
    if(thrustersEnabled[i]){
        for(int j = 0; j < 5; j++){
            //Transform X, Y, Z to COM reference frame
            if(j < 3){
                thrusters(j, i) = properties["properties"]["thrusters"][i]["pose"】[j].as<double>() - center_of_mass[j];
            }
            else{
                thrusters(j, i) = properties["properties"]["thrusters"][i]["pose"][j].as<double>();
            }
        }
    }
}

      推进器姿态参数的读取，这里需要注意有推进器位置相对于质心位置向量的求取。

//rotate around z, y axis
float psi = thrusters(3, i) * PI / 180;
float theta = thrusters(4, i) * PI / 180;
thrustCoeffs(0, i) = cos(psi) * cos(theta);  //Effective contribution along X-axis
thrustCoeffs(1, i) = sin(psi) * cos(theta);  //Effective contribution along Y-axis
thrustCoeffs(2, i) = -sin(theta);            //Effective contribution along Z-axis

      计算每个推进器在x轴，y轴，z轴平移方向的作用因子，简单的几何关系。

//cross-product
//Determine the effective moment arms for each thruster about the B-frame axes
thrustCoeffs.block<3, 1>(3, i) = thrusters.block<3, 1>(0, i).cross(thrustCoeffs.block<3, 1>(0, i));

      这里为计算每个推进器在x轴，y轴，z轴旋转方向的作用因子，推进器相对于质心位置向量叉乘推力方向作用因子。

      最终，8个推机器的推力组成的矩阵F（8x1），以及thrustCoeffs矩阵（8X6），有：

      F * thrustCoeffs = [Fx，Fy，Fz，Mx，My，Mz]^T

四、初始化

weightLoad_eig.setZero();
isSubmerged = false;

for(int i = 0; i < 6; i++){
    weightLoad[i] = 0;
    transportThm[i] = 0;
    command[i] = 0;
    //solver_forces[i] = 0;
}
    
for(int i = 0; i < 8; i++){
    solver_forces[i] = 0;
}

for(int i = 0; i < 3; i++){
    solver_cob[i] = 0;
    Fb_vector[i] = 0;
}

state_sub = nh.subscribe<cabin_msgs::Imu>("/state/imu", 1, &ThrusterController::ImuCB, this);
cmd_sub = nh.subscribe<cabin_msgs::NetLoad>("/command/netLoad", 1, &ThrusterController::NetLoadCB, this);
thrust_pub = nh.advertise<cabin_msgs::ThrustStamped>("/command/thrust", 1);
cob_pub = nh.advertise<geometry_msgs::Vector3Stamped>("/properties/cob", 1);

ThrusterController::InitThrustMsg();

      这部分也没什么好说的。

五、浮力及浮心动态配置模块

ThrusterController::InitDynamicReconfigure();

      在测试时借助ros的工具rqt_reconfigure，可以动态调节浮力的大小和质心的位置，非常方便。虽然此次demo测试我并未进行浮力的相关计算。

      具体看函数ThrusterController::DynamicReconfigCallback（）。

六、EOM（Equation of Motion，运动方程）的求解

      这里要深究代码的话，需要对google的非线性优化库ceres-solver的使用有所了解。搞slam的开发者可能接触的比较多。

      对应的代码为：

//EOM problem
problemEOM.AddResidualBlock(new ceres::AutoDiffCostFunction<EOM, 6, 8>(new EOM(numThrusters, thrustCoeffs, inertia, weightLoad, transportThm, command)), NULL, solver_forces);
optionsEOM.max_num_iterations = 100;
optionsEOM.linear_solver_type = ceres::DENSE_QR;

      运动方程的构建在thruster_controller.h中：

//Class EOM defines the 6 equations of motiion that ceres needs to solve
class EOM{
    ...
}

      这里我直接给出待优化的运动方程函数：

      1、command为期望的作用于机器人的力与力矩，即[Fx，Fy，Fz，Mx，My，Mz]，理解为输入未尝不可；
      2、force为推进器推力；
      3、residual为推进器总的推力作用于机器人的力和力矩和期望值的偏差，之后通过最小二乘或其它算法求解推进器推力F；
      4、等式左边第一项看过thrustCoeffs的介绍应该能理解；
      5、weightLoad为浮力影响的约束，需要借助IMU，考虑机器人重力，浮力，机器人姿态的因素，最后计算得到机器人定深时  推进器所需提供的推力。具体程序可以看函数ThrusterController::ImuCB()。原来俄亥俄州大学的程序中是有深度计和IMU部分的，当深度大于阈值时，认为机器人已完全没入水中，此时isSubmerged置为true，weightLoad起生效。由于我在测试中并未启用深度计与IMU，这个模块并未启用； 
      6、thransportThm程序也是在函数ImuCB中，需要IMU角速度数据及转动惯量，从程序上看是需要抵消掉机器人旋转时的一些影响，我请教了一些机械方向的同事，也没得到答案；
      7、对于程序中的inertia这一项，并未参与到运动方程的计算，所以我没将其写入运动方程中。也许俄亥俄州大学最新版本的程序中有对其有进一步的开发吧。
      8、solver_forces就是最终计算得到的各推进器推力，就理解为输出吧。

      实际上可以将thrustCoeffs，weightLoad，transportThm理解为机器人的运动约束，然后就是优化问题的求解。当然我们可以根据实际需求设计添加或减少机器人的运动约束。

七、浮心的计算

//Buoyancy Problem
problemBuoyancy.AddResidualBlock(new ceres::AutoDiffCostFunction<FindCoB, 3, 3>(new FindCoB(numThrusters, thrustCoeffs, Fb_vector, solver_forces)), NULL, solver_cob);
optionsBuoyancy.max_num_iterations = 100;
optionsBuoyancy.linear_solver_type = ceres::DENSE_QR;

    demo中没有用到，程序结构参考EOM problem。

八、ThrusterController::NetLoadCB（）函数

      每收到一个力及力矩[Fx，Fy，Fz，Mx，My，Mz]的topic，计算出满足运动需求的每个推进器的推力，然后发布。