Two joint Manipulator —— Point Tracking (几何法求解)

- 几何关系几何关系shoulder——肩部；Elbow——肘部；Wrist——腕部；

手腕Wrist本身不是一个关节，但是我们可以把它视为我们的末端执行器(end-effector)。

将shoulder肩膀约束到原点，根据Forward Kinematics，我们可以编写肘部和腕部的运动学方程如下：

由于手腕是我们的末端执行器，因此我们将其坐标称为x和y。 这样我们末端执行器的正向运动学就可以得到：

-

求解从末端执行器到机械臂底座的距离，我们记为

，

与

有如下关系：

接着，根据余弦定理，

又可以写成：

根据该式，有：

-

求解根据该几何关系，有

，又

, 所以，剩余的joint angle

的表达如下：

又很容易得到，所以：

转换成代码的atan2的形式，有：

- 代码首先，定义一个类来简单的plot手臂：

#! /usr/bin/env python

# -*- coding: utf-8 -*-

# author: IrvingHe

# email: irvinghe1518@gmail.com

"""2轴机械臂点控制"""

"""1.Robot Arm简单可视化"""

from math import cos,sin

import numpy as np

import matplotlib.pyplot as plt

class TwoLinkArm:

"""2关节机械臂可视化"""

def __init__(self,joint_angles=[0,0]):

self.shoulder = np.array([0,0]) # 肩膀为0点

self.link_lengths = [1,1] # 两臂长度均为1,1

self.update_joints(joint_angles) # 更新joint位置

def update_joints(self,joint_angles):

self.joint_angles = joint_angles

self.forward_kinematics()

def forward_kinematics(self):

"""顺向运动学"""

theta0 = self.joint_angles[0]

theta1 = self.joint_angles[1]

l0 = self.link_lengths[0]

l1 = self.link_lengths[1]

self.elbow = self.shoulder+np.array([l0*cos(theta0),l0*sin(theta0)]) # 肘的位置更新

self.wrist = self.elbow+np.array([l1*cos(theta0+theta1),l1*sin(theta0+theta1)]) # 腕部位置更新

def plot(self):

plt.plot([self.shoulder[0], self.elbow[0]],

[self.shoulder[1], self.elbow[1]],

'r-') # 肩部到肘部的Link

plt.plot([self.elbow[0], self.wrist[0]],

[self.elbow[1], self.wrist[1]],

'r-') # 肘部到腕部的Link

plt.plot(self.shoulder[0], self.shoulder[1], 'ko') # 肩部Origin

plt.plot(self.elbow[0], self.elbow[1], 'ko') # 肘 Joint

plt.plot(self.wrist[0], self.wrist[1], 'ko') # 腕 Joint

if __name__== '__main__':

joint_angles = [np.deg2rad(20),np.deg2rad(20)] # 弧度制

TwoLinkArm = TwoLinkArm(joint_angles)

TwoLinkArm.update_joints(joint_angles)

TwoLinkArm.plot()

plt.show()用20°进行测试，如下：现在我们有一个TwoLinkArm的类来帮助我们可视化机械臂，接下来，进行point tracking...

#! /usr/bin/env python

# -*- coding: utf-8 -*-

# author: IrvingHe

# email: irvinghe1518@gmail.com

"""

2轴机械臂点控制

"""

"""1.Robot Arm简单可视化"""

from math import cos,sin

import numpy as np

import matplotlib.pyplot as plt

class TwoLinkArm:

"""

2关节机械臂可视化

"""

def __init__(self,joint_angles=[0,0]):

self.shoulder = np.array([0,0]) # 肩膀为0点

self.link_lengths = [1,1] # 两臂长度均为1,1

self.update_joints(joint_angles) # 更新joint位置

def update_joints(self,joint_angles):

self.joint_angles = joint_angles

self.forward_kinematics()

def forward_kinematics(self):

"""顺向运动学"""

theta0 = self.joint_angles[0]

theta1 = self.joint_angles[1]

l0 = self.link_lengths[0]

l1 = self.link_lengths[1]

self.elbow = self.shoulder+np.array([l0*cos(theta0),l0*sin(theta0)]) # 肘的位置更新

self.wrist = self.elbow+np.array([l1*cos(theta0+theta1),l1*sin(theta0+theta1)]) # 腕部位置更新

def plot(self):

plt.plot([self.shoulder[0], self.elbow[0]],

[self.shoulder[1], self.elbow[1]],

'r-') # 肩部到肘部的Link

plt.plot([self.elbow[0], self.wrist[0]],

[self.elbow[1], self.wrist[1]],

'r-') # 肘部到腕部的Link

plt.plot(self.shoulder[0], self.shoulder[1], 'ko') # 肩部Origin

plt.plot(self.elbow[0], self.elbow[1], 'ko') # 肘 Joint

plt.plot(self.wrist[0], self.wrist[1], 'ko') # 腕 Joint

# 仿真参数

Kp = 15

dt = 0.01

# Link Lengths

l1=l2=1

# 将初始目标位置设置为初始末端执行器位置

x = 2

y = 0

show_animation = True

if show_animation:

plt.ion() # 动态视图

def two_joint_arm(GOAL_TH=0.0,theta1=0.0,theta2=0.0):

"""

计算平面2DOF臂的逆运动学

GOAL_TH: 距离目标的判别阈值

"""

while True:

try:

"""Inverse Kinematics"""

theta2_goal = np.arccos((x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2))

theta1_goal = np.math.atan2(y, x) - np.math.atan2(l2 *\

np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal)))

if theta1_goal < 0:

theta2_goal = -theta2_goal

theta1_goal = np.math.atan2(y, x) - np.math.atan2(l2 *

np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal)))

# 更新theta1,theta2

theta1 = theta1 + Kp * ang_diff(theta1_goal, theta1) * dt

theta2 = theta2 + Kp * ang_diff(theta2_goal, theta2) * dt

except ValueError as e:

print("无法到达的位置")

wrist = plot_arm(theta1,theta2,x,y)

# 检查goal

d2goal = np.hypot(wrist[0] - x, wrist[1] - y)

if abs(d2goal) < GOAL_TH and x is not None:

return theta1, theta2

def ang_diff(theta1, theta2):

# 返回两个角度之差，范围在[-pi,pi]

return (theta1 - theta2 + np.pi) % (2 * np.pi) - np.pi

def plot_arm(theta1, theta2, x, y):

"""改版，动态可视化"""

shoulder = np.array([0, 0])

elbow = shoulder + np.array([l1 * np.cos(theta1), l1 * np.sin(theta1)])

wrist = elbow + \

np.array([l2 * np.cos(theta1 + theta2), l2 * np.sin(theta1 + theta2)])

if show_animation:

plt.cla()

plt.plot([shoulder[0], elbow[0]], [shoulder[1], elbow[1]], 'k-')

plt.plot([elbow[0], wrist[0]], [elbow[1], wrist[1]], 'k-')

plt.plot(shoulder[0], shoulder[1], 'ro')

plt.plot(elbow[0], elbow[1], 'ro')

plt.plot(wrist[0], wrist[1], 'ro')

plt.plot([wrist[0], x], [wrist[1], y], 'g--')

plt.plot(x, y, 'g*')

plt.xlim(-2, 2)

plt.ylim(-2, 2)

plt.show()

plt.pause(dt)

return wrist

def click(event):

# 鼠标点击触发事件，作为导航点

global x, y

x = event.xdata

y = event.ydata

def animation():

from random import random

global x, y

theta1 = theta2 = 0.0

for i in range(5):

x = 2.0 * random() - 1.0 # 末端随机位置

y = 2.0 * random() - 1.0

theta1, theta2 = two_joint_arm(

GOAL_TH=0.01, theta1=theta1, theta2=theta2)

def main(): # pragma: no cover

fig = plt.figure()

fig.canvas.mpl_connect("button_press_event", click)

# 使用esc键停止模拟。

fig.canvas.mpl_connect('key_release_event',

lambda event: [exit(0) if event.key == 'escape' else None])

two_joint_arm()

if __name__== '__main__':

# joint_angles = [np.deg2rad(20),np.deg2rad(20)] # 弧度制

# TwoLinkArm = TwoLinkArm(joint_angles)

# TwoLinkArm.update_joints(joint_angles)

# TwoLinkArm.plot()

# plt.show()

main()

最终效果：

后续实践笔记：基于强化学习的point tracking；

多轴(>2)的point tracking；

3D tracking；