第3期 冷静，等：实时避碰的无人水面机器人在线路径规划方法 ·345. 3)避碰角： 把机器人本体动力学、运动学以及环境不确定性等 △yuo= 约束考虑进去，并且能解决障碍物转向的“或”的逻 1 辑问题，得到一条关于目标函数最优且满足所有约 √voLoP-p 束条件的最优路径。由于MLP要求目标函数和约 (1) 束均为线性，因此将这些约束和目标函数线性化。 即在规划时间内，相对速度夹角的改变量。本文避 2.1目标函数 碰策略是首先将相对速度增量线性化为避碰角，然 1)第1类目标函数：距离收敛。 后通过避碰角选择方向来脱离避碰区，最终实现避 USV通过调整速度大小和方向，每规划一个时 碰过程。式(1)的线性化推导流程见文献[9]。 间间隔都使下一时刻USV离目标越来越近，即 4)碰撞时间：T=Lo/"o|,即当相对速度 Luc→0，min:pu-pol 处于碰撞区时，USV与障碍物的碰撞时间。 4=l-(+△y)△1，j=1,2(2) 5)碰撞危险度：p=(a×DCPA)2+ 式中j表示维数，即将到目标的距离分解为X、Y轴， (b×TCPA)2,其中a,b分别表示DCPA和TCPA的 这样才可以用于线性规划器中。△y:是所要规划的变 权值。碰撞危险度通过DCPA和TCPA加权确定。 量，即为每个规划时间，相对目标的速度改变量。 6)规划时间：T=0p+w2/ヶ，即规划时间与碰 2)第2类目标函数：速度最优。 撞危险度成正比，与碰撞时间成反比。因为受到 将相对目标的速度分解为指向目标的分量V。 USV本体运动性能约束，其转角加速度和纵向加速 和垂直目标的分量V,。要想使到达障碍物的速度 度都有限制，所以碰撞危险度越大，碰撞时间越短， 最优，只需最大化Vc和最小化V,即可。即 规划时间就越短，否则无法及时进行航速和方向的 max:Vel=|Vuc lcos Yuc 规划。 min:Vrl=Vuc lsin Yuc (3) 1.2.2避碰策略 所以最小化为|V,2,即 从上述避障法的原理可以看出，要使USV不与 min:g(Vue)=IVr2=IVucl2-Pic/IVicll Lucl2 障碍物发生碰撞，其相对速度"0必须产生一个转 由泰勒公式可得 角，使其脱离碰撞区。从图1可以看出它有2个方 向可以选择，当向左转时，即使相对速度产生△y △g(Vc)=Vg·△V+o(‖AVcI) 的转动，向右转时，使相对速度产生△y。的转动。 Vg =2Vr-2Puc/|Luc2 LuG min:g+△g 这个转动是由规划时间内相对速度矢量△"0的变 因此推理公式(3)转化为 化产生的，式(1)可以看出。因此，将相对速度矢量 的增量△o作为待规划的变量。这样就形成了 q1=g(Vc）+Vg·AVic USV的避碰模型： 9=-Vc-V.·AVc (4) 综上所述可得线性规划器的目标函数： y≤- Yw Lwo min:J=∑m,d+m91+m42 △n≤T ∑m+m+m,=1,m>0,m3>0,m>0,i=1,2 OR Yw Lw -T≤ 式中：m1、m2,m3、m4分别代表上述4个目标函数 √/"oPLoo'-P 的权值。 Aviw≤△yo 2.2USV本体动力学自身约束 1)加速度的约束：-△r≤△，≤△。 2混合线性规划方法 2)速度的约束：-Vx≤△y;+;≤Vmso 速度避障法解决的是局部实时避碰的问题，但 因为线性规划只有一个上下限，所以速度变量 容易陷入局部最小值，所以在对USV进行在线路径 的上下限整合为 规划的时候，必须加入全局规划器。在基本LP中， max{-△mr,-Vo-"g}≤△y,≤min{△s,Vt-vg} 各约束条件之间形成“与”的逻辑关系，对于具有复 角加速度的约束：-△y0≤△yuo≤△yo。 杂逻辑关系的优化问题不能直接求解。用MLP方 2.3障碍物约束 法解决动态环境下的路径规划问题，可以很容易地 根据上一节对速度避障法的避碰策略可知：３）避碰角： ΔγＵＯ ＝ － １ ｖＵＯ ２ ＬＵＯ ２ － Ｐ ２ ＬＵＯ － Ｐ ｖＵＯ ２ ｖＵＯ é ë ê ê ù û ú ú·ΔｖＵＯ （１） 即在规划时间内，相对速度夹角的改变量。 本文避 碰策略是首先将相对速度增量线性化为避碰角，然 后通过避碰角选择方向来脱离避碰区，最终实现避 碰过程。 式（１）的线性化推导流程见文献［９］。 ４）碰撞时间： τ ＝ ＬＵＤ ／ ｖＵＯ ，即当相对速度 处于碰撞区时，ＵＳＶ 与障碍物的碰撞时间。 ５ ） 碰 撞 危 险 度： ρ ＝ （ａ × ＤＣＰＡ） ２ ＋ （ｂ × ＴＣＰＡ） ２ ，其中 ａ， ｂ 分别表示 ＤＣＰＡ 和 ＴＣＰＡ 的 权值。 碰撞危险度通过 ＤＣＰＡ 和 ＴＣＰＡ 加权确定。 ６）规划时间： Ｔ ＝ ｗ１ ρ ＋ ｗ２ ／ τ ，即规划时间与碰 撞危险度成正比，与碰撞时间成反比。 因为受到 ＵＳＶ 本体运动性能约束，其转角加速度和纵向加速 度都有限制，所以碰撞危险度越大，碰撞时间越短， 规划时间就越短，否则无法及时进行航速和方向的 规划。 １．２．２ 避碰策略 从上述避障法的原理可以看出，要使 ＵＳＶ 不与 障碍物发生碰撞，其相对速度 ｖＵＯ 必须产生一个转 角，使其脱离碰撞区。 从图 １ 可以看出它有 ２ 个方 向可以选择，当向左转时，即使相对速度产生 Δγ Ｒ ＵＯ 的转动，向右转时，使相对速度产生 Δγ Ｌ ＵＯ 的转动。 这个转动是由规划时间内相对速度矢量 ΔｖＵＯ 的变 化产生的，式（１）可以看出。 因此，将相对速度矢量 的增量 ΔｖＵＯ 作为待规划的变量。 这样就形成了 ＵＳＶ 的避碰模型： ＯＲ Δγ Ｒ ＵＯ ≤－ ｖＵＯ ＬＵＯ ｖＵＯ ２ ＬＵＯ ２ － Ｐ ２ ＬＵＯ － Ｐ ｖＵＯ ２ ｖＵＯ é ë ê ê ù û ú ú· Δｖ Ｔ ＵＯ ≤π － π ≤－ ｖＵＯ ＬＵＯ ｖＵＯ ２ ＬＵＯ ２ － Ｐ ２ ＬＵＯ － Ｐ ｖＵＯ ２ ｖＵＯ é ë ê ê ù û ú ú· Δｖ Ｔ ＵＯ ≤Δγ Ｌ ＵＯ ì î í ï ï ï ï ï ï ï ï ïï ２ 混合线性规划方法 速度避障法解决的是局部实时避碰的问题，但 容易陷入局部最小值，所以在对 ＵＳＶ 进行在线路径 规划的时候，必须加入全局规划器。 在基本 ＬＰ 中， 各约束条件之间形成“与”的逻辑关系，对于具有复 杂逻辑关系的优化问题不能直接求解。 用 ＭＩＬＰ 方 法解决动态环境下的路径规划问题，可以很容易地 把机器人本体动力学、运动学以及环境不确定性等 约束考虑进去，并且能解决障碍物转向的“或”的逻 辑问题，得到一条关于目标函数最优且满足所有约 束条件的最优路径。 由于 ＭＩＬＰ 要求目标函数和约 束均为线性，因此将这些约束和目标函数线性化。 ２．１ 目标函数 １）第 １ 类目标函数：距离收敛。 ＵＳＶ 通过调整速度大小和方向，每规划一个时 间间隔都使下一时刻 ＵＳＶ 离目标越来越近，即 ＬＵＧ → ０，ｍｉｎ： ｐＵ － ｐＯ ｄｊ ＝ ｌ ｊ － （ｖｊ ＋ Δｖｊ）Δｔ ， ｊ ＝ １， ２ （２） 式中：ｊ 表示维数，即将到目标的距离分解为 Ｘ、Ｙ 轴， 这样才可以用于线性规划器中。 Δｖｊ 是所要规划的变 量，即为每个规划时间，相对目标的速度改变量。 ２）第 ２ 类目标函数：速度最优。 将相对目标的速度分解为指向目标的分量 ＶＣ 和垂直目标的分量 ＶＴ 。 要想使到达障碍物的速度 最优，只需最大化 ＶＣ 和最小化 ＶＴ 即可。 即 ｍａｘ： ＶＣ ＝ ＶＵＧ ｃｏｓ γＵＧ ｍｉｎ： ＶＴ ＝ ＶＵＧ ｓｉｎ γＵＧ （３） 所以最小化为 ＶＴ ２ ，即 ｍｉｎ：ｇ（ＶＵＧ） ＝ ＶＴ ２ ＝ ＶＵＧ ２ － Ｐ ２ ＵＧ ／ Ｖ ２ ＵＧ ＬＵＧ ２ 由泰勒公式可得 Δｇ（ＶＵＧ ） ＝ Ñｇ·ΔＶ Ｔ ＵＧ ＋ ｏ（‖ΔＶＵＧ‖） Ñｇ ＝ ２ ＶＴ － ２ＰＵＧ ／ ＬＵＧ ２ ＬＵＧ ｍｉｎ：ｇ ＋ Δｇ 因此推理公式（３）转化为 ｑ１ ＝ ｇ（ＶＵＧ） ＋ Ñｇ·ΔＶ Ｔ ＵＧ ｑ２ ＝ － ＶＣ － ÑＶｃ·ΔＶ Ｔ ＵＧ （４） 综上所述可得线性规划器的目标函数： ｍｉｎ：Ｊ ＝ ∑ｉ ｍｉｄｉ ＋ ｍ３ ｑ１ ＋ ｍ４ ｑ２ ∑ｉ ｍｉ ＋ ｍ３ ＋ ｍ４ ＝ １，ｍｉ ＞ ０，ｍ３ ＞ ０，ｍ４ ＞ ０，ｉ ＝ １，２ 式中： ｍ１ 、 ｍ２、 ｍ３、 ｍ４ 分别代表上述 ４ 个目标函数 的权值。 ２．２ ＵＳＶ 本体动力学自身约束 １）加速度的约束： － Δｍａｘ £Δｖｊ £Δｍａｘ 。 ２）速度的约束： － Ｖｍａｘ £Δｖｊ ＋ ｖｊ £Ｖｍａｘ 。 因为线性规划只有一个上下限，所以速度变量 的上下限整合为 ｍａｘ｛ － Δｍａｘ， － Ｖ ｍａｘ Ｕ － ｖＵｊ｝ £Δｖｊ £ｍｉｎ｛Δｍａｘ，Ｖ ｍａｘ Ｕ － ｖＵｊ｝ 角加速度的约束： － Δγ ｍａｘ ＵＯ £Δγ ＵＯ £Δγ ｍａｘ ＵＯ 。 ２．３ 障碍物约束 根据上一节对速度避障法的避碰策略可知： 第 ３ 期 冷静，等：实时避碰的无人水面机器人在线路径规划方法 ·３４５·