D-H坐标解算

第3章运动学数学建模3.1引言在研究一个复杂的系统之前，首先建立其数学模型。双足竞步机器人作一个复杂的机电系统具有多方面的特性。其中最主要的两个特性就是运动学和动力学特性，这些特性直接决定了其控制系统的设计等相关内容。双足竞步机器人运动学建模的目的旨在给定各个关节运动的前提下，确定双足机器人各个部分的运动学关系；双足竞步机器人本体设计结构复杂，在步行过程中，开链的单脚支撑区与闭链的双脚支撑期交替出现，而且，摆动腿着地时存在冲击，这些因素导致对其很难进行精确的数学建模。为了实现稳定行走和进行有效地步行控制，有必要深入的研究其数学模型。3.2数学基础3.2.1位姿描述刚体参考点的位置和刚体的姿态统称为刚体的位姿，描述方法非常多，例如：齐次变换法、矢量法、旋转法和四元数法等等。在空间中，要确定一个物体的几何状态需要确定其3个位移坐标（或称位置自由度）和3个旋转坐标（或称姿态自由度）。在机器人学术语中，将一个空间物体上述的6个自由度状态称为该物体的位姿。同理，为了研究机器人的运动与操作，往往不仅要表示空间某个点的位置，而且需要表示物体的方位,有的关节有3个自由度---横滚、俯仰和侧摆的概念。本文采用了齐次变换法，其优点在于它能将运动、变换和映射与矩阵联系起来，可以方便的运用矩阵理论解决相关问题。在直角坐标系中通常用一个位置矢量表示一个点。在坐标系{A}中用PA=[PxPyPz]T表示某一点P，Px,Py,Pz分别表示三个坐标分量：为了描述空间某刚体B的方位。另设一对直角坐标系{B}与此刚体固接，用坐标系{B}的三个单位主失量XB,YB,ZB相对坐标系{A}的方向余弦组成的3×3矩阵RBA=[xBAyBAzBA]来表示刚体B相对于坐标系{A}的方位，RBA称为旋转矩阵，RBA有9个元素，6个约束方程，所以只有3个元素是独立的。同时RBA也是正交矩阵，满足R−1BA=RTBA，|RBA|=1。为了完全描述刚体B在空间的位姿，通常将物体B与某一坐标{B}相固接，{B}的坐标原点一般选在物体B的特征点上，如质心或对称中心等。相对参考系{A}，由位置矢量PB0A和旋转矩阵RBA分别描述坐标系{B}的原点位置和坐标轴方位。因此，刚体B的位姿可由坐标系{B}来描述，即{B}={RBA，PB0A}。当上式表示位置时，RBA=1，当表示方位时，PB0A=0。3.2.2齐次坐标和其次变换坐标系{B}的原点与{A}的不重合，且坐标轴方位不同，此时可以用式通过PB描述PA，PA=RBAPB+PB0A（3-1）定义三维空间内某点P的直角坐标和齐次坐标分别为P=[xyz1]T和P=[ωxωyωzω]T（其中ω是非零常数），并且规定[1000]T代表x轴上的无穷远点，非无穷远点[0001]T代表坐标原点。这样，利用齐次坐标不仅可以规定点的位置，还可以用来规定矢量的方向。当第四个元素非零时代代表点的位置；为零时，代表方向。式（3-1）所表示的复合变换对于PB是非齐次的，通过齐次坐标可变换为：01011AAABBBPPPP（3-2）若仍然把齐次坐标记作AP和BP，并令001AAABBBPPT，上式即为：AABBPTP，称为齐次变换矩阵。3.2.3连杆描述在机器人运动学求解过程中，为了进一步的求解与计算，需将机器人腿简化成一系列的杆件和连接各杆件的关节，并将其参数化。本文将各参数定义如下(如图3.1)：(1)杆件长度il：关节iA轴线和1iA轴线公法线的长度；(2)扭转角i：关节iA轴线与1iA轴线在垂直于il平面内的夹角；(3)杆件距离id：杆件长度il和1il在iA轴线上的交点之间的距离；(4)回转角i：杆件长度il和1il之间的夹角，由右手定则决定正负。图3.1转动关节连杆四参数示意图3.3运动学模型3.3.1机器人运动方程的表示在求解四足机器人运动方程之前需要两个准备工作，(1)建立各关节D-H坐标；(2)通过一定的矩阵变化，将各坐标系之间建立起一定的变换关系。具体定义请参考文献[55]，本文主要引用其变换公式。如式(3-3)就可以表示第i个连杆在基坐标中的位置。iiAAAT21（3-3）3.3.2机器人D-H坐标建立原则机器人关节坐标系的建立主要是为了描述机器人各杆件的终端之间的相对运动，对建立运动方程和动力学研究是基础性的工作。D-H坐标建立法是目前机器人坐标系建立最常用的方法，它是1955年西门子公司两位工程师Denavit和Hartenberg提出的。其建立原则如下(如图3.2)：(1)原点iO：设在杆件长度il与轴线1iA的交点上；(2)iZ轴：与1iA关节轴重合，指向任意；(3)iX轴：与杆件长度il重合，指向沿il由iA轴线指向1iA轴线；(4)iY轴：按右手定则。图3.2D-H坐标建立示意图3.3.3广义变换矩阵广义变换矩阵是用来描述两杆件1i与i之间的相对关系的，具体定义请参考文献[55]，本文引用其变换公式及展开矩阵。iiiiixRotlTransdTranszRotA,0,0,,0,0,(3-4)展开可得：1000cossin0sincossincoscossincossinsinsincoscos1iiiiiiiiiiiiiiiiiiidllA(3-5)矩阵中表面看上去有4个未知数，其实真正的变量只有一个。对于旋转关节，杆件长度il，杆件距离id，扭转角i均为常量，只有回转角i为变量。这样，iA矩阵就成为关节变量i的函数。001112iiiTAAA(3-6)3.4双足机器人的运动方程3.4.1双足机器人单腿D-H坐标的建立依据D-H坐标建立方法，将交叉足印双足机器人单腿坐标系建立如图3.3所示，狭窄足印双足机器人单腿坐标系建立如图3.4所示。图3.3交叉足印单腿示意图图3.4狭窄足印单腿示意图3.4.2交叉足印双足机器人坐标解算根据双足机器人杆件参数定义，将交叉足印双足机器人单腿的坐标参数如表3-1所示，其中只有回转角i为变量。表3-1双足机器人单腿的连杆参数关节ii/(°)i/(°)li/(mm)di/(mm)1001l02002l03003l0单腿正解：根据式(3-5)和表2-1所示连杆参数，可求得各连杆变换矩阵如下1111111101cossin0cossincos0sin00100001llA10000100sin0cossincos0sincos2222222221llA3333333323cossin0cossincos0sin00100001llA依据式（3-6）将各连杆变换矩阵相乘，得四足机器人单腿的变换矩阵12312311212312312312311212312303c0cccsc000100001sllllslslsT其中123123ccos()123123sin()s1212ccos()1212sin()s于是，可求的坐标的变换矩阵030001xxxxyyyyzzzznoapnoapTnoap=123123112123123123123112123123c0cccsc000100001sllllslsls等式两边相等，即可得到位姿矩阵03T有效元素的表达式为123123123123112123123112123123cccccxyxyxynnsosoplllplslsls{为校核所得03T的正确性，计算1230,0,0时手臂变换矩阵03T的值，计算结果为12303100010000100001lllT与简图所示情况完全一致。单腿逆解：单腿的运动方程可写为001231230001xxxxyyyyzzzznoapnoapTAAAnoap（3-7）用未知的连杆逆变换左乘方程（3-7）两边，把关节变量分离出来，从而求解。具体步骤如下：322130110AATA其中：10013AT=11111111111111111cccc0001xyxyxyxyxyxyxyxyzzzznnsoosaaslppsnsncosocasacpspcnoap（3-8）1223AA=232322323232322323cs0ccsc000100001lllsls（3-9）将式（3-8）与式（3-9）两矩阵第四列元素对应相等,且有123，得111231cossincoscosxylppll（3-10）112231sincossinsinxyppll（3-11）将式（3-10）与式（3-11）分别平方，求得2  222231312122  cos()2xyxppllplll1110022133AATA其中11100213AAT=1212121212121221212121212121212121212ccccc0001xyxyxyxyxyxyxyxyzzzznnsoosaasllppsnsncosocasaclspspcnoap（3-12）3333333323cossin0cossincos0sin00100001llA（3-13）将式（3-12）与式（3-13）两矩阵的第四列对应相等得1223333coscossincosxyllppl（3-14）123333sinsincossinxylppl（3-15）将式（3-14）与（3-15）两边分别平方，消去3得12211222233sin(((cos)sin())/(())yxyxplllplppl3123.4.3狭窄足印双足机器人坐标解算根据双足机器人杆件参数定义，将交叉足印双足机器人单腿的坐标参数如表3-2所示，其中只有回转角i为变量。表3-2双足机器人单腿的连杆参数关节ii/(°)i/(°)li/(mm)di/(mm)1001l02002l030903l0单腿正解：根据式(3-5)和表3-2所示连杆参数，可求得各连杆变换矩阵如下1111111101cossin0cossincos0sin00100001llA10000100sin0cossincos0sincos2222222221llA3333333323cos0sincossin0cossin01000001llA依据式（3-6）将各连杆变换矩阵相乘，得双足机器人单腿的变换矩阵12312311212312312312311212312303c0scs0c01000001llclclslslsT其中123123ccos()123123sin()s1212ccos()1212sin()s于是，可求的坐标的变换矩阵12312311212312312312311212312303c0scs0c010000010001xxxxyyyyzzz