当前位置: 首页 > article >正文

论文实现:Reactive Nonholonomic Trajectory Generation via Parametric Optimal Control

1. 多项式螺旋

曲率:
κ ( s ) = a 0 + a 1 s + a 2 s 2 + a 3 s 3 + a 4 s 4 + a 5 s 5 \begin{align} \kappa(s) = a_0 + a_1s + a_2s^2 + a_3s^3 + a_4s^4 + a_5s^5 \end{align} κ(s)=a0+a1s+a2s2+a3s3+a4s4+a5s5
机器人朝向:
θ ( s ) = a 0 s + a 1 s 2 2 + a 2 s 3 3 + a 3 s 4 4 + a 4 s 5 5 + a 5 s 6 6 \begin{align} \theta(s) = a_0s + \frac{a_1s^2}{2} + \frac{a_2s^3}{3} + \frac{a_3s^4}{4} + \frac{a_4s^5}{5} + \frac{a_5s^6}{6} \end{align} θ(s)=a0s+2a1s2+3a2s3+4a3s4+5a4s5+6a5s6
轨迹:
x ( s ) = ∫ 0 s cos ⁡ ( θ ( s ) ) d s \begin{align} x(s) = \int_0^s{\cos(\theta(s))ds} \end{align} x(s)=0scos(θ(s))ds
y ( s ) = ∫ 0 s sin ⁡ ( θ ( s ) ) d s \begin{align} y(s) = \int_0^s{\sin(\theta(s))ds} \end{align} y(s)=0ssin(θ(s))ds

2. 边界条件

初始条件: s = 0 , x = 0 , y = 0 , θ = 0 s = 0,x = 0, y = 0, \theta = 0 s=0x=0,y=0,θ=0
结束条件: s = s f , x = x f , y = y f , θ = θ f s = s_f, x = x_f, y = y_f, \theta = \theta_f s=sf,x=xf,y=yf,θ=θf
x b = [ x f   y f   θ f ] T \begin{align} \bf{x_b} = \left[ x_f \ y_f \ \theta_f \right]^T \end{align} xb=[xf yf θf]T
参数:
q = [ a 0   a 1   a 2   a 3   a 4   a 5   s f ] T \begin{align} \bf{q} = \left[a_0 \ a_1 \ a_2 \ a_3 \ a_4 \ a_5 \ s_f \right]^T \end{align} q=[a0 a1 a2 a3 a4 a5 sf]T
边界条件:
g ( q ) = h ( q ) − x b = { x ( s f ) − x f = 0 y ( s f ) − y f = 0 θ ( s f ) − θ f = 0 \begin{align} \bf{g(q)} = \bf{h(q)} - \bf{x_b} = \begin{cases} x(s_f) - x_f = 0 \\ y(s_f) - y_f = 0 \\ \theta(s_f) - \theta_f = 0 \end{cases} \end{align} g(q)=h(q)xb= x(sf)xf=0y(sf)yf=0θ(sf)θf=0

3. 优化问题

minimize:
J ( q ) = 1 2 ∫ 0 s f [ κ ( q ) ] 2 d s \begin{align} J(\bf{q}) = \frac{1}{2}\int_0^{s_f}[\kappa(\bf{q})]^2ds \end{align} J(q)=210sf[κ(q)]2ds
subject to:
g ( q ) = 0 s f f r e e \begin{align} \bf{g(q)} = 0 \quad s_f \quad free \end{align} g(q)=0sffree

4. Hamiltonian

H ( q , λ ) = J ( q ) + λ T g ( q ) \begin{align} \bf{H}(\bf{q}, \bf{\lambda}) = J(\bf{q}) + \bf{\lambda}^T \bf{g(q)} \end{align} H(q,λ)=J(q)+λTg(q)

5. 参数更新方程

看着和牛顿二阶梯度法形式一致:
H Δ x = − J T \begin{align} \bf{H}\Delta \bf{x} = -\bf{J}^T \end{align} HΔx=JT
H = [ δ 2 H δ q 2 ( q , λ ) δ δ q g ( q ) T δ δ q g ( q ) 0 ] \begin{align} \bf{H} = \begin{bmatrix} \frac{\delta^2 \bf{H}}{\delta \bf{q}^2}(\bf{q}, \bf{\lambda}) & \frac{\delta}{\delta \bf{q}} \bf{g}(\bf{q})^T \\ \frac{\delta}{\delta \bf{q}}\bf{g}(\bf{q}) & \bf{0} \end{bmatrix} \end{align} H=[δq2δ2H(q,λ)δqδg(q)δqδg(q)T0]
Δ x = [ Δ q Δ λ ] \begin{align} \Delta \bf{x} = \begin{bmatrix} \Delta \bf{q} \\ \Delta \bf{\lambda} \end{bmatrix} \end{align} Δx=[ΔqΔλ]
J = [ δ δ q H ( q , λ ) g ( q ) ] \begin{align} \bf{J} = \begin{bmatrix} \frac{\delta}{\delta \bf{q}}\bf{H}(\bf{q},\bf{\lambda}) \\ \bf{g}(\bf{q}) \end{bmatrix} \end{align} J=[δqδH(q,λ)g(q)]

H = [ H 7 x 7 H 3 x 7 T H 3 x 7 0 3 x 3 ] \begin{align} \bf{H} = \begin{bmatrix} \bf{H}_{7x7} & \bf{H}_{3x7}^T \\ \bf{H}_{3x7} & \bf{0}_{3x3} \end{bmatrix} \end{align} H=[H7x7H3x7H3x7T03x3]

H 7 x 7 = δ 2 J ( q ) δ q 2 + δ 2 ( λ T g ( q ) ) δ q 2 = \begin{align} \bf{H}_{7x7} &= \frac{\delta ^ 2 \bf{J}(\bf{q})}{\delta \bf{q}^2} + \frac{\delta ^ 2 (\bf{\lambda} ^ T \bf{g}(\bf{q}))}{\delta \bf{q}^2} \\ &= \end{align} H7x7=δq2δ2J(q)+δq2δ2(λTg(q))=
待续…


http://www.kler.cn/a/461205.html

相关文章:

  • Vue3 简介
  • C++初步认识函数
  • @RestControllerAdvice注解
  • OneOS操作系统入门-驱动-03:I2C总线及驱动
  • java实现excel导入参考资料合集
  • Zookeeper在中间件的应用和在Spring Boot业务系统中实现分布式锁和注册中心的解决方案
  • CT 扫描显示 USB-C 电缆可能隐藏复杂的恶意硬件
  • 强化学习方法分类详解
  • 电脑cxcore.dll文件缺失怎么办?cxcore100.dll缺失问题解决办法
  • svn不能添加.a文件
  • 从零开发一套UWB定位系统需要多长时间?UWB超宽带定位系统源码
  • 计算机网络•自顶向下方法:IP分片与重组
  • Dell服务器升级ubuntu 22.04失败解决
  • 基于Django+python的Python在线自主评测系统设计与实现
  • caddyfile如何托管静态文件,首页默认为index.html
  • 如何在谷歌浏览器中创建安全的密码
  • 深入理解 pytest Fixture 方法及其应用
  • Java子线程无法获取Attributes的解决方法
  • 【Linux】基础IO-----软硬链接与动静态库
  • docker安装Home Assistant正常下载笔记