simucpp系列教程(7)坟墓

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

  1. 系统非常简单
    simucpp的代码量大致是 y = k 1 x y=k_1x y=k1x,ode4的代码量大致是 y = k 2 x + b y=k_2x+b y=k2x+b,且 k 1 > k 2 k_1>k_2 k1>k2, b b 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 ) (0.6,0.6,0.6,0.6) (0.6,0.6,0.6,0.6) 时需要修正到4个 0.5 0.5 0.5,但每个积分器都不知道其它积分器输出的是 0.6 0.6 0.6还是 0.5 0.5 0.5还是其它值,必须要等每一步仿真结束后另外设置每个积分器输出值。积分器后面另外添加归一化模块也是不行的,因为这样就相当于是积分器内部状态是 0.6 0.6 0.6,但下一步更新却用的是 0.5 0.5 0.5的错误值,最后导致误差越来越大。simucpp可以使用等每一步仿真结束后另外设置的方法,但simulink不行,大概查了一下可能要用到"自重置积分器"的概念,具体可以见积分器模块的帮助文档,我没研究过具体怎么用。四元数微分方程的例子并不唯一,例如用3个积分器的输出表示速度向量,当速度大小达到设定的最大速度限制时,再给正向的加速度时只能改变速度方向,此时3个积分器也有同样的问题。

附录:simucpp与ode4代码比较

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

四阶龙格库塔法

cpp 复制代码
#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法

cpp 复制代码
/**************************************************************
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

主程序

cpp 复制代码
#include <string>
#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;
}
相关推荐
可编程芯片开发13 小时前
基于MPPT最大功率跟踪的光伏发电蓄电池控制系统simulink建模与仿真
simulink·光伏发电·mppt·最大功率跟踪·蓄电池控制
顶呱呱程序2 天前
2-140 基于Solidworks和Matlab Simulink Simscape仿真的机器人手臂仿真
开发语言·matlab·机器人·simulink·simscape·机器人手臂仿真
淋雨的蜗牛21 天前
Simulink模型使用
simulink
bu_shuo2 个月前
Simulink仿真理想二极管模型
matlab·simulink·二极管
NeXT_Vision2 个月前
Matlab Simulink 主时间步(major time step)、子时间步(minor time step)
matlab·simulink·系统仿真·s-function
非常规定义M2 个月前
Day25_0.1基础学习MATLAB学习小技巧总结(25)——四维图形的可视化
开发语言·学习·数学建模·matlab·simulink
非常规定义M2 个月前
Day20_0.1基础学习MATLAB学习小技巧总结(20)——MATLAB绘图篇(3)
开发语言·学习·matlab·simulink
电网论文源程序2 个月前
168号资源-simulink仿真:燃料电池电动汽车Simulink模型-----已提供下载资源
simulink
梦想科研社2 个月前
【无人机设计与控制】使用 Simulink 进行四轴飞行器/四旋翼飞行器仿真
开发语言·算法·机器学习·matlab·无人机·simulink
可编程芯片开发2 个月前
基于爬山法MPPT和PI的直驱式永磁同步风力发电机控制系统simulink建模与仿真
simulink·mppt·爬山法·pi·直驱式永磁同步风力发电机