六自由度运动方程计算
6DOF
尺寸类型
描述
第一个
向量
三个力作用在弹体坐标系中。
第二个
向量
三个力矩作用在弹体坐标系中。
输出
尺寸类型
描述
第一个
三元素向量
地面参考坐标系中的三个分速度分量。
第二个
三元素向量
地面参考坐标系中的三个位移分量。
第三
三元素向量
欧拉旋转角度(横滚、俯仰、偏航),弧度制。
第四
3×3矩阵
从地面坐标系到弹体坐标系的转换矩阵。
物体固定坐标系的旋转动力学,应用时刻在哪里(L M N)T,惯性张量我是关于原点O。
物体固定角速度向量之间的关系,p q r T,欧拉角的变化率,,可以确定通过向物体固定利率解决欧拉坐标系。
反相J然后给所需的关系确定欧拉向量。
对话框
单位
指定输入和输出单位:
单位
力
力矩
加速度
速度
位置
质量
转动惯量
Metric (MKS)米制
N
m
量纲
English英制(Velocity in ft/s)
英镑
磅 英尺
英尺/秒平方
英尺/秒
英尺
斯勒格
斯勒格 平方英尺
English英制(Velocity in kts)
英镑
磅
英尺
英尺/秒平方
海里/小时
英尺
斯勒格
斯勒格 平方英尺
质量类型
Fixed
在整个仿真过程质量是恒定的。
Simple Variable
质量和转动惯量线型变化。
Custom Variable
质量和转动惯量变化是可定制的。
选择要使用的类型的质量:
船舶动力学
)] i
[(
[(
dt dp dr 2 2 ru pw ) y ( r p ) z ( qr ) x ( qp )] j dt dt dq dp 2 2 pv qu ) z ( p q ) x ( rq ) y ( rq )] k } dxdydz dt dt
1
cos cos cos sin sin sin cos cos sin cos sin sin
sin cos sin sin sin cos cos sin sin cos cos sin
V i ( u qz ry ) i ( v rx pz ) j ( w py qx ) k
1.2.1 作用于船舶的微元上的速度和加速度
di i ( p i q j rk ) i rj q k dt dj j ( p i q j rk ) j p k r i dt dk k ( p i q j rk ) k q i p j dt
1.1.2 船体坐标系-动坐标系
原点O取船体上任意一点，纵轴Ox平行 于船舶横摇轴并指向船首，横轴Oy平行 于纵摇轴并指向左舷，垂直轴Oz指向船 体。 O-xyz构成一个左手坐标系 常将O取在船舶重心上 Ox 横摇轴 Oy纵摇轴 Oz首摇轴 不是惯性坐标系
船舶六个自由度的运动
沿三个坐标轴的直线运动 围绕三个坐标轴的旋转运动
v
M
p
M
r
0
X H 1 X u 0 YH 1 Z H 1 Z u 0 K H1 M M u H1 N H1 0
飞行器运动方程
绕
轴转 得到 x1 y1 z g oz
x1 cos y1 sin zg 0
2、线运动学方程式
得到 再绕轴 oy 1 转
xy1 z 2
x cos y 0 1 z2 sin
相垂直，向右为正。
：沿ox轴向量，向前为正。
p、q、r为飞机绕机体三轴的角速度。 当 0, 0时，没有一个角速度分量是水 平或垂直的。
1、角运动学方程式
把 向机体三轴投影的话，只有 p 包含 的 ,,
先令
的投影分量。为简单起见， 全部，p,q,r都包含 ,
2、线运动学方程式
xg V cos cos yg V cos sin h V sin
飞机六自由度方程组（1）
状态向量：
u v w Fx u vr wq g sin m v ur wp g cos sin Fy m Fz w uq vp g cos cos m
3、角运动方程式
飞机动量矩的推导：
r
dm
dL r ( r )dm
3、飞机运动方程
方程应包括动力学方程及运动学方程：
运动学方程——通过体轴系与地轴系的关系，找 出体轴系下角速度、位移量与地面轴系下角速 度、位移量的关系。
2、线运动方程
用机体系表示绝对参数变化时： ~ dv dv Iv v dt dt ~ dv 1v V dt 为速度向量 V 相对于动坐标系的变化率， 为由于动坐标系转动而引起的向量变化率，是牵连 加速度。
L dL r ( r )dm iLx jLy kLz
六自由度机器人运动学及主要构件的有限元分析
第6期2021年2月No.6February ，2021六自由度机器人运动学及主要构件的有限元分析摘要：文章以六自由度机器人为研究对象，根据实际的作业情况，对机器人进行运动学分析以及主要构件的有限元分析。
运动学分析分为正运动学分析和逆运动学分析，解决的是机器人的手臂转向何方，分析的是手部的速度、加速度和位移。
有限元分析主要是机械系统静力学分析。
对主要构件建立模型、模型简化、网格划分，根据危险工况的受力情况，分析了各构件的应力、形变等性能，确保结构设计合理。
对于工业机器人机械结构、传动等方面，运动学和有限元分析能够判断整机设计是否达到设计目标，对结构件的优化设计具有重要的意义。
关键词：六自由度；机器人；运动学；有限元分析中图分类号：TP242.2文献标志码：A 程锴（南京以禾电子科技有限公司，江苏南京210039）作者简介：程锴（1981—），男，江苏南京人，工程师，硕士；研究方向：电子产品总体结构设计。
江苏科技信息Jiangsu Science &Technology Information引言在当前科学技术不断进步和快速发展的背景下，很多先进的技术手段被广泛应用在各个领域中［1］。
特别是机器人在工业中得到广泛的应用，在实际运行过程中，类似于码垛搬运的六自由度机器人在搬运货物中节省大量劳动力，但安全性与可靠性一直备受考验。
因此，本文主要对六自由度机器人进行运动学和静力学分析［2］。
机器人运动学研究解决的是机器人的手臂转向何方，分析的是手部的速度、加速度和位移。
运动学方程是进行机器人位移分析的基本方程，也称为位姿方程。
机器人运动学分为正运动学分析和逆运动学分析。
正运动学是机器人运用各个关节角度、各个构件车长度等已知条件来判断末端执行器在三维空间中的位置；而逆运动学正好相反，它解决的是机器人需要如何运动才能使得末端执行器到达指定位置这一问题。
静力学分析用来分析结构在给定静力载荷作用下的响应。
六自由度机械臂逆运动学算法
六自由度机械臂逆运动学算法六自由度机械臂逆运动学算法六自由度机械臂逆运动学算法朱齐丹王欣璐（哈尔滨工程大学，哈尔滨，150001）摘要：根据D-H参数法确定六自由度机械臂的运动学方程，结合平面几何法和欧拉角变换法将机械臂的逆运动学求解问题分为两部分，一通过平面几何法确定机械臂腕部点的坐标与前三个关节角的关系，二通过欧拉角变换法确定机械臂末端姿态与后三个关节角的关系，根据逆运动解的选取原则从八组解中选取最优解；利用MATLAB中的Robotics Toolbox建立机械臂的正运动学模型，通过多组位姿下的正逆运动解对比验证逆运动学求解算法的准确性；利用VC++中的QueryPerformanceCounter函数和MATLAB中tic-toc 语句得到不同算法所消耗的平均时间，通过消耗时间的对比说明该算法的快速性；利用VC++编程实现机械臂写字的过程，通过对比输入字的形状与机械臂末端的实际运动轨迹，进一步验证该算法是一种快速而准确的逆运动学求解算法。
关键词：机器人，六自由度，机械臂，逆运动解，平面几何法，欧拉角变换法0 引言机械臂被广泛应用于机械制造、航空航天、医疗和原子能等领域，机械臂的逆运动学问题是其轨迹规划与控制的重要基础，逆运动学求解是否快速准确将直接影响到机械臂轨迹规划与控制的精度，因此针对工业中常用的六自由度机械臂，设计一种快速准确的逆运动学求解方法是十分重要的。
目前，机械臂逆运动学的求解方法主要有：迭代法、解析法和几何法。
迭代法虽然在大多数情况下是可行的，但却无法得到全部解；解析法计算较为复杂，但可以得到全部根；几何法针对机械臂的某些特殊结构进行简化，再进行求解，虽然对于一般机械臂不通用，但是其形式简单，求解所需的计算量远远小于迭代法和解析法。
Paul 等[1]于1981年提出的解析算法对后来的机械臂逆运动学问题研究有着指导性意义。
Regnier[2]于1997年提出一种基于迭代法和分布式的算法，能够求出多种结构的六自由度机械臂的位置逆解，但相应的计算时间也会变长。
六自由度混联机构运动学分析
六自由度混联机构运动学分析周辉;周睿;朱景原;丁泽华;曹浩峰;曹毅【摘要】混联机构结合了并联机构和串联机构两者的优点,有效地扩大了机器人的应用范围.提出了一种由两个不同的三自由度并联机构串接而成的混联机构,对其进行运动学分析,分别求取上下两个并联模块的运动学正解方程,从而得到整个混联机构的运动学正解.通过给定机构结构参数和驱动输入参数,用一组算例求得运动学动平台位姿,并画出三维机构位姿状态图模型,更加直观地了解机构的位姿状态.然后求解其运动学逆解,按照运动学正解结果,给定机构的两组位姿,求得此时机构的驱动输入参数及各转动副的状态,对比正反解的结果,进而验证了双并联型混联机构运动学正反解模型的正确性.最后,在运动学分析的基础上,利用极限坐标搜索法,结合混联机构的运动学反解,给出机构工作空间求解的算法.给定机构的结构参数,考虑杆长约束、关节转动副约束及杆件干涉等影响条件下,利用数值搜索法在圆柱坐标系中搜索工作空间的边界.在Matlab软件中仿真得到混联机构在给定不同姿态下的位置工作空间和给定位置下的姿态工作空间.%Hybrid mechanisms combine the advantages of parallel mechanisms and series mechanisms,which can effectively expand the application field of robot.A hybrid mechanism is presented,which is combined by two different 3-DoF parallel mechanisms connected in series.Through the analysis of the kinematics of the upper and lower parallel modules,the forward kinematics solution of the whole hybrid mechanism is obtained.A set of numerical examples of position and posture of the moving platform is obtained by given the structural parameters and driving parameters,meanwhile,the 3D models of the mechanism are obtained in order to more intuitive understanding of themechanism.Then the inverse kinematics solution is solved.According to the forward kinematic results,a group of position and posture is given to obtain the driving input parameters of the mechanism and the state of each rotation pair,by compared with the result of the forward and inverse solutions,the correctness of the forward and inverse solution is verified.Finally,on the basis of kinematics analysis,using limit coordinate searching method,algorithm of working space is put out.Considering the length constraints,joint constraint and the interference of the member,the boundary of workspace is obtained in cylindrical coordinates by given the structural parameters.Position workspace respect to a variety of postures posture and orientationworkspace respect to a variety of positions are simulated in Matlab.【期刊名称】《东华大学学报（自然科学版）》【年(卷),期】2017(043)005【总页数】11页(P709-719)【关键词】混联机构;运动学;极限坐标搜索法;工作空间【作者】周辉;周睿;朱景原;丁泽华;曹浩峰;曹毅【作者单位】江南大学机械工程学院,无锡江苏214122;江苏省食品先进制造装备技术重点实验室,无锡江苏214122;江南大学机械工程学院,无锡江苏214122;江苏省食品先进制造装备技术重点实验室,无锡江苏214122;江南大学机械工程学院,无锡江苏214122;江苏省食品先进制造装备技术重点实验室,无锡江苏214122;江南大学机械工程学院,无锡江苏214122;江苏省食品先进制造装备技术重点实验室,无锡江苏214122;江南大学机械工程学院,无锡江苏214122;江苏省食品先进制造装备技术重点实验室,无锡江苏214122;江南大学机械工程学院,无锡江苏214122;上海交通大学系统控制与信息处理教育部重点实验室,上海200240;江苏省食品先进制造装备技术重点实验室,无锡江苏214122【正文语种】中文【中图分类】TH112混联机构结合了传统的串联机构和并联机构的优点，具有承载能力强、刚度大、精度高、工作空间大等优点，有效地扩大了机器人的应用领域[1-2].混联位置分析是机构分析中的基础问题，并联机构的位置分析具有反解简单、正解复杂的特点，而串联机构正好相反.两者综合起来，混联机构的位置正解和反解都变得极为复杂[3]. 目前对于混联机构整体位置正反解的研究较少.Pisla等[4]研究了一种由PARAMIS 机器人和一个新型串联定位模块组成的混联机构，并给出了数值解.Zeng等[5]研究了空间多回路运动支链的串并联型混联机构，提出了一种分析输出位移子集和空间多支链机构的自由度的方法.文献[6]提出了一种适用于重载操作的串并联锻造机械臂，根据机构的几何约束求得了封闭形式的运动学正反解.刘海涛等[7]研究了一类由二自由度球面并联机构和一条通过移动副与之串接的两转动一移动串联运动链组成的五自由度可重构混联机械手的设计和尺度综合问题.Lu等[8]提出了一种上端由3-RPS并联机构，下端由3-SPR并联机构串接起来的混联机构，分析了机构的位置正反解、速度、加速度、位置工作空间和姿态工作空间.本文提出的混联机构由两个并联机构模块串接而成，每个并联模块由两条SPS六自由度的支链，由于其具有6个自由度，故只需对另外一条支链进行运动学分析即可.然后分析了机构的位置工作空间和姿态工作空间，对该混联机构的推广应用提供一定的理论基础.混联机构由两个并联机构串联起来，每个并联机构都有3个自由度.如图1所示，位于混联机构下端部分的并联机构具有两条SPS支链和一条RPR支链，其中RPR 支链3个运动副呈R1⊥P⊥R2.上面的并联机构具有两条SPS支链和一条RRR支链，其中RR支链的3个转动副相互垂直，呈R3⊥R4⊥R5. R2和R3的转动轴线相互垂直.下面并联机构模块的驱动是位于3条分支上的3个移动副，上面并联机构模块的驱动是两个位于分支上的移动副和另一条支链上中间的R4转动副.上下两个并联机构的动平台和静平台都是正三角形.正确分析计算机构自由度是机构结构分析的基础，是机构运动学分析的重要内容.机构自由度分析为确定机构驱动数目及进行运动学分析提供了依据.位于底部的并联机构2SPS+RPR称为并联模块1，位于上端的并联机构2SPS+RRR称为并联模块2.整个混联机构总的构件数目n=15，包括一个静平台，一个动平台，一个中间平台.运动副有g= 18，包括8个球副S，5个移动副P，5个转动副R.由于SPS支链都可以绕着自身两端的球副旋转，所以每条SPS支链存在一个局部自由度[9]，因此ζ=4.运用计算空间机构自由度的Grübler-Kutzbach公式得到此混联机构的自由度[10]为式中：fi为第i个运动副的自由度数；ν为并联冗余约速，这里不存在.由式(1)可知，此双并联型混联机构具有6个自由度.混联机构附坐标系的机构简图如图2所示.R2和R5转动副的轴线分别垂直于支链A3B3和B3D3，它们分别与B3B2和D3D2重合.参考坐标系OXYZ以A3为原点置于静平台上，OX轴与A3A2共线，坐标原点O与A3点重合.坐标系O1X1Y1Z1的Z1轴附于支链A3B3上，坐标原点O1与A3点重合.O2X2Y2Z2附于中间平台上，O2X2轴与B3B2共线，坐标原点O2与B3点重合.R2和R3的转动轴线相互垂直.坐标系O3X3Y3Z3和坐标系O4X4Y4Z4分别固定在支链B3D3的两端，Z3轴和Z4轴与B3D3重合，坐标原点O3和O4分别与B3和D3点重合.O5X5Y5Z5位于上部并联机构的动平台上，O5X5的轴线即转动副的R5的轴线沿着D3D2方向，坐标原点O5与D3点重合.由机构的坐标关系可得相应的变换矩阵为式中：si和ci(i=1， 2， 3， 4， 5)分别表示sin θi和cos θi，θi表示i个转动副的角度；L3=‖A3B3‖；L6=‖B3D3‖；Ai表示坐标系OiXiYiZi相对于坐标系Oi-1Xi-1Yi-1Zi-1的变化(i=1， 2， 3， 4， 5).混联机构的变换矩阵可以写为首先，给定驱动支链坐标系及驱动支链的长度为L1， L2， L3， L4， L5和驱动角度θ4，分别求取两个并联机构的运动学正解，然后再求整个混联机构的运动学正解.末端操作器的位置相对于参考坐标系可以写为式中: AM1=A1A2， AM2=A3A4A5分别为上下两个并联机构的齐次变换矩阵.AM1是描述中间动平台相对于参考坐标系的位置和姿态的齐次变换矩阵，AM2是描述上面并联机构动平台相对于坐标系O2X2Y2Z2的位置和姿态的齐次变换矩阵.5rp是动平台相对于坐标系O5X5Y5Z5的向量.本文中左上标表示向量相对的坐标系，相对于参考坐标系OXYZ则没有上标.首先定义静平台和动平台上各端点关于各自坐标原点的向量，得到下面4个向量如式(4)～(7)所示.位置向量OBi(i=1， 2)相对于参考坐标系可以写为支链长度L1和L2(L1=A1B1， L2=A2B2)可以写为式中：‖·‖表示欧式范数.将其展开得到支链一和支链二的表达式为化简式(10)和(11)可以得到下列带有未知角θ1和θ2的方程：式中： p1、 p2、 q1、 f1、 f2、 f3、 f4、 f5、 f6、 L3的表达式为由式(12)可以得到θ1的表达式.化简式(12)并用三角函数万能公式进行代换，得到θ1的解为得到θ1解之后，整理式(13)就可以写为式中： p3=f3+f5s1； p4=f4； q2=-f1s1-f2c1-f6.这样，式(15)就和式(12)保持相同的形式，使用相同的三角函数万能公式代换可求出θ2的解为由于θ1的解有两个，所以p3， q2的解有两个，所以代入求θ2的解就有4种情况.得到θ1和θ2的解，把它们代入A1和A2中，就可以得到下端并联机构变换矩阵AM1的形式.这样就得到了下端并联机构的位置正解.下端并联模块正解有4组解.上端并联机构的位置正解可以通过和下部并联机构类似的求解方法得到.首先，得到以下向量：位置向量O2Di(i=1， 2)相对于坐标系O2X2Y2Z2可以写成：支链长度L4和L5(L4=B1D1， L5=B2D2)可以写为将式(17)、(18)和式(21)所得的2O2D1、 2O2D2代入式(22)就可以得到：将式(23)和(24)展开，并化简可以得到下列两个有关于未知角θ3和θ5的方程为式中：式(25)是一个关于θ3的表达式.化简式(25)并用三角函数万能公式进行代换，得到θ3的解为得到θ3的解之后，就可以将式(26)写为式中：v3=w3+w5s3； v4=w4+w6c3； q4=-w1s3-w2c3-w7.运用三角函数万能公式，同理就可以求出θ5的解为由θ3和θ5的表达式可知，θ3和θ5都有两种表达形式，故θ3有两组解，而θ5表达式中的v3、 v4和q4都是关于θ3的表达式，将不同的θ3值代入v3、 v4和q4中，会得到两组不同的解，所以θ5也就有4组解.因此上部并联机构模块也有4种情形.此六自由度混联机构的两个并联模块都有4种位置正解，这样每个并联模块都可以通过4种方式达到指定的位置.通过上述推导得到θ1，θ2，θ3和θ5的表达式，就可以求出整个六自由度混联机构的位置正解变化矩阵A.式中：nx=c4(c1c3-c2s1s3)+s1s2s4；ny=c2s4+c4s2s3；nz=c1s2s4-c4(c3s1+c1c2s3)；ox=s5(c1s3+c2c3s1)-c5[s4(c1c3-c2s1s3)-c4s1s2]；oy=c5(c2c4-s2s3s4)-c3s2s5；oz=c5[s4(c3s1+c1c2s3)+c1c4s2)-s5(s1s3-c1c2c3)；ax=s5[s4(c1c3-c2s1s3)-c4s1s2]+c5(c1s3+c2c3s1)；ay=-s5(c2c4-s2s3s4)-c3c5s2；az=-s5[s4(c3s1+c1c2s3)+c1c4s2]-c5(s1s3-c1c2c3)；px=L3s1+L6(c1s3+c2c3s1)；py=-L6c3s2；pz=L3c1-L6(s1s3-c1c2c3).对于矩阵A前三行三列表示混联机构末端执行器的姿态方程，式(31)为混联机构的位置方程.根据六自由度混联机构的运动学位置正解，通过具体算例来验证此混联机构的位置正解.定义整个混联机构的结构尺寸参数并给定一组驱动构件的输入参数L1、 L2、L3、 L4、 L5、θ4，根据运动学正解求解公式，采用数值法分别求出上下两个并联机构模块的位置正解的解析解.分别得到上端和下端并联机构模块静平台和动平台正三角形三个端点相对于世界坐标系的位置坐标，给出位姿的三维位姿图，更加直观地了解机构的位置和姿态.给定整个混联机构的尺寸参数如下：上下两个并联机构模块的动平台和静平台都是正三角形，假定下面并联机构模块的静平台边长为0.5 m，动平台的边长为0.4 m.下端并联机构模块结构参数如表1所示.由于上端并联机构模块的静平台和下面并联机构模块的动平台重合，所以上面并联机构模块的静平台边长也为0.4 m，假定上面并联机构模块的动平台边长为0.3 m.上端并联机构模块的结构参数如表2所示.同时，上端并联机构模块的第三条支链长度给定L6=0.5 m.给定六自由度混联机构运动输入如表3所示.根据上述输入，就可以得到下端并联机构模块的各端点相对于参考坐标系的位置坐标数值，如表4所示.上端并联机构模块各端点相对于参考坐标系的位置坐标数值，如表5所示.上述分析得到了上下两个并联机构模块动平台和静平台各端点相对于各自静平台的位置坐标.由于上端并联机构模块的静平台与下端并联机构模块的动平台重合，故将上端并联机构模块的静平台位置与下端并联机构模块的动平台位置保持一致.只需将上端并联机构模块静平台的3个点B1、 B2、 B3的坐标值乘以变换矩阵AM1，就可以得到B1、 B2、 B3在坐标系OXYZ下的值.同理，将上述求得的D1、 D2、 D3点的坐标值也需要相应地乘以变换矩阵AM1，就可以得到如表6所示的D1、 D2、 D3点在坐标系OXYZ下的值.对于整个六自由度混联机构，可以得到机构在此输入状态下的4种位姿情况，如图3所示.这4种不同解析解情况下的位姿与表6给出的解析解对应.由图4可以直观地看出，第四组数值解支链之间不会发生干涉，符合要求.在给定双并联型混联机构上平台的位姿参数，求整个机构输入关节的运动状态是运动学位姿反解问题.求此混联机构的位置反解，就是给定上端并联机构模块动平台上末端的位姿，求解6个驱动，即5个移动副Li(i=1， 2，…， 5)和1个转动副θ4.假定给定混联机构末端执行器的位姿用变换矩阵表示为式中：px， py， pz表示固连在上端并联机构模块末端执行器上的坐标系O5X5Y5Z5原点O5的位置坐标.由式(30)可以得到混联机构的位置向量可以写成p=(px， py， pz， 1)T，式中px， py， pz的值如式(31)所示.将式(2)左右两边同时右乘可以得到：代入展开可得：令方程(34)左右两边的(1， 3)，(2， 3)，(3， 3)元素相等，可得：观察发现式(31)中py的表达式与式(36)表达式右边都有s2c3，所以把式(31)中py 的表达式代入式(36)可以得到：式中：q5=py/L6.同理，将式(35)代入式(31)中px，式(37)代入式(31)中pz，可以得到：将式(39)和(40)左右两边同时取平方然后相加就可以得到驱动L3的表达式为由于θ5有两组解，故L3也就有两组解.将L3的值代入式(39)和(40)中，从而可以得到θ1的表达式为式中：s1=；c1=.将式(2)两边同时左乘可以得到：由式(43)第二行第四列左右两边的元素相等可得：从而可以得到θ2的表达式为由θ2的表达式可知，θ2有两组解.将式(31)中px的表达式写成：式中：f1=c1； f2=s1c2； f3=.由式(46)，并用三角函数万能公式就可以得到θ3的表达式为由θ3的表达式可知，θ3有两组解.由式(43)所得的左右两个矩阵，令第一行第一列左右两边的元素与第二行第一列左右两边的元素分别相等可得：从而就可以得到θ4的表达式为由式(38)、(42)、(45)、(47)、(50)就得到了θi(i=1， 2，…， 5) 的值，由此可以写出所有的变换矩阵Ai(i=1， 2，…， 5)的表达式.从而可以写出移动副驱动的表达式为式中：OBi=A1A22O2Bi；ODi=A5O5Di.至此，推导得到了六自由度双并联型混联机构的运动学逆解.已知运动学输出位姿，就可以求解得到机构5个移动副输入L1、 L2、 L3、 L4、 L5和1个转动副输入θ4的解.由于θ5、θ2、θ3都有两组解，所以此混联机构具有8组解.根据双并联型混联机构的位姿逆解的分析，给定混联机构结构参数，然后给定动平台的位置与姿态参数.选取与之前正解算例相同的结构参数，并且以混联机构位置正解算例中得到的数值解作为反解的输入，从而实现双并联型混联机构位置正反解模型的相互验证.给定混联机构的结构参数如表1和表2所示.由位置正解分析的三维模型位姿图3可知，位置正解的第四组数值解支链之间不会发生干涉，符合要求.所以取i=4时的结果，此双并联型混联机构的位姿变换矩阵为将此时的位姿状态作为输入，代入双并联型混联机构运动学逆解求解公式中，利用Matlab软件求得各运动副的状态如表7所示.对逆解所得的8组数据进行分析可以发现,第八组输入参数与位置正解算例中的第四组输入参数相同.在此将两组数据单独提炼出来进行比较分析，如表8所示.经过比较之后发现，双并联型混联机构的两者之间存在微小误差，且误差可以忽略不计.经过两组解的对比，发现用正解得到的双并联型混联机构末端位姿作为输入，求得的机构驱动输入参数与一开始求正解的输入参数相同，两者之间的误差微乎其微.验证了运动学正反解的正确性.工作空间分析在混联机构的分析中占有非常重要的地位[11].混联机构工作空间会受到很多因素的影响，机构整体尺寸的大小，移动副P行程的约束，转动副R转角(包括虎克铰U、球铰S)角度值的限制，机构中各杆件之间的干涉现象都会对其产生影响[12].机构在不同的限定条件下，其工作空间的形状和体积都会不一样[13].不同的机构根据不同的用途和应用场合会提出不一样的限制因素，每种因素的限制条件也不尽相同.在实际应用中，也会考虑成本因素提出不同的限制条件[14].对本文混联机构的位置可达工作空间进行分析，运用运动学反解分析结果，结合极限边界数值搜索算法，求出其位置工作空间的形状.求解位置工作空间内所有点的算法如图4所示.给定合适的机构尺寸，通过圆柱坐标系逐个搜索，将Z坐标等分成n份，然后在每个Z坐标截面下，搜索边界轮廓，直到在整个Z坐标下都搜索完成后，所有的边界轮廓组合起来就是整个混联机构的工作空间边界图[15].运用Matlab数学软件采用上述的算法，结合混联机构的运动学反解和约束条件对工作空间的算法进行编程，求解了给定算例的工作空间.给定机构参数与运动学算例保持一致.针对此双并联型六自由度混联机构，为了得到其位置工作空间，可以先将其末端固定在给定的姿态下，从而得到在此姿态下机构所能到达的所有满足约束条件的位置空间.由于混联机构上下两个并联模块的机构尺寸不一样，给定双并联混联机构的移动副的运动范围为0.5 m≤Li≤0.8 m (i=1， 2， 3)，0.3 m≤Li≤0.5 m (i=4， 5)，转动副的运动范围为-≤θi≤ (i=1， 2， 3， 4， 5).首先由蒙特卡洛法根据输入参数的变化范围得到机构的大致位置点云图，判断机构的大致位置.限定Z坐标的变化范围为[0.75， 1.7]，r≤2.利用上面给定的约束条件，用Matlab软件编程，给出机构在给定不同姿态情况下的定姿态位置工作空间，结果如图5所示，其中用YXZ欧拉角表示机构动平台的姿态.由图5可知，改变机构的姿态角，就可以得到不同定姿态的位置工作空间.这4种姿态条件下得到的位置工作空间边界图大致是一个从上到下边界逐渐增大的近似拉长的连续锥形体.当姿态角达到(20°，20°，20°)时，机构的位置可达工作空间有一个凹洞，说明机构在此位置范围内存在奇异位形.根据定姿态位置工作空间图可以看出该混联机构位置在(0， 0， 1.3)时，具有较为规则的位置工作空间边界，所以选择在此位置进行机构的姿态工作空间搜索，其得到的姿态工作空间是指与末端执行器固结在一起的坐标系在该位置所能实现的所有的姿态坐标的集合，如图6所示.(1) 本文介绍了双并联型六自由度混联机构的结构，求解了机构的自由度数目.指定机构驱动构件，通过分别对下端并联机构模块和上端并联机构模块的运动学正解分析，得到整个混联机构的运动学正解.通过给定机构结构参数和驱动输入参数，用一组算例求得运动学动平台位姿，并画出三维机构位姿模型.然后求解运动学逆解，按照运动学正解结果，给定机构的两组位姿，求得此时机构的驱动输入参数及各转动副的状态，对比正反解结果，进而验证了双并联型混联机构运动学正反解模型的正确性.(2) 根据混联机构的运动学反解，利用极限边界搜索法给出机构工作空间的算法.给定机构的结构参数，考虑工作空间影响因素，利用数值搜索法在圆柱坐标系中搜索工作空间的边界.在Matlab软件中仿真得到了混联机构在给定不同姿态下的位置工作空间和给定位置下的姿态工作空间.【相关文献】[1] 姜铭，孙钊，秦康生，等.混联机器人的分析与研究[J].制造业自动化，2009，31(1)：61-65.[2] 伞红军.新型五轴并串联机床的运动学分析与结构参数设计[D].哈尔滨：哈尔滨工业大学机电工程学院，2009.[3] 高建设，李明祥，侯伯杰，等.新型四足步行机器人串并混联腿的运动学分析[J].光学精密工程，2015，23(11)：3147-3160.[4] PISLA D， SZILAGHYI A， VAIDA C， et al. Kinematics and workspace modeling of a new hybrid robot used in minimally invasive surgery[J]. Robotics and Computer-Integrated Manufacturing， 2013， 29(2)：463-474.[5] ZENG Q， FANG Y. Structural synthesis and analysis of serial-parallel hybrid mechanisms with spatial multi-loop kinematic chains[J]. Mechanism & Machine Theory，2012， 49(3)：198-215.[6] CHANGYA YAN， FENG GAO， YONG ZHANG. Kinematic modeling of a serial-parallel forging manipulator with application to heavy-duty manipulations#[J]. Mechanics Based Design of Structures & Machines， 2010， 38(1)：105-129.[7] 刘海涛，黄田，CHETWYND D G，等.5自由度大工作空间/支链行程比混联机械手的概念设计与尺度综合[J].机械工程学报，2007，43(6)：14-20.[8] LU Y， DAI Z， YE N， et al. Kinematics/statics analysis of a novel serial-parallel robotic arm with hand[J]. Journal of Mechanical Science & Technology， 2015， 29(10)：4407-4416.[9] 石岩，路懿.一种2SPS+RPRR并联机构的运动学与工作空间分析[J].燕山大学学报，2010，34(1)：18-23.[10] 黄真，刘婧芳，李艳文.论机构自由度[M].科学出版社，2011.[11] CAMMARATA A. Optimized design of a large-workspace 2-DOF parallel robot for solar tracking systems[J]. Mechanism & Machine Theory， 2015， 83：175-186.[12] 韩方元.并联机器人运动学正解新算法及工作空间本体研究[D].长春：吉林大学机械科学与工程学院，2011.[13] HAY A M， SNYMAN J A. A multi-level optimization methodology for determiningthe dextrous workspaces of planar parallel manipulators[J]. Structural & Multidisciplinary Optimization， 2005， 30(30)：422-427.[14] 黄秀琴，辛秀梅，沈惠平，等.一种新型3自由度并联机构的设计和运动学分析[J].机械设计，2010，27(7)：4-6.[15] 陈伟海，徐颖俊，王建华，等.并联式下肢康复外骨骼运动学及工作空间分析[J].机械工程学报，2015，51(13)：158-166.。
六自由度平台
（一）六自由运动平台介绍六自由度液压平台技术参数六自由度运动平台是由六支油缸，上、下各六只万向铰链和上、下两个平台组成，下平台固定在基础上，借助六只油缸的伸缩运动，完成上平台在空间六个自由度（α，β，γ， X，Y，Z）的运动，从而可以模拟出各种空间运动姿态。
六自由度运动平台涉及到机械、液压、电气、控制、计算机、传感器，空间运动数学模型、实时信号传输处理等一系列高科技领域，因此六自由度运动平台是液压和控制领域水平的标志性象征。
主要包括平台的空间运动机构、空间运动模型、液压系统、控制系统。
1 六自由度平台空间机构技术参数六自由度平台结构效果图如图1所示。
图1 六自由度平台六自由度运动平台由上下平台和六个液压油缸组成。
六个液压缸上端点两两组成上平台三个支点，六个液压缸下端点两两组成下平台三个支点。
上下三个支点分别在假设的圆周上，并且是120o等分，既分别是两个等边三角形的顶点。
根据不同的运动范围，油缸的行程和上下平台半径不同。
结构如图2所示。
图2 六自由度平台结构图根据标书要求，六自由度平台结构参数如下：上平台半径： 0.8m；下平台半径：0.85m ；油缸最低行程时上下平台垂直距离：约1.17m；油缸行程：±0.20m。
2 六自由度平台空间运动空间运动的目标是实现平台在空间运动的三个姿态角度和三个平动位移，即俯仰、滚转、偏航、上下垂直运动、前后平移和左右平移，及六个姿态的复合运动姿态。
而空间目标是通过六个液压缸的行程实现的，这就需要一个空间的运动模型完成空间运动的转换，假设空间运动的目标俯仰、滚转、偏航、上下垂直位移、前后平移和左右平移用α，β，γ，X，Y，Z表示，六个油缸的行程用L（i）（i＝1、2、3、4、5、6）表示。
整个运动模型如下：L（i）＝TT（α，β，γ，X，Y，Z）其中，TT是一个空间转换矩阵模型。
由此实时算出每一运动时刻液压油缸的行程。
液压油缸的理论行程再通过D/A接口的转换，给出实际行程值。
飞机运动方程
刚体飞机运动方程
基本假设 飞行器是刚体，质量为常数（非必要条件）； 假设地球不动，地面坐标系为惯性坐标系； 忽略地球曲率，认为地面为平面； 重力加速度为常数，不随高度变化； 机体坐标系平面为飞机对称平面，飞机几何外形对 称，质量分布也对称，惯性积满足 I = ∫ xydm = 0 ， I zy = ∫ zydm = 0
牵连运动的加速度合成定理
G r = x i + yj + z k 点在动系中的矢量为
则
其中 i, j,k 分别是动系各轴上的单位矢量，如果从动系中观察，它们都 是常矢量，其导数为零。则（ 1）式为 G
dr i + y j + z k =x dt
G dr d （1） i + y j + z k + x = ( xi + yj + zk ) = x i + y j + zk dt dt
Iy ， I xz ， I z的时间导数为零。 假定飞行器的质量不会移动，质量不变，则 I x， ~ ~ ~ d d d I x − r I xz Hx = p I y Hy = q I z − p I xz Hz = r dt dt dt
j q Hy k r Hz
i G G 展开 Ω × H = p Hx
，
表示矢量 r
G
d 在动系中的导数，称为相对矢导数，导数符号记为 dt 。
G
i, j,k 都是变矢量，当动系以角速度转动时，利 如果从惯性坐标系观察，
= x(ω × i ) + y (ω × j) + z (ω × k )+ = ω × r 用泊桑公式有 x i + y j + zk G G G d r dr = +ω ×r 则 dt dt G 此为矢量 r 在惯性坐标系中的导数，称为绝对矢导数。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
求 θ2 px py pz 1
−1 由 A3 A2−1 A1−1T6 = A4 A5 A6
C3 0 左边 = S3 0
S3 0 − C3 0
0 1 0 0
0 C 2 0 − S 2 0 0 1 0 S23 0 − C23 0
p x C1 C23 + p y S1 C23 + p z S 23 − l 2 C3 = 0 p x C1 S 23 + p y S1 S 23 − p z C 23 − l 2 S 3 = d 4
1 =90 2 =0 3 =90 4 =-90 5 =90 6 =0
n
杆长 ln (mm) l1 =0 l2 = l3 =0 l4 =0 l5 =0 l6 =0
)
Cθn Sθ An = n 0 0 C1 S A1 = 1 0 0
− Sθ n Cαn Cθ n Cαn Sαn 0 0 S1 0 − C1 1 0 0 0
0 S3 0 − C3 1 0 0 0
0 0 ， 0 1
0 C5 S 0 ， A5 = 5 0 d4 1 0 由连杆 ６ 开始，依次导出下列方程： C6 S 5 T6 = A6 = 6 0 0 − S6 C6 0 0 0 0 1 0 0 0 0 1 S5 − C5 0 0
n x n 0 由正运动学方程 T6 = T6 = A1 A2 A3 A4 A5 A6 = y nz 0
2
ox oy oz 0
ax ay az 0
px py 可得 pz 1
n x = C1 [C23 (C4 C5 C 6 − S 4 S 6 ) − S 23 S 5 C 6 ] + S1 (C 4 S 6 + S 4 C5 C6 ) n y = − C1 (C4 S 6 + S 4 C5 C 6 ) + S1 [C 23 (C 4 C5 C6 − S 4 S 6 ) − S 23 S 5 C6 ] n z = C23 S 5 C 6 + S 23 (C4 C5 C 6 − S 4 S 6 ) o x = C1 [−C 23 (C 4 C5 S 6 + S 4 C6 ) + S 23 S 5 S 6 ] + S1 ( C4 C6 − S 4 C5 S 6 ) o y = − C1 (C4 C6 − S 4 C5 S 6 ) + S1 [−C 23 (C 4 C5 S 6 + S 4 C6 ) + S 23 S 5 S 6 ] o z = − C23 S 5 S 6 − S 23 (C 4 C 5 S 6 + S 4 C6 ) a x = C1 (C23 C 4 S 5 + S 23 C5 ) + S1 S 4 S 5 a y = − C1 S 4 S 5 + S1 (C 23 C4 S 5 + S 23 C5 ) a z = − C23 C5 + S 23 C4 S 5 p x = C1 (l 2 C2 + d 4 S 23 ) p y = S1 (l 2 C 2 + d 4 S 23 ) p z = l 2 S 2 − d 4 C23 三 机器人逆运动学分析 S1 0 − C1 0 S3 0 − C3 0 0 1 0 0 0 0 0 1 0 1 0 0 C2 − S −1 A2 = 2 0 0 0 0 0 1 S2 C2 0 0 0 0 1 0 S4 0 C4 0 − l2 0 0 1 0 −1 0 0 C1 0 A1−1 = S1 0 C 3 0 −1 A3 = S3 0 １ 求 θ1 ox oy oz 0 ax ay az 0 px py pz 1
C 3 (C 4 C 5C 6 − S 4 S 6 ) − S 3 S 5C 6 = S3 (C 4C 5 C 6 − S 4 S 6 ) + C 3 S 5C4 S6 0
1
T6 = A2 A3 A4 A5 A6
− C23 (C4C5S 6 + S 4 C6 ) + S 23S 5 S 6 − S 23( C4C5S 6 + S 4C6 ) − C23S 5 S 6 − S 4C5S 6 + C4C6 0 C23C4S 5 + S 23C5 S 23C4S 5 − C23C5 S4 S5 0 l2C2 + S 23d 4 l2 S 2 − C23d 4 0 1
T6 = A3 A4 A5 A6
− C 3 (C 4 C 5 S 6 + S 4C 6 ) + S3 S5 S 6 − S 3 (C 4 C 5 S 6 + S 4C 6 ) − C3 S5 S 6 − S 4 C 5 S 6 + C 4 C6 0 C 3 C 4 S 5 + S 3C 5 S 3C 4 S 5 − C 3C 5 S4 S5 0 S3d4 − C3 d 4 0 1
Sθn Sαn − Cθn Sαn Cαn 0
l n Cθn l n Sθn dn 1 − S2 C2 0 0
1
0 C 2 0 ， A2 = S 2 0 0 1 0
0 l 2 C2 C3 0 l2 S 2 ， A3 = S 3 0 1 0 0 1 0
4
联立
有
S 23 =
( d 4 + l 2 S3 ) A + p z l 2 C 3 A2 + p z
2
C23 =
Al2 C3 − ( d 4 + l 2 S 3 ) p z A2 + pz
2
其中 A = p x C1 + p y S 1
令 U = ( d 4 + l 2 S 3 ) A + p z l 2 C3
C1 n x + S 1 n y nz = S1 n x − C1 n y 0 根据
C1 o x + S1 o y oz S 1 ox − C1 o y 0
C1 a x + S1 a y az S1 a x − C1 a y 0
C1 p x + S1 p y pz S1 p x − C1 p y 1 则有
− l 2C3 n x ox ax px 0 n y oy a y p y − l2 S3 nz oz az pz 1 0 0 0 1 等式两边矩阵的第四列的第一 三行对应相等 则有 S1C23 − C1 S1 S23 0
3
C4 0 −1 A4 = − S 4 0
0 d4 0 1
由 A1−1 T6 = A2 A3 A4 A5 A6 C1 0 左边 = S1 0 S1 0 − C1 0 0 1 0 0
0 n x 0 ny 0 n z 1 0
机器人运动学分析
一 机器人齐次坐标系的建立
D—H 坐标图
二
机器人正运动学分析
杆件 1 2 3 4 5 6 变量为转角 n (图示角度值) 1 (0)
2 (90 3 (0) 4 (0) 5 (0) 6 (0)
偏距 dn (mm) d1 =0 d2 =0 d3 =0 d4 = d5 =0 d6 =0
扭角
２
θ1
a tan 2( p y , p x )
代入
２
２
中得
2d 4 l 2
Sinθ3 = K
其中 K =
2 2 2 2 p2 x + p y + p z − d 4 − l2
（则 Cosθ3 = ± 1 − K 2 ） 所以 ３ θ3 = arcsin K ，（ θ3 = a tan 2( K ,± 1 − K 2 ) ）
− C5 S 6 − S5 S6 C6 0
0 0 0 1 0 0 d4 1
C4 C5 C6 − S 4 S 6 S C C + C S 4 6 3 T6 = A4 A5 A6 = 4 5 6 − S 5C6 0
2
− C4C5 S 6 − S 4 C6 C4 S5 − S 4 C5 S6 + C4 C6 S4 S5 S5 S6 C5 0 0
C 4 S A4 = 4 0 0
0 − S4 0 C4 −1 0 0 0
0 S5 0 − C5 1 0 0 0
0 C6 S 0 ， A6 = 6 0 0 1 0
− S6 C6 0 0
0 0 1 0
0 0 ， 0 1
C5 C6 S C 4 T6 = A5 A6 = 5 6 S6 0
等式两边矩阵的第四列的前三行对应相等
C1 p x + S1 p y = d 4 S 23 + l 2 C 2 p z = −d 4 C 23 + l 2 S 2 S1 p x − C1 p y = 0 由 ２ 将 得 求 θ3
S2 C2 0 0
0 − l 2 C1 0 0 0 1 0 S1 0 1 0
S1 0 − C1 0
0 1 0 0
0 0 0 1
n x n y nz 0
ox oy oz 0
ax ay az 0
C1 C23 S = 1 C1 S 23 0 根据
C23( C4C5C6 − S 4 S 6 ) − S 23S 5C6 = S 23 (C4 C5C6 − S 4 S 6 ) + C23S 5C6 S 4C5C6 + C4 S 6 0
0
T6 = A1 A2 A3 A4 A5 A6
C1[ C23 (C 4C5 C6 − S4 S6 ) − S 23S5C 6 ] + S1( S 4C5 C6 + C4 S6 ) C1 [ −C23 ( C4C 5S 6 + S 4C6 ) + S23S 5S6 ] − S1 ( S4 C5 S6 − C 4C 6 ) C1 (C 23C 4 S5 + S23C 5 ) + S1S 4 S5 C1 ( l2C 2 + S 23d4 ) S1[ C23 ( C4C 5C6 − S4 S6 ) − S23S5 C6 ] − C1( S 4C5 C6 + C 4 S6 ) S1 [ −C 23( C4 C5 S6 + S4C 6 ) + S 23S5S 6 ] + C1 ( S4 C5 S6 − C 4C 6 ) S1 ( C23C4 S5 + S23C5 ) − C1S 4S 5 S1( l2 C2 + S23d 4 ) = S23 ( C4C 5C6 − S4 S6 ) + C23S5C 6 S23C4 S5 − C23C5 l2 S2 − C 23d4 − S23 (C 4C 5S 6 + S 4C6 ) − C 23S5 S6 0 0 0 1