機(jī)器人理論與技術(shù)基礎(chǔ)CONTENTSIntroductionandConceptualProblems緒論SystemModelofRobot機(jī)器人系統(tǒng)分析基礎(chǔ)RobotKinematics運(yùn)動(dòng)學(xué)RobotDynamics動(dòng)力學(xué)Thecourseisdividedintoeightmodulescoveringthefollowingareas:RobotControl機(jī)器人控制RobotMotionPlanning機(jī)器人運(yùn)動(dòng)規(guī)劃機(jī)器人編程語言ProgrammingLanguageofRobot典型機(jī)器人系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)DesignandImplementationofRobotSystem串聯(lián)機(jī)器人動(dòng)力學(xué)串聯(lián)機(jī)械臂運(yùn)動(dòng)學(xué)并聯(lián)機(jī)器人動(dòng)力學(xué)移動(dòng)機(jī)器人動(dòng)力學(xué)（飛行、水下…)動(dòng)力學(xué)00正動(dòng)力學(xué)問題求解，主要用于機(jī)器人的運(yùn)動(dòng)仿真，判別運(yùn)動(dòng)過程中其加速度是否連續(xù)，系統(tǒng)是否魯棒穩(wěn)定。逆動(dòng)力學(xué)問題研究，可以將這個(gè)力/力矩作為控制目標(biāo)，實(shí)現(xiàn)機(jī)器人精確的運(yùn)動(dòng)控制，改善其動(dòng)力學(xué)特性。動(dòng)力學(xué)特性的數(shù)學(xué)量：矢量、張量、矩陣（坐標(biāo)系變換）以及旋量(運(yùn)動(dòng)旋量、力旋量)等力學(xué)原理：能量守恒定理、達(dá)郎貝爾原理、虛功原理、拉格朗日方程、動(dòng)量矩定理、哈密爾頓原理、牛頓-歐拉方程、凱恩方程等數(shù)學(xué)方法：封閉解（矢量法、矩陣變換法)、數(shù)值法（一階歐拉迭代、四階龍格-庫塔迭代）機(jī)器人動(dòng)力學(xué)DynamicsofRobots機(jī)器人動(dòng)力學(xué)動(dòng)力學(xué)DynamicsWhatIsDynamicsApplicationHowWhystudyDynamicsDynamicsofRobots動(dòng)力學(xué)JointSpaceCartesianSpace逆解(InverseDynamics)正解(ForwardDynamics)

JointSpaceCartesianSpace1978年，Lagrange出版了《分析力學(xué)》一書，引入廣義坐標(biāo)，運(yùn)用達(dá)郎貝爾原理得到的力學(xué)方程，把力學(xué)體系的運(yùn)動(dòng)方程從以力為基本概念的牛頓形式，改變?yōu)橐阅芰繛榛靖拍畹姆治隽W(xué)形式。Lagrange動(dòng)力學(xué)方法01現(xiàn)代機(jī)器人動(dòng)力學(xué)，已經(jīng)很少純粹使用Lagrange動(dòng)力學(xué)方程進(jìn)行求解了，但Lagrange逆動(dòng)力學(xué)方程的一般形式通常被用來結(jié)合迭代算法或其他機(jī)器學(xué)習(xí)算法進(jìn)行求解LagrangianFormulationLagrange動(dòng)力學(xué)方法

牛頓第二定律拉格朗日分析力學(xué)

牛頓方程和拉格朗日方程推導(dǎo)的結(jié)果是一致的LagrangianFormulationLagrange動(dòng)力學(xué)方法在重力作用下的2R機(jī)械臂

連桿1連桿2

a)在重力作用下的2R機(jī)械臂

LagrangianFormulationLagrange動(dòng)力學(xué)方法在重力作用下的2R機(jī)械臂

連桿1連桿2

LagrangianFormulation

連桿1連桿2LagrangianFormulationLagrange動(dòng)力學(xué)方法拉格朗日動(dòng)力學(xué)方程LagrangianFormulation

2R操作臂動(dòng)力學(xué)

對稱正定的質(zhì)量矩陣向心力和科氏力重力矩

依據(jù)牛頓第二運(yùn)動(dòng)定律，建立的力平衡方程稱為牛頓方程；而歐拉方程建立在角動(dòng)量定理的基礎(chǔ)上，用繞質(zhì)心旋轉(zhuǎn)角速度、角加速度去描述這個(gè)三維空間旋轉(zhuǎn)運(yùn)動(dòng)。將兩個(gè)公式組合起來去解決機(jī)器人的正逆動(dòng)力學(xué)求解的方程被稱為牛頓-歐拉方程。基于矢量三角形法的遞歸牛頓-歐拉動(dòng)力學(xué)方法(RNEA)基于矢量三角形法的遞歸牛頓-歐拉動(dòng)力學(xué)方法(RNEA)02Newton-EulerDynamics基于矢量三角形法的遞歸牛頓-歐拉動(dòng)力學(xué)方法(RNEA)牛頓-歐拉方程

牛頓-歐拉方程推導(dǎo)123計(jì)算力和力矩的內(nèi)推法作用在連桿上的力和力矩計(jì)算速度和加速度的外推法已知各關(guān)節(jié)的位置、速度、加速度，可以采用遞推的方式，從連桿1開始向外遞推，直到連桿n,依次得到各連桿質(zhì)心的速度和加速度。計(jì)算出連桿的角速度、角加速度以及線加速度后，采用牛頓-歐拉方程計(jì)算出作用在連桿質(zhì)心的慣性力和力矩。計(jì)算出每個(gè)連桿上的力和力矩后，從末端開始遞推計(jì)算產(chǎn)生這些力和力矩所需要的關(guān)節(jié)力或力矩。Newton-EulerDynamics基于矢量三角形法的遞歸牛頓-歐拉動(dòng)力學(xué)方法(RNEA)牛頓-歐拉方程推導(dǎo)第一步：計(jì)算速度和加速度的外推法

連桿質(zhì)心角速度、角加速度“傳遞”公式Newton-EulerDynamics基于矢量三角形法的遞歸牛頓-歐拉動(dòng)力學(xué)方法(RNEA)牛頓-歐拉方程推導(dǎo)

線速度、線加速度矢量三角形位置：

線速度：

旋轉(zhuǎn)關(guān)節(jié)平移關(guān)節(jié)

注意，當(dāng)遞推到連桿1的速度和加速度時(shí)：第一步：計(jì)算速度和加速度的外推法Newton-EulerDynamics基于矢量三角形法的遞歸牛頓-歐拉動(dòng)力學(xué)方法(RNEA)牛頓-歐拉方程推導(dǎo)第一步：計(jì)算速度和加速度的外推法

求導(dǎo)這兩個(gè)公式對于旋轉(zhuǎn)關(guān)節(jié)和平移關(guān)節(jié)都是一樣。利用連桿坐標(biāo)系的線速度和線加速度，求出連桿質(zhì)心的線速度、線加速度Newton-EulerDynamics基于矢量三角形法的遞歸牛頓-歐拉動(dòng)力學(xué)方法(RNEA)牛頓-歐拉方程推導(dǎo)第二步：計(jì)算作用在連桿上的力和力矩已計(jì)算出連桿質(zhì)心的角速度、角加速度以及線加速度

牛頓方程歐拉方程N(yùn)ewton-EulerDynamics基于矢量三角形法的遞歸牛頓-歐拉動(dòng)力學(xué)方法(RNEA)牛頓-歐拉方程推導(dǎo)第三步：計(jì)算力和力矩的內(nèi)推法已計(jì)算出作用在連桿的力和力矩后，需要計(jì)算產(chǎn)生這些力和力矩所需要的關(guān)節(jié)力或力矩。根據(jù)達(dá)郎貝爾原理建立連桿的力平衡方程和力矩平衡方程

無重力狀態(tài)下連桿受力分析Newton-EulerDynamics基于矢量三角形法的遞歸牛頓-歐拉動(dòng)力學(xué)方法(RNEA)(4-29)牛頓-歐拉方程推導(dǎo)第三步：計(jì)算力和力矩的內(nèi)推法已計(jì)算出作用在連桿的力和力矩后，需要計(jì)算產(chǎn)生這些力和力矩所需要的關(guān)節(jié)力或力矩。無重力狀態(tài)下連桿受力分析

旋轉(zhuǎn)關(guān)節(jié)平移關(guān)節(jié)考慮重力：

Newton-EulerDynamics基于矢量三角形法的遞歸牛頓-歐拉動(dòng)力學(xué)方法(RNEA)采用運(yùn)動(dòng)旋量-力旋量的概念建立牛頓-歐拉方程。基于旋量法的牛頓-歐拉方程03/mhuasong/Basics-of-Robotics-Theory-and-Technology/tree/main/ch4

運(yùn)動(dòng)旋量力旋量

空間慣量矩陣空間動(dòng)量TwistandWrench運(yùn)動(dòng)旋量-力旋量單剛體運(yùn)動(dòng)旋量-力旋量方程推導(dǎo)

物體坐標(biāo)系下的運(yùn)動(dòng)旋量(Twist)

力矩

將作用在剛體上的力和力矩的總和表達(dá)為力旋量(Wrench)

TwistandWrench運(yùn)動(dòng)旋量-力旋量單剛體運(yùn)動(dòng)旋量-力旋量方程推導(dǎo)

旋轉(zhuǎn)項(xiàng)簡化平移項(xiàng)簡化牛頓-歐拉方程FormulationofTwistandWrench運(yùn)動(dòng)旋量-力旋量方程運(yùn)動(dòng)旋量-力旋量方程(牛頓-歐拉方程）

運(yùn)動(dòng)旋量力旋量空間慣量矩陣空間動(dòng)量FormulationofTwistandWrench運(yùn)動(dòng)旋量-力旋量方程運(yùn)動(dòng)旋量-力旋量方程(牛頓-歐拉方程）李括號(Liebracket)

FormulationofTwistandWrench運(yùn)動(dòng)旋量-力旋量方程運(yùn)動(dòng)旋量-力旋量方程(牛頓-歐拉方程）

等價(jià)單剛體運(yùn)動(dòng)旋量-力旋量方程可以改寫為FormulationofTwistandWrench運(yùn)動(dòng)旋量-力旋量方程基于旋量法的牛頓-歐拉逆動(dòng)力學(xué)方程推導(dǎo)逆動(dòng)力學(xué)方程

FormulationofTwistandWrench運(yùn)動(dòng)旋量-力旋量方程基于旋量法的牛頓-歐拉逆動(dòng)力學(xué)方程推導(dǎo)第一步：計(jì)算速度和加速度的外推法連桿速度遞歸方程

連桿加速度遞歸方程

考慮重力:

FormulationofTwistandWrench運(yùn)動(dòng)旋量-力旋量方程基于旋量法的牛頓-歐拉逆動(dòng)力學(xué)方程推導(dǎo)第二步：計(jì)算力和力矩的內(nèi)推法從基座向外推導(dǎo)得到所有連桿的運(yùn)動(dòng)旋量和加速度之后，即可計(jì)算各關(guān)節(jié)的力和力矩，旋量法可以直接通過計(jì)算力旋量得到。

考慮末端執(zhí)行器的接觸力

關(guān)節(jié)控制所需力矩表4.1旋量法遞歸牛頓歐拉逆動(dòng)力學(xué)算法FormulationofTwistandWrench運(yùn)動(dòng)旋量-力旋量方程基于旋量法的牛頓-歐拉逆動(dòng)力學(xué)方程/mhuasong/Basics-of-Robotics-Theory-and-Technology/tree/main/ch4仿真實(shí)驗(yàn)代碼及說明參見：

組合形式的動(dòng)力學(xué)方程封閉解04DynamicEquationsinClosedForm組合形式的動(dòng)力學(xué)方程封閉解封閉形式方程

封閉形式方程能否一次性給出？DynamicEquationsinClosedForm組合形式的動(dòng)力學(xué)方程封閉解組合向量

stackedvectors

DynamicEquationsinClosedForm組合形式的動(dòng)力學(xué)方程封閉解將前面的逆動(dòng)力學(xué)遞歸算法組合成矩陣方程組

DynamicEquationsinClosedForm組合形式的動(dòng)力學(xué)方程封閉解

將前面的組合成矩陣方程組改寫

逆動(dòng)力學(xué)方程封閉解在描述復(fù)合機(jī)器人、人形機(jī)器人等具有復(fù)雜運(yùn)動(dòng)鏈的動(dòng)力學(xué)計(jì)算中，隨著多體動(dòng)力學(xué)的發(fā)展，機(jī)器人動(dòng)力學(xué)多采用空間向量表示法中的運(yùn)動(dòng)樹、運(yùn)動(dòng)環(huán)來對機(jī)器人的機(jī)構(gòu)進(jìn)行抽象，分別對應(yīng)串聯(lián)機(jī)器人和并聯(lián)機(jī)器人的動(dòng)力學(xué)分析?？臻g向量表示法05SpatialVectorNotation空間向量表示法運(yùn)動(dòng)向量和力向量

笛卡爾坐標(biāo)系

三維向量:坐標(biāo)向量:

普呂克坐標(biāo)系

SpatialVectorNotation空間向量表示法運(yùn)動(dòng)向量和力向量笛卡爾坐標(biāo)系普呂克坐標(biāo)系單位運(yùn)動(dòng)向量:

剛體的運(yùn)動(dòng)向量采用空間向量的形式表示為：

SpatialVectorNotation空間向量表示法運(yùn)動(dòng)向量和力向量

空間向量表示法中描述的剛體運(yùn)動(dòng)向量和力向量實(shí)際上和前面描述的運(yùn)動(dòng)旋量及力旋量的概念是一致。運(yùn)動(dòng)向量和力向量遵從一般向量的運(yùn)算規(guī)則。SpatialVectorNotation空間向量表示法空間向量的運(yùn)算規(guī)則（1）加法和數(shù)乘:顯式方式（2）點(diǎn)乘

（3）坐標(biāo)變換

運(yùn)動(dòng)向量與力轉(zhuǎn)換之間的關(guān)系：

運(yùn)動(dòng)向量的坐標(biāo)系變換可通過平移變換矩陣以及旋轉(zhuǎn)變換矩陣來描述

SpatialVectorNotation空間向量表示法空間向量的運(yùn)算規(guī)則（4）叉乘

叉乘算子

兩個(gè)運(yùn)動(dòng)向量的叉乘運(yùn)動(dòng)向量與力向量的叉乘SpatialVectorNotation空間向量表示法空間向量的運(yùn)算規(guī)則（5）微分

空間向量的導(dǎo)數(shù)定義對任意空間向量s求微分

SpatialVectorNotation空間向量表示法空間加速度

空間加速度與經(jīng)典加速度的關(guān)系

空間加速度向量和速度向量一樣可以簡單進(jìn)行相加，采用空間加速度的好處是不用考慮科氏力(Coriolis)或離心力項(xiàng)。SpatialVectorNotation空間向量表示法空間動(dòng)量與空間慣量

剛體的空間動(dòng)量表示為空間慣量與速度的積：

SpatialVectorNotation空間向量表示法空間動(dòng)量與空間慣量

SpatialVectorNotation空間向量表示法空間運(yùn)動(dòng)方程在空間運(yùn)動(dòng)方程中，作用于剛體上的力等于其動(dòng)量的變化率：

在第2章中，我們將機(jī)器人簡化成剛體樹機(jī)器人模型(RigidBodyTreeRobotModel)。剛體樹由通過關(guān)節(jié)(Joint)連接的剛體(RigidBody)組成，每個(gè)剛體連接一個(gè)關(guān)節(jié)，該關(guān)節(jié)定義了該剛體如何相對于其父節(jié)點(diǎn)在樹中的運(yùn)動(dòng)（旋轉(zhuǎn)或平移）。而機(jī)器人的動(dòng)力學(xué)分析，同樣可以采用這樣的剛體樹機(jī)器人模型進(jìn)行分析?；诳臻g向量表示法的機(jī)器人剛體樹動(dòng)力學(xué)模型由四部分組成：連接圖、連桿和關(guān)節(jié)幾何參數(shù)、連桿慣性參數(shù)，以及一組關(guān)節(jié)模型。對于這種模型，還可以加上各種產(chǎn)生力的元件，例如彈簧、阻尼器、關(guān)節(jié)摩擦、執(zhí)行器、驅(qū)動(dòng)器等。這些元件中有些屬于柔性元件，其動(dòng)力學(xué)問題也會(huì)很復(fù)雜。本章僅討論簡單剛體樹動(dòng)力學(xué)基本模型?；诳臻g向量表示法的剛體動(dòng)力學(xué)模型06RigidBodyDynamicModelbasedonSpatialVectorNotation基于空間向量表示法的剛體動(dòng)力學(xué)模型機(jī)器人剛體樹動(dòng)力學(xué)模型基于空間向量表示法的機(jī)器人剛體樹動(dòng)力學(xué)模型由四部分組成：連接圖、連桿和關(guān)節(jié)幾何參數(shù)、連桿慣性參數(shù)，以及一組關(guān)節(jié)模型RoyFeatherstone:《剛體動(dòng)力學(xué)算法》人形機(jī)器人連接圖實(shí)例RigidBodyDynamicModelbasedonSpatialVectorNotation基于空間向量表示法的剛體動(dòng)力學(xué)模型機(jī)器人剛體樹動(dòng)力學(xué)模型RoyFeatherstone:《剛體動(dòng)力學(xué)算法》

連桿和關(guān)節(jié)幾何參數(shù)RigidBodyDynamicModelbasedonSpatialVectorNotation基于空間向量表示法的剛體動(dòng)力學(xué)模型機(jī)器人剛體樹動(dòng)力學(xué)模型RoyFeatherstone:《剛體動(dòng)力學(xué)算法》連桿與關(guān)節(jié)之間速度、加速度以及力的傳遞

利用空間向量法將遞歸牛頓-歐拉逆動(dòng)力學(xué)推廣到多剛體遞歸牛頓-歐拉逆動(dòng)力學(xué)一般算法：根據(jù)連桿之間通過關(guān)節(jié)進(jìn)行的傳遞，可以將該算法分為兩個(gè)步驟去進(jìn)行遞歸求解：第一步進(jìn)行二階正運(yùn)動(dòng)學(xué)的正向傳遞計(jì)算，計(jì)算連桿的空間運(yùn)動(dòng)方程；第二步通過力向量公式反向傳遞，計(jì)算關(guān)節(jié)力和力矩。基于空間向量表示法的遞歸牛頓-歐拉逆動(dòng)力學(xué)07/mhuasong/Basics-of-Robotics-Theory-and-Technology/tree/main/ch4SpatialVectorNotation基于空間向量表示法的遞歸牛頓-歐拉逆動(dòng)力學(xué)

SpatialVectorNotation基于空間向量表示法的遞歸牛頓-歐拉逆動(dòng)力學(xué)空間向量法遞歸牛頓歐拉逆動(dòng)力學(xué)算法/mhuasong/Basics-of-Robotics-Theory-and-Technology/tree/main/ch4ABA:Articulated-BodyAlgorithmABA算法中的空間向量采用了連桿坐標(biāo)系，算法前面2個(gè)循環(huán)，按照RNEA算法進(jìn)行運(yùn)動(dòng)樹的迭代，第一遍從基座（或虛擬底座）向運(yùn)動(dòng)樹末端迭代，計(jì)算連桿速度和速度積項(xiàng)；第二遍從末端反向向底座迭代，計(jì)算關(guān)節(jié)體慣量和每根連桿的偏置力；第三遍從底座向末端迭代，得到連桿和關(guān)節(jié)加速度。相比于拉格朗日動(dòng)力學(xué)方法求解正動(dòng)力學(xué)，ABA正運(yùn)動(dòng)學(xué)算法更便于建模以及計(jì)算，不僅適用于串聯(lián)工業(yè)機(jī)械臂，也適用于人形、并聯(lián)等復(fù)雜機(jī)構(gòu)類型的機(jī)器人。求解出機(jī)器人關(guān)節(jié)的加速度，便可以用數(shù)值積分進(jìn)行機(jī)器人運(yùn)動(dòng)的模擬?；陉P(guān)節(jié)體算法的正動(dòng)力學(xué)08SpatialVectorNotation基于關(guān)節(jié)體算法(ABA)的正動(dòng)力學(xué)關(guān)節(jié)體i的定義SpatialVectorNotation基于關(guān)節(jié)體算法(ABA)的正動(dòng)力學(xué)相比于拉格朗日動(dòng)力學(xué)方法求解正動(dòng)力學(xué)，ABA正運(yùn)動(dòng)學(xué)算法更便于建模以及計(jì)算，不僅適用于串聯(lián)工業(yè)機(jī)械臂，也適用于人形、并聯(lián)等復(fù)雜機(jī)構(gòu)類型的機(jī)器人。求解出機(jī)器人關(guān)節(jié)的加速度，便可以用數(shù)值積分進(jìn)行機(jī)器人運(yùn)動(dòng)的模擬。ABA正動(dòng)力學(xué)算法/mhuasong/Basics-of-Robotics-Theory-and-Technology/tree/main/ch4

利用逆動(dòng)力學(xué)求解開鏈機(jī)器人正動(dòng)力學(xué)09SpatialVectorNotation利用逆動(dòng)力學(xué)求解開鏈機(jī)器人正動(dòng)力學(xué)

SpatialVectorNotation利用逆動(dòng)力學(xué)求解開鏈機(jī)器人正動(dòng)力學(xué)

采用歐拉積分法進(jìn)行正運(yùn)動(dòng)學(xué)數(shù)值計(jì)算10ForwardDynamicsofOpenChains采用歐拉積分法進(jìn)行正運(yùn)動(dòng)學(xué)數(shù)值計(jì)算串聯(lián)機(jī)器人的正動(dòng)力學(xué)迭代法求解

正動(dòng)力學(xué)

一階歐拉迭代

正動(dòng)力學(xué)的歐拉積分為

ForwardDynamicsofOpenChains串聯(lián)機(jī)器人的正動(dòng)力學(xué)數(shù)值解串聯(lián)機(jī)器人的正動(dòng)力學(xué)迭代法算法

歐拉一階迭代高階迭代算法？DynamicsofRobots機(jī)器人動(dòng)力學(xué)機(jī)器人動(dòng)力學(xué)實(shí)驗(yàn)實(shí)例AUBO-i5PeterCorke:robottoolbox加載aubo-i5動(dòng)力學(xué)模型圖/petercorke/robotics-toolbox-matlabMatlab+CoppeliaSim可以加載URDF可視化要好一些/mhuasong/AUBO-Robot-on-ROSOldDynamicsofRobots機(jī)器人動(dòng)力學(xué)機(jī)器人動(dòng)力學(xué)實(shí)驗(yàn)實(shí)例AUBO-i5/mhuasong/AUBO-Robot-on-ROSOldDynamicsofRobots機(jī)器人動(dòng)力學(xué)機(jī)器人動(dòng)力學(xué)實(shí)驗(yàn)實(shí)例AUBO-i55次多項(xiàng)式圓弧作業(yè)的關(guān)節(jié)角規(guī)劃/mhuasong/AUBO-Robot-on-ROSOldDynamicsofRobots機(jī)器人動(dòng)力學(xué)機(jī)器人動(dòng)力學(xué)實(shí)驗(yàn)實(shí)例AUBO-i5/mhuasong/AUBO-Robot-on-ROSOldDynamicsofRobots機(jī)器人動(dòng)力學(xué)機(jī)器人動(dòng)力學(xué)實(shí)驗(yàn)實(shí)例AUBO-i5/mhuasong/AUBO-Robot-on-ROSOldDynamicsofRobots機(jī)器人動(dòng)力學(xué)機(jī)器人動(dòng)力學(xué)實(shí)驗(yàn)實(shí)例AUBO-i5/mhuasong/AUBO-Robot-on-ROSOldDynamicsofRobots機(jī)器人動(dòng)力學(xué)機(jī)器人動(dòng)力學(xué)實(shí)驗(yàn)實(shí)例AUBO-i5/NxRLab/ModernRobotics旋量法、封閉解、數(shù)值法代碼參見《ModernRobotics》一書的開源代碼：/mhuasong/AUBO-Robot-on-ROSPeterCorke:roboticstoolbox/petercorke/robotics-toolbox-matlabOld以AUBO_i5為例講解基于空間向量法的RNEA算法步驟計(jì)算實(shí)例，以及ABA算法結(jié)合歐拉積分法模擬仿真實(shí)例六軸協(xié)作機(jī)器人動(dòng)力學(xué)分析實(shí)例11連桿坐標(biāo)系{i}偏置0-----1π/200.0985θ1:-175o至175o020-0.4080θ2: