CN109291047A - 基于轴不变量及dh参数1r/2r/3r逆解建模方法 - Google Patents
基于轴不变量及dh参数1r/2r/3r逆解建模方法 Download PDFInfo
- Publication number
- CN109291047A CN109291047A CN201810933485.8A CN201810933485A CN109291047A CN 109291047 A CN109291047 A CN 109291047A CN 201810933485 A CN201810933485 A CN 201810933485A CN 109291047 A CN109291047 A CN 109291047A
- Authority
- CN
- China
- Prior art keywords
- formula
- axis
- vector
- parameter
- invariant
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 26
- 238000005259 measurement Methods 0.000 claims abstract description 15
- 239000013598 vector Substances 0.000 claims description 137
- 239000011159 matrix material Substances 0.000 claims description 17
- 238000013519 translation Methods 0.000 claims description 15
- 230000033001 locomotion Effects 0.000 claims description 9
- 238000007596 consolidation process Methods 0.000 claims description 7
- 238000006467 substitution reaction Methods 0.000 claims description 4
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 3
- 230000008859 change Effects 0.000 claims description 2
- 241001061260 Emmelichthys struhsakeri Species 0.000 abstract description 24
- 238000004364 calculation method Methods 0.000 abstract description 3
- 230000036544 posture Effects 0.000 description 31
- 230000009466 transformation Effects 0.000 description 6
- 230000008569 process Effects 0.000 description 5
- 238000004891 communication Methods 0.000 description 4
- NJPPVKZQTLUDBO-UHFFFAOYSA-N novaluron Chemical compound C1=C(Cl)C(OC(F)(F)C(OC(F)(F)F)F)=CC=C1NC(=O)NC(=O)C1=C(F)C=CC=C1F NJPPVKZQTLUDBO-UHFFFAOYSA-N 0.000 description 4
- 238000004458 analytical method Methods 0.000 description 3
- 238000012512 characterization method Methods 0.000 description 3
- 238000001514 detection method Methods 0.000 description 3
- 230000000007 visual effect Effects 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000007667 floating Methods 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 238000011835 investigation Methods 0.000 description 2
- 239000000203 mixture Substances 0.000 description 2
- 210000000056 organ Anatomy 0.000 description 2
- 239000007787 solid Substances 0.000 description 2
- 230000017105 transposition Effects 0.000 description 2
- 238000012795 verification Methods 0.000 description 2
- 241000208340 Araliaceae Species 0.000 description 1
- PEDCQBHIVMGVHV-UHFFFAOYSA-N Glycerine Chemical compound OCC(O)CO PEDCQBHIVMGVHV-UHFFFAOYSA-N 0.000 description 1
- 239000006126 MAS system Substances 0.000 description 1
- 235000005035 Panax pseudoginseng ssp. pseudoginseng Nutrition 0.000 description 1
- 235000003140 Panax quinquefolius Nutrition 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 230000000903 blocking effect Effects 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 125000004122 cyclic group Chemical group 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000009792 diffusion process Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000009472 formulation Methods 0.000 description 1
- 235000008434 ginseng Nutrition 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000003754 machining Methods 0.000 description 1
- 238000007726 management method Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000003012 network analysis Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 210000004789 organ system Anatomy 0.000 description 1
- 238000010248 power generation Methods 0.000 description 1
- 230000005855 radiation Effects 0.000 description 1
- 230000009469 supplementation Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator
Landscapes
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Length Measuring Devices With Unspecified Measuring Means (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种基于轴不变量及D‑H参数1R/2R/3R机器人姿态逆解建模与解算方法。本发明的方法，以自然坐标系为基础，解决了基于轴不变量的1R姿态逆解、基于轴不变量及D‑H参数的2R及3R姿态逆解问题，并经CE3巡视器工程应用验证了本方法的正确性。其特征在于：具有简洁的链符号***及轴不变量的表示，具有伪代码的功能，物理含义准确，保证了工程实现的可靠性；基于轴不变量的结构参数，不需要建立中间坐标系，避免引入中间坐标系导致的测量误差，保证了姿态逆解的精确性。同时，由于实现了坐标系、极性、结构参量的参数化，保证了工程应用的通用性。
Description
技术领域
本发明涉及一种多轴机器人姿态逆解建模与解算方法，属于机器人技术领域。
背景技术
在应用名义D-H系及D-H参数计算机器人***运动学逆解时，由于存在机加工及装配误差，导致机器人***绝对定位及定姿精度远低于***的重复精度；同时，D-H系建立及D-H参数确定过程较烦琐，当***自由度较高时，手工完成这一过程可靠性低。因此，需要解决由计算机完成机器人***D-H系及D-H参数的确定问题。同时，高精度的D-H系及D-H参数是机器人进行精确作业的基础，也是“示教-再现”(Teaching and Playback)机器人向自主机器人发展的基础。
发明内容
本发明所要解决的技术问题是提供一种基于轴不变量及D-H参数1R/2R/3R机器人姿态逆解建模与解算方法，避免引入中间坐标系导致的测量误差，保证姿态逆解的精确性。
为解决上述技术问题，本发明采用以下技术方案：
一种基于轴不变量及DH参数1R/2R/3R逆解建模方法，其特征是，
用于控制多轴机器装置，所述多轴机器装置包含杆件集合与关节集合，所述杆件集合中之杆件透过所述关节集合的关节结合，将所述关节集合转换成对应的轴集合，关节集合中的一个关节对应成所述轴集合的子轴集合，所述轴集合的轴包含平动轴与转动轴两种类型；
使用所述轴集合来对应描述所述多轴机器装置，并且利用所述轴集合来建立动力学方程，以控制这个多轴机器装置；
在***处于零位时，以自然坐标系为参考，测量得到连接杆件及杆件l的坐标轴矢量在运动副运动时，轴矢量是不变量；轴矢量及关节变量唯一确定运动副的转动关系；
当给定关节变量转动角度后，其正、余弦及其半角的正、余弦均是常数；为方便表达，记
由式(1)得
定义
则
给定运动链iln，建立基于轴不变量的机器人3D矢量姿态方程：
式(5)是关于的n维2阶多项式方程；式中的表达形式幂符表示的x次幂；右上角角标∧或表示分隔符；是轴不变量的叉乘矩阵；1为三维单位矩阵；Vector表示取轴矢量；
建立轴不变量的二阶多项式：
式(6)是关于和的多重线性方程，是轴不变量的二阶多项式；为旋转变换矩阵；给定自然零位矢量作为的零位参考，则及分别表示零位矢量及径向矢量；式(6)即为对称部分表示零位轴张量，反对称部分表示径向轴张量，分别与轴向外积张量正交，从而确定三维自然轴空间；
式(6)表示为
由式(7)得规范的机器人姿态方程：
式中，iQn表示姿态，轴矢量 是轴不变量的叉乘矩阵。
若运动副R表示转动副，iQn表示姿态，仅三个独立的自由度；则当|iln|＝3时，存在3R姿态逆解；
给定单位矢量由式(8)得
式中，为单位矢量在大地坐标系的投影矢量。
若 表示需要确定的方向，则当|iln|＝2时，存在2R姿态逆解；
给定单位矢量及由式(8)得
式中，为单位矢量在大地坐标系的投影矢量。
给定单位矢量及由式(8)得
若 表示期望的投影，则当|iln|＝1时，存在1R姿态逆解；
由式(5)及式(10)得
使固结的单位矢量与期望单位矢量的投影最优，满足最小的解为
其中：为与的夹角。
将基于轴不变量的2R指向问题转化为基于D-H参数的2R指向问题，定向逆解计算步骤为：
对于给定2R转动链其中，l,k均为杆件；由初始单位矢量指向期望单位矢量为单位矢量在D-H系的投影矢量；求杆件的自然关节坐标及φl：
其中约定自然坐标系对应的D-H系记为根据D-H坐标***的编号习惯，运动副对应的轴记为即D-H***中的指标习惯遵从父指标，与自然坐标***下的参数遵从子指标不同；转动角度为时，定义：其中是由轴至轴l′z的扭角；
令D-H参数指标遵从子指标，即因故用D-H参数表示得
由式(36)最后一行得
式中，若用表示属性占位，则式中的表达形式表示成员访问符；
故有
即有
其中：
故有
由式(36)第一行得
故有
即
其中：
因式(38及式(42)不一定满足式(36)的第2行，由式(38)及式(42)获得的及φl只是可能解；再将可能解代入式(36)的第2行，若仍成立，则得到真实解。
给定3R转动链及期望姿态轴不变量序列求关节变量序列其中，l,k均为杆件；将基于轴不变量的3R姿态问题转化为基于D-H参数的3R姿态问题，姿态逆解计算式为：
由式(38)及式(42)得由得故有
式中，表示矩阵的第4行元素。
本发明所达到的有益效果：
本发明的方法，以自然坐标系为基础，解决了基于轴不变量的1R姿态逆解、基于轴不变量及D-H参数的2R及3R姿态逆解问题，并经CE3巡视器工程应用验证了本方法的正确性。其特征在于：具有简洁的链符号***及轴不变量的表示，具有伪代码的功能，物理含义准确，保证了工程实现的可靠性；基于轴不变量的结构参数，不需要建立中间坐标系，避免引入中间坐标系导致的测量误差，保证了姿态逆解的精确性。同时，由于实现了坐标系、极性、结构参量的参数化，保证了工程应用的通用性。
附图说明
图1自然坐标系与轴链；
图2固定轴不变量；
图3月面巡视器太阳翼坐标系；
图4天线与太阳翼的机械干涉；
图5解耦机械臂2组姿态逆解；
图6月面巡视器2DOF桅杆。
具体实施方式
下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。
在工程应用中，自然坐标系不仅简单、方便，而且有助于提高工程测量精度，增强建模的通用性。同时，多轴***的运动学及动力学建模的困难主要是因为存在转动，而转动描述的关键在于转动轴。本发明基于自然坐标系，研究1R、2R及3R的姿态逆解建模与解算问题。主要目的是为后续阐述基于轴不变量的多轴***逆运动学奠定基础。
定义1自然坐标轴：称与运动轴或测量轴共轴的，具有固定原点的单位参考轴为自然坐标轴，亦称为自然参考轴。
定义2自然坐标系：如图1所示，若多轴***D处于零位，所有笛卡尔体坐标系方向一致，且体坐标系原点位于运动轴的轴线上，则该坐标***为自然坐标***，简称自然坐标系。
自然坐标系优点在于：(1)坐标***易确定；(2)零位时的关节变量为零；(3)零位时的***姿态一致；(4)不易引入测量累积误差。
由定义2可知，在***处于零位时，所有杆件的自然坐标系与底座或世界系的方向一致。***处于零位即时，自然坐标系绕轴矢量转动角度将转至F[l]；在下的坐标矢量与在F[l]下的坐标矢量恒等，即有
由上式知，或不依赖于相邻的坐标系及F[l]；故称或为轴不变量。在不强调不变性时，可以称之为坐标轴矢量(简称轴矢量)。或表征的是体与体l共有的参考单位坐标矢量，与参考点及Ol无关。体与体l即为杆件或轴。
轴不变量与坐标轴具有本质区别：
(1)坐标轴是具有零位及单位刻度的参考方向，可以描述沿该方向平动的位置，但不能完整描述绕该方向的转动角度，因为坐标轴自身不具有径向参考方向，即不存在表征转动的零位。在实际应用时，需要补充该轴的径向参考。例如：在笛卡尔系F[l]中，绕lx转动，需以ly或lz为参考零位。坐标轴自身是1D的，3个正交的1D参考轴构成3D的笛卡尔标架。
(2)轴不变量是3D的空间单位参考轴，其自身就是一个标架。其自身具有径向参考轴，即参考零位。空间坐标轴及其自身的径向参考轴可以确定笛卡尔标架。空间坐标轴可以反映运动轴及测量轴的三个基本参考属性。
已有文献将无链指标的轴矢量记为并称之为欧拉轴(Euler Axis)，相应的关节角称为欧拉角(Euler Angle)。本申请之所以不再沿用欧拉轴，而称之为轴不变量，是因为轴不变量具有以下属性：
【1】给定旋转变换阵因其是实矩阵，其模是单位的，故其有一个实特征值λ1及两个互为共轭的复特征值λ2＝eiφ及λ3＝e-iφ；其中：i为纯虚数。因此，|λ1|·||λ2||·||λ3||＝1，得λ1＝1。轴矢量是实特征值λ1＝1对应的特征矢量，是不变量；
【2】是3D参考轴，不仅具有轴向参考方向，而且具有径向参考零位，将在3.3.1节予以阐述。
【3】在自然坐标系下：即轴不变量是非常特殊的矢量，它对时间的导数也具有不变性，且有非常优良的数学操作性能；
对轴不变量而言，其绝对导数就是其相对导数。因轴不变量是具有不变性的自然参考轴，故其绝对导数恒为零矢量。因此，轴不变量具有对时间微分的不变性。有：
【4】在自然坐标***中，通过轴矢量及关节变量可以直接描述旋转坐标阵没有必要为除根之外的杆件建立各自的体系。同时，以唯一需要定义的根坐标系为参考，可以提高***结构参数的测量精度；
【5】应用轴矢量的优良操作，将建立包含拓扑结构、坐标系、极性、结构参量及力学参量的完全参数化的统一的多轴***运动学及动力学模型。
因基矢量el是与F[l]固结的任一矢量，基矢量是与固结的任一矢量，又是F[l]及共有的单位矢量，故是F[l]及共有的基矢量。因此，轴不变量是F[l]及共有的参考基。轴不变量是参数化的自然坐标基，是多轴***的基元。固定轴不变量的平动与转动与其固结的坐标系的平动与转动等价。
在***处于零位时，以自然坐标系为参考，测量得到坐标轴矢量在运动副运动时，轴矢量是不变量；轴矢量及关节变量唯一确定运动副的转动关系。
因此，应用自然坐标***，当***处于零位时，只需确定一个公共的参考系，而不必为***中每一杆件确定各自的体坐标系，因为它们由轴不变量及自然坐标唯一确定。当进行***分析时，除底座系外，与杆件固结的其它自然坐标系只发生在概念上，而与实际的测量无关。自然坐标***对于多轴***(MAS)理论分析及工程作用在于：
(1)***的结构参数测量需要以统一的参考系测量；否则，不仅工程测量过程烦琐，而且引入不同的体系会引入更大的测量误差。
(2)应用自然坐标***，除根杆件外，其它杆件的自然坐标***由结构参量及关节变量自然确定，有助于MAS***的运动学与动力学分析。
(3)在工程上，可以应用激光跟踪仪等光学测量设备，实现对固定轴不变量的精确测量。
(4)由于运动副R及P、螺旋副H、接触副O是圆柱副C的特例，可以应用圆柱副简化MAS运动学及动力学分析。
定义3不变量：称不依赖于一组坐标系进行度量的量为不变量。
定义4转动坐标矢量：绕坐标轴矢量转动到角位置的坐标矢量为
定义5平动坐标矢量：沿坐标轴矢量平动到线位置的坐标矢量为
定义6自然坐标：以自然坐标轴矢量为参考方向，相对***零位的角位置或线位置，记为ql，称为自然坐标；称与自然坐标一一映射的量为关节变量；其中：
定义7机械零位：对于运动副在初始时刻t0时，关节绝对编码器的零位不一定为零，该零位称为机械零位；
故关节的控制量为
定义8自然运动矢量：将由自然坐标轴矢量及自然坐标ql确定的矢量称为自然运动矢量。其中：
自然运动矢量实现了轴平动与转动的统一表达。将由自然坐标轴矢量及关节确定的矢量，例如称为***矢量，亦称为自由螺旋。显然，轴矢量是特定的自由螺旋。
定义9关节空间：以关节自然坐标ql表示的空间称为关节空间。
定义10位形空间：称表达位置及姿态(简称位姿)的笛卡尔空间为位形空间，是双矢量空间或6D空间。
定义11自然关节空间：以自然坐标系为参考，通过关节变量表示，在***零位时必有的关节空间，称为自然关节空间。
如图2所示，给定链节原点Ol受位置矢量约束的轴矢量为固定轴矢量，记为其中：
轴矢量是关节自然坐标的自然参考轴。因是轴不变量，故称为固定轴不变量，它表征了运动副的结构关系，即确定了自然坐标轴。固定轴不变量是链节结构参数的自然描述。
定义12自然坐标轴空间：以固定轴不变量作为自然参考轴，以对应的自然坐标表示的空间称为自然坐标轴空间，简称自然轴空间。它是具有1个自由度的3D空间。
如图2所示，及不因杆件Ωl的运动而改变，是不变的结构参考量。确定了轴l相对于轴的五个结构参数；与关节变量ql一起，完整地表达了杆件Ωl的6D位形。给定时，杆件固结的自然坐标系可由结构参数及关节变量唯一确定。称轴不变量固定轴不变量关节变量及为自然不变量。显然，由固定轴不变量及关节变量构成的关节自然不变量与由坐标系至F[l]确定的空间位形具有一一映射关系，即
给定多轴***D＝{T,A,B,K,F,NT}，在***零位时，只要建立底座系或惯性系，以及各轴上的参考点Ol，其它杆件坐标系也自然确定。本质上，只需要确定底座系或惯性系。
给定一个由运动副连接的具有闭链的结构简图，可以选定回路中任一个运动副，将组成该运动副的定子与动子分割开来；从而，获得一个无回路的树型结构，称之为Span树。T表示带方向的span树，以描述树链运动的拓扑关系。
I为结构参数；A为轴序列，F为杆件参考系序列，B为杆件体序列，K为运动副类型序列，NT为约束轴的序列即非树。为取轴序列的成员。转动副R，棱柱副P，螺旋副H，接触副O是圆柱副C的特例。
描述运动链的基本拓扑符号及操作是构成运动链拓扑符号***的基础，定义如下：
【1】运动链由偏序集合(]标识。
【2】A [l]为取轴序列A的成员；因轴名l具有唯一的编号对应于A [l]的序号，故A [l]计算复杂度为O(1)。
【3】为取轴l的父轴；轴的计算复杂度为O(1)。计算复杂度O()表示计算过程的操作次数，通常指浮点乘与加的次数。以浮点乘与加的次数表达计算复杂度非常烦琐，故常采用算法循环过程中的主要操作次数；比如：关节位姿、速度、加速度等操作的次数。
【4】为取轴序列的成员；计算复杂度为O(1)。
【5】llk为取由轴l至轴k的运动链，输出表示为且基数记为|llk|。llk执行过程：执行若则执行否则，结束。llk计算复杂度为O(|llk|)。
【6】ll为取轴l的子。该操作表示在中找到成员l的地址k；从而，获得轴l的子A[k]。因不具有偏序结构，故ll的计算复杂度为
【7】lL表示获得由轴l及其子树构成的闭子树， l L为不含l的子树；递归执行ll，计算复杂度为
【8】支路、子树及非树弧的增加与删除操作也是必要的组成部分；从而，通过动态Span树及动态图描述可变拓扑结构。在支路llk中，若则记 即表示在支路中取成员m的子。
定义以下表达式或表达形式：
轴与杆件具有一一对应性；轴间的属性量及杆件间的属性量具有偏序性。
约定：表示属性占位；若属性p或P是关于位置的，则应理解为坐标系的原点至F[l]的原点；若属性p或P是关于方向的，则应理解为坐标系至F[l]。
及应分别理解为关于时间t的函数及且及是t0时刻的常数或常数阵列。但是正体的及应视为常数或常数阵列。
本申请中约定：在运动链符号演算***中，具有偏序的属性变量或常量，在名称上包含表示偏序的指标；要么包含左上角及右下角指标，要么包含右上角及右下角指标；它们的方向总是由左上角指标至右下角指标，或由右上角指标至右下角指标，本申请中为叙述简便，有时省略方向的描述，即使省略，本领域技术人员通过符号表达式也可以知道，本申请中采用的各参数，对于某种属性符，它们的方向总是由偏序指标的左上角指标至右下角指标，或由右上角指标至右下角指标。例如：可简述为(表示由k至l)平动矢量；表示(由k至l的)线位置；表示(由k至l的)平动矢量；其中：r表示“平动”属性符，其余属性符对应为：属性符φ表示“转动”；属性符Q表示“旋转变换矩阵”；属性符l表示“运动链”；属性符u表示“单位矢量”；属性符w表示“角速度”；角标为i表示惯性坐标系或大地坐标系；其他角标可以为其他字母，也可以为数字。
本申请的符号规范与约定是根据运动链的偏序性、链节是运动链的基本单位这两个原则确定的，反映了运动链的本质特征。链指标表示的是连接关系，右上指标表征参考系。采用这种符号表达简洁、准确，便于交流与书面表达。同时，它们是结构化的符号***，包含了组成各属性量的要素及关系，便于计算机处理，为计算机自动建模奠定基础。指标的含义需要通过属性符的背景即上下文进行理解；比如：若属性符是平动类型的，则左上角指标表示坐标系的原点及方向；若属性符是转动类型的，则左上角指标表示坐标系的方向。
(1)lS－杆件l中的点S；而S表示空间中的一点S。
(2)－杆件k的原点Ok至杆件l的原点Ol的平动矢量；
在自然坐标系F[k]下的坐标矢量，即由k至l的坐标矢量；
(3)－原点Ok至点lS的平动矢量；
在F[k]下的坐标矢量；
(4)－原点Ok至点S的平动矢量；
在F[k]下的坐标矢量；
(5)－连接杆件及杆件l的运动副；
－运动副的轴矢量；
及分别在及F[l]下的坐标矢量；是轴不变量，为一结构常数；
为转动矢量，转动矢量/角矢量是自由矢量，即该矢量可自由平移；
(6)－沿轴的线位置(平动位置)，
－绕轴的角位置，即关节角、关节变量，为标量；
(7)左下角指标为0时，表示机械零位；如：
－平动轴的机械零位，
－转动轴的机械零位；
(8)0－三维零矩阵；1－三维单位矩阵；
(9)约定：“\”表示续行符；表示属性占位；则
幂符表示的x次幂；右上角角标∧或表示分隔符；如：或为的x次幂。
表示的转置，表示对集合转置，不对成员执行转置；如：
为投影符，表示矢量或二阶张量对参考基的投影矢量或投影序列，即坐标矢量或坐标阵列，投影即是点积运算“·”；如：位置矢量在坐标系F[k]中的投影矢量记为
为叉乘符；如：是轴不变量的叉乘矩阵；给定任一矢量的叉乘矩阵为叉乘矩阵是二阶张量。
叉乘符运算的优先级高于投影符的优先级。投影符的优先级高于成员访问符或成员访问符优先级高于幂符
(10)单位矢量在大地坐标系的投影矢量单位零位矢量
(11)－零位时由原点至原点Ol的平动矢量，且记表示位置结构参数。
(12)iQl，相对绝对空间的旋转变换阵；
(13)以自然坐标轴矢量为参考方向，相对***零位的角位置或线位置，记为ql，称为自然坐标；关节变量自然关节坐标为φl；
(14)对于一给定有序的集合r＝[1,4,3,2]T，记r[x]表示取集合r的第x行元素。常记[x]、[y]、[z]及[w]表示取第1、2、3及4列元素。
(15)ilj表示由i到j的运动链；llk为取由轴l至轴k的运动链；
给定运动链若n表示笛卡尔直角系，则称为笛卡尔轴链；若n表示自然参考轴，则称为自然轴链。
(16)Rodrigues四元数表达形式：
欧拉四元数表达形式：
不变量的四元数(也称为轴四元数)表达形式
1、基于轴不变量的1R姿态逆解方法
投影是旋转矢量在线性空间下的度量。给定链节R为转动副；控制关节变量使固结的单位矢量与期望单位矢量的投影最优；其中：为与的夹角。称该问题为投影逆解问题。
当给定角度后，其正、余弦及其半角的正、余弦均是常数；为方便表达，记
约定：记D-H参数指标遵从父指标，与自然坐标***下的参数遵从子指标不同。转动角度为时，定义：
由式(1)得
定义
故有
给定运动链iln，建立基于轴不变量的3D矢量姿态方程：
式(5)是关于的n维2阶多项式方程。Vector表示取轴矢量。
建立轴不变量的二阶多项式：
式(6)是关于和的多重线性方程，是轴不变量的二阶多项式。给定自然零位矢量作为的零位参考，则及分别表示零位矢量及径向矢量。式(6)即为对称部分表示零位轴张量，反对称部分表示径向轴张量，分别与轴向外积张量正交，从而确定三维自然轴空间；式(6)仅含一个正弦及余弦运算、6个积运算及6个和运算，计算复杂度低；同时，通过轴不变量及关节变量实现了坐标系及极性的参数化。
式(6)可表示为
由式(7)得规范的姿态方程
给定运动链iln，考虑式(8)，若 iQn表示姿态，仅三个独立的自由度；则当|iln|＝3时，存在3R姿态逆解。给定单位矢量由式(8)得
若 表示需要确定的方向，则当|iln|＝2时，存在2R姿态逆解。给定单位矢量及由式(8)得
若 表示期望的投影，则当|iln|＝1时，存在1R姿态逆解。
由式(5)及式(10)得
即
其中：
若解式(11)得
若式(11)退化为一次式：
由式(14)得
记式(13)根号部分
因τl是关于的连续函数。因故 关于单调减。当时，由式(12)及式(16)得
由式(17)得
此时，满足最小的解为
下面采用上述方法对CE3太阳翼姿态逆解建模与解算：
如图3所示CE3巡视器太阳翼体系p，Op位于转动副cRp轴线中心，xp过转动副cRp的轴并指向巡视器前向，yp指向巡视器左侧，zp由右手规则确定，即指向+Y光敏元件阵列法向。巡视器体系记为c。
其中：—太阳翼转动角，Sf、Sr—分别是太阳翼前外侧点及后外侧点；—巡视器体系原点Oc至太阳翼体系原点Op位置矢量在巡视器体系下坐标；—巡视器至太阳的单位矢量在导航系n下坐标。
由式(5)得
太阳翼上任一点S在其体系下坐标记为则有齐次坐标变换
记巡视器相对导航系的旋转变换阵为nQc，则有nQp＝nQc·cQp，故有
cuS＝cQn·nuS， (22)
puS＝pQn·nuS。 (23)
记器日矢量相对太阳翼高度角为其由式(23)确定
记由太阳翼法向至太阳单位矢量夹角记为则有
CE3巡视器的太阳翼控制包含两种模式：
①太阳翼调节控制
太阳翼调节控制是指：给定的最小阈值控制既要保证太阳翼产生足够的功率，又要保证太阳翼由于太阳辐射不致过热，即由式(13)或(15)可解得τp。显然，
②太阳翼最优控制
太阳翼最优控制是指：控制保证太阳翼最大的发电量。由式(19)可解得τp，显然，下面通过特例验证式(13)、式(15)及式(19)的正确性。若
将式(26)代入式(18)得
将式(26)代入式(19)得
当时，由式(27)得由式(28)得
当时，由式(27)得由式(28)得
当时，由式(27)得由式(28)得
显然，上述结果与直观的物理含义一致，证明了基于轴不变量的1R投影逆解方法的正确性。
由上述太阳翼逆解可知，存在两组最优解。由于太阳翼转动角度受结构约束、太阳翼温度约束、太阳翼与数传天线或全向天线可能存在机械干涉，需要对太阳翼工作区间进行限定。在允许的工作区间内控制太阳翼，保证发电量的最大化。
如图4所示，因太阳翼距数传天线及全向天线较近，易遮挡电磁波的传输，致使数传通信或全向通信中断或功率衰减，称之为太阳翼与天线的机械干涉。避免机械干涉是巡视器任务规划、桅杆控制、太阳翼控制的基本约束条件。
判断巡视器数传天线与太阳翼或全向天线与太阳翼机械干涉的方法如下：记全向发射天线及接收天线顶点分别为Sl及Sr，记数传天线波束(Wave beam)轴与发射面交点为S。在巡视器体系c下，建立Sl至测控站的射线方程、Sr至测控站的射线方程、S至数据接收站的射线方程，通过全向通信或数传通信射线方程与太阳翼平面方程求解交点。若交点存在且位于太阳翼面内，则视为机械干涉。记射线的起点为A，射线单位矢量为cnt，参数为t，其对应的点记为crt，在巡视器体系c下射线参数方程为
crt＝crA+cnt·t， (29)
即
记太阳翼前内侧角点为B，太阳翼法向为cnp，射线与太阳翼平面任一交点记为crt。太阳翼平面方程为
(crt-crB)T·cnp＝0， (31)
即
将式(31)代入式(29)得
式(33)中时，说明射线与太阳翼法向正交，显然不存在干涉，即
因将式(33)代入式(29)可得射线与太阳翼平面交点crt。
若则检测射线与太阳翼干涉。当然，工程实现时，需要进行更多的射线检测，并考虑干涉阈度。
CE3巡视器太阳翼控制是巡视器任务规划***、巡视器遥操作控制***的基本组成部分。
太阳翼的行为控制通过3D场景显示，可以直观地反映“日地月”及地面站、巡视器姿态、太阳翼运动状态。不仅使用户能准确地把握巡视器在轨时的情景状态，而且有助于提高软件的可靠性。在仿真测试时，可以用来分析探测区域、月面地貌、探测时间区间、太阳翼及左太阳翼发电性能等与月面巡视探测任务的适应性，可以优化巡视器电源***的设计。
2、基于轴不变量及D-H参数的2R及3R姿态逆解
对于任一个杆件，D-H参数仅有3个结构参数及1个关节变量，有利于简化姿态方程的消元。由于D-H参数通常是名义的，难以得到准确的工程参数，需要通过固定轴不变量的精确测量，并通过计算获得相应的准确的D-H系及D-H参数。因此，基于轴不变量的2R指向与3R姿态问题可以转化为基于D-H参数的2R指向与3R姿态问题。
给定2R转动链由初始单位矢量指向期望单位矢量求及φl，这是定向逆解问题。
其中约定自然坐标系对应的D-H系记为根据D-H坐标***的编号习惯，运动副对应的轴记为即D-H***中的指标习惯遵从父指标，与自然坐标***下的参数遵从子指标不同；转动角度为时，定义：其中是由轴至轴l′z的扭角；
若令D-H参数指标遵从子指标，因故用D-H参数表示得
由式(36)最后一行得
故有
即有
其中：
故有
由式(36)第一行得
故有
即
其中：
因式(38)及式(42)不一定满足式(36)的第2行，由式(38)及式(42)获得的及φl只是可能解；故需将可能解代入式(36)的第2行；若仍成立，才可得到真实解。
给定3R转动链及期望姿态轴不变量序列求关节变量序列这是3R姿态逆解问题。
由式(38)及式(42)得由得故有
至此，解决了基于笛卡尔轴链的姿态逆解方法缺乏通用性的问题。由式(42)及式(44)可知通常存在两组解，如图5所示。
实施例
下面采用上述方法进行CE3数传机构姿态逆解建模与解算：
如图6所示，CE3巡视器的数传机构转动链为clm＝(c,d,m]，轴不变量序列为[cnd,dnm]。地面数据接收站单位矢量为cuS。求其角度序列[φd,φm]。
经过精测获得轴不变量表达的结构参数为
基于自然坐标系与D-H系的关系，F＝{F[l]|l∈A}，其中：F[l]为自然坐标系，F[l′]为D-H系；且有
确定中间点及D-H系原点Ol′。
令和zl′分别经过轴不变量和且
定义为到nl的公垂线。是轴的单位坐标矢量。用来表示零位方向。
令由轴扭角的定义得
令由关节转动角定义得
其中：al和cl分别为轴到轴l的轴距和偏距，al为轴到轴l的扭角，为轴的零位。
综上所述，通过固定轴不变量和可以方便地表达D-H参数及同时可以表达零位
将式(45)代入式(48)及式(49)得桅杆D-H参数，如表表1-1所示。
表1-1桅杆D-H参数
将表中参数分别代入式(39)及式(43)得
将式(52)代入式(40)得两组解
将式(53)代入式(40)得
因需要代入式(36)的第2行检验才能得到真实解，故φl最多存在两组解。
考虑式(50)及式(51)，通过特例验证式(54)及式(55)的正确性：
在数值计算时由于存在数字截断误差，可能导致无解；此时，需要将加上一个微小增量，再重新计算，以保证解的存在性。
CE3数传机构控制模块经仿真显示，调整巡视器偏航后，进行数传天线控制，天线波束轴向始终指向地球方向。巡视器经纬度为[-28.6,40.06]°，天线波束方向始终指向东南方位。当巡视器经纬度为[28.6,40.06]°时，天线波束方向始终指向西南方位。在不同的经纬度，数传天线控制结果均正确。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。
Claims (6)
1.一种基于轴不变量及DH参数1R/2R/3R逆解建模方法，其特征是，
用于控制多轴机器装置，所述多轴机器装置包含杆件集合与关节集合，所述杆件集合中之杆件透过所述关节集合的关节结合，将所述关节集合转换成对应的轴集合，关节集合中的一个关节对应成所述轴集合的子轴集合，所述轴集合的轴包含平动轴与转动轴两种类型；
使用所述轴集合来对应描述所述多轴机器装置，并且利用所述轴集合来建立动力学方程，以控制这个多轴机器装置；
在***处于零位时，以自然坐标系为参考，测量得到连接杆件及杆件l的坐标轴矢量在运动副运动时，轴矢量是不变量；轴矢量及关节变量唯一确定运动副的转动关系；
当给定关节变量转动角度后，其正、余弦及其半角的正、余弦均是常数；为方便表达，记
由式(1)得
定义
则
给定运动链iln，建立基于轴不变量的机器人3D矢量姿态方程：
式(5)是关于的n维2阶多项式方程；式中的表达形式幂符表示的x次幂；右上角角标∧或表示分隔符；是轴不变量的叉乘矩阵；1为三维单位矩阵；Vector表示取轴矢量；
建立轴不变量的二阶多项式：
式(6)是关于和的多重线性方程，是轴不变量的二阶多项式；为旋转变换矩阵；给定自然零位矢量作为的零位参考，则及分别表示零位矢量及径向矢量；式(6)即为对称部分表示零位轴张量，反对称部分表示径向轴张量，分别与轴向外积张量正交，从而确定三维自然轴空间；
式(6)表示为
由式(7)得规范的机器人姿态方程：
式中，iQn表示姿态，轴矢量 是轴不变量的叉乘矩阵。
2.根据权利要求1所述的基于轴不变量及DH参数1R/2R/3R逆解建模方法，其特征是，
若运动副R表示转动副，iQn表示姿态，仅三个独立的自由度；则当|iln|＝3时，存在3R姿态逆解；
给定单位矢量由式(8)得
式中，为单位矢量在大地坐标系的投影矢量。
3.根据权利要求1所述的基于轴不变量及DH参数1R/2R/3R逆解建模方法，其特征是，
若 表示需要确定的方向，则当|iln|＝2时，存在2R姿态逆解；
给定单位矢量及由式(8)得
式中，为单位矢量在大地坐标系的投影矢量。
4.根据权利要求1所述的基于轴不变量及DH参数1R/2R/3R逆解建模方法，其特征是，
给定单位矢量及由式(8)得
若 表示期望的投影，则当|iln|＝1时，存在1R姿态逆解；
由式(5)及式(10)得
使固结的单位矢量luS与期望单位矢量的投影最优，满足最小的解为
其中：为luS与的夹角。
5.根据权利要求3所述的基于轴不变量及DH参数1R/2R/3R逆解建模方法，其特征是，
将基于轴不变量的2R指向问题转化为基于D-H参数的2R指向问题，定向逆解计算步骤为：
对于给定2R转动链其中，l,k均为杆件；由初始单位矢量指向期望单位矢量为单位矢量在D-H系的投影矢量；求杆件的自然关节坐标及φl：
其中约定自然坐标系对应的D-H系记为根据D-H坐标***的编号习惯，运动副对应的轴记为即D-H***中的指标习惯遵从父指标，与自然坐标***下的参数遵从子指标不同；转动角度为时，定义：其中是由轴至轴l′z的扭角；
令D-H参数指标遵从子指标，即因故用D-H参数表示得
由式(36)最后一行得
式中，若用表示属性占位，则式中的表达形式表示成员访问符；
故有
即有
其中：
故有
由式(36)第一行得
故有
即
其中：
因式(38及式(42)不一定满足式(36)的第2行，由式(38)及式(42)获得的及φl只是可能解；再将可能解代入式(36)的第2行，若仍成立，则得到真实解。
6.根据权利要求5所述的基于轴不变量及DH参数1R/2R/3R逆解建模方法，其特征是，
给定3R转动链及期望姿态轴不变量序列求关节变量序列其中，l,k均为杆件；将基于轴不变量的3R姿态问题转化为基于D-H参数的3R姿态问题，姿态逆解计算式为：
由式(38)及式(42)得由得故有
式中，表示矩阵的第4行元素。
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810933485.8A CN109291047B (zh) | 2018-08-16 | 2018-08-16 | 基于轴不变量及dh参数1r/2r/3r逆解建模方法 |
| PCT/CN2018/112768 WO2020034418A1 (zh) | 2018-08-16 | 2018-10-30 | 基于轴不变量及dh参数1r/2r/3r逆解建模方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810933485.8A CN109291047B (zh) | 2018-08-16 | 2018-08-16 | 基于轴不变量及dh参数1r/2r/3r逆解建模方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109291047A true CN109291047A (zh) | 2019-02-01 |
| CN109291047B CN109291047B (zh) | 2019-10-22 |
Family
ID=65165146
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810933485.8A Active CN109291047B (zh) | 2018-08-16 | 2018-08-16 | 基于轴不变量及dh参数1r/2r/3r逆解建模方法 |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN109291047B (zh) |
| WO (1) | WO2020034418A1 (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111967099A (zh) * | 2020-07-20 | 2020-11-20 | 居鹤华 | 多自由度机械臂矢量多项式***最优求解方法 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103620506A (zh) * | 2011-06-10 | 2014-03-05 | 惠普发展公司，有限责任合伙企业 | 光学扫描装置、***和方法 |
| CN107756400A (zh) * | 2017-10-13 | 2018-03-06 | 北京工业大学 | 一种基于旋量理论的6r机器人逆运动学几何求解方法 |
| CN107866823A (zh) * | 2017-10-24 | 2018-04-03 | 南京工程学院 | 一种基于位置矢量法的工业机器人几何参数标定方法 |
| CN108274472A (zh) * | 2018-01-23 | 2018-07-13 | 深圳创源航天科技有限公司 | 一种工业机器人加工工艺自适应的空间运动方法 |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105522577B (zh) * | 2015-11-29 | 2017-09-12 | 上海新时达电气股份有限公司 | 一种用于五轴折弯机器人笛卡尔轨迹规划的方法及其装置 |
| CN105975795B (zh) * | 2016-05-23 | 2017-08-25 | 湖北工业大学 | 一种高精度的多关节串联机械臂运动学反解解法 |
| CN106041932B (zh) * | 2016-07-01 | 2018-04-17 | 广东工业大学 | 一种ur机器人的运动控制方法 |
| CN109952178B (zh) * | 2016-11-17 | 2022-07-05 | 株式会社富士 | 作业机器人及作业位置修正方法 |
| CN108255058A (zh) * | 2018-01-18 | 2018-07-06 | 山东大学深圳研究院 | 智能空间下的服务机器人逆运动学求解方法和装置 |
-
2018
- 2018-08-16 CN CN201810933485.8A patent/CN109291047B/zh active Active
- 2018-10-30 WO PCT/CN2018/112768 patent/WO2020034418A1/zh active Application Filing
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103620506A (zh) * | 2011-06-10 | 2014-03-05 | 惠普发展公司，有限责任合伙企业 | 光学扫描装置、***和方法 |
| CN107756400A (zh) * | 2017-10-13 | 2018-03-06 | 北京工业大学 | 一种基于旋量理论的6r机器人逆运动学几何求解方法 |
| CN107866823A (zh) * | 2017-10-24 | 2018-04-03 | 南京工程学院 | 一种基于位置矢量法的工业机器人几何参数标定方法 |
| CN108274472A (zh) * | 2018-01-23 | 2018-07-13 | 深圳创源航天科技有限公司 | 一种工业机器人加工工艺自适应的空间运动方法 |
Non-Patent Citations (2)
| Title |
|---|
| HEHUA JU: "A Dynamics Method for 2-DOF Manipulator based on Ju-Kane Dynamics Method", 《I-CREATE 2018 PROCEEDINGS OR THE 12TH INTERNATIONAL CONVENTION ON REHABILITATION ENGINEERING AND ASSISTIVE TECHNOLOGY》 * |
| HEHUA JU: "Inverse Kinematics of 2-Dof Space manipulator based on D-H Parameters", 《I-CREATE 2018 PROCEEDINGS OR THE 12TH INTERNATIONAL CONVENTION ON REHABILITATION ENGINEERING AND ASSISTIVE TECHNOLOGY》 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111967099A (zh) * | 2020-07-20 | 2020-11-20 | 居鹤华 | 多自由度机械臂矢量多项式***最优求解方法 |
| CN111967099B (zh) * | 2020-07-20 | 2021-04-27 | 居鹤华 | 多自由度机械臂矢量多项式***最优求解方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109291047B (zh) | 2019-10-22 |
| WO2020034418A1 (zh) | 2020-02-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108942942B (zh) | 一种基于轴不变量的多轴机器人逆运动学建模与解算方法 | |
| CN108972558B (zh) | 一种基于轴不变量的多轴机器人动力学建模方法 | |
| CN107588771B (zh) | 基于李群描述的捷联惯性导航解算方法 | |
| CN108983705A (zh) | 一种基于轴不变量的多轴机器人***正运动学建模与解算方法 | |
| CN104656097A (zh) | 基于旋转式二维激光三维重构***的标定装置及方法 | |
| CN108015763A (zh) | 一种抗噪声干扰的冗余度机械臂路径规划方法 | |
| CN102354123A (zh) | 一种跨平台可扩展的卫星动态仿真测试*** | |
| CN104019799A (zh) | 一种利用局部参数优化计算基础矩阵的相对定向方法 | |
| CN106249057B (zh) | 一种大型绕焦点转动波束扫描天线辐射特性等效测试方法 | |
| CN107480356B (zh) | 基于catia和激光跟踪仪的部件设计检验一体化方法 | |
| CN109079784A (zh) | 一种基于轴不变量的多轴机器人***建模与控制方法 | |
| CN113283075A (zh) | 一种面向大规模卫星场景的轻量化仿真架构设计方法 | |
| CN109291047A (zh) | 基于轴不变量及dh参数1r/2r/3r逆解建模方法 | |
| Di Domenico et al. | STASIS: an Attitude Testbed for Hardware-in-the-Loop Simulations of Autonomous Guidance, Navigation, and Control Systems | |
| CN109015641A (zh) | 基于轴不变量的通用6r机械臂逆解建模与解算方法 | |
| Yamao et al. | Calibration of a rotating 2D LRF in unprepared environments by minimizing redundant measurement errors | |
| Fernandez-Cortizas et al. | Multi S-Graphs: an Efficient Real-time Distributed Semantic-Relational Collaborative SLAM | |
| CN109079850A (zh) | 基于轴不变量多轴机器人d-h系及d-h参数确定方法 | |
| Astad et al. | Vive for robotics: Rapid robot cell calibration | |
| Chen | Quadrotor Attitude Control Using Special Orthogonal Matrix | |
| CN109033688A (zh) | 基于轴不变量的通用7r机械臂逆解建模与解算方法 | |
| CN108927825B (zh) | 基于轴不变量的多轴机器人结构参数精测方法 | |
| CN108959828A (zh) | 基于轴不变量的通用3r机械臂逆解建模与解算方法 | |
| CN109117451A (zh) | 基于轴不变量的树链机器人动力学建模与解算方法 | |
| CN108942943A (zh) | 基于轴不变量的多轴机器人正运动学计算方法 |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20200617 Address after: Room 8011, innovation and entrepreneurship building, national thousand talents plan, No.7, Yingcui Road, Jiangning Development Zone, Nanjing, Jiangsu Province Patentee after: Nanjing ansheman Robotics Research Institute Co., Ltd Address before: 211100 No. 2289 Tianyuan East Road, Jiangning District, Nanjing, Jiangsu (Jiangning Gao Xinyuan) Patentee before: Ju Hehua |