Introduction
这篇文章提示了一下feedback的本质作用:avert the issue of unmodeled dynamics,但是通常会有三方面的问题:
- There is no obvious error signal available at the rate and scale of a single, rapid maneuver: GPS errors are too large to be helpful on the scale of meters; visual odometry is slow and dependent on favorable, feature-rich environments; accurate inertial navigation is too expensive and bulky; and odometry is subject to the same dynamics errors that we are trying to use feedback to counteract!
- Feedback assumes that we approximately understand the relationship between control signals and error dynamics and that this relationship can be linearized. In reality, vehicle platforms have motor saturation, time delays, and velocity profiling on top of the nonlinear effects of terrain interaction.
- Trajectory tracking for nonlinear systems works best when incorporated into a two-degree-of-freedom controller that requires feed-forward planning in addition to feedback.
这篇文章的主要目的就是:建立一个skid-steer vehicle dynamics并评估一下在feed-forward control framework中作为motion predictor使用的合理性。
Related Work
Central to the model is that the concept that turning motions with slip have instantaneous centers of rotation that are located forward or backward from the centerline of the platform, in contrast to the ideal differential drive model.
Method
The system model must be composed of two separate pieces:
- the interaction between tread and ground to produce motion
- the platform dynamics at work when turning commanded velocities into actual tread velocities
Kinematic Motion Models
(1) ρ ˙ = [ x ˙ y ˙ θ ˙ ] = [ c o s ( θ ) − s i n ( θ ) 0 s i n ( θ ) c o s ( θ ) 0 0 0 1 ] v ( v x , v θ , α ) \dot{\rho} = \begin{bmatrix} \dot{x}\\ \dot{y}\\ \dot{\theta} \end{bmatrix}= \begin{bmatrix} cos(\theta) & -sin(\theta) & 0\\ sin(\theta) & cos(\theta) & 0 \\ 0 & 0 & 1 \end{bmatrix}\mathbf{v}(v_{x}, v_{\theta}, \alpha) \tag{1} ρ˙=⎣⎡x˙y˙θ˙⎦⎤=⎣⎡cos(θ)sin(θ)0−sin(θ)cos(θ)0001⎦⎤v(vx,vθ,α)(1)
v \mathbf{v} v其实就是effective velocities和commanded velocity之间的tranformation model
-
ideal differential drive (point contact with no slip)
v = [ v x 0 v θ ] \mathbf{v} = \begin{bmatrix} v_{x}\\ 0\\ v_{\theta} \end{bmatrix} v=⎣⎡vx0vθ⎦⎤ -
effective wheel-base (rolling resistance from the treads skidding over the ground during turning can reduce the effective turning velocity and scale it accordingly)
v = [ v x 0 v θ ⋅ α ] \mathbf{v} = \begin{bmatrix} v_{x}\\ 0\\ v_{\theta} \cdot \alpha \end{bmatrix} v=⎣⎡vx0vθ⋅α⎦⎤ -
general kinematic slip (slip conditions may lead to velocity changes in both the longitudinal and lateral directions)
v = [ v x 0 v θ ] + C ( v x , v θ ) ⋅ α \mathbf{v} = \begin{bmatrix} v_{x}\\ 0\\ v_{\theta} \end{bmatrix} + C(v_{x}, v_{\theta}) \cdot \alpha v=⎣⎡vx0vθ⎦⎤+C(vx,vθ)⋅α
其中,
v
x
v_{x}
vx,
v
θ
v_{\theta}
vθ are the forward and turning velocities we would expect to achieve under an ideal differential drive model given our tread velocities.
这里就引入了
α
\alpha
α——set of kinematic correction parameters
For a platform tread width of
B
B
B, these can be found from the left and right tread velocities (
v
L
v_{L}
vL,
v
R
v_{R}
vR) as:
v
x
=
v
L
+
v
R
2
v_{x} = \frac{v_{L} + v_{R}}{2}
vx=2vL+vR
v
θ
=
v
R
−
v
L
B
v_{\theta} = \frac{v_{R} - v_{L}}{B}
vθ=BvR−vL
所以,接下来的任务就是辨识这个 α \alpha α参数
- Premise
- know v L , v R v_{L}, v_{R} vL,vR as measured based on encoders situated on the motors
- get accurate relative pose information over extended time-slices by VO or a tactical-grade INS
We seek to choose
α
\alpha
α to minimize the blow objective function:
f
(
α
)
=
1
2
∫
t
0
t
1
∥
ρ
(
t
;
α
)
−
ρ
m
(
t
)
∥
2
d
t
f(\alpha) = \frac{1}{2} \int_{t_{0}}^{t_{1}}\left \| \rho(t; \alpha) - \rho_{m}(t) \right \| ^{2}dt
f(α)=21∫t0t1∥ρ(t;α)−ρm(t)∥2dt
where ρ m ( t ) \rho_{m}(t) ρm(t)——measured trajectory, ρ ( t ; α ) \rho(t; \alpha) ρ(t;α)——predicted trajectory
We can solve this more quickly by finding the sensitivity of the integrated trajectory to the parameters, i.e. recognizing that the derivative of Eq.1 with respect to the parameters:
- 这块的数学推导暂时没看懂
Driving-System Modeling
作用:predict the platform motion given time-varying control inputs to the treads (没有里程计)
We can model the vehicle with two independent closed-loop motor system
We consider a driving-system consisting of three components as:
- command-velocity time-delay, shaping, and/or profiling
- a closed-loop control system, e.g. PID
- the second-order system describing the electrodynamic system including the direct-current motors and gearing
This model can be used to generate differential equations which can be simulated for given system parameters and control inputs.
Trajectory Design
Optimization-based trajectory design is the process of minimizing a cost function defined over the vehicle trajectory ρ ( t ) \rho(t) ρ(t) by choosing time-varying control inputs u , t ∈ [ 0 , t f ] \mathbf{u}, t \in [0, t_{f}] u,t∈[0,tf]
这篇文章的点是trajectory design on the continuous space of control input functions
第一步,we will parameterize our input function as:
u
(
t
)
=
[
v
x
(
t
)
,
κ
(
t
)
]
\mathbf{u}(t) = [v_{x}(t), \kappa(t)]
u(t)=[vx(t),κ(t)] where
κ
(
t
)
=
v
θ
(
t
)
v
x
(
t
)
\kappa(t) = \frac{v_{\theta}(t)}{v_{x}(t)}
κ(t)=vx(t)vθ(t) is the curvature of the path.
在这里为什么这样的参数化,还是没懂,原文的解释是:
Using curvature rather than angular velocity directly allows us to decouple the shape of the path and the desired speed along the path.
第二步,for the purpose of this work, we assume that a desired trajectory for the platform:
ρ
s
(
s
)
,
∀
s
∈
[
0
,
s
f
]
\rho_{s}(s), \forall s \in [0, s_{f}]
ρs(s),∀s∈[0,sf]
We wish to track it as closely and quickly as possible.
第三步, 我们要定义cost function:
ψ
(
p
)
=
w
t
t
f
+
w
p
∫
o
t
0
m
i
n
s
∥
ρ
(
t
)
−
ρ
d
(
s
)
∥
2
d
t
\psi (\mathbf{p}) = w_{t}t_{f} + w_{p} \int_{o}^{t_{0}} \underset{s}{min} \left \| \rho(t) - \rho_{d}(s) \right \| ^{2}dt
ψ(p)=wttf+wp∫ot0smin∥ρ(t)−ρd(s)∥2dt
第四步,the optimization problem to be solved can be written as:
m
i
n
p
ψ
(
p
)
\underset{\mathbf{p}}{min} \ \ \ \psi(\mathbf{p})
pmin ψ(p)
s
u
b
j
e
c
t
t
o
ρ
(
t
f
)
=
ρ
d
(
s
f
)
subject \ \ to \ \ \rho(t_{f}) = \rho_{d}(s_{f})
subject to ρ(tf)=ρd(sf)
ρ
(
t
)
=
ρ
(
0
)
+
∫
0
t
ρ
˙
(
τ
)
d
τ
\rho(t) = \rho(0) + \int_{0}^{t} \dot{\rho}(\tau)d\tau
ρ(t)=ρ(0)+∫0tρ˙(τ)dτ
第五步,那么是如何解决这个问题呢?
From the point of view of a practical and efficient implementation, we rely on the NLOpt library. In particular, we employ the constrained optimization by linear approximations (COBYLA) method.
Experiments
实验主要分为两个部分,一部分是参数辨识,另一部分就是验证optimization-based trajectory generation。
Driving-System Model Identification
- time-delay
τ
\tau
τ
Over 100 experimental trials, we measure the time between the commanded step input and the first non-zero measured tread speed to estimate delay τ = 0.049 s \tau =0.049s τ=0.049s. - acceleration limits
a
m
a
x
a_{max}
amax
Examining the initial response of the tread velocity to a step input, we find a linear response indicating an acceleration limit of approximately a m a x = 5.5 m / s 2 a_{max} = 5.5 m/s^{2} amax=5.5m/s2.
前两个参数辨识的作用其实主要是input shaping, 为了最后一个parameters identification
- parameters of the second-order model
Using the commanded input and measured output, we can optimize the parameters of the second-order model with gradient-free methods such as Nelder-Mead given reasonable initial conditions.
Kinematic Slip Model Identification
首先是数据收集。
To ensure that the sample trajectories adequately represent all possible control inputs, we employ a trapezoidal profile to specify both the linear velocity and the curvature of the input.
Given the large set of trajectory data with measured tread speeds and accompanying ground truth positions, we start to learn parameters and compare the performance of the different kinematic motion models. (这里有个问题,我们都已经有driving system model了,为什么还要measure tread speeds?)