[发明专利]一种多关节机器人动力学仿真装置及方法

权利要求书

1.一种多关节机器人动力学仿真方法，其特征在于，所述方法包括：

步骤1：通过上位机发送机器人配置文件及环境对机器人施加的作用信息至处理单元；

所述机器人配置文件及环境对机器人施加的作用信息包括：机器人初始位置下的变换矩阵M_i，j、机器人第i个关节的转轴在基坐标系下的矩阵机器人重力加速度g以及机器人第i个连杆的空间惯性矩阵

所述机器人第i个关节的转轴在基坐标系下的矩阵表示为：

其中，为第i个关节的转轴在其自身坐标系下的表示，为M_i，j的伴随阵；

所述机器人第i个连杆的空间惯性矩阵如公式(2)表示：

其中I为单位阵，为第i个连杆的转动惯量，m_i为第i个连杆的质量；

通过所述机器人初始位置下的变换矩阵M_i，j，创建机器人在运动中的姿态变换矩阵T_i，j：

其中，为自M_i-1，i到T_i-1，i的变换矩阵，θ_i为机器人第i个关节位置；

步骤2：使用输入单元对处理单元设置机器人配置参数及仿真初始条件；

所述机器人配置参数及仿真初始条件包括：机器人关节加速度参数机器人关节速度机器人关节位置θ、末端扭力矢量以及机器人关节力矩矢量τ；

若进行逆动力学仿真，输入机器人关节加速度参数机器人关节速度机器人关节位置θ、末端扭力矢量输出关节力矩矢量τ，

若进行正动力学仿真，输入末端扭力矢量和机器人关节力矩矢量τ，输出机器人关节加速度参数机器人关节速度和机器人关节位置θ；

步骤3：根据机器人关节位置θ建立质量矩阵M(θ)，所述质量矩阵M(θ)表示为：

其中，

根据机器人关节位置θ和机器人关节速度建立科氏矩阵所述科氏矩阵表示为：

其中，

根据机器人关节位置θ建立重力矩阵g(θ)，所述重力矩阵g(θ)表示为：

其中，

为{i}参考系在其自身坐标系下的扭矢量，为基坐标系扭矢量的微分，ω为角速度；

根据机器人关节位置θ建立雅可比矩阵J(θ)，所述雅可比矩阵J(θ)表示为：

步骤3.5：基于公式(4)～(7)，所述机器人动力学模型表示为：

其中，

步骤4：通过最小系统内固化的数值算法库对所述基于Twist-wrench公式建立机器人动力学模型进行解算；

所述数值算法库包括Runge-Kutta法、线性多步法和Rosenbrock改进法；

基于Twist-wrench公式建立的机器人动力学模型，通过公式(9)进行Rosenbrock改进法描述：

其中y₁＝θ，t为仿真时间；

将所述公式(9)改写为常微分方程形式：

其中Y＝[y₁ y₂]^T；

通过公式(9)和(10)得到t_n时刻Y_n的值，使用Rosenbrock改进法对基于Twist-wrench公式建立的机器人动力学模型进行数值解算，所述数值解算过程如公式(11)～(16)所示：

F₀＝F(t_n，Y_n) (11)；

其中，F₀表示在函数F在t_n时刻的值；

k₁＝W^-1(F₀+hdT) (12)；

其中，k_i为中间变量，h为机器人仿真步长，T为F对t的偏微分，W＝I-hdJ，J为F对Y的偏微分，

F₁＝F(t_n+0.5h，Y_n+0.5hk₁) (13)；

k₂＝W^-1(Fx-k₁)+k₁ (14)；

Y_n+1＝Y_n+hk₂ (15)；

F₂＝F(t_n+1，Y_n+1) (16)；

结合公式(11)～(16)求取Rosenbrock改进法的截断误差error，所述截断误差error表示为：

k₃＝W^-1[F₂-e₃₂(k₂-F₁)-2(k₁-F₀)+hdT] (17)；

其中，

步骤5：触摸屏(1)将机器人的运动状态仿真结果以三维图形的方式进行展示。