simucpp系列教程(7)坟墓

本文主要是介绍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 <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_PLOTplt::named_plot("x1", plottime, plotx1);plt::named_plot("x2", plottime, plotx2);matplotlibcpp::legend();plt::show();
#endifreturn 0;
}

这篇关于simucpp系列教程(7)坟墓的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---