[发明专利]一种自主避障陆空两栖装置及其控制方法

权利要求书

1.一种自主避障陆空两栖装置的控制方法，用于控制所述自主避障陆空两栖装置，其特征在于，包括以下步骤：

步骤1、获取装置的地面可行驶区域、空中可通行区域以及运行状态，并建立所述装置所处环境的三维地图；

步骤2、根据所述装置所处环境的三维地图，分别规划出所述装置的最优飞行轨迹和最优行驶轨迹，比较得到所述装置的最优轨迹；

步骤3、根据最优轨迹，获取装置的飞行控制量和行驶控制量；

所述步骤1包括：

通过探测装置采集的环境点云\深度信息，获得地面可行驶区域和空中可通行区域；

所述步骤2包括：

根据所述装置所处环境的三维地图，采用混合A*算法分别获得满足运动约束的最优飞行轨迹和最优行驶轨迹，比较得到的最优飞行轨迹代价值和最优行驶轨迹代价值，以最小代价值对应的轨迹作为最优轨迹；

所述最优飞行轨迹代价值通过飞行轨迹代价函数进行计算；其中，所述飞行轨迹代价函数为：

J_fly＝w₁J_{f_target}+w₂J_{f_energy}

式中，w₁和w₂为代价权重，J_{f_target}为终点代价，J_{f_energy}为能量消耗代价；

J_{f_target}＝(x_N-x_target)²+(y_N-y_target)²+(z_N-z_target)²

式中，m为陆空两栖装置质量，g为重力加速度，P为悬停功率，N为规划步数，Δ_t为规划时间步长；C为空气阻力系数，ρ为空气密度，S为迎风面积，x_N、y_N、z_N分别为规划终点的x轴坐标、y轴坐标、z轴坐标，x_target、y_target、z_target分别为目标点的x轴坐标、y轴坐标、z轴坐标，x₀、y₀、z₀分别为起始点的x轴坐标、y轴坐标、z轴坐标，ux_i、uy_i、uz_i分别为第i步的飞行速度；

当J_{f_energy}≥P_soc，J_{f_energy}＝+∞

式中，P_soc为剩余电池电量。

2.根据权利要求1所述的控制方法，其特征在于，所述步骤3包括：

当最优轨迹为飞行轨迹时，该轨迹包括期望的位置、速度、加速度，其与当前位置、速度、加速度误差，通过串级PID控制率得到期望的姿态角和油门推力，再通过姿态控制率得到多旋翼电机控制器输入，发送给多旋翼电机控制器；获取的所述飞行控制量包括期望的姿态角、油门推力和多旋翼电机控制器输入；

当最优轨迹为行驶轨迹时，该轨迹包括期望的位置、航向角和车速，其与当前位置、航向角和车速误差，通过PID控制率得到两侧驱动电机控制器输入，发送给驱动电机控制器；获取的所述行驶控制量包括两侧驱动电机控制器输入。