[發明專利]一種機器人先逆時針再順時針運動控制方法

有效

• 申請號: 201310597933.9
• 申請日: 2013-11-22
• 公開（公告）號: CN103576688A
• 公開（公告）日: 2014-02-12
• 發明（設計）人: 王碩;王宇;李恩;魏清平
• 申請（專利權）人: 中國科學院自動化研究所;常州科學與藝術融合技術研究所
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 中科專利商標代理有限責任公司 11021
• 代理人: 宋焰琴

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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