機(jī)器人操作系統(tǒng)ROS應(yīng)用實(shí)踐 課件 第7、8章 機(jī)械臂運(yùn)動控制、計算機(jī)視覺

第七章機(jī)械臂運(yùn)動控制機(jī)器人操作系統(tǒng)ROS應(yīng)用實(shí)踐目錄content機(jī)械臂建模7.17.3機(jī)械臂控制—MoveIt7.2MoveIt編程——機(jī)械臂運(yùn)動規(guī)劃7.4本章小結(jié)7.1機(jī)械臂建模1ROS中常用的機(jī)械臂UR機(jī)器人UniversalRobots（優(yōu)傲機(jī)器人）公司2005年成立于丹麥，針對不同的負(fù)載級別，其主要的機(jī)器人產(chǎn)品有：UR3、UR5和UR10，如圖所示。7.1機(jī)械臂建模1ROS中常用的機(jī)械臂UR機(jī)器人優(yōu)傲機(jī)器人公司在2009年推出了第一款協(xié)作機(jī)器人——UR5，自重18kg，有效負(fù)載可達(dá)5kg，工作半徑為85cm，不僅顛覆了人們對于傳統(tǒng)工業(yè)機(jī)器人的認(rèn)識，還自定義了“協(xié)作機(jī)器人”，其與人類操作員可以在同一個空間中協(xié)作工作，具有安全度高、無須安全圍欄等特點(diǎn)。2015年3月，優(yōu)傲機(jī)器人公司推出UR3，自重僅11kg，有效負(fù)載達(dá)到3kg，所有腕關(guān)節(jié)均可360°旋轉(zhuǎn)，而末端關(guān)節(jié)可進(jìn)行無限旋轉(zhuǎn)。進(jìn)一步地，UR10的有效負(fù)載為10kg，工作半徑為130cm。這三款機(jī)器人均具有編程簡單、靈活度高的特點(diǎn)，可以與人類在一起安全可靠地協(xié)作工作，完成如分揀、抓取、裝配、打磨等任務(wù)。7.1機(jī)械臂建模1ROS中常用的機(jī)械臂FrankaPandaFrankaPanda是FrankaEmika公司設(shè)計的一款七自由度機(jī)械臂。其每個關(guān)節(jié)上都安裝有力傳感器，性能強(qiáng)大。它提供開源接口（FCI），用戶可通過C++和ROS進(jìn)行編程，其內(nèi)置的機(jī)器人控制系統(tǒng)可以通過手機(jī)App或Web頁面進(jìn)行操作，使用方便。此外，其系統(tǒng)靈敏度高，針對大多數(shù)重復(fù)且單調(diào)的操作，如精細(xì)裝配、旋擰和連接作業(yè)，以及測試、檢查和組裝等，都可以實(shí)現(xiàn)自動化。7.1機(jī)械臂建模2機(jī)械臂URDF模型URDF（UnifiedRobotDescriptionFormat，統(tǒng)一機(jī)器人描述格式）是ROS中非常重要的機(jī)器人模型描述格式，ROS提供了URDF文件的C++解析器，可以解析URDF文件中使用XML格式描述的機(jī)器人模型。一般來說，任意的機(jī)器人模型都可以被分解為兩大部分：連桿（link）和關(guān)節(jié)（joint），如圖所示。7.1機(jī)械臂建模2機(jī)械臂URDF模型<link>標(biāo)簽<link>標(biāo)簽描述機(jī)器人某個剛體部分的外觀和物理屬性，包括尺寸、顏色，形狀，慣性矩陣，碰撞屬性等。link結(jié)構(gòu)的URDF描述語法如下：7.1機(jī)械臂建模2機(jī)械臂URDF模型<joint>標(biāo)簽<joint>標(biāo)簽描述機(jī)器人關(guān)節(jié)的運(yùn)動學(xué)和動力學(xué)屬性，包括關(guān)節(jié)運(yùn)動的位置和速度限制。根據(jù)機(jī)器人的關(guān)節(jié)運(yùn)動形式，可以將機(jī)器人的關(guān)節(jié)分為六種類型，如表所示。7.1機(jī)械臂建模2機(jī)械臂URDF模型<joint>標(biāo)簽<joint>標(biāo)簽的描述語法如下：7.1機(jī)械臂建模2機(jī)械臂URDF模型<joint>標(biāo)簽其中，必須要指定joint的<parentlink>和<childlink>，還可以設(shè)置joint的其他屬性：<calibration>：關(guān)節(jié)的參考位置，用來校準(zhǔn)關(guān)節(jié)的絕對位置。<dynamics>：描述關(guān)節(jié)的物理屬性，如阻尼值、物理靜摩擦力等，在動力學(xué)仿真中會用到。7.1機(jī)械臂建模2機(jī)械臂URDF模型<joint>標(biāo)簽其中，必須要指定joint的<parentlink>和<childlink>，還可以設(shè)置joint的其他屬性：<limit>：描述運(yùn)動的極限值，包括關(guān)節(jié)運(yùn)動的上下限位置、速度限制、力矩限制等。<mimic>：描述該關(guān)節(jié)與已有關(guān)節(jié)的關(guān)系。<safety_controller>：描述安全控制器參數(shù)。7.1機(jī)械臂建模2機(jī)械臂URDF模型<robot>標(biāo)簽<robot>是完整機(jī)器人模型的最頂層標(biāo)簽，<link>標(biāo)簽和<joint>標(biāo)簽都必須包含在<robot>標(biāo)簽內(nèi)。一個完整的機(jī)器人模型，由一系列<link>標(biāo)簽和<joint>標(biāo)簽組成，<robot>標(biāo)簽的說明如圖所示。7.1機(jī)械臂建模2機(jī)械臂URDF模型<robot>標(biāo)簽<robot>標(biāo)簽內(nèi)可以設(shè)置機(jī)器人的名稱，其基本語法如下：7.1機(jī)械臂建模3機(jī)械臂URDF建模在大部分場景下，先使用三維設(shè)計軟件完成機(jī)器人模型的設(shè)計，再將其導(dǎo)入ROS環(huán)境下。Solidworks是一款常用的三維設(shè)計軟件，ROS社區(qū)為該軟件提供了模型轉(zhuǎn)換插件——sw2urdf。下面以UR5機(jī)器人的三維模型為例，學(xué)習(xí)如何使用該插件完成URDF模型的轉(zhuǎn)換。7.1機(jī)械臂建模3機(jī)械臂URDF建模首先，獲取UR5的三維模型，可以從官網(wǎng)的下載鏈接中找到stp格式的模型文件。然后進(jìn)入sw2urdf插件下載頁面，如圖所示。下載后，在裝有Solidworks軟件的計算機(jī)上安裝sw2urdf插件。7.1機(jī)械臂建模3機(jī)械臂URDF建模安裝成功后，打開Solidworks軟件，Solidworks軟件中的UR5機(jī)器人模型如圖所示。7.1機(jī)械臂建模3機(jī)械臂URDF建模在導(dǎo)出模型之前，需要使用Solidworks軟件為模型設(shè)置全局坐標(biāo)系及每個關(guān)節(jié)的基準(zhǔn)軸，設(shè)置后的效果如圖所示。7.1機(jī)械臂建模3機(jī)械臂URDF建模然后，選擇“工具”?>File?>ExportasURDF選項，打開安裝好的sw2urdf插件，如圖所示。在打開的插件配置界面中，按照配置說明，選擇每個連桿的名稱和模型、關(guān)節(jié)的名稱和類型、旋轉(zhuǎn)軸，單擊Numberofchildlinks選項下的上下箭頭按鈕，即可添加或減少串聯(lián)關(guān)節(jié)。7.1機(jī)械臂建模3機(jī)械臂URDF建模UR5是一個六關(guān)節(jié)、七連桿的串聯(lián)機(jī)器人，需要依次完成所有配置，如圖所示。7.1機(jī)械臂建模3機(jī)械臂URDF建模完成配置后，單擊PreviewandExport按鈕，可自動生成所有參考坐標(biāo)系，并打開一個導(dǎo)出參數(shù)的確認(rèn)界面，如圖所示。7.1機(jī)械臂建模3機(jī)械臂URDF建模確認(rèn)配置的連桿和關(guān)節(jié)參數(shù)正確后，單擊界面右下角的ExportURDFandMeshes按鈕，即可自動將UR5模型轉(zhuǎn)換成一個ROS功能包，其中包含了UR5的URDF模型及其鏈接的連桿meshes（網(wǎng)格）文件，如圖所示。7.1機(jī)械臂建模3機(jī)械臂URDF建模將該功能包放置在ROS工作空間下編譯，執(zhí)行如下命令即可看到URDF模型，如圖所示。7.1機(jī)械臂建模3機(jī)械臂URDF建模接下來，就可以使用Gazebo仿真軟件和MoveIt搭建完整的機(jī)械臂運(yùn)動仿真系統(tǒng)了。Gazebo是一款功能強(qiáng)大的三維物理仿真軟件，具備強(qiáng)大的物理引擎、高質(zhì)量的圖形渲染功能、方便的編程與圖形接口。ROS為Gazebo提供了底層驅(qū)動包，開發(fā)者可以在ROS環(huán)境下輕松使用Gazebo進(jìn)行機(jī)器人仿真。如圖所示，Gazebo中的機(jī)器人模型除與rviz使用的模型相同之外，還可以根據(jù)需要在模型中加入機(jī)器人和周圍環(huán)境的物理屬性，如質(zhì)量、摩擦系數(shù)、彈性系數(shù)等。機(jī)器人的傳感器信息也可以通過插件的形式加入仿真環(huán)境，進(jìn)行可視化顯示。7.2機(jī)械臂控制——MoveIt1MoveIt簡介最早使用ROS框架的PR2不僅是一個移動機(jī)器人，還帶有兩個多自由度的機(jī)械臂。在PR2機(jī)器人的基礎(chǔ)上，ROS提供了不少針對機(jī)械臂的功能包，這些功能包在2012年被集成到一個單獨(dú)的ROS軟件——MoveIt中。MoveIt為開發(fā)者提供了一個易于使用的集成化開發(fā)平臺，由一系列功能包組成，包含運(yùn)動規(guī)劃、操作控制、3D感知、運(yùn)動學(xué)、控制與導(dǎo)航算法等，而且提供友好的GUI，廣泛應(yīng)用于工業(yè)、商業(yè)、研發(fā)和其他領(lǐng)域。7.2機(jī)械臂控制——MoveIt1MoveIt簡介MoveIt目前已經(jīng)支持如圖所示的多款常用的機(jī)器人，也可以非常靈活地應(yīng)用到各種機(jī)器人系統(tǒng)中。7.2機(jī)械臂控制——MoveIt2SetupAssistant配置機(jī)械臂使用MoveIt，第一步是使用其自帶的

SetupAssistant（配置助手）完成一系列配置工作。SetupAssistant會根據(jù)用戶導(dǎo)入的機(jī)器人URDF模型，生成SRDF（SemanticRobotDescriptionFormat）文件，創(chuàng)建一個MoveIt配置的功能包，完成機(jī)器人的配置、可視化和仿真。7.2機(jī)械臂控制——MoveIt3MoveIt可視化控制按照上一節(jié)介紹的配置步驟完成后，會生成一個名為ur5_moveit_config的功能包，其包含了大部分MoveIt啟動所需要的配置文件和啟動文件，還會包含一個簡單的演示demo，用來測試配置是否成功，使用以下命令即可運(yùn)行該演示demo：7.2機(jī)械臂控制——MoveIt3MoveIt可視化控制運(yùn)行成功后，可以看到如圖所示的界面。在界面中可以進(jìn)行拖動規(guī)劃和隨機(jī)規(guī)劃。7.2機(jī)械臂控制——MoveIt3MoveIt可視化控制拖動規(guī)劃拖動機(jī)器人的前端，可以改變機(jī)器人的姿態(tài)。在左下方的Planning選項卡中，單擊PlanandExecute按鈕，MoveIt開始規(guī)劃路徑，控制機(jī)器人向目標(biāo)位置移動，在右側(cè)界面中可以看到機(jī)器人運(yùn)動的全部過程，如圖所示。7.2機(jī)械臂控制——MoveIt3MoveIt可視化控制隨機(jī)規(guī)劃在Query菜單中，有一個SelectGoalState子菜單，在下拉列表中選擇<randomvalid>，然后單擊Update按鈕，MoveIt會在機(jī)器人的工作范圍內(nèi)，隨機(jī)生成一個目標(biāo)位姿，接著單擊PlanandExecute按鈕，機(jī)器人會自動運(yùn)動到隨機(jī)產(chǎn)生的目標(biāo)位姿，如圖所示。7.2機(jī)械臂控制——MoveIt4機(jī)械臂運(yùn)動學(xué)機(jī)械臂D-H建模UR5是六自由度機(jī)械臂，使用D-H參數(shù)法可確定其運(yùn)動學(xué)模型，如圖所示。7.2機(jī)械臂控制——MoveIt4機(jī)械臂運(yùn)動學(xué)機(jī)械臂D-H建模UR5的D-H參數(shù)如表所示，轉(zhuǎn)動關(guān)節(jié)θi是關(guān)節(jié)變量，連桿偏移di是常數(shù)。7.2機(jī)械臂控制——MoveIt4機(jī)械臂運(yùn)動學(xué)正運(yùn)動學(xué)建立坐標(biāo)系i在坐標(biāo)系i?1下的齊次變換矩陣，然后把D-H參數(shù)代入這些變換矩陣，得到所有相鄰坐標(biāo)系的變換矩陣：7.2機(jī)械臂控制——MoveIt4機(jī)械臂運(yùn)動學(xué)正運(yùn)動學(xué)建立坐標(biāo)系i在坐標(biāo)系i?1下的齊次變換矩陣，然后把D-H參數(shù)代入這些變換矩陣，得到所有相鄰坐標(biāo)系的變換矩陣：則末端坐標(biāo)系到基座坐標(biāo)系的變換矩陣為：7.2機(jī)械臂控制——MoveIt4機(jī)械臂運(yùn)動學(xué)正運(yùn)動學(xué)輸入機(jī)械臂的六個關(guān)節(jié)角度θi，就可以得到末端坐標(biāo)系到基座坐標(biāo)系的變換矩陣T：變換矩陣T左上角的3×3的矩陣是旋轉(zhuǎn)矩陣，右上角的3×1矩陣是空間位置[px,py,pz]T，由變換矩陣T可得到機(jī)械臂末端位姿，得到正運(yùn)動學(xué)解。7.2機(jī)械臂控制——MoveIt4機(jī)械臂運(yùn)動學(xué)逆運(yùn)動學(xué)逆運(yùn)動學(xué)相對正運(yùn)動學(xué)要復(fù)雜，需要根據(jù)機(jī)械臂末端的空間位置和姿態(tài)，求解機(jī)械臂每個關(guān)節(jié)的旋轉(zhuǎn)角度。逆運(yùn)動學(xué)的求解方法有解析法、數(shù)值迭代法和幾何法。其中解析法用數(shù)學(xué)公式進(jìn)行推導(dǎo)，可以得到全部根，但是計算復(fù)雜。主要推導(dǎo)過程如下：首先，求變換矩陣T的過程中，有一些中間矩陣：其中，ci=cos(θi)，si=sin(θi)，c23=cos(θ2+θ3)，s23=sin(θ2+θ3)。7.2機(jī)械臂控制——MoveIt4機(jī)械臂運(yùn)動學(xué)逆運(yùn)動學(xué)然后，由

，得到

。7.2機(jī)械臂控制——MoveIt4機(jī)械臂運(yùn)動學(xué)逆運(yùn)動學(xué)根據(jù)：計算得到：7.2機(jī)械臂控制——MoveIt4機(jī)械臂運(yùn)動學(xué)逆運(yùn)動學(xué)根據(jù)等式兩邊矩陣的行列相等，可解得：接著由：7.2機(jī)械臂控制——MoveIt4機(jī)械臂運(yùn)動學(xué)逆運(yùn)動學(xué)可得：可解得：7.3MoveIt編程——機(jī)械臂運(yùn)動規(guī)劃1關(guān)節(jié)空間運(yùn)動規(guī)劃關(guān)節(jié)空間運(yùn)動是以機(jī)械臂關(guān)節(jié)角度為控制量的運(yùn)動。雖然各關(guān)節(jié)到達(dá)期望位置所經(jīng)過的時間相同，但是各關(guān)節(jié)運(yùn)動時是相互獨(dú)立、互不影響的。機(jī)械臂狀態(tài)使用各關(guān)節(jié)軸位置描述，指定運(yùn)動目標(biāo)的機(jī)械臂姿態(tài)后，可通過控制各關(guān)節(jié)軸運(yùn)動，到達(dá)目標(biāo)位姿。使用如下命令，進(jìn)行UR5在關(guān)節(jié)空間下的運(yùn)動測試：7.3MoveIt編程——機(jī)械臂運(yùn)動規(guī)劃1關(guān)節(jié)空間運(yùn)動規(guī)劃關(guān)節(jié)空間下的機(jī)械臂運(yùn)動規(guī)劃效果如圖所示，可以看到機(jī)械臂運(yùn)動到指定位姿。7.3MoveIt編程——機(jī)械臂運(yùn)動規(guī)劃2工作空間運(yùn)動規(guī)劃在機(jī)器人應(yīng)用系統(tǒng)中，機(jī)械臂末端的姿態(tài)非常重要，與機(jī)械臂關(guān)節(jié)空間運(yùn)動規(guī)劃相對應(yīng)的是工作空間運(yùn)動規(guī)劃，在工作空間運(yùn)動規(guī)劃時，機(jī)械臂的目標(biāo)位姿不再使用各關(guān)節(jié)軸位置給定，而是通過機(jī)械臂末端的三維坐標(biāo)位置和姿態(tài)給定。在工作空間運(yùn)動規(guī)劃時，需要先通過逆運(yùn)動學(xué)求解各關(guān)節(jié)軸位置，再進(jìn)行運(yùn)動規(guī)劃，控制機(jī)械臂運(yùn)動。MoveIt支持工作空間下的目標(biāo)位姿設(shè)置，使用如下命令運(yùn)行工作空間下的運(yùn)動規(guī)劃例程：7.3MoveIt編程——機(jī)械臂運(yùn)動規(guī)劃2工作空間運(yùn)動規(guī)劃運(yùn)行成功后，機(jī)械臂運(yùn)動到了指定位姿。工作空間下的機(jī)械臂運(yùn)動規(guī)劃效果如圖所示。7.3MoveIt編程——機(jī)械臂運(yùn)動規(guī)劃2工作空間運(yùn)動規(guī)劃同時，在命令行中，可以看到運(yùn)動規(guī)劃過程中的輸出日志，包含KDL運(yùn)動學(xué)求解器完成逆運(yùn)動學(xué)求解的時間，如圖所示。7.3MoveIt編程——機(jī)械臂運(yùn)動規(guī)劃3笛卡爾空間運(yùn)動規(guī)劃在工作空間運(yùn)動規(guī)劃時，并沒有對機(jī)械臂末端軌跡有任何約束，機(jī)械臂末端目標(biāo)位姿給定后，通過逆運(yùn)動學(xué)求解可獲得關(guān)節(jié)空間下的各關(guān)節(jié)軸的弧度，然后進(jìn)行關(guān)節(jié)空間運(yùn)動規(guī)劃，完成機(jī)械臂運(yùn)動控制。但在很多應(yīng)用場景中，我們不僅需要關(guān)心機(jī)械臂的初始和目標(biāo)位姿，還對運(yùn)動過程中的軌跡和位姿有所要求，比如，我們希望機(jī)械臂末端能夠按直線或者圓弧軌跡運(yùn)動。MoveIt同樣提供笛卡爾空間運(yùn)動規(guī)劃的接口，使用以下命令運(yùn)行笛卡爾空間下的運(yùn)動規(guī)劃例程：7.3MoveIt編程——機(jī)械臂運(yùn)動規(guī)劃3笛卡爾空間運(yùn)動規(guī)劃運(yùn)行成功后，可以看到如圖所示的運(yùn)動軌跡。7.3MoveIt編程——機(jī)械臂運(yùn)動規(guī)劃4機(jī)械臂碰撞檢測在很多應(yīng)用場景下，機(jī)械臂周圍會有一些物體，這些物體有可能在機(jī)械臂的工作空間內(nèi)，成為機(jī)械臂運(yùn)動規(guī)劃過程中的障礙物，所以運(yùn)動規(guī)劃過程中需要考慮避障問題。MoveIt支持避障規(guī)劃，可使用move_group中planningscene插件的相關(guān)接口引入障礙物模型，并維護(hù)機(jī)器人工作的場景信息。執(zhí)行如下命令，啟動避障規(guī)劃例程：7.3MoveIt編程——機(jī)械臂運(yùn)動規(guī)劃4機(jī)械臂碰撞檢測啟動成功后，可以看到圖所示的避障規(guī)劃的運(yùn)行效果。7.4本章小結(jié)本章首先介紹機(jī)器人的URDF建模和MoveIt可視化配置；然后通過在MoveIt和Gazebo在仿真環(huán)境中控制機(jī)械臂，加深讀者對機(jī)械臂運(yùn)動學(xué)的理解；最后介紹了幾種不同坐標(biāo)空間下的機(jī)械臂運(yùn)動規(guī)劃方法，并通過一個簡易的數(shù)字孿生實(shí)驗，讓讀者體驗真實(shí)機(jī)械臂與仿真機(jī)械臂互為映射的同步運(yùn)動。第八章計算機(jī)視覺機(jī)器人操作系統(tǒng)ROS應(yīng)用實(shí)踐目錄content認(rèn)識OpenCV8.18.7單目視覺傳感器的使用8.2本章小結(jié)相機(jī)標(biāo)定8.3圖像變換與處理8.4常見的圖像特征點(diǎn)檢測算法8.4目標(biāo)識別8.68.1認(rèn)識OpenCV1安裝OpenCV一般來說，ROS中已經(jīng)集成了OpenCV，所以不需要進(jìn)行額外的安裝操作了。如果沒有安裝OpenCV，推薦安裝ROSperception功能包，perception功能包集成了OpenCV和開源點(diǎn)云處理庫（PointCloudLibrary，PCL），以及其他機(jī)器人感知相關(guān)的功能包。輸入下面的命令進(jìn)行安裝：注意：如果想要使用GPU加速等高級功能，需要自行編譯相關(guān)OpenCV庫，ROS集成的OpenCV不包含這些拓展模塊。8.1認(rèn)識OpenCV2使用OpenCV首先，新建一個功能包，包名為ros_opencv：在包下放一個img_pub.cpp文件，作用是讀取一個圖像并發(fā)布出去，內(nèi)容如下：8.1認(rèn)識OpenCV2使用OpenCV首先，新建一個功能包，包名為ros_opencv：然后新建一個img_sub.cpp文件，接收圖像并顯示，內(nèi)容如下：8.1認(rèn)識OpenCV2使用OpenCV首先，新建一個功能包，包名為ros_opencv：編寫配置文件CMakeLists.txt，內(nèi)容如下：8.2單目視覺傳感器的使用在ROS系統(tǒng)中，常用的視覺傳感器是相機(jī)，其價格低廉且易使用。和其他傳感器不同的是，在正式使用相機(jī)的數(shù)據(jù)之前需要對它進(jìn)行標(biāo)定。標(biāo)定可以使用ROS自帶的camera_calibration包進(jìn)行。當(dāng)相機(jī)通過USB與計算機(jī)連接之后，下一步便是通過ROS來調(diào)取相機(jī)圖像了。不過，在ROS中，相機(jī)并不被原生支持，需要先安裝驅(qū)動來獲取相機(jī)圖像并將其發(fā)布出去，相機(jī)的驅(qū)動安裝完成之后，就可以使用image_view包來顯示圖像了，然后進(jìn)一步將圖像提供給其他算法來做處理。8.3相機(jī)標(biāo)定相機(jī)標(biāo)定在計算機(jī)視覺中有著非常廣泛的應(yīng)用，如測量、導(dǎo)航、三維重建等領(lǐng)域，其主要目的是通過標(biāo)定算法獲得相機(jī)的內(nèi)參（焦距、光心等）、外參（坐標(biāo)變換關(guān)系）和畸變系數(shù)，從而得到物體二維圖像與三維空間之間的對應(yīng)關(guān)系。標(biāo)定方法主要是，使用一種已知的標(biāo)定圖案（制作成標(biāo)定板），對不同角度的視圖進(jìn)行辨識。常用的標(biāo)定板有三種：棋盤格（Check-board）、April-grid和實(shí)心圓陣列（Circle-grid），如圖所示。后面的相機(jī)標(biāo)定介紹使用經(jīng)典的棋盤格圖案。8.3相機(jī)標(biāo)定1針孔相機(jī)模型相機(jī)成像的過程即通過透視投影變換，將三維世界中的坐標(biāo)點(diǎn)映射到二維圖像平面上的過程。這個過程可以用一個幾何模型進(jìn)行描述，通常稱為針孔成像模型，對應(yīng)的相機(jī)成像模型稱為針孔相機(jī)模型。在不考慮透鏡畸變的情況下，建立針孔相機(jī)模型，如圖所示。在相機(jī)前建立一個虛擬成像平面，得到虛擬成像圖像，并定義焦點(diǎn)、焦距。8.3相機(jī)標(biāo)定1針孔相機(jī)模型在針孔相機(jī)模型的基礎(chǔ)上，建立如圖所示的四個坐標(biāo)系，包括像素坐標(biāo)系Op（以像素為單位，坐標(biāo)原點(diǎn)圖像在左上角）、成像平面坐標(biāo)系Oi、相機(jī)坐標(biāo)系Oc（以相機(jī)光心為原點(diǎn)）和世界坐標(biāo)系Ow。8.3相機(jī)標(biāo)定1針孔相機(jī)模型像素坐標(biāo)與成像平面坐標(biāo)之間的變換以像素為單位建立像素直角坐標(biāo)系Op，用某點(diǎn)坐標(biāo)(u,v)來描述某一像素在圖像中的位置。成像平面坐標(biāo)系Oi建立在相機(jī)的CCD（電荷耦合器件）或CMOS元件上，為簡化變換矩陣，將原點(diǎn)設(shè)為感光元件的中心，則可以得到像素坐標(biāo)(u,v)與成像平面坐標(biāo)(x1,y1)之間的對應(yīng)關(guān)系：x1=udx???u0dx=>u=x1/dx+u0y1=vdy???v0dy=>v=y1/dy+v0式中，(u0,v0)表示成像平面坐標(biāo)系Oi的原點(diǎn)在像素坐標(biāo)系中的坐標(biāo)，dx和dy表示每個像素在成像平面坐標(biāo)系x和y方向上的實(shí)際物理長度，即單個像素點(diǎn)的實(shí)際物理尺寸。8.3相機(jī)標(biāo)定1針孔相機(jī)模型像素坐標(biāo)與成像平面坐標(biāo)之間的變換這樣，圖像的像素坐標(biāo)與成像平面坐標(biāo)（感光單元）的變換用齊次變換方程可以表示為8.3相機(jī)標(biāo)定1針孔相機(jī)模型成像平面坐標(biāo)與相機(jī)坐標(biāo)之間的變換以相機(jī)的光心（相機(jī)鏡頭與光軸的交點(diǎn)）為原點(diǎn)，建立三維的相機(jī)坐標(biāo)系Oc，其x軸、y軸、z軸分別與成像平面坐標(biāo)系的x軸、y軸、光軸平行。成像平面坐標(biāo)系與相機(jī)坐標(biāo)系的原點(diǎn)均位于光軸上，將兩原點(diǎn)之間的歐氏距離定義為相機(jī)的焦距f（一般為mm級別）。設(shè)相機(jī)坐標(biāo)系中有某目標(biāo)點(diǎn)P(x,y,z)，點(diǎn)P在成像平面坐標(biāo)系中的投影為點(diǎn)p(x1,y1)，由相似原理，可得兩點(diǎn)之間的對應(yīng)關(guān)系為：f/z=x1/x=y1/y=>x1z=fx,y1z=fy8.3相機(jī)標(biāo)定1針孔相機(jī)模型成像平面坐標(biāo)與相機(jī)坐標(biāo)之間的變換這樣，成像平面坐標(biāo)系中的點(diǎn)p的坐標(biāo)與相機(jī)坐標(biāo)系中的點(diǎn)P的坐標(biāo)的變換用齊次變換方程可以表示為8.3相機(jī)標(biāo)定1針孔相機(jī)模型相機(jī)坐標(biāo)與世界坐標(biāo)之間的變換為了描述相機(jī)和世界坐標(biāo)系中的物體之間的相對位置關(guān)系，可以根據(jù)實(shí)際情況，選取世界坐標(biāo)系的原點(diǎn)。設(shè)相機(jī)坐標(biāo)系中的點(diǎn)P在對應(yīng)于世界坐標(biāo)系中的點(diǎn)

，由于相機(jī)坐標(biāo)系與世界坐標(biāo)系之間可以通過旋轉(zhuǎn)和平移進(jìn)行變換，因此兩點(diǎn)坐標(biāo)的對應(yīng)關(guān)系為其中，R為3×3的旋轉(zhuǎn)矩陣，t為三維平移向量。8.3相機(jī)標(biāo)定1針孔相機(jī)模型像素坐標(biāo)與世界坐標(biāo)之間的變換像素坐標(biāo)與世界坐標(biāo)之間的變換關(guān)系如下：8.3相機(jī)標(biāo)定2畸變模型透鏡制造精度及組裝工藝的偏差會引入畸變，導(dǎo)致原始圖像失真。鏡頭的畸變分為由透鏡形狀引起的徑向畸變，以及由透鏡平面與傳感器像素平面安裝不平行引起的切向畸變。徑向畸變是沿著透鏡半徑方向產(chǎn)生的畸變，產(chǎn)生原因是光線在離透鏡中心遠(yuǎn)的地方比靠近透鏡中心的地方更加彎曲，離圖像中心越遠(yuǎn)的位置，畸變越嚴(yán)重。這種畸變在普通廉價的鏡頭中表現(xiàn)得更加明顯。徑向畸變主要包括桶形畸變和枕形畸變兩種。8.3相機(jī)標(biāo)定2畸變模型可以通過泰勒展開式表示徑向畸變，有：其中，

為矯正前的歸一化像素坐標(biāo)，

為矯正后的歸一化像素坐標(biāo)。r為坐標(biāo)點(diǎn)到坐標(biāo)系原點(diǎn)的距離，

。k1、k2、k3為透鏡徑向畸變系數(shù)，一般取前兩個系數(shù)進(jìn)行校正，對于畸變較大的相機(jī)可以取到k3。當(dāng)圖像的中間部分失真程度較小時，用k1矯正畸變，而對于失真程度較大的圖像邊緣部分，可以加入k2進(jìn)行矯正。8.3相機(jī)標(biāo)定2畸變模型切向畸變是由透鏡本身與相機(jī)傳感器平面（成像平面）或圖像平面不平行而產(chǎn)生的，這種情況多是由于將透鏡粘貼到鏡頭模組上時的安裝偏差導(dǎo)致的。切向畸變模型可以用兩個額外的參數(shù)p1和p2來描述：其中，p1

、p2是透鏡切向畸變系數(shù)。8.3相機(jī)標(biāo)定2畸變模型徑向畸變和切向畸變模型中共有五個畸變參數(shù)，求得這五個參數(shù)后，就可以校正由于鏡頭畸變引起的圖像的變形失真了。8.3相機(jī)標(biāo)定3相機(jī)標(biāo)定的原理和過程根據(jù)針孔相機(jī)模型和畸變模型，可以得到四個相機(jī)參數(shù)和五個畸變參數(shù)，只要確定相機(jī)參數(shù)和畸變參數(shù)就可以唯一確定針孔相機(jī)模型，這個過程稱為“相機(jī)標(biāo)定”。一旦相機(jī)結(jié)構(gòu)固定，即鏡頭結(jié)構(gòu)固定、對焦距離固定，就可以用這些參數(shù)近似模擬這個相機(jī)。如圖所示，使用棋盤格標(biāo)定板進(jìn)行單目相機(jī)標(biāo)定。8.3相機(jī)標(biāo)定3相機(jī)標(biāo)定的原理和過程將標(biāo)定板的左上角點(diǎn)定義為世界坐標(biāo)系的原點(diǎn)Ow，并且將Xw軸及Yw軸定義在標(biāo)定板平面上，有：根據(jù)上式，定義單應(yīng)性矩陣H：8.3相機(jī)標(biāo)定3相機(jī)標(biāo)定的原理和過程單應(yīng)性矩陣H用于描述像素坐標(biāo)系與世界坐標(biāo)系之間的變換關(guān)系，它同時包含了相機(jī)的內(nèi)參和外參。由于標(biāo)定板尺寸已知，因此各角點(diǎn)的世界坐標(biāo)（x,y）是已知的，且各角點(diǎn)在圖像中的像素坐標(biāo)（u,v）也是已知的，有：8.3相機(jī)標(biāo)定3相機(jī)標(biāo)定的原理和過程由于H可進(jìn)行任意尺度的縮放，且不會影響上式的計算結(jié)果。因此，通常限制H的模為1：根據(jù)式（8-10）和式（8-11），利用棋盤格上四個角點(diǎn)的像素坐標(biāo)及對應(yīng)的世界坐標(biāo)，列出9個等式，即可求得矩陣H。于矩陣H是相機(jī)內(nèi)參和外參的混合體，因此在求得矩陣H后還需要進(jìn)一步分離出相機(jī)內(nèi)參矩陣。根據(jù)旋轉(zhuǎn)矩陣的性質(zhì)，r1和r2正交，且模為1，可得：8.3相機(jī)標(biāo)定3相機(jī)標(biāo)定的原理和過程若設(shè)

，則有：進(jìn)一步地，選取多張圖像計算出對應(yīng)的H矩陣，并結(jié)合上式列出方程組，即可求解出相機(jī)的標(biāo)準(zhǔn)內(nèi)參矩陣K。在上述相機(jī)內(nèi)參求解過程中，并未考慮相機(jī)畸變及圖像噪聲，為減小不確定因素對標(biāo)定結(jié)果產(chǎn)生的影響，在實(shí)際的標(biāo)定過程中，通常將相機(jī)內(nèi)參和畸變參數(shù)的求解視為極大似然估計問題。8.3相機(jī)標(biāo)定4相機(jī)標(biāo)定功能包ROS提供了許多成熟的相機(jī)標(biāo)定功能包，如visp_camera_calibration，camera_calibration等。下面選擇camera_calibration功能包進(jìn)行標(biāo)定，該功能包采用張正友標(biāo)定法，并利用OpenCV相關(guān)函數(shù)的Python接口實(shí)現(xiàn)。為了簡化計算過程，增加求解的穩(wěn)定性，張正友標(biāo)定法只考慮了影響最大的徑向畸變，且僅選取二階畸變系數(shù)，通過線性模型計算得到內(nèi)參矩陣，隨后用最小二乘法估計，并用極大似然估計進(jìn)行優(yōu)化，得到畸變系數(shù)。camera_calibration功能包提供了友好的GUI界面與API，通過啟動camera_calibrator節(jié)點(diǎn)，即可加載GUI界面完成圖像的采集、參數(shù)的計算與保存等。8.3相機(jī)標(biāo)定4相機(jī)標(biāo)定功能包若不需要使用image_view功能包來顯示圖像，則可以精簡8.2節(jié)所使用的test.launch文件，去掉開啟image_view功能包相關(guān)的語句，修改如下：運(yùn)行修改后的test.launch文件之后，執(zhí)行下面的命令：8.3相機(jī)標(biāo)定4相機(jī)標(biāo)定功能包上面的命令中，傳遞了幾個參數(shù)，意義如下。size：當(dāng)前標(biāo)定所使用的棋盤格標(biāo)定板的大小，棋盤格角點(diǎn)的數(shù)量。square：每個棋盤格的邊長，以米為單位。image：使用的圖像主題（Topic）。no-service-check：在啟動時禁用檢查set_camera_info的服務(wù)。8.3相機(jī)標(biāo)定4相機(jī)標(biāo)定功能包運(yùn)行文件后，顯示的界面如圖所示。8.3相機(jī)標(biāo)定4相機(jī)標(biāo)定功能包界面正常出現(xiàn)后，我們需要手持棋盤格標(biāo)定板在不同的位置、不同的角度讓程序識別，如圖所示。注意，需要確保棋盤格全部出現(xiàn)在圖像中。8.3相機(jī)標(biāo)定4相機(jī)標(biāo)定功能包在每個不同的位置保持片刻，等棋盤格被彩色高亮標(biāo)識后再移動。在移動過程中，可以看到，窗口右上角的幾個進(jìn)度條在慢慢變長，同時顏色也漸漸趨向于綠色，如圖所示。當(dāng)右側(cè)的CALIBRATE按鈕亮起之后，表示已經(jīng)采集完標(biāo)定所需的數(shù)據(jù)，單擊CALIBRATE按鈕，系統(tǒng)便會自動計算并顯示結(jié)果。8.4圖像變換與處理1透視變換在平面圖像處理中，由于鏡頭角度的不同，可能會出現(xiàn)圖像傾斜、變形等情況，為了方便后續(xù)處理，常常需要進(jìn)行圖像矯正，其中主要技術(shù)包括兩種變換：仿射變換（AffineTransformation）和透視變換（PerspectiveTransformation）。仿射變換是二維坐標(biāo)之間的線性變換，故變換后的圖像仍然具有原圖的一些性質(zhì)，包括“平直性”及“平行性”，常用于圖像翻轉(zhuǎn)（Flip）、旋轉(zhuǎn)（Rotation）、平移（Translation）、縮放（ScaleOperation）等。但是仿射變換不能矯正一些變形，如果矩形區(qū)域發(fā)生部分形變，變成了梯形，這時就需要用到透視變換進(jìn)行矯正。透視變換是三維空間上的非線性變換，可視為仿射變換的更一般形式。8.4圖像變換與處理1透視變換簡單地說，透視變換通過一個3×3的變換矩陣，將原圖投影到一個新的視平面（ViewingPlane）上，因此，也稱為投影映射（ProjectiveMapping）或投射變換，其在視覺上的直觀表現(xiàn)就是產(chǎn)生或消除了遠(yuǎn)近感，其通用的變換公式為式中，(u,v)是像素坐標(biāo)，對應(yīng)得到的變換后的圖像坐標(biāo)為(x,y)，其中，

。8.4圖像變換與處理1透視變換下面介紹OpenCV中，實(shí)現(xiàn)上述過程的兩個相關(guān)函數(shù)：圖像的透視變換函數(shù)和點(diǎn)的透視變換函數(shù)。圖像的透視變換函數(shù)利用透視變換矩陣對圖像進(jìn)行透視變換，函數(shù)名為warpPerspective()，返回透視變換后的結(jié)果圖像：8.4圖像變換與處理1透視變換warpPerspective()函數(shù)的參數(shù)如下。InputArraysrc：輸入的源圖像。OutputArraydst：輸出的圖像，即變換后的圖像。InputArrayM：透視變換矩陣。Sizedsize：輸出圖像的大小。intflags=INTER_LINEAR：輸出圖像的插值方法，可選參數(shù)。intborderMode=BORDER_CONSTANT：邊界像素模式，可選參數(shù)。constScalar&borderValue=Scalar()：邊界填充值，可選參數(shù)。8.4圖像變換與處理1透視變換其中，透視變換矩陣需要使用findHomography()函數(shù)計算，其是一個單映射變換矩陣。findHomography()函數(shù)通過輸入和輸出圖像中的兩組點(diǎn)計算透視變換矩陣，函數(shù)如下：8.4圖像變換與處理1透視變換findHomography()函數(shù)的參數(shù)如下。InputArraysrcPoints：源圖像中點(diǎn)的坐標(biāo)矩陣。InputArraydstPoints：輸出圖像中點(diǎn)的坐標(biāo)矩陣。intmethod=0：可選參數(shù)，計算單映射變換矩陣所使用的算法。0代表利用所有點(diǎn)的常規(guī)算法，RANSAC代表基于RANSAC的魯棒算法，LMEDS代表最小中值魯棒算法，RHO代表PROSAC魯棒算法。doubleransacReprojThreshold=3：可選參數(shù)，將點(diǎn)對視為內(nèi)點(diǎn)的最大允許重投影錯誤閾值（僅用于method為RANSAC和RHO時）。8.4圖像變換與處理1透視變換點(diǎn)的透視變換函數(shù)利用透視變換矩陣對一組點(diǎn)進(jìn)行透視變換，函數(shù)名為perspectiveTransform()：perspectiveTransform()函數(shù)的參數(shù)如下。InputArraysrc：輸入的源圖像中一組點(diǎn)的坐標(biāo)。OutputArraydst：輸出圖像中對應(yīng)該組點(diǎn)的坐標(biāo)，即變換后該組點(diǎn)的坐標(biāo)。InputArrayM：透視變換矩陣。8.4圖像變換與處理1透視變換圖是一個透視變換的例子。8.4圖像變換與處理2圖像匹配透視變換可以為我們提供更好的圖像視野，而圖像匹配則可以讓我們從一個圖像中找到自己關(guān)心的目標(biāo)圖像，在實(shí)際應(yīng)用中，圖像匹配有著重要的作用。圖像匹配的常見方法就是模板匹配，模板匹配是在一個圖像中尋找與模板圖像最匹配（相似）部分。例如，有一個原圖像和一個模板圖像，在原圖像中，我們希望找到一塊和模板圖像匹配的區(qū)域。圖像匹配就是從原圖像中搜索檢測出與模板圖像匹配的目標(biāo)區(qū)域的過程，如圖所示。8.4圖像變換與處理2圖像匹配在OpenCV中，進(jìn)行圖像匹配的函數(shù)是cvMatchTemplate()：cvMatchTemplate()函數(shù)的參數(shù)如下。image：待搜索的原圖像。templ：模板圖像。result：匹配結(jié)果，用來存放滑動窗口與模板圖像的相似度。method：計算滑動窗口與模板圖像相似度的方法，具體如下。8.4圖像變換與處理3圖像拼接除圖像匹配之外，圖像拼接也是圖像處理中需要經(jīng)常用到的方法。圖像拼接用于把多個圖像拼接成一個全景圖像，或者把同一場景、不同視角下的圖像拼接在一起，在實(shí)際應(yīng)用中非常重要。圖像拼接算法相對比較復(fù)雜，在OpenCV中，stitcher作為圖像拼接類，簡單實(shí)用。這個類中，主要的成員函數(shù)有createDefault()、estimateTransform()、composePanorama()、stitch()。8.4圖像變換與處理3圖像拼接在OpenCV中，使用stitcher類進(jìn)行圖像拼接的主要步驟如下。創(chuàng)建一個stitcher對象：選擇拼接方式，有以下幾種。平面拼接：柱面拼接：8.4圖像變換與處理3圖像拼接在OpenCV中，使用stitcher類進(jìn)行圖像拼接的主要步驟如下。創(chuàng)建一個stitcher對象：選擇拼接方式，有以下幾種。立體畫面拼接：8.4圖像變換與處理3圖像拼接在OpenCV中，使用stitcher類進(jìn)行圖像拼接的主要步驟如下。生成全景圖像：圖給出了不同視角下的三個原始圖像8.4圖像變換與處理3圖像拼接在OpenCV中，使用stitcher類進(jìn)行圖像拼接的主要步驟如下。生成全景圖像：圖像拼接后的全景圖像如圖所示。8.5常見的圖像特征點(diǎn)檢測算法1SIFT算法首先介紹一下“尺度”的概念，如果一個圖像中的像素數(shù)目被不斷壓縮，或者觀察者距離圖像越來越遠(yuǎn)，那么圖像將逐漸變得模糊。導(dǎo)致圖像的呈現(xiàn)內(nèi)容發(fā)生變化的連續(xù)自變量就稱為尺度。觀察物體時的尺度不同，物體呈現(xiàn)的方式也不同。8.5常見的圖像特征點(diǎn)檢測算法1SIFT算法特征點(diǎn)檢測的尺度不變性是一個非常重要的概念。但是要解決尺度不變性問題，難度很大。為解決這一問題，計算機(jī)視覺領(lǐng)域引入了尺度不變特征的概念，即不僅在任何尺度下拍攝的物體都能檢測到一致的特征點(diǎn)，而且每個被檢測的特征點(diǎn)都對應(yīng)一個尺度因子。理想情況下，對于兩個圖像中不同尺度的同一個物體點(diǎn)，計算得到的兩個尺度因子之間的比率應(yīng)該等于兩個圖像尺度的比率。1999年Lowe提出了SIFT（Scale-InvariantFeatureTransform，尺度不變特征變換）算法，并于2003年對其進(jìn)行了完善和總結(jié)。8.5常見的圖像特征點(diǎn)檢測算法2SURF算法SURF（SpeededUpRobustFeature）算法是SIFT算法的加速版，也具有尺度不變性，全稱為“加速穩(wěn)健特征”，其不僅具有尺度不變特征，而且具有較高的計算效率。SURF算法相對于SIFT算法而言，特征點(diǎn)檢測的速度有極大的提升，所以在實(shí)時視頻流中的物體匹配方面，有著廣泛的應(yīng)用。而SIFT算法因為其巨大的特征點(diǎn)計算量，使得特征點(diǎn)提取的過程非常耗時，所以在一些注重速度的場合難有應(yīng)用。但是SIFT算法相對于SURF算法的優(yōu)點(diǎn)是：SIFT算法檢測的特征在空間和尺度上的定位更加精確，所以在要求匹配極精準(zhǔn)且不考慮匹配速度的場合，可以考慮使用SIFT算法。8.5常見的圖像特征點(diǎn)檢測算法3FAST算法在圖像中搜索有價值的特征點(diǎn)時，使用角點(diǎn)是一種不錯的方法。角點(diǎn)是很容易在圖像中定位的局部特征，并且大量存在于人造物體（如墻壁、門、窗戶、桌子等）中。角點(diǎn)的價值在于它是兩條邊緣線的結(jié)合點(diǎn)，是一種二維特征，可以被精確地定位。