[5](含源码)基于非线性模型预测控制(NMPC)的四旋翼轨迹跟踪

学术圈外野生菌

分类：建模仿真

发布时间 2021.11.09阅读数 5238 评论数 0

最近有小白反映上一道菜内容过于丰富，吃撑了，本菜鸡今天给大家做一道开胃小菜，下面开始表演。

主料 -- 无人机动力学模型，NMPC模型

辅料 -- Quadrotor场景文件

炊具 -- V-REP，CasADi，MATLAB

1. 无人机动力学模型

直接上图，就是这么简单粗暴

可能有些小白还是一脸懵逼。简而言之就是，我们把无人机(确切滴说是四旋翼类型的)看做一个刚体，有四个旋翼提供力和力矩，控制无人机的位姿和速度。回顾刚体的运动学与动力学，我们知道描述刚体运动一种方式就是在刚体上任意点(可以是刚体所占据空间内的，也可以是刚体以外空间的)固联一个坐标系

$O_{B}-X_{B}Y_{B}Z_{B}$ (一般放在质心，本菜在这里也放在质心，有些情形放在其他位置更好，比如多刚体系统放在刚体连接处)，描述这个坐标系相对于某参考坐标系

$O_O-XYZ$ 的位姿(位置和姿态)，速度(线速度，角速度以及姿态参数变化)。

平动部分运动学很简单，没啥说的。

转动部分运动学的推导需要先弄清楚以下几个东东：

$^B\omega_{B}$ 与

$^{O}\omega_B$ 的关系

先引入一个符号 [·] X表示把一个三维向量变成反对称阵的形式，也就是叉乘映射的矩阵表示。根据旋转矩阵求导的定义有，

$^O\dot{R}_B=\left[ ^O\omega_{B} \right]\times{^OR_B}=^OR_B\left[ ^B\omega_{B} \right]\times$ ，即

$\left[ ^B\omega_{B} \right]\times={^OR_B^{T}}\left[^O \omega_{B} \right]\times{^OR_B}\\$

这个式子的意义其实就是同一线性映射(这里是叉乘)在不同基的变换。直观一点看，把这个等式左右两边右乘一个向量在

$O_{B}-X_{B}Y_{B}Z_{B}$ 坐标系的坐标，那么左边得到叉乘后的向量在

$O_{B}-X_{B}Y_{B}Z_{B}$ 坐标系的坐标；右边的话，先是一个

$^OR_B$ 作用得到在

$O_O-XYZ$ 坐标系的坐标，然后得到叉乘后向量在

$O_O-XYZ$ 坐标系的坐标，最后再用

$^OR^{T}_B$ 拉回到

$O_{B}-X_{B}Y_{B}Z_{B}$ 坐标系。同时，我们也得到

$\left[ ^O\omega_{B} \right]\times={^OR_B}\left[ ^B\omega_{B} \right]\times{^OR_B^{T}}$

关于绕坐标轴旋转矩阵的导数

进一步滴，我们得到，当角速度和 R 矩阵的等效转轴共线，那么这两个角速度的表示是一样的。也就是说，对于绕坐标轴的基本旋转，我们可以有如下一组等式(记转角为 α ，三阶单位矩阵 I3X3 的三列分别为

$e_{x},e_{y},e_{z}$ ):

$\begin{align} \dot{R}_{x}R_{x}^{T}=R_{x}^{T}\dot{R}_{x}=\dot{\alpha}\left[e_{x}\right]\times\\ \dot{R}_{y}R_{y}^{T}=R_{y}^{T}\dot{R}_{y}=\dot{\alpha}\left[ e_{y} \right]\times\\ \dot{R}_{z}R_{z}^{T}=R_{z}^{T}\dot{R}_{z}=\dot{\alpha}\left[ e_{z} \right]\times \end{align}\\$

一个有用等式

其实这个等式不需要什么高深的知识，就是同一个向量在不同坐标系的变换而已。考虑

$^O\omega_{B}$ 和

$^{B}\omega_B$ 是坐标系

$O_{B}-X_{B}Y_{B}Z_{B}$ 相对于

$O_O-XYZ$ 坐标系转动的角速度向量在不同坐标系的表示而已，直接有

$^B\omega_{B}={^OR_B^{T}}{^O\omega_{B}}$ 和

$^O\omega_{B}={^OR_B}{^B\omega_{B}}$ ，然后把这俩式子同时写成反对称矩阵形式，有

$\left[ ^B\omega_{B} \right]\times=\left[ {^OR_B^{T}}{^O\omega_{B}} \right]\times$ 和

$\left[ {^O\omega_{B}} \right]\times=\left[ {^OR_B}{^B\omega_{B}}\right]\times$ ，再联系前面，我们有：

$\begin{align} \left[{^B\omega_{B}}\right]\times={^OR^{T}_B}{^O\dot{R}_B}=\left[{^OR^{T}_B}{^O\omega_{B}} \right]\times={^OR^{T}_B}\left[ {^O\omega_{B}} \right]\times{^OR_B} \\ \left[{^O\omega_{B}}\right]\times={^O\dot{R}_B}{^OR^{T}_B}=\left[ {^OR_B}{^B\omega_{B}} \right]\times={^OR_B}\left[ {^B\omega_{B}} \right]\times{^OR^{T}_B} \end{align}\\$

现在可以开始推导了。

用欧拉角参数化旋转矩阵

$^OR_B=R_{z}\left( \psi \right)R_{y}\left( \theta \right)R_{x}\left( \phi \right) \\$

将参数化的旋转矩阵及其导数带入旋转矩阵定义式

$\left[ {^B\omega_{B}} \right]\times={^OR^{T}_B}{^O\dot{R}_B}=R_{x}^{T}\left( \phi \right)R_{y}^{T}\left( \theta \right)R_{z}^{T}\left( \psi \right)\left( \dot{R}_{z}\left( \psi \right) R_{y}\left( \theta \right)R_{x}\left( \phi \right)+R_{z}\left( \psi \right)\dot{R}_{y}\left( \theta \right)R_{x}\left( \phi \right)+R_{z}\left( \psi \right)R_{y}\left( \theta \right)\dot{R}_{x}\left( \phi \right)\right) \\$

乘进去，有

$\left[ {^B\omega_{B}} \right]\times=R_{x}^{T}\left( \phi \right)R_{y}^{T}\left( \theta \right)R_{z}^{T}\left( \psi \right) \dot{R}_{z}\left( \psi \right)R_{y}\left( \theta \right)R_{x}\left( \phi \right)+R_{x}^{T}\left( \phi \right)R_{y}^{T}\left( \theta \right)\dot{R}_{y}\left( \theta \right)R_{x}\left( \phi \right)+R_{x}^{T}\dot{R}_{x}\left( \phi \right) \\$

带入绕坐标轴旋转矩阵的导数，有

$\left[ {^B\omega_{B}} \right]\times=R_{x}^{T}\left( \phi \right)R_{y}^{T}\left( \theta \right)\dot{\psi}\left[ e_{z} \right]\times{R}_{y}\left( \theta \right)R_{x}\left( \phi \right)+R_{x}^{T}\left( \phi \right)\dot{\theta}\left[ e_{y} \right]\times{R}_{x}\left( \phi \right)+\dot{\phi}\left[ e_{x} \right]\times \\$

带入一个有用等式，有

$\left[ {^B\omega_{B}} \right]\times=\left[{R}_{x}^{T}\left( \phi \right)R_{y}^{T}\left( \theta \right)\dot{\psi}{e}_{z} \right]\times+\left[ R_{x}^{T}\left( \phi \right)\dot{\theta}e_{y} \right]\times+\left[\dot{\phi}e_{x} \right]\times \\$

两边一起脱贫摘帽，即得到角速度与欧拉角参数变化率之间的关系

${^B\omega_{B}}=\left( e_{x}, R_{x}^{T}\left( \phi \right)e_{y},{R}_{x}^{T}\left( \phi \right)R_{y}^{T}\left( \theta \right){e}_{z} \right)\left( \dot{\phi},\dot{\theta},\dot{\psi}\right)^{T} \\$

简记为 ${^B\omega_{B}}=W\left( \phi,\theta,\psi \right)\left(\dot{\phi},\dot{\theta},\dot{\psi}\right)^{T} \\$

话说，有些小白是不是纳闷为啥用

$\omega_{B}$ 而不用

$\omega_{O}$ 。那是因为我们要用Lagrangian 方法推导动力学，而转动惯量在机体坐标系是常数矩阵，便于转动动能的表示。

动力学推导

系统广义坐标与广义速度分别为

$q=\left( x,y,z,\phi,\theta,\psi\right)^{T} \\$ $\dot{q}=\left( \dot{x},\dot{y},\dot{z},\dot{\phi},\dot{\theta},\dot{\psi}\right)^{T} \\$

系统状态变量为12维向量

$X=\left( q^{T},\dot{q}^{T} \right)^{T} \\$

控制输入为4维向量，分量为各个旋翼的角速度平方

$U=\left( u_{1},u_{2},u_{3},u_{4} \right)^{T} \\$

旋翼的推进力和力矩系数分别为 k,b ，旋翼转轴距离机体坐标系原点距离为 l ，则机体所受到的合力和合力矩在坐标系

$O_{B}-X_{B}Y_{B}Z_{B}$ 内表示为

$^Bf_{B}=\left( 0,0,k\sum_{i=1}^{4}{u}_{i} \right)^{T} \\$

$^B\tau_{B}=\left( kl\left( -u_{2}+ u_{4}\right),kl\left( -u_{1}+ u_{3}\right), b\sum_{i=1}^{4}{u_{i}}\right)^{T} \\$

系统的Lagrangian

$L=\frac{1}{2}\left( \dot{x},\dot{y},\dot{z} \right)M\left( \dot{x},\dot{y},\dot{z} \right)^{T}+\frac{1}{2}\omega_{B}^{T}I\omega_{B}-mgz \\$

将如下各式带入 L

$M=diag(\left[ m,m,m \right]) \\$

${^B\omega_{B}}=W\left( \phi,\theta,\psi \right)\left( \dot{\phi},\dot{\theta},\dot{\psi} \right)^{T} \\$

$^Bf_B=\left( 0,0,k\sum_{i=1}^{4}{u}_{i} \right)^{T} \\$

$^Of_{B}={^OR_B}{^Bf_{B}} \\$

$^OR_B=R_{z}\left( \psi \right)R_{y}\left( \theta \right)R_{x}\left( \phi \right) \\$

$^B\tau_{B}=\left( kl\left( -u_{2}+ u_{4}\right),kl\left( -u_{1}+ u_{3}\right), b\sum_{i=1}^{4}{u_{i}}\right)^{T} \\$

根据

$\frac{d}{dt}\left( \frac{\partial{L}}{\partial\left( {dq/dt} \right)} \right)-\frac{\partial{L}}{\partial{q}}=\left(^O f_{B} ,^B\tau_{B}\right)^{T}$ 即可得动力学方程

$\frac{dX}{dt}=f\left( X ,U\right) \\$

2. NMPC模型

这个例程套用标准形式(二次型损失函数，动力学约束，控制与状态约束)。在编写代码时，用CasADi帮助实现符号计算与自动微分，得到1中动力学模型与损失函数的符号表示，然后用CasADi做好的非线性优化器调用ipopt进行NLP求解，具体的可以看NMPC_problem_formulation.m文件。

CasADi链接

CasADiweb.casadi.org/

这里要说一说CasADi求jacobian的一个坑。

首先记 $J\left( \phi,\theta,\psi \right)=W^{T}\left( \phi,\theta,\psi \right)IW\left( \phi,\theta,\psi \right) \\$

如果你去求解1中的动力学方程，你会遇到求

$\frac{dJ\left( \phi,\theta,\psi \right)}{dt}$ 的问题。如果你用MATLAB的符号计算，可以在定义符号变量的直接采用如下定义：

syms phi(t) theta(t) psi(t)

这样接着构建

$J\left( \phi,\theta,\psi \right)$ 之后，可直接对t求导数，一步到位。然而CasADi不支持这样定义，所以你只能用链式法则求，在此过程中你得特别注意矩阵对于向量求导的结果排列顺序，这一点上MATLAB和CasADi是相同的，即把矩阵按照列的顺序扯成一个列向量，然后结果矩阵的每一行就是这个大向量的每个元素对于自变量的每一个分量的偏导数。

我们的控制目标是实现轨迹跟踪，系统的输出就是状态变量的前六个。目标轨迹我们给的是位置走8字形，欧拉角一直为零，详见QuadrotorReferenceTrajectory.m文件。如果想在V-REP中显示期望路径点以便于实际运行的结果对比，可以利用QuadrotorReferenceTrajectory.m文件产生对应时间序列的期望点，然后抽取出位置值部分，保存成CSV文件，通过V-REP中的File->Import->Path from CSV加载显示。

3. 制作场景文件

菜鸡不知道怎么在V-REP实现流体仿真，所以这里只能调用sim.addForceAndTorque函数直接给无人机施加力和力矩了。无人机模型的构造极其简单，添加一个圆盘作为机身，然后添加四个力传感器(与旋翼转轴重合)，与机身固联，接着再添加四个圆盘作为旋翼，与力传感器固联。把这五个圆盘的动力学参数设定好(这里需要等效质量与惯量的计算)之后，隐藏。最后添加可视化模型(可以从V-REP的模型库中拷贝)，成品大概这样：

4. 运行

做初始欧拉角为零和不为零的仿真。

初始姿态无误差

初始姿态有误差

结果见原文：https://zhuanlan.zhihu.com/p/144823874

----------------------------------------------------

本例程源文件链接(切记只可用于学习，科研等非商业用途)

xuhuairuogu/V-REP-Simulation-Projectsgithub.com/xuhuairuogu/V-REP-Simulation-Projects/tree/master/NMPC_CasADi_Quadrotor_trajectory_tracking

-----------------------------------------------------

做人呐，最重要的就是开心喽。能帮到别人是最大的快乐，希望本菜鸡能对小白们有所帮助，希望大佬们的批评能帮助本菜鸡成长！

建模仿真四旋翼运动控制 vrep 非线性控制

打赏 0

上一篇：[4](含源码)基于图像的视觉阻抗控制在一双臂空中机械臂上的实现

下一篇：[7](含源码)阻抗控制在平面二连杆机械臂上的实现

[5](含源码)基于非线性模型预测控制(NMPC)的四旋翼轨迹跟踪

学术圈外野生菌

1. 无人机动力学模型

2. NMPC模型

3. 制作场景文件

4. 运行

为你推荐

Gazebo中Turtlebot3激光雷达最远距离调整后测距信息不正常问题解决方法

全向轮运动学与V-rep中全向移动机器人仿真

HOG算法中用到三线性插值（Trilinear Interpolation）的作用及其用法（包括HOG的matlab源代码分析）

Solidworks导出URDF模型

MATLAB/simulink中PID Tuner工具箱使用方法（针对Plant cannot be linearized问题）

系统辨识专题（三）——多旋翼飞行器模型参数辨识问题

关于作者

学术圈外野生菌

8

0

2

1

[7](含源码)阻抗控制在平面二连杆机械臂上的实现

[1](含源码)通过关节力矩指令控制LBR/iiwa机械臂运动

[5](含源码)基于非线性模型预测控制(NMPC)的四旋翼轨迹跟踪

相关推荐

ADAMS并联机器人动力学仿真【附源文件】

23. 卡尔曼滤波器详解——从零开始(3) Kalman Filter from Zero

【自动驾驶】运动规划丨轨迹规划丨Bspline

Solidworks机构运动仿真

从零开始的分析力学基础 2.质点动力学(1)

VINS-Mono源码解读(三)：视觉惯性联合初始化

热门泡泡

30积分失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

[5](含源码)基于非线性模型预测控制(NMPC)的四旋翼轨迹跟踪

学术圈外野生菌

1. 无人机动力学模型

2. NMPC模型

3. 制作场景文件

4. 运行

为你推荐

Gazebo中Turtlebot3激光雷达最远距离调整后测距信息不正常问题解决方法

全向轮运动学与V-rep中全向移动机器人仿真

HOG算法中用到三线性插值（Trilinear Interpolation）的作用及其用法（包括HOG的matlab源代码分析）

Solidworks导出URDF模型

MATLAB/simulink中PID Tuner工具箱使用方法（针对Plant cannot be linearized问题）

系统辨识专题（三）——多旋翼飞行器模型参数辨识问题

评论（0）

关于作者

学术圈外野生菌

8

0

2

1

[7](含源码)阻抗控制在平面二连杆机械臂上的实现

[1](含源码)通过关节力矩指令控制LBR/iiwa机械臂运动

[5](含源码)基于非线性模型预测控制(NMPC)的四旋翼轨迹跟踪

相关推荐

ADAMS并联机器人动力学仿真【附源文件】

23. 卡尔曼滤波器详解——从零开始(3) Kalman Filter from Zero

【自动驾驶】运动规划丨轨迹规划丨Bspline

Solidworks机构运动仿真

从零开始的分析力学基础 2.质点动力学(1)

VINS-Mono源码解读(三)：视觉惯性联合初始化

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板