TWI662388B - 機器人之避障控制系統及方法 - Google Patents

機器人之避障控制系統及方法 Download PDF

Info

Publication number
TWI662388B
TWI662388B TW106135246A TW106135246A TWI662388B TW I662388 B TWI662388 B TW I662388B TW 106135246 A TW106135246 A TW 106135246A TW 106135246 A TW106135246 A TW 106135246A TW I662388 B TWI662388 B TW I662388B
Authority
TW
Taiwan
Prior art keywords
robot
obstacle
model
distance
predetermined
Prior art date
Application number
TW106135246A
Other languages
English (en)
Other versions
TW201835703A (zh
Inventor
周濤濤
周寶
肖京
Original Assignee
平安科技（深圳）有限公司
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 平安科技（深圳）有限公司 filed Critical 平安科技（深圳）有限公司
Publication of TW201835703A publication Critical patent/TW201835703A/zh
Application granted granted Critical
Publication of TWI662388B publication Critical patent/TWI662388B/zh

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B25J9/1666—Avoiding collision or forbidden zones
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1674—Programme controls characterised by safety, monitoring, diagnostic
    • B25J9/1676—Avoiding collision or forbidden zones
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0242—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using non-visible light signals, e.g. IR or UV signals
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0255—Control of position or course in two dimensions specially adapted to land vehicles using acoustic signals, e.g. ultra-sonic singals
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
    • G05D1/0891—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for land vehicles
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2210/00—Indexing scheme for image generation or computer graphics
    • G06T2210/12—Bounding box

Abstract

本揭示公開了一種機器人之避障控制系統及方法，該方法包括：獲取機器人當前的定位資料，並根據當前定位資料及預先確定的移動區域內各個障礙物的位置資料，確定當前定位位置至目標位置路徑中是否有離當前定位位置的距離小於預設距離的障礙物；若有離當前定位位置的距離小於預設距離的障礙物，則根據獲取的定位資料、預先確定的機器人的3D模型及預先確定的該障礙物的3D模型，計算出機器人與該障礙物的最短距離，進而計算出當前機器人應運動的方向，並依此控制機器人的運動姿態，以避開障礙物。本揭示能有效地控制機器人避障。

Description

機器人之避障控制系統及方法
本揭示是關於電腦技術領域，特別是關於一種機器人之避障控制系統及方法。
目前，自主移動機器人能夠廣泛應用於許多場景，比如擔任展覽館的導覽工作，帶領參觀者從一個展區介紹到另一個展區；餐廳的服務工作，主動歡迎客人，並帶領客人到空位上點餐；公共場所的引導、巡邏工作，沿著程式設定的路線移動，有人需要幫助停下回答提問等等。在這些場景下，如何避免機器人在運動中碰撞環境中的障礙物，是一個重要的技術問題。目前，自主移動機器人是依靠自身的感測器來定位及避障，業界通常的避障方案是：在機器人上安裝接近感測器(例如，超聲波、紅外、雷射等感測器)，如果機器人檢測到距障礙物一定距離(比如10cm)，則進行避障。
習知的避障方案有以下缺點：第一、只能在感測器所在高度平面檢測到障礙物，對於四腿桌子等情况，假設感測器所在高度為30釐米，而桌面高度為60釐米，那麽感測器無法檢測到障礙物，最終會導致機器人撞上桌面；第二、只能在安裝有感測器的方向檢測到障礙物，假如機器人 後面沒有感測器，則後退會導致撞上障礙物。
因此，如何在感測器無法全面偵測的情况下，有效控制機器人避障已經成為一個極待解决的技術問題。
本揭示的主要目的在於提供一種機器人之避障控制系統及方法，旨在有效控制機器人避障。
為實現上述目的，本揭示提供的一種機器人之避障控制系統，該避障控制系統包括：確定模組，用於實時或者定時獲取機器人當前的定位資料，並根據當前定位資料及預先確定的移動區域內各個障礙物的位置資料，確定當前定位位置至目標位置路徑中是否有離當前定位位置的距離小於預設距離的障礙物；計算模組，用於若有離當前定位位置的距離小於預設距離的障礙物，則根據獲取的定位資料、預先確定的機器人的3D模型及預先確定的該障礙物的3D模型，計算出機器人與該障礙物的最短距離；控制模組，用於根據獲取的定位資料、計算的最短距離及該障礙物的3D模型，計算出當前機器人應運動的方向，根據計算出的運動方向控制機器人的運動姿態，以避開障礙物。
較佳地，該計算模組還用於：對預先確定的機器人的3D模型及該障礙物的3D模型進行預處理，針對機器人的每一個關節，利用預先確定的算法找出包圍各個關節的最小凸多面體，以將機器人的非凸模型轉換為凸體模型；對該障礙物3D 模型扇形均分，並對均分後的單個扇形進行凸分解；對獲取的定位資料、預處理後的機器人3D模型資料及預處理後的該障礙物3D模型資料，利用預先確定的距離計算規則計算出機器人與該障礙物的最短距離。
較佳地，該計算模組還用於：建立待扇形均分的障礙物3D模型的球形包圍盒，找到球形包圍盒的球心；設定一經過該球心的初始扇形均分平面，將該初始扇形均分平面按預設的旋轉角繞該球心進行多次旋轉，以將球形包圍盒均分為n個扇形部分；該球形包圍盒的n個扇形部分作為障礙物3D模型的n個模型部分。
較佳地，該預先確定的距離計算規則包括：根據機器人當前的定位資料及預先確定的篩選算法，對障礙物3D模型扇形均分後獲得的各個模型部分進行篩選，篩選出待進行距離計算的模型部分；對獲取的定位資料、篩選出的模型部分，利用預先確定的距離計算算法計算出機器人與篩選出的模型部分的最短距離，該最短距離為機器人與障礙物的最短距離；該預先確定的篩選算法包括：將障礙物3D模型的n個模型部分分別作為障礙物的n個節點，建立鍵值為相對於初始扇形均分平面的偏角的哈希表，以進行模型節點管理；對各個模型部分進行標號，根據標號，建立標號為i的模型部分的偏角映射關係，定義該偏角映射關係的哈希函數為：
其中，Hash(i)代表標號為i的扇形模型部分與障礙物座標系的X軸正軸的偏角；建立機器人的運動學方程，根據建立的運動學方程計算出機器人各個關節的位姿，該運動學方程為：T i=A 0 A 1 A 2…A i-1 A i
其中，A k (k=1,2,...,i)為機器人關節座標系之間的齊次變換矩陣，A 0為機器人當前位置矩陣，T i 為第i個關節相對於機器人座標系的位姿；通過T i 計算出機器人運動過程中各個關節在機器人座標系下的座標Q i (x,y,z)，並計算出機器人座標系變換到障礙物座標系的變換矩陣T r ，則機器人關節在障礙物座標系下的座標Q i (x t ,y t ,z t )為：Q i (x t ,y t ,z t )=T r Q i (x,y,z)
通過如下公式得到關節在障礙物座標系下的偏角α：
根據偏角α及哈希函數Hash(i)計算得到對應標號的模型部分，並基於對應標號的模型部分篩選出待進行距離計算的模型部分。
較佳地，該控制模組還用於：將機器人及該障礙物3D模型投影到同一座標系平面中；根據預先確定的投影分析規則及障礙物3D模型投影到該座標系平面的投影區域外輪廓各個點的座標，計算出該障礙物3D模型的投影相對於機器人的當前定位位置及目標位置有效遮擋區域的面積； 根據計算的最短距離及有效遮擋區域的面積確定出第一預設類型避障參數，根據目標位置與機器人當前定位位置的距離確定出第二預設類型避障參數，根據該第一預設類型避障參數及該第二預設類型避障參數確定出機器人當前應運動的方向。
較佳地，該預先確定的投影分析規則為：設定座標系平面的P 1位置點為機器人的當前定位位置，P 2位置點為目標位置，投影區域P3為障礙物3D模型在該座標系平面中的投影，並在該座標系平面中連接P 1 P 2，得到直線J；若直線J與投影區域P3沒有交點或者交點只有一個，則確定不存在有效遮擋區域；若直線J與投影區域P3的交點個數大於1，則直線J將投影分割為兩部分；在投影區域P3中任意找一點P S ，過P S 作直線J的垂線，設定 垂線與直線J的交點為P J ，得到向量，計算最短距離的向量與向量 的夾角θ；若判斷夾角θ是銳角，則確定P S 點所在區域是有效遮擋區域；若判斷夾角θ不是銳角，則確定P S 點所在區域不是有效遮擋區域。
較佳地，該第一預設類型避障參數為虛擬斥力，該第二預設類型避障參數為虛擬引力，該控制模組還用於：根據計算的最短距離和有效遮擋區域的面積，利用第一計算規則計算出作用在機器人上的虛擬斥力；根據當前定位位置與目標點位置的距離，利用第二計算規則計算出作用在機器人上的虛擬引力；計算出該虛擬引力和虛擬斥力的合力方向作為機器人當前 應運動的方向。
較佳地，該第一計算規則為： 設定機器人與障礙物的最短距離的向量為，有效遮擋區域 的面積為S，障礙物對機器人的虛擬斥力為，則計算公式如下：
其中，k r 、b r 為預設的虛擬斥力係數，s 0為預設的有效遮擋區域面積閾值，d 0為預設的距離閾值；虛擬斥力方向與最短距離方向相同。
較佳地，該第二計算規則為： 設定目標位置對機器人的虛擬引力為，則計算公式為： ；其中，k t 為預設的引力係數，d t 為目標位置與機器人當前定位位 置的距離，方向朝向目標位置。
此外，為實現上述目的，本揭示還提供一種機器人避障方法，該方法包括以下步驟：A1、實時或者定時獲取機器人當前的定位資料，並根據當前定位資料及預先確定的移動區域內各個障礙物的位置資料，確定當前定位位置至目標位置路徑中是否有離當前定位位置的距離小於預設距離的障礙物；A2、若有離當前定位位置的距離小於預設距離的障礙物，則根據獲取的定位資料、預先確定的機器人的3D模型及預先確定的該障礙 物的3D模型，計算出機器人與該障礙物的最短距離；A3、根據獲取的定位資料、計算的最短距離及該障礙物的3D模型，計算出當前機器人應運動的方向，根據計算出的運動方向控制機器人的運動姿態，以避開障礙物。
本揭示提出的機器人之避障控制系統及方法，通過機器人當前的定位資料檢測到有離當前定位位置的距離小於預設距離的障礙物時，根據機器人當前的定位資料、預先確定的機器人的3D模型及預先確定的該障礙物的3D模型，計算出機器人與該障礙物在三維空間的最短距離，並計算出當前機器人應運動的方向，以控制機器人的運動姿態。由於能通過機器人與障礙物在三維空間的最短距離來控制機器人的運動方向，從而實現檢測並避開機器人在三維空間中各個方向的障礙物，有效地控制機器人避障。
S10~S30‧‧‧步驟
1、2、3、4、8、16、24、31、32、i-1、i、i+1、i+2、n-2、n-1、n‧‧‧扇形模型部分的對應標號
X、Y、Z‧‧‧座標
O‧‧‧座標系原點
S1、S2‧‧‧面積
J‧‧‧直線
P1、P2、(x1,y1,z1)、(x2,y2,z2)‧‧‧點
‧‧‧向量
1‧‧‧機器人
10‧‧‧避障控制系統
11‧‧‧儲存器
12‧‧‧處理器
13‧‧‧顯示器
01‧‧‧確定模組
02‧‧‧計算模組
03‧‧‧控制模組
α‧‧‧偏角
第1圖為本揭示機器人避障方法一實施例的流程示意圖；第2a圖為本揭示機器人避障方法一實施例中對障礙物3D模型扇形均分的示意圖；第2b圖為本揭示機器人避障方法一實施例中標號為k的扇形模型部分的示意圖；第3a圖為本揭示機器人避障方法一實施例中機器人與障礙物的3D模型示意圖；第3b圖為本揭示機器人避障方法一實施例中對立方體障礙物模型進行 扇形均分的示意圖；第3c圖為本揭示機器人避障方法一實施例中對模型部分的篩選示意圖；第3d圖為本揭示機器人避障方法一實施例中計算最短距離向量的示意圖；第4圖為本揭示機器人避障方法一實施例中確定有效遮擋區域的示意圖。
第5圖為本揭示避障控制系統10較佳實施例的運行環境示意圖；以及第6圖為本揭示避障控制系統10較佳實施例的功能模組圖。
本揭示目的的實現、功能特點及優點將結合實施例，參照所附圖式做進一步說明。
為了使本揭示所要解决的技術問題、技術方案及有益效果更加清楚、明白，以下結合所附圖式和實施例，對本揭示進行進一步詳細說明。應當理解，此處所描述的具體實施例僅僅用以解釋本揭示，並不用於限定本揭示。
本揭示提供一種機器人避障方法。
參照第1圖，第1圖為本揭示機器人避障方法一實施例的流程示意圖。
在一實施例中，該機器人避障方法包括：步驟S10、機器人之避障控制系統實時或者定時(例如，每隔2秒)獲取機器人當前的定位資料(例如在室內的位置、姿態等)，並根據當前定位資料及預先確定的移動區域內各個障礙物的位置資料，確定當 前定位位置至目標位置路徑中是否有離當前定位位置的距離小於預設距離的障礙物。例如，可依靠機器人自身的感測器來定位並判斷與預先確定的移動區域內各個障礙物的距離，如可在機器人上安裝接近感測器(例如，超聲波、紅外、雷射等感測器)來判斷機器人當前定位位置至目標位置路徑中是否有離當前定位位置的距離小於預設距離的障礙物。
步驟S20、若有離當前定位位置的距離小於預設距離的障礙物，則根據獲取的定位資料、預先確定的機器人的3D模型及預先確定的該障礙物的3D模型，計算出機器人與該障礙物的最短距離。
在檢測到機器人的當前定位位置與預先確定的移動區域內各個障礙物的距離之後，若判斷沒有障礙物離當前定位位置的距離小於預設距離，則繼續沿目標位置路徑移動並實時或者定時檢測機器人與移動區域內各個障礙物的距離。若判斷有離當前定位位置的距離小於預設距離的障礙物，則根據獲取的定位資料、預先確定的機器人的3D模型及預先確定的該障礙物的3D模型計算出機器人與該障礙物的最短距離，以利用該最短距離來判斷在三維空間中機器人沿目標位置路徑移動時是否會碰撞到該障礙物，從而實現不僅能在機器人的感測器所在高度平面檢測到障礙物，還能檢測到三維空間中潛在的障礙物，以在機器人安裝有感測器的方向和機器人沒有安裝感測器的其他方向上均能檢測到三維空間中各個方向潛在的障礙物。其中，該預先確定的機器人的3D模型及移動區域內各個障礙物的3D模型可以預先儲存於機器人的儲存單元中，或者，可以由機器人通過無線通訊單元訪問物聯網系統伺服器獲取，在此不做限定。
步驟S30、根據獲取的定位資料、計算的最短距離及該障礙 物的3D模型，計算出當前機器人應運動的方向，根據計算出的運動方向控制機器人的運動姿態，以避開三維空間中各個方向潛在的障礙物，有效控制機器人在沿目標位置路徑移動時的避障。
本實施例通過機器人當前的定位資料檢測到有離當前定位位置的距離小於預設距離的障礙物時，根據機器人當前的定位資料、預先確定的機器人的3D模型及預先確定的該障礙物的3D模型，計算出機器人與該障礙物在三維空間的最短距離，並計算出當前機器人應運動的方向，以控制機器人的運動姿態。由於能通過機器人與障礙物在三維空間的最短距離來控制機器人的運動方向，從而實現檢測並避開機器人在三維空間中各個方向的障礙物，有效地控制機器人避障。
進一步地，該步驟S20包括：步驟S201、對預先確定的機器人的3D模型及該障礙物的3D模型進行預處理。
步驟S202、對獲取的定位資料、預處理後的機器人3D模型資料及預處理後的障礙物3D模型資料，利用預先確定的距離計算規則計算出機器人與障礙物的最短距離。
例如，由於機器人和障礙物一般為非凸體，因此，可對機器人和障礙物的3D模型進行預處理如轉換為凸體，以便後續更加準確和快速地計算出最短距離。
進一步地，該步驟S201中的機器人3D模型預處理包括：針對機器人的每一個關節，直接利用預先確定的算法(例如，QuickHull快速凸包算法)找出包圍各個關節的最小凸多面體，以將機器人非凸模型轉換 為凸體模型。通過上述凸處理的機器人3D模型在後續計算最短距離向量時能有效提高計算速度和計算精度。
障礙物3D模型預處理的方式包括三種：第一種、構建非凸多面體的凸包圍盒使之轉換為凸體進行碰撞檢測；第二種、對非凸多面體進行凸分解，使非凸模型轉換為多個凸體進行碰撞檢測；第三種、對障礙物3D模型扇形均分(即扇形剖分)，然後對均分後的單個扇形進行凸分解，這種先扇形均分再凸分解的方式相對於前兩種不僅計算速度更快，而且計算精度更高。
進一步地，該對障礙物3D模型扇形均分的步驟包括：建立待扇形均分的障礙物3D模型的球形包圍盒，找到球形包圍盒的球心；設定一經過該球心的初始扇形均分平面，將該初始扇形均分平面按預設的旋轉角繞該球心進行多次旋轉，以將球形包圍盒均分為n個扇形部分；該球形包圍盒的n個扇形部分作為障礙物3D模型的n個模型部分。
例如，在一種具體實施方式中，可包括如下步驟：X1、建立要扇形均分的障礙物3D模型M的球形包圍盒B，找到球形包圍盒B的球心O，然後在球心O處建立三維座標系Oxyz；X2、過球心O做一條與三維座標系Oxyz中z軸重合的直線L，則xoz平面即為初始扇形均分平面，設xoz平面為α 1，α 1將障礙物3D模型分為2部分；X3、將α 1繞直線L選擇一定角度β(β代表相鄰扇形偏角)得到另外一個新平面α 2，將新平面繼續旋轉β可以得到平面α 3，旋轉m-1次 可以得到第m個平面α n； X4、設，則m個平面可以把球形包圍盒B均分為2m 部分，障礙物3D模型即被分為2m個模型部分。通過上述步驟可以完成對任意模型，包括非凸模型的簡單剖分，並通過哈希表進行管理剖分好的模型部分。
進一步地，該對均分後的單個扇形進行凸分解的步驟包括：採用Delaunay三角剖分算法對障礙物3D模型進行表面三角剖分，產生三角面片(凸片)集合；並針對每一個三角面片構造與之對應的凸塊。例如，將厚度為零的三角面片在其平面法向量方向進行預設厚度的拉伸，變為凸塊。
進一步地，該預先確定的距離計算規則包括：根據機器人當前的定位資料(如室內位置、姿態等)及預先確定的篩選算法，對障礙物3D模型扇形均分後獲得的各個模型部分進行篩選，篩選出待進行距離計算的模型部分；對獲取的定位資料、篩選出的模型部分，利用預先確定的距離計算算法(例如，GJK算法)計算出機器人與篩選出的模型部分的最短距離，該最短距離即為機器人與障礙物3D模型的最短距離。
進一步地，如第2a圖、第2b圖所示，第2a圖為本揭示機器人避障方法一實施例中對障礙物3D模型扇形均分的示意圖。第2b圖為本揭示機器人避障方法一實施例中標號為k的扇形模型部分的示意圖。該預先確定的篩選算法包括：Y1、將障礙物3D模型扇形均分後獲得的n個模型部分分別作 為障礙物的n個節點，建立key-value鍵值分別是相對於初始扇形均分平面(即xoz平面)的旋轉角即偏角和模型幾何訊息資料的哈希表，以進行模型節點管理；Y2、對扇形均分獲得的各個模型部分進行標號，從1開始進 行標號；均分的n個扇形模型部分，相鄰扇形偏角為，根據標號，建 立於初始標號為i的扇形模型部分的偏角映射關係，代表該偏角映射關係的哈希函數為：
其中，i為標號為i的扇形模型部分，Hash(i)代表標號為i的扇形模型部分與障礙物座標系的X軸正軸的偏角；Y3、建立機器人的運動學，根據建立的運動學計算出機器人各個關節的位姿，從建立的哈希表中查詢出機器人附近的障礙物扇形區域。如下圖2所示；當機器人在運動過程中，通過機器人運動學，運動學方程為：T i=A 0 A 1 A 2…A i-1 A i
其中，A k (k=1,2,...,i)為機器人關節座標系之間的齊次變換矩陣(可以通過機器人各關節的D-H參數確定)，A 0表示機器人當前位置矩陣(與機器人當前定位資料對應)，T i 為第i個關節相對於機器人座標系的位姿；通過T i 計算出機器人運動過程中各個關節局部座標系原點座標的實時更新值Q i (x,y,z)，進一步可以得到關節在障礙物座標系下的偏角α： α=f(Q i (x,y,z))
其中，Q i (x,y,z)表示機器人關節在機器人座標系下的座標；T r 表示機器人座標系變換到障礙物座標系的變換矩陣(為4*4的矩陣，機器人座標系和障礙物座標系已確定，該矩陣可以直接計算出來)，則機器人關節在障礙物座標系下的座標Q i (x t ,y t ,z t )為：Q i (x t ,y t ,z t )=T r Q i (x,y,z)
假定障礙物座標系Z軸正向朝上，遵循右手座標系，設關節在障礙物座標系下的偏角為α，則
向求解三角函數即可得到關節在障礙物座標系下的偏角為α，獲取到偏角α之後，即可根據代表該偏角映射關係的哈希函數Hash(i)計算得到對應標號的扇形模型部分，並基於對應標號的模型部分篩選出待進行距離計算的模型部分。例如，計算得到的扇形模型部分的標號為k，則可選取標號在[k-M,k+N]範圍內的扇形模型部分進行最短距離計算。其中M、N為一預設數值，以選取標號為k的扇形模型部分附近的多個扇形模型部分作為待進行最短距離計算的模型部分。
如第3a圖所示，第3a圖為本揭示機器人避障方法一實施例中機器人與障礙物的3D模型示意圖。在一種具體實施方式中，機器人採用只有底盤的運動、沒有手臂等其他運動關節的機器人，機器人3D模型採用高為1500mm，運動底盤半徑為320mm的機器人3D模型，障礙物3D模型採用一個簡單立方體模型，尺寸為2200mm*2200mm*1000mm，在障礙物模型座 標系下機器人當前的座標為(1800,-100)。
第3b圖為本揭示機器人避障方法一實施例中對立方體障礙物模型進行扇形均分的示意圖。對障礙物模型進行預處理中，預處理主要是對障礙物模型進行扇形均分，如第3b圖所示，障礙物模型被扇形均分為32份，從X軸逆時針對扇形均分的模型部分進行編號，1,2，...，15,16，...，31,32；每一個模型塊的夾角為：360/32=11.25度，可以看出，編號1模型塊與X軸正向偏角11.25度，編號2模型塊與X軸正向偏角11.25*2=22.5度，編號為i的模型塊與X軸正向偏角：i*(360/32)。
第3c圖為本揭示機器人避障方法一實施例中對模型部分的篩選示意圖。在對模型部分的篩選過程中，因本實施例採用的機器人只有底盤的運動，沒有手臂等其他運動關節，所以底盤位姿代表機器人的整體位姿，當前機器人的位置為(1800，-100)(相對於障礙物座標系下座標)，可以計算出機器人與障礙物座標系的X軸正軸的偏角為354度；進而電腦器人對應扇形模型部分的標號為354/11.25=31.5，向上取整得到32，所以待進行距離計算的對應扇形塊編號32，也就是說機器人離編號為32的障礙物塊最近。接下來選取K=32附近的障礙物塊，採用GJK計算與機器人之間的最短距離及最短距離點；選取M=1，N=2，則得到障礙物塊範圍是[31,34]，編號超過32的需要做簡單轉換，33轉換為對應編號為1的障礙物塊，34轉換為對應編號為2的障礙物塊；如第3c圖所示，最終選取編號是31,32,1,2的障礙物塊進行最短距離計算。
第3d圖為本揭示機器人避障方法一實施例中計算最短距離向量的示意圖。在計算最短距離時，通過上述處理，已縮小障礙物塊的範圍(1,2,31,32)，直接採用GJK算法計算出機器人與障礙物間的最短距離點， 如第3d圖所示，分別為障礙物上的點(x1,y1,z1)=(1100,-100,-235)，機器人上的點(x2,y2,z2)=(1477,-100,-235)；則機器人與障礙物之間的最短距離向量
進一步地，該步驟S30包括：機器人之避障控制系統根據計算的最短距離分析是否需要避障；如若計算的最短距離大於預設距離閾值，則確定不需要避障，或者，若計算的最短距離小於或者等於預設距離閾值，則確定需要避障。若確定需要避障，則機器人之避障控制系統根據獲取的定位資料、計算的最短距離及該障礙物的3D模型，計算出當前機器人應運動的方向，並根據計算出的運動方向控制機器人的運動姿態。
進一步地，該根據獲取的定位資料、計算的最短距離及該障礙物的3D模型，計算出當前機器人應運動的方向的步驟包括：將機器人及該障礙物投影到同一座標系平面中；根據預先確定的投影分析規則及障礙物3D模型投影到該座標系平面的投影區域外輪廓各個點的座標，計算出該障礙物的投影相對於機器人當前位置及目標位置有效遮擋區域的面積；根據計算的最短距離及有效遮擋區域的面積確定出第一預設類型避障參數(例如，虛擬斥力)，根據目標點位置與機器人當前定位位置的距離確定出第二預設類型避障參數(例如，虛擬引力)，根據第一預設類型避障參數及第二預設類型避障參數確定出機器人當前應運動的方向。
進一步地，如第4圖所示，第4圖為本揭示機器人避障方法一實施例中確定有效遮擋區域的示意圖。該預先確定的投影分析規則為：設座標系平面的P 1位置點表示機器人所在位置即當前定位位置，P 2位置點表示目標點所在位置即目標位置，投影區域P3表示障礙物 3D模型在座標系平面中的投影，並在座標系平面中連接P 1 P 2，得到一條直線J；若直線J與投影區域P3沒有交點或者交點只有一個，則確定不存在有效遮擋區域；若直線J與投影區域P3的交點個數大於1，則直線J將投影分割為兩部分(如第4圖所示的S 1區域和S 2區域)，在投影區域P3(例如S 1或S 2區域中)中任意找一點P S ，過P S 作直線J的垂線，垂線與直線J的交點為 P J ，進而得到向量，計算最短距離的向量與向量的夾角 θ，若θ是銳角，則確定P S 點所在區域為有效遮擋區域(例如，第4圖中有效遮擋投影區域S 2)，或者，若θ不是銳角，則確定P S 點所在區域不是有效遮擋區域。
進一步地，該第一預設類型避障參數為虛擬斥力，該第二預設類型避障參數為虛擬引力，該根據計算的最短距離及有效遮擋區域的面積確定出第一預設類型避障參數，根據目標位置與機器人當前定位位置的距離確定出第二預設類型避障參數，根據該第一預設類型避障參數及該第二預設類型避障參數確定出機器人當前應運動的方向的步驟包括：對計算的最短距離和有效遮擋投影區域的面積，利用第一計算規則計算出作用在機器人上的一個虛擬斥力；對當前定位位置與目標點位置的距離，利用第二計算規則計算出作用在機器人上的一個虛擬引力；計算出該虛擬引力和虛擬斥力的合力方向，該合力方向即為機器人當前應運動的方向。
進一步地，該第一計算規則為： 設定機器人與障礙物的最短距離的向量為，有效遮擋區域 的面積用符號S表示，是障礙物對機器人的虛擬斥力，通過最短距離 向量和有效遮擋面積S計算，計算公式如下：
表示最短距離向量和有效遮擋區域的面積轉換為障 礙物對機器人虛擬排斥力的關係式。該關係式的實現可以有多種方法，在一種可選的實施例中，該關係式為：
其中，k r 、b r 表示預設的虛擬斥力係數，s 0表示預設的有效遮擋區域面積閾值，s 0>0；d 0表示預設的距離閾值，d 0>0；虛擬斥力方向與最短距離方向相同。
依據上述關係式，當機器人與障礙物距離較遠，超過設定的 距離閾值d 0時不進行避障，的大小為0；進入避障距離範圍內(最短距離 小於d 0)，當有效遮擋區域的面積s比較大，超過設定的值s 0時，>1，會 使變大，距離較遠時就可以進行避障，提前避障，以繞開較大的障礙物。
進一步地，該第二計算規則為： 是目標位置對機器人的虛擬引力，大小計算公式為： F t =k t ＊d t ，其中k t 表示預設的引力係數，d t 表示目標位置與機器人當前定位 位置的距離，方向朝向目標位置。
本揭示進一步提供一種機器人之避障控制系統。
請參閱第5圖，是本揭示避障控制系統10較佳實施例的運行環境示意圖。
在本實施例中，所述的避障控制系統10安裝並運行於機器人1中。該機器人1可包括，但不僅限於，儲存器11、處理器12及顯示器13。第1圖僅示出了具有組件11-13的機器人1，但是應理解的是，並不要求實施所有示出的組件，可以替代的實施更多或者更少的組件。
該儲存器11在一些實施例中可以是該機器人1的內部儲存單元，例如該機器人1的硬碟或記憶體。該儲存器11在另一些實施例中也可以是該機器人1的外部儲存設備，例如該機器人1上配備的插接式硬碟，智慧型儲存卡(Smart Media Card,SMC)，安全數位(Secure Digital,SD)卡，閃卡(Flash Card)等。進一步地，該儲存器11還可以既包括該機器人1的內部儲存單元也包括外部儲存設備。該儲存器11用於儲存安裝於該機器人1的應用軟體及各類資料，例如該避障控制系統10的程式代碼等。該儲存器11還可以用於暫時地儲存已經輸出或者將要輸出的資料。
該處理器12在一些實施例中可以是一中央處理器(Central Processing Unit,CPU)，微處理器或其他資料處理晶片，用於運行該儲存器11中儲存的程式代碼或處理資料，例如執行該避障控制系統10等。
該顯示器13在一些實施例中可以是LED顯示器、液晶顯示器、觸控式液晶顯示器以及OLED(Organic Light-Emitting Diode，有機發光二極體)觸控裝置等。該顯示器13用於顯示在該機器人1中處理的訊息以及用於顯示可視化的用戶介面，例如應用菜單介面、應用圖標介面等。該機器人1的部件11-13通過系統總線相互通訊。
請參閱第6圖，是本揭示避障控制系統10較佳實施例的功能模組圖。在本實施例中，所述的避障控制系統10可以被分割成一個或多個模組，該一個或者多個模組被儲存於該儲存器11中，並由一個或多個處理器(本實施例為該處理器12)所執行，以完成本揭示。例如，在第6圖中， 該避障控制系統10可以被分割成確定模組01、計算模組02、控制模組03。本揭示所稱的模組是指能夠完成特定功能的一系列電腦程式指令段，比程式更適合於描述該避障控制系統10在該機器人1中的執行過程。以下描述將具體介紹該確定模組01、計算模組02、控制模組03的功能。
該確定模組01，用於實時或者定時(例如，每隔2秒)獲取機器人當前的定位資料(例如在室內的位置、姿態等)，並根據當前定位資料及預先確定的移動區域內各個障礙物的位置資料，確定當前定位位置至目標位置路徑中是否有離當前定位位置的距離小於預設距離的障礙物。例如，可依靠機器人自身的感測器來定位並判斷與預先確定的移動區域內各個障礙物的距離，如可在機器人上安裝接近感測器(例如，超聲波、紅外、雷射等感測器)來判斷機器人當前定位位置至目標位置路徑中是否有離當前定位位置的距離小於預設距離的障礙物。
該計算模組02，用於若有離當前定位位置的距離小於預設距離的障礙物，則根據獲取的定位資料、預先確定的機器人的3D模型及預先確定的該障礙物的3D模型，計算出機器人與該障礙物的最短距離。
在檢測到機器人的當前定位位置與預先確定的移動區域內各個障礙物的距離之後，若判斷沒有障礙物離當前定位位置的距離小於預設距離，則繼續沿目標位置路徑移動並實時或者定時檢測機器人與移動區域內各個障礙物的距離。若判斷有離當前定位位置的距離小於預設距離的障礙物，則根據獲取的定位資料、預先確定的機器人的3D模型及預先確定的該障礙物的3D模型計算出機器人與該障礙物的最短距離，以利用該最短距離來判斷在三維空間中機器人沿目標位置路徑移動時是否會碰撞到該障礙物，從而實現不僅能在機器人的感測器所在高度平面檢測到障礙物，還能檢測到三維空間中潛在的障礙物，以在機器人安裝有感測器的方向和機 器人沒有安裝感測器的其他方向上均能檢測到三維空間中各個方向潛在的障礙物。其中，該預先確定的機器人的3D模型及移動區域內各個障礙物的3D模型可以預先儲存於機器人的儲存單元中，或者，可以由機器人通過無線通訊單元訪問物聯網系統伺服器獲取，在此不做限定。
該控制模組03，用於根據獲取的定位資料、計算的最短距離及該障礙物的3D模型，計算出當前機器人應運動的方向，根據計算出的運動方向控制機器人的運動姿態，以避開三維空間中各個方向潛在的障礙物，有效控制機器人在沿目標位置路徑移動時的避障。
本實施例通過機器人當前的定位資料檢測到有離當前定位位置的距離小於預設距離的障礙物時，根據機器人當前的定位資料、預先確定的機器人的3D模型及預先確定的該障礙物的3D模型，計算出機器人與該障礙物在三維空間的最短距離，並計算出當前機器人應運動的方向，以控制機器人的運動姿態。由於能通過機器人與障礙物在三維空間的最短距離來控制機器人的運動方向，從而實現檢測並避開機器人在三維空間中各個方向的障礙物，有效地控制機器人避障。
進一步地，上述計算模組02還用於：對預先確定的機器人的3D模型及該障礙物的3D模型進行預處理；對獲取的定位資料、預處理後的機器人3D模型資料及預處理後的障礙物3D模型資料，利用預先確定的距離計算規則計算出機器人與障礙物的最短距離。
例如，由於機器人和障礙物一般為非凸體，因此，可對機器人和障礙物的3D模型進行預處理如轉換為凸體，以便後續更加準確和快速地計算出最短距離。
進一步地，上述計算模組02還用於：針對機器人的每一個關 節，直接利用預先確定的算法(例如，QuickHull快速凸包算法)找出包圍各個關節的最小凸多面體，以將機器人非凸模型轉換為凸體模型。通過上述凸處理的機器人3D模型在後續計算最短距離向量時能有效提高計算速度和計算精度。
障礙物3D模型預處理的方式包括三種：第一種、構建非凸多面體的凸包圍盒使之轉換為凸體進行碰撞檢測；第二種、對非凸多面體進行凸分解，使非凸模型轉換為多個凸體進行碰撞檢測；第三種、對障礙物3D模型扇形均分(即扇形剖分)，然後對均分後的單個扇形進行凸分解，這種先扇形均分再凸分解的方式相對於前兩種不僅計算速度更快，而且計算精度更高。
進一步地，上述計算模組02還用於：建立待扇形均分的障礙物3D模型的球形包圍盒，找到球形包圍盒的球心；設定一經過該球心的初始扇形均分平面，將該初始扇形均分平面按預設的旋轉角繞該球心進行多次旋轉，以將球形包圍盒均分為n個扇形部分；該球形包圍盒的n個扇形部分作為障礙物3D模型的n個模型部分。
例如，在一種具體實施方式中，可包括如下步驟：X1、建立要扇形均分的障礙物3D模型M的球形包圍盒B，找到球形包圍盒B的球心O，然後在球心O處建立三維座標系Oxyz；X2、過球心O做一條與三維座標系Oxyz中z軸重合的直線L，則xoz平面即為初始扇形均分平面，設xoz平面為α 1，α 1將障礙物3D模型分為2部分；X3、將α 1繞直線L選擇一定角度β(β代表相鄰扇形偏角)得到另外一個新平面α 2，將新平面繼續旋轉β可以得到平面α 3，旋轉m-1次可以得到第m個平面α n； X4、設，則m個平面可以把球形包圍盒B均分為2m 部分，障礙物3D模型即被分為2m個模型部分。通過上述步驟可以完成對任意模型，包括非凸模型的簡單剖分，並通過哈希表進行管理剖分好的模型部分。
進一步地，上述計算模組02還用於：採用Delaunay三角剖分算法對障礙物3D模型進行表面三角剖分，產生三角面片(凸片)集合；並針對每一個三角面片構造與之對應的凸塊。例如，將厚度為零的三角面片在其平面法向量方向進行預設厚度的拉伸，變為凸塊。
進一步地，該預先確定的距離計算規則包括：根據機器人當前的定位資料(如室內位置、姿態等)及預先確定的篩選算法，對障礙物3D模型扇形均分後獲得的各個模型部分進行篩選，篩選出待進行距離計算的模型部分；對獲取的定位資料、篩選出的模型部分，利用預先確定的距離計算算法(例如，GJK算法)計算出機器人與篩選出的模型部分的最短距離，該最短距離即為機器人與障礙物3D模型的最短距離。
進一步地，如第2a圖、第2b圖所示，該預先確定的篩選算法包括：Y1、將障礙物3D模型扇形均分後獲得的n個模型部分分別作為障礙物的n個節點，建立key-value鍵值分別是相對於初始扇形均分平面(即xoz平面)的旋轉角即偏角和模型幾何訊息資料的哈希表，以進行模型節點管理； Y2、對扇形均分獲得的各個模型部分進行標號，從1開始進 行標號；均分的n個扇形模型部分，相鄰扇形偏角為，根據標號，建 立於初始標號為i的扇形模型部分的偏角映射關係，代表該偏角映射關係的哈希函數為：
其中，i為標號為i的扇形模型部分，Hash(i)代表標號為i的扇形模型部分與障礙物座標系的X軸正軸的偏角；Y3、建立機器人的運動學，根據建立的運動學計算出機器人各個關節的位姿，從建立的哈希表中查詢出機器人附近的障礙物扇形區域。如下圖2所示；當機器人在運動過程中，通過機器人運動學，運動學方程為：T i=A 0 A 1 A 2…A i-1 A i
其中，A k (k=1,2,...,i)為機器人關節座標系之間的齊次變換矩陣(可以通過機器人各關節的D-H參數確定)，A 0表示機器人當前位置矩陣(與機器人當前定位資料對應)，T i 為第i個關節相對於機器人座標系的位姿；通過T i 計算出機器人運動過程中各個關節局部座標系原點座標的實時更新值Q i (x,y,z)，進一步可以得到關節在障礙物座標系下的偏角α：α=f(Q i (x,y,z))
其中，Q i (x,y,z)表示機器人關節在機器人座標系下的座標；T r 表示機器人座標系變換到障礙物座標系的變換矩陣(為4*4的矩陣，機器人座標系和障礙物座標系已確定，該矩陣可以直接計算出來)，則機器人關節在障礙物座標系下的座標Q i (x t ,y t ,z t )為：Q i (x t ,y t ,z t )=T r Q i (x,y,z)
假定障礙物座標系Z軸正向朝上，遵循右手座標系，設關節 在障礙物座標系下的偏角為α，則
向求解三角函數即可得到關節在障礙物座標系下的偏角為α，獲取到偏角α之後，即可根據代表該偏角映射關係的哈希函數Hash(i)計算得到對應標號的扇形模型部分，並基於對應標號的模型部分篩選出待進行距離計算的模型部分。例如，計算得到的扇形模型部分的標號為k，則可選取標號在[k-M,k+N]範圍內的扇形模型部分進行最短距離計算。其中M、N為一預設數值，以選取標號為k的扇形模型部分附近的多個扇形模型部分作為待進行最短距離計算的模型部分。
如第3a圖所示，在一種具體實施方式中，機器人採用只有底盤的運動、沒有手臂等其他運動關節的機器人，機器人3D模型採用高為1500mm，運動底盤半徑為320mm的機器人3D模型，障礙物3D模型採用一個簡單立方體模型，尺寸為2200mm*2200mm*1000mm，在障礙物模型座標系下機器人當前的座標為(1800,-100)。
對障礙物模型進行預處理中，預處理主要是對障礙物模型進行扇形均分，如第3b圖所示，障礙物模型被扇形均分為32份，從X軸逆時針對扇形均分的模型部分進行編號，1,2，...，15,16，...，31,32；每一個模型塊的夾角為：360/32=11.25度，可以看出，編號1模型塊與X軸正向偏角11.25度，編號2模型塊與X軸正向偏角11.25*2=22.5度，編號為i的模型塊與X軸正向偏角：i*(360/32)。
在對模型部分的篩選過程中，因本實施例採用的機器人只有底盤的運動，沒有手臂等其他運動關節，所以底盤位姿代表機器人的整體位姿，當前機器人的位置為(1800，-100)(相對於障礙物座標系下座標)，可以計算出機器人與障礙物座標系的X軸正軸的偏角為354度；進而電腦器 人對應扇形模型部分的標號為354/11.25=31.5，向上取整得到32，所以待進行距離計算的對應扇形塊編號32，也就是說機器人離編號為32的障礙物塊最近。
接下來選取K=32附近的障礙物塊，採用GJK計算與機器人之間的最短距離及最短距離點；選取M=1，N=2，則得到障礙物塊範圍是[31,34]，編號超過32的需要做簡單轉換，33轉換為對應編號為1的障礙物塊，34轉換為對應編號為2的障礙物塊；如第3c圖所示，最終選取編號是31,32,1,2的障礙物塊進行最短距離計算。
在計算最短距離時，通過上述處理，已縮小障礙物塊的範圍(1,2,31,32)，直接採用GJK算法計算出機器人與障礙物間的最短距離點，如第3d圖所示，分別為障礙物上的點(x1,y1,z1)=(1100,-100,-235)，機器人上的點(x2,y2,z2)=(1477,-100,-235)；則機器人與障礙物之間的最短距離向量
進一步地，上述控制模組03還用於：根據計算的最短距離分析是否需要避障；如若計算的最短距離大於預設距離閾值，則確定不需要避障，或者，若計算的最短距離小於或者等於預設距離閾值，則確定需要避障。若確定需要避障，則根據獲取的定位資料、計算的最短距離及該障礙物的3D模型，計算出當前機器人應運動的方向，並根據計算出的運動方向控制機器人的運動姿態。
進一步地，上述控制模組03還用於：將機器人及該障礙物投影到同一座標系平面中；根據預先確定的投影分析規則及障礙物3D模型投影到該座標系平面的投影區域外輪廓各個點的座標，計算出該障礙物的投影相對於機器人當前位置及目標位置有效遮擋區域的面積； 根據計算的最短距離及有效遮擋區域的面積確定出第一預設類型避障參數(例如，虛擬斥力)，根據目標點位置與機器人當前定位位置的距離確定出第二預設類型避障參數(例如，虛擬引力)，根據第一預設類型避障參數及第二預設類型避障參數確定出機器人當前應運動的方向。
進一步地，如第4圖所示，該預先確定的投影分析規則為：設座標系平面的P 1位置點表示機器人所在位置即當前定位位置，P 2位置點表示目標點所在位置即目標位置，投影區域P3表示障礙物3D模型在座標系平面中的投影，並在座標系平面中連接P 1 P 2，得到一條直線J；若直線J與投影區域P3沒有交點或者交點只有一個，則確定不存在有效遮擋區域；若直線J與投影區域P3的交點個數大於1，則直線J將投影分割為兩部分(如第4圖所示的S 1區域和S 2區域)，在投影區域P3(例如S 1或S 2區域中)中任意找一點P S ，過P S 作直線J的垂線，垂線與直線J的交點為 P J ，進而得到向量，計算最短距離的向量與向量的夾角 θ，若θ是銳角，則確定P S 點所在區域為有效遮擋區域(例如，第4圖中有效遮擋投影區域S 2)，或者，若θ不是銳角，則確定P S 點所在區域不是有效遮擋區域。
進一步地，該第一預設類型避障參數為虛擬斥力，該第二預設類型避障參數為虛擬引力，上述控制模組03還用於：對計算的最短距離和有效遮擋投影區域的面積，利用第一計算規則計算出作用在機器人上的一個虛擬斥力；對當前定位位置與目標點位置的距離，利用第二計算規則計算出作用在機器人上的一個虛擬引力； 計算出該虛擬引力和虛擬斥力的合力方向，該合力方向即為機器人當前應運動的方向。
進一步地，該第一計算規則為： 設定機器人與障礙物的最短距離的向量為，有效遮擋區域 的面積用符號S表示，是障礙物對機器人的虛擬斥力，通過最短距離 向量和有效遮擋面積S計算，計算公式如下：
表示最短距離向量和有效遮擋區域的面積轉換為障 礙物對機器人虛擬排斥力的關係式。該關係式的實現可以有多種方法，在一種可選的實施例中，該關係式為：
其中，k r 、b r 表示預設的虛擬斥力係數，s 0表示預設的有效遮擋區域面積閾值，s 0>0；d 0表示預設的距離閾值，d 0>0；虛擬斥力方向與最短距離方向相同。
依據上述關係式，當機器人與障礙物距離較遠，超過設定的 距離閾值d 0時不進行避障，的大小為0；進入避障距離範圍內(最短距離 小於d 0)，當有效遮擋區域的面積s比較大，超過設定的值s 0時，>1，會 使變大，距離較遠時就可以進行避障，提前避障，以繞開較大的障礙物。
進一步地，該第二計算規則為： 是目標位置對機器人的虛擬引力，大小計算公式為： F t =k t ＊d t ，其中k t 表示預設的引力係數，d t 表示目標位置與機器人當前定位 位置的距離，方向朝向目標位置。
需要說明的是，在本文中，術語「包括」、「包含」或者其任何其他變體意在涵蓋非排他性的包含，從而使得包括一系列要素的過程、方法、物品或者裝置不僅包括那些要素，而且還包括沒有明確列出的其他要素，或者是還包括為這種過程、方法、物品或者裝置所固有的要素。在沒有更多限制的情况下，由語句「包括一個......」限定的要素，並不排除在包括該要素的過程、方法、物品或者裝置中還存在另外的相同要素。
通過以上的實施方式的描述，所屬領域具有通常知識者可以清楚地瞭解到上述實施例方法可借助軟體加必需的通用硬體平臺的方式來實現，當然也可以通過硬體來實現，但很多情况下前者是更佳的實施方式。基於這樣的理解，本揭示的技術方案本質上或者說對習知技術做出貢獻的部分可以以軟體產品的形式體現出來，該電腦軟體產品儲存在一個儲存介質(如ROM/RAM、磁碟、光碟)中，包括若干指令用以使得一台終端設備(可以是手機，電腦，伺服器，空調器，或者網路設備等)執行本揭示各個實施例所述的方法。
以上參照所附圖式說明了本揭示的較佳實施例，並非因此局限本揭示的權利範圍。上述本揭示實施例序號僅僅為了描述，不代表實施例的優劣。另外，雖然在流程圖中示出了邏輯順序，但是在某些情况下，可以以不同於此處的順序執行所示出或描述的步驟。
所屬領域具有通常知識者不脫離本揭示的範圍和實質，可以有多種變型方案實現本揭示，比如作為一個實施例的特徵可用於另一實施例而得到又一實施例。凡在運用本揭示的技術構思之內所作的任何修改、等同替換和改進，均應在本揭示的申請專利範圍之內。

Claims (9)

  1. 一種機器人之避障控制系統，包括：一確定模組，用於實時或者定時獲取一機器人之一當前定位資料，並根據該當前定位資料及一預先確定的移動區域內各個障礙物的位置資料，確定一當前定位位置至一目標位置路徑中是否有離該當前定位位置的距離小於一預設距離的障礙物；一計算模組，用於若有離該當前定位位置的距離小於該預設距離的該障礙物，則根據一獲取的定位資料、預先確定的該機器人的3D模型及預先確定的該障礙物的3D模型，計算出該機器人與該障礙物的一最短距離；一控制模組，用於根據該獲取的定位資料、計算的該最短距離及該障礙物的3D模型，計算出當前該機器人應運動的方向，根據計算出的運動方向控制該機器人的運動姿態，以避開該障礙物，其中該計算模組還用於：對該預先確定的該機器人的3D模型及該障礙物的3D模型進行預處理，針對該機器人的每一個關節，利用預先確定的算法找出包圍各個關節的最小凸多面體，以將該機器人的一非凸模型轉換為一凸體模型；對該障礙物3D模型扇形均分，並對均分後的單個扇形進行凸分解；對該獲取的定位資料、預處理後的機器人3D模型資料及預處理後的該障礙物3D模型資料，利用預先確定的距離計算規則計算出該機器人與該障礙物的該最短距離。
  2. 如申請專利範圍第1項所述的機器人之避障控制系統，其中該計算模組還用於：建立一待扇形均分的該障礙物3D模型的球形包圍盒，找到該球形包圍盒的球心；設定一經過該球心的一初始扇形均分平面，將該初始扇形均分平面按一預設的旋轉角繞該球心進行多次旋轉，以將該球形包圍盒均分為n個扇形部分；該球形包圍盒的該n個扇形部分作為該障礙物3D模型的n個模型部分。
  3. 如申請專利範圍第1項或第2項其中之一項所述的機器人之避障控制系統，其中該預先確定的距離計算規則包括：根據該機器人之該當前定位資料及預先確定的篩選算法，對該障礙物3D模型扇形均分後獲得的各個模型部分進行篩選，篩選出待進行距離計算的模型部分；對該獲取的定位資料、篩選出的模型部分，利用預先確定的距離計算法計算出該機器人與該篩選出的模型部分的最短距離，該最短距離為該機器人與該障礙物的最短距離；該預先確定的篩選算法包括：將該障礙物3D模型的n個模型部分分別作為該障礙物的n個節點，建立鍵值為相對於初始扇形均分平面的偏角的哈希表，以進行模型節點管理；對各個模型部分進行標號，根據標號，建立標號為i的模型部分的偏角映射關係，定義該偏角映射關係的哈希函數為：其中，Hash(i)代表標號為i的扇形模型部分與障礙物座標系的X軸正軸的偏角；建立該機器人的運動學方程，根據建立的該運動學方程計算出該機器人各個關節的位姿，該運動學方程為：T i=A 0 A 1 A 2…A i-1 A i其中，Ak(k=1,2,...,i)為機器人關節座標系之間的齊次變換矩陣，A0為機器人當前位置矩陣，Ti為第i個關節相對於機器人座標系的位姿；通過Ti計算出機器人運動過程中各個關節在該機器人座標系下的座標Q i (x,y,z)，並計算出該機器人座標系變換到該障礙物座標系的變換矩陣Tr，則機器人關節在該障礙物座標系下的座標Q i (x t ,y t ,z t )為：Q i (x t ,y t ,z t )=T r Q i (x,y,z)通過如下公式得到關節在該障礙物座標系下的偏角α：根據偏角α及哈希函數Hash(i)計算得到對應標號的模型部分，並基於該對應標號的模型部分篩選出待進行距離計算的模型部分。
  4. 如申請專利範圍第1項所述的機器人之避障控制系統，其中該控制模組還用於：將該機器人及該障礙物3D模型投影到同一座標系平面中；根據預先確定的投影分析規則及該障礙物3D模型投影到該座標系平面的投影區域外輪廓各個點的座標，計算出該障礙物3D模型的投影相對於該機器人的該當前定位位置及該目標位置有效遮擋區域的面積；根據計算的該最短距離及該有效遮擋區域的面積確定出一第一預設類型避障參數，根據該目標位置與該機器人之該當前定位位置的距離確定出一第二預設類型避障參數，根據該第一預設類型避障參數及該第二預設類型避障參數確定出該機器人當前應運動的方向。
  5. 如申請專利範圍第4項所述的機器人之避障控制系統，其中該預先確定的投影分析規則為：設定該座標系平面的P 1位置點為該機器人之該當前定位位置，P 2位置點為該目標位置，投影區域P3為該障礙物3D模型在該座標系平面中的投影，並在該座標系平面中連接P 1位置點和P 2位置點，得到直線J；若該直線J與該投影區域P3沒有交點或者交點只有一個，則確定不存在該有效遮擋區域；若該直線J與投該影區域P3的交點個數大於1，則該直線J將投影分割為兩部分；在該投影區域P3中任意找一點P S ，過P S 作該直線J的垂線，設定該垂線與該直線J的交點為P J ，得到向量，計算最短距離的向量與該向量的夾角θ；若判斷該夾角θ是銳角，則確定該P S 點所在區域是該有效遮擋區域；若判斷該夾角θ不是銳角，則確定該P S 點所在區域不是該有效遮擋區域。
  6. 如申請專利範圍第4項或第5項其中之一項所述的機器人之避障控制系統，其中該第一預設類型避障參數為虛擬斥力，該第二預設類型避障參數為虛擬引力，該控制模組還用於：根據計算的該最短距離和該有效遮擋區域的面積，利用第一計算規則計算出作用在該機器人上的該虛擬斥力；根據該當前定位位置與該目標位置的距離，利用第二計算規則計算出作用在該機器人上的該虛擬引力；計算出該虛擬引力和該虛擬斥力的合力方向作為該機器人當前應運動的方向。
  7. 如申請專利範圍第6項所述的機器人之避障控制系統，其中該第一計算規則為：設定該機器人與該障礙物的該最短距離的向量為，該有效遮擋區域的面積為S，該障礙物對該機器人的該虛擬斥力為，則計算公式如下：其中，k r 、b r 為預設的虛擬斥力係數，s 0為預設的有效遮擋區域面積閾值，d 0為預設的距離閾值；該虛擬斥力之方向與該最短距離之方向相同。
  8. 如申請專利範圍第6項所述的機器人之避障控制系統，其中該第二計算規則為：設定該目標位置對該機器人的該虛擬引力為，則計算公式為：F t =k t * d t ；其中，k t 為預設的引力係數，d t 為該目標位置與該機器人當前定位位置的距離，方向朝向該目標位置。
  9. 一種機器人避障方法，包括以下步驟：A1、實時或者定時獲取一機器人之一當前定位資料，並根據該當前定位資料及預先確定的移動區域內各個障礙物的位置資料，確定一當前定位位置至一目標位置路徑中是否有離該當前定位位置的距離小於一預設距離的障礙物；A2、若有離該當前定位位置的距離小於該預設距離的該障礙物，則根據獲取的定位資料、預先確定的該機器人的3D模型及預先確定的該障礙物的3D模型，計算出該機器人與該障礙物的一最短距離；A3、根據該獲取的定位資料、計算的該最短距離及該障礙物的3D模型，計算出當前該機器人應運動的方向，根據計算出的運動方向控制該機器人的運動姿態，以避開該障礙物，其中該步驟A2還包含：對該預先確定的該機器人的3D模型及該障礙物的3D模型進行預處理，針對該機器人的每一個關節，利用預先確定的算法找出包圍各個關節的最小凸多面體，以將該機器人的一非凸模型轉換為一凸體模型；對該障礙物3D模型扇形均分，並對均分後的單個扇形進行凸分解；對該獲取的定位資料、預處理後的機器人3D模型資料及預處理後的該障礙物3D模型資料，利用預先確定的距離計算規則計算出該機器人與該障礙物的該最短距離。
TW106135246A 2017-03-27 2017-10-13 機器人之避障控制系統及方法 TWI662388B (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
??201710186581.6 2017-03-27
CN201710186581.6A CN107688342B (zh) 2017-03-27 2017-03-27 机器人的避障控制系统及方法

Publications (2)

Publication Number Publication Date
TW201835703A TW201835703A (zh) 2018-10-01
TWI662388B true TWI662388B (zh) 2019-06-11

Family

ID=61152364

Family Applications (1)

Application Number Title Priority Date Filing Date
TW106135246A TWI662388B (zh) 2017-03-27 2017-10-13 機器人之避障控制系統及方法

Country Status (9)

Country Link
US (1) US11059174B2 (zh)
EP (1) EP3410246B1 (zh)
JP (1) JP6716178B2 (zh)
KR (1) KR102170928B1 (zh)
CN (1) CN107688342B (zh)
AU (1) AU2017404562B2 (zh)
SG (1) SG11201809892QA (zh)
TW (1) TWI662388B (zh)
WO (1) WO2018176668A1 (zh)

Families Citing this family (44)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108717302B (zh) * 2018-05-14 2021-06-25 平安科技（深圳）有限公司 机器人跟随人物方法、装置及存储介质、机器人
CN109461185B (zh) * 2018-09-10 2021-08-17 西北工业大学 一种适用于复杂场景的机器人目标主动避障方法
CN109465835A (zh) * 2018-09-25 2019-03-15 华中科技大学 一种动态环境下双臂服务机器人作业的事前安全预测方法
TWI721324B (zh) * 2018-10-10 2021-03-11 鴻海精密工業股份有限公司 電子裝置及立體物體的判斷方法
CN109284574B (zh) * 2018-10-25 2022-12-09 西安科技大学 一种串联桁架结构体系非概率可靠性分析方法
KR102190101B1 (ko) * 2019-03-08 2020-12-11 (주)아이로텍 로봇의 장애물 충돌 회피를 위한 경로 안내툴 표시 방법 및 이를 이용한 충돌 회피 시뮬레이션 시스템
CN110442126A (zh) * 2019-07-15 2019-11-12 北京三快在线科技有限公司 一种移动机器人及其避障方法
CN110390829A (zh) * 2019-07-30 2019-10-29 北京百度网讯科技有限公司 交通信号灯识别的方法及装置
CN112445209A (zh) * 2019-08-15 2021-03-05 纳恩博（北京）科技有限公司 机器人的控制方法和机器人、存储介质及电子装置
CN111046743B (zh) * 2019-11-21 2023-05-05 新奇点智能科技集团有限公司 一种障碍物信息标注方法、装置、电子设备和存储介质
CN111207750A (zh) * 2019-12-31 2020-05-29 合肥赛为智能有限公司 一种机器人动态路径规划方法
CN111168675B (zh) * 2020-01-08 2021-09-03 北京航空航天大学 一种家用服务机器人的机械臂动态避障运动规划方法
CN111449666B (zh) * 2020-03-09 2023-07-04 北京东软医疗设备有限公司 距离监测方法、装置、血管机、电子设备及存储介质
JP2021146905A (ja) * 2020-03-19 2021-09-27 本田技研工業株式会社 制御装置、制御方法およびプログラム
CN111571582B (zh) * 2020-04-02 2023-02-28 上海钧控机器人有限公司 一种艾灸机器人人机安全监控系统及监控方法
CN111427355B (zh) * 2020-04-13 2023-05-02 京东科技信息技术有限公司 障碍物数据处理方法、装置、设备及存储介质
JP7447670B2 (ja) * 2020-05-15 2024-03-12 トヨタ自動車株式会社 自律移動装置制御システム、その制御方法及びその制御プログラム
US11292132B2 (en) * 2020-05-26 2022-04-05 Edda Technology, Inc. Robot path planning method with static and dynamic collision avoidance in an uncertain environment
CN111857126A (zh) * 2020-05-29 2020-10-30 深圳市银星智能科技股份有限公司 一种机器人避障方法、机器人以及存储介质
CN111958590B (zh) * 2020-07-20 2021-09-28 佛山科学技术学院 一种复杂三维环境中机械臂防碰撞方法及系统
CN112148013A (zh) * 2020-09-25 2020-12-29 深圳优地科技有限公司 机器人避障方法、机器人及存储介质
CN112415532B (zh) * 2020-11-30 2022-10-21 上海炬佑智能科技有限公司 灰尘检测方法、距离检测装置以及电子设备
CN112704878B (zh) * 2020-12-31 2023-04-18 深圳市乙亥互娱信息科技有限公司 集群游戏中的单位位置调整方法、系统、设备及存储介质
CN112991527B (zh) * 2021-02-08 2022-04-19 追觅创新科技(苏州)有限公司 目标对象的躲避方法及装置、存储介质、电子装置
CN113119109A (zh) * 2021-03-16 2021-07-16 上海交通大学 基于伪距离函数的工业机器人路径规划方法和系统
CN113110594B (zh) * 2021-05-08 2022-05-17 北京三快在线科技有限公司 控制无人机避障的方法、装置、存储介质及无人机
CN113282984A (zh) * 2021-05-21 2021-08-20 长安大学 一种公共场所人员应急疏散模拟方法
CN113459090A (zh) * 2021-06-15 2021-10-01 中国农业大学 码垛机器人的智能避障方法、电子设备及介质
US11753045B2 (en) * 2021-06-22 2023-09-12 Waymo Llc Modeling positional uncertainty of moving objects using precomputed polygons
CN113601497B (zh) * 2021-07-09 2024-02-06 广东博智林机器人有限公司 一种方法、装置、机器人及存储介质
CN113752265B (zh) * 2021-10-13 2024-01-05 国网山西省电力公司超高压变电分公司 一种机械臂避障路径规划方法、系统及装置
KR102563074B1 (ko) * 2021-10-20 2023-08-02 금오공과대학교 산학협력단 차동 구동형 이동로봇의 운동역학을 고려한 장애물 회피 및 경로 추종방법
CN114035569B (zh) * 2021-11-09 2023-06-27 中国民航大学 一种航站楼载人机器人路径拓展通行方法
CN114161047B (zh) * 2021-12-23 2022-11-18 南京衍构科技有限公司 一种用于增材制造的焊枪头自动避障方法
CN114355914B (zh) * 2021-12-27 2022-07-01 盐城工学院 用于无人船的自主巡航系统及控制方法
CN114859904B (zh) * 2022-04-24 2023-04-07 汕头大学 一种基于e-grn的集群围捕方法、执行装置和系统
CN114859914B (zh) * 2022-05-09 2023-04-07 广东利元亨智能装备股份有限公司 障碍物检测方法、装置、设备及存储介质
CN115202350B (zh) * 2022-07-15 2023-06-09 盐城工学院 一种agv小车的自动运输系统
CN115437388B (zh) * 2022-11-09 2023-01-24 成都朴为科技有限公司 一种全向移动机器人脱困方法和装置
CN115507857B (zh) * 2022-11-23 2023-03-14 常州唯实智能物联创新中心有限公司 高效机器人运动路径规划方法及系统
CN116755562B (zh) * 2023-07-04 2024-04-05 深圳市仙瞬科技有限公司 一种避障方法、装置、介质及ar/vr设备
CN117093005B (zh) * 2023-10-16 2024-01-30 华东交通大学 一种智能汽车自主避障方法
CN117207202B (zh) * 2023-11-09 2024-04-02 国网山东省电力公司东营供电公司 带电作业机器人防碰撞约束控制方法、系统、终端及介质
CN117406758B (zh) * 2023-12-14 2024-03-12 双擎科技(杭州)有限公司 一种机器人避障装置及机器人智能防碰系统

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
TW201540281A (zh) * 2014-04-30 2015-11-01 Univ Nat Chiao Tung 機器人的行動輔助系統

Family Cites Families (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7164117B2 (en) * 1992-05-05 2007-01-16 Automotive Technologies International, Inc. Vehicular restraint system control system and method using multiple optical imagers
JP3425760B2 (ja) 1999-01-07 2003-07-14 富士通株式会社 干渉チェック装置
US7734387B1 (en) * 2006-03-31 2010-06-08 Rockwell Collins, Inc. Motion planner for unmanned ground vehicles traversing at high speeds in partially known environments
DE602006003435D1 (de) 2006-09-14 2008-12-11 Abb Research Ltd Verfahren und Vorrichtung zur Vermeidung von Kollisionen zwischen einem Industrieroboter und einem Objekt
US8840838B2 (en) * 2011-09-25 2014-09-23 Theranos, Inc. Centrifuge configurations
JP2014018912A (ja) 2012-07-18 2014-02-03 Seiko Epson Corp ロボット制御装置、ロボット制御方法およびロボット制御プログラムならびにロボットシステム
JP2014056506A (ja) 2012-09-13 2014-03-27 Toyota Central R&D Labs Inc 障害物検出装置及びそれを備えた移動体
US9102055B1 (en) 2013-03-15 2015-08-11 Industrial Perception, Inc. Detection and reconstruction of an environment to facilitate robotic interaction with the environment
US20150202770A1 (en) * 2014-01-17 2015-07-23 Anthony Patron Sidewalk messaging of an autonomous robot
US9454151B2 (en) * 2014-05-20 2016-09-27 Verizon Patent And Licensing Inc. User interfaces for selecting unmanned aerial vehicles and mission plans for unmanned aerial vehicles
KR101664575B1 (ko) 2014-11-07 2016-10-10 재단법인대구경북과학기술원 모바일 로봇의 장애물 회피 시스템 및 방법
US10586464B2 (en) * 2015-07-29 2020-03-10 Warren F. LeBlanc Unmanned aerial vehicles
CN106227218A (zh) * 2016-09-27 2016-12-14 深圳乐行天下科技有限公司 一种智能移动设备的导航避障方法及装置
CN106406312B (zh) 2016-10-14 2017-12-26 平安科技（深圳）有限公司 导览机器人及其移动区域标定方法

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
TW201540281A (zh) * 2014-04-30 2015-11-01 Univ Nat Chiao Tung 機器人的行動輔助系統

Also Published As

Publication number Publication date
AU2017404562B2 (en) 2020-01-30
CN107688342A (zh) 2018-02-13
EP3410246A4 (en) 2019-11-06
CN107688342B (zh) 2019-05-10
SG11201809892QA (en) 2018-12-28
US11059174B2 (en) 2021-07-13
JP2019516146A (ja) 2019-06-13
AU2017404562A1 (en) 2018-10-11
KR102170928B1 (ko) 2020-10-29
KR20190022435A (ko) 2019-03-06
US20210078173A1 (en) 2021-03-18
EP3410246B1 (en) 2021-06-23
WO2018176668A1 (zh) 2018-10-04
EP3410246A1 (en) 2018-12-05
TW201835703A (zh) 2018-10-01
JP6716178B2 (ja) 2020-07-01

Similar Documents

Publication Publication Date Title
TWI662388B (zh) 機器人之避障控制系統及方法
AU2018271237B2 (en) Method for building a map of probability of one of absence and presence of obstacles for an autonomous robot
JP6105092B2 (ja) 光学式文字認識を用いて拡張現実を提供する方法と装置
Wang et al. Robot manipulator self-identification for surrounding obstacle detection
EP3401815A1 (en) Determining an architectural layout
TWI677413B (zh) 用於機械手臂系統之校正方法及裝置
Krajník et al. External localization system for mobile robotics
CN109947118B (zh) 一种使用gpu加速的代价地图快速更新方法
US11704881B2 (en) Computer systems and methods for navigating building information models in an augmented environment
JP2021531524A (ja) ３次元仮想空間モデルを利用したユーザポーズ推定方法および装置
Kabutan et al. Motion planning by T‐RRT with potential function for vertical articulated robots
WO2019183928A1 (zh) 一种室内机器人定位的方法及机器人
US20220237875A1 (en) Methods and apparatus for adaptive augmented reality anchor generation
Han et al. Optimization‐based humanoid robot navigation using monocular camera within indoor environment
US20210107156A1 (en) Robot and method of controlling same
Radmard et al. Resolving occlusion in active visual target search of high-dimensional robotic systems
Lee et al. Robust multithreaded object tracker through occlusions for spatial augmented reality
Wang et al. Combination of the ICP and the PSO for 3D-SLAM
KR20190075356A (ko) 객체를 식별하기 위한 장치 및 방법
WO2022259600A1 (ja) 情報処理装置、情報処理システム、および情報処理方法、並びにプログラム
WO2022227939A1 (zh) 地面障碍物检测方法以及使用该方法的移动机器
US20220129006A1 (en) Spatial blind spot monitoring systems and related methods of use
Gautam et al. Experience based localization in wide open indoor environments
Petrou et al. RoboCup Rescue 2015-Robot League Team PANDORA (Greece)
CN116597001A (zh) 室内顶部边界位置检测方法、装置、机器人及存储介质