基于运动学模型的无人机模型预测控制(MPC)-1_无人机运动路线公式(1)

既有适合小白学习的零基础资料，也有适合3年以上经验的小伙伴深入学习提升的进阶课程，涵盖了95%以上C C++开发知识点，真正体系化！

由于文件比较多，这里只是将部分目录截图出来，全套包含大厂面经、学习笔记、源码讲义、实战项目、大纲路线、讲解视频，并且后续会持续更新

如果你需要这些资料，可以戳这里获取

˙

r

=

v

x

r

v

x

r

˙

=

u

x

r

y

r

=

v

y

r

v

y

r

˙

=

u

y

r

\left{ \begin{aligned} \dot x_r & = v_{x_r} \qquad \dot{v_{x_r}}=u_{x_r}\ y_r & = v_{y_r} \qquad \dot{v_{y_r}}=u_{y_r} \ \end{aligned} \right.

{x˙r​yr​​=vxr​​vxr​​˙​=uxr​​=vyr​​vyr​​˙​=uyr​​​
令误差向量为：

{

x

~

=

x

−

x

r

y

~

=

y

−

y

r

v

~

x

=

v

x

−

v

x

r

v

~

y

=

v

y

−

v

y

r

\left{ \begin{aligned} \widetilde{x}&=x-x_r \qquad\widetilde{y}=y-y_r\ \widetilde{v}_x&=v_x-v_{x_r} \qquad \widetilde{v}_y=v_y-v_{y_r}\ \end{aligned} \right.

{x

v

x​​=x−xr​y

​=y−yr​=vx​−vxr​​v

y​=vy​−vyr​​​
其中

n

x

：

状

态

变

量

量

个

数

，

n

u

:

控

制

变

量

个

数

，

n

m

:

输

出

变

量

个

数

n_x：状态变量量个数，n_u:控制变量个数，n_m:输出变量个数

nx​：状态变量量个数，nu​:控制变量个数，nm​:输出变量个数，我们得到如下状态空间：

[

x

~

˙

v

~

˙

x

y

~

˙

v

~

˙

y

]

=

[

0

1

0

0

0

0

0

0

0

0

0

1

0

0

0

0

]

[

x

~

v

~

x

y

~

v

~

y

]

[

0

0

1

0

0

0

0

1

]

[

u

~

x

u

~

y

]

\begin{bmatrix} \dot{\widetilde{x}}\ \dot{\widetilde{v}}_x \ \dot{\widetilde{y}}\ \dot{\widetilde{v}}_y \end{bmatrix}= \begin{bmatrix} 0 & 1 & 0 & 0\ 0 & 0 & 0 & 0\ 0 & 0 & 0 & 1\ 0 & 0 & 0 & 0\ \end{bmatrix} \begin{bmatrix} \widetilde{x}\ \widetilde{v}_x \ \widetilde{y}\ \widetilde{v}_y \end{bmatrix}+ \begin{bmatrix} 0 & 0\ 1 & 0 \ 0 & 0\ 0 & 1\ \end{bmatrix} \begin{bmatrix} \widetilde{u}_x \ \widetilde{u}_y\ \end{bmatrix}

⎣⎢⎢⎡​x

˙v

˙x​y

​˙​v

˙y​​⎦⎥⎥⎤​=⎣⎢⎢⎡​0000​1000​0000​0010​⎦⎥⎥⎤​⎣⎢⎢⎡​x

v

x​y

​v

y​​⎦⎥⎥⎤​+⎣⎢⎢⎡​0100​0001​⎦⎥⎥⎤​[u

x​u

y​​]
其中

A

=

[

0

1

0

0

0

0

0

0

0

0

0

1

0

0

0

0

]

B

=

[

0

0

1

0

0

0

0

1

]

x

(

k

)

=

[

x

~

v

~

x

y

~

v

~

y

]

u

(

k

)

=

[

u

~

x

u

~

y

]

A = \begin{bmatrix} 0 & 1 & 0 & 0\ 0 & 0 & 0 & 0\ 0 & 0 & 0 & 1\ 0 & 0 & 0 & 0\ \end{bmatrix} \quad B = \begin{bmatrix} 0 & 0\ 1 & 0 \ 0 & 0\ 0 & 1\ \end{bmatrix} \quad x(k)=\begin{bmatrix} \widetilde{x}\ \widetilde{v}_x \ \widetilde{y}\ \widetilde{v}_y \end{bmatrix} \quad u(k)=\begin{bmatrix} \widetilde{u}_x \ \widetilde{u}_y\ \end{bmatrix} \quad

A=⎣⎢⎢⎡​0000​1000​0000​0010​⎦⎥⎥⎤​B=⎣⎢⎢⎡​0100​0001​⎦⎥⎥⎤​x(k)=⎣⎢⎢⎡​x

v

x​y

​v

y​​⎦⎥⎥⎤​u(k)=[u

x​u

y​​]
将上述模型离散化，我们得到：

A

k

=

A

∗

Δ

t

I

B

k

=

B

∗

Δ

t

\begin{aligned} &A_k=A*\Delta t+I\ &B_k=B*\Delta t \end{aligned}

​Ak​=A∗Δt+IBk​=B∗Δt​
即我们得到系统方程：

x

(

k

1

)

=

A

k

∗

x

(

k

)

B

k

u

(

k

)

x

(

k

1

)

∈

n

x

×

1

A

k

∈

n

x

×

n

x

B

k

∈

n

x

×

n

u

\begin{aligned} &x(k+1)=A_k*x(k)+B_ku(k)\ &x(k+1)\in{n_{x}\times1}\quad A_k\in{n_{x}\times n_{x}}\quad B_k\in{n_{x}\times n_{u}} \end{aligned}

​x(k+1)=Ak​∗x(k)+Bk​u(k)x(k+1)∈nx​×1Ak​∈nx​×nx​Bk​∈nx​×nu​​

递推公式推导：

{

x

(

k

i

1

∣

k

i

)

=

A

k

x

(

k

i

)

B

k

u

(

k

i

)

x

(

k

i

2

∣

k

i

)

=

A

k

2

x

(

k

i

)

A

k

B

k

u

(

k

i

)

B

k

u

(

k

i

1

)

x

(

k

i

3

∣

k

i

)

=

A

k

3

x

(

k

i

)

A

k

2

B

k

u

(

k

i

)

A

k

B

k

u

(

k

i

1

)

B

k

u

(

k

i

2

)

⋮

x

(

k

i

N

p

∣

k

i

)

=

A

k

N

p

x

(

k

i

)

A

k

N

p

−

1

B

k

u

(

k

i

)

A

k

N

p

−

2

B

k

u

(

k

i

1

)

⋯

A

k

N

p

−

N

c

B

k

u

(

k

i

N

c

−

1

)

\left{ \begin{aligned} &x(k_i+1|k_i)=A_kx(k_i)+B_ku(k_i)\ &x(k_i+2|k_i)=A_k^{2}x(k_i)+A_kB_ku(k_i)+B_ku(k_i+1)\ &x(k_i+3|k_i)=A_k^{{3}x(k_i)+A_k}{2}B_ku(k_i)+A_kB_ku(k_i+1)+B_ku(k_i+2)\ &\qquad\vdots\ &x(k_i+N_p|k_i)=A_k^{{N_p}x(k_i)+A_k}{N_p-1}B_ku(k_i)+A_k^{N_p-2}B_ku(k_i+1)+\cdots +A_k^{N_p-N_c}B_ku(k_i+N_c-1)\ \end{aligned} \right.

⎩⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎧​​x(ki​+1∣ki​)=Ak​x(ki​)+Bk​u(ki​)x(ki​+2∣ki​)=Ak2​x(ki​)+Ak​Bk​u(ki​)+Bk​u(ki​+1)x(ki​+3∣ki​)=Ak3​x(ki​)+Ak2​Bk​u(ki​)+Ak​Bk​u(ki​+1)+Bk​u(ki​+2)⋮x(ki​+Np​∣ki​)=AkNp​​x(ki​)+AkNp​−1​Bk​u(ki​)+AkNp​−2​Bk​u(ki​+1)+⋯+AkNp​−Nc​​Bk​u(ki​+Nc​−1)​
令

Δ

X

=

[

x

(

k

i

1

∣

k

i

)

x

(

k

i

2

∣

k

i

)

x

(

k

i

3

∣

k

i

)

⋯

x

(

k

i

N

p

∣

k

i

)

]

(

N

p

∗

n

x

)

×

1

T

Δ

U

=

[

u

(

k

i

)

u

(

k

i

1

)

⋯

u

(

k

i

N

u

)

]

(

N

c

∗

n

u

)

×

1

网上学习资料一大堆，但如果学到的知识不成体系，遇到问题时只是浅尝辄止，不再深入研究，那么很难做到真正的技术提升。

需要这份系统化的资料的朋友，可以添加戳这里获取

一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人，都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！

A

k

N

p

−

2

B

k

u

(

k

i

1

)

⋯

A

k

N

p

−

N

c

B

k

u

(

k

i

N

c

−

1

)

\left{ \begin{aligned} &x(k_i+1|k_i)=A_kx(k_i)+B_ku(k_i)\ &x(k_i+2|k_i)=A_k^{2}x(k_i)+A_kB_ku(k_i)+B_ku(k_i+1)\ &x(k_i+3|k_i)=A_k^{{3}x(k_i)+A_k}{2}B_ku(k_i)+A_kB_ku(k_i+1)+B_ku(k_i+2)\ &\qquad\vdots\ &x(k_i+N_p|k_i)=A_k^{{N_p}x(k_i)+A_k}{N_p-1}B_ku(k_i)+A_k^{N_p-2}B_ku(k_i+1)+\cdots +A_k^{N_p-N_c}B_ku(k_i+N_c-1)\ \end{aligned} \right.

⎩⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎧​​x(ki​+1∣ki​)=Ak​x(ki​)+Bk​u(ki​)x(ki​+2∣ki​)=Ak2​x(ki​)+Ak​Bk​u(ki​)+Bk​u(ki​+1)x(ki​+3∣ki​)=Ak3​x(ki​)+Ak2​Bk​u(ki​)+Ak​Bk​u(ki​+1)+Bk​u(ki​+2)⋮x(ki​+Np​∣ki​)=AkNp​​x(ki​)+AkNp​−1​Bk​u(ki​)+AkNp​−2​Bk​u(ki​+1)+⋯+AkNp​−Nc​​Bk​u(ki​+Nc​−1)​
令

Δ

X

=

[

x

(

k

i

1

∣

k

i

)

x

(

k

i

2

∣

k

i

)

x

(

k

i

3

∣

k

i

)

⋯

x

(

k

i

N

p

∣

k

i

)

]

(

N

p

∗

n

x

)

×

1

T

Δ

U

=

[

u

(

k

i

)

u

(

k

i

1

)

⋯

u

(

k

i

N

u

)

]

(

N

c

∗

n

u

)

×

1

[外链图片转存中…(img-4GJ55VU4-1715803093224)]
[外链图片转存中…(img-Q3dauha9-1715803093225)]

网上学习资料一大堆，但如果学到的知识不成体系，遇到问题时只是浅尝辄止，不再深入研究，那么很难做到真正的技术提升。

需要这份系统化的资料的朋友，可以添加戳这里获取

一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人，都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！