WO2020034418A1 - 基于轴不变量及dh参数1r/2r/3r逆解建模方法 - Google Patents
基于轴不变量及dh参数1r/2r/3r逆解建模方法 Download PDFInfo
- Publication number
- WO2020034418A1 WO2020034418A1 PCT/CN2018/112768 CN2018112768W WO2020034418A1 WO 2020034418 A1 WO2020034418 A1 WO 2020034418A1 CN 2018112768 W CN2018112768 W CN 2018112768W WO 2020034418 A1 WO2020034418 A1 WO 2020034418A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- axis
- vector
- equation
- invariant
- inverse
- Prior art date
Links
Images
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
Definitions
- the invention relates to a multi-axis robot attitude inverse modeling and calculation method, and belongs to the technical field of robots.
- the absolute positioning and orientation accuracy of the robot system is much lower than the repeat accuracy of the system due to machining and assembly errors.
- the DH system is established and the DH parameters The determination process is cumbersome.
- the system has a high degree of freedom, the reliability of completing the process manually is low. Therefore, it is necessary to solve the problem of determining the D-H system and D-H parameters of the robot system by a computer.
- the high-precision D-H system and D-H parameters are the basis for the robot to perform accurate operations, and also the basis for the development of "Teaching and Playback" robots to autonomous robots.
- the technical problem to be solved by the present invention is to provide an inverse attitude modeling and solving method of a 1R / 2R / 3R robot based on an axis invariant and D-H parameters, avoiding measurement errors caused by the introduction of an intermediate coordinate system, and ensuring the accuracy of the inverse attitude solution.
- the present invention adopts the following technical solutions:
- a 1R / 2R / 3R inverse solution modeling method based on axis invariants and DH parameters which is characterized by:
- the multi-axis machine device includes a rod set and a joint set, and the rods in the rod set are combined through the joints of the joint set to convert the joint set into corresponding An axis set, where a joint in the joint set corresponds to a sub-axis set of the axis set, and the axis of the axis set includes two types of translational and rotational axes;
- the connecting rod is measured.
- the axis vector of the rod l During kinematics, the axis vector Is an invariant; axis vector And joint variables Uniquely determine the rotation relationship of the motion pair;
- Equation (5) is about N-dimensional second-order polynomial equations; power expressions in expressions Represents the xth power of ⁇ ; Delimiter Is axis invariant Cross product matrix; 1 is a three-dimensional identity matrix; Vector represents the axis vector;
- Equation (6) is about with Multivariable linear equation is an axis invariant Second-order polynomial of Is the rotation transformation matrix; given the natural zero vector l l lS as Zero reference, and Respectively the zero vector and the radial vector; Equation (6) is Symmetry Represents zero-axis tensor, antisymmetric part Represents the radial axial tensor, and the axial outer product tensor, respectively Orthogonal to determine the three-dimensional natural axis space;
- Equation (6) is expressed as
- i Q n represents the attitude and the axis vector Is axis invariant Cross product matrix.
- the 2R pointing problem based on the axis invariant is converted into the 2R pointing problem based on the D-H parameter.
- the calculation steps of the inverse directional solution are:
- the corresponding DH system is recorded as According to the numbering convention of the DH coordinate system, the motion pair The corresponding axis is written as That is, the indicators in the DH system are used to follow the parent indicators, and the parameters in the natural coordinate system are different from the child indicators; the rotation angle is When defining: among them Is by the shaft Twist angle to axis l′ z;
- the factors (38 and 42) may not satisfy the second line of equation (36). And ⁇ l are only possible solutions; then the possible solutions are substituted into the second line of equation (36), and if they still hold true solutions are obtained.
- the method of the present invention is based on the natural coordinate system, and solves the inverse 1R attitude inverse solutions based on axis invariants, and the 2R and 3R inverse attitude solutions based on axis invariants and DH parameters.
- the correctness of the method It is characterized by a simple chain symbol system and the representation of the axis invariant, with the function of pseudo code, accurate physical meaning, and ensuring the reliability of the engineering implementation; based on the structural parameters of the axis invariant, there is no need to establish an intermediate coordinate system, The measurement error caused by the introduction of the intermediate coordinate system is avoided, and the accuracy of the inverse attitude solution is guaranteed.
- the universality of engineering applications is guaranteed.
- Figure 6 2DOF mast of the lunar inspector.
- the natural coordinate system is not only simple and convenient, but also helps to improve the accuracy of engineering measurement and enhance the versatility of modeling.
- the difficulty of modeling kinematics and dynamics of a multi-axis system is mainly due to the existence of rotation, and the key to rotation description is the rotation axis.
- the present invention studies the inverse attitude modeling and solving problems of 1R, 2R, and 3R. The main purpose is to lay the foundation for the subsequent elaboration of inverse kinematics of multi-axis systems based on axis invariants.
- Natural coordinate axis The unit reference axis with a fixed origin that is coaxial with the motion axis or measurement axis is called the natural coordinate axis, also known as the natural reference axis.
- Natural coordinate system As shown in Figure 1, if the multi-axis system D is at zero position, all Cartesian body coordinate systems have the same direction, and the origin of the body coordinate system is on the axis of the motion axis, then the coordinate system is a natural coordinate system. , Referred to as the natural coordinate system.
- the advantages of the natural coordinate system are: (1) the coordinate system is easy to determine; (2) the joint variables at the zero position are zero; (3) the system attitude at the zero position is consistent; (4) it is not easy to introduce a measurement error.
- the coordinate axis is a reference direction with a zero position and a unit scale. It can describe the position of translation in that direction, but it cannot fully describe the rotation angle around the direction, because the coordinate axis itself does not have a radial reference direction, that is There is a zero position that characterizes rotation. In practical applications, the radial reference of this shaft needs to be supplemented. For example: in Cartesian F [l] , to rotate around lx, you need to use ly or lz as the reference zero.
- the coordinate axis itself is 1D, and three orthogonal 1D reference axes constitute a 3D Cartesian frame.
- the axis invariant is a 3D spatial unit reference axis, which is itself a frame. It has its own radial reference axis, the reference mark.
- the spatial coordinate axis and its own radial reference axis determine the Cartesian frame.
- the spatial coordinate axis can reflect the three basic reference attributes of the motion axis and the measurement axis.
- [2] is a 3D reference axis, which not only has an axial reference direction, but also has a radial reference zero position, which will be explained in section 3.3.1.
- the axis invariant For axis invariants, its absolute derivative is its relative derivative. Since the axis invariant is a natural reference axis with invariance, its absolute derivative is always a zero vector. Therefore, the axis invariant is invariant to time differentiation. Have:
- the basis vector e l is any vector consolidated with F [l] .
- the basis vector With Any vector of consolidation, Is F [l] and Common unit vector, so Is F [l] and Shared basis vector. So the axis is invariant Is F [l] and Common reference base.
- Axis invariants are parameterized natural coordinate bases and primitives for multi-axis systems. The translation and rotation of a fixed axis invariant are equivalent to the translation and rotation of a fixed coordinate system.
- optical measurement equipment such as laser trackers can be applied to achieve accurate measurement of invariants of fixed axes.
- the cylindrical pair can be applied to simplify the MAS kinematics and dynamics analysis.
- Invariant A quantity that does not depend on a set of coordinate systems for measurement is called an invariant.
- Natural coordinates take the natural coordinate axis vector as the reference direction, the angular position or line position relative to the zero position of the system, and record it as q l , which is called natural coordinates;
- natural motion vector will be determined by the natural axis vector And the vector determined by natural coordinates q l Called the natural motion vector. among them:
- the natural motion vector realizes the unified expression of axis translation and rotation.
- a vector to be determined by the natural axis vector and the joint such as Called the free motion vector, also known as the free spiral.
- the axis vector Is a specific free spiral.
- joint space The space represented by the joint natural coordinates q l is called joint space.
- the Cartesian space that expresses the position and pose (posture for short) is called a shape space, which is a double vector space or a 6D space.
- Natural joint space using the natural coordinate system as a reference, through joint variables Means that there must be when the system is zero The joint space is called natural joint space.
- Axis vector Is the natural reference axis of the natural coordinates of the joint. Because Is an axis invariant, so Is a fixed axis invariant, which characterizes the motion pair The structural relationship is determined by the natural coordinate axis. Fixed axis invariant Is a link Natural description of structural parameters.
- Natural coordinate axis space The fixed axis invariant is used as the natural reference axis, and the space represented by the corresponding natural coordinate is called the natural coordinate axis space, which is referred to as the natural axis space. It is a 3D space with 1 degree of freedom.
- any motion pair in the loop can be selected, and the stator and the mover constituting the motion pair can be separated; thus, a loop-free tree structure is obtained.
- the Span tree represents a span tree with directions to describe the topological relationship of the tree chain movement.
- I is a structural parameter; A is an axis sequence; F is a bar reference sequence; B is a bar body sequence; K is a motion pair type sequence; NT is a sequence of constrained axes, that is, a non-tree.
- Axis sequence a member of.
- Rotary pair R, prism pair P, spiral pair H, and contact pair O are special examples of cylindrical pair C.
- the motion chain is identified by a partial order set ().
- O () represents the number of operations in the calculation process, and usually refers to the number of floating-point multiplications and additions. It is very tedious to express the calculation complexity by the number of floating-point multiplication and addition. Therefore, the main operation times in the algorithm cycle are often used;
- l l k is the kinematic chain from axis l to axis k, and the output is expressed as And
- the cardinality is written as
- l l k execution process execution If Then execute Otherwise, end.
- the computational complexity of l l k is O (
- l L means to obtain a closed subtree composed of axis l and its subtrees, Is a subtree without l; recursively execute l l with a computational complexity of
- ⁇ means attribute placeholder; if the attribute p or P is about position, then Should be understood as a coordinate system To the origin of F [l] ; if the attribute p or P is about direction, then Should be understood as a coordinate system To F [l] .
- attribute variables or constants with partial order include the indicators indicating partial order in the name; either include the upper left corner and lower right corner indexes, or include the upper right corner and lower right corner indexes; Their directions are always from the upper left corner indicator to the lower right corner indicator, or from the upper right corner indicator to the lower right corner indicator.
- the description of the direction is sometimes omitted. Even if omitted, those skilled in the art can also use symbolic expressions. It is known that, for each parameter used in this application, for a certain attribute symbol, their directions are always from the upper left corner index to the lower right corner index of the partial order index, or from the upper right corner index to the lower right corner index.
- the symbol specifications and conventions of this application are determined based on the two principles of the partial order of the kinematic chain and the chain link being the basic unit of the kinematic chain, reflecting the essential characteristics of the kinematic chain.
- the chain indicator represents the connection relationship, and the upper right indicator represents the reference system.
- This symbolic expression is concise and accurate, which is convenient for communication and written expression.
- they are structured symbol systems that contain the elements and relationships that make up each attribute quantity, which is convenient for computer processing and lays the foundation for computer automatic modeling.
- the meaning of the indicator needs to be understood through the background of the attribute, that is, the context; for example: if the attribute is a translation type, the indicator at the upper left corner indicates the origin and direction of the coordinate system; if the attribute is a rotation type, the indicator at the top left The direction of the coordinate system.
- the coordinate vector in the natural coordinate system F [k] that is, the coordinate vector from k to l;
- rotation vector / angle vector I is a free vector, that is, the vector can be freely translated
- the angular position that is, the joint angle and joint variables, are scalars
- T means transpose of ⁇ , which means transpose the collection, and do not perform transpose on the members; for example:
- Projection symbol ⁇ represents vector or tensor of second order group reference projection vector or projection sequence, i.e. the vector of coordinates or coordinate array, that is, the dot product projection "*"; as: position vector
- the projection vector in the coordinate system F [k] is written as
- Is a cross multiplier for example: Is axis invariant Cross product matrix; given any vector
- the cross product matrix is
- the cross product matrix is a second-order tensor.
- i l j represents a kinematic chain from i to j
- l l k is a kinematic chain from axis l to k
- n represents the Cartesian Cartesian system, then Is a Cartesian axis chain; if n represents a natural reference axis, then For natural shaft chains.
- the projection is a measure of the rotation vector in linear space. Given link R is the rotating pair; controls joint variables Make consolidated unit vector l u S and desired unit vector Projection Optimal; of which: For l u S with Angle. This problem is called the inverse projection problem.
- Equation (5) is about N-dimensional 2nd order polynomial equation.
- Vector represents the axis vector.
- Equation (6) is about with Multivariable linear equation is an axis invariant Second-order polynomial. Given the natural zero vector l l lS as Zero reference, and Respectively the zero vector and the radial vector.
- Equation (6) is Symmetry Represents zero-axis tensor, antisymmetric part Represents the radial axial tensor, and the axial outer product tensor, respectively Orthogonal to determine the three-dimensional natural axis space; Equation (6) contains only one sine and cosine operation, six product operations, and six sum operations, and the calculation complexity is low; And joint variables The coordinate system and polarity are parameterized.
- Equation (6) can be expressed as
- Equation (8) Given the motion chain i l n , consider equation (8), if i Q n represents the attitude, with only three independent degrees of freedom; then when
- 3, there is an inverse 3R attitude solution. Given unit vector From Equation (8)
- Equation (11) degenerates into a linear equation:
- the CE3 inspector solar wing system p, Op is located at the center of the axis of the rotating pair c R p , x p passes the axis of the rotating pair c R p and points to the forward direction of the inspector, y p points to the left of the inspector, and p is determined by the right-hand rule, that is, pointing to the + Y photosensitive element array normal.
- the inspector system is denoted as c.
- the CE3 Inspector's solar wing control includes two modes:
- Solar wing adjustment control refers to: given Minimum threshold control It is necessary to ensure that the solar wing generates sufficient power and that the solar wing does not overheat due to solar radiation, that is, ⁇ p can be obtained from formula (13) or (15). Obviously,
- Solar wing optimal control refers to: control Ensure the maximum power generation of the solar wing. From equation (19), ⁇ p can be solved. Obviously, In the following, the correctness of equations (13), (15), and (19) is verified through special cases. If
- the solar wing is close to the digital transmission antenna and the omnidirectional antenna, it is easy to block the transmission of electromagnetic waves, resulting in the interruption or power attenuation of digital transmission or omnidirectional communication, which is called the mechanical interference between the solar wing and the antenna. Avoiding mechanical interference is the basic constraint of the mission planning, mast control, and solar wing control.
- the method for judging the mechanical interference between the patrol digital antenna and the solar wing or the omnidirectional antenna and the solar wing is as follows:
- the vertices of the omnidirectional transmitting antenna and the receiving antenna are S l and S r respectively .
- the intersection of the launch surface is S.
- C In the inspection system is C, S l established to control station of the ray equation, S R & lt monitoring stations to ray equations, S ray equations to the data receiving station, an omnidirectional radiation communication or data transmission communication with the solar wing plane equation of Equation Solve the intersection. If the intersection exists and lies within the solar airfoil, it is considered a mechanical interference.
- the detection rays interfere with the solar wing.
- the interference threshold must be considered.
- CE3 patrol solar wing control is the basic component of patrol mission planning system and patrol remote operation control system.
- the control of the behavior of the solar wing is displayed through a 3D scene, which can intuitively reflect the "sun and earth and moon” and the ground station, the attitude of the inspector, and the state of motion of the solar wing. Not only enables users to accurately grasp the state of the scene when the inspector is on track, but also helps improve the reliability of the software.
- the simulation test it can be used to analyze the adaptability of the detection area, lunar landform, detection time interval, solar wing and left solar wing power generation performance to the lunar patrol detection task, and it can optimize the design of the patrol power system.
- the D-H parameter has only 3 structural parameters and 1 joint variable, which is helpful to simplify the elimination of the attitude equation. Because the D-H parameters are usually nominal, it is difficult to obtain accurate engineering parameters. It is necessary to accurately measure the invariants of the fixed axis and obtain the corresponding accurate D-H series and D-H parameters through calculation. Therefore, the 2R pointing and 3R attitude problems based on axis invariants can be transformed into 2R pointing and 3R attitude problems based on D-H parameters.
- the corresponding DH system is recorded as According to the numbering convention of the DH coordinate system, the motion pair The corresponding axis is written as That is, the indicators in the DH system are used to follow the parent indicators, and the parameters in the natural coordinate system are different from the child indicators; the rotation angle is When defining: among them Is by the shaft Twist angle to axis l′ z;
- the unit vector of the ground data receiving station is c u S. Find its angular sequence [ ⁇ d , ⁇ m ].
- F ⁇ F [l]
- F [l] is the natural coordinate system
- F [l ′] is the DH system
- a l and c l are the axes Wheelbase and offset to axis l, For axis Twist angle to axis l, For axis Zero position.
- equations (50) and (51) the correctness of equations (54) and (55) is verified through special cases:
- the CE3 digital transmission mechanism control module shows through simulation that after adjusting the yaw of the patrol, the digital transmission antenna is controlled.
- the antenna beam axis always points to the earth.
- the latitude and longitude of the inspector is [-28.6,40.06] °, and the antenna beam direction always points to the southeast.
- the patrol latitude and longitude is [28.6,40.06] °, the antenna beam direction always points to the southwest azimuth.
- the digital antenna control results are correct.
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)得
定义
则
给定运动链
il
n，建立基于轴不变量的机器人3D矢量姿态方程：
式(5)是关于
的n维2阶多项式方程；式中的表达形式幂符
表示□的x次幂；右上角角标∧或
表示分隔符；
是轴不变量
的叉乘矩阵；1为三维单位矩阵；Vector表示取轴矢量；
建立轴不变量
的二阶多项式：
式(6)是关于
和
的多重线性方程，是轴不变量
的二阶多项式；
为旋转变换矩阵；给定自然零位矢量
ll
lS作为
的零位参考，则
及
分别表示零位矢量及径向矢量；式(6)即为
对称部分
表示零位轴张量，反对称部分
表示径向轴张量，分别与轴向外积张量
正交，从而确定三维自然轴空间；
式(6)表示为
由式(7)得规范的机器人姿态方程：
式中，
iQ
n表示姿态，轴矢量
是轴不变量
的叉乘矩阵。
若运动副
R表示转动副，
iQ
n表示姿态，仅三个独立的自由度；则当|
il
n|＝3时，存在3R姿态逆解；
给定单位矢量
由式(8)得
式中，
为单位矢量
在大地坐标系的投影矢量。
若
表示需要确定的方向，则当|
il
n|＝2时，存在2R姿态逆解；
给定单位矢量
及
由式(8)得
式中，
为单位矢量
在大地坐标系的投影矢量。
给定单位矢量
及
由式(8)得
若
表示期望的投影，则当|
il
n|＝1时，存在1R姿态逆解；
由式(5)及式(10)得
使固结的单位矢量
lu
S与期望单位矢量
的投影
最优，满足
最小的解为
其中：
为
lu
S与
的夹角。
将基于轴不变量的2R指向问题转化为基于D-H参数的2R指向问题，定向逆解计算步骤为：
对于给定2R转动链
其中，
均为杆件；由初始单位矢量
指向期望单位矢量
为单位矢量
在D-H系的投影矢量；求杆件的自然关节坐标
及φ
l：
其中约定C(□)＝cos(□)，S(□)＝sin(□)；自然坐标系
对应的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】给定旋转变换阵
因其是实矩阵，其模是单位的，故其有一个实特征值λ
1及两个互为共轭的复特征值λ
2＝e
iφ及λ
3＝e
-iφ；其中：i为纯虚数。因此，|λ
1|·||λ
2||·||λ
3||＝1，得λ
1＝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
0时，关节绝对编码器的零位
不一定为零，该零位称为机械零位；
故关节
的控制量
为
定义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及其子树构成的闭子树，
为不含l的子树；递归执行
ll，计算复杂度为
【8】支路、子树及非树弧的增加与删除操作也是必要的组成部分；从而，通过动态Span树及动态图描述可变拓扑结构。在支路
ll
k中，若
则记
即
表示在支路中取成员m的子。
定义以下表达式或表达形式：
轴与杆件具有一一对应性；轴间的属性量
及杆件间的属性量
具有偏序性。
约定：“□”表示属性占位；若属性p或P是关于位置的，则
应理解为坐标系
的原点至F
[l]的原点；若属性p或P是关于方向的，则
应理解为坐标系
至F
[l]。
本申请中约定：在运动链符号演算系统中，具有偏序的属性变量或常量，在名称上包含表示偏序的指标；要么包含左上角及右下角指标，要么包含右上角及右下角指标；它们的方向总是由左上角指标至右下角指标，或由右上角指标至右下角指标，本申请中为叙述简便，有时省略方向的描述，即使省略，本领域技术人员通过符号表达式也可以知道，本申请中采用的各参数，对于某种属性符，它们的方向总是由偏序指标的左上角指标至右下角指标，或由右上角指标至右下角指标。例如：
可简述为(表示由k至l)平动矢量；
表示(由k至l的)线位置；
kr
l表示(由k至l的)平动矢量；其中：r表示“平动”属性符，其余属性符对应为：属性符φ表示“转动”；属性符Q表示“旋转变换矩阵”；属性符l表示“运动链”；属性符u表示“单位矢量”；属性符ω表示“角速度”；角标为i表示惯性坐标系或大地坐标系；其他角标可以为其他字母，也可以为数字。
本申请的符号规范与约定是根据运动链的偏序性、链节是运动链的基本单位这两个原则确定的，反映了运动链的本质特征。链指标表示的是连接关系，右上指标表征参考系。采用这种符号表达简洁、准确，便于交流与书面表达。同时，它们是结构化的符号系统，包含了组成各属性量的要素及关系，便于计算机处理，为计算机自动建模奠定基础。指标的含义需要通过属性符的背景即上下文进行理解；比如：若属性符是平动类型的，则左上角指标表示坐标系的原点及方向；若属性符是转动类型的，则左上角指标表示坐标系的方向。
(1)l
S－杆件l中的点S；而S表示空间中的一点S。
(2)
－杆件k的原点O
k至杆件l的原点O
l的平动矢量；
(3)
－原点O
k至点l
S的平动矢量；
(4)
－原点O
k至点S的平动矢量；
(5)
－连接杆件
及杆件l的运动副；
(6)
－沿轴
的线位置(平动位置)，
(7)左下角指标为0时，表示机械零位；如：
(8)0－三维零矩阵；1－三维单位矩阵；
(9)约定：“\”表示续行符；“□”表示属性占位；则
幂符
表示□的x次幂；右上角角标∧或
表示分隔符；如：
或
为
的x次幂。
[□]
T表示□的转置，表示对集合转置，不对成员执行转置；如：
|□为投影符，表示矢量或二阶张量对参考基的投影矢量或投影序列，即坐标矢量或坐标阵列，投影即是点积运算“·”；如：位置矢量
在坐标系F
[k]中的投影矢量记为
叉乘符运算的优先级高于投影符
|□的优先级。投影符
|□的优先级高于成员访问符□
[□]或□
[□]，成员访问符□
[□]优先级高于幂符
(10)单位矢量在大地坐标系的投影矢量
单位零位矢量
(11)
－零位时由原点
至原点O
l的平动矢量，且记
表示位置结构参数。
(12)
iQ
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)
il
j表示由i到j的运动链；
ll
k为取由轴l至轴k的运动链；
给定运动链
若n表示笛卡尔直角系，则称
为笛卡尔轴链；若n表示自然参考轴，则称
为自然轴链。
(16)Rodrigues四元数表达形式：
欧拉四元数表达形式：
不变量的四元数(也称为轴四元数)表达形式
1、基于轴不变量的1R姿态逆解方法
投影是旋转矢量在线性空间下的度量。给定链节
R为转动副；控制关节变量
使固结的单位矢量
lu
S与期望单位矢量
的投影
最优；其中：
为
lu
S与
的夹角。称该问题为投影逆解问题。
当给定角度
后，其正、余弦及其半角的正、余弦均是常数；为方便表达，记
约定：C(□)＝cos(□)，S(□)＝sin(□)。记D-H参数指标遵从父指标，与自然坐标系统下的参数遵从子指标不同。转动角度为
时，定义：
由式(1)得
定义
故有
给定运动链
il
n，建立基于轴不变量的3D矢量姿态方程：
式(5)是关于
的n维2阶多项式方程。Vector表示取轴矢量。
建立轴不变量
的二阶多项式：
式(6)是关于
和
的多重线性方程，是轴不变量
的二阶多项式。给定自然零位矢量
ll
lS作为
的零位参考，则
及
分别表示零位矢量及径向矢量。式(6)即为
对称部分
表示零位轴张量，反对称部分
表示径向轴张量，分别与轴向外积张量
正交，从而确定三维自然轴空间；式(6)仅含一个正弦及余弦运算、6个积运算及6个和运算，计算复杂度低；同时，通过轴不变量
及关节变量
实现了坐标系及极性的参数化。
式(6)可表示为
由式(7)得规范的姿态方程
给定运动链
il
n，考虑式(8)，若
iQ
n表示姿态，仅三个独立的自由度；则当|
il
n|＝3时，存在3R姿态逆解。给定单位矢量
由式(8)得
若
表示需要确定的方向，则当|
il
n|＝2时，存在2R姿态逆解。给定单位矢量
及
由式(8)得
若
表示期望的投影，则当|
il
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—分别是太阳翼前外侧点及后外侧点；
cr
p—巡视器体系原点O
c至太阳翼体系原点O
p位置矢量在巡视器体系下坐标；
—巡视器至太阳的单位矢量在导航系n下坐标。
由式(5)得
太阳翼上任一点S在其体系下坐标记为
则有齐次坐标变换
记巡视器相对导航系的旋转变换阵为
nQ
c，则有
nQ
p＝
nQ
c·
cQ
p，故有
cu
S＝
cQ
n·
nu
S， (22)
pu
S＝
pQ
n·
nu
S。 (23)
记器日矢量相对太阳翼高度角为
其由式(23)确定
记由太阳翼法向至太阳单位矢量夹角记为
Rad，则有
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 beam)轴与发射面交点为S。在巡视器体系c下，建立S
l至测控站的射线方程、S
r至测控站的射线方程、S至数据接收站的射线方程，通过全向通信或数传通信射线方程与太阳翼平面方程求解交点。若交点存在且位于太阳翼面内，则视为机械干涉。记射线的起点为A，射线单位矢量为
cn
t，参数为t，其对应的点记为
cr
t，在巡视器体系c下射线参数方程为
cr
t＝
cr
A+
cn
t·t， (29)
即
记太阳翼前内侧角点为B，太阳翼法向为
cn
p，射线与太阳翼平面任一交点记为
cr
t。太阳翼平面方程为
即
将式(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，这是定向逆解问题。
其中约定C(□)＝cos(□)，S(□)＝sin(□)；自然坐标系
对应的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′分别经过轴不变量
和
且
令
由轴扭角的定义得
令
由关节转动角定义得
其中：a
l和c
l分别为轴
到轴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]°时，天线波束方向始终指向西南方位。在不同的经纬度，数传天线控制结果均正确。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。
Claims (6)
- 一种基于轴不变量及DH参数1R/2R/3R逆解建模方法，其特征是，用于控制多轴机器装置，所述多轴机器装置包含杆件集合与关节集合，所述杆件集合中之杆件透过所述关节集合的关节结合，将所述关节集合转换成对应的轴集合，关节集合中的一个关节对应成所述轴集合的子轴集合，所述轴集合的轴包含平动轴与转动轴两种类型；使用所述轴集合来对应描述所述多轴机器装置，并且利用所述轴集合来建立动力学方程，以控制这个多轴机器装置；在系统处于零位时，以自然坐标系为参考，测量得到连接杆件及杆件l的坐标轴矢量
在运动副运动时，轴矢量
是不变量；轴矢量
及关节变量
唯一确定运动副的转动关系；
当给定关节变量转动角度后，其正、余弦及其半角的正、余弦均是常数；为方便表达，记
由式(1)得
定义
则
给定运动链 il n，建立基于轴不变量的机器人3D矢量姿态方程：
式(5)是关于的n维2阶多项式方程；式中的表达形式幂符
表示□的x次幂；右上角角标∧或
表示分隔符；
是轴不变量
的叉乘矩阵；1为三维单位矩阵；Vector表示取轴矢量；
建立轴不变量的二阶多项式：
式(6)是关于和
的多重线性方程，是轴不变量
的二阶多项式；
为旋转变换矩阵；给定自然零位矢量 ll lS作为
的零位参考，则
及
分别表示零位矢量及径向矢量；式(6)即为
对称部分
表示零位轴张量，反对称部分
表示径向轴张量，分别与轴向外积张量
正交，从而确定三维自然轴空间；
式(6)表示为
由式(7)得规范的机器人姿态方程：
式中， iQ n表示姿态，轴矢量是轴不变量
的叉乘矩阵。
- 根据权利要求1所述的基于轴不变量及DH参数1R/2R/3R逆解建模方法，其特征是，若运动副R表示转动副， iQ n表示姿态，仅三个独立的自由度；则当| il n|＝3时，存在3R姿态逆解；
给定单位矢量由式(8)得
式中，为单位矢量
在大地坐标系的投影矢量。
- 根据权利要求1所述的基于轴不变量及DH参数1R/2R/3R逆解建模方法，其特征是，若表示需要确定的方向，则当| il n|＝2时，存在2R姿态逆解；
给定单位矢量及
由式(8)得
式中，为单位矢量
在大地坐标系的投影矢量。
- 根据权利要求1所述的基于轴不变量及DH参数1R/2R/3R逆解建模方法，其特征是，给定单位矢量及
由式(8)得
若表示期望的投影，则当| il n|＝1时，存在1R姿态逆解；
由式(5)及式(10)得
使固结的单位矢量 lu S与期望单位矢量的投影
最优，满足
最小的解为
其中：为 lu S与
的夹角。
- 根据权利要求3所述的基于轴不变量及DH参数1R/2R/3R逆解建模方法，其特征是，将基于轴不变量的2R指向问题转化为基于D-H参数的2R指向问题，定向逆解计算步骤为：对于给定2R转动链其中，
均为杆件；由初始单位矢量
指向期望单位矢量
为单位矢量
在D-H系的投影矢量；求杆件的自然关节坐标
及φ l：
其中约定C(□)＝cos(□)，S(□)＝sin(□)；自然坐标系对应的D-H系记为
根据D-H坐标系统的编号习惯，运动副
对应的轴记为
即D-H系统中的指标习惯遵从父指标，与自然坐标系统下的参 数遵从子指标不同；转动角度为
时，定义：
其中
是由轴
至轴l′z的扭角；
令D-H参数指标遵从子指标，即因
故用D-H参数表示得
由式(36)最后一行得
式中，若用“□”表示属性占位，则式中的表达形式□ [□]表示成员访问符；故有
即有
其中：
故有
由式(36)第一行得
故有
即
其中：
因式(38及式(42)不一定满足式(36)的第2行，由式(38)及式(42)获得的及φ l只是可能解；再将可能 解代入式(36)的第2行，若仍成立，则得到真实解。
- 根据权利要求5所述的基于轴不变量及DH参数1R/2R/3R逆解建模方法，其特征是，给定3R转动链及期望姿态
轴不变量序列
求关节变量序列
其中，
均为杆件；将基于轴不变量的3R姿态问题转化为基于D-H参数的3R姿态问题，姿态逆解计算式为：
由式(38)及式(42)得由
得
故有
式中，表示矩阵
的第4行元素。
Applications Claiming Priority (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810933485.8A CN109291047B (zh) | 2018-08-16 | 2018-08-16 | 基于轴不变量及dh参数1r/2r/3r逆解建模方法 |
| CN201810933485.8 | 2018-08-16 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| WO2020034418A1 true WO2020034418A1 (zh) | 2020-02-20 |
Family
ID=65165146
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| PCT/CN2018/112768 WO2020034418A1 (zh) | 2018-08-16 | 2018-10-30 | 基于轴不变量及dh参数1r/2r/3r逆解建模方法 |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN109291047B (zh) |
| WO (1) | WO2020034418A1 (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111967099B (zh) * | 2020-07-20 | 2021-04-27 | 居鹤华 | 多自由度机械臂矢量多项式系统最优求解方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105522577A (zh) * | 2015-11-29 | 2016-04-27 | 上海新时达电气股份有限公司 | 一种用于五轴折弯机器人笛卡尔轨迹规划的方法及其装置 |
| CN105975795A (zh) * | 2016-05-23 | 2016-09-28 | 湖北工业大学 | 一种高精度的多关节串联机械臂运动学反解解法 |
| CN106041932A (zh) * | 2016-07-01 | 2016-10-26 | 广东工业大学 | 一种ur机器人的运动控制方法 |
| WO2018092236A1 (ja) * | 2016-11-17 | 2018-05-24 | 株式会社Ｆｕｊｉ | 作業ロボットおよび作業位置補正方法 |
| CN108255058A (zh) * | 2018-01-18 | 2018-07-06 | 山东大学深圳研究院 | 智能空间下的服务机器人逆运动学求解方法和装置 |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9348137B2 (en) * | 2011-06-10 | 2016-05-24 | Hewlett-Packard Development Company, L.P. | Optical scanning apparatus, system and method |
| CN107756400B (zh) * | 2017-10-13 | 2020-12-04 | 北京工业大学 | 一种基于旋量理论的6r机器人逆运动学几何求解方法 |
| CN107866823B (zh) * | 2017-10-24 | 2019-10-11 | 南京工程学院 | 一种基于位置矢量法的工业机器人几何参数标定方法 |
| CN108274472B (zh) * | 2018-01-23 | 2020-09-15 | 深圳创源航天科技有限公司 | 一种工业机器人加工工艺自适应的空间运动方法 |
-
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 (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105522577A (zh) * | 2015-11-29 | 2016-04-27 | 上海新时达电气股份有限公司 | 一种用于五轴折弯机器人笛卡尔轨迹规划的方法及其装置 |
| CN105975795A (zh) * | 2016-05-23 | 2016-09-28 | 湖北工业大学 | 一种高精度的多关节串联机械臂运动学反解解法 |
| CN106041932A (zh) * | 2016-07-01 | 2016-10-26 | 广东工业大学 | 一种ur机器人的运动控制方法 |
| WO2018092236A1 (ja) * | 2016-11-17 | 2018-05-24 | 株式会社Ｆｕｊｉ | 作業ロボットおよび作業位置補正方法 |
| CN108255058A (zh) * | 2018-01-18 | 2018-07-06 | 山东大学深圳研究院 | 智能空间下的服务机器人逆运动学求解方法和装置 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109291047B (zh) | 2019-10-22 |
| CN109291047A (zh) | 2019-02-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11491649B2 (en) | Axis-invariant based multi-axis robot kinematics modeling method | |
| US20200055192A1 (en) | Axis-Invariant based Multi-axis robot inverse kinematics modeling and solving method | |
| CN108983705B (zh) | 一种基于轴不变量的多轴机器人系统正运动学建模与解算方法 | |
| CN108015763A (zh) | 一种抗噪声干扰的冗余度机械臂路径规划方法 | |
| Klug et al. | A complete workflow for automatic forward kinematics model extraction of robotic total stations using the Denavit-Hartenberg convention | |
| CN107480356B (zh) | 基于catia和激光跟踪仪的部件设计检验一体化方法 | |
| CN113160334B (zh) | 一种基于手眼相机的双机器人系统标定方法 | |
| CN113858217B (zh) | 多机器人交互的三维视觉位姿感知方法及系统 | |
| CN114216456A (zh) | 一种基于imu与机器人本体参数融合的姿态测量方法 | |
| Qiu et al. | An underwater micro cable-driven pan-tilt binocular vision system with spherical refraction calibration | |
| WO2020034418A1 (zh) | 基于轴不变量及dh参数1r/2r/3r逆解建模方法 | |
| WO2020034415A1 (zh) | 基于轴不变量的通用6r机械臂逆解建模与解算方法 | |
| CN105424060A (zh) | 一种飞行器星敏感器与捷联惯组安装误差的测量方法 | |
| WO2020034417A1 (zh) | 基于轴不变量多轴机器人d-h系及d-h参数确定方法 | |
| Yan et al. | Intergrating UAV development technology with augmented reality toward landscape tele-simulation | |
| WO2020034416A1 (zh) | 基于轴不变量的通用7r机械臂逆解建模与解算方法 | |
| WO2020034405A1 (zh) | 基于轴不变量的树链机器人动力学建模与解算方法 | |
| WO2020034407A1 (zh) | 基于轴不变量的通用3r机械臂逆解建模与解算方法 | |
| Dabney et al. | Modeling closed kinematic chains via singular perturbations | |
| WO2020034403A1 (zh) | 基于轴不变量的多轴机器人正运动学计算方法 | |
| CN114536351A (zh) | 冗余双臂机器人示教方法、装置、电子设备及系统 | |
| WO2020034404A1 (zh) | 基于轴不变量的非理想关节机器人动力学建模与解算方法 | |
| WO2020034402A1 (zh) | 基于轴不变量的多轴机器人结构参数精测方法 | |
| WO2020034399A1 (zh) | 基于轴不变量的闭链机器人动力学建模与解算方法 | |
| Gross et al. | Solar field modeling and verification |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| 121 | Ep: the epo has been informed by wipo that ep was designated in this application |
Ref document number: 18930381 Country of ref document: EP Kind code of ref document: A1 |
|
| NENP | Non-entry into the national phase |
Ref country code: DE |
|
| 122 | Ep: pct application non-entry in european phase |
Ref document number: 18930381 Country of ref document: EP Kind code of ref document: A1 |
|
| 122 | Ep: pct application non-entry in european phase |
Ref document number: 18930381 Country of ref document: EP Kind code of ref document: A1 |
|
| 32PN | Ep: public notification in the ep bulletin as address of the adressee cannot be established |
Free format text: NOTING OF LOSS OF RIGHTS PURSUANT TO RULE 112(1) EPC (EPO FORM 1205A DATED 13.08.2021) |
|
| 122 | Ep: pct application non-entry in european phase |
Ref document number: 18930381 Country of ref document: EP Kind code of ref document: A1 |