- q是当前节点坐标(x,y,z)
- 引力系数 ξ \xi ξ,斥力系数 η \eta η
- d i s ( q ) dis(q) dis(q)表示q到目标点的距离 d i s ( q , q g o a l ) dis(q,q_{goal}) dis(q,qgoal)
- D ( q ) D(q) D(q)表示q到障碍物(体积中心)的距离
引力
U a t t ( q ) = { ξ 2 d i s ( q ) 2 if d i s ( q ) ≤ d i s g o a l ∗ d i s g o a l ∗ ξ d i s ( q ) − ξ 2 ( d i s g o a l ∗ ) 2 if d i s ( q ) > d i s g o a l ∗ U_{att}(q)=\begin{cases} \frac{\xi}{2}dis(q)^2 &\text{if } dis(q)\leq dis_{goal}^* \\ dis_{goal}^*\xi dis(q)-\frac{\xi}{2}(dis_{goal}^*)^2 &\text{if } dis(q)>dis_{goal}^* \end{cases} Uatt(q)={2ξdis(q)2disgoal∗ξdis(q)−2ξ(disgoal∗)2if dis(q)≤disgoal∗if dis(q)>disgoal∗
求导得
∇ U a t t ( q ) = { ξ ( q − q g o a l ) if d i s ( q ) ≤ d i s g o a l ∗ d i s g o a l ∗ ξ ( q − q g o a l ) ∗ 1 d i s ( q ) if d i s ( q ) > d i s g o a l ∗ \nabla U_{att}(q)=\begin{cases} {\xi}(q-q_{goal}) &\text{if } dis(q)\leq dis_{goal}^* \\ {dis_{goal}^*\xi(q-q_{goal})}*\frac 1{dis(q)} &\text{if } dis(q)>dis_{goal}^* \end{cases} ∇Uatt(q)={ξ(q−qgoal)disgoal∗ξ(q−qgoal)∗dis(q)1if dis(q)≤disgoal∗if dis(q)>disgoal∗
斥力
U r e p ( q ) = { η 2 ( 1 D ( q ) − 1 Q ∗ ) 2 if D ( q ) ≤ Q ∗ 0 if D ( q ) > Q ∗ U_{rep}(q)=\begin{cases} \frac{\eta}{2}(\frac 1 {D(q)}-\frac 1 {Q^*})^2 &\text{if } D(q)\leq Q^* \\ 0 &\text{if } D(q)>Q^* \end{cases} Urep(q)={2η(D(q)1−Q∗1)20if D(q)≤Q∗if D(q)>Q∗
求导得
∇ U r e p ( q ) = { η ( 1 D ( q ) − 1 Q ∗ ) / D ( q ) 2 ∗ ∇ D ( q ) if D ( q ) ≤ Q ∗ 0 if D ( q ) > Q ∗ \nabla U_{rep}(q)=\begin{cases} \eta(\frac 1 {D(q)}-\frac 1 {Q^*})/{D(q)^2}*\nabla{D(q)} &\text{if } D(q)\leq Q^* \\ 0 &\text{if } D(q)>Q^* \end{cases} ∇Urep(q)={η(D(q)1−Q∗1)/D(q)2∗∇D(q)0if D(q)≤Q∗if D(q)>Q∗
- (宋建辉2017)在斥力势场函数中引入q和目标点之间的距离 d i s ( q , q g o a l ) t dis(q,q_{goal})^t dis(q,qgoal)t[一般t=2]
∇ U r e p = η ( 1 D ( q ) − 1 Q ∗ ) / D ( q ) 2 ∗ d i s ( q ) t ∗ ∇ D ( q ) + η 2 ∗ t ( 1 D ( q ) − 1 Q ∗ ) 2 ∗ d i s ( q ) t − 1 ∗ ∇ d i s ( q ) \nabla U_{rep} = \eta(\frac 1 {D(q)}-\frac 1 {Q^*})/{D(q)^2}*dis(q)^t*\nabla{D(q)}+\frac\eta 2*t(\frac 1 {D(q)}-\frac 1 {Q^*})^2*{dis(q)}^{t-1}*\nabla{dis(q)} ∇Urep=η(D(q)1−Q∗1)/D(q)2∗dis(q)t∗∇D(q)+2η∗t(D(q)1−Q∗1)2∗dis(q)t−1∗∇dis(q)
合力&其他力
U ( q ) = U a t t ( q ) − U r e p ( q ) F ( q ) = ∇ U ( q ) U(q)=U_{att}(q)-U_{rep}(q) \\ F(q)=\nabla{U(q)} U(q)=Uatt(q)−Urep(q)F(q)=∇U(q)
- (邓叶2021)引入协调力,g为协调增益系数
F o r d i = ∇ U o r d i ( q ) = 0.5 ∗ g ∗ ( 1 D ( q ) − 1 Q ∗ ) 2 ∗ d i s ( q ) ∗ ∇ D ( q ) F_{ordi} = \nabla U_{ordi}(q) = 0.5*g*(\frac 1 {D(q)}-\frac 1 {Q^*})^2*dis(q)*\nabla D(q) Fordi=∇Uordi(q)=0.5∗g∗(D(q)1−Q∗1)2∗dis(q)∗∇D(q)
F ( q ) = ∇ ( U a t t ( q ) − U r e p ( q ) − U o r d i ( q ) ) F(q) = \nabla (U_{att}(q)-U_{rep}(q)-U_{ordi}(q)) F(q)=∇(Uatt(q)−Urep(q)−Uordi(q))
- (韩彬2022)引入逃逸力,脱离极小值
判定极小值: F a t t − F r e p F a t t < ε → 0 , cos ( F a t t − F r e p ) = σ ∈ [ − 1 , 0 ] \frac{F_{att}-F_{rep}}{F_{att}}<\varepsilon \rightarrow 0,\cos(F_{att}-F_{rep})=\sigma \isin[-1,0] FattFatt−Frep<ε→0,cos(Fatt−Frep)=σ∈[−1,0]
F e s c = ∇ U e s c ( q ) = a / ( α o b s x 1 + α 0 x 2 ) , α 0 表示障碍物作用范围 F_{esc} = \nabla U_{esc}(q) = a/({\alpha_{obs}^{x1}+\alpha_0^{x2}}),\alpha_0表示障碍物作用范围 Fesc=∇Uesc(q)=a/(αobsx1+α0x2),α0表示障碍物作用范围
F ( q ) = F a t t − F r e p + F e s c F(q) = F_{att}-F_{rep}+F_{esc} F(q)=Fatt−Frep+Fesc
创新点
振荡检测:避免“反复横跳”
局部极小值是引力等于斥力的陷阱点(U型)[dg>0&&rep==att]
简化障碍物: 只考虑在目标点一侧障碍物的斥力函数影响
预测距离: 通过比较移动机器人到简化障碍物之间的距离r0来判断自己的下一步位置
r0<预测距离2=就转角设置 虚拟目标点
虚拟目标点: 最远的障碍物为半径的另一端设置虚拟目标点
设定角度阈值,实现加减速调节(前后看到为物体的角度差判定方向)
2022 : 引入自旋场+三个方向场(控制无人机)
角度骤变:与目标点的夹角作为角度判断依据
2020 : 黑洞势场法(BHPF),kstr的值远远大于katt,并且应小于krep,以避免碰撞。-Kstr/2*(ps-(q-qg))**2【黑洞场范围内】