simucpp系列教程(7)坟墓

  最近使用simucpp在做某预研航天项目中逐渐发现，simucpp没有明显优势，不如直接手撕ODE4，从代码量上来看两种方法其实差不多，而且simucpp更容易出现编程上的错误，比如空指针和参数配置等，而且运行速度肯定是直接用ODE4更快。我目前觉得只有在下面两种情况下使用simucpp有优势。

1. 系统非常简单
   simucpp的代码量大致是 $y=k_{1}x$，ode4的代码量大致是 $y=k_{2}x+b$，且 $k_1>k_2$，$b$ 是ode4的4个公式。不管系统简单还是复杂，这4个公式都要有而且只能有一个，当需要仿真简单的一阶或二阶线性或非线性系统时，用simucpp可以快速出结果，但当系统复杂了以后，模块之间复杂的连接和封装都引入了额外的代码量，但ode4仍然只需要4个公式，几乎没有多余代码量。
2. 多智能体系统
   ode4的4个公式只能用一次也带来了一些缺点，例如多智能体系统中，每个个体都是独立的，但用ode4写代码的时候又得通过这一组公式耦合到一起，于是这种情况下使用simucpp更合适。但这种情况应该有其它更简单的方法，以后遇到再说。

  使用ODE4的一些坑记录在 向量形式四阶龙格库塔法的仿真细节，文中也解释了为什么ode4的4个公式只能用一次。至于为什么不用simulink，在专栏的第一篇里也都解释了，对于特别复杂的项目来说，编程就是最简单的方法。
  后来还遇到了一个simucpp和sinulink都不好解决的问题。在四元数微分方程中，每一步迭代后需要将四元数归一化，这本来是很简单的操作，但simucpp和sinulink都是面向对象的模块式编程，四元数微分方程里的4个积分模块都互相不知道其它模块的状态，在归一化时也就无法通过其它积分器的状态来修正自己的状态。例如，当某时刻的状态是 $(0.6,0.6,0.6,0.6)$ 时需要修正到4个 $0.5$，但每个积分器都不知道其它积分器输出的是$0.6$还是$0.5$还是其它值，必须要等每一步仿真结束后另外设置每个积分器输出值。积分器后面另外添加归一化模块也是不行的，因为这样就相当于是积分器内部状态是$0.6$，但下一步更新却用的是$0.5$的错误值，最后导致误差越来越大。simucpp可以使用等每一步仿真结束后另外设置的方法，但simulink不行，大概查了一下可能要用到“自重置积分器”的概念，具体可以见积分器模块的帮助文档，我没研究过具体怎么用。四元数微分方程的例子并不唯一，例如用3个积分器的输出表示速度向量，当速度大小达到设定的最大速度限制时，再给正向的加速度时只能改变速度方向，此时3个积分器也有同样的问题。

附录：simucpp与ode4代码比较

下面是同样实现刚体姿态控制时两种方法的代码。后面是主函数展示仿真结果。
四阶龙格库塔法

#ifndef ATTITUDEDYNAMICSODE4_H
#define ATTITUDEDYNAMICSODE4_H
#include "zhnmat.hpp"
using namespace zhnmat;
typedef std::vector<double>  vecdble;

class AttitudeDynamicsODE4 {
public:
    AttitudeDynamicsODE4(
        Mat initQ = Mat(vecdble{1,0,0,0}),
        Mat initW = Mat(vecdble{0,0,0}),
        Mat J = eye(3)
    ): _J(J) {
        _states = VConcat(initQ, initW);
        _Jinv = _J.inv();
        _torque = Mat(3, 1);
    }
    void Simulate_OneStep() {
        const double h = 0.01;
        Mat K1 = ODE4Function(_t, _states);
        Mat K2 = ODE4Function(_t+h/2, _states + h/2*K1);
        Mat K3 = ODE4Function(_t+h/2, _states + h/2*K2);
        Mat K4 = ODE4Function(_t+h, _states + h*K3);
        _states += h/6*(K1 + 2*K2 + 2*K3 + K4);
        // Step_Normalize();
    }
    void set_torque(Mat torque) { _torque = Mat(torque); }
    Mat get_quaternion() { return _states(Rect(0, 0, 4, 1)); }
    Mat get_angular_velocity() { return _states(Rect(4, 0, 3, 1)); }
private:
    Mat ODE4Function(double t, Mat x) {
        Mat Q = x(Rect(0, 0, 4, 1));
        Mat W = x(Rect(4, 0, 3, 1));
        Mat matW(4, 4, vecdble{
            0, -W.at(0, 0), -W.at(1, 0), -W.at(2, 0),
            W.at(0, 0), 0, W.at(2, 0), -W.at(1, 0),
            W.at(1, 0), -W.at(2, 0), 0, W.at(0, 0),
            W.at(2, 0), W.at(1, 0), -W.at(0, 0), 0,
        });
        Mat dQ = 0.5*matW*Q;
        Mat dW = _Jinv*(_torque - (Vector3d(W) & Vector3d(_J*W)));
        return VConcat(dQ, dW);
    }
    void Step_Normalize() {
        Mat Q = _states(Rect(0, 0, 4, 1));
        // Q = Quaternion_Normalize(Q);
        _states = VConcat(Q, _states(Rect(4, 0, 3, 1)));
    }
    Mat _states, _torque, _J, _Jinv;
    double _t;
};
#endif // ATTITUDEDYNAMICSODE4_H

simucpp法

/**************************************************************
simucpp版本：V2.1.16
**************************************************************/
#ifndef ATTITUDEDYNAMICSSIMUCPP_H
#define ATTITUDEDYNAMICSSIMUCPP_H
#include "simucpp.hpp"
using namespace simucpp;

class AttitudeDynamicsSimucpp {
public:
    AttitudeDynamicsSimucpp(
        Mat initQ = Mat(vecdble{1,0,0,0}),
        Mat initW = Mat(vecdble{0,0,0}),
        Mat J = eye(3)
    ): _J(J) {
        _Jinv = _J.inv();
        _intQ = new simucpp::MStateSpace(&sim1, simucpp::BusSize(4, 1), true, "intQ");
        _intW = new simucpp::MStateSpace(&sim1, simucpp::BusSize(3, 1), true, "intW");
        _misoQ = new simucpp::MFcnMISO(&sim1, simucpp::BusSize(4, 1), "misoQ");
        _misoW = new simucpp::MFcnMISO(&sim1, simucpp::BusSize(3, 1), "misoW");
        _mcstTorque = new simucpp::MConstant(&sim1, zhnmat::Mat(3, 1), "cnstSigma");
        sim1.connectM(_intQ, _misoQ);
        sim1.connectM(_intW, _misoQ);
        sim1.connectM(_misoQ, _intQ);
        sim1.connectM(_intW, _misoW);
        sim1.connectM(_mcstTorque, _misoW);
        sim1.connectM(_misoW, _intW);
        sim1.Set_SimStep(0.01);
        _intQ->Set_InitialValue(initQ);
        _intW->Set_InitialValue(initW);
        _misoQ->Set_Function([](Mat *u){
            // u[0]为姿态四元数(4x1)，u[1]为固连系下的角速度向量(3x1)
            Mat W(4, 4, vecdble{  // 角速度向量w1对应的四元数乘法左矩阵(4x4)
                0, -u[1].at(0, 0), -u[1].at(1, 0), -u[1].at(2, 0),
                u[1].at(0, 0), 0, u[1].at(2, 0), -u[1].at(1, 0),
                u[1].at(1, 0), -u[1].at(2, 0), 0, u[1].at(0, 0),
                u[1].at(2, 0), u[1].at(1, 0), -u[1].at(0, 0), 0,
            });
            return 0.5*W*u[0];
        });
        _misoW->Set_Function([&](Mat *u){
            Vector3d omega = u[0];  // 固连系下的角速度向量(3x1)
            Vector3d tau = Mat(3, 1, vecdble{  // 固连系下的力矩向量(3x1)
                u[1].at(0, 0), u[1].at(1, 0), u[1].at(2, 0)
            });
            Vector3d ans = _Jinv*(tau - (omega & (_J*omega)));
            return Mat(ans);
        });
        sim1.Initialize();
    };
    void set_init_QW(Mat Q, Mat W) {
        _intQ->Set_InitialValue(Q);
        _intW->Set_InitialValue(W);
    }
    void set_torque(Mat torque) { _mcstTorque->Set_OutValue(torque); }
    // void _set_quaternion(Mat Q) { _intQ->ForceSet_OutValue(Q); }
    Mat get_quaternion() { return _intQ->Get_OutValue(); }
    Mat get_angular_velocity() { return _intW->Get_OutValue(); }
    Simulator sim1;
    simucpp::MStateSpace *_intQ=nullptr;
    simucpp::MStateSpace *_intW=nullptr;
    simucpp::MFcnMISO *_misoQ=nullptr;
    simucpp::MFcnMISO *_misoW=nullptr;
    simucpp::MConstant *_mcstTorque=nullptr;
    zhnmat::Mat _J, _Jinv;
};
#endif // ATTITUDEDYNAMICSSIMUCPP_H

主程序

#include 
#include "tracelog.h"
#include "AttitudeDynamicsSimucpp.hpp"
#include "AttitudeDynamicsODE4.hpp"
#define READCSV_IMPLEMENTATION
#include "readcsv.hpp"
#define QUATERNIONMATH_IMPLEMENTATION
#include "quaternionmath.hpp"
using namespace std;
#define DO_PLOT
#ifdef DO_PLOT
#include "matplotlibcpp.h"
namespace plt = matplotlibcpp;
#endif

// 控制律
Mat Control_Law(Mat eulerTarget, Mat Qcurrent, Mat omega) {
    Mat eulerCurrent = Quaternion_to_Euler(Qcurrent);
    Mat eulerError = (Mat(eulerTarget) - Mat(eulerCurrent));
    Mat torque = eulerError*5;
    torque -= omega*1;
    return torque;
}

Mat data1;
/**************************************************************
 * 主函数
**************************************************************/
int main(void) {
    SetLogLevel(LOG_FATAL);
    Mat initQ(vecdble{1, 0, 0, 0});
    Mat initW(vecdble{0, 0, 0});
    Mat J(3, 3, vecdble{1, 0, 0, 0, 1, 0, 0, 0, 3});
    data1 = read_csv("../data.csv");
    auto usv1 = AttitudeDynamicsSimucpp(initQ, initW, J);
    auto usv2 = AttitudeDynamicsODE4(initQ, initW, J);

    vecdble plottime, plotx1, plotx2;
    Mat curq, curw, torque, euler;
    double t = 0;  // 全局时间
    cout.precision(16);
    /*测试*/
    while (t < 5) {
        for (int i = 0; i < 20; i++) {
            plottime.push_back(t);
            usv1.sim1.Simulate_OneStep();
            curq = usv1.get_quaternion();
            euler = Quaternion_to_Euler(curq).To_Vector();
            plotx1.push_back(euler.at(1,0));
            usv2.Simulate_OneStep();
            curq = usv2.get_quaternion();
            euler = Quaternion_to_Euler(curq).To_Vector();
            plotx2.push_back(euler.at(1,0));
            t += 0.01;
        }
        curq = usv1.get_quaternion();
        curw = usv1.get_angular_velocity();
        torque = Control_Law(Mat(3, 1, vecdble{-0.5, 1, 0}), curq, curw);
        usv1.set_torque(torque);
        curq = usv2.get_quaternion();
        curw = usv2.get_angular_velocity();
        torque = Control_Law(Mat(3, 1, vecdble{-0.5, 1, 0}), curq, curw);
        usv2.set_torque(torque);
    }
#ifdef DO_PLOT
    plt::named_plot("x1", plottime, plotx1);
    plt::named_plot("x2", plottime, plotx2);
    matplotlibcpp::legend();
    plt::show();
#endif
    return 0;
}