[發(fā)明專利]一種控制雙輪機(jī)器人直線行走的方法和系統(tǒng)

有效

• 申請?zhí)枺?/td>: 201710267244.X
• 申請日: 2017-04-21
• 公開（公告）號: CN106990783B
• 公開（公告）日: 2023-09-01
• 發(fā)明（設(shè)計(jì)）人: 趙艷麗
• 申請（專利權(quán)）人: 歌爾科技有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 北京市隆安律師事務(wù)所 11323
• 代理人: 權(quán)鮮枝;吳昊

1.一種控制雙輪機(jī)器人直線行走的方法，其特征在于，所述雙輪機(jī)器人內(nèi)設(shè)置有壓力傳感器、陀螺儀和左右電機(jī)，所述方法包括：

利用所述陀螺儀獲取所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度值，并根據(jù)所述角速度值計(jì)算所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值；

通過所述壓力傳感器獲取所述雙輪機(jī)器人的負(fù)重值，并根據(jù)所述負(fù)重值計(jì)算所述雙輪機(jī)器人進(jìn)行PD直線控制的比例控制參數(shù)P值和微分控制參數(shù)D值；

將所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值與預(yù)存的零角度目標(biāo)值之差進(jìn)行所述P值的比例控制，將所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度值進(jìn)行所述D值的微分控制，計(jì)算所述雙輪機(jī)器人進(jìn)行PD直線控制的輸出量；

將所述輸出量輸出至所述左右電機(jī)，根據(jù)所述輸出量控制所述左右電機(jī)的步進(jìn)數(shù)，以實(shí)現(xiàn)對所述雙輪機(jī)器人直線行走的控制。

2.如權(quán)利要求1所述的方法，其特征在于，所述雙輪機(jī)器人內(nèi)設(shè)置有兩個(gè)壓力傳感器，分別對應(yīng)所述雙輪機(jī)器人的左右輪設(shè)置，

所述通過所述壓力傳感器獲取所述雙輪機(jī)器人的負(fù)重值，并根據(jù)所述負(fù)重值計(jì)算所述雙輪機(jī)器人進(jìn)行PD直線控制的比例控制參數(shù)P值和微分控制參數(shù)D值，包括：通過所述兩個(gè)壓力傳感器分別獲取所述雙輪機(jī)器人的左右輪的負(fù)重值，并根據(jù)所述左右輪的負(fù)重值分別計(jì)算左右輪進(jìn)行PD直線控制的比例控制參數(shù)P值和微分控制參數(shù)D值；

所述將所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值與預(yù)存的零角度目標(biāo)值之差進(jìn)行所述P值的比例控制，將所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度值進(jìn)行所述D值的微分控制，計(jì)算所述雙輪機(jī)器人進(jìn)行PD直線控制的輸出量，包括：對于所述雙輪機(jī)器人的左右輪，分別將所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值與預(yù)存的零角度目標(biāo)值之差進(jìn)行對應(yīng)所述P值的比例控制，將所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度值進(jìn)行對應(yīng)所述D值的微分控制，分別計(jì)算所述雙輪機(jī)器人左右輪的PD直線控制的輸出量；

所述將所述輸出量輸出至所述左右電機(jī)，根據(jù)所述輸出量控制所述左右電機(jī)的步進(jìn)數(shù)，以實(shí)現(xiàn)對所述雙輪機(jī)器人直線行走的控制，包括：將對應(yīng)左右輪的兩個(gè)所述輸出量分別輸出至所述左右電機(jī)，根據(jù)所述輸出量分別控制所述左右電機(jī)的步進(jìn)數(shù)，以實(shí)現(xiàn)對所述雙輪機(jī)器人直線行走的控制。

3.如權(quán)利要求1所述的方法，其特征在于，所述通過所述壓力傳感器獲取所述雙輪機(jī)器人的負(fù)重值，并根據(jù)所述負(fù)重值計(jì)算所述雙輪機(jī)器人進(jìn)行PD直線控制的比例控制參數(shù)P值和微分控制參數(shù)D值包括：

根據(jù)公式P_out＝k_p*Weight+P₀計(jì)算P值，其中，P_out為雙輪機(jī)器人適應(yīng)當(dāng)前負(fù)重的P值，k_p為根據(jù)雙輪機(jī)器人的結(jié)構(gòu)和本身重量通過試驗(yàn)標(biāo)定的系數(shù)，Weight為所述壓力傳感器獲得的當(dāng)前所述雙輪機(jī)器人的負(fù)重值，P₀為雙輪機(jī)器人的負(fù)重為零時(shí)的P值；

根據(jù)公式D_out＝k_d*Weight+D₀計(jì)算D值，其中，D_out為雙輪機(jī)器人適應(yīng)當(dāng)前負(fù)重的D值，k_d為根據(jù)雙輪機(jī)器人的結(jié)構(gòu)和本身重量通過試驗(yàn)標(biāo)定的系數(shù)，Weight為所述壓力傳感器獲得的當(dāng)前所述雙輪機(jī)器人的負(fù)重值，D₀為雙輪機(jī)器人的負(fù)重為零時(shí)的D值。

4.如權(quán)利要求1所述的方法，其特征在于，所述將所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值與預(yù)存的零角度目標(biāo)值之差進(jìn)行所述P值的比例控制，將所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度值進(jìn)行所述D值的微分控制，計(jì)算進(jìn)行PD直線控制的輸出量包括：

根據(jù)公式StraightControlOut＝P*(Yaw-CAR_ZERO)+D*Gyro[2]計(jì)算進(jìn)行PD直線控制的輸出量，其中，StraightControlOut為PD直線控制的輸出量；Yaw為所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值；CAR_ZERO_ANGLE為預(yù)存的零角度目標(biāo)值；gyro[2]為陀螺儀測得的雙輪機(jī)器人在行走過程中繞z軸旋轉(zhuǎn)的角速度值；P為根據(jù)所述負(fù)重值計(jì)算得到的比例控制參數(shù)值，D為根據(jù)所述負(fù)重值計(jì)算得到的微分控制參數(shù)值。