码垛机器人最优轨迹运动控制指令实现 空间连续指令MC_ContinuousCurve实现了控制码垛机器人末端按照型值点引脚PointsArray以及连续曲线参数CurveP~配置好的空间曲线轨迹运动。其实现过程如图4.巧所示。 (2)机器人模型 机器人模型对应于机器人运动学中位置反解，由第二章分析可知该机器人模型为圆柱坐标系模型，如图4.17所示。 用户输入空间位置(X,Y,Z,A)后，经过该模型变换，转换为对应的腰部、水平、竖直、手爪四个轴上的脉冲输出。其映射关系对应于码垛机器人运动学位置反解，如公式(4.27)所示。 (3)点序列管理 点序列管理模块引进双缓冲队列管理机制，插补点队列和脉冲队列。其中插补点队列用来缓冲功能块或插补器插补之后离散出来的点位信息。脉冲队列接收插补点队列的信息，并转换成标有频率和该频率持续时长的脉冲点信息。 其工作流程图如图4.18所示。 其中，插补定时器和出点定时器为MCU控制的普通定时器，当插补定时中断触发时，判断当前轨迹插补是否结束，如果结束，则退出当前流程，如果未结束，则检查插补点队列是否可以入队，如果队列非满，则可以入队。出点定时中断触发时，检查脉冲队列是否可以入队，如果插补点队列非空，而脉冲点队列达到接近队满的状态下，则可以入队。这些脉冲点以设定的插补周期，通过定时中断，发送给FPGA，由FPGA控制驱动器动作。 (4)轴组处理 码垛机器人的高速运动时不仅需要单个执行机构运动的位置，而且需要所有执行机构的同步运动。因为码垛机器人的四个自由度的驱动机构是完全解祸的，所以在控制一个轴运动时，无需考虑其他轴的影响。 系统为每个轴分配一个队列，缓存功能块或插补器计算输出的点位信息，通过系统定时器，以固定的间隔将数据发送给IO设备，通过时钟同步协调多个轴一起运行。 数据队列中保存的是IO设备运行的实际可用的参数，其包括运行位移、运行速度及运行起始时间几个参数，有了这些数据，IO设备不关心曲线的内容，只要接收到数据就进行相应的输出，从而对电机进行控制。在控制IO设备运行的同时，读取设备的运行状态，当设置值与实际运行值存在偏差时，停止设备运行。 (5)位置补偿因子 由于进行数据交换的坐标点信息为浮点数，而输出的脉冲个数只能为整数，这样如果舍弃小数部分，会造成累积误差。为了消除累积误差，两个周期的点数据做差其整数部分控制脉冲个数，而小数部分并不舍弃而是保存起来，这样每个周期的小数部分都会累加保存，当累加的浮点数大于1时，则将其减1，并在本轮的高速脉冲个数中加1,将误差减少到最小。其运行流程如图4.19所示。 这样目标位置的小数部分会不断累加，并会在大于1的情况下补偿到高速输出中，消除实际运行中的误差。 本章主要针对码垛机器人的轨迹规划的要求以及轨迹规划方案进行理论介绍，分析了不同的速度曲线，选取T型速度曲线进行运动约束，并对最优加速度运动轨迹速度曲线进行了理论计算与仿真分析，从理论上验证了所推导出算法的可行性。并且详细介绍了该算法的实现过程。

• 上一篇：码垛机器人动力学性能的研究
• 下一篇：东京大学打造新型运动人形机器人：技能满满还会“流汗”