[发明专利]一种六旋翼球形机器人的控制系统及其轨迹控制方法

权利要求书

1.一种六旋翼球形机器人的轨迹控制方法，其特征在于，采用串级PID算法进行轨迹控制；所述串级PID算法具体包括以下步骤：

步骤一：获取轨迹航点与位置传感器获得的位置信息作为一级PID输入，利用PID算法进行积分分离，得到期望滚动角速度；

步骤二：将期望滚动角速度与测得的角速度数据作为第二级PID的输入，计算角速度偏差，输出期望驱动力矩；

步骤三：根据六旋翼球形机器人中电机布置的空间构型及检测的当前姿态信息，将期望驱动力矩解算为六个电机的控制信号；

所述步骤二具体包括：通过PI控制器构造惯性坐标系下描述的角速度控制信号

其中，为期望滚动角速度，ω_e为传感器测得的角速度数据在惯性坐标系下的描述；所述ω_e的计算公式为：

ω_e＝R_b2eω_B

其中，ω_B为传感器测得的角速度数据在机体坐标系为参考系下的描述，c代表cos，s代表sin，θ、ψ是3个Euler角；

将惯性坐标系下描述的角加速度控制信号解算为机体坐标系下描述的驱动力矩控制信号其中，I为惯性矩阵。

2.根据权利要求1所述的六旋翼球形机器人的轨迹控制方法，其特征在于，所述步骤一中位置传感器获得的位置信息依次经过均值滤波处理和卡尔曼滤波处理。

3.根据权利要求1或2所述的六旋翼球形机器人的轨迹控制方法，其特征在于，所述步骤一中利用PID算法进行积分分离，得到期望滚动角速度的方法：利用比例系数与输入值求导值的乘积取代原来的微分，并对偏差微分进行积分分离，基于计算结果输出期望滚动角速度。

4.根据权利要求3所述的六旋翼球形机器人的轨迹控制方法，其特征在于，期望滚动角速度的计算公式为：

其中，K_p为比例系数，k_I为积分系数，k_D为微分系数，e为期望位置与实际位置的偏差值，为期望滚动角速度。

5.根据权利要求1所述的六旋翼球形机器人的轨迹控制方法，其特征在于，所述步骤三具体包括：设旋翼i的转速达到ω_i，i＝1,2,…,6，作用效果分解为电机轴线方向上的升力F_i和旋转力矩M_i，F_i＝k_Fω_i²，M_i＝k_Mω_i²，其中，k_F是电机转动力系数，k_M是电机转动力矩系数；

设计控制分配器如下：

其中，L为同一方向上布置的两电机间轴距，为机体坐标系下描述的驱动力矩控制信号，为六个电机的期望滚动角速度。

6.根据权利要求1所述的六旋翼球形机器人的轨迹控制方法，其特征在于，所述步骤三中采用力矩驱动的控制分配策略，即在同一直径上布置的两电机采取等大且同向的控制信号。

7.一种六旋翼球形机器人的控制系统，其特征在于，采用如权利要求1-6之一所述的六旋翼球形机器人的轨迹控制方法，包括位置控制器、姿态控制器、控制分配器；

所述位置控制器，输入轨迹航点和传感器接收到的当前位置信息,计算偏差，输出期望滚动角速度，并传递给姿态控制器；

所述姿态控制器，输入期望滚动角速度与传感器测得的角速度数据，计算角速度偏差，输出期望驱动力矩，并传递给控制分配器；

所述控制分配器，根据六旋翼球形机器人中电机布置的空间构型，以及当前的姿态信息，将期望驱动力矩解算为电机的控制信号。

8.根据权利要求7所述的六旋翼球形机器人的控制系统，其特征在于，还包括球形滚动动力学模块，所述球形滚动动力学模块，分别向位置控制器、姿态控制器、控制分配器，输出相应的传感器测量得到的位置信息、角速度数据、姿态信息。