[易懂]兩連桿關節機械臂機器人給定位置求解各關節轉動角度教程模擬Python實現

兩連桿關節機械臂機器人給定位置求解各關節轉動角度教程模擬Python實現

github代碼地址：https://github.com/varyshare/easy_slam_tutorial/tree/master/joint_robot_simulation

我們要解決的問題是已知一個目標點坐標(x,y)，已知兩個連桿的長度a1,a2，我們的目標是求q1,q2這兩個關節角.如下圖所示：

因為已知坐標(x,y)即我們已知下圖中的三角形的兩個直角邊。根據勾股定理可以得到斜邊的長度為 $r=\sqrt{x^2+y^2}$.

因此下面這個三角形所有的邊都是已知的了。

高中的幾何學告訴我們三條邊已知的話那就可以根據余弦定理求出一個角。因此我們是計划把那個大角 $\alpha$求出來。為什么？因為求出 $\alpha$那么我們就可以求出關節角q2，因為它們是互為補角。 $q2=180-\alpha$。現在我們已經求出一個關節角了。

現在我們知道了角度q2.而且知道第2個桿的長度a2. 因此我們可以解出下面這個三角形的兩條邊.

於是乎，我們現在已知下面這個直角三角形的兩條直角邊。根據反切公式可以求出 $\beta$這個銳角。為什么要求 $\beta$這個銳角？請看后面的分析。

下面這個圖的三角形的兩個直角邊就是目標點的橫坐標和縱坐標x,y。那么我們是可以求出邊y對着的那個角，並且 $\beta$我們已經求出了。因此我們可以求出我們想要的關節角q1.

總結：

現在我們得到了兩個關節角的求解方式。

但是由於cos函數是一個對稱函數，所以同一個值會對應兩個可能的角度。這也符合我們的預期，因為除非兩個桿完全在同一條線上，否則任意給定一個目標位置就一定可以給出兩種不同的彎曲方式。
情況1：

情況2：

Python編程實踐模擬一個二連桿機器人

前向運動可視化

""" @author 李韜——知乎@Ai醬 教程地址：https://blog.csdn.net/varyshare/article/details/96885179 """
import numpy as np
from math import sin,cos,pi
import matplotlib.pyplot as plt
class TwoLinkArm:
    """ 兩連桿手臂模擬。 所使用的變量與模擬實體對應關系如下所示： (joint0)——連桿0——(joint1)——連桿1——[joint2] 注意：joint0是基座也是坐標原點(0,0) """
    def __init__(self,_joint_angles=[0,0]):
        # 第0個關節是基座所以坐標固定是原點(0,0)
        self.joint0 = np.array([0,0])
        # 機器人兩段連桿（手臂）的長度
        self.link_lengths = [1,1]
        self.update_joints(_joint_angles)
        self.forward_kinematics()
        

    def update_joints(self, _joint_angles):
        self.joint_angles = _joint_angles
    
    def forward_kinematics(self):
        """ 根據各個關節角計算各個關節的位置. 注意：所使用的變量與模擬實體對應關系如下所示： (joint0)——連桿0——(joint1)——連桿1——[joint2] """

        # 計算關節1的位置
        # q0,q1分別是第0和第1個關節的關節角
        q0 = self.joint_angles[0]
        a0 = self.link_lengths[0]
        self.joint1 = self.joint0 + [a0*cos(q0), a0*sin(q0)]

        # 計算關節2的位置
        q1 = self.joint_angles[1]
        a1 = self.link_lengths[1]
        # 注意：q1是桿1相對於桿0的延長線的轉角，而桿0相對水平線的轉角是q0
        # 所以桿1相對水平線的轉角是(q0+q1), 而joint2是桿1的末端
        self.joint2 = self.joint1 + [a1*cos(q0+q1), a1*sin(q0+q1)]
    
    def plot(self):
        """ 繪制當前狀態下的機械臂 """
        # 三個關節的坐標
        x = [self.joint0[0],self.joint1[0],self.joint2[0]]
        y = [self.joint0[1],self.joint1[1],self.joint2[1]]
        # 繪制這樣的一條線——連桿0————連桿1——
        plt.plot(x, y,c='red',zorder=1)
        # 繪制三個黑圓點代表關節,zorder=2是為了讓繪制的點蓋在直線上面
        plt.scatter(x,y,c='black',zorder=2)
        plt.show()
    def transform(_points,_theta,_origin):
        """ 求這些點_points繞_origin這個點旋轉_theta度后相對世界坐標系的坐標。 注意_points的坐標是相對以_origin為原點的坐標系下的坐標。 _origin: 旋轉中心點在世界坐標系下的坐標 _points: 相對旋轉中心這個 _theta: 旋轉的角度 """
        T = np.array([
            [cos(_theta), -sin(_theta),  _origin[0]],
            [sin(_theta), cos(_theta),   _origin[1]],
            [0,           0,                      1]
        ])
        

arm_robot = TwoLinkArm([pi/6,pi/4])
arm_robot.plot()

運行截圖：

鼠標選定屏幕上一點，然后求逆解進行運動Python實現代碼

下面是效果圖，打開你的編輯器跟着我寫的代碼實踐吧，你的贊和關注是我持續分享的動力。

""" @author 李韜——知乎@Ai醬 教程地址：https://blog.csdn.net/varyshare/article/details/96885179 """
import numpy as np
from numpy import cos, sin, arccos, arctan2, sqrt
import matplotlib.pyplot as plt

(target_x,target_y) = (1,1) # 機器人要到達的目標點
class TwoLinkArm:
    """ 兩連桿手臂模擬。 所使用的變量與模擬實體對應關系如下所示： (joint0)——連桿0——(joint1)——連桿1——[joint2] 注意：joint0是基座也是坐標原點(0,0) """

    def __init__(self, _joint_angles=[0, 0]):
        # 第0個關節是基座所以坐標固定是原點(0,0)
        self.joint0 = np.array([0, 0])
        # 機器人兩段連桿（手臂）的長度
        self.link_lengths = [1, 1]
        self.update_joints(_joint_angles)

    def update_joints(self, _joint_angles):
        self.joint_angles = _joint_angles
        self.forward_kinematics()

    def forward_kinematics(self):
        """ 根據各個關節角計算各個關節的位置. 注意：所使用的變量與模擬實體對應關系如下所示： (joint0)——連桿0——(joint1)——連桿1——[joint2] """

        # 計算關節1的位置
        # q0,q1分別是第0和第1個關節的關節角
        q0 = self.joint_angles[0]
        a0 = self.link_lengths[0]
        self.joint1 = self.joint0 + [a0 * cos(q0), a0 * sin(q0)]
        # 計算關節2的位置
        q1 = self.joint_angles[1]
        a1 = self.link_lengths[1]
        # 注意：q1是桿1相對於桿0的延長線的轉角，而桿0相對水平線的轉角是q0
        # 所以桿1相對水平線的轉角是(q0+q1), 而joint2是桿1的末端
        self.joint2 = self.joint1 + [a1 * cos(q0 + q1), a1 * sin(q0 + q1)]

    def plot(self):
        """ 繪制當前狀態下的機械臂 """
        
        # 清理坐標系中的內容
        plt.cla()

        # 三個關節的坐標
        x = [self.joint0[0], self.joint1[0], self.joint2[0]]
        y = [self.joint0[1], self.joint1[1], self.joint2[1]]
        # 繪制這樣的一條線——連桿0————連桿1——
        plt.plot(x, y, c="red", zorder=1)
        # 繪制三個黑圓點代表關節,zorder=2是為了讓繪制的點蓋在直線上面
        plt.scatter(x, y, c="black", zorder=2)
        # 繪制目標點
        global target_x,target_y
        plt.scatter(target_x,target_y,c='blue',marker='*')
        # 固定住坐標系，
        # 不讓它亂變，不讓我點擊的坐標和它顯示的坐標不是一個坐標
        plt.xlim(-2, 2)
        plt.ylim(-2, 2)
        


    def inverse_kinematic(self, x, y):
        """ 逆運動學求解要達到(x,y)需要轉動的角度， 返回機器人各關節需要轉動的角度 """
        a0 = self.link_lengths[0]
        a1 = self.link_lengths[1]
        q1 = arccos((x ** 2 + y ** 2 - a0 ** 2 - a1 ** 2) / (2 * a0 * a1))
        q0 = arctan2(y, x) - arctan2(a1 * sin(q1), a1 * cos(q1) + a0)
        return [q0, q1]

    def animation(self,x,y):
        _joint_angles = self.inverse_kinematic(x, y)

        # 將這個角度變化過程分解成一個1s內的執行15步的慢動作
        duration_time_seconds = 1
        actions_num = 15
        angles_per_action = (np.array(_joint_angles) - np.array(self.joint_angles))/actions_num
        plt.ion() # 開啟交互模式不然沒有動畫效果
        for action_i in range(actions_num):
            
            self.joint_angles = np.array(self.joint_angles) + angles_per_action
            self.update_joints(self.joint_angles)
            self.plot()
            dt = duration_time_seconds/actions_num
            plt.pause(dt)


    
    def to_mouse_posi(self,event):
        """ 鼠標點擊事件處理函數：記錄鼠標在坐標系中的位置(x,y) 然后將其設置為機器人要到達的目標點 """
        global target_x, target_y
        if event.xdata == None or event.ydata == None:
            print("請在坐標系內選擇一個點")
            return
        target_x = event.xdata
        target_y = event.ydata
        self.animation(target_x,target_y)
            

# ---------------------------------
def main():
    fig = plt.figure()
    arm_robot = TwoLinkArm()
    arm_robot.animation(target_x,target_y)
    fig.canvas.mpl_connect("button_press_event", arm_robot.to_mouse_posi)
    plt.ioff() # 一定要終止交互模式不然會一閃而過
    plt.show()


if __name__ == "__main__":
    main()
    pass

參考文獻：
[1] https://robotacademy.net.au/lesson/inverse-kinematics-for-a-2-joint-robot-arm-using-geometry/