MatlabS函数求解二阶微分方程或二阶动态方程

近几天没事&＃xff0c;开始学习一下Matlab S函数求解二阶微分方程&＃xff0c;具体方程表达式如下&＃xff1a;
D(q)q¨&＃43;C(q,q˙)q˙&＃61;τD\left( q \right)\ddot q &＃43; C(q,\dot q)\dot q &＃61; \tauD(q)q¨​&＃43;C(q,q˙​)q˙​&＃61;τ (2.1)
τ&＃61;Kde˙&＃43;Kpe\tau &＃61; {K_d}\dot e &＃43; {K_p}eτ&＃61;Kd​e˙&＃43;Kp​e (2.2)
其中&＃xff0c;e˙&＃61;q˙d−q˙\dot e &＃61; {\dot q_d} - \dot qe˙&＃61;q˙​d​−q˙​&＃xff0c;e&＃61;qd−qe &＃61; {q_d} - qe&＃61;qd​−q。$q$实际输出结果&＃xff0c;qdq_dqd​期望输出的结果。
仿真参数设置如下&＃xff1a;
p&＃61;[2.90,0.76,0.87,3.04,0.87]Tp&＃61;[2.90, \space 0.76, \space 0.87, \space 3.04, \space 0.87]^Tp&＃61;[2.90, 0.76, 0.87, 3.04, 0.87]T&＃xff0c;q0&＃61;[0.0,0.0]Tq_0 &＃61; [0.0,\space 0.0]^Tq0​&＃61;[0.0, 0.0]T&＃xff0c;q˙0&＃61;[0.0,0.0]T\dot q_0 &＃61; [0.0,\space 0.0]^Tq˙​0​&＃61;[0.0, 0.0]T&＃xff0c;qd(0)&＃61;[1.0,1.0]Tq_d(0) &＃61; [1.0,\space 1.0]^Tqd​(0)&＃61;[1.0, 1.0]T&＃xff0c;Kp&＃61;diag([100,100])K_p &＃61; diag([100, \space 100])Kp​&＃61;diag([100, 100])&＃xff0c;Kd&＃61;diag([100,100])K_d &＃61; diag([100, \space 100])Kd​&＃61;diag([100, 100])&＃xff0c;D(q)&＃61;[p1&＃43;p2&＃43;2p3cosq2,p2&＃43;p3cosq2;p2&＃43;p3cosq2,p2]D\left( q \right) &＃61; [p_1&＃43;p_2&＃43;2p_3cosq_2,\space p_2&＃43;p_3cosq_2;\space p_2&＃43;p_3cosq_2,\space p_2]D(q)&＃61;[p1​&＃43;p2​&＃43;2p3​cosq2​, p2​&＃43;p3​cosq2​; p2​&＃43;p3​cosq2​, p2​]&＃xff0c;C(q,q˙)&＃61;[−p3q˙2sinq2,−p3(q˙1&＃43;q˙2)sinq2;p3q˙1sinq2,0]C(q,\dot q)&＃61;[-p_3\dot q_2 sin q_2,\space -p_3(\dot q_1 &＃43; \dot q_2) sin q_2; \space p_3\dot q_1 sin q_2,\space 0]C(q,q˙​)&＃61;[−p3​q˙​2​sinq2​, −p3​(q˙​1​&＃43;q˙​2​)sinq2​; p3​q˙​1​sinq2​, 0]
编程分析&＃xff0c;首先等式&＃xff08;2.1&＃xff09;左右都含有带求解未知数q&＃xff0c;所以把等式左右两边都写成S函数求解。等式&＃xff08;2.2&＃xff09;S函数编写分析&＃xff1a;左边$\tau$当成输出&＃xff0c;是个二维向量。右边有两个输入变量q˙,q\dot q,qq˙​,q&＃xff0c;其中q˙d,qd\dot q_d,q_dq˙​d​,qd​是常数已知。每个变量又都是二维向量&＃xff0c;所以S函数的输入数量 4&＃xff0c;输出数量 2&＃xff0c;并且输出和输入是有关系的直接反馈参数设置1&＃xff0c;其他参数默认。

编写等式&＃xff08;2.2&＃xff09;分析&＃xff0c;等式可以改写成&＃xff1a;D(q)q¨&＃61;τ−C(q,q˙)q˙D\left( q \right)\ddot q &＃61; \tau - C(q,\dot q)\dot qD(q)q¨​&＃61;τ−C(q,q˙​)q˙​&＃xff0c;这个一个矩阵求解&＃xff1a;可以写成两种形式&＃xff1a;q¨&＃61;inv(D(q))∗(τ−C(q,q˙)q˙)\ddot q &＃61; inv(D\left( q \right))*\left( \tau - C(q,\dot q)\dot q \right)q¨​&＃61;inv(D(q))∗(τ−C(q,q˙​)q˙​)或者q¨&＃61;(D(q))\(τ−C(q,q˙)q˙)\ddot q &＃61; (D\left( q \right)) \backslash \left( \tau - C(q,\dot q)\dot q \right)q¨​&＃61;(D(q))\(τ−C(q,q˙​)q˙​)。第二种求解方法比较快并且精度高&＃xff0c;推荐第二种方法求解。由于是个二阶微分方程&＃xff0c;且每个变量是二维所以S函数的连续状态变量数是 4&＃xff0c;输出分别是变量q,q˙q,\dot qq,q˙​&＃xff0c;所以输出变量数也是4&＃xff0c;此处的4个输出会作为等式&＃xff08;2.1&＃xff09;的输入&＃xff0c;数量必须相同。输入变量是$\tau$&＃xff0c;所以输入变量数是 2。由于输出函数sys&＃61;mdlOutputs(t,x,u)中&＃xff0c;sys &＃61; x&＃xff0c;x是状态变量&＃xff0c;输出和输入没有直接关系&＃xff0c;所以直接反馈参数理论上应该设置0&＃xff0c;但实际上设置1结果也是正确的&＃xff0c;由于能力有限暂时不清楚原因。

由于CSND插入MATLAB代码失败所以&＃xff0c;把文件压缩上传到CSDN共大家参考。
https://download.csdn.net/download/cswh876908060/12151665
如有问题或者需要代写&＃xff0c;代做&＃xff0c;指导&＃xff0c;请联系QQ&＃xff1a;876908060