[发明专利]基于Python的桌面级机器人离线编程仿真系统

权利要求书

1.基于Python的桌面级机器人离线编程仿真系统，其特征在于：

离线编程仿真系统依次包括用户交互模块、模型管理模块、运动轨迹规划模块、离线编程模块、仿真与后置处理器模块、机器人运动模块；用户交互模块通过向模型管理模块，运动轨迹规划模块，离线编程模块发送控制指令，给仿真与后置处理器模块提供所需的全部数据；机器人运动模块根据仿真与后置处理器模块发送的控制命令来实现相应操作；

各模块的具体构成是：

1)用户交互模块，是一个良好的人机交互和机器人控制界面；用户交互模块接受来自用户输入的触发信号后，分别激活模型管理模块，运动轨迹规划模块，离线编程模块；用户在用户交互模块上点击各模块的操作命令，以网络通信的方式向模型管理模块，运动轨迹规划模块和离线编程模块发送任务操作指令；用户交互模块通过串口通讯方式，接受来自机器人运动模块的关节角度信息；

2)模型管理模块，用于导入机器人3D模型并建立相应的参考坐标系；模型管理模块接受来自用户交互模块的激活命令时，进入执行状态；获取来自用户交互模块的操作指令后，在用户操作系统的指定文件夹中导出机器人3D模型到模型管理模块，同时根据用户输入的坐标设置要求，设置机器人3D模型的基坐标系位置和各关节坐标系相对位置，建立机器人模型的工作空间，添加各关节的调试控件，最后把设置好的机器人3D模型通过网络通信发送给仿真与后置处理器模块，同时把模型的各坐标参数发送给运动轨迹规划模块；

3)运动轨迹规划模块，接受来自模型管理模块中的机器人3D模型的基坐标系位置参数，各关节轴的相对位置参数；分析桌面机器人末端执行器在空间中的瞬时位置和姿态，使用D-H参数建模机制，建立机器人每个连杆在各关节处的笛卡尔坐标系，使用正运动学公式求解末端执行器相对固定参考坐标系的位置和姿态，获得机器人运动轨迹，使用笛卡尔轨迹规划算法优化轨迹曲线，基于机器人逆运动学知识，把轨迹曲线转化为机器人关节参数数组；最后把该串机器人关节参数发送给离线编程模块；

4)离线编程模块，接受来自运动轨迹规划模块的机器人关节参数数组，首先对该数组进行类型分析，根据Python内核指令调用RoboDK动态函数库，把关节参数数组直接转化为Python编程代码，添加脚本头部和运行主函数后，通过网络通讯的方式把生成的Python脚本发送给机器人仿真与后置处理器模块；

5)仿真与后置处理器模块，接受分别来自模型管理模块的机器人3D模型文件，运动轨迹规划模块的机器人末端执行器运行曲线参数以及离线编程模块的机器人仿真运行脚本后，仿真与后置处理器模块等待以上各模块传输完各自的参数内容后，自动运行机器人仿真程序；先对运动轨迹规划模块接受的运行轨迹和离线编程模块接受的机器人末端执行器运行曲线指令串进行时间轴匹配和优化，并把优化结果发送给后置处理器程序；后置处理器程序根据匹配的机器人规格和RoboDK中机器人D-H参数库，把仿真程序直接转化为实际机器人可执行代码，通过USB有线传输的方式发送给机器人运动模块；

6)机器人运动模块，通过USB有线传输方式，接受来自仿真与后置处理器模块输出的机器人可执行代码脚本，并把代码设置成执行队列的形式，控制机器人执行运行任务，同时把机器人运行参数，机器人各关节角运动参数，实际机器人发生碰撞时反馈回来的故障信息参数发送给用户交互模块。

2.根据权利要求1所述的基于Python的桌面级机器人离线编程仿真系统，其特征在于：所述的运动轨迹规划模块，结合了正逆运动学求解和笛卡尔轨迹规划算法来优化运行轨迹，再将优化后的轨迹转化为机器人各关节角度值输出给离线编程模块和仿真与后置处理器模块使用，所述的正逆运动学求解和笛卡尔轨迹规划算法如下：

2.1)选择机器人四个关节的D-H参数作为输入量，四个关节的D-H参数是关节1≤i≤6的连杆长度a_i、连杆转角α_i、连杆偏距d_i和关节角θ_i，选择工作机器人各个关节的关节角θ_i作为输出量；

2.2)首先对机器人构建其D-H参数模型，由于各杆件相对参考坐标系有转动和平移两个动作，因此对每个杆件沿关节轴建立一个关节坐标系，用以下四个参数来描述杆件：

(1)连杆长度a_i：关节轴i和关节轴i+1之间公垂线的长度；

(2)连杆转角α_i：作一个与两关节轴之间的公垂线垂直的平面，将关节轴i和关节轴i+1投影到该平面，在平面内轴i按右手法则绕a_i转向轴i+1，其转角即为连杆转角；

(3)连杆偏距d_i：公垂线a_i-1与关节轴i的交点到公垂线a_i与关节轴i+1的交点的有向距离长度；

(4)关节角θ_i：a_i-1的延长线与a_i之间绕关节轴i旋转所形成的夹角，即采集得到的示教机器人关节角度；

2.3)以基坐标系作为T₀坐标系，设表示i关节坐标系相对i-1关节坐标系的位置和姿态，则：

其中，sθ_i＝sinθ_i，cθ_i＝cosθ_i，sα_i＝sinα_i，cα_i＝cosα_i；

2.4)对于四自由度机器人，获得机器人末端执行器位姿相对固定参考坐标系的变换矩阵，即机器人末端位置点坐标的变换矩阵：

其中，是机器人第四轴末端执行器相对基坐标系的变换矩阵；

2.5)求得规划运行后的机器人末端执行器位姿相对固定参考坐标系的变换矩阵后，即可获得由每个时间段机器人的末端执行器位姿相对固定参考坐标系的坐标点组成的离散运行轨迹；

2.6)使用笛卡尔圆弧插补法，先取出规划运行轨迹中的p₁(x₁,y₁,z₁)、p₂(x₂,y₂,z₂)和p₃(x₃,y₃,z₃)三点，这三点组成的平面方程为Ax+By+Cz＝1，则可由下式解得平面方程参数A、B和C的值：

2.7)求出p₁p₂和p₂p₃两条线段的中点p₁₂(x₁₂,y₁₂,z₁₂)，p₂₃(x₂₃,y₂₃,z₂₃)，设圆心坐标为p_o(x_o,y_o,z_o)，求解获得圆心坐标，以圆心为球心，r为半径的球参数方程为：

2.8)将上式代入平面方程式Ax+By+Cz＝1中，得到：

A(rcosφcosα+x_o)+B(rcosφsinα+y_o)+C(rsinα+z_o)＝1 (5)

从而获得空间圆的参数方程为：

其中φ为空间圆弧的反正切值；

2.9)取矢量为X₁轴向量，平面法向量为Z₁轴向量，建立T₁坐标系，由于姿态变换矩阵为单位正交矩阵，则有Y₁＝X₁×Z₁，进而可以求得Y₁轴向量，其姿态变换矩阵R和平移向量T为：

设则有：

姿态变换矩阵R和平移向量T可以得到坐标变换矩阵Q为：

2.10)取末端轨迹在T₁的位置坐标为p'(x',y',z')，通过姿态变换矩阵和平移向量可以求得末端轨迹在T₀中的位置p(x,y,z)，即为圆弧插补的规划轨迹；

2.11)在优化后的轨迹上选择需要的插补点，并列出每个点p(x,y,z)在该时刻机器人的末端执行器位姿相对固定参考坐标系的变换矩阵：

2.12)对每个优化后的插补点p(x,y,z)进行逆运动学求解，其中r_ij取与插补点最近的末端坐标点的变换矩阵中的值，则可按顺序求解得到：

θ₁表示第一个关节角度参数，求解方法为

由第一个关节角，通过中间运算公式可得第二个和第三个关节角为

其中，a₂为连杆2的长度，d₄为连杆4的偏距，c₃是cosθ₃的缩写，s₃是sinθ₃的缩写，K为逆运动学求解时的中间变量；

以此类推可以求出第四关节角θ₄，并通过c₅和s₅求出θ₅

θ₄＝Atan2(-r₁₃s₁+r₂₃c₁,-r₁₃c₁c₂₃-r₂₃s₁c₂₃+r₃₃s₂₃) (13)

2.13)在每个关节角度求出后，接着在每个关节可活动的范围内通过对关节角加减360°，找出所有的可能值，再利用连续性原则从所有的可能值中选择唯一合理值，即与上一时刻该关节的角度值最接近的值；求出每个优化后的末端位姿对应的各个关节角，发送给离线编程模块，把关节角参数转化为仿真与后置处理器模块可直接执行的代码。