[发明专利]基于多体系统离散时间传递矩阵法的机器人动力学建模方法

摘要

本发明公开了一种履带式移动机器人的多体系统离散时间传递矩阵法统一动力学建模方法。其核心包括：建立多体系统动力学模型化整为零，确定状态矢量；确定各元件传递矩阵U和传递方程z_O＝Uz_I；由元件的传递矩阵拼装系统的总传递矩阵U_all和总传递矩阵方程U_allz_all＝0；根据边界条件，求解系统传递方程，得到t_i时刻边界状态矢量；应用元件的传递方程，求得t_i时刻系统各联接点状态矢量。本发明避免了以往研究移动式机器人时对移动平台和机械臂分别建模，而忽略其耦合和相互影响所带来的误差，同时理论性强，思路清晰，编程容易实现，可达到对这类移动机器人精确控制的目的。

主权项

基于多体系统离散时间传递矩阵法的机器人动力学建模方法，其特征在于该方法包括：第一、归纳履带式移动机器人的结构模型，即按照传统的方法提取待研究的履带式移动机器人的主要特征，包括车体、机械臂杆件、转动关节亦即中心刚体和机械臂末端；第二、化整为零，将该机器人整体机械结构进行编号：0—地面，1—车体，2,4,6—中心刚体，3,5,7—机械臂杆件，8—机械臂末端；第三、确定该机器人各个元件的质心坐标，外形尺寸，质量的基本参数信息：给出车体运动规律为：$x_I=\left\{\begin{array}{cc}0& x\≤T_a\\ \frac{t-T_a}{T_b-T_a}-\frac{1}{2\mathrm{\π}}\sin \frac{2\mathrm{\πt}}{T_b-T_a}& T_a\≤x\≤T_b\\ 0& x\≥T_b\end{array}\right.$各中心刚体运动规律：$\mathrm{\θ}=\left\{\begin{array}{cc}0& x\≤T_a\\ \frac{\mathrm{\π}}{6}\·[\frac{t-T_a}{T_b-T_a}-\frac{1}{2\mathrm{\π}}\sin \frac{2\mathrm{\πt}}{T_b-T_a}]& T_a\≤x\≤T_b\\ 0& x\≥T_b\end{array}\right.$式中，t—历时时间；T_a—运动起始的延时时间；T_b—终止时间；第四、由各个刚体连体坐标系的坐标经转换得到惯性系下的坐标，r_O＝r_I+r_IO，r_C＝r_I+r_IC，即由原点固定在连体坐标系输入端I的位置坐标、质心C的位置坐标(c₁，c₂，c₃)、输出端O的位置坐标(b₁，b₂，b₃)和转角计算出刚体输出端相对于惯性系原点的矢径r_O，刚体质心相对于惯性系原点的矢径r_C，r_IC＝Al_IC，r_IO＝Al_IO及其一、二阶导数，式中，r_O输出点O在惯性系中的位置坐标；r_I输入点I在惯性系中的位置坐标；r_C质心C在惯性系中的位置坐标；r_IO输出点O相对于输入点I在惯性系中的位置坐标；r_IC质心C相对于输入点I在惯性系中的位置坐标；A—连体坐标系向惯性系的坐标转换矩阵；$A=\left[\begin{array}{ccc}{c}_y{c}_z& s_x{c}_y{c}_z-c_x{s}_z& c_x{s}_y{c}_z+s_x{c}_z\\ c_y{s}_z& s_x{s}_y{s}_z+c_x{c}_z& c_x{s}_y{s}_z-s_x{c}_z\\ -s_y& s_x{c}_y& c_x{c}_y\end{array}\right]$$A\left(\mathrm{ti}\right)=A\left(t_{i-1}\right){\tilde{T}}_1\left(t_{i-1}\right){\mathrm{\θ}}_x\left(t_i\right)+A\left(t_{i-1}\right){\tilde{T}}_2\left(t_{i-1}\right){\mathrm{\θ}}_y\left(t_i\right)+A\left(t_{i-1}\right){\tilde{T}}_3\left(t_{i-1}\right){\mathrm{\θ}}_z\left(t_i\right)+\mathrm{\Φ}\left(t_{i-1}\right)$其中：$\begin{array}{c}\mathrm{\Φ}\left(t_{i-1}\right)=A\left(t_{i-1}\right)\{I-{\tilde{T}}_1\left(t_{i-1}\right){\mathrm{\θ}}_x\left(t_{i-1}\right)-{\tilde{T}}_2\left(t_{i-1}\right){\mathrm{\θ}}_y\left(t_{i-1}\right)-{\tilde{T}}_3\left(t_{i-1}\right){\mathrm{\θ}}_z\left(t_{i-1}\right)+[\frac{1}{2}{{\tilde{T}}_x}^2\left(t_{i-1}\right){{\stackrel{.}{\mathrm{\θ}}}_x}^2\left(t_{i-1}\right)\\ +\frac{1}{2}{{\tilde{T}}_2}^2\left(t_{i-1}\right){{\stackrel{.}{\mathrm{\θ}}}_y}^2\left(t_{i-1}\right)+\frac{1}{2}{{\tilde{T}}_3}^2\left(t_{i-1}\right){{\stackrel{.}{\mathrm{\θ}}}_z}^2\left(t_{i-1}\right)+{\tilde{T}}_2\left(t_{i-1}\right){\tilde{T}}_1\left(t_{i-1}\right){\stackrel{.}{\mathrm{\θ}}}_y\left(t_{i-1}\right){\stackrel{.}{\mathrm{\θ}}}_x\left(t_{i-1}\right)\\ +{\tilde{T}}_3\left(t_{i-1}\right){\tilde{T}}_2\left(t_{i-1}\right){\stackrel{.}{\mathrm{\θ}}}_z\left(t_{i-1}\right){\stackrel{.}{\mathrm{\θ}}}_y\left(t_{i-1}\right)+{\tilde{T}}_3\left(t_{i-1}\right){\tilde{T}}_1\left(t_{i-1}\right){\stackrel{.}{\mathrm{\θ}}}_z\left(t_{i-1}\right){\stackrel{.}{\mathrm{\θ}}}_x\left(t_{i-1}\right)\left]{\mathrm{\ΔT}}^2\right\}\end{array}$l_IC质心C相对于输入点I在连体坐标系中的位置坐标；l_IO输出点O相对于输入点I在连体坐标系中的位置坐标；$T_1=\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right],T_2=\left[\begin{array}{c}0\\ c_x\\ -s_x\end{array}\right],T_3=\left[\begin{array}{c}-s_y\\ s_x{c}_y\\ c_x{c}_y\end{array}\right],$是叉乘矩阵；s_x、s_y、s_z分别为不共面三轴的sin角位移，c_x、c_y、c_z分别为不共面三轴的cos角位移；第五、采用速度或角速度和加速度或角加速度的线性化、坐标转换矩阵的线性化以及Newmark‑β逐步积分法对状态矢量进行线性化计算，推导空间运动刚体的传递矩阵U_i，$U_i=\left[\begin{array}{ccccc}{I}_3& {\mathrm{\Ψ}}_{\mathrm{IO}}& O_{3\×3}& O_{3\×3}& \mathrm{\Φ}\left(t_{i-1}\right)l_{\mathrm{IO}}\\ O_{3\×3}& I_3& O_{3\×3}& O_{3\×3}& O_{3\×3}\\ U_{31}& U_{32}& I_3& U_{34}& U_{35}\\ -m{\mathrm{AI}}_3& -m{\mathrm{A\Ψ}}_{\mathrm{IC}}& O_{3\×3}& I_3& U_{45}\\ O_{1\×3}& O_{1\×3}& O_{1\×3}& O_{1\×3}& 1\end{array}\right]$式中，${\mathrm{\Ψ}}_{\mathrm{IO}}=A\left(t_{i-1}\right)\left[\begin{array}{ccc}{\tilde{T}}_1\left(t_{i-1}\right)l_{\mathrm{IO}}& {\tilde{T}}_2\left(t_{i-1}\right)l_{\mathrm{IO}}& {\tilde{T}}_3\left(t_{i-1}\right)l_{\mathrm{IO}}\end{array}\right];$I₃＝[1,0,0；0,1,0；0,0,1]；O_3X3＝[0,0,0；0,0,0；0,0,0]；O_1X3＝[0,0,0]；m为刚体质量；U₃₁、U₃₂、U₃₄、U₃₅、U₄₅分别为对应关节的传递矩阵；第六、根据第三步确定的已知参数信息，并应用MATLAB分别编写i＝1时的传递矩阵，即得到t_i时刻编号分别为0‑7的各个元件的传递矩阵U₁，U₂…U₇，由得到的传递矩阵再求得i>1时的传递矩阵；第七、将各个中心刚体相对运动坐标值变换到坐标系的绝对值，即将传递矩阵最后一列重新附值；如下所示：UU₁＝eye(13)；UU₂＝eye(13)；UU₃＝eye(13)；UU₁(5,13)＝UU₁(5,13)+thy_10(i)；UU₂(5,13)＝UU₂(5,13)+thy_20(i)；UU₂(6,13)＝UU₂(6,13)+thz_20(i)；UU₃(5,13)＝UU₃(5,13)+thy_30(i)；UU₃(6,13)＝UU₃(6,13)+thz_30(i)；第八、由各个元件的传递矩阵拼装系统的总传递矩阵U_all；U_all＝U₇U₆U₅U₄U₃U₂U₁；第九、确定边界条件，$\left\{\begin{array}{c}{z}_{\mathrm{1,0}}={{[x}_I,y_I,z_I,\mathrm{0,0,0,0,0,0},F,\mathrm{0,0,1}]}_{\mathrm{1,0}}^T\\ z_{\mathrm{8,7}}=[x_o,y_o,z_o,{\mathrm{\θ}}_{o1},{\mathrm{\θ}}_{o2},{\mathrm{\θ}}_{o3},\mathrm{0,0,0,0,0,0,1}{]}_{\mathrm{8,7}}^T\end{array}\right.$第十、应用MATLAB编程实现求解，求解系统传递方程Z_8,7＝U_allZ_0,1；得到各个元件的位移、角位移、内力矩与内力曲线图。