這是用戶在 2024-12-10 24:40 為 https://app.immersivetranslate.com/pdf-pro/5caaf7a8-8b0a-45e1-abda-b21be341c253 保存的雙語快照頁面，由 沉浸式翻譯 提供雙語支持。了解如何保存？

6

Manipulator  操縱器

Dynamics 動力學

Optimal performance can be obtained from a robotic manipulator if sophisticated control strategies are employed. However, precise control of high-speed motion requires the use of a realistic dynamic model of the arm. The dynamic equations of motion of a general six-axis manipulator can be rather complex. We therefore illustrate the derivation of the equations of motion using relatively simple robotic arms. Two basic approaches are presented. The first is the Lagrange-Euler formulation, which is based on the concepts of generalized coordinates, energy, and generalized force. This approach has the advantage that each of the terms in the final closedform equation has a simple physical interpretation in terms of such things as manipulator inertia, gravity, friction, and Coriolis and centrifugal forces. Complete dynamic models for two robots, a two-axis planar articulated arm, and a three-axis SCARA type arm, are derived.
如果採用複雜的控制策略，可以從機器人操作器中獲得最佳性能。然而，高速運動的精確控制需要使用臂的現實動態模型。一般六軸操作器的動態運動方程可能相當複雜。因此，我們使用相對簡單的機器臂來說明運動方程的推導。提出了兩種基本方法。第一種是拉格朗日-歐拉公式，基於廣義坐標、能量和廣義力的概念。這種方法的優點在於，最終封閉形式方程中的每一項在操作器慣性、重力、摩擦以及科里奧利和離心力等方面都有簡單的物理解釋。推導了兩個機器人的完整動態模型，一個是兩軸平面關節臂，另一個是三軸 SCARA 型臂。
Next an alternative approach, called the recursive Newton-Euler formulation, is presented. This approach has the advantage that it is very amenable to computer impiementation, and furthermore it is computationally more efficient than the La-grange-Euler formulation, particularly as the number of axes increases. A third example of a dynamic model, a simple one-axis arm, is derived using both the La-grange-Euler method and the recursive Newton-Euler method. The treatment of robot arm dynamics presented here is patterned primarily after discussions found in Asada and Slotine (1986) and Fu et al. (1987). Additional investigations of robot arm dynamics can be found in Brady et al. (1982), Craig (1986), Paul (1981), and Wolovich (1987).
接下來介紹一種替代方法，稱為遞歸牛頓-歐拉公式。這種方法的優點在於它非常適合計算機實現，而且在計算上比拉格朗日-歐拉公式更有效率，特別是在軸數增加的情況下。第三個動態模型的例子，一個簡單的單軸臂，使用拉格朗日-歐拉方法和遞歸牛頓-歐拉方法推導而來。這裡所呈現的機器人臂動力學的處理主要是基於 Asada 和 Slotine（1986）以及 Fu 等人（1987）的討論。對機器人臂動力學的進一步研究可以在 Brady 等人（1982）、Craig（1986）、Paul（1981）和 Wolovich（1987）中找到。

6-1 LAGRANGE'S EQUATION 6-1 拉格朗日方程

Complex dynamic systems can be modeled in a relatively simple, elegant fashion using an approach called the Lagrangian formulation. The Lagrangian formulation is based on the notion of generalized coordinates, energy, and generalized forces. For an n n nn-axis robotic arm, an appropriate set of generalized coordinates is the vector of n n nn joint variables q q qq. Recall that the components of q q qq represent the joint angles of revolute joints and the joint distances of prismatic joints. Let T T TT and U U UU represent the k i k i kik i netic energy and potential energy of the arm, respectively. We then define the Lagrangian function as the difference between the kinetic and potential energy as follows:
複雜的動態系統可以使用一種稱為拉格朗日公式的方法以相對簡單、優雅的方式進行建模。拉格朗日公式基於廣義坐標、能量和廣義力的概念。對於一個 n n nn -軸機器手臂，適當的廣義坐標集是 n n nn 關節變數 q q qq 的向量。回想一下， q q qq 的組件代表了旋轉關節的關節角度和滑動關節的關節距離。讓 T T TT 和 U U UU 分別表示手臂的 k i k i kik i 動能和位能。我們然後定義拉格朗日函數為動能和位能之間的差異，如下所示：
L ( q , q ˙ ) ≜ T ( q , q ˙ ) − U ( q ) L ( q , q ˙ ) ≜ T ( q , q ˙ ) − U ( q ) L(q,q^(˙))≜T(q,q^(˙))-U(q)L(q, \dot{q}) \triangleq T(q, \dot{q})-U(q)
Here q ˙ = d q / d t q ˙ = d q / d t q^(˙)=dq//dt\dot{q}=d q / d t is the vector of joint velocities. Note that the kinetic energy T T TT depends on both the position and the velocity of the arm, while the potential energy U U UU depends on only the arm position. The general equations of motion of a robotic arm can be formulated in terms of the Lagrangian function as follows (Torby, 1984):
這裡 q ˙ = d q / d t q ˙ = d q / d t q^(˙)=dq//dt\dot{q}=d q / d t 是關節速度的向量。請注意，動能 T T TT 取決於手臂的位置和速度，而位能 U U UU 僅取決於手臂的位置。機器手臂的運動一般方程可以用拉格朗日函數來表達，如下所示（Torby, 1984）：
d d t ∂ ∂ q ˙ i L ( q , q ˙ ) − ∂ ∂ q i L ( q , q ˙ ) = F i 1 ≤ i ≤ n d d t ∂ ∂ q ˙ i L ( q , q ˙ ) − ∂ ∂ q i L ( q , q ˙ ) = F i 1 ≤ i ≤ n (d)/(dt)(del)/(delq^(˙)_(i))L(q,q^(˙))-(del)/(delq_(i))L(q,q^(˙))=F_(i)quad1 <= i <= n\frac{d}{d t} \frac{\partial}{\partial \dot{q}_{i}} L(q, \dot{q})-\frac{\partial}{\partial q_{i}} L(q, \dot{q})=F_{i} \quad 1 \leq i \leq n
Here F i F i F_(i)F_{i} is the generalized force acting on the i i ii th joint. It is the residual force left over after the effects of inertial forces and gravity have been removed. The Lagrangian formulation of robot arm dynamics in Eq. (6-1-2) consists of a system of n n nn sec-ond-order nonlinear differential equations in the vector of joint variables q q qq. To specify these equations in more detail, we must formulate expressions for the kinetic energy T T TT, the potential energy U U UU, and the generalized force F F FF.
這裡 F i F i F_(i)F_{i} 是作用在 i i ii 號關節上的廣義力。它是去除慣性力和重力影響後剩餘的力。機器人手臂動力學的拉格朗日公式在方程 (6-1-2) 中由一組 n n nn 階的非線性二階微分方程組成，變量為關節變數 q q qq 。為了更詳細地指定這些方程，我們必須為動能 T T TT 、位能 U U UU 和廣義力 F F FF 形成表達式。

6-2 KINETIC AND POTENTIAL ENERGY
6-2 動能與位能

The most complicated term in the Lagrangian function of a robotic arm is the total kinetic energy of the arm T ( q , q ˙ ) T ( q , q ˙ ) T(q,q^(˙))T(q, \dot{q}). To obtain an expression for the total kinetic energy, we begin by examining the kinetic energy of the k k kk th link of the arm shown in Fig. 6-1.
機械臂拉格朗日函數中最複雜的術語是機械臂的總動能 T ( q , q ˙ ) T ( q , q ˙ ) T(q,q^(˙))T(q, \dot{q}) 。為了獲得總動能的表達式，我們首先檢查圖 6-1 中所示的機械臂第 k k kk 個連桿的動能。
Figure 6-1 Motion of link k k kk.
圖 6-1 連桿 k k kk 的運動。
The k k kk th link will be moving in the three-dimensional space with both a linear velocity and an angular velocity. Let v ¯ k ∈ R 3 v ¯ k ∈ R 3 bar(v)^(k)inR^(3)\bar{v}^{k} \in \mathbf{R}^{3} denote the linear velocity of the center of mass of link k k kk with respect to the robot base frame, and let ω ¯ k ∈ R 3 ω ¯ k ∈ R 3 bar(omega)^(k)inR^(3)\bar{\omega}^{k} \in \mathbf{R}^{3} denote the angular velocity about the center of mass with respect to the base frame. For convenience, all quantities are expressed with respect to the robot base frame L 0 L 0 L_(0)L_{0}, unless specifically noted otherwise. The total kinetic energy of the arm is the sum of the kinetic energies of its links:
第 k k kk 個連桿將在三維空間中以線速度和角速度運動。令 v ¯ k ∈ R 3 v ¯ k ∈ R 3 bar(v)^(k)inR^(3)\bar{v}^{k} \in \mathbf{R}^{3} 表示連桿 k k kk 的質心相對於機器人基座框架的線速度，並令 ω ¯ k ∈ R 3 ω ¯ k ∈ R 3 bar(omega)^(k)inR^(3)\bar{\omega}^{k} \in \mathbf{R}^{3} 表示相對於基座框架的質心的角速度。為了方便起見，所有量都相對於機器人基座框架 {{4 }} 表示，除非另有特別說明。手臂的總動能是其連桿動能的總和：
T ( q , q ˙ ) = ∑ k = 1 n ( v ¯ k ) T m k v ¯ k + ( ω ¯ k ) T D k ω ¯ k 2 T ( q , q ˙ ) = ∑ k = 1 n   v ¯ k T m k v ¯ k + ω ¯ k T D k ω ¯ k 2 T(q,q^(˙))=sum_(k=1)^(n)(( bar(v)^(k))^(T)m_(k) bar(v)^(k)+( bar(omega)^(k))^(T)D_(k) bar(omega)^(k))/(2)T(q, \dot{q})=\sum_{k=1}^{n} \frac{\left(\bar{v}^{k}\right)^{T} m_{k} \bar{v}^{k}+\left(\bar{\omega}^{k}\right)^{T} D_{k} \bar{\omega}^{k}}{2}
Here m k m k m_(k)m_{k} represents the mass of link k k kk, and D k D k D_(k)D_{k} is the 3 × 3 3 × 3 3xx33 \times 3 inertia tensor of link k k kk about its center of mass expressed with respect to the base frame. To formulate the equations of motion of the arm we must express the kinetic energy as an explicit function of q q qq and q ˙ q ˙ q^(˙)\dot{q}. We begin by examining the link inertia tensor D k D k D_(k)D_{k}.
這裡 m k m k m_(k)m_{k} 代表連桿 k k kk 的質量，而 D k D k D_(k)D_{k} 是連桿 k k kk 關於其質心的 3 × 3 3 × 3 3xx33 \times 3 慣性張量，以基準框架為基礎來表達。為了制定手臂的運動方程，我們必須將動能明確地表達為 q q qq 和 q ˙ q ˙ q^(˙)\dot{q} 的函數。我們首先檢查連桿慣性張量 D k D k D_(k)D_{k} 。
An inertia tensor is a 3 × 3 3 × 3 3xx33 \times 3 matrix which characterizes the distribution of mass of a rigid object. To express the inertia tensor with respect to the base coordinate frame, we first consider the case of representing an inertia tensor of a rigid object with respect to a frame whose origin is located at the center of mass of the object. Let ρ ρ rho\rho denote the mass density of the rigid object, and let V V VV represent the volume occupied by the object. Then the inertia tensor of the object about its center of mass, expressed with respect to a coordinate frame L c = { x c , y c , z c } L c = x c , y c , z c L_(c)={x^(c),y^(c),z^(c)}L_{c}=\left\{x^{c}, y^{c}, z^{c}\right\} located at the center of mass, is:
慣性張量是一個 3 × 3 3 × 3 3xx33 \times 3 矩陣，用於描述剛體的質量分佈。為了將慣性張量表示為基準坐標系，我們首先考慮將剛體的慣性張量表示為其質量中心位於坐標系原點的情況。令 ρ ρ rho\rho 表示剛體的質量密度，令 V V VV 表示物體佔據的體積。那麼，物體關於其質量中心的慣性張量，表示為位於質量中心的坐標系 L c = { x c , y c , z c } L c = x c , y c , z c L_(c)={x^(c),y^(c),z^(c)}L_{c}=\left\{x^{c}, y^{c}, z^{c}\right\} ，為：
D c ≜ [ ∫ V ( y 2 + z 2 ) ρ d V − ∫ V x y ρ d V − ∫ V x z ρ d V − ∫ V x y ρ d V ∫ V ( x 2 + z 2 ) ρ d V − ∫ V y z ρ d V − ∫ V x z ρ d V − ∫ V y z ρ d V ∫ V ( x 2 + y 2 ) ρ d V ] D c ≜ ∫ V   y 2 + z 2 ρ d V − ∫ V   x y ρ d V − ∫ V   x z ρ d V − ∫ V   x y ρ d V ∫ V   x 2 + z 2 ρ d V − ∫ V   y z ρ d V − ∫ V   x z ρ d V − ∫ V   y z ρ d V ∫ V   x 2 + y 2 ρ d V D_(c)≜[[int_(V)(y^(2)+z^(2))rho dV,-int_(V)xy rho dV,-int_(V)xz rho dV],[-int_(V)xy rho dV,int_(V)(x^(2)+z^(2))rho dV,-int_(V)yz rho dV],[-int_(V)xz rho dV,-int_(V)yz rho dV,int_(V)(x^(2)+y^(2))rho dV]]D_{c} \triangleq\left[\begin{array}{ccc} \int_{V}\left(y^{2}+z^{2}\right) \rho d V & -\int_{V} x y \rho d V & -\int_{V} x z \rho d V \\ -\int_{V} x y \rho d V & \int_{V}\left(x^{2}+z^{2}\right) \rho d V & -\int_{V} y z \rho d V \\ -\int_{V} x z \rho d V & -\int_{V} y z \rho d V & \int_{V}\left(x^{2}+y^{2}\right) \rho d V \end{array}\right]
The integrals in D c D c D_(c)D_{c} are three-dimensional integrals over the volume V V VV occupied by the object. Note that D c D c D_(c)D_{c} is a symmetric matrix containing six independent terms. The three diagonal terms are called moments of inertia, while the three distinct offdiagonal terms are called products of inertia. The moments of inertia are clearly positive quantities, but the products of inertia can be either positive or negative. If the axes of the coordinate frame L c L c L_(c)L_{c} are aligned with the principal axes of the object, then the products of inertia will be zero. In this case, the inertia tensor is a diagonal matrix, and the three diagonal elements are called the principal moments of inertia. Expressions for the principal moments of inertia of simple geometric shapes can be found in Appendix 2.
D c D c D_(c)D_{c} 中的積分是對物體佔據的體積 V V VV 進行的三維積分。注意 D c D c D_(c)D_{c} 是一個包含六個獨立項的對稱矩陣。三個對角項稱為慣性矩，而三個不同的非對角項稱為慣性積。慣性矩顯然是正量，但慣性積可以是正的或負的。如果坐標系 L c L c L_(c)L_{c} 的軸與物體的主軸對齊，則慣性積將為零。在這種情況下，慣性張量是一個對角矩陣，三個對角元素稱為主慣性矩。簡單幾何形狀的主慣性矩的表達式可以在附錄 2 中找到。

Example 6-2-1: Inertia Tensor
例 6-2-1：慣性張量

As an example of an inertia tensor of a rigid object, consider the b × c b × c b xx cb \times c rectangular rod of length a a aa and mass m m mm shown in Fig. 6-2. If the rod is homogeneous, then the mass density is constant and equal to ρ = m / ( a b c ) ρ = m / ( a b c ) rho=m//(abc)\rho=m /(a b c). Since the axes of the L c L c L_(c)L_{c} coordinate frame are aligned with the principal axes of the rod, the products of inertia are all zero. The three principal moments of inertia can be obtained by referring to Appendix 2 , and the resulting diagonal inertia tensor is:
作為剛體慣性張量的例子，考慮圖 6-2 中顯示的長度為 a a aa 和質量為 m m mm 的 b × c b × c b xx cb \times c 矩形棒。如果該棒是均勻的，則質量密度是恆定的，等於 ρ = m / ( a b c ) ρ = m / ( a b c ) rho=m//(abc)\rho=m /(a b c) 。由於 L c L c L_(c)L_{c} 坐標系的軸與該棒的主軸對齊，因此慣性乘積均為零。三個主慣性矩可以參考附錄 2 獲得，得到的對角慣性張量為：
D c = m [ b 2 + c 2 12 0 0 0 a 2 + c 2 12 0 0 0 a 2 + b 2 12 ] D c = m b 2 + c 2 12 0 0 0 a 2 + c 2 12 0 0 0 a 2 + b 2 12 D_(c)=m[[(b^(2)+c^(2))/(12),0,0],[0,(a^(2)+c^(2))/(12),0],[0,0,(a^(2)+b^(2))/(12)]]D_{c}=m\left[\begin{array}{ccc} \frac{b^{2}+c^{2}}{12} & 0 & 0 \\ 0 & \frac{a^{2}+c^{2}}{12} & 0 \\ 0 & 0 & \frac{a^{2}+b^{2}}{12} \end{array}\right]
Figure 6-2 A rectangular rod.
圖 6-2 一根矩形棒。
The rectangular rod in Fig. 6-2 has a relatively simple geometry. For objects which are of an irregular shape, computing the inertia tensor using volumetric integration as in Eq. (6-2-2) may be quite difficult. In these cases, one can instead measure the inertia tensor experimentally (Klafter et al., 1989).
圖 6-2 中的矩形棒具有相對簡單的幾何形狀。對於不規則形狀的物體，使用體積積分計算慣性張量，如公式(6-2-2)所示，可能會相當困難。在這些情況下，可以通過實驗測量慣性張量（Klafter 等，1989）。
The link inertia tensor D k D k D_(k)D_{k} in the expression for kinetic energy in Eq. (6-2-1) is the inertia tensor of link k k kk about its center of mass expressed relative to the robot base frame L 0 L 0 L_(0)L_{0}. That is, D k D k D_(k)D_{k} is the inertia tensor obtained by translating the base frame to the center of mass of link k k kk and then applying Eq. (6-2-2). Recall that the D-H algorithm assigns coordinate frame L k = { x k , y k , z k } L k = x k , y k , z k L_(k)={x^(k),y^(k),z^(k)}L_{k}=\left\{x^{k}, y^{k}, z^{k}\right\} to the end of link k k kk. Frame L k L k L_(k)L_{k} and frame L 0 L 0 L_(0)L_{0} are related by the composite homogeneous coordinate transformation matrix T 0 k ( q ) T 0 k ( q ) T_(0)^(k)(q)T_{0}^{k}(q) where:
連桿 D k D k D_(k)D_{k} 在動能表達式 Eq. (6-2-1) 中是連桿 k k kk 相對於其質心的慣性張量，並以機器人基座框架 L 0 L 0 L_(0)L_{0} 為參考。也就是說， D k D k D_(k)D_{k} 是通過將基座框架平移到連桿 k k kk 的質心，然後應用 Eq. (6-2-2) 獲得的慣性張量。回想一下，D-H 算法將坐標框架 L k = { x k , y k , z k } L k = x k , y k , z k L_(k)={x^(k),y^(k),z^(k)}L_{k}=\left\{x^{k}, y^{k}, z^{k}\right\} 指派給連桿 k k kk 的末端。框架 L k L k L_(k)L_{k} 和框架 L 0 L 0 L_(0)L_{0} 由複合齊次坐標變換矩陣 T 0 k ( q ) T 0 k ( q ) T_(0)^(k)(q)T_{0}^{k}(q) 相關聯，其中：
The rotation matrix R 0 k ( q ) R 0 k ( q ) R_(0)^(k)(q)R_{0}^{k}(q) represents the orientation of frame L k L k L_(k)L_{k} relative to frame L 0 L 0 L_(0)L_{0}, while the translation vector p k ( q ) p k ( q ) p^(k)(q)p^{k}(q) represents the position of the origin of frame L k L k L_(k)L_{k} relative to frame L 0 L 0 L_(0)L_{0}. The kinetic energy due to the angular velocity of link k k kk about its center of mass is expressed in Eq. (6-2-1) in terms of frame L 0 L 0 L_(0)L_{0} coordinates. To reexpress this energy in terms of frame L k L k L_(k)L_{k} coordinates, we can use R k 0 = ( R 0 k ) − 1 = ( R 0 k ) T R k 0 = R 0 k − 1 = R 0 k T R_(k)^(0)=(R_(0)^(k))^(-1)=(R_(0)^(k))^(T)R_{k}^{0}=\left(R_{0}^{k}\right)^{-1}=\left(R_{0}^{k}\right)^{T} to convert the angular velocity ω ¯ k ω ¯ k bar(omega)^(k)\bar{\omega}^{k} from frame L 0 L 0 L_(0)L_{0} coordinates
旋轉矩陣 R 0 k ( q ) R 0 k ( q ) R_(0)^(k)(q)R_{0}^{k}(q) 表示框架 L k L k L_(k)L_{k} 相對於框架 L 0 L 0 L_(0)L_{0} 的方向，而平移向量 p k ( q ) p k ( q ) p^(k)(q)p^{k}(q) 表示框架 L k L k L_(k)L_{k} 的原點相對於框架 L 0 L 0 L_(0)L_{0} 的位置。由於連桿 k k kk 圍繞其質心的角速度所產生的動能在方程式 (6-2-1) 中以框架 L 0 L 0 L_(0)L_{0} 的坐標表示。要將這個能量重新表達為框架 L k L k L_(k)L_{k} 的坐標，我們可以使用 R k 0 = ( R 0 k ) − 1 = ( R 0 k ) T R k 0 = R 0 k − 1 = R 0 k T R_(k)^(0)=(R_(0)^(k))^(-1)=(R_(0)^(k))^(T)R_{k}^{0}=\left(R_{0}^{k}\right)^{-1}=\left(R_{0}^{k}\right)^{T} 將角速度 ω ¯ k ω ¯ k bar(omega)^(k)\bar{\omega}^{k} 從框架 L 0 L 0 L_(0)L_{0} 的坐標轉換過來。
to frame L k L k L_(k)L_{k} coordinates. This yields the following equivalent expression for the kinetic energy of link k k kk due to its angular velocity about its center of mass:
以框架 L k L k L_(k)L_{k} 坐標。這產生了以下等效表達式，用於鏈接 k k kk 由於其相對於質心的角速度而產生的動能：
( ω ¯ k ) T D k ω ¯ k 2 = ( R k 0 ω ¯ k ) T D ¯ k ( R k 0 ω ¯ k ) 2 = ( ω ¯ k ) T ( R k 0 ) T D ¯ k R k 0 ω ¯ k 2 = ( ω ¯ k ) T R 0 k D ¯ k ( R 0 k ) T ω ¯ k 2 ω ¯ k T D k ω ¯ k 2 = R k 0 ω ¯ k T D ¯ k R k 0 ω ¯ k 2 = ω ¯ k T R k 0 T D ¯ k R k 0 ω ¯ k 2 = ω ¯ k T R 0 k D ¯ k R 0 k T ω ¯ k 2 {:[(( bar(omega)^(k))^(T)D_(k) bar(omega)^(k))/(2)=((R_(k)^(0) bar(omega)^(k))^(T) bar(D)_(k)(R_(k)^(0) bar(omega)^(k)))/(2)],[=(( bar(omega)^(k))^(T)(R_(k)^(0))^(T) bar(D)_(k)R_(k)^(0) bar(omega)^(k))/(2)],[=(( bar(omega)^(k))^(T)R_(0)^(k) bar(D)_(k)(R_(0)^(k))^(T) bar(omega)^(k))/(2)]:}\begin{aligned} \frac{\left(\bar{\omega}^{k}\right)^{T} D_{k} \bar{\omega}^{k}}{2} & =\frac{\left(R_{k}^{0} \bar{\omega}^{k}\right)^{T} \bar{D}_{k}\left(R_{k}^{0} \bar{\omega}^{k}\right)}{2} \\ & =\frac{\left(\bar{\omega}^{k}\right)^{T}\left(R_{k}^{0}\right)^{T} \bar{D}_{k} R_{k}^{0} \bar{\omega}^{k}}{2} \\ & =\frac{\left(\bar{\omega}^{k}\right)^{T} R_{0}^{k} \bar{D}_{k}\left(R_{0}^{k}\right)^{T} \bar{\omega}^{k}}{2} \end{aligned}
Here D ¯ k D ¯ k bar(D)_(k)\bar{D}_{k} denotes the inertia tensor of link k k kk about its center of mass expressed with respect to coordinate frame L k L k L_(k)L_{k}. That is, D ¯ k D ¯ k bar(D)_(k)\bar{D}_{k} is the inertia tensor obtained by translating frame L k L k L_(k)L_{k} to the center of mass of link k k kk and then applying Eq. (6-2-2). Since L k L k L_(k)L_{k} is attached to link k k kk and rotates with it, the inertia tensor D ¯ k D ¯ k bar(D)_(k)\bar{D}_{k} is constant. The left side of Eq. (6-2-4) represents the kinetic energy due to angular velocity in base frame coordinates, while the right side of Eq. (6-2-4) represents the same energy term expressed in frame L k L k L_(k)L_{k} coordinates. Since Eq. (6-2-4) must hold for all values of angular velocity ω ¯ k ω ¯ k bar(omega)^(k)\bar{\omega}^{k}, it follows that:
這裡 D ¯ k D ¯ k bar(D)_(k)\bar{D}_{k} 表示連桿 k k kk 相對於坐標系 L k L k L_(k)L_{k} 的質量中心的慣性張量。也就是說， D ¯ k D ¯ k bar(D)_(k)\bar{D}_{k} 是通過將坐標系 L k L k L_(k)L_{k} 移動到連桿 k k kk 的質量中心，然後應用公式 (6-2-2) 獲得的慣性張量。由於 L k L k L_(k)L_{k} 附著在連桿 k k kk 上並隨其旋轉，慣性張量 D ¯ k D ¯ k bar(D)_(k)\bar{D}_{k} 是常數。公式 (6-2-4) 的左側表示基座坐標系中由角速度引起的動能，而公式 (6-2-4) 的右側則表示在坐標系 L k L k L_(k)L_{k} 中表達的相同能量項。由於公式 (6-2-4) 必須對所有角速度 ω ¯ k ω ¯ k bar(omega)^(k)\bar{\omega}^{k} 的值成立，因此：
D k ( q ) = R 0 k ( q ) D ¯ k [ R 0 k ( q ) ] T D k ( q ) = R 0 k ( q ) D ¯ k R 0 k ( q ) T D_(k)(q)=R_(0)^(k)(q) bar(D)_(k)[R_(0)^(k)(q)]^(T)D_{k}(q)=R_{0}^{k}(q) \bar{D}_{k}\left[R_{0}^{k}(q)\right]^{T}
Thus the dependence of D k D k D_(k)D_{k} on q q qq arises through the rotation matrix R 0 k ( q ) R 0 k ( q ) R_(0)^(k)(q)R_{0}^{k}(q) in Eq. (6-2-3). Note that in Eq. (6-2-5), ( R 0 k ) T = R k 0 R 0 k T = R k 0 (R_(0)^(k))^(T)=R_(k)^(0)\left(R_{0}^{k}\right)^{T}=R_{k}^{0} transforms from frame L 0 L 0 L_(0)L_{0} to frame L k L k L_(k)L_{k} and then R 0 k R 0 k R_(0)^(k)R_{0}^{k} transforms from frame L k L k L_(k)L_{k} back to frame L 0 L 0 L_(0)L_{0}.
因此， D k D k D_(k)D_{k} 對 q q qq 的依賴是通過方程(6-2-3)中的旋轉矩陣 R 0 k ( q ) R 0 k ( q ) R_(0)^(k)(q)R_{0}^{k}(q) 產生的。請注意，在方程(6-2-5)中， ( R 0 k ) T = R k 0 R 0 k T = R k 0 (R_(0)^(k))^(T)=R_(k)^(0)\left(R_{0}^{k}\right)^{T}=R_{k}^{0} 從框架 L 0 L 0 L_(0)L_{0} 轉換到框架 L k L k L_(k)L_{k} ，然後 R 0 k R 0 k R_(0)^(k)R_{0}^{k} 從框架 L k L k L_(k)L_{k} 轉換回框架 L 0 L 0 L_(0)L_{0} 。
To compute the link inertia tensor D k D k D_(k)D_{k}, we first compute the inertia tensor with respect to a coordinate frame obtained by translating frame L k L k L_(k)L_{k} from the end of link k k kk to the center of mass of link k k kk. If the D-H algorithm is applied with some care, frame L k L k L_(k)L_{k} can often be chosen in such a way that D ¯ k D ¯ k bar(D)_(k)\bar{D}_{k} is diagonal. Whether it is diagonal or not, it is constant, and once this constant matrix D ¯ k D ¯ k bar(D)_(k)\bar{D}_{k} is obtained, the k k kk th-link inertia tensor with respect to the base frame can then be obtained from it using Eq. (6-2-5).
要計算連桿慣性張量 D k D k D_(k)D_{k} ，我們首先計算相對於一個坐標框架的慣性張量，該框架是通過將框架 L k L k L_(k)L_{k} 從連桿 k k kk 的末端平移到連桿 k k kk 的質心來獲得的。如果小衛星-哈根算法應用得當，框架 L k L k L_(k)L_{k} 通常可以選擇成使得 D ¯ k D ¯ k bar(D)_(k)\bar{D}_{k} 是對角的。無論它是否是對角的，它都是常數，一旦獲得這個常數矩陣 D ¯ k D ¯ k bar(D)_(k)\bar{D}_{k} ，則可以使用公式 (6-2-5) 從中獲得相對於基準框架的 k k kk 條連桿慣性張量。
To develop an explicit formulation of the total kinetic energy of the arm, we must also express the velocities v ¯ k v ¯ k bar(v)^(k)\bar{v}^{k} and ω ¯ k ω ¯ k bar(omega)^(k)\bar{\omega}^{k} in Eq. (6-2-1) in terms of q q qq and q ˙ q ˙ q^(˙)\dot{q}. Fortunately, much of the work for doing this is already in place. Recall from the static analysis in Chap. 5 that the manipulator Jacobian matrix J ( q ) J ( q ) J(q)J(q) relates infinitesimal displacements of the joint variables to infinitesimal linear and angular displacements of the tool. If we divide both sides of Eq. (5-7-2) by d t d t dtd t and recall Eq. (5-7-1), we see that:
為了發展手臂總動能的明確公式，我們還必須將方程式 (6-2-1) 中的速度 v ¯ k v ¯ k bar(v)^(k)\bar{v}^{k} 和 ω ¯ k ω ¯ k bar(omega)^(k)\bar{\omega}^{k} 表達為 q q qq 和 q ˙ q ˙ q^(˙)\dot{q} 。幸運的是，這方面的工作已經有很多基礎。回想一下第 5 章的靜態分析，操控器雅可比矩陣 J ( q ) J ( q ) J(q)J(q) 將關節變數的無窮小位移與工具的無窮小線性和角位移相關聯。如果我們將方程式 (5-7-2) 的兩邊除以 d t d t dtd t 並回顧方程式 (5-7-1)，我們可以看到：
[ p ˙ ϕ ˙ ] = J ( q ) q ˙ p ˙ ϕ ˙ = J ( q ) q ˙ [[p^(˙)],[phi^(˙)]]=J(q)q^(˙)\left[\begin{array}{c} \dot{p} \\ \dot{\phi} \end{array}\right]=J(q) \dot{q}
Thus J ( q ) J ( q ) J(q)J(q) maps instantaneous joint velocity into instantaneous linear and angular tool velocity. To express v ¯ k v ¯ k bar(v)^(k)\bar{v}^{k} and ω ¯ k ω ¯ k bar(omega)^(k)\bar{\omega}^{k} in terms of q q qq and q ˙ q ˙ q^(˙)\dot{q}, we need to find an analogous link Jacobian matrix, J k ( q ) J k ( q ) J^(k)(q)J^{k}(q). We can reformulate J ( q ) J ( q ) J(q)J(q) as J k ( q ) J k ( q ) J^(k)(q)J^{k}(q) by regarding the center
因此 J ( q ) J ( q ) J(q)J(q) 將瞬時關節速度映射為瞬時線性和角度工具速度。要將 v ¯ k v ¯ k bar(v)^(k)\bar{v}^{k} 和 ω ¯ k ω ¯ k bar(omega)^(k)\bar{\omega}^{k} 表達為 q q qq 和 q ˙ q ˙ q^(˙)\dot{q} ，我們需要找到一個類似的鏈接雅可比矩陣 J k ( q ) J k ( q ) J^(k)(q)J^{k}(q) 。我們可以通過將中心視為 J ( q ) J ( q ) J(q)J(q) 重新表述為 J k ( q ) J k ( q ) J^(k)(q)J^{k}(q) 。
of mass of link k k kk as if it were the tool tip. Once J k ( q ) J k ( q ) J^(k)(q)J^{k}(q) is determined in this manner, then:
將鏈接 k k kk 的質量視為工具提示。一旦以這種方式確定了 J k ( q ) J k ( q ) J^(k)(q)J^{k}(q) ，則：
[ v ¯ k ω ¯ k ] = J k ( q ) q ˙ 1 ≤ k ≤ n v ¯ k ω ¯ k = J k ( q ) q ˙ 1 ≤ k ≤ n [[ bar(v)^(k)],[ bar(omega)^(k)]]=J^(k)(q)q^(˙)quad1 <= k <= n\left[\begin{array}{c} \bar{v}^{k} \\ \bar{\omega}^{k} \end{array}\right]=J^{k}(q) \dot{q} \quad 1 \leq k \leq n
Let c ¯ k c ¯ k bar(c)^(k)\bar{c}^{k} denote the center of mass of link k k kk in base coordinates as shown in Fig. 6-1. Recall that the D-H algorithm assigns frame L k = { x k , y k , z k } L k = x k , y k , z k L_(k)={x^(k),y^(k),z^(k)}L_{k}=\left\{x^{k}, y^{k}, z^{k}\right\} to the end of link k k kk. Next let Δ c k Δ c k Deltac^(k)\Delta c^{k} be the homogeneous coordinates of the center of mass of link k k kk expressed with respect to frame L k L k L_(k)L_{k} rather than frame L 0 L 0 L_(0)L_{0}. That is:
讓 c ¯ k c ¯ k bar(c)^(k)\bar{c}^{k} 表示如圖 6-1 所示的基座坐標中連桿 k k kk 的質心。回想一下，D-H 算法將框架 L k = { x k , y k , z k } L k = x k , y k , z k L_(k)={x^(k),y^(k),z^(k)}L_{k}=\left\{x^{k}, y^{k}, z^{k}\right\} 指派給連桿 k k kk 的末端。接下來，讓 Δ c k Δ c k Deltac^(k)\Delta c^{k} 成為相對於框架 L k L k L_(k)L_{k} 而非框架 L 0 L 0 L_(0)L_{0} 表示的連桿 k k kk 的質心的齊次坐標。也就是說：
Δ c k = [ c ¯ k ] k , 1 ≤ k ≤ n Δ c k = c ¯ k k , 1 ≤ k ≤ n Deltac^(k)=[ bar(c)^(k)]^(k),quad1 <= k <= n\Delta c^{k}=\left[\bar{c}^{k}\right]^{k}, \quad 1 \leq k \leq n
Since L k L k L_(k)L_{k} is attached to the end of link k k kk, it follows that Δ c k Δ c k Deltac^(k)\Delta c^{k} is a constant displacement vector that depends on the physical size and shape of link k k kk. The position of the center of mass of link k k kk can then be expressed in base coordinates by transforming from frame L k L k L_(k)L_{k} coordinates to frame L 0 L 0 L_(0)L_{0} coordinates using the composite homogeneous coordinate transformation matrix:
由於 L k L k L_(k)L_{k} 附加在連結 k k kk 的末端，因此 Δ c k Δ c k Deltac^(k)\Delta c^{k} 是一個依賴於連結 k k kk 的物理大小和形狀的恆定位移向量。連結 k k kk 的質心位置可以通過使用復合齊次坐標變換矩陣，將從框架 L k L k L_(k)L_{k} 坐標轉換到框架 L 0 L 0 L_(0)L_{0} 坐標來表示。
c ¯ k ( q ) = H 1 T 0 k ( q ) Δ c k 1 ≤ k ≤ n c ¯ k ( q ) = H 1 T 0 k ( q ) Δ c k 1 ≤ k ≤ n bar(c)^(k)(q)=H_(1)T_(0)^(k)(q)Deltac^(k)quad1 <= k <= n\bar{c}^{k}(q)=H_{1} T_{0}^{k}(q) \Delta c^{k} \quad 1 \leq k \leq n
Here we have used the 3 × 4 3 × 4 3xx43 \times 4 homogeneous coordinate conversion matrix H 1 H 1 H_(1)H_{1} defined in Eq. (2-4-1) to convert from four-dimensional homogeneous coordinates to threedimensional physical coordinates. Once the location of the center of mass of link k k kk is known, the Jacobian matrix with respect to this point of reference can then be formulated in a manner similar to the method used in Chap. 5 where the point of reference was the tool tip. In this case the Jacobian matrix for link k k kk is the following 6 × n 6 × n 6xx n6 \times n matrix:
在這裡，我們使用了在方程式 (2-4-1) 中定義的 3 × 4 3 × 4 3xx43 \times 4 齊次坐標轉換矩陣 H 1 H 1 H_(1)H_{1} ，將四維齊次坐標轉換為三維物理坐標。一旦知道連桿 k k kk 的質心位置，就可以以類似於第 5 章中使用的方法來構建相對於此參考點的雅可比矩陣，當時的參考點是工具尖端。在這種情況下，連桿 k k kk 的雅可比矩陣為以下 6 × n 6 × n 6xx n6 \times n 矩陣：
J k ( q ) = [ ∂ c ¯ k ∂ q 1 ⋯ ∂ c ¯ k ∂ q k 0 ξ 1 z 0 ⋯ ξ k z k − 1 0 ] ≜ [ A k ( q ) B k ( q ) ] J k ( q ) = ∂ c ¯ k ∂ q 1 ⋯ ∂ c ¯ k ∂ q k 0 ξ 1 z 0 ⋯ ξ k z k − 1 0 ≜ A k ( q ) B k ( q ) J^(k)(q)=[[(del bar(c)^(k))/(delq_(1)),cdots,(del bar(c)^(k))/(delq_(k)),0],[xi_(1)z^(0),cdots,xi_(k)z^(k-1),0]]≜[[A^(k)(q)],[B^(k)(q)]]J^{k}(q)=\left[\begin{array}{ccc:c} \frac{\partial \bar{c}^{k}}{\partial q_{1}} & \cdots & \frac{\partial \bar{c}^{k}}{\partial q_{k}} & 0 \\ \hdashline \xi_{1} z^{0} & \cdots & \xi_{k} z^{k-1} & 0 \end{array}\right] \triangleq\left[\begin{array}{c} A^{k}(q) \\ \hdashline B^{k}(q) \end{array}\right]
Recall that the scalar ξ i ξ i xi_(i)\xi_{i} is a joint type parameter that takes on a value of 1 if joint i i ii is revolute or 0 if joint i i ii is prismatic. The vector z i z i z^(i)z^{i} represents the third unit vector of frame L i L i L_(i)L_{i} with respect to the base frame. Therefore:
請記住，標量 ξ i ξ i xi_(i)\xi_{i} 是一個聯合類型參數，如果關節 i i ii 是旋轉的，則取值為 1；如果關節 i i ii 是滑動的，則取值為 0。向量 z i z i z^(i)z^{i} 代表框架 L i L i L_(i)L_{i} 相對於基準框架的第三個單位向量。因此：
z i ( q ) = R 0 i ( q ) i 3 0 ≤ i ≤ n z i ( q ) = R 0 i ( q ) i 3 0 ≤ i ≤ n z^(i)(q)=R_(0)^(i)(q)i^(3)quad0 <= i <= nz^{i}(q)=R_{0}^{i}(q) i^{3} \quad 0 \leq i \leq n
Note that the last n − k n − k n-kn-k columns of J k ( q ) J k ( q ) J^(k)(q)J^{k}(q) are zero. This is because the motion of link k k kk is not affected by any of the distal joint variables { q k + 1 , … , q n } q k + 1 , … , q n {q_(k+1),dots,q_(n)}\left\{q_{k+1}, \ldots, q_{n}\right\}.
請注意， J k ( q ) J k ( q ) J^(k)(q)J^{k}(q) 的最後 n − k n − k n-kn-k 列為零。這是因為連接 k k kk 的運動不受任何遠端關節變數 { q k + 1 , … , q n } q k + 1 , … , q n {q_(k+1),dots,q_(n)}\left\{q_{k+1}, \ldots, q_{n}\right\} 的影響。

6-2-3 Manipulator Inertia Tensor
6-2-3 操縱器慣性張量

An expression for the total kinetic energy of the arm can now be written explicitly in terms of q q qq and q ˙ q ˙ q^(˙)\dot{q}. First note from Eq. ( 6 − 2 − 10 ) ( 6 − 2 − 10 ) (6-2-10)(6-2-10) that if we use the partitioned submatrices A k A k A^(k)A^{k} and B k B k B^(k)B^{k}, then Eq. (6-2-7) can be decomposed into two separate equations:
手臂的總動能的表達式現在可以明確地用 q q qq 和 q ˙ q ˙ q^(˙)\dot{q} 來寫。首先從方程式 ( 6 − 2 − 10 ) ( 6 − 2 − 10 ) (6-2-10)(6-2-10) 注意到，如果我們使用分區的子矩陣 A k A k A^(k)A^{k} 和 B k B k B^(k)B^{k} ，那麼方程式(6-2-7)可以分解成兩個獨立的方程式：
v ¯ k ( q , q ˙ ) = A k ( q ) q ˙ 1 ≤ k ≤ n v ¯ k ( q , q ˙ ) = A k ( q ) q ˙ 1 ≤ k ≤ n bar(v)^(k)(q,q^(˙))=A^(k)(q)q^(˙)quad1 <= k <= n\bar{v}^{k}(q, \dot{q})=A^{k}(q) \dot{q} \quad 1 \leq k \leq n
ω ¯ k ( q , q ˙ ) = B k ( q ) q ˙ 1 ≤ k ≤ n ω ¯ k ( q , q ˙ ) = B k ( q ) q ˙ 1 ≤ k ≤ n bar(omega)^(k)(q,q^(˙))=B^(k)(q)q^(˙)quad1 <= k <= n\bar{\omega}^{k}(q, \dot{q})=B^{k}(q) \dot{q} \quad 1 \leq k \leq n
The first equation specifies the linear velocity of the center of mass of link k k kk, while the second specifies the angular velocity about the center of mass. If we now substitute Eqs. (6-2-12) and (6-2-13) in the original expression for the total kinetic energy in Eq. (6-2-1), the result is:
第一個方程式指定了連桿 k k kk 質心的線速度，而第二個方程式指定了圍繞質心的角速度。如果我們現在將方程式 (6-2-12) 和 (6-2-13) 代入方程式 (6-2-1) 中的總動能原始表達式，結果是：
T ( q , q ˙ ) = ∑ k = 1 n ( v ¯ k ) T m k v ¯ k + ( ω ¯ k ) T D k ω ¯ k 2 = ∑ k = 1 n ( A k q ˙ ) T m k A k q ˙ + ( B k q ˙ ) T D k B k q ˙ 2 = ∑ k = 1 n q ˙ T ( A k ) T m k A k q ˙ + q ˙ T ( B k ) T D k B k q ˙ 2 = ∑ k = 1 n q ˙ T [ ( A k ) T m k A k + ( B k ) T D k B k ] q ˙ 2 = q ˙ T ∑ k = 1 n [ ( A k ) T m k A k + ( B k ) T D k B k ] 2 q ˙ T ( q , q ˙ ) = ∑ k = 1 n   v ¯ k T m k v ¯ k + ω ¯ k T D k ω ¯ k 2 = ∑ k = 1 n   A k q ˙ T m k A k q ˙ + B k q ˙ T D k B k q ˙ 2 = ∑ k = 1 n   q ˙ T A k T m k A k q ˙ + q ˙ T B k T D k B k q ˙ 2 = ∑ k = 1 n   q ˙ T A k T m k A k + B k T D k B k q ˙ 2 = q ˙ T ∑ k = 1 n   A k T m k A k + B k T D k B k 2 q ˙ {:[T(q","q^(˙))=sum_(k=1)^(n)(( bar(v)^(k))^(T)m_(k) bar(v)^(k)+( bar(omega)^(k))^(T)D_(k) bar(omega)^(k))/(2)],[=sum_(k=1)^(n)((A^(k)(q^(˙)))^(T)m_(k)A^(k)(q^(˙))+(B^(k)(q^(˙)))^(T)D_(k)B^(k)(q^(˙)))/(2)],[=sum_(k=1)^(n)(q^(˙)^(T)(A^(k))^(T)m_(k)A^(k)(q^(˙))+q^(˙)^(T)(B^(k))^(T)D_(k)B^(k)(q^(˙)))/(2)],[=sum_(k=1)^(n)(q^(˙)^(T)[(A^(k))^(T)m_(k)A^(k)+(B^(k))^(T)D_(k)B^(k)](q^(˙)))/(2)],[=q^(˙)^(T)(sum_(k=1)^(n)[(A^(k))^(T)m_(k)A^(k)+(B^(k))^(T)D_(k)B^(k)])/(2)q^(˙)]:}\begin{aligned} T(q, \dot{q}) & =\sum_{k=1}^{n} \frac{\left(\bar{v}^{k}\right)^{T} m_{k} \bar{v}^{k}+\left(\bar{\omega}^{k}\right)^{T} D_{k} \bar{\omega}^{k}}{2} \\ & =\sum_{k=1}^{n} \frac{\left(A^{k} \dot{q}\right)^{T} m_{k} A^{k} \dot{q}+\left(B^{k} \dot{q}\right)^{T} D_{k} B^{k} \dot{q}}{2} \\ & =\sum_{k=1}^{n} \frac{\dot{q}^{T}\left(A^{k}\right)^{T} m_{k} A^{k} \dot{q}+\dot{q}^{T}\left(B^{k}\right)^{T} D_{k} B^{k} \dot{q}}{2} \\ & =\sum_{k=1}^{n} \frac{\dot{q}^{T}\left[\left(A^{k}\right)^{T} m_{k} A^{k}+\left(B^{k}\right)^{T} D_{k} B^{k}\right] \dot{q}}{2} \\ & =\dot{q}^{T} \frac{\sum_{k=1}^{n}\left[\left(A^{k}\right)^{T} m_{k} A^{k}+\left(B^{k}\right)^{T} D_{k} B^{k}\right]}{2} \dot{q} \end{aligned}
Note that the 3 × n 3 × n 3xx n3 \times n Jacobian submatrices A k A k A^(k)A^{k} and B k B k B^(k)B^{k} and the 3 × 3 3 × 3 3xx33 \times 3 link inertia tensor D k D k D_(k)D_{k} depend on q q qq. The expression for the total kinetic energy of the arm can be simplified if we introduce the following n × n n × n n xx nn \times n matrix:
請注意， 3 × n 3 × n 3xx n3 \times n 雅可比子矩陣 A k A k A^(k)A^{k} 和 B k B k B^(k)B^{k} 以及 3 × 3 3 × 3 3xx33 \times 3 連結慣性張量 D k D k D_(k)D_{k} 依賴於 q q qq 。如果我們引入以下 n × n n × n n xx nn \times n 矩陣，則手臂的總動能表達式可以簡化：
D ( q ) ≜ ∑ k = 1 n { [ A k ( q ) ] T m k A k ( q ) + [ B k ( q ) ] T D k ( q ) B k ( q ) } D ( q ) ≜ ∑ k = 1 n   A k ( q ) T m k A k ( q ) + B k ( q ) T D k ( q ) B k ( q ) D(q)≜sum_(k=1)^(n){[A^(k)(q)]^(T)m_(k)A^(k)(q)+[B^(k)(q)]^(T)D_(k)(q)B^(k)(q)}D(q) \triangleq \sum_{k=1}^{n}\left\{\left[A^{k}(q)\right]^{T} m_{k} A^{k}(q)+\left[B^{k}(q)\right]^{T} D_{k}(q) B^{k}(q)\right\}
We call D ( q ) D ( q ) D(q)D(q) the manipulator inertia tensor. Like the n n nn individual link inertia tensors, the manipulator inertia tensor is a symmetric, positive-definite matrix. Substituting Eq. (6-2-15) in Eq. (6-2-14), we see that the total kinetic energy of the arm can be expressed in terms of the manipulator inertia tensor and the joint velocity as follows:
我們稱 D ( q ) D ( q ) D(q)D(q) 為操控器慣性張量。與 n n nn 各個連桿慣性張量類似，操控器慣性張量是一個對稱的正定矩陣。將方程式 (6-2-15) 代入方程式 (6-2-14)，我們可以看到手臂的總動能可以用操控器慣性張量和關節速度來表示，如下所示：
T ( q , q ˙ ) = q ˙ T D ( q ) q ˙ 2 T ( q , q ˙ ) = q ˙ T D ( q ) q ˙ 2 T(q,q^(˙))=(q^(˙)^(T)D(q)(q^(˙)))/(2)T(q, \dot{q})=\frac{\dot{q}^{T} D(q) \dot{q}}{2}
It is clear that the kinetic energy satisfies T ( q , q ˙ ) ≥ 0 T ( q , q ˙ ) ≥ 0 T(q,q^(˙)) >= 0T(q, \dot{q}) \geq 0. Furthermore, T ( q , q ˙ ) = 0 T ( q , q ˙ ) = 0 T(q,q^(˙))=0T(q, \dot{q})=0 if and only if q ˙ = 0 q ˙ = 0 q^(˙)=0\dot{q}=0, which corresponds to the arm being motionless.
顯然，動能滿足 T ( q , q ˙ ) ≥ 0 T ( q , q ˙ ) ≥ 0 T(q,q^(˙)) >= 0T(q, \dot{q}) \geq 0 。此外， T ( q , q ˙ ) = 0 T ( q , q ˙ ) = 0 T(q,q^(˙))=0T(q, \dot{q})=0 當且僅當 q ˙ = 0 q ˙ = 0 q^(˙)=0\dot{q}=0 ，這對應於手臂靜止不動。

6-2-4 Gravity 6-2-4 重力

In order to complete the formulation of the Lagrangian function of a robotic manipulator, it remains to examine the total potential energy of the arm, U ( q ) U ( q ) U(q)U(q). The potential energy stored in the k k kk th link of the arm is the amount of work required to displace the center of mass of link k k kk from a horizontal reference plane in the presence of gravity. Let g ∈ R 3 g ∈ R 3 g inR^(3)g \in \mathbf{R}^{3} denote the gravitational acceleration vector with respect to the base frame, which is an inertial frame. Then in base frame coordinates, the work
為了完成機器人操作臂的拉格朗日函數的制定，還需要檢查手臂的總位能， U ( q ) U ( q ) U(q)U(q) 。儲存在手臂的 k k kk 號連桿中的位能是將 k k kk 號連桿的質心從水平參考平面在重力作用下移動所需的功。令 g ∈ R 3 g ∈ R 3 g inR^(3)g \in \mathbf{R}^{3} 表示相對於基準框架的重力加速度向量，該框架是一個慣性框架。然後在基準框架坐標中，功
required to displace link k k kk to position c ¯ k ( q ) c ¯ k ( q ) bar(c)^(k)(q)\bar{c}^{k}(q) is − m k g T c ¯ k ( q ) − m k g T c ¯ k ( q ) -m_(k)g^(T) bar(c)^(k)(q)-m_{k} g^{T} \bar{c}^{k}(q). Hence the total potential energy stored in the arm is:
需要將連結 k k kk 移動到位置 c ¯ k ( q ) c ¯ k ( q ) bar(c)^(k)(q)\bar{c}^{k}(q) 的力量是 − m k g T c ¯ k ( q ) − m k g T c ¯ k ( q ) -m_(k)g^(T) bar(c)^(k)(q)-m_{k} g^{T} \bar{c}^{k}(q) 。因此，手臂中儲存的總潛在能量為：
U ( q ) = − ∑ k = 1 n m k g r c ¯ k ( q ) U ( q ) = − ∑ k = 1 n   m k g r c ¯ k ( q ) U(q)=-sum_(k=1)^(n)m_(k)g^(r) bar(c)^(k)(q)U(q)=-\sum_{k=1}^{n} m_{k} g^{r} \bar{c}^{k}(q)
If the robot is upright and located near the surface of the earth, then the gravitational acceleration vector is proportional to − i 3 − i 3 -i^(3)-i^{3}, where the constant of proportionality is:
如果機器人是直立的並且位於地球表面附近，那麼重力加速度向量與 − i 3 − i 3 -i^(3)-i^{3} 成正比，其中比例常數為：
g 0 = ‖ g ‖ = 9.8062 m / sec 2 g 0 = ‖ g ‖ = 9.8062 m / sec 2 g_(0)=||g||=9.8062m//sec^(2)g_{0}=\|g\|=9.8062 \mathrm{~m} / \mathrm{sec}^{2}
As was the case with the kinetic energy of the arm, we can simplify the final expression for the potential energy by introducing a new quantity defined as follows:
與手臂的動能情況相同，我們可以通過引入一個新量來簡化位能的最終表達式，該量定義如下：
c ¯ ( q ) ≜ ∑ k = 1 n m k c ¯ k ( q ) c ¯ ( q ) ≜ ∑ k = 1 n   m k c ¯ k ( q ) bar(c)(q)≜sum_(k=1)^(n)m_(k) bar(c)^(k)(q)\bar{c}(q) \triangleq \sum_{k=1}^{n} m_{k} \bar{c}^{k}(q)
Thus c ¯ ( q ) c ¯ ( q ) bar(c)(q)\bar{c}(q) is a weighted sum of the centers of mass of the n n nn links, with the weighting coefficients being the link masses. From Eq. (6-2-17) we see that the coefficient g T g T g^(T)g^{T} can be factored out on the left. Thus, combining Eqs. ( 6 − 2 − 17 ) ( 6 − 2 − 17 ) (6-2-17)(6-2-17) and ( 6 − 2 − 19 ) ( 6 − 2 − 19 ) (6-2-19)(6-2-19), the total potential energy of the arm can be expressed in terms of the weighted sum c ¯ ( q ) c ¯ ( q ) bar(c)(q)\bar{c}(q) as:
因此 c ¯ ( q ) c ¯ ( q ) bar(c)(q)\bar{c}(q) 是 n n nn 鏈的質量中心的加權總和，加權係數為鏈的質量。從方程式 (6-2-17) 我們可以看到係數 g T g T g^(T)g^{T} 可以在左側提取出來。因此，結合方程式 ( 6 − 2 − 17 ) ( 6 − 2 − 17 ) (6-2-17)(6-2-17) 和 ( 6 − 2 − 19 ) ( 6 − 2 − 19 ) (6-2-19)(6-2-19) ，手臂的總位能可以用加權總和 c ¯ ( q ) c ¯ ( q ) bar(c)(q)\bar{c}(q) 表示為：
U ( q ) = − g T c ¯ ( q ) U ( q ) = − g T c ¯ ( q ) U(q)=-g^(T) bar(c)(q)U(q)=-g^{T} \bar{c}(q)
Note that U ( q ) U ( q ) U(q)U(q) is maximum when all links of the arm are directly opposing g g gg, zero when all links are orthogonal to g g gg, and minimum when all links are aligned with g g gg. A constant could be added to the expression for U ( q ) U ( q ) U(q)U(q) to make the minimum potential energy zero. However, this is not really necessary, because from Eq. (6-1-2) we see that it is only the derivative of U ( q ) U ( q ) U(q)U(q) that is important.
注意，當手臂的所有連接直接對抗 g g gg 時， U ( q ) U ( q ) U(q)U(q) 達到最大值；當所有連接與 g g gg 正交時，為零；當所有連接與 g g gg 對齊時，達到最小值。可以在 U ( q ) U ( q ) U(q)U(q) 的表達式中添加一個常數，使最小潛能能量為零。然而，這並不是必需的，因為從方程（6-1-2）中我們看到，只有 U ( q ) U ( q ) U(q)U(q) 的導數是重要的。
Simplified expressions for the kinetic and potential energy of the arm are now available, and we can take their difference to produce the Lagrangian function of a robotic arm. Combining Eqs. (6-1-1), (6-2-16), and (6-2-20), the manipulator Lagrangian function is:
機械臂的動能和位能的簡化表達式現在已經可用，我們可以取它們的差來產生機器臂的拉格朗日函數。結合方程式 (6-1-1)、(6-2-16) 和 (6-2-20)，操作器的拉格朗日函數為：
L ( q , q ˙ ) = q ˙ T D ( q ) q ˙ 2 + g T c ¯ ( q ) L ( q , q ˙ ) = q ˙ T D ( q ) q ˙ 2 + g T c ¯ ( q ) L(q,q^(˙))=(q^(˙)^(T)D(q)(q^(˙)))/(2)+g^(T) bar(c)(q)L(q, \dot{q})=\frac{\dot{q}^{T} D(q) \dot{q}}{2}+g^{T} \bar{c}(q)
It remains to take derivatives of L ( q ˙ , q ˙ ) L ( q ˙ , q ˙ ) L(q^(˙),q^(˙))L(\dot{q}, \dot{q}) with respect to q , q ˙ q , q ˙ q,q^(˙)q, \dot{q}, and t t tt to formulate the dynamic equations of motion of the arm. However, before doing this, we first develop a more detailed formulation of the term appearing on the right-hand side of Lagrange’s equation, the generalized force.
仍然需要對 L ( q ˙ , q ˙ ) L ( q ˙ , q ˙ ) L(q^(˙),q^(˙))L(\dot{q}, \dot{q}) 對 q , q ˙ q , q ˙ q,q^(˙)q, \dot{q} 和 t t tt 進行求導，以制定手臂的動態運動方程。然而，在此之前，我們首先對拉格朗日方程右側出現的項，即廣義力，進行更詳細的公式化。

6-3 GENERALIZED FORCE 6-3 一般化力

Generalized forces are the residual forces acting on the arm once the forces associated with kinetic energy (inertial forces) and potential energy (gravitational forces) have been removed. Generalized forces can be defined by examining the work
廣義力是指在去除與動能（慣性力）和位能（重力）相關的力後，作用於臂部的殘餘力。廣義力可以通過檢查功來定義。
produced by a virtual displacement of the joints. A virtual displacement is an instantaneous infinitesimal displacement δ q δ q delta q\delta q. A virtual displacement must satisfy the geometric constraints imposed by the linkages of the arm, but it does not have to satisfy temporal constraints, because it is assumed to occur without passage of time. The work produced by a virtual displacement, virtual work, is denoted δ W δ W delta W\delta W. The force that relates a virtual displacement to the virtual work it produces is generalized force. Thus the generalized force F F FF, is an n × 1 n × 1 n xx1n \times 1 vector defined implicitly by the following equation:
由關節的虛位移產生。虛位移是一個瞬時的無窮小位移 δ q δ q delta q\delta q 。虛位移必須滿足臂部連桿施加的幾何約束，但不必滿足時間約束，因為假設它在沒有時間流逝的情況下發生。虛位移所產生的功稱為虛功，記作 δ W δ W delta W\delta W 。將虛位移與其產生的虛功相關聯的力稱為廣義力。因此，廣義力 F F FF 是一個 n × 1 n × 1 n xx1n \times 1 向量，通過以下方程式隱式定義：
δ W = F T δ q δ W = F T δ q delta W=F^(T)delta q\delta W=F^{T} \delta q
Note that, depending on the types of joints, the components of the generalized force vector will be either forces (for prismatic joints) or torques (for revolute joints). We investigate two contributions to the generalized force.
請注意，根據關節的類型，廣義力向量的組成部分將是力（對於滑動關節）或扭矩（對於旋轉關節）。我們研究廣義力的兩個貢獻。

6-3-1 Actuators 6-3-1 執行器

Each joint is driven or powered by an actuator such as an electric motor or a hydraulic or pneumatic cylinder. Let τ ∈ R n τ ∈ R n tau inR^(n)\tau \in \mathbf{R}^{n} denote the joint torque vector generated by the n n nn actuators. For convenience, τ τ tau\tau is referred to as a “torque,” but it is understood that τ k τ k tau_(k)\tau_{k} represents a torque if joint k k kk is revolute and a force if joint k k kk is prismatic. Note that τ k τ k tau_(k)\tau_{k} denotes the torque or force actually delivered to axis k k kk. Therefore the effects of actuator dynamics, gears, and mechanical power transmission devices are implicit in τ τ tau\tau. The virtual work produced by the actuator torque is:
每個關節由驅動器驅動或供電，例如電動馬達或液壓或氣壓缸。讓 τ ∈ R n τ ∈ R n tau inR^(n)\tau \in \mathbf{R}^{n} 表示由 n n nn 驅動器產生的關節扭矩向量。為了方便起見， τ τ tau\tau 被稱為“扭矩”，但可以理解的是，如果關節 k k kk 是旋轉的，則 τ k τ k tau_(k)\tau_{k} 代表扭矩；如果關節 k k kk 是滑動的，則 τ k τ k tau_(k)\tau_{k} 代表力。請注意， τ k τ k tau_(k)\tau_{k} 表示實際傳遞到軸 k k kk 的扭矩或力。因此，驅動器動力學、齒輪和機械功率傳輸裝置的影響隱含在 τ τ tau\tau 中。驅動器扭矩產生的虛擬功是：
δ W 1 = τ T δ q δ W 1 = τ T δ q deltaW_(1)=tau^(T)delta q\delta W_{1}=\tau^{T} \delta q
Comparing Eq. (6-3-2) with Eq. (6-3-1), we see that F = τ F = τ F=tauF=\tau. Thus the generalized force is simply the actuator torque in this case. However, there is a second source of generalized force to consider.
將公式 (6-3-2) 與公式 (6-3-1) 進行比較，我們看到 F = τ F = τ F=tauF=\tau 。因此，在這種情況下，廣義力僅僅是執行器的扭矩。然而，還有第二個廣義力的來源需要考慮。

6-3-2 Friction 6-3-2 摩擦

The effects of friction can also be modeled as a generalized force applied to the joints of the arm. Friction is a complex nonlinear force that is difficult to model accurately, yet in many cases it can have a significant effect on robot arm dynamics. We examine three distinct components of friction using the followng frictional force model for joint k k kk :
摩擦的影響也可以被建模為施加在手臂關節上的一種廣義力。摩擦是一種複雜的非線性力，難以準確建模，但在許多情況下，它對機器手臂的動力學可以產生重大影響。我們使用以下關節 k k kk 的摩擦力模型來檢查摩擦的三個不同組成部分：
b k ( q ˙ ) = b k v q ˙ k + sgn ( q ˙ k ) [ b k d + ( b k s − b k d ) exp ⁡ − | q ˙ k | ϵ ] 1 ≤ k ≤ n b k ( q ˙ ) = b k v q ˙ k + sgn q ˙ k b k d + b k s − b k d exp ⁡ − q ˙ k ϵ 1 ≤ k ≤ n b_(k)(q^(˙))=b_(k)^(v)q^(˙)_(k)+sgn(q^(˙)_(k))[b_(k)^(d)+(b_(k)^(s)-b_(k)^(d))exp((-|q^(˙)_(k)|)/(epsilon))]quad1 <= k <= nb_{k}(\dot{q})=b_{k}^{\mathrm{v}} \dot{q}_{k}+\operatorname{sgn}\left(\dot{q}_{k}\right)\left[b_{k}^{d}+\left(b_{k}^{s}-b_{k}^{d}\right) \exp \frac{-\left|\dot{q}_{k}\right|}{\epsilon}\right] \quad 1 \leq k \leq n
Here sgn denotes the signum or sign function. The first term in Eq. (6-3-3) represents viscous friction where b k v b k v b_(k)^(v)b_{k}^{v} is the coefficient of viscous friction for joint k k kk. The second term in Eq. (6-3-3) represents dynamic friction where b k d b k d b_(k)^(d)b_{k}^{d} is the coefficient of dynamic friction for joint k k kk. Finally, the third term in Eq. (6-3-3) represents static friction or Coulomb friction where b k s b k s b_(k)^(s)b_{k}^{s} is the coefficient of static friction for joint k k kk and ϵ ϵ epsilon\epsilon is a small positive parameter. Observe from Eq. (6-3-3) that as the velocity of joint k k kk approaches zero, the frictional force approaches ± b k s ± b k s +-b_(k)^(s)\pm b_{k}^{s}. Consequently, b k s b k s b_(k)^(s)b_{k}^{s} can
這裡的 sgn 表示符號函數。方程式(6-3-3)中的第一項表示粘性摩擦，其中 b k v b k v b_(k)^(v)b_{k}^{v} 是關節 k k kk 的粘性摩擦係數。方程式(6-3-3)中的第二項表示動態摩擦，其中 b k d b k d b_(k)^(d)b_{k}^{d} 是關節 k k kk 的動態摩擦係數。最後，方程式(6-3-3)中的第三項表示靜態摩擦或庫倫摩擦，其中 b k s b k s b_(k)^(s)b_{k}^{s} 是關節 k k kk 的靜態摩擦係數， ϵ ϵ epsilon\epsilon 是一個小的正參數。從方程式(6-3-3)中觀察到，當關節 k k kk 的速度接近零時，摩擦力接近 ± b k s ± b k s +-b_(k)^(s)\pm b_{k}^{s} 。因此， b k s b k s b_(k)^(s)b_{k}^{s} 可以
be interpreted as the torque or force required to overcome friction when motion of joint k k kk is initiated. It is clear that viscous friction is a linear function of q ˙ k q ˙ k q^(˙)_(k)\dot{q}_{k} whereas dynamic and static friction are discontinuous nonlinear functions. The model of frictional forces acting on the k k kk th joint is summarized in graphical form in Fig. 6-3.
可解釋為啟動關節 k k kk 時克服摩擦所需的扭矩或力量。顯然，粘性摩擦是 q ˙ k q ˙ k q^(˙)_(k)\dot{q}_{k} 的線性函數，而動態和靜態摩擦則是不連續的非線性函數。作用於 k k kk 號關節的摩擦力模型以圖形形式總結在圖 6-3 中。
Figure 6.3 Friction model for joint k k kk.
圖 6.3 關節 k k kk 的摩擦模型。
For a well-designed robotic arm, particularly a direct-drive arm, the coefficients of friction will be small. Note that Eq. (6-3-3) is only a rough model of the frictional forces. A more comprehensive representation of friction might employ a term of the form b ( q , q ˙ ) b ( q , q ˙ ) b(q,q^(˙))b(q, \dot{q}) which depends on joint position as well as joint velocity. This would be necessary, for example, if the shafts about which the links rotate or translate were not perfectly round and uniform. The virtual work produced by the robot due to the frictional forces opposing the motion is:
對於設計良好的機器手臂，特別是直接驅動的手臂，摩擦係數會很小。請注意，方程式 (6-3-3) 只是摩擦力的粗略模型。更全面的摩擦表示可能會使用一個形式為 b ( q , q ˙ ) b ( q , q ˙ ) b(q,q^(˙))b(q, \dot{q}) 的項，該項依賴於關節位置以及關節速度。例如，如果連桿旋轉或移動的軸不是完全圓形和均勻的，這將是必要的。由於摩擦力對抗運動而產生的機器人虛擬功是：
δ W 2 = − b ( q ˙ ) T δ q δ W 2 = − b ( q ˙ ) T δ q deltaW_(2)=-b(q^(˙))^(T)delta q\delta W_{2}=-b(\dot{q})^{T} \delta q
The two contributions to the generalized force can now be combined. From Eqs. ( 6 − 3 − 2 ) ( 6 − 3 − 2 ) (6-3-2)(6-3-2) and (6-3-4), the total virtual work produced by the actuators and friction is:
現在可以將對廣義力的兩個貢獻結合起來。根據方程式 ( 6 − 3 − 2 ) ( 6 − 3 − 2 ) (6-3-2)(6-3-2) 和 (6-3-4)，執行器和摩擦產生的總虛功為：
δ W = δ W 1 + δ W 2 = [ τ − b ( q ˙ ) ] T δ q δ W = δ W 1 + δ W 2 = [ τ − b ( q ˙ ) ] T δ q delta W=deltaW_(1)+deltaW_(2)=[tau-b(q^(˙))]^(T)delta q\delta W=\delta W_{1}+\delta W_{2}=[\tau-b(\dot{q})]^{T} \delta q
If we now compare Eq. (6-3-5) with Eq. (6-3-1), we see that the expression for the generalized force applied to a robotic arm is:
如果我們現在將方程式 (6-3-5) 與方程式 (6-3-1) 進行比較，我們可以看到施加於機器手臂的廣義力的表達式為：
F = τ − b ( q ˙ ) F = τ − b ( q ˙ ) F=tau-b(q^(˙))F=\tau-b(\dot{q})
The friction term in Eq. (6-3-6) has a negative sign because frictional forces oppose the motion generated by the actuators. It is also possible to add a third term to the generalized force vector, the joint torques induced by an end-of-arm force and moment vector F tool F tool  F^("tool ")F^{\text {tool }}. However, to keep the dynamic model simple, we will assume
在方程式 (6-3-6) 中，摩擦項有一個負號，因為摩擦力會抵抗由致動器產生的運動。還可以將由末端執行器的力和力矩向量 F tool F tool  F^("tool ")F^{\text {tool }} 引起的關節扭矩添加到廣義力向量中。然而，為了保持動態模型的簡單性，我們將假設
that the arm is moving freely in its workspace and that any payload it is carrying is implicitly included in the physical description of the last link. Under these conditions, the end-of-arm force and moment vector is F tool = 0 F tool  = 0 F^("tool ")=0F^{\text {tool }}=0.
該手臂在其工作空間中自由移動，並且它所承載的任何有效載荷隱含在最後一個連接的物理描述中。在這些條件下，末端手臂的力和力矩向量為 F tool = 0 F tool  = 0 F^("tool ")=0F^{\text {tool }}=0 。

6-4 LAGRANGE-EULER DYNAMIC MODEL
6-4 拉格朗日-歐拉動態模型

Now that detailed expressions for kinetic energy, potential energy, and generalized force are available, we can apply Lagrange’s equation in Eq. (6-1-2) to develop a general dynamic model of a robotic arm. To facilitate computing the required derivatives, we expand the formula for kinetic energy in Eq. (6-2-16) and that for potential energy in Eq. (6-2-20) into sums of scalars, as follows:
現在，動能、位能和廣義力的詳細表達式已經可用，我們可以在方程式 (6-1-2) 中應用拉格朗日方程來開發機器手臂的一般動態模型。為了方便計算所需的導數，我們將方程式 (6-2-16) 中的動能公式和方程式 (6-2-20) 中的位能公式展開為標量的總和，如下所示：
T ( q , q ˙ ) = ∑ k = 1 n ∑ j = 1 n D k j ( q ) q ˙ k q ˙ j 2 U ( q ) = − ∑ k = 1 3 ∑ j = 1 n g k m j c ¯ k j ( q ) T ( q , q ˙ ) = ∑ k = 1 n   ∑ j = 1 n   D k j ( q ) q ˙ k q ˙ j 2 U ( q ) = − ∑ k = 1 3   ∑ j = 1 n   g k m j c ¯ k j ( q ) {:[T(q","q^(˙))=(sum_(k=1)^(n)sum_(j=1)^(n)D_(kj)(q)q^(˙)_(k)q^(˙)_(j))/(2)],[U(q)=-sum_(k=1)^(3)sum_(j=1)^(n)g_(k)m_(j) bar(c)_(k)^(j)(q)]:}\begin{aligned} T(q, \dot{q}) & =\frac{\sum_{k=1}^{n} \sum_{j=1}^{n} D_{k j}(q) \dot{q}_{k} \dot{q}_{j}}{2} \\ U(q) & =-\sum_{k=1}^{3} \sum_{j=1}^{n} g_{k} m_{j} \bar{c}_{k}^{j}(q) \end{aligned}
Recall from Eq. (6-1-1) that the Lagrangian is L ( q , q ˙ ) = T ( q , q ˙ ) − U ( q ) L ( q , q ˙ ) = T ( q , q ˙ ) − U ( q ) L(q,q^(˙))=T(q,q^(˙))-U(q)L(q, \dot{q})=T(q, \dot{q})-U(q). Since D ( q ) D ( q ) D(q)D(q) is symmetric, the derivative of the Lagrangian with respect to q ˙ i q ˙ i q^(˙)_(i)\dot{q}_{i} can be formulated as:
回顧公式 (6-1-1)，拉格朗日量為 L ( q , q ˙ ) = T ( q , q ˙ ) − U ( q ) L ( q , q ˙ ) = T ( q , q ˙ ) − U ( q ) L(q,q^(˙))=T(q,q^(˙))-U(q)L(q, \dot{q})=T(q, \dot{q})-U(q) 。由於 D ( q ) D ( q ) D(q)D(q) 是對稱的，拉格朗日量對 q ˙ i q ˙ i q^(˙)_(i)\dot{q}_{i} 的導數可以表述為：
∂ ∂ q ˙ i L ( q , q ˙ ) = ∂ ∂ q ˙ i T ( q , q ˙ ) = ∂ ∂ q i ∑ k = 1 n ∑ j = 1 n D k j ( q ) q ˙ k q ˙ j 2 = ∂ ∂ q i D i i ( q ) q ˙ i 2 + q ˙ i [ ∑ j ≠ i D i j ( q ) q ˙ j ] + q ˙ i [ ∑ k ≠ i D k i ( q ) q ˙ k ] 2 = 2 D i i ( q ) q ˙ i + ∑ j ≠ i D i j ( q ) q ˙ j + ∑ k ≠ i D k i ( q ) q ˙ k 2 = D i i ( q ) q ˙ i + ∑ j ≠ i D i j ( q ) q ˙ j + ∑ k ≠ i D i k ( q ) q ˙ k 2 = ∑ j = 1 n D i j ( q ) q ˙ j ∂ ∂ q ˙ i L ( q , q ˙ ) = ∂ ∂ q ˙ i T ( q , q ˙ ) = ∂ ∂ q i ∑ k = 1 n   ∑ j = 1 n   D k j ( q ) q ˙ k q ˙ j 2 = ∂ ∂ q i D i i ( q ) q ˙ i 2 + q ˙ i ∑ j ≠ i   D i j ( q ) q ˙ j + q ˙ i ∑ k ≠ i   D k i ( q ) q ˙ k 2 = 2 D i i ( q ) q ˙ i + ∑ j ≠ i   D i j ( q ) q ˙ j + ∑ k ≠ i   D k i ( q ) q ˙ k 2 = D i i ( q ) q ˙ i + ∑ j ≠ i   D i j ( q ) q ˙ j + ∑ k ≠ i   D i k ( q ) q ˙ k 2 = ∑ j = 1 n   D i j ( q ) q ˙ j {:[(del)/(delq^(˙)_(i))L(q","q^(˙))=(del)/(delq^(˙)_(i))T(q","q^(˙))],[=(del)/(delq_(i))(sum_(k=1)^(n)sum_(j=1)^(n)D_(kj)(q)q^(˙)_(k)q^(˙)_(j))/(2)],[=(del)/(delq_(i))(D_(ii)(q)q^(˙)_(i)^(2)+q^(˙)_(i)[sum_(j!=i)D_(ij)(q)q^(˙)_(j)]+q^(˙)_(i)[sum_(k!=i)D_(ki)(q)q^(˙)_(k)])/(2)],[=(2D_(ii)(q)q^(˙)_(i)+sum_(j!=i)D_(ij)(q)q^(˙)_(j)+sum_(k!=i)D_(ki)(q)q^(˙)_(k))/(2)],[=D_(ii)(q)q^(˙)_(i)+(sum_(j!=i)D_(ij)(q)q^(˙)_(j)+sum_(k!=i)D_(ik)(q)q^(˙)_(k))/(2)],[=sum_(j=1)^(n)D_(ij)(q)q^(˙)_(j)]:}\begin{aligned} \frac{\partial}{\partial \dot{q}_{i}} L(q, \dot{q}) & =\frac{\partial}{\partial \dot{q}_{i}} T(q, \dot{q}) \\ & =\frac{\partial}{\partial q_{i}} \frac{\sum_{k=1}^{n} \sum_{j=1}^{n} D_{k j}(q) \dot{q}_{k} \dot{q}_{j}}{2} \\ & =\frac{\partial}{\partial q_{i}} \frac{D_{i i}(q) \dot{q}_{i}^{2}+\dot{q}_{i}\left[\sum_{j \neq i} D_{i j}(q) \dot{q}_{j}\right]+\dot{q}_{i}\left[\sum_{k \neq i} D_{k i}(q) \dot{q}_{k}\right]}{2} \\ & =\frac{2 D_{i i}(q) \dot{q}_{i}+\sum_{j \neq i} D_{i j}(q) \dot{q}_{j}+\sum_{k \neq i} D_{k i}(q) \dot{q}_{k}}{2} \\ & =D_{i i}(q) \dot{q}_{i}+\frac{\sum_{j \neq i} D_{i j}(q) \dot{q}_{j}+\sum_{k \neq i} D_{i k}(q) \dot{q}_{k}}{2} \\ & =\sum_{j=1}^{n} D_{i j}(q) \dot{q}_{j} \end{aligned}
It then follows that the first term in Lagrange’s equation for a robotic arm is:
因此，拉格朗日方程中機器手臂的第一項是：
d d t ∂ ∂ q ˙ i L ( q , q ˙ ) = d d t ∑ j = 1 n D i j ( q ) q ˙ j = ∑ j = 1 n D i j ( q ) q ¨ j + ∑ j = 1 n ( d D i j ( q ) d t ) q ˙ j = ∑ j = 1 n D i j ( q ) q ¨ j + ∑ j = 1 n { ∑ k = 1 n [ ∂ D i j ( q ) ∂ q k ] q ˙ k } q ˙ j = ∑ j = 1 n D i j ( q ) q ¨ j + ∑ k = 1 n ∑ j = 1 n [ ∂ D i j ( q ) ∂ q k ] q ˙ k q ˙ j d d t ∂ ∂ q ˙ i L ( q , q ˙ ) = d d t ∑ j = 1 n   D i j ( q ) q ˙ j = ∑ j = 1 n   D i j ( q ) q ¨ j + ∑ j = 1 n   d D i j ( q ) d t q ˙ j = ∑ j = 1 n   D i j ( q ) q ¨ j + ∑ j = 1 n   ∑ k = 1 n   ∂ D i j ( q ) ∂ q k q ˙ k q ˙ j = ∑ j = 1 n   D i j ( q ) q ¨ j + ∑ k = 1 n   ∑ j = 1 n   ∂ D i j ( q ) ∂ q k q ˙ k q ˙ j {:[(d)/(dt)(del)/(delq^(˙)_(i))L(q","q^(˙))=(d)/(dt)sum_(j=1)^(n)D_(ij)(q)q^(˙)_(j)],[=sum_(j=1)^(n)D_(ij)(q)q^(¨)_(j)+sum_(j=1)^(n)((dD_(ij)(q))/(dt))q^(˙)_(j)],[=sum_(j=1)^(n)D_(ij)(q)q^(¨)_(j)+sum_(j=1)^(n){sum_(k=1)^(n)[(delD_(ij)(q))/(delq_(k))]q^(˙)_(k)}q^(˙)_(j)],[=sum_(j=1)^(n)D_(ij)(q)q^(¨)_(j)+sum_(k=1)^(n)sum_(j=1)^(n)[(delD_(ij)(q))/(delq_(k))]q^(˙)_(k)q^(˙)_(j)]:}\begin{aligned} \frac{d}{d t} \frac{\partial}{\partial \dot{q}_{i}} L(q, \dot{q}) & =\frac{d}{d t} \sum_{j=1}^{n} D_{i j}(q) \dot{q}_{j} \\ & =\sum_{j=1}^{n} D_{i j}(q) \ddot{q}_{j}+\sum_{j=1}^{n}\left(\frac{d D_{i j}(q)}{d t}\right) \dot{q}_{j} \\ & =\sum_{j=1}^{n} D_{i j}(q) \ddot{q}_{j}+\sum_{j=1}^{n}\left\{\sum_{k=1}^{n}\left[\frac{\partial D_{i j}(q)}{\partial q_{k}}\right] \dot{q}_{k}\right\} \dot{q}_{j} \\ & =\sum_{j=1}^{n} D_{i j}(q) \ddot{q}_{j}+\sum_{k=1}^{n} \sum_{j=1}^{n}\left[\frac{\partial D_{i j}(q)}{\partial q_{k}}\right] \dot{q}_{k} \dot{q}_{j} \end{aligned}
Next we examine the second term in Eq. (6-1-2), which is the derivative of the Lagrangian function with respect to q i q i q_(i)q_{i}. Using Eq. (6-2-12), we have:
接下來我們檢查方程式 (6-1-2) 中的第二項，即拉格朗日函數對 q i q i q_(i)q_{i} 的導數。使用方程式 (6-2-12)，我們得到：
∂ ∂ q i L ( q , q ˙ ) = ∂ ∂ q i T ( q , q ˙ ) − ∂ ∂ q i U ( q ) = ∂ ∂ q i ∑ k = 1 n ∑ j = 1 n D k j ( q ) q ˙ k q ˙ j 2 + ∂ ∂ q i ∑ k = 1 3 ∑ j = 1 n g k m j c ¯ k j ( q ) = ∑ k = 1 n ∑ j = 1 n [ ∂ D k j ( q ) / ∂ q i ] q ˙ k q ˙ j 2 + ∑ k = 1 3 ∑ j = 1 n g k m j [ ∂ c ¯ k j ( q ) ∂ q i ] = ∑ k = 1 n ∑ j = 1 n [ ∂ D k j ( q ) / ∂ q i ] q ˙ k q ˙ j 2 + ∑ k = 1 3 ∑ j = 1 n g k m j A k i j ( q ) = ∑ k = 1 n ∑ j = 1 n [ ∂ D k j ( q ) / ∂ q i ] q ˙ k q ˙ j 2 + ∑ k = 1 3 ∑ j = i n g k m j A k i j ( q ) ∂ ∂ q i L ( q , q ˙ ) = ∂ ∂ q i T ( q , q ˙ ) − ∂ ∂ q i U ( q ) = ∂ ∂ q i ∑ k = 1 n   ∑ j = 1 n   D k j ( q ) q ˙ k q ˙ j 2 + ∂ ∂ q i ∑ k = 1 3   ∑ j = 1 n   g k m j c ¯ k j ( q ) = ∑ k = 1 n   ∑ j = 1 n   ∂ D k j ( q ) / ∂ q i q ˙ k q ˙ j 2 + ∑ k = 1 3   ∑ j = 1 n   g k m j ∂ c ¯ k j ( q ) ∂ q i = ∑ k = 1 n   ∑ j = 1 n   ∂ D k j ( q ) / ∂ q i q ˙ k q ˙ j 2 + ∑ k = 1 3   ∑ j = 1 n   g k m j A k i j ( q ) = ∑ k = 1 n   ∑ j = 1 n   ∂ D k j ( q ) / ∂ q i q ˙ k q ˙ j 2 + ∑ k = 1 3   ∑ j = i n   g k m j A k i j ( q ) {:[(del)/(delq_(i))L(q","q^(˙))=(del)/(delq_(i))T(q","q^(˙))-(del)/(delq_(i))U(q)],[=(del)/(delq_(i))(sum_(k=1)^(n)sum_(j=1)^(n)D_(kj)(q)q^(˙)_(k)q^(˙)_(j))/(2)+(del)/(delq_(i))sum_(k=1)^(3)sum_(j=1)^(n)g_(k)m_(j) bar(c)_(k)^(j)(q)],[=(sum_(k=1)^(n)sum_(j=1)^(n)[delD_(kj)(q)//delq_(i)]q^(˙)_(k)q^(˙)_(j))/(2)+sum_(k=1)^(3)sum_(j=1)^(n)g_(k)m_(j)[(del bar(c)_(k)^(j)(q))/(delq_(i))]],[=(sum_(k=1)^(n)sum_(j=1)^(n)[delD_(kj)(q)//delq_(i)]q^(˙)_(k)q^(˙)_(j))/(2)+sum_(k=1)^(3)sum_(j=1)^(n)g_(k)m_(j)A_(ki)^(j)(q)],[=(sum_(k=1)^(n)sum_(j=1)^(n)[delD_(kj)(q)//delq_(i)]q^(˙)_(k)q^(˙)_(j))/(2)+sum_(k=1)^(3)sum_(j=i)^(n)g_(k)m_(j)A_(ki)^(j)(q)]:}\begin{aligned} \frac{\partial}{\partial q_{i}} L(q, \dot{q}) & =\frac{\partial}{\partial q_{i}} T(q, \dot{q})-\frac{\partial}{\partial q_{i}} U(q) \\ & =\frac{\partial}{\partial q_{i}} \frac{\sum_{k=1}^{n} \sum_{j=1}^{n} D_{k j}(q) \dot{q}_{k} \dot{q}_{j}}{2}+\frac{\partial}{\partial q_{i}} \sum_{k=1}^{3} \sum_{j=1}^{n} g_{k} m_{j} \bar{c}_{k}^{j}(q) \\ & =\frac{\sum_{k=1}^{n} \sum_{j=1}^{n}\left[\partial D_{k j}(q) / \partial q_{i}\right] \dot{q}_{k} \dot{q}_{j}}{2}+\sum_{k=1}^{3} \sum_{j=1}^{n} g_{k} m_{j}\left[\frac{\partial \bar{c}_{k}^{j}(q)}{\partial q_{i}}\right] \\ & =\frac{\sum_{k=1}^{n} \sum_{j=1}^{n}\left[\partial D_{k j}(q) / \partial q_{i}\right] \dot{q}_{k} \dot{q}_{j}}{2}+\sum_{k=1}^{3} \sum_{j=1}^{n} g_{k} m_{j} A_{k i}^{j}(q) \\ & =\frac{\sum_{k=1}^{n} \sum_{j=1}^{n}\left[\partial D_{k j}(q) / \partial q_{i}\right] \dot{q}_{k} \dot{q}_{j}}{2}+\sum_{k=1}^{3} \sum_{j=i}^{n} g_{k} m_{j} A_{k i}^{j}(q) \end{aligned}
The index on the last summation goes from j = i j = i j=ij=i rather than j = 1 j = 1 j=1j=1 because, from Eq. (6-2-10), the last n − j n − j n-jn-j columns of A j A j A^(j)A^{j} are zero. To simplify the final equations of motion, we introduce two new quantities:
最後總和的指數從 j = i j = i j=ij=i 開始而不是 j = 1 j = 1 j=1j=1 ，因為根據方程 (6-2-10)，最後 n − j n − j n-jn-j 列的 A j A j A^(j)A^{j} 為零。為了簡化最終的運動方程，我們引入兩個新量：
C k j i ( q ) ≜ ∂ ∂ q k D i j ( q ) − 1 2 ∂ ∂ q i D k j ( q ) 1 ≤ i , j , k ≤ n h i ( q ) ≜ − ∑ k = 1 3 ∑ j = i n g k m j A k i j ( q ) 1 ≤ i ≤ n C k j i ( q ) ≜ ∂ ∂ q k D i j ( q ) − 1 2 ∂ ∂ q i D k j ( q ) 1 ≤ i , j , k ≤ n h i ( q ) ≜ − ∑ k = 1 3   ∑ j = i n   g k m j A k i j ( q ) 1 ≤ i ≤ n {:[C_(kj)^(i)(q)≜(del)/(delq_(k))D_(ij)(q)-(1)/(2)(del)/(delq_(i))D_(kj)(q),1 <= i","j","k <= n],[h_(i)(q)≜-sum_(k=1)^(3)sum_(j=i)^(n)g_(k)m_(j)A_(ki)^(j)(q),1 <= i <= n]:}\begin{array}{cl} C_{k j}^{i}(q) \triangleq \frac{\partial}{\partial q_{k}} D_{i j}(q)-\frac{1}{2} \frac{\partial}{\partial q_{i}} D_{k j}(q) & 1 \leq i, j, k \leq n \\ h_{i}(q) \triangleq-\sum_{k=1}^{3} \sum_{j=i}^{n} g_{k} m_{j} A_{k i}^{j}(q) & 1 \leq i \leq n \end{array}
We refer to the n × n n × n n xx nn \times n matrix C i C i C^(i)C^{i} as the velocity coupling matrix for joint i i ii, and the n × 1 n × 1 n xx1n \times 1 vector h h hh as the gravity loading vector. With the aid of C i C i C^(i)C^{i} and h h hh, we can now develop a concise formulation of a dynamic model of a robotic arm.
我們將 n × n n × n n xx nn \times n 矩陣 C i C i C^(i)C^{i} 稱為關節 i i ii 的速度耦合矩陣，並將 n × 1 n × 1 n xx1n \times 1 向量 h h hh 稱為重力載荷向量。在 C i C i C^(i)C^{i} 和 h h hh 的幫助下，我們現在可以開發機器手臂動態模型的簡明公式。
Proposition 6-4-1: Lagrange-Euler Equations. Let q q qq denote the joint variables and let τ τ tau\tau denote the actuator torques of an n n nn-axis robotic arm. If the manipulator is moving freely in its workspace, then its dynamic equations of motion are:
命題 6-4-1：拉格朗日-歐拉方程。令 q q qq 表示聯合變量，令 τ τ tau\tau 表示 n n nn 軸機器手臂的驅動扭矩。如果操控器在其工作空間中自由移動，則其動態運動方程為：
∑ j = 1 n D i j ( q ) q ¨ j + ∑ k = 1 n ∑ j = 1 n C k j i ( q ) q ˙ k q ˙ j + h i ( q ) + b i ( q ˙ ) = τ i 1 ≤ i ≤ n ∑ j = 1 n   D i j ( q ) q ¨ j + ∑ k = 1 n   ∑ j = 1 n   C k j i ( q ) q ˙ k q ˙ j + h i ( q ) + b i ( q ˙ ) = τ i 1 ≤ i ≤ n sum_(j=1)^(n)D_(ij)(q)q^(¨)_(j)+sum_(k=1)^(n)sum_(j=1)^(n)C_(kj)^(i)(q)q^(˙)_(k)q^(˙)_(j)+h_(i)(q)+b_(i)(q^(˙))=tau_(i)quad1 <= i <= n\sum_{j=1}^{n} D_{i j}(q) \ddot{q}_{j}+\sum_{k=1}^{n} \sum_{j=1}^{n} C_{k j}^{i}(q) \dot{q}_{k} \dot{q}_{j}+h_{i}(q)+b_{i}(\dot{q})=\tau_{i} \quad 1 \leq i \leq n
Proof. Let 1 ≤ i ≤ n 1 ≤ i ≤ n 1 <= i <= n1 \leq i \leq n. Then, substituting Eqs. (6-3-6), (6-4-4), and (6-4-5) in Eq. (6-1-2) and using Eqs. (6-4-6) and (6-4-7), we have:
證明。設 1 ≤ i ≤ n 1 ≤ i ≤ n 1 <= i <= n1 \leq i \leq n 。然後，將方程式 (6-3-6)、(6-4-4) 和 (6-4-5) 代入方程式 (6-1-2)，並使用方程式 (6-4-6) 和 (6-4-7)，我們得到：
τ i = F i + b i ( q ˙ ) = d d t ∂ ∂ q ˙ i L ( q , q ˙ ) − ∂ ∂ q i L ( q , q ˙ ) + b i ( q ˙ ) = ∑ j = 1 n D i j ( q ) q ¨ j + ∑ k = 1 n ∑ j = 1 n [ ∂ D i j ( q ) ∂ q k ] q ˙ k q ˙ j − ∂ ∂ q i L ( q , q ˙ ) + b i ( q ˙ ) = ∑ j = 1 n D i j ( q ) q ¨ j + ∑ k = 1 n ∑ j = 1 n [ ∂ D i j ( q ) ∂ q k − ∂ D k j ( q ) / ∂ q i 2 ] q ˙ k q ˙ j + h i ( q ) + b i ( q ˙ ) = ∑ j = 1 n D i j ( q ) q ¨ j + ∑ k = 1 n ∑ j = 1 n C k j i ( q ) q ˙ k q ˙ j + h i ( q ) + b i ( q ˙ ) τ i = F i + b i ( q ˙ ) = d d t ∂ ∂ q ˙ i L ( q , q ˙ ) − ∂ ∂ q i L ( q , q ˙ ) + b i ( q ˙ ) = ∑ j = 1 n   D i j ( q ) q ¨ j + ∑ k = 1 n   ∑ j = 1 n   ∂ D i j ( q ) ∂ q k q ˙ k q ˙ j − ∂ ∂ q i L ( q , q ˙ ) + b i ( q ˙ ) = ∑ j = 1 n   D i j ( q ) q ¨ j + ∑ k = 1 n   ∑ j = 1 n   ∂ D i j ( q ) ∂ q k − ∂ D k j ( q ) / ∂ q i 2 q ˙ k q ˙ j + h i ( q ) + b i ( q ˙ ) = ∑ j = 1 n   D i j ( q ) q ¨ j + ∑ k = 1 n   ∑ j = 1 n   C k j i ( q ) q ˙ k q ˙ j + h i ( q ) + b i ( q ˙ ) {:[tau_(i)=F_(i)+b_(i)(q^(˙))],[=(d)/(dt)(del)/(delq^(˙)_(i))L(q","q^(˙))-(del)/(delq_(i))L(q","q^(˙))+b_(i)(q^(˙))],[=sum_(j=1)^(n)D_(ij)(q)q^(¨)_(j)+sum_(k=1)^(n)sum_(j=1)^(n)[(delD_(ij)(q))/(delq_(k))]q^(˙)_(k)q^(˙)_(j)-(del)/(delq_(i))L(q","q^(˙))+b_(i)(q^(˙))],[=sum_(j=1)^(n)D_(ij)(q)q^(¨)_(j)+sum_(k=1)^(n)sum_(j=1)^(n)[(delD_(ij)(q))/(delq_(k))-(delD_(kj)(q)//delq_(i))/(2)]q^(˙)_(k)q^(˙)_(j)+h_(i)(q)+b_(i)(q^(˙))],[=sum_(j=1)^(n)D_(ij)(q)q^(¨)_(j)+sum_(k=1)^(n)sum_(j=1)^(n)C_(kj)^(i)(q)q^(˙)_(k)q^(˙)_(j)+h_(i)(q)+b_(i)(q^(˙))]:}\begin{aligned} \tau_{i} & =F_{i}+b_{i}(\dot{q}) \\ & =\frac{d}{d t} \frac{\partial}{\partial \dot{q}_{i}} L(q, \dot{q})-\frac{\partial}{\partial q_{i}} L(q, \dot{q})+b_{i}(\dot{q}) \\ & =\sum_{j=1}^{n} D_{i j}(q) \ddot{q}_{j}+\sum_{k=1}^{n} \sum_{j=1}^{n}\left[\frac{\partial D_{i j}(q)}{\partial q_{k}}\right] \dot{q}_{k} \dot{q}_{j}-\frac{\partial}{\partial q_{i}} L(q, \dot{q})+b_{i}(\dot{q}) \\ & =\sum_{j=1}^{n} D_{i j}(q) \ddot{q}_{j}+\sum_{k=1}^{n} \sum_{j=1}^{n}\left[\frac{\partial D_{i j}(q)}{\partial q_{k}}-\frac{\partial D_{k j}(q) / \partial q_{i}}{2}\right] \dot{q}_{k} \dot{q}_{j}+h_{i}(q)+b_{i}(\dot{q}) \\ & =\sum_{j=1}^{n} D_{i j}(q) \ddot{q}_{j}+\sum_{k=1}^{n} \sum_{j=1}^{n} C_{k j}^{i}(q) \dot{q}_{k} \dot{q}_{j}+h_{i}(q)+b_{i}(\dot{q}) \end{aligned}
The first term in Prop. 6-4-1 is an acceleration term that represents the inertial forces and torques generated by the motion of the links of the arm. The second term is a product velocity term associated with Coriolis and centrifugal forces. The third term is a position term representing loading due to gravity. Finally, the fourth term is a velocity term representing the friction opposing the motion of the arm.
在命題 6-4-1 中的第一項是加速度項，代表由手臂連桿運動產生的慣性力和扭矩。第二項是與科里奧利和離心力相關的速度乘積項。第三項是位置項，代表由重力引起的負載。最後，第四項是速度項，代表抵抗手臂運動的摩擦力。
The n n nn separate scalar equations in Prop. 6-4-1 can be combined into a single vector equation that represents a dynamic model of the arm. To facilitate this, consider the following quadratic function of the joint velocity:
在命題 6-4-1 中的 n n nn 個獨立標量方程可以合併成一個單一的向量方程，該方程表示手臂的動態模型。為了方便這一點，考慮以下關於關節速度的二次函數：
c i ( q , q ˙ ) ≜ q ˙ T C i ( q ) q ˙ 1 ≤ i ≤ n c i ( q , q ˙ ) ≜ q ˙ T C i ( q ) q ˙ 1 ≤ i ≤ n c_(i)(q,q^(˙))≜q^(˙)^(T)C^(i)(q)q^(˙)quad1 <= i <= nc_{i}(q, \dot{q}) \triangleq \dot{q}^{T} C^{i}(q) \dot{q} \quad 1 \leq i \leq n
Note that c i ( q , q ˙ ) c i ( q , q ˙ ) c_(i)(q,q^(˙))c_{i}(q, \dot{q}) is the product velocity term in Prop. 6-4-1. Hence the n n nn separate scalar equations in Prop. 6-4-1 can be written as a single vector equation, as follows:
請注意， c i ( q , q ˙ ) c i ( q , q ˙ ) c_(i)(q,q^(˙))c_{i}(q, \dot{q}) 是命題 6-4-1 中的產品速度項。因此，命題 6-4-1 中的 n n nn 個獨立標量方程可以寫成一個單一的向量方程，如下所示：
D ( q ) q ¨ + c ( q , q ˙ ) + h ( q ) + b ( q ˙ ) = τ D ( q ) q ¨ + c ( q , q ˙ ) + h ( q ) + b ( q ˙ ) = τ D(q)q^(¨)+c(q,q^(˙))+h(q)+b(q^(˙))=tauD(q) \ddot{q}+c(q, \dot{q})+h(q)+b(\dot{q})=\tau
The vector c ( q , q ˙ ) c ( q , q ˙ ) c(q,q^(˙))c(q, \dot{q}) is called the velocity coupling vector. It is clear from Eq. ( 6 − 4 − 8 ) ( 6 − 4 − 8 ) (6-4-8)(6-4-8) that c ( q , 0 ) = 0 c ( q , 0 ) = 0 c(q,0)=0c(q, 0)=0. There are two distinct types of interaxis velocity coupling that arise. To see this, we expand the velocity coupling vector as follows:
向量 c ( q , q ˙ ) c ( q , q ˙ ) c(q,q^(˙))c(q, \dot{q}) 被稱為速度耦合向量。從方程 ( 6 − 4 − 8 ) ( 6 − 4 − 8 ) (6-4-8)(6-4-8) 可以清楚地看出 c ( q , 0 ) = 0 c ( q , 0 ) = 0 c(q,0)=0c(q, 0)=0 。出現了兩種不同類型的軸間速度耦合。為了看清這一點，我們將速度耦合向量展開如下：
c i ( q , q ˙ ) = ∑ k = 1 n C k k i ( q ) q ˙ k 2 + ∑ k = 1 n ∑ j ≠ k C k j i ( q ) q ˙ k q ˙ j c i ( q , q ˙ ) = ∑ k = 1 n   C k k i ( q ) q ˙ k 2 + ∑ k = 1 n   ∑ j ≠ k   C k j i ( q ) q ˙ k q ˙ j c_(i)(q,q^(˙))=sum_(k=1)^(n)C_(kk)^(i)(q)q^(˙)_(k)^(2)+sum_(k=1)^(n)sum_(j!=k)C_(kj)^(i)(q)q^(˙)_(k)q^(˙)_(j)c_{i}(q, \dot{q})=\sum_{k=1}^{n} C_{k k}^{i}(q) \dot{q}_{k}^{2}+\sum_{k=1}^{n} \sum_{j \neq k} C_{k j}^{i}(q) \dot{q}_{k} \dot{q}_{j}
The two summations in Eq. (6-4-10) are generated by the diagonal and offdiagonal components of the velocity coupling matrices, respectively. The first sum-
方程 (6-4-10) 中的兩個總和分別由速度耦合矩陣的對角和非對角分量生成。第一個總和-
mation corresponds to squared velocity terms associated with centrifugal forces. For example, the term C k k i ( q ) q ˙ k 2 C k k i ( q ) q ˙ k 2 C_(kk)^(i)(q)q^(˙)_(k)^(2)C_{k k}^{i}(q) \dot{q}_{k}^{2} represents the centrifugal force acting on joint i i ii due to the motion of joint k k kk.
該信息對應於與離心力相關的平方速度項。例如，術語 C k k i ( q ) q ˙ k 2 C k k i ( q ) q ˙ k 2 C_(kk)^(i)(q)q^(˙)_(k)^(2)C_{k k}^{i}(q) \dot{q}_{k}^{2} 表示由於關節 k k kk 的運動而作用於關節 i i ii 的離心力。
The second summation in Eq. (6-4-10) corresponds to product velocity terms associated with Coriolis forces. In this case the term C k j i ( q ) q ˙ k q ˙ j C k j i ( q ) q ˙ k q ˙ j C_(kj)^(i)(q)q^(˙)_(k)q^(˙)_(j)C_{k j}^{i}(q) \dot{q}_{k} \dot{q}_{j} represents the Coriolis force acting on joint i i ii due to the combined motions of joints k k kk and j j jj where j ≠ k j ≠ k j!=kj \neq k.
方程式 (6-4-10) 中的第二個總和對應於與科里奧利力相關的產品速度項。在這種情況下，項 C k j i ( q ) q ˙ k q ˙ j C k j i ( q ) q ˙ k q ˙ j C_(kj)^(i)(q)q^(˙)_(k)q^(˙)_(j)C_{k j}^{i}(q) \dot{q}_{k} \dot{q}_{j} 代表由於關節 k k kk 和 j j jj 的聯合運動而作用於關節 i i ii 的科里奧利力，其中 j ≠ k j ≠ k j!=kj \neq k 。
Exercise 6-4-1: One-Axis Robot. Show that the velocity coupling vector of a one-axis robot is always c ( q , q ˙ ) = 0 c ( q , q ˙ ) = 0 c(q,q^(˙))=0c(q, \dot{q})=0.
練習 6-4-1：單軸機器人。證明單軸機器人的速度耦合向量始終為 c ( q , q ˙ ) = 0 c ( q , q ˙ ) = 0 c(q,q^(˙))=0c(q, \dot{q})=0 。
The derivation of the closed-form equations of motion of a robotic arm based on the Lagrange-Euler formulation can be summarized in the form of an algorithm.
基於拉格朗日-歐拉公式的機器手臂運動閉式方程的推導可以總結為一個算法。

Algorithm 6-4-1: Lagrange-Euler Equations
算法 6-4-1：拉格朗日-歐拉方程

  1. Apply the D-H algorithm to assign link coordinates { L 0 , L 1 , … , L n } L 0 , L 1 , … , L n {L_(0),L_(1),dots,L_(n)}\left\{L_{0}, L_{1}, \ldots, L_{n}\right\} orienting frame L i L i L_(i)L_{i} with the principal axes of link i i ii.
    應用 D-H 算法為連桿 { L 0 , L 1 , … , L n } L 0 , L 1 , … , L n {L_(0),L_(1),dots,L_(n)}\left\{L_{0}, L_{1}, \ldots, L_{n}\right\} 分配坐標，將框架 L i L i L_(i)L_{i} 定向至連桿 i i ii 的主軸。
  2. Set T 0 0 = I , i = 1 , D ( q ) = 0 T 0 0 = I , i = 1 , D ( q ) = 0 T_(0)^(0)=I,i=1,D(q)=0T_{0}^{0}=I, i=1, D(q)=0. 設置 T 0 0 = I , i = 1 , D ( q ) = 0 T 0 0 = I , i = 1 , D ( q ) = 0 T_(0)^(0)=I,i=1,D(q)=0T_{0}^{0}=I, i=1, D(q)=0 。
  3. Find Δ c i Δ c i Deltac^(i)\Delta c^{i}, the homogeneous coordinates of the center of mass of link i i ii with respect to frame L i L i L_(i)L_{i}.
    找到 Δ c i Δ c i Deltac^(i)\Delta c^{i} ，連桿 i i ii 相對於框架 L i L i L_(i)L_{i} 的質心的齊次座標。
  4. Let L c L c L_(c)L_{c} be the translation of frame L i L i L_(i)L_{i} to the center of mass of link i i ii. Use Appendix 2 or Eq. (6-2-2) to compute D ¯ i D ¯ i bar(D)_(i)\bar{D}_{i}, the inertia tensor of link i i ii about its center of mass expressed with respect to frame L c L c L_(c)L_{c}.
    讓 L c L c L_(c)L_{c} 成為框架 L i L i L_(i)L_{i} 到連桿 i i ii 的質心的翻譯。使用附錄 2 或公式 (6-2-2) 計算 D ¯ i D ¯ i bar(D)_(i)\bar{D}_{i} ，連桿 i i ii 關於其質心的慣性張量，並以框架 L c L c L_(c)L_{c} 表示。
  5. Compute: 計算：
z i − 1 ( q ) = R 0 i − 1 ( q ) i 3 T 0 i ( q ) = T 0 i − 1 ( q ) T i − 1 i ( q ) c ¯ i ( q ) = H 1 T 0 i ( q ) Δ c i D i ( q ) = R 0 i ( q ) D ¯ i [ R 0 i ( q ) ] T z i − 1 ( q ) = R 0 i − 1 ( q ) i 3 T 0 i ( q ) = T 0 i − 1 ( q ) T i − 1 i ( q ) c ¯ i ( q ) = H 1 T 0 i ( q ) Δ c i D i ( q ) = R 0 i ( q ) D ¯ i R 0 i ( q ) T {:[z^(i-1)(q)=R_(0)^(i-1)(q)i^(3)],[T_(0)^(i)(q)=T_(0)^(i-1)(q)T_(i-1)^(i)(q)],[ bar(c)^(i)(q)=H_(1)T_(0)^(i)(q)Deltac^(i)],[D_(i)(q)=R_(0)^(i)(q) bar(D)_(i)[R_(0)^(i)(q)]^(T)]:}\begin{aligned} z^{i-1}(q) & =R_{0}^{i-1}(q) i^{3} \\ T_{0}^{i}(q) & =T_{0}^{i-1}(q) T_{i-1}^{i}(q) \\ \bar{c}^{i}(q) & =H_{1} T_{0}^{i}(q) \Delta c^{i} \\ D_{i}(q) & =R_{0}^{i}(q) \bar{D}_{i}\left[R_{0}^{i}(q)\right]^{T} \end{aligned}
  1. Compute J i ( q ) J i ( q ) J^(i)(q)J^{i}(q) using Eq. (6-2-10).
    使用公式 (6-2-10) 計算 J i ( q ) J i ( q ) J^(i)(q)J^{i}(q) 。
  2. Partition J i ( q ) J i ( q ) J^(i)(q)J^{i}(q) as in Eq. ( 6 − 2 − 10 ) ( 6 − 2 − 10 ) (6-2-10)(6-2-10) and compute:
    將 J i ( q ) J i ( q ) J^(i)(q)J^{i}(q) 分割為如同 Eq. ( 6 − 2 − 10 ) ( 6 − 2 − 10 ) (6-2-10)(6-2-10) 並計算：
D ( q ) = D ( q ) + [ A k ( q ) ] T m k A k ( q ) + [ B k ( q ) ] T D k ( q ) B k ( q ) D ( q ) = D ( q ) + A k ( q ) T m k A k ( q ) + B k ( q ) T D k ( q ) B k ( q ) D(q)=D(q)+[A^(k)(q)]^(T)m_(k)A^(k)(q)+[B^(k)(q)]^(T)D_(k)(q)B^(k)(q)D(q)=D(q)+\left[A^{k}(q)\right]^{T} m_{k} A^{k}(q)+\left[B^{k}(q)\right]^{T} D_{k}(q) B^{k}(q)
  1. Set i = i + 1 i = i + 1 i=i+1i=i+1. If i ≤ n i ≤ n i <= ni \leq n, go to step 3 ; else, set i = 1 i = 1 i=1i=1 and continue.
    設置 i = i + 1 i = i + 1 i=i+1i=i+1 。如果 i ≤ n i ≤ n i <= ni \leq n ，則轉到步驟 3；否則，設置 i = 1 i = 1 i=1i=1 並繼續。
  2. Compute C i ( q ) , h i ( q ) C i ( q ) , h i ( q ) C^(i)(q),h_(i)(q)C^{i}(q), h_{i}(q), and b i ( q ˙ ) b i ( q ˙ ) b_(i)(q^(˙))b_{i}(\dot{q}) using Eqs. (6-4-6), (6-4-7), and (6-3-3), respectively.
    計算 C i ( q ) , h i ( q ) C i ( q ) , h i ( q ) C^(i)(q),h_(i)(q)C^{i}(q), h_{i}(q) 和 b i ( q ˙ ) b i ( q ˙ ) b_(i)(q^(˙))b_{i}(\dot{q}) ，分別使用方程式 (6-4-6)、(6-4-7) 和 (6-3-3)。
  3. Formulate the ith equation as:
    將第 i 個方程式表達為：
∑ j = 1 n D i j ( q ) q ¨ j + ∑ k = 1 n ∑ j = 1 n C k j i ( q ) q ˙ k q ˙ j + h i ( q ) + b i ( q ˙ ) = τ i ∑ j = 1 n   D i j ( q ) q ¨ j + ∑ k = 1 n   ∑ j = 1 n   C k j i ( q ) q ˙ k q ˙ j + h i ( q ) + b i ( q ˙ ) = τ i sum_(j=1)^(n)D_(ij)(q)q^(¨)_(j)+sum_(k=1)^(n)sum_(j=1)^(n)C_(kj)^(i)(q)q^(˙)_(k)q^(˙)_(j)+h_(i)(q)+b_(i)(q^(˙))=tau_(i)\sum_{j=1}^{n} D_{i j}(q) \ddot{q}_{j}+\sum_{k=1}^{n} \sum_{j=1}^{n} C_{k j}^{i}(q) \dot{q}_{k} \dot{q}_{j}+h_{i}(q)+b_{i}(\dot{q})=\tau_{i}
  1. Set i = i + 1 i = i + 1 i=i+1i=i+1. If i ≤ n i ≤ n i <= ni \leq n, go to step 9 .
    設置 i = i + 1 i = i + 1 i=i+1i=i+1 。如果 i ≤ n i ≤ n i <= ni \leq n ，則轉到步驟 9 。
Note that Algorithm 6-4-1 is a two-pass algorithm in terms of the manipulator. Initialization is performed in steps 1 and 2. Pass 1 then consists of steps 3 through 7 repeated for each link, proceeding from link 1 through link n n nn. At the end of pass 1 , the manipulator inertia tensor D ( q ) D ( q ) D(q)D(q) and the link Jacobian matrices { J k ( q ) } J k ( q ) {J^(k)(q)}\left\{J^{k}(q)\right\} are available. These quantities serve as inputs to the second pass through the links of the manipulator in steps 9 and 10 .
請注意，算法 6-4-1 是一個關於操控器的兩次通過算法。初始化在步驟 1 和 2 中進行。第一次通過由步驟 3 到 7 組成，對每個連接進行重複，從連接 1 到連接 n n nn 。在第一次通過結束時，操控器的慣性張量 D ( q ) D ( q ) D(q)D(q) 和連接雅可比矩陣 { J k ( q ) } J k ( q ) {J^(k)(q)}\left\{J^{k}(q)\right\} 可用。這些量作為第二次通過操控器連接的輸入，在步驟 9 和 10 中使用。

6-5 DYNAMIC MODEL OF A TWO-AXIS PLANAR ARTICULATED ROBOT
6-5 兩軸平面關節機器人的動態模型

A complete dynamic model of an n n nn-axis robotic manipulator can be rather complex. To illustrate the derivation of a complete dynamic model, we first examine a simple two-axis planar articulated robotic arm as shown in Fig. 6-4.
一個完整的 n n nn 軸機器人操作臂的動態模型可能相當複雜。為了說明完整動態模型的推導，我們首先檢查一個簡單的兩軸平面關節機器手臂，如圖 6-4 所示。
Figure 6-4 A two-axis planar articulated robot.
圖 6-4 一個雙軸平面關節機器人。
For convenience, we assume that the two physical links are thin cylinders or rods of mass m 1 m 1 m_(1)m_{1} and m 2 m 2 m_(2)m_{2}, respectively. We also assume that the mass of any tool attached to the end of link 2 is sufficiently small in comparison with m 2 m 2 m_(2)m_{2} that it can be ignored.
為了方便起見，我們假設這兩個物理連結是質量分別為 m 1 m 1 m_(1)m_{1} 和 m 2 m 2 m_(2)m_{2} 的細圓柱或棒。我們還假設附加在連結 2 末端的任何工具的質量與 m 2 m 2 m_(2)m_{2} 相比足夠小，可以忽略不計。
Applying step 1 of Algorithm 6-4-1 yields the link-coordinate diagram shown in Fig. 6-4. Note that the link-coordinate frames have been assigned in such a manner that the axes of frames L 1 L 1 L_(1)L_{1} and L 2 L 2 L_(2)L_{2} are aligned with the principal axes of rods m 1 m 1 m_(1)m_{1} and m 2 m 2 m_(2)m_{2}, respectively. Since the robot is articulated, the vector of joint variables is q = θ q = θ q=thetaq=\theta. The kinematic parameters of the two-axis planar articulated robot are listed in Table 6-1.
應用算法 6-4-1 的第 1 步產生了圖 6-4 所示的連桿坐標圖。請注意，連桿坐標框架的分配方式使得框架 L 1 L 1 L_(1)L_{1} 和 L 2 L 2 L_(2)L_{2} 的軸與桿 m 1 m 1 m_(1)m_{1} 和 m 2 m 2 m_(2)m_{2} 的主軸對齊。由於機器人是關節式的，關節變量的向量為 q = θ q = θ q=thetaq=\theta 。兩軸平面關節機器人的運動參數列在表 6-1 中。
TABLE 6-1 KINEMATIC PARAMETERS OF THE TWO-AXIS PLANAR ARTICULATED ROBOT
表 6-1 雙軸平面關節機器人的運動參數
Joint 關節 d d dd a a aa θ θ theta\theta α α alpha\alpha Home 家
1 0 a 1 a 1 a_(1)a_{1} q 1 q 1 q_(1)q_{1} 0 π / 3 π / 3 pi//3\pi / 3
2 0 a 2 a 2 a_(2)a_{2} q 2 q 2 q_(2)q_{2} 0 − π / 3 − π / 3 -pi//3-\pi / 3
Joint d a theta alpha Home 1 0 a_(1) q_(1) 0 pi//3 2 0 a_(2) q_(2) 0 -pi//3| Joint | $d$ | $a$ | $\theta$ | $\alpha$ | Home | | :--- | :--- | :--- | :--- | :--- | ---: | | 1 | 0 | $a_{1}$ | $q_{1}$ | 0 | $\pi / 3$ | | 2 | 0 | $a_{2}$ | $q_{2}$ | 0 | $-\pi / 3$ |
Next we apply steps 3 through 7 of Algorithm 6 − 4 − 1 6 − 4 − 1 6-4-16-4-1 with i = 1 i = 1 i=1i=1. Assuming that the links are homogeneous, we see from Fig. 6-4 that the center of mass of link 1 in frame L 1 L 1 L_(1)L_{1} coordinates is:
接下來，我們應用算法 6 − 4 − 1 6 − 4 − 1 6-4-16-4-1 的第 3 到第 7 步，使用 i = 1 i = 1 i=1i=1 。假設連桿是均質的，我們從圖 6-4 中可以看到，連桿 1 在框架 L 1 L 1 L_(1)L_{1} 坐標系中的質心是：
Δ c 1 = [ − a 1 2 , 0 , 0 , 1 ] T Δ c 1 = − a 1 2 , 0 , 0 , 1 T Deltac^(1)=[-(a_(1))/(2),0,0,1]^(T)\Delta c^{1}=\left[-\frac{a_{1}}{2}, 0,0,1\right]^{T}
Let L c L c L_(c)L_{c} denote the coordinate frame obtained by translating frame L 1 L 1 L_(1)L_{1} along axis x 1 x 1 x^(1)x^{1} by an amount − a 1 / 2 − a 1 / 2 -a_(1)//2-a_{1} / 2. L c L c L_(c)L_{c} is then at the center of mass of link 1 , and the axes of L c L c L_(c)L_{c} are aligned with the principal axes of link 1 . If D ¯ 1 D ¯ 1 bar(D)_(1)\bar{D}_{1} is the inertia tensor of link 1 about its center of mass with respect to frame L c L c L_(c)L_{c}, then, from Appendix 2:
讓 L c L c L_(c)L_{c} 表示通過沿著軸 x 1 x 1 x^(1)x^{1} 移動 L 1 L 1 L_(1)L_{1} 框架 − a 1 / 2 − a 1 / 2 -a_(1)//2-a_{1} / 2 的量而獲得的坐標框架。 L c L c L_(c)L_{c} 然後位於連桿 1 的質心， L c L c L_(c)L_{c} 的軸與連桿 1 的主軸對齊。如果 D ¯ 1 D ¯ 1 bar(D)_(1)\bar{D}_{1} 是相對於框架 L c L c L_(c)L_{c} 的連桿 1 在其質心處的慣性張量，那麼，根據附錄 2：
D ¯ 1 = ( m 1 a 1 2 12 ) diag { 0 , 1 , 1 } D ¯ 1 = m 1 a 1 2 12 diag { 0 , 1 , 1 } bar(D)_(1)=((m_(1)a_(1)^(2))/(12))diag{0,1,1}\bar{D}_{1}=\left(\frac{m_{1} a_{1}^{2}}{12}\right) \operatorname{diag}\{0,1,1\}
Here the notation diag { a , b , c } { a , b , c } {a,b,c}\{a, b, c\} is short for a diagonal matrix with diagonal elements { a , b , c } { a , b , c } {a,b,c}\{a, b, c\}. Next, we apply step 5 where z 0 = i 3 z 0 = i 3 z^(0)=i^(3)z^{0}=i^{3}. Using Table 6 − 1 6 − 1 6-16-1 and Prop. 2-6-1, or using Eq. (3-7-1) directly, the homogeneous coordinate transformation matrix for link 1 is:
這裡的符號 diag { a , b , c } { a , b , c } {a,b,c}\{a, b, c\} 是指對角元素為 { a , b , c } { a , b , c } {a,b,c}\{a, b, c\} 的對角矩陣。接下來，我們應用第 5 步，其中 z 0 = i 3 z 0 = i 3 z^(0)=i^(3)z^{0}=i^{3} 。使用表 6 − 1 6 − 1 6-16-1 和命題 2-6-1，或直接使用方程 (3-7-1)，連接 1 的齊次坐標變換矩陣為：
T 0 1 = [ C 1 − S l 0 a 1 C 1 S 1 C 1 0 a 1 S 1 0 0 1 0 0 0 0 1 ] T 0 1 = C 1 − S l 0 a 1 C 1 S 1 C 1 0 a 1 S 1 0 0 1 0 0 0 0 1 T_(0)^(1)=[[C_(1),-S_(l),0,a_(1)C_(1)],[S_(1),C_(1),0,a_(1)S_(1)],[0,0,1,0],[0,0,0,1]]T_{0}^{1}=\left[\begin{array}{ccc:c} \mathrm{C}_{1} & -\mathrm{S}_{\mathrm{l}} & 0 & a_{1} \mathrm{C}_{1} \\ \mathrm{~S}_{1} & \mathrm{C}_{1} & 0 & a_{1} \mathrm{~S}_{1} \\ 0 & 0 & 1 & 0 \\ \hdashline 0 & 0 & 0 & 1 \end{array}\right]
Combining Eqs. (6-5-1) and (6-5-3) and recalling Eq. (2-4-1), the center of mass of link 1 in base frame coordinates is:
結合方程式 (6-5-1) 和 (6-5-3)，並回顧方程式 (2-4-1)，連桿 1 在基座框架坐標中的質心為：
c ¯ 1 = H 1 T 0 1 Δ c 1 = [ a 1 C 1 2 , a 1 S 1 2 , 0 ] T c ¯ 1 = H 1 T 0 1 Δ c 1 = a 1 C 1 2 , a 1 S 1 2 , 0 T {:[ bar(c)^(1)=H_(1)T_(0)^(1)Deltac^(1)],[=[(a_(1)C_(1))/(2),(a_(1)S_(1))/(2),0]^(T)]:}\begin{aligned} \bar{c}^{1} & =H_{1} T_{0}^{1} \Delta c^{1} \\ & =\left[\frac{a_{1} \mathrm{C}_{1}}{2}, \frac{a_{1} \mathrm{~S}_{1}}{2}, 0\right]^{T} \end{aligned}
Finally, using Eq. (6-5-2) and the rotation part of T 0 1 T 0 1 T_(0)^(1)T_{0}^{1} in Eq. (6-5-3), the inertia tensor of link 1 in base coordinates is:
最後，使用公式 (6-5-2) 和公式 (6-5-3) 中 T 0 1 T 0 1 T_(0)^(1)T_{0}^{1} 的旋轉部分，基座坐標中連桿 1 的慣性張量為：
D 1 = R 0 1 D ¯ 1 ( R 0 1 ) T = m 1 a 1 2 12 [ S 1 2 − S 1 C 1 0 − S 1 C 1 C 1 2 0 0 0 1 ] D 1 = R 0 1 D ¯ 1 R 0 1 T = m 1 a 1 2 12 S 1 2 − S 1 C 1 0 − S 1 C 1 C 1 2 0 0 0 1 {:[D_(1)=R_(0)^(1) bar(D)_(1)(R_(0)^(1))^(T)],[=(m_(1)a_(1)^(2))/(12)[[S_(1)^(2),-S_(1)C_(1),0],[-S_(1)C_(1),C_(1)^(2),0],[0,0,1]]]:}\begin{aligned} D_{1} & =R_{0}^{1} \bar{D}_{1}\left(R_{0}^{1}\right)^{T} \\ & =\frac{m_{1} a_{1}^{2}}{12}\left[\begin{array}{ccc} \mathrm{S}_{1}^{2} & -\mathrm{S}_{1} \mathrm{C}_{1} & 0 \\ -\mathrm{S}_{1} \mathrm{C}_{1} & \mathrm{C}_{1}^{2} & 0 \\ 0 & 0 & 1 \end{array}\right] \end{aligned}
Next, the Jacobian matrix for link 1 is computed in step 6. Using Eqs. (6-2-10) and (6-5-4), and recalling that z 0 = i 3 z 0 = i 3 z^(0)=i^(3)z^{0}=i^{3} and ξ 1 = 1 ξ 1 = 1 xi_(1)=1\xi_{1}=1, we have the following 6 × 2 6 × 2 6xx26 \times 2 link Jacobian matrix:
接下來，連結 1 的雅可比矩陣在第 6 步中計算。使用方程式 (6-2-10) 和 (6-5-4)，並回想 z 0 = i 3 z 0 = i 3 z^(0)=i^(3)z^{0}=i^{3} 和 ξ 1 = 1 ξ 1 = 1 xi_(1)=1\xi_{1}=1 ，我們得到了以下 6 × 2 6 × 2 6xx26 \times 2 連結的雅可比矩陣：
J 1 = [ − a 1 S 1 / 2 0 a 1 C 1 / 2 0 0 0 0 0 0 1 0 ] J 1 = − a 1 S 1 / 2 0 a 1 C 1 / 2 0 0 0 0 0 0 1 0 J^(1)=[[-a_(1)S_(1)//2,0],[a_(1)C_(1)//2,0],[0,0],[],[,0],[0,0],[1,0]]J^{1}=\left[\begin{array}{cc} -a_{1} \mathrm{~S}_{1} / 2 & 0 \\ a_{1} \mathrm{C}_{1} / 2 & 0 \\ 0 & 0 \\ \hdashline \\ \hdashline & 0 \\ 0 & 0 \\ 1 & 0 \end{array}\right]
Partitioning J 1 J 1 J^(1)J^{1} into A 1 A 1 A^(1)A^{1} and B 1 B 1 B^(1)B^{1} as in Eq. (6-2-10), the contribution to the manipulator inertia tensor from link 1 is:
將 J 1 J 1 J^(1)J^{1} 分割為 A 1 A 1 A^(1)A^{1} 和 B 1 B 1 B^(1)B^{1} ，如公式 (6-2-10) 所示，來自連桿 1 的操控器慣性張量的貢獻為：
D ( q ) = ( A 1 ) T m 1 A 1 + ( B 1 ) T D 1 B 1 = ( m 1 a 1 2 4 ) diag { 1 , 0 } + ( m 1 a 1 2 12 ) diag { 1 , 0 } = ( m 1 a 1 2 3 ) diag { 1 , 0 } D ( q ) = A 1 T m 1 A 1 + B 1 T D 1 B 1 = m 1 a 1 2 4 diag { 1 , 0 } + m 1 a 1 2 12 diag { 1 , 0 } = m 1 a 1 2 3 diag { 1 , 0 } {:[D(q)=(A^(1))^(T)m_(1)A^(1)+(B^(1))^(T)D_(1)B^(1)],[=((m_(1)a_(1)^(2))/(4))diag{1","0}+((m_(1)a_(1)^(2))/(12))diag{1","0}],[=((m_(1)a_(1)^(2))/(3))diag{1","0}]:}\begin{aligned} D(q) & =\left(A^{1}\right)^{T} m_{1} A^{1}+\left(B^{1}\right)^{T} D_{1} B^{1} \\ & =\left(\frac{m_{1} a_{1}^{2}}{4}\right) \operatorname{diag}\{1,0\}+\left(\frac{m_{1} a_{1}^{2}}{12}\right) \operatorname{diag}\{1,0\} \\ & =\left(\frac{m_{1} a_{1}^{2}}{3}\right) \operatorname{diag}\{1,0\} \end{aligned}
This completes the analysis for link 1 . Next, we set i = 2 i = 2 i=2i=2 and repeat steps 3 through 7. From Fig. 6-4, the center of mass of link 2 in frame L 2 L 2 L_(2)L_{2} coordinates is:
這完成了連桿 1 的分析。接下來，我們設置 i = 2 i = 2 i=2i=2 並重複步驟 3 到 7。從圖 6-4，連桿 2 在框架 L 2 L 2 L_(2)L_{2} 坐標系中的質心為：
Δ c 2 = [ − a 2 2 , 0 , 0 , 1 ] T Δ c 2 = − a 2 2 , 0 , 0 , 1 T Deltac^(2)=[(-a_(2))/(2),0,0,1]^(T)\Delta c^{2}=\left[\frac{-a_{2}}{2}, 0,0,1\right]^{T}
Let L c L c L_(c)L_{c} denote the coordinate frame obtained by translating frame L 2 L 2 L_(2)L_{2} along axis x 2 x 2 x^(2)x^{2} by an amount − a 2 / 2 . L c − a 2 / 2 . L c -a_(2)//2.L_(c)-a_{2} / 2 . L_{c} is then at the center of mass of link 2 , and the axes of L c L c L_(c)L_{c} are aligned with the principal axes of link 2 . If D ¯ 2 D ¯ 2 bar(D)_(2)\bar{D}_{2} is the inertia tensor of link 2 about its center of mass with respect to frame L c L c L_(c)L_{c}, then, from Appendix 2:
讓 L c L c L_(c)L_{c} 表示通過沿著軸 x 2 x 2 x^(2)x^{2} 移動 L 2 L 2 L_(2)L_{2} 框架 − a 2 / 2 . L c − a 2 / 2 . L c -a_(2)//2.L_(c)-a_{2} / 2 . L_{c} 的量而獲得的坐標框架，然後位於連桿 2 的質心處，並且 L c L c L_(c)L_{c} 的軸與連桿 2 的主軸對齊。如果 D ¯ 2 D ¯ 2 bar(D)_(2)\bar{D}_{2} 是關於其質心的連桿 2 相對於框架 L c L c L_(c)L_{c} 的慣性張量，那麼，根據附錄 2：
D ¯ 2 = ( m 2 a 2 2 12 ) diag { 0 , 1 , 1 } D ¯ 2 = m 2 a 2 2 12 diag { 0 , 1 , 1 } bar(D)_(2)=((m_(2)a_(2)^(2))/(12))diag{0,1,1}\bar{D}_{2}=\left(\frac{m_{2} a_{2}^{2}}{12}\right) \operatorname{diag}\{0,1,1\}
Next, we apply step 5. From Eq. (6-5-3), extracting the third column of R 0 1 R 0 1 R_(0)^(1)R_{0}^{1} yields z 1 = R 0 1 i 3 = i 3 z 1 = R 0 1 i 3 = i 3 z^(1)=R_(0)^(1)i^(3)=i^(3)z^{1}=R_{0}^{1} i^{3}=i^{3}. From Table 6-1, Prop. 2-6-1, and Eq. (6-5-3), or from Eq. (3-7-1) directly, the homogeneous coordinate transformation matrix for link 2 is:
接下來，我們應用步驟 5。從方程式(6-5-3)中，提取 R 0 1 R 0 1 R_(0)^(1)R_{0}^{1} 的第三列得到 z 1 = R 0 1 i 3 = i 3 z 1 = R 0 1 i 3 = i 3 z^(1)=R_(0)^(1)i^(3)=i^(3)z^{1}=R_{0}^{1} i^{3}=i^{3} 。從表 6-1、命題 2-6-1 和方程式(6-5-3)，或直接從方程式(3-7-1)，連桿 2 的齊次坐標變換矩陣為：
T 0 2 = [ C 12 − S 12 0 a 1 C 1 + a 2 C 12 S 12 C 12 0 a 1 S 1 + a 2 S 12 0 0 1 0 0 0 0 1 ] T 0 2 = C 12 − S 12 0 a 1 C 1 + a 2 C 12 S 12 C 12 0 a 1 S 1 + a 2 S 12 0 0 1 0 0 0 0 1 T_(0)^(2)=[[C_(12),-S_(12),0,a_(1)C_(1)+a_(2)C_(12)],[S_(12),C_(12),0,a_(1)S_(1)+a_(2)S_(12)],[0,0,1,0],[0,0,0,1]]T_{0}^{2}=\left[\begin{array}{ccc:c} \mathrm{C}_{12} & -\mathrm{S}_{12} & 0 & a_{1} \mathrm{C}_{1}+a_{2} \mathrm{C}_{12} \\ \mathrm{~S}_{12} & \mathrm{C}_{12} & 0 & a_{1} \mathrm{~S}_{1}+a_{2} \mathrm{~S}_{12} \\ 0 & 0 & 1 & 0 \\ \hdashline 0 & 0 & 0 & 1 \end{array}\right]
Combining Eqs. (6-5-8) and (6-5-10), the center of mass of link 2 in base frame coordinates is:
結合方程式 (6-5-8) 和 (6-5-10)，連桿 2 在基座框架坐標系中的質心為：
c ¯ 2 = H 1 T 0 2 Δ c 2 = [ a 1 C 1 + a 2 C 12 2 , a 1 S 1 + a 2 S 12 2 , 0 ] T c ¯ 2 = H 1 T 0 2 Δ c 2 = a 1 C 1 + a 2 C 12 2 , a 1 S 1 + a 2 S 12 2 , 0 T {:[ bar(c)^(2)=H_(1)T_(0)^(2)Deltac^(2)],[=[a_(1)C_(1)+(a_(2)C_(12))/(2),a_(1)S_(1)+(a_(2)S_(12))/(2),0]^(T)]:}\begin{aligned} \bar{c}^{2} & =H_{1} T_{0}^{2} \Delta c^{2} \\ & =\left[a_{1} \mathrm{C}_{1}+\frac{a_{2} \mathrm{C}_{12}}{2}, a_{1} \mathrm{~S}_{1}+\frac{a_{2} \mathrm{~S}_{12}}{2}, 0\right]^{T} \end{aligned}
Finally, using Eq. (6-5-9) and the rotation part of T 0 2 T 0 2 T_(0)^(2)T_{0}^{2} in Eq. (6-5-10), the inertia tensor of link 2 in base coordinates is:
最後，使用公式 (6-5-9) 和公式 (6-5-10) 中 T 0 2 T 0 2 T_(0)^(2)T_{0}^{2} 的旋轉部分，基座坐標中連桿 2 的慣性張量為：
D 2 = R 0 2 D ¯ 2 ( R 0 2 ) T = m 2 a 2 2 12 [ S 12 2 − S 12 C 12 0 − S 12 C 12 C 12 2 0 0 0 1 ] D 2 = R 0 2 D ¯ 2 R 0 2 T = m 2 a 2 2 12 S 12 2 − S 12 C 12 0 − S 12 C 12 C 12 2 0 0 0 1 {:[D_(2)=R_(0)^(2) bar(D)_(2)(R_(0)^(2))^(T)],[=(m_(2)a_(2)^(2))/(12)[[S_(12)^(2),-S_(12)C_(12),0],[-S_(12)C_(12),C_(12)^(2),0],[0,0,1]]]:}\begin{aligned} D_{2} & =R_{0}^{2} \bar{D}_{2}\left(R_{0}^{2}\right)^{T} \\ & =\frac{m_{2} a_{2}^{2}}{12}\left[\begin{array}{ccc} S_{12}^{2} & -S_{12} C_{12} & 0 \\ -S_{12} C_{12} & C_{12}^{2} & 0 \\ 0 & 0 & 1 \end{array}\right] \end{aligned}
The Jacobian matrix for link 2 is now computed in step 6. Using Eqs. (6-2-10) and (6-5-11) and recalling that z 1 = i 3 z 1 = i 3 z^(1)=i^(3)z^{1}=i^{3} and ξ 2 = 1 ξ 2 = 1 xi_(2)=1\xi_{2}=1, we have the following 6 × 2 6 × 2 6xx26 \times 2 link Jacobian matrix:
連結 2 的雅可比矩陣現在在第 6 步中計算。使用方程式(6-2-10)和(6-5-11)，並回想 z 1 = i 3 z 1 = i 3 z^(1)=i^(3)z^{1}=i^{3} 和 ξ 2 = 1 ξ 2 = 1 xi_(2)=1\xi_{2}=1 ，我們得到了以下 6 × 2 6 × 2 6xx26 \times 2 連結的雅可比矩陣：
J 2 = [ − a 1 S 1 − a 2 S 12 / 2 − a 2 S 12 / 2 a 1 C 1 + a 2 C 12 / 2 a 2 C 12 / 2 0 0 ⋯ 0 0 0 0 1 1 ] J 2 = − a 1 S 1 − a 2 S 12 / 2 − a 2 S 12 / 2 a 1 C 1 + a 2 C 12 / 2 a 2 C 12 / 2 0 0 ⋯ 0 0 0 0 1 1 J^(2)=[[-a_(1)S_(1)-a_(2)S_(12)//2,-a_(2)S_(12)//2],[a_(1)C_(1)+a_(2)C_(12)//2,a_(2)C_(12)//2],[0,0],[cdots],[0,0],[0,0],[1,1]]J^{2}=\left[\begin{array}{cc} -a_{1} S_{1}-a_{2} S_{12} / 2 & -a_{2} S_{12} / 2 \\ a_{1} C_{1}+a_{2} C_{12} / 2 & a_{2} C_{12} / 2 \\ 0 & 0 \\ \cdots \\ 0 & 0 \\ 0 & 0 \\ 1 & 1 \end{array}\right]
Partitioning J 2 J 2 J^(2)J^{2} into A 2 A 2 A^(2)A^{2} and B 2 B 2 B^(2)B^{2} as in Eq. (6-2-10), we update the expression for the manipulator inertia tensor in Eq. (6-5-7) by adding the contribution from link 2. Since n = 2 n = 2 n=2n=2, this generates the complete manipulator inertia tensor of the two-axis planar articulated robot:
將 J 2 J 2 J^(2)J^{2} 分割為 A 2 A 2 A^(2)A^{2} 和 B 2 B 2 B^(2)B^{2} ，如公式 (6-2-10) 所示，我們通過添加來自連桿 2 的貢獻來更新公式 (6-5-7) 中的操縱器慣性張量表達式。由於 n = 2 n = 2 n=2n=2 ，這生成了雙軸平面關節機器人的完整操縱器慣性張量：
D ( q ) = D ( q ) + ( A 2 ) T m 2 A 2 + ( B 2 ) T D 2 B 2 = D ( q ) + m 2 [ a 1 2 + a 1 a 2 C 2 + a 2 2 4 a 1 a 2 C 2 2 + a 2 2 4 a 1 a 2 C 2 2 + a 2 2 4 a 2 2 4 ] + m 2 a 2 2 12 [ 1 1 1 1 ] = [ m 1 a 1 2 3 + m 2 ( a 1 2 + a 1 a 2 C 2 + a 2 2 3 ) m 2 ( a 1 a 2 C 2 2 + a 2 2 3 ) m 2 ( a 1 a 2 C 2 2 + a 2 2 3 ) m 2 a 2 2 3 ] D ( q ) = D ( q ) + A 2 T m 2 A 2 + B 2 T D 2 B 2 = D ( q ) + m 2 a 1 2 + a 1 a 2 C 2 + a 2 2 4 a 1 a 2 C 2 2 + a 2 2 4 a 1 a 2 C 2 2 + a 2 2 4 a 2 2 4 + m 2 a 2 2 12 1 1 1 1 = m 1 a 1 2 3 + m 2 a 1 2 + a 1 a 2 C 2 + a 2 2 3 m 2 a 1 a 2 C 2 2 + a 2 2 3 m 2 a 1 a 2 C 2 2 + a 2 2 3 m 2 a 2 2 3 {:[D(q)=D(q)+(A^(2))^(T)m_(2)A^(2)+(B^(2))^(T)D_(2)B_(2)],[=D(q)+m_(2)[[a_(1)^(2)+a_(1)a_(2)C_(2)+(a_(2)^(2))/(4),(a_(1)a_(2)C_(2))/(2)+(a_(2)^(2))/(4)],[(a_(1)a_(2)C_(2))/(2)+(a_(2)^(2))/(4),(a_(2)^(2))/(4)]]+(m_(2)a_(2)^(2))/(12)[[1,1],[1,1]]],[=[[(m_(1)a_(1)^(2))/(3)+m_(2)(a_(1)^(2)+a_(1)a_(2)C_(2)+(a_(2)^(2))/(3)),m_(2)((a_(1)a_(2)C_(2))/(2)+(a_(2)^(2))/(3))],[m_(2)((a_(1)a_(2)C_(2))/(2)+(a_(2)^(2))/(3)),(m_(2)a_(2)^(2))/(3)]]]:}\begin{aligned} D(q) & =D(q)+\left(A^{2}\right)^{T} m_{2} A^{2}+\left(B^{2}\right)^{T} D_{2} B_{2} \\ & =D(q)+m_{2}\left[\begin{array}{cc} a_{1}^{2}+a_{1} a_{2} \mathrm{C}_{2}+\frac{a_{2}^{2}}{4} & \frac{a_{1} a_{2} \mathrm{C}_{2}}{2}+\frac{a_{2}^{2}}{4} \\ \frac{a_{1} a_{2} \mathrm{C}_{2}}{2}+\frac{a_{2}^{2}}{4} & \frac{a_{2}^{2}}{4} \end{array}\right]+\frac{m_{2} a_{2}^{2}}{12}\left[\begin{array}{ll} 1 & 1 \\ 1 & 1 \end{array}\right] \\ & =\left[\begin{array}{cc} \frac{m_{1} a_{1}^{2}}{3}+m_{2}\left(a_{1}^{2}+a_{1} a_{2} \mathrm{C}_{2}+\frac{a_{2}^{2}}{3}\right) & m_{2}\left(\frac{a_{1} a_{2} \mathrm{C}_{2}}{2}+\frac{a_{2}^{2}}{3}\right) \\ m_{2}\left(\frac{a_{1} a_{2} \mathrm{C}_{2}}{2}+\frac{a_{2}^{2}}{3}\right) & \frac{m_{2} a_{2}^{2}}{3} \end{array}\right] \end{aligned}
This completes the first pass of Algorithm 6-4-1. Next we apply steps 9 and 10. Setting i = 1 i = 1 i=1i=1 in Eq. (6-4-6) and using Eq. (6-5-14), the velocity coupling matrix for joint 1 is
這完成了演算法 6-4-1 的第一次運行。接下來我們應用步驟 9 和 10。在方程 (6-4-6) 中設置 i = 1 i = 1 i=1i=1 ，並使用方程 (6-5-14)，關節 1 的速度耦合矩陣為
C 1 = m 2 a 1 a 2 S 2 2 [ 0 0 − 2 − 1 ] C 1 = m 2 a 1 a 2 S 2 2 0 0 − 2 − 1 C^(1)=(m_(2)a_(1)a_(2)S_(2))/(2)[[0,0],[-2,-1]]C^{1}=\frac{m_{2} a_{1} a_{2} S_{2}}{2}\left[\begin{array}{rr} 0 & 0 \\ -2 & -1 \end{array}\right]
If we assume that the y 0 y 0 y^(0)y^{0} axis in Fig. 6-4 points straight up, directly opposing gravity, then g = [ 0 , − g 0 , 0 ] T g = 0 , − g 0 , 0 T g=[0,-g_(0),0]^(T)g=\left[0,-g_{0}, 0\right]^{T}, where g 0 = 9.8062 m / sec 2 g 0 = 9.8062 m / sec 2 g_(0)=9.8062m//sec^(2)g_{0}=9.8062 \mathrm{~m} / \mathrm{sec}^{2}. Applying Eq. (6-4-7) and using Eqs. (6-5-6) and (6-5-13), the gravitational loading on joint 1 is:
如果我們假設圖 6-4 中的 y 0 y 0 y^(0)y^{0} 軸直指向上方，正好對抗重力，那麼 g = [ 0 , − g 0 , 0 ] T g = 0 , − g 0 , 0 T g=[0,-g_(0),0]^(T)g=\left[0,-g_{0}, 0\right]^{T} ，其中 g 0 = 9.8062 m / sec 2 g 0 = 9.8062 m / sec 2 g_(0)=9.8062m//sec^(2)g_{0}=9.8062 \mathrm{~m} / \mathrm{sec}^{2} 。應用公式(6-4-7)並使用公式(6-5-6)和(6-5-13)，關節 1 上的重力負載為：
h 1 = g 0 ( m 1 A 21 1 + m 2 A 21 2 ) = g 0 [ ( m 1 2 + m 2 ) a 1 C 1 + m 2 a 2 C 12 2 ] h 1 = g 0 m 1 A 21 1 + m 2 A 21 2 = g 0 m 1 2 + m 2 a 1 C 1 + m 2 a 2 C 12 2 {:[h_(1)=g_(0)(m_(1)A_(21)^(1)+m_(2)A_(21)^(2))],[=g_(0)[((m_(1))/(2)+m_(2))a_(1)C_(1)+(m_(2)a_(2)C_(12))/(2)]]:}\begin{aligned} h_{1} & =g_{0}\left(m_{1} A_{21}^{1}+m_{2} A_{21}^{2}\right) \\ & =g_{0}\left[\left(\frac{m_{1}}{2}+m_{2}\right) a_{1} \mathrm{C}_{1}+\frac{m_{2} a_{2} \mathrm{C}_{12}}{2}\right] \end{aligned}
Finally, using Eqs. (6-5-14) through (6-5-16), we can formulate the dynamic model for the first joint of the manipulator using step 10, as follows:
最後，使用方程式 (6-5-14) 到 (6-5-16)，我們可以根據步驟 10 為機械手臂的第一個關節制定動態模型，如下所示：
τ 1 = [ ( m 1 3 + m 2 ) a 1 2 + m 2 a 1 a 2 C 2 + m 2 a 2 2 3 ] q ¨ 1 + ( m 2 a 1 a 2 C 2 2 + m 2 a 2 2 3 ) q ¨ 2 − m 2 a 1 a 2 S 2 ( q ˙ 1 q ˙ 2 + q ˙ 2 2 2 ) + g 0 [ ( m 1 2 + m 2 ) a 1 C 1 + m 2 a 2 C 12 2 ] + b 1 ( q ˙ 1 ) τ 1 = m 1 3 + m 2 a 1 2 + m 2 a 1 a 2 C 2 + m 2 a 2 2 3 q ¨ 1 + m 2 a 1 a 2 C 2 2 + m 2 a 2 2 3 q ¨ 2 − m 2 a 1 a 2 S 2 q ˙ 1 q ˙ 2 + q ˙ 2 2 2 + g 0 m 1 2 + m 2 a 1 C 1 + m 2 a 2 C 12 2 + b 1 q ˙ 1 {:[tau_(1)={:[((m_(1))/(3)+m_(2))a_(1)^(2)+m_(2)a_(1)a_(2)C_(2)+(m_(2)a_(2)^(2))/(3)]q^(¨)_(1)+((m_(2)a_(1)a_(2)C_(2))/(2)+(m_(2)a_(2)^(2))/(3))q^(¨)_(2):}],[-m_(2)a_(1)a_(2)S_(2)(q^(˙)_(1)q^(˙)_(2)+(q^(˙)_(2)^(2))/(2))+g_(0)[((m_(1))/(2)+m_(2))a_(1)C_(1)+(m_(2)a_(2)C_(12))/(2)]+b_(1)(q^(˙)_(1))]:}\begin{aligned} \tau_{1}= & {\left[\left(\frac{m_{1}}{3}+m_{2}\right) a_{1}^{2}+m_{2} a_{1} a_{2} \mathrm{C}_{2}+\frac{m_{2} a_{2}^{2}}{3}\right] \ddot{q}_{1}+\left(\frac{m_{2} a_{1} a_{2} \mathrm{C}_{2}}{2}+\frac{m_{2} a_{2}^{2}}{3}\right) \ddot{q}_{2} } \\ & -m_{2} a_{1} a_{2} \mathrm{~S}_{2}\left(\dot{q}_{1} \dot{q}_{2}+\frac{\dot{q}_{2}^{2}}{2}\right)+g_{0}\left[\left(\frac{m_{1}}{2}+m_{2}\right) a_{1} \mathrm{C}_{1}+\frac{m_{2} a_{2} \mathrm{C}_{12}}{2}\right]+b_{1}\left(\dot{q}_{1}\right) \end{aligned}
The equation for the first joint is typically the most complex, because its inertial and gravitational terms will depend on the remaining links all the way out to the end of the arm. As we proceed toward the end of the arm, the dynamic equations become progressively simpler. We again apply steps 9 and 10 to produce the dynamic equation for joint 2 . Setting i = 2 i = 2 i=2i=2 in Eq. (6-4-6) and using Eq. (6-5-14), the velocity coupling matrix for joint 2 is:
第一個關節的方程式通常是最複雜的，因為它的慣性和重力項將依賴於所有剩餘的連桿，直到手臂的末端。隨著我們向手臂的末端推進，動態方程變得逐漸簡單。我們再次應用步驟 9 和 10 來產生關節 2 的動態方程。將 i = 2 i = 2 i=2i=2 設置在方程(6-4-6)中，並使用方程(6-5-14)，關節 2 的速度耦合矩陣為：
C 2 = m 2 a 1 a 2 S 2 4 [ 2 1 − 1 0 ] C 2 = m 2 a 1 a 2 S 2 4 2 1 − 1 0 C^(2)=(m_(2)a_(1)a_(2)S_(2))/(4)[[2,1],[-1,0]]C^{2}=\frac{m_{2} a_{1} a_{2} S_{2}}{4}\left[\begin{array}{rr} 2 & 1 \\ -1 & 0 \end{array}\right]
Applying Eq. (6-4-7) and using Eq. (6-5-13), the gravitational loading on joint 2 is then:
應用公式 (6-4-7) 並使用公式 (6-5-13)，關節 2 的重力負載為：
h 2 = g 0 m 2 A 22 2 = g 0 m 2 a 2 C 12 2 h 2 = g 0 m 2 A 22 2 = g 0 m 2 a 2 C 12 2 {:[h_(2)=g_(0)m_(2)A_(22)^(2)],[=(g_(0)m_(2)a_(2)C_(12))/(2)]:}\begin{aligned} h_{2} & =g_{0} m_{2} A_{22}^{2} \\ & =\frac{g_{0} m_{2} a_{2} \mathrm{C}_{12}}{2} \end{aligned}
Finally, using Eqs. (6-5-14), (6-5-18), and (6-5-19), we can formulate the dynamic model for the second joint of the manipulator using step 10, as follows:
最後，使用方程式 (6-5-14)、(6-5-18) 和 (6-5-19)，我們可以根據步驟 10 為機械手的第二個關節制定動態模型，如下所示：
τ 2 = ( m 2 a 1 a 2 C 2 2 + m 2 a 2 2 3 ) q ¨ 1 + m 2 a 2 2 q ¨ 2 3 + m 2 a 1 a 2 S 2 q ˙ 1 2 2 + g 0 m 2 a 2 C 12 2 + b 2 ( q ˙ 2 ) τ 2 = m 2 a 1 a 2 C 2 2 + m 2 a 2 2 3 q ¨ 1 + m 2 a 2 2 q ¨ 2 3 + m 2 a 1 a 2 S 2 q ˙ 1 2 2 + g 0 m 2 a 2 C 12 2 + b 2 q ˙ 2 {:[tau_(2)=((m_(2)a_(1)a_(2)C_(2))/(2)+(m_(2)a_(2)^(2))/(3))q^(¨)_(1)+(m_(2)a_(2)^(2)q^(¨)_(2))/(3)+(m_(2)a_(1)a_(2)S_(2)q^(˙)_(1)^(2))/(2)],[+(g_(0)m_(2)a_(2)C_(12))/(2)+b_(2)(q^(˙)_(2))]:}\begin{aligned} \tau_{2}= & \left(\frac{m_{2} a_{1} a_{2} \mathrm{C}_{2}}{2}+\frac{m_{2} a_{2}^{2}}{3}\right) \ddot{q}_{1}+\frac{m_{2} a_{2}^{2} \ddot{q}_{2}}{3}+\frac{m_{2} a_{1} a_{2} \mathrm{~S}_{2} \dot{q}_{1}^{2}}{2} \\ & +\frac{g_{0} m_{2} a_{2} \mathrm{C}_{12}}{2}+b_{2}\left(\dot{q}_{2}\right) \end{aligned}
Clearly, the dynamic equation for joint 2 is simpler than the dynamic equation for joint 1 , as expected. However, it is evident that even for this simple two-axis robot, the dynamic model in Eqs. (6-5-17) and (6-5-20) is fairly complex. The robot in Fig. 6-4 demonstrates all of the essential features of robot dynamics, including inertial forces, Coriolis and centrifugal velocity coupling, gravitational effects, and friction.
顯然，關節 2 的動態方程比關節 1 的動態方程簡單，這是預期的。然而，很明顯，即使對於這個簡單的雙軸機器人，方程式(6-5-17)和(6-5-20)中的動態模型也相當複雜。圖 6-4 中的機器人展示了機器人動力學的所有基本特徵，包括慣性力、科里奧利和離心速度耦合、重力效應和摩擦。

6-6 DYNAMIC MODEL OF A THREE-AXIS SCARA ROBOT
三軸 SCARA 機器人的動態模型

As a second example of a complete dynamic model of a robotic arm, consider the SCARA robot shown in Fig. 6-5. The first three axes of a SCARA robot position the tool tip, while the fourth axis orients the tool through a roll motion. To keep the dynamic model relatively simple, we assume that the mass of the fourth link and any tool attached to it are sufficiently small in comparison with the masses of the other links that they can be ignored. This is not an unreasonable assumption, because the links of a robotic manipulator tend to become progressively smaller and less massive
作為機器手臂完整動態模型的第二個例子，考慮圖 6-5 中顯示的 SCARA 機器人。SCARA 機器人的前三個軸定位工具尖端，而第四個軸則通過滾動運動來定向工具。為了保持動態模型相對簡單，我們假設第四個連桿及其上任何附加的工具的質量與其他連桿的質量相比足夠小，因此可以忽略。這並不是一個不合理的假設，因為機器人操作器的連桿往往會逐漸變小且質量減輕。
Figure 6-5 A three-axis SCARA robot.
圖 6-5 三軸 SCARA 機器人。
as one proceeds from the base joint to the tool. We assume that the three links are thin cylinders or rods of mass m 1 , m 2 m 1 , m 2 m_(1),m_(2)m_{1}, m_{2}, and m 3 m 3 m_(3)m_{3}, respectively.
隨著從基部關節到工具的進行。我們假設這三個連桿是質量分別為 m 1 , m 2 m 1 , m 2 m_(1),m_(2)m_{1}, m_{2} 和 m 3 m 3 m_(3)m_{3} 的細圓柱或棒。
Applying step 1 of Algorithm 6-4-1 yields the link-coordinate diagram shown in Fig. 6-5. Note that the link-coordinate frames have been assigned in such a manner that the axes of frame L k L k L_(k)L_{k} are aligned with the principal axes of rod m k m k m_(k)m_{k} for 1 ≤ k ≤ 3 1 ≤ k ≤ 3 1 <= k <= 31 \leq k \leq 3. In this case the vector of joint variables is q = [ θ 1 , θ 2 , d 3 ] T q = θ 1 , θ 2 , d 3 T q=[theta_(1),theta_(2),d_(3)]^(T)q=\left[\theta_{1}, \theta_{2}, d_{3}\right]^{T}. The kinematic parameters of the three-axis SCARA robot are listed in Table 6-2.
應用算法 6-4-1 的第 1 步驟產生了圖 6-5 所示的連桿坐標圖。請注意，連桿坐標框架的分配方式使得框架 L k L k L_(k)L_{k} 的軸與桿 m k m k m_(k)m_{k} 的主軸對齊，針對 1 ≤ k ≤ 3 1 ≤ k ≤ 3 1 <= k <= 31 \leq k \leq 3 。在這種情況下，關節變量的向量為 q = [ θ 1 , θ 2 , d 3 ] T q = θ 1 , θ 2 , d 3 T q=[theta_(1),theta_(2),d_(3)]^(T)q=\left[\theta_{1}, \theta_{2}, d_{3}\right]^{T} 。三軸 SCARA 機器人的運動參數列在表 6-2 中。
TABLE 6-2 KINEMATIC PARAMETERS OF THE THREE-AXIS SCARA ROBOT
表 6-2 三軸 SCARA 機器人的運動參數
Joint 關節 d d dd a a aa θ θ theta\theta α α alpha\alpha Home 家
1 d 1 d 1 d_(1)d_{1} a 1 a 1 a_(1)a_{1} q 1 q 1 q_(1)q_{1} π π pi\pi 0
2 0 a 2 a 2 a_(2)a_{2} q 1 q 1 q_(1)q_{1} 0 0
3 q 3 q 3 q_(3)q_{3} 0 0 0 l 3 / 2 l 3 / 2 l_(3)//2l_{3} / 2
Joint d a theta alpha Home 1 d_(1) a_(1) q_(1) pi 0 2 0 a_(2) q_(1) 0 0 3 q_(3) 0 0 0 l_(3)//2| Joint | $d$ | $a$ | $\theta$ | $\alpha$ | Home | | :--- | :--- | :--- | :--- | :--- | :---: | | 1 | $d_{1}$ | $a_{1}$ | $q_{1}$ | $\pi$ | 0 | | 2 | 0 | $a_{2}$ | $q_{1}$ | 0 | 0 | | 3 | $q_{3}$ | 0 | 0 | 0 | $l_{3} / 2$ |
Next we apply steps 3 through 7 of Algorithm 6-4-1 with i = 1 i = 1 i=1i=1. Note from Fig. 6-5 that the vertical column of height d 1 d 1 d_(1)d_{1} is stationary; it is only the rod of length a 1 a 1 a_(1)a_{1} and mass m 1 m 1 m_(1)m_{1} that rotates when joint 1 is activated. Assuming that the links are homogeneous, we see from Fig. 6-5 that the center of mass of link 1 in frame L 1 L 1 L_(1)L_{1} coordinates is:
接下來，我們應用算法 6-4-1 的第 3 步到第 7 步，使用 i = 1 i = 1 i=1i=1 。從圖 6-5 可以看出，高度為 d 1 d 1 d_(1)d_{1} 的垂直柱是靜止的；只有當關節 1 被激活時，長度為 a 1 a 1 a_(1)a_{1} 和質量為 m 1 m 1 m_(1)m_{1} 的桿才會旋轉。假設連桿是均勻的，我們從圖 6-5 可以看出，連桿 1 在框架 L 1 L 1 L_(1)L_{1} 坐標系中的質心為：
Δ c 1 = [ − a 1 2 , 0 , 0 , 1 ] T Δ c 1 = − a 1 2 , 0 , 0 , 1 T Deltac^(1)=[-(a_(1))/(2),0,0,1]^(T)\Delta c^{1}=\left[-\frac{a_{1}}{2}, 0,0,1\right]^{T}
Let L c L c L_(c)L_{c} denote the coordinate frame obtained by translating frame L 1 L 1 L_(1)L_{1} along axis x 1 x 1 x^(1)x^{1} by an amount − a 1 / 2 . L c − a 1 / 2 . L c -a_(1)//2.L_(c)-a_{1} / 2 . L_{c} is then at the center of mass of link 1 , and the axes of L c L c L_(c)L_{c}
讓 L c L c L_(c)L_{c} 表示通過沿著軸 x 1 x 1 x^(1)x^{1} 移動 L 1 L 1 L_(1)L_{1} 框架 − a 1 / 2 . L c − a 1 / 2 . L c -a_(1)//2.L_(c)-a_{1} / 2 . L_{c} 的坐標框架，然後位於連桿 1 的質心，並且 L c L c L_(c)L_{c} 的軸
are aligned with the principal axes of link 1 . If D ¯ 1 D ¯ 1 bar(D)_(1)\bar{D}_{1} is the inertia tensor of link 1 about its center of mass with respect to frame L c L c L_(c)L_{c}, then, from Appendix 2:
與連接 1 的主軸對齊。如果 D ¯ 1 D ¯ 1 bar(D)_(1)\bar{D}_{1} 是連接 1 相對於框架 L c L c L_(c)L_{c} 的質心慣性張量，那麼，根據附錄 2：
D ¯ 1 = ( m 1 a 1 2 12 ) diag { 0 , 1 , 1 } D ¯ 1 = m 1 a 1 2 12 diag { 0 , 1 , 1 } bar(D)_(1)=((m_(1)a_(1)^(2))/(12))diag{0,1,1}\bar{D}_{1}=\left(\frac{m_{1} a_{1}^{2}}{12}\right) \operatorname{diag}\{0,1,1\}
Next we apply step 5 where z 0 = i 3 z 0 = i 3 z^(0)=i^(3)z^{0}=i^{3}. Using Table 6-2 and Prop. 2-6-1, or using Eq. (2-8-3) directly, the homogeneous coordinate transformation matrix for link 1 is:
接下來我們應用第 5 步，其中 z 0 = i 3 z 0 = i 3 z^(0)=i^(3)z^{0}=i^{3} 。使用表 6-2 和性質 2-6-1，或直接使用公式(2-8-3)，連桿 1 的齊次坐標變換矩陣為：
T 0 1 = [ C 1 S 1 0 a 1 C 1 S 1 − C 1 0 a 1 S 1 0 0 − 1 d 1 0 0 0 1 ] T 0 1 = C 1 S 1 0 a 1 C 1 S 1 − C 1 0 a 1 S 1 0 0 − 1 d 1 0 0 0 1 T_(0)^(1)=[[C_(1),S_(1),0,a_(1)C_(1)],[S_(1),-C_(1),0,a_(1)S_(1)],[0,0,-1,d_(1)],[0,0,0,1]]T_{0}^{1}=\left[\begin{array}{ccc:c} \mathrm{C}_{1} & \mathrm{~S}_{1} & 0 & a_{1} \mathrm{C}_{1} \\ \mathrm{~S}_{1} & -\mathrm{C}_{1} & 0 & a_{1} \mathrm{~S}_{1} \\ 0 & 0 & -1 & d_{1} \\ \hdashline 0 & 0 & 0 & 1 \end{array}\right]
Combining. Eqs. (6-6-1) and (6-6-3) and recalling Eq. (2-4-1), the center of mass of link 1 in base frame coordinates is:
結合方程式 (6-6-1) 和 (6-6-3)，並回顧方程式 (2-4-1)，連桿 1 在基座框架坐標中的質心為：
c ¯ 1 = H 1 T 0 1 Δ c 1 = [ a 1 C 1 2 , a 1 S 1 2 , d 1 ] T c ¯ 1 = H 1 T 0 1 Δ c 1 = a 1 C 1 2 , a 1 S 1 2 , d 1 T {:[ bar(c)^(1)=H_(1)T_(0)^(1)Deltac^(1)],[=[(a_(1)C_(1))/(2),(a_(1)S_(1))/(2),d_(1)]^(T)]:}\begin{aligned} \bar{c}^{1} & =H_{1} T_{0}^{1} \Delta c^{1} \\ & =\left[\frac{a_{1} \mathrm{C}_{1}}{2}, \frac{a_{1} \mathrm{~S}_{1}}{2}, d_{1}\right]^{T} \end{aligned}
Finally, using Eq. (6-6-2) and the rotation part of T 0 1 T 0 1 T_(0)^(1)T_{0}^{1} in Eq. (6-6-3), the inertia tensor of link 1 in base coordinates is:
最後，使用公式 (6-6-2) 和公式 (6-6-3) 中 T 0 1 T 0 1 T_(0)^(1)T_{0}^{1} 的旋轉部分，基座坐標中連桿 1 的慣性張量為：
D 1 = R 0 1 D ¯ 1 ( R 0 1 ) T = m 1 a 1 2 12 [ S 1 2 − S 1 C 1 0 − S 1 C 1 C 1 2 0 0 0 1 ] D 1 = R 0 1 D ¯ 1 R 0 1 T = m 1 a 1 2 12 S 1 2 − S 1 C 1 0 − S 1 C 1 C 1 2 0 0 0 1 {:[D_(1)=R_(0)^(1) bar(D)_(1)(R_(0)^(1))^(T)],[=(m_(1)a_(1)^(2))/(12)[[S_(1)^(2),-S_(1)C_(1),0],[-S_(1)C_(1),C_(1)^(2),0],[0,0,1]]]:}\begin{aligned} D_{1} & =R_{0}^{1} \bar{D}_{1}\left(R_{0}^{1}\right)^{T} \\ & =\frac{m_{1} a_{1}^{2}}{12}\left[\begin{array}{ccc} \mathrm{S}_{1}^{2} & -\mathrm{S}_{1} \mathrm{C}_{1} & 0 \\ -\mathrm{S}_{1} \mathrm{C}_{1} & \mathrm{C}_{1}^{2} & 0 \\ 0 & 0 & 1 \end{array}\right] \end{aligned}
Next, the Jacobian matrix for link 1 is computed in step 6. Using Eqs. (6-2-10) and (6-6-4), and recalling that z 0 = i 3 z 0 = i 3 z^(0)=i^(3)z^{0}=i^{3} and ξ 1 = 1 ξ 1 = 1 xi_(1)=1\xi_{1}=1, we have the following 6 × 3 6 × 3 6xx36 \times 3 link Jacobian matrix:
接下來，連結 1 的雅可比矩陣在第 6 步中計算。使用方程式 (6-2-10) 和 (6-6-4)，並回想 z 0 = i 3 z 0 = i 3 z^(0)=i^(3)z^{0}=i^{3} 和 ξ 1 = 1 ξ 1 = 1 xi_(1)=1\xi_{1}=1 ，我們得到了以下 6 × 3 6 × 3 6xx36 \times 3 連結的雅可比矩陣：
J 1 = [ − a 1 S 1 / 2 0 0 a 1 C 1 / 2 0 0 0 0 0 ⋯ 0 0 0 0 0 0 0 0 ] J 1 = − a 1 S 1 / 2 0 0 a 1 C 1 / 2 0 0 0 0 0 ⋯ 0 0 0 0 0 0 0 0 J^(1)=[[-a_(1)S_(1)//2,0,0],[a_(1)C_(1)//2,0,0],[0,0,0],[cdots,0,0],[0,0,0],[0,0,0]]J^{1}=\left[\begin{array}{ccc} -a_{1} \mathrm{~S}_{1} / 2 & 0 & 0 \\ a_{1} \mathrm{C}_{1} / 2 & 0 & 0 \\ 0 & 0 & 0 \\ \hdashline \cdots & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{array}\right]
Partitioning J 1 J 1 J^(1)J^{1} into A 1 A 1 A^(1)A^{1} and B 1 B 1 B^(1)B^{1} as in Eq. (6-2-10), the contribution to the manipulator inertia tensor from link 1 is:
將 J 1 J 1 J^(1)J^{1} 分割為 A 1 A 1 A^(1)A^{1} 和 B 1 B 1 B^(1)B^{1} ，如公式 (6-2-10) 所示，來自連桿 1 的操控器慣性張量的貢獻為：
D ( q ) = ( A 1 ) T m 1 A 1 + ( B 1 ) T D 1 B 1 = ( m 1 a 1 2 4 ) diag { 1 , 0 , 0 } + ( m 1 a 1 2 12 ) diag { 1 , 0 , 0 } D ( q ) = A 1 T m 1 A 1 + B 1 T D 1 B 1 = m 1 a 1 2 4 diag { 1 , 0 , 0 } + m 1 a 1 2 12 diag { 1 , 0 , 0 } {:[D(q)=(A^(1))^(T)m_(1)A^(1)+(B^(1))^(T)D_(1)B^(1)],[=((m_(1)a_(1)^(2))/(4))diag{1","0","0}+((m_(1)a_(1)^(2))/(12))diag{1","0","0}]:}\begin{aligned} D(q) & =\left(A^{1}\right)^{T} m_{1} A^{1}+\left(B^{1}\right)^{T} D_{1} B^{1} \\ & =\left(\frac{m_{1} a_{1}^{2}}{4}\right) \operatorname{diag}\{1,0,0\}+\left(\frac{m_{1} a_{1}^{2}}{12}\right) \operatorname{diag}\{1,0,0\} \end{aligned}
= ( m 1 a 1 2 3 ) diag { 1 , 0 , 0 } = m 1 a 1 2 3 diag { 1 , 0 , 0 } =((m_(1)a_(1)^(2))/(3))diag{1,0,0}=\left(\frac{m_{1} a_{1}^{2}}{3}\right) \operatorname{diag}\{1,0,0\}
This completes the analysis for link 1. Next, we set i = 2 i = 2 i=2i=2 and repeat steps 3 through 7. From Fig. 6-5, the center of mass of link 2 in frame L 2 L 2 L_(2)L_{2} coordinates is:
這完成了對連桿 1 的分析。接下來，我們設置 i = 2 i = 2 i=2i=2 並重複步驟 3 到 7。從圖 6-5 中，連桿 2 在框架 L 2 L 2 L_(2)L_{2} 坐標系中的質心是：
Δ c 2 = [ − a 2 2 , 0 , 0 , 1 ] T Δ c 2 = − a 2 2 , 0 , 0 , 1 T Deltac^(2)=[-(a_(2))/(2),0,0,1]^(T)\Delta c^{2}=\left[-\frac{a_{2}}{2}, 0,0,1\right]^{T}
Again, let L c L c L_(c)L_{c} denote the coordinate frame obtained by translating frame L 2 L 2 L_(2)L_{2} along axis x 2 x 2 x^(2)x^{2} by an amount − a 2 / 2 . L c − a 2 / 2 . L c -a_(2)//2.L_(c)-a_{2} / 2 . L_{c} is then at the center of mass of link 2 , and the axes of L c L c L_(c)L_{c} are aligned with the principal axes of link 2 . If D ¯ 2 D ¯ 2 bar(D)_(2)\bar{D}_{2} is the inertia tensor of link 2 about its center of mass with respect to frame L c L c L_(c)L_{c}, then, from Appendix 2:
再次，讓 L c L c L_(c)L_{c} 表示通過沿著軸 x 2 x 2 x^(2)x^{2} 移動 L 2 L 2 L_(2)L_{2} 框架 − a 2 / 2 . L c − a 2 / 2 . L c -a_(2)//2.L_(c)-a_{2} / 2 . L_{c} 的量而獲得的坐標框架，然後位於連桿 2 的質心處，並且 L c L c L_(c)L_{c} 的軸與連桿 2 的主軸對齊。如果 D ¯ 2 D ¯ 2 bar(D)_(2)\bar{D}_{2} 是關於其質心相對於框架 L c L c L_(c)L_{c} 的連桿 2 的慣性張量，那麼，根據附錄 2：
D → 2 = ( m 2 a 2 2 12 ) diag { 0 , 1 , 1 } D → 2 = m 2 a 2 2 12 diag { 0 , 1 , 1 } vec(D)_(2)=((m_(2)a_(2)^(2))/(12))diag{0,1,1}\vec{D}_{2}=\left(\frac{m_{2} a_{2}^{2}}{12}\right) \operatorname{diag}\{0,1,1\}
Next, we apply step 5. Extracting the third column of R 0 1 R 0 1 R_(0)^(1)R_{0}^{1} in Eq. (6-6-3) yields z 1 = R 0 1 i 3 = − i 3 z 1 = R 0 1 i 3 = − i 3 z^(1)=R_(0)^(1)i^(3)=-i^(3)z^{1}=R_{0}^{1} i^{3}=-i^{3}. From Table 6-2, Prop. 2-6-1, and Eq. (6-6-3), or from Eq. (2-8-3) directly, the homogeneous coordinate transformation matrix for link 2 is:
接下來，我們應用步驟 5。從方程式(6-6-3)中提取 R 0 1 R 0 1 R_(0)^(1)R_{0}^{1} 的第三列得到 z 1 = R 0 1 i 3 = − i 3 z 1 = R 0 1 i 3 = − i 3 z^(1)=R_(0)^(1)i^(3)=-i^(3)z^{1}=R_{0}^{1} i^{3}=-i^{3} 。從表 6-2、命題 2-6-1 和方程式(6-6-3)，或直接從方程式(2-8-3)，連桿 2 的齊次坐標變換矩陣為：
T 0 2 = [ C 1 − 2 S 1 − 2 0 a 1 C 1 + a 2 C 1 − 2 S 1 − 2 − C 1 − 2 0 a 1 S 1 + a 2 S 1 − 2 0 0 − 1 d 1 − − − − − − − − − − − − − − − − − − ] T 0 2 = C 1 − 2 S 1 − 2 0 a 1 C 1 + a 2 C 1 − 2 S 1 − 2 − C 1 − 2 0 a 1 S 1 + a 2 S 1 − 2 0 0 − 1 d 1 − − − − − − − − − − − − − − − − − − T_(0)^(2)=[[C_(1-2),S_(1-2),0,a_(1)C_(1)+a_(2)C_(1-2)],[S_(1-2),-C_(1-2),0,a_(1)S_(1)+a_(2)S_(1-2)],[0,0,-1,d_(1)],[------------------]]T_{0}^{2}=\left[\begin{array}{ccc:c} \mathrm{C}_{1-2} & \mathrm{~S}_{1-2} & 0 & a_{1} \mathrm{C}_{1}+a_{2} \mathrm{C}_{1-2} \\ \mathrm{~S}_{1-2} & -\mathrm{C}_{1-2} & 0 & a_{1} \mathrm{~S}_{1}+a_{2} \mathrm{~S}_{1-2} \\ 0 & 0 & -1 & d_{1} \\ \hdashline------------------ \end{array}\right]
Combining Eqs. (6-6-8) and (6-6-10), the center of mass of link 2 in base frame coordinates is:
結合方程式 (6-6-8) 和 (6-6-10)，連桿 2 在基座框架坐標系中的質心為：
c ¯ 2 ^ = H 1 T 0 2 Δ c 2 = [ a 1 C 1 + a 2 C 1 − 2 2 , a 1 S 1 + a 2 S 1 − 2 2 , d 1 ] T c ¯ 2 ^ = H 1 T 0 2 Δ c 2 = a 1 C 1 + a 2 C 1 − 2 2 , a 1 S 1 + a 2 S 1 − 2 2 , d 1 T {:[ bar(c)^( hat(2))=H_(1)T_(0)^(2)Deltac^(2)],[=[a_(1)C_(1)+(a_(2)C_(1-2))/(2),a_(1)S_(1)+(a_(2)S_(1-2))/(2),d_(1)]^(T)]:}\begin{aligned} \bar{c}^{\hat{2}} & =H_{1} T_{0}^{2} \Delta \mathrm{c}^{2} \\ & =\left[a_{1} \mathrm{C}_{1}+\frac{a_{2} \mathrm{C}_{1-2}}{2}, a_{1} \mathrm{~S}_{1}+\frac{a_{2} \mathrm{~S}_{1-2}}{2}, d_{1}\right]^{T} \end{aligned}
Finally, using Eq. (6-6-9) and the rotation part of T 0 2 T 0 2 T_(0)^(2)T_{0}^{2} in Eq. (6-6-10), the inertia tensor of link 2 in base coordinates is:
最後，使用公式 (6-6-9) 和公式 (6-6-10) 中 T 0 2 T 0 2 T_(0)^(2)T_{0}^{2} 的旋轉部分，連桿 2 在基座坐標中的慣性張量為：
D 2 = R 0 2 D ¯ 2 ( R 0 2 ) T = m 2 a 2 2 12 [ S 1 − 2 2 − S 1 − 2 C 1 − 2 0 − S 1 − 2 C 1 − 2 C 1 − 2 2 0 0 0 1 ] D 2 = R 0 2 D ¯ 2 R 0 2 T = m 2 a 2 2 12 S 1 − 2 2 − S 1 − 2 C 1 − 2 0 − S 1 − 2 C 1 − 2 C 1 − 2 2 0 0 0 1 {:[D_(2)=R_(0)^(2) bar(D)_(2)(R_(0)^(2))^(T)],[=(m_(2)a_(2)^(2))/(12)[[S_(1-2)^(2),-S_(1-2)C_(1-2),0],[-S_(1-2)C_(1-2),C_(1-2)^(2),0],[0,0,1]]]:}\begin{aligned} D_{2} & =R_{0}^{2} \bar{D}_{2}\left(R_{0}^{2}\right)^{T} \\ & =\frac{m_{2} a_{2}^{2}}{12}\left[\begin{array}{ccc} S_{1-2}^{2} & -S_{1-2} C_{1-2} & 0 \\ -S_{1-2} \mathrm{C}_{1-2} & \mathrm{C}_{1-2}^{2} & 0 \\ 0 & 0 & 1 \end{array}\right] \end{aligned}
Next, we compute the Jacobian matrix for link 2 in step 6. Using Eqs. (6-2-10) and (6-6-11), and recalling that z 1 = − i 3 z 1 = − i 3 z^(1)=-i^(3)z^{1}=-i^{3} and ξ 2 = 1 ξ 2 = 1 xi_(2)=1\xi_{2}=1, we have the following 6 × 3 6 × 3 6xx36 \times 3 link Jacobian matrix:
接下來，我們計算第 6 步中連結 2 的雅可比矩陣。使用方程式(6-2-10)和(6-6-11)，並回想 z 1 = − i 3 z 1 = − i 3 z^(1)=-i^(3)z^{1}=-i^{3} 和 ξ 2 = 1 ξ 2 = 1 xi_(2)=1\xi_{2}=1 ，我們得到了以下 6 × 3 6 × 3 6xx36 \times 3 連結的雅可比矩陣：
J 2 = [ − a 1 S 1 − a 2 S 1 − 2 / 2 a 2 S 1 − 2 / 2 0 a 1 C 1 + a 2 C 1 − 2 / 2 − a 2 C 1 − 2 / 2 0 0 0 0 ⋯ 0 0 0 0 0 0 − 1 0 ] J 2 = − a 1 S 1 − a 2 S 1 − 2 / 2 a 2 S 1 − 2 / 2 0 a 1 C 1 + a 2 C 1 − 2 / 2 − a 2 C 1 − 2 / 2 0 0 0 0 ⋯ 0 0 0 0 0 0 − 1 0 J^(2)=[[-a_(1)S_(1)-a_(2)S_(1-2)//2,a_(2)S_(1-2)//2,0],[a_(1)C_(1)+a_(2)C_(1-2)//2,-a_(2)C_(1-2)//2,0],[0,0,0],[cdots,0,0],[0,0,0],[0,-1,0]]J^{2}=\left[\begin{array}{ccc} -a_{1} S_{1}-a_{2} S_{1-2} / 2 & a_{2} S_{1-2} / 2 & 0 \\ a_{1} C_{1}+a_{2} C_{1-2} / 2 & -a_{2} C_{1-2} / 2 & 0 \\ 0 & 0 & 0 \\ \cdots & 0 & 0 \\ 0 & 0 & 0 \\ 0 & -1 & 0 \end{array}\right]
Partitioning J 2 J 2 J^(2)J^{2} into A 2 A 2 A^(2)A^{2} and B 2 B 2 B^(2)B^{2} as in Eq. (6-2-10), we can add the contribution to the manipulator inertia tensor from link 2 to the previous expression in Eq. (6-6-7):
將 J 2 J 2 J^(2)J^{2} 分割為 A 2 A 2 A^(2)A^{2} 和 B 2 B 2 B^(2)B^{2} ，如公式 (6-2-10) 所示，我們可以將來自連桿 2 的貢獻添加到公式 (6-6-7) 中的操縱器慣性張量的先前表達式中：
D ( q ) = D ( q ) + ( A 2 ) T m 2 A 2 + ( B 2 ) T D 2 B 2 = D ( q ) + m 2 [ a 1 2 + a 1 a 2 C 2 + a 2 2 4 − ( a 1 a 2 C 2 2 + a 2 2 4 ) 0 − ( a 1 a 2 C 2 2 + a 2 2 4 ) a 2 2 4 0 0 0 0 ] + m 2 a 2 2 12 [ 1 − 1 0 − 1 1 0 0 0 0 ] = [ ( m 1 3 + m 2 ) a 1 2 + m 2 C 2 a 1 a 2 + m 2 a 2 2 3 − m 2 a 1 a 2 C 2 2 − m 2 a 2 2 3 0 − m 2 a 1 a 2 C 2 2 − m 2 a 2 2 3 m 2 a 2 2 3 0 0 0 0 ] D ( q ) = D ( q ) + A 2 T m 2 A 2 + B 2 T D 2 B 2 = D ( q ) + m 2 a 1 2 + a 1 a 2 C 2 + a 2 2 4 − a 1 a 2 C 2 2 + a 2 2 4 0 − a 1 a 2 C 2 2 + a 2 2 4 a 2 2 4 0 0 0 0 + m 2 a 2 2 12 1 − 1 0 − 1 1 0 0 0 0 = m 1 3 + m 2 a 1 2 + m 2 C 2 a 1 a 2 + m 2 a 2 2 3 − m 2 a 1 a 2 C 2 2 − m 2 a 2 2 3 0 − m 2 a 1 a 2 C 2 2 − m 2 a 2 2 3 m 2 a 2 2 3 0 0 0 0 {:[D(q)=D(q)+(A^(2))^(T)m_(2)A^(2)+(B^(2))^(T)D_(2)B^(2)],[=D(q)+m_(2)[[a_(1)^(2)+a_(1)a_(2)C_(2)+(a_(2)^(2))/(4),-((a_(1)a_(2)C_(2))/(2)+(a_(2)^(2))/(4)),0],[-((a_(1)a_(2)C_(2))/(2)+(a_(2)^(2))/(4)),(a_(2)^(2))/(4),0],[0,0,0]]+(m_(2)a_(2)^(2))/(12)[[1,-1,0],[-1,1,0],[0,0,0]]],[=[[((m_(1))/(3)+m_(2))a_(1)^(2)+m_(2)C_(2)a_(1)a_(2)+(m_(2)a_(2)^(2))/(3),-(m_(2)a_(1)a_(2)C_(2))/(2)-(m_(2)a_(2)^(2))/(3),0],[-(m_(2)a_(1)a_(2)C_(2))/(2)-(m_(2)a_(2)^(2))/(3),(m_(2)a_(2)^(2))/(3),0],[0,0,0]]]:}\begin{aligned} D(q) & =D(q)+\left(A^{2}\right)^{T} m_{2} A^{2}+\left(B^{2}\right)^{T} D_{2} B^{2} \\ & =D(q)+m_{2}\left[\begin{array}{ccc} a_{1}^{2}+a_{1} a_{2} \mathrm{C}_{2}+\frac{a_{2}^{2}}{4} & -\left(\frac{a_{1} a_{2} \mathrm{C}_{2}}{2}+\frac{a_{2}^{2}}{4}\right) & 0 \\ -\left(\frac{a_{1} a_{2} \mathrm{C}_{2}}{2}+\frac{a_{2}^{2}}{4}\right) & \frac{a_{2}^{2}}{4} & 0 \\ 0 & 0 & 0 \end{array}\right]+\frac{m_{2} a_{2}^{2}}{12}\left[\begin{array}{rrr} 1 & -1 & 0 \\ -1 & 1 & 0 \\ 0 & 0 & 0 \end{array}\right] \\ & =\left[\begin{array}{ccc} \left(\frac{m_{1}}{3}+m_{2}\right) a_{1}^{2}+m_{2} \mathrm{C}_{2} a_{1} a_{2}+\frac{m_{2} a_{2}^{2}}{3} & -\frac{m_{2} a_{1} a_{2} \mathrm{C}_{2}}{2}-\frac{m_{2} a_{2}^{2}}{3} & 0 \\ -\frac{m_{2} a_{1} a_{2} \mathrm{C}_{2}}{2}-\frac{m_{2} a_{2}^{2}}{3} & \frac{m_{2} a_{2}^{2}}{3} & 0 \\ 0 & 0 & 0 \end{array}\right] \end{aligned}
This completes the analysis for link 2 . Next, we set i = 3 i = 3 i=3i=3 and repeat steps 3 through 7. Unlike the first two joints, which are revolute, the third joint of the SCARA robot is prismatic. If we assume that the entire rod of length l 3 l 3 l_(3)l_{3} and mass m 3 m 3 m_(3)m_{3} slides up and down when joint 3 is activated, then, from Fig. 6-5, the center of mass of link 3 in frame L 3 L 3 L_(3)L_{3} coordinates is:
這完成了對連接 2 的分析。接下來，我們設置 i = 3 i = 3 i=3i=3 並重複步驟 3 到 7。與前兩個旋轉關節不同，SCARA 機器人的第三個關節是滑動的。如果我們假設整根長度為 l 3 l 3 l_(3)l_{3} 和質量為 m 3 m 3 m_(3)m_{3} 的杆在啟動關節 3 時上下滑動，那麼，根據圖 6-5，連接 3 在框架 L 3 L 3 L_(3)L_{3} 坐標系中的質心為：
Δ c 3 = [ 0 , 0 , − l 3 2 , 1 ] T Δ c 3 = 0 , 0 , − l 3 2 , 1 T Deltac^(3)=[0,0,-(l_(3))/(2),1]^(T)\Delta c^{3}=\left[0,0,-\frac{l_{3}}{2}, 1\right]^{T}
As before, let L c L c L_(c)L_{c} denote the coordinate frame obtained by translating frame L 3 L 3 L_(3)L_{3} along axis z 3 z 3 z^(3)z^{3} by an amount − l 3 / 2 . L c − l 3 / 2 . L c -l_(3)//2.L_(c)-l_{3} / 2 . L_{c} is then at the center of mass of link 3 , and the axes of L c L c L_(c)L_{c} are aligned with the principal axes of link 3. If D ¯ 3 D ¯ 3 bar(D)_(3)\bar{D}_{3} is the inertia tensor of link 3 about its center of mass with respect to frame L c L c L_(c)L_{c}, then, from Appendix 2:
如前所述，讓 L c L c L_(c)L_{c} 表示通過沿著軸 z 3 z 3 z^(3)z^{3} 移動 L 3 L 3 L_(3)L_{3} 幾何框架 − l 3 / 2 . L c − l 3 / 2 . L c -l_(3)//2.L_(c)-l_{3} / 2 . L_{c} 的坐標框架，然後位於連桿 3 的質心處，且 L c L c L_(c)L_{c} 的軸與連桿 3 的主軸對齊。如果 D ¯ 3 D ¯ 3 bar(D)_(3)\bar{D}_{3} 是相對於框架 L c L c L_(c)L_{c} 的連桿 3 在其質心處的慣性張量，那麼，根據附錄 2：
D ¯ 3 = ( m 3 l 3 2 12 ) diag { 1 , 1 , 0 } D ¯ 3 = m 3 l 3 2 12 diag { 1 , 1 , 0 } bar(D)_(3)=((m_(3)l_(3)^(2))/(12))diag{1,1,0}\bar{D}_{3}=\left(\frac{m_{3} l_{3}^{2}}{12}\right) \operatorname{diag}\{1,1,0\}
Next, we apply step 5 . Extracting the third column of R 0 2 R 0 2 R_(0)^(2)R_{0}^{2} in Eq. (6-6-10) yields z 2 = R 0 2 i 3 = − i 3 z 2 = R 0 2 i 3 = − i 3 z^(2)=R_(0)^(2)i^(3)=-i^(3)z^{2}=R_{0}^{2} i^{3}=-i^{3}. From Table 6-2, Prop. 2-6-1, and Eq. (6-6-10), or from Eq. (2-8-3) directly, the homogeneous coordinate transformation matrix for link 3 is:
接下來，我們應用步驟 5。從方程式(6-6-10)中提取 R 0 2 R 0 2 R_(0)^(2)R_{0}^{2} 的第三列得到 z 2 = R 0 2 i 3 = − i 3 z 2 = R 0 2 i 3 = − i 3 z^(2)=R_(0)^(2)i^(3)=-i^(3)z^{2}=R_{0}^{2} i^{3}=-i^{3} 。從表 6-2、命題 2-6-1 和方程式(6-6-10)，或直接從方程式(2-8-3)，連桿 3 的齊次坐標變換矩陣為：
T 0 3 = [ C 1 − 2 S 1 − 2 0 a 1 C 1 + a 2 C 1 − 2 S 1 − 2 − C 1 − 2 0 a 1 S 1 + a 2 S 1 − 2 0 0 − 1 d 1 − q 3 0 0 0 1 ] T 0 3 = C 1 − 2 S 1 − 2 0 a 1 C 1 + a 2 C 1 − 2 S 1 − 2 − C 1 − 2 0 a 1 S 1 + a 2 S 1 − 2 0 0 − 1 d 1 − q 3 0 0 0 1 T_(0)^(3)=[[C_(1-2),S_(1-2),0,a_(1)C_(1)+a_(2)C_(1-2)],[S_(1-2),-C_(1-2),0,a_(1)S_(1)+a_(2)S_(1-2)],[0,0,-1,d_(1)-q_(3)],[0,0,0,1]]T_{0}^{3}=\left[\begin{array}{ccc:c} \mathrm{C}_{1-2} & \mathrm{~S}_{1-2} & 0 & a_{1} \mathrm{C}_{1}+a_{2} \mathrm{C}_{1-2} \\ \mathrm{~S}_{1-2} & -\mathrm{C}_{1-2} & 0 & a_{1} \mathrm{~S}_{1}+a_{2} \mathrm{~S}_{1-2} \\ 0 & 0 & -1 & d_{1}-q_{3} \\ \hdashline 0 & 0 & 0 & 1 \end{array}\right]