基于轴不变量及DH参数1R/2R/3R逆解建模方法

摘要

本发明公开了一种基于轴不变量及D‑H参数1R/2R/3R机器人姿态逆解建模与解算方法。本发明的方法，以自然坐标系为基础，解决了基于轴不变量的1R姿态逆解、基于轴不变量及D‑H参数的2R及3R姿态逆解问题，并经CE3巡视器工程应用验证了本方法的正确性。其特征在于：具有简洁的链符号系统及轴不变量的表示，具有伪代码的功能，物理含义准确，保证了工程实现的可靠性；基于轴不变量的结构参数，不需要建立中间坐标系，避免引入中间坐标系导致的测量误差，保证了姿态逆解的精确性。同时，由于实现了坐标系、极性、结构参量的参数化，保证了工程应用的通用性。

说明书

技术领域

本发明涉及一种多轴机器人姿态逆解建模与解算方法，属于机器人技术领域。

背景技术

在应用名义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)得

定义

则

给定运动链ⁱl_n，建立基于轴不变量的机器人3D矢量姿态方程：

式(5)是关于的n维2阶多项式方程；式中的表达形式幂符表示的x次幂；右上角角标∧或表示分隔符；是轴不变量的叉乘矩阵；1为三维单位矩阵；Vector表示取轴矢量；

建立轴不变量的二阶多项式：

式(6)是关于和的多重线性方程，是轴不变量的二阶多项式；为旋转变换矩阵；给定自然零位矢量作为的零位参考，则及分别表示零位矢量及径向矢量；式(6)即为对称部分表示零位轴张量，反对称部分表示径向轴张量，分别与轴向外积张量正交，从而确定三维自然轴空间；

式(6)表示为

由式(7)得规范的机器人姿态方程：

式中，ⁱQ_n表示姿态，轴矢量是轴不变量的叉乘矩阵。

若运动副R表示转动副，ⁱQ_n表示姿态，仅三个独立的自由度；则当|ⁱl_n|＝3时，存在3R姿态逆解；

给定单位矢量由式(8)得

式中，为单位矢量在大地坐标系的投影矢量。

若表示需要确定的方向，则当|ⁱl_n|＝2时，存在2R姿态逆解；

给定单位矢量及由式(8)得

式中，为单位矢量在大地坐标系的投影矢量。

给定单位矢量及由式(8)得

若表示期望的投影，则当|ⁱl_n|＝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共有的参考单位坐标矢量，与参考点及O_l无关。体与体l即为杆件或轴。

轴不变量与坐标轴具有本质区别：

(1)坐标轴是具有零位及单位刻度的参考方向，可以描述沿该方向平动的位置，但不能完整描述绕该方向的转动角度，因为坐标轴自身不具有径向参考方向，即不存在表征转动的零位。在实际应用时，需要补充该轴的径向参考。例如：在笛卡尔系F[^l]中，绕lx转动，需以ly或lz为参考零位。坐标轴自身是1D的，3个正交的1D参考轴构成3D的笛卡尔标架。

(2)轴不变量是3D的空间单位参考轴，其自身就是一个标架。其自身具有径向参考轴，即参考零位。空间坐标轴及其自身的径向参考轴可以确定笛卡尔标架。空间坐标轴可以反映运动轴及测量轴的三个基本参考属性。

已有文献将无链指标的轴矢量记为并称之为欧拉轴(Euler Axis)，相应的关节角称为欧拉角(Euler Angle)。本申请之所以不再沿用欧拉轴，而称之为轴不变量，是因为轴不变量具有以下属性：

【1】给定旋转变换阵因其是实矩阵，其模是单位的，故其有一个实特征值λ₁及两个互为共轭的复特征值λ₂＝e^iφ及λ₃＝e^-iφ；其中：i为纯虚数。因此，|λ₁|·||λ₂||·||λ₃||＝1，得λ₁＝1。轴矢量是实特征值λ₁＝1对应的特征矢量，是不变量；

【2】是3D参考轴，不仅具有轴向参考方向，而且具有径向参考零位，将在3.3.1节予以阐述。

【3】在自然坐标系下：即轴不变量是非常特殊的矢量，它对时间的导数也具有不变性，且有非常优良的数学操作性能；

对轴不变量而言，其绝对导数就是其相对导数。因轴不变量是具有不变性的自然参考轴，故其绝对导数恒为零矢量。因此，轴不变量具有对时间微分的不变性。有：

【4】在自然坐标系统中，通过轴矢量及关节变量可以直接描述旋转坐标阵没有必要为除根之外的杆件建立各自的体系。同时，以唯一需要定义的根坐标系为参考，可以提高系统结构参数的测量精度；

【5】应用轴矢量的优良操作，将建立包含拓扑结构、坐标系、极性、结构参量及力学参量的完全参数化的统一的多轴系统运动学及动力学模型。

因基矢量e_l是与F^[l]固结的任一矢量，基矢量是与固结的任一矢量，又是F^[l]及共有的单位矢量，故是F^[l]及共有的基矢量。因此，轴不变量是F^[l]及共有的参考基。轴不变量是参数化的自然坐标基，是多轴系统的基元。固定轴不变量的平动与转动与其固结的坐标系的平动与转动等价。

在系统处于零位时，以自然坐标系为参考，测量得到坐标轴矢量在运动副运动时，轴矢量是不变量；轴矢量及关节变量唯一确定运动副的转动关系。

因此，应用自然坐标系统，当系统处于零位时，只需确定一个公共的参考系，而不必为系统中每一杆件确定各自的体坐标系，因为它们由轴不变量及自然坐标唯一确定。当进行系统分析时，除底座系外，与杆件固结的其它自然坐标系只发生在概念上，而与实际的测量无关。自然坐标系统对于多轴系统(MAS)理论分析及工程作用在于：

(1)系统的结构参数测量需要以统一的参考系测量；否则，不仅工程测量过程烦琐，而且引入不同的体系会引入更大的测量误差。

(2)应用自然坐标系统，除根杆件外，其它杆件的自然坐标系统由结构参量及关节变量自然确定，有助于MAS系统的运动学与动力学分析。

(3)在工程上，可以应用激光跟踪仪等光学测量设备，实现对固定轴不变量的精确测量。

(4)由于运动副R及P、螺旋副H、接触副O是圆柱副C的特例，可以应用圆柱副简化MAS运动学及动力学分析。

定义3不变量：称不依赖于一组坐标系进行度量的量为不变量。

定义4转动坐标矢量：绕坐标轴矢量转动到角位置的坐标矢量为

定义5平动坐标矢量：沿坐标轴矢量平动到线位置的坐标矢量为

定义6自然坐标：以自然坐标轴矢量为参考方向，相对系统零位的角位置或线位置，记为q_l，称为自然坐标；称与自然坐标一一映射的量为关节变量；其中：

定义7机械零位：对于运动副在初始时刻t₀时，关节绝对编码器的零位不一定为零，该零位称为机械零位；

故关节的控制量为

定义8自然运动矢量：将由自然坐标轴矢量及自然坐标q_l确定的矢量称为自然运动矢量。其中：

自然运动矢量实现了轴平动与转动的统一表达。将由自然坐标轴矢量及关节确定的矢量，例如称为自由运动矢量，亦称为自由螺旋。显然，轴矢量是特定的自由螺旋。

定义9关节空间：以关节自然坐标q_l表示的空间称为关节空间。

定义10位形空间：称表达位置及姿态(简称位姿)的笛卡尔空间为位形空间，是双矢量空间或6D空间。

定义11自然关节空间：以自然坐标系为参考，通过关节变量表示，在系统零位时必有的关节空间，称为自然关节空间。

如图2所示，给定链节原点O_l受位置矢量约束的轴矢量为固定轴矢量，记为其中：

轴矢量是关节自然坐标的自然参考轴。因是轴不变量，故称为固定轴不变量，它表征了运动副的结构关系，即确定了自然坐标轴。固定轴不变量是链节结构参数的自然描述。

定义12自然坐标轴空间：以固定轴不变量作为自然参考轴，以对应的自然坐标表示的空间称为自然坐标轴空间，简称自然轴空间。它是具有1个自由度的3D空间。

如图2所示，及不因杆件Ω_l的运动而改变，是不变的结构参考量。确定了轴l相对于轴的五个结构参数；与关节变量q_l一起，完整地表达了杆件Ω_l的6D位形。给定时，杆件固结的自然坐标系可由结构参数及关节变量唯一确定。称轴不变量固定轴不变量关节变量及为自然不变量。显然，由固定轴不变量及关节变量构成的关节自然不变量与由坐标系至F^[l]确定的空间位形具有一一映射关系，即

给定多轴系统D＝{T,A,B,K,F,NT}，在系统零位时，只要建立底座系或惯性系，以及各轴上的参考点O_l，其它杆件坐标系也自然确定。本质上，只需要确定底座系或惯性系。

给定一个由运动副连接的具有闭链的结构简图，可以选定回路中任一个运动副，将组成该运动副的定子与动子分割开来；从而，获得一个无回路的树型结构，称之为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】^ll_k为取由轴l至轴k的运动链，输出表示为且基数记为|^ll_k|。^ll_k执行过程：执行若则执行否则，结束。^ll_k计算复杂度为O(|^ll_k|)。

【6】^ll为取轴l的子。该操作表示在中找到成员l的地址k；从而，获得轴l的子A^[k]。因不具有偏序结构，故^ll的计算复杂度为

【7】^lL表示获得由轴l及其子树构成的闭子树，^lL为不含l的子树；递归执行^ll，计算复杂度为

【8】支路、子树及非树弧的增加与删除操作也是必要的组成部分；从而，通过动态Span树及动态图描述可变拓扑结构。在支路^ll_k中，若则记即表示在支路中取成员m的子。

定义以下表达式或表达形式：

轴与杆件具有一一对应性；轴间的属性量及杆件间的属性量具有偏序性。

约定：表示属性占位；若属性p或P是关于位置的，则应理解为坐标系的原点至F^[l]的原点；若属性p或P是关于方向的，则应理解为坐标系至F^[l]。

及应分别理解为关于时间t的函数及且及是t₀时刻的常数或常数阵列。但是正体的及应视为常数或常数阵列。

本申请中约定：在运动链符号演算系统中，具有偏序的属性变量或常量，在名称上包含表示偏序的指标；要么包含左上角及右下角指标，要么包含右上角及右下角指标；它们的方向总是由左上角指标至右下角指标，或由右上角指标至右下角指标，本申请中为叙述简便，有时省略方向的描述，即使省略，本领域技术人员通过符号表达式也可以知道，本申请中采用的各参数，对于某种属性符，它们的方向总是由偏序指标的左上角指标至右下角指标，或由右上角指标至右下角指标。例如：可简述为(表示由k至l)平动矢量；表示(由k至l的)线位置；表示(由k至l的)平动矢量；其中：r表示“平动”属性符，其余属性符对应为：属性符φ表示“转动”；属性符Q表示“旋转变换矩阵”；属性符l表示“运动链”；属性符u表示“单位矢量”；属性符w表示“角速度”；角标为i表示惯性坐标系或大地坐标系；其他角标可以为其他字母，也可以为数字。

本申请的符号规范与约定是根据运动链的偏序性、链节是运动链的基本单位这两个原则确定的，反映了运动链的本质特征。链指标表示的是连接关系，右上指标表征参考系。采用这种符号表达简洁、准确，便于交流与书面表达。同时，它们是结构化的符号系统，包含了组成各属性量的要素及关系，便于计算机处理，为计算机自动建模奠定基础。指标的含义需要通过属性符的背景即上下文进行理解；比如：若属性符是平动类型的，则左上角指标表示坐标系的原点及方向；若属性符是转动类型的，则左上角指标表示坐标系的方向。

(1)l_S－杆件l中的点S；而S表示空间中的一点S。

(2)－杆件k的原点O_k至杆件l的原点O_l的平动矢量；

在自然坐标系F^[k]下的坐标矢量，即由k至l的坐标矢量；

(3)－原点O_k至点l_S的平动矢量；

在F^[k]下的坐标矢量；

(4)－原点O_k至点S的平动矢量；

在F^[k]下的坐标矢量；

(5)－连接杆件及杆件l的运动副；

－运动副的轴矢量；

及分别在及F^[l]下的坐标矢量；是轴不变量，为一结构常数；

为转动矢量，转动矢量/角矢量是自由矢量，即该矢量可自由平移；

(6)－沿轴的线位置(平动位置)，

－绕轴的角位置，即关节角、关节变量，为标量；

(7)左下角指标为0时，表示机械零位；如：

－平动轴的机械零位，

－转动轴的机械零位；

(8)0－三维零矩阵；1－三维单位矩阵；

(9)约定：“”表示续行符；表示属性占位；则

幂符表示的x次幂；右上角角标∧或表示分隔符；如：或为的x次幂。

表示的转置，表示对集合转置，不对成员执行转置；如：

为投影符，表示矢量或二阶张量对参考基的投影矢量或投影序列，即坐标矢量或坐标阵列，投影即是点积运算“·”；如：位置矢量在坐标系F^[k]中的投影矢量记为

为叉乘符；如：是轴不变量的叉乘矩阵；给定任一矢量的叉乘矩阵为叉乘矩阵是二阶张量。

叉乘符运算的优先级高于投影符的优先级。投影符的优先级高于成员访问符或成员访问符优先级高于幂符

(10)单位矢量在大地坐标系的投影矢量单位零位矢量

(11)－零位时由原点至原点O_l的平动矢量，且记表示位置结构参数。

(12)ⁱQ_l，相对绝对空间的旋转变换阵；

(13)以自然坐标轴矢量为参考方向，相对系统零位的角位置或线位置，记为q_l，称为自然坐标；关节变量自然关节坐标为φ_l；

(14)对于一给定有序的集合r＝[1,4,3,2]^T，记r^[x]表示取集合r的第x行元素。常记[x]、[y]、[z]及[w]表示取第1、2、3及4列元素。

(15)ⁱl_j表示由i到j的运动链；^ll_k为取由轴l至轴k的运动链；

给定运动链若n表示笛卡尔直角系，则称为笛卡尔轴链；若n表示自然参考轴，则称为自然轴链。

(16)Rodrigues四元数表达形式：

欧拉四元数表达形式：

不变量的四元数(也称为轴四元数)表达形式

1、基于轴不变量的1R姿态逆解方法

投影是旋转矢量在线性空间下的度量。给定链节R为转动副；控制关节变量使固结的单位矢量与期望单位矢量的投影最优；其中：为与的夹角。称该问题为投影逆解问题。

当给定角度后，其正、余弦及其半角的正、余弦均是常数；为方便表达，记

约定：记D-H参数指标遵从父指标，与自然坐标系统下的参数遵从子指标不同。转动角度为时，定义：

由式(1)得

定义

故有

给定运动链ⁱl_n，建立基于轴不变量的3D矢量姿态方程：

式(5)是关于的n维2阶多项式方程。Vector表示取轴矢量。

建立轴不变量的二阶多项式：

式(6)是关于和的多重线性方程，是轴不变量的二阶多项式。给定自然零位矢量作为的零位参考，则及分别表示零位矢量及径向矢量。式(6)即为对称部分表示零位轴张量，反对称部分表示径向轴张量，分别与轴向外积张量正交，从而确定三维自然轴空间；式(6)仅含一个正弦及余弦运算、6个积运算及6个和运算，计算复杂度低；同时，通过轴不变量及关节变量实现了坐标系及极性的参数化。

式(6)可表示为

由式(7)得规范的姿态方程

给定运动链ⁱl_n，考虑式(8)，若ⁱQ_n表示姿态，仅三个独立的自由度；则当|ⁱl_n|＝3时，存在3R姿态逆解。给定单位矢量由式(8)得

若表示需要确定的方向，则当|ⁱl_n|＝2时，存在2R姿态逆解。给定单位矢量及由式(8)得

若表示期望的投影，则当|ⁱl_n|＝1时，存在1R姿态逆解。

由式(5)及式(10)得

即

其中：

若解式(11)得

若式(11)退化为一次式：

由式(14)得

记式(13)根号部分

因τ_l是关于的连续函数。因故关于单调减。当时，由式(12)及式(16)得

由式(17)得

此时，满足最小的解为

下面采用上述方法对CE3太阳翼姿态逆解建模与解算：

如图3所示CE3巡视器太阳翼体系p，O_p位于转动副^cR_p轴线中心，x_p过转动副^cR_p的轴并指向巡视器前向，y_p指向巡视器左侧，z_p由右手规则确定，即指向+Y光敏元件阵列法向。巡视器体系记为c。

其中：—太阳翼转动角，S_f、S_r—分别是太阳翼前外侧点及后外侧点；—巡视器体系原点O_c至太阳翼体系原点O_p位置矢量在巡视器体系下坐标；—巡视器至太阳的单位矢量在导航系n下坐标。

由式(5)得

太阳翼上任一点S在其体系下坐标记为则有齐次坐标变换

记巡视器相对导航系的旋转变换阵为ⁿQ_c，则有ⁿQ_p＝ⁿQ_c·^cQ_p，故有

^cu_S＝^cQ_n·ⁿu_S，(22)

^pu_S＝^pQ_n·ⁿu_S。(23)

记器日矢量相对太阳翼高度角为其由式(23)确定

记由太阳翼法向至太阳单位矢量夹角记为则有

CE3巡视器的太阳翼控制包含两种模式：

①太阳翼调节控制

太阳翼调节控制是指：给定的最小阈值控制既要保证太阳翼产生足够的功率，又要保证太阳翼由于太阳辐射不致过热，即由式(13)或(15)可解得τ_p。显然，

②太阳翼最优控制

太阳翼最优控制是指：控制保证太阳翼最大的发电量。由式(19)可解得τ_p，显然，下面通过特例验证式(13)、式(15)及式(19)的正确性。若

将式(26)代入式(18)得

将式(26)代入式(19)得

当时，由式(27)得由式(28)得

当时，由式(27)得由式(28)得

当时，由式(27)得由式(28)得

显然，上述结果与直观的物理含义一致，证明了基于轴不变量的1R投影逆解方法的正确性。

由上述太阳翼逆解可知，存在两组最优解。由于太阳翼转动角度受结构约束、太阳翼温度约束、太阳翼与数传天线或全向天线可能存在机械干涉，需要对太阳翼工作区间进行限定。在允许的工作区间内控制太阳翼，保证发电量的最大化。

如图4所示，因太阳翼距数传天线及全向天线较近，易遮挡电磁波的传输，致使数传通信或全向通信中断或功率衰减，称之为太阳翼与天线的机械干涉。避免机械干涉是巡视器任务规划、桅杆控制、太阳翼控制的基本约束条件。

判断巡视器数传天线与太阳翼或全向天线与太阳翼机械干涉的方法如下：记全向发射天线及接收天线顶点分别为S_l及S_r，记数传天线波束(Wave>l至测控站的射线方程、S_r至测控站的射线方程、S至数据接收站的射线方程，通过全向通信或数传通信射线方程与太阳翼平面方程求解交点。若交点存在且位于太阳翼面内，则视为机械干涉。记射线的起点为A，射线单位矢量为^cn_t，参数为t，其对应的点记为^cr_t，在巡视器体系c下射线参数方程为

^cr_t＝^cr_A+^cn_t·t，(29)

即

记太阳翼前内侧角点为B，太阳翼法向为^cn_p，射线与太阳翼平面任一交点记为^cr_t。太阳翼平面方程为

(^cr_t-^cr_B)^T·^cn_p＝0，(31)

即

将式(31)代入式(29)得

式(33)中时，说明射线与太阳翼法向正交，显然不存在干涉，即

因将式(33)代入式(29)可得射线与太阳翼平面交点^cr_t。

若则检测射线与太阳翼干涉。当然，工程实现时，需要进行更多的射线检测，并考虑干涉阈度。

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巡视器的数传机构转动链为^cl_m＝(c,d,m]，轴不变量序列为[^cn_d,^dn_m]。地面数据接收站单位矢量为^cu_S。求其角度序列[φ_d,φ_m]。

经过精测获得轴不变量表达的结构参数为

基于自然坐标系与D-H系的关系，F＝{F^[l]|l∈A}，其中：F^[l]为自然坐标系，F^[l′]为D-H系；且有

确定中间点及D-H系原点O_l′。

令和z_l′分别经过轴不变量和且

定义为到n_l的公垂线。是轴的单位坐标矢量。用来表示零位方向。

令由轴扭角的定义得

令由关节转动角定义得

其中：a_l和c_l分别为轴到轴l的轴距和偏距，a_l为轴到轴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]°时，天线波束方向始终指向西南方位。在不同的经纬度，数传天线控制结果均正确。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。