已知双足机器人动力学方程角度1、角度2、角度3变量,角度1对机器人角度2的求偏导MATLAB
时间: 2023-06-26 11:04:45 浏览: 42
假设角度1、角度2、角度3分别为 $\theta_1$、$\theta_2$、$\theta_3$,则双足机器人动力学方程可以表示为:
$$
M(\theta)\ddot{\theta}+C(\theta,\dot{\theta})\dot{\theta}+g(\theta)=\tau
$$
其中,$M(\theta)$ 是质量矩阵,$C(\theta,\dot{\theta})$ 是科里奥利矩阵,$g(\theta)$ 是重力向量,$\tau$ 是关节力矩向量。
我们需要求解 $\frac{\partial \theta_2}{\partial \theta_1}$,可以通过对动力学方程关于 $\theta_1$ 取一次偏导,得到:
$$
\frac{\partial}{\partial \theta_1}\left(M(\theta)\ddot{\theta}+C(\theta,\dot{\theta})\dot{\theta}+g(\theta)\right)=\frac{\partial \tau}{\partial \theta_1}
$$
由于 $\ddot{\theta}$ 和 $\dot{\theta}$ 都是关于时间的函数,而 $\theta$ 和 $\tau$ 都是关于关节角度的函数,因此我们需要用链式法则对式子进行求导。具体来说,我们可以按照以下步骤进行计算:
1. 对动力学方程的每一项分别计算偏导数:
$$
\begin{aligned}
\frac{\partial}{\partial \theta_1}\left(M(\theta)\ddot{\theta}\right)&=\frac{\partial M(\theta)}{\partial \theta_1}\ddot{\theta}+M(\theta)\frac{\partial \ddot{\theta}}{\partial \theta_1}\\
&=\frac{\partial M(\theta)}{\partial \theta_1}\ddot{\theta}+M(\theta)\frac{\partial}{\partial t}\left(\frac{\partial \dot{\theta}}{\partial \theta_1}\right)\\
&=\frac{\partial M(\theta)}{\partial \theta_1}\ddot{\theta}+M(\theta)\frac{\partial^2 \theta}{\partial t\partial \theta_1}\dot{\theta}\\
\frac{\partial}{\partial \theta_1}\left(C(\theta,\dot{\theta})\dot{\theta}\right)&=\frac{\partial C(\theta,\dot{\theta})}{\partial \theta_1}\dot{\theta}+C(\theta,\dot{\theta})\frac{\partial \dot{\theta}}{\partial \theta_1}\\
&=\frac{\partial C(\theta,\dot{\theta})}{\partial \theta_1}\dot{\theta}+C(\theta,\dot{\theta})\frac{\partial^2 \theta}{\partial t\partial \theta_1}\\
\frac{\partial}{\partial \theta_1}\left(g(\theta)\right)&=\frac{\partial g(\theta)}{\partial \theta_1}
\end{aligned}
$$
2. 将上述三个偏导数相加,得到动力学方程关于 $\theta_1$ 的一次偏导数:
$$
\frac{\partial}{\partial \theta_1}\left(M(\theta)\ddot{\theta}+C(\theta,\dot{\theta})\dot{\theta}+g(\theta)\right)=\frac{\partial M(\theta)}{\partial \theta_1}\ddot{\theta}+M(\theta)\frac{\partial^2 \theta}{\partial t\partial \theta_1}\dot{\theta}+\frac{\partial C(\theta,\dot{\theta})}{\partial \theta_1}\dot{\theta}+C(\theta,\dot{\theta})\frac{\partial^2 \theta}{\partial t\partial \theta_1}+\frac{\partial g(\theta)}{\partial \theta_1}
$$
3. 将上述式子中的 $\ddot{\theta}$ 和 $\dot{\theta}$ 替换成动力学方程中的未知量和已知量:
$$
\frac{\partial}{\partial \theta_1}\left(M(\theta)\ddot{\theta}+C(\theta,\dot{\theta})\dot{\theta}+g(\theta)\right)=\frac{\partial M(\theta)}{\partial \theta_1}\left(M(\theta)\right)^{-1}\left(\tau-C(\theta,\dot{\theta})\dot{\theta}-g(\theta)\right)+\frac{\partial C(\theta,\dot{\theta})}{\partial \theta_1}\dot{\theta}+C(\theta,\dot{\theta})\frac{\partial^2 \theta}{\partial t\partial \theta_1}+\frac{\partial g(\theta)}{\partial \theta_1}
$$
4. 整理得到 $\frac{\partial \theta_2}{\partial \theta_1}$:
$$
\frac{\partial \theta_2}{\partial \theta_1}=-\frac{\left(\frac{\partial M_{22}}{\partial \theta_1}M_{11}-\frac{\partial M_{11}}{\partial \theta_1}M_{22}\right)\left(\tau_2-C_{22}\dot{\theta}_2-g_2\right)-\left(M_{22}\frac{\partial C_{22}}{\partial \theta_1}-M_{11}\frac{\partial C_{11}}{\partial \theta_1}\right)\dot{\theta}_2}{\left(M_{11}M_{22}-M_{12}M_{21}\right)^2}
$$
其中,$M_{ij}$、$C_{ij}$ 和 $g_i$ 分别表示质量矩阵、科里奥利矩阵和重力向量中的元素。$\tau_2$ 和 $\dot{\theta}_2$ 分别表示关节力矩向量和关节角速度向量中的第二个元素。