基于轴角矢量的多旋翼姿态自抗扰控制方法及系统.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN113759706A(43)申请公布日2021.12.07(21)申请号202111303198.7(22)申请日2021.11.05(71)申请人普宙科技（深圳）有限公司地址518054广东省深圳市南山区东滨路4078号永新汇1号楼1006(72)发明人黄立张原艺洪亮王龙宋帅薛源刘华斌(74)专利代理机构北京集智东方知识产权代理有限公司11578代理人吴倩龚建蓉(51)Int.Cl.G05B13/02(2006.01)权利要求书7页说明书20页附图1页(54)发明名称基于轴角矢量的多旋翼姿态自抗扰控制方法及系统(57)摘要本发明涉及一种基于轴角矢量的多旋翼姿态自抗扰控制方法及系统，包括以下步骤：基于预先得到的机体目标姿态角、机体当前姿态角，获取三轴的目标角速度；获取当前机体角速度，估计当前角加速度和扰动量，并基于所述目标角速度向量获取最终的姿态控制量。本发明将姿态运动视为一种空间内基于旋转矢量的旋转运动，通过目标角速度、当前机体角速度，并估计当前角加速度和扰动量，得到最终的姿态控制量，克服了丢失控制精度的问题。同时引入扰动量，从而具有自抗扰功能，以提高姿态的响应速度和控制器的适应性。CN113759706ACN113759706A权利要求书1/7页1.一种基于轴角矢量的多旋翼姿态自抗扰控制方法，其特征在于，包括：基于预先得到的机体目标姿态角、机体当前姿态角，获取三轴的目标角速度；获取当前机体角速度，估计当前角加速度和扰动量，并基于所述目标角速度向量，获取最终的姿态控制量。2.根据权利要求1所述的基于轴角矢量的多旋翼姿态自抗扰控制方法，其特征在于，所述获取三轴的目标角速度包括：将机体目标姿态角、机体当前姿态角带入欧拉角转四元数公式可得到目标姿态角的四元数、当前姿态角的四元数；根据目标姿态角的四元数，获取目标姿态四元数；基于当前姿态角的四元数、目标姿态四元数，获取姿态误差四元数，并计算得到目标角速度向量。3.根据权利要求2所述的基于轴角矢量的多旋翼姿态自抗扰控制方法，其特征在于，所述获取目标姿态四元数包括：基于目标姿态角的四元数，利用公式（1）计算倾转误差四元数向量；（1）其中，，表示目标姿态坐标系的Z轴在NED坐标系下的向量且有（2）且，为目标横滚角，为目标俯仰角，为目标航向角；表示目标姿态坐标系的Z轴在NED坐标系下的向量且有2CN113759706A权利要求书2/7页（3）且，为当前横滚角，为当前俯仰角，为当前航向角；得到倾转角目标四元数：（4）为当前姿态的姿态四元数；通过公式（5）获取多旋翼航向目标四元数；（5）为姿态目标四元数向量；、为航向目标四元数向量中的元素；利用公式（6）对旋转运动目标四元数进行限制，得到限制后的四元数：（6）为调节因子；根据倾转角目标四元数、限制后的四元数，利用公式（7）最终得到限制后的目标姿态四3CN113759706A权利要求书3/7页元数；（7）。4.根据权利要求3所述的基于轴角矢量的多旋翼姿态自抗扰控制方法，其特征在于，所述获取姿态误差四元数包括：利用公式（8）获取最终的姿态误差四元数向量；（8）；表示误差的符号，、、分别为欧拉角、、的误差。5.根据权利要求4所述的基于轴角矢量的多旋翼姿态自抗扰控制方法，其特征在于，所述计算得到目标角速度向量包括：基于姿态误差四元数，利用公式（9）可得到目标角速度向量，为横滚目标角速度，为俯仰目标角速度，为航向目标角速度；的计算公式为：（9）其中、、为可调参数。6.根据权利要求4所述的基于轴角矢量的多旋翼姿态自抗扰控制方法，其特征在于，获取最终的姿态控制量的步骤包括：采用快速跟踪函数fhan得到目标角加速度，计算公式为：4CN113759706A权利要求书4/7页（10）其中为计算出的目标角速度，为计算出的目标角加速度，为公式（9）计算出的目标角速度，k为第k个时刻，为执行周期时间，（11）其中，均为函数输入，为跟踪步长，sign为符号函数，，（12）其中，（13）；据测量得到的当前机体角速度，使用状态观测器计算出k时刻到k+1时刻的观测角加速度和观测扰动量，计算公式为：（14）其中为观测角速度，为观测角加速度，为观测扰动量，为控制器给出的控制量，、、、b均为可调参数，为观测角速度和当前角速度的观测误差；根据目标角速度、角加速度和当前实际的角速度、角加速度可得到最终控制量为5CN113759706A权利要求书5/7页（15）（16）其中为目标角速度与观测角速度之间的误差，为目标角加速度和观测角加速度之间的误差，c为阻尼系数。7.一种基于轴角矢量的多旋翼姿态自抗扰控制系统，其特征在于，包括：获取模块，用于基于机体目标姿态角、机体当前姿态角，获取三轴的目标角速度；计算模块，用于获取