FuShenxiao的个人空间动态-电子工程世界

FuShenxiao

动态发布点评好友关于

2025-04-03
回复了主题帖： NUCLEO-C092RC开发板测评（三）——ADC测试

秦天qintian0303 发表于 2025-4-3 09:08 C0系列的ADC是多少位的？应该也是12位的吧对，12位的，C0的ADC只有一路，但是通道数还挺多的，这个价格也是很不错的了
2025-04-02
发表了主题帖： NUCLEO-C092RC开发板测评（三）——ADC测试

本帖最后由 FuShenxiao 于 2025-4-2 17:12 编辑该测试通过读取ADC引脚的值，并使用串口将电压值输出到串口工具。右侧倒数第三个排针，对应引脚PB10，有ADC输入的功能在CubeMX中设置PB10为ADC输入，并配置如下为了实现串口输出电压值，还需初始化虚拟串口生成程序后，首先进行printf重定向（这个重定向方法用的ST官方给出的代码，这种方法不用在头文件加入#include <stdio.h>） /* USER CODE BEGIN PFP */ #if defined(__ICCARM__) __ATTRIBUTES size_t __write(int, const unsigned char *, size_t); #endif /* __ICCARM__ */ #if defined(__ICCARM__) /* New definition from EWARM V9, compatible with EWARM8 */ int iar_fputc(int ch); #define PUTCHAR_PROTOTYPE int iar_fputc(int ch) #elif defined ( __CC_ARM ) || defined(__ARMCC_VERSION) /* ARM Compiler 5/6*/ #define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f) #elif defined(__GNUC__) #define PUTCHAR_PROTOTYPE int __io_putchar(int ch) #endif /* __ICCARM__ */ /* USER CODE END PFP */ /* USER CODE BEGIN 4 */ /** * [url=home.php?mod=space&uid=159083]@brief[/url] Retargets the C library printf function to the USART. * @param None * @retval None */ PUTCHAR_PROTOTYPE { /* Place your implementation of fputc here */ /* e.g. write a character to the USART2 and Loop until the end of transmission */ HAL_UART_Transmit(&huart2, (uint8_t *)&ch, 1, 0xFFFF); return ch; } #if defined(__ICCARM__) size_t __write(int file, unsigned char const *ptr, size_t len) { size_t idx; unsigned char const *pdata = ptr; for (idx = 0; idx < len; idx++) { iar_fputc((int)*pdata); pdata++; } return len; } #endif /* __ICCARM__ */ /* USER CODE END 4 */ 接着定义获取adc值代码 /* USER CODE BEGIN 0 */ uint16_t adc_get(void) { uint16_t adc = 0; HAL_ADC_Start(&hadc1); adc = HAL_ADC_GetValue(&hadc1); return adc; } /* USER CODE END 0 */ 在主函数中，不断获取adc值并将其处理后的真实值打印在串口工具上 int main(void) { /* USER CODE BEGIN 1 */ /* USER CODE END 1 */ /* MCU Configuration--------------------------------------------------------*/ /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ HAL_Init(); /* USER CODE BEGIN Init */ /* USER CODE END Init */ /* Configure the system clock */ SystemClock_Config(); /* USER CODE BEGIN SysInit */ /* USER CODE END SysInit */ /* Initialize all configured peripherals */ MX_GPIO_Init(); MX_ADC1_Init(); MX_USART2_UART_Init(); /* USER CODE BEGIN 2 */ /* USER CODE END 2 */ /* Infinite loop */ /* USER CODE BEGIN WHILE */ float adc_value = 0; while (1) { adc_value = 3.3f * adc_get() / 4095; printf("Voltage:%.2fV\r\n", adc_value); HAL_GPIO_TogglePin(LD1_GPIO_Port, LD1_Pin); HAL_Delay(1000); /* USER CODE END WHILE */ /* USER CODE BEGIN 3 */ } /* USER CODE END 3 */ } 在STM32CubeIDE中，由于输出的值为浮点数，因此还需要勾选浮点数输出的选项按照如下图所示的方式接线，并通过调节三合一万用表输出电压观察获取到的adc值得到最终输出结果如下工程代码如下：
发表了主题帖： NUCLEO-C092RC开发板测评（二）——PWM测试

观察开发板原理图以及数据手册，可以看到用户LED灯连接的PA5和PC9引脚均有PWM输出功能。分别选取PA5为TIM2_CH1，PC9为TIM3_CH4，在CubeMX中配置如下在程序中，首先开启PWM的时钟中断 HAL_TIM_PWM_Start_IT(&htim2, TIM_CHANNEL_1); HAL_TIM_PWM_Start_IT(&htim3, TIM_CHANNEL_4); 引入PWM配置结构体 TIM_OC_InitTypeDef sConfigOC = {0}; 在主函数中，通过不断调节占空比实现两盏不同频率的LED呼吸灯 uint16_t cnt1 = 0; uint16_t cnt2 = 0; uint8_t flag1 = 1; uint8_t flag2 = 1; while (1) { if(flag1 == 1) { cnt1 += 10; if(cnt1 == 1000) flag1 = 0; } else { cnt1 -= 10; if(cnt1 == 0) flag1 = 1; } if(flag2 == 1) { cnt2 += 20; if(cnt2 == 1000) flag2 = 0; } else { cnt2 -= 20; if(cnt2 == 0) flag2 = 1; } sConfigOC.OCMode = TIM_OCMODE_PWM1; sConfigOC.Pulse = cnt1; sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_1); sConfigOC.Pulse = cnt2; HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_4); HAL_Delay(20); /* USER CODE END WHILE */ /* USER CODE BEGIN 3 */ } 得到效果如下： [localvideo]01da1e243e0c64965b8b4c11e776c5a0[/localvideo] 代码实现如下：
2025-04-01
发表了主题帖： NUCLEO-C092RC开发板测评（一）——GPIO测试

本帖最后由 FuShenxiao 于 2025-4-1 09:53 编辑感谢EEWORLD提供的NUCLEO-C092RC开发板的测评机会。STM32C0系列的芯片经济实惠并且性能并不差，还搭载了FDCAN、USB这样的通信接口，适合低成本的工业开发。拿到开发板，首先进行点灯测试。在开发板接口上，有一个按键和两个用户LED可供GPIO测试。注意用户LED灯电路，LD1点亮条件为PA5高电平，LD2点亮条件为PC9低电平。开始在CubeMX中对引脚进行配置观察原理图，发现LSE和HSE都有外接晶振在RCC的时钟信号中选取晶振时钟树默认设置设置DEBUG为Serial Wire，这样可以配置SWDIO和SWCLK 最后设置GPIO，得到配置结果如下图所示程序部分，首先设置按键输入并消抖代码 uint8_t KeyScan(void) { if(HAL_GPIO_ReadPin(BUTTON_GPIO_Port, BUTTON_Pin) == GPIO_PIN_RESET) { HAL_Delay(10); if(HAL_GPIO_ReadPin(BUTTON_GPIO_Port, BUTTON_Pin) == GPIO_PIN_RESET) { while(HAL_GPIO_ReadPin(BUTTON_GPIO_Port, BUTTON_Pin) == GPIO_PIN_RESET); return 1; } } return 0; } 在主程序中首先设置相关变量参数 uint8_t key = 0; uint8_t cnt = 0; 在主循环中，不断读取按键值并控制用户灯，让两盏灯以二进制的方式点亮 while (1) { key = KeyScan(); if(key == 1) cnt++; cnt = cnt % 4; HAL_GPIO_WritePin(LD1_GPIO_Port, LD1_Pin, cnt & 0x01); HAL_GPIO_WritePin(LD2_GPIO_Port, LD2_Pin, !(cnt & 0x02)); /* USER CODE END WHILE */ /* USER CODE BEGIN 3 */ } 得到最终效果如下： [localvideo]9442574da767611daa7b20280075ffe6[/localvideo] 工程代码如下：
2025-03-30
发表了主题帖：【嵌入式Rust修炼营】初级任务3：单片机串口下载

我使用的操作系统为Ubuntu20.04，因此开发环境的搭建可以参照https://hysonglet.github.io/rust-embedded-start/environment/chapter.html复制相关指令轻易完成。使用如下指令下载开发板相关的参考代码。 git clone https://github.com/hysonglet/py32f030-hal.git 将开发板通过typeA转typeC线连接到电脑的USB接口，并使用pyisp -p指令查看开发板所接到的USB接口。以烧录embassy_blinky.rs为例，该程序代码如下 #![no_std] #![no_main] use embassy_executor::Spawner; use embassy_time::Timer; use py32f030_hal::{ self as hal, gpio::{AnyPin, Output, PinIoType, Speed}, prelude::*, }; use {defmt_rtt as _, panic_probe as _}; #[embassy_executor::task(pool_size = 2)] async fn run_led(led: AnyPin, delay_ms: u64) { let mut led = Output::new(led, PinIoType::PullDown, Speed::Low); loop { let _ = led.toggle(); Timer::after_millis(delay_ms).await; } } #[embassy_executor::main] async fn main(spawner: Spawner) { let p = hal::init(Default::default()); defmt::info!("Testing the flashing of different LEDs in multi-tasking."); let gpioa = p.GPIOA.split(); // spawner.must_spawn(run_led(gpioa.PA9.degrade(), 1000)); // spawner.must_spawn(run_led(gpioa.PA10.degrade(), 2000)); spawner.spawn(run_led(gpioa.PA9.degrade(), 1000)).unwrap(); spawner.spawn(run_led(gpioa.PA10.degrade(), 500)).unwrap(); loop { Timer::after_secs(2).await; } } 首先编译该程序 cargo build --example embassy_blinky 接着生成二进制bin文件 cargo objcopy --example embassy_blinky -- -O binary embassy_blinky.bin 当然，在使用cargo objcopy之前还需要安装cargo-binutils和LLVM工具。使用如下代码完成安装 cargo install cargo-binutils rustup component add llvm-tools 最后使用如下指令完成程序烧录烧录程序时对按键有如下操作：1. 按下boot键； 2. 按下reset键（保持boot键按下状态）保持1s以上； 3. 释放reset键； 4. 开始下载后释放boot键 pyisp -s /dev/ttyUSB0 -g -c -f embassy_blinky.bin 下载结果如下最终效果展示： [localvideo]a6a9232b9e02d55502daf1468a730474[/localvideo]
发表了主题帖：【嵌入式Rust修炼营】初级任务2：打印文本文件的每行数据

首先导入标准库中的文件系统和输入/输出模块 use std::{ fs::File, io::{self, BufRead}, }; 在主函数中，首先进行命令行参数处理，并将参数收集到字符串向量中 let arg = std::env::args(); let files: Vec<_> = arg.collect(); 接着创建一个迭代器来逐个处理文件 let mut iter = files.iter(); println!("exe name: {}", iter.next().unwrap()); 使用while let模式匹配处理文件 while let Some(file_name) = iter.next() { 尝试打开和读取文件，如果打开成功，则逐行打印，如果打开失败，则打印错误信息 match File::open(file_name) { Ok(file) => { let reader = io::BufReader::new(file); for line in reader.lines() { if let Ok(line) = line { println!("{}", line) } } } Err(e) => { println!("{:?}", e) } } 完整程序如下所示 use std::{ fs::File, io::{self, BufRead}, }; fn main() { let arg = std::env::args(); let files: Vec<_> = arg.collect(); println!("file count: {}", files.len()); let mut iter= files.iter(); println!("exe name: {}", iter.next().unwrap()); while let Some(file_name) = iter.next() { println!("file: {}", file_name); match File::open(file_name) { Ok(file) => { let reader = io::BufReader::new(file); for line in reader.lines() { if let Ok(line) = line { println!("{}", line) } } } Err(e) => { println!("{:?}", e) } } } } 打印main.rs 打印Cargo.toml 工程文件：
发表了主题帖：【嵌入式Rust修炼营】初级任务1：使用冒泡排序处理数组

本帖最后由 FuShenxiao 于 2025-3-30 19:22 编辑冒泡排序可以说是在学习一门新语言时最先学习的一个算法，该算法的运行方式也如同它的名字一样，通过遍历数组将最大/最小的值移动到数组的一边。在Rust中实现冒泡排序，首先定义冒泡排序函数在这个函数中，由于需要对数组arr进行操作，说明这是一个可变的切片，因此需要使用mut；此外在mut前加入引用符号，使得所有权转移到函数内部，从而可以对arr数组进行操作。 fn bubble_sort(arr: &mut [i32]) { for i in 0..arr.len() - 1 { for j in i + 1 ..arr.len() { if arr[i] > arr[j] { let tmp = arr[i]; arr[i] = arr[j]; arr[j] = tmp; } } } } 引入相关库函数和常量 use rand::random_range; const ARR_LEN:usize=20; 编写主函数如下在主函数中首先定义一个数组，并用rand库为这个数组填充随机整数，最后利用冒泡排序算法完成数组的从小到大排序。 fn main() { let mut test_arr: [i32; ARR_LEN] = [0; ARR_LEN]; for i in &mut test_arr { *i = random_range(-100..100); } println!("before sort:{:?}", test_arr); bubble_sort(&mut test_arr); println!("after sort:{:?}", test_arr); } 由于引入了rand库函数，因此需要在Cargo.toml文件中引入相关依赖 [package] name = "bubble_sort" version = "0.1.0" edition = "2024" [dependencies] rand = "0.9.0" 最后运行cargo run，得到输出结果如下工程文件：
2025-03-29
回复了主题帖：测评颁奖： NXP MCX A系列 FRDM-MCXA156开发板（含NXP官方福利礼品）

邮寄地址无误
2025-03-27
回复了主题帖：测评入围名单：支持CANFD的NUCLEO-C092RC

个人信息无误，确认可以完成测评分享计划
2025-03-12
回复了主题帖：嵌入式Rust修炼营入围名单来啦，一起度过嵌入式Rust学习时光的小伙伴集合啦~

个人信息无误
2025-03-07
回复了主题帖：毕设记录（二）——无刷直流电机的模糊PID控制

极限零发表于 2025-3-7 09:44 和我的毕设差不多，感慨啊，满满的回忆满满的泪啊我的毕设是做无人船控制的，寒假在家里没有无人船，只能用手头的东西做做算法验证了
2025-03-06
发表了主题帖：毕设记录（二）——无刷直流电机的模糊PID控制

本帖最后由 FuShenxiao 于 2025-3-6 20:55 编辑复现论文《基于模糊PID的水下航行器运动控制研究》根据论文，水下航行器的动力学模型传递函数为：建立传统PID和模糊PID控制框图如图1 图1 水下航行器控制仿真框图根据论文，传统PID中取Kp=20，Ki=0.5，Kd=15；模糊PID中取Kp=300，Ki=0.5，Kd=35。模糊控制器以误差e和误差的导数ec为输入，输出ΔKp，ΔKi，ΔKd。在MATLAB中设置模糊控制器输入输出关系如图2。图2 模糊控制器输入输出关系模糊规则采用Mamdani标准，所有输入输出参数范围的论域均取[-3, 3]，选择七级模糊子集{NB, NM, NS, ZO, PS, PM, PB}，其中N表示负，P表示正，B表示大，M表示中，S表示小，ZO表示0，模糊子集的隶属度函数均采用三角函数形式，在MATLAB中设置e的隶属度函数如图3。图3 输入量e的隶属度函数设置模糊控制规则如表1，在MATLAB中设置模糊控制规则如图4。表1 ΔKp，ΔKi，ΔKd模糊控制规则图4 模糊控制规则设置采用质心法对ΔKp，ΔKi，ΔKd进行反模糊化处理，得到反模糊化的精确值其中xi为模糊变量元素，μ(xi)为元素xi的隶属度。得到仿真结果如图5，可见在该论文中利用模糊控制有效减小了超调和振荡幅值。图5 阶跃响应曲线可以看出在对无刷电机的控制中，模糊PID的目的在于缩短上升时间，又要防止超调，在该无刷电机电机控制案例中，e的值一般为正，或者在电机平稳运行时转速在目标转速附近波动时，e变为一个较小的负数，因此模糊控制规则应该要满足以下条件：当e较大时，Kp可适当增大，提高系统快速性；当e较小时，Kp应适当减小，以防止振荡由于原系统无超调，因此与复现论文（阶跃响应有明显超调）不同的是，在e较大时，可以适当增大Ki，从而提高系统快速性（后来证明这一步是不对的，会导致系统出现振荡，而且难以逼近目标转速） Kp和Ki的增大可能会导致系统的不稳定，而在原始代码中仅采用了PI控制，后续可以尝试引入微分调节提高系统阻尼复现论文如下： MATLAB代码如下：电机模糊PID控制基于以上复现的论文，以1000rpm为目标转速，设计隶属度函数，如图6-图10。图6 e的隶属度函数图7 ec的隶属度函数图8 ΔKp的隶属度函数图9 ΔKi的隶属度函数图10 ΔKd的隶属度函数设置模糊控制规则如表2-表4。 ΔKp ec NB NM NS ZO PS PM PB e NB PB PB PM PM PS ZO ZO NM PB PB PM PS PS ZO NS NS PM PM PM PS ZO NS NS ZO PM PM PS ZO NS NM NM PS PS PS ZO NS NS NM NM PM PS ZO NS NM NM NM NB PB ZO ZO NM NM NM NB NB ΔKi ec NB NM NS ZO PS PM PB e NB NB NB NM NM NS ZO ZO NM NB NM NS NS ZO PS PS NS NM NM NS ZO PS PM PM ZO NM NS ZO PS PS PM PB PS ZO ZO PS PS PM PB PB PM ZO ZO PS PM PM PB PB PB NB NB NM NM NS ZO ZO ΔKd ec NB NM NS ZO PS PM PB e NB PS ZO ZO ZO ZO ZO PS NM ZO ZO ZO ZO ZO ZO ZO NS ZO ZO ZO ZO ZO ZO ZO ZO ZO ZO ZO ZO ZO ZO ZO PS PB PS PS PS PS PS PB PM PB PM PM PM PS PS PB PB PS ZO ZO ZO ZO ZO PS 以1000rpm为目标转速，得到利用模糊PID控制的转速曲线与经典PID控制的电机转速曲线如图11，可见利用模糊PID使系统的快速性得到提升。图11 经典PID控制和模糊PID控制电机转速随时间变化曲线 STM32G4的工程代码如下：
2025-03-05
发表了主题帖：《控制之美（卷2）——最优化控制MPC与卡尔曼滤波器》——卡尔曼滤波器

本帖最后由 FuShenxiao 于 2025-3-6 14:21 编辑卡尔曼滤波器是一种最优化的、递归的、数字处理的算法。它兼具滤波器和观测器的特性。在某些应用中给，卡尔曼滤波器主要用于对系统状态的估计和预测，因此被视为状态观测器；在其他应用中，卡尔曼滤波器主要用于对噪声信号的滤波和去噪，因此被视为信号滤波器。卡尔曼滤波器通过将多个传感器或测量其的数据进行融合，利用概率论和线性系统理论对状态进行估计和预测，从而有效地解决了由不确定性带来的问题。它能够根据先验知识和测量信息来递归地更新状态估计，并提供对未来状态的最优预测。卡尔曼滤波器的研究模型考虑一个带有过程噪声的线性离散系统的状态空间方程其中过程噪声服从于期望为0、协方差矩阵为Qc的正态分布，即其中协方差矩阵Qc为如果过程噪声相互独立，则可以化简为对角矩阵，其对角元素为每一个状态变量过程噪声的方差，即定义观测向量其中Hm为观测矩阵，这一矩阵代表了观测值与状态向量之间的线性关系。v[k]为测量噪声，它服从于期望为0、协方差矩阵为Rc的正态分布，与Qc类似，如果测量噪声项目独立，则协方差矩阵为对角矩阵由于存在过程噪声w和测量噪声v，无法得出精确的状态向量x[k]。通过设计卡尔曼滤波器将这两个式子中的结果进行数据融合，便可以得到一个相对准确的估计值。首先定义先验状态估计对测量值做同样的处理，引入测量估计于是测量估计为将以上二者结合，定义k时刻的后验状态估计将观测向量代入，得到后验状态估计令，整理得到其中K[k]被称为卡尔曼增益，通过它将先验状态估计与测量值融合在一起得到更加准确的后验状态估计。特别的这种情况说明测量噪声远大于过程噪声，因此系统更加相信先验状态估计。得到的后验状态估计将很靠近先验状态估计。相反，如果测量误差很小，则系统将更加线性测量值。卡尔曼增益求解引入先验状态估计误差和后验状态估计误差代入得到先验状态估计误差先验状态估计误差的协方差矩阵为代入得到后验状态估计误差后验状态估计误差的协方差矩阵为卡尔曼滤波器的设计目标为找到迹最小的后验状态估计误差协方差矩阵的卡尔曼增益，即经过推导得到最优估计的卡尔曼增益其中Hm和Rc都可以当作已知量，因此只需要得到先验状态估计的协方差矩阵即可求得卡尔曼增益，于是得到根据以上的推导，得到卡尔曼滤波器的运算过程如下同时得到以下几点讨论观察卡尔曼滤波器的公式，可以发现k时刻的预测和校正都只与k-1时刻的预测和k时刻的测量值相关，这是递归算法的特性，每次计算只需要保存前一时刻的状态量和协方差矩阵，而不需要保存所有历史数据。这种递归方式可以有效地节省存储空间，提高运算效率，特别是在处理大量数据时尤为明显。若过程噪声的协方差矩阵Qc和测量噪声的协方差矩阵Rc是常数矩阵，随着时间的推移，系统收集到的测量数据增多，会使得状态估计更加准确，卡尔曼增益K[k]和估计误差协方差矩阵P[k]将趋于稳定（常数矩阵）。这一结论可以通过数值方法得到验证。因此在实际应用中，可以通过离线测试的方法预先得到稳定地卡尔曼增益和协方差矩阵。需要注意的是，现实中Qc和Rc一般很难保持常数，通常会随着环境发生改变，可以使用自适应卡尔曼滤波器来提高性能，或者采用其他的方法使用时变的噪声矩阵。选择Qc和Rc是卡尔曼滤波器设计中非常重要的一个环节，它会直接影响滤波器的性能和稳定性。在实际应用中，可以采用以下两个步骤确定这两个重要的矩阵。初始估计：根据系统的物理模型和传感器特性，对Qc和Rc进行初始估计。其中Rc通常可以通过观测或查询传感器的参数得到。实验测试：使用实验数据对Qc和Rc进行调整。在实验中，需要记录系统的状态和测量值，并根据测量数据计算出卡尔曼增益和误差协方差矩阵。然后，根据卡尔曼滤波器的状态估计结果对Qc和Rc进行调整，使得滤波器的性能能够达到预期的要求。编写线性卡尔曼滤波器程序模块框图如下 MATLAB代码实现如下 %% 输入：系统矩阵 A，B；协方差矩阵 Q_c，R_c；测量矩阵 H； %% 第k-1次的：测量值 z；后验估计误差协方差矩阵 P；后验估计值 x_hat；输入 u； %% 输出：第k次的：后验估计值: x_hat；后验估计误差协方差矩阵 P; function [x_hat, x_hat_minus, P]= F8_LinearKalmanFilter(A,B,Q_c,R_c,H,z,x_hat,P,u) %% 计算先验状态估计 x_hat_minus = A*x_hat + B*u; %% 计算先验估计误差协方差矩阵 P_minus = A*P*transpose(A) + Q_c; %% 计算卡尔曼增益 K = P_minus*transpose(H)/(H*P_minus*transpose(H)+R_c); %% 更新后验估计 x_hat = x_hat_minus + K*(z-H*x_hat_minus); %% 后验估计误差协方差矩阵 P = (eye(size(A,1)) - K*H)*P_minus; end 案例分析以无人机高度观测为例过程噪声的协方差矩阵为测量矩阵测量噪声的协方差矩阵为注意，在实际编程中测量噪声的协方差矩阵Rca的右下角的0应该取一个不为0的值（不影响最终结果），因为取0的话由于矩阵不满秩，会导致计算过程中出现奇异值，在实际手推过程中，可以把Rca看作一个分块矩阵分析。 MATLAB代码实现如下 clear clc %%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% % 构建系统矩阵A，n x n A = [0 1 0; 0 0 1 ;0 0 0]; % 计算A矩阵维度 n = size (A,1); % 构建输入矩阵B，n x p B = [0; 1; 0]; % 计算输入矩阵维度 p = size(B,2); % 定义测量矩阵H_m, n x n H_m = [1 0 0; 0 1 0; 0 0 1]; % 重力加速度常数 g = 10; C = [1, 0, 0]; D = 0; %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% % 离散时间步长 Ts = 0.1; % 连续系统转离散系统 sys_d = c2d(ss(A,B,C,D),Ts); % 提取离散系统A矩阵 A = sys_d.a; % 提取离散系统B矩阵 B = sys_d.b; %%%%%%%%%%%%%%%%%参数设计%%%%%%%%%%%%%%%%%%%%% % 定义过程噪声协方差矩阵 Q_c = [0.05 0 0; 0 0.05 0; 0 0 0]; % 定义测量噪声协方差矩阵 R_c = [1 0 0; 0 1 0; 0 0 0.001]; %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% % 初始化系统状态 x0 = [0; 1 ; -10]; % 初始化状态赋值 x = x0; % 系统输入初始化 u0 = g; % 初始输入赋值 u = u0; % 初始化后验估计 x_hat0 = [0; 1; -10]; % 初始化后验估计赋值 x_hat = x_hat0; % 初始化后验估计误差协方差矩阵 P0 = [1 0 0;0 1 0; 0 0 0]; % 初始后验估计误差协方差矩阵赋值 P = P0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 定义系统运行步数 k_steps = 100; % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_steps x_history = zeros(n,k_steps); % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_steps u_history = zeros(p,k_steps); % 定义x_hat_history零矩阵，用于储存后验估计结果，维度n x k_steps x_hat_history = zeros(n,k_steps); % 定义x_hat_minus_history零矩阵，用于储存先验估计结果，维度n x k_steps x_hat_minus_history = zeros(n,k_steps); % 定义z_historyy零矩阵，用于测量结果，维度n x k_steps z_history = zeros(n,k_steps); %%%%%%%%%%%%%%%定义仿真环境%%%%%%%%%%%%%%%%%%%% % 定义过程噪声矩阵W，维度n x k_steps w = zeros (n,k_steps); % 定义测量噪声矩阵V，维度n x k_steps v = zeros (n,k_steps); % 从文件NoiseData.csv中读取数据 % 数据来自于系统随机生成，保存为文件可以方便进行多组实验之间的对比 data = readmatrix('NoiseData.csv'); w = data(2:4,:); v = data(6:8,:); %%%%%%%%%%%%%生成过程与测量噪声%%%%%%%%%%%%%%%%%% % 使用以下代码生成随机噪声，之后保存在NoiseData.csv中，方便下次读取。%%%%%% % 定义真实的过程噪声协方差矩阵 % Q_ca = [0.05 0; 0 0.05]; % 定义真实的测量噪声协方差矩阵 % R_ca = [1 0; 0 1]; % 随机生成过程噪声 % w(1:2,:) = chol(Q_ca)* randn(2,k_steps); % 随机生成测量噪声 % v(1:2,:) = chol(R_ca)* randn(2,k_steps); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 仿真开始，建立for循环 for k = 1:k_steps % 系统状态空间方程，计算实际状态变量 x = A * x + B*u + w(:,k); % 计算实际测量值，在实际应用中，这一项来自传感器测量 z = H_m * x + v(:,k); % 使用卡尔曼滤波器 [x_hat,x_hat_minus, P]= F8_LinearKalmanFilter(A,B,Q_c,R_c,H_m,z,x_hat,P,u); % 保存系统状态到预先定义矩阵的相应位置 x_history (:,k+1) = x; % 保存测量值到预先定义矩阵的相应位置 z_history (:,k+1) = z; % 保存先验估计到预先定义矩阵的相应位置 x_hat_minus_history (:,k+1) = x_hat_minus; % 保存后验估计到预先定义矩阵的相应位置 x_hat_history (:,k+1) = x_hat; end %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% % x1真实结果 plot (0:length(x_history)-1,x_history(1,:),'--','LineWidth',2); hold on % x1测量值 plot ( 0:length(z_history)-1,z_history(1,:),'*','MarkerSize',8) ylim([-2 12]); % x1先验估计值 plot (0:length(x_hat_minus_history)-1,x_hat_minus_history(1,:),'o','MarkerSize',8); % x1后验估计值 plot ( 0:length(x_hat_history)-1,x_hat_history(1,:),'LineWidth',2); legend(' 真实值 ',' 测量值 ',' 先验估计值 ',' 后验估计值 ') set(legend, 'Location', 'southeast','FontSize', 20); hold off; grid on % 计算Ems 均方误差 Ems=(x_hat_history(1,:)-x_history(1,:))*(x_hat_history(1,:)-x_history(1,:))'/k_steps; 得到仿真结果如下图所示当过程噪声协方差矩阵不能确定时，可以首先确定一个较大的过程噪声协方差矩阵，运行并观察系统的表现，之后不断下调过程噪声并找到最合适的Qc，这就是系统识别的过程。
2025-03-03
发表了主题帖：《控制之美（卷2）——最优化控制MPC与卡尔曼滤波器》——含有约束的模型预测控制

本帖最后由 FuShenxiao 于 2025-3-3 10:39 编辑在实际的控制应用中，通常会遇到各种约束条件，如输出限制、输入限制、状态限制等。这些约束条件往往是由物理限制、安全限制以及经济因素等各方面综合确定的。相较于LQR方法，MPC的一个重要优势在于可以将约束条件直接嵌入控制器中进行求解。约束转化为标准形式考虑一个线性时不变系统，其离散形式的状态空间方程为从调节器（归零调节）入手，考虑初始点为k，预测区间为Np，在k时刻做出预测时其二次型性能指标为考虑系统的线性约束条件约束条件可以写成定义，同时定义于是约束条件化简为用U[k]表示X[k]，整理得到进一步简化，定义于是优化目标在于最小化代价函数同时满足约束控制量和状态变量上下限约束转化为标准形式对输入变量和状态变量施加上下限的约束，即可以写成矩阵形式通过变换，转化为标准形式，且约束矩阵为常数矩阵，不随k变化，即其中可以推导得到其中根据以上的推导思路，得到含约束的MPC控制器设计思路如下图所示其中，约束条件矩阵转换模块MATLAB代码实现如下 %% 输入：系统状态上下限x_low,x_high；输入上下限u_low,u_high；矩阵Phi，Gamma；预测区间N_P %% 输出：含约束二次规划矩阵 M,Beta_bar,b function [M,Beta_bar,b]= F6_MPC_Matrices_Constraints(x_low,x_high,u_low,u_high,N_P,Phi,Gamma) % 计算系统状态维度 n = size (x_low,1); % 计算系统输入维度 p = size (u_low,1); % 构建M矩阵，参考式（5.5.14c） M = [zeros(p,n);zeros(p,n);-eye(n);eye(n)]; % 构建F矩阵，参考式（5.5.14c） F = [-eye(p);eye(p);zeros(n,p);zeros(n,p)]; % 构建Beta矩阵，参考式（5.5.14c） Beta = [-u_low; u_high; -x_low; x_high]; % 构建M_N矩阵，参考式（5.5.14c） M_Np = [-eye(n);eye(n)]; % 构建Beta_N矩阵，参考式（5.5.14c） Beta_N = [-x_low; x_high]; % 构建M_bar矩阵，参考式（5.5.15b） M_bar = zeros((2*n+2*p)*N_P+2*n,n); M_bar (1: (2*n+2*p) , :) = M; % 构建Beta_bar矩阵，参考式（5.5.15e），模块输出 Beta_bar = [repmat(Beta,N_P,1); Beta_N]; % 初始化M_2bar矩阵 M_2bar = M; % 初始化F_2bar矩阵 F_2bar = F; % for循环创建M_2bar和F_2bar矩阵 for i=1:N_P-2 M_2bar = blkdiag(M_2bar, M); F_2bar = blkdiag(F_2bar, F); end M_2bar = blkdiag(M_2bar,M_Np); % 构建M_2bar矩阵最终形式（加入顶部一行的零矩阵） M_2bar = [zeros(2*n+2*p,n*N_P); M_2bar]; F_2bar = blkdiag(F_2bar, F); % 构建F_2bar矩阵最终形式（加入底部一行的零矩阵） F_2bar = [F_2bar; zeros(2*n,p*N_P)]; % 构建b矩阵，参考式（5.5.9） b = -(M_bar + M_2bar*Phi); % 构建M矩阵，参考式（5.5.9） M = M_2bar*Gamma + F_2bar; end 含约束二次规划求解模块MATLAB代码实现如下 %% 输入：二次规划矩阵 F，H；系统控制量维度 p；系统状态：x 约束条件矩阵 M,Beta_bar,b %% 输出：系统控制（输入） U，u function [U, u]= F7_MPC_Controller_withConstriants(x,F,H,M,Beta_bar,b,p) % 利用二次规划求解系统控制（输入） U = quadprog(H,F*x,M,Beta_bar+b*x,[],[],[],[]); % 根据模型预测控制的策略，仅选取所得输入的第一项，参考（5.3.18） u = U(1:p,1); end 案例分析——软约束与硬约束的讨论含约束的MPC轨迹追踪控制器设计思路如下图所示硬约束是指系统必须严格满足的条件，在使用MPC控制器时，可以将它融入优化问题的求解过程中。软约束则相对灵活，可以通过调整性能指标中的权重系数来实现。当硬约束和软约束互相矛盾时，系统会调整在满足硬约束条件下尽量满足软约束。根据之前的弹簧-阻尼系统案例，状态变量出现了超调。如果我们完全不希望在控制过程中产生超调量，可以为系统增加一个硬约束，即编写MATLAB代码如下 clear clc %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 定义系统参数 % 定义质量块质量 m_sys = 1; % 定义阻尼系数 b_sys = 0.5; % 定义弹簧弹性系数 k_sys = 1; %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% % 构建系统矩阵A，n x n A = [0 1 ; -k_sys/m_sys -b_sys/m_sys]; % 计算A矩阵维度 n = size (A,1); % 构建输入矩阵B，n x p B = [0; 1/m_sys]; % 计算输入矩阵维度 p = size(B,2); C = [1, 0]; D = 0; %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% % 离散时间步长 Ts = 0.1; % 连续系统转离散系统 sys_d = c2d(ss(A,B,C,D),Ts); % 提取离散系统A矩阵 A = sys_d.a; % 提取离散系统B矩阵 B = sys_d.b; %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% % 设计状态权重系数矩阵, n x n Q = [1 0 ; 0 1]; % 设计终值权重系数矩阵, n x n S = [1 0; 0 1]; % 设计输入权重系数矩阵, p x p R = 1; %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% % 系统状态参考值 xd = [1 ; 0]; % 构建目标转移矩阵 AD = eye(n); % 计算目标输入 ud = mldivide(B,(eye(n)-A)*xd); %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% % 初始化系统状态 x0 = [0; 0]; x = x0; % 初始化增广状态矩阵 xa = [x; xd]; % 初始化系统输入 u0 = 0; u = 0; %%%%%%%%%%%%%%系统约束定义%%%%%%%%%%%%%%%%%%%%% % 输入下限 u_low = -inf; % 输入上限 u_high = inf; % 状态下限 x_low = [0;-inf]; % 状态上限 x_high = [1;inf]; % 增广状态下限 xa_low = [x_low;-inf;-inf]; % 增广状态上限 xa_high = [x_high;inf;inf]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 定义系统运行步数 k_steps = 100; % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step x_history = zeros(n,k_steps); % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step u_history = zeros(p,k_steps); % 定义x_history_noconstraint零矩阵，用于储存系统状态结果，维度n x k_step x_history_noconstraint = zeros(n,k_steps); % 定义u_history_noconstraint零矩阵，用于储存系统输入结果，维度p x k_step u_history_noconstraint = zeros(p,k_steps); % 定义预测区间 N_P = 20; % 调用模块[F2]，计算系统增广矩阵Aa，Ba，Qa，Sa，R以及目标输入ud [Aa,Ba,Qa,Sa,R,ud] = F2_InputAugmentMatrix_SS_U(A,B,Q,R,S,xd); % 调用模块[F4]计算二次规划需用到的矩阵 [Phi,Gamma,Omega,Psi,F,H]= F4_MPC_Matrices_PM(Aa,Ba,Qa,R,Sa,N_P); % 调用模块[F6]计算含约束二次规划需用到的矩阵 [M,Beta_bar,b]= F6_MPC_Matrices_Constraints(xa_low,xa_high,u_low,u_high,N_P,Phi,Gamma); % for循环开始仿真有约束 for k = 1 : k_steps % 调用模块[F7]计算系统系统控制（输入增量） [delta_U, delta_u]= F7_MPC_Controller_withConstriants(xa,F,H,M,Beta_bar,b,p); % 根据输入增量计算系统输入 u = delta_u+ud; % 系统输入代入系统方程，计算系统响应 x = A * x + B * u; % 更新增广矩阵xa xa = [x; xd]; % 保存系统状态到预先定义矩阵的相应位置 x_history (:,k+1) = x; % 保存系统输入到预先定义矩阵的相应位置 u_history (:,k) = u ; end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%计算无约束情况作对比%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% % 初始化系统状态 x0 = [0; 0]; x = x0; % 初始化增广状态矩阵 xa = [x; xd]; % 初始化系统输入 u0 = 0; u = 0; % for循环开始仿真无约束 for k = 1 : k_steps % 调用模块[F5]利用二次规划计算无约束系统的控制 [delta_U,delta_u]= F5_MPC_Controller_noConstraints(xa,F,H,p); % 计算系统给响应 x = A * x + B * (delta_u + ud); % 构建增广状态xa xa = [x; xd]; % 保存系统状态 x_history_noconstraint (:,k+1) = x; % 保存系统输入 u_history_noconstraint (:,k) = delta_u + ud; end %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% % 状态变量结果图 subplot (2, 2, 1); hold; % 系统状态x1结果图，质量块位移 plot (0:length(x_history)-1,x_history(1,:)); % 系统状态x2结果图，质量块速度 plot (0:length(x_history)-1,x_history(2,:),'--'); grid on legend("x1","x2") hold off; xlim([0 k_steps]); ylim([-0.2 1.2]); % 系统输入结果图 subplot (2, 2, 3); % 系统输入结果图，施加在质量块上的力，f stairs (0:length(u_history)-1,u_history(1,:)); legend("u") grid on xlim([0 k_steps]); ylim([0 3]); % 状态X1对比图 subplot (2, 2, 2); hold; % 系统状态x1结果图，质量块位移，有约束 plot (0:length(x_history)-1,x_history(1,:)); % 系统状态x1结果图，质量块速度，无约束 plot (0:length(x_history_noconstraint)-1,x_history_noconstraint(1,:),'--'); grid on legend("有约束","无约束") hold off; xlim([0 k_steps]); ylim([-0.2 1.2]); % 系统输入对比图 subplot (2, 2, 4); hold; % 系统输入结果图，施加在质量块上的力，f，有约束 stairs (0:length(u_history)-1,u_history(1,:)); % 系统输入结果图，施加在质量块上的力，f，无约束 stairs (0:length(u_history_noconstraint)-1,u_history_noconstraint(1,:),'--'); legend("有约束","无约束") grid on xlim([0 k_steps]); ylim([0 3]); 得到结果如下，可以看到新的控制器将硬约束考虑在内，成功限制了x1[k]的最大超调案例分析——无人机高度控制在LQR处理无人机高度控制问题时遇到了两个问题：1. 无法对系统状态施加约束；2. 对系统输入的约束实在计算完成之后强制执行的。使用MPC控制器可以很好地解决这两个问题。控制器的构建与结果分析建立系统状态空间方程在预测区间Np内的性能指标系统约束条件将其转化为标准形式的二次型约束 MATLAB代码实现如下 clear clc %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 定义系统参数 % 定义无人机质量 m = 1; % 定义重力加速度常数 g = 10; %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% % 构建系统矩阵A，n x n A = [0 1 0; 0 0 1 ;0 0 0]; % 计算A矩阵维度 n= size (A,1); % 构建输入矩阵B，n x p B = [0; 1/m; 0]; % 计算输入矩阵维度 p = size(B,2); C = [1, 0, 0]; D = 0; %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% % 离散时间步长 Ts = 0.1; % 连续系统转离散系统 sys_d = c2d(ss(A,B,C,D),Ts); % 提取离散系统A矩阵 A = sys_d.a; % 提取离散系统B矩阵 B = sys_d.b; %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% % 设计状态权重系数矩阵, n x n Q=[1 0 0; 0 1 0; 0 0 0]; % 设计终值权重系数矩阵, n x n S=[1 0 0; 0 1 0; 0 0 0]; % 设计输入权重系数矩阵, p x p R= 0.1; %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% % 系统状态参考值 xd = [10 ; 0 ; -g]; % 构建目标转移矩阵 AD = eye(n); % 计算目标输入 ud = mldivide(B,(eye(n)-A)*xd); %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% % 初始化系统状态 x0 = [0; 0; -g ]; x = x0; % 初始化增广状态矩阵 xa = [x; xd]; % 初始化系统输入 u0 = 0; u = u0; %%%%%%%%%%%%%%系统约束定义%%%%%%%%%%%%%%%%%%%%% % 输入下限 u_low = -3; % 输入上限 u_high = 2; % 状态下限 x_low = [0;0;-g]; % 状态上限 x_high = [10;3;-g]; % 增广状态下限 xa_low = [x_low;-inf;-inf;-inf]; % 增广状态上限 xa_high = [x_high;inf;inf;inf]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 定义系统运行步数 k_steps = 100; % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step x_history = zeros(n,k_steps); % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step u_history = zeros(p,k_steps); % 定义预测区间 N_P = 20; % 调用模块[F2]，计算系统增广矩阵Aa，Ba，Qa，Sa，R以及目标输入ud [Aa,Ba,Qa,Sa,R,ud] = F2_InputAugmentMatrix_SS_U(A,B,Q,R,S,xd); % 调用模块[F4]计算二次规划需用到的矩阵 [Phi,Gamma,Omega,Psi,F,H]= F4_MPC_Matrices_PM(Aa,Ba,Qa,R,Sa,N_P); % 调用模块[F6]计算含约束二次规划需用到的矩阵 [M,Beta_bar,b]= F6_MPC_Matrices_Constraints(xa_low,xa_high,u_low,u_high,N_P,Phi,Gamma); % for循环开始仿真 for k = 1 : k_steps % 调用模块[F7]计算系统系统控制（输入增量） [delta_U, delta_u]= F7_MPC_Controller_withConstriants(xa,F,H,M,Beta_bar,b,p); % 根据输入增量计算系统输入 u = delta_u+ud; % 系统输入代入系统方程，计算系统响应 x = A * x + B * u; % 更新增广矩阵xa xa = [x; xd]; % 保存系统状态到预先定义矩阵的相应位置 x_history (:,k+1) = x; % 保存系统输入到预先定义矩阵的相应位置 u_history (:,k) = u ; end %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% % 状态变量结果图 subplot (2, 1, 1); hold; % 无人机高度 plot (0:length(x_history)-1,x_history(1,:)); % 无人机速度 plot (0:length(x_history)-1,x_history(2,:),'--'); grid on legend("x1","x2") hold off; xlim([0 k_steps]); ylim([0 10.2]); % 系统输入 subplot (2, 1, 2); hold; stairs (0:length(u_history)-1,u_history(1,:)); legend("u") grid on xlim([0 k_steps]); ylim([7 12]); 得到结果如下。可以看到使用含约束的MPC进行计算时得到的结果已经非常接近使用数值方法求得的最优解。然而，两者之间存在着一些区别。首先，数值解法并没有考虑任何的输入能耗，而在MPC算法中，控制量的输入是作为性能指标的一部分考虑的。其次，数值算法仅考虑无人机达到目标位置，并不考虑之后会发生什么；而在使用MPC时，由于它是滚动优化的方法，因此在达到目标位置后仍需要考虑系统最终稳定，从而结果表现出无人机会缓慢地将速度降低到0，以保持系统的稳定性。预测区间的影响预测区间决定了控制器在未来多久时间内进行优化和计划，预测区间的选择主要从控制性能、计算复杂度以及可行性三个方面进行分析。例如上一张图为预测区间Np=20，现取Np=10，Np=5 Np=10时，结果如下图所示 Np=5时，结果如下图所示可见当Np减小时，相当于无人机需要考虑的时间变大，导致控制器无法充分预测和适应未来的系统变化，使得系统的控制性能下降。采样时间的选择采样时间与预测和计算机性能都有关系。对于采样时间的长短，如果只关注眼前或短期内的情况，就可能会错过长远的趋势。然而，如果“深虑”过远，就会影响当下的行动。分别测试Ts=0.1和Ts=0.5时的运行时间比较 Ts=0.1时的结果如下 Ts=0.5时的结果如下可见采样时间并不一定是越短越好，还得综合考虑。
2025-03-02
发表了主题帖：《控制之美（卷2）——最优化控制MPC与卡尔曼滤波器》——无约束的模型预测控制

本帖最后由 FuShenxiao 于 2025-3-2 12:20 编辑模型预测控制是一种广泛应用于工业控制领域的高级控制策略。MPC的基本思想是利用当前时刻系统的状态及约束条件，对未来一段时间内的状态、输入变量进行预测，并求解出一组最优的控制输入序列。随后，只选取最优控制序列中的第一组结果，将其应用于系统中。在下一时刻，重复同样的操作，得到新的最优控制序列，直到系统达到期望状态。相比于LQR，MPC的一个重要的特点是可以考虑到多种约束条件，如输入限制、状态变量限制等，因此可以适用于多种工业控制领域。 MPC是一种滚动优化的控制方法。在每个采样时刻，MPC会通过求解一个有限时间内的最优化问题来计算最优控制序列，这一有限时间称为预测区间。考虑系统的不确定性、测量误差等因素，在实际控制中，只选取预测区间内最优控制序列的第一项施加到系统中。MPC通常针对离散系统，因此预测区间通常指的是预测的离散步数。例如，设置离散型状态空间方程设置性能指标滚动优化概念如下图所示。其中预测区间定义为Np，控制区间定义为Nc，其中设置Np=Nc=5.系统从k时刻开始，初始状态为x[k]。滚动优化控制会通过求解预测区间内的最优化问题（Np=5时的性能指标J最小），计算出最优控制序列u。同时系统模型预测系统在这样的控制序列下状态值的变化x。在完成控制量的计算和模型预测之后，只需对系统施加第一个u而社区其余控制序列。在k+1时刻，重复上一步操作，并依此类推。在实际中，为了节省计算资源，会选择Nc<Np。当时间超过控制区间之后，控制量将保持为常数（与前值一致），如下图所示。二次规划问题在模型预测中，我们需要在每个时刻通过预测模型类计算未来一段时间内的最优控制输入，而这个过程就可以转化为一个带有约束的二次规划问题。对于无约束情况，可以令得到最优解例如对于性能指标求其最小值时的u1和u2，可以令J分别对u1和u2求偏导，得到偏导为0的点即为最小值。 MATLAB代码实现如下 clear clc % 定义二次规划问题的H和f H = [1 0; 0 1]; f = [1; 1]; % 求解二次规划问题 u = -inv(H)*f; % 绘制二次规划问题的可行域和最优解点（3D图） [U1,U2] = meshgrid(-2:0.1:0); J = 0.5*(U1.^2 + U2.^2)+U1+U2; subplot(1,2,1); surf(U1,U2,J,'FaceAlpha', 0.1); hold on; plot3(u(1), u(2), 0.5*(u(1)^2 + u(2)^2)+u(1)+u(2), 'r^', 'MarkerSize', 20,'MarkerFaceColor', 'red'); xlabel('u1'); ylabel('u2'); zlabel('J(u1,u2)'); xlim([-2 0]); ylim([-2 0]); zlim([-1.05 0]); set(gca,'FontSize',20); % 绘制等高线图 subplot(1,2,2); contour(U1,U2,J,30); hold on; plot(u(1), u(2), 'r*', 'MarkerSize', 10); xlabel('u1'); ylabel('u2'); set(gca,'FontSize',20); sgtitle('二次规划问题'); 得到结果对于等式约束，可以采用拉格朗日乘数法。例如对于上面的问题引入约束条件。这就相当于在原本的空间中新加入一个平面，与原始性能指标相交的部分形成了一条曲线，即约束条件在性能指标上的投影。最小值点则需要在这一条投影曲线中做出选择。 MATLAB代码实现如下 clear clc % 定义二次规划问题的H,f,Meq H = [1 0; 0 1]; f = [1; 1]; n = size(H,1); % 定义等式约束的Meq和beq Meq = [1 -1]; beq =1; m = size(Meq,1); % 定义二次规划问题的u和lamda u = zeros(n,1); lamda = zeros(m,1); % 求解二次规划问题 u_lamda = inv([H,Meq';Meq,zeros(m,m)])*[-f;beq]; u = u_lamda(1:n,:); % 绘制二次规划问题的可行域和最优解点（3D图） [U1,U2] = meshgrid(-2:0.1:0); J = 0.5*(U1.^2 + U2.^2)+U1+U2; subplot(1,2,1); surf(U1,U2,J,'FaceAlpha', 0.1); hold on; u1_proj = -2:0.1:0; u2_proj = u1_proj - 1; J_proj = 0.5 * (u1_proj .^ 2 + u2_proj .^ 2) + u1_proj + u2_proj; plot3(u1_proj, u2_proj, J_proj, 'b', 'LineWidth', 5); plot3(u(1), u(2), 0.5*(u(1)^2 + u(2)^2)+u(1)+u(2), 'r^', 'MarkerSize', 20,'MarkerFaceColor', 'red'); [J_proj, U1_proj] = meshgrid(-1:0.1:0, u1_proj); U2_proj = U1_proj - 1; surf(U1_proj, U2_proj, J_proj, 'FaceColor','blue','FaceAlpha', 0.2,'EdgeColor', 'none'); xlabel('u1'); ylabel('u2'); zlabel('J(u1,u2)'); xlim([-2 0]); ylim([-2 0]); zlim([-1.05 0]); set(gca,'FontSize',20); % 绘制等高线图 subplot(1,2,2); contour(U1,U2,J,30); hold on; plot(u(1), u(2), 'r*', 'MarkerSize', 10); u1_con = -2:0.1:2; u2_con = u1_con - 1; plot(u1_con, u2_con, 'k', 'LineWidth', 2); xlabel('u1'); ylabel('u2'); set(gca,'FontSize',20); sgtitle('二次规划问题'); 得到结果如下对于不等式约束，这往往采用数值或解析方法来求解最优解。例如对于上述问题加入约束编写MATLAB代码如下 clear clc % 定义二次规划问题的H和f H = [1 0; 0 1]; f = [1; 1]; % 定义不等式约束的A和b A = [-1 1; 1 1]; b = [1; 2]; % 定义变量的边界条件 lb = [0; 0]; ub = [1; 2]; % 求解二次规划问题 options = optimset('Display','off'); [u, J] = quadprog(H, f, A, b, [], [], lb, ub, [], options); % 绘制等高线图和可行域 figure('position',[150 150 800 600]); % 绘制等高线图 subplot(1,1,1); [U1, U2] = meshgrid(-1:0.1:2); J = 0.5*(U1.^2 + U2.^2) + U1 + U2; contour(U1, U2, J, 60); hold on; % 绘制可行域 plot([-0.5, 1], [0.5, 2], 'k', 'LineWidth', 1.5); hold on; plot([0, 1.5], [2, 0.5], 'k', 'LineWidth', 1.5); plot([0, 0], [-1, 2], 'k', 'LineWidth', 1.5); plot([-0.5, 1.5], [0, 0], 'k', 'LineWidth', 1.5); plot([1, 1], [-1, 2], 'k', 'LineWidth', 1.5); fill([0, 0.5, 1], [1, 1.5, 1], 'green', 'FaceAlpha', 0.2, 'EdgeColor', 'none'); fill([0, 0, 1, 1], [0, 1, 1, 0], 'green', 'FaceAlpha', 0.2, 'EdgeColor', 'none'); % 绘制最优解点 plot(u(1), u(2), 'r^', 'MarkerSize', 20,'MarkerFaceColor', 'red'); % 添加坐标轴标签和图标题 xlabel('u1'); ylabel('u2'); xlim([-0.5 1.5]); ylim([-0.5 2]); set(gca, 'FontSize', 20); 得到结果如下模型预测控制推导——无约束调节问题线性离散系统转化为标准形式设置离散线性时不变系统的状态空间方程定义二次型性能指标其中，S，Q，R均为对角矩阵，分别代表系统的末端代价、运行代价以及控制量代价的权重矩阵根据滚动优化的概念，得到k+1时刻的状态变量预测值 k+2时刻的状态变量预测值代入原始的状态空间方程得到以此类推，得到k+Np时刻的状态为了简化系统分析，定义于是得到更紧凑的状态空间方程其中此时设计目标就是找到最优的控制序列U*[k]，使得性能指标J最小。相较于LQR采用逆向分级方法逐一计算每一时刻的最优控制量，MPC的控制序列被写成了紧凑的向量形式，整体求解预测区间内的最优控制序列。将性能指标转化为二次规划的标准形式当预测区间为Np时，性能指标为定义代换整理得到最终的代价函数根据以上得出的代价函数，在MATLAB中编写性能指标矩阵转换模块如下 %% 输入：系统矩阵 A，B；权重矩阵 Q，R，S；预测区间：N_P %% 输出：二次规划矩阵F，H 中间过程矩阵Phi，Gamma，Omega，Psi function [Phi,Gamma,Omega,Psi,F,H]= F4_MPC_Matrices_PM(A,B,Q,R,S,N_P) % 计算系统矩阵维度，n n = size(A,1); % 计算输入矩阵维度，p p = size(B,2); % 初始化Phi矩阵并定义维度 Phi = zeros(N_P*n,n); % 初始化Gamma矩阵并定义维度 Gamma = zeros(N_P*n,N_P*p); % 定义临时对角单位矩阵 tmp = eye(n); % Create a n x n "I" matrix % 定义for循环行向量 rows = 1:n; % for循环，用于构建Phi和Gamma矩阵 for i = 1:N_P % 构建Phi矩阵，参考式（5.3.5b） Phi((i-1)*n+1:i*n,:) = A^i; % 构建Gamma矩阵，参考式（5.3.5b） Gamma(rows,:) = [tmp*B,Gamma(max(1,rows-n), 1:end-p)]; % Gamma矩阵行数更新，由于Gamma矩阵是由一系列“小”矩阵组成的，因此下一个循环要更新n行 rows =i*n+(1:n); % 构建临时矩阵用于矩阵A的幂计算 tmp= A*tmp; end % 构建Omega矩阵，包含Q矩阵的部分 Omega = kron(eye(N_P-1),Q); % 构建最终Omega矩阵，包含S矩阵 Omega = blkdiag(Omega,S); % 构建Psi矩阵，其为R矩阵组成的对角阵 Psi = kron(eye(N_P),R); % 计算二次规划矩阵F F = Gamma'*Omega*Phi; % 计算二次规划矩阵H H = Psi+Gamma'*Omega*Gamma; end 编写无约束二次规划求解模块如下 %% 输入：二次规划矩阵 F，H；系统控制量维度 p；系统状态：x %% 输出：系统控制（输入） U，u function [U,u]= F5_MPC_Controller_noConstraints(x,F,H,p) % 选取最优化求解模式 options = optimset('MaxIter', 200); % 利用二次规划求解系统控制（输入） [U, FVAL, EXITFLAG, OUTPUT, LAMBDA] = quadprog(H,F*x,[],[],[],[],[],[],[],options); % 根据模型预测控制的策略，仅选取所得输入的第一项，参考（5.3.18） u = U(1:p,1); end 无约束条件下的解析解无约束MPC控制器的控制思路如下图所示在无约束条件下，可以尝试求解析解。得到最优控制序列为其中的第一项u*[k|k]表达为与LQR控制策略一致，这是一个负反馈系统，实际上它的结果也与LQR所得到的结果保持一致。一维案例分析——与LQR的比较 MPC与LQR方法求解的是同一个最优化问题。LQR使用贝尔曼最优化理论从末端向前递归求解；而MPC则将未来的控制序列作为求解项，预测未来的情况并使用二次规划进行求解。需要注意的是，通过LQR求解得到的是一个反馈矩阵，控制量则需要使用反馈矩阵诚意当前状态变量得到；通过MPC求解得到的最优控制序列则是当前预测区间内的一系列最优控制量。一维案例分析——MPC控制器的反馈特性在MPC的实际应用中，指挥将求解出的最优控制序列U*[k]的第一项u*[k|k]施加到系统中，然后在下一时刻重新计算新的控制序列。为什么要舍弃第一项之后的结果，而不是通过离线计算好之后每一时刻的最优控制量再按照时间序列输入到系统中？这是因为离线和在线方式得到的结果几乎一致。 MATLAB代码实现如下 clear clc %%%%%%%%%%%%%%%%%%%%%%%%%% % 系统定义 A = 1; B = 1; % 系统状态维度计算 n = size (A,1); % p = size (B,2); % 状态代价权重系数 Q = 1; % 终值代价权重系数 S = 1; % 系统输入代价权重系数 R = 1; % 定义系统运行步数 k_steps = 5; %%%%%%%%%%%%%%%%初始化系统%%%%%%%%%%%%%%%%%%% % 初始化系统状态矩阵 - 在线 x_history = zeros(n,k_steps); % 初始化系统状态矩阵 - 离线 x_history_2 = zeros(n,k_steps); % 初始化系统输入矩阵 - 在线 u_history = zeros(p,k_steps); % 初始化系统输入矩阵 - 离线 u_history_2 = zeros(p,k_steps); % 系统状态初始化，第一步 x_0 = 1; % 系统初始状态赋值 - 在线 x = x_0; % 系统初始状态赋值 - 离线 x2 = x_0; % 系统初始状态存入状态矩阵的第一个位置 - 在线 x_history (:,1) = x; % 系统初始状态存入状态矩阵的第一个位置 - 离线 x_history_2(:,1) = x2; % 定义预测区间，预测区间要小于系统运行步数 N_P = 5; %Predict horizon % 调用模块[F4]计算二次规划需用到的矩阵 [Phi,Gamma,Omega,Psi,F,H] = F4_MPC_Matrices_PM(A,B,Q,R,S,N_P); % 调用模块[F5]利用二次规划计算无约束系统的控制 - 离线 [U_offline, u_offline]= F5_MPC_Controller_noConstraints(x,F,H,p); for k = 1 : k_steps % 调用模块[F5]利用二次规划计算无约束系统的控制 - 在线 [U,u] = F5_MPC_Controller_noConstraints(x,F,H,p); % 计算系统给响应 - 在线 x = A * x + B * u ; %% 将在线、离线系统状态以及输入保存进相应的矩阵 % 保存系统状态 - 在线 x_history (:,k+1) = x; % 保存系统输入 - 在线 u_history (:,k) = u; % 系统输入赋值 - 离线 u_offline = U_offline(k); % 计算系统给响应 - 离线 x2 = A * x2 + B * u_offline ; % 保存系统状态 - 离线 x_history_2 (:,k+1) = x2; % 保存系统输入 - 离线 u_history_2 (:,k) = u_offline; end %%%%%%%%%%%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 系统状态结果视图在线vs.离线 subplot (2, 1, 1); hold; plot (0:length(x_history)-1,x_history(1,:)); plot (0:length(x_history_2)-1,x_history_2(1,:),'--'); grid on legend("在线 ","离线 ") hold off; xlim([0 k_steps-1]); % 系统输入结果视图在线vs.离线 subplot (2, 1, 2); hold; stairs (0:length(u_history)-1,u_history(1,:)); stairs (0:length(u_history_2)-1,u_history_2(1,:),'--'); legend("在线 ","离线 ") legend('location', 'southeast'); grid on xlim([0 k_steps-1]); 得到结果如下，可见二者结果几乎一致然而，如果引入了扰动，那么在线算法可以很好处理扰动并最终将系统调节到[0]，MATLAB代码实现如下 clear clc %%%%%%%%%%%%%%%%%%%%%%%%%% % 系统定义 A = 1; B = 1; % 系统状态维度计算 n = size (A,1); % p = size (B,2); % 状态代价权重系数 Q = 1; % 终值代价权重系数 S = 1; % 系统输入代价权重系数 R = 1; % 定义系统运行步数 k_steps = 5; %%%%%%%%%%%%%%%%初始化系统%%%%%%%%%%%%%%%%%%% % 初始化系统状态矩阵 - 在线 x_history = zeros(n,k_steps); % 初始化系统状态矩阵 - 离线 x_history_2 = zeros(n,k_steps); % 初始化系统输入矩阵 - 在线 u_history = zeros(p,k_steps); % 初始化系统输入矩阵 - 离线 u_history_2 = zeros(p,k_steps); % 系统状态初始化，第一步 x_0 = 1; % 系统初始状态赋值 - 在线 x = x_0; % 系统初始状态赋值 - 离线 x2 = x_0; % 系统初始状态存入状态矩阵的第一个位置 - 在线 x_history (:,1) = x; % 系统初始状态存入状态矩阵的第一个位置 - 离线 x_history_2(:,1) = x2; % 定义预测区间，预测区间要小于系统运行步数 N_P = 5; %Predict horizon % 调用模块[F4]计算二次规划需用到的矩阵 [Phi,Gamma,Omega,Psi,F,H] = F4_MPC_Matrices_PM(A,B,Q,R,S,N_P); % 调用模块[F5]利用二次规划计算无约束系统的控制 - 离线 [U_offline, u_offline]= F5_MPC_Controller_noConstraints(x,F,H,p); for k = 1 : k_steps % 调用模块[F5]利用二次规划计算无约束系统的控制 - 在线 [U,u] = F5_MPC_Controller_noConstraints(x,F,H,p); % 计算系统给响应 - 在线 x = A * x + B * u ; if k == 2 x = x +0.2; end %% 将在线、离线系统状态以及输入保存进相应的矩阵 % 保存系统状态 - 在线 x_history (:,k+1) = x; % 保存系统输入 - 在线 u_history (:,k) = u; % 系统输入赋值 - 离线 u_offline = U_offline(k); % 计算系统给响应 - 离线 x2 = A * x2 + B * u_offline ; if k == 2 x2 = x2 +0.2 ; end % 保存系统状态 - 离线 x_history_2 (:,k+1) = x2; % 保存系统输入 - 离线 u_history_2 (:,k) = u_offline; end %%%%%%%%%%%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 系统状态结果视图在线vs.离线 subplot (2, 1, 1); hold; plot (0:length(x_history)-1,x_history(1,:)); plot (0:length(x_history_2)-1,x_history_2(1,:),'--'); grid on legend("在线 ","离线 ") hold off; xlim([0 k_steps-1]); % 系统输入结果视图在线vs.离线 subplot (2, 1, 2); hold; stairs (0:length(u_history)-1,u_history(1,:)); stairs (0:length(u_history_2)-1,u_history_2(1,:),'--'); legend("在线 ","离线 ") legend('location', 'southeast'); grid on xlim([0 k_steps-1]); 得到结果如下，可见离线算法会导致系统在“错误”的基础上继续按照原计划执行轨迹追踪问题分析稳态非零参考值控制以弹簧-阻尼系统纹理，分析使用MPC对其的调节能力。其控制思路如下图所示 MATLAB代码实现如下 clear clc %%%%%%%%%%%%%%%%%%%%%%%%%% %% 定义系统参数 % 定义质量块质量 m_sys = 1; % 定义阻尼系数 b_sys = 0.5; % 定义弹簧弹性系数 k_sys = 1; %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% % 构建系统矩阵A，n x n A = [0 1 ; -k_sys/m_sys -b_sys/m_sys]; % 计算A矩阵维度 n = size (A,1); % 构建输入矩阵B，n x p B = [0; 1/m_sys]; % 计算输入矩阵维度 p = size(B,2); C = [1, 0]; D = 0; %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% % 离散时间步长 Ts = 0.1; % 连续系统转离散系统 sys_d = c2d(ss(A,B,C,D),Ts); % 提取离散系统A矩阵 A = sys_d.a; % 提取离散系统B矩阵 B = sys_d.b; %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% % 设计状态权重系数矩阵, n x n Q = [1 0 ; 0 1]; % 设计终值权重系数矩阵, n x n S = [1 0; 0 1]; % 设计输入权重系数矩阵, p x p R = 1; %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% % 系统状态参考值 xd = [1 ; 0]; % 构建目标转移矩阵，参考式（4.5.4） AD = eye(n); ud = mldivide(B,(eye(n)-A)*xd); %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% % 初始化系统状态 x0 = [0; 0]; % 初始化状态赋值 x = x0; % 构建初始化增广矩阵，参考式（4.5.5b） xa = [x; xd]; % 系统输入初始化 u = 0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 定义系统运行步数 k_steps = 100; % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step x_history = zeros(n,k_steps); % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step u_history = zeros(p,k_steps); delta_u = 0; % 定义预测区间，预测区间要小于系统运行步数 N_P = 20; % 调用模块[F2]，计算增广矩阵以及ud [Aa,Ba,Qa,Sa,R,ud] = F2_InputAugmentMatrix_SS_U(A,B,Q,R,S,xd); % 调用模块[F4]计算二次规划需用到的矩阵 [Phi,Gamma,Omega,Psi,F,H]=F4_MPC_Matrices_PM(Aa,Ba,Qa,R,Sa,N_P); for k = 1 : k_steps % 调用模块[F5]利用二次规划计算无约束系统的控制 [delta_U,delta_u]= F5_MPC_Controller_noConstraints(xa,F,H,p); % 计算系统给响应 x = A * x + B * (delta_u + ud); % 构建增广状态xa xa = [x; xd]; % 保存系统状态 x_history (:,k+1) = x; % 保存系统输入 u_history (:,k) = delta_u + ud; end %%%%%%%%%%%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 状态变量结果图 subplot (2, 1, 1); hold; % 系统状态x1结果图，质量块位移 plot (0:length(x_history)-1,x_history(1,:)); % 系统状态x2结果图，质量块速度 plot (0:length(x_history)-1,x_history(2,:),'--'); grid on legend("x1","x2") hold off; xlim([0 k_steps]); ylim([-0.2 1.2]); % 系统输入结果图 subplot (2, 1, 2); % 系统输入结果图，施加在质量块上的力，f stairs (0:length(u_history)-1,u_history(1,:)); legend("u") grid on xlim([0 k_steps]); ylim([0 3]); 输出结果如下。可见在没有约束的条件下且当Np足够大时，MPC和LQR两种算法在本质上没有区别。输入增量控制其控制思路如下图所示以弹簧-阻尼系统为例，编写MATLAB代码如下 clear clc %%%%%%%%%%%%%%%%%%%%%%%%%% %% 定义系统参数 % 定义质量块质量 m_sys = 1; % 定义阻尼系数 b_sys = 0.5; % 定义弹簧弹性系数 k_sys = 1; %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% % 构建系统矩阵A，n x n A = [0 1 ; -k_sys/m_sys -b_sys/m_sys]; % 计算A矩阵维度 n = size (A,1); % 构建输入矩阵B，n x p B = [0; 1/m_sys]; % 计算输入矩阵维度 p = size(B,2); C = [1, 0]; D = 0; %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% % 离散时间步长 Ts = 0.1; % 连续系统转离散系统 sys_d = c2d(ss(A,B,C,D),Ts); % 提取离散系统A矩阵 A = sys_d.a; % 提取离散系统B矩阵 B = sys_d.b; %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% % 设计状态权重系数矩阵, n x n Q = [1 0 ; 0 1]; % 设计终值权重系数矩阵, n x n S = [1 0; 0 1]; % 设计输入权重系数矩阵, p x p R = 0.1; %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% % 系统状态参考值 xd = [1 ; 0]; % 构建目标转移矩阵，参考式（4.5.4） AD = eye(n); %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% % 初始化系统状态 x0 = [0; 0]; % 初始化状态赋值 x = x0; % 系统输入初始化 u0 = 0; % 初始输入赋值 u = u0; % 构建初始化增广状态矩阵，参考式（4.5.23） xa = [x; xd; u]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 定义系统运行步数 k_steps = 100; % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step x_history = zeros(n,k_steps); % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step u_history = zeros(p,k_steps); % 定义预测区间，预测区间要小于系统运行步数 N_P = 20; % 调用模块[F3]，计算Aa，Ba，Qa，Sa，和R [Aa,Ba,Qa,Sa,R] = F3_InputAugmentMatrix_Delta_U(A,B,Q,R,S,AD); % 调用模块[F4]计算二次规划需用到的矩阵 [Phi,Gamma,Omega,Psi,F,H]=F4_MPC_Matrices_PM(Aa,Ba,Qa,R,Sa,N_P); % for循环 for k = 1 : k_steps % 调用模块[F5]利用二次规划计算无约束系统的控制 [Delta_U, Delta_u] = F5_MPC_Controller_noConstraints(xa,F,H,p) % 结合输入增量计算系统输入 u = Delta_u + u; % 系统输入代入系统方程，计算系统响应 x = A * x + B * u; % 更新系统增广状态 xa = [x; xd; u]; % 保存系统状态到预先定义矩阵的相应位置 x_history (:,k+1) = x; % 保存系统输入到预先定义矩阵的相应位置 u_history (:,k) = u ; end %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% % 状态变量结果图 subplot (2, 1, 1); hold; % 系统状态x1结果图，质量块位移 plot (0:length(x_history)-1,x_history(1,:)); % 系统状态x2结果图，质量块速度 plot (0:length(x_history)-1,x_history(2,:),'--'); grid on legend("x1","x2") hold off; xlim([0 k_steps]); ylim([-0.2 1.2]); % 系统输入结果图 subplot (2, 1, 2); % 系统输入结果图，施加在质量块上的力，f stairs (0:length(u_history)-1,u_history(1,:)); legend("u") grid on xlim([0 k_steps]); ylim([0 3]); 得到结果如下，效果与LQR一致同时，使用这种方法追踪非常数的目标轨迹，MATLAB代码实现如下 clear clc %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 定义系统参数 % 定义质量块质量 m_sys = 1; % 定义阻尼系数 b_sys = 0.5; % 定义弹簧弹性系数 k_sys = 1; %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% % 构建系统矩阵A，n x n A = [0 1 ; -k_sys/m_sys -b_sys/m_sys]; % 计算A矩阵维度 n = size (A,1); % 构建输入矩阵B，n x p B = [0; 1/m_sys]; % 计算输入矩阵维度 p = size(B,2); C = [1, 0]; D = 0; %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% % 离散时间步长 Ts = 0.1; % 连续系统转离散系统 sys_d = c2d(ss(A,B,C,D),Ts); % 提取离散系统A矩阵 A = sys_d.a; % 提取离散系统B矩阵 B = sys_d.b; %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% % 设计状态权重系数矩阵, n x n Q = [1 0 ; 0 1]; % 设计终值权重系数矩阵, n x n S = [1 0; 0 1]; % 设计输入权重系数矩阵, p x p R = 0.1; %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% % 系统状态参考值 xd = [0 ; 0.2]; % 构建目标转移矩阵 AD = c2d(ss( [0 1; 0 0],[0;0],[1,0],0),0.1).A; %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% % 初始化系统状态 x0 = [0; 0]; % 初始化状态赋值 x = x0; % 系统输入初始化 u0 = 0; % 初始输入赋值 u = u0; % 构建初始化增广状态矩阵，参考式（4.5.23） xa = [x; xd; u]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 定义系统运行步数 k_steps = 200; % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_steps x_history = zeros(n,k_steps); % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_steps u_history = zeros(p,k_steps); % 定义xd_history零矩阵，用于储存系统目标状态，维度n x k_steps xd_history = zeros(n,k_steps); % 定义预测区间，预测区间要小于系统运行步数 N_P = 20; % 调用模块[F3]，计算Aa，Ba，Qa，Sa，和R [Aa,Ba,Qa,Sa,R] = F3_InputAugmentMatrix_Delta_U(A,B,Q,R,S,AD); % 调用模块[F4]计算二次规划需用到的矩阵 [Phi,Gamma,Omega,Psi,F,H] = F4_MPC_Matrices_PM(Aa,Ba,Qa,R,Sa,N_P); % for循环，建立可变参考值 for k = 1 : k_steps % 系统目标状态不再是常熟，if else语句定义不同阶段的系统目标状态 if (k==50) xd = [xd(1) ; -0.2]; elseif (k==100) xd = [xd(1) ; 0.2]; elseif (k==150) xd = [xd(1) ; -0.2]; elseif (k==200) xd = [xd(1) ; 0.2]; end % 调用模块[F5]利用二次规划计算无约束系统的控制（输入增量） [Delta_U, Delta_u] = F5_MPC_Controller_noConstraints(xa,F,H,p); % 计算系统输入 u = Delta_u + u; % 系统输入代入系统方程，计算系统响应 x = A * x + B * u; % 更新系统增广状态，参考式（4.5.23） xd = AD * xd; xa = [x; xd; u]; % 保存系统状态到预先定义矩阵的相应位置 x_history (:,k+1) = x; % 保存系统输入到预先定义矩阵的相应位置 u_history (:,k) = u ; % 保存系统目标状态到预先定义的矩阵的相应位置 xd_history (:,k+1) = xd; end %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% % 系统状态x1结果图，质量块位移 subplot (3, 1, 1); hold; plot (0:length(x_history)-1,x_history(1,:),"linewidth",1); % 目标值 plot (0:length(xd_history)-1,xd_history(1,:),'--',"linewidth",2); grid on legend("x1","x1d") hold off; xlim([0 k_steps]); ylim([-0.2 1.2]); % 系统状态x2结果图，质量块速度 subplot (3, 1, 2); hold; plot (0:length(x_history)-1,x_history(2,:),"linewidth",1); % 目标值 plot (0:length(xd_history)-1,xd_history(2,:),'--',"linewidth",2); grid on legend("x2","x2d") hold off; xlim([0 k_steps]); ylim([-0.5 0.5]); % 系统输入结果图 subplot (3, 1, 3); stairs (0:length(u_history)-1,u_history(1,:)); legend("u") grid on xlim([0 k_steps]); ylim([-0.5 1.4]); 得到结果如下，可见在无约束条件下，MPC没有展现出其优势与特点。
2025-02-27
回复了主题帖：《控制之美（卷2）——最优化控制MPC与卡尔曼滤波器》——轨迹追踪

lb8820265 发表于 2025-2-27 22:48 好巧，我刚好也买了这本书，这本书是写的真的好，帖主有在哪里用到MPC算法吗？轨迹规划的下一章就是MPC了，过几天我就更新
发表了主题帖：《控制之美（卷2）——最优化控制MPC与卡尔曼滤波器》——轨迹追踪

本帖最后由 FuShenxiao 于 2025-2-27 11:11 编辑在实际应用中，系统的控制目标在很多情况下都是追踪一个给定的参考轨迹或将系统稳定在一个非零的参考点。线性二次型调节器需要改进从而获得跟踪的性能。以弹簧阻尼系统为例，其中m=1kg，k=1N/m，b=0.5Ns/m，构建动态方程定义状态变量，代入得到使采样时间Ts=0.1s，得到离散后的系统其中考虑初始位置x[0]=[0; 0]，控制到达目标位置xd=[1; 0]，此时控制目标xd非零，因此不再是个调节问题，无法直接使用LQR结论进行处理。引入控制目标误差根据离散型状态空间方程，假设其控制目标为xd[k]，其动态方程为其中AD为目标状态的转移矩阵。当控制目标xd为常数时，AD=I，可得定义新的状态变量、状态矩阵以及输入矩阵如下得到新的状态空间方程定义状态误差令，得到引入误差之后，控制目标从轨迹追踪转化为对误差的“调节”，因此定义性能指标为经过整理代换得到最终性能指标此时我们可以使用反馈矩阵求解模块进行求解，构建MATLAB代码如下 clear clc %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 定义系统参数 % 定义质量块质量 m_sys = 1; % 定义阻尼系数 b_sys = 0.5; % 定义弹簧弹性系数 k_sys = 1; %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% % 构建系统矩阵A，n x n A = [0 1 ; -k_sys/m_sys -b_sys/m_sys]; % 计算A矩阵维度 n = size (A,1); % 构建输入矩阵B，n x p B = [0; 1/m_sys]; % 计算输入矩阵维度 p = size(B,2); C = [1, 0]; D = 0; %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% % 离散时间步长 Ts = 0.1; sys = ss(A,B,C,D); % 连续系统转离散系统 sys_d = c2d(sys,Ts); % 提取离散系统A矩阵 A = sys_d.a; % 提取离散系统B矩阵 B = sys_d.b; %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% % 设计状态权重系数矩阵, n x n Q = [1 0 ; 0 1]; % 设计终值权重系数矩阵, n x n S = [1 0; 0 1]; % 设计输入权重系数矩阵, p x p R = 1; %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% % 系统状态参考值 xd = [1 ; 0]; % 构建目标转移矩阵，参考式（4.5.4） AD = eye(n); %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% % 初始化系统状态 x0 = [0; 0]; % 初始化状态赋值 x = x0; % 构建初始化增广矩阵，参考式（4.5.5b） xa = [x; xd]; % 系统输入初始化 u = 0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 定义系统运行步数 k_steps = 100; % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step x_history = zeros(n,k_steps); % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step u_history = zeros(p,k_steps); % 构建增广矩阵Ca，参考式（4.5.6b） Ca =[eye(n) -eye(n)]; % 构建增广矩阵Aa，参考式（4.5.5b） Aa = [A zeros(n);zeros(n) AD]; % 构建增广矩阵Ba，参考式（4.5.5b） Ba = [B;zeros(n,1)]; % 构建增广矩阵Sa，参考式（4.5.8b） Sa = transpose(Ca)*S*Ca; % 构建增广矩阵Qa，参考式（4.5.8b） Qa = transpose(Ca)*Q*Ca; % 读取模块[F1]，计算系统反馈增益，F [F] = F1_LQR_Gain(Aa,Ba,Qa,R,Sa); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 仿真开始，建立for循环 for k = 1 : k_steps % 计算系统输入 u = -F * (xa); % 系统输入代入系统方程，计算系统响应 x = A * x + B * u; % 更新增广状态xa xa = [x; xd]; % 保存系统状态到预先定义矩阵的相应位置 x_history (:,k+1) = x; % 保存系统输入到预先定义矩阵的相应位置 u_history (:,k) = u ; end %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% % 状态变量结果图 subplot (2, 1, 1); hold; % 系统状态x1结果图，质量块位移 plot (0:length(x_history)-1,x_history(1,:)); % 系统状态x2结果图，质量块速度 plot (0:length(x_history)-1,x_history(2,:),'--'); grid on legend("x1","x2") hold off; xlim([0 k_steps]); ylim([-0.2 1.2]); % 系统输入结果图 subplot (2, 1, 2); % 系统输入结果图，施加在质量块上的力，f stairs (0:length(u_history)-1,u_history(1,:)); legend("u") grid on xlim([0 k_steps]); ylim([0 3]); 得到结果如下图可见这种方法无法将状态变量控制在目标值的位置。对于这个系统，若要将质量块稳定在平衡位置，需要持续施加拉力，然而性能指标却要求输入（即拉力）越小越好，因此为了实现更小的能耗，系统将无法达到目标。稳态非零参考值控制假设控制目标xd为常数，且系统达到目标xd时处于稳定状态，可得此时输入为目标输入ud时，状态变量将稳定在xd，此时定义状态误差定义稳态输入的误差定义系统的性能指标经过一些列代换，得到最终的性能指标为 MATLAB代码实现如下 clear clc %%%%%%%%%%%%%%%%%%%%%%%%%% %% 定义系统参数 % 定义质量块质量 m_sys = 1; % 定义阻尼系数 b_sys = 0.5; % 定义弹簧弹性系数 k_sys = 1; %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% % 构建系统矩阵A，n x n A = [0 1 ; -k_sys/m_sys -b_sys/m_sys]; % 计算A矩阵维度 n = size (A,1); % 构建输入矩阵B，n x p B = [0; 1/m_sys]; % 计算输入矩阵维度 p = size(B,2); C = [1, 0]; D = 0; %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% % 离散时间步长 Ts = 0.1; % 连续系统转离散系统 sys_d = c2d(ss(A,B,C,D),Ts); % 提取离散系统A矩阵 A = sys_d.a; % 提取离散系统B矩阵 B = sys_d.b; %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% % 设计状态权重系数矩阵, n x n Q = [1 0 ; 0 1]; % 设计终值权重系数矩阵, n x n S = [1 0; 0 1]; % 设计输入权重系数矩阵, p x p R = 0.1; %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% % 系统状态参考值 xd = [1 ; 0]; % 构建目标转移矩阵，参考式（4.5.4） AD = eye(n); %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% % 初始化系统状态 x0 = [0; 0]; % 初始化状态赋值 x = x0; % 构建初始化增广矩阵，参考式（4.5.5b） xa = [x; xd]; % 系统输入初始化 u = 0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 定义系统运行步数 k_steps = 100; % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step x_history = zeros(n,k_steps); % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step u_history = zeros(p,k_steps); % 调用模块[F2]，计算系统增广矩阵Aa，Ba，Qa，Sa，R以及目标输入ud [Aa,Ba,Qa,Sa,R,ud] = F2_InputAugmentMatrix_SS_U(A,B,Q,R,S,xd); % 调用模块[F1]，计算系统反馈增益，F [F] = F1_LQR_Gain(Aa,Ba,Qa,R,Sa); % 仿真开始，建立for循环 for k = 1 : k_steps % 计算系统输入 u = -F * (xa) + ud; % 系统输入代入系统方程，计算系统响应 x = A * x + B * u; % 更新增广状态，xa xa = [x; xd]; % 保存系统状态到预先定义矩阵的相应位置 x_history (:,k+1) = x; % 保存系统输入到预先定义矩阵的相应位置 u_history (:,k) = u ; end %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% % 状态变量结果图 subplot (2, 1, 1); hold; % 系统状态x1结果图，质量块位移 plot (0:length(x_history)-1,x_history(1,:)); % 系统状态x2结果图，质量块速度 plot (0:length(x_history)-1,x_history(2,:),'--'); grid on legend("x1","x2") hold off; xlim([0 k_steps]); ylim([-0.2 1.2]); % 系统输入结果图 subplot (2, 1, 2); % 系统输入结果图，施加在质量块上的力，f stairs (0:length(u_history)-1,u_history(1,:)); legend("u") grid on xlim([0 k_steps]); ylim([0 3]); 其中稳态非零控制矩阵转化模块代码如下 function [Aa,Ba,Qa,Sa,R,ud] = F2_InputAugmentMatrix_SS_U(A,B,Q,R,S,xd); % 计算系统矩阵维度，n n=size(A,1); % 计算输入矩阵维度，p p=size(B,2); % 构建增广矩阵Ca，参考式（4.5.17） Ca =[eye(n) -eye(n)]; % 构建增广矩阵Aa，参考式（4.5.16b） Aa = [A eye(n)-A;zeros(n) eye(n)]; % 构建增广矩阵Ba，参考式（4.5.16b） Ba = [B;zeros(n,p)]; % 构建增广矩阵Qa，参考式（4.5.18） Qa = transpose(Ca)*Q*Ca; % 构建增广矩阵Sa，参考式（4.5.18） Sa = transpose(Ca)*S*Ca; % 设计权重矩阵R（这里R不变） R = R; % 计算稳态控制输入ud ud = mldivide(B,(eye(n)-A)*xd); end 稳态非零参考值控制框图如下输出结果如下，可见利用该方法可以将控制器保持在稳态输入点附近。输入增量控制对于轨迹追踪控制，一种更灵活的方法是使用输入增量控制，这种方法可以追踪非常数参考值。定义输入增量代入状态方程设定系统性能指标为整理代换得到 MATLAB代码实现如下 clear clc %%%%%%%%%%%%%%%%%%%%%%%%%% %% 定义系统参数 % 定义质量块质量 m_sys = 1; % 定义阻尼系数 b_sys = 0.5; % 定义弹簧弹性系数 k_sys = 1; %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% % 构建系统矩阵A，n x n A = [0 1 ; -k_sys/m_sys -b_sys/m_sys]; % 计算A矩阵维度 n = size (A,1); % 构建输入矩阵B，n x p B = [0; 1/m_sys]; % 计算输入矩阵维度 p = size(B,2); C = [1, 0]; D = 0; %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% % 离散时间步长 Ts = 0.1; % 连续系统转离散系统 sys_d = c2d(ss(A,B,C,D),Ts); % 提取离散系统A矩阵 A = sys_d.a; % 提取离散系统B矩阵 B = sys_d.b; %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% % 设计状态权重系数矩阵, n x n Q = [1 0 ; 0 1]; % 设计终值权重系数矩阵, n x n S = [1 0; 0 1]; % 设计输入权重系数矩阵, p x p R = 0.1; %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% % 系统状态参考值 xd = [1 ; 0]; % 构建目标转移矩阵，参考式（4.5.4） AD = eye(n); %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% % 初始化系统状态 x0 = [0; 0]; % 初始化状态赋值 x = x0; % 系统输入初始化 u0 = 0; % 初始输入赋值 u = u0; % 构建初始化增广状态矩阵，参考式（4.5.23） xa = [x; xd; u]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 定义系统运行步数 k_steps = 100; % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step x_history = zeros(n,k_steps); % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step u_history = zeros(p,k_steps); % 调用模块[F3]，计算Aa，Ba，Qa，Sa，和R [Aa,Ba,Qa,Sa,R] = F3_InputAugmentMatrix_Delta_U(A,B,Q,R,S,AD); % 调用模块[F1]，计算系统反馈增益，F [F] = F1_LQR_Gain(Aa,Ba,Qa,R,Sa); % 仿真开始，建立for循环 for k = 1 : k_steps % 计算系统输入增量 Delta_u = -F * xa; % 计算系统输入，参考式（4.5.20） u = Delta_u + u; % 系统输入代入系统方程，计算系统响应 x = A * x + B * u; % 更新系统增广状态，参考式（4.5.23） xd = AD * xd; xa = [x; xd; u]; % 保存系统状态到预先定义矩阵的相应位置 x_history (:,k+1) = x; % 保存系统输入到预先定义矩阵的相应位置 u_history (:,k) = u ; end %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% % 状态变量结果图 subplot (2, 1, 1); hold; % 系统状态x1结果图，质量块位移 plot (0:length(x_history)-1,x_history(1,:)); % 系统状态x2结果图，质量块速度 plot (0:length(x_history)-1,x_history(2,:),'--'); grid on legend("x1","x2") hold off; xlim([0 k_steps]); ylim([-0.2 1.2]); % 系统输入结果图 subplot (2, 1, 2); % 系统输入结果图，施加在质量块上的力，f stairs (0:length(u_history)-1,u_history(1,:)); legend("u") grid on xlim([0 k_steps]); ylim([0 3]); 其中输入增量控制矩阵转化模块代码实现如下 function [Aa,Ba,Qa,Sa,R] = F3_InputAugmentMatrix_Delta_U(A,B,Q,R,S,AD); % 计算系统矩阵维度，n n=size(A,1); % 计算输入矩阵维度，p p=size(B,2); % 构建增广矩阵Ca，参考式（4.5.25） Ca =[eye(n) -eye(n) zeros(n,p)]; % 构建增广矩阵Aa，参考式（4.5.24b） Aa = [A zeros(n) B;zeros(n) AD zeros(n,p);zeros(p,n) zeros(p,n) eye(p,p)]; % 构建增广矩阵Ba，参考式（4.5.24b） Ba = [B;zeros(n,p);eye(p)]; % 构建增广矩阵Qa，参考式（4.5.26） Qa = transpose(Ca)*Q*Ca; % 构建增广矩阵Sa，参考式（4.5.26） Sa = transpose(Ca)*S*Ca; % 设计权重矩阵R（这里R不变） R = R; end 输入增量控制框图如下输出结果如下，这种方法的快速性略有下降，但是可以有效减少超调。输入增量控制——追踪非常数参考值使用输入增量控制还可以令系统追踪一个非常数的随时间变化的线性目标状态。假设零弹簧阻尼系统匀速运动，其微分方程为希望质量块以0.2m/s的速度匀速运动并且每5s转换一次方向，利用输入增量控制编写MATLAB代码如下 clear clc %%%%%%%%%%%%%%%%%%%%%%%%%% %% 定义系统参数 % 定义质量块质量 m_sys = 1; % 定义阻尼系数 b_sys = 0.5; % 定义弹簧弹性系数 k_sys = 1; C = [1, 0]; D = 0; %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% % 构建系统矩阵A，n x n A = [0 1 ; -k_sys/m_sys -b_sys/m_sys]; % 计算A矩阵维度 n = size (A,1); % 构建输入矩阵B，n x p B = [0; 1/m_sys]; % 计算输入矩阵维度 p = size(B,2); %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% % 离散时间步长 Ts = 0.1; % 连续系统转离散系统 sys_d = c2d(ss(A,B,C,D),Ts); % 提取离散系统A矩阵 A = sys_d.a; % 提取离散系统B矩阵 B = sys_d.b; %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% % 设计状态权重系数矩阵, n x n Q = [1 0 ; 0 1]; % 设计终值权重系数矩阵, n x n S = [1 0; 0 1]; % 设计输入权重系数矩阵, p x p R = 1; %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% % 系统状态参考值 xd = [0 ; 0.2]; % 构建目标转移矩阵 AD = c2d(ss( [0 1; 0 0],[0;0],[1,0],0),0.1).A; %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% % 初始化系统状态 x0 = [0; 0]; % 初始化状态赋值 x = x0; % 系统输入初始化 u0 = 0; % 初始输入赋值 u = u0; % 构建初始化增广状态矩阵，参考式（4.5.23） xa = [x; xd; u]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 定义系统运行步数 k_steps = 200; % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_steps x_history = zeros(n,k_steps); % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_steps u_history = zeros(p,k_steps); % 定义xd_history零矩阵，用于储存系统目标状态，维度n x k_steps xd_history = zeros(n,k_steps); % 调用模块[F3]，计算Aa，Ba，Qa，Sa，和R [Aa,Ba,Qa,Sa,R] = F3_InputAugmentMatrix_Delta_U(A,B,Q,R,S,AD); % 调用模块[F1]，计算系统反馈增益，F [F] = F1_LQR_Gain(Aa,Ba,Qa,R,Sa); % 仿真开始，建立for循环 for k = 1 : k_steps % 系统目标状态不再是常数，if else语句定义不同阶段的系统目标状态 if (k==50) xd = [xd(1) ; -0.2]; elseif (k==100) xd = [xd(1) ; 0.2]; elseif (k==150) xd = [xd(1) ; -0.2]; elseif (k==200) xd = [xd(1) ; 0.2]; end % 计算系统输入增量 Delta_u = -F * xa; % 计算系统输入 u = Delta_u + u; % 系统输入代入系统方程，计算系统响应 x = A * x + B * u; % 更新系统增广状态，参考式（4.5.23） xd = AD * xd; xa = [x; xd; u]; % 保存系统状态到预先定义矩阵的相应位置 x_history (:,k+1) = x; % 保存系统输入到预先定义矩阵的相应位置 u_history (:,k) = u ; % 保存系统目标状态到预先定义的矩阵的相应位置 xd_history (:,k+1) = xd; end %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% % 系统状态x1结果图，质量块位移 subplot (3, 1, 1); hold; plot (0:length(x_history)-1,x_history(1,:),"linewidth",1); plot (0:length(xd_history)-1,xd_history(1,:),'--',"linewidth",2); grid on legend("x1","x1d") hold off; xlim([0 k_steps]); ylim([-0.2 1.2]); % 系统状态x2结果图，质量块速度 subplot (3, 1, 2); hold; plot (0:length(x_history)-1,x_history(2,:),"linewidth",1); plot (0:length(xd_history)-1,xd_history(2,:),'--',"linewidth",2); grid on legend("x2","x2d") hold off; xlim([0 k_steps]); ylim([-0.5 0.5]); % 系统输入结果图 subplot (3, 1, 3); stairs (0:length(u_history)-1,u_history(1,:)); legend("u") grid on xlim([0 k_steps]); ylim([-0.5 1.4]); 输出结果如下，可见跟踪性能较好。在质量块转变方向时，可能会出现超调。对于含饱和函数的轨迹追踪控制，以无人机控制为例，其控制框图如下 MATLAB代码实现如下 clear clc %%%%%%%%%%%%%%%%%%%%%%%%%% %% 定义系统参数 % 定义无人机质量 m = 1; % 定义重力加速度常数 g = 10; %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% % 构建系统矩阵A，n x n A = [0 1 0; 0 0 1 ;0 0 0]; % 计算A矩阵维度 n= size (A,1); % 构建输入矩阵B，n x p B = [0; 1/m; 0]; % 计算输入矩阵维度 p = size(B,2); C = [1, 0, 0]; D = 0; %%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%%%%%%%%%%%% % 离散时间步长 Ts = 0.1; % 连续系统转离散系统 sys_d = c2d(ss(A,B,C,D),Ts); % 提取离散系统A矩阵 A = sys_d.a; % 提取离散系统B矩阵 B = sys_d.b; %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% % 设计状态权重系数矩阵, n x n Q=[1 0 0; 0 1 0; 0 0 0]; % 设计终值权重系数矩阵, n x n S=[1 0 0; 0 1 0; 0 0 0]; % 设计输入权重系数矩阵, p x p R=0.1; %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% % 系统状态参考值 xd = [10 ; 0 ; -10]; % 构建目标转移矩阵 AD = eye(n); %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% % 初始化系统状态 x0 = [0; 0; -10 ]; x = x0; % 初始化增广状态矩阵 xa = [x; xd]; % 初始化系统输入 u0 = 0; u = 0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 定义系统运行步数 k_steps = 100; % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step x_history = zeros(n,k_steps); % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step u_history = zeros(p,k_steps); % 调用模块[F2]，计算系统增广矩阵Aa，Ba，Qa，Sa，R以及目标输入ud [Aa,Ba,Qa,Sa,R,ud] = F2_InputAugmentMatrix_SS_U(A,B,Q,R,S,xd); % 调用模块[F1]，计算系统反馈增益，F [F] = F1_LQR_Gain(Aa,Ba,Qa,R,Sa); % 系统输入限制 u_max = 12; %上限 u_min = 7; %下限 % 仿真开始，建立for循环 for k = 1 : k_steps % 计算系统输入 u = -F * (xa) + ud; % 施加系统输入限制（基于饱和函数的硬限制） u = min(max(u,u_min),u_max); % 系统输入代入系统方程，计算系统响应 x = A * x + B * u; % 更新增广矩阵xa xa = [x; xd]; % 保存系统状态到预先定义矩阵的相应位置 x_history (:,k+1) = x; % 保存系统输入到预先定义矩阵的相应位置 u_history (:,k) = u ; end %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% % 状态变量结果图 subplot (2, 1, 1); hold; % 无人机高度 plot (0:length(x_history)-1,x_history(1,:)); % 无人机速度 plot (0:length(x_history)-1,x_history(2,:),'--'); grid on legend("x1","x2") hold off; xlim([0 k_steps]); ylim([0 10.2]); subplot (2, 1, 2); hold; % 系统输入 stairs (0:length(u_history)-1,u_history(1,:)); legend("u") grid on xlim([0 k_steps]); ylim([7 12]); 输出结果如下
2025-02-25
回复了主题帖：嵌入式Rust修炼营：动手写串口烧录工具和MCU例程，Rust达人Hunter直播带你入门Rust

-参与理由&个人编程基础：针对嵌入式软件开发已经有一定的经验，且在各个电子类网站测评多款开发板，希望能借助Rust开发拓宽未来的开发思路。编程基础上，对于单片机的应用较为熟悉，C语言的应用较为熟练；C++的熟练程度仅停留在与C语言对应的部分，面向对象的部分不是很熟悉；Python主要用于算法验证和深度学习，主要依赖其易用性和丰富的包，而对于偏向底层编程的MicroPython不是很熟练。 -查看修炼任务和活动时间表，预估可以跟着完成几级任务（初级、中级、高级）：本人根据修炼任务和活动时间表，预估可以跟着完成初级、中级、高级任务。 -如探索过Rust，请说明Rust学习过程遇到难点，希望在参与活动中收获什么？尚未学习过Rust，希望在此次活动中能够入门Rust，体验其高性能、内存友好等优点，并且拓宽未来的开发思路。
发表了主题帖：《控制之美（卷2）——最优化控制MPC与卡尔曼滤波器》——线性二次型调节器

本帖最后由 FuShenxiao 于 2025-2-25 20:55 编辑线性二次型调节器（LQR）应用于一类最为典型的问题：系统为线性且性能指标采用二次型的形式，控制目标是将状态变量稳定在0（调节问题）。离散型线性二次型系统离散型线性系统状态空间方程为定义二次型性能指标其中采用逆向分级求解方法，系统从k=N运行到k=N的代价为定义，推导得到经过矩阵求导，整理得到系统从k=N-1运行到k=N的最优剩余代价为继续计算系统从k=N-2到k=N的性能指标，得到最优剩余代价为从而归纳出最优控制策略的一般形式其中初始条件其求解过程如下图所示。当N=∞时，F[N-k]将趋向于常数矩阵F。在使用中可以离线计算出这一常数矩阵并将其施加到系统中。使用常数反馈矩阵也是LQR最为常用的方法，为了确定这一常数矩阵，可以尝试循环求解多组结果直到F收敛为零。 LQR反馈控制系统如下图所示。通过离线计算出反馈矩阵F，根据当前状态变量x[k]计算最优控制量u*[k]并输入系统中，得到的新状态变量x[k+1]将倍用作下一时刻的反馈量，计算下一时刻的最优控制输入u*[k+1]，以此循环。离散型一维案例分析——LQR方法定义一维系统初始条件，控制目标。系统性能指标其中A=B=[1]，P[0]=S=R=[1]。考虑末端时刻N=2。当k=1时，F[N-k]=F[2-1]=F[1]，求得反馈得到当k=2时，F[N-k]=F[2-2]=F[0]，得到反馈得到依此类推，循环迭代得到一个收敛值F[k] MATLAB代码实现如下 clear clc %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% % 定义系统矩阵A A = 1; % 计算系统矩阵A的维度 n = size (A,1); % 定义输入矩阵B B = 1; % 计算输入矩阵B的维度 p = size(B,2); %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% % 设计系统状态权重矩阵Q，维度n x n Q = 1; % 设计系统终值权重矩阵S，维度n x n S = 1; % 设计系统输入权重矩阵R，维度p x p R = 1; %%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%% % 定义系统参考值xd xd = 0; %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% % 状态初始化 x0 = 1; x = x0; % 输入初始化 u0 = 0; u = u0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 定义系统运行步数 k_steps = 20; % 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step x_history = zeros(n,k_steps); % 将系统状态初始值赋值到x_history矩阵第一个位置 x_history (:,1) = x; % 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step u_history = zeros(p,k_steps); % 将系统输入初始值赋值到u_history矩阵第一个位置 u_history (:,1) = u; % 定义控制区间，对LQR控制而言，控制区间与运行步数，k_steps，一致 N = k_steps; % 读取模块[F1]，计算系统反馈增益，F [F] = F1_LQR_Gain(A,B,Q,R,S); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 仿真开始，建立for循环 for k = 1 : k_steps % 计算系统输入 u = -F * x; % 系统输入代入系统方程，计算系统响应 x = A * x + B * u; % 保存系统状态到预先定义矩阵的相应位置 x_history (:,k+1) = x; % 保存系统输入到预先定义矩阵的相应位置 u_history (:,k) = u; end %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 结果视图：系统状态vs.运行步数 subplot (2, 1, 1); plot (x_history(1,:)); legend("x1") grid on %% 结果视图：系统输入vs.运行步数 subplot (2, 1, 2); hold; plot (u_history(1,:)); legend("u1") grid on 其中计算系统反馈增益函数F1_LQR_Gain()代码如下 %% 输入：系统矩阵 A，B；权重矩阵 Q，R，S %% 输出：反馈增益矩阵F function [F] = F1_LQR_Gain(A,B,Q,R,S); % 计算系统矩阵维度，n n=size(A,1); % 计算输入矩阵维度，p p=size(B,2); % 系统终值代价权重矩阵，定义为P0 P0 = S; % 定义最大迭代次数，用于限制程序运行时间 max_iter = 200; % 初始化矩阵P为0矩阵，后续用于存放计算得到的一系列矩阵P[k] P = zeros(n,n*max_iter); % 初始化矩阵P的第一个位置为P0 P(:,1:n) = P0; % 定义P[k-1]的初值为P0，即当k=1时，参考式（4.4.23）与（4.4.24） P_k_min_1 = P0; % 定义系统稳态误差阈值，用于判断系统是否到达稳态 tol = 1e-3; % 初始化系统误差为无穷 diff = Inf; % 初始化系统反馈增益为无穷 F_N_min_k = Inf; % 初始化系统迭代步 k = 1; % 判断系统是否达到稳态，即相邻步的增益差是否小于预设阈值，如达到稳态跳出while循环 while diff > tol % 将系统增益F[N-k]赋值给Fpre[N-k]，此步骤用于判断系统是否达到稳态 F_N_min_k_pre = F_N_min_k; % 计算F[N-k],参考式（4.4.23b） F_N_min_k = inv(R+B'*P_k_min_1*B)*B'*P_k_min_1*A; % 计算P[k]，参考式（4.4.24b） P_k = (A-B*F_N_min_k)'*P_k_min_1*(A-B*F_N_min_k)+(F_N_min_k)'*R*(F_N_min_k)+Q; % 将P[k]矩阵存入P矩阵的相应位置 P(:,n*k-n+1:n*k) = P_k; % 更新P[k-1],用于下一次迭代 P_k_min_1 = P_k; % 计算系统相邻步增益差值 diff = abs(max(F_N_min_k - F_N_min_k_pre)); % 迭代步加 1 k = k + 1; % 如程序超过预设最大迭代步，则报错 if k > max_iter error('Maximum Number of Iterations Exceeded'); end end % 输出系统迭代步 fprintf('No. of Interation is %d \n', k); % 模块输出：系统增益F F=F_N_min_k; end 得到系统状态和系统输入变化结果如下图所示连续型二次型系统考虑如下形式的连续型线性动态系统系统的二次型性能指标定义为其中构建哈密顿项为了求最小的哈密顿项，可令哈密顿项对于u(t)的偏导为0，经过一系列计算得到代数Riccati方程，这是利用贝尔曼最优化理论推导出的解决连续型二次型系统的最优化控制问题的有效工具最终得到连续型一维案例分析——LQR方法以平衡车控制为例，其系统为倒立摆系统，构建状态空间方程其中x(t)为小球参考竖直方向的角度和角速度，u(t)是单位化后的小车加速度。令x(t0)=[π/20;0]，定义性能指标其中，为小球角度与角速度的权重矩阵。得到得到反馈控制策略在MATLAB中仿真不同的权重矩阵Q对于系统的影响，代码实现如下 clear clc %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 定义系统参数 % 定义重力加速度 g = 10; % 定义平衡车连杆长度 d = 1; %%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%% % 定义系统矩阵A A = [0 1; g/d 0]; % 计算系统矩阵A的维度 n= size (A,1); % 定义输入矩阵B B = [0; 1]; % 计算输入矩阵B的维度 p = size(B,2); % 定义矩阵C C = [1, 0]; % 定义矩阵D D = 0; %%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%% % 设计系统状态权重矩阵q1，维度n x n,案例1 q1 = [100 0;0 1]; % 设计系统状态权重矩阵q2，维度n x n,案例2 q2 = [1 0;0 100]; % 设计系统状态权重矩阵q3，维度n x n,案例3 q3 = [1 0;0 1]; % 设计系统输入权重矩阵r1，维度p x p，案例1 r1 = 1; % 设计系统输入权重矩阵r2，维度p x p，案例2 r2 = 1; % 设计系统输入权重矩阵r3，维度p x p，案例3 r3 = 1; %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% % 状态初始化 x0 = [pi/20;0]; x = x0; %%%%%%%%%%%%%代数Riccati方程求解%%%%%%%%%%%%%%%% % Riccati方程求解，案例1 [P1,L1,G1] = care(A,B,q1,r1); % 计算反馈增益，参考式（4.4.52） K1=inv(r1)*B'*P1; % Riccati方程求解，案例2 [P2,L2,G2] = care(A,B,q2,r2); % 计算反馈增益，参考式（4.4.52） K2=inv(r2)*B'*P2; % Riccati方程求解，案例3 [P3,L3,G3] = care(A,B,q3,r3); % 计算反馈增益，参考式（4.4.52） K3=inv(r3)*B'*P3; %%%%%%%%%%%%%%%构建闭环系统%%%%%%%%%%%%%%%%%%%%% % 构建闭环状态空间方程，案例1 sys_cl1=ss(A-B*K1,[0;0],C,D); % 构建闭环状态空间方程，案例2 sys_cl2=ss(A-B*K2,[0;0],C,D); % 构建闭环状态空间方程，案例3 sys_cl3=ss(A-B*K3,[0;0],C,D); %%%%%%%%%%%%%%%系统仿真开始%%%%%%%%%%%%%%%%%%%%% t_span = 0.01; % 系统时间离散 t=0:t_span:5; % 闭环系统初值响应，案例1 [y1,t,x1]=initial(sys_cl1,x,t); % 闭环系统初值响应，案例2 [y2,t,x2]=initial(sys_cl2,x,t); % 闭环系统初值响应，案例3 [y3,t,x3]=initial(sys_cl3,x,t); %%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 系统响应视图，状态量x1（角度） subplot(3,1,1); % 测试1结果 plot(t,x1(:,1),"linewidth",2); % 近似计算x1的积分 x1_1_cost = x1(:,1)'*x1(:,1)*t_span; hold on; % 测试2结果 plot(t,x2(:,1),'--',"linewidth",2); % 近似计算x1的积分 x2_1_cost = x2(:,1)'*x2(:,1)*t_span; hold on; % 测试3结果 plot(t,x3(:,1),'-.',"linewidth",2); % 近似计算x1的积分 x3_1_cost = x3(:,1)'*x3(:,1)*t_span; legend('测试一','测试二','测试三',"FontSize",14); xlim([0 4]); grid on; hold off; %% 系统响应结果视图，状态量x2（角速度） subplot(3,1,2); % 测试1结果 plot(t,x1(:,2),"linewidth",2); % 近似计算x2的积分 x1_2_cost = x1(:,2)'*x1(:,2)*t_span; hold on; % 测试2结果 plot(t,x2(:,2),'--',"linewidth",2); % 近似计算x2的积分 x2_2_cost = x2(:,2)'*x2(:,2)*t_span; hold on; % 测试3结果 plot(t,x3(:,2),'-.',"linewidth",2); % 近似计算x2的积分 x3_2_cost = x3(:,2)'*x3(:,2)*t_span; legend('测试一','测试二','测试三',"FontSize",14); xlim([0 4]); grid on; hold off; %% 系统响应结果视图，输入（加速度） subplot(3,1,3); % 测试1结果 plot(t,-K1*x1',"linewidth",2); % 近似计算u的总代价 u1_cost = (-K1*x1')*(-K1*x1')'*t_span'; hold on; % 测试2结果 plot(t,-K2*x2','--',"linewidth",2); % 近似计算u的总代价 u2_cost = (-K2*x2')*(-K2*x2')'*t_span; hold on; % 测试3结果 plot(t,-K3*x3','-.',"linewidth",2); % 近似计算u的总代价 u3_cost = (-K3*x3')*(-K3*x3')'*t_span; legend('测试一','测试二','测试三',"FontSize",14); xlim([0 4]); grid on; hold off; 得到输出结果如下图，可见不同的控制系统都能成功将系统稳定到平衡点，然而不同权重的收敛速度和曲线包含的“面积”不同，实际操作中需要做到多个性能指标的平衡
2025-02-22
发表了主题帖：《控制之美（卷2）——最优化控制MPC与卡尔曼滤波器》——动态规划

本帖最后由 FuShenxiao 于 2025-2-22 15:42 编辑动态规划是贝尔曼最优控制的一个方法，是一种针对最优控制问题的优化方法，它将问题分解为一些列子问题，并使用递归的方式求解这些子问题，最终得到最优解。以无人机高度控制问题为例，首先建立无人机动态方程其输入与状态变量分别为：假设无人机质量m=1kg，重力加速度g=10m/s^2，得到无人机加速度为了简化分析，假设系统输入为控制无人机在最短时间内从地面上升到的高度，并保证无人机在静止位置出发，在达到目标高度的同时速度降为0m/s，即。t0为初始时间，tf为末端时间。同时为无人机设置约束条件：为了解决这个问题，由数值方法和解析方法两种方式数值方法暴力算法这种方法也可以称为穷举法。首先将系统进行离散化分析，即分析无人机所有的可能状态，包括速度和距离的离散化，然后根据这些状态求出所有可能路径所需的时间，具体如下图所示。这其中需要将不满足约束条件的路径过滤掉，再对所有可行路径所需的时间进行比较，选择最短的时间作为最优结果。然而，如果将离散区间缩小，离散区间个数增加，那么穷举法需要计算的路径数将大大增加，计算时间也会随之增长，因此暴力算法很难应用在实时控制中。逆向分级求解方法动态规划中最优路径在最后的阶段总会殊途同归，因此可以采用逆向分级的分析方法，将原问题分解为若干子问题（级）进行求解，可以有效地避免重复计算。简单来说，逆向分级求解方法与暴力算法类似，也需要将系统进行离散化分析，然而不同的地方在于：暴力算法将所有的可能性都列举出来，并全部计算代价逆向分级求解方法从终点出发向起点分析，每一级在计算剩余代价的同时还会计算得到的结果是否符合约束条件，如果不符合，则舍弃这个路径，然后根据前一级的剩余代价，得到从这一节点出发到终点的最小代价，直至计算到起点，从而得到最优路径，其实现结果如下图所示。动态规划查表法上述的这些计算都可以提前离线完成。将每个节点出发到重点的最小代价列在上图对应的表中。控制算法可以通过实时查表实现。子啊一些参数固定的应用中，可以利用这个方法提前将表格计算好并存入控制器中。在实时控制时只需读取表格中的内容即可选择最优的控制策略。将高度离散100，速度离散500，编写MATLAB代码如下。 clear clc %%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%% % 无人机高度初始化 h_init = 0 ; % 无人机速度初始化 v_init = 0 ; %%%%%%%%%%%%%%%%%系统终值定义%%%%%%%%%%%%%%%%%% % 无人机终点高度 h_final = 10; % 无人机终点速度 v_final = 0; %%%%%%%%%%%%%%%%%边界条件定义%%%%%%%%%%%%%%%%%% % 高度下限 h_min = 0; % 高度上限 h_max = 10; % 高度离散数 N_h = 100; % 速度下限 v_min = 0; % 速度上限 v_max = 3; % 速度离散数 N_v = 500; %%%%%%%%%%%%%%%%%创建离散向量%%%%%%%%%%%%%%%%%% % 高度向量 Hd = h_min : (h_max - h_min)/N_h: h_max; % 速度向量 Vd = v_min: (v_max - v_min)/N_v : v_max; % 无人机加速度上下限设置 u_min = -3; u_max = 2; % 定义初始剩余代价矩阵 J_costtogo = zeros(N_h + 1, N_v + 1); % 定义系统输入矩阵 Input_acc = zeros(N_h + 1, N_v + 1); %%%%%%%%%%%%%%%计算最后一级的情况%%%%%%%%%%%%%%% % 计算最后一级至上一级间平均速度矩阵 v_avg = 0.5 * (v_final + Vd); % 计算最后一级至上一级间用时矩阵 T_delta = (h_max - h_min)./(N_h * v_avg); % 计算最后一级至上一级间加速度矩阵 acc = (v_final - Vd)./T_delta; % 将用时存入代价矩阵 J_temp = T_delta; % 筛选超限的系统输入（加速度需满足上下限） [acc_x,acc_y] = find(acc < u_min | acc > u_max); % 通过线性检索命令找到加速度超限的位置 Ind_lin_acc = sub2ind (size(acc),acc_x,acc_y); % 将加速度超限位置的系统代价人为赋值为无穷大 J_temp (Ind_lin_acc) = inf; % 将更新的代价存入相应的剩余代价矩阵的对应元素的位置 J_costtogo(2,:) = J_temp; % 将对应的加速度存入系统输入矩阵的相应位置 Input_acc (2,:) = acc; %%%%%%%%%%%%%%%倒数第二级至第二级的情况%%%%%%%%%%%%%%% % 设计for循环进行逆向级间的更新（参考图4.2.7） for k = 3 : 1 : N_h % 构建速度方阵，代表相邻两级间速度的所有组合 [Vd_x, Vd_y] = meshgrid(Vd , Vd); % 计算级间平均速度矩阵 v_avg = 0.5 * (Vd_x + Vd_y); % 计算级间用时矩阵，这也是级间代价矩阵 T_delta = (h_max - h_min)./(N_h * v_avg); % 计算级间加速度矩阵 acc = (Vd_y - Vd_x)./T_delta; % 将级间用时赋值代价矩阵 J_temp = T_delta; % 筛选超限的系统输入（加速度需满足上下限） [acc_x, acc_y] = find(acc < u_min | acc > u_max); % 通过线性检索命令找到加速度超限的位置 Ind_lin_acc = sub2ind (size(acc),acc_x,acc_y); % 将加速度超限位置的系统代价人为赋值为无穷大 J_temp (Ind_lin_acc) = inf; %%%%%%%%%%%%%%%%%% 重要的步骤! %%%%%%%%%%%%%%%%%%% % 生成代价矩阵，注意需要将临时代价与上一步的剩余代价结合 J_temp = J_temp + meshgrid(J_costtogo(k-1,:))'; % 提取剩余代价矩阵的最小值 [J_costtogo(k,:), l] = min(J_temp) ; % 线性索引找到最小值的位置 Ind_lin_acc = sub2ind (size(J_temp), l, 1:length(l)); % 保存相应的系统输入 Input_acc (k,:) = acc(Ind_lin_acc) ; end %%%%%%%%%%%%%%%%第二级至第一级的情况%%%%%%%%%%%%%%%%% % 计算级间平均速度矩阵 v_avg = 0.5 * (Vd + v_init); % 计算级间用时矩阵 T_delta = (h_max - h_min)./(N_h * v_avg); % 计算级间加速度矩阵 acc = (Vd - v_init)./T_delta; % 级间用时存入临时代价矩阵 J_temp = T_delta; % 筛选超限的系统输入（加速度需满足上下限） [acc_x, acc_y] = find(acc < u_min | acc > u_max); % 通过线性检索命令找到加速度超限的位置 Ind_lin_acc = sub2ind (size(acc),acc_x,acc_y); % 将加速度超限位置的系统代价人为赋值为无穷大 J_temp (Ind_lin_acc) = inf; % Let certain elements to infitiy %%%%%%%%%%%%%%%%%% 重要的步骤! %%%%%%%%%%%%%%%%%%% % 生成代价矩阵，注意需要将临时代价与上一步的剩余代价结合 J_temp = J_temp + J_costtogo(N_h,:); % 提取剩余代价矩阵的最小值 [J_costtogo(N_h+1,1), l] = min(J_temp); % 线性索引找到最小值的位置 Ind_lin_acc = sub2ind (size(J_temp), l); % 保存相应的系统输入 Input_acc (N_h+1,1) = acc(Ind_lin_acc); %%%%%%%%%%%%%%%%%结果（画图）%%%%%%%%%%%%%%%%%%%%% % 初始化高度 h_plot_init = 0; % 初始化速度 v_plot_init = 0; % 初始化时间 t_plot_init = 0; % 定义加速度结果维度 acc_plot = zeros(length(Hd),1); % 定义速度结果维度 v_plot = zeros(length(Hd),1); % 定义高度结果维度 h_plot = zeros(length(Hd),1); % 定义时间维度 t_plot = zeros(length(Hd),1); % 定义高度初值 h_plot (1) = h_plot_init; % 定义速度初值 v_plot (1) = v_plot_init; % 定义时间初值 t_plot (1) = t_plot_init; %% 查表确定最优路线 for k = 1 : 1 : N_h % 确认高度最优索引 [min_h,h_plot_index] = min(abs(h_plot(k) - Hd)); % 群人速度最优索引 [min_v,v_plot_index] = min(abs(v_plot(k) - Vd)); % 查表确定最优系统输入位置 acc_index = sub2ind(size(Input_acc), N_h+2-h_plot_index, v_plot_index); % 将最优系统输入存入表格 acc_plot (k) = Input_acc(acc_index); % 计算无人机速度 v_plot (k + 1) = sqrt((2 * (h_max - h_min)/N_h * acc_plot(k))+ v_plot (k)^2); % Calculate speed and height % 计算无人机高度 h_plot (k + 1) = h_plot(k) + (h_max - h_min)/N_h; % 计算系统相应时刻 t_plot (k + 1) = t_plot (k)+2*(h_plot (k + 1) - h_plot(k))/(v_plot (k + 1) + v_plot (k)); end %%绘制视图%% % 绘制速度vs.高度视图 subplot(3,2,1); plot(v_plot,h_plot,'--o'),grid on; ylabel('h(m)'); xlabel('v(m/s)'); % 绘制加速度vs.高度视图 subplot(3,2,2); plot(acc_plot,h_plot,'--o'),grid on; ylabel('h(m)'); xlabel('a(m/s^2)'); % 绘制速度vs.时间视图 subplot(3,2,3); plot(t_plot,v_plot,'--o'),grid on; ylabel('v(m/s)'); xlabel('t(s)'); % 绘制高度vs.时间视图 subplot(3,2,4); plot(t_plot,h_plot,'--o'),grid on; ylabel('h(m)'); xlabel('t'); % 绘制加速度vs.时间视图 subplot(3,2,5); plot(t_plot,acc_plot,'--o'),grid on; ylabel('a(m/s^2)'); xlabel('t'); 得到结果如下解析方法——动态规划的递归关系使用数值方法非常灵活和方便，可以应用于非线性系统或者线性系统，同时也可以处理含有约束的控制问题。然而这种方法并不通用，针对每一个问题都需要具体分析。如果系统为线性时不变系统，则可以通过数学分析找到一个通用且准确的解析表达式。离散系统考虑一般形式的离散系统其中为状态变量，为系统输入（控制量）。当状态变量从初始状态x[0]向末端状态x[N]运行时，定义其性能指标为最优控制的目标是找到一系列合适的最优控制量u，使J最小。采用逆向分级的思想，从最后一级开始考虑，系统从k=N运行到k=N的代价当k=N-1时，系统从k=N-1运行到末端k=N时的代价为代入x[N]得到 x[N-1]是系统在N-1时刻的状态变量值，是一个定值。可以求出合适的u[N-1]使上式最小，因此最优的性能指标，即剩余代价，可以写成为了求解这个剩余价值的最小值，只需求解等式得到依此类推，可以得到最优剩余代价的一般形式为可以通过求解得到最优控制策略u[N-k] 连续系统考虑一般形式的不含约束的连续系统从任意的中间时刻t到末端时间tf的系统的性能指标为其中h()为末端代价，g()为运行代价，最优代价函数可以写成使用逆向分级的思想，将时间间隔[t,tf]分成两段[t,t+Δt]和[t+Δt,tf]，最优代价函数变为其中于是最优化代价函数可以写成假设的二阶偏导数存在且有界，可对其在点(x(t),t)附近进行泰勒级数展开代入最优代价函数当时，有最优代价对时间和状态变量的偏导数同时当时，最优代价函数中泰勒级数展开的高阶项也趋于0。控制区间τ∈[t,t+Δt]将变为τ=t，于是得到代入整理可得该式为一个偏微分方程式，当t=tf时，其边界条件为上两式称为HJB方程，其中min{}中的部分称为哈密顿项，即简化HJB方程得到其中第一项是最有代价对时间的偏导数，第二项是最小的哈密顿项。在使用过程中，首先需要求得最小的哈密顿项，之后再求解微分方程即可得到最优的控制策略。