求一份工业机器人轨迹优化算法

本人需要点六轴工业机械人的轨迹优化算法，软件不限制，理论部分知识加上后续的算法仿真结果，并应用于西门子S7-200smart PLC上不一定要全应用上，至少逻辑上是连贯通顺就行。机械手如下图所示。