机械臂控制方法、装置、系统、存储介质及机械臂与流程

1.本发明涉及工程器械领域，尤其是涉及一种机械臂控制方法、装置、系统、存储介质及机械臂。


背景技术：

2.在工地、矿山、巷道、隧道及某些特种作业现场，由于其作业空间比较狭小，环境恶劣，危险系数高，就要求特种作业机器人具备体积小，工作负载大，作业效率高的特点。并且在这些场合经常需要进行一些如破碎，钻孔，重物的取放等操作。在这些操作中，经常会涉及液压机械臂末端在笛卡尔坐标系下的直线行驶，如需要液压机械臂垂直于破碎物体表面直线前进一段距离，钻孔时需垂直孔表面直线前进一段距离，重物取放时需要垂直于重物直线前进一段距离。目前液压机械臂关节数较少，末端直线前进的能力有限，对操作者的技术提出较高要求。
3.基于上述问题，市面上出现了液压机械臂末端的直线推进控制技术，该技术可解决以上的痛点，极大的降低了对操作者的要求。但是该技术仍然需要操作人员人为操作机械臂末端的推进速度，无法实现对机械臂末端控制的全自动化操作，市面上也在此基础上推出过全自动化直线推进的方法，但是无法很好的调节机械臂末端的推进时间以及推进速度，使得机械臂推进过程中存在不平稳的问题，从而会直接影响后续机械臂的精确抓取和精准钻孔等功能的实现。


技术实现要素：

4.本发明旨在至少解决现有技术中存在的技术问题之一。为此，本发明提出一种机械臂控制方法，解决了现有机械臂末端自动化直线推进过程不平稳的问题。
5.本发明还提出了一种机械臂控制装置、系统、计算机可读存储介质和机械臂。
6.根据本发明的第一方面实施例的机械臂控制方法，包括：获取机械臂末端的终点目标位置信息以及当前位置信息；根据所述终点目标位置信息和所述当前位置信息确定所述机械臂末端的直线位移轨迹；根据所述直线位移轨迹和多个直线运行预设方程确定所述机械臂末端的直线运行轨迹通用方程，多个所述直线运行预设方程按照时域连续排列，每个所述直线运行预设方程皆表征时间与位移的对应关系，其中，排列首位的所述直线运行预设方程用于控制所述机械臂末端先进行加速直线运动后进行减速直线运动，排列尾端的所述直线运行预设方程用于控制所述机械臂末端进行减速直线运动，相邻两个所述直线运行预设方程之间速度连续、加速度连续、位移连续；利用群体优化算法确定每个所述直线运行预设方程对应的分段控制优化时长，并根据多个所述分段控制优化时长和所述直线运行预设方程得到直线运行轨迹执行方程；根据所述直线运行轨迹执行方程控制所述机械臂末端直线运动。
7.根据本发明实施例的机械臂控制方法，至少具有如下有益效果：通过预设多个直线运行预设方程并对多个直线运行预设方程对应的推进加速度进行限定，为机械臂末端先加速后减速的控制方式提供了基础，之后再利用群体优化算法得到每个直线运行预设方程对应的分段控制优化时长，从而可以实现以最短时间完成对直线位移轨迹的完整推进，且可以实现对每一个直线运行预设方程对应的分段控制优化时长的合理设置，最后，基于分段控制优化时长确定最终的直线运行轨迹执行方程，即可根据直线运行轨迹执行方程完成对机械臂末端平稳、快速的直线推进。本发明实施例的机械臂控制方法相较于传统的直线推进方式而言，预设了初始加速阶段和结尾减速阶段，为机械臂平稳控制提供了基础，并且，本发明实施例的机械臂控制方法可以实现对直线推进总时长的优化，以及对每一阶段的分段控制优化时长的合理设置，从而可以在有效提高推进速度的同时，保证推进的平稳。
8.根据本发明的一些实施例，排列首位的所述直线运行预设方程和排列尾端的所述直线运行预设方程皆采用三次多项式，排列在首位和尾端之间的所述直线运行预设方程皆采用一次多项式。
9.根据本发明的一些实施例，所述直线运行预设方程有三个；所述利用群体优化算法确定每个所述直线运行预设方程对应的分段控制优化时长，包括：构建三维欧式空间，并建立三维空间坐标系，三个所述直线运行预设方程中的时间分别为所述三维空间坐标系的x坐标轴、y坐标轴和z坐标轴；在所述三维欧式空间中初始化优化群体，所述优化群体包括多个待优化个体；计算每个所述待优化个体的适应度值，并按照适应度值大小进行排序；从多个所述待优化个体中选取适应度值最小的一个作为中间目标位置；基于所述中间目标位置执行迭代计算策略，直至迭代次数超过预设的迭代次数阈值或所述迭代计算策略输出的中间目标位置对应的适应度值小于等于预设的适应度阈值，得到最终目标位置；根据所述最终目标位置确定每个所述直线运行预设方程对应的分段控制优化时长；其中，所述迭代计算策略，包括：根据多个所述适应度值将多个所述待优化个体划分为领导者部分和跟随者部分，并分别对所述领导者部分中所述待优化个体和所述跟随者部分中所述待优化个体进行数据更新；从更新后的多个待优化个体中选取出中间目标位置。
10.根据本发明的一些实施例，所述从更新后的多个待优化个体中选取出中间目标位置，包括：对更新后的所述优化群体中每个所述待优化个体计算适应度值，并一一对应比较每个所述待优化个体计算的适应度值与前一次相同位置的所述待优化个体对应的适应度值，得到适应度比较结果；根据所述适应度比较结果对所述适应度值进行更新；选取更新之后最小的所述适应度值对应的所述待优化个体作为新的中间目标位
置。
11.根据本发明的一些实施例，在所述选取更新之后最小的所述适应度值对应的所述待优化个体作为新的中间目标位置之后，还包括：根据所述中间目标位置对应的所述待优化个体确定每个所述直线运行预设方程对应的速度和加速度；当所述速度超过最大预设速度值和/或所述加速度超过最大预设加速度值，将所述中间目标位置对应的所述待优化个体对应的适应度值设置为极大预设常数，所述极大预设常数用于使得对应的所述待优化个体的位置调整至多个所述待优化个体的尾端。
12.根据本发明的一些实施例，所述根据所述直线运行轨迹执行方程控制所述机械臂末端直线运动，包括：在机械臂末端直线运动轨迹中选取多个中间控制节点，所述机械臂末端直线运动轨迹根据所述直线运行轨迹执行方程得到；确定多个所述中间控制节点对应的中间速度、中间位移、中间时刻；根据多个所述中间控制节点对应的中间速度、中间位移、中间时刻完成对所述机械臂运动的分段直线控制。
13.根据本发明的一些实施例，所述根据所述直线运行轨迹执行方程控制所述机械臂末端直线运动，包括：以所述机械臂末端的当前位置信息为球心，以控制预设半径为半径求得控制球的球体方程；根据所述球体方程和所述直线运行轨迹执行方程确定下一个中间控制节点，并根据所述直线运行轨迹执行方程得到所述下一个中间控制节点对应的中间速度、中间位移、中间时刻；根据所述下一个中间控制节点对应的中间速度、中间位移、中间时刻控制所述机械臂末端运动至所述下一个中间控制节点。
14.根据本发明的第二方面实施例的机械臂控制装置，包括：数据获取单元，用于获取机械臂末端的终点目标位置信息以及当前位置信息；轨迹确定单元，用于根据所述终点目标位置信息和所述当前位置信息确定所述机械臂末端的直线位移轨迹；轨迹通用方程确定单元，用于根据所述直线位移轨迹和多个直线运行预设方程确定所述机械臂末端的直线运行轨迹通用方程，多个所述直线运行预设方程按照时域连续排列，每个所述直线运行预设方程皆表征时间与位移的对应关系，其中，排列首位的所述直线运行预设方程用于控制所述机械臂末端先进行加速直线运动后进行减速直线运动，排列尾端的所述直线运行预设方程用于控制所述机械臂末端进行减速直线运动，相邻两个所述直线运行预设方程之间速度连续、加速度连续、位移连续；轨迹执行方程确定单元，用于利用群体优化算法确定每个所述直线运行预设方程对应的分段控制优化时长，并根据多个所述分段控制优化时长和所述直线运行预设方程得到直线运行轨迹执行方程；运动控制单元，用于根据所述直线运行轨迹执行方程控制所述机械臂末端直线运动。
15.根据本发明实施例的机械臂控制装置，至少具有如下有益效果：通过预设多个直线运行预设方程并对多个直线运行预设方程对应的推进加速度进行限定，为机械臂末端先加速后减速的控制方式提供了基础，之后再利用群体优化算法得到每个直线运行预设方程对应的分段控制优化时长，从而可以实现以最短时间完成对直线位移轨迹的完整推进，且可以实现对每一个直线运行预设方程对应的分段控制优化时长的合理设置，最后，基于分段控制优化时长确定最终的直线运行轨迹执行方程，即可根据直线运行轨迹执行方程完成对机械臂末端平稳、快速的直线推进。本发明实施例的机械臂控制装置相较于传统的直线推进方式而言，预设了初始加速阶段和结尾减速阶段，为机械臂平稳控制提供了基础，并且，本发明实施例的机械臂控制装置可以实现对直线推进总时长的优化，以及对每一阶段的分段控制优化时长的合理设置，从而可以在有效提高推进速度的同时，保证推进的平稳。
16.根据本发明的第三方面实施例的机械臂控制系统，所述机械臂包括多个节臂，所述机械臂控制系统包括：数据采集模块，用于获取所述机械臂当前位置信息；控制模块，用于执行如上述的机械臂控制方法；执行模块，用于根据所述控制模块发送指令调整多个所述节臂的动作状态。
17.根据本发明实施例的机械臂控制系统，至少具有如下有益效果：由于控制模块采用了上述实施例的机械臂控制方法的全部技术方案，因此至少具有上述实施例的技术方案所带来的所有有益效果。
18.根据本发明的第四方面实施例的机械臂，包括多个节臂和如第三方面实施例的机械臂控制系统。由于包括如第三方面实施例的机械臂控制系统，因此至少具有上述实施例的技术方案所带来的所有有益效果。
19.根据本发明的第五方面实施例的计算机可读存储介质，存储有计算机可执行指令，所述计算机可执行指令用于执行如上述第一方面实施例所述的机械臂控制方法。由于计算机可读存储介质采用了上述实施例的机械臂控制方法的全部技术方案，因此至少具有上述实施例的技术方案所带来的所有有益效果。
20.本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。
附图说明
21.本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：图1是本发明一实施例提供的机械臂控制方法的流程图；图2是本发明一实施例提供的机械臂末端推进过程中推进位移和时间的相关关系图；图3是本发明一实施例提供的确定分段控制时长的流程图；图4是本发明一实施例提供的控制机械臂直线推进的流程图；图5是本发明另一实施例提供的控制机械臂直线推进的流程图；图6是图5所示方法控制机械臂直线推进的示意图。
具体实施方式
22.下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。
23.在本发明的描述中，如果有描述到第一、第二等只是用于区分技术特征为目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量或者隐含指明所指示的技术特征的先后关系。
24.在本发明的描述中，需要理解的是，涉及到方位描述，例如上、下等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
25.本发明的描述中，需要说明的是，除非另有明确的限定，设置、安装、连接等词语应做广义理解，所属技术领域技术人员可以结合技术方案的具体内容合理确定上述词语在本发明中的具体含义。
26.下面将结合附图对本发明的技术方案进行清楚、完整的描述，显然，以下所描述的实施例是本发明一部分实施例，并非全部实施例。
27.为了更好的完成对本发明实施例提供的机械臂控制方法、装置、系统、存储介质及机械臂的描述，这里提供了一种应用场景，该应用场景中，机械臂包括多个依次连接的节臂，每个节臂皆实现一个自由度的转动动作，每个节臂由对应的执行油缸驱动转动，每个节臂中具有检测该节臂转动的编码器或其他功能类似的传感器，多个节臂对应的多个执行油缸可以由同一组液压泵、液压主油箱完成供能。每个节臂对应的执行油缸皆由一个比例多路阀完成动作控制。在需要控制机械臂动作时，则可以通过控制模块向需要动作的节臂的比例阀发送控制电流信号调整比例阀流量即可完成对执行油缸动作的调整，从而实现对机械臂的驱动。需要说明的是，如果需要节臂动作的速度发生变化，则只需要对所有节臂对应的执行油缸的流量进行适应性调整即可。
28.需要说明的是，上述的应用场景仅仅是实施本发明实施例的机械臂控制方法、装置、系统、存储介质及机械臂的一种场景，并非对本发明保护范围的限定。
29.基于上述场景，提出本技术的各个实施例。下面结合附图，对本技术实施例作进一步阐述。
30.参见图1所示，图1是本发明一个实施例提供的机械臂控制方法的流程图，该机械臂控制方法包括但不限于以下步骤：获取机械臂末端的终点目标位置信息以及当前位置信息；根据终点目标位置信息和当前位置信息确定机械臂末端的直线位移轨迹；根据直线位移轨迹和多个直线运行预设方程确定机械臂末端的直线运行轨迹通用方程，多个直线运行预设方程按照时域连续排列，每个直线运行预设方程皆表征时间与位移的对应关系，其中，排列首位的直线运行预设方程用于控制机械臂末端先进行加速直线运动后进行减速直线运动，排列尾端的直线运行预设方程用于控制机械臂末端进行减速直线运动，相邻两个直线运行预设方程之间速度连续、加速度连续、位移连续；利用群体优化算法确定每个直线运行预设方程对应的分段控制优化时长，并根据
多个分段控制优化时长和直线运行预设方程得到直线运行轨迹执行方程；根据直线运行轨迹执行方程控制机械臂末端直线运动。
31.在控制机械臂末端直线推进前，需要先确定机械臂末端的当前位置信息，以及确定机械臂末端需要运动的终点目标位置信息，从可以利用这两个位置信息确定出一条直线运动轨迹。需要说明的是，当前位置信息可以直接通过读取每个节臂对应编码器的数据，然后利用机械臂运动学正解算法得到，而如何由当前位置信息移动到终点目标位置信息则可以直接通过机械臂运动学逆解算法来确定路径中每一个中间位置对应的节臂姿态，进而完成对机械臂末端直线推进的控制。
32.多个直线运行预设方程皆是表征时间和位移的对应关系，并且多个直线运行预设方程按照时序连续排列，在此基础上，可以为后续确定机械臂末端到达直线轨迹中任何一点的时间和速度提供基础，而为了更好的完成对机械臂末端推进平稳性的把控，则会在初始阶段采用加速推进，并在加速完成后逐步减速至匀速，在结尾阶段采用减速推进，而中间阶段则可以匀速推进，从而保证推进的平稳性。需要说明的是，基于直线轨迹构建的直线运行轨迹通用方程，本身并没有对各个直线运行预设方程的定义域进行限定，而实际操作中，直线轨迹本身长度也是不确定的，因此，不能简单的对各个直线运行预设方程进行固定的时间分段或固定的距离比例分段，如果使用时间分段，则会导致短距离推进也需要花费大量的时间，如果使用距离比例分段，则会导致推进的提速和减速阶段受到影响，从而极大的影响推进的效率。基于此，本发明实施例提出了利用群体优化算法来确定每一个直线运行预设方程的分段控制优化时长的技术手段，即，可以在保证机械推进安全性的前提下，得到推完整个直线轨迹的最短时间，并完成对各阶段推进时长的合理分配。在得到每一个分段控制优化时长后，便可以确定每一个直线运行预设方程的定义域，从而确定出最终的直线运行轨迹执行方程，而直线运行轨迹执行方程就确定了机械臂末端推进到每一个点所需的时间，即为精准控制提供了基础。
33.最后，控制器则可以根据执行油缸推进速度与比例阀控制电流的对应关系、执行油缸推进速度与机械臂末端推进速度的转换关系，完成对机械臂推进的控制，考虑到液压推进存在一定迟滞的影响，可以通过实时获取机械臂末端的推进位移与理论推进位移的比较差值作为负反馈来对推进速度进行一定的修正，从而保证整体推进过程的完全可控。
34.本发明实施例的机械臂控制方法通过预设多个直线运行预设方程并对多个直线运行预设方程对应的推进加速度进行限定，为机械臂末端先加速后减速的控制方式提供了基础，之后再利用群体优化算法得到每个直线运行预设方程对应的分段控制优化时长，从而可以实现以最短时间完成对直线位移轨迹的完整推进，且可以实现对每一个直线运行预设方程对应的分段控制优化时长的合理设置，最后，基于分段控制优化时长确定最终的直线运行轨迹执行方程，即可根据直线运行轨迹执行方程完成对机械臂末端平稳、快速的直线推进。本发明实施例的机械臂控制方法相较于传统的直线推进方式而言，预设了初始加速阶段和结尾减速阶段，为机械臂平稳控制提供了基础，并且，本发明实施例的机械臂控制方法可以实现对直线推进总时长的优化，以及对每一阶段的分段控制优化时长的合理设置，从而可以在有效提高推进速度的同时，保证推进的平稳。
35.在一些实施例中，排列首位的直线运行预设方程和排列尾端的直线运行预设方程皆采用三次多项式，排列在首位和尾端之间的直线运行预设方程皆采用一次多项式。
36.采用三次多项式控制加速和减速，既可以保证加速的平稳也可以保证加速的效率，而中间阶段采用一次多项式，则可以控制机械臂末端进行匀速运动，从而进一步提高推进的平稳，也可以避免机械臂超负荷工作的情况出现。本实施例中，直线运行预设方程有三个，直线运行轨迹通用方程公式可以参考如下：；；；式中，、、分别对应三段位移，t为时间；其中，三个直线运行预设方程最终确定后，会保持加速度连续、速度连续、位置连续，从而可以避免出现控制参数突变的情况出现。
37.在一些实施例中，参考图3，利用群体优化算法确定每个直线运行预设方程对应的分段控制优化时长，包括：构建三维欧式空间，并建立三维空间坐标系，三个直线运行预设方程中的时间分别为三维空间坐标系的x坐标轴、y坐标轴和z坐标轴；在三维欧式空间中初始化优化群体，优化群体包括多个待优化个体；计算每个待优化个体的适应度值，并按照适应度值大小进行排序；从多个待优化个体中选取适应度值最小的一个作为中间目标位置；基于中间目标位置执行迭代计算策略，直至迭代次数超过预设的迭代次数阈值或迭代计算策略输出的中间目标位置对应的适应度值小于等于预设的适应度阈值，得到最终目标位置；根据最终目标位置确定每个直线运行预设方程对应的分段控制优化时长；其中，迭代计算策略，包括：根据多个适应度值将多个待优化个体划分为领导者部分和跟随者部分，并分别对领导者部分中待优化个体和跟随者部分中待优化个体进行数据更新；从更新后的多个待优化个体中选取出中间目标位置。
38.在确定了直线运行轨迹通用方程后，便需要进一步确定出直线运行轨迹执行方程，而确定直线运行轨迹执行方程则需要优先确定出每个直线运行预设方程对应的分段控制优化时长，然后利用分段控制优化时长确定出直线运行轨迹执行方程。为了实现对每个直线运行预设方程对应的分段控制优化时长的合理设置，本实施例利用群体优化算法来确定分段控制优化时长。下面描述具体获取过程。
39.首先需要构建三维欧式空间，并构建对应的三维空间坐标系，而三个直线运行预设方程对应的自变量都是时间，这里为了便于描述分别将三个直线运行预设方程的结束时刻设置为、、（如图2所示），并将三个时间变量作为三维空间坐标系的三个坐标轴对应参数使用，因此，三维空间坐标系中的每个位置对应的坐标事实上分别表示了三个直线运行预设方程各自定义域结束的时刻，因此，只需要找到最优的位置，即最终目标位置，便可以确定出三个直线运行预设方程各自最优的控制时长。
40.构建完三维欧式空间后，则需要初始化群体，每个群体包含了多个待优化个体，多个待优化个体采用随机产生的方式诞生，从而可以避免人为因素的过多干扰，而群体中个
体数量则可以实际需求进行人为设定，具体设定数量则需要根据控制器的算力来进行设置，通常可以选用50至1000，既可以避免过少的个体导致选取的结果不够优秀，也可以避免过多的个体导致对算力进行大量占用，影响控制效率和其他功能的实现。可以理解的是，群体中的每个个体实质上都是一个三维欧式空间中的坐标点，因此，每个个体可以对应一个对直线运行轨迹通用方程的限定。需要说明的是，每一个、、都有着上边界和下边界的限制，即，不能过大也不能过小，这样可以有效的防止得到数据脱离实际物理意义，避免无法执行的方案出现。
41.初始化之后，则需要计算出每个个体的适应度，本实施例中，对于适应度的计算则可以直接采用、、的和来进行表征。适应度计算完成后，则需要对适应度进行一个排序，排序按照时间和的大小完成，并将时间和最小的排在队列首位，排在首位的个体，理论上便是本次计算中最优的结果，即可以算作本次计算的中间目标位置。而单次的计算结果通常很难直接满足适应度阈值的需求，因此会在得到本次的中间目标位置之后，执行迭代计算策略，直到找到满足适应度阈值的结果，或者需要迭代次数超过预设的迭代次数阈值，才能够结束，这样得到的中间目标位置，才是最终满足控制需求的结果，而该中间目标位置则会记作最终目标位置，记作最终目标位置对应的、、则可以用作确定每个直线运行预设方程对应的分段控制优化时长。
42.这里对迭代计算策略做进一步介绍。
43.在每一次迭代过程中，会将适应度值较小一半划分为领导者，将适应度值较大的一般划分为跟随者，然后先对领导者中的个体数据进行数据更新，再对更随者中个体数据进行更新，得到多个新的待优化个体，之后再重新计算多个待优化个体的适应度值，并基于新的适应度值确定出新的中间目标位置。
44.通过多次执行迭代计算策略最终选取出满足需求的中间目标位置作为最终目标位置。
45.为了更好的描述数据更新的过程，这里以一个具体的更新公式进行进一步的描述。
46.其中，领导者更新公式为：)；；式中，j表示维数；ub表示t1,t2,t3的上边界；lb表示t1,t2,t3的下边界；表示中间目标位置；r2,r3是0-1的随机数，当与本实施例中方法对应的程序执行好之后，便会一直处于随机变化的状态；,t表示当前迭代次数，t表示最大迭代次数。
47.利用上述公式，便可以快速的实现对领导者中个体数据的更新。
48.对应的追随者更新公式为：；式中，j表示维数，i表示当前迭代次数。即追随者中个体数据更新采用了本次数据与上一次数据取平均的方式进行更新。
49.在一些实施例中，从更新后的多个待优化个体中选取出中间目标位置，包括：对更新后的优化群体中每个待优化个体计算适应度值，并一一对应比较每个待优化个体计算的适应度值与前一次相同位置的待优化个体对应的适应度值，得到适应度比较结果；根据适应度比较结果对适应度值进行更新；选取更新之后最小的适应度值对应的待优化个体作为新的中间目标位置。
50.计算更新之后的待优化个体对应的适应度值，并比较每个待优化个体对应的当前适应度值与队列中相同位置的优化个体上一次计算得到的适应度值之间的大小，如果小于上一次得到的，则说明本次更新之后更加优异，则应当保留，如果更新之后的变大，则说明更新之后效果更差，则需要保留上一次计算的适应度值对应的结果，完成对所有位置的更新操作后，同样选取新的队列中适应度值最小的个体作为中间目标位置。
51.在一些实施例中，需要进一步考虑选取的中间目标位置是否符合逻辑需求，因此，在选取更新之后最小的适应度值对应的待优化个体作为新的中间目标位置之后，还包括以下步骤：根据中间目标位置对应的待优化个体确定每个直线运行预设方程对应的速度和加速度；当速度超过最大预设速度值和/或加速度超过最大预设加速度值，将中间目标位置对应的待优化个体对应的适应度值设置为极大预设常数，极大预设常数用于使得对应的待优化个体的位置调整至多个待优化个体的尾端。
52.这里设置了最大预设速度值和最大预设加速度值两个阈值，作为对实际选取的中间目标位置对应的时间参数是否具有实际意义的进一步限制，当根据得到中间目标位置得到的速度和加速度超过最大预设速度值和最大预设加速度值，则说明如果继续按照该组数据执行，会存在极大的安全隐患，或者说会超过机械臂运行的安全门限。此时，则会将该个体的适应度值设置为极大预设常数，使得该个体在后续数据更新过程中，会一直处于更随者队列，即，相当于在不改变优化群体中个体数目的同时实现了对该个体的有效剔除。需要说明的是，最大预设速度值和最大预设加速度值的设定，是根据机械臂或者说工程器械的自身的结构性能来确定的，因此，对于不同的工程器械而言，对应的最大预设速度值和最大预设加速度值会存在一定的变化，当然，最大预设速度值和最大预设加速度值也可以根据人为需求进行适当调小，但是禁止随意调大，避免出现安全事故。
53.经过了多次迭代，以及对每一次迭代输出的中间目标位置的筛选后，便可以确定出符合要求的中间目标位置，并记作最终目标位置。
54.在一些实施例中，参考图4，根据直线运行轨迹执行方程控制机械臂末端直线运动，包括：在机械臂末端直线运动轨迹中选取多个中间控制节点，机械臂末端直线运动轨迹根据直线运行轨迹执行方程得到；确定多个中间控制节点对应的中间速度、中间位移、中间时刻；根据多个中间控制节点对应的中间速度、中间位移、中间时刻完成对机械臂运动的分段直线控制。
55.在确定了直线运行轨迹执行方程后，便需要完成对机械臂末端运动轨迹的有效的
控制，为了保证控制平稳性和精度，这里使用了积分的思想，即，将原本的整条直线运动轨迹划分为多个连续子轨迹，从而可以确定出多个中间控制节点，因为直线轨迹和直线运行轨迹执行方程都已经确定，因此，可以确定出每一个中间控制节点对应的中间速度、中间位移、中间时刻，在此基础上，只需要控制机械臂末端在对应的中间时刻以中间速度倒到对应的中间位移即可，即，本实施例中通过分段控制的思想完成了机械臂直线运动的有效控制，避免了直接当前位置信息和终点目标位置信息作为起始点直接控制带来的误差。
56.在一些实施例中，参考图5，根据直线运行轨迹执行方程控制机械臂末端直线运动，包括：以机械臂末端的当前位置信息为球心，以控制预设半径为半径求得控制球的球体方程；根据球体方程和直线运行轨迹执行方程确定下一个中间控制节点，并根据直线运行轨迹执行方程得到下一个中间控制节点对应的中间速度、中间位移、中间时刻；根据下一个中间控制节点对应的中间速度、中间位移、中间时刻控制机械臂末端运动至下一个中间控制节点。
57.为了更好的实现对机械臂末端直线推进的平稳控制，本实施例中还提出了一种动态控制的方法。结合图6，首先以机械臂末端的当前位置信息为球心，以控制预设半径为半径，这个控制预设半径为人为指定的参数(理论上越小越好，考虑到控制器算力问题会进行合理的半径划分，具体大小可以依据算力来确定，即算力越强半径可以设置越小)，求得控制球的方程；通过联立机械臂末端直线轨迹方程与球的方程，获取到两个交点，此时舍去在机械臂末端后面的一个交点，即，机械臂末端在当前处于直线推进需求下，不会出现向后推进的可能性。求得交点后，便可以确定该点对应的速度、位移、时间，以及通过机械臂运动学逆解获取的各个关节的角度值。结合当前机械臂末端各个关节所处的位置，可以求得每个关节需要转动的角度，再结合时间，就可以求出每个关节运动的速度。这样机械臂从静止开始运动，当机械臂运动到时间，继续求球与直线轨迹的交点，这时获取时间,当机械臂到达时间后继续寻找时间，以此类推，直到机械臂走完全程的轨迹。
58.本实施例中动态控制的当时，相较于一次性直接计算出所有节点的方式而言，容错性更强，也可以将一次大量运算变成多次少量的运算，减少对控制器性能的影响。
59.本发明实施例还提供了一种机械臂控制装置，该机械臂控制装置包括：数据获取单元、轨迹确定单元、轨迹通用方程确定单元、轨迹执行方程确定单元和运动控制单元；数据获取单元，用于获取机械臂末端的终点目标位置信息以及当前位置信息；轨迹确定单元，用于根据终点目标位置信息和当前位置信息确定机械臂末端的直线位移轨迹；轨迹通用方程确定单元，用于根据直线位移轨迹和多个直线运行预设方程确定机械臂末端的直线运行轨迹通用方程，多个直线运行预设方程按照时域连续排列，每个直线运行预设方程皆表征时间与位移的对应关系，其中，排列首位的直线运行预设方程用于控制机械臂末端先进行加速直线运动后进行减速直线运动，排列尾端的直线运行预设方程用于控制机械臂末端进行减速直线运动，相邻两个直线运行预设方程之间速度连续、加速度
连续、位移连续；轨迹执行方程确定单元，用于利用群体优化算法确定每个直线运行预设方程对应的分段控制优化时长，并根据多个分段控制优化时长和直线运行预设方程得到直线运行轨迹执行方程；运动控制单元，用于根据直线运行轨迹执行方程控制机械臂末端直线运动。
60.在控制机械臂末端直线推进前，需要先确定机械臂末端的当前位置信息，以及确定机械臂末端需要运动的终点目标位置信息，从可以利用这两个位置信息确定出一条直线运动轨迹。需要说明的是，当前位置信息可以直接通过读取每个节臂对应编码器的数据，然后利用机械臂运动学正解算法得到，而如何由当前位置信息移动到终点目标位置信息则可以直接通过机械臂运动学逆解算法来确定路径中每一个中间位置对应的节臂姿态，进而完成对机械臂末端直线推进的控制。
61.多个直线运行预设方程皆是表征时间和位移的对应关系，并且多个直线运行预设方程按照时序连续排列，在此基础上，可以为后续确定机械臂末端到达直线轨迹中任何一点的时间和速度提供基础，而为了更好的完成对机械臂末端推进平稳性的把控，则会在初始阶段采用加速推进，并在加速完成后逐步减速至匀速，在结尾阶段采用减速推进，而中间阶段则可以匀速推进，从而保证推进的平稳性。需要说明的是，基于直线轨迹构建的直线运行轨迹通用方程，本身并没有对各个直线运行预设方程的定义域进行限定，而实际操作中，直线轨迹本身长度也是不确定的，因此，不能简单的对各个直线运行预设方程进行固定的时间分段或固定的距离比例分段，如果使用时间分段，则会导致短距离推进也需要花费大量的时间，如果使用距离比例分段，则会导致推进的提速和减速阶段受到影响，从而极大的影响推进的效率。基于此，本发明实施例提出了利用群体优化算法来确定每一个直线运行预设方程的分段控制优化时长的技术手段，即，可以在保证机械推进安全性的前提下，得到推完整个直线轨迹的最短时间，并完成对各阶段推进时长的合理分配。在得到每一个分段控制优化时长后，便可以确定每一个直线运行预设方程的定义域，从而确定出最终的直线运行轨迹执行方程，而直线运行轨迹执行方程就确定了机械臂末端推进到每一个点所需的时间，即为精准控制提供了基础。
62.最后，控制器则可以根据执行油缸推进速度与比例阀控制电流的对应关系、执行油缸推进速度与机械臂末端推进速度的转换关系，完成对机械臂推进的控制，考虑到液压推进存在一定迟滞的影响，可以通过实时获取机械臂末端的推进位移与理论推进位移的比较差值作为负反馈来对推进速度进行一定的修正，从而保证整体推进过程的完全可控。
63.本发明实施例的机械臂控制装置通过预设多个直线运行预设方程并对多个直线运行预设方程对应的推进加速度进行限定，为机械臂末端先加速后减速的控制方式提供了基础，之后再利用群体优化算法得到每个直线运行预设方程对应的分段控制优化时长，从而可以实现以最短时间完成对直线位移轨迹的完整推进，且可以实现对每一个直线运行预设方程对应的分段控制优化时长的合理设置，最后，基于分段控制优化时长确定最终的直线运行轨迹执行方程，即可根据直线运行轨迹执行方程完成对机械臂末端平稳、快速的直线推进。本发明实施例的机械臂控制装置相较于传统的直线推进方式而言，预设了初始加速阶段和结尾减速阶段，为机械臂平稳控制提供了基础，并且，本发明实施例的机械臂控制装置可以实现对直线推进总时长的优化，以及对每一阶段的分段控制优化时长的合理设
置，从而可以在有效提高推进速度的同时，保证推进的平稳。
64.本发明实施例还提供了一种机械臂控制系统，机械臂包括多个节臂，机械臂控制系统包括：数据采集模块，用于获取机械臂当前位置信息；控制模块，用于执行如上述的机械臂控制方法；执行模块，用于根据控制模块发送指令调整多个节臂的动作状态。
65.本发明实施例的机械臂控制系统由于控制模块采用了上述实施例的机械臂控制方法的全部技术方案，因此至少具有上述实施例的技术方案所带来的所有有益效果。
66.本发明实施例还提供了一种机械臂，包括多个节臂和如上述的机械臂控制系统。由于包括如上述的机械臂控制系统，因此至少具有上述实施例的技术方案所带来的所有有益效果。
67.此外，本发明的一个实施例还提供了一种计算机可读存储介质，该计算机可读存储介质存储有计算机可执行指令，该计算机可执行指令被一个处理器或控制模块执行，可使得上述处理器执行上述实施例中的机械臂控制方法，例如，执行以上描述的方法。
68.本领域普通技术人员可以理解，上文中所公开方法中的全部或某些步骤、系统可以被实施为软件、固件、硬件及其适当的组合。某些物理组件或所有物理组件可以被实施为由处理器，如中央处理器、数字信号处理器或微处理器执行的软件，或者被实施为硬件，或者被实施为集成电路，如专用集成电路。这样的软件可以分布在计算机可读介质上，计算机可读介质可以包括计算机存储介质或非暂时性介质和通信介质或暂时性介质。如本领域普通技术人员公知的，术语计算机存储介质包括在用于存储信息诸如计算机可读指令、数据结构、程序模块或其他数据的任何方法或技术中实施的易失性和非易失性、可移除和不可移除介质。计算机存储介质包括但不限于ram、rom、eeprom、闪存或其他存储器技术、cd-rom、数字多功能盘dvd或其他光盘存储、磁盒、磁带、磁盘存储或其他磁存储装置、或者可以用于存储期望的信息并且可以被计算机访问的任何其他的介质。此外，本领域普通技术人员公知的是，通信介质通常包含计算机可读指令、数据结构、程序模块或者诸如载波或其他传输机制之类的调制数据信号中的其他数据，并且可包括任何信息递送介质。
69.上面结合附图对本发明实施例作了详细说明，但是本发明不限于上述实施例，在所属技术领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下作出各种变化。

技术特征：
1.一种机械臂控制方法，其特征在于，包括：获取机械臂末端的终点目标位置信息以及当前位置信息；根据所述终点目标位置信息和所述当前位置信息确定所述机械臂末端的直线位移轨迹；根据所述直线位移轨迹和多个直线运行预设方程确定所述机械臂末端的直线运行轨迹通用方程，多个所述直线运行预设方程按照时域连续排列，每个所述直线运行预设方程皆表征时间与位移的对应关系，其中，排列首位的所述直线运行预设方程用于控制所述机械臂末端先进行加速直线运动后进行减速直线运动，排列尾端的所述直线运行预设方程用于控制所述机械臂末端进行减速直线运动，相邻两个所述直线运行预设方程之间速度连续、加速度连续、位移连续；利用群体优化算法确定每个所述直线运行预设方程对应的分段控制优化时长，并根据多个所述分段控制优化时长和所述直线运行预设方程得到直线运行轨迹执行方程；根据所述直线运行轨迹执行方程控制所述机械臂末端直线运动。2.根据权利要求1所述的机械臂控制方法，其特征在于，排列首位的所述直线运行预设方程和排列尾端的所述直线运行预设方程皆采用三次多项式，排列在首位和尾端之间的所述直线运行预设方程皆采用一次多项式。3.根据权利要求1所述的机械臂控制方法，其特征在于，所述直线运行预设方程有三个；所述利用群体优化算法确定每个所述直线运行预设方程对应的分段控制优化时长，包括：构建三维欧式空间，并建立三维空间坐标系，三个所述直线运行预设方程中的时间分别为所述三维空间坐标系的x坐标轴、y坐标轴和z坐标轴；在所述三维欧式空间中初始化优化群体，所述优化群体包括多个待优化个体；计算每个所述待优化个体的适应度值，并按照适应度值大小进行排序；从多个所述待优化个体中选取适应度值最小的一个作为中间目标位置；基于所述中间目标位置执行迭代计算策略，直至迭代次数超过预设的迭代次数阈值或所述迭代计算策略输出的中间目标位置对应的适应度值小于等于预设的适应度阈值，得到最终目标位置；根据所述最终目标位置确定每个所述直线运行预设方程对应的分段控制优化时长；其中，所述迭代计算策略，包括：根据多个所述适应度值将多个所述待优化个体划分为领导者部分和跟随者部分，并分别对所述领导者部分中所述待优化个体和所述跟随者部分中所述待优化个体进行数据更新；从更新后的多个待优化个体中选取出中间目标位置。4.根据权利要求3所述的机械臂控制方法，其特征在于，所述从更新后的多个待优化个体中选取出中间目标位置，包括：对更新后的所述优化群体中每个所述待优化个体计算适应度值，并一一对应比较每个所述待优化个体计算的适应度值与前一次相同位置的所述待优化个体对应的适应度值，得到适应度比较结果；
根据所述适应度比较结果对所述适应度值进行更新；选取更新之后最小的所述适应度值对应的所述待优化个体作为新的中间目标位置。5.根据权利要求4所述的机械臂控制方法，其特征在于，在所述选取更新之后最小的所述适应度值对应的所述待优化个体作为新的中间目标位置之后，还包括：根据所述中间目标位置对应的所述待优化个体确定每个所述直线运行预设方程对应的速度和加速度；当所述速度超过最大预设速度值和/或所述加速度超过最大预设加速度值，将所述中间目标位置对应的所述待优化个体对应的适应度值设置为极大预设常数，所述极大预设常数用于使得对应的所述待优化个体的位置调整至多个所述待优化个体的尾端。6.根据权利要求1所述的机械臂控制方法，其特征在于，所述根据所述直线运行轨迹执行方程控制所述机械臂末端直线运动，包括：以所述机械臂末端的当前位置信息为球心，以控制预设半径为半径求得控制球的球体方程；根据所述球体方程和所述直线运行轨迹执行方程确定下一个中间控制节点，并根据所述直线运行轨迹执行方程得到所述下一个中间控制节点对应的中间速度、中间位移、中间时刻；根据所述下一个中间控制节点对应的中间速度、中间位移、中间时刻控制所述机械臂末端运动至所述下一个中间控制节点。7.一种机械臂控制装置，其特征在于，包括：数据获取单元，用于获取机械臂末端的终点目标位置信息以及当前位置信息；轨迹确定单元，用于根据所述终点目标位置信息和所述当前位置信息确定所述机械臂末端的直线位移轨迹；轨迹通用方程确定单元，用于根据所述直线位移轨迹和多个直线运行预设方程确定所述机械臂末端的直线运行轨迹通用方程，多个所述直线运行预设方程按照时域连续排列，每个所述直线运行预设方程皆表征时间与位移的对应关系，其中，排列首位的所述直线运行预设方程用于控制所述机械臂末端先进行加速直线运动后进行减速直线运动，排列尾端的所述直线运行预设方程用于控制所述机械臂末端进行减速直线运动，相邻两个所述直线运行预设方程之间速度连续、加速度连续、位移连续；轨迹执行方程确定单元，用于利用群体优化算法确定每个所述直线运行预设方程对应的分段控制优化时长，并根据多个所述分段控制优化时长和所述直线运行预设方程得到直线运行轨迹执行方程；运动控制单元，用于根据所述直线运行轨迹执行方程控制所述机械臂末端直线运动。8.一种机械臂控制系统，其特征在于，所述机械臂包括多个节臂，所述机械臂控制系统包括：数据采集模块，用于获取所述机械臂当前位置信息；控制模块，用于执行如权利要求1至6任一所述的机械臂控制方法；执行模块，用于根据所述控制模块发送指令调整多个所述节臂的动作状态。9.一种机械臂，其特征在于，包括多个节臂和如权利要求8所述的机械臂控制系统。10.一种计算机可读存储介质，其特征在于：所述计算机可读存储介质存储有计算机可
执行指令，所述计算机可执行指令用于使计算机执行如权利要求1至6任一所述的机械臂控制方法。

技术总结
本发明公开了一种机械臂控制方法、装置、系统、存储介质及机械臂，通过预设多个直线运行预设方程并对多个直线运行预设方程对应的推进加速度进行限定，之后再利用群体优化算法得到每个直线运行预设方程对应的分段控制优化时长，从而可以实现以最短时间完成对直线位移轨迹的完整推进，且可以实现对每一个直线运行预设方程对应的分段控制优化时长的合理设置，最后，基于分段控制优化时长确定最终的直线运行轨迹执行方程，即可根据直线运行轨迹执行方程完成对机械臂末端平稳、快速的直线推进。本发明实现了对直线推进总时长的优化，以及对每一阶段的分段控制优化时长的合理设置，在有效提高推进速度的同时保证推进的平稳。在有效提高推进速度的同时保证推进的平稳。在有效提高推进速度的同时保证推进的平稳。


技术研发人员：卢由 孟祥晶 黄旺斌 王中文
受保护的技术使用者：湖南尖山智能科技有限责任公司
技术研发日：2023.07.13
技术公布日：2023/8/14