基於增量PID算法的無人機跟蹤模塊

其中O-XY爲圖像像素座標系，而C-XYZ爲攝像機座標系。

圖1 圖像像素座標系和攝像機座標系示意圖

他們的轉換關係如下：

 

 

建立如下座標系：

圖2 攝像機座標系示意圖

其中設置攝像頭的角度爲向下傾斜45度，那麼地面上的P2正好能夠投影在攝像頭CMOS平面上的正中央P2’點，如果物點離得太遠或太近，則投影在CMOS平面的其他地方，甚至投影在CMOS平面之外。無人機在跟蹤的時候，應該儘量保證物點的投影像點儘量保持在鏡頭所拍攝畫面的中心。當跟蹤目標爲P1點時，無人機應當攜帶攝像頭像Z軸的正方向進行運動。當目標爲爲P2點時，無人機應當向Z軸的負方向進行運動。

當投影點在圖像的偏上或者偏下部位時，此時無人機應當向前後進行移動；而當投影點在圖像的偏左或者偏右部位時，此時無人機應當旋轉偏航角，在此方針進行分析時，僅先考慮前者情況。

圖3 攝像機座標系平面圖

圖4 拍攝所得的圖像

 

所以應當仿真的函數功能爲：

         輸入：1.初始的P的像素座標xy，用[u,v]進行表示。

                     2.無人機的初始飛行高度X

                     3.攝像機的內參矩陣

                     4.無人機的鏡頭角度（現考慮爲45）

         輸出：1.每個時刻輸出飛機應當前進的速度，若爲負說明倒退。

 

現在採用PDI算法對其進行操控，在PDI算法當中需要設定目標值，在此，我們的目標是使得投影點能夠出現在圖像的中央，當設定攝像機的角度爲45度的時候，那麼我們的等效目標值是使得無人機攝像頭和物點之間的連線同X軸方向成45度，即∠α=45。

而在攝像機座標系同圖像座標系的換算當中：

而∠β=Xc/Zc=∠γ，即可以將PID算法的目標設定爲使得∠β爲45度。

 

圖5 算法流程圖

 

根據網上現有的資料編寫增量PID算法的函數代碼：

function [pid_add,last_error,current_error]=mypid(feb,ref,current_error,last_error,pid_add,kp,kd,ki)
 
%   ref 設置參數
%   feb 採樣反饋
%   pid_add 輸出，本次的調節量
%   pwm_value 上次的調節量
%   kp 純比例控制係數
%   kd 微分控制係數
%   ki 積分控制係數
 
if nargin == 5
kp = 0.6;
kd = 0.2;
ki = 0.00;
end;
 
 
%誤差更新 
prev_error = last_error;
last_error = current_error;
current_error = ref -feb;
% 純比例控制
P = kp*(current_error-last_error);
% 微分控制
D = kd*(current_error-2*last_error+prev_error);
% 積分控制
I = ki*current_error;
pwm_value = P+I+D;
 
if pwm_value > 20
   pwm_value = 10;
end
 
if pwm_value < -20
   pwm_value = -10;
end
 
pid_add = pid_add+pwm_value;
 
if pid_add>20
    pid_add = 20;
end
if pid_add<-20
    pid_add = -20;
end

 

 

再寫主函數：

% 基於PID算法的無人機跟蹤模塊（俯仰角度的控制）
clc;clear;closeall;
kp =0.45;
kd = 0;
ki =0.01;
%相機內參數
Cx =147;Cy = 92;Fx = 287;Fy = 293;             %單位爲mm
%起始飛行高度，在飛行過程中可能有變動
x =1e4;                                %單位爲mm，即10m高
%初始u，v
u0 =340;v = 92;
 
%首次值
ref =45;feb =atand((u0-Cx)/Fx);
current_error= 0;last_error =0;pid_add = 0;
 
fujiao(1)= feb + 45;
u(1) =u0; 
pid(1) =pid_add;
i = 2;
 
raodong(1)= 0;
yidong=[];
yidongcishu= 3;
flag =1;
 
while(1)
    %pid迭代
    [pid_add,last_error,current_error] =mypid(feb+45,ref,current_error,last_error,pid_add,kp,kd,ki); %PID迭代
    pid(i) = pid_add;
    %速度計算和存儲
    sudu(i-1) =x/tand(feb+45)-x/tand(feb+45+pid_add);
    %速度大小限制在2m每秒
    if sudu(i-1) > 3000
       sudu(i-1) = 3000;
    end
    if sudu(i-1) < -3000
       sudu(i-1) = -3000;
    end
   
    %[u,v]的更新獲取（實際爲通過攝像機獲得,現在用rand來進行模擬）
    u(i) = Cx + Fx*tand(feb+pid_add);
    raodong(i) = 2*Cx*0.2*(rand-0.5);
    u(i) = u(i) + raodong(i);
   
    %x的更新獲取（實際通過超聲波獲得，現在用rand來進行模擬）
    x = x*(1+(rand-0.5)*0.5);
   
    %利用新獲取的u對當前的俯仰角度進行更新
    feb = atand((u(i)-Cx)/Fx); 
    %對當前的俯仰角度進行存儲
    fujiao(i) = feb +45;              
    %如果當前位置和目標值相差較小，說明已經跟蹤到，此時將人再次移動
    if abs(feb+45-ref)<2
        if (flag == yidongcishu + 1)
            break;
        end
        i = i+1;
        u(i) = 2*Cx*0.3*rand;
        raodong(i) = 0;
        feb = atand((u(i)-Cx)/Fx);
        fujiao(i) = feb + 45;
        yidong(flag) = i;
        flag = flag+1;     
    end
    i = i+1;   
end;
sudu(i)= 0;
 
 
figure(1)
subplot(4,1,1);plot(1:i,u,'-ok','LineWidth',3);gridon;title('像素座標');hold on;
plot(1:i,ones(1,i)*147,'--','LineWidth',1.5);holdon;
forj=1:yidongcishu
   plot(yidong(j),u(yidong(j)),'-or','LineWidth',5)
end
 
subplot(4,1,2);plot(1:i,sudu/1000,'-ok','LineWidth',3);gridon;title('速度（米每秒）');hold on;
forj=1:yidongcishu
   plot(yidong(j),sudu(yidong(j))/1000,'-or','LineWidth',5)
end
 
subplot(4,1,3);plot(1:i,fujiao,'-ok','LineWidth',3);gridon;title('角度');hold on;
plot(1:i,ones(1,i)*45,'--','LineWidth',1.5);holdon;
forj=1:yidongcishu
   plot(yidong(j),fujiao(yidong(j)),'-or','LineWidth',5)
end
 
subplot(4,1,4);plot(1:i,raodong,'-ok','LineWidth',3);gridon;title('像素擾動');hold on;
forj=1:yidongcishu
   plot(yidong(j),raodong(yidong(j)),'-or','LineWidth',5)
end

最終效果：

                                                        圖6  PID調節效果圖

其中藍色的線代表目標值，每次迭代都輸出一個速度，通過速度對飛機的姿態進行更新，使得輸出值向目標值靠攏，同時紅色的點模擬人的走動。人發生走動後，投影點不在攝像機的中心，此時算法將再次迭代使其穩定，從圖中看來人一共發生了三次走動，最終都能通過迭代，使得投影出現在圖像的中心。

在調節PID算法的三個參數的時候應當注意一下細節：

其中kp 爲比例係數，kd爲微分系數，ki爲積分系數。調節時，應當先使得kd和ki爲零，調節kp使得輸出儘量能夠通過少的迭代次數就達到目標值，如果超調量過高，或者輸出穩定在非目標值，說明存在靜態誤差，應當加入積分系數。而如果輸出值經常超調，應當增加微分系數，使得輸出能夠有預見性，爲減小誤差而提前做出調整策略。

   

詳細的三個係數的解釋在下邊鏈接的博客裏有解釋：

http://blog.gkong.com/liaochangchu_117560.ashx