CN115520376A - 一种基于矢量旋翼的空地两用移动操作平台及其位姿控制系统及控制方法 - Google Patents

Abstract

一种基于矢量旋翼的空地两用移动操作平台及其位姿控制系统及控制方法，本发明涉及空地两用移动操作平台及其位姿控制系统及控制方法。本发明的目的是为了解决普通旋翼飞行器的旋翼旋转轴固定，不能很好的抵抗所搭载的机械臂操作时引入的扰动问题；同时，实现一种不需要动力轮的地面移动遥操作平台。过程为：一、得到期望控制力和控制力矩；二、飞行模式时执行三；地面模式时执行五；三、计算飞行模式时f_a和m_a；四、计算飞行模式时机器人的控制分配；判断控制分配是否符合驱动机构约束；若符合响应控制分配；否则机器人暂停作业返回降落；五、计算地面模式时f_a和m_a；同时完成机器人控制分配。本发明用于空地两用移动操作平台及其位姿控制领域。

Description

一种基于矢量旋翼的空地两用移动操作平台及其位姿控制系 统及控制方法

技术领域

本发明涉及基于矢量旋翼的空地两用移动操作平台及其位姿控制系统及控制方法。

背景技术

旋翼飞行器因其出色的垂直起降能力、空中悬停能力，已经在航拍、植保、环境检测等领域发挥了巨大作用。近年来，搭载机械臂的旋翼飞行器成为了备受瞩目的研究热点，被用来进行空中无人作业。然而，普通旋翼飞行器的旋翼旋转轴固定，无法同时提供飞行器的控制力和控制力矩，使得机械臂进行操作时，不能很好的抵抗所搭载的机械臂操作时引入的扰动。

发明内容

本发明的目的是为了解决普通旋翼飞行器的旋翼旋转轴固定，无法同时提供飞行器机体系下的三维控制力和三维控制力矩，使得机械臂进行操作时，不能很好的抵抗所搭载的机械臂操作时引入的扰动问题，而提出一种基于矢量旋翼的空地两用移动操作平台及其位姿控制系统及控制方法。

一种基于矢量旋翼的空地两用移动操作平台及其位姿控制系统包括：动力平台、被动滑动装置和遥操作装置；

所述遥操作装置为机械臂；

所述动力平台由若干个可双轴倾转的旋翼驱动装置构成；

舵机带动无刷电机，无刷电机驱动旋翼转动获得多个矢量升力，使升力方向可以在三维空间变化；多个矢量升力的合力提供控制力和控制力矩；

所述被动滑动装置由若干支架腿和被动轮构成，支架腿将被动轮和动力平台连接在一起；

当空地两用机器人降落在地面时，动力平台通过调整旋翼转轴方向，提供侧向移动力；在被动轮的作用下，空地两用机器人在地面滑动，此时被动滑动装置仅起到支撑作用，而动力平台提供控制力和控制力矩以保证空地两用移动机器人系统的位姿稳定。

一种基于矢量旋翼的空地两用移动操作平台及其位姿控制方法具体过程为：

步骤一、根据期望位姿和当前位姿的偏差，位姿控制器计算得到期望控制力f_c和控制力矩m_c；

步骤二、判断空地两用移动机器人的遥操作工作模式：

当空地两用移动机器人的遥操作工作模式为飞行模式时，执行步骤三；

当空地两用移动机器人的遥操作工作模式为地面模式时，执行步骤五；

步骤三、计算飞行模式时空地两用移动机器人旋翼提供的升力总和f_a和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a；

步骤四、基于飞行模式时空地两用移动机器人旋翼提供的升力总和f_a和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a，计算飞行模式时空地两用移动机器人的控制分配；

判断飞行模式时空地两用移动机器人的控制分配是否符合驱动机构约束；

所述控制分配的结果为：第i个驱动装置的径向转动舵机转角为α_i，切向转动舵机转角为λ_i，以及螺旋桨转速ν_i；

所述驱动机构约束为：

其中，

分别为径向转动舵机转角的最小值、最大值，切向转动舵机转角的最小值、最大值，螺旋桨转速的最小值、最大值；

若符合，可双轴倾转的驱动装置响应控制分配；

若不符合，空地两用移动机器人暂停作业，返回降落；

步骤五、计算地面模式时空地两用移动机器人旋翼提供的升力总和f_a和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a；同时完成地面模式时空地两用移动机器人控制分配。

本发明的有益效果为：

本发明提出一种由可倾转旋翼作为驱动装置的空地两用移动操作平台，可倾转旋翼通过调整旋翼旋转轴方向，同时对扰动力和扰动扭矩进行补偿，可以提高平台空中悬停时的位姿精度，提高空中遥操作的稳定性。同时，搭载的被动轮用于平台降落在地面时的滑行，此时旋翼只需通过侧向倾转，提供侧向力使飞行器在地面滑行。

附图说明

图1为本发明空地两用机器人示意图；

图2为本发明坐标系建立方法图；

图3为本发明地面模式下，地面支撑力力矩范围示意图；

图4为本发明地面模式下，气动力、力矩解算过程流程图。

具体实施方式

具体实施方式一：本实施方式一种基于矢量旋翼的空地两用移动操作平台及其位姿控制系统包括：动力平台、被动滑动装置和遥操作装置；

所述遥操作装置为机械臂；

所述动力平台由若干个可双轴倾转的旋翼驱动装置构成；

舵机带动无刷电机，无刷电机驱动旋翼转动获得多个矢量升力，使升力方向可以在三维空间变化；多个矢量升力的合力提供控制力和控制力矩；明显区别于普通的固定转轴旋翼飞行器，每个旋翼的转轴都是可变的。

所述被动滑动装置由若干支架腿和被动轮构成，支架腿将被动轮和动力平台连接在一起；被动轮为附图中的轮子；

当空地两用机器人降落在地面时，动力平台通过调整旋翼转轴方向，提供侧向移动力；在被动轮的作用下，空地两用机器人在地面滑动，此时被动滑动装置仅起到支撑作用，而动力平台提供控制力和控制力矩以保证空地两用移动机器人系统的位姿稳定。

遥操作装置一般为机械臂，通过螺栓等固定在动力平台上方，其末端可根据任务类型安装不同的末端执行器；

本发明进一步拓展旋翼飞行器的应用场景，将矢量倾转旋翼、被动轮和小型遥操作装置结合，组成空地两用移动操作平台。

机器人本体的一种典型应用示例如图1所示，遥操作装置为机械臂；

具体实施方式二：本实施方式一种基于矢量旋翼的空地两用移动操作平台及其位姿控制方法具体过程为：

控制器设计时，仅设计动力平台的位姿控制器，即通过控制可双轴倾转的驱动装置的倾转角度和旋翼转速来控制动力平台的位姿稳定；而遥操作装置自身的控制由其自带的控制器完成，不包含在本专利的内容内。

步骤一、根据期望位姿和当前位姿的偏差，位姿控制器计算得到期望控制力f_c和控制力矩m_c；

步骤二、判断空地两用移动机器人的遥操作工作模式：

当空地两用移动机器人的遥操作工作模式为飞行模式时，执行步骤三；

当空地两用移动机器人的遥操作工作模式为地面模式时，执行步骤五；

步骤三、计算飞行模式时空地两用移动机器人旋翼提供的升力总和f_a和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a；

步骤四、基于飞行模式时空地两用移动机器人旋翼提供的升力总和f_a和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a，计算飞行模式时空地两用移动机器人的控制分配；

判断飞行模式时空地两用移动机器人的控制分配是否符合驱动机构约束；

所述控制分配的结果为：第i个驱动装置的径向转动舵机转角为α_i，切向转动舵机转角为λ_i，以及螺旋桨转速ν_i；

所述驱动机构约束为：

其中，

分别为径向转动舵机转角的最小值、最大值，切向转动舵机转角的最小值、最大值，螺旋桨转速的最小值、最大值；

若符合，可双轴倾转的驱动装置响应控制分配；

若不符合，空地两用移动机器人暂停作业，返回降落；

步骤五、计算地面模式时空地两用移动机器人旋翼提供的升力总和f_a和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a；同时完成地面模式时空地两用移动机器人控制分配。

具体实施方式三：本实施方式与具体实施方式二不同的是，所述步骤一中根据期望位姿和当前位姿的偏差，位姿控制器计算得到期望控制力f_c和控制力矩m_c；具体过程为：

步骤一一、

在动力平台质心处建立机体坐标系C_b，如图2所示，选取NED(东北天坐标系)作为惯性参考坐标系C_e，初始状态下机体坐标系C_b和惯性参考坐标系C_e两者重合；

设机体坐标系C_b原点在惯性参考坐标系C_e的坐标点为p_b，机体坐标系C_b相对惯性参考坐标系C_e的姿态为四元数q_b(旋转矩阵R_b)；

对动力平台进行动力学分析，其受重力、旋翼的升力、遥操作装置的扰动力(即动力平台和遥操作装置之间的相互作用力)和地面支撑力(当处于飞行模式时，此力为零)的影响；

对动力平台建立动力学模型如下：

其中，f_a表示旋翼提供的升力总和，f_u表示地面支撑力，f_g表示动力平台的重力，f_d表示遥操作装置对动力平台的总力，m_b表示动力平台的质量，

表示动力平台的质心在地面参考系下的加速度；m_a表示旋翼提供的升力所产生的力矩和旋翼反扭距的总和，m_u表示地面支撑力对动力平台的支撑力所产生的力矩，m_g表示动力平台的重力对动力平台的力矩，m_d表示遥操作装置对动力平台的总扭矩，J表示动力平台的惯性张量，ω表示动力平台的角速度，

表示动力平台的角加速度；

所述遥操作装置为机械臂；

所述动力平台由若干个可双轴倾转的旋翼驱动装置构成；

舵机带动无刷电机，无刷电机驱动旋翼转动获得多个矢量升力，使升力方向可以在三维空间变化；多个矢量升力的合力提供控制力和控制力矩；

明显区别于普通的固定转轴旋翼飞行器，每个旋翼的转轴都是可变的；

所述f_g、f_d、m_g、m_d是已知量；

所述f_a和m_a由旋翼提供；

所述f_u和m_u由地面提供，机器人处于飞行模式时f_u和m_u均为零，机器人处于地面模式时，f_u和m_u分别为地面提供的支撑力和支撑力产生的力矩；

通过准确的建模或者传感器测量，f_g、f_d、m_g、m_d可以准确测量，可以认为是已知量。f_a和m_a由旋翼提供。f_u和m_u由地面提供，机器人处于飞行模式时均为零，机器人处于地面模式时，分别为被动力和被动力矩。

空地两用遥操作机器人位姿控制系统分为两部分：控制器部分和控制分配部分；

控制器部分通过动力学建模、控制器设计，根据期望位姿和当前位姿的偏差产生期望控制力f_c和控制力矩m_c，再解算出f_a和m_a；

控制分配部分根据实际采用的动力装置的数量和分布方式，将f_a和m_a解算为驱动装置的指令信号，即螺旋桨的转速和舵机的转动角度。

控制器部分设计以采用PID控制方法为例。系统控制器的目标是本专利所述的平台平台可以对目标位姿进行有效跟踪，为遥操作装置提供位姿稳定的基底。

步骤一二、

设期望位置为p_t，期望姿态为q_t；

动力平台的位姿控制器输出的期望位姿和当前位姿的偏差产生的期望控制力f_c和控制力矩m_c计算式如下所示：

m_c＝J(K₄q_ev+K₅ω+K₆∫q_evdt) (4)

其中，K₁、K₂、K₃、K₄、K₅、K₆为正定的3维参数矩阵，p_b为动力平台的质心在地面参考系下的位置，

为动力平台的质心在地面参考系下的期望位置的速度，

为动力平台的质心在地面参考系下的速度，t为时间，J为动力平台的惯性张量，ω_b为动力平台的角速度；q_ev为姿态偏差q_e使用四元数表述时的矢量部分，计算方式如下

为对q_ev求转置，q_e0为姿态四元数偏差的实部，

为姿态四元数的共轭，q_t为期望姿态；f_c为期望位姿和当前位姿的偏差产生的期望控制力，m_c为期望位姿和当前位姿的偏差产生的期望控制力矩。

其它步骤及参数与具体实施方式一相同。

具体实施方式四：本实施方式与具体实施方式二或三不同的是，所述步骤三中计算飞行模式时空地两用移动机器人旋翼提供的升力总和f_a和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a；具体过程为：

当机器人处于飞行模式时，f_u和m_u均为零，按式(5)(6)直接计算出f_a和m_a，实现对飞行平台的位姿控制；

f_a＝f_c-f_d-f_g-f_u (5)

m_a＝m_c-m_d-m_g-m_u+ω×Jω (6)

而当机器人处于地面模式时，整个系统受到地面支撑力这一被动力影响；当机器人进行地面加减速滑动时，此时需要特别注意整个系统的力矩平衡，以防止发生侧倾。由于被动力的存在，无法根据式(5)(6)直接计算出f_a和m_a。

其它步骤及参数与具体实施方式二或三相同。

具体实施方式五：本实施方式与具体实施方式二至四之一不同的是，所述步骤四中基于飞行模式时空地两用移动机器人旋翼提供的升力总和f_a和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a，计算飞行模式时空地两用移动机器人的控制分配；判断飞行模式时空地两用移动机器人的控制分配是否符合驱动机构约束；

所述控制分配的结果为：第i个驱动装置的径向转动舵机转角为α_i，切向转动舵机转角为λ_i，以及螺旋桨转速ν_i；

所述驱动机构约束为：

其中，

分别为径向转动舵机转角的最小值、最大值，切向转动舵机转角的最小值、最大值，螺旋桨转速的最小值、最大值；

若符合，可双轴倾转的驱动装置响应控制分配；

若不符合，空地两用移动机器人暂停作业，返回降落；

具体过程为：

矢量驱动装置驱动指令信号解算被称为控制分配，把f_a和m_a解算为驱动装置的指令信号，即旋翼的转轴倾转角度和旋转速度；

其中，n表示可双轴倾转的驱动装置总个数，f_i为第i个可双轴倾转的驱动装置的升力，n_i为旋翼的反扭距，满足

w_i为旋翼的旋转方向，根据旋翼的旋转方向确定，取值为±1；c_f为旋翼提供的升力总和f_a(气动力)系数，c_n为旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a(气动反扭距)系数；L_i为倾转中心的坐标；I_3×3表示3×3单位向量，[L_i]_×表示矢量L_i的斜对角矩阵，A_6×3n表示控制矩阵；

使用伪逆法计算可双轴倾转的旋翼驱动装置的升力，计算如下：

其中，A表示A_6×3n，A^T表示对A求转置；

采用不同双轴倾转结构的驱动装置有不同的计算方法，进一步将f_i解算为实际的倾转角度和旋翼的转速。如本专利图1中给出的结构所采用的矢量倾转机构，第i个驱动装置的径向转动舵机转角为α_i，切向转动舵机转角为λ_i，以及螺旋桨转速ν_i按如下公式计算：

其中，R_ai为第i个驱动装置自身坐标系和机体坐标系的旋转矩阵，在此坐标系下，倾转角度可以根据驱动装置设计者所采用的规则解算。实际使用中，需要明确考虑转角的大小约束和螺旋桨转速最大值限制；

表示对R_ai求转置；

判断飞行模式时空地两用移动机器人的控制分配是否符合驱动机构约束；

所述控制分配的结果为：第i个驱动装置的径向转动舵机转角为α_i，切向转动舵机转角为λ_i，以及螺旋桨转速ν_i；

所述驱动机构约束为：

其中，

分别为径向转动舵机转角的最小值、最大值，切向转动舵机转角的最小值、最大值，螺旋桨转速的最小值、最大值；

若符合，可双轴倾转的驱动装置响应控制分配；

若不符合，空地两用移动机器人暂停作业，返回降落。

其它步骤及参数与具体实施方式二至四之一相同。

具体实施方式六：本实施方式与具体实施方式二至五之一不同的是，所述步骤五中计算地面模式时空地两用移动机器人旋翼提供的升力总和f_a和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a；同时完成地面模式时空地两用移动机器人控制分配；具体过程为：

当机器人处于地面模式时，f_u和m_u分别为地面提供的支撑力和支撑力产生的力矩，根据f_u和m_u计算出f_a和m_a；

步骤五一、

当机器人处于地面模式时，需保证动力平台在重力方向的合力为零，确定f_a在重力方向的分量的范围：

(1)在重力方向，动力平台的质心位置不变，加速度为零，得到如下的受力平衡关系：

f_a(3)+f_d(3)+f_g(3)+f_u(3)＝f_c(3)，

其中，f_a(3)表示旋翼提供的升力总和在重力方向的分量，f_d(3)表示遥操作装置对动力平台的总力在重力方向的分量，f_g(3)表示重力在重力方向的分量，f_u(3)表示地面支撑力在重力方向的分量，f_c(3)表示期望控制力在重力方向的分量，(3)表示力在重力方向的分量；

地面支撑力在重力方向的分量满足f_u(3)≤0，从而得到当机器人处于地面模式时，旋翼提供的升力总和在重力方向的分量f_a(3)的约束式：

f_a(3)≥f_c(3)-f_d(3)-f_g(3) (7)

(2)考虑到驱动旋翼转动的无刷电机存在最大转速限制，即旋翼能提供的升力有最大限制，设动力平台选用的可双轴倾转的旋翼驱动装置能提供的最大升力为|τ_max|，即当机器人处于地面模式时，旋翼提供的升力总和在重力方向(Z轴方向)的分量f_a(3)的最大值约束为：

|f_a(3)|≤|τ_max| (8)

步骤五二、

当机器人处于地面模式时，飞行平台保持姿态稳定，忽略四个万向轮和地面接触的摩擦力，只考虑地面提供的竖直向上的支撑力f_uz，此时地面支撑力通过四个万向轮，对动力平台提供绕机器人机体X轴和Y轴的力矩m_ux和m_uy；即f_u＝[0 0 f_uz]^T，m_u＝[m_ux m_uy 0]^T

f_u的作用点为四个万向轮的支撑点，f_u是四个支撑力的合力，而m_u是四个支撑力的合力矩；

其中，f_uz为地面提供的竖直向上的支撑力，T表示转置，m_ux为地面支撑力通过四个万向轮，对动力平台提供绕机器人机体X轴的力矩；m_uy为地面支撑力通过四个万向轮，对动力平台提供绕机器人机体Y轴的力矩；

在机体坐标系下，四个支撑点组成一个正方形。在极端情况下，支撑力的合力位于此正方形的边界处，根据支撑力的大小可以确定支撑力力矩(m_ux,m_uy)应位于如图3阴影部分定义的力矩正方形内。

(m_ux,m_uy)应满足如下公式：

其中，a表示被动轮的作用点与机体坐标系原点的连线在机体坐标系XY平面的投影的长度；

步骤五三、

根据步骤五一、步骤五二，结合公式(5)(6)，直接计算出f_a在机体坐标系下的X轴、Y轴分量和m_a在机体坐标系下的Z轴分量：

f_a(1)＝f_c(1)-f_d(1)-f_g(1) (9)

f_a(2)＝f_c(2)-f_d(2)-f_g(2) (10)

m_a(3)＝m_c(3)-m_d(3)-m_g(3)+[0 0 1](ω×Jω) (11)

其中，f_a(1)为f_a在机体坐标系下的X轴分量，f_a(2)为f_a在机体坐标系下的Y轴分量，f_c(1)为f_c在机体坐标系下的X轴分量，f_c(2)为f_c在机体坐标系下的Y轴分量，f_d(1)为f_d在机体坐标系下的X轴分量，f_d(2)为f_d在机体坐标系下的Y轴分量，f_g(1)为f_g在机体坐标系下的X轴分量，f_g(2)为f_g在机体坐标系下的Y轴分量，m_a(3)为m_a在机体坐标系下的Z轴分量，m_c(3)为m_c在机体坐标系下的Z轴分量，m_d(3)为m_d在机体坐标系下的Z轴分量，m_g(3)为m_g在机体坐标系下的Z轴分量，(1)表示三维矢量的第一个分量，(2)表示三维矢量的第二个分量，(3)表示三维矢量的第三个分量；

步骤五四、

旋翼提供的升力总和f_a在机体坐标系下的Z轴分量和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a在机体坐标系下的X轴、Y轴分量无法直接计算，满足如下等式关系：

f_a(3)＝f_c(3)-f_d(3)-f_uz-f_g(3) (12)

m_a(1)＝m_c(1)-m_d(1)-m_g(1)-m_ux+[1 0 0]ω×Jω (13)

m_a(2)＝m_c(2)-m_d(2)-m_g(2)-m_uy+[0 1 0]ω×Jω (14)

其中，m_a(1)表示m_a在机体坐标系下的X轴分量，m_a(2)表示m_a在机体坐标系下的Y轴分量，m_c(1)表示m_c在机体坐标系下的X轴分量，m_c(2)表示m_c在机体坐标系下的Y轴分量，m_d(1)表示m_d在机体坐标系下的X轴分量，m_d(2)表示m_d在机体坐标系下的Y轴分量，m_g(1)表示m_g在机体坐标系下的X轴分量，m_g(2)表示m_g在机体坐标系下的Y轴分量；

步骤五五、

结合不等式约束式(7)(8)，以及正方形约束

以f_a(3)＝f_c(3)-f_d(3)-f_g(3)为初值对f_a(3)、m_a(1)和m_a(2)进行可行解搜索，得到地面模式时空地两用移动机器人旋翼提供的升力总和f_a和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a的全部分量均计算完毕；同时完成地面模式时空地两用移动机器人控制分配。

其它步骤及参数与具体实施方式二至五之一相同。

具体实施方式七：本实施方式与具体实施方式二至六之一不同的是，所述步骤五五中结合不等式约束式(7)(8)，以及正方形约束

以f_a(3)＝f_c(3)-f_d(3)-f_g(3)为初值对f_a(3)、m_a(1)和m_a(2)进行可行解搜索，得到地面模式时空地两用移动机器人旋翼提供的升力总和f_a和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a的全部分量均计算完毕；同时完成地面模式时空地两用移动机器人控制分配；具体过程为：

1)令f_a(3)＝f_c(3)-f_d(3)-f_g(3)，设f_a(3)、m_a(1)和m_a(2)的可行解集合为Σ，初始为空集合；

2)判断f_a(3)是否符合不等式约束(8)，若符合，通过式(12)计算出f_uz，然后进入3)；若不符合，进入6)；

3)设m_a(1)＝m_a(2)＝0，带入式(13)(14)计算m_ux和m_uy，判断(m_ux,m_uy)是否满足正方形约束

若满足，m_a(1)＝m_a(2)＝0，结合公式(9)(10)(11)得到可行解f_a和m_a；

若不满足，再把m_ux和m_uy带入式(13)(14)计算m_a(1)和m_a(2)的数值，结合公式(9)(10)(11)得到可行解f_a和m_a；

把f_a和m_a组合带入公式(15)(16)(17)中，得到对应的可双轴倾转的旋翼驱动机构的舵机转角α_i、λ_i和电机转速ν_i，判断这些数值是否满足实际物理约束，即判断求解出的舵机转角是否符合实际转角约束，判断电机转速是否超过最大转速限制；

若求解出的舵机转角符合实际转角约束，且电机转速未超过最大转速限制，把可行解f_a和m_a组合加入集合Σ，进入4)；

若求解出的舵机转角不符合实际转角约束或电机转速超过最大转速限制(不符合约)束，则进入5)；

矢量驱动装置驱动指令信号解算被称为控制分配，把f_a和m_a解算为驱动装置的指令信号，即旋翼的转轴倾转角度和旋转速度；

其中，n表示可双轴倾转的驱动装置总个数，f_i为第i个可双轴倾转的驱动装置的升力，n_i为旋翼的反扭距，满足

w_i为旋翼的旋转方向，根据旋翼的旋转方向确定，取值为±1；c_f为旋翼提供的升力总和f_a(气动力)系数，c_n为旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a(气动反扭距)系数；L_i为倾转中心的坐标；I_3×3表示3×3单位向量，[L_i]_×表示矢量L_i的斜对角矩阵，A_6×3n表示控制矩阵；

使用伪逆法计算可双轴倾转的旋翼驱动装置的升力，计算如下：

其中，A表示A_6×3n，A^T表示对A求转置；

采用不同双轴倾转结构的驱动装置有不同的计算方法，进一步将f_i解算为实际的倾转角度和旋翼的转速。如本专利图1中给出的结构所采用的矢量倾转机构，第i个驱动装置的径向转动舵机转角为α_i，切向转动舵机转角为λ_i，以及螺旋桨转速ν_i按如下公式计算：

其中，R_ai为第i个驱动装置自身坐标系和机体坐标系的旋转矩阵，在此坐标系下，倾转角度可以根据驱动装置设计者所采用的规则解算。实际使用中，需要明确考虑转角的大小约束和螺旋桨转速最大值限制；

表示对R_ai求转置；

4)判断可行解集合Σ中的可行解数量是否超过30(可行解f_a(1)、f_a(2)和m_a(3)这三个数组成一个元组)，若满足，进入6)；若不满足，进入5)；

5)f_a(3)＝f_a(3)+0.01；进入2)；

6)若集合Σ为空集，则说明没有可行解，移动机器人处于不可控状态，此时退出搜索，并且移动机器人暂停全部的作业工作；

若集合Σ不是空集，则对集合Σ中的所有可行解求取全部电机转速平方和

选取平方和最小的解作为f_a和m_a的解；退出搜索；

至此，地面模式时空地两用移动机器人旋翼提供的升力总和f_a和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a的全部分量均计算完毕；同时完成地面模式时空地两用移动机器人控制分配。

整个计算过程如图4所示。

其它步骤及参数与具体实施方式二至六之一相同。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (7)

1.一种基于矢量旋翼的空地两用移动操作平台及其位姿控制系统，其特征在于：所述系统包括：动力平台、被动滑动装置和遥操作装置；

所述遥操作装置为机械臂；

所述动力平台由若干个可双轴倾转的旋翼驱动装置构成；

舵机带动无刷电机，无刷电机驱动旋翼转动获得多个矢量升力，使升力方向可以在三维空间变化；多个矢量升力的合力提供控制力和控制力矩；

所述被动滑动装置由若干支架腿和被动轮构成，支架腿将被动轮和动力平台连接在一起；

当空地两用机器人降落在地面时，动力平台通过调整旋翼转轴方向，提供侧向移动力；在被动轮的作用下，空地两用机器人在地面滑动，此时被动滑动装置仅起到支撑作用，而动力平台提供控制力和控制力矩以保证空地两用移动机器人系统的位姿稳定。

2.一种基于权利要求1所述的基于矢量旋翼的空地两用移动操作平台及其位姿控制系统的控制方法，其特征在于：所述方法具体过程为：

步骤一、根据期望位姿和当前位姿的偏差，位姿控制器计算得到期望控制力f_c和控制力矩m_c；

步骤二、判断空地两用移动机器人的遥操作工作模式：

当空地两用移动机器人的遥操作工作模式为飞行模式时，执行步骤三；

当空地两用移动机器人的遥操作工作模式为地面模式时，执行步骤五；

步骤三、计算飞行模式时空地两用移动机器人旋翼提供的升力总和f_a和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a；

步骤四、基于飞行模式时空地两用移动机器人旋翼提供的升力总和f_a和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a，计算飞行模式时空地两用移动机器人的控制分配；

判断飞行模式时空地两用移动机器人的控制分配是否符合驱动机构约束；

所述控制分配的结果为：第i个驱动装置的径向转动舵机转角为α_i，切向转动舵机转角为λ_i，以及螺旋桨转速ν_i；

所述驱动机构约束为：

其中，

分别为径向转动舵机转角的最小值、最大值，切向转动舵机转角的最小值、最大值，螺旋桨转速的最小值、最大值；

若符合，可双轴倾转的驱动装置响应控制分配；

若不符合，空地两用移动机器人暂停作业，返回降落；

步骤五、计算地面模式时空地两用移动机器人旋翼提供的升力总和f_a和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a；同时完成地面模式时空地两用移动机器人控制分配。

3.根据权利要求2所述的一种基于矢量旋翼的空地两用移动操作平台及其位姿控制方法，其特征在于：所述步骤一中根据期望位姿和当前位姿的偏差，位姿控制器计算得到期望控制力f_c和控制力矩m_c；具体过程为：

步骤一一、

在动力平台质心处建立机体坐标系C_b，选取NED作为惯性参考坐标系C_e，初始状态下机体坐标系C_b和惯性参考坐标系C_e两者重合；

设机体坐标系C_b原点在惯性参考坐标系C_e的坐标点为p_b，机体坐标系C_b相对惯性参考坐标系C_e的姿态为四元数q_b；

对动力平台建立动力学模型如下：

其中，f_a表示旋翼提供的升力总和，f_u表示地面支撑力，f_g表示动力平台的重力，f_d表示遥操作装置对动力平台的总力，m_b表示动力平台的质量，

表示动力平台的质心在地面参考系下的加速度；m_a表示旋翼提供的升力所产生的力矩和旋翼反扭距的总和，m_u表示地面支撑力对动力平台的支撑力所产生的力矩，m_g表示动力平台的重力对动力平台的力矩，m_d表示遥操作装置对动力平台的总扭矩，J表示动力平台的惯性张量，ω表示动力平台的角速度，

表示动力平台的角加速度；

所述遥操作装置为机械臂；

所述动力平台由若干个可双轴倾转的旋翼驱动装置构成；

舵机带动无刷电机，无刷电机驱动旋翼转动获得多个矢量升力，使升力方向可以在三维空间变化；多个矢量升力的合力提供控制力和控制力矩；

所述f_g、f_d、m_g、m_d是已知量；

所述f_a和m_a由旋翼提供；

所述f_u和m_u由地面提供，机器人处于飞行模式时f_u和m_u均为零，机器人处于地面模式时，f_u和m_u分别为地面提供的支撑力和支撑力产生的力矩；

步骤一二、

设期望位置为p_t，期望姿态为q_t；

动力平台的位姿控制器输出的期望位姿和当前位姿的偏差产生的期望控制力f_c和控制力矩m_c计算式如下所示：

m_c＝J(K₄q_ev+K₅ω+K₆∫q_evdt) (4)

其中，K₁、K₂、K₃、K₄、K₅、K₆为正定的3维参数矩阵，p_b为动力平台的质心在地面参考系下的位置，

为动力平台的质心在地面参考系下的期望位置的速度，

为动力平台的质心在地面参考系下的速度，t为时间，J为动力平台的惯性张量，ω_b为动力平台的角速度；q_ev为姿态偏差q_e使用四元数表述时的矢量部分，计算方式如下

为对q_ev求转置，q_e0为姿态四元数偏差的实部，

为姿态四元数的共轭，q_t为期望姿态；f_c为期望位姿和当前位姿的偏差产生的期望控制力，m_c为期望位姿和当前位姿的偏差产生的期望控制力矩。

4.根据权利要求3所述的一种基于矢量旋翼的空地两用移动操作平台及其位姿控制方法，其特征在于：所述步骤三中计算飞行模式时空地两用移动机器人旋翼提供的升力总和f_a和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a；具体过程为：

当机器人处于飞行模式时，f_u和m_u均为零，按式(5)(6)直接计算出f_a和m_a，实现对飞行平台的位姿控制；

f_a＝f_c-f_d-f_g-f_u (5)

m_a＝m_c-m_d-m_g-m_u+ω×Jω (6)。

5.根据权利要求4所述的一种基于矢量旋翼的空地两用移动操作平台及其位姿控制方法，其特征在于：所述步骤四中基于飞行模式时空地两用移动机器人旋翼提供的升力总和f_a和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a，计算飞行模式时空地两用移动机器人的控制分配；判断飞行模式时空地两用移动机器人的控制分配是否符合驱动机构约束；

所述控制分配的结果为：第i个驱动装置的径向转动舵机转角为α_i，切向转动舵机转角为λ_i，以及螺旋桨转速ν_i；

所述驱动机构约束为：

其中，

分别为径向转动舵机转角的最小值、最大值，切向转动舵机转角的最小值、最大值，螺旋桨转速的最小值、最大值；

若符合，可双轴倾转的驱动装置响应控制分配；

若不符合，空地两用移动机器人暂停作业，返回降落；

具体过程为：

其中，n表示可双轴倾转的驱动装置总个数，f_i为第i个可双轴倾转的驱动装置的升力，n_i为旋翼的反扭距，满足

w_i为旋翼的旋转方向；c_f为旋翼提供的升力总和f_a系数，c_n为旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a系数；L_i为倾转中心的坐标；I_3×3表示3×3单位向量，[L_i]_×表示矢量L_i的斜对角矩阵，A_6×3n表示控制矩阵；

使用伪逆法计算可双轴倾转的旋翼驱动装置的升力，计算如下：

其中，A表示A_6×3n，A^T表示对A求转置；

第i个驱动装置的径向转动舵机转角为α_i，切向转动舵机转角为λ_i，以及螺旋桨转速ν_i按如下公式计算：

其中，R_ai为第i个驱动装置自身坐标系和机体坐标系的旋转矩阵；

表示对R_ai求转置；

判断飞行模式时空地两用移动机器人的控制分配是否符合驱动机构约束；

所述控制分配的结果为：第i个驱动装置的径向转动舵机转角为α_i，切向转动舵机转角为λ_i，以及螺旋桨转速ν_i；

所述驱动机构约束为：

其中，

分别为径向转动舵机转角的最小值、最大值，切向转动舵机转角的最小值、最大值，螺旋桨转速的最小值、最大值；

若符合，可双轴倾转的驱动装置响应控制分配；

若不符合，空地两用移动机器人暂停作业，返回降落。

6.根据权利要求5所述的一种基于矢量旋翼的空地两用移动操作平台及其位姿控制方法，其特征在于：所述步骤五中计算地面模式时空地两用移动机器人旋翼提供的升力总和f_a和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a；同时完成地面模式时空地两用移动机器人控制分配；具体过程为：

当机器人处于地面模式时，f_u和m_u分别为地面提供的支撑力和支撑力产生的力矩，根据f_u和m_u计算出f_a和m_a；

步骤五一、

当机器人处于地面模式时，需保证动力平台在重力方向的合力为零，确定f_a在重力方向的分量的范围：

(1)在重力方向，动力平台的质心位置不变，加速度为零，得到如下的受力平衡关系：

f_a(3)+f_d(3)+f_g(3)+f_u(3)＝f_c(3)，

其中，f_a(3)表示旋翼提供的升力总和在重力方向的分量，f_d(3)表示遥操作装置对动力平台的总力在重力方向的分量，f_g(3)表示重力在重力方向的分量，f_u(3)表示地面支撑力在重力方向的分量，f_c(3)表示期望控制力在重力方向的分量，(3)表示力在重力方向的分量；

地面支撑力在重力方向的分量满足f_u(3)≤0，从而得到当机器人处于地面模式时，旋翼提供的升力总和在重力方向的分量f_a(3)的约束式：

f_a(3)≥f_c(3)-f_d(3)-f_g(3) (7)

(2)设动力平台选用的可双轴倾转的旋翼驱动装置能提供的最大升力为|τ_max|，即当机器人处于地面模式时，旋翼提供的升力总和在重力方向的分量f_a(3)的最大值约束为：

|f_a(3)|≤|τ_max| (8)

步骤五二、

当机器人处于地面模式时，只考虑地面提供的竖直向上的支撑力f_uz，此时地面支撑力通过四个万向轮，对动力平台提供绕机器人机体X轴和Y轴的力矩m_ux和m_uy；即f_u＝[0 0 f_uz]^T，m_u＝[m_ux m_uy 0]^T

f_u的作用点为四个万向轮的支撑点，f_u是四个支撑力的合力，而m_u是四个支撑力的合力矩；

其中，f_uz为地面提供的竖直向上的支撑力，T表示转置，m_ux为地面支撑力通过四个万向轮，对动力平台提供绕机器人机体X轴的力矩；m_uy为地面支撑力通过四个万向轮，对动力平台提供绕机器人机体Y轴的力矩；

(m_ux,m_uy)应满足如下公式：

其中，a表示被动轮的作用点与机体坐标系原点的连线在机体坐标系XY平面的投影的长度；

步骤五三、

根据步骤五一、步骤五二，结合公式(5)(6)，直接计算出f_a在机体坐标系下的X轴、Y轴分量和m_a在机体坐标系下的Z轴分量：

f_a(1)＝f_c(1)-f_d(1)-f_g(1) (9)

f_a(2)＝f_c(2)-f_d(2)-f_g(2) (10)

m_a(3)＝m_c(3)-m_d(3)-m_g(3)+[0 0 1](ω×Jω) (11)

其中，f_a(1)为f_a在机体坐标系下的X轴分量，f_a(2)为f_a在机体坐标系下的Y轴分量，f_c(1)为f_c在机体坐标系下的X轴分量，f_c(2)为f_c在机体坐标系下的Y轴分量，f_d(1)为f_d在机体坐标系下的X轴分量，f_d(2)为f_d在机体坐标系下的Y轴分量，f_g(1)为f_g在机体坐标系下的X轴分量，f_g(2)为f_g在机体坐标系下的Y轴分量，m_a(3)为m_a在机体坐标系下的Z轴分量，m_c(3)为m_c在机体坐标系下的Z轴分量，m_d(3)为m_d在机体坐标系下的Z轴分量，m_g(3)为m_g在机体坐标系下的Z轴分量，(1)表示三维矢量的第一个分量，(2)表示三维矢量的第二个分量，(3)表示三维矢量的第三个分量；

步骤五四、

旋翼提供的升力总和f_a在机体坐标系下的Z轴分量和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a在机体坐标系下的X轴、Y轴分量满足如下等式关系：

f_a(3)＝f_c(3)-f_d(3)-f_uz-f_g(3) (12)

m_a(1)＝m_c(1)-m_d(1)-m_g(1)-m_ux+[1 0 0]ω×Jω (13)

m_a(2)＝m_c(2)-m_d(2)-m_g(2)-m_uy+[0 1 0]ω×Jω (14)

其中，m_a(1)表示m_a在机体坐标系下的X轴分量，m_a(2)表示m_a在机体坐标系下的Y轴分量，m_c(1)表示m_c在机体坐标系下的X轴分量，m_c(2)表示m_c在机体坐标系下的Y轴分量，m_d(1)表示m_d在机体坐标系下的X轴分量，m_d(2)表示m_d在机体坐标系下的Y轴分量，m_g(1)表示m_g在机体坐标系下的X轴分量，m_g(2)表示m_g在机体坐标系下的Y轴分量；

步骤五五、

结合不等式约束式(7)(8)，以及正方形约束

以f_a(3)＝f_c(3)-f_d(3)-f_g(3)为初值对f_a(3)、m_a(1)和m_a(2)进行可行解搜索，得到地面模式时空地两用移动机器人旋翼提供的升力总和f_a和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a的全部分量均计算完毕；同时完成地面模式时空地两用移动机器人控制分配。

7.根据权利要求6所述的一种基于矢量旋翼的空地两用移动操作平台及其位姿控制方法，其特征在于：所述步骤五五中结合不等式约束式(7)(8)，以及正方形约束

以f_a(3)＝f_c(3)-f_d(3)-f_g(3)为初值对f_a(3)、m_a(1)和m_a(2)进行可行解搜索，得到地面模式时空地两用移动机器人旋翼提供的升力总和f_a和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a的全部分量均计算完毕；同时完成地面模式时空地两用移动机器人控制分配；具体过程为：

1)令f_a(3)＝f_c(3)-f_d(3)-f_g(3)，设f_a(3)、m_a(1)和m_a(2)的可行解集合为Σ，初始为空集合；

2)判断f_a(3)是否符合不等式约束(8)，若符合，通过式(12)计算出f_uz，然后进入3)；若不符合，进入6)；

3)设m_a(1)＝m_a(2)＝0，带入式(13)(14)计算m_ux和m_uy，判断(m_ux,m_uy)是否满足正方形约束

若满足，m_a(1)＝m_a(2)＝0，结合公式(9)(10)(11)得到可行解f_a和m_a；

若不满足，再把m_ux和m_uy带入式(13)(14)计算m_a(1)和m_a(2)的数值，结合公式(9)(10)(11)得到可行解f_a和m_a；

把f_a和m_a组合带入公式(15)(16)(17)中，得到对应的可双轴倾转的旋翼驱动机构的舵机转角α_i、λ_i和电机转速ν_i，判断求解出的舵机转角是否符合实际转角约束，判断电机转速是否超过最大转速限制；

若求解出的舵机转角符合实际转角约束，且电机转速未超过最大转速限制，把可行解f_a和m_a组合加入集合Σ，进入4)；

若求解出的舵机转角不符合实际转角约束或电机转速超过最大转速限制束，则进入5)；

其中，n表示可双轴倾转的驱动装置总个数，f_i为第i个可双轴倾转的驱动装置的升力，n_i为旋翼的反扭距，满足

w_i为旋翼的旋转方向；c_f为旋翼提供的升力总和f_a系数，c_n为旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a系数；L_i为倾转中心的坐标；I_3×3表示3×3单位向量，[L_i]_×表示矢量L_i的斜对角矩阵，A_6×3n表示控制矩阵；

使用伪逆法计算可双轴倾转的旋翼驱动装置的升力，计算如下：

其中，A表示A_6×3n，A^T表示对A求转置；

第i个驱动装置的径向转动舵机转角为α_i，切向转动舵机转角为λ_i，以及螺旋桨转速ν_i按如下公式计算：

其中，R_ai为第i个驱动装置自身坐标系和机体坐标系的旋转矩阵；

表示对R_ai求转置；

4)判断可行解集合Σ中的可行解数量是否超过30，若满足，进入6)；若不满足，进入5)；

5)f_a(3)＝f_a(3)+0.01；进入2)；

6)若集合Σ为空集，则说明没有可行解，移动机器人处于不可控状态，此时退出搜索，并且移动机器人暂停全部的作业工作；

若集合Σ不是空集，则对集合Σ中的所有可行解求取全部电机转速平方和

选取平方和最小的解作为f_a和m_a的解；退出搜索；

至此，地面模式时空地两用移动机器人旋翼提供的升力总和f_a和旋翼提供的升力所产生的力矩和旋翼反扭距的总和m_a的全部分量均计算完毕；同时完成地面模式时空地两用移动机器人控制分配。

Images