【技术实现步骤摘要】
本专利技术涉及控制，具体涉及一种基于轴角误差埃尔米特插值的全姿态平滑控制算法。
技术介绍
1、姿态控制算法在机器人、航空器、车辆控制等领域有广泛应用，被控对象可能会遇到各种风力和其他外界扰动因素，使其姿态发生变化。如果不能及时有效地进行姿态控制，就可能导致被控对象出现失控，造成功能异常甚至坠毁等灾难性故障。因此，研究如何通过控制系统和算法，对被控对象的姿态进行精确、快速和稳定的控制，是一项重要课题。
2、对于无人机、水下航行器等，这类具有全姿态运动能力的运载体，它们不但可以实现任意姿态的静态保持，还可以做出倒立、翻滚等各种复杂的动态变换。
3、目前在姿态控制领域，有两类算法占据主导，其一是基于欧拉角的控制算法，其二是基于方向余弦矩阵或四元数的控制算法。欧拉角控制算法是基于偏航角、俯仰角、滚转角的控制，较为直观，工程人员易于接受，但是由于万向死锁的固有特性，第二次有序旋转不能为90度，导致被控对象的姿态变换过程受限，只能应用在简单的场景中；而方向余弦矩阵或四元数控制算法，虽然算法的实现较为复杂，但是没有万向死锁的限制，因此可应用于全姿态控制。
4、在使用方向余弦矩阵或四元数做姿态控制过程时，一般是把当前姿态作为初始姿态，把期望姿态作为终了姿态，虽然可以实现全姿态控制，但是存在一些可以优化的空间，一方面是该算法(尤其是四元数作为超复数)不够直观，导致工程人员在编程或调试时极易出错，另一方面是控制过程中仅考虑到了满足终了时刻的期望姿态，没有考虑到终了时刻的三轴旋转角速度，因此姿态变化过程不够平滑，如
技术实现思路
1、本专利技术提供了一种基于轴角误差埃尔米特插值的全姿态平滑控制算法，其目的在于解决运载体的全姿态控制问题。
2、为实现上述目的，本专利技术的技术方案为：
3、本专利技术提供了一种基于轴角误差埃尔米特插值的全姿态平滑控制算法，包括：步骤1、计算轴角误差；
4、步骤2、埃尔米特插值规划姿态和角速度；
5、步骤3、角速度闭环控制。
6、进一步的，步骤1中，1.1运载体的x轴指向右边，y轴指向前边，z轴指向上边，
7、假设当前姿态四元数为假设期望的姿态四元数为
8、根据实时四元数和期望四元数分别计算对应的方向余弦矩阵：
9、
10、其中，a为相应的实时四元数和期望四元数的为实部；
11、b、c、d为相应的实时四元数和期望四元数的为虚部；
12、1.2根据方向余弦矩阵分别求出运载体的x、y轴的坐标向量
13、
14、
15、同理，求出期望的运载体的x、y轴的坐标向量xg_target、yg_target；
16、1.3分别计算出调整坐标轴姿态所需的虚拟转轴
17、以三维向量求外积的方式可构造出虚拟转轴的坐标：
18、
19、其中：
20、xg_x为当前运载体x轴在大地坐标系的x坐标；
21、xg_y为当前运载体x轴在大地坐标系的y坐标；
22、xg_z为当前运载体x轴在大地坐标系的z坐标；
23、xg_target_x为运载体期望的x轴在大地坐标系的x坐标；
24、xg_target_y为运载体期望的x轴在大地坐标系的y坐标；
25、xg_target_z为运载体期望的x轴在大地坐标系的z坐标；
26、同理求出虚拟转轴的坐标；
27、1.4分别计算出绕虚拟转轴旋转所需的转角：
28、
29、
30、进一步的，步骤2中，2.1列出埃尔米特插值多项式方程组
31、
32、2.2分别在初始时刻和终了时刻构造增广矩阵m求解插值系数，即可获得在[0，t]时间区间内规划的姿态和角速度，
33、
34、进一步的，步骤3中，3.1对规划角速度进行闭环控制
35、
36、其中wx_error(t)＝wx(t)-wx_current(t)为t时刻的角速度误差；
37、ux(t)为执行机构的控制量；
38、kp为比例控制系数；
39、ki为积分控制系数；
40、kd为微分控制系数；
41、3.2对执行机构限幅处理
42、
43、其中，umax为执行机构可耐受的最大控制量；
44、umin为执行机构可耐受的最小控制量；
45、3.3进入下一个迭代周期。
46、本专利技术所达到的有益效果为：
47、本专利技术针对运载体的全姿态控制问题，提出了一种基于轴角误差的控制方法，通过构造虚拟转轴，并计算转角误差，以消除轴角误差的方式来控制姿态，具有调试直观的特点，利用埃尔米特插值时函数值、导函数值均相等的特性，通过求解埃尔米特插值方程组，对时间区间内的姿态控制过程和角速度控制过程进行平滑处理，减轻了对执行机构的冲击。
本文档来自技高网...【技术保护点】
1.一种基于轴角误差埃尔米特插值的全姿态平滑控制算法，其特征在于，包括：
2.根据权利要求1所述的一种基于轴角误差埃尔米特插值的全姿态平滑控制算法，其特征在于：步骤1中，1.1运载体的x轴指向右边，y轴指向前边，z轴指向上边，假设当前姿态四元数为假设期望的姿态四元数为
3.根据权利要求2所述的一种基于轴角误差埃尔米特插值的全姿态平滑控制算法，其特征在于：
4.根据权利要求3所述的一种基于轴角误差埃尔米特插值的全姿态平滑控制算法，其特征在于：
【技术特征摘要】
1.一种基于轴角误差埃尔米特插值的全姿态平滑控制算法，其特征在于，包括：
2.根据权利要求1所述的一种基于轴角误差埃尔米特插值的全姿态平滑控制算法，其特征在于：步骤1中，1.1运载体的x轴指向右边，y轴指向前边，z轴指向上边，假...
【专利技术属性】
技术研发人员：刘月娟，耿玉豪，周燕，陈柄言，毛现艳，
申请(专利权)人：青岛职业技术学院，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。