Matlab S函數求解PD控制的二階微分或者二階狀態方程

近幾天時間比較充足，便學習一下S函數求解微分方程。其求解方程如下：
$D\left( q \right)\ddot q + C(q,\dot q)\dot q{\rm{ + G}}\left( q \right) + \omega = \tau$ (2.7)
$\omega = {d_1} + {d_2}\left\| e \right\| + {d_3}\left\| {\dot e} \right\|$ (2.10)
$y = \dot e + \gamma e$ (2.11)
${\dot q_r} = {\dot q_d} - \gamma e$ (2.12)
其中$e = q - {q_d}$，$\dot e = \dot q - {\dot q_d}$，$q$是實際輸出變量，$q_d$是期望輸出變量
$\tau {\rm{ = - }}{K_p}e - {K_v}\dot e + \phi \left( {q,\dot q,{{\dot q}_r},{{\ddot q}_r}} \right)\hat P + u$ (2.16)
$u{\rm{ = }}{\left[ {{u_1} \cdots {u_n}} \right]^T},{u_i} = - ({d_1} + {d_2}\left\| e \right\| + {d_3}\left\| {\dot e} \right\|){\mathop{\rm sgn}} \left( {{y_i}} \right)$ (2.17)
$\dot { \hat {P} }= - \Gamma {\phi ^T}(q,\dot q,{\dot q_r},{\ddot q_r})y$ (2.18)
${K_p} = {K_{p1}} + {K_{p2}}{B_p}\left( e \right),{K_v} = {K_{v1}} + {K_{v2}}B\left( {\dot e} \right)$
${K_{p1}} = diag\left( {{k_{p11}},{k_{p12}}, \cdots ,{k_{p1n}}} \right),{K_{p2}} = diag\left( {{k_{p21}},{k_{p22}}, \cdots ,{k_{p2n}}} \right)$
${K_{v1}} = diag\left( {{k_{v11}},{k_{v12}}, \cdots ,{k_{v1n}}} \right),{K_{v2}} = diag\left( {{k_{v21}},{k_{v22}}, \cdots ,{k_{v2n}}} \right)$
${B_p}\left( e \right) = diag\left( {\frac{1}{{{a_1} + \left| {{e_1}} \right|}},\frac{1}{{{a_2} + \left| {{e_2}} \right|}}, \cdots ,\frac{1}{{{a_n} + \left| {{e_n}} \right|}}} \right)$
${B_v}\left( e \right) = diag\left( {\frac{1}{{{\beta _1} + \left| {{{\dot e}_1}} \right|}},\frac{1}{{{\beta _2} + \left| {{{\dot e}_2}} \right|}}, \cdots ,\frac{1}{{{\beta _n} + \left| {{{\dot e}_n}} \right|}}} \right)$
仿真參數設置如下：
$D_{11}(q_2)=(m_1+m_2)r_1^2+m_2r_2^2+2m_2r_1r_2cosq_2$
$D_{12}(q_2)=m_2r_2^2 + m_2r_1r_2cosq_2$
$D_{22}(q_2)=m_2r_2^2$
$D = [D_{11},D_{12};D_{12},D_{22}]$
$C_{12}(q_2)=m_2r_1r_2sinq_2$
$C=[-C_{12}\dot q_2,-C_{12}(\dot q_1 + \dot q_2);C_{12}\dot q_1, 0]$
$G_1(q_1,q_2)=(m_1+m_2)r_1cosq_2+m_2r_2cos(q_1+q_2)$
$G_2(q_1,q_2)=m_1r_2cos(q_1+q_2)$
$G=[G_1g;G_2g]$
$\phi_{11}=\ddot q_{1r}+e_1cosq_2$
$\phi_{12}=\ddot q_{1r}+\ddot q_{2r}$
$\phi_{13}=2\ddot q_{1r}cosq_2+\ddot q_{2r}cosq_2-\dot q_2 \dot q_{1r}sinq_2-(\dot q_1 + \dot q_2)\dot q_{2r}sinq_2+e_1cos(q_1+q_2)$
$\phi_{21}=0$
$\phi_{22}=\phi_{12}$
$\phi_{23}=\dot q_1 \dot q_{1r}sinq_2+\ddot q_{1r}cosq_2+e_1cos(q_1+q_2)$
$e_1=g/r_1,g=9.8$
$r_1=1,r_2=0.8,m_1=0.5,m_2=0.5,d_1=2,d_2=3,d_3=6,\omega=d_1+d_2\left\| { e} \right\|+d_3\left\| {\dot e} \right\|$
$q_{1d}=sin(2*\pi*t),q_{2d}=sin(2*\pi*t)$
$[q_1;\dot q_1;q_2;\dot q_2]=[0.1;0;0.1;0]$
$K_{p1}=diag(180,190),K_{p2}=diag(150,150)$
$K_{v1}=diag(180,180),K_{v2}=diag(150,150)$
$\alpha_i=\beta_i =1(i=1,2),\gamma=5,\Gamma=[5\space0\space 0;0 \space5 \space0;0\space 0 \space5]$
編程分析：等式2.7右邊寫成一個S函數。首先分析輸入參數個數，由於等式2.16中有$q,\dot q,q_d,\dot q_d,\ddot q_d$五個變量，並且每個變量是二維的所以輸入參數設置爲10。輸出變量$\tau$是個二維的，$p$是三維的，所以右邊S函數的輸出設置爲5。直接反饋設置爲1中間需要求解公式2.18，所以此函數需要用到微分模塊sys=mdlDerivatives(t,x,u)，所以連續變量狀態數設置爲3。

等式2.7左邊的S函數編寫，求解過程中需要$q_,\dot q_d,\tau$三個變量，所以輸入個數設置6，輸出變量$q,\dot q$,所以設置爲4。等式2.7左邊是個二維二階微分方程求解，所以連續狀態設置爲4。
由於CSDN粘貼Matlab代碼不方便，具體的函數編寫放在相應的附件中。https://download.csdn.net/download/cswh876908060/12155057。如需代寫，或者諮詢，請聯繫：