[发明专利]基于作业飞行机器人重心偏移的位姿控制器设计方法

权利要求书

1.一种基于作业飞行机器人重心偏移的位姿控制器设计方法，其特征在于，包括如下步骤：

步骤S1：考虑重心偏移，对搭载机械臂的四旋翼无人机系统进行建模；

步骤S2：通过引入二阶滑模函数，在滑模面上求解出位置控制律，使无人机平台可以按目标轨迹d_T飞行；

步骤S3：姿态解耦时考虑重心偏移系统参数，解算出无人机平台按目标轨迹d_T飞行所需的翻滚角俯仰角θ_d和升力

步骤S4：在姿态控制器中考虑重心偏移控制参数，在反演控制器中加入自适应，使控制律自适应重心偏移控制参数，解算出翻滚、俯仰、偏航的输入力矩

步骤S5：通过升力翻滚力矩俯仰力矩偏航力矩解算出四个旋翼的转速ω_i，i＝1,2,3,4；

利用牛顿-欧拉方程法对搭载机械臂的四旋翼无人机系统进行建模，根据力平衡和力矩平衡得到：

其中，F为系统所受的外力，M为系统所受的外力矩，m为系统总质量，r'为重心偏移在无人机平台坐标系内的位置，r₀为无人机平台在世界坐标系的位置，B为系统的推动力，Ω为无人机平台在世界坐标系的角速度矢量，I为系统的惯性张量，表示对Ω进行一次微分，表示对r₀进行二次微分；

所述步骤S2具体包括以下步骤：

步骤S21：输入无人机平台在惯性坐标的目标轨迹d_T＝[x_T y_T z_T]^T；

步骤S22：利用卡尔曼滤波融合超宽带定位测量无人机平台，获取当前无人机平台位置信息d＝[x y z]^T；

步骤S23：定义一个跟踪误差向量δ，实时测量无人机平台与目标轨迹的误差；

δ＝d_T-d，δ∈R³ (2)

步骤S24：用一个二阶滑模函数，定义状态空间中的时变曲面s：

步骤S25：在滑模面上，求解出控制律；令s＝0，有：

其中，对于期望d_T的跟踪问题等价于当t0时轨迹必须进入滑模面，一旦进入滑模面，就不会离开，一直待在滑模面上；

所述步骤S3具体包括以下步骤：

步骤S31：当机械臂开始作业时，机械臂各连杆的重心在无人机平台坐标系F_UAV内的坐标发生改变，r'的位置也发生改变，即造成了重心的偏移；估算无人机平台坐标系内的重心偏移坐标r'＝[x' y' z']^T：

其中，^AP_j是第j个关节臂的重心在坐标系F_UAV内的坐标，是抓取目标物在坐标系F_UAV内的坐标，无人机搭载的机械臂有四个关节臂，即有四个连杆，每个连杆的质量为m_j，j＝1,2,3,4，每个连杆重心在无人机平台的坐标为^AP_j，抓取目标物的质量为m_T，系统总质量为m_s；

步骤S32：在系统建模时考虑重心位置发生偏移，从而在姿态解耦时多出了重心偏移系统参数，重心偏移系统参数的估计值为：

其中，为无人机平台在世界坐标系内的速度矢量，由积分解得，为速度矢量是位置d的一阶微分，其中u为速度矢量在x轴方向的分量，v为速度矢量在y轴方向的分量，w为速度矢量在z轴方向的分量；通过卡尔曼滤波融合三轴惯性测量传感器实时测得，微分解得Ω＝[p q r]^T，其中p为角速度矢量Ω在x轴方向的分量，q为角速度矢量Ω在y轴方向的分量，r为角速度矢量Ω在z轴方向的分量；

步骤S33：结合式(5)、(7)，求解位置追踪时所需要的偏航角ψ_d、翻滚角俯仰角θ_d，设定偏航角ψ_d为已知值，从而输出所需要翻滚角俯仰角θ_d：

其中，g为重力加速度，为重心偏移系统参数的估计值；

步骤S34：结合式(5)、(7)、(8)，求得升力

所述步骤S4具体包括以下步骤：

步骤S41：按式(10)求得无人机平台当前的俯仰角的一次微分和绕y轴俯仰的角速度的一次微分

其中，I_x、I_y、I_z分别为系统绕无人机平台坐标系的x、y、z轴的转动惯量，c₂为重心偏移控制参数；

步骤S42：通过卡尔曼滤波融合三轴惯性测量传感器，获得无人机平台当前的俯仰角θ；

步骤S43：定义跟踪误差e₁，实时测量无人机平台与期望俯仰角的误差：

步骤S44：定义跟踪误差ε₁，实时测量无人机平台与期望俯仰角速度的误差：

ε₁＝q-q_d (12)

步骤S45：通过卡尔曼滤波融合三轴惯性测量传感器实时测得，微分解得Ω＝[p q r]^T；

步骤S46：对期望俯仰角θ_d求二阶微分得

步骤S47：为了使李雅普诺夫函数正定，一阶微分李雅普诺夫半负定，使重心偏移控制参数的估计值为：

其中，t为时间；

步骤S48：求得俯仰力矩为：

其中，k₁、k₂为正标量参数；

步骤S49：同理，求得翻滚力矩偏航力矩为：

同俯仰角的输出一样，k₃、k₄、k₅、k₆为正标量参数。

2.根据权利要求1所述的基于作业飞行机器人重心偏移的位姿控制器设计方法，其特征在于，所述步骤S5具体包括以下步骤：

步骤S51：四个旋翼的角速度与升力翻滚力矩俯仰力矩偏航力矩的关系为：

其中，C₁为无人机平台外部力系数，C₂为无人机平台外部力矩系数；

步骤S52：解算出四个旋翼的的转速ω_i，i＝1,2,3,4。