[發(fā)明專利]一種機(jī)器人先逆時針再順時針運動控制方法

有效

• 申請?zhí)枺?/td>: 201310597933.9
• 申請日: 2013-11-22
• 公開（公告）號: CN103576688A
• 公開（公告）日: 2014-02-12
• 發(fā)明（設(shè)計）人: 王碩;王宇;李恩;魏清平
• 申請（專利權(quán)）人: 中國科學(xué)院自動化研究所;常州科學(xué)與藝術(shù)融合技術(shù)研究所
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 中科專利商標(biāo)代理有限責(zé)任公司 11021
• 代理人: 宋焰琴

1.一種機(jī)器人先逆時針再順時針運動運動控制方法，其特征在于，該方法包括以下步驟：

步驟S1：設(shè)定并輸入多個機(jī)器人運動參數(shù)，所述運動參數(shù)至少包括：機(jī)器人運動初始點S坐標(biāo)（x_s，y_s），機(jī)器人在初始點S處的初始方向單位向量Ps（p_xs，p_ys），機(jī)器人運動目標(biāo)點G坐標(biāo)（x_g，y_g），機(jī)器人在目標(biāo)點G處的目標(biāo)方向單位向量Pg（p_xg，p_yg），以及機(jī)器人容許的轉(zhuǎn)彎半徑R；

步驟S2：基于初始點S、初始方向單位向量Ps和轉(zhuǎn)彎半徑R計算得到機(jī)器人從初始點S沿初始方向單位向量Ps以轉(zhuǎn)彎半徑R做逆時針繞第一圓心Os運動的第一圓心Os的坐標(biāo)(x_os,y_os)；

步驟S3：基于目標(biāo)點G、目標(biāo)方向單位向量Pg和轉(zhuǎn)彎半徑R計算得到機(jī)器人以轉(zhuǎn)彎半徑R沿順時針方向繞第二圓心Og運動以到達(dá)目標(biāo)點G和目標(biāo)方向單位向量Pg的第二圓心Og的坐標(biāo)(x_og,y_og)；

步驟S4：基于第一圓心Os和第二圓心Og的坐標(biāo)計算得到由第一圓心Os指向第二圓心Og的第一單位向量Q(q_x,q_y)；

步驟S5：基于第一圓心Os、第二圓心Og和轉(zhuǎn)彎半徑R計算得到第一單位向量Q與第二單位向量W之間的夾角γ；

步驟S6：基于所述第一圓心Os、轉(zhuǎn)彎半徑R和第一單位向量Q計算得到機(jī)器人從繞第一圓心Os逆時針運動轉(zhuǎn)為沿第二單位向量W方向作直線運動的第一轉(zhuǎn)換點Ws的坐標(biāo)(x_ws,y_ws)；

步驟S7：基于所述第二圓心Og、轉(zhuǎn)彎半徑R和第一單位向量Q計算得到機(jī)器人從沿第二單位向量W方向直線運動轉(zhuǎn)為繞第二圓心Og順時針運動的第二轉(zhuǎn)換點Wg的坐標(biāo)(x_wg,y_wg)；

步驟S8：計算得到由第一轉(zhuǎn)換點Ws指向第二轉(zhuǎn)換點Wg的第二單位向量W(w_x,w_y)；

步驟S9：基于第一圓心Os、初始點S、第一轉(zhuǎn)換點Ws計算得到機(jī)器人從初始點S逆時針繞第一圓心Os運動到第一轉(zhuǎn)換點Ws轉(zhuǎn)過的第一轉(zhuǎn)角α_s；

步驟S10：基于第二圓心Og、目標(biāo)點G、第二轉(zhuǎn)換點Wg計算得到機(jī)器人從第二轉(zhuǎn)換點Wg沿順時針繞第二圓心Og運動到目標(biāo)點G轉(zhuǎn)過的第二轉(zhuǎn)角β_g；

步驟S11：基于所述步驟S2-S10計算得到的運動路徑參數(shù)，對于機(jī)器人的先逆時針后順時針運動進(jìn)行控制，其中，所述機(jī)器人的先逆時針后順時針運動路徑為：由初始點S以轉(zhuǎn)彎半徑R繞第一圓心Os逆時針運動轉(zhuǎn)過第一轉(zhuǎn)角α_s到達(dá)第一轉(zhuǎn)換點Ws，由第一轉(zhuǎn)換點Ws沿第二單位向量W方向直線運動到第二轉(zhuǎn)換點Wg，由第二轉(zhuǎn)換點Wg以轉(zhuǎn)彎半徑R繞第二圓心Og順時針運動轉(zhuǎn)過第二轉(zhuǎn)角β_g即到達(dá)目標(biāo)點G。

2.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述步驟S2具體為：先將初始方向單位向量Ps逆時針旋轉(zhuǎn)90度，然后與轉(zhuǎn)彎半徑R相乘，再將相乘結(jié)果與初始點S坐標(biāo)相加即得第一圓心Os的坐標(biāo)。

3.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述步驟S3具體為：先將目標(biāo)方向單位向量Pg順時針旋轉(zhuǎn)90度，然后與轉(zhuǎn)彎半徑R相乘，再將相乘結(jié)果與目標(biāo)點G坐標(biāo)相加即得第二圓心Og的坐標(biāo)。

4.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述步驟S4具體為：用第二圓心Og的坐標(biāo)減去第一圓心Os的坐標(biāo)，再將相減結(jié)果除以第一圓心Os和第二圓心Og兩點間的長度值即得第一單位向量Q。

5.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述步驟S5具體為：用2乘以R除以第一圓心Os和第二圓心Og兩點間的長度值，再將計算結(jié)果進(jìn)行反正弦即得夾角γ。

6.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述步驟S6具體為：先將第一單位向量Q順時針旋轉(zhuǎn)(90-γ)度，然后與轉(zhuǎn)彎半徑R相乘，再將相乘結(jié)果與第一圓心Os的坐標(biāo)相加即得第一轉(zhuǎn)換點Ws的坐標(biāo)。