[發明專利]一種基于路徑規劃的無人機剎車改進方法

有效

• 申請號: 201710947462.8
• 申請日: 2017-10-12
• 公開（公告）號: CN108663929B
• 公開（公告）日: 2021-05-14
• 發明（設計）人: 康騰;黃濤;鄔靜云;劉國良;費鵬;曹鵬蕊
• 申請（專利權）人: 深圳禾苗通信科技有限公司
• 主分類號: G05B11/42
• 分類號: G05B11/42;G05D1/08;G05D1/10;G06F17/16
• 代理公司: 深圳市卓科知識產權代理有限公司 44534
• 代理人: 邵妍;張金玲

1.一種基于路徑規劃的無人機剎車改進方法，其特征在于：包括路徑規劃模塊、位置控制模塊、姿態控制模塊、電機控制模塊以及剛體動力學模塊，

由基于姿態規劃的路徑規劃模塊計算得到剎車過程中飛行器的位置設定值，并輸出給位置控制模塊；

由位置控制模塊計算得到剎車過程中飛行器的姿態設定值和推力設定值，并分別輸出給姿態控制模塊和電機控制模塊；

由姿態控制模塊計算得到剎車過程中飛行器的力矩設定值，并輸出給電機控制模塊；

由電機控制模塊計算得到剎車過程中飛行器的電機轉速，并輸出給飛行器的剛體動力學模塊；

由剛體動力學模塊計算得到剎車過程中飛行器的位置、速度信息和角度、角速度信息，并分別輸出給飛控的位置控制模塊和姿態控制模塊，使飛行器進行位置回路和姿態回路的PID控制；

飛行階段：即飛行器接收遙控器搖桿信號保持飛行姿態角向前飛行，水平方向滿桿飛行時，飛行器速度由零逐步加速到水平最大飛行速度后保持勻速飛行；

姿態過渡階段：即飛行器接收遙控器搖桿回中信號由前飛姿態角過渡到剎車姿態角的姿態過渡階段；

剎車階段：即飛行器保持剎車姿態角將飛行速度降到位置控制臨界速度的階段；不同的剎車姿態角會產生不同大小的反向加速度，從而導致飛行器的剎車時間和剎車距離都有所不同；通過調節剎車姿態角參數來實現不同飛行速度下合理的剎車距離和剎車效果；

姿態調整階段：即飛行器由剎車姿態角調整到水平懸停姿態，進而進入位置控制模式的階段；當飛行器飛行速度降到位置控制臨界速度時，將飛行器姿態調整到水平姿態有利于飛行器平滑地過渡到位置控制模式；

所述位置控制模塊：

式中，是NED坐標系下飛行器三個方向的加速度設定值；φ_des、θ_des、ψ_T分別為飛行器的滾轉角設定值、俯仰角設定值和偏航角設定值；g是重力加速度常數；m為飛行器質量；u₁為Z方向飛行器推力，由位置控制模塊計算得到的俯仰角設定值和滾轉角的姿態角設定值分別為

對于懸停控制，水平方向的速度設定值都為零：p_des＝0，q_des＝0、r_des＝0，Z方向推力由PD控制器計算得到：

2.如權利要求1所述的基于路徑規劃的無人機剎車改進方法，其特征在于：所述姿態控制模塊：

式中，φ、θ、ψ分別為飛行器的滾轉角、俯仰角、偏航角；p、q、r為滾轉角、俯仰角、偏航角對應的角速度；φ_des、θ_des、ψ_des分別為滾轉角設定值、俯仰角設定值、偏航角設定值；p_des、q_des、r_des為滾轉角設定值、俯仰角設定值、偏航角設定值對應的姿態角速度設定值；k_p,φ、k_p,θ、k_p,ψ、k_d,φ、k_d,θ、k_d,ψ為PD控制參數。

3.如權利要求1所述的基于路徑規劃的無人機剎車改進方法，其特征在于：還包括保直線方法、XY同步剎車方法。

4.如權利要求3所述的基于路徑規劃的無人機剎車改進方法，其特征在于：所述保直線方法具體步驟如下：

(1)、當無人飛行器由飛行狀態進入剎車狀態時，首先記錄此時的剎車初始速度init_vel方向向量；

(2)、每次計算相鄰時刻的位置設定值差值向量delta_L時，delta_L＝pos_sp-pos_sp_pre，都將delta_L向量往剎車初始速度方向作投影，使得每次計算得到的位置設定值都在init_vel方向上，從而達到保直線的目的，其中，投影的夾角由公式計算得到。

5.如權利要求3所述的基于路徑規劃的無人機剎車改進方法，其特征在于：所述XY同步剎車方法具體步驟如下：

(1)、當無人飛行器由飛行狀態進入剎車狀態時，首先記錄此時的剎車初始速度init_vel方向向量；

(2)、每次對剎車初始速度方向設計合適的剎車姿態角設定值angle_sp，再將剎車姿態角分別沿XY方向投影，計算XY方向對應的剎車姿態角angle_x、angle_y，此時，init_vel分解到XY方向的速度分量會對應不同的剎車姿態角，理論上XY方向對應的速度會同時減速到零，從而達到XY同步剎車的目的，其中，XY方向對應的剎車姿態角angle_x、angle_y由公式計算得到。