图5 处理后800?800的平面场景图
在原问题中，若没进行确定圆弧位置与转弯半径以及平面场景的处理，原问题求解将会很难，并且所有的转弯点均是未知，经处理后，我们将问题转化为在已知转弯点，寻找合适的转弯点，使得路径最短，即我们将问题转化为了最短路径的优化问题。
5.2避障最短路径模型的建立
问题转化为最短路径的优化问题后，我们易知优化的目标函数为机器人在行进过程中最短路径，通过0-1变量来选取经过的转弯点，因此可建立0-1规划模型。 5.2.1目标函数
目标函数为机器人出区域中一点到达另一点的避障最短路径，避障最短路径Z为行进过程中直线路径与转弯路径之和，于是有
minZ???Xij(aij?bij)
i?1j?1nn（1）
其中，aij为圆弧i切点到圆弧j切点的直线距离，即机器人从圆弧i切点到圆弧j切点的直线路径；bij为机器人从圆弧i行进至圆弧j切点时的转弯路径；Xij为0-1变量，表示若选择行走圆弧i后行走圆弧j，Xij为1，否则为0，i,j?1...n,n?36,i?j。
7
我们设(xik,yik)为圆弧i的切点坐标，i?1...36，k为切点的次序，i为各圆弧的编码，
k?1,2，如(xi1,yi1)表示为圆弧i的第一个切点坐标。
机器人从圆弧i切点到圆弧j切点的直线路径aij为
aij?(xi2?xj2)2?(yi2?yj1)2i,j?1..36,i?j
（2）
机器人从圆弧i行进至圆弧j切点时的bij为 其中
(xi1?xi2)2?(yi1?yi2)2??2??2?2?2cos?
bij???i,j?1..36,i?j
（3）
?为转弯半径，??10。
5.2.2约束条件
1.避障条件的约束
在本问题中，障碍物边界可分为直线段与圆弧两种情况，针对障碍物边界不同的两种情况，我们列出避障条件的约束：
（1）避障约束条件1
任意一条可行路径与所有障碍物的边界线段间的最短距离大于10个单位.
设平面内有两条线段AB和CD，A点的坐标为 (xi2,yi2),B点的坐标为 (xj1,yj1) ，
C点的坐标为(xp,yp),D点的坐标为(xq,yq)，其中A、B可视为圆弧的切点坐标，C、
D为圆弧的切点坐标AB线段两圆弧的切点的连线，CD视为障碍物的某一条边界线段。
mmm设Ppq是直线AB上的一点，Ppq点的坐标(xmpq,ypq)可以表示为
??X?xi2?s(xj1?xi2) ???Y?yi2?s(yj1?yi2)mm当参数0?s?1时，Ppq是线段AB 上的点；当参数s?0时，Ppq是BA延长线上的m点；当参数s?1时，Ppq是AB延长线上的点。
设Qij是直线CD上的一点，Qij点的坐标 (U,V) 可以表示为
??U?xP?t(xq?xp) ?Y?y?s(y?y)?pqp?Qij是线段CD上的点；Qij是DC延长线上的点；当参数0?t?1时，当参数t?0时，
8
当参数 t?1 时，Qij是CD延长线上的点。
mPpq,Qij两点之间的距离为
mPpqQij?(X?U)2?(Y?V)2
距离的平方为
mf(s,t)?PpqQij?(X?U)2?(Y?V)2
2?[(xi2?xp)?s(xj1?xi2)?t(xq?xp)]2?[(yi2?yp)?s(yj1?yi2)?t(yq?yp)]2 要求直线AB，CD之间的最短距离，也就是要求函数f(s,t)的最小值。对f(s,t)分别求关于s，t的偏导数，并令偏导数为零：
??f(s,t)??s?0 ??f(s,t)??0??t展开并整理后，得到下列方程组：9
?[(xj1?xi2)2?(yj1?yi2)2]s?2??[(xj1?xi2)(xq?xp)?(yj1?yi2)(yq?yp)]t???(xi2?xj1)(xi2?xp)?(yi2?yj1)(yi2?yp)???[(xj1?xi2)(xq?xp)?(yj1?yi2)(yq?yp)]t ?22??[(xq?xp)?(yq?yp)]t??(x?x)(x?x)?(y?y)(y?y)i2pqpi2pqp?m如果从这个方程组求出的参数s，t的值满足0?s?1，0?t?1，说明Ppq点落在线mQij的长度为 段AB上，Qij点落在线段CD上，这时PpqmPpqQij?(X?U)2?(Y?V)2
mQij就是线段AB与CD的最短距离。 此时Ppq如果从方程组求出的参数s，t的值不满足0?s?1，0?t?1，说明不可能在线段
mmQij的长度就是线段AB与，在线段CD内部找到一点Qij，使得PpqAB内部找到一点Ppq CD 的最短距离。这时，还需要进行考虑的是：平面中一个点到一条线段的最短距离。
mmm设编号为m的障碍物的圆心坐标Ppq和一条线段AB，设Ppq点的坐标为(xmApq,ypq)，
点的坐标为(xi2,yi2)，B点的坐标为(xj1,yj1)。
此时，直线AB的参数形式的方程为
9
??x?xi2?t(xj1?xi2) ?y?y?t(y?y)?i2j1i2?直线上的点(x,y)，当参数0?t?1时，是线段AB上的点；当参数t?0时，是BA延长线上的点；当参数 t?1 时，是AB延长线上的点。
m 通过Ppq点，与直线AB垂直的平面方程为
m(xj1?xi2)(x?xmpq)?(yj1?yi2)(y?ypq)?0
mmQij?AB，所以Qij点也是从Ppq下面求这个平面与直线AB的交点Qij。显然，Ppq点
向直线AB作垂线的垂足点。
将
??x?xi2?t(xj1?xi2) ?y?y?t(y?y)?i2j1i2?代入平面方程，化简后解得
t?m(xi2?xmpq)(xi2?xj1)?(yi2?ypq)(yi2?yj1)(xi2?xj1)?(yi2?yj1)22
然后，将上面得到的t的值代入直线方程，得到
??X?xi2?t(xj1?xi2) ?Y?y?t(y?y)?i2j1i2?其中，(X,Y)为垂足点Qij的坐标。
mmQij的长度，也就是Ppq 此时线段Ppq点到直线AB的垂直距离为
m2m2PpqQij?(X?xmpq)?(Y?ypq)
mQij 如果前面求出的参数t的值满足0?t?1，说明垂足Qij点落在线段AB上，这时Ppqm的长度就是Ppq点到线段AB的最短距离。
如果前面求出的参数t的值满足t?0，说明垂足Qij点不落在线段AB上，而是落在
mmmA点到线段AB的最短距离，就是Ppq点到A点的距离，即PpqBA的延长线上，这时Ppq2m2?(xi2?xmpq)?(yi2?ypq) 。
如果前面求出的参数t的值满足t?1，说明垂足Qij点不落在线段AB上，而是落在
mmmB这时，Ppq点到线段AB的最短距离，就是Ppq点到B点的距离，即PpqAB的延长线上，
10
