概要
東京大学GLP- GEfILから奨学金を頂いて、アーヘン工科大学のサマースクール、「Robotics for Future Industrial Applications」というプログラムに参加しました。そのときの勉強内容についてまとめています。また、ロボティクスに関しては初学者であるため、誤った箇所もあるかと思います。その際はコメント等で指摘いただけると嬉しいです。
※RWTHでの授業スライドから適宜引用しています。
Topology in Robotics
In the context of Robotics, "topology" refers to how the various parts are connected aside from specific geometric details or sizes. Topology can be determined by following notation rules.
- First joint axis is identical with the $z_{0}$-axis
- Second axis should also be parallel to the $z_{0}$-axis, if not possible choose the $y_{0}$-axis
- Third axis should also be parallel to the $z_{0}$-axis, if not possible choose the $y_{0}$-axis, else choose the $x_{0}$-axis
Quiz1
- Specify the topology of the shown SCARA robot
Answer1
Two joints are located in parallel. So just set $z_{0}$-axis parallel to the two joints.
This topology is categorized as "CCZ111"
$x_{0}$-axis and $y_{0}$-axis can be put freely, but please make sure it is a "right-handed system".
Degree of Freedom (DoF)
The degree of Freedom can be calculated by using the Chebychev-Grübler-Kutzbach criterion.
- DoF of planer structure
M = 3(n-1)-3j+\sum f_{i} - f_{id}
- DoF of spatial structure
M = 6(n-1)-6j+\sum f_{i} - f_{id}
$n$ → number of rigid links
Do not forget the base
$j$ → number of joints
$f_{i}$ → DoF of joint i
$f_{id}$ → sum of identical DoF
The following structure is one of the identical DoF
Quiz2
Calculate DoF for the following structure
Answer2
$n$ = 1(base) + 7$j$ = 9
$f_{i}$ = 1
$f_{id}$ = 0
→ $M = 3$
Quiz3
Calculate DoF for the following structure
Answer3
$n$ = 1(base) + 6$j$ = 6
$f_{i}$ = 1
$f_{id}$ = 0
→ $M = 6$
Kinematics
This section is about "direct kinematics" and "inverse kinematics" (Also added "differential kinematics." ). These are the concepts often used in the field of Robotics to describe the relationship between the position of a robot's components and its end effector. The following figure shows the concepts of Direct Kinematics and Inverse Kinematics.
Direct kinematics
In direct kinematics, the position of the end effector is determined based on its joint space, the angle or length of the joints.
Let's think about the following 2D model.
There are two joints in the model, and joint space can be written as
$$\mathbf{q} = [\theta_{1},\theta_{2}] ^T$$
Now we want to describe the position of the end effector
$$\mathbf{r} = [p_{x},p_{y}] ^T$$
We can get the following result by simple math.
$$p_{x} = l_{1}cos(\theta_{1})+l_{2}cos(\theta_{1}+\theta_{2})$$
$$p_{y} = l_{1}sin(\theta_{1})+l_{2}sin(\theta_{1}+\theta_{2})$$
In this case, we can easily calculate but what should we do, if the structure becomes complicated? Homogeneous transformation comes into play.
I won't explain the detail of the Homogeneous Transformation Matrices in this article. The 2D version is as follows.
So transformation matrix $\mathbf{T}$ is
\mathbf{T} =
\begin{bmatrix}cos(\theta_{1}) & -sin(\theta_{1}) & L_{1}cos(\theta_{1})\\sin(\theta_{1}) & cos(\theta_{1}) & L_{1}sin(\theta_{1})\\0 & 0 & 1 \end{bmatrix}\begin{bmatrix}cos(\theta_{2}) & -sin(\theta_{2}) & L_{2}cos(\theta_{2})\\sin(\theta_{2}) & cos(\theta_{2}) & L_{2}sin(\theta_{2})\\0 & 0 & 1 \end{bmatrix} \\
= \begin{bmatrix}? & ? & L_{1}cos(\theta_{1})+L_{2}cos(\theta_{1}+\theta_{2})\\? & ? & L_{1}sin(\theta_{1})+L_{2}sin(\theta_{1}+\theta_{2})\\0 & 0 & 1 \end{bmatrix}
last columns of $\mathbf{T}$ represents joint space.
You can do exactly the same thing in 3D models. In complicated cases, the Denavit-Hartenberg convention (DH convention) is useful. By deploying the DH convention, all we need to do is to determine 4 DH parameters.
| Parameter | Description |
|---|---|
| Link Length ($a_{i}$) | Distance along the link's axis from the old joint to the new joint |
| Joint Twist ($\alpha_{i}$) | Twist angle about the Z-axis |
| Link Offset ($d_{i}$) | Offset along the Z-axis |
| Joint Angle ($θ_{i}$) | Rotation angle of the joints |
Let's think about the following 3D model.
For those who can't understand the difference between $a_{i}$ and $d_{i}$ Just think as follows.
- $d_{i}$ is the distance along the Z-axis, and $a_{i}$ is the distance in XY-plane
Let's use this DH convention in the following 3D model.
First, we need to determine the topology for the model. Just recall the naming rule we discussed in the previous section. Here is an example of the topology. (I think the topology in the lecture slide doesn't match the calculation. → I asked the teacher and he said $\theta_{1}$ can include any constant angle. given that, the topology taught in the lecture is also correct.)
In this case, DH parameters are as follows.
Now we can calculate Homogeneous Transformation Matrices on each joint. Here are the Homogeneous Transformation Matrices for rotation in 3D$^{[1]}$. We can also add the translation part to the last column as Matrices in 2D.
The result of the calculation is as follows.
Inverse kinematics
"Inverse kinematics" is a method in robotics that involves determining the joint angles of a robot's manipulator to achieve a desired end effector position and orientation. It's the opposite of direct kinematics, which calculates the end effector's position based on given joint angles.
I'll explain 3 solutions for inverse kinematics.
analytical (position)
Differential kinematics(editing)
Differential kinematics shows the relationship between the joint velocities and the corresponding end-effector translational and angular velocity. Thus, the goal of differential kinematics is to determine the Jacobian in the following equation.
$$
\mathbf{v_{e}} = J_{G}(\mathbf{q})\dot{\mathbf{q}}
$$
$\mathbf{v_{e}}$ is the vector of operational velocities, and $\mathbf{q}$ is the vector of joint variables. Jacobian can be determined as follows.
- Prismatic Joint
$$
\mathbf{v_{e}} = J_{G}(\mathbf{q})\dot{\mathbf{q}}
$$ - Revolute Joint
\begin{bmatrix}\mathbf{j}_{GPi} \\\mathbf{j}_{GOi} \end{bmatrix}
= \begin{bmatrix}\overset{0}{\mathbf{e}_{z,}} \\\mathbf{0} \end{bmatrix}
Bayes Filter
A Bayes filter is a method used to estimate the position of the robot by combining newly measured data with previous data using probability.
To understand the Bayes Filter, some knowledge of statics is needed. Let me introduce "Bays Rule" and "Markov assumption" first.
Here is the Bays Rule.
And here is the Markov assumption. Please make sure the Markov assumption is allowed to use only when the parameters have "Markov property".
Let's think about the following model.
And the parameters are as follows.
There are 2 parts to the Beyes filter, That is the "prediction part" and the "update part". The estimated position is updated by implementing these two parts alternately.
prediction
$$ p(x_{t}|u_{t},x_{t-1}) $$
For the prediction part, we estimate the robot's location based on $x_{t-1}previously estimated location) and $u_{t}$ (intended action). I think this part is easy to understand.
updating
$$ p(x_{t}|u_{1:t},z_{1:t}) $$
For updating part, we updated the robot's location estimated in the prediction part. Just make sure $z_{t}$ (measured location) is newly used. It can be said that we "modify" the location estimated in the prediction part by utilizing the measured data.
Then How to "modify" the location $p(x_{t}|u_{1:t},z_{1:t})$? We will use statistical knowledge as follows.
The part underlined green is the estimated location in the prediction part. So we can "modify" the location by substituting the data from the prediction part.
Finally, just take a look at the simple case.
supposed there is a robot equipped with a position sensor that moves one cell, either horizontally or vertically. However, just make sure both sensing and moving are error-prone. Measurement probability $p(z_t|x_t)$ is as follows. $d_1(x_t,z_t)$ denotes the Manhattan distance between the real position $x_t$ and the measurement $z_t$.
| $d_1(x_t,z_t)=0$ | $d_1(x_t,z_t)=1$ | $d_1(x_t,z_t)=2$ | $d_1(x_t,z_t)>2$ |
|---|---|---|---|
| $p(z_t|x_t)=0.2$ | $p(z_t|x_t)=0.1$ | $p(z_t|x_t)=0.05$ | $p(z_t|x_t)=0$ |
Here is the prediction part. In this function, previous_belief is updated by action. When updating the action, the error rate of the moving (STAY_PROB = 0.2) takes care into consideration.
def predict(previous_belief, action):
# error rate for moving
MOVE_PROB = 0.8
STAY_PROB = 0.2
prediction = np.ndarray(previous_belief.shape)
for pos, _ in np.ndenumerate(prediction):
if (action == Action.STAY):
prediction[pos] = previous_belief[pos]
elif (action == Action.LEFT):
if pos[0] == 0:
# We hit the wall
prediction[pos] = previous_belief[
pos] + MOVE_PROB * previous_belief[1, pos[1]]
elif pos[0] == prediction.shape[0] - 1:
# No movement from the right
prediction[pos] = STAY_PROB * previous_belief[pos]
else:
prediction[pos] = MOVE_PROB * previous_belief[
pos[0] + 1, pos[1]] + STAY_PROB * previous_belief[pos]
elif action == Action.UP:
#omit other parts
return prediction
Here is the updating part. (I haven't tested this part.)
def update(prediction, measurement):
for distance in range(len(prediction)):
liste_of_cordinates = name(prediction,measurement,distance) # liste of cordinates for distance distance
for i in range(len(prediction)): # 0
for j in range(len(prediction[0])):
for element in liste_of_cordinates:
if(element == (i,j)):
if(distance > 2):
prediction[i][j] = 0
else:
prediction[i][j] *= 0.2/2**distance
total = 0
for i in range(len(prediction)):
get_sum = np.sum(prediction[i])
total += get_sum
for i in range(len(prediction)):
for j in range(len(prediction[0])):
prediction[i][j] *= 1 / total
return prediction
Monte-Carlo Localization
When there is data about odometry of the robot (odometry data) and positional relationship between the robot and landmarks (distance & angle data), the robot's position can be estimated by Monte-Carlo Localization.
Odometry is estimated amount of change in the robot's position. the data about wheel rotations, encoder readings, or other motion sensors is usually used for estimating odometry.
In other words, if a robot wants to utilize Monte-Carlo Localization, the robot needs 2 sensors. one for measuring odometry and another for measuring the distance and angle between the robots and landmarks.
This is an example of visualization. Each particle represents the probability distribution of the Robot's position and angle. In the following picture, probability distributions have not converged yet.
Monte-Carlo Localization consists of the following 2 steps.
- initialize particles
- resample particles (repeating)
2-1 Using odometry data
2-2 Using distance & angle data
1
As for step 1, just initialize particles using a uniform distribution. From now on, each particle can be regarded as the robot's potential position. (particles are only probability distribution though)
2
In step 2, initialized particles should be repeatedly updated. First, using odometry data
to update each particle in the following algorithm. The function sample($\sigma^2$) in L5〜7 returns a sample from a Gaussian distribution with mean $\mu = 0 $ and variance $\sigma^2$.$\alpha_{1〜4}$ are given parameters.
The following memo is my understanding of the algorithm. The algorithm is very simple except for the part adding uncertainty based on Gaussian(L5〜7).
And after updating all particles by odometry, the particle needs to be updated by distance & angle data. Please keep in mind we have 2 types of distance & angle data. That is
-
actual distance & angle data (we know each particular position and landmark position, so we can easily calculate this)
-
measured distance & angle data (the data measured by the sensor)
difference between these two data can be calculated (1), and then the probability of each particle can also be calculated from a Gaussian distribution with mean $\mu = 0$ (2). After that particles are resampled using the probabilities as weights (3). If there are several landmarks, we can just repeat multiplying provability as following codes.
def landmark_model(self, pos, landmark_measurements):
for landmark_name, measurement in landmark_measurements.items():
dx = self.landmarks[landmark_name][0] - pos[0]
dy = self.landmarks[landmark_name][1] - pos[1]
true_distance = np.sqrt(dx**2 + dy**2)
true_angle = np.arctan2(dy, dx) - pos[2]
# calculatedifferencess (1)
delta_distance = measurement[0] - true_distance
delta_angle = measurement[1] - true_angle
# calculatethe probability of each particle (2)
prob_distance = np.exp(-0.5 * (delta_distance / sigma_r)**2) / (sigma_r * np.sqrt(2 * np.pi))
prob_angle = np.exp(-0.5 * (delta_angle / sigma_phi)**2) / (sigma_phi * np.sqrt(2 * np.pi))
probability *= prob_distance * prob_angle
return probability
def resample(self, particles, weights):
# weights is the kist of each probability
# Resample from the given particles according to the given weights (3)
particle_indexes = np.random.choice(self.num_particles,self.num_particles,p=weights)
resampled_particles = []
for i in range(self.num_particles):
resampled_particles.append(particles[particle_indexes[i]])
return resampled_particles
Following is an easy example of resampling.
example1
In the following odometry, the robot's position will be estimated like this. landmark is at (5,5).
# For example, the first one means the robots rotate pi / 4
[
((0, 0, 0), (0, 0, np.pi / 4)),
((0, 0, 0), (0, 0, np.pi / 4)),
((0, 0, 0), (0, 0, np.pi / 4)),
((0, 0, 0), (0, 0, np.pi / 4)),
((0, 0, 0), (0, 0, np.pi / 4)),
((0, 0, 0), (0, 0, np.pi / 4)),
((0, 0, 0), (0, 0, np.pi / 4)),
((0, 0, 0), (0, 0, np.pi / 4)),
((0, 0, 0), (1, 0, 0)),
((0, 0, 0), (1, 0, 0)),
((0, 0, 0), (1, 0, 0)),
]
From $t=1〜8$ there are 2 clusters of particles. This is because, robots rotate at the same place in $t=1〜8$, and the following 2 cases cannot be distinguished by only angle information. (When the robots begin moving, the probability of an inappropriate cluster will be declined and disappear.)
Quotation
- Slides used in RWTH summer school
- [1] https://imagingsolution.net/math/rotation-scaling-translation-3d-matrix/