CN115847410A - 一种面向灵巧性和刚度提升的机械臂站位规划方法 - Google Patents

一种面向灵巧性和刚度提升的机械臂站位规划方法 Download PDF

Info

Publication number
CN115847410A
CN115847410A CN202211542675.XA CN202211542675A CN115847410A CN 115847410 A CN115847410 A CN 115847410A CN 202211542675 A CN202211542675 A CN 202211542675A CN 115847410 A CN115847410 A CN 115847410A
Authority
CN
China
Prior art keywords
mechanical arm
coordinate system
mobile platform
connecting rod
relative
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202211542675.XA
Other languages
English (en)
Inventor
宋韬
颜哲
罗贤鹏
彭江涛
崔国华
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai University of Engineering Science
University of Shanghai for Science and Technology
Original Assignee
Shanghai University of Engineering Science
University of Shanghai for Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai University of Engineering Science, University of Shanghai for Science and Technology filed Critical Shanghai University of Engineering Science
Priority to CN202211542675.XA priority Critical patent/CN115847410A/zh
Publication of CN115847410A publication Critical patent/CN115847410A/zh
Pending legal-status Critical Current

Links

Images

Landscapes

  • Numerical Control (AREA)

Abstract

本发明属于移动机械臂技术领域，公开一种面向灵巧性和刚度提升的机械臂站位规划方法，分别基于灵巧性指标与刚度指标对移动机械臂站位进行分析，通过优化移动平台相对于目标对象的站位，提高机械臂灵巧性与末端刚度，对于提升系统传速性能与机械臂末端作业精度具有重要意义。以综合判据最小化值为目标，利用遗传算法进行合理的综合站位优化来提升执行任务过程中系统的传速性能与机械臂末端作业精度。

Description

一种面向灵巧性和刚度提升的机械臂站位规划方法
技术领域
本发明属于移动机械臂技术领域，具体涉及一种面向灵巧性和刚度提升的机械臂站位规划方法。
背景技术
移动机械臂兼具移动机器人的移动性能和工业机器人的操作性能。移动机械臂也可称之为复合机器人。目前广泛应用于国民生产的各个领域。在半导体工业中，移动机械臂用来抓取晶元并搬运至指定位置上；在风机叶片制造中，移动机械臂用来对粘贴好的风机叶片毛坯件进行大尺寸的打磨；在建筑3D打印中，移动机械臂用来完成对混凝土的打印；在肢体运动康复中，移动机械臂还可用来完成对病人上肢的运动康复，等等。
现如今，随着技术的发展，越来越多实际作业场景中将六自由度工业机器人的操作能力和移动平台的移动能力相结合，形成了一种冗余自由度移动机械臂系统；在进行作业任务时，由于系统的冗余性，面对同一作业任务，存在多个移动平台相对于该作业任务的停靠点均能完成任务，当其运动到提前规划好的停靠点静止后，此时在该停靠点对应移动平台相对于墙体的位姿被称为站位；再由工业机器人完成该作业任务，如此循环交替工作，直至完成全部作业，但该系统同时也将面临如下问题：与固定基座机械臂相比，该系统为融合了六自由度工业机器人和移动平台的冗余自由度系统，附加了移动平台相对于任务的站位移动环节；现如今关于移动机械臂站位规划还是依靠人工根据现场情况进行移动平台的站位布置，难以规划至最优的站位，而产生了移动平台站位移动次数多、整体作业精度低和效率低等问题；因此，必须针对各种作业任务开发一种普适性的算法来对移动平台相对于具体任务的站位进行优化，从而以最高的工作效率、作业精度实现作业任务。
发明内容
本发明的目的在于提供一种在执行多类型实际作业场景中能够提高整体作业精度、提高站位规划效率的面向灵巧性和刚度提升的机械臂站位规划方法。
基于上述目的，本发明采用如下技术方案：
一种面向灵巧性和刚度提升的机械臂站位规划方法，包括以下步骤：
步骤1，建立坐标系：确定移动平台坐标系、机械臂基坐标系、机械臂各连杆坐标系、法兰坐标系、打印单元坐标系和墙体坐标系。
步骤2，根据步骤1建立的所有的坐标系，建立整个系统的运动学模型，根据整个系统的运动学模型及实际作业任务确定移动平台相对实际作业任务的可达站位空间。
步骤3，根据步骤1建立的所有的坐标系和机械臂末端所期望的运动轨迹，计算连杆构型、连杆质量、末端载荷和各关节角速度的因素作用下的各关节驱动力矩，建立机械臂的力学模型。
步骤4，根据步骤2建立的整个系统管的运动学模型和步骤3建立的机械臂的力学模型，确定机械臂末端变形量作为机械臂刚度评价指标，确定机械臂速度方向可操作度作为机械臂灵巧性评价指标；确定综合判据最小化值作为机械臂综合评价指标。
步骤5，根据步骤4得到的机械臂刚度和机械臂灵巧性的综合评价指标做为优化目标，以移动平台相对实际作业任务在可达站位空间内的站位做为设计变量，建立单次作业的优化函数；根据作业的性质，优化函数中各部分的权重，利用遗传算法进行合理的站位优化。
进一步的，在步骤4中，确定机械臂综合判据最小化值作为机械臂综合评价指标的具体过程为：综合机械臂灵巧性和刚度性能，其中，综合判据f([x,y,θ])记作
f([x，y，θ])＝-w1·fdexterity([x，y，θ])+w2·fstiffness([x，y，θ])
Figure BDA0003978431720000021
式中：μz是机械臂末端沿Z轴方向的变形量，
Figure BDA0003978431720000022
是对关节速度规范化后的速度方向可操作度，w1、w2是权重系数，分别衡量灵巧性、刚度对于移动机械臂当前站位的重要程度；在系统执行不同的任务过程时，w1和w2的选取是不同的，w1和w2由任务性质决定取值，其中，w2＝1-w1。
对于移动机械臂载荷较大的情况，如果其位姿变化小且工作范围不大，此时，我们会增大刚度的权重，即通常将w2＝0.7～0.9。例如：重载搬运。
对于移动机械臂载荷较小的情况，如果其位姿变化大且工作范围大，此时，我们会增大灵巧性的权重，即通常将w1＝0.7～0.9。例如：上肢康复、曲面打磨。
对于其他情况，如果移动机械臂的应用没有特定的要求，通常我们会将两个权重取值接近，即将w1＝0.3～0.6。
进一步的，机械臂为六连杆机械臂，包括第一连杆、第二连杆、第三连杆、第四连杆、第五连杆和第六连杆，根据改进的DH参数法分别确定机械臂各连杆坐标系。机械臂各连杆坐标系包括第一连杆坐标系、第二连杆坐标系、第三连杆坐标系、第四连杆坐标系、第五连杆坐标系和第六连杆坐标系；第一连杆坐标系、第二连杆坐标系、第三连杆坐标系、第四连杆坐标系、第五连杆坐标系和第六连杆坐标系分别对应机械臂的第一连杆、第二连杆、第三连杆、第四连杆、第五连杆和第六连杆。
进一步的，在步骤2中，建立整个系统的运动学模型的步骤具体为：首先根据机械臂各连杆坐标系和机械臂各连杆的结构尺寸确定机械臂运动学模型；然后根据移动平台的尺寸、机械臂安装在移动平台上的位置和作业任务确定整个系统的运动学模型，即
Figure BDA0003978431720000031
式中：
Figure BDA0003978431720000032
为移动平台坐标系相对于机械臂基坐标系的齐次变换矩阵；/>
Figure BDA0003978431720000033
为打印单元坐标系相对于法兰坐标系的齐次变换矩阵；/>
Figure BDA0003978431720000034
为打印单元坐标系相对墙体坐标系的齐次变换矩阵；/>
Figure BDA0003978431720000035
为移动平台坐标系{M}相对墙体坐标系{W}的齐次变换矩阵；/>
Figure BDA0003978431720000036
为法兰坐标相对于第六连杆坐标系的齐次变换矩阵；/>
Figure BDA0003978431720000037
为第六连杆坐标系相对机械臂基坐标系的齐次变换矩阵。
其中，第六连杆的坐标系相对机械臂基坐标系的齐次变换矩阵
Figure BDA0003978431720000038
由整个系统的运动学模型求得，为
Figure BDA0003978431720000039
根据机械臂运动学模型列出机械臂的逆运动学方程，根据机械臂的逆运动学方程校验移动平台相对实际作业任务在可达站位空间内规划的某一站位下，判断机械臂的末端位姿序列中是否各点均存在运动学逆解且满足各轴关节角度限制。
进一步的，在步骤2中，根据整个系统的运动学模型及实际作业任务确定移动平台相对实际作业任务的可达站位空间的具体过程为：首先根据机械臂运动学模型绘制机械臂的可达工作空间云图；然后根据实际作业任务的尺寸、机械臂安装在移动平台上的位置和机械臂末端工具的尺寸确定移动平台相对作业任务的可达站位空间；移动平台相对作业任务的可达站位空间转化为移动平台坐标系在墙体坐标系下的位姿：
Figure BDA00039784317200000310
式中：x,y为移动平台坐标系的原点在墙体坐标系下的位置，θ为移动平台坐标系相对于墙体坐标系竖直方向Z轴的姿态旋转角度，r表示移动平台相对于作业任务的站位空间的最远距离。
进一步的，在步骤2中，根据机械臂运动学模型列出机械臂的逆运动学方程，具体步骤为：首先根据机械臂运动学模型列出机械臂的正运动学方程，然后根据代数法进行机械臂逆运动学求解，得机械臂逆运动学方程如下式：
Figure BDA0003978431720000041
式中：θ1、θ2、θ3、θ4、θ5、θ6为各个关节的角度值，
Figure BDA0003978431720000042
为第一连杆坐标系相对机械臂基坐标系的齐次变换矩阵，/>
Figure BDA0003978431720000043
为第二连杆坐标系相对第一连杆的坐标系的齐次变换矩阵，/>
Figure BDA0003978431720000044
为第三连杆坐标系相对第二连杆坐标系的齐次变换矩阵，/>
Figure BDA0003978431720000045
为第四连杆坐标系相对第三连杆坐标系的齐次变换矩阵，/>
Figure BDA0003978431720000046
为第五连杆坐标系相对第六连杆坐标系的齐次变换矩阵，/>
Figure BDA0003978431720000047
为第六连杆坐标系相对第五连杆坐标系的齐次变换矩阵，/>
Figure BDA0003978431720000048
为第六连杆坐标系相对机械臂基坐标系的齐次变换矩阵：/>
Figure BDA0003978431720000049
进一步的，在步骤3中，械臂力学模型被建立的步骤具体为：首先由外递推计算各连杆的速度与加速度，再由牛顿-欧拉公式计算出各连杆的惯性力和力矩；向内递推计算各连杆相互作用的力和力矩，以及各关节驱动力矩。
机械臂各连杆的惯性力矩：
fj+fa＝m[(aj-g)+α×bc+ω×(ω×bc)]
式中：m表示连杆质量；α表示为角加速度；ω表示角速度；fj表达连杆j的作用力；fa表示连杆i+1作用力；aj表示连杆j的线加速度；bc表示关节与连杆质心的偏距。
机械臂各连杆的力矩：
Figure BDA00039784317200000410
式中：
Figure BDA00039784317200000411
Figure BDA00039784317200000412
Figure BDA00039784317200000413
是bc的反对称矩阵，/>
Figure BDA00039784317200000414
表示/>
Figure BDA00039784317200000415
的转置矩阵。
求得各关节绕转轴的驱动力矩：
Figure BDA00039784317200000416
式中：
Figure BDA0003978431720000051
为连杆坐标系{i}在机械臂基坐标系下的方向，i＝1、2、3、4、5、6，mi为关节i上的约束力矩矢量，mi＝[mix miy miz]，mix,miy,miz表示沿x,y,z三个方向上的约束力矩，Zi＝[0 0 1]T。
进一步的，在步骤4中，确定机械臂末端变形量作为机械臂刚度评价指标的具体过程为：以机械臂末端沿Z轴方向的变形量为机械臂刚度评价指标，其中，机械臂末端沿Z轴方向的变形量记作μz
μz＝Z·ΔX＝Z·J(θ)·(Kθ)-1·T
式中：Z＝[0 0 1]，ΔX为机械臂末端产生的广义变形量，J(θ)为机械臂的雅可比矩阵，τ为τi的矩阵式，Kθ为机械臂关节空间的刚度矩阵。
进一步的，在步骤4中，确定机械臂速度方向可操作度作为机械臂灵巧性评价指标的具体过程为：在当前位形状态下沿指定方向的速度传递特性，以机械臂速度方向可操作度为机械臂灵巧度评价指标，其中，机械臂在方向p上的速度方向可操作度记作β
Figure BDA0003978431720000052
式中：
Figure BDA0003978431720000053
J(θ)为机械臂的雅可比矩阵，/>
Figure BDA0003978431720000054
为[0 0 0],0pv＝[0 0 1]T。
进一步的，在步骤5中，优化函数以步骤2确定的移动平台相对实际作业任务的可达站位空间为界限约束，以移动平台在移动平台相对实际作业任务的可达站位空间内相对实际作业任务的位姿满足整个系统的逆运动学方程为性态约束。
进一步的，在步骤1中，移动平台坐标系、机械臂基坐标系、机械臂各连杆坐标系、法兰坐标系、打印单元坐标系和墙体坐标系均为右手坐标系。
与现有技术相比，本发明具有如下有益效果：
1.本发明提供一种面向灵巧性和刚度提升的机械臂站位规划方法，该方法可应用于多类型实际作业场景中；本发明根据建立的坐标系，建立整个系统的运动学模型，根据整个系统的运动学模型计算移动平台相对作业任务的理论可达站位空间，在根据机械臂的逆运动学方程对理论可达站位空间进行验证；然后根据整个系统的力学模型提出以综合判据为机械臂综合评价指标，求出移动平台坐标系在墙体坐标系不同位姿下的综合判据，以移动平台坐标系在墙体坐标系位姿下的综合判据最小化值为最优站位；从而极大减少了移动机械臂的站位移动次数，使得机器人的作业效率得到显著提升。
2.本发明应用牛顿-欧拉方法建立机械臂动力学方程过程中，除了考虑连杆构型与末端载荷对各关节驱动力矩影响外，还引入了机械臂各连杆质量、各关节角速度和角加速度这些影响因素。改进DH法建立机械臂运动学模型，这些使得后续根据该动力学方程建立的机械臂模型更为合理，求解得到的机械臂末端变形量和速度方向可操作度更加精确。
3.由于本发明提供的一种移动机械臂站位规划方法可应用于多类型实际场景中，将针对具体实际作业场景提出机械臂综合评价指标，即综合机械臂末端沿某个轴方向变形量和机械臂在当前位姿下各个方向运动的能力来综合进行移动机械臂的站位规划，可以使得机械臂综合判据在移动平台理论上的可达站位空间内达到最小，整体的机械臂末端作业精度达到最优。
4.本发明使用遗传算法在整个站位空间中搜寻移动平台的最优站位，能够跳出局部最优而找到全局最优点，而且遗传算法允许使用非常的目标函数，并对变量的变化范围可以加以限制。基于遗传算法在整个站位空间中搜寻移动平台的最优站位的过程是利用MATLAB遗传算法工具箱，其扩展了MATLAB在处理优化问题方面的能力，使结果更具有准确性与高效性，解决了依靠人工根据现场情况进行移动平台的站位布置而使得移动平台站位移动次数多、整体打印精度低和效率低等问题。
附图说明
图1为实施例1的系统结构示意图以及系统各坐标系关系图；
图2为实施例1的KUKA KR90 R3100型机械臂各连杆坐标系示意图；
图3为实施例1的KUKA KR90 R3100型机械臂可达工作空间云朵图；
图4为实施例1的移动平台可达性站位空间分析示意图；
图5为实施例1的机械臂简化示意图；
图6为实施例1的单连杆力平衡示意图；
图7为实施例1的相邻连杆间力关系示意图；
图8为实施例1的ADAMS仿真虚拟样机示意图；
图9为实施例1的民用建筑实例的平面结构及尺寸示意图；
图10为实施例1在机械臂不同位形下机械臂末端沿X、Y、Z方向的变形量大小示意图；
图11为实施例1的针对一字墙体3D打印任务移动机械臂系统的最优站位示意图；
图12为实施例1的针对直角墙体3D打印任务移动机械臂系统的最优站位示意图；
图13为实施例1的针对圆弧墙体3D打印任务移动机械臂系统的最优站位示意图；
图14为实施例1的针对T形墙体3D打印任务移动机械臂系统的最优站位示意图；
图15为实施例1的速度方向可操作度椭球原理图；
图16为实施例1的一字墙体综合优化最优站位下所有点的速度方向可操作度示意图；
图17为实施例1的一字墙体综合优化最优站位下所有点的机械臂末端沿Z轴方向变形量图；
图18为实施例1的直角墙体综合优化最优站位下所有点的速度方向可操作度示意图；
图19为实施例1的直角墙体综合优化最优站位下所有点的机械臂末端沿Z轴方向变形量图；
图20为实施例1的圆弧墙体综合优化最优站位下所有点的速度方向可操作度示意图；
图21为实施例1的圆弧墙体综合优化最优站位下所有点的机械臂末端沿Z轴方向变形量图；
图22为实施例1的T形墙体综合优化最优站位下所有点的速度方向可操作度示意图；
图23为实施例1的T形墙体综合优化最优站位下所有点的机械臂末端沿Z轴方向变形量图；
图24为实施例1的单次作业任务四种类型墙体站位示意图；
图中：移动平台1、KUKA KR90 R3100机械臂2、混凝土3D打印单元3、直角墙体4。
具体实施方式
实施例1
如图1所示，本实施例以直角墙体4作为示例，为便于对本实施例进行理解，首先对本实施例所公开的一种移动机械臂整车系统进行简单介绍；移动机械臂系统如图1所示，移动机械臂系统包括移动平台1、机械臂和混凝土3D打印单元3；机械臂为KUKA KR90 R3100机械臂2，包括机械臂基座、第一连杆、第二连杆、第三连杆、第四连杆、第五连杆、第六连杆和法兰；机械臂基座固定安装在移动平台1上，混凝土3D打印单元3固定安装在机械臂末端法兰上；作业任务为所需打印的墙体。
基于上述移动机械臂系统，本实施例提供一种面向灵巧性和刚度提升的机械臂站位规划方法，具体步骤如下：
步骤1，建立坐标系：确定移动平台1的坐标系为移动平台1坐标系，记作{M}；机械臂基座的坐标系为机械臂基坐标系，记作{0}；法兰的坐标系为法兰坐标系，记作{F}；混凝土3D打印单元3的坐标系为打印单元坐标系，记作{T}；所需打印墙体的坐标系为墙体坐标系，记作{W}；机械臂各连杆的坐标系为机械臂各连杆坐标系。移动平台1坐标系、机械臂基坐标系、机械臂各连杆坐标系、法兰坐标系、打印单元坐标系和墙体坐标系均为右手坐标系。
选取移动平台1中心为移动平台1坐标系的原点，机械臂基座中心为机械臂基坐标系的原点，机械臂基座中心与移动平台1中心有一距离偏置L；机械臂基坐标系与移动平台1坐标系的x轴方向均平行于移动平台1长边方向，移动平台1坐标系的x轴正方向由移动平台1坐标系的原点指向机械臂基坐标系的原点，即移动平台1坐标系与机械臂基坐标系x轴方向共线；移动平台1坐标系和机械臂基坐标系的Z轴正方向平行且竖直向上；移动平台1坐标系与机械臂基坐标系的y轴方向符合右手法则，根据右手法则可对机械臂基坐标系和移动平台1坐标系的y轴方向进行判断。
根据改进的DH参数法分别确定机械臂各连杆坐标系。如图2所示，机械臂各连杆坐标系分别为第一连杆坐标系，记作{1}；第二连杆坐标系，记作{2}；第三连杆坐标系，记作{3}；第四连杆坐标系，记作{4}；第五连杆坐标系，记作{5}；第六连杆坐标系，记作{6}。第一连杆坐标系{1}、第二连杆坐标系{2}、第三连杆坐标系{3}、第四连杆坐标系{4}、第五连杆坐标系{5}和第六连杆坐标{6}系分别对应机械臂的第一连杆、第二连杆、第三连杆、第四连杆、第五连杆和第六连杆。
DH参数法是由Denavit和Hartenberg于1955年提出的一种描述串联式链路上连杆和关节的系统方法；1986年Khalil，Kleinfinger提出了一种改进的DH参数，在建立关节坐标系时将坐标系固结于该连杆的近端，而非远端，更符合直观理解；为了便于区分，这种方法成为改进DH(Modified DH)，而将之前的方法称为标准DH(Standard DH)。
步骤2，根据步骤1建立的坐标系，建立整个系统的运动学模型：首先根据机械臂各连杆坐标系和机械臂各连杆的结构尺寸确定机械臂运动学模型；然后根据移动平台1的尺寸、机械臂安装在移动平台1上的位置和作业任务确定整个系统的运动学模型，具体方法如下：
根据建立的机械臂各连杆坐标系和机械臂各连杆的结构尺寸得到该型号机械臂的DH参数表，如表1所示，其中θ1,θ2,θ3,θ4,θ5,θ6为各个关节的角度值，且均为未知数。各连杆间位姿的关系就能够通过坐标系间的变换得到，即可明确表示出各连杆之间的运动关系，相邻两坐标系间的位移关系由连杆扭角α、连杆长度a、关节转角θ和关节偏置d这4个参数描述。
表1 DH参数表
关节i a<sub>i-1</sub>(mm) α<sub>i-1</sub>(°) d<sub>i</sub>(mm) θ<sub>i</sub>(°)
1 0 180 -675 θ<sub>1</sub>(0)
2 350 90 0 θ<sub>2</sub>(0)
3 1350 0 0 θ<sub>3</sub>(+90)
4 41 -90 -1400 θ<sub>4</sub>(0)
5 0 90 0 θ<sub>5</sub>(+180)
6 0 90 0 θ<sub>6</sub>(0)
连杆变换可定义为从连杆坐标系{i -1}到连杆坐标系{i}的变换，i＝1,2……6。这个其次变换可以当作坐标系{i}经过四个子变换得到，因这些子变换是相对动坐标系描述，按照坐标变换的原则，得到：
Figure BDA0003978431720000091
式中：
Figure BDA0003978431720000092
表示连杆坐标系{i}相对连杆坐标系{i-1}的齐次变换矩阵；Rot表示旋转变换；Tran表示平移变换；x表示方向x；z表示方向z；di表示关节i偏置；θi表示关节i转角；ai-1表示连杆i长度；
由式(1)可以得到连杆变换
Figure BDA0003978431720000093
的通式：
Figure BDA0003978431720000094
将所有连杆变换
Figure BDA0003978431720000095
依次相乘，得末端齐次变换矩阵/>
Figure BDA0003978431720000096
即机械臂正运动学方程/>
Figure BDA0003978431720000097
式中：
Figure BDA0003978431720000098
为第一连杆坐标系{1}相对机械臂基坐标系{0}的齐次变换矩阵，/>
Figure BDA0003978431720000099
为第二连杆坐标系{2}相对第一连杆{1}的坐标系的齐次变换矩阵，/>
Figure BDA00039784317200000910
为第三连杆坐标系{3}相对第二连杆坐标系{2}的齐次变换矩阵，/>
Figure BDA00039784317200000911
为第四连杆坐标系{4}相对第三连杆坐标系{3}的齐次变换矩阵，/>
Figure BDA00039784317200000912
为第五连杆坐标系{5}相对第四连杆坐标系{4}的齐次变换矩阵，/>
Figure BDA00039784317200000913
为第六连杆坐标系{6}相对第五连杆坐标系{5}的齐次变换矩阵，/>
Figure BDA00039784317200000914
为第六连杆坐标系{6}相对机械臂基坐标系{0}的齐次变换矩阵。
机械臂的逆运动学问题是在已知工作时最后一个连杆坐标系即连杆坐标系{6}相对于基坐标系{0}的齐次变换矩阵
Figure BDA00039784317200000915
求解满足要求作业任务时机械臂各关节变量值(θ1,θ2,θ3,θ4,θ5,θ6)的问题，DH参数表中其他的参数均为已知。
采用代数法(也称反变换法)进行机械臂逆运动学求解，机械臂逆运动学方程为：
Figure BDA00039784317200000916
式中：
Figure BDA0003978431720000101
式中：px、py、pz表示平移数据：r11、r12、r13、r21、r22、r23、r31、r32、r33表示旋转角度数据。
在公式(4)中，已知等号左侧的位置与姿态组成的矩阵中16个元素，需要求解等号右侧的6个关节角度变量；因此，逆运动学可以看作依次将等号右侧含有待求角度变量的矩阵求逆，将关节角度变量分离，且求得各个关节角度的过程。
根据建系结果可以得到法兰坐标系{F}相对于连杆坐标系{6}的连杆参数，故可得到法兰坐标系{F}相对于连杆坐标系{6}的齐次变换矩阵
Figure BDA0003978431720000102
根据移动平台1坐标系{M}在墙体坐标系{W}下的位姿[x,y,θ]，按照坐标变换的原则，得到为移动平台1坐标系{M}相对墙体坐标系{W}的齐次变换矩阵
Figure BDA0003978431720000103
其中：x,y为移动平台1坐标系{M}的原点在墙体坐标系{W}下的位置，θ为移动平台1坐标系{M}相对于墙体坐标系{W}竖直方向Z轴的姿态旋转角度。
因为在进行墙体3D打印过程中，打印单元坐标系{T}走过的坐标即为墙体离散出的任务点，该任务点可以从墙体坐标系中获取，并且可以得到工具坐标系{T}相对墙体坐标系{W}的齐次变换矩阵
Figure BDA0003978431720000104
由于混凝土3D打印单元3相对于机械臂末端法兰是固定安装，在不考虑其工作中撞击等原因造成变形的情况下，其结构尺寸固定不变，所以可以得出打印单元坐标系{T}相对于法兰坐标系{F}的齐次变换矩阵
Figure BDA0003978431720000105
根据移动平台1坐标系{M}与机械臂基坐标系{0}之间的偏置距离和连杆变换可以得出移动平台1坐标系{M}相对于机械臂基坐标系{0}的齐次变换矩阵
Figure BDA0003978431720000106
综上所述，列出整个系统的运动学方程，即
Figure BDA0003978431720000107
根据公式(5)得
Figure BDA0003978431720000108
根据机械臂运动学模型列出机械臂的逆运动学方程，根据机械臂的逆运动学方程校验移动平台1相对实际作业任务在可达站位空间内规划的某一站位下，判断机械臂的末端位姿序列中是否各点均存在运动学逆解且满足各轴关节角度限制。
根据整个系统的运动学模型及实际作业任务确定移动平台1的站位空间：如图3所示，采用几何法可绘制出KUKA KR90 R3100机械臂2的可达工作空间云朵图，包络线内的区域表示机械臂末端可达空间；在3D打印墙体时，为保证储料桶不发生倾漏，混凝土3D打印单元3始终竖直向下；当混凝土3D打印单元3出料口恰好接触地面打印最低层墙体时，应保证混凝土3D打印单元3的打印头不与机械臂本体发生干涉(碰撞)；根据机械臂各连杆参数、移动平台1高度以及打印头尺寸参数，通过几何作图可以得出料口接触地面时，出料口距机械臂基座中心的最小距离为1328mm；当打印最高层墙体时，出料口距地面达到2800mm，且要求墙体厚度200mm在机械臂工作范围内，通过几何作图可以得到出料口距机械臂基座中心最大距离为2679mm，分析简图如图4(a)所示；如图4(b)为本项目建筑3D打印移动机械臂系统的目标墙体打印范围俯视图。
依据上述分析可知，为满足目标墙体的打印，墙体在机械臂基坐标系下的范围1328-2679mm的圆环区域，因为本文目标是对移动平台1相对于墙体的站位进行规划，根据建立的系统各坐标系中机械臂基座中心距移动平台1中心的距离548mm，得到目标墙体在移动平台1坐标系下的最远可达距离为3227mm；综上移动平台1相对于墙体的站位空间为半径为3227mm的圆形区域；将移动平台1相对于墙体的理论站位空间转化为移动平台1坐标系{M}在墙体坐标系{W}下的表达为：
Figure BDA0003978431720000111
式中：x,y为移动平台1坐标系{M}的原点在墙体坐标系{W}下的位置，θ为移动平台1坐标系{M}相对于墙体坐标系{W}竖直方向Z轴的姿态旋转角度；r为移动平台1相对于墙体的站位空间的最远距离，即半径，计算得r＝3227mm。
受移动平台1相对于墙体的姿态以及墙体的类型、尺寸影响，该区域仅为移动平台1理论上的可达站位空间，在该空间内并不是所有的移动平台1站位都能保证目标墙体所有点均可达，在下文中选取移动平台1站位进行站位优化时，首先会利用机械臂逆运动学进行可达性校验：根据公式(6)得出移动平台1坐标系{M}相对墙体坐标系{W}不同位姿[x,y,θ]下的
Figure BDA0003978431720000112
然后通过公式(4)机械臂逆运动学方程进行验证，存在逆解即表示在此站位下，打印工具可达作业单元的所有点；反之，若逆解不存在，则此站位不符合可达性要求，应不予考虑。
步骤3，建立机械臂的力学模型：本实施例主要采用牛顿-欧拉方法进行机械臂的动力学建模：主要研究已知机械臂末端所期望的运行轨迹，计算连杆构型、连杆质量、末端载荷、各关节角速度多种因素作用下的各关节驱动力矩；整个方法主要由两个步骤组成：首先由外递推计算各连杆的速度与加速度，由牛顿-欧拉方法计算出各连杆的惯性力和力矩，第二步向内递推计算各连杆相互作用的力和力矩，以及各关节驱动力矩。
由于在机械臂末端额定负载情况下，机械臂前三关节比后三关节对于末端精度的影响更大，所以在动力学计算部分，将仅考虑机械臂前三关节；该型号机械臂前三个连杆相关参数如表1，其中连杆质量参数合并了机械臂的肘部、前臂、手腕机构和末端法兰。
如图5所示，关节与连杆的运动学建模包含了静态部分和动态部分；静态部分代表了每个连杆的初始配置；动态部分代表了每个关节或连杆的运动效用。
静态部分：即每个连杆的初始配置，包含两个部分，静态旋转矩阵Rsi和静态体向量b′si，表达如下：
Rsi＝RxRyRz (8)
b′si＝xiX′i+yiY′i+ziZ′i (9)
式中：Rsi为静态旋转矩阵；b′si为静态体向量；Rx、Ry与Rz是局部坐标系{i}相对局部坐标系{i -1}关于X、Y、Z三个轴的旋转矩阵；X′i、Y′i和Z′i是局部坐标系{i}的三个单位矢量；b′si是在局部坐标系{i}下连杆i初始构型的矢量；“”表示局部坐标系；xi、yi和zi表示局部坐标系{i}的三个方向上的分量。
动态部分，同样也包含两个部分：动态旋转矩阵Rmi与动态体向量b′mi。
连杆i的总平移和旋转表达如下：
Figure BDA0003978431720000121
b′i＝b′si+b′mi (11)
式中：Rsi为静态旋转矩阵；b′si为静态体向量；Rmi为动态旋转矩阵；b′mi为动态体向量；
Figure BDA0003978431720000122
表达相邻连杆之间的旋转矩阵；b′i表示相邻连杆之间的平移变换；
实际上，动态旋转矩阵Rmi可视为机械臂关节的旋转电机所驱动的旋转副，Rmi与Rz(θi)等价，θi为关节i的旋转角；动态体向量b′mi可视为直线电机驱动的移动副；Rz(θi)表示绕Z轴旋转θi的旋转矩阵。
连杆坐标系{i}的位置pi和方向
Figure BDA0003978431720000123
在基坐标系下可表达如下：
Figure BDA0003978431720000131
Figure BDA0003978431720000132
式中：pi为连杆坐标系{i}在机械臂基坐标系{0}下的方向位置；
Figure BDA0003978431720000133
为连杆坐标系{i}在机械臂基坐标{0}系下的方向；b′i-1表示表示相邻连杆之间的平移变换；/>
Figure BDA0003978431720000134
表示相邻连杆之间的旋转矩阵。
对公式(12)和公式(13)分别求导可得到线速度与角速度的向前迭代方程：
Figure BDA0003978431720000135
Figure BDA0003978431720000136
/>
式中：vi为连杆坐标系{i}原点的线速度矢量；ωi为连杆i的角速度矢量；
Figure BDA0003978431720000137
表示相邻连杆之间的平移变换的导数，/>
Figure BDA0003978431720000138
表示相邻连杆之间的平移变换的导数，/>
Figure BDA0003978431720000139
其中/>
Figure BDA00039784317200001310
表示动态体向量的导数；/>
Figure BDA00039784317200001311
表示连杆i相对于连杆i-1的角速度，/>
Figure BDA00039784317200001312
其中/>
Figure BDA00039784317200001313
为关节i的角速度，即/>
Figure BDA00039784317200001314
Figure BDA00039784317200001315
表示连杆i相对于连杆i-1的静态旋转矩阵。
进一步，对公式(14)与公式(15)求导，可得到线加速度和角加速度的向前迭代方程：
Figure BDA00039784317200001316
Figure BDA00039784317200001317
式中：ai为连杆坐标系{i}原点的线加速度矢量；αi为连杆i的角加速度矢量；
Figure BDA00039784317200001318
表示连杆i相对于连杆i-1的角加速度矢量/>
Figure BDA00039784317200001319
其中/>
Figure BDA00039784317200001320
为关节i的角加速度，即/>
Figure BDA00039784317200001321
Figure BDA00039784317200001322
Figure BDA00039784317200001323
表示为连杆i相对于连杆i-1的角加速度矢量；/>
Figure BDA00039784317200001324
表示相邻连杆之间的平移变换的二阶导数。
将机械臂的连杆看成刚体，它的质心加速度ac、总质量m与产生这一加速度的作用力f之间的关系满足牛顿第二运动定律：
f＝mac (18)
当刚体绕过质心的轴线旋转时，角速度ω、角加速度α、惯性张量Ic与作用力矩m之间满足欧拉方程：
m＝Icα+ω×(Icω) (19)
如图6所示，根据公式(18)和公式(19)推导出单连杆质心处的力平衡方程和力矩平衡方程为：
mac＝mg+fj+fa (20)
Icα+ω×(Icω)＝ma+mj+fj×bc+fa×bf (21)
式中：fj表示连杆i的作用力；fa表示连杆i+1的作用力；bf表示关节i相对于连杆i的偏距；bc表示关节i-1相对于连杆i的偏距；ma表示作用在关节i上的作用力矩；mj表示作用在关节i-1上的作用力矩；g表示重力加速度。
因为连杆质心处线速度为：
vc＝vj+ω×bc (22)
式中：vc表示为连杆质心处线速度；vj表示为关节处的线速度。
对公式(22)求导得质心处线加速度为：
ac＝aj+α×bc+ω×(ω×bc) (23)
式中：ac表示质心处的加速度；aj表示关节处的线加速度；
将公式(23)带入公式(20)，得：
fj+fa＝m[(aj-g)+α×bc+ω×(ω×bc)] (24)
通过公式(24)求出fj，将其带入公式(21)，得：
ma+mj+fa×(bf-bc)＝Icα+m[(aj-g)+α×bc+ω×(ω×bc)]×(-bc)+ω×(Icω)(25)
综合公式(24)与(25)，得：
Figure BDA0003978431720000141
式中：
Figure BDA0003978431720000142
Figure BDA0003978431720000143
Figure BDA0003978431720000144
是bc的反对称矩阵，/>
Figure BDA0003978431720000145
表示/>
Figure BDA0003978431720000146
的转置矩阵。
所以根据以上推导与计算，如图7得出机械臂各关节处约束力与约束力矩向后迭代方程如下：
Figure BDA0003978431720000147
式中：wi包含关节i上的关节约束力矢量fi和约束力矩矢量mi，且wi是在基坐标系{0}下的表示，即：
Figure BDA0003978431720000151
式中：
Figure BDA0003978431720000152
表示作用力矩阵，/>
Figure BDA0003978431720000153
表示作用力矩矩阵，fiXfiY，fiZ表示作用力三个方向的分量，mix miY miZ表示作用力矩三个方向的分量。
Mi是连杆i的广义质量矩阵：
Figure BDA0003978431720000154
式中：mi为连杆i的质量；bic为连杆i的质心相对于关节i的矢量；
Figure BDA0003978431720000155
是bic的反对称矩阵；1为3×3的单位矩阵；Ii为连杆i相对关节i旋转轴线的惯量矩阵在基坐标系{0}下的表示。
Figure BDA0003978431720000156
是关节i的线加速度矢量与角加速度矢量：
Figure BDA0003978431720000157
Bi是代表包括向心力和陀螺力矩的交叉项矩阵：
Figure BDA0003978431720000158
Figure BDA0003978431720000159
代表关节i+1相对于关节i的力与力矩的转换矩阵，表达如下：
Figure BDA00039784317200001510
式中：
Figure BDA00039784317200001511
是bi的反对称矩阵；转换矩阵/>
Figure BDA00039784317200001512
是对机械臂构型的一种描述，即：在给定机械臂的情况下，该矩阵仅受到各杆件尺寸的影响。
编号为n+1的力和力矩为作用在机械臂末端点上的力和力矩，即：
Figure BDA00039784317200001513
式中：
Figure BDA00039784317200001514
表示末端的作用力矩阵，/>
Figure BDA00039784317200001515
表示末端的作用力矩矩阵，fTipX，fTipY，fTipZ表示三个方向的作用力，mTipX，mTipY，mTipZ表示三个方向的作用力矩。
wn+1由混凝土3D打印单元3以及打印单元储料桶内混凝土材料在内负载的重力与惯性力和力矩组成，因为由表2可知机械臂末端额定有效负载为90kg，所以将机械臂末端所受负载假设为恒定质量90kg。在建筑3D打印任务中，混凝土3D打印单元3出料口挤出混凝土材料时相互作用力非常小，所以此项不予考虑。
表2连杆对应的相对关机的转动惯量
Figure BDA00039784317200001516
Figure BDA0003978431720000161
通过公式(27)反向迭代计算各关节约束力/力矩，该方法首先从编号为n+1的机械臂末端执行器计算到机械臂最后一个关节n，直至计算到机械臂关节1。
为进一步求解机械臂各关节驱动力矩，因为上一节中各关节约束力/约束力矩wi是机械臂基坐标系{0}下的表示，得将wi中的关节约束力矩mi利用坐标系间的齐次变换转化为局部坐标系{i}下的表示，再取Z轴方向分量得到各关节绕关节转轴的驱动力矩τi，即：
Figure BDA0003978431720000162
式中：mi＝[mix miY miZ]，miX miY miz表示沿x，y，z三个方向上的约束力矩；Zi＝[0 01]T，
Figure BDA0003978431720000163
为连杆坐标系{i}在机械臂基坐标系下的方向。
如图8所示为在ADAMS软件中建立ADAMS虚拟样机，可在ADAMS软件中验证所建立的动力学模型的正确性。
步骤4，根据步骤2建立的整个系统管的运动学模型和步骤3建立的机械臂的力学模型，确定机械臂末端变形量作为机械臂刚度评价指标，确定机械臂速度方向可操作度作为机械臂灵巧性评价指标；确定综合判据最小化值作为机械臂综合评价指标。
确定机械臂末端变形量作为机械臂刚度评价指标的具体过程如下：
建筑3D打印移动机械臂执行任务过程中机械臂末端受到混凝土3D打印单元3以及储料桶内混凝土材料在内较大负载的作用，机械臂各个关节驱动系统需要输出相应的力矩与该末端载荷平衡，同时，驱动系统本身由于反馈系统的增益有限而具有的刚度是有限的，即各个关节刚度有限；系统执行3D打印任务过程，末端会产生相应的变形，机械臂末端抵御负载变形的能力称为机械臂笛卡尔空间刚度。
为了精确计算出工业机器人的刚度特性，建立合理的刚度模型十分重要，本发明在进行机械臂刚度建模时，做出如下假设：
(1)混凝土3D打印单元3刚体假设：建筑3D打印移动机械臂系统在执行打印任务过程中，所有的变形均来自工业机器人本体，末端执行器变形忽略不计。
(2)工业机器人关节柔性假设：机械臂受力后产生的变形全部是由于关节发生弹性扭转引起的，机械臂刚度模型旨在建立关节空间刚度矩阵与笛卡尔空间刚度矩阵的映射关系，此时认为机械臂连杆是刚性的，末端受力后连杆不发生形变。
(3)弹性变形假设：影响机械臂关节刚度的因素主要来源于关节处的伺服驱动系统和传动结构刚度，将两者的影响视作关节处发生的线弹性扭转变形。
机械臂n个关节的驱动力矩组成n维矢量τ＝[τ1，τ2，…，τn]T，n个关节对应的角度值记为θ＝[θ1，θ2，…，θn]T，在传递驱动力矩时，每个关节都会产生变形，Δθ是各关节角度变形矢量，记为Δθ＝[Δθ1，Δθ2，…，Δθn]T；每个关节的刚度用弹簧系数kθi表示，则关节所受力矩可表示为：
τi＝kθi·Δθi(i＝1，2，…，n) (36)
公式(36)可写成矩阵形式：
τ＝Kθ·Δθ (37)
式中：Kθ＝diag(kθ1，kθ2，…kθn)，是机械臂关节空间的刚度矩阵，反映了每个关节的刚度系数。
再由微分运动关系可得：
ΔX＝J(θ)·Δθ (38)
式中：J(θ)是机械臂的雅可比矩阵。
联立公式(37)和公式(38)求解得出机械臂末端产生的广义变形量：
ΔX＝J(θ)·(Kθ)-1·τ (39)
式中：ΔX为机械臂末端产生的广义变形量。
由于机械臂的前三个关节决定机械臂末端的空间位置，后三个关节决定机械臂末端的空间姿态，在机械臂执行任务过程中，末端载荷引起的末端角位移变形量很小，通常情况下可以忽略其作用，且机械臂第四、五、六关节对末端刚度的影响相比于前三关节要小的多，即可以将机械臂第四、五、六关节看作是刚体；综上所述，影响建筑3D打印过程中机械臂末端定位精度和打印质量的主要因素是机械臂前三关节产生的末端线位移变形。
KUKA KR90 R3100 extra型工业机器人前三关节刚度值：Kθ＝diag(5.0×106，6.9×106，2.8×106)，单位为：
Figure BDA0003978431720000171
对于建筑3D打印工况中，机械臂末端受到的负载包含混凝土3D打印单元3以及打印单元储料桶内混凝土材料，总体负载较大，在本文中将机械臂末端受到的总负载设为该型号机械臂的额定负载，即90kg，且始终竖直向下。下面将考虑机械臂末端X、Y、Z方向的变形情况与末端所受负载方向之间的关系，为了定量分析两者之间的关系，对四种类型的墙体分别随机选取移动平台1在某一站位下三个建筑3D打印的任务点，编号为1～12，其任务点对应的各关节角度见表3，并根据墙体轨迹计算出以下任务点对应的末端笛卡尔空间速度，通过运动微分关系式
Figure BDA0003978431720000181
计算出对应的各关节角速度。/>
表3任务点对应的各关节角度
Figure BDA0003978431720000182
基于各任务点对应的各关节角度、各关节角速度运用上述推到的机械臂动力学模型求解机械臂各关节力矩值。
其中根据墙体轨迹计算出以下任务点对应的末端笛卡尔空间速度步骤如下：
机械臂末端执行器混凝土3D打印单元3在操作空间中的运动速度
Figure BDA0003978431720000186
是一个矢量，在进行墙体3D打印过程中，混凝土3D打印单元3出料口始终竖直向下，运行过程仅包含在操作空间中的平移运动，并不包含旋转运动，故混凝土3D打印单元3末端的角速度大小为0；每一个任务点对应的末端线速度单位方向向量可通过运行轨迹前后两点在墙体坐标系{W}下坐标值计算得出，并将其转换为在机械臂基坐标系{0}下的表达如下：
(1)当运行轨迹前后两点在同一高度时，
Figure BDA0003978431720000183
式中：0pv表示在机械臂基坐标系{0}下末端的线速度；
Figure BDA0003978431720000184
表示墙体坐标系{W}相对于机械臂基坐标系{0}的旋转矩阵；wpv表示在墙体坐标系{W}下的末端线速度；/>
Figure BDA0003978431720000185
表示机械臂基坐标系{0}相对于墙体坐标系{W}的旋转矩阵；yi+1表示轨迹点i+1的y轴坐标分量；yi表示轨迹点i的y轴坐标分量；xi+1表示轨迹点i+1的x轴坐标分量；xi表示轨迹点i的x轴坐标分量。
(2)当运行轨迹前后两点不在同一高度时，此时恰好打印结束某一层，混凝土3D打印单元3竖直方向抬升，继续下一层的打印作业。
0pv＝[0 0 1]T (41)
混凝土3D打印单元3出料口始终竖直向下，运行过程仅包含在操作空间中的平移运动，并不包含旋转运动，故混凝土3D打印单元3末端的角速度大小为0。考虑3D打印材料的粘稠度、工作环境以及天气等因素，其末端在操作空间中的线速度大小设置为
Figure BDA0003978431720000191
根据上述求出的运动方向，以及线速度大小即可以得到任务点对应的末端笛卡尔空间速度。/>
图10所示为上表各任务点利用公式(39)模型计算得出的机械臂末端产生的广义变形量，其包含X、Y、Z方向的变形量大小与方向；比较发现，机械臂末端X、Y、Z方向都有不同程度的变形，但具体比较各个方向变形量的大小，可以得出机械臂末端沿X、Y轴方向的变形量大小均在0～0.5mm之间，该变形量极小，对3D打印的实现影响不大，可忽略不计；因此，将以执行3D打印过程中机械臂末端沿Z轴方向的变形量作为刚度性能评价指标，记为μz。
μz＝Z·ΔX＝Z·J(θ)·(Kθ)-1·τ (42)
式中：Z＝[0 0 1]，μz单位为：mm。
确定机械臂速度方向可操作度作为机械臂灵巧性评价指标的具体过程如下：
机械臂速度方向可操作度是指在当前位形状态下沿指定方向的速度传递特性。在机械臂的当前位姿下，系统若能以较小的关节速度使机械臂末端沿指定方向获得较大的运动速率，则认为沿此方向机械臂的速度方向可操作性好；若机械臂末端沿指定方向不能运动或运动速率低，则认为沿此方向的速度方向可操作性差。
将机械臂的关节速度定义为一个单位球，即
Figure BDA0003978431720000192
通过雅可比矩阵J(θ)映射为操作空间的一个椭球，即
Figure BDA0003978431720000193
机械臂末端执行器混凝土3D打印单元3在操作空间中的运动速度
Figure BDA0003978431720000194
是一个矢量，在进行墙体3D打印过程中，混凝土3D打印单元3出料口始终竖直向下，运行过程仅包含在操作空间中的平移运动，并不包含旋转运动，故混凝土3D打印单元3末端的角速度大小为0。每一个任务点对应的末端线速度单位方向向量可通过运行轨迹前后两点在墙体坐标系{W}下坐标值计算得出，并将其转换为在机械臂基坐标系{0}下的表达如下：
(1)当运行轨迹前后两点在同一高度时，
Figure BDA0003978431720000201
(2)当运行轨迹前后两点不在同一高度时，此时恰好打印结束某一层，混凝土3D打印单元3竖直方向抬升，继续下一层的打印作业。
0pv＝[0 0 1]T (46)
记混凝土3D打印单元3末端的运动速度为：
Figure BDA0003978431720000202
式中：
Figure BDA0003978431720000203
为混凝土3D打印单元3末端的速度方向，/>
Figure BDA0003978431720000204
通过公式(45)或者公式(46)获得，/>
Figure BDA0003978431720000205
由上述可知为[0 0 0]；β为混凝土3D打印单元3末端运行方向p上的线速度大小。
因为
Figure BDA0003978431720000206
为速度椭球上的一点，于是将公式(47)带入公式(44)中有
(βp)T(J(θ)JT(θ))-1(βp)＝1 (48)
由上式可求得
Figure BDA0003978431720000207
定义β为机械臂在方向p上的速度方向可操作度，单位：
Figure BDA0003978431720000208
如图15所示，β的物理意义为：速度椭球的中心沿方向p到椭球表面的距离，也就是混凝土3D打印单元3末端在运行方向p上的线速度极限值，它反映了机械臂关节速度和末端速度之间的传递效率，是一种衡量系统可操作度的动态性能指标。由于Jacobian矩阵为机械臂关节广义坐标和机构参数的函数，它完全取决于机械臂本身的固有性质，故以上定义的速度方向可操作度(VDM，Velocity Directional Manipulability)是从系统固有性质出发衡量机械臂在操作空间中的速度传递能力。
机械臂的运动学方程可概括为：
x＝x(q) (50)
对公式(50)的两边同时对时间t取其一阶导，得到q与x之间的微分关系为：
Figure BDA0003978431720000209
式中：
Figure BDA00039784317200002010
称为操作空间的广义速度，简称操作速度；/>
Figure BDA00039784317200002011
为关节速度；J(q)称为机械臂的雅可比矩阵。
如前所述，令关节速度在空间Rn中为单位球，当这一假设不成立时，需要对关节速度进行如下的规范化，
Figure BDA0003978431720000211
Figure BDA0003978431720000212
为关节i的最大关节角速度值。将公式(52)带入公式(51)得，
Figure BDA0003978431720000213
将公式(52)带入公式(43)得，
Figure BDA0003978431720000214
再将公式(47)带入公式(54)得，
Figure BDA0003978431720000215
公式(55)即为对关节速度规范化后的速度方向可操作度，以下简称速度方向可操作度，速度方向可操作度作为机械臂灵巧性评价指标。
基于刚度和灵巧性评价指标，确定综合指标作为机械臂综合评价指标，综合优化判据包含下面两方面内容，如式(56)所示。
Figure BDA0003978431720000216
从灵巧性判据fdexterity([x，y，θ])、刚度判据fstiffness([x，y，θ])的定义及取值范围可知，灵巧性判据fdexterity([x，y，θ])越大越好，刚度判据fstiffness([x，y，θ])越小越好。所以，对fdexterity([x，y，θ])、fstiffness([x，y，θ])做去单位处理，保留其原有计算数值结果，两者均变成无量纲量。此时，融合灵巧性与刚度性能的3D打印移动机械臂站位规划综合判据可定义为：
f([x，y，θ])＝-w1·fdexterity([x，y，θ])+w2·fstiffness([x，y，θ]) (57)
式中：w1、w2是权重系数，分别衡量灵巧性、刚度对于3D打印移动机械臂当前站位的重要程度，w2＝1-w1。
步骤5，根据步骤4得到的机械臂刚度和机械臂灵巧性的综合评价指标做为优化目标，以移动平台1相对实际作业任务在可达站位空间内的站位做为设计变量，建立单次作业的优化函数；利用遗传算法进行合理的站位优化。
针对民用建筑中常见的四种类型墙体基于遗传算法在整个可达站位空间中搜寻移动平台1的最优站位；如图9所示为本发明提供的民用建筑实例的平面结构及尺寸示意图，将其大致划分为如图11、12、13、14所示的四种常见墙体类型，并且移动平台1相对于墙体站位如图24所示。下面将针对每一类型墙体求解出一个移动平台1的最优站位。
基于遗传算法在整个站位空间中搜寻移动平台1的最优站位，最优站位求解过程是利用MATLAB遗传算法工具箱，MATLAB遗传算法工具箱扩展了MATLAB在处理优化问题方面的能力，可以用于处理传统的优化技术难以解决的问题，还可以用于解决目标函数较复杂的问题。利用MATLAB遗传算法工具箱在系统执行3D打印任务移动平台1的整个站位空间内搜寻移动平台1的最优站位，该最优站位能使得系统在执行单次3D打印任务时所有任务点中最小速度方向可操作度、机械臂末端沿Z轴方向的最大变形量综合达到最优，建筑3D打印系统执行任务过程中整体的末端作业精度得到显著提高。
下面是优化模型的建立过程：
设计变量：将移动平台1坐标系{M}在墙体坐标系{W}下的位姿[x，y，θ]作为最优站位的参数，也就是该优化模型的设计变量。
优化目标：优化目的是选择最优站位使得单次建筑3D打印工作所有任务点中综合判据f([x，y，θ])在移动平台1的可达任务工作空间中达到最小，也就是选择合理的站位使得单次建筑3D打印工作所有任务点中最小速度方向可操作度、机械臂末端沿Z轴方向的最大变形量综合达到最优。
约束条件：该问题的约束条件分为界限约束和性态约束。界限约束上述得出的移动平台1理论上可达站位空间，即公式(7)；性态约束即满足系统执行建筑3D打印过程中混凝土3D打印单元3的位置和姿态约束，即保证混凝土3D打印单元3坐标系{T}的原点与墙体轨迹重合，且其出料口姿态始终竖直向下直至完成任务。也就是需要保证墙体所有的任务点均能通过公式(6)系统运动学方程与机械臂逆运动学方程求解出至少一组解。
综上所述表明该问题是一个多变量、单目标、有约束的优化问题，遗传算法是一种全局搜索的方法，更适合工程优化问题的求解。使用遗传算法在整个站位空间中搜寻移动平台1的最优站位，能够跳出局部最优而找到全局最优点，而且遗传算法允许使用非常复杂的目标函数，并对变量的变化范围可以加以限制。
基于遗传算法在整个站位空间中搜寻移动平台1的最优站位的过程是利用MATLAB遗传算法工具箱，其扩展了MATLAB在处理优化问题方面的能力，使结果更具有准确性与高效性，解决了依靠人工根据现场情况进行移动平台1的站位布置，而使得移动平台1站位移动次数多、整体打印精度低和效率低等问题。
根据以上所述求解出了四种类型墙体的移动平台1坐标系{M}在墙体坐标系{W}下的最优位姿[x，y，θ]。计算出的结果见表4
表4融合灵巧性与刚度性能综合提升的移动平台1最优站位及综合判据结果
Figure BDA0003978431720000221
Figure BDA0003978431720000231
如图11、图12、图13、图14为分别针对一字墙体、直角墙体、圆弧墙体、T形墙体作业任务建筑3D打印移动机械臂基于综合判据的最优站位示意图。如图16-23为上述四种墙体最优站位下的所有点的速度方向可操作度和机械臂末端沿Z轴方向变形量示意图。且表5可以看出最优站位下对应的机械臂灵巧性与刚度性能，即上述四种类型墙体最小速度方向可操作度、机械臂末端沿Z轴方向的最大变形量计算结果。由此表明移动平台1相对于墙体的站位经过遗传算法优化后，系统在执行墙体打印任务时，整体的机械臂末端作业精度得到了显著的提高，由此优化了建筑3D打印出的墙体质量。
表5最优站位下对应的最小速度方向可操作度、机械臂末端沿Z轴方向的最大变形量
Figure BDA0003978431720000232
实施例2
本实施例与实施例1相比其他部分相同，不同之处在于：在对于移动机械臂载荷较大的情况，如果其位姿变化小且工作范围不大，此时，我们会增大刚度的权重，即通常将w2＝0.7～0.9。例如：重载搬运。
对于移动机械臂载荷较小的情况，如果其位姿变化大且工作范围大，此时，我们会增大灵巧性的权重，即通常将w1＝0.7～0.9。例如：上肢康复、曲面打磨。
对于其他情况，如果移动机械臂的应用没有特定的要求，通常我们会将两个权重取值接近，即将w1＝0.3～0.6。

Claims (10)

1.一种面向灵巧性和刚度提升的机械臂站位规划方法，其特征在于，包括以下步骤：
步骤1，建立坐标系：确定移动平台坐标系、机械臂基坐标系、机械臂各连杆坐标系、法兰坐标系、打印单元坐标系和墙体坐标系；
步骤2，根据步骤1建立的所有的坐标系，建立整个系统的运动学模型，根据整个系统的运动学模型及实际作业任务确定移动平台相对实际作业任务的可达站位空间；
步骤3，根据步骤1建立的所有的坐标系和机械臂末端所期望的运动轨迹，计算连杆构型、连杆质量、末端载荷和各关节角速度的因素作用下的各关节驱动力矩，建立机械臂的力学模型；
步骤4，根据步骤2所述的运动学模型和步骤3所述的力学模型，确定机械臂末端变形量作为机械臂刚度评价指标，确定机械臂速度方向可操作度作为机械臂灵巧性评价指标；确定综合判据最小化值作为机械臂综合评价指标；
步骤5，根据步骤4得到的机械臂刚度和机械臂灵巧性的综合评价指标做为优化目标，以移动平台相对实际作业任务在可达站位空间内的站位做为设计变量，建立单次作业的优化函数；根据作业的性质，优化函数中各部分的权重，利用遗传算法进行合理的站位优化。
2.如权利要求1所述的方法，其特征在于，在步骤4中，确定机械臂综合判据最小化值作为机械臂综合评价指标的具体过程为：综合机械臂灵巧性和刚度性能，其中，综合判据f([x,y,θ])记作
f([x,y,θ])＝-w1·fdexterity([x,y,θ])+w2·fstiffness([x,y,θ])
Figure FDA0003978431710000011
式中：μZ是机械臂末端沿Z轴方向的变形量，
Figure FDA0003978431710000017
是对关节速度规范化后的速度方向可操作度，w1、w2是权重系数，分别衡量灵巧性、刚度对于移动机械臂当前站位的重要程度；w1和w2由任务性质决定取值，其中，w2＝1-w1。
3.如权利要求2所述的方法，其特征在于，所述机械臂为六连杆机械臂，包括第一连杆、第二连杆、第三连杆、第四连杆、第五连杆和第六连杆，根据改进的DH参数法分别确定机械臂各连杆坐标系。
4.如权利要求3所述的方法，其特征在于，在步骤2中，建立整个系统的运动学模型的步骤具体为：首先根据机械臂各连杆坐标系和机械臂各连杆的结构尺寸确定机械臂运动学模型；然后根据移动平台的尺寸、机械臂安装在移动平台上的位置和作业任务确定整个系统的运动学模型，即
Figure FDA0003978431710000012
式中：
Figure FDA0003978431710000013
为移动平台坐标系相对于机械臂基坐标系的齐次变换矩阵；/>
Figure FDA0003978431710000014
为打印单元坐标系相对于法兰坐标系的齐次变换矩阵；/>
Figure FDA0003978431710000015
为打印单元坐标系相对墙体坐标系的齐次变换矩阵；/>
Figure FDA0003978431710000016
为移动平台坐标系{M}相对墙体坐标系{W}的齐次变换矩阵；/>
Figure FDA0003978431710000021
为法兰坐标相对于第六连杆坐标系的齐次变换矩阵；/>
Figure FDA0003978431710000022
为第六连杆坐标系相对机械臂基坐标系的齐次变换矩阵；/>
其中，第六连杆的坐标系相对机械臂基坐标系的齐次变换矩阵
Figure FDA0003978431710000023
由整个系统的运动学模型求得，为
Figure FDA0003978431710000024
根据机械臂运动学模型列出机械臂的逆运动学方程，根据机械臂的逆运动学方程校验移动平台相对实际作业任务在可达站位空间内规划的某一站位下，判断机械臂的末端位姿序列中是否各点均存在运动学逆解且满足各轴关节角度限制。
5.如权利要求4所述的方法，其特征在于，在步骤2中，根据整个系统的运动学模型及实际作业任务确定移动平台相对实际作业任务的可达站位空间的具体过程为：首先根据机械臂运动学模型绘制机械臂的可达工作空间云图；然后根据实际作业任务的尺寸、机械臂安装在移动平台上的位置和机械臂末端工具的尺寸确定移动平台相对作业任务的可达站位空间；移动平台相对作业任务的可达站位空间转化为移动平台坐标系在墙体坐标系下的位姿：
Figure FDA0003978431710000025
式中：x,y为移动平台坐标系的原点在墙体坐标系下的位置，θ为移动平台坐标系相对于墙体坐标系竖直方向Z轴的姿态旋转角度，r表示移动平台相对于作业任务的站位空间的最远距离。
6.如权利要求5所述的方法，其特征在于，在步骤3中，械臂力学模型被建立的步骤具体为：首先由外递推计算各连杆的速度与加速度，再由牛顿-欧拉公式计算出各连杆的惯性力和力矩；向内递推计算各连杆相互作用的力和力矩，以及各关节驱动力矩；
机械臂各连杆的惯性力矩：
fj+fa＝m[(aj-g)+α×bc+ω×(ω×bc)]
式中：m表示连杆质量；α表示为角加速度；ω表示角速度；fj表达连杆j的作用力；fa表示连杆i+1作用力；aj表示连杆j的线加速度；bc表示关节与连杆质心的偏距；
机械臂各连杆的力矩：
Figure FDA0003978431710000026
式中：
Figure FDA0003978431710000027
Figure FDA0003978431710000028
Figure FDA0003978431710000029
是bc的反对称矩阵，/>
Figure FDA00039784317100000210
表示/>
Figure FDA00039784317100000211
的转置矩阵；
求得各关节绕转轴的驱动力矩：
Figure FDA0003978431710000031
式中：
Figure FDA0003978431710000032
为连杆坐标系{i}在机械臂基坐标系下的方向，i＝1、2、3、4、5、6，mi为关节i上的约束力矩矢量，mi＝[mix miy miz]，mix,miy,miz表示沿x,y,z三个方向上的约束力矩，Zi＝[00 1]T。
7.如权利要求6所述的方法，其特征在于，在步骤4中，确定机械臂末端变形量作为机械臂刚度评价指标的具体过程为：以机械臂末端沿Z轴方向的变形量为机械臂刚度评价指标，其中，机械臂末端沿Z轴方向的变形量记作μz
μz＝Z·ΔX＝Z·J(θ)·(Kθ)-1·τ
式中：Z＝[0 0 1]，ΔX为机械臂末端产生的广义变形量，J(θ)为机械臂的雅可比矩阵，τ为τi的矩阵式，Kθ为机械臂关节空间的刚度矩阵。
8.如权利要求7所述的方法，其特征在于，在步骤4中，确定机械臂速度方向可操作度作为机械臂灵巧性评价指标的具体过程为：在当前位形状态下沿指定方向的速度传递特性，以机械臂速度方向可操作度为机械臂灵巧度评价指标，其中，机械臂在方向p上的速度方向可操作度记作β
Figure FDA0003978431710000033
式中：
Figure FDA0003978431710000034
J(θ)为机械臂的雅可比矩阵，/>
Figure FDA0003978431710000035
为[0 0 0],0pv＝[0 01]T。
9.如权利要求8所述的方法，其特征在于，在步骤5中，优化函数以步骤2确定的移动平台相对实际作业任务的可达站位空间为界限约束，以移动平台在移动平台相对实际作业任务的可达站位空间内相对实际作业任务的位姿满足整个系统的逆运动学方程为性态约束。
10.如权利要求9所述的方法，其特征在于，在步骤1中，移动平台坐标系、机械臂基坐标系、机械臂各连杆坐标系、法兰坐标系、打印单元坐标系和墙体坐标系均为右手坐标系。
CN202211542675.XA 2022-12-02 2022-12-02 一种面向灵巧性和刚度提升的机械臂站位规划方法 Pending CN115847410A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211542675.XA CN115847410A (zh) 2022-12-02 2022-12-02 一种面向灵巧性和刚度提升的机械臂站位规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211542675.XA CN115847410A (zh) 2022-12-02 2022-12-02 一种面向灵巧性和刚度提升的机械臂站位规划方法

Publications (1)

Publication Number Publication Date
CN115847410A true CN115847410A (zh) 2023-03-28

Family

ID=85669609

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211542675.XA Pending CN115847410A (zh) 2022-12-02 2022-12-02 一种面向灵巧性和刚度提升的机械臂站位规划方法

Country Status (1)

Country Link
CN (1) CN115847410A (zh)

Similar Documents

Publication Publication Date Title
Zhuang et al. Method for kinematic calibration of Stewart platforms
CN107703748B (zh) 一种基于偏置板设计的重载机器人静态刚度辨识方法
US8560122B2 (en) Teaching and playback method based on control of redundancy resolution for robot and computer-readable medium controlling the same
CN102207988B (zh) 一种多自由度机械臂高效动力学建模方法
CN111890349A (zh) 一种四自由度机械臂运动规划方法
Adam et al. Modeling and simulation of 5DOF robot manipulator and trajectory using MATLAB and CATIA
Xu et al. Design, analysis and optimization of Hex4, a new 2R1T overconstrained parallel manipulator with actuation redundancy
CN114888812B (zh) 一种基于刚度性能优化的移动机械臂站位规划方法
CN114055467A (zh) 基于五自由度机器人的空间位姿在线仿真系统
Hassani et al. Full dynamic model of 3-UPU translational parallel manipulator for model-based control schemes
CN115847410A (zh) 一种面向灵巧性和刚度提升的机械臂站位规划方法
Kim et al. Kinematic Implementation of 3-DOF 2-Link Type Vehicle Simulator: Kinematic analysis and motion control method for 3-DOF 2-link type vehicle simulator
Liu et al. Design and motion planning of a 7-DOF assembly robot with heavy load in spacecraft module
Chávez et al. Kinematic and dynamic modeling of the phantomx ax metal hexapod mark iii robot using quaternions
Szep et al. Kinematics, workspace, design and accuracy analysis of RPRPR medical parallel robot
Patel et al. Simulation of kinematic and workspace analysis of 3-PRS parallel manipulator
Hu et al. The kinematic analyses of the 3-DOF parallel machine tools
Ren et al. A self-tuning iterative calculation approach for the forward kinematics of a Stewart-Gough platform
Park et al. Reliable computation of minimum‐time motions for manipulators moving in obstacle fields using a successive search for minimum‐overload trajectories
Binford Computer integrated assembly systems
Akhbari et al. Kinematic and workspace analyses of a novel 4-dof parallel mechanism
Ren et al. A Heuristic Iterative Method for Solving the Inverse Solution of Kinematics of Manipulators
Fraczek et al. Calibration of multi-robot system without and under load using electronic theodolites
Park Convergence properties of gradient-based numerical motion-optimizations for manipulator arms amid static or moving obstacles
Ghafil et al. Optimum dynamic analysis of a robot arm using flower pollination algorithm

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination