三轴相机云台稳定器论文(可为diy提供理论帮助)
该项目的目的是实施3轴反馈控制系统相机云台稳定器。目标是制造轻巧的主动稳定器对稳定相机很有用。该系统基于配对的电动伺服电机带有位置传感器，以补偿不必要的运动。这使得确保相机始终与地面保持水平，并能保持稳定视频素材。为了简化问题，平台运动分为三个部分方向：偏航（ψ），俯仰（φ）和横滚（θ）。稳定器臂采用3D打印PLC材料并与伺服电机配对以建立两个轴彼此垂直。整个设备是手持式的，并且可以两个操作手柄，方便用户操作。最终产品表现相对能够很好地补偿俯仰和横滚轴周围的运动。的该系统体积庞大，但重量轻，可以轻松控制双手。系统有时也会崩溃，需要重新设置才能恢复操作。总而言之，系统按要求运行，从而实现了其目的。为了进一步发展，主要的改进领域是调节器补偿更流畅。此外，系统可能会大幅缩减至减小尺寸和重量。
1引言
1.1目的
总体目标是设计一个满足以下条件的系统。首先是建立明确的系统声明和性能目标。接下来是开发系统模型并对简化进行基本了解在实际系统上进行。第三是设计和实现一个控制系统符合设定的性能规格。然后，分析控制的鲁棒性必须了解系统的局限性。最后是硬件必须进行验证以证明系统按预期执行。
1.2motion
选择3轴万向稳定器是因为它符合所有标准并且可以制造并在一个月内进行了测试。稳定器的目标是保持顶部平台，即摄像机可以附加到3D空间中的水平参考平面上。使用的参考平面在校准阶段设置了将相机固定到位的功能。校准后，IMU传感器读取系统的预期运动。微控制器生成沿着滚动，俯仰和偏航的电动机命令，以稳定平台以实现任何预期的用途和意外的动作。需要进行这些调整，以免过冲并且足够快跟上缓慢到中度的运动。此外，系统该模型及其动力学是通过使用递归最小二乘法来发现的。该系统模型可用于将来的仿真和零误差跟踪控制。
1.3设计选择和局限性
由于时间限制，预算和潜在的复杂性，存在一些限制。设计云台稳定器。最初，设计包括带编码器的直流电动机以最小的约束来驱动沿着滚动，俯仰和偏航轴的运动。代替，伺服电机价格便宜，易于使用，不需要额外的控制器
实施。但是，这导致沿三轴运动，如MG996R伺服马达的最大限制为90◦
在两个方向上。因此，云台系统可以沿滚动，俯仰和偏航轴的运动最多补偿±90°。另一个硬件限制是MG996R伺服电机的运行速度为0.14s / 60°， 哪一个不能足够快地减轻大的干扰。
2实验细节
2.1机械构造
图1：三轴云台系统
图1显示了3轴的CAD模型为该项目设计的万向节稳定器。原型印刷有PCA塑料，并用M3螺栓组装。三MG996R伺服电机连接到云台由德州仪器（TI）的MSP432微控制器控制，并由四个串联的AA电池供电。在横轴上的小平台，摄像机可以放置。MPU9250 IMU传感器为放在手柄上以简化电线放置并仅阅读有意IMU传感器读取角度偏航角（ψ），俯仰角（φ）和侧倾角（θ）。使系统尽可能紧凑，安装臂设计为将质量接近滚动轴和俯仰轴的中点。
2.2电子组装
电机驱动器如何插入的完整原理图如图2所示。IMU传感器将数据发送到主微控制器，然后由主微控制器处理数据连续地生成三个伺服电机的命令，如框图如图3所示。
2.3 IMU传感器
该项目中使用的IMU传感器是Invensense的MPU-9250。它由3轴组成
加速度计，3轴陀螺仪和3轴磁力计。此外，六个16位模数转换器将传感器输出数字化。它与其他设备使用I2C通信设备并支持高达400kb / s的数据传输速度。编程到芯片中的是数字处理单元（DMP），可以对原始数据进行计算，从而可以MCU更快收集的位置值。此外，灵敏度不同可以为陀螺仪和加速度计读数设置。对于此项目，默认设置使用陀螺仪时为±250°/ s，加速度计为±8g。
图二
图三
2.4伺服电机
项目中使用的伺服电机是金属齿轮MS996R伺服电机，最大失速扭矩为11kg / cm。电机绕中性位置旋转约±90°提供给其信号引脚的PWM波的占空比。对于MG996R伺服电机，在20ms的时间周期内，有效占空比在5％至10％之间。占空比为7.5％适用于电机保持在空档位置。
2.5编程
编程是在Code Composer Studio（CCS）9.2.0中完成的，该软件是一个集成开发环境（IDE），支持德州仪器（TI）的微控制器。这包括优化的C / C ++编译器，源代码编辑器，项目构建环境，调试器，分析器和许多其他功能。使用的编程语言是Embedded C，使用的微控制器是MSP432P401R。
3理论
3.1主动稳定器
当今可用的两种主要类型的稳定器是无源和有源稳定器。被动式稳定器依靠与配重保持平衡，而主动式稳定器使用电动机将相机保持在正确的方向，因此选择了稳定器项目。为了抵消不必要的运动，系统需要确定摄像机是否处于
旋转离开其参考平面。这通常是通过惯性测量完成的单元（IMU）传感器，可感应加速度和角速度以确定角度三维空间中的相机。呈现角度的一种直观方法是将其分为三个不同的轴，即音高，偏航和滚动。该定义通常用于描述飞机的旋转角度，但可以很好地解释为相机的动作也一样。的方向轴如图4所示。所有三个轴均为彼此垂直，因此所有可能的旋转都可以分解为其滚动，俯仰和滚动组件。对于主动式相机稳定系统，俯仰和横滚轴是最重要的，因为围绕这些轴的意外移动使录制的视频似乎不稳定。偏航中的稳定例如，在运动应该平稳且一致的情况下，轴也可能是理想的在全景镜头中。
图4：滚动，俯仰和偏航轴
3.2 IMU传感器放置
IMU传感器的位置会显着影响控制系统的方法。有两个主要选项可将传感器放置在相机侧或支架上侧。如果将IMU放在摄像机平台上，则摄像机和IMU共享同一参考平面。这导致相机平台的三维角度为与传感器的角度相同。通过完全了解相机的确切方向可以非常精确地控制摄像机的位置。但是，所有角度
电动机的补偿会移动IMU，从而影响传感器数据。如果控制系统不充分，则由电动机引起的突然和急速运动可能会导致传感器值错误，并且相机平台将很容易围绕其参考振动点。或者，可以将传感器放置在系统的支撑侧，在这种情况下，案件是把柄。这种定位使平台和传感器的角度能够彼此独立旋转。这种方法大大降低了振动的风险相机侧，因为电动机不会影响传感器读数。不幸的是，这个传感器放置会加剧确定摄像机角度的能力。因为相机和传感器不共享相同的参考平面，必须估算平台的角度从传感器数据。这可以有效地完成，并且在适当的电动机控制下也可以很好地工作，小的估计误差可能会导致永久的角度偏移，直到重新校准系统为止。
4方向和传感器融合
4.1坐标系
为了将摄像机平台稳定在固定位置，重要的是可以相对于固定的物体（即固定的物体）测量摄像机的位置参考系（惯性系）。在此项目中，东北-东北（NED）参考框架已用于定向，其中“向下”与重力方向对齐。但是，传感器的测量值不能总是在惯性系中直接测量因为传感器测量的是相对于其自身参考系的量，而不是固定在空间中，请参见图5。这意味着必须转换测量数据
完成以获得惯性框架表示。一种方法是将欧拉角用作表示传感器框架相对于惯性框架的方向。
图5：相对于未固定传感器框架[1]的惯性参考系
4.2欧拉角
欧拉角用于描述惯性框架相对于运动的方向帧。角度分别表示为ψ，θ和φ，代表三个旋转的结果关于不同的轴。旋转称为偏航，俯仰和横滚，可以表示为
如公式1 [2]所示。较低和较高的索引代表旋转的开始和结束帧。得到代表所有三个的旋转矩阵旋转，所有三个旋转矩阵都必须按照偏航，俯仰和滚动的顺序相乘。这意味着偏航旋转矩阵将矢量从惯性系（I）旋转到所谓的第一中间帧（In1）。俯仰旋转矩阵从In1旋转到第二个中间框架（In2），最后侧倾旋转矩阵从In2到传感器框架（S）。公式2和最终旋转序列的旋转矩阵可以在公式3中看到，其中c和s表示余弦和正弦。
公式3可用于根据加速度计读数估算俯仰角和侧倾角。但是，陀螺仪读数无使用公式3在惯性系中表示。为此，需要公式4中的旋转矩阵。IMU传感器框架的轴为表示为Xs，Ys和Zs，并且当相机水平时Zs处于相反方向引力 如图所示，Xs与俯仰轴对齐，Y s与横滚轴对齐因此，在图4中，如果传感器框架中的角速度由ωx，ωy和ωz表示然后可以按照公式5计算欧拉角速率。
通过积分方程式5，可以用陀螺仪估算所有三个角度关于公式5的重要注意事项是，俯仰角为90°会引起一些矩阵元素向无穷大发散。这就是所谓的万向节锁现象设置欧拉角的限制。
4.3互补滤波器
传感器融合用于组合来自多个传感器的测量值，以获取更多信息可靠的信息。当分别使用陀螺仪和加速度计测量方向时传感器有其缺点。加速度计在以下情况下无法提供可靠的测量结果态度在变化，陀螺仪遭受时变偏差的困扰。互补过滤器是将每个传感器的最佳部分组合在一起，以获得更好地对应的估计
进入现实世界。频域可以用公式6 [5]描述。
其中A（s）和G（s）是加速度计信号的频域表示，而陀螺仪信号θ（s）欧拉角和τ每个滤波器的时间常数。所以这个主意互补滤波器用于低通滤波加速度计提供的数据，对陀螺仪的数据进行积分和高通滤波，然后将两个读数合并
给出更可靠的估计。嘈杂的加速度计读数被滤除并补充陀螺仪数据，同时陀螺仪的时变偏差也过滤掉并通过加速度计数据进行补充。因此，过滤器会提供一个估算值这是基于每个传感器的最佳测量结果。由于方程式6是连续的
时域，不能直接在计算机上实现，后向差异是用来离散化它。等式6变为
其中α=τ/（h +τ）。公式7可以在计算机上轻松实现，并且需要由于其简单性，计算量很少。
图6：读取IMU传感器的示意图
在硬件中，互补滤波器已根据公式7实现但是过滤器无法直接处理原始传感器数据。一些处理已经完成在滤波之前，请参见图6。为了从加速度计获得欧拉角必须使用三角法。公式8可用于转换加速度读数从IMU传感器获取ax，ay和az到俯仰φ和侧倾θ角[3]。
无法使用加速度计估算ψ角，因为它不会改变静态读数。还必须处理陀螺仪数据以获得接地框架表示，请参见公式5。
5轨迹控制
通常通过过程或执行器上的约束来设置轨迹。此外，轨迹生成可以在开环和闭环系统中使用。因为，没有来自伺服电机的反馈，为开环控制实现了轨迹控制仅系统。图7显示了开环轨迹系统的控制块。选择的轨迹是恒定的偏航旋转，以模拟捕获全景的相机。扰动在滚动轴或俯仰轴上均被独立排除。
图7：开环轨迹系统的控制块
6递归最小二乘
过去的研究表明，R / C伺服电动机在电动机之间具有三阶响应旋转角α和指令电压V [4]。离散关系在公式10中给出。
其中g，h，i，a，b，c是未知参数。通过将等式10转换为时间使用零保持一阶方法的域，电流输出α近似为方程式11。
其中k − i是在k读数之前i个时间步长的读数，βk是先前α的向量，并且V和γ是未知参数的向量。递归最小二乘（RLS）方法是通过以下等式估算参数。
其中E是估计的误差α-α，Pk是定义为的协方差矩阵
其中Pk-1是先前的协方差矩阵，而λ是强制E达到的标量因子在Pk之前变为零。使用试错法选择0.98的λ进行实施。假定在低频干扰下，电机角位移相等
由于万向架可以稳定慢速运动，因此可以在相反方向上进行角度位移。因此，IMU传感器读数可用于估计电动机旋转。
7结果与讨论
7.1原型
已构建了一个原型，如图4所示。已经对其进行了测试和评估，以评估其活动性。稳定。结果表明，它能够稳定所有方向的不耦合运动。三轴在一定程度上。相比俯仰轴，沿俯仰轴的运动非常平稳其他两个轴。对于俯仰轴，对于小角度来说，运动是平滑的。当音高角度接近零时，会发生万向节锁定，如第4.2节所述。另外，第三轴偏航轴无法正常运行，因为由于以下原因，未在最终结构中使用IMU传感器的磁力计组件微控制器计算能力的局限性.
7.2 IMU传感器
从IMU传感器获取的原始数据信号经过处理和滤波以获得摄像机角度的可靠位置。加速度计数据非常嘈杂，陀螺仪数据随时间漂移。为了解决这个问题引入了传感器融合算法通过对加速度计和陀螺仪数据应用互补滤波器。图??
突出显示嘈杂的加速度计数据和陀螺仪数据的漂移。漂流来自陀螺仪数据的误差是由积分误差引起的，即使出现偏差是零。漂移是累积积分读数的白噪声的结果。
（a）加速度计噪音
（b）陀螺仪漂移
图8：来自不带互补滤波器的IMU传感器的加速度计和陀螺仪角度
通过执行传感器融合算法，根据等式6，通过赋予信号不同的权重，将来自陀螺仪和加速度计的数据合并为一个信号。对于互补滤波器，将加速度计数据的权重设置为4％并将陀螺仪设置为90％，以获得平滑且经过滤波的信号，如在
图9。
图9：使用互补滤波器的滤波信号
7.3轨迹控制
观察到，控制器在低频干扰下稳定了平台。在执行轨迹的同时进行俯仰，滚动和偏航。但是，控制器无法以精确稳定平台，以防止耦合运动的干扰。这个限制基于控制器独立考虑角度的假设。进一步12此外，还需要逆运动学来获得平台的真实位置，但不是由于MSP432微控制器的计算限制而无法实现。
7.4递归最小二乘
RLS实现的结果在图10中找到。所有估计参数误差变为零时收敛到奇异值。每个电机的最终模型是总结于表1。
图10：递归最小二乘结果
表1：伺服电机的估计参数
图11比较了RLS和实际模型的估计模型的系统响应发动机。结果表明，估计模型可以合理地捕捉到发动机。针对变桨电动机和偏航电动机获得的模型显示出相似的结果。
图11：滚动电机的估计值与实际响应
8 结论与未来工作
三轴万向节满足导言中概述的目标。主要目标万向节的作用是通过保持顶部来减轻用户引入的干扰平台在同一参考平面上。系统模型是使用RLS和
电机角度和输入电压之间的关系为第三的假设订单系统。另一个假设是电动机旋转相等，并且缓慢移动手柄时，与手柄旋转方向相反。然后是一个控制器
创建用于稳定顶层平台的低频干扰，同时完成弹道。实验表明，通用控制系统仅有效动作缓慢。云台的构造和IMU传感器的位置读数可验证系统是否按预期执行。云台的下一步是稳定平台以进行耦合运动。这个可以通过将MSP432替换为Raspberry Pi微控制器来实现，运行逆运动学所需的计算能力。可以排除干扰通过在平台顶部添加IMU传感器来获得真正的电动机来进行改进
位置。该辅助IMU传感器允许实施反馈控制器可能会阻止大麻烦。该控制器的增益可以通过以下方式确定：由于获得了估计模型，因此实施了线性二次调节器（LQR）。一种长期目标是将3轴云台放置在移动平台上以镜像相机
薄膜工业中使用的稳定剂。
参考文献
[1]威廉·安德琳和埃拉·赫特伯格。“定位单元的陀螺仪稳定”。在（2019）。
[2]机器人。“了解欧拉角”。在2012年）。
[3]马克·佩德利。“使用三轴加速度计进行倾斜感测”。于：飞思卡尔半导体应用说明1（2013），第2012–2013页。
[4]和田隆等人。“ R / C伺服电机的实用建模和系统识别”。于：2009 IEEE控制应用程序（CCA）和智能控制（ISIC）。IEEE。2009年
第1378–1383页。
[5]若愚之。“ 3d中消除漂移的姿态和位置估计算法”。在：（2016）。
附录
答：最重要的反馈收到此词
1.将实验数据压缩到尽可能少的数字/仅显示相关数据数据
2.在讨论中介绍每个人物后对其进行分析
3.显示图形以清楚地显示信息（例如，增加字体大小和标记大小）
B. CCS通用用法和轨迹控制规范
/* DriverLib Defines */#include "driverlib.h"#include <math.h>/* Standard Defines */#include <string.h>#include <stdio.h>/* Slave Address for I2C Slave */#define SLAVE_ADDRESS 0x68#define pi 3.14159#define Gyro_Sensitivity 131.0#define Acc_Sensitivity 16384.0#define Readings 5000/* IMU sensor registers */uint8_t accXReg[] = {0x3B, 0x3C};uint8_t accYReg[] = {0x3D, 0x3E};uint8_t accZReg[] = {0x3F, 0x40};uint8_t gyrXReg[] = {0x43, 0x44};uint8_t gyrYReg[] = {0x45, 0x46};uint8_t gyrZReg[] = {0x47, 0x48};uint8_t WhoAmI;/* IMU Sensor Parameters */volatile float accRoll, accPitch; // Roll calculated from accelerometervolatile float gyroRoll = 0, gyroPitch = 0; // Roll calculated from gyroscopevolatile float roll = 0, pitch = 0, yaw = 0;float desired_angleX, desired_angleY;/* Calibration Parameters */volatile float AccErrorX = 0.0, AccErrorY = 0.0; // Accelerometer reading offsetvolatile float GyroErrorX = 0.0, GyroErrorY = 0.0, GyroErrorZ = 0.0; // Gyroscope readingoffset /* Time and PWM signal Parameter */ volatile int time = 0;volatile int currentTime = 0, previousTime;volatile float dT = 0; // in sec/* Variables to save */volatile int counter = 0;//volatile float data_Vroll[Readings] = {};//volatile float data_Vpitch[Readings] = {};//volatile float data_Vyaw[Readings] = {};volatile float data_roll[Readings] = {};volatile float data_pitch[Readings] = {};volatile float data_yaw[Readings] = {};/* Declare functions */void initialize_TIMER_A0(void);void initialize_PWM(void);void initializeI2C(void);void TA0_0_IRQHandler(void);int configRegRead(uint8_t *Register);int configSingleByteRegRead(uint8_t Register);void configRegWrite(uint8_t Register, uint8_t Data);void calculate_IMU_error(void);void angleCalculations(void);int servoMotorRoll(float angle);int servoMotorPitch(float angle);int servoMotorYaw(float angle);/* Timer_A UpMode Configuration Parameter */const Timer_A_UpModeConfig upConfig = // Configure counter in Up mode { TIMER_A_CLOCKSOURCE_SMCLK, // Tie Timer A to SMCLK TIMER_A_CLOCKSOURCE_DIVIDER_6, // Increment every 1ms 500, // Period of Timer A (this value placed in TAxCCR0) TIMER_A_TAIE_INTERRUPT_DISABLE, // Disable Timer A rollover interrupt TIMER_A_CCIE_CCR0_INTERRUPT_ENABLE, // Enable Capture Compare interrupt TIMER_A_DO_CLEAR // Clear counter upon initialization};/* Timer_A PWM Configuration Parameter */const Timer_A_PWMConfig pwmConfig1 = { TIMER_A_CLOCKSOURCE_SMCLK, // Tie Timer A to SMCLK TIMER_A_CLOCKSOURCE_DIVIDER_20, // Increment counter every 20 clock cycles 3000, // Timer Period (TAxCCR0) TIMER_A_CAPTURECOMPARE_REGISTER_1, // Compare Register (y) TIMER_A_OUTPUTMODE_RESET_SET, // Compare Output Mode (OUTMOD__7) 225 // Duty Cycle (TAxCCRy)};const Timer_A_PWMConfig pwmConfig2 = { TIMER_A_CLOCKSOURCE_SMCLK, // Tie Timer A to SMCLK TIMER_A_CLOCKSOURCE_DIVIDER_20, // Increment counter every 20 clock cycles 3000, // Timer Period (TAxCCR0) TIMER_A_CAPTURECOMPARE_REGISTER_4, // Compare Register (y) TIMER_A_OUTPUTMODE_RESET_SET, // Compare Output Mode (OUTMOD__7) 215 // Duty Cycle (TAxCCRy)};const Timer_A_PWMConfig pwmConfig3 = { TIMER_A_CLOCKSOURCE_SMCLK, // Tie Timer A to SMCLK TIMER_A_CLOCKSOURCE_DIVIDER_20, // Increment counter every 20 clock cycles 3000, // Timer Period (TAxCCR0) TIMER_A_CAPTURECOMPARE_REGISTER_3, // Compare Register (y) TIMER_A_OUTPUTMODE_RESET_SET, // Compare Output Mode (OUTMOD__7) 238 // Duty Cycle (TAxCCRy)};/* I2C Master Configuration Parameter */const eUSCI_I2C_MasterConfig i2cConfig = { EUSCI_B_I2C_CLOCKSOURCE_SMCLK, // SMCLK Clock Source 3000000, // SMCLK = 3MHz EUSCI_B_I2C_SET_DATA_RATE_400KBPS, // Desired I2C Clock of 400kHz 0, // No byte counter threshold EUSCI_B_I2C_NO_AUTO_STOP // No Autostop};void main(void){ /* Halting the Watchdog */ MAP_WDT_A_holdTimer(); /* Configure clock */ uint32_t dcoFrequency = 3E+6; // 3MHz MAP_CS_setDCOFrequency(dcoFrequency); // Set clock source to 3MHz MAP_CS_initClockSignal(CS_SMCLK, CS_DCOCLK_SELECT, CS_CLOCK_DIVIDER_1); /** Configure GPIO pins **/ /* Configuring P1.0 as output */ MAP_GPIO_setAsOutputPin(GPIO_PORT_P1, GPIO_PIN0); MAP_GPIO_setOutputLowOnPin(GPIO_PORT_P1, GPIO_PIN0); /* Configuring P2.0 as output */ MAP_GPIO_setAsOutputPin(GPIO_PORT_P2, GPIO_PIN0 || GPIO_PIN1 || GPIO_PIN2); MAP_GPIO_setOutputLowOnPin(GPIO_PORT_P2, GPIO_PIN0 || GPIO_PIN1 || GPIO_PIN2); /* Initializing Timer_A2 for PWM */ initialize_PWM(); /* Initialize I2C communication protocol */ initializeI2C(); /* Verify sensor communication */ WhoAmI = configSingleByteRegRead(0x75); // Used to verify the identity of sensor, this should read 0x71 if (WhoAmI == 0x71) { printf("IMU sensor communication established. \r\n"); MAP_GPIO_setOutputHighOnPin(GPIO_PORT_P2, GPIO_PIN2); } else { MAP_GPIO_setOutputHighOnPin(GPIO_PORT_P2, GPIO_PIN2); } /* Initializing Timer_A0 for interrupts */ initialize_TIMER_A0(); /* Starting calibration */ calculate_IMU_error(); MAP_GPIO_setOutputHighOnPin(GPIO_PORT_P1, GPIO_PIN0); double accX, accY, accZ; accX = configRegRead(accXReg) / Acc_Sensitivity; accY = configRegRead(accYReg) / Acc_Sensitivity; accZ = configRegRead(accZReg) / Acc_Sensitivity; /* Obtaining angle from accelerometer readings */ desired_angleX = (atan2(accY, sqrt(pow(accX, 2) + pow(accZ, 2))) * 180 / pi) - AccErrorX; // in degrees desired_angleY = (atan2(-accX, sqrt(pow(accY, 2) + pow(accZ, 2))) * 180 / pi) - AccErrorY; // in degrees gyroRoll = desired_angleX; gyroPitch = desired_angleY; MAP_GPIO_setOutputHighOnPin(GPIO_PORT_P2, GPIO_PIN0); /* Enabling MASTER interrupts */ MAP_Interrupt_enableMaster(); while (1) { float errorX, errorY, errorZ; /* Read sensor registers and calculate angles */ angleCalculations(); /* Calculate the error between current and desired angle */ errorX = roll - desired_angleX; errorY = pitch - desired_angleY; errorZ = yaw; /* Turn servo motor */ TA2CCR1 = servoMotorRoll(errorY) - 5; TA2CCR4 = servoMotorPitch(errorX) - 15; TA2CCR3 = servoMotorYaw(errorZ) + 8; }}void TA0_0_IRQHandler(void){ /* Clear the interrupt flag */ MAP_Timer_A_clearCaptureCompareInterrupt(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_0); time++;}void initializeI2C(void){ /* Select I2C function for I2C_SCL & I2C_SDA */ GPIO_setAsPeripheralModuleFunctionOutputPin(GPIO_PORT_P6, GPIO_PIN5, GPIO_PRIMARY_MODULE_FUNCTION); // I2C_SCL GPIO_setAsPeripheralModuleFunctionOutputPin(GPIO_PORT_P6, GPIO_PIN4, GPIO_PRIMARY_MODULE_FUNCTION); // I2C_SDA /* Initializing I2C Master to SMCLK at 400kbs with no autostop */ MAP_I2C_initMaster(EUSCI_B1_BASE, &i2cConfig); /* Enable I2C Module to start operations */ MAP_I2C_enableModule(EUSCI_B1_BASE); /* Specify slave address */ MAP_I2C_setSlaveAddress(EUSCI_B1_BASE, SLAVE_ADDRESS); /* Make reset, place a 0 into the 0x6B register */ configRegWrite(0x6B, 0x00); configRegWrite(0x1D, 0x06); // Acc Config: Programming digitally low pass filter}void initialize_TIMER_A0(void){ // Configuring Timer_A0 for Up Mode using above strut MAP_Timer_A_configureUpMode(TIMER_A0_BASE, &upConfig); // Enabling interrupts and starting the timer MAP_Interrupt_enableInterrupt(INT_TA0_0); // Start counting timer MAP_Timer_A_startCounter(TIMER_A0_BASE, TIMER_A_UP_MODE);}void initialize_PWM(void){ /* Set P5.6(T2.1), P5.7(2.2), and P6.6(T2.3) to be PWM functionality */ MAP_GPIO_setAsPeripheralModuleFunctionOutputPin(GPIO_PORT_P5, GPIO_PIN6, GPIO_PRIMARY_MODULE_FUNCTION); MAP_GPIO_setAsPeripheralModuleFunctionOutputPin(GPIO_PORT_P6, GPIO_PIN7, GPIO_PRIMARY_MODULE_FUNCTION); MAP_GPIO_setAsPeripheralModuleFunctionOutputPin(GPIO_PORT_P6, GPIO_PIN6, GPIO_PRIMARY_MODULE_FUNCTION); /* Configuring Timer_A to have a period of approximately 500ms and an initial duty cycle of10% of that (3200 ticks) */ MAP_Timer_A_generatePWM(TIMER_A2_BASE, &pwmConfig1); MAP_Timer_A_generatePWM(TIMER_A2_BASE, &pwmConfig2); MAP_Timer_A_generatePWM(TIMER_A2_BASE, &pwmConfig3);}int configRegRead(uint8_t *Register){ int i = 0; uint8_t RXData[2]; int16_t Data; for (i = 0; i < 2; i++) { /* Set write mode. */ MAP_I2C_setMode(EUSCI_B1_BASE, EUSCI_B_I2C_TRANSMIT_MODE); // Write START + address + register to the sensor. MAP_I2C_masterSendSingleByte(EUSCI_B1_BASE, Register[i]); /* Making sure that the last transaction has been completely sent */ while (MAP_I2C_masterIsStopSent(EUSCI_B1_BASE)) { } // Indicates whether STOP got sent. // Set read mode. MAP_I2C_setMode(EUSCI_B1_BASE, EUSCI_B_I2C_RECEIVE_MODE); // RESTART. MAP_I2C_masterReceiveStart(EUSCI_B1_BASE); // Read two bytes from the sensor, STOP. RXData[i] = MAP_I2C_masterReceiveMultiByteFinish(EUSCI_B1_BASE); } /* Parse the sensor's data. */ Data = (int16_t)(RXData[0] << 8 | RXData[1]); return Data;}int configSingleByteRegRead(uint8_t Register){ uint8_t RxData; /* Set write mode. */ MAP_I2C_setMode(EUSCI_B1_BASE, EUSCI_B_I2C_TRANSMIT_MODE); // Write START + address + register to the sensor. MAP_I2C_masterSendSingleByte(EUSCI_B1_BASE, Register); /* Making sure that the last transaction has been completely sent */ while (MAP_I2C_masterIsStopSent(EUSCI_B1_BASE)) { } // Indicates whether STOP got sent. // Set read mode. MAP_I2C_setMode(EUSCI_B1_BASE, EUSCI_B_I2C_RECEIVE_MODE); // RESTART. MAP_I2C_masterReceiveStart(EUSCI_B1_BASE); // Read two bytes from the sensor, STOP. RxData = MAP_I2C_masterReceiveMultiByteFinish(EUSCI_B1_BASE); return RxData;}void configRegWrite(uint8_t Register, uint8_t Data){ int i = 0; // Set write mode. MAP_I2C_setMode(EUSCI_B1_BASE, EUSCI_B_I2C_TRANSMIT_MODE); /* Send start bit and register */ MAP_I2C_masterSendMultiByteStart(EUSCI_B1_BASE, Register); // Delay for (i = 0; i < 2000; i++) { } /* Now write the data byte */ MAP_I2C_masterSendMultiByteFinish(EUSCI_B1_BASE, Data); while (MAP_I2C_masterIsStopSent(EUSCI_B1_BASE)) { } // Indicates whether STOP got sent.}void calculate_IMU_error(void){ /* Note: Place the IMU flat in order to get the proper values. * This is the calibration process */ int c = 0; float accX, accY, accZ; // Store accelerometer register raw sensor reading float GyroX, GyroY, GyroZ; // Store gyroscope register raw sensor reading // Read accelerometer and gyroscope values 200 times while (c < 200) { GyroX = configRegRead(gyrXReg) / Gyro_Sensitivity; GyroY = configRegRead(gyrYReg) / Gyro_Sensitivity; GyroZ = configRegRead(gyrZReg) / Gyro_Sensitivity; accX = configRegRead(accXReg) / Acc_Sensitivity; accY = configRegRead(accYReg) / Acc_Sensitivity; accZ = configRegRead(accZReg) / Acc_Sensitivity; AccErrorX = AccErrorX + (atan2(accY, sqrt(pow(accX, 2) + pow(accZ, 2))) * 180 / pi); AccErrorY = AccErrorY + (atan2(-accX, sqrt(pow(accY, 2) + pow(accZ, 2))) * 180 / pi); GyroErrorX = GyroErrorX + GyroX; GyroErrorY = GyroErrorY + GyroY; GyroErrorZ = GyroErrorZ + GyroZ; c++; } /* Average 200 values to get the error value */ AccErrorX = AccErrorX / 200.0; AccErrorY = AccErrorY / 200.0; GyroErrorX = GyroErrorX / 200.0; GyroErrorY = GyroErrorY / 200.0; GyroErrorZ = GyroErrorZ / 200.0;}void angleCalculations(void){ double accX, accY, accZ; // Store accelerometer register raw sensor reading float GyroX, GyroY, GyroZ; // Store gyroscope register raw sensor reading /* Euler Angle transformation parameters */ float GyroRateRoll, GyroRatePitch, GyroRateYaw; /* Accelerometer and gyroscope register readings */ /************************************************************** Gyroscope Register Readings **************************************************************/ GyroX = configRegRead(gyrXReg) / Gyro_Sensitivity - GyroErrorX; GyroY = configRegRead(gyrYReg) / Gyro_Sensitivity - GyroErrorY; GyroZ = configRegRead(gyrZReg) / Gyro_Sensitivity - GyroErrorZ; /************************************************************** Accelerometer Register Readings **************************************************************/ accX = configRegRead(accXReg) / Acc_Sensitivity; accY = configRegRead(accYReg) / Acc_Sensitivity; accZ = configRegRead(accZReg) / Acc_Sensitivity; /* Obtaining angle from accelerometer readings */ accRoll = (atan2(accY, sqrt(pow(accX, 2) + pow(accZ, 2))) * 180 / pi) - AccErrorX; // in degrees accPitch = (atan2(-accX, sqrt(pow(accY, 2) + pow(accZ, 2))) * 180 / pi) - AccErrorY; // in degrees /* Converting body-frame angular rates to Euler angular rates */ GyroRateRoll = GyroX + sin(roll * pi / 180.0) * tan(pitch * pi / 180.0) * GyroY + cos(roll * pi / 180.0) * tan(pitch * pi / 180.0) * GyroZ; GyroRatePitch = cos(roll * pi / 180.0) * GyroY - sin(roll * pi / 180.0) * GyroZ; GyroRateYaw = sin(roll * pi / 180.0) / cos(pitch * pi / 180) * GyroY + cos(roll * pi / 180.0) / cos(pitch * pi / 180.0) * GyroZ; /* Read time */ previousTime = currentTime; // Previous time is stored before the actual time read currentTime = time; // Current time actual time read dT = (currentTime - previousTime) / 1000.0; // in sec data_roll[counter] = roll; data_pitch[counter] = pitch; data_yaw[counter] = yaw; /* Integrating the angular velocity to get angles */ gyroRoll = gyroRoll + GyroRateRoll * dT; gyroPitch = gyroPitch + GyroRatePitch * dT; yaw = yaw + GyroRateYaw * dT; /* Complementary filter - combine accelerometer and gyro angle values */ roll = 0.94 * gyroRoll + 0.06 * accRoll; pitch = 0.94 * gyroPitch + 0.06 * accPitch; counter++;}int servoMotorRoll(float angle){ int duty_cycle; duty_cycle = 230 - (5 * angle) / 3; if (duty_cycle >= 390) { duty_cycle = 390; } else if (duty_cycle <= 60) { duty_cycle = 60; } return duty_cycle;}int servoMotorPitch(float angle){ int duty_cycle; duty_cycle = 230 + (5 * angle) / 3; if (duty_cycle >= 4000) { duty_cycle = 400; } else if (duty_cycle <= 55) { duty_cycle = 55; } return duty_cycle;}int servoMotorYaw(float angle){ int duty_cycle; duty_cycle = 230 - (5 * angle) / 3; if (duty_cycle >= 400) { duty_cycle = 400; } else if (duty_cycle <= 55) { duty_cycle = 55; } return duty_cycle;}matlab
C. Matlab Code for RLS Implementationclear;clc;close all;angleArr{1} = xlsread('data_roll.csv');angleArr{2} = xlsread('data_pitch.csv');angleArr{3} = xlsread('data_yaw.csv');L = [.97 0.98 0.99];range = 300 : 600;estimateParameters = cell(1, 3);estimateY = estimateParameters;actualAngle = estimateY;E = estimateY;for i = 1 : 3 num = angleArr{i};lambda = L(i);angleMotor = -num(range, 1) * pi / 180;DutyCycle = num(range, 2);Voltage = (DutyCycle / 3000) * 6.4;VoltageOld = zeros(1, 3);% 1 AngleOld = zeros(1, 3);% 3 m = length(VoltageOld) + length(AngleOld);n = length(range);P_1 = eye(m);theta = ones(m, 1);T = zeros(m, n);error = zeros(1, n);actual = error;estimate = error;theta_1 = theta; for k = 1 : n y = angleMotor(k); Phi = [VoltageOld - AngleOld]'; P = (P_1 - (P_1 * Phi * Phi ' * P_1) / (1 + Phi' * P_1 * Phi)) / lambda; T( :, k) = theta; error(k) = (y-(Phi'*theta_1)); estimate(k) = Phi'*theta_1; theta = theta_1 + P*Phi*error(k); VoltageOld = [Voltage(k) VoltageOld(1:end-1)]; AngleOld = [angleMotor(k) AngleOld(1:end-1)]; P_1 = P; theta_1 = theta; actual(k) = y; end estimateParameters{i} = T; E{i} = error; y_est = angleMotor(1:length(AngleOld))'; for k = (length(y_est) + 1):length(range) y_est = [y_est [Voltage(k-1) Voltage(k-2) Voltage(k-3) -actual(k-1) -actual(k-2) -actual(k3)]*theta]; end estimateY{i} = y_est; figure subplot(2,2,1)for j = 1:length(theta) plot(1:length(T), T(j,:), '-', 'LineWidth', 1.5) hold on end grid on%legend('g', 'h', 'i', 'a', 'b', 'c')%title('Parameters') set(gca, 'FontSize', 12) xlabel('Number of Iterations') ylabel('Parameters') subplot(2,2,2) plot(1:length(error), error, 'LineWidth', 2) %title('Error') xlabel('Number of Iterations') ylabel('Model Error') set(gca, 'FontSize', 12) grid on figure % estimate = tf([theta(3) theta(2) theta(1)], [1 theta(4) theta(5) theta(6)], 0.025);% [y, t] = impulse(estimate); plot(1:length(y_est), y_est, '-', 'LineWidth', 1.5); hold on plot(1:length(actual), actual, '-', 'LineWidth', 1.5) legend('Estimate', 'Actual') set(gca, 'FontSize', 12) xlabel('Time (s)') ylabel('Response (rad)') grid on subplot(2,2,4) n = floor(length(theta)/2); plot(1:length(theta), [theta(n:end)' theta(1:n-1)'], '*', 'MarkerSize', 10) legend('Estimate') set(gca, 'FontSize', 12) %title('Actual vs. Estimate Parameters') xlabel('a b c g h i') ylabel('Parameter Value') grid on%endfigurename{1} = 'Roll Response';name{2} = 'Pitch Response';name{3} = 'Yaw Response';marker = {'r-', 'g-', 'b-', 'k-', 'm-', 'c-'};for i = 1:3 subplot(2,2,i) theta = estimateParameters{i}; [m,n] = size(theta); for k = 1:m time = 0:0.025:0.025*(length(theta)-1); plot(time,theta(k,:), marker{k}, 'LineWidth', 1.5) hold on end xlim([0 time(end)]) grid on xlabel('Time (s)') ylabel('Parameter') title(name{i}) set(gca, 'FontSize', 12) legend('g', 'h', 'i', 'a', 'b', 'c', 'Location', 'northeastoutside')endsubplot(2,2,4)marker = {'r-', 'k-.', 'b:'};for i = 1:3 error = E{i}; time = 0:0.025:0.025*(length(error)-1); plot(time, error, marker{i}, 'LineWidth', 1.5) hold onendxlim([0 time(end)])grid onxlabel('Time (s)')ylabel('Angle (rad)')title('Recursive Least Square Error')set(gca, 'FontSize', 12)legend('Roll', 'Pitch', 'Yaw', 'Location', 'northeastoutside')嘿,看的还过瘾吗?微信对公式排版不好,见谅