一种冗余自由度机械臂运动规划方法及装置与流程
本申请涉及智能控制技术领域，具体而言，涉及一种冗余自由度机械臂运动规划方法及装置。
背景技术：
冗余自由度机械臂，即具有七个连杆的机械臂，其具有七个自由度，相比六个连杆的机械臂，额外的连杆允许机械臂躲避某些特定的目标，便于末端执行器到达特定的位置，可以更加灵活的适应某些特殊工作环境。
运动规划(motionplanning)就是在给定的位置a与位置b之间为机器人找到一条符合约束条件的路径，这个约束可以是无碰撞、路径最短、机械功最小等。
目前，冗余自由度机械臂的运动规划大多通过moveit运动规划框架进行，moveit运动规划框架主要采用牛顿法和序列二次规划法来计算冗余自由度机械臂运动规划中的运动学逆解，然而，采用牛顿法和序列二次规划法来计算冗余自由度机械臂运动规划中的运动学逆解往往计算时间较长，且计算难度较高，从而导致冗余自由度机械臂运动规划的总时间较长、规划效率较低。
技术实现要素：
本申请实施例的目的在于提供一种冗余自由度机械臂运动规划方法及装置，能对冗余自由度机械臂运动规划中的运动学逆解进行降维处理，减少计算时间，降低计算难度，从而能够更快速、更容易地得到冗余自由度机械臂的控制方案，完成运动规划，减少运动规划的总时间，提高规划效率。
第一方面，本申请实施例提供了一种冗余自由度机械臂运动规划方法，包括：
获取与所述冗余自由度机械臂的末端执行器的期望位姿相对应的位姿数据；
根据预设的约束条件对所述冗余自由度机械臂运动规划中的七维优化问题进行降维处理，得到二维优化问题；
对所述二维优化问题进行求解，得到求解结果；
根据所述位姿数据和所述求解结果，得到所述冗余自由度机械臂的控制方案。
在上述实现过程中，通过预设的约束条件对冗余自由度机械臂运动规划中的运动学逆解的七维优化问题进行降维处理，得到二维优化问题；再对二维优化问题进行求解，得到求解结果；根据获取的与冗余自由度机械臂的末端执行器的期望位姿相对应的位姿数据和前述求解结果，得到冗余自由度机械臂的控制方案，二维优化问题的求解时间较短，求解难度较低，减少了冗余自由度机械臂运动规划中的运动学逆解的计算时间，降低了冗余自由度机械臂运动规划中的运动学逆解的计算难度，从而在冗余自由度机械臂的运动规划时能够更快速、更容易地得到冗余自由度机械臂的控制方案，完成运动规划，减少运动规划的总时间，提高规划效率。
进一步地，所述预设的约束条件包括：
所述冗余自由度机械臂的七个关节分为三组，其中，第一组包括所述冗余自由度机械臂的第一关节至第四关节，第二组包括所述冗余自由度机械臂的第五关节和第六关节，第三组包括所述冗余自由度机械臂的第七关节；以及
所述冗余自由度机械臂第五关节的位置以所述位姿数据和第七关节的角度确定；以及
所述冗余自由度机械臂的第三关节固定；以及
所述冗余自由度机械臂第四关节与第五关节的轴线和第五关节与第六关节的轴线相垂直。
在上述实现过程中，冗余自由度机械臂的七个关节分为三组，第一组包括冗余自由度机械臂的第一关节至第四关节，第二组包括冗余自由度机械臂的第五关节和第六关节，第三组包括冗余自由度机械臂的第七关节；冗余自由度机械臂第五关节的位置以上述位姿数据和第七关节的角度确定；冗余自由度机械臂的第三关节固定，即第三关节是相对固定的，从而冗余自由度机械臂的第一关节、第二关节、第四关节的角度存在有限组解；若冗余自由度机械臂的第一关节至第四关节的角度确定后，冗余自由度机械臂的第五关节与第六关节的角度可根据第四关节与第五关节的轴线和第五关节与第六关节的轴线相垂直得到有限组解，前述分析是根据预设的约束条件对冗余自由度机械臂运动规划中的运动学逆解的七维优化问题进行降维处理，得到二维优化问题的思路，在此约束条件下，可以理解的是，第三关节的角度及第七关节的角度为自由变量，其余关节的角度为参变量，冗余自由度机械臂的第三关节对机械臂的臂型影响较大，此约束条件不仅能较好地将冗余自由度机械臂运动规划中的运动学逆解的七维优化问题降维成二维优化问题，还能够保障在此约束条件下冗余自由度机械臂具有较高的灵活度。
进一步地，在所述根据预设的约束条件对所述冗余自由度机械臂运动规划中的七维优化问题进行降维处理，得到二维优化问题的步骤之后，所述对所述二维优化问题进行求解，得到求解结果的步骤之前，所述方法还包括：
建立所述冗余自由度机械臂的第五关节至第七关节的碰撞检测模型；
以所述碰撞检测模型为依据，对所述冗余自由度机械臂的第五关节至第七关节进行碰撞检测。
在上述实现过程中，在冗余自由度机械臂运动规划中的运动学逆解的七维优化问题降维成二维优化问题后，建立冗余自由度机械臂的第五关节至第七关节的碰撞检测模型，并以碰撞检测模型为依据，对冗余自由度机械臂的第五关节至第七关节进行碰撞检测，能减少二维优化问题求解时的计算量，从而进一步地缩短二维优化问题的求解时间，降低二维优化问题的求解难度，进而进一步地减少冗余自由度机械臂运动规划中的运动学逆解的计算时间，降低冗余自由度机械臂运动规划中的运动学逆解的计算难度，以进一步地减少运动规划的总时间，提高规划效率。
进一步地，所述二维优化问题的数学规划形式为：
s.t.ml14＝mr14,ml24＝mr24,ml34＝mr34
其中，q3为所述冗余自由度机械臂的第三关节的角度，q7为所述冗余自由度机械臂的第七关节的角度，mr11为mr转移矩阵中第1行第1列的元素，ml21为ml转移矩阵中第2行第1列的元素。
在上述实现过程中，二维优化问题的数学规划形式是在某种预设的约束条件下，以冗余自由度机械臂的第三关节的角度及第七关节的角度为自由变量的数学规划形式，冗余自由度机械臂的第三关节对机械臂的臂型影响较大，在此二维优化问题的数学规划形式下，冗余自由度机械臂能具有较高的灵活度。
进一步地，所述对所述二维优化问题进行求解，得到求解结果的步骤包括：
通过全局优化方法和/或局部优化方法，对所述二维优化问题进行求解，得到求解结果。
在上述实现过程中，在冗余自由度机械臂运动规划中的运动学逆解的七维优化问题降维成二维优化问题后，二维优化问题的求解难度较二维优化问题的求解难度要低得多，二维优化问题的求解可通过全局优化方法和/或局部优化方法多种求解方式进行求解。
第二方面，本申请实施例提供了一种冗余自由度机械臂运动规划装置，包括：
获取单元，用于获取与所述冗余自由度机械臂的末端执行器的期望位姿相对应的位姿数据；
处理单元，用于根据预设的约束条件对所述冗余自由度机械臂运动规划中的七维优化问题进行降维处理，得到二维优化问题；
求解单元，用于对所述二维优化问题进行求解，得到求解结果；
生成单元，用于根据所述位姿数据和所述求解结果，得到所述冗余自由度机械臂的控制方案。
在上述实现过程中，通过预设的约束条件对冗余自由度机械臂运动规划中的运动学逆解的七维优化问题进行降维处理，得到二维优化问题；再对二维优化问题进行求解，得到求解结果；根据获取的与冗余自由度机械臂的末端执行器的期望位姿相对应的位姿数据和前述求解结果，得到冗余自由度机械臂的控制方案，二维优化问题的求解时间较短，求解难度较低，减少了冗余自由度机械臂运动规划中的运动学逆解的计算时间，降低了冗余自由度机械臂运动规划中的运动学逆解的计算难度，从而在冗余自由度机械臂的运动规划时能够更快速、更容易地得到冗余自由度机械臂的控制方案，完成运动规划，减少运动规划的总时间，提高规划效率。
进一步地，所述预设的约束条件包括：
所述冗余自由度机械臂的七个关节分为三组，其中，第一组包括所述冗余自由度机械臂的第一关节至第四关节，第二组包括所述冗余自由度机械臂的第五关节和第六关节，第三组包括所述冗余自由度机械臂的第七关节；以及
所述冗余自由度机械臂第五关节的位置以所述位姿数据和第七关节的角度确定；以及
所述冗余自由度机械臂的第三关节固定；以及
所述冗余自由度机械臂第四关节与第五关节的轴线和第五关节与第六关节的轴线相垂直。
在上述实现过程中，冗余自由度机械臂的七个关节分为三组，第一组包括冗余自由度机械臂的第一关节至第四关节，第二组包括冗余自由度机械臂的第五关节和第六关节，第三组包括冗余自由度机械臂的第七关节；冗余自由度机械臂第五关节的位置以上述位姿数据和第七关节的角度确定；冗余自由度机械臂的第三关节固定，即第三关节是相对固定的，从而冗余自由度机械臂的第一关节、第二关节、第四关节的角度存在有限组解；若冗余自由度机械臂的第一关节至第四关节的角度确定后，冗余自由度机械臂的第五关节与第六关节的角度可根据第四关节与第五关节的轴线和第五关节与第六关节的轴线相垂直得到有限组解，前述分析是根据预设的约束条件对冗余自由度机械臂运动规划中的运动学逆解的七维优化问题进行降维处理，得到二维优化问题的思路，在此约束条件下，可以理解的是，第三关节的角度及第七关节的角度为自由变量，其余关节的角度为参变量，冗余自由度机械臂的第三关节对机械臂的臂型影响较大，此约束条件不仅能较好地将冗余自由度机械臂运动规划中的运动学逆解的七维优化问题降维成二维优化问题，还能够保障在此约束条件下冗余自由度机械臂具有较高的灵活度。
进一步地，所述冗余自由度机械臂运动规划装置还包括：
建模单元，用于建立所述冗余自由度机械臂的第五关节至第七关节的碰撞检测模型；
检测单元，用于以所述碰撞检测模型为依据，对所述冗余自由度机械臂的第五关节至第七关节进行碰撞检测。
在上述实现过程中，在冗余自由度机械臂运动规划中的运动学逆解的七维优化问题降维成二维优化问题后，建立冗余自由度机械臂的第五关节至第七关节的碰撞检测模型，并以碰撞检测模型为依据，对冗余自由度机械臂的第五关节至第七关节进行碰撞检测，能减少二维优化问题求解时的计算量，从而进一步地缩短二维优化问题的求解时间，降低二维优化问题的求解难度，进而进一步地减少冗余自由度机械臂运动规划中的运动学逆解的计算时间，降低冗余自由度机械臂运动规划中的运动学逆解的计算难度，以进一步地减少运动规划的总时间，提高规划效率。
进一步地，所述二维优化问题的数学规划形式为：
s.t.ml14＝mr14,ml24＝mr24,ml34＝mr34
其中，q3为所述冗余自由度机械臂的第三关节的角度，q7为所述冗余自由度机械臂的第七关节的角度，mr11为mr转移矩阵中第1行第1列的元素，ml21为ml转移矩阵中第2行第1列的元素。
在上述实现过程中，二维优化问题的数学规划形式是在某种预设的约束条件下，以冗余自由度机械臂的第三关节的角度及第七关节的角度为自由变量的数学规划形式，冗余自由度机械臂的第三关节对机械臂的臂型影响较大，在此二维优化问题的数学规划形式下，冗余自由度机械臂能具有较高的灵活度。
进一步地，所述求解单元具体用于：
通过全局优化方法和/或局部优化方法，对所述二维优化问题进行求解，得到求解结果。
在上述实现过程中，在冗余自由度机械臂运动规划中的运动学逆解的七维优化问题降维成二维优化问题后，二维优化问题的求解难度较二维优化问题的求解难度要低得多，二维优化问题的求解可通过全局优化方法和/或局部优化方法多种求解方式进行求解。
第三方面，本申请实施例提供了一种计算机设备，包括存储器以及处理器，所述存储器用于存储计算机程序，所述处理器运行所述计算机程序以使所述计算机设备执行上述的冗余自由度机械臂运动规划方法。
第四方面，本申请实施例提供了一种计算机可读存储介质，其存储有上述的计算机设备中所使用的计算机程序。
附图说明
为了更清楚地说明本申请实施例的技术方案，下面将对本申请实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1为本申请实施例一提供的一种冗余自由度机械臂运动规划方法的流程图；
图2为本申请实施例二提供的一种冗余自由度机械臂运动规划方法的流程图；
图3为本申请实施例三提供的一种冗余自由度机械臂运动规划装置的结构示意图。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行描述。
应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本申请的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
针对现有技术中的问题，本申请提供了一种冗余自由度机械臂运动规划方法及装置，通过预设的约束条件对冗余自由度机械臂运动规划中的运动学逆解的七维优化问题进行降维处理，得到二维优化问题；再对二维优化问题进行求解，得到求解结果；根据获取的与冗余自由度机械臂的末端执行器的期望位姿相对应的位姿数据和前述求解结果，得到冗余自由度机械臂的控制方案，二维优化问题的求解时间较短，求解难度较低，减少了冗余自由度机械臂运动规划中的运动学逆解的计算时间，降低了冗余自由度机械臂运动规划中的运动学逆解的计算难度，从而在冗余自由度机械臂的运动规划时能够更快速、更容易地得到冗余自由度机械臂的控制方案，完成运动规划，减少运动规划的总时间，提高规划效率。
实施例一
参见图1，图1为本申请实施例提供的一种冗余自由度机械臂运动规划方法的流程图。
本申请实施例的冗余自由度机械臂运动规划方法，包括如下步骤：
步骤s101，获取与冗余自由度机械臂的末端执行器的期望位姿相对应的位姿数据。
冗余自由度机械臂通常具有七个连杆、一个末端执行器及七个关节，冗余自由度机械臂的七个连杆分别为第一连杆、第二连杆、第三连杆、第四连杆、第五连杆、第六连杆及第七连杆；冗余自由度机械臂的末端执行器连接冗余自由度机械臂的第七连杆；冗余自由度机械臂的第一连杆与第二连杆之间有第一关节，冗余自由度机械臂的第二连杆与第三连杆之间有第二关节，冗余自由度机械臂的第三连杆与第四连杆之间有第三关节，冗余自由度机械臂的第四连杆与第五连杆之间有第四关节，冗余自由度机械臂的第五连杆与第六连杆之间有第五关节，冗余自由度机械臂的第六连杆与第七连杆之间有第六关节，冗余自由度机械臂的第七连杆与末端执行器之间有第七关节。
末端执行器的期望位姿，即末端执行器的期望位置和姿态，可以理解为，冗余自由度机械臂在某种应用场景下进行某种规划任务时冗余自由度机械臂的末端执行器的期望位置和姿态，例如，冗余自由度机械臂在货物分拣的应用场景下进行货物拿取时冗余自由度机械臂的末端执行器的期望位置和姿态。
在本实施例中，本实施例以sawyer冗余自由度机械臂作为示例来进行相应的说明。
步骤s102，根据预设的约束条件对冗余自由度机械臂运动规划中的七维优化问题进行降维处理，得到二维优化问题。
在本实施例中，冗余自由度机械臂的第i关节的角度为qi，冗余自由度机械臂的第一关节为a，冗余自由度机械臂的第二关节为b，冗余自由度机械臂的第三关节为c，冗余自由度机械臂的第四关节为d，冗余自由度机械臂的第五关节为e，冗余自由度机械臂的第六关节为f，冗余自由度机械臂的第七关节为g，冗余自由度机械臂的末端执行器为t。
冗余自由度机械臂绕三个坐标轴的齐次旋转矩阵分别为trotx(·)、troty(·)、trotz(·)，平移矩阵为transl(x,y,z)；|oa|＝l0，|ab|＝l1，|bc|＝l2，|cd|＝l3，|de|＝l4，|ef|＝l5，|fg|＝l6，|gt|＝l7；记q＝[q1q2…q7]t为关节角度组成的列向量，
定义坐标系如下:
oxyz--基坐标系，其中，原点位于基座底部中心，x轴指向正前方，y轴竖直向上，z轴满足右手定则；
oaxyz--原点为a点，oxyz到oaxyz的齐次旋转平移矩阵为to2a＝transl(0,0,l0)；
obxyz--原点为b点，oaxyz到obxyz的齐次旋转平移矩阵为ta2b＝trotz(q1)transl(0,l1,0)；
ocxyz--原点为c点，obxyz到ocxyz的齐次旋转平移矩阵为tb2c＝troty(q2)transl(l2,0,0)；
odxyz--原点为d点，ocxyz到odxyz的齐次旋转平移矩阵为tc2d＝trotx(q3)transl(0,-l3,0)；
oexyz--原点为e点，odxyz到oexyz的齐次旋转平移矩阵为td2e＝troty(q4)transl(l4,0,0)；
ofxyz--原点为f点，oexyz到ofxyz的齐次旋转平移矩阵为te2f＝trotx(q5)transl(0,l5,0)；
ogxyz--原点为g点，ofxyz到ogxyz的齐次旋转平移矩阵为tf2g＝troty(q6)transl(l6,0,0)；
otxyz--原点为t点，ogxyz到otxyz的齐次旋转平移矩阵为tg2t＝trotx(q7)transl(l7,0,0)；
记基坐标系到末端执行器的齐次旋转平移矩阵为：
根据链式法则可得to2t＝to2ata2btb2ctc2dtd2ete2ftf2gtg2t，则有
此为运动学方程。
需要说明的是，在已知冗余自由度机械臂的各关节角度q时，根据(1.1)式计算冗余自由度机械臂的末端执行器的位姿，即为求解正运动学问题x＝ffk(q)；已知冗余自由度机械臂的末端执行器的位姿x，根据(1.1)式计算冗余自由度机械臂各关节角度q，即为求解逆运动学问题q＝fik(x)。
在本实施例中，冗余自由度机械臂的末端执行器的期望位姿
以sawyer冗余自由度机械臂为示例，sawyer冗余自由度机械臂具有7个自由度，其运动规划中的运动学逆解的七维优化问题的数学规划形式为：
q为7维的自由变量，在进行七维优化问题的求解时，求解计算时间较长，且求解计算难度较高。
在本实施例中，上述预设的约束条件包括：
冗余自由度机械臂的七个关节分为三组，其中，第一组包括冗余自由度机械臂的第一关节至第四关节，第二组包括冗余自由度机械臂的第五关节和第六关节，第三组包括冗余自由度机械臂的第七关节；以及
冗余自由度机械臂第五关节的位置以上述位姿数据和第七关节的角度确定；以及
冗余自由度机械臂的第三关节固定；以及
冗余自由度机械臂第四关节与第五关节的轴线和第五关节与第六关节的轴线相垂直。
在上述约束条件下，冗余自由度机械臂的七个关节分为三组，第一组包括冗余自由度机械臂的第一关节至第四关节，第二组包括冗余自由度机械臂的第五关节和第六关节，第三组包括冗余自由度机械臂的第七关节；冗余自由度机械臂第五关节的位置以上述位姿数据和第七关节的角度确定；冗余自由度机械臂的第三关节固定，即第三关节是相对固定的，从而冗余自由度机械臂的第一关节、第二关节、第四关节的角度存在有限组解；若冗余自由度机械臂的第一关节至第四关节的角度确定后，冗余自由度机械臂的第五关节与第六关节的角度可根据第四关节与第五关节的轴线和第五关节与第六关节的轴线相垂直得到有限组解，前述分析是根据预设的约束条件对冗余自由度机械臂运动规划中的运动学逆解的七维优化问题进行降维处理，得到二维优化问题的思路，在此约束条件下，可以理解的是，第三关节的角度及第七关节的角度为自由变量，其余关节的角度为参变量，冗余自由度机械臂的第三关节对机械臂的臂型影响较大，此约束条件不仅能较好地将冗余自由度机械臂运动规划中的运动学逆解的七维优化问题降维成二维优化问题，还能够保障在此约束条件下冗余自由度机械臂具有较高的灵活度。
需要说明的是，本申请的其他实施例还可以采用其他预设的约束条件，只要其他预设的约束条件合理，且在其他合理的预设的约束条件下，冗余自由度机械臂运动规划中的运动学逆解的七维优化问题降维成二维优化问题即可。
在上述约束条件下，记e点的三维坐标为ex、ey、ez，将(1.1)式改为：
在(1.3)式中，
令，
mr(q1,q2,q3,q4)＝to2ata2btb2ctc2dtd2e(1.5)
(1.3)式等价于求解七维优化问题的数学规划形式：
其中，ml和mr为转移矩阵，
根据上述的几何约束，可以将冗余自由度机械臂运动规划中的七维优化问题进行降维处理，得到二维优化问题。
二维优化问题的数学规划形式为：
s.t.ml14＝mr14,ml24＝mr24,ml34＝mr34
其中，q3为冗余自由度机械臂的第三关节的角度，q7为冗余自由度机械臂的第七关节的角度，mr11为mr转移矩阵中第1行第1列的元素，ml21为ml转移矩阵中第2行第1列的元素。
在本实施例中，上述二维优化问题的数学规划形式是在上述约束条件下，以冗余自由度机械臂的第三关节的角度及第七关节的角度为自由变量的数学规划形式，由于冗余自由度机械臂的第三关节对机械臂的臂型影响较大，在此二维优化问题的数学规划形式下，冗余自由度机械臂能具有较高的灵活度。
需要说明的是，上述二维优化问题的数学规划形式还可适用其他降维思路相似的约束条件，例如，在上述约束条件的基础上，将冗余自由度机械臂第四关节与第五关节的轴线和第五关节与第六关节的轴线相垂直，改成冗余自由度机械臂第四关节与第五关节的轴线和第五关节与第六关节的轴线成一定角度，可以是30°或45°等。
本申请的其他实施例还可以采用其他二维优化问题的数学规划形式，具体二维优化问题的数学规划形式根据其他预设的约束条件所设定的自由变量及具体约束条件得到。
步骤s103，对上述二维优化问题进行求解，得到求解结果。
在本实施例中，可通过全局优化方法和/或局部优化方法，对上述二维优化问题进行求解，得到求解结果。
作为一种可选的实施方式，二维优化问题的求解，可通过粒子群算法进行求解，粒子群算法是一种全局优化方法。
粒子群算法属于进化算法的一种，和模拟退火算法相似，它也是从随机解出发，通过迭代寻找最优解，它也是通过适应度来评价解的品质，但它比遗传算法规则更为简单，它没有遗传算法的“交叉”(crossover)和“变异”(mutation)操作，它通过追随当前搜索到的最优值来寻找全局最优。这种算法具有实现容易、精度高、收敛快等优点，并且在解决实际问题中具有优越性。
需要说明的是，在本实施例中，二维优化问题的求解，还可以通过其他全局优化方法，例如，网格法。
作为一种可选的实施方式，二维优化问题的求解，可通过序列二次规划法进行求解，序列二次规划法是一种局部优化方法。
需要说明的是，在本实施例中，二维优化问题的求解，还可以通过其他局部优化方法，例如，牛顿法。
作为一种可选的实施方式，二维优化问题的求解，可先通过粒子群算法得到全局近似解，再通过序列二次规划法求精确解。
作为一种可选的实施方式，二维优化问题的求解，可先通过网格法得到全局近似解，再通过序列二次规划法求精确解。
在上述实现过程中，在冗余自由度机械臂运动规划中的运动学逆解的七维优化问题降维成二维优化问题后，二维优化问题的求解难度较二维优化问题的求解难度要低得多，二维优化问题的求解可通过全局优化方法和/或局部优化方法多种求解方式进行求解。
步骤s104，根据上述位姿数据和求解结果，得到冗余自由度机械臂的控制方案。
冗余自由度机械臂的控制方案用于控制冗余自由度机械臂在某种应用场景下进行某种期望操作，以sawyer冗余自由度机械臂作为示例，可以是用于控制sawyer冗余自由度机械臂在货物分拣的应用场景下拿取某种货物或某位置的货物的控制方案。
在上述示例外，冗余自由度机械臂的控制方案，也可以是，用于控制其他冗余自由度机械臂在住宅内完成打开或关闭某种开关的操作等等，例如，关闭住宅内主卧室电灯的开关。
需要说明的是，根据位姿数据和求解结果得到的冗余自由度机械臂的控制方案，可以不仅只是通过位姿数据和求解结果得到，还可以通过结合其他数据或信息得到。
本申请实施例的冗余自由度机械臂运动规划方法，通过预设的约束条件对冗余自由度机械臂运动规划中的运动学逆解的七维优化问题进行降维处理，得到二维优化问题；再对二维优化问题进行求解，得到求解结果；根据获取的与冗余自由度机械臂的末端执行器的期望位姿相对应的位姿数据和前述求解结果，得到冗余自由度机械臂的控制方案，二维优化问题的求解时间较短，求解难度较低，减少了冗余自由度机械臂运动规划中的运动学逆解的计算时间，降低了冗余自由度机械臂运动规划中的运动学逆解的计算难度，从而在冗余自由度机械臂的运动规划时能够更快速、更容易地得到冗余自由度机械臂的控制方案，完成运动规划，减少运动规划的总时间，提高规划效率。
实施例二
参见图2，图2为本申请实施例提供的一种冗余自由度机械臂运动规划方法的流程图。
本申请实施例的冗余自由度机械臂运动规划方法，包括如下步骤：
步骤s201，获取与冗余自由度机械臂的末端执行器的期望位姿相对应的位姿数据。
步骤s202，根据预设的约束条件对冗余自由度机械臂运动规划中的七维优化问题进行降维处理，得到二维优化问题。
步骤s203，建立冗余自由度机械臂的第五关节至第七关节的碰撞检测模型。
在本实施例中，上述的碰撞检测模型为用于对冗余自由度机械臂的第五关节至第七关节进行碰撞检测的模型，其不包括对冗余自由度机械臂的第一关节至第四关节进行检测，因此，上述的碰撞检测模型的建模时间会较短。
步骤s204，以上述碰撞检测模型为依据，对冗余自由度机械臂的第五关节至第七关节进行碰撞检测。
在上述实现过程中，在冗余自由度机械臂运动规划中的运动学逆解的七维优化问题降维成二维优化问题后，建立冗余自由度机械臂的第五关节至第七关节的碰撞检测模型，并以碰撞检测模型为依据，对冗余自由度机械臂的第五关节至第七关节进行碰撞检测，能减少二维优化问题求解时的计算量，从而进一步地缩短二维优化问题的求解时间，降低二维优化问题的求解难度。
在本实施例中，以sawyer冗余自由度机械臂作为示例，步骤s203和步骤s204会更为适用于在网格类型的货物架上进行货物分拣的应用场景。
步骤s205，对上述二维优化问题进行求解，得到求解结果。
步骤s206，根据上述位姿数据和求解结果，得到冗余自由度机械臂的控制方案。
本申请实施例的冗余自由度机械臂运动规划方法，在冗余自由度机械臂运动规划中的运动学逆解的七维优化问题降维成二维优化问题后，建立冗余自由度机械臂的第五关节至第七关节的碰撞检测模型，并以碰撞检测模型为依据，对冗余自由度机械臂的第五关节至第七关节进行碰撞检测，能减少二维优化问题求解时的计算量，从而进一步地缩短二维优化问题的求解时间，降低二维优化问题的求解难度，进而进一步地减少冗余自由度机械臂运动规划中的运动学逆解的计算时间，降低冗余自由度机械臂运动规划中的运动学逆解的计算难度，以进一步地减少运动规划的总时间，提高规划效率。
本申请实施例的其余内容可参照上述实施例一的内容，在本实施例中，不再进行赘述。
实施例三
参见图3，图3为本申请实施例提供的一种冗余自由度机械臂运动规划装置的结构示意图。
本申请实施例的冗余自由度机械臂运动规划装置，包括：
获取单元310，用于获取与冗余自由度机械臂的末端执行器的期望位姿相对应的位姿数据。
处理单元320，用于根据预设的约束条件对冗余自由度机械臂运动规划中的七维优化问题进行降维处理，得到二维优化问题。
在本实施例中，上述预设的约束条件包括：
冗余自由度机械臂的七个关节分为三组，其中，第一组包括冗余自由度机械臂的第一关节至第四关节，第二组包括冗余自由度机械臂的第五关节和第六关节，第三组包括冗余自由度机械臂的第七关节；以及
冗余自由度机械臂第五关节的位置以上述位姿数据和第七关节的角度确定；以及
冗余自由度机械臂的第三关节固定；以及
冗余自由度机械臂第四关节与第五关节的轴线和第五关节与第六关节的轴线相垂直。
在上述约束条件下，冗余自由度机械臂的七个关节分为三组，第一组包括冗余自由度机械臂的第一关节至第四关节，第二组包括冗余自由度机械臂的第五关节和第六关节，第三组包括冗余自由度机械臂的第七关节；冗余自由度机械臂第五关节的位置以上述位姿数据和第七关节的角度确定；冗余自由度机械臂的第三关节固定，即第三关节是相对固定的，从而冗余自由度机械臂的第一关节、第二关节、第四关节的角度存在有限组解；若冗余自由度机械臂的第一关节至第四关节的角度确定后，冗余自由度机械臂的第五关节与第六关节的角度可根据第四关节与第五关节的轴线和第五关节与第六关节的轴线相垂直得到有限组解，前述分析是根据预设的约束条件对冗余自由度机械臂运动规划中的运动学逆解的七维优化问题进行降维处理，得到二维优化问题的思路，在此约束条件下，可以理解的是，第三关节的角度及第七关节的角度为自由变量，其余关节的角度为参变量，冗余自由度机械臂的第三关节对机械臂的臂型影响较大，此约束条件不仅能较好地将冗余自由度机械臂运动规划中的运动学逆解的七维优化问题降维成二维优化问题，还能够保障在此约束条件下冗余自由度机械臂具有较高的灵活度。
在本实施例中，二维优化问题的数学规划形式为：
s.t.ml14＝mr14,ml24＝mr24,ml34＝mr34
其中，q3为冗余自由度机械臂的第三关节的角度，q7为冗余自由度机械臂的第七关节的角度，mr11为mr转移矩阵中第1行第1列的元素，ml21为ml转移矩阵中第2行第1列的元素。
在本实施例中，上述二维优化问题的数学规划形式是在上述约束条件下，以冗余自由度机械臂的第三关节的角度及第七关节的角度为自由变量的数学规划形式，由于冗余自由度机械臂的第三关节对机械臂的臂型影响较大，在此二维优化问题的数学规划形式下，冗余自由度机械臂能具有较高的灵活度。
求解单元330，用于对上述二维优化问题进行求解，得到求解结果。
在本实施例中，可通过全局优化方法和/或局部优化方法，对上述二维优化问题进行求解，得到求解结果。
在上述实现过程中，在冗余自由度机械臂运动规划中的运动学逆解的七维优化问题降维成二维优化问题后，二维优化问题的求解难度较二维优化问题的求解难度要低得多，二维优化问题的求解可通过全局优化方法和/或局部优化方法多种求解方式进行求解。
生成单元340，用于根据上述位姿数据和求解结果，得到冗余自由度机械臂的控制方案。
作为一种可选的实施方式，本申请实施例的冗余自由度机械臂运动规划装置，还包括：
建模单元，用于建立冗余自由度机械臂的第五关节至第七关节的碰撞检测模型。
检测单元，用于以上述碰撞检测模型为依据，对冗余自由度机械臂的第五关节至第七关节进行碰撞检测。
在上述实现过程中，在冗余自由度机械臂运动规划中的运动学逆解的七维优化问题降维成二维优化问题后，建立冗余自由度机械臂的第五关节至第七关节的碰撞检测模型，并以碰撞检测模型为依据，对冗余自由度机械臂的第五关节至第七关节进行碰撞检测，能减少二维优化问题求解时的计算量，从而进一步地缩短二维优化问题的求解时间，降低二维优化问题的求解难度。
本申请实施例的冗余自由度机械臂运动规划装置，通过预设的约束条件对冗余自由度机械臂运动规划中的运动学逆解的七维优化问题进行降维处理，得到二维优化问题；再对二维优化问题进行求解，得到求解结果；根据获取的与冗余自由度机械臂的末端执行器的期望位姿相对应的位姿数据和前述求解结果，得到冗余自由度机械臂的控制方案，二维优化问题的求解时间较短，求解难度较低，减少了冗余自由度机械臂运动规划中的运动学逆解的计算时间，降低了冗余自由度机械臂运动规划中的运动学逆解的计算难度，从而在冗余自由度机械臂的运动规划时能够更快速、更容易地得到冗余自由度机械臂的控制方案，完成运动规划，减少运动规划的总时间，提高规划效率。
上述的冗余自由度机械臂运动规划装置可实施上述实施例一和实施例二的冗余自由度机械臂运动规划方法。上述实施例一和实施例二中的任何可选项也适用于本实施例，这里不再详述。
本申请实施例的其余内容可参照上述实施例一和实施例二的内容，在本实施例中，不再进行赘述。
在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和框图显示了根据本申请的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
另外，在本申请各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。
所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，read-onlymemory)、随机存取存储器(ram，randomaccessmemory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述仅为本申请的实施例而已，并不用于限制本申请的保护范围，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。
以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应所述以权利要求的保护范围为准。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
- 还没有人留言评论。精彩留言会获得点赞！