[發明專利]基于三維成像的多足機器人復雜路況離散成落腳點方法

在審

• 申請號: 201710763243.4
• 申請日: 2017-08-30
• 公開（公告）號: CN107644441A
• 公開（公告）日: 2018-01-30
• 發明（設計）人: 陳春林;唐開強;洪俊;王嵐;王子輝;劉力鋒;朱張青;辛博;陳文玉;于躍文;吳濤
• 申請（專利權）人: 南京大學
• 主分類號: G06T7/70
• 分類號: G06T7/70;G06T1/00;G06T17/05;G06T5/00;G06T5/20;G06K9/62;B25J9/16
• 代理公司: 南京天翼專利代理有限責任公司32112
• 代理人: 于忠洲

1.基于三維成像的多足機器人復雜路況離散成落腳點方法，其特征在于，包括如下步驟：

步驟1，讀取多足機器人慣性導航系統測量的加速度數據以及測距系統測量的距離數據，再利用互補濾波法對加速度數據和距離數據進行數據融合，從而獲得多足機器人的位移數據；

步驟2，讀取多足機器人慣性導航系統測量的慣性測量數據，并利用卡爾曼濾波對慣性測量數據進行數據融合，獲得多足機器人的姿態角度數據；

步驟3，讀取多足機器人前進方向上激光雷達采集的深度斷層圖像數據，再對深度斷層圖像數據進行自適應中值濾波，并進行基于位移和角度的補償，獲得深度斷層圖像；

步驟4，根據步驟1的位移數據和步驟2的姿態角度數據確定多足機器人運動軌跡，并對運動軌跡上的相鄰數據采集點的深度斷層圖像使用滑動窗口牛頓插值法獲取復雜地形環境的三維圖像；

步驟5，將步驟4的三維圖像進行離散化，結合多足機器人自身運動參數和運動軌跡，將三維圖像顯示的復雜地形環境離散為與機器人足端大小相近的落腳點，為多足機器人提供安全落腳點。

2.根據權利要求1所述的基于三維成像的多足機器人復雜路況離散成落腳點方法，其特征在于，步驟1中，獲得多足機器人的位移數據的具體步驟為：

步驟1.1，讀取多足機器人慣性導航系統測量的加速度數據為(a_x(t),a_y(t),a_z(t))，進行二重積分得到t_n-1到t_n時刻的近似位移(x_t,y_t,z_t)為：

xtytzt=∫tn-1tn(vx(tn-1)+∫tn-1tnax(t)dt)dt∫tn-1tn(vy(tn-1)+∫tn-1tnay(t)dt)dt∫tn-1tn(vz(tn-1)+∫tn-1tnaz(t)dt)dt---(1)]]>

式中，(v_x(t_n-1),v_y(t_n-1),v_z(t_n-1))為t_n-1時刻的速度；

步驟1.2，讀取多足機器人測距系統測量的t_n-1和t_n時刻的距離數據為和計算t_n-1到t_n時刻的近似位移(mx_t,my_t,mz_t)為：

mxtmytmzt=xmt-xmt-1ymt-ymt-1zmt-zmt-1---(2)]]>

步驟1.3，利用互補濾波法進行數據融合獲得多足機器人t_n時刻的位移數據為：

x‾ty‾tz‾t=ax(x‾t-1+xt*Tu)+(1-ax)*mxtay(y‾t-1+yt*Tu)+(1-ay)*mytaz(z‾t-1+zt*Tu)+(1-az)*mzt---(3)]]>

axayaz=τx/(τx+Tu)τy/(τy+Tu)τz/(τz+Tu)---(4)]]>

式中，為t_n-1時刻進行數據融合后的位移，T_u為超聲波傳感器采樣周期，(τ_x,τ_y,τ_z)為置信程度，(a_x,a_y,a_z)為動態加權系數。