Transcript 下載/瀏覽

Collision-free Path Planning of Dual-arm
Robots Based on Improved Ant Colony
Algorithm
2009 Chinese Control and Decision Conference (CCDC 2009)
WANG Jian-hui, GUO Min, LI Lin, SUN Shen-gqi, GU Shu-sheng
指導教授:邱俊賢
學生: 鄭世文
Outline







Abstract
Introduction
System Description
The Improvement of Ant Colony Algorithm
Based on C Space
Simulation Results
Conclusion
References
Abstract

Using the three-dimensional C space, we combined the
method of path planning based on sensor information and
the algorithm of the intelligent ant colony optimization,
researched on the most effective way on Dual-arm robots
collision-free path planning.
使用三維C空間，我們結合了感測器上資訊的路徑規劃方法和智
能蟻群優化的演算法，去研究雙臂機器人無碰撞路徑規劃的最有
效方法。

Based on the traditional algorithm’s limitation on space
path searching and the joint movement’s characteristics of
Dual-arm robots in the C space, we enhanced the search
strategy for ant colony algorithm, discussed the influence of
the size of compartmentalize grid on path planning’s speed
and accuracy; improved the techniques of local pheromone
update, provided the local pheromone update conditions
when shortest path, life of electrical and energy
consumption are all considered.
在C空間中，基於傳統演算法的限制，在空間路徑搜索和雙臂機器
人關節移動的特點，對蟻群演算法，我們加強了搜尋的策略，而
討論了區域化網格大小的影響於路徑規劃的速度和準確性，改善
局部費洛蒙更新的技術，而當最短路徑、電氣壽命和能源消耗在
被考慮之下，提供了局部費洛蒙更新的條件。

For the circumstances that ants’ feasible region maybe
empty in the process of path searching, we introduced the
ants back strategy in order to improve the algorithm’s
adaptability substantially.
對於在某情況下，在路徑搜索過程中，螞蟻的行徑區域可能是空
的，我們引入螞蟻回覆策略來改善演算法的適應能力。

The simulations show the searching performance and
searching speed of the improved algorithm proposed by
this article are better than the traditional one.
在模擬顯示，藉由本文所提出的改進演算法其搜尋性能和搜尋速
度優於傳統的演算法。
Introduction

With the development of modern industry and the progress of
scientific technological, single-manipulator has been unable to
complete lot of missions. In order to adapt to the demands of
mission complexity and ever-increasing intelligent, the
advantages of double mechanical arm in this area gradually
appear. They are not just the simple combination of two singlemechanical arms. Apart from their goal of control
implementation, their coordination exercise control, and
adaptability to the external environment have become a key
portfolio too.
隨著現代工業的發展和科學技術的進步，單手臂已經無法完成很多任
務。為了適應任務需要的複雜性和不斷增加的智能，機械雙手臂的優
勢也在這領域逐漸顯現。他們不只是簡單的組合兩個單一機械手臂。
除了他們實施控制的目標外，協調運動控制，適應外部環境已經成為
一項重要的因素。

The overseas dual-arm robot research began in the 20th century,
the early 90s. The study mainly concentrated in the movement
trajectory planning of the arms (including collision avoidance),
dual-arm coordination control algorithm and the control of
operation power or torque and other aspects. The study of
movement trajectory planning is based on multi-robot to start
work without a collision in the same environment. The study is
usually conducted in two parts as the path planning and
trajectory planning.
國外的雙臂機器人是在20世紀90年代初開始研究的。這項研究主要集
中在手臂的移動軌跡規劃（包括碰撞、躲避），雙臂協調控制演算法
及行動力的控制或扭矩和其他方面。這項研究的移動軌跡規劃是基於
多機器人在一個相同的環境中開始工作、沒有碰撞。而通常是分為路
徑規劃和軌跡規劃兩部分。

Some scholars on the issue of the robot arms control of sport
down a specific path make more in-depth studies. They set up
the optimal trajectory planning algorithm to consider the
robot dynamic characteristics using the robot dynamic
equation. In addition, they use the computer simulation to
find the best route to exercise every step, so as to solve the
collision avoidance problem.
一些學者在機器人手臂運動控制下的特定路徑的議題進行更深入的
研究。 他們使用機器人動態方程式，建立最優軌跡規劃演算法去考
量機器人動態特性。 此外，他們使用電腦模擬出最佳線路去實行的
每一步驟，從而解決碰撞躲避問題。

Robot path planning methods generally can be divided into
three parts [3]:

(1) The path planning based on environment model – the
treatment to the robot path planning of completely known
environment;

(2) The path planning based on sensor information – the
implementation of robot movement path planning in a
dynamic unknown environment;
機器人路徑規劃方法一般可分為三部分[3]：
(1) 基於環境模型的路徑規劃 - 完全熟悉環境的機器人路徑規劃;
(2) 基於感測器訊息的路徑規劃 - 在動態未知環境中執行機器人移
動路徑規劃;

(3) The path planning based on behavior - the navigation
problem is decomposed into many relatively antagonism
navigation units to complete the movement mission
through the coordination work.
(3) 基於行為上的路徑規劃- 導航問題被分解成許多相對對立的導
航單元，透過協調工作去完成動態任務。

Ant colony algorithm is applied to the path planning robot
based on robot configuration space (that is, C space), and
it is different at the path planning based on general ant
colony algorithm on planar grid, but place ants on space
grid to do excellent path search [1,2,4]. Therefore, we
have combined the path planning method based on sensor
information with the intelligent ant colony optimization
algorithm to study an effective way of the path planning in
this article.
蟻群演算法應用於機器人的路徑規劃基於機器人配置空間（即C
空間 ），而在路徑規劃，它的不同是基於在平面網格上的一般蟻
群演算法，但空間網格的螞蟻做出優越的路徑搜索[1,2,4]。 因此，
在本文，我們結合路徑規劃方法基於感測器訊息和智能蟻群演算
法去探討一個有效路徑規劃的方法。
System Description

The system has chosen AS-MRobot three freedom degrees
of dual-arm robots produced by Shanghai Grandar Robotics
Co. Ltd. Schematic diagram of its structure as shown in
Figure 2.1 (because the third freedom degree of mechanical
arm can only change the crawling direction of the robot’s
claw-hand, and there’s no impact to the end location, it’s
omitted in the diagram).
該系統選擇了三個自由度的雙臂機器人產品AS-MRobot，結構示意
圖如圖 2.1（因為機械手臂的第三個自由角度只能改變機器人爪手
的抓取方向，而也沒有影響到最後的位置，以至於被省略於圖中)。
Fig. 2.1 Structure diagram of AS-MRobot robot

According to Figure 2.1 we can obtain the kinematics
equations of left mechanical arm as follows.
根據圖 2.1，我們可以得到左邊機械手臂的運動方程式。

And the kinematics equations of right mechanical arm
as follows.
而我們也可以得到右邊機械手臂的運動方程式

The goal in this system is to achieve not only collision-free
movement of dual-arm robots but also optimal path, when
the two robot arms moving from their respective starting
points to different target point locations. To simplify the
problem, in the path planning process, we assume robot’s
left arm as the main arm, robot’s right as the subordinate arm.
The path of the main arm is planned in advance, and they do
an approximate straight line movement between its initial
position and target position.
在本系統，當兩個機械手臂正從各自的出發點到不同目標點的位置，
其目的不僅實現雙臂機器人的無碰撞移動還有最優路徑。 為了簡化
問題，在路徑規劃過程中，我們假設機器人的左手臂為主要手臂，
機器人的右手臂是次要手臂。主要手臂的路徑是被預先規劃的，並
且他們介於初始位置和目標位置做近似的直線移動。

The subordinate arm always regards the main arm as a
dynamic obstacle in its movement process, and accords
the main arm’s movement laws to planning its own realtime path. It can ensure both of arriving at an accurate
location of its own goals and having collision-free with the
main arm. Therefore, this article is focused on the
collision-free optimal path planning of the robot’s
subordinate arm.
在移動過程中，次要的手臂被當成主要手臂的動態障礙物，和協
定主要手臂的移動規律來規劃自己的即時路徑。 這可以確保兩個
到達精確的目標位置，並和主要手臂無碰撞。因此，本文的重點
是機器人次要手臂的無碰撞最佳路徑規劃。
The Improvement of Ant Colony
Algorithm Based on C Space

Problem Description and Improvement of Search
Strategy
問題描述和搜索策略的改進

Grid Segmentation
網格分割

The Further Optimization Method of Multi-feasible
Path
多重可行路徑的進一步優化方法

The Continue Optimization Method of No Feasible
Path
沒有可行路徑的持續優化方法
Problem Description and Improvement of
Search Strategy

It is important to determine the planning space before using
ant colony algorithm to make path planning. To take into
account the two main factors as mapping the main arm’s
location information every moment to the subordinate arm
planning space and the mechanical arm’s joint angle of
robot which are the direct control volumes of arm’s
movement; we select the subordinate arm configuration
space as the path searching space of ant colony algorithm.
在用蟻群演算法進行路徑規劃前，先確定規劃空間是重要的。 而
要考慮到兩個主要因素為，每一時間點測繪出主要手臂的位置資訊
去對次要手臂的規劃空間和機械手臂的關節角度，而手臂的移動量
是直接控制的，我們選擇次要手臂配置空間來做為蟻群演算法的路
徑搜索空間。

The searching space is the three-dimensional C space
constructed by two of the joint movement angle axis of
mechanical arm and a time-axis. Therefore, ants searching
range is expanded from two-dimensional space to threedimensional space [5-7].
搜索空間是藉由機械手臂的兩個關節移動角度軸和時間軸所構成
的三維C空間。 因此，螞蟻的搜索範圍廣大，從二維空間到三維
空間 [5-7]。

In this paper, ant colony algorithm is applied to the
subordinate arm C space. In this system, the variable in axis
is the movement angle of mechanical arm rather than the
distance. Therefore, every step of the ant path is the change
quantity of mechanical arm joint angle. The sum of ant
searching path in once process of searching food is the total
value of mechanical arm’s joint angle change amount. The
optimal path which ant colony algorithm finally searches
takes the smallest value of the sum of the two mechanical
arm’s joint angle change amount as an optimizing basis.
在本文中，蟻群演算法被應用在次要手臂C空間裡。 系統中，在軸
上的變化量是機器手臂的移動角度，而不是距離。因此，螞蟻路徑
的每一步是機械手臂關節角度的變化量。在一次搜尋食物的過程中，
螞蟻搜尋路徑的總和是機械手臂的關節角度變化量的總值。最佳路
徑的蟻群演算法最後搜索到兩個機械手臂的關節角度變化量總和的
最小值作為優化的基礎。

Because of the change of ant searching scope and object, the
search strategy is accordingly improved in this article. In
basic ant colony algorithm when the ant steps to a node, the
current node will be added to the taboo table tabuk , in order
to avoid ant choosing the duplicate path. But as for the
system, ant takes incremental of time layer as the next search
range. The obstruction regional of ant searching space is
changed every moment, that is, ant is likely to be
encountered the situation that there isn’t other optional node
apart from the current node in the next moment.
由於螞蟻搜索範圍和物件的改變，而在文章中的搜索策略也被改善
了。在基本蟻群演算法中，當螞蟻步驟到一個節點，而目前的節點
將被增加到禁止表 tabuk 中，是為了避免螞蟻選擇重複的路徑。但
對於該系統，螞蟻需要增量的時間層到下一個搜索範圍。螞蟻搜尋
空間的障礙區域是隨著時間改變，則螞蟻很可能會遇到的情況在於，
再下一個時間點，除了目前的節點，沒有其他可選節點。

Therefore, in order to improve the adaptability of ant
algorithm, we set the current node and its adjacent 8
nodes which the whole node number is 9 as the next
step’s searching scope. The improved algorithm as
follows: after ant steps to a node, it will add previous
node to the taboo table tabuk , at the same time delete
current node from the taboo table tabuk if it has existed.
因此，為了改善蟻群演算法的適應性，我們設置了目前節點及
其鄰近 8個節點，整個節點的數目是9，作為下一步的搜索範圍。
而改進後的演算法如下：螞蟻的步驟到1節點後，將以前的節點
添加到禁止表
中，如果它已經存在，則從禁止表
中k
tabu
tabu
k
同時刪除目前節點。
Grid Segmentation

The whole subordinate arm time-varying C space is divided
into l ×m×n C square spaces. The values of l , m and n are
closely related to collision-free path planning of dual-arm
robot. The excessive mesh density will increase the number
of square spaces in the path searching space. It will not only
increase computation but also extent the frequently brakes’
possibility of mechanical arm’s joints caused by the planned
optimal path.
整個次要手臂時變 C 空間被分為 l ×m ×n 的方形空間。 l，m和n的
數值和雙臂機器人的無碰撞路徑規劃是有密切關係的。在路徑搜索
空間中，過多的網格密度會增加數個方形空間。它將不僅增加計算
而且機械手臂關節的頻率制動器的可能性程度會造成最優路徑規劃。

Grid over-thinning, in the process of setting up obstacle
space of subordinate arm, will cause the C space of
obstacles edge invisible expanding, and even will take up
certain number free-movement space of subordinate arm,
which will undoubtedly affect the result of mechanical
arm optimal path planning.
網格的細小，在次要手臂的障礙物空間設置的過程中，也導致了
障礙物邊緣的無形擴大，甚至將會佔用次要手臂的數個自由移
動空間，這無疑的影響到機械手臂最優路徑規劃的結果。

From the analysis above we can see that the grid
number l , m and n of the subordinate arm timevarying C space along every axis are decided by the
following two aspects:
從以上分析可以看出，次要手臂時變C空間的網格數 l，m 和
n，每個軸是由以下兩個方面被決定的：


(1) The effective move angle of joint
Assumed that the main arm begins moving since t = 0 , and
stops moving at time te , you can get the each time layer
width of t axis direction is  t  te / l . Since the subordinate
arm steps from one gird to the adjacent grid of next time
layer, the limited angular velocity of each joint are  R1 and R 2 ,
so the width of each C space grid along the axis  R1 and  R 2
as follows:
 R1   R1   t ,  R 2   R 2   t


(1) 有效的關節移動角度
假設的主要手臂從 t = 0 時開始移動，並在 te 時停止移動，可以得到 t
軸方向的每次時間層寬度為  t  te / l 。次要手臂步驟從一個網格到
下一時間層的鄰近網格，每個關節的有限角速度是  R1 和  R 2 ，所
以每個 C 空間網格的寬度沿軸線  R1 和  R 2 如下。

Thus the relationship of girds l、m and n along axis  R1、 R 2
and t in subordinate arm time-varying C space as follows:




m   R1max   R1min  /  R1   R1max   R1min  /  R1   t

  R1max   R1min   l /  R1  te

n   R 2max   R 2min  /  R 2   R 2max   R 2min  /  R 2   t

  R1max   R1min   l /  R1  te

因此，在次要手臂時變C空間中，網格l、m 和 n 沿軸線  R1 ， R 2
和 t 的關係




(2) The precision of joint movement angle
In this experiment system, the control volume of two
mechanical arm is the absolute value of the angle changes,
rather than relative value, that is to say when the mechanical
arm continuous move between the discrete points of the
planning path, it will not result in a cumulative error, and
the error only depends on the movement precision inherent
in the hardware device.
(2) 關節移動角度的精確
在這個實驗系統中，兩個機械手臂的控制量是角度變化的絕對值，
而不是相對值，則當機械手臂介於路徑規劃的離散點之間連續的移
動，則它將不會導致一個累積的誤差，而在硬體設備，誤差僅取決
於移動精度。

In the system the movement angle precision of each joint
is 0.02 . It can be completely compensated by robot’s
claw-hand whose maximum tension is 68 mm. Therefore,
the values of m and n will have no effect to precision.
在該系統中，每個關節的移動角度精度為 0.02 。它可以完全補償
機器人爪手的最大張力為 68毫米。因此，m 和n 的值對精度沒有任
何影響。
The Further Optimization Method of Multifeasible Path

Fig. 3.1 The movement path from A to B in time
period (t − 4,t)


Aiming at above problem, we do following improvements for
original ant colony algorithm: When the ant k detects that the
changes of  R1 and  R 2 at time n − 2、n −1、n are consistent
with the following two situations:
(1)  R1 and  R 2 are both continuously changed in three
moments above.
針對上述問題，我們對原始蟻群演算法做了以下改進： 當螞蟻 K 在
n - 2，n - 1，n次裡所檢測到 R1 和  R 2 的變化符合以下兩種情況：
(1)  R1 和  R 2 連續的改變三個以上的時間點。

(2) There is only one continuously joint angle change in
three moments, and the other always remains unchanged,
then the local pheromone update as follows:
(2)在三個時間裡，只有一個關節角度不斷的變化，而其他始終
保持不變，那麼局部費洛蒙更新如下
 tabu  n2tabu  n   n  1  1    tabu  n2tabu  n 
k
k
k
2
k
tabuk  n  2 tabuk  n 
k
The Continue Optimization Method of No
Feasible Path

The feasible region when ant is in the detection path may
be empty, that is, ant has no path to move.
當螞蟻在偵查路徑時可行路徑可能是空的，那麼螞蟻沒有路徑去
移動。
Fig. 3.2(a) The position of
ant k at time n
(b) The optional position
at time n +1

It is easier to see from the figure that at time n +1 optional
path grids belongs to obstacle space, and it has no followup grids to choose. That is, the ant k falling into the trap, in
the "death" state, which will enable the entire algorithm at a
standstill state at a certain extent and influence the
algorithm adaptation to the complex environment.
從圖上較容易看出，在n +1 次的可選路徑網格屬於障礙空間，而
它沒有後續網格可選擇。 也就是說，螞蟻 K 掉進了陷阱，在“死
亡”狀態，這將使整個算法處於停滯狀態，並在一定程度上影響
了演算法去適應複雜的環境。

Aiming at these questions, we adopt the back strategy
when the ant falling into trap in the system, so that each
ant can be able to effectively complete the path searching.
針對這些問題，在系統裡，當螞蟻落入了陷阱時，我們採取的回
覆策略，以至於使每一個螞蟻可以能夠有效地完成路徑搜索。

The improved algorithm as follows: When the ant k
searches an empty feasible region at time n +1, that is, no
follow-up feasible path to choose, we make the judge that
ant is falling into trap. At this point, the ant k back to the
location of time n −1 , and we do the taboo table update and
local pheromone update as follows:
tabuk  n  1  tabuk  n  1
 tabu  n1tabu  n   n  1  1    tabu  n1tabu  n   n 
k
k
k
k
k
 tabu
k  n 1tabuk  n 
改進的演算法如下：當螞蟻 K 在n +1次時搜索到一個空的可行區
域，而沒有後續可行的路徑選擇，我們作出判斷，螞蟻是落入陷
阱。此時，螞蟻 K 回到 n -1 次的位置，並且我們對禁止表做更新
和局部費洛蒙更新如下


Then it re-selects the next path in feasible region, and ant k
at this time can escape the trap.
Through the introduction of ant back strategy, the algorithm
in the implementation process ensure that each ant can step
from the initial position to the destination position safely,
which greatly enhanced the algorithm adaptability.
然後在可行的區域再選擇下一個路徑，而在幾次時 螞蟻 K可以逃脫
這個陷阱。
經過螞蟻回覆策略的介紹，在實施過程中該演算法確保每隻螞蟻的
每一步可以從初始位置安全的到目標位置，大大提高了演算法的適
應性。
Simulation Results


We carry out a simulation experiment for the path searching
capabilities of improved ant colony algorithm on the
programming platform VC++ in this system.
Experiment is divided into 3 groups, corresponding different
objectives of the positions. Each experiment is the average
result after running 10 times. Among each experiment we set
the ant number m =10 , cycle number N =15 , parameters ρ =
0.1 ,λ = 0.1 .
在系統中，我們用VC + +編譯平台對改進蟻群演算法的路徑搜索功
能進行模擬實驗。
實驗分為 3組，對應不同位置的目標。跑完10次後每個實驗的平均
結果。在每個實驗中，我們設置了螞蟻數 m = 10，週期數 N = 15，
參數ρ= 0.1，λ= 0.1。

Fig. 4.1 The path
planning results of
different target
points of the main
and subordinate arm

The initial and target position’s corresponding coordinate
values of main, subordinate arm at three positions in
subordinate arm C space is shown in Table 4.1.

在次要手臂C空間中，主要和次要手臂在三個位置上的原始和目標位
置的相對應座標值，見表 4.1。
Table4.1.
The Comparison of
Main, Subordinate
Arm Position in
Subordinate Arm C
Space


From Figure 4.1 we can see that the result of the path
planning has implemented collision-free movement of two
arms when main and subordinate arm move at the same time.
The experimental result data is shown in Table 4.2.
從圖 4.1我們可以看出，路徑規劃的結果在同一時間當主要次要手
臂移動已實行兩手臂的無碰撞移動。實驗結果數據顯示在表4.2。
Table4.2.
The Comparison of
Local Information
Searching Update
Before and After
Improvement

It can be seen from Table 4.2, although the ant colony
algorithm improved or not both can be able to search the
optimal path of subordinate arm, the improved path
planning algorithm have been significantly improved in
both the planning steps and the start-brake times. The
optimizing capacity is also increased. The average running
time of subordinate arm has reduced obviously.
從表4.2可以看出，雖然蟻群演算法的改進與否都能夠搜索到次要
手臂的最優路徑，但改進的路徑規劃演算法有了顯著改善，無論
是在規劃步驟和啟動剎車次數。在優化能力有所增加。則次要手
臂的平均運行時間已明顯減少。
Conclusion

We have researched the ant colony algorithm applied for path
planning in mechanical arm’s configuration space in this
paper. Combining with the characteristics of new search
space, we have increased our instruction to the original ant
colony algorithm definition. Aiming at the characteristics of
obstacle space in searching space, we have focused on
algorithm improvement of the further optimization method of
multi-feasible path and the continue optimization method of
non-feasible path.
在這篇文章中，我們已經研究了蟻群演算法應用於機械手臂配置空
間的路徑規劃。結合新搜索空間的特色，我們增加了我們的指示到
原始蟻群演算法的定義。在搜尋空間中針對障礙空間的特點，我們
把重點放在多可行路徑的進一步優化方法的改進演算法和無可行路
徑的持續優化方法。

By the detailed analysis in this article and the simulation
results of the optimization path by using the improved
algorithms, we can see that through the improvement of
taboo table update and local pheromone update, the two
questions above are effectively solved, and the
adaptability of the improved ant colony algorithm is
highly enhanced. In this paper, the research results have
provided a new effective way for dual-arm robot’s
collision-free optimal path planning.
在文章中詳細分析和使用改進的演算法優化路徑的模擬結果，
我們可以看出，經過禁止表更新的改進和局部費洛蒙更新，以
上兩個問題有效的被解決，以及改進蟻群演算法的適應性也大
大的提升。 在本文的研究成果對雙臂機器人的無碰撞優化路徑
規劃提供了一個新的有效方法。
References





[1] Ling Qin, Yixin Chen, Ling Chen, Yuan Yao. A new
optimization algorithm based on ant colony system with
density control strategy, ISNN, 971: 385-390, 2006.
[2] MEI Hao, TIAN Yantao, ZU Linan. Algorithm for route
planning of robot in dynamtic environment, Journal of
Jinlin University (Information Science Edition), 24(2): 148152, 2006.
[3] Li Shuangyan. Path Planning Research of Robot in
Dynamic Environments, Central South University, 2005.
[4] Li Guo, Liu Shaojun. Path Planning of Mobile Robot
Based on Improved Ant Colony Algorithm, Control
Engineering of China, 473-485, 2005.
[5] Sungshun Weng, Yuanhung Liu. Mining time series data
for segmentation by using ant colony optimization,
European Journal of Operational Research, 173: 921-937,
2006.




[6] Silvia Mazzeo, Irene Loiseau. An ant colony algorithm
for the capacitated vehicle routing, Electronic Notes in
Discrete Machematics, 18, 181-186, 2004.
[7] Zhu Qingbao, Zhang Yulan. An Ant Colony Algorithm
Based on Grid Method for Mobile Robot Path Planning,
ROBOT, 132-135, 2005
[8] Chen Feng, Ding Fuqiang, Zhao Xifang. Collision-free
Path Planning of Dual-arm Robot, ROBOT, 112-121, 2002.
[9] Ding Fuqiang, Han Weijun, Zhao Xifang. Collision-Free
Motion Planning of Dual-Arm Robot Based on C-space,
Journal of Shanghai Jiaotong University, 54-58, 2001.
Thank You
!!