最近使用simucpp在做某预研航天项目中逐渐发现,simucpp没有明显优势,不如直接手撕ODE4,从代码量上来看两种方法其实差不多,而且simucpp更容易出现编程上的错误,比如空指针和参数配置等,而且运行速度肯定是直接用ODE4更快。我目前觉得只有在下面两种情况下使用simucpp有优势。
- 系统非常简单
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个公式,几乎没有多余代码量。 - 多智能体系统
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;
}